C++ Bottle类代码示例

bool icubFinger::calibrateIndexMiddle(){

    // Open the finger
    Finger::open();

    // Set the proximal to 0 and distal to 180
    setAngle(_proximalJointIndex, 0, 30);
    setAngle(_distalJointIndex, 180, 30);
    while(!checkMotionDone()){
        ;
    }



    // TODO: remove this for the simulation. There is not such data
    // available during simulation
    Bottle *fingerEnc;
    for(int i = 0; i <= _fingerEncoders->getPendingReads(); i++)
        fingerEnc = _fingerEncoders->read();

    if(!fingerEnc->isNull())
    {
        _maxProximal = fingerEnc->get(_proximalEncoderIndex).asDouble();
        _minMiddle = fingerEnc->get(_middleEncoderIndex).asDouble();
        _minDistal = fingerEnc->get(_distalEncoderIndex).asDouble();
    }



    setAngle(_distalJointIndex, 0, 30);
    while(!checkMotionDone()){
        ;
    }

    setAngle(_proximalJointIndex, 90, 30);
    while(!checkMotionDone()){
        ;
    }



    // TODO: remove this for the simulation. There is not such data
    // available during simulation
    for(int i = 0; i < _fingerEncoders->getPendingReads(); i++)
        fingerEnc = _fingerEncoders->read();
    if(!fingerEnc->isNull())
    {
        _minProximal = fingerEnc->get(_proximalEncoderIndex).asDouble();
        _maxMiddle = fingerEnc->get(_middleEncoderIndex).asDouble();
        _maxDistal = fingerEnc->get(_distalEncoderIndex).asDouble();
    }

    setAngle(_proximalJointIndex, 0);
    while(!checkMotionDone()){
        ;
    }

    checkMinMax(_minDistal, _maxDistal);
    checkMinMax(_minMiddle, _minMiddle);
    checkMinMax(_minProximal, _minProximal);

    cout << "Calibration mins:" <<
            _minProximal << "\t" <<
            _minMiddle << "\t" <<
            _minDistal << "\t" << endl;

    cout << "Calibration maxes:" <<
            _maxProximal << "\t" <<
            _maxMiddle << "\t" <<
            _maxDistal << "\t" << endl;
}

/*
*   Dump in the port DumperPort the human skeleton, and the object of the OPC: sObjectToDump
*
*/
void humanRobotDump::DumpHumanObject()
{
    Agent* ag = iCub->opc->addOrRetrieveEntity<Agent>(sAgentName);

    Bottle bDump;
    bDump.addString(sActionName);
    bDump.addString(sObjectToDump);
    bDump.addList() = ag->m_body.asBottle();

    iCub->opc->checkout();
    list<Entity*> lEntity = iCub->opc->EntitiesCacheCopy();
    for (list<Entity*>::iterator itEnt = lEntity.begin(); itEnt != lEntity.end(); itEnt++)
    {
        if ((*itEnt)->entity_type() == EFAA_OPC_ENTITY_AGENT ||
            (*itEnt)->entity_type() == EFAA_OPC_ENTITY_OBJECT ||
            (*itEnt)->entity_type() == EFAA_OPC_ENTITY_RTOBJECT)
        {
            if ((*itEnt)->name() != "icub")
            {
                Object* ob = iCub->opc->addOrRetrieveEntity<Object>((*itEnt)->name());
                Bottle bObject;
                bObject.addString(ob->name());
                bObject.addDouble(ob->m_ego_position[0]);
                bObject.addDouble(ob->m_ego_position[1]);
                bObject.addDouble(ob->m_ego_position[2]);
                bObject.addInt(ob->m_present);
                bDump.addList() = bObject;
            }
        }
    }

    bDump.addInt(m_iterator);
    
    DumperPort.write(bDump);
}

/**
Add an other interaction in the adjKnowledge (event adj is not this one, this is used for a comparaison)
bInput format:
(action sAction_name)
(timing timing)
(coordinate X Y dX dY)
*/
void    adjKnowledge::addOtherInteraction(Bottle bInput)
{
	string sAction = bInput.check("action", Value("none")).asString();
	//    vdNoGnlTiming.push_back(bInput.check("timing", Value(0.)).asDouble());
	//   mActionTiming[sAction].second.push_back(bInput.check("timing", Value(0.)).asDouble());
}

bool InterpersonalDistanceRegulator::respond(const Bottle& cmd, Bottle& reply)
{
	reply.addString("NACK");
    return true;
}

 virtual void onRead(Bottle& b) {
      // process data in b
     printf("Got %s\n", b.toString().c_str());
 }

void Eye2world::WorkerThread::run() {
	Vector head6d(6);
	Vector torso3d(3);
	double prevZOffset = 0.0;

#ifndef DEBUG_OFFLINE
	bool positionUpdated = false;
	bool transformationAvailable = false;
	BufferedPort < Bottle > *in = (BufferedPort<Bottle>*) dataPorts[id.Input_Coordinates2d];
	BufferedPort < Bottle > *head = (BufferedPort<Bottle>*) dataPorts[id.Input_HeadPosition];
	BufferedPort < Bottle > *torso = (BufferedPort<Bottle>*) dataPorts[id.Input_TorsoPosition];
#endif
	BufferedPort < Bottle > *out = (BufferedPort<Bottle>*) dataPorts[id.Output_Coordinates3d];

	while (!isStopping()) {

		string camera = "left";
		Vector object2d(2);
		double zOffset = 0;

#ifdef DEBUG_OFFLINE
		yarp::os::Time::delay(1);
#else
		inputCoordinates = in->read(true);

		if (isStopping() || inputCoordinates == NULL) {
            break;
        }

		unsigned int coordinatesPos = 0;
		if (inputCoordinates->get(0).isString()) {
			camera = inputCoordinates->get(0).asString().c_str();
			// optional! default: 0.0
			coordinatesPos = 1;
		} else {
			// Assume (x,y) pair!
			// This is for testing with YARP viewer click port.
		}

		double motor2eye = moduleOptions["motor2eye"].getValue().asDouble();

		for (unsigned int i = 0; i < 2; i++) {
			object2d[i] = inputCoordinates->get(coordinatesPos + i).asDouble();
		}
		zOffset = moduleOptions["heightOffset"].getValue().asDouble();
		zOffset += motor2eye;
		zOffset += inputCoordinates->get(coordinatesPos + 2).asDouble();

		positionUpdated = zOffset != prevZOffset;

		Bottle* headPosition = head->read(!transformationAvailable);
		if (headPosition != NULL && headPosition->size() >= 6) {
			//get data and convert from degrees to radiant
			for (unsigned int i = 0; i < 6; i++) {
				head6d[i] = headPosition->get(i).asDouble() * M_PI / 180.0;
			}
			positionUpdated = true;
		}

        if (isStopping()) {
            break;
        }

		Bottle* torsoPosition = torso->read(!transformationAvailable);
		if (torsoPosition != NULL && torsoPosition->size() >= 3) {
			//get data and convert from degrees to radiant
			for (unsigned int i = 0; i < 3; i++) {
				torso3d[i] = torsoPosition->get(i).asDouble() * M_PI / 180.0;
			}
			positionUpdated = true;
		}

        if (isStopping()) {
            break;
        }

		if (positionUpdated || !transformationAvailable) { // The 2nd one is just to be sure ;)
			projections[camera]->setHeightOffset(zOffset, false);
			projections[camera]->setBaseTransformation(torso3d, head6d);
			transformationAvailable = true;
		}
#endif
		Vector object3d(3);
		projections[camera]->project(object2d, object3d);

		// Add scalor to the result
		double scale = moduleOptions["scale"].getValue().asDouble();
		object3d[0] = object3d[0] * scale;
		object3d[1] = object3d[1] * scale;

		object3d[2] = object3d[2] - motor2eye;

		Bottle &outputCoordinates = out->prepare();
		outputCoordinates.clear();
		for (unsigned int i = 0; i < 3; i++) {
			outputCoordinates.addDouble(object3d[i]);
		}
		cout << "out = " << outputCoordinates.toString() << endl;
		out->write();
	}
//.........这里部分代码省略.........

bool AgentDetector::updateModule()
{
    LockGuard lg(m);

    bool isRefreshed = client.getDepthAndPlayers(depth,players);
    client.getRgb(rgb);

    bool tracked;

    if (handleMultiplePlayers)
        tracked=client.getJoints(joints);
    else
        tracked=client.getJoints(joint, EFAA_KINECT_CLOSEST_PLAYER);
    //cout<<"Tracking value = "<<tracked<<endl;

    if (tracked)
    {
        if (handleMultiplePlayers)
            client.getSkeletonImage(joints,skeletonImage);
        else
        {
            client.getSkeletonImage(joint,skeletonImage);
            joints.clear();
            joints.push_back(joint);
        }
    } 

    client.getPlayersImage(players,playersImage);
    client.getDepthImage(depth,depthToDisplay);

    if (depthPort.getOutputCount()>0)
    {
        depthPort.prepare()=depthToDisplay;
        depthPort.write();
    }

    if (imagePort.getOutputCount()>0)
    {
        imagePort.prepare()=rgb;
        imagePort.write();
    }

    if (playersPort.getOutputCount()>0)
    {
        playersPort.prepare()=playersImage;
        playersPort.write();
    }

    if (skeletonPort.getOutputCount()>0)
    {
        skeletonPort.prepare()=skeletonImage;
        skeletonPort.write();
    }

    if (showImages)
    {        
        cvConvertScale((IplImage*)depthToDisplay.getIplImage(),depthTmp,1.0/255);
        cvCvtColor((IplImage*)rgb.getIplImage(),rgbTmp,CV_BGR2RGB);

        string mode=showMode;
        string submode;
        while (!mode.empty())
        {
            if (showImageParser(mode,submode))
            {            
                if (submode=="rgb")
                    cvShowImage("rgb",rgbTmp);
                else if (submode=="depth")
                    cvShowImage("depth",depthTmp);
                else if (submode=="skeleton")
                    cvShowImage("skeleton",(IplImage*)skeletonImage.getIplImage());
                else if (submode=="players")
                    cvShowImage("players",(IplImage*)playersImage.getIplImage());
                else
                    yError("unrecognized show mode!");
            }
        }

        cvWaitKey(1);
    }

    //Send the players information to the OPC
    bool localIsCalibrated=checkCalibration();

    //Allow click calibration
    if (!localIsCalibrated)
    {
        if (AgentDetector::clicked==clicked_left)
        {
            AgentDetector::clicked=idle;

            //Get the clicked point coordinate in Kinect space
            Vector clickedPoint(3);
            cout<<"Processing a click on ("<<AgentDetector::clickX<<" "<<AgentDetector::clickY<<") --> ";
            client.get3DPoint((int)AgentDetector::clickX,(int)AgentDetector::clickY,clickedPoint);
            cout<<clickedPoint.toString(3,3)<<endl;

            Bottle bCond;
            Bottle bObject;
            Bottle bRTObject;
//.........这里部分代码省略.........

    virtual bool read(ConnectionReader& reader) {
        YTRACE("NameServer::read start");
        ConstString ref = "NAME_SERVER ";
        bool ok = true;
        ConstString msg = "?";
        bool haveMessage = false;
        if (ok) {
            if (reader.isTextMode()) {
                msg = reader.expectText().c_str();
            } else {
                // migrate to binary mode support, eventually optimize
                Bottle b;
                b.read(reader);
                msg = b.toString().c_str();
            }
            haveMessage = (msg!="");
            msg = ref + msg;
        }
        if (reader.isActive()&&haveMessage) {
            YARP_DEBUG(Logger::get(),ConstString("name server got message ") + msg);
            size_t index = msg.find("NAME_SERVER");
            if (index==0) {
                Contact remote = reader.getRemoteContact();
                YARP_DEBUG(Logger::get(),
                           ConstString("name server receiving from ") +
                           remote.toURI());
                YARP_DEBUG(Logger::get(),
                           ConstString("name server request is ") + msg);
                ConstString result = server->apply(msg,remote);
                ConnectionWriter *os = reader.getWriter();
                if (os!=YARP_NULLPTR) {
                    if (result=="") {
                        result = ns_terminate(ConstString("unknown command ") +
                                              msg + "\n");
                    }
                    // This change is just to make Microsoft Telnet happy
                    ConstString tmp;
                    for (unsigned int i=0; i<result.length(); i++) {
                        if (result[i]=='\n') {
                            tmp += '\r';
                        }
                        tmp += result[i];
                    }
                    tmp += '\r';
                    os->appendString(tmp.c_str(),'\n');

                    YARP_DEBUG(Logger::get(),
                               ConstString("name server reply is ") + result);
                    ConstString resultSparse = result;
                    size_t end = resultSparse.find("\n*** end of message");
                    if (end!=ConstString::npos) {
                        resultSparse[end] = '\0';
                    }
                    YARP_INFO(Logger::get(),resultSparse);
                }
            } else {
                YARP_INFO(Logger::get(),
                          ConstString("Name server ignoring unknown command: ")+msg);
                ConnectionWriter *os = reader.getWriter();
                if (os!=YARP_NULLPTR) {
                    // this result is necessary for YARP1 support
                    os->appendString("???????????????????????????????????????",'\n');
                    //os->flush();
                    //os->close();
                }
            }
        }
        YTRACE("NameServer::read stop");
        return true;
    }

Contact NameServer::registerName(const ConstString& name,
                                 const Contact& address,
                                 const ConstString& remote) {
    bool reusablePort = false;
    bool reusableIp = false;

    //YARP_DEBUG(Logger::get(),"in registerName...");

    if (name!="...") {
        unregisterName(name);
    }

    Contact suggestion = address;

    if (!suggestion.isValid()) {
        suggestion = Contact(name, "...", "...", 0);
    }

    ConstString portName = name;
    if (portName == "...") {
        portName = tmpNames.get();
    }

    ConstString carrier = suggestion.getCarrier();
    if (carrier == "...") {
        carrier = "tcp";
    }

    ConstString machine = suggestion.getHost();
    int overridePort = 0;
    if (machine == "...") {
        if (carrier!="mcast") {
            if (remote=="...") {
                YARP_ERROR(Logger::get(),"remote machine name was not found!  can only guess it is local...");
                machine = "127.0.0.1";
            } else {
                machine = remote;
            }
        } else {
            machine = mcastRecord.get();
            overridePort = mcastRecord.lastPortNumber();
            reusableIp = true;
        }
    }

    int port = suggestion.getPort();
    if (port == 0) {
        if (overridePort) {
            port = overridePort;
        } else {
            port = getHostRecord(machine).get();
            reusablePort = true;
        }
    }

    suggestion = Contact(portName, carrier, machine, port);

    YARP_DEBUG(Logger::get(),ConstString("Registering ") +
               suggestion.toURI() + " for " + suggestion.getRegName());

    NameRecord& nameRecord = getNameRecord(suggestion.getRegName());
    nameRecord.setAddress(suggestion,reusablePort,reusableIp);

    Bottle event;
    event.addVocab(Vocab::encode("add"));
    event.addString(suggestion.getRegName().c_str());
    onEvent(event);

    return nameRecord.getAddress();
}

yarp::os::Bottle NameServer::ncmdVersion(int argc, char *argv[]) {
    Bottle response;
    response.addString("version");
    response.addString(Companion::version().c_str());
    return response;
}

Bottle NameServer::botify(const Contact& address) {
    Bottle result;
    if (address.isValid()) {
        Bottle bname;
        bname.addString("name");
        bname.addString(address.getRegName().c_str());
        Bottle bip;
        bip.addString("ip");
        bip.addString(address.getHost().c_str());
        Bottle bnum;
        bnum.addString("port_number");
        bnum.addInt(address.getPort());
        Bottle bcarrier;
        bcarrier.addString("carrier");
        bcarrier.addString(address.getCarrier().c_str());

        result.addString("port");
        result.addList() = bname;
        result.addList() = bip;
        result.addList() = bnum;
        result.addList() = bcarrier;
    } else {
        Bottle bstate;
        bstate.addString("error");
        bstate.addInt(-2);
        bstate.addString("port not known");
        result.addString("port");
        result.addList() = bstate;
    }
    return result;
}

bool ObjectReconstr::respond(const Bottle& command, Bottle& reply) 
{
    if (command.get(0).asString()=="help")
    {
        reply.addVocab(ACK);
        reply.addString("To obtain a reconstruction, the module needs a segmentation algorithm that returns the set of pixels belonging to the object");
        reply.addString("It typically uses the graphBasedSegmentation module");
        reply.addString("Provide a point of the object first, in the format x y, for instance clicking on the segmentation image.");
        reply.addString("Then, when you send the command 3Drec, you will be provided with the reconstructed object along with the minimum enclosing bounding box on the output port of the module -- typically /object-reconstruction/mesh:o.");
        reply.addString("If you also want to visualize the result, the command is 3Drec visualize.");
        reply.addString("If you want to reconstruct a single pixel, the command is get point (x y).");
        return true;
    }

    if (command.get(0).asString()=="set")
    {
        if (command.get(1).asString()=="write")
        {
            if (command.size()>2)
            {
                if (command.get(2).asString()=="on")
                {
                    reply.addVocab(ACK);
                    write=true;
                }
                else if (command.get(2).asString()=="off")
                {
                    write=false;
                    reply.addVocab(ACK);
                }
                else
                    reply.addVocab(NACK);
                return true;
            }
        }
        else
        {
            reply.addVocab(NACK);
            return true;
        }
    }
    if (command.get(0).asString()=="3Drec")
    {
        if (middlex==-1 || middley==-1)
        {
            reply.addVocab(NACK);
            reply.addString("I need a pixel of the object");
            return true;
        }

        currentState=STATE_RECONSTRUCT;

        visualizationOn=false;

        if (command.size()==2)
            if (command.get(1).asString()=="visualize")
                visualizationOn=true;

        reply.addVocab(ACK);
        return true;
    }

    if (command.get(0).asString()=="name")
    {
        if (command.size()>=2)
        {
            fileName=command.get(1).asString().c_str();
            reply.addVocab(ACK);
 	    }
        else
        {
            reply.addVocab(NACK);
	        reply.addString("No name was provided");
        }

        return true;
    }

    if (command.get(0).asString()=="get")
    {
        if (command.get(1).asString()=="point" && command.size()==3)
        {
            if (currentState!=STATE_RECONSTRUCT)
            {
                IplImage* imgL;
                IplImage* imgR;

                ImageOf<PixelRgb> *tmpL = imagePortInLeft.read(true);
                ImageOf<PixelRgb> *tmpR = imagePortInRight.read(true);

                if(tmpL!=NULL && tmpR!=NULL)
                {
                    imgL= (IplImage*) tmpL->getIplImage();
                    imgR= (IplImage*) tmpR->getIplImage();
                }
                else
                {
                    reply.addVocab(NACK);
                    return true;
                }
//.........这里部分代码省略.........

bool RpLidar2::open(yarp::os::Searchable& config)
{
    string   serial;
    int      baudrate;
    u_result result;

    m_device_status = DEVICE_OK_STANBY;
    m_min_distance  = 0.1; //m
    m_max_distance  = 2.5;  //m
    m_pwm_val       = 0;
    bool br       = config.check("GENERAL");
    if (br != false)
    {
        yarp::os::Searchable& general_config = config.findGroup("GENERAL");
        m_clip_max_enable = general_config.check("clip_max");
        m_clip_min_enable = general_config.check("clip_min");
        if (m_clip_max_enable)                                    { m_max_distance = general_config.find("clip_max").asFloat64(); }
        if (m_clip_min_enable)                                    { m_min_distance = general_config.find("clip_min").asFloat64(); }
        if (general_config.check("max_angle")   == false)         { yError()  << "Missing max_angle param in GENERAL group"; return false;    }
        if (general_config.check("min_angle")   == false)         { yError()  << "Missing min_angle param in GENERAL group"; return false;    }
        if (general_config.check("resolution")  == false)         { yError()  << "Missing resolution param in GENERAL group"; return false;  }
        if (general_config.check("serial_port") == false)         { yError()  << "Missing serial_port param in GENERAL group"; return false; }
        if (general_config.check("serial_baudrate") == false)     { yError()  << "Missing serial_baudrate param in GENERAL group"; return false; }
        if (general_config.check("sample_buffer_life") == false)  { yError()  << "Missing sample_buffer_life param in GENERAL group"; return false; }

        baudrate    = general_config.find("serial_baudrate").asInt32();
        serial      = general_config.find("serial_port").asString();
        m_max_angle   = general_config.find("max_angle").asFloat64();
        m_min_angle   = general_config.find("min_angle").asFloat64();
        m_resolution  = general_config.find("resolution").asFloat64();
        m_buffer_life = general_config.find("sample_buffer_life").asInt32();
        m_do_not_clip_infinity_enable = (general_config.find("allow_infinity").asInt32()!=0);
        if (general_config.check("motor_pwm"))
        {
            m_pwm_val     = general_config.find("motor_pwm").asInt32();
        }
        if (general_config.check("thread_period"))
        {
            double thread_period = general_config.find("thread_period").asInt32() / 1000.0;
            this->setPeriod(thread_period);
        }
    }
    else
    {
        yError() << "Missing GENERAL section";
        return false;
    }

    bool bs = config.check("SKIP");
    if (bs != false)
    {
        yarp::os::Searchable& skip_config = config.findGroup("SKIP");

        Bottle mins   = skip_config.findGroup("min");
        Bottle maxs   = skip_config.findGroup("max");
        size_t s_mins = mins.size();
        size_t s_maxs = mins.size();

        if (s_mins == s_maxs && s_maxs > 1 )
        {
            for (size_t s = 1; s < s_maxs; s++)
            {
                Range_t range;
                range.max = maxs.get(s).asFloat64();
                range.min = mins.get(s).asFloat64();
                if (range.max >= 0 && range.max <= 360 &&
                    range.min >= 0 && range.min <= 360 &&
                    range.max > range.min)
                {
                    m_range_skip_vector.push_back(range);
                }
                else
                {
                    yError() << "Invalid range in SKIP section";
                    return false;
                }
            }
        }

    }

    if (m_max_angle <= m_min_angle)            { yError() << "max_angle should be > min_angle";  return false; }
    double fov = (m_max_angle - m_min_angle);
    if (fov >360)                          { yError() << "max_angle - min_angle <= 360";  return false; }
    m_sensorsNum = (int)(fov/m_resolution);
    m_laser_data.resize(m_sensorsNum, 0.0);

    m_drv = RPlidarDriver::CreateDriver(RPlidarDriver::DRIVER_TYPE_SERIALPORT);
    if (!m_drv)
    {
            yError() << "Create Driver fail, exit\n";
            return false;
    }

    if (IS_FAIL(m_drv->connect(serial.c_str(), (_u32)baudrate)))
    {
        yError() << "Error, cannot bind to the specified serial port:", serial.c_str();
        RPlidarDriver::DisposeDriver(m_drv);
        return false;
    }
//.........这里部分代码省略.........

bool repeaterModule::respond(const Bottle& command, Bottle& reply) 
{
    bool ok = false;
    bool rec = false; // is the command recognized?
    string helpMessage =  string(getName().c_str()) + 
                " commands are: \n" +  
                "help \n" +
                "quit \n";
    reply.clear(); 

    if (command.get(0).asString()=="quit") {
        reply.addString("quitting");
        return false;     
    }
    else if (command.get(0).asString()=="help") {
        cout << helpMessage;
        reply.addString("ok");
    }
    else if ((command.get(0).asString()=="sus") || (command.get(0).asString()=="\"sus\"")) {
        //prioritiser->waitMotionDone();
        //prioritiser->suspend();
        reply.addString("ok");
    }
    else if (command.get(0).asString()=="res" || command.get(0).asString()=="\"res\"" ) {
        //prioritiser->resume();
        reply.addString("ok");
    }
    
    mutex.wait();
    switch (command.get(0).asVocab()) {
    case COMMAND_VOCAB_HELP:
        rec = true;
        {
            reply.addVocab(Vocab::encode("many"));
            reply.addString("help");

            //reply.addString();
            reply.addString("set fn \t: general set command ");
            reply.addString("get fn \t: general get command ");
            //reply.addString();

            
            //reply.addString();
            reply.addString("seek red \t : looking for a red color object");
            reply.addString("seek rgb \t : looking for a general color object");
            reply.addString("sus  \t : suspending");
            reply.addString("res  \t : resuming");
            //reply.addString();

            ok = true;
        }
        break;
    case COMMAND_VOCAB_SUSPEND:
        rec = true;
        {
            //prioritiser->suspend();
            ok = true;
        }
        break;
    case COMMAND_VOCAB_STOP:
        rec = true;
        {
            //prioritiser->suspend();
            //prioritiser->resume();
            ok = true;
        }
        break;
    case COMMAND_VOCAB_RESUME:
    rec = true;
        {
            //prioritiser->resume();
            ok = true;
        }
        break;
    case COMMAND_VOCAB_SEEK:
        rec = true;
        {
            //prioritiser->suspend();
            //prioritiser->seek(command);
            //prioritiser->resume();
            ok = true;
        }
        break;
    case COMMAND_VOCAB_FIX:
        rec = true;
        {
            switch (command.get(1).asVocab()) {
            case COMMAND_VOCAB_CENT:
                {
                    printf("Fixating in Center \n");
                    //prioritiser->fixCenter(1000);
                }
                break;
            }
            ok = true;
        }
        break;
    case COMMAND_VOCAB_PUSH:
        rec = true;
        {
//.........这里部分代码省略.........

bool parametricCalibrator::open(yarp::os::Searchable& config)
{
    yTrace();
    Property p;
    p.fromString(config.toString());

    if (p.check("GENERAL")==false)
    {
      yError() << "Parametric calibrator: missing [GENERAL] section"; 
      return false;
    } 

    if(p.findGroup("GENERAL").check("deviceName"))
    {
      deviceName = p.findGroup("GENERAL").find("deviceName").asString();
    } 
    else
    {
      yError() << "Parametric calibrator: missing deviceName parameter"; 
      return false;
    } 

    std::string str;
    if(config.findGroup("GENERAL").find("verbose").asInt())
    {
        str=config.toString().c_str();
        yTrace() << deviceName.c_str() << str;
    }  

    // Check clearHwFaultBeforeCalibration
    Value val_clearHwFault = config.findGroup("GENERAL").find("clearHwFaultBeforeCalibration");
    if(val_clearHwFault.isNull())
    {
        clearHwFault = false;
    }
    else
    {
        if(!val_clearHwFault.isBool())
        {
            yError() << deviceName.c_str() << ": clearHwFaultBeforeCalibration bool param is different from accepted values (true / false). Assuming false";
            clearHwFault = false;
        }
        else
        {
            clearHwFault = val_clearHwFault.asBool();
            if(clearHwFault)
                yInfo() << deviceName.c_str() << ":  clearHwFaultBeforeCalibration option enabled\n";
        }
    }

    // Check Vanilla = do not use calibration!
    skipCalibration =config.findGroup("GENERAL").find("skipCalibration").asBool() ;// .check("Vanilla",Value(1), "Vanilla config");
    skipCalibration = !!skipCalibration;
    if(skipCalibration )
    {
        yWarning() << deviceName << ": skipping calibration!! This option was set in general.xml file.";
        yWarning() << deviceName << ": BE CAREFUL USING THE ROBOT IN THIS CONFIGURATION! See 'skipCalibration' param in config file";
    } 

    int nj = 0;
    if(p.findGroup("GENERAL").check("joints"))
    {
        nj = p.findGroup("GENERAL").find("joints").asInt();
    }
    else if(p.findGroup("GENERAL").check("Joints"))
    {
        // This is needed to be backward compatibile with old iCubInterface
        nj = p.findGroup("GENERAL").find("Joints").asInt();
    }
    else
    {
        yError() << deviceName.c_str() <<  ": missing joints parameter" ;
        return false;
    }

    type = new unsigned char[nj];
    param1 = new double[nj];
    param2 = new double[nj];
    param3 = new double[nj];
    maxPWM = new int[nj];

    zeroPos = new double[nj];
    zeroVel = new double[nj];
    currPos = new double[nj];
    currVel = new double[nj];
    homePos = new double[nj];
    homeVel = new double[nj];
    zeroPosThreshold = new double[nj];

    int i=0;

    Bottle& xtmp = p.findGroup("CALIBRATION").findGroup("calibration1");
    if (xtmp.size()-1!=nj) {yError() << deviceName << ": invalid number of Calibration1 params " << xtmp.size()<< " " << nj; return false;}
    for (i = 1; i < xtmp.size(); i++) param1[i-1] = xtmp.get(i).asDouble();

    xtmp = p.findGroup("CALIBRATION").findGroup("calibration2");
    if (xtmp.size()-1!=nj) {yError() << deviceName << ": invalid number of Calibration2 params"; return false;}
    for (i = 1; i < xtmp.size(); i++) param2[i-1] = xtmp.get(i).asDouble();

    xtmp = p.findGroup("CALIBRATION").findGroup("calibration3");
//.........这里部分代码省略.........