C++ Stamp::update方法代码示例

    bool updateModule()
    {
        LockGuard lg(mutex);

        double dumpTime=Time::now();
        Bottle &bDump=dumpPort.prepare();
        bDump.clear();

        bDump.addString(actionTag);
        bDump.addString(objectTag);

        opc.checkout();        

        // agent body + position
        agentName = getPartnerName();
        if (Entity *e=opc.getEntity(agentName))
        {
            if (Agent *agent=dynamic_cast<Agent*>(e))
            {
                bDump.addList()=agent->m_body.asBottle();
                Bottle &bAgent=bDump.addList();
                bAgent.addString(agent->name());
                bAgent.addDouble(agent->m_ego_position[0]);
                bAgent.addDouble(agent->m_ego_position[1]);
                bAgent.addDouble(agent->m_ego_position[2]);
                bAgent.addInt(agent->m_present==1.0?1:0);
            }
        }

        // objects position
        list<Entity*> lEntity=opc.EntitiesCache();
        for (list<Entity*>::iterator itEnt=lEntity.begin(); itEnt!=lEntity.end(); itEnt++)
        {
            string entityName=(*itEnt)->name();
            string entityType=(*itEnt)->entity_type();
            if (entityType==EFAA_OPC_ENTITY_OBJECT)
            {
                if (Object *object=dynamic_cast<Object*>(*itEnt))
                {
                    Bottle &bObject=bDump.addList();
                    bObject.addString(object->name());
                    bObject.addDouble(object->m_ego_position[0]);
                    bObject.addDouble(object->m_ego_position[1]);
                    bObject.addDouble(object->m_ego_position[2]);
                    bObject.addInt(object->m_present==1.0?1:0);
                }
            }
        }

        bDump.addInt(gate);        

        dumpStamp.update(dumpTime);
        dumpPort.setEnvelope(dumpStamp);
        dumpPort.writeStrict();

        yInfo()<<bDump.toString();

        return true;
    }

void ThreeDModule::getEncoderPositions(double *headjnt_pos, double *torsojnt_pos, Stamp stamp) {	
	mutex.wait();
		
	if(stamp.getTime() == 0) {
		std::cout << "WARNING: No timestamp found in this bottle!! check your implementation!! " << std::endl;
		stamp.update();
	}
	
	// store the current (start) indices in the circular buffer
	int tIdx = torsoIdx;
	int hIdx = headIdx;
	
	// helpers
	int idxH = -1, idxT = -1;
	int hH, hT;
	double smallestDiffH = 10000;
	double smallestDiffT = 10000;	
	double tDiff;

	// the bottles to be copied out of the circ buffer
	Bottle head, torso;
	
	// going through the circular buffer
    idxH = 1;
    idxT = 1;
//	for(int k = 0; k < LIST_LENGTH; k++) {
//		hT = (LIST_LENGTH + tIdx - k) % LIST_LENGTH;	
//		hH = (LIST_LENGTH + hIdx - k) % LIST_LENGTH;
//		
//		tDiff = fabs(stamp.getTime() - torsoStamp[hT].getTime());
//		std::cout << stamp.getTime() << " idx: " << hT <<  " getTime: " << torsoStamp[hT].getTime() << ", diffT: "<< tDiff << std::endl;		
//		if( tDiff < smallestDiffT ) {
//			idxT = hT;				
//			smallestDiffT = tDiff;
//		}
//
//		tDiff = fabs(stamp.getTime() - torsoStamp[hH].getTime());
//		if( tDiff < smallestDiffH ) { 
//			idxH = hH;		
//			smallestDiffH = tDiff;
//		}
//	}
	
	if( idxH != -1 ) head = headState[idxH];	
	if( idxT != -1 ) torso = torsoState[idxT];
	std::cout <<  "idxT: " << idxT << "/" << idxT-tIdx <<  " val: " << torso.toString() <<  std::endl;			
	std::cout <<  "idxH: " << idxH << "/" << idxH-hIdx << " val: " << head.toString() <<  std::endl;				
	
	for(int i = 0; i < 6; i++) {
		headjnt_pos[i] = head.get(i).asDouble();
		if(i < 3) torsojnt_pos[i] = torso.get(i).asDouble();
	}
	
	mutex.post();
}

void comBalancingThread::run()
{    
    // if(!Opt_display){
    
   
        //updating Time Stamp
        static Stamp timeStamp;
        timeStamp.update();
        
        //***************************** Reading F/T measurements and encoders ****************

        #ifdef FOR_PLOTS_ONLY 
                Vector* F_ext_RLt = port_ft_foot_right->read(true);
                Vector* F_ext_LLt = port_ft_foot_left->read(true);

                if (F_ext_LL==0) F_ext_LL= new Vector(6,0.0);
                if (F_ext_RL==0) F_ext_RL = new Vector(6,0.0);

                (*F_ext_LL) = -1.0*((*F_ext_LLt) - (Offset_Lfoot));
                (*F_ext_RL) = -1.0*((*F_ext_RLt) - (Offset_Rfoot));

                //Transformation from ankle to f/t sensor
                Matrix foot_hn(4,4); foot_hn.zero();
                foot_hn(0,2)=1; foot_hn(0,3)=-7.75;
                foot_hn(1,1)=-1;
                foot_hn(2,0)=-1;
                foot_hn(3,3)=1;           

                Vector tmp1, tmp2;
                tmp1 = (*F_ext_RL).subVector(0,2); tmp1.push_back(0.0); tmp1 = foot_hn * tmp1;
                tmp2 = (*F_ext_RL).subVector(3,5); tmp2.push_back(0.0); tmp2 = foot_hn * tmp2;

                for (int i=0; i<3; i++) (*F_ext_RL)[i] = tmp1[i];
                for (int i=3; i<6; i++) (*F_ext_RL)[i] = tmp2[i-3];

                tmp1 = (*F_ext_LL).subVector(0,2); tmp1.push_back(0.0); tmp1 = foot_hn * tmp1;
                tmp2 = (*F_ext_LL).subVector(3,5); tmp2.push_back(0.0); tmp2 = foot_hn * tmp2;

                for (int i=0; i<3; i++) (*F_ext_LL)[i] = tmp1[i];
                for (int i=3; i<6; i++) (*F_ext_LL)[i] = tmp2[i-3];

                //fprintf(stderr, "F_EXT_RL from module: %s\n", (*F_ext_RL).toString().c_str());

        #else
                if(Opt_ankles_sens)
                {
                    F_ext_RL = EEWRightAnkle->read(true);
                    F_ext_LL = EEWLeftAnkle->read(true);

                }
                else{
                    F_ext_RL = EEWRightLeg->read(true);
                    F_ext_LL = EEWLeftLeg->read(true);
                }   
        #endif      


        Ienc_RL->getEncoders(encs_r.data());
        Ienc_LL->getEncoders(encs_l.data());


        // check if the robot is not in contact with the ground
        static bool on_ground = true;
        //printf("%f %f \n", (*F_ext_LL)[0], (*F_ext_RL)[0]);
        if ((*F_ext_LL)[0] > 50 &&
            (*F_ext_RL)[0] > 50  )
            {
              on_ground = true;
            }
        else
            {
              on_ground = false;
              for (int jj=0; jj<6; jj++) (*F_ext_LL)[jj] =  (*F_ext_RL)[jj] = 1e-20;
              (*F_ext_LL)[0] =  (*F_ext_RL)[0] = 1e+20;
            }
        //printf("%s\n", (*F_ext_LL).toString().c_str());
        //gif ((*F_ext_LL)[3])
        //***************************** Computing ZMP ****************************************

        computeZMP(&zmp_xy, F_ext_LL, F_ext_RL, encs_l, encs_r);
        
        #ifdef VERBOSE
        fprintf(stderr, "ZMP coordinates: %f %f       %d \n",zmp_xy[0],zmp_xy[1],(int)(torso));
        fprintf(stderr, "ZMP desired: %f %f\n",zmp_des[0], zmp_des[1]);
        #endif

        //*********************** SENDING ZMP COORDINATES TO GuiBalancer *********************
        // if (objPort->getOutputCount() > 0)
        // {
            objPort->prepare() = zmp_xy;
            objPort->setEnvelope(timeStamp);
            objPort->write();
        // }
        
        //********************** SENDING ZMP COORDS TO BE PLOT BY ICUBGUI *****************************
        if (objPort2->getOutputCount() > 0)
        {
            Matrix Trans_lastRot(4,4); Trans_lastRot.zero();
            Trans_lastRot.setSubmatrix(rot_f,0,0);
            Trans_lastRot(3,3) = 1;
//.........这里部分代码省略.........

int main(int argc, char *argv[]) {
    // initialization

    // Open the network
    Network yarp;
    //BufferedPort<Sound> p;
    Port p;
    p.open("/sender");

    // Get a portaudio read device.
    Property conf;
    conf.put("device","portaudio");
    conf.put("read", "");
    // conf.put("samples", rate * rec_seconds);
    conf.put("samples", fixedNSample);
    conf.put("rate", rate);
    PolyDriver poly(conf);
    IAudioGrabberSound *get;

    // Make sure we can read sound
    poly.view(get);
    if (get==NULL) {
        printf("cannot open interface");
        return 1;
    }
    else{
        printf("correctly opened the interface rate: %d, number of samples: %d, number of channels %d \n",rate, rate*rec_seconds, 2);
    }

    //Grab and send
    Sound s;
    //Bottle b;
    //b.addString("hello");

    //unsigned char* dataSound;
    //short* dataAnalysis;
    //int v1, v2;
    //int i = 0, j = 0;
    //NetInt16 v;
    // i = sample amd j = channels;

    get->startRecording(); //this is optional, the first get->getsound() will do this anyway.
    Stamp ts;
    while (true)
    {
      double t1=yarp::os::Time::now();
      ts.update();  
      //s = p.prepare();           
      
      get->getSound(s);        
      
      //v1 = s.get(i,j);
      //v = (NetInt16) v1;
      //v2 = s.get(i+1,j+1); 
      //dataAnalysis = (short*) dataSound;        
      
      p.setEnvelope(ts);
      p.write(s);

      double t2=yarp::os::Time::now();
      printf("acquired %f seconds \n", t2-t1);
    }
    get->stopRecording();  //stops recording.

    return 0;
}

    virtual void run()
    {    
        int  i_touching_left=0;
        int  i_touching_right=0;
        int  i_touching_diff=0;
        info.update();
        
        skinContactList *skinContacts  = port_skin_contacts->read(false);
        if(skinContacts)
        {
            for(skinContactList::iterator it=skinContacts->begin(); it!=skinContacts->end(); it++){
                if(it->getBodyPart() == LEFT_ARM)
                    i_touching_left += it->getActiveTaxels();
                else if(it->getBodyPart() == RIGHT_ARM)
                    i_touching_right += it->getActiveTaxels();
            }
        }
        i_touching_diff=i_touching_left-i_touching_right;

        if (abs(i_touching_diff)<5)
        {
            fprintf(stdout,"nothing!\n");
        }
        else
        if (i_touching_left>i_touching_right)
        {
            fprintf(stdout,"Touching left arm! \n");
            if (!left_arm_master) change_master();
        }
        else
        if (i_touching_right>i_touching_left)
        {
            fprintf(stdout,"Touching right arm! \n");
            if (left_arm_master) change_master();
        }

        if (left_arm_master)
        {
            robot->ienc[LEFT_ARM] ->getEncoders(encoders_master);
            robot->ienc[RIGHT_ARM]->getEncoders(encoders_slave);
            if (port_left_arm->getOutputCount()>0)
            {
                port_left_arm->prepare()= Vector(16,encoders_master);
                port_left_arm->setEnvelope(info);
                port_left_arm->write();
            }
            if (port_right_arm->getOutputCount()>0)
            {
                port_right_arm->prepare()= Vector(16,encoders_slave);
                port_right_arm->setEnvelope(info);
                port_right_arm->write();
            }            
            
        //    robot->ipos[RIGHT_ARM] ->positionMove(3,encoders_master[3]);
            for (int i=jjj; i<5; i++)
            {
                robot->ipid[RIGHT_ARM]->setReference(i,encoders_master[i]);
            }
        }
        else
        {
            robot->ienc[RIGHT_ARM]->getEncoders(encoders_master);
            robot->ienc[LEFT_ARM] ->getEncoders(encoders_slave);
            for (int i=jjj; i<5; i++)
            {
                robot->ipid[LEFT_ARM]->setReference(i,encoders_master[i]);
            }

        }
    }