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

/* Respond function */
bool opcManager::respond(const yarp::os::Bottle& bCommand, yarp::os::Bottle& bReply)
{
    string helpMessage = string(getName().c_str()) +
        " commands are: \n" +
        "help \n" +
        "connect + name\n" +
        "quit \n";

    bReply.clear();
    string keyWord = bCommand.get(0).asString().c_str();

    if (keyWord == "quit") {
        bReply.addString("quitting");
        return false;
    }
    else if (keyWord == "help") {
        cout << helpMessage;
        bReply.addString("ok");
    }
    else if (keyWord == "connect") {
        bReply = connect(bCommand);
    }
    else if (keyWord == "updateBeliefs") {
        bReply.addString("nack");
        if (bCommand.size() == 2)
        {
            if (bCommand.get(1).isString())
            {
                bReply.addList() = updateBelief(bCommand.get(1).toString().c_str());
            }
        }
    }

    else if (keyWord == "synchronise")
    {
        bReply.addString("ack");
        bReply.addList() = synchoniseOPCs();
    }

    else if (keyWord == "executeActivity")
    {
        bReply.addString("ack");
        bReply.addList() = simulateActivity(bCommand);
    }

    else if (keyWord == "diffOPC")
    {
        bReply.addString("ack");
        bReply.addList() = diffOPC();
    }

    return true;
}

bool SubscriberOnSql::listSubscriptions(const ConstString& port,
                                        yarp::os::Bottle& reply) {
    mutex.wait();
    sqlite3_stmt *statement = NULL;
    char *query = NULL;
    if (ConstString(port)!="") {
        query = sqlite3_mprintf("SELECT s.srcFull, s.DestFull, EXISTS(SELECT topic FROM topics WHERE topic = s.src), EXISTS(SELECT topic FROM topics WHERE topic = s.dest), s.mode FROM subscriptions s WHERE s.src = %Q OR s.dest= %Q ORDER BY s.src, s.dest",port.c_str(),port.c_str());
    } else {
        query = sqlite3_mprintf("SELECT s.srcFull, s.destFull, EXISTS(SELECT topic FROM topics WHERE topic = s.src), EXISTS(SELECT topic FROM topics WHERE topic = s.dest), s.mode FROM subscriptions s ORDER BY s.src, s.dest");
    }
    if (verbose) {
        printf("Query: %s\n", query);
    }
    int result = sqlite3_prepare_v2(SQLDB(implementation),query,-1,&statement,
                                    NULL);
   if (result!=SQLITE_OK) {
        const char *msg = sqlite3_errmsg(SQLDB(implementation));
        if (msg!=NULL) {
            fprintf(stderr,"Error: %s\n", msg);
        }
    }
    reply.addString("subscriptions");
    while (result == SQLITE_OK && sqlite3_step(statement) == SQLITE_ROW) {
        char *src = (char *)sqlite3_column_text(statement,0);
        char *dest = (char *)sqlite3_column_text(statement,1);
        int srcTopic = sqlite3_column_int(statement,2);
        int destTopic = sqlite3_column_int(statement,3);
        char *mode = (char *)sqlite3_column_text(statement,4);
        Bottle& b = reply.addList();
        b.addString("subscription");
        Bottle bsrc;
        bsrc.addString("src");
        bsrc.addString(src);
        Bottle bdest;
        bdest.addString("dest");
        bdest.addString(dest);
        b.addList() = bsrc;
        b.addList() = bdest;
        if (mode!=NULL) {
            if (mode[0]!='\0') {
                Bottle bmode;
                bmode.addString("mode");
                bmode.addString(mode);
                b.addList() = bmode;
            }
        }
        if (srcTopic||destTopic) {
            Bottle btopic;
            btopic.addString("topic");
            btopic.addInt(srcTopic);
            btopic.addInt(destTopic);
            b.addList() = btopic;
        }
    }
    sqlite3_finalize(statement);
    sqlite3_free(query);
    mutex.post();

    return true;
}

 void appendEntry(yarp::os::Bottle& reply, const Contact& c) {
   Bottle& info = reply.addList();
   info.addString("registration");
   info.addString("name");
   info.addString(c.getName().c_str());
   info.addString("ip");
   info.addString(c.getHost().c_str());
   info.addString("port");
   info.addInt(c.getPort());
   info.addString("type");
   info.addString(c.getCarrier().c_str());
 }

 void appendEntry(yarp::os::Bottle& reply, const Entry& e) {
   Bottle& info = reply.addList();
   info.addString("registration");
   info.addString("name");
   info.addString(e.name.c_str());
   info.addString("ip");
   info.addString(e.machine.c_str());
   info.addString("port");
   info.addInt(e.portNumber);
   info.addString("type");
   info.addString(e.carrier.c_str());    
 }

void HandIKModule::prepareData(yarp::os::Bottle &data)
{
    data.addString(tag.c_str());
    Bottle &handB=data.addList();
    handB.addString("hand");
    handB.addString(hand.c_str());
    Bottle &cost=data.addList();
    cost.addString("cost");
    cost.addDouble(bestObjValue);

    rot_tran=zeros(4,4);
    
    rot_tran(1,0)=1.0;
    rot_tran(0,1)=-1.0;
    rot_tran(2,2)=1.0;
    rot_tran(3,3)=1.0;
    rot_tran(0,3)=center[0];
    rot_tran(1,3)=center[1];
    rot_tran(2,3)=center[2];

    yarp::sig::Vector ee_ob=bestSolution.xyz_ee;
    ee_ob.push_back(1.0);

    yarp::sig::Vector ee_root=rot_tran*ee_ob;
    yarp::sig::Matrix tmp=rpy2dcm(bestSolution.rpy_ee);
    yarp::sig::Matrix tmp2=rot_tran*tmp;
    yarp::sig::Vector or_root=dcm2axis(tmp2);

    Bottle &ee=data.addList();
    ee.addString("ee");
    Bottle &ee_coord=ee.addList();
    ee_coord.addDouble(ee_root[0]);
    ee_coord.addDouble(ee_root[1]);
    ee_coord.addDouble(ee_root[2]);
    Bottle &orientation=data.addList();
    orientation.addString("or");
    Bottle &or_coord=orientation.addList();
    yarp::sig::Vector or_axisangle=dcm2axis(rpy2dcm(bestSolution.rpy_ee));
    or_coord.addDouble(or_root[0]);
    or_coord.addDouble(or_root[1]);
    or_coord.addDouble(or_root[2]);
    or_coord.addDouble(or_root[3]);
    Bottle &j=data.addList();
    j.addString("joints");
    Bottle &joints=j.addList();
    for (unsigned int i=0; i<bestSolution.joints.size(); i++)
        joints.addDouble(bestSolution.joints[i]);
    Bottle &order=data.addList();
    order.addString("combination");
    Bottle &combination=order.addList();
    combination.addInt((int)combinations.at(winnerIndex)[0]);
    combination.addInt((int)combinations.at(winnerIndex)[1]);
    combination.addInt((int)combinations.at(winnerIndex)[2]);
}

bool ThreeDModule::respond(const yarp::os::Bottle& in, yarp::os::Bottle& out, yarp::os::Stamp stamp) {
//?	Stamp stamp;
	std::cout << "responding: " << in.toString() << std::endl;

	//TODO sanity check
	//...
	

	out.clear();	
	// process data "in", prepare "out"
	out.append(in);
	out.addList() = calculatePosition(in, stamp);
	
	
	// reply
	return true;
	
	
}

//.........这里部分代码省略.........
			}
            break;

		case VOCAB_GET:
			{
				*rec = true;

				int tmp = 0;
				double dtmp = 0.0;
				response.addVocab(VOCAB_IS);
				response.add(cmd.get(1));

				switch(cmd.get(2).asVocab()) 
                    {
					case VOCAB_AXES:
						{
							int tmp;
							*ok = torque->getAxes(&tmp);
							response.addInt(tmp);
						}
                        break;

					case VOCAB_TRQ:
						{
							*ok = torque->getTorque(cmd.get(3).asInt(), &dtmp);
							response.addDouble(dtmp);
						}
                        break;

					case VOCAB_TRQS:
						{
							double *p = new double[controlledJoints];
							*ok = torque->getTorques(p);
							Bottle& b = response.addList();
							int i;
							for (i = 0; i < controlledJoints; i++)
								b.addDouble(p[i]);
							delete[] p;
						}
                        break;

				    case VOCAB_ERR: 
						{
							*ok = torque->getTorqueError(cmd.get(3).asInt(), &dtmp);
							response.addDouble(dtmp);
						}
						break;

					case VOCAB_ERRS: 
						{
							double *p = new double[controlledJoints];
							*ok = torque->getTorqueErrors(p);
							Bottle& b = response.addList();
							int i;
							for (i = 0; i < controlledJoints; i++)
								b.addDouble(p[i]);
							delete[] p;
						}
						break;

					case VOCAB_OUTPUT: 
						{
							*ok = torque->getTorquePidOutput(cmd.get(3).asInt(), &dtmp);
							response.addDouble(dtmp);
						}
						break;

/*! @brief Fill a bottle with the contents of an object.
 @param[in] jct The %JavaScript engine context.
 @param[in,out] aBottle The bottle to be filled.
 @param[in] theData The value to be sent.
 @param[in] topLevel @c true if this is the outermost list of an object. */
static void
fillBottleFromValue(JSContext *        jct,
                    yarp::os::Bottle & aBottle,
                    JS::Value          theData,
                    const bool         topLevel)
{
    ODL_ENTER(); //####
    ODL_P2("jct = ", jct, "aBottle = ", &aBottle); //####
    ODL_B1("topLevel = ", topLevel); //####
    JS::RootedValue asRootedValue(jct);

    asRootedValue = theData;
    if (theData.isBoolean())
    {
        aBottle.addInt(JS::ToBoolean(asRootedValue) ? 1 : 0);
    }
    else if (theData.isDouble())
    {
        double aValue;

        if (JS::ToNumber(jct, asRootedValue, &aValue))
        {
            aBottle.addDouble(aValue);
        }
    }
    else if (theData.isInt32())
    {
        int32_t aValue;

        if (JS::ToInt32(jct, asRootedValue, &aValue))
        {
            aBottle.addInt(aValue);
        }
    }
    else if (theData.isString())
    {
        JSString * asString = theData.toString();
        char *     asChars = JS_EncodeString(jct, asString);

        aBottle.addString(asChars);
        JS_free(jct, asChars);
    }
    else if (theData.isObject())
    {
        JS::RootedObject asObject(jct);

        if (JS_ValueToObject(jct, asRootedValue, &asObject))
        {
            bool processed = false;
#if (47 <= MOZJS_MAJOR_VERSION)
            bool isArray;
#endif // 47 <= MOZJS_MAJOR_VERSION

#if (47 <= MOZJS_MAJOR_VERSION)
            if (JS_IsArrayObject(jct, asObject, &isArray))
#else // 47 > MOZJS_MAJOR_VERSION
            if (JS_IsArrayObject(jct, asObject))
#endif // 47 > MOZJS_MAJOR_VERSION
            {
                uint32_t arrayLength;

                if (JS_GetArrayLength(jct, asObject, &arrayLength))
                {
                    // Treat as a list
                    if (topLevel)
                    {
                        for (uint32_t ii = 0; arrayLength > ii; ++ii)
                        {
                            JS::RootedValue anElement(jct);

                            if (JS_GetElement(jct, asObject, ii, &anElement))
                            {
                                fillBottleFromValue(jct, aBottle, anElement, false);
                            }
                        }
                    }
                    else
                    {
                        yarp::os::Bottle & innerList(aBottle.addList());

                        for (uint32_t ii = 0; arrayLength > ii; ++ii)
                        {
                            JS::RootedValue anElement(jct);

                            if (JS_GetElement(jct, asObject, ii, &anElement))
                            {
                                fillBottleFromValue(jct, innerList, anElement, false);
                            }
                        }

                    }
                    processed = true;
                }
            }
            if (! processed)
//.........这里部分代码省略.........

bool GazeboYarpControlBoardDriver::getRemoteVariable(yarp::os::ConstString key, yarp::os::Bottle& val)
{
    val.clear();
    if (key == "hardwareDamping")
    {
        yarp::os::Bottle& r = val.addList(); for (size_t i = 0; i< m_numberOfJoints; i++) { double tmp = m_jointPointers[i]->GetDamping(0);  r.addDouble(tmp); }
        return true;
    }
    if (key == "hardwareFriction")
    {
        yarp::os::Bottle& r = val.addList(); for (size_t i = 0; i< m_numberOfJoints; i++) { double tmp = m_jointPointers[i]->GetParam(std::string("friction"),0);  r.addDouble(tmp); }
        return true;
    }
    if (key == "hardwareHiStop")
    {
        yarp::os::Bottle& r = val.addList();
        for (size_t i = 0; i< m_numberOfJoints; i++)
        {
#if GAZEBO_MAJOR_VERSION >= 8
            double upperLimit = m_jointPointers[i]->UpperLimit(0);
#else
            double upperLimit = m_jointPointers[i]->GetUpperLimit(0).Radian();
#endif
            double tmp = convertGazeboToUser(i, upperLimit);
            r.addDouble(tmp);
        }
        return true;
    }
    if (key == "hardwareLowStop")
    {
        yarp::os::Bottle& r = val.addList();
        for (size_t i = 0; i< m_numberOfJoints; i++) {
#if GAZEBO_MAJOR_VERSION >= 8
            double lowerLimit = m_jointPointers[i]->LowerLimit(0);
#else
            double lowerLimit = m_jointPointers[i]->GetLowerLimit(0).Radian();
#endif
            double tmp = convertGazeboToUser(i, lowerLimit);
            r.addDouble(tmp);
        }
        return true;
    }
    if (key == "hardwareEffortLimit")
    {
        yarp::os::Bottle& r = val.addList(); for (size_t i = 0; i< m_numberOfJoints; i++) { double tmp = m_jointPointers[i]->GetEffortLimit(0);  r.addDouble(tmp); }
        return true;
    }
    if (key == "hardwareVelocityLimit")
    {
        yarp::os::Bottle& r = val.addList(); for (size_t i = 0; i< m_numberOfJoints; i++) { double tmp = m_jointPointers[i]->GetVelocityLimit(0);  r.addDouble(tmp); }
        return true;
    }
    if (key == "yarp_jntMaxVel")
    {
        yarp::os::Bottle& r = val.addList(); for (size_t i = 0; i< m_numberOfJoints; i++) { double tmp_min,tmp_max; getVelLimits(i,&tmp_min,&tmp_max);  r.addDouble(tmp_max); }
        return true;
    }
    if (key == "yarp_jntMaxPos")
    {
        yarp::os::Bottle& r = val.addList(); for (size_t i = 0; i< m_numberOfJoints; i++) { double tmp_min,tmp_max; getLimits(i,&tmp_min,&tmp_max);  r.addDouble(tmp_max); }
        return true;
    }
    if (key == "yarp_kPWM")
    {
        yarp::os::Bottle& r = val.addList(); for (size_t i = 0; i< m_numberOfJoints; i++) { r.addDouble(m_kPWM[i]); }
        return true;
    }
    if (key == "yarp_jntMinPos")
    {
        yarp::os::Bottle& r = val.addList(); for (size_t i = 0; i< m_numberOfJoints; i++) { double tmp_min,tmp_max; getLimits(i,&tmp_min,&tmp_max);  r.addDouble(tmp_min); }
        return true;
    }
    if (key == "SHORTCUT_all_pos_kp")
    {
        yarp::os::Bottle& r = val.addList(); for (size_t i = 0; i< m_numberOfJoints; i++) { yarp::dev::Pid tmp_pid; getPid(VOCAB_PIDTYPE_POSITION, i,&tmp_pid);  r.addDouble(tmp_pid.kp); }
        return true;
    }
    if (key == "SHORTCUT_all_pos_kd")
    {
        yarp::os::Bottle& r = val.addList(); for (size_t i = 0; i< m_numberOfJoints; i++) { yarp::dev::Pid tmp_pid; getPid(VOCAB_PIDTYPE_POSITION, i,&tmp_pid);  r.addDouble(tmp_pid.kd); }
        return true;
    }
    if (key == "SHORTCUT_all_pos_ki")
    {
        yarp::os::Bottle& r = val.addList(); for (size_t i = 0; i< m_numberOfJoints; i++) { yarp::dev::Pid tmp_pid; getPid(VOCAB_PIDTYPE_POSITION, i,&tmp_pid);  r.addDouble(tmp_pid.ki); }
        return true;
    }
    if (key == "VelocityTimeout")
    {
        yarp::os::Bottle& r = val.addList(); for (size_t i = 0; i< m_numberOfJoints; i++) { r.addDouble(m_velocity_watchdog[i]->getDuration()); }
        return true;
    }
    yWarning("getRemoteVariable(): Unknown variable %s", key.c_str());
    return false;
}

/* Respond function */
bool abmReasoning::respond(const yarp::os::Bottle& bCommand, yarp::os::Bottle& bReply)
{

    bReply.clear();
    if (!bReady)
    {
        bReply.addString("module not initialised yet, please wait.");
        yInfo() << "\t" << "reply : " << bReply.toString().c_str() << "\n";
        handlerPort.reply(bReply);
        return true;
    }

    if (bCommand.get(0).asString() == "quit") {
        bReply.addString("ack");
        bReply.addString("quitting");
        return false;
    }

    else if (bCommand.get(0).asString() == "sqlQueryTest") {
        bReply.addString("ack");
        //      bReply.addList() = sqlQueryTest();
    }


    // FIND ACTIVITY

    else if (bCommand.get(0).asString() == "findActivity") {

        yInfo() << "\t" << "bCommand.size() = " << bCommand.size();

        if (bCommand.size() <= 2) {
            bReply.addString("nack");
            bReply.addString("Usage : findActivity <actionName> 'begin'|'end'|'both' [columns,to,select]");
        }
        else if (bCommand.get(2).asString() != "begin" && bCommand.get(2).asString() != "end" && bCommand.get(2).asString() != "both") {
            bReply.addString("nack");
            bReply.addString("Usage : findOPC <actionName> 'begin'|'end'|'both' [<colums,to,select>] => check your begin/end/both");
        }
        else {
            bReply.addString("ack");

            string actionName = bCommand.get(1).asString().c_str();
            string beginOrEnd = bCommand.get(2).asString().c_str();
            if (bCommand.size() == 3) {
                bReply.addList() = findActivity(actionName, beginOrEnd);
            }
            else {
                string select = bCommand.get(3).asString().c_str();
                bReply.addList() = findActivity(actionName, beginOrEnd, select);
            }
        }
    }


    // FIND OPC

    else if (bCommand.get(0).asString() == "findOPC") {

        if (bCommand.size() <= 2) {
            bReply.addString("nack");
            bReply.addString("Usage : findOPC <actionName> 'begin'|'end'|'both' [<colums,to,select>]");
        }
        else if (bCommand.get(2).asString() != "begin" && bCommand.get(2).asString() != "end" && bCommand.get(2).asString() != "both") {
            bReply.addString("nack");
            bReply.addString("Usage : findOPC <actionName> 'begin'|'end'|'both' [<colums,to,select>] => check your begin/end/both");
        }
        else {
            bReply.addString("ack");
            string actionName = bCommand.get(1).asString().c_str();
            string beginOrEnd = bCommand.get(2).asString().c_str();
            //          bReply.addList() = findOPC(actionName, beginOrEnd);
        }
    }


    // GET CONSEQUENCES DRIVES

    else if (bCommand.get(0).asString() == "getActionConsequenceDrives") {

        if (bCommand.size() <= 2) {
            bReply.addString("nack");
            bReply.addString("Usage : getActionConsequenceDrives 'actionName' 'arg'");
        }

        else {
            bReply.addString("ack");
            pair<string, string> pAction(bCommand.get(1).asString().c_str(), bCommand.get(2).asString().c_str());
            bReply.addList() = getActionConsequenceDrives(pAction);
        }
    }

    // PRINT PDDL DOMAIN

    else if (bCommand.get(0).asString() == "printPDDLContextualKnowledgeDomain") {

        if (bCommand.size() >= 2) {
            bReply.addString("nack");
            bReply.addString("Usage : printPDDLContextualKnowledge");
        }
//.........这里部分代码省略.........

/*
* Message handler. RPC and from the terminal
*/
bool CoreModule::respond(const yarp::os::Bottle& command, yarp::os::Bottle& reply)
{
	printDebug("Message Received: (echo is on)");
	
	// debug
	printf("GOT MESSAGE: %s\n", command.toString().c_str());
	
	
	// QUIT
	if( command.get(0).asString() == "quit" ||
	    command.get(0).asString() == "exit" || 
	    command.get(0).asString() == "stop" )
	{
		isRunning = false;
		return false;
	}
	
	reply.clear();
	reply.addString("The command is not valid! Try: quit|list|add|del");	

	// nothing there
	if( command.size() < 1 ) return true;
	if( command.size() == 1 && command.get(0).asString() == "") return true;


	// LIST
	if( command.get(0).asString() == "list" || command.get(0).asString() == "ls" ){
		
		reply.clear();
		reply.addString("list");
		
		std::vector<ModuleInfo>::iterator itr;
		for ( itr = listOfModules.begin(); itr != listOfModules.end(); ++itr ) {
			std::cout << (*itr).toStdString() << std::endl;

			Bottle b;
			(*itr).toBottle(b);
			
			reply.addList() = b;
		}
		
	}
	
	// ADDING A MODULE
	if( command.get(0).asString() == "add" || command.get(0).asString() == "launch" ){
		reply.clear();		
		
		if( command.size() < 2 ) {
			reply.addString("ERROR: The syntax should be:");
			reply.addString("add <name>");	
			return true;
		} 
		
		int thisModuleID = nextModuleID++;
		
		ModuleInfo i;
		i.set(thisModuleID, command);
		listOfModules.push_back(i);
		
		reply.addString("OK");
		reply.addInt(thisModuleID);
	}

	// DELETING A MODULE
	if(command.get(0).asString() == "del" ||
	   command.get(0).asString() == "rm"  ||
	   command.get(0).asString() == "delete") {
		reply.clear();		
		
		if( command.size() < 2 ) {
			reply.addString("ERROR: The syntax should be:");
			reply.addString("del <moduleID>");	
			return true;
		} 
		
	    if( command.get(1).isInt() ) {
		   int thisModuleID = command.get(1).asInt();
		   reply.addString("OK");
		   // delete from vector
		   std::vector<ModuleInfo>::iterator itr;
		   for ( itr = listOfModules.begin(); itr != listOfModules.end(); ++itr ) {
			   if(thisModuleID == (*itr).ID) {
				   listOfModules.erase(itr);
				   break;
			   }
		   }
	    } else {
			reply.addString("ERROR: Could not parse integer! the syntax should be: del <moduleID as integer>");
		}
		
	}
	
//	if( command.get(0).asString() == "set"){
//		
//		if( command.get(1).asString() == "tgt" || command.get(1).asString() == "target" ) {
//			userSetTargetName = command.get(2).asString();
//			reply.clear();	
//.........这里部分代码省略.........