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

bool WorldRpcInterface::handler( const yarp::os::Bottle& command, yarp::os::Bottle& reply ) 
{
	int n = 0;	// identifier of the current bottle element
	int cmd;	// the command (see command vocabs in header)
	
	yarp::os::ConstString prefix = command.get(n).asString();
	if ( prefix=="help" )
	{
			reply.addVocab(yarp::os::Vocab::encode("many"));
			reply.addString("\nMoBeE world interface, arguments within brackets: \n");
			reply.addString(	"ls: list objects");
			reply.addString(	"mk sph [radius] [xpos] [ypos] [zpos]: create sphere");
			reply.addString(	"mk cyl [radius] [height] [xpos] [ypos] [zpos]: create sphere");
			reply.addString(	"mk box [xsize] [ysize] [zsize] [xpos] [ypos] [zpos]: create sphere");
			reply.addString(	"set [objectname] [xpos] [ypos] [zpos]: set object location (m)");
			reply.addString(	"def [objectname] [targ/obs]: set object class to target or obstacle");
			reply.addString(	"get [objectname]: return object state");
			reply.addString(	"rot [objectname] [xrot] [yrot] [zrot]: set object rotation (degrees)");
			reply.addString(    "rot [objectname] [1*9 rotation matrix]: set object rotation cosine matrix");
			reply.addString(	"rm [objectname]: remove object (persistent objects cannot be removed)");
			reply.addString(	"clr: remove all but persistent objects from the world, and reset object counters");
			reply.addString(	"grab [objectname] [robotname] [markername]: attach object to robot marker");
			reply.addString(	"grab [objectname] [robotname]: detach object from robot");
			reply.addString("\niCub simulator synchronization commands:");
			reply.addString(    "sim [objectname] [xpos] [ypos] [zpos] [xrot] [yrot] [zrot]: set rototranslation as returned in iCubSim coordinates");
			reply.addString(	"srun [period]: run iCubSim synchronization thread");
			reply.addString(	"sstp: stop iCubSim synchronization thread");
			reply.addString(	"sync: do one iCubSim synchronization step");
		return true;
	}
	else if ( prefix == "ls, mk (sph, cyl, box), set, def (obs/tgt), get, rot, rm, clr, grab, sim, srun, sstp, sync" ) { n++; }
	
	cmd  = command.get(n).asVocab(); n++;

	switch (cmd)
	{
		case VOCAB_LS:	getList(reply); break;
		case VOCAB_MK:	make(command,reply,n); break;
		case VOCAB_SET:	set(command,reply,n); break;
		case VOCAB_DEF: respClass(command,reply,n); break;
		case VOCAB_GET:	getState(command,reply,n); break;
		case VOCAB_ROT:	setRot(command,reply,n); break;
		case VOCAB_REM:	removeObject(command,reply,n); break;
		case VOCAB_GRAB: grabObject(command,reply,n); break;
		case VOCAB_SIM: setRTfromSim(command, reply, n); break;
		case VOCAB_SIMSYNC_RUN: startSimSyncer(command, reply, n); break;
		case VOCAB_SIMSYNC_STOP: model->getSimSyncer().stop(); reply.addString("ok"); break;
		case VOCAB_SIMSYNC_NOW: model->getSimSyncer().step(); reply.addString("ok"); break;
		case VOCAB_CLEAR: 
				printf("CLEARING THE WORLD\n");
				model->clearTheWorld();
				s = 0; c = 0; b = 0; ss = 0; sc = 0; sb = 0; // set counters to 0, for icub simulator compatibility
				reply.addString("Removed all world objects");
				printf("FINISHED CLEARING THE WORLD\n");
				break;
			
		default: reply.addString("Unknown RPC command"); return false;
	}
	return true;
}

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.");
		}

	}
}

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

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

 /**
 * 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 skinManager::bottleToVector(const yarp::os::Bottle& b, yarp::sig::Vector& v){
    for(int i=0; i<b.size(); i++)
        if(b.get(i).isDouble() || b.get(i).isInt())
            v.push_back(b.get(i).asDouble());
        else
            return false;
    return true;
}

 void YarpManager::onRead(yarp::os::Bottle& b) {     
     std::string name = b.get(0).asString().c_str();
     int value = b.get(1).asInt();
     
     if(name == "acuity") setAcuity(value);
     else if(name == "fov") setFov(value);
     else if(name == "brightness") setBrightness(value);
     else if(name == "threshold") setTreshold(value);
     else std::cout<<"Bottle message incorrect"<<std::endl;
 }

bool GaussianAE::decode(const yarp::os::Bottle &packet, size_t &pos)
{
    if (LabelledAE::decode(packet, pos) && pos + 3 <= packet.size())
    {

        _gaei[0] = packet.get(pos++).asInt();
        _gaei[1] = packet.get(pos++).asInt();
        _gaei[2] = packet.get(pos++).asInt();
        return true;
    }
    return false;
}

/* 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;
}

int merge(yarp::os::Bottle& mergeArg, folderType fType, bool verbose)
{
    ConstString contextName;
    if (mergeArg.size() >1 )
        contextName=mergeArg.get(1).asString().c_str();
    if (contextName=="")
    {
        printf("No %s name provided\n", fType==CONTEXTS ? "context" : "robot");
        return 0;
    }
    yarp::os::ResourceFinder rf;
    rf.setVerbose(verbose);

    if (mergeArg.size() >2 )
    {
        for (int i=2; i<mergeArg.size(); ++i)
        {
            ConstString fileName=mergeArg.get(i).asString();
            if(fileName != "")
            {
                ResourceFinderOptions opts;
                opts.searchLocations=ResourceFinderOptions::User;
                ConstString userFileName=rf.findPath((getFolderStringName(fType) + PATH_SEPARATOR +contextName + PATH_SEPARATOR + fileName).c_str(), opts);

                ConstString hiddenFileName=rf.findPath((getFolderStringNameHidden(fType) + PATH_SEPARATOR +contextName+ PATH_SEPARATOR + fileName).c_str(), opts);

                opts.searchLocations=ResourceFinderOptions::Installed;
                ConstString installedFileName=rf.findPath((getFolderStringName(fType) + PATH_SEPARATOR +contextName+ PATH_SEPARATOR + fileName).c_str(), opts);

                if (userFileName!="" && hiddenFileName != "" && installedFileName !="")
                    fileMerge(installedFileName, userFileName, hiddenFileName);
                else if (userFileName!=""  && installedFileName !="")
                    printf("Need to use mergetool\n");
                else
                    printf("Could not merge file %s\n", fileName.c_str());
            }
        }
    }
    else
    {
        ResourceFinderOptions opts;
        opts.searchLocations=ResourceFinderOptions::User;
        ConstString userPath=rf.findPath((getFolderStringName(fType) + PATH_SEPARATOR +contextName).c_str(), opts);

        ConstString hiddenUserPath=rf.findPath((getFolderStringNameHidden(fType) + PATH_SEPARATOR +contextName).c_str(), opts);

        opts.searchLocations=ResourceFinderOptions::Installed;
        ConstString installedPath=rf.findPath((getFolderStringName(fType) + PATH_SEPARATOR +contextName).c_str(), opts);

        recursiveMerge(installedPath, userPath, hiddenUserPath);
    }
    return 0;
}

void WorldRpcInterface::set( const yarp::os::Bottle& command, yarp::os::Bottle& reply, int& n  )
{
	KinematicModel::CompositeObject* object = getObject( command, reply, n );
	
	if ( object )
	{
		double x = command.get(n).asDouble(); n++;  //std::cout << x << std::endl; // x position
		double y = command.get(n).asDouble(); n++;  //std::cout << y << std::endl; // y position  
		double z = command.get(n).asDouble(); n++;  //std::cout << z << std::endl; // z position
		object->setPosition( QVector3D(x,y,z) );
		reply.addString("Set Cartesian position of object.");
	}
}

void Rangefinder2DInputPortProcessor::onRead(yarp::os::Bottle &b)
{
    now=SystemClock::nowSystem();
    mutex.wait();

    if (count>0)
    {
        double tmpDT=now-prev;
        deltaT+=tmpDT;
        if (tmpDT>deltaTMax)
            deltaTMax=tmpDT;
        if (tmpDT<deltaTMin)
            deltaTMin=tmpDT;

        //compare network time
        if (tmpDT*1000<LASER_TIMEOUT)
        {
            state = b.get(1).asInt();
        }
        else
        {
            state = IRangefinder2D::DEVICE_TIMEOUT;
        }
    }

    prev=now;
    count++;

    lastBottle=b;
    Stamp newStamp;
    getEnvelope(newStamp);

    //initialialization (first received data)
    if (lastStamp.isValid()==false)
    {
        lastStamp = newStamp;
    }

    //now compare timestamps
    if ((1000*(newStamp.getTime()-lastStamp.getTime()))<LASER_TIMEOUT)
    {
        state = b.get(1).asInt();
    }
    else
    {
        state = IRangefinder2D::DEVICE_TIMEOUT;
    }
    lastStamp = newStamp;

    mutex.post();
}

bool
RandomRequestHandler::processRequest(const YarpString &           request,
                                     const yarp::os::Bottle &     restOfInput,
                                     const YarpString &           senderChannel,
                                     yarp::os::ConnectionWriter * replyMechanism)
{
#if (! defined(ODL_ENABLE_LOGGING_))
# if MAC_OR_LINUX_
#  pragma unused(request,senderChannel)
# endif // MAC_OR_LINUX_
#endif // ! defined(ODL_ENABLE_LOGGING_)
    ODL_OBJENTER(); //####
    ODL_S3s("request = ", request, "restOfInput = ", restOfInput.toString(), //####
            "senderChannel = ", senderChannel); //####
    ODL_P1("replyMechanism = ", replyMechanism); //####
    bool result = true;

    try
    {
        int count;

        _response.clear();
        if (0 < restOfInput.size())
        {
            yarp::os::Value number(restOfInput.get(0));

            if (number.isInt())
            {
                count = number.asInt();
            }
            else
            {
                count = -1;
            }
        }
        else
        {
            count = 1;
        }
        if (count > 0)
        {
            for (int ii = 0; ii < count; ++ii)
            {
                _response.addDouble(yarp::os::Random::uniform());
            }
        }
        else
        {
            ODL_LOG("! (count > 0)"); //####
        }
        sendResponse(replyMechanism);
    }
    catch (...)
    {
        ODL_LOG("Exception caught"); //####
        throw;
    }
    ODL_OBJEXIT_B(result); //####
    return result;
} // RandomRequestHandler::processRequest

/*! @brief Convert a YARP list into a %JavaScript object.
 @param[in] jct The %JavaScript engine context.
 @param[in,out] theData The output object.
 @param[in] inputValue The value to be processed. */
static void
convertList(JSContext *              jct,
            JS::MutableHandleValue   theData,
            const yarp::os::Bottle & inputValue)
{
    ODL_ENTER(); //####
    ODL_P2("jct = ", jct, "inputValue = ", &inputValue); //####
    JSObject * valueArray = JS_NewArrayObject(jct, 0);

    if (valueArray)
    {
        JS::RootedObject arrayRooted(jct);
        JS::RootedValue  anElement(jct);
        JS::RootedId     aRootedId(jct);

        arrayRooted = valueArray;
        for (int ii = 0, mm = inputValue.size(); mm > ii; ++ii)
        {
            yarp::os::Value aValue(inputValue.get(ii));

            convertValue(jct, &anElement, aValue);
            if (JS_IndexToId(jct, ii, &aRootedId))
            {
                JS_SetPropertyById(jct, arrayRooted, aRootedId, anElement);
            }
        }
        theData.setObject(*valueArray);
    }
    ODL_EXIT(); //####
} // convertList

    /**
    * 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;
    }