Ejemplo n.º 1
0
void Kick::Recovering_state_code(void)
{
//BUILDER COMMENT. DO NOT REMOVE. begin Recovering_state_code
	ALValue path;
	path.arraySetSize(6);
	path[0] = 0.0;
	path[2] = 0.0;
	path[3] = 0.0;
	path[4] = 0.0;
	path[5] = 0.0;
	if(foot == LEFT)
	{
		path[1] = 0.05;
		pmotion->positionInterpolation("LLeg", 2, path, 63, 1.5, false);
		path[1] = -0.05;
		pmotion->positionInterpolation("RLeg", 2, path, 63, 1.5, false);
	}else
	{
		path[1] = -0.05;
		pmotion->positionInterpolation("RLeg", 2, path, 63, 1.5, false);
		path[1] = 0.05;
		pmotion->positionInterpolation("LLeg", 2, path, 63, 1.5, false);
	}

	path[0] = 0.0;
	path[1] = 0.00;
	path[2] = -0.03;
	path[3] = 0.0;
	path[4] = 0.0;
	path[5] = 0.0;

	pmotion->positionInterpolation("Torso", 2, path, 63, 1.0, false);

//BUILDER COMMENT. DO NOT REMOVE. end Recovering_state_code
}
void APIDemonstration::move_joints(const ALValue& joints,
                                   const ALValue& target_angles,
                                   const ALValue& target_times,
                                   const bool &restore_pos,
                                   const std::string& phrase,
                                   const float& phrase_lag) {

    try{
        bool useSensors = false;
        std::vector<float> angles_before = motion_proxy.getAngles(joints, useSensors);
        std::vector<float> stiffness_before = motion_proxy.getStiffnesses(joints);

        int n = joints.isArray() ? joints.getSize() : 1;
        motion_proxy.setStiffnesses(joints, std::vector<float>(n, 1.0));
        
        bool isAbsolute = true;
        int id = motion_proxy.post.angleInterpolation(joints, target_angles, target_times, isAbsolute);

        qi::os::sleep(phrase_lag);
        if (phrase != "") {
            TTS_proxy.setLanguage("English");
            TTS_proxy.post.say(phrase);
        }

        if (restore_pos)
            motion_proxy.angleInterpolation(joints, angles_before, std::vector<float>(n, 1.0), true);
        motion_proxy.setStiffnesses(joints, stiffness_before);
        motion_proxy.wait(id, 0);
    }
    catch (const ALError& e) {
        std::cerr << "Caught exception: " << e.what() << std::endl;
    }
}
Ejemplo n.º 3
0
/**
 * saveImageRemote : test remote image
 * @param pName path of the file
 */
void vision::testRemote(){

  //Now you can get the pointer to the video structure.
  ALValue results;
  results.arraySetSize(7);

  try
  {
    results = ( camera->call<ALValue>( "getImageRemote", name ) );
  }catch( ALError& e)
  {
    log->error( "vision", "could not call the getImageRemote method of the NaoCam module" );
  }

  if (results.getType()!= ALValue::TypeArray) return;

  const char* dataPointerIn =  static_cast<const char*>(results[6].GetBinary());
  int size = results[6].getSize();

  //You can get some informations of the image.
  int width = (int) results[0];
  int height = (int) results[1];
  int nbLayers = (int) results[2];
  int colorSpace = (int) results[3];
  long long timeStamp = ((long long)(int)results[4])*1000000LL + ((long long)(int)results[5]);

  // now you create an openCV image and you save it in a file.
  IplImage* image = cvCreateImage( cvSize( width, height ), 8, nbLayers );

//  image->imageData = ( char* ) imageIn->getFrame();
  image->imageData = ( char* ) dataPointerIn;

  std::string pName = "aaa";

  //const char* imageName = ( pName + DecToString(results[4]) + ".jpg").c_str();
  const char* imageName = ( pName + "test" + ".jpg").c_str();

printf("imageName %s\n", imageName);
  cvSaveImage( imageName, image );
printf("image saved\n");

try
{
  results = ( camera->call<ALValue>( "releaseImage", name ) );
}catch( ALError& e)
{
  log->error( "vision", "could not call the releaseImage method of the NaoCam module" );
}

printf("image memory released\n");

//  cvReleaseImage( &image );
  cvReleaseImageHeader(&image);
printf("image released\n");
printf("testRemote finished\n");
}
void APIDemonstration::face_detected() {
    
    qiLogInfo("module.example") << "Executing callback method on face_detected event" << std::endl;

    ALCriticalSection section(fCallbackMutexFaceDetection);
    ALValue data =  memory_proxy.getData("FaceDetected");
    if (data.getSize() > 0) {
        TTS_proxy.say("Yep");
        b_face_detected = true;
    }
}
Ejemplo n.º 5
0
/**
 * @brief 
 */
void oru_walk::initPosition()
{
    /// @attention Hardcoded parameter!
    unsigned int init_time = 1200;


    ALValue initPositionCommands;

    initPositionCommands.arraySetSize(6);
    initPositionCommands[0] = string("jointActuator");
    initPositionCommands[1] = string("ClearAll");
    initPositionCommands[2] = string("time-separate");
    initPositionCommands[3] = 0;
    initPositionCommands[4].arraySetSize(1);
    initPositionCommands[5].arraySetSize(JOINTS_NUM);
    for (int i = 0; i < JOINTS_NUM; i++)
    {
        initPositionCommands[5][i].arraySetSize(1);
    }
    initJointAngles (initPositionCommands[5]);
    for (int i = 0; i < LOWER_JOINTS_NUM; i++)
    {
        ref_joint_angles[i] = initPositionCommands[5][i][0];
    }



    // set time
    try
    {
        initPositionCommands[4][0] = dcm_proxy->getTime(init_time);
    }
    catch (const ALError &e)
    {
        ORUW_THROW_ERROR("Error on DCM getTime : ", e);
    }


    // send commands
    try
    {
        dcm_proxy->setAlias(initPositionCommands);
    }
    catch (const AL::ALError &e)
    {
        ORUW_THROW_ERROR("Error with DCM setAlias : ", e);
    }

    qi::os::msleep(init_time);
    qiLogInfo ("module.oru_walk", "Execution of initPosition() is finished.");
}
Ejemplo n.º 6
0
int Sensors::Execute()
{
	if(firstrun)
	{
		//Starting US Sensors
		ALValue commands;
		commands.arraySetSize(3);
		commands[0] = string("Device/SubDeviceList/US/Actuator/Value");
		commands[1] = string("Merge");
		commands[2].arraySetSize(1);
		commands[2][0].arraySetSize(2);
		commands[2][0][0] = 68.0;
		commands[2][0][1] = dcm->getTime(10);
		dcm->set(commands);
		rtm.start();
#ifndef KROBOT_IS_REMOTE
		KAlBroker::Instance().GetBroker()->getProxy("DCM")->getModule()->atPostProcess(KALBIND(&Sensors::synchronisedDCMcallback , this));
#endif
		firstrun = false;
	}

#ifdef KROBOT_IS_REMOTE
	//Fetch into vectors
	jointaccess.GetValues(jointValues);
	sensoraccess.GetValues(sensorValues);
	fetchValues();
	publishData(ASM, "sensors");

	if(updateButtons())
	{
		publishSignal(BM, "buttonevents");
	}

#endif
	float oldvalue;
	vector<float> RobotValues = motion->getRobotPosition(true);

	//A vector containing the World Absolute Robot Position. (Absolute Position X, Absolute Position Y, Absolute Angle Z)
	for (unsigned i = 0; i < RobotValues.size(); i++)
	{
		oldvalue = RPM.sensordata(i).sensorvalue();
		RPM.mutable_sensordata(i)->set_sensorvalue(RobotValues[i]);
		RPM.mutable_sensordata(i)->set_sensorvaluediff(RobotValues[i] - oldvalue);
	}

	RPM.set_timediff(timediff);
	publishData(RPM, "sensors");
	return 0;
}
Ejemplo n.º 7
0
void Kick::Initial_state_code(void)
{
//BUILDER COMMENT. DO NOT REMOVE. begin Initial_state_code

//Primero ir a PoseInit
pmotion->setWalkTargetVelocity(0.0, 0.0, 0.0, 0.7);

ALValue path;
path.arraySetSize(6);
path[0] = 0.0;
path[2] = 0.0;
path[3] = 0.0;
path[4] = 0.0;
path[5] = 0.0;


path[1] = 0.03;
pmotion->positionInterpolation("LLeg", 2, path, 63, 0.5, false);
path[1] = -0.03;
pmotion->positionInterpolation("RLeg", 2, path, 63, 0.5, false);

HPoint3D ball3D;
/*HPoint2D ball2D;

ball2D.x = _BallDetectorOld->getX()*160.0+80.0;
ball2D.y = _BallDetectorOld->getY()*120.0+60.0;
ball2D.h = 1.0;

_Kinematics->get3DPositionZ(ball3D, ball2D, 0.04);

path[0] = 0.0;
path[2] = -0.03;
path[3] = 0.0;
path[4] = 0.0;
path[5] = 0.0;
*/

ball3D.Y = 1.0;

if(ball3D.Y > 0.0)
	path[1] = -0.07;
else
	path[1] = 0.07;

pmotion->positionInterpolation("Torso", 2, path, 63, 1.0, false);

//BUILDER COMMENT. DO NOT REMOVE. end Initial_state_code
}
Ejemplo n.º 8
0
void HWController::setJointData(const ALValue &values) {
    if (values.getSize() < NUM_OF_JOINTS) return;
    shared_ptr<vector<float> > local_actuator_values = make_shared<vector<float> >(NUM_OF_JOINTS);
    for (int i = 0; i < NUM_OF_JOINTS; ++i) local_actuator_values->at(i) = (float) values[i];
    lock_guard<mutex> guard(this->actuator_mutex);
    this->work_actuator_values.swap(local_actuator_values);
}
Ejemplo n.º 9
0
/**
 * @brief Set stiffness of joints.
 *
 * @param[in] stiffnessValue value of stiffness [0;1]
 */
void oru_walk::setStiffness(const float &stiffnessValue)
{
    ALValue stiffnessCommands;


    if ((stiffnessValue < 0) || (stiffnessValue > 1))
    {
        ORUW_THROW("Wrong parameters");
    }


    // Prepare one dcm command:
    // it will linearly "Merge" all joint stiffness
    // from last value to "stiffnessValue" in 1 seconde
    stiffnessCommands.arraySetSize(3);
    stiffnessCommands[0] = std::string("jointStiffness");
    stiffnessCommands[1] = std::string("Merge");
    stiffnessCommands[2].arraySetSize(1);
    stiffnessCommands[2][0].arraySetSize(2);
    stiffnessCommands[2][0][0] = stiffnessValue;


    /// @attention Hardcoded parameter!
    unsigned int stiffness_change_time = 1000;
    try
    {
        stiffnessCommands[2][0][1] = dcm_proxy->getTime(stiffness_change_time);
    }
    catch (const ALError &e)
    {
        ORUW_THROW_ERROR("Error on DCM getTime : ", e);
    }


    try
    {
        dcm_proxy->set(stiffnessCommands);
    }
    catch (const ALError &e)
    {
        ORUW_THROW_ERROR("Error when sending stiffness to DCM : ", e);
    }

    qi::os::msleep(stiffness_change_time);
    qiLogInfo ("module.oru_walk", "Execution of setStiffness() is finished.");
}
Ejemplo n.º 10
0
/**
 * Creates the appropriate aliases with the DCM
 */
void NaoEnactor::initDCMAliases(){
    ALValue positionCommandsAlias;
    positionCommandsAlias.arraySetSize(3);
    positionCommandsAlias[0] = string("AllActuatorPosition");
    positionCommandsAlias[1].arraySetSize(Kinematics::NUM_JOINTS);

    ALValue hardCommandsAlias;
    hardCommandsAlias.arraySetSize(3);
    hardCommandsAlias[0] = string("AllActuatorHardness");
    hardCommandsAlias[1].arraySetSize(Kinematics::NUM_JOINTS);

    for (unsigned int i = 0; i<Kinematics::NUM_JOINTS; i++){
        positionCommandsAlias[1][i] = jointsP[i];
        hardCommandsAlias[1][i] = jointsH[i];
    }

    dcmProxy->createAlias(positionCommandsAlias);
    dcmProxy->createAlias(hardCommandsAlias);
}
Ejemplo n.º 11
0
JNIEXPORT jlong JNICALL Java_jp_ac_fit_asura_naoji_jal_JALMemory__1createQuery(
		JNIEnv *env, jclass, jlong jmemoryPtr, jobjectArray jstrings) {
	JALMemory *jmemory = reinterpret_cast<JALMemory*> (jmemoryPtr);
	assert(jmemory != NULL);
	Query *query = new Query(jmemory);
	jsize size = env->GetArrayLength(jstrings);
	ALValue names;
	names.arraySetSize(size);
	for (int i = 0; i < size; i++) {
		jstring jstr = static_cast<jstring> (env->GetObjectArrayElement(
				jstrings, i));
		jassert(env, jstr != NULL);
		names[i] = toString(env, jstr);
		env->DeleteLocalRef(jstr);
	}
	query->names = names;

	return reinterpret_cast<jlong> (query);
}
Ejemplo n.º 12
0
JNIEXPORT
void JNICALL Java_jp_ac_fit_asura_naoji_jal_JALMemory__1updateStringQuery(
		JNIEnv *env, jclass, jlong queryPtr) {
	Query *query = reinterpret_cast<Query*> (queryPtr);
	assert(query != NULL);

	try {
		ALValue data = query->jmemory->getProxy()->getListData(query->names);
		int size = data.getSize();
		assert(query->names.getSize() == size);

		for (int i = 0; i < size; i++) {
			env->SetObjectArrayElement(query->buffer.s, i, env->NewStringUTF(
					((string)data[i]).c_str()));
		}
	} catch (AL::ALError err) {
		std::cerr << err.toString() << std::endl;
		assert(false);
	}
}
Ejemplo n.º 13
0
JNIEXPORT
void JNICALL Java_jp_ac_fit_asura_naoji_jal_JALMemory__1updateIntQuery(
		JNIEnv *, jclass, jlong queryPtr) {
	Query *query = reinterpret_cast<Query*> (queryPtr);
	assert(query != NULL);

	try {
		ALValue data = query->jmemory->getProxy()->getListData(query->names);
		int size = data.getSize();
		assert(query->names.getSize() == size);

		jint* buf = reinterpret_cast<jint*> (query->buffer.b);
		for (int i = 0; i < size; i++) {
			buf[i] = data[i];
		}
	} catch (AL::ALError err) {
		std::cerr << err.toString() << std::endl;
		assert(false);
	}
}
Ejemplo n.º 14
0
void SimuNaoProvider::initDCMAliases()
{
	ALValue positionCommandsAlias;
	positionCommandsAlias.arraySetSize(2);
	positionCommandsAlias[0] = std::string("AllActuatorPosition");
	
	ALValue hardCommandsAlias;
	hardCommandsAlias.arraySetSize(2);
	hardCommandsAlias[0] = std::string("AllActuatorHardness");

	positionCommandsAlias[1].arraySetSize(numOfJoints);
	hardCommandsAlias[1].arraySetSize(numOfJoints);
	for (int i=0;i<numOfJoints;i++)
	{
		positionCommandsAlias[1][i] = std::string(jointNames[i]) + "/Position/Actuator/Value";
		hardCommandsAlias[1][i] = std::string(jointNames[i]) + "/Hardness/Actuator/Value";
	}
	
	dcmProxy->createAlias(positionCommandsAlias);
	dcmProxy->createAlias(hardCommandsAlias);

}
Ejemplo n.º 15
0
struct timespec KImageExtractor::fetchImage(IplImage *img)
{
    struct timespec rt;//Timestamp
    cout<<"KImageExtractor::fetchimage():"<<endl;
    if (doneSubscribe==false)
    {
        cout<<"KImageExtractor: Warning! fetchImage()  called although GVM Subscription has failed!"<<endl;
        rt.tv_sec=0;
        rt.tv_nsec=0;
        return rt;
    }

#ifdef REMOTE_ON
    //		cout << "Remote method on" << endl;
    //		sleep(1);
    ALValue results;
#ifdef RAW

    results = (c->call<ALValue> ("getDirectRawImageRemote", GVM_name));
#else
    results = (c->call<ALValue> ("getImageRemote", GVM_name));
#endif
    if (results.getType() != ALValue::TypeArray && results.getSize() != 7)
    {
        throw ALError("KImageExtractor", "saveImageRemote", "Invalid image returned.");
    }
    //const int size = results[6].getSize();
    // You can get some image information that you may find useful.
    //	const int width = (int) results[0];
    //	const int height = (int) results[1];
    //	const int nbLayers = (int) results[2];
    //	const int colorSpace = (int) results[3];
    //const long long timeStamp = ((long long) (int) results[4]) * 1000000LL + ((long long) (int) results[5]);
    //	const int seconds = (int) (timeStamp / 1000000LL);
    // Set the buffer we received to our IplImage header.
    //fIplImageHeader->imageData = (char*) (results[6].GetBinary());
    //cout << "Size" << size << endl;



    int width = (int) results[0];
    int height = (int) results[1];
    int nChannels = (int) results[2];
    int colorSpace = (int) results[3];

    int size =width*height*nChannels;

    //Fetch TimeStamp;
    rt.tv_sec=(time_t)((int) results[4]);
    rt.tv_nsec=(int)  results[5]*1000L;

    //Change of image data size
    assert(img!=NULL);
    //cout<<img->imageSize<<" "<<size<<endl;
    if (img->imageSize!=size )
    {
        //cout<<img->width<<" "<<img->height<<endl;
        cout<<"KImageExtractor::fetchImage():allocating new imagedata"<<endl;
        //cout<<"Delete old"<<endl;
        //delete img->imageData;
        //img->imageData=NULL;
        cout<<"cvInitImage"<<endl;
        cvInitImageHeader(img,  cvSize(width,height),IPL_DEPTH_8U, nChannels);
        //img->imageData=NULL;
        cout<<" Done"<<endl;
        //img->imageData=(char*)malloc(img->imageSize);
    }


    if (img->imageData != NULL)
    {
        //free( fIplImageHeader->imageData)
        memcpy(img->imageData, (char*) (results[6].GetBinary()), results[6].getSize() * sizeof(unsigned char));
    }
    else
    {
        img->imageData = new char[img->imageSize];
        memcpy(img->imageData, (char*) (results[6].GetBinary()), results[6].getSize() * sizeof(char));
    }
#else
    //cout << "Remote method off" << endl;
    //sleep(1);
    ALImage* imageIn = NULL;
    // Now you can get the pointer to the video structure.
#ifdef RAW
    imageIn = (ALImage*) (c->call<int> ("getDirectRawImage", GVM_name));
#else
    imageIn = (ALImage*) (c->call<int> ("getImageLocal", GVM_name));
#endif
    if (!imageIn)
    {
        throw ALError("KImageExtractor", "saveImageLocal", "Invalid image returned.");
    }
    //fLogProxy->info(getName(), imageIn->toString());
    // You can get some image information that you may find useful.
    int width = imageIn->fWidth;
    int height = imageIn->fHeight;
    const int nChannels = imageIn->fNbLayers;
    //		const int colorSpace = imageIn->fColorSpace;
    const long long timeStamp = imageIn->fTimeStamp;
    //		const int seconds = (int) (timeStamp / 1000000LL);
    const int size = width*height*nChannels;
    // Set the buffer we received to our IplImage header.
    //Fetch TimeStamp;
    rt.tv_sec=(time_t) (timeStamp / 1000000LL);
    rt.tv_nsec=(long)  ((timeStamp-rt.tv_sec*1LL)*1000LL);


    //Change of image data size
    if (img->imageSize!=size*sizeof(char) )
    {
        free(img->imageData);
        cvInitImageHeader(img,  cvSize(width,height),IPL_DEPTH_8U, nChannels);
        img->imageData=NULL;
        //img->imageData=(char*)malloc(img->imageSize);
    }

    if (img->imageData!=NULL)
    {
        //free( fIplImageHeader->imageData);
        memcpy ( img->imageData, (char*) imageIn->getFrame(), size*sizeof(char) );
    }
    else
    {
        img->imageData = new char[size];
        memcpy ( img->imageData, (char*) imageIn->getFrame(), size*sizeof(char) );
    }
    //fIplImageHeader->imageData = (char*) imageIn->getFrame();
    //saveIplImage(fIplImageHeader, name, pImageFormat, seconds);
    // Now that you're done with the (local) image, you have to release it from the V.I.M.
    c->call<int> ("releaseImage", GVM_name);
#endif
    return rt;
};
Ejemplo n.º 16
0
void * urgThread(void * arg) {

#if defined (__linux__)
  // thread name
  prctl(PR_SET_NAME, "ALLaser::urgThread", 0, 0, 0);
#endif

  ALValue urgdata;
  long *data = NULL;
  int data_max;
  int ret;
  int n;
  int i,imemory,refTime,end,sampleTime, beginThread;
  std::string valueName="Device/Laser/Value";

  /* Connection */

  connectToLaser();

  /* Reserve the Receive data buffer */
  data_max = urg_dataMax(&urg);
  if (data_max <= 0) {
    perror("data_max is less than 0");
    pthread_exit((void *)NULL);
  }
  data = (long*)malloc(sizeof(long) * data_max);
  memset(data, 0, sizeof(long) * data_max);

  if (data == NULL) {
    perror("data buffer");
    pthread_exit((void *)NULL);
  }
  /* prepare ALValue for ALMemory*/
  urgdata.arraySetSize(data_max);
  for (i=0; i< data_max;i++)
  {
    urgdata[i].arraySetSize(4);
    urgdata[i][0]= (int)0;
    urgdata[i][1]= (double)0.0;
    urgdata[i][2]= (int)0;
    urgdata[i][3]= (int)0;
  }
  /* insert ALvalue in ALMemory*/

  gSTM->insertData(valueName,urgdata);

  gSTM->insertData("Device/Laser/LaserEnable", (bool) 1);
  gSTM->insertData("Device/Laser/MinAngle",(float)((2.0 * M_PI)
          * (DEFAULT_MIN_ANGLE - MIDDLE_ANGLE) / RESOLUTION_LASER));
  gSTM->insertData("Device/Laser/MaxAngle",(float)((2.0 * M_PI)
          * (DEFAULT_MAX_ANGLE - MIDDLE_ANGLE) / RESOLUTION_LASER));
  gSTM->insertData("Device/Laser/MinLength",(float)(length_min));
  gSTM->insertData("Device/Laser/MaxLength",(float)(length_max));

  stringstream ss;
  ss << "ALLaser running";
  qiLogInfo("hardware.laser") << ss.str() << std::endl;

  while(1)
  {
    if(mode==SEND_MODE_ON){
      ret = urg_laserOn(&urg);
      if (ret < 0) {
        urg_exit(&urg, "urg_requestData()");
      }
      mode=MODE_ON;
      /* Connection */
      connectToLaser();
    }
    if(mode==SEND_MODE_OFF){
      scip_qt(&urg.serial_, NULL, ScipNoWaitReply);
      mode=MODE_OFF;
    }
    if(mode==MODE_ON){
      /* Request Data using GD-Command */
      ret = urg_requestData(&urg, URG_GD, URG_FIRST, URG_LAST);
      if (ret < 0) {
        urg_exit(&urg, "urg_requestData()");
      }

      refTime = getLocalTime();
      /* Obtain Data */
      n = urg_receiveData(&urg, data, data_max);
      qiLogDebug("hardware.laser") << " n " << n << " expected " <<
            angle_max - angle_min << std::endl;
      if (n < 0) {
        urg_exit(&urg, "urg_receiveData()");
      }
      end= getLocalTime();
      sampleTime=end-refTime;

      imemory=0;
      for (i = 0; i < n; ++i) {
        int x, y;
        double angle = urg_index2rad(&urg, i);

        qiLogDebug("hardware.laser") << i << " angle " << angle <<
              " urgAngle " << urg_index2rad(&urg, i) <<
              " dist " << data[i] << std::endl;

        int length = data[i];

        if( length >= length_min && length <= length_max
            && i >= angle_min && i <= angle_max ){
          x = (int)((double)length * cos(angle));
          y = (int)((double)length * sin(angle));
          urgdata[imemory][0]= length;
          urgdata[imemory][1]= angle;
          urgdata[imemory][2]= x;
          urgdata[imemory][3]= y;
          imemory++;
        }
      }
      for(;imemory<data_max;imemory++){
        urgdata[imemory][0]= 0;
        urgdata[imemory][1]= 0;
        urgdata[imemory][2]= 0;
        urgdata[imemory][3]= 0;
      }
      gSTM->insertData(valueName,urgdata);
      usleep(1000);
    }else{
      usleep(10000);
    }
  }
  urg_disconnect(&urg);

  free(data);
}
Agent::Agent(AL::ALPtr<AL::ALBroker> pBroker, const std::string& pName)
   : ALModule(pBroker, pName),
     alwalk_state(INACTIVE),
     shuttingDown(false),
     skipped_frames(0),
     sit_step(-1.0f),
     limp(true),
     head_limp(false),
     chest_down(0),
     chest_up(0),
     chest_presses(0),
     old_battery(0),
     old_battery_status(0) {
   uint8_t i;

   // need to initialise first, before using log
   log = new ALLoggerProxy(pBroker);
   if (log == NULL)
      throw ALERROR(name, "constructor", "log == NULL");

   log->info(name, "Constructing");

   try {
      po::options_description cmdline_options =
            store_and_notify(std::vector<string>(0), vm, NULL);
      wirelessIwconfigArgs = vm["network.wireless.iwconfig_flags"].as<string>();
      wirelessIfconfigArgs =
                      vm["network.wireless.static.ifconfig_flags"].as<string>();
      wiredIfconfigArgs =
                         vm["network.wired.static.ifconfig_flags"].as<string>();

      wirelessStatic = vm["network.wireless.static"].as<bool>();
      wiredStatic = vm["network.wired.static"].as<bool>();

      playerNum = vm["player.number"].as<int>();
      teamNum = vm["player.team"].as<int>();

      doNetworking();
   } catch(po::error& e) {
      log->error(name, "failed parsing command line arguments");
      log->error(name, e.what());
   } catch(std::exception& e) {
      log->error(name, "failed parsing command line arguments");
      log->error(name, e.what());
   }

   dcm = new DCMProxy(pBroker);
   if (dcm == NULL)
      throw ALERROR(name, "constructor", "dcm == NULL");

   memory = new ALMemoryProxy(pBroker);
   if (memory == NULL)
      throw ALERROR(name, "constructor", "memory == NULL");

   motion = new ALMotionProxy(pBroker);
   motion->setWalkArmsEnable(false, false);
   ALValue config;
   config.arraySetSize(13);
   for (i = 0; i < 13; ++i)
      config[i].arraySetSize(2);
   config[0][0]  = "WALK_MAX_TRAPEZOID",       config[0][1]  = 4.5;    // 4.5
   config[1][0]  = "WALK_MIN_TRAPEZOID",       config[1][1]  = 3.5;    // 3.5
   config[2][0]  = "WALK_STEP_MAX_PERIOD",     config[2][1]  = 30;     // 30
   config[3][0]  = "WALK_STEP_MIN_PERIOD",     config[3][1]  = 21;     // 21
   config[4][0]  = "WALK_MAX_STEP_X",          config[4][1]  = 0.04;   // 0.04
   config[5][0]  = "WALK_MAX_STEP_Y",          config[5][1]  = 0.04;   // 0.04
   config[6][0]  = "WALK_MAX_STEP_THETA",      config[6][1]  = 20;     // 20
   config[7][0]  = "WALK_STEP_HEIGHT",         config[7][1]  = 0.015;  // 0.015
   config[8][0]  = "WALK_FOOT_SEPARATION",     config[8][1]  = 0.095;  // 0.095
   config[9][0]  = "WALK_FOOT_ORIENTATION",    config[9][1]  = 0;      // 0
   config[10][0] = "WALK_TORSO_HEIGHT",        config[10][1] = 0.31;   // 0.31
   config[11][0] = "WALK_TORSO_ORIENTATION_X", config[11][1] = 0;      // 0
   config[12][0] = "WALK_TORSO_ORIENTATION_Y", config[12][1] = 0;      // 0
   motion->setMotionConfig(config);

   leg_names.push_back("LHipYawPitch"), stand_angles.push_back(0.f);
   leg_names.push_back("LHipRoll"), stand_angles.push_back(0.f);
   leg_names.push_back("LHipPitch"), stand_angles.push_back(DEG2RAD(-30.f));
   leg_names.push_back("LKneePitch"), stand_angles.push_back(DEG2RAD(60.f));
   leg_names.push_back("LAnklePitch"), stand_angles.push_back(DEG2RAD(-30.f));
   leg_names.push_back("LAnkleRoll"), stand_angles.push_back(0.f);
   leg_names.push_back("RHipRoll"), stand_angles.push_back(0.f);
   leg_names.push_back("RHipPitch"), stand_angles.push_back(DEG2RAD(-30.f));
   leg_names.push_back("RKneePitch"), stand_angles.push_back(DEG2RAD(60.f));
   leg_names.push_back("RAnklePitch"), stand_angles.push_back(DEG2RAD(-30.f));
   leg_names.push_back("RAnkleRoll"), stand_angles.push_back(0.f);

   // open shared memory as RW, create if !exist, permissions 600
   shared_fd = shm_open(AGENT_MEMORY, O_RDWR | O_CREAT, 0600);
   if (shared_fd < 0)
      throw ALERROR(name, "constructor", "shm_open() failed");
   // make shared memory file correct size
   if (ftruncate(shared_fd, sizeof(AgentData)) == -1)
      throw ALERROR(name, "constructor", "ftruncate() failed");
   // map shared memory to process memory
   shared_data = (AgentData*) mmap(NULL, sizeof(AgentData),
                                   PROT_READ | PROT_WRITE,
                                   MAP_SHARED, shared_fd, 0);
   if (shared_data == MAP_FAILED)
      throw ALERROR(name, "constructor", "mmap() failed");

   // Initialise shared memory
   shared_data->sensors_read = 0;
   shared_data->sensors_latest = 0;
   shared_data->actuators_read = 0;
   shared_data->actuators_latest = 0;

   SensorValues null_sensors;
   JointValues null_joints;
   ActionCommand::LED null_leds;
   for (i = 0; i < Joints::NUMBER_OF_JOINTS; ++i) {
      null_joints.angles[i] = 0.0f;
      null_joints.stiffnesses[i] = 0.0f;
      null_joints.temperatures[i] = 0.0f;
   }
   null_sensors.joints = null_joints;
   for (i = 0; i < Sensors::NUMBER_OF_SENSORS; ++i)
      null_sensors.sensors[i] = 0.0f;
   for (i = 0; i < 3; ++i) {
      shared_data->sensors[i] = null_sensors;
      shared_data->joints[i] = null_joints;
      shared_data->leds[i] = null_leds;
      shared_data->sonar[i] = 0.0f;
   }
   shared_data->standing = false;
   pthread_mutexattr_t attr;
   pthread_mutexattr_init(&attr);
   pthread_mutexattr_settype(&attr, PTHREAD_MUTEX_ERRORCHECK);
   pthread_mutex_init(&shared_data->lock, &attr);

   // create semaphore with permissions 600, value 0
   semaphore = sem_open(AGENT_SEMAPHORE, O_RDWR | O_CREAT, 0600, 0);
   if (semaphore < 0)
      throw ALERROR(name, "constructor", "sem_open() failed");

   // Initialise ALMemory pointers
   std::vector<std::string> key_names;
   for (i = 0; i < Sensors::NUMBER_OF_SENSORS; ++i)
      sensor_pointers.push_back((float*) memory->getDataPtr(
                                std::string("Device/SubDeviceList/") +
                                Sensors::sensorNames[i] + "/Sensor/Value"));
   for (i = 0; i < Joints::NUMBER_OF_JOINTS; ++i) {
      joint_pointers.push_back((float*) memory->getDataPtr(
                               std::string("Device/SubDeviceList/") +
                               Joints::jointNames[i] +
                               "/Position/Sensor/Value"));
      temperature_pointers.push_back((float*) memory->getDataPtr(
                                    std::string("Device/SubDeviceList/") +
                                    Joints::jointNames[i] +
                                    "/Temperature/Sensor/Value"));
   }
   for (i = 0; i < Sonar::NUMBER_OF_READINGS; ++i)
      sonar_pointers.push_back((float*) memory->getDataPtr(
                               std::string("Device/SubDeviceList/") +
                               Sonar::readingNames[i]));
   battery_status_pointer = (int*) memory->
      getDataPtr("Device/SubDeviceList/Battery/Charge/Sensor/Status");

   // DCM Command Aliases
   ALValue alias;
   alias.arraySetSize(2);
   alias[0] = string("JointAngles");
   alias[1].arraySetSize(Joints::NUMBER_OF_JOINTS - 2);
   for (i = Joints::LShoulderPitch; i < Joints::NUMBER_OF_JOINTS; ++i) {
      alias[1][i-2] = string(Joints::jointNames[i]).
                      append("/Position/Actuator/Value");
   }
   dcm->createAlias(alias);
   alias[0] = string("HeadAngles");
   alias[1].arraySetSize(2);
   for (i = Joints::HeadYaw; i <= Joints::HeadPitch; ++i) {
      alias[1][i] = string(Joints::jointNames[i]).
                    append("/Position/Actuator/Value");
   }
   dcm->createAlias(alias);
   alias[0] = string("LEDs");
   alias[1].arraySetSize(LEDs::NUMBER_OF_LEDS);
   for (i = 0; i < LEDs::NUMBER_OF_LEDS; ++i) {
      alias[1][i] = string(LEDs::ledNames[i]);
   }
   dcm->createAlias(alias);
   alias[0] = string("JointStiffnesses");
   alias[1].arraySetSize(Joints::NUMBER_OF_JOINTS - 2);
   for (i = Joints::LShoulderPitch; i < Joints::NUMBER_OF_JOINTS; ++i) {
      alias[1][i-2] = string(Joints::jointNames[i]).
                      append("/Hardness/Actuator/Value");
   }
   dcm->createAlias(alias);
   alias[0] = string("HeadStiffnesses");
   alias[1].arraySetSize(2);
   for (i = Joints::HeadYaw; i <= Joints::HeadPitch; ++i) {
      alias[1][i] = string(Joints::jointNames[i]).
                    append("/Hardness/Actuator/Value");
   }
   dcm->createAlias(alias);
   alias[0] = string("Sonar");
   alias[1].arraySetSize(1);
   alias[1][0] = Sonar::actuatorName;
   dcm->createAlias(alias);

   // Set up Commands ALValue
   angle_command.arraySetSize(6);
   angle_command[0] = string("JointAngles");
   angle_command[1] = string("ClearAfter");
   angle_command[2] = string("time-separate");
   angle_command[3] = 0;
   angle_command[4].arraySetSize(1);
   angle_command[5].arraySetSize(Joints::NUMBER_OF_JOINTS - 2);
   for (i = Joints::LShoulderPitch; i < Joints::NUMBER_OF_JOINTS; ++i) {
      angle_command[5][i-2].arraySetSize(1);
   }
   head_angle_command.arraySetSize(6);
   head_angle_command[0] = string("HeadAngles");
   head_angle_command[1] = string("ClearAfter");
   head_angle_command[2] = string("time-separate");
   head_angle_command[3] = 0;
   head_angle_command[4].arraySetSize(1);
   head_angle_command[5].arraySetSize(2);
   for (i = Joints::HeadYaw; i <= Joints::HeadPitch; ++i) {
      head_angle_command[5][i].arraySetSize(1);
   }
   led_command.arraySetSize(6);
   led_command[0] = string("LEDs");
   led_command[1] = string("ClearAfter");
   led_command[2] = string("time-separate");
   led_command[3] = 0;
   led_command[4].arraySetSize(1);
   led_command[5].arraySetSize(LEDs::NUMBER_OF_LEDS);
   for (i = 0; i < LEDs::NUMBER_OF_LEDS; ++i) {
      led_command[5][i].arraySetSize(1);
   }
   stiffness_command.arraySetSize(6);
   stiffness_command[0] = string("JointStiffnesses");
   stiffness_command[1] = string("ClearAfter");
   stiffness_command[2] = string("time-separate");
   stiffness_command[3] = 0;
   stiffness_command[4].arraySetSize(1);
   stiffness_command[5].arraySetSize(Joints::NUMBER_OF_JOINTS - 2);
   for (i = Joints::LShoulderPitch; i < Joints::NUMBER_OF_JOINTS; ++i) {
      stiffness_command[5][i-2].arraySetSize(1);
   }
   head_stiffness_command.arraySetSize(6);
   head_stiffness_command[0] = string("HeadStiffnesses");
   head_stiffness_command[1] = string("ClearAfter");
   head_stiffness_command[2] = string("time-separate");
   head_stiffness_command[3] = 0;
   head_stiffness_command[4].arraySetSize(1);
   head_stiffness_command[5].arraySetSize(2);
   for (i = Joints::HeadYaw; i <= Joints::HeadPitch; ++i) {
      head_stiffness_command[5][i].arraySetSize(1);
   }
   sonar_command.arraySetSize(6);
   sonar_command[0] = string("Sonar");
   sonar_command[1] = string("ClearAfter");
   sonar_command[2] = string("time-separate");
   sonar_command[3] = 0;
   sonar_command[4].arraySetSize(1);
   sonar_command[4][0] = dcm->getTime(20);
   sonar_command[5].arraySetSize(1);
   sonar_command[5][0].arraySetSize(1);
   sonar_command[5][0][0] = 68.0f;
   dcm->setAlias(sonar_command);

   // Get offset between our clock and the DCM's
   time_offset = (int)dcm->getTime(0) - timeNow();

   // Set up callbacks
   dcm->getModule()->atPostProcess(boost::bind(postCallback_, this));
   dcm->getModule()->atPreProcess(boost::bind(preCallback_, this));

   // Fix the clock if an ntp server is available
   ret = system("/bin/su -c '/etc/init.d/openntpd restart'");
   ret = system("/usr/bin/amixer -qs < /home/nao/data/volumeinfo.dat");


   // Play the 'oneg-nook' jingle
   ret =
      system("/usr/bin/aplay -q /opt/naoqi/share/naoqi/wav/start_jingle.wav");

   // Start rUNSWift
   // Uncomment this to have rUNSWift start when you turn the robot on
   /*
   SAY("loaded runswift");
   if (!fork()) {
      struct sched_param s;
      s.sched_priority = 0;
      int ret = sched_setscheduler(0, SCHED_OTHER, &s);
      ret = execlp("/home/nao/bin/runswift", "runswift",
            (char*)NULL);
      ::exit(ret);
   }
   */

   log->info(name, "Constructed");
}
void Agent::preCallback() {
   // Avoid race condition on shutdown
   if (shuttingDown) {
      return;
   }

   const unsigned int now = timeNow();
   shared_data->actuators_read = shared_data->actuators_latest;
   JointValues& joints = shared_data->joints[shared_data->actuators_read];
   SensorValues& sensors = shared_data->sensors[shared_data->sensors_latest];

   doLEDs(shared_data->leds[shared_data->actuators_read]);
   if (limp || shared_data->standing) {
      uint8_t i;
      for (i = Joints::HeadYaw; i <= Joints::HeadPitch; ++i) {
         head_angle_command[5][i][0] = sit_angles[i];
         head_stiffness_command[5][i][0] = 0.0f;
      }
      for (i = Joints::LShoulderPitch; i < Joints::NUMBER_OF_JOINTS; ++i) {
         angle_command[5][i-2][0] = sit_angles[i];
         stiffness_command[5][i-2][0] = 0.0f;
      }

      // Make chest button blink green if limp but still in contact
      // or blink red if not in contact or purple if ready to stand
      float blink = now / 200 & 1;
      if (shared_data->standing) {
         led_command[5][LEDs::ChestRed][0] = blink;
         led_command[5][LEDs::ChestGreen][0] = blink;
         led_command[5][LEDs::ChestBlue][0] = 0.0;
      } else if (skipped_frames <= MAX_SKIPS) {
         led_command[5][LEDs::ChestRed][0] = 0.0f;
         led_command[5][LEDs::ChestGreen][0] = blink;
         led_command[5][LEDs::ChestBlue][0] = 0.0f;
      } else {
         led_command[5][LEDs::ChestRed][0] = blink;
         led_command[5][LEDs::ChestGreen][0] = 0.0f;
         led_command[5][LEDs::ChestBlue][0] = 0.0f;
      }
   } else if (skipped_frames <= MAX_SKIPS && sit_step == -1.0f) {
      uint8_t i;
      for (i = Joints::HeadYaw; i <= Joints::HeadPitch; ++i) {
         head_angle_command[5][i][0] = joints.angles[i];
         head_stiffness_command[5][i][0] = joints.stiffnesses[i];
      }
      if (joints.AL_command == AL_ON) {
         static int stand_t = 0;
         motion->setStiffnesses("Body", 0.66);
         ALValue config;
         switch (alwalk_state) {
         case INACTIVE:
            config.arraySetSize(13);
            for (i = 0; i < 13; ++i)
               config[i].arraySetSize(2);
            config[0][0]  = "WALK_MAX_TRAPEZOID";
            config[0][1]  = 4.5;    // 4.5
            config[1][0]  = "WALK_MIN_TRAPEZOID";
            config[1][1]  = 3.5;    // 3.5
            config[2][0]  = "WALK_STEP_MAX_PERIOD";
            config[2][1]  = 30;     // 30
            config[3][0]  = "WALK_STEP_MIN_PERIOD";
            config[3][1]  = 21;     // 21
            config[4][0]  = "WALK_MAX_STEP_X";
            config[4][1]  = 0.04;   // 0.04
            config[5][0]  = "WALK_MAX_STEP_Y";
            config[5][1]  = 0.04;   // 0.04
            config[6][0]  = "WALK_MAX_STEP_THETA";
            config[6][1]  = 20;     // 20
            config[7][0]  = "WALK_STEP_HEIGHT";
            config[7][1]  = 0.015;  // 0.015
            config[8][0]  = "WALK_FOOT_SEPARATION";
            config[8][1]  = 0.095;  // 0.095
            config[9][0]  = "WALK_FOOT_ORIENTATION";
            config[9][1]  = 0;      // 0
            config[10][0] = "WALK_TORSO_HEIGHT";
            config[10][1] = joints.AL_height;   // 0.31
            config[11][0] = "WALK_TORSO_ORIENTATION_X";
            config[11][1] = 0;      // 0
            config[12][0] = "WALK_TORSO_ORIENTATION_Y";
            config[12][1] = 0;      // 0
            motion->setMotionConfig(config);
            stand_angles.clear();
            stand_angles.push_back(0.f);
            stand_angles.push_back(0.f);
            stand_angles.push_back(-joints.AL_bend);
            stand_angles.push_back(2*joints.AL_bend);
            stand_angles.push_back(-joints.AL_bend);
            stand_angles.push_back(0.f);
            stand_angles.push_back(0.f);
            stand_angles.push_back(-joints.AL_bend);
            stand_angles.push_back(2*joints.AL_bend);
            stand_angles.push_back(-joints.AL_bend);
            stand_angles.push_back(0.f);
            alwalk_state = WALKING;
            break;
         case WALKING:
            if (joints.AL_stop) {
               // motion->walkTo(0.001, 0.001, 0.001);
               motion->post.angleInterpolation(leg_names, stand_angles,
                                               0.25, true);
               alwalk_state = STANDING;
            } else {
               motion->setWalkTargetVelocity(joints.AL_x, joints.AL_y,
                                             joints.AL_theta,
                                             joints.AL_frequency);
            }
            break;
         case STOPPING:
            if (!motion->walkIsActive()) {
               motion->post.angleInterpolation(leg_names, stand_angles,
                                               0.25, true);
               alwalk_state = STANDING;
            }
            break;
         case STANDING:
            if (stand_t++ > 25) {
               stand_t = 0;
               motion->killWalk();
               alwalk_state = STOPPED;
            }
            break;
         case STOPPED:
            alwalk_state = INACTIVE;
            break;
         }
      } else {
         for (i = Joints::LShoulderPitch; i < Joints::NUMBER_OF_JOINTS; ++i) {
            angle_command[5][i-2][0] = joints.angles[i];
            stiffness_command[5][i-2][0] = joints.stiffnesses[i];
         }
      }
   } else {
      if (sit_step == -1.0f) {
         sit_step = 0.0f;
         for (uint8_t i = 0; i < Joints::NUMBER_OF_JOINTS; ++i)
            sit_joints.angles[i] = sensors.joints.angles[i];
         SAY("lost contact");
         motion->killAll();
      }
      if (sit_step < 1.0f) {
         // interpolate legs over 2s, arms over 0.2s
         sit_step += 0.005f;
         uint8_t i;
         for (i = Joints::HeadYaw; i <= Joints::HeadPitch; ++i) {
            head_angle_command[5][i][0] = (1 - sit_step) * sit_joints.angles[i]
                                        + sit_step * sit_angles[i];
            head_stiffness_command[5][i][0] = 1.0f;
         }
         for (i = Joints::LShoulderPitch; i < Joints::NUMBER_OF_JOINTS; ++i) {
            float k = (i >= Joints::LShoulderPitch &&
                       i <= Joints::LElbowRoll) ||
                      (i >= Joints::RShoulderPitch &&
                       i <= Joints::RElbowRoll) ? 10 : 1;
            k = MIN(1.0f, k * sit_step);
            angle_command[5][i-2][0] = (1 - k) * sit_joints.angles[i] +
                                     k * sit_angles[i];
            stiffness_command[5][i-2][0] = 1.0f;
         }
      } else {
         limp = true;
         sit_step = -1.0f;
         uint8_t i;
         for (i = Joints::HeadYaw; i <= Joints::HeadPitch; ++i) {
            head_angle_command[5][i][0] = sit_angles[i];
            head_stiffness_command[5][i][0] = 0.0f;
         }
         for (i = Joints::LShoulderPitch; i < Joints::NUMBER_OF_JOINTS; ++i) {
            angle_command[5][i-2][0] = sit_angles[i];
            stiffness_command[5][i-2][0] = 0.0f;
         }
      }

      // Make chest button solid red/green if sitting down
      // (depending on whether or not we are back in contact)
      if (skipped_frames > MAX_SKIPS) {
         led_command[5][LEDs::ChestRed][0] = 1.0f;
         led_command[5][LEDs::ChestGreen][0] = 0.0f;
      } else {
         led_command[5][LEDs::ChestRed][0] = 0.0f;
         led_command[5][LEDs::ChestGreen][0] = 1.0f;
      }
      led_command[5][LEDs::ChestBlue][0] = 0.0f;
   }
   // sonar_command[5][0][0] = shared_data->sonar[shared_data->actuators_read];
   sonar_command[5][0][0] = Sonar::Mode::CONTINUOUS + Sonar::Mode::ON;

   if (head_limp) {
      float blink = now / 200 & 1;
      led_command[5][LEDs::LeftEyeGreen8][0] = blink;
      led_command[5][LEDs::RightEyeGreen8][0] = blink;
      head_stiffness_command[5][Joints::HeadYaw][0] = 0.0f;
      head_stiffness_command[5][Joints::HeadPitch][0] = 0.0f;
   }

   head_stiffness_command[4][0] = (int)now + time_offset;
   head_angle_command[4][0] = (int)now + time_offset;
   stiffness_command[4][0] = (int)now + time_offset;
   angle_command[4][0] = (int)now + time_offset;
   led_command[4][0] = (int)now + time_offset;
   sonar_command[4][0] = (int)now + time_offset;
   dcm->setAlias(head_stiffness_command);
   dcm->setAlias(head_angle_command);
   if (limp || shared_data->standing ||
       !(skipped_frames <= MAX_SKIPS && sit_step == -1.0f &&
         joints.AL_command == AL_ON)) {
      dcm->setAlias(stiffness_command);
      dcm->setAlias(angle_command);
   }
   dcm->setAlias(led_command);
   // dcm->setAlias(sonar_command);
}
Ejemplo n.º 19
0
void ALSoundGeneration::playSineWaves()
{
			
	// set output sample rate of audiodevice
	try
	{
		audioDeviceProxy->callVoid("setParameter", std::string("outputSampleRate"), sample_rate);
	}
	catch(ALError &e)
	{
		throw AL::ALError("ALSoundGeneration", "playSineWaves()", e.getDescription());
	}
	
	// initialise
	short* buffer_mono = new short[num_samples];
	short* buffer_stereo = new short[num_samples * 2];
	double* data_norm = new double[num_bins];
	double activation;
	int bin_size = sample_rate / 2 / num_bins;
	ALValue pDataBin;
    
	while (true)
	{
		
		// DEBUG
		clock_t clockBegin = clock();
		
		// read data from ALMemory
		for(int i=0; i<num_bins; i++)
		{
			try
			{
				ALValue value = fProxyToALMemory.getData(fALMemoryKeys[i]);
				data_norm[i] = (float) value;
			}
			catch(AL::ALError &e)
			{
				//throw ALError("ALSoundGeneration", "playSineWaves()", e.getDescription());
				data_norm[i] = 0.0;
			}
		}
		
		// read activation from ALMemory
		try
		{
			ALValue value = fProxyToALMemory.getData("ALSoundGeneration/activation");
			activation = (float) value;
		}
		catch(AL::ALError &e)
		{
			//throw ALError("ALSoundGeneration", "playSineWaves()", e.getDescription());
			activation = 0.0;
		}		
		
		// (de)amplify
		for(int i=0; i<num_bins; i++)
		{
			data_norm[i] *= activation; // use activation to set volume			
		}
		
		// DEBUG
		for(int i=0; i<num_bins; i++)
		{
			printf("%.1f   ", data_norm[i]);
		}
		printf("    activation: %.1f", activation);
		printf("\n");
				
		// clear audio buffer
		for(int j=0; j<num_samples; j++)
		{
			buffer_mono[j] = 0;
		}
		
		// fill audio buffer
		int freq_sine;
		double amplitude;
		double samples_per_sine;
		double silencing_factor = 0.1; // DEBUG: silence a bit	
		for(int i=0; i<num_bins; i++)
		{
			freq_sine = i * bin_size; // equal bin size assumed
			freq_sine += bin_size / 2; // shift by half bin size
			amplitude = data_norm[i];			
			// threshold
			if(amplitude > 0.2)
			{
				samples_per_sine = (double) sample_rate / freq_sine;
				for(int j=0; j<num_samples; j++)
				{
					buffer_mono[j] += (short) round(sin((double) j / samples_per_sine * 2 * PI) * silencing_factor * amplitude * SHRT_MAX);
				}
			}
		}
		
		// limit audio buffer
		for(int j=0; j<num_samples; j++)
		{
			if(buffer_mono[j] > SHRT_MAX)
				buffer_mono[j] = SHRT_MAX;
			if(buffer_mono[j] < SHRT_MIN)
				buffer_mono[j] = SHRT_MIN;
		}
		
		// convert to stereo
		int i = 0;
		for(int j=0; j<num_samples; j++)
		{
			buffer_stereo[i] = buffer_mono[j];
			buffer_stereo[i+1] = buffer_mono[j];
			i += 2;
		}
		
		// transform to binary samples
		pDataBin.SetBinary(buffer_stereo, num_samples * sizeof(short) * 2);
		
		// send to audiodevice module
		try
		{
			audioDeviceProxy->call<bool>("sendRemoteBufferToOutput", num_samples, pDataBin);
		}
		catch(AL::ALError &e)
		{
			throw ALError("ALSoundGeneration", "playSineWaves()", e.getDescription());
		}

		// DEBUG
		clock_t clockEnd = clock();
		printf("processing took %.1f ms\n", (double) (clockEnd - clockBegin) / CLOCKS_PER_SEC * 1000);		
		
	}
	
}
Ejemplo n.º 20
0
ALValue vision::getBalls() {

	ALValue resultBallRect;
	resultBallRect.arraySetSize(4);

	resultBallRect[0] = 0;
	resultBallRect[1] = 0;
	resultBallRect[2] = 0;
	resultBallRect[3] = 0;

	//First you have to declare an ALVisionImage to get the video buffer.
	// ( definition included in alvisiondefinitions.h and alvisiondefinitions.cpp )
	ALVisionImage* imageIn;

	//Now you can get the pointer to the video structure.
	try
	{
	imageIn = ( ALVisionImage* ) ( camera->call<int>( "getImageLocal", name ) );
	}catch( ALError& e)
	{
	log->error( "vision", "could not call the getImageLocal method of the NaoCam module" );
	}

	//You can get some informations of the image.
	int width = imageIn->fWidth;
	int height = imageIn->fHeight;
	int nbLayers = imageIn->fNbLayers;
	int colorSpace = imageIn->fColorSpace;
	long long timeStamp = imageIn->fTimeStamp;
	int seconds = (int)(timeStamp/1000000LL);

//	log->info( "vision", "Creating OpenCV image" );

	//You can get the pointer of the image.
	uInt8 *dataPointerIn = imageIn->getFrame();

	// now you create an openCV image and you save it in a file.
	IplImage* src = cvCreateImage( cvSize( width, height ), 8, nbLayers );

	//  src->imageData = ( char* ) imageIn->getFrame();
	src->imageData = ( char* ) dataPointerIn;

//log->info( "vision", "Searching field" );

	IplImage* mask = 0;
	IplImage* imageClipped = 0;

	// Get field
	CvRect* fieldRect = new CvRect[1];

//printf("before getLargestColoredContour\n");
	// Green field
	// parameters for pan/tilt camera
	//CvSeq* field = getLargestColoredContour(src, 155, 5, 100, 300, fieldRect);
	// parameters for Nao camera in lab
//    CvSeq* field = getLargestColoredContour(src, 175, 30, 25, 1000, fieldRect);
    // Params for WEBOTS
    CvSeq* field = getLargestColoredContour(src, 125, 30, 25, 100, &fieldRect, 1)[0];

    if (field != NULL) {
//    	printf("Field: %d, %d, %d, %d\n", fieldRect.x, fieldRect.y, fieldRect.width, fieldRect.height);

//log->info( "vision", "Searching ball1" );

		CvSize imageSize = cvSize(src->width, src->height);
		mask = cvCreateImage( imageSize, 8, 1 );
		cvZero(mask);

		CvScalar colorWHITE = CV_RGB(255, 255, 255);

		int elementCount = field->total;
		CvPoint* temp = new CvPoint[elementCount];
		CvPoint pt0 = **CV_GET_SEQ_ELEM( CvPoint*, field, elementCount - 1 );
		for (int i = 0; i < elementCount; i++) {
			CvPoint pt = **CV_GET_SEQ_ELEM( CvPoint*, field, i );
			temp[i].x = pt.x;
			temp[i].y = pt.y;
		}
		cvFillConvexPoly(mask, temp, elementCount, colorWHITE, 8, 0);

		imageClipped = cvCreateImage( imageSize, 8, 3 );
		cvZero(imageClipped);
		cvCopy(src, imageClipped, mask);

		// Get ball
		CvRect* ballRect= new CvRect[10];

//log->info( "vision", "Searching ball2" );
	    // parameters for pan/tilt camera
	    //getLargestColoredContour(imageClipped, 17, 10, 100, 50, ballRect);
	    // parameters for Nao camera in lab
//		CvSeq* ballHull = getLargestColoredContour(imageClipped, 40, 25, 50, 30, ballRect);
		// Params for webots
		CvSeq** ballHull = getLargestColoredContour(imageClipped, 55, 125, 50, 30, &ballRect, 0);

//log->info( "vision", "Searching ball3" );
        int* X_Arr= new int[10];
        int* Y_Arr= new int[10];
        int* Width_Arr= new int[10];
        int* Height_Arr= new int[10];

        for(int i=0; ballHull[i] != NULL; i++) {
            X_Arr[i]= ballRect[i].x;
            Y_Arr[i]= ballRect[i].y;
            Width_Arr[i]= ballRect[i].width;
            Height_Arr[i]= ballRect[i].height;
        }
		if (ballHull != NULL) {
//			printf("ballrect: %d, %d, %d, %d\n", ballRect.x, ballRect.y, ballRect.width, ballRect.height);
			resultBallRect[0] = X_Arr;
			resultBallRect[1] = Y_Arr;
			resultBallRect[2] = Width_Arr;
			resultBallRect[3] = Height_Arr;

//			printf("Clearing ball Hull\n");
//			cvClearSeq(ballHull);
//			printf("Ball Hull cleared\n");
		} else {
//	    	printf("Ball not found!\n");
			resultBallRect[0] = -2;
			resultBallRect[1] = -2;
			resultBallRect[2] = -2;
			resultBallRect[3] = -2;
		}
    } else {
Ejemplo n.º 21
0
/** Run the kick. */
void
NaoQiMotionKickTask::run()
{
  const char *shoot_hip_roll_name = NULL;
  const char *support_hip_roll_name = NULL;
  const char *shoot_hip_pitch_name = NULL;
  const char *support_hip_pitch_name = NULL;
  const char *shoot_knee_pitch_name = NULL;
  const char *shoot_ankle_pitch_name = NULL;
  const char *shoot_ankle_roll_name = NULL;
  const char *support_ankle_roll_name = NULL;

  float shoot_hip_roll = 0;
  float support_hip_roll = 0;
  float shoot_hip_pitch = 0;
  float support_hip_pitch = 0;
  float shoot_knee_pitch = 0;
  float shoot_ankle_pitch = 0;
  float shoot_ankle_roll = 0;
  float support_ankle_roll = 0;

  float BALANCE_HIP_ROLL = 0;
  float BALANCE_ANKLE_ROLL = 0;
  float STRIKE_OUT_HIP_ROLL = 0;

  if ( __leg == fawkes::HumanoidMotionInterface::LEG_LEFT ) {
    shoot_hip_roll_name = "LHipRoll";
    support_hip_roll_name = "RHipRoll";
    shoot_hip_pitch_name = "LHipPitch";
    support_hip_pitch_name = "RHipPitch";
    shoot_knee_pitch_name = "LKneePitch";
    shoot_ankle_pitch_name = "LAnklePitch";
    shoot_ankle_roll_name = "LAnkleRoll";
    support_ankle_roll_name = "RAnkleRoll";

    BALANCE_HIP_ROLL = 20;
    BALANCE_ANKLE_ROLL = -25;
    STRIKE_OUT_HIP_ROLL = 30;
  } else if (__leg == fawkes::HumanoidMotionInterface::LEG_RIGHT ) {
    shoot_hip_roll_name = "RHipRoll";
    support_hip_roll_name = "LHipRoll";
    shoot_hip_pitch_name = "RHipPitch";
    support_hip_pitch_name = "LHipPitch";
    shoot_knee_pitch_name = "RKneePitch";
    shoot_ankle_pitch_name = "RAnklePitch";
    shoot_ankle_roll_name = "RAnkleRoll";
    support_ankle_roll_name = "LAnkleRoll";

    BALANCE_HIP_ROLL = -20;
    BALANCE_ANKLE_ROLL = 25;
    STRIKE_OUT_HIP_ROLL = -30;
  }

  if (__quit)  return;
  goto_start_pos(0.2);

  ALValue names;
  ALValue target_angles;
  float speed = 0;

  // Balance on supporting leg
  names.arraySetSize(4);
  target_angles.arraySetSize(4);

  support_hip_roll = BALANCE_HIP_ROLL;
  shoot_hip_roll = BALANCE_HIP_ROLL;
  shoot_ankle_roll = BALANCE_ANKLE_ROLL;
  support_ankle_roll = BALANCE_ANKLE_ROLL;

  names = ALValue::array(support_hip_roll_name, shoot_hip_roll_name,
      support_ankle_roll_name, shoot_ankle_roll_name);
  target_angles = ALValue::array(deg2rad(support_hip_roll), deg2rad(shoot_hip_roll),
      deg2rad(support_ankle_roll), deg2rad(shoot_ankle_roll));
  speed = 0.15;

  //if (__quit)  return;
  __almotion->angleInterpolationWithSpeed(names, target_angles, speed);

  names.clear();
  target_angles.clear();

  // Raise shooting leg
  names.arraySetSize(3);
  target_angles.arraySetSize(3);

  shoot_hip_roll = STRIKE_OUT_HIP_ROLL;
  shoot_knee_pitch = 90;
  shoot_ankle_pitch = -50;

  names = ALValue::array(shoot_hip_roll_name, shoot_knee_pitch_name, shoot_ankle_pitch_name);
  target_angles = ALValue::array(deg2rad(shoot_hip_roll), deg2rad(shoot_knee_pitch),
      deg2rad(shoot_ankle_pitch));
  speed = 0.2;

  if (__quit)  return;
  __almotion->angleInterpolationWithSpeed(names, target_angles, speed);

  names.clear();
  target_angles.clear();

  // Strike out
  names.arraySetSize(2);
  target_angles.arraySetSize(2);

  shoot_hip_pitch = 20;
  support_hip_pitch = -50;

  names = ALValue::array(shoot_hip_pitch_name, support_hip_pitch_name);
  target_angles = ALValue::array(deg2rad(shoot_hip_pitch), deg2rad(support_hip_pitch));
  speed = 0.1;

  if (__quit)  return;
  __almotion->angleInterpolationWithSpeed(names, target_angles, speed);

  names.clear();
  target_angles.clear();

  // Shoot
  names.arraySetSize(4);
  target_angles.arraySetSize(4);

  shoot_hip_pitch = -80;
  support_hip_pitch = -40;
  shoot_knee_pitch = 50;
  shoot_ankle_pitch = -30;

  names = ALValue::array(shoot_hip_pitch_name, support_hip_pitch_name,
      shoot_knee_pitch_name, shoot_ankle_pitch_name);
  target_angles = ALValue::array(deg2rad(shoot_hip_pitch), deg2rad(support_hip_pitch),
      deg2rad(shoot_knee_pitch), deg2rad(shoot_ankle_pitch));
  speed = 1.0;
  if (__quit)  return;
  __almotion->angleInterpolationWithSpeed(names, target_angles, speed);

  names.clear();
  target_angles.clear();

  // Move to upright position
  names.arraySetSize(2);
  target_angles.arraySetSize(2);

  shoot_hip_pitch = -25;
  support_hip_pitch = -25;

  names = ALValue::array(shoot_hip_pitch_name, support_hip_pitch_name);
  target_angles = ALValue::array(deg2rad(shoot_hip_pitch), deg2rad(support_hip_pitch));
  speed = 0.1;

  if (__quit)  return;
  __almotion->angleInterpolationWithSpeed(names, target_angles, speed);

  //names.clear();
  //target_angles.clear();

  if (__quit)  return;
  goto_start_pos(0.1);
}