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

void WorldRpcInterface::grabObject( const yarp::os::Bottle& command, yarp::os::Bottle& reply, int& n  )
{
	KinematicModel::CompositeObject* object = getObject(command, reply, n);
	if (!object)
		return;

	QString robotName = command.get(n).asString().c_str(); n++;
	KinematicModel::Robot *robot = model->getRobot(robotName);
	if (!robot) {
		reply.addString("Robot not found.");
		return;
	}
	reply.addString("Robot found.");

	QString markerName = command.get(n).asString().c_str(); n++;
	int markerIndex = -1;
	//KinematicModel::RobotObservation robotObs = robot->observe();
	//for (int i = 0; i<robotObs.getNumMarkers(); i++) {
	//	if (robotObs.markerName(i).compare(markerName) == 0)
	//		markerIndex = i;
	//}

	if (markerIndex < 0) {
		reply.addString("Marker not found, releasing object.");
	//	return;
	} else
		reply.addString("Marker found, grabbing object.");
 
	model->grabObject(object, robot, markerIndex);

	//printf("Grabbing object \"%s\" with marker \"%s\" on robot \"%s\"\n", object->getName().toStdString().c_str(), markerName.toStdString().c_str(), robotName.toStdString().c_str());
}

void WorldRpcInterface::setRot( const yarp::os::Bottle& command, yarp::os::Bottle& reply, int& n  )
{
	KinematicModel::CompositeObject* object = getObject( command, reply, n );
	
	if ( object )
	{
		if ((command.size() - n) == 3) {
			double x = command.get(n).asDouble()*M_PI/180.; n++;  //std::cout << x << std::endl; // x position
			double y = command.get(n).asDouble()*M_PI/180.; n++;  //std::cout << y << std::endl; // y position  
			double z = command.get(n).asDouble()*M_PI/180.; n++;  //std::cout << z << std::endl; // z position
			object->setCartesianOrientation( QVector3D(x,y,z) );
			reply.addString("Set rotation (about x,y,z in degrees).");
		} else {
			// replace rotation part of object's T matrix
			QMatrix4x4 rt = object->getT();
			for (int i = 0; i<3; i++) {
				for (int j = 0; j<3; j++) {
					rt(i, j) = command.get(n).asDouble(); n++;
				}
			}
			object->setT(rt);
			reply.addString("Set full rotation matrix.");
		}

	}
}

    /**
    * Parser for string commands. It is called by virtual bool respond().
    * @param command the bottle containing the user command
    * @param reply the bottle which will be returned to the RPC client
    * @return true if the command was successfully parsed
    */
    bool parse_respond_string(const yarp::os::Bottle& command, yarp::os::Bottle& reply)
    {
        if (command.get(0).isString() && command.get(0).asString() == "getLoc")
        {
            std::string s = std::string("Current Location is: ") + m_localization_data.toString();
            reply.addString(s);
        }

        else if (command.get(0).isString() && command.get(0).asString() == "initLoc")
        {
            yarp::dev::Map2DLocation loc;
            loc.map_id = command.get(1).asString();
            loc.x = command.get(2).asDouble();
            loc.y = command.get(3).asDouble();
            loc.theta = command.get(4).asDouble();
            initializeLocalization(loc);
            std::string s = std::string("Localization initialized to: ") + loc.toString();
            reply.addString(s);
        }
        else
        {
            reply.addString("Unknown command.");
        }
        return true;
    }

bool GuiUpdaterModule::respond(const yarp::os::Bottle& command, yarp::os::Bottle& reply)
{  
    string helpMessage =  string(getName().c_str()) + 
        " commands are: \n" +
        "reset \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"<<endl;
        resetGUI();
        reply.addString("ok");
    }
    return true;
}

void WorldRpcInterface::respClass( const yarp::os::Bottle& command, yarp::os::Bottle& reply, int& n  )
{
	KinematicModel::CompositeObject* object = getObject( command, reply, n );
	
	if ( object )
	{
		//model->clearWorldObject(object);
        model->removeWorldObject(object);

		int type = command.get(n).asVocab();
		QColor collidingColor,freeColor;
		switch (type)
		{
			case VOCAB_OBSTACLE:
				
				freeColor = Qt::blue;
				freeColor = freeColor.lighter();

				collidingColor = freeColor;
                
                freeColor.setAlphaF(0.5);
				collidingColor.setAlphaF(0.5);
				
				object->setResponseClass(model->OBSTACLE());
				object->setFreeColor( freeColor );
				object->setCollidingColor( collidingColor );
				
				reply.addString("Changed object type to 'obstacle'.");
				
				break;
				
			case VOCAB_TARGET:
				
				freeColor = Qt::green;
				freeColor = freeColor.lighter();
				collidingColor = freeColor;
				
				freeColor.setAlphaF(1.0);
				collidingColor.setAlphaF(0.5);
				
				object->setResponseClass(model->TARGET());
				object->setFreeColor( freeColor );
				object->setCollidingColor( collidingColor );
	
				reply.addString("Changed object type to 'target'.");
				
				break;
				
			default:
				
				reply.addString("Unknown definition, use 'obs' or 'tgt'.");
		}
		
		model->appendObject( object );
	}
}

void WorldRpcInterface::startSimSyncer(const yarp::os::Bottle& command, yarp::os::Bottle& reply, int& n) {
	if ((command.size() - n) != 1) {
		reply.addString("Please provide the refresh period for the synchronization thread in seconds");
		return;
	}
	double period = command.get(n).asDouble(); n++;
	if (model->getSimSyncer().isRunning()) {
		model->getSimSyncer().stop();
	}
	model->getSimSyncer().setRefreshPeriod(period);
	model->getSimSyncer().start();
	reply.addString("ok");
}

 /**
 * Parser for VOCAB commands. It is called by virtual bool respond().
 * @param command the bottle containing the user command
 * @param reply the bottle which will be returned to the RPC client
 * @return true if the command was successfully parsed
 */
 bool parse_respond_vocab(const yarp::os::Bottle& command, yarp::os::Bottle& reply)
 {
     int request = command.get(1).asVocab();
     if (request == VOCAB_NAV_GET_CURRENT_POS)
     {
         //plannerThread->setNewAbsTarget(loc);
         reply.addVocab(VOCAB_OK);
         reply.addString(m_localization_data.map_id);
         reply.addDouble(m_localization_data.x);
         reply.addDouble(m_localization_data.y);
         reply.addDouble(m_localization_data.theta);
     }
     else if (request == VOCAB_NAV_SET_INITIAL_POS)
     {
         yarp::dev::Map2DLocation loc;
         loc.map_id = command.get(2).asString();
         loc.x = command.get(3).asDouble();
         loc.y = command.get(4).asDouble();
         loc.theta = command.get(5).asDouble();
         initializeLocalization(loc);
         reply.addVocab(VOCAB_OK);
     }
     else
     {
         reply.addVocab(VOCAB_ERR);
     }
     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
ExtraArgumentDescriptor::addValueToBottle(yarp::os::Bottle & container)
{
    ODL_ENTER(); //####
    ODL_P1("container = ", &container); //####
    container.addString(getProcessedValue());
    ODL_EXIT(); //####
} // ExtraArgumentDescriptor::addValueToBottle

void WorldRpcInterface::removeObject( const yarp::os::Bottle& command, yarp::os::Bottle& reply, int& n  )
{
	KinematicModel::CompositeObject* object = getObject( command, reply, n );
	if ( object )
	{ 
		object->kill();
		reply.addString("Removed object");
	}
}

void WorldRpcInterface::getList(yarp::os::Bottle& reply)
{
	//printf("called getList\n");
	QVector<QString>::iterator i;
	QVector<QString> list = model->listWorldObjects();
	for ( i=list.begin(); i!=list.end(); ++i )
	{
		reply.addString( (*i).toStdString().c_str() );
    }
}

 virtual bool respond(const yarp::os::Bottle &command, yarp::os::Bottle &reply)
 {
     reply.clear(); 
     if (command.get(0).isString())
     {
         if (command.get(0).asString()=="help")
         {
             reply.addVocab(Vocab::encode("many"));
             reply.addString("Available commands:");
             reply.addString("currently nothing");
             return true;
         }
         else if (command.get(0).asString()=="***")
         {
             return true;
         }
     }
     reply.addString("Unknown command");
     return true;
 }

  virtual bool cmdList(yarp::os::Bottle& cmd, 
		       yarp::os::Bottle& reply,
		       yarp::os::Contact& remote) {
    reply.addString("old");
    for (map<string,Entry>::iterator it = names.begin(); 
	 it!=names.end();
	 it++) {
      appendEntry(reply,it->second);
    }
    return true;
  }

 virtual bool cmdQuery(yarp::os::Bottle& cmd,
                       yarp::os::Bottle& reply,
                       yarp::os::Contact& remote) {
   reply.addString("old");
   ConstString name = cmd.get(1).asString();
   Contact c = Network::queryName(name);
   if (c.isValid()) {
     appendEntry(reply,c);
   }
   return true;
 }

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