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

bool TactileLearning::identifyCommand(Bottle commandBot, TactileLearningCommand &com, Bottle& params)
{
	for(unsigned int i = 0; i < tactileLearningCommandSize; i++)
	{
		stringstream stream(TactileLearningCommandList[i]);
		string word;
		int j = 0;
		bool found = true;

		while(stream>>word)
		{
			if (commandBot.get(j).asString() != word.c_str())
			{
				found=false;
				break;
			}
			j++;
		}
        if(found)
		{
			com = (TactileLearningCommand)i;
            params = commandBot.tail();
            for(int k = 1; k < j; k++) params = params.tail();
			return true;
		}
	}

	return false;
}

void RosNameSpace::run() {
    int pct = 0;
    do {
        mutex.wait();
        pct = pending.size();
        mutex.post();
        if (pct>0) {
            mutex.wait();
            Bottle *bot = pending.get(0).asList();
            Bottle curr = *bot;
            mutex.post();

            dbg_printf("ROS connection begins: %s\n", curr.toString().c_str());
            ContactStyle style;
            style.admin = true;
            style.carrier = "tcp";
            Bottle cmd = curr.tail();
            Contact contact = Contact::fromString(curr.get(0).asString());
            contact.setName("");
            Bottle reply;
            NetworkBase::write(contact, cmd, reply, style);
            dbg_printf("ROS connection ends: %s\n", curr.toString().c_str());

            mutex.wait();
            pending = pending.tail();
            pct = pending.size();
            mutex.post();
        }
    } while (pct>0);
}

bool TactileLearning::respond(const Bottle& command, Bottle& reply) 
{
	stringstream temp;

	// reply contains a "command not recognized" by default
	reply.clear();

	TactileLearningCommand com;
    Bottle params;
    if(command.get(0).isInt())
	{
        // if first value is int then it is the id of the command
        com = (TactileLearningCommand)command.get(0).asInt();
        params = command.tail();
    }
	else if(!identifyCommand(command, com, params))
	{
		reply.addString("Unknown command. Input 'help' to get a list of the available commands.");
		return true;
	}

	// external declarations
	vector<Vector> referenceTrajectory;
	vector<Vector> tactileState;

	switch(com)
	{
		case get_optimal_trajectory:
			reply.addString("Module should provide the optimal trajectory if computation is finished");
			//thread->getState(tactileState);
			//addToBottle(reply, tactileState);
			return true;

		case get_info:
            reply.addString("Sorry but there is no info available for the moment!");
			return true;

		case get_help:
            reply.addString("Sorry but there is no help available for the moment!");
			return true;

		case quit:
			reply.addString("quitting...");
			return false;

		default:
			reply.addString("ERROR: This command is known but it is not managed in the code.");
			return true;
	}

	return true;
}

int Manager::processHumanCmd(const Bottle &cmd, Bottle &b)
{
    int ret=Vocab::encode(cmd.get(0).asString().c_str());
    b.clear();
    if (cmd.size()>1)
    {
        if (cmd.get(1).isList())
            b=*cmd.get(1).asList();
        else
            b=cmd.tail();
    }
    return ret;
}

bool BehaviorManager::respond(const Bottle& cmd, Bottle& reply)
{
    yDebug() << "RPC received in BM";
    yDebug() << cmd.toString();
    
    reply.clear();

    if (cmd.get(0).asString() == "help" )
    {   string help = "\n";
        help += " ['behavior_name']  : Triggers corresponding behavior \n";
        reply.addString(help);
    }
    else if (cmd.get(0).asString() == "manual") {
        for(auto& beh : behaviors) {
            if (beh->behaviorName == "followingOrder") {
                if (cmd.get(1).asString() == "on") {
                    yInfo() << "followingOrder behavior manual mode on";
                    dynamic_cast<FollowingOrder *>(beh)->manual = true;
                } else if (cmd.get(1).asString() == "off") {
                    yInfo() << "followingOrder behavior manual mode off";
                    dynamic_cast<FollowingOrder *>(beh)->manual = false;
                }
            }
            else if (beh->behaviorName == "recognitionOrder") {
                if (cmd.get(1).asString() == "on") {
                    yInfo() << "recognitionOrder behavior manual mode on";
                    dynamic_cast<RecognitionOrder *>(beh)->manual = true;
                } else if (cmd.get(1).asString() == "off") {
                    yInfo() << "recognitionOrder behavior manual mode off";
                    dynamic_cast<RecognitionOrder *>(beh)->manual = false;
                }
            }
        }
        reply.addString("ack");
    } else if (cmd.get(0).asString() == "names" ) {
        Bottle names;
        for(auto& beh : behaviors) {
            names.addString(beh->behaviorName);
        }
        reply.addList() = names;
    } else if (cmd.get(0).asString() == "is_available" ) {
        if (mut.tryLock()) {
            mut.unlock();
            reply.addInt(1);
        } else {
            reply.addInt(0);
        }
    }    
    else
    {
        bool behavior_triggered = false;
        for(auto& beh : behaviors) {
            if (cmd.get(0).asString() == beh->behaviorName) {
        //         Bottle args;
        //         args.clear();
        //         for (int a = 1; a < cmd.size(); a++)
        //         {
        //             args.add(&cmd.get(a));
        //         }
                bool aux;
                if (beh->from_sensation_port_name != "None") {
                    if (!Network::isConnected(beh->from_sensation_port_name, beh->sensation_port_in.getName())) {
                        aux =Network::connect(beh->from_sensation_port_name, beh->sensation_port_in.getName());
                        yInfo()<< "The sensations port for "<< beh->behaviorName <<" was not connected. \nReconnection status: " << aux;
                    }
                }
                if (beh->external_port_name != "None") {
                    if (!Network::isConnected(beh->rpc_out_port.getName(), beh->external_port_name)) {
                        aux = Network::connect(beh->rpc_out_port.getName(), beh->external_port_name);
                        yInfo()<< "The external port for "<< beh->behaviorName <<" was not connected. \nReconnection status: " << aux;
                    }   
                }

                Bottle args;
                if (cmd.size()>1){
                    args = cmd.tail();
                }
                yDebug() << "arguments are " << args.toString().c_str();
                // beh->trigger(args);
                behavior_triggered = beh->trigger(args);

                // Add event into ABM
                if (iCub->getABMClient()->Connect()) {
                    std::list<std::pair<std::string, std::string> > lArgument;
                    lArgument.push_back(std::pair<std::string, std::string>("iCub", "agent"));
                    lArgument.push_back(std::pair<std::string, std::string>(cmd.get(0).asString(), "predicate"));
                    lArgument.push_back(std::pair<std::string, std::string>("unknown_object", "object"));
                    lArgument.push_back(std::pair<std::string, std::string>("partner", "recipient"));
                    iCub->getABMClient()->sendActivity("action",
                        cmd.get(0).asString(),
                        "behavior",  // expl: "pasar", "drives"...
                        lArgument,
                        true);
                    yInfo() << cmd.get(0).asString() + " behavior has been recorded in the ABM";
                }
                else{
                    yDebug() << "ABM not connected; no recording of action.";
                }
            }
        }
//.........这里部分代码省略.........

//.........这里部分代码省略.........
				if (reply.size() == 0)
					reply.addString("--> Sorry, tabula rasa");
				speaker.reply(reply);
			}

			else if (first == "relax")
			{
				int done = planner->Relax();
				switch(done)
					{ 
					case 0:		reply.addString("--> Ok, I relax!"); break;
					case -1:	reply.addString("--> Sorry, I can't relax"); break;
					}
				speaker.reply(reply);
			}

			else if (first == "prepare")
			{
				int done = planner->Prepare();
				switch(done)
					{ 
					case 0:		reply.addString("--> Ok, I'm ready!"); break;
					case -1:	reply.addString("--> Sorry, something went wrong"); break;
					}
				speaker.reply(reply);
			}
			// sequence reaching
			else if (first == "reach")
			{
				if(cmd.size() == 1)
					reply.addString("--> Sorry, I don't know what I should reach with my finger");
				else
				{
					int done = planner->SequenceReach(cmd.tail());
					switch(done)
					{
					case 0: reply.addString("--> Ok, I have reached the object!"); break;
					case -1: reply.addString("--> Sorry, I don't know this object"); break;
					case -2: reply.addString("--> Sorry, I don't know where my hands are"); break;
					case -3: reply.addString("--> Sorry, I did not understand what you said"); break;
					case -4: reply.addString("--> Sorry, my ports are not connected"); break;
					case -5: reply.addString("--> Sorry, I don't know this command"); break;
					case -8: reply.addString("--> Sorry, I couldn't reach the object"); break;
					case -9: reply.addString("--> Sorry, my wrist orientation is wrong"); break;
					}
				}
				speaker.reply(reply);
			}

			// finger reaching
			else if (first == "reach1")
			{
				if(cmd.size() != 3 && cmd.size() != 2)
					reply.addString("--> Sorry, I don't know what I should reach with my finger");
				else
				{
					
					int done = planner->Prepare();
					if(done != 0) reply.addString("--> Sorry, I'm lazy");
					else if (cmd.size() == 3)
					{
						if (cmd.get(2).asString() == "top" || cmd.get(2).asString() == "side")
						{
							done = planner->FingerReach(cmd.get(1).asString().c_str(),"",cmd.get(2).asString().c_str(),false);
							if (done == 0)
							{

bool PMPthread::setVTGSparam(Bottle cmdBot, string side)
{
	VTGSparam cmd;

	if(cmdBot.size() < 2)
	{
		printf("Error: No value specified\n");
		return false;
	}
	if(!identifyCmd(cmdBot,cmd))
	{
		printf("Error: Parameter not recognized\n");
		return false;
	}

	VirtualTrajectoryGenerator * VTGS;

	if (side == "right")	VTGS = VTGS_r;
	if (side == "left")		VTGS = VTGS_l;
	
	// remove the keyword from bottle, so as to have only parameter value inside
	Bottle bot = cmdBot.tail();
	Vector vtemp(9);
	Matrix mtemp(3,3);

	bool wasSuspended = isSuspended();
	string active;
	Vector deltaindex(3);

	switch(cmd)
	{
		// VTGS parameters
		// Before any change, suspend the thread. Probably not effective ...
		// At the end of the switch the thread initial state will be restored
		updateSem.wait();
		if(!isSuspended())	suspend();

		case target:

			// set target as first in matrix
			if(bot.size() == 3)
			{
				mtemp.resize(1,3);
				mtemp.zero();

				Bottle2Matrix(bot,mtemp);
				VTGS->setTarget(mtemp);
				cout << mtemp.toString() << endl;

				updateSem.post();
				break;
			}

			else if(bot.get(0).isInt() && (bot.size()-1)%3 == 0)
			{
				mtemp.resize(bot.get(0).asInt(),3);
				mtemp.zero();
				if (mtemp.rows() <= 3)
				{	
					Bottle2Matrix(bot.tail(),mtemp);
					VTGS->setTarget(mtemp);
					cout << mtemp.toString() << endl;
				}
				else
				{
					printf("Error: parameter value size mismatch\n");
					if(!wasSuspended) resume();
					updateSem.post();
					return false;
				}

				updateSem.post();
				break;
			}

			// set target in tail: (FIFO mode)
/*			else if(bot.size() == 3)
			{
				// new critical point coordinates: if at least one VTGS has a new target, then the thread flag
				// is set to false: tha target saved has not yet been reached.
				// the target should be set only when the thread is not running (is suspended).
					
				VTGS->setTarget(Bottle2Vector(bot));
				VTGS->numTargetPoints++;
				//readyToStart = false;

				updateSem.post();
				break;
			}
*/
			else
			{
				updateSem.post();
				return false;
			}

		case weights:
			if(bot.get(0).isInt() && (bot.size()-1)%3 == 0)
			{
				mtemp.resize(bot.get(0).asInt(),3);
//.........这里部分代码省略.........

bool PMPthread::setPMPparam(Bottle cmdBot)
{
	// mettere tutti i printf in 1 stringa messaggio che viene letta dal modulo ogni volta che respond riceve un false
	// così scrivo tutto da un unico programma, il modulo, ed elimino tutti i printf.
	PMPparam cmd;
	if(cmdBot.size() < 2)
	{
		printf("Error: No value specified\n");
		return false;
	}
	if(!identifyCmd(cmdBot,cmd))
	{
		printf("Error: Parameter not recognized\n");
		return false;
	}
	
	// remove the keyword from bottle, so as to have only parameter value inside
	Bottle bot = cmdBot.tail();
	Bottle test;
	Bottle &list = test.addList();

	bool wasSuspended = isSuspended();

	switch(cmd)
	{
		// PMP parameters
		updateSem.wait();
		if(!isSuspended())	suspend();

		case xr_target:
			if(bot.size() != PMP->x_tgR.size())
			{
				printf("Error: parameter value size mismatch\n");
				if(!wasSuspended) resume();
				updateSem.post();
				return false;
			}
			PMP->x_tgR = Bottle2Vector(bot);
			break;

		case xl_target:
			if(bot.size() != PMP->x_tgL.size())
			{
				printf("Error: parameter value size mismatch\n");
				if(!wasSuspended) resume();
				updateSem.post();
				return false;
			}
			PMP->x_tgL = Bottle2Vector(bot);
			break;

		case q_rightArm:
			if(bot.size() != PMP->q_rightArm.size())
			{
				printf("Error: parameter value size mismatch\n");
				if(!wasSuspended) resume();
				updateSem.post();
				return false;
			}
			PMP->set_q_rightArm(Bottle2Vector(bot));
			break;

		case q_leftArm:
			if(bot.size() != PMP->q_leftArm.size())
			{
				printf("Error: parameter value size mismatch\n");
				if(!wasSuspended) resume();
				updateSem.post();
				return false;
			}
			PMP->set_q_leftArm(Bottle2Vector(bot));
			break;

		case qr_initial:
			if(bot.size() != PMP->q_0R.size())
			{
				printf("Error: parameter value size mismatch\n");
				if(!wasSuspended) resume();
				updateSem.post();
				return false;
			}
			PMP->q_0R = Bottle2Vector(bot);
			break;

		case ql_initial:
			if(bot.size() != PMP->q_0L.size())
			{
				printf("Error: parameter value size mismatch\n");
				if(!wasSuspended) resume();
				updateSem.post();
				return false;
			}
			PMP->q_0L = Bottle2Vector(bot);
			break;

		case Kr_virt:
			if(bot.size() != PMP->K_right.cols())
			{
				printf("Error: parameter value size mismatch\n");
				if(!wasSuspended) resume();
//.........这里部分代码省略.........

bool skinManager::respond(const Bottle& command, Bottle& reply) {
	stringstream temp;
	reply.clear();

	SkinManagerCommand com;
    Bottle params;
    if(command.get(0).isInt()){
        // if first value is int then it is the id of the command
        com = (SkinManagerCommand)command.get(0).asInt();
        params = command.tail();
    }
	else if(!identifyCommand(command, com, params)){
		reply.addString("Unknown command. Input 'help' to get a list of the available commands.");
		return true;
	}

	switch( com ){
		case quit:
			reply.addString("quitting");
			return false;

		case help:
            reply.addVocab(Vocab::encode("many"));				// print every string added to the bottle on a new line
			reply.addString((string(getName().c_str()) + " commands are: ").c_str());
			for(unsigned int i=0; i< SkinManagerCommandSize; i++){
				reply.addString( ("- "+SkinManagerCommandList[i]+": "+SkinManagerCommandDesc[i]).c_str() );
			}
			return true;

		case calibrate:
			myThread->calibrate();
			break;

		case get_touch_thr:
			{
			Vector touchThreshold = myThread->getTouchThreshold();
			for(size_t i=0; i< touchThreshold.size(); i++) 
				reply.addDouble(touchThreshold[i]);
			return true;
			}

		case set_binarization:
			{
			if(params.size()<1){
				reply.addString("Binarization state missing! Specify either on or off.");
				return true;
			}
			string value = params.get(0).asString().c_str();
			if(value.compare("on")==0)
				myThread->setBinarization(true);
			else if(value.compare("off")==0)
				myThread->setBinarization(false);
			else{
				reply.addString("Value not recognized.");
				return true;
			}
			break;
			}

		case get_binarization:
			if(myThread->getBinarization())
				reply.addString("on");
			else
				reply.addString("off");
			return true;

		case set_smooth_filter:
			{
			if(params.size()<1){
				reply.addString("Smooth filter state missing! Specify either on or off.");
				return true;
			}
			string value = params.get(0).asString().c_str();
			if(value.compare("on")==0)
				myThread->setSmoothFilter(true);
			else if(value.compare("off")==0)
				myThread->setSmoothFilter(false);
			else{
				reply.addString("Value not recognized.");
				return true;
			}
			break;
			}

		case get_smooth_filter:
			if(myThread->getSmoothFilter())
				reply.addString("on");
			else
				reply.addString("off");
			return true;

		case set_smooth_factor:
			{
			if(params.size()<1 || (!params.get(0).isDouble() && !params.get(0).isInt())){
				reply.addString("New smooth factor value missing or not a number! Smooth factor not updated.");
				return true;
			}

			stringstream temp;
			if(myThread->setSmoothFactor((float)params.get(0).asDouble())){
//.........这里部分代码省略.........

bool XmlRpcCarrier::write(Protocol& proto, SizedWriter& writer) {
    //XmlRpc::setVerbosity(10);
    StringOutputStream sos;
    StringInputStream sis;
    writer.write(sos);
    sis.reset(sos.toString());
    String header;
    if (sender) {
        header = NetType::readLine(sis);
    }
    String body = NetType::readLine(sis);
    //printf("Asked to write: hdr %s body %s\n",
    //     header.c_str(), body.c_str());
    Value v;
    //printf("HEADER %s\n", header.c_str());
    if (header[0]=='q') {
        body = "yarp.quit";
        // XMLRPC does not need a quit message, this should get stripped
        return false;
    }
    Bottle *bot = v.asList();
    //Bottle aux;
    bot->fromString(body.c_str());
    ConstString methodName;
    if (sender) {
        methodName = bot->get(0).toString();
        *bot = bot->tail();
    }
    XmlRpcValue args;
    if (bot->size()==1) {
        toXmlRpcValue(bot->get(0),args);
    } else {
        toXmlRpcValue(v,args);
    }
    //printf("xmlrpc block to write is %s\n", args.toXml().c_str());
    std::string req;
    if (sender) {
        const Address& addr = host.isValid()?host:proto.getStreams().getRemoteAddress();
        XmlRpcClient c(addr.getName().c_str(),(addr.getPort()>0)?addr.getPort():80);
        c.generateRequest(methodName.c_str(),args);
        req = c.getRequest();
    } else {
        XmlRpcServerConnection c(0,NULL);
        c.generateResponse(args.toXml());
        req = c.getResponse();
    }
    int start = 0;
    //printf("converts to %s\n", req.c_str());
    if (sender) {
        if (req.length()<8) {
            fprintf(stderr, "XmlRpcCarrier fail, %s:%d\n", __FILE__, __LINE__);
            return false;
        }
        for (int i=0; i<(int)req.length(); i++) {
            if (req[i] == '\n') {
                start++;
                break;
            }
            start++;
        }
        if (!firstRound) {
            Bytes b((char*)http.c_str(),http.length());
            proto.os().write(b);
        }
        firstRound = false;
    }
    Bytes b((char*)req.c_str()+start,req.length()-start);
    //printf("WRITING [%s]\n", req.c_str()+start);
    proto.os().write(b);

    return proto.os().isOk();
}

bool XmlRpcCarrier::write(ConnectionState& proto, SizedWriter& writer) {
    StringOutputStream sos;
    StringInputStream sis;
    writer.write(sos);
    sis.reset(sos.toString());
    ConstString header;
    if (sender) {
        header = sis.readLine();
    }
    ConstString body = sis.readLine();
    Value v;
    if (header.length()>0 && header[0]=='q') {
        body = "yarp.quit";
        // XMLRPC does not need a quit message, this should get stripped
        return false;
    }
    Bottle *bot = v.asList();
    bot->fromString(body.c_str());
    ConstString methodName;
    if (sender) {
        methodName = bot->get(0).toString();
        *bot = bot->tail();
    }
    XmlRpcValue args;
    if (bot->size()==1) {
        toXmlRpcValue(bot->get(0),args);
    } else {
        toXmlRpcValue(v,args);
    }
    std::string req;
    if (sender) {
        const Contact& addr = host.isValid()?host:proto.getStreams().getRemoteAddress();
        XmlRpcClient c(addr.getHost().c_str(),(addr.getPort()>0)?addr.getPort():80);
        c.generateRequest(methodName.c_str(),args);
        req = c.getRequest();
    } else {
        XmlRpcServerConnection c(0,NULL);
        c.generateResponse(args.toXml());
        req = c.getResponse();
    }
    int start = 0;
    if (sender) {
        if (req.length()<8) {
            fprintf(stderr, "XmlRpcCarrier fail, %s:%d\n", __FILE__, __LINE__);
            return false;
        }
        for (int i=0; i<(int)req.length(); i++) {
            if (req[i] == '\n') {
                start++;
                break;
            }
            start++;
        }
        if (!firstRound) {
            Bytes b((char*)http.c_str(),http.length());
            proto.os().write(b);
        }
        firstRound = false;
    }
    Bytes b((char*)req.c_str()+start,req.length()-start);
    proto.os().write(b);

    return proto.os().isOk();
}