C++ Bottle::addDouble方法代码示例

Bottle SimCyl::rotateObjectBottle() {

    Bottle cmd;
    cmd.addString("world");
    cmd.addString("rot");
    cmd.addString("cyl");
    cmd.addInt   (objSubIndex);
    cmd.addDouble(rotationX);
    cmd.addDouble(rotationY);
    cmd.addDouble(rotationZ);
    return cmd;
}

Bottle SimCyl::moveObjectBottle() {

    Bottle cmd;
    cmd.addString("world");
    cmd.addString("set");
    cmd.addString("cyl");
    cmd.addInt   (objSubIndex);
    cmd.addDouble(positionX);
    cmd.addDouble(positionY);
    cmd.addDouble(positionZ);
    return cmd;
}

void Controller::notifyEvent(const string &event, const double checkPoint)
{
    if (port_event.getOutputCount()>0)
    {
        Bottle bottle;
        bottle.addString(event.c_str());
        bottle.addDouble(q_stamp);

        if (checkPoint>=0.0)
            bottle.addDouble(checkPoint);

        port_event.write(bottle);
    }
}

// if the object is located in both cameras, return its stereo position
bool VisuoThread::getObject(const std::string &object_name, Bottle &bStereo)
{
    Vector stereo;

    bool ok=opcPort.getStereoPosition(object_name,stereo);

    if(!ok)
    {
        double t=Time::now();
        while(Time::now()-t<objectWaitThresh && !interrupted)
        {
            MILMutex.wait();
            if(locations.count(object_name)>0)
            {
                stereo.resize(4);
                stereo[0]=locations[object_name].x;
                stereo[1]=locations[object_name].y;
                stereo[2]=160.0;
                stereo[3]=120.0;

                ok=true;
            }
            MILMutex.post();
        }
    }

    for(size_t i=0; i<stereo.size(); i++)
        bStereo.addDouble(stereo[i]);

    return ok;
}

bool VisuoThread::getTrack(Bottle &bStereo)
{
    Vector stereo;

    bool ok=false;

    trackMutex.wait();
    if(stereoTracker.vec.size()==12)
    {
        stereo.resize(4);
        stereo[0]=stereoTracker.vec[0];
        stereo[1]=stereoTracker.vec[1];
        stereo[2]=stereoTracker.vec[6];
        stereo[3]=stereoTracker.vec[7];
    }
    trackMutex.post();

    for(size_t i=0; i<stereo.size(); i++)
        bStereo.addDouble(stereo[i]);

    trackMutex.wait();
    tracking=true;
    trackMutex.post();

    trackMode=MODE_TRACK_TEMPLATE;

    return ok;
}

bool VisuoThread::getFixation(Bottle &bStereo)
{
    Vector stereo(4);

    imgMutex.wait();
    if(img[LEFT]!=NULL)
    {
        stereo[0]=stereo[2]=0.5*img[LEFT]->width();
        stereo[1]=stereo[3]=0.5*img[LEFT]->height();
    }
    else
    {
        stereo[0]=stereo[2]=160;
        stereo[1]=stereo[3]=120;
    }
    imgMutex.post();

    for(size_t i=0; i<stereo.size(); i++)
        bStereo.addDouble(stereo[i]);

    int side=40;
    startTracker(stereo,side);

    return true;
}

int main(int argc, char *argv[]) {
  Network yarp;
  Port portRpc, portCommand, portState;
  if (!portRpc.open("/motor/server/rpc:i")) {
    fprintf(stderr,"Failed to open a port, maybe run: yarpserver\n");
    return 1;
  }
  portCommand.open("/motor/server/command:i");
  portState.open("/motor/server/state:o");
  MotorImpl motor;
  motor.serve(portRpc);
  //motor.serve(portCommand);
  double start = Time::now();
  while (true) {
    double now = Time::now();
    if (now-start>10) {
      printf("Motor server running happily\n");
      start += 10;
      if (now-start>10) start = now;
    }
    // stream motor state every now and then
    Bottle b;
    b.addDouble(motor.get_enc(0));
    b.addDouble(motor.get_enc(1));
    portState.write(b);
    Time::delay(0.02);
  }
  return 0;
}

	virtual ArActionDesired *fire (ArActionDesired currentDesired)
	{
		if(BeenCorrectedByStar || !usestar) // if we have updated odo from star or is star isn't being used
		{
		 ArPose MyPose;
		 MyPose =	myRobot->getPose();
		Bottle PoseBottle;
		
		PoseBottle.addDouble(MyPose.getX()/1000);
		PoseBottle.addDouble(MyPose.getY()/1000);
		PoseBottle.addDouble(MyPose.getTh());
		Mycopyofmodule->SendBottleData("MAPout",PoseBottle);
		printf("my x:%f y:%f rot:%f \n",MyPose.getX()/1000,MyPose.getY()/1000,MyPose.getTh());
		}
	return &myDesired;
	}

void boardDumperThread::run()
{
    //printf("Entering the main thread\n");
    //enc->getEncoders(data);
    //Bottle bData;
    //for (int i = 0; i < numberOfJointsRead; i++)
    //  {
    //    dataRead[i] = data[dataMap[i]];
    //    bData.addDouble(dataRead[i]);
    //  }
    //port->write(bData);

    if (getter)
    {
        //printf("Getter is getting something\n");
        getter -> getData(data);

        //fprintf(stderr, "Time is %lf \n", stmp.getTime());

        Bottle bData;
        for (int i = 0; i < numberOfJointsRead; i++)
        {
            //printf("%.2f \n", data[dataMap[i]]);
            dataRead[i] = data[dataMap[i]];
            bData.addDouble(dataRead[i]);
        }

        if (getter->getStamp(stmp)) 
        {
            if (stmp.isValid())
            {
                port->setEnvelope(stmp);
            }
            else
            {
                //stmp.update();
                stmp=Stamp(-1,0.0);
                port->setEnvelope(stmp);
            }
        }
        else
        {
            fprintf(stderr, "boardDumperThread::warning. Trying to get a stamp without a proper IPreciselyTimed defined. \n");
        }

        if (logFile)
        {
            char buff [20];
            sprintf(buff,"%d ",stmp.getCount());
            fputs (buff,logFile);
            sprintf(buff,"%f ",stmp.getTime());
            fputs (buff,logFile);
            fputs (bData.toString().c_str(),logFile);
            fputs ("\n",logFile);
        }

        port->write(bData);
    }
}  

void skinEventsAggregThread::run()
{
    int indexOfBiggestContact = -1;
    skinContact biggestContactInSkinPart;
    Vector geoCenter(3,0.0), normalDir(3,0.0);
    double activation = 0.0;
    Bottle & out = skinEvAggregPortOut.prepare(); out.clear();
    Bottle b;
    b.clear();
        
    ts.update();
        
    skinContactList *scl = skinEventsPortIn.read(false);
        
    if(scl)
    {
        if(!(scl->empty()))
        {  
            //Probably source of crazy inefficiencies, here just to reach a working state as soon as possible \todo TODO
            map<SkinPart, skinContactList> contactsPerSkinPart = scl->splitPerSkinPart();
            
            for(map<SkinPart,skinContactList>::iterator it=contactsPerSkinPart.begin(); it!=contactsPerSkinPart.end(); it++)
            {
                indexOfBiggestContact = getIndexOfBiggestContactInList(it->second);
                
                if (indexOfBiggestContact != -1)
                {
                    b.clear();
                    biggestContactInSkinPart = (it->second)[indexOfBiggestContact];
                    //the output prepared should have identical format to the one prepared in  void vtRFThread::manageSkinEvents()    
                    b.addInt(biggestContactInSkinPart.getSkinPart());
                    vectorIntoBottle(biggestContactInSkinPart.getGeoCenter(),b);
                    vectorIntoBottle(biggestContactInSkinPart.getNormalDir(),b);
                    //we add dummy geoCenter and normalDir in Root frame to keep same format as vtRFThread manageSkinEvents 
                    b.addDouble(0.0); b.addDouble(0.0); b.addDouble(0.0);
                    b.addDouble(0.0); b.addDouble(0.0); b.addDouble(0.0);
                    b.addDouble(std::max(1.0,(biggestContactInSkinPart.getPressure()/SKIN_ACTIVATION_MAX))); // % pressure "normalized" with ad hoc constant
                    b.addString(biggestContactInSkinPart.getSkinPartName()); //this one just for readability
                    out.addList().read(b);
                }
            }
            skinEvAggregPortOut.setEnvelope(ts);
            skinEvAggregPortOut.write();     // send something anyway (if there is no contact the bottle is empty)
      }
    }
}

int TRACKERModule::track(const double fix_x, const double fix_y)
{
    Bottle b;
    b.addDouble(fix_x);
    b.addDouble(fix_y);
    int rep = trackerManager->processFixationPoint(b);
    return rep;
}

    Bottle IncomingEvent::toBottle()
    {
        Bottle b;
        b.clear();

        b.addDouble(Pos[0]);
        b.addDouble(Pos[1]);
        b.addDouble(Pos[2]);
        b.addDouble(Vel[0]);
        b.addDouble(Vel[1]);
        b.addDouble(Vel[2]);
        b.addDouble(Radius);
        b.addString(Src);
        b.addDouble(Threat); //keep it last for now - for backward compatibility

        return b;
    }

Bottle PMPthread::initTorso(Bottle angles)
{
	Bottle reply;
	reply.clear();

	string msg;

	//check if the connection is established. If not, try to connect to DevDriver input ports
	if (!Network::exists(("/" + rpcServerName + "/rpc").c_str()) )
	{
		//printf("Error: device ports not active\n");
		reply.addString("Error: device ports not active");
		return reply;
	}
	if( !Network::isConnected(rpcPort.getName().c_str(), ("/" + rpcServerName + "/rpc").c_str()) )
	{
		if(!Network::connect(rpcPort.getName().c_str(), ("/" + rpcServerName + "/rpc").c_str()) )
		{
			//printf("Error: unable to connect to device port %s\n", ("/" + rpcServerName + "/head:i").c_str());
			msg = "Error: unable to connect to device port /" + rpcServerName + "/rpc";
			reply.addString(msg.c_str());
			return reply;
		}
	}

	double t[3] = {0.0, 0.0, 0.0};

	// default initialization
	if (angles.size() == 1)
	{		
		for (int i=0; i<3; i++)		angles.addDouble(t[i]);
	}
	// else user defined initialization

	updateSem.wait();
	rpcPort.write(angles,reply);

	// update PMP joint angles 
	if (reply.get(0).asString() == "Done")
	{
		PMP->q_rightArm(0) = PMP->q_leftArm(0) = angles.get(0).asDouble()*M_PI/180;
		PMP->q_rightArm(1) = PMP->q_leftArm(1) = angles.get(1).asDouble()*M_PI/180;
		PMP->q_rightArm(2) = PMP->q_leftArm(2) = angles.get(2).asDouble()*M_PI/180;

		PMP->Rchain->setAng(PMP->q_rightArm);
		PMP->Lchain->setAng(PMP->q_leftArm);

		PMP->x_0R = PMP->get_EEPose("right");
		PMP->x_0L = PMP->get_EEPose("left");

		//update VTGS starting position
		VTGS_r->setStartingPoint(PMP->x_0R);
		VTGS_l->setStartingPoint(PMP->x_0L);
	}

	updateSem.post();
	return reply;
}

void vtWThread::sendGuiTarget()
{
    if (outPortGui.getOutputCount()>0)
    {
        Bottle obj;
        obj.addString("object");
        obj.addString("Target");
     
        // size 
        obj.addDouble(50.0);
        obj.addDouble(50.0);
        obj.addDouble(50.0);
    
        // positions
        obj.addDouble(1000.0*events[0].Pos[0]);
        obj.addDouble(1000.0*events[0].Pos[1]);
        obj.addDouble(1000.0*events[0].Pos[2]);
    
        // orientation
        obj.addDouble(0.0);
        obj.addDouble(0.0);
        obj.addDouble(0.0);
    
        // color
        obj.addInt(255);
        obj.addInt(125);
        obj.addInt(125);
    
        // transparency
        obj.addDouble(0.9);
    
        outPortGui.write(obj);
    }
}

void GuiUpdaterModule::addObject(Object* o, const string &opcTag)
{
    //Get the position of the object in the current reference frame of the robot (not the initial one)
    Vector inCurrentRootReference = iCub->getSelfRelativePosition(o->m_ego_position);
    //cout<<o->name()<<" init Root: \t \t"<<o->m_ego_position.toString(3,3)<<endl
    //    <<o->name()<<" current Root: \t \t"<<inCurrentRootReference.toString(3,3)<<endl;

    Bottle cmd;
    cmd.addString("object");
    cmd.addString(opcTag.c_str());
                                
    cmd.addDouble(o->m_dimensions[0] *1000.0);    // dimX in [mm]
    cmd.addDouble(o->m_dimensions[1] *1000.0);    // dimY in [mm]
    cmd.addDouble(o->m_dimensions[2] *1000.0);    // dimZ in [mm]
    cmd.addDouble(inCurrentRootReference[0] *1000.0);        // posX in [mm]
    cmd.addDouble(inCurrentRootReference[1] *1000.0);        // posY in [mm]
    cmd.addDouble(inCurrentRootReference[2] *1000.0);        // posZ in [mm]
    cmd.addDouble(o->m_ego_orientation[0] - iCub->m_ego_orientation[0]);             // Deal with the object orientation that is moving with the base
    cmd.addDouble(o->m_ego_orientation[1] - iCub->m_ego_orientation[1]);             // "
    cmd.addDouble(o->m_ego_orientation[2] - iCub->m_ego_orientation[2]);              // "
    cmd.addInt((int)o->m_color[0]);            // color R
    cmd.addInt((int)o->m_color[1]);            // color G
    cmd.addInt((int)o->m_color[2]);            // color B
    cmd.addDouble(1);                     // alpha coefficient [0,1]
    toGui.write(cmd);
}