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

Bottle Action::asBottle()
{
    Bottle b = this->Entity::asBottle();
    Bottle bSub;
    bSub.addString("description");
    bSub.addList() = initialDescription.asBottle();
    b.addList() = bSub;
    bSub.clear();
    bSub.addString("subactions");
    Bottle& subs = bSub.addList();
    for(list<Action>::iterator sIt = subActions.begin() ; sIt != subActions.end(); sIt++)
    {
        subs.addList()=sIt->asBottle();
    }
    b.addList() = bSub;
        
    bSub.clear();
    bSub.addString("estimatedDriveEffects");
    Bottle &subss = bSub.addList();
    for(map<string, double>::iterator sIt = estimatedDriveEffects.begin() ; sIt != estimatedDriveEffects.end(); sIt++)
    {
        Bottle &ss = subss.addList();
        ss.addString(sIt->first.c_str());
        ss.addDouble(sIt->second);
    }
    b.addList() = bSub;
    return b;
}

bool utManagerThread::getPointFromStereo()
{
    Bottle cmdSFM;
    Bottle respSFM;
    cmdSFM.clear();
    respSFM.clear();
    cmdSFM.addString("Root");
    cmdSFM.addInt(int(templatePFTrackerPos(0)));
    cmdSFM.addInt(int(templatePFTrackerPos(1)));
    SFMrpcPort.write(cmdSFM, respSFM);

    // Read the 3D coords and compute the distance to the set reference frame origin
    if (respSFM.size() == 3)
    {
        Vector SFMtmp(3,0.0);
        SFMtmp(0) = respSFM.get(0).asDouble(); // Get the X coordinate
        SFMtmp(1) = respSFM.get(1).asDouble(); // Get the Y coordinate
        SFMtmp(2) = respSFM.get(2).asDouble(); // Get the Z coordinate

        if (SFMtmp(0) == 0.0 && SFMtmp(1) == 0.0 && SFMtmp(2) == 0.0)
        {
            return false;
        }

        SFMPos = SFMtmp;
        return true;
    } 

    return false;
}

void CB::YARPConfigurationVariables::setVelocityControlMode(bool mode, string portName) {

    bool ok = true;

    // specify the local port names that connect to the velocityControl module
    string velocityOutputPortName = "/cb/configuration" + deviceName + "/vel:o";
    string velocityRPCOutputPortName = "/cb/configuration" + deviceName + "/vel/rpc:o";
    //    velocityPortName = portName + "/command";      
    velocityPortName = portName + "/fastCommand";      
    velocityRPCPortName = portName + "/input";      

    Bottle b;

    if(mode && !velocityControlMode) {

        // if we're turning on the velocityControlMode (and it wasnt previously on)

        // open and connect the data and config portsport
        ok &= velocityPort.open(velocityOutputPortName.c_str());
        ok &= Network::connect(velocityOutputPortName.c_str(),velocityPortName.c_str(),"tcp");
        Time::delay(0.5);
        ok &= velocityRPCPort.open(velocityRPCOutputPortName.c_str());
        ok &= Network::connect(velocityRPCOutputPortName.c_str(),velocityRPCPortName.c_str(),"tcp");

        // send gain and maxVel paramaters to the vc module 
        for(int i=0; i<mask.size(); i++) {
            if(mask[i]) {
                cout << "setting vc params joint[" << i << "]: gain=" << velocityGain << ", svel=" << maxSetVal << endl;
                b.clear();
                b.addString("gain");
                b.addInt(i);
                b.addDouble(velocityGain);
                velocityRPCPort.write(b);
                Time::delay(0.1);
                b.clear();
                b.addString("svel");
                b.addInt(i);
                b.addDouble(maxSetVal);
                velocityRPCPort.write(b);
                Time::delay(0.1);
            }
        }


    } else if(!mode && velocityControlMode) {

        // turning off velocity control mode by disconnecting and closing ports
        ok &= Network::disconnect(velocityOutputPortName.c_str(),velocityPortName.c_str());
        ok &= Network::disconnect(velocityRPCOutputPortName.c_str(),velocityRPCPortName.c_str());
        velocityRPCPort.close();
        velocityPort.close();

    }

    // set the current mode
    velocityControlMode = mode;


}

    void testSequence(char *seq,
                      size_t len,
                      const char *fmt,
                      Bottle ref,
                      bool testWrite = true)
    {
        char err[1024];
        printf("\n");
        printf("================================================\n");
        printf(" READ %s\n", fmt);
        Bytes b1(seq, len);
        WireTwiddler tt;
        tt.configure(fmt, fmt);
        printf(">>> %s\n", tt.toString().c_str());
        Bottle bot;

        checkTrue(tt.read(bot, b1), "Read failed");
        snprintf(err, 1024, "%s: read %s, expected %s", fmt, bot.toString().c_str(), ref.toString().c_str());
        checkTrue(bot == ref, err);
        printf("[1] %s: read %s as expected\n", fmt, bot.toString().c_str());

        StringInputStream sis;
        sis.add(b1);
        sis.add(b1);
        WireTwiddlerReader twiddled_input(sis, tt);
        Route route;
        bot.clear();
        twiddled_input.reset();
        ConnectionReader::readFromStream(bot, twiddled_input);

        snprintf(err, 1024, "%s: read %s, expected %s", fmt, bot.toString().c_str(), ref.toString().c_str());
        checkTrue(bot == ref, err);
        printf("[2] %s: read %s as expected\n", fmt, bot.toString().c_str());

        bot.clear();
        twiddled_input.reset();
        ConnectionReader::readFromStream(bot, twiddled_input);

        snprintf(err, 1024, "%s: read %s, expected %s", fmt, bot.toString().c_str(), ref.toString().c_str());
        checkTrue(bot == ref, err);
        printf("[3] %s: read %s as expected\n", fmt, bot.toString().c_str());

        if (testWrite) {

            printf("\n");
            printf("================================================\n");
            printf(" WRITE %s\n", fmt);
            ManagedBytes output;
            checkTrue(tt.write(ref, output), "WRITE FAILED");
            snprintf(err, 1024, "WRITE MISMATCH, length %zd, expected %zd", output.length(), len);
            checkTrue(output.length() == len, err);

            for (size_t i = 0; i < output.length(); i++) {
                snprintf(err, 1024, "WRITE MISMATCH, at %zd, have [%d:%c] expected [%d:%c]\n", i, output.get()[i], output.get()[i], seq[i], seq[i]);
                checkTrue(output.get()[i] == seq[i], err);
            }
            printf("[4] %s: wrote %s as expected\n", fmt, bot.toString().c_str());
        }
    }

bool demoModule::respond(const Bottle& command, Bottle& reply) {
  string helpMessage =  string(getName().c_str()) + 
                        " commands are: \n" +  
                        "help \n" + 
                        "quit \n" + 
                        "set thr <n> ... set the threshold \n" + 
                        "(where <n> is an integer number) \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()=="set") {
      if (command.get(1).asString()=="thr") {
         thresholdValue = command.get(2).asInt(); // set parameter value
         reply.addString("ok");
      }
   }
   return true;
}

bool visualFilterModule::respond(const Bottle& command, Bottle& reply) 
{
    string helpMessage =  string(getName().c_str()) + 
                        " commands are: \n" +  
                        "help \n" + 
                        "quit \n" + 
                        "par  <n> <d> ... set parameter's value \n" + 
                        "(where <n> and <d> are an integer and a double respectively) \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()=="par") {
		int p = command.get(1).asInt();
		double v = command.get(2).asDouble();
		if(p>0 && p<8 && v>=0){
			vfThread->setPar(p,v);
			reply.addString("New kernel generated with updated parameters.");
		}
		else {
			reply.addString("Invalid values");
		}
    	}
    return true;
}

bool PARTICLEModule::respond(const Bottle& command, Bottle& reply) 
{
    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()=="reset") 
    {
        cout << "reset has been asked "<< endl;
        particleManager->shouldSend=false;
        reply.addString("ok");
    }
    else
    {
		cout << "command not known - type help for more info" << endl;
	}
    return true;
}

    // rpcPort commands handler
    bool respond(const Bottle &      command,
                 Bottle &      reply)
    {
        // This method is called when a command string is sent via RPC

        // Get command string
        string receivedCmd = command.get(0).asString().c_str();
        reply.clear();  // Clear reply bottle
        
        if (receivedCmd == "help")
        {
            reply.addVocab(Vocab::encode("many"));
            reply.addString("Available commands are:");
            reply.addString("help");
            reply.addString("quit");
        }
        else if (receivedCmd == "quit")
        {
            reply.addString("Quitting.");
            return false; //note also this
        }
        else
            reply.addString("Invalid command, type [help] for a list of accepted commands.");

        return true;
    }

//*************************************************************************************************************************
MotorFrictionExcitationThread::MotorFrictionExcitationThread(string _name, string _robotName, int _period, ParamHelperServer *_ph,
    wholeBodyInterface *_wbi, ResourceFinder &rf, ParamHelperClient *_identificationModule)
    :  RateThread(_period), name(_name), robotName(_robotName), paramHelper(_ph), robot(_wbi), identificationModule(_identificationModule)
{
    status = EXCITATION_OFF;
    sendCmdToMotors = SEND_COMMANDS_TO_MOTORS;
    printCountdown = 0;
    freeExcCounter = 0;
    contactExcCounter = 0;
    fricStdDevThrMonitor = 0.0;
    ktStdDevThrMonitor = 0.0;
    _n = robot->getDoFs();
    isFrictionStdDevBelowThreshold = false;

    Bottle reply;
    if(!contactExc.readFromConfigFile(rf, reply))
        printf("Error while reading contact excitation from config file: \n%s\n", reply.toString().c_str());
    printf("Results of contact excitation reading: %s\n", reply.toString().c_str());
    printf("Contact excitation value read:\n%s\n", contactExc.toString().c_str());
    reply.clear();
    if(!freeMotionExc.readFromConfigFile(rf, reply))
        printf("Error while reading free motion excitation from config file: \n%s\n", reply.toString().c_str());
    printf("Results of free motion excitation reading: %s\n", reply.toString().c_str());
    printf("Free motion excitation value read:\n%s\n", freeMotionExc.toString().c_str());
}

Bottle PMPthread::initHead(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;
		}
	}

	//cout << "thread: " << angles.toString() << endl;

	updateSem.wait();
	rpcPort.write(angles,reply);
	//cout << "out of thread!!!!!!!" << endl;

	updateSem.post();
	return reply;
}

bool ImageSource::respond(const Bottle& command, Bottle& reply) 
{
  string helpMessage =  string(getName().c_str()) + 
                        " commands are: \n" +  
                        "help \n" + 
                        "quit \n" + 
                        "set noise <n> ... set the noise level \n" + 
                        "(where <n> is an integer number in the range 0-255) \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()=="set") {
      if (command.get(1).asString()=="noise") {
         noiseLevel = command.get(2).asInt(); // set parameter value
         reply.addString("ok");
      }
   }
   return true;
}

    bool threadInit()
    {
        fprintf(stdout,"init \n");
        port_in0.open(string("/simCOM0:i").c_str());
        port_in1.open(string("/simCOM1:i").c_str());
        port_in2.open(string("/simCOM2:i").c_str());
        port_in3.open(string("/simCOM3:i").c_str());
        port_in4.open(string("/simCOM4:i").c_str());
        port_in5.open(string("/simCOM5:i").c_str());
        port_in6.open(string("/simCOM6:i").c_str());
        port_in7.open(string("/simCOM7:i").c_str());
        port_in8.open(string("/simCOM8:i").c_str());
        port_out.open(string("/simCOM:o").c_str());
        yarp::os::Network::connect("/wholeBodyDynamics/com:o","/simCOM0:i");
        yarp::os::Network::connect("/wholeBodyDynamics/left_leg/com:o","/simCOM1:i");
        yarp::os::Network::connect("/wholeBodyDynamics/left_arm/com:o","/simCOM2:i");
        yarp::os::Network::connect("/wholeBodyDynamics/right_leg/com:o","/simCOM3:i");
        yarp::os::Network::connect("/wholeBodyDynamics/right_arm/com:o","/simCOM4:i");
        yarp::os::Network::connect("/wholeBodyDynamics/head/com:o","/simCOM5:i");
        yarp::os::Network::connect("/wholeBodyDynamics/torso/com:o","/simCOM6:i");
        yarp::os::Network::connect("/wholeBodyDynamics/upper_body/com:o","/simCOM7:i");
        yarp::os::Network::connect("/wholeBodyDynamics/lower_body/com:o","/simCOM8:i");
        yarp::os::Network::connect("/simCOM:o","/iCubGui/objects");

        Bottle a;
        a.clear();
        a.addString("reset");
        port_out.prepare() = a;
        port_out.write();

        return true;
    }

    void getObjectList(Bottle &reply)
    {
        double t0=Time::now();

        int max_size=0;
        vector<Blob> max_blobs;

        while(Time::now()-t0<0.5)
        {
            mutex.wait();
            reply.clear();

            int size=0;
            for(unsigned int i=0; i<blobs.size(); i++)
                if(blobs[i].getBest()!="")
                    size++;

            if(max_size<size)
            {
                max_size=size;
                max_blobs=blobs;
            }
            mutex.post();   

            Time::delay(0.05);        
        }


        reply.addInt(max_size);

        for(unsigned int i=0; i<max_blobs.size(); i++)
            if(max_blobs[i].getBest()!="")
                reply.addString(max_blobs[i].getBest().c_str());
    }

bool LRH::respond(const Bottle& command, Bottle& reply) {
    string helpMessage = string(getName().c_str()) +
        " commands are: \n" +
        "help \n" +
        "production objectFocus \n" +
        "meaning sentence \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() == "production"  && command.size() == 2) {
        reply.addString("ack");
        reply.addString("OK, send the language production");
        cout << "command.get(1).asString() : " << command.get(1).asString() << endl;
        spatialRelation(command.get(1).asString());
    }
    else if (command.get(0).asString() == "meaning" && command.size() == 2) {
        reply.addString("ack");
        reply.addString("OK, send the language comprehension");
        sentenceToMeaning(command.get(1).asString());
    }

    yInfo("sending reply from rpc");
    handlerPort.reply(reply);
    return true;
}

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)
      }
    }
}