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());
}
Beispiel #4
0
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;
}
Beispiel #5
0
 /**
 * 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;
 }
Beispiel #6
0
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;
}
Beispiel #7
0
 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;
}
Beispiel #9
0
/* 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.");
	}
}
Beispiel #12
0
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
Beispiel #15
0
    /**
    * 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;
    }
Beispiel #16
0
void wysiwyd::wrdac::SubSystem_Reactable::SendOSC(yarp::os::Bottle &oscMsg)
{
    yarp::os::Bottle cmd;
    cmd.addString("osc");
    for(int i=0; i<oscMsg.size(); i++)
        cmd.add(oscMsg.get(i));
    std::cout<<"OSC>>"<<cmd.toString().c_str()<<std::endl;
    portRTrpc.write(cmd);
}
Beispiel #17
0
  virtual bool cmdRegister(yarp::os::Bottle& cmd, 
			   yarp::os::Bottle& reply,
			   yarp::os::Contact& remote) {
    ConstString name = cmd.get(1).asString();
    ConstString carrier = cmd.get(2).asString();
    ConstString machine = cmd.get(3).asString();
    int number = cmd.get(4).asInt();
    if (name=="...") {
      name = "/tmp/";
      for (int i=0; i<20; i++) {
	double x = Rand::scalar('a','z'+1);
	name = name + (char)x;
      }
    }
    if (carrier==""||carrier=="...") {
      carrier = "tcp";
    }
    if (machine==""||machine=="...") {
      machine = remote.getHost();
    }
    if (number==0) {
      number = allocatePortNumber();
      if (number==0) {
	printf("Out of port numbers!\n");
      } else {
	lastNumber = number;
      }
    }
    if (numbers.find(number)==numbers.end()) {
      numbers[number] = 1;
    } else {
      numbers[number]++;
    }
    Entry entry;
    entry.name = name.c_str();
    entry.carrier = carrier.c_str();
    entry.machine = machine.c_str();
    entry.portNumber = number;
    names[name.c_str()] = entry;
    Bottle cmd2;
    cmd2.add("query");
    cmd2.add(name.c_str());
    return cmdQuery(cmd2,reply,remote);
  }
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 objectGeneratorSim::createObject(std::string ob, yarp::os::Bottle size, yarp::os::Bottle pos, yarp::os::Bottle colour, bool target)
{
    yarp::os::Bottle pair;
    pair.clear();

    cmd.clear();
    cmd.addString("world");
    cmd.addString("mk");
    cmd.addString(ob.c_str());
    
    pair.addString(ob);
    
    if(ob=="box"){    
        cmd.addDouble(size.get(0).asDouble());
        cmd.addDouble(size.get(1).asDouble());
        cmd.addDouble(size.get(2).asDouble());
        listSize[0] +=1;
        pair.addInt(listSize[0]);
    }else if(ob == "sph"){
        cmd.addDouble(size.get(0).asDouble());
        listSize[1] +=1;
        pair.addInt(listSize[1]);
    }else if (ob == "cyl"){
        cmd.addDouble(size.get(0).asDouble());
        listSize[2] +=1;
        pair.addInt(listSize[2]);
    }else{
        cout<<"nothing created!!!!"<<endl;
    }

    cmd.addDouble(pos.get(0).asDouble());
    cmd.addDouble(pos.get(1).asDouble());
    cmd.addDouble(pos.get(2).asDouble());
    
    cmd.addDouble(colour.get(0).asDouble());
    cmd.addDouble(colour.get(1).asDouble());
    cmd.addDouble(colour.get(2).asDouble());
    portSim.write(cmd);
    //increase ob list and add ob to the list
    //add radius
    pair.addDouble(size.get(0).asDouble());
    if (target==true){
        targets.append(pair);
    }else{
        objects.append(pair);
    }
}
Beispiel #20
0
/**
 * Handles command line commands to interact with the motors and run procedures
 * @return true unless quit is called
 */
bool SarsaLearner::respond(const yarp::os::Bottle& command, yarp::os::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");
    }

    return true;
}
Beispiel #21
0
 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;
 }
Beispiel #22
0
yarp::sig::Vector iCub::skinDynLib::vectorFromBottle(const yarp::os::Bottle b, int in, const int size)
{
    yarp::sig::Vector v(size,0.0);

    for (int i = 0; i < size; i++)
    {
        v[i] = b.get(in).asDouble();
        in++;
    }
    return v;
}
Beispiel #23
0
 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;
 }
Beispiel #24
0
    void findFileBase(Property& config, const char *name,
                      bool isDir,
                      Bottle& output, bool justTop) {

        ConstString cap =
            config.check("capability_directory",Value("app")).asString();
        Bottle defCaps =
            config.findGroup("default_capability").tail();

        // check current directory
		if (ConstString(name)==""&&isDir) {
            output.addString(getPwd());
            if (justTop) return;
        }
        ConstString str = check(getPwd(),"","",name,isDir);
        if (str!="") {
            output.addString(str);
            if (justTop) return;
        }

        if (configFilePath!="") {
            ConstString str = check(configFilePath.c_str(),"","",name,isDir);
            if (str!="") {
                output.addString(str);
                if (justTop) return;
            }
        }

        // check app dirs
        for (int i=0; i<apps.size(); i++) {
            str = check(root.c_str(),cap,apps.get(i).asString().c_str(),
                        name,isDir);
            if (str!="") {
                output.addString(str);
                if (justTop) return;
            }
        }

        // check ROOT/app/default/
        for (int i=0; i<defCaps.size(); i++) {
            str = check(root.c_str(),cap,defCaps.get(i).asString().c_str(),
                        name,isDir);
            if (str!="") {
                output.addString(str);
                if (justTop) return;
            }
        }

        if (justTop) {
            if (!quiet) {
                fprintf(RTARGET,"||| did not find %s\n", name);
            }
        }
    }
bool
TruncateFloatFilterInputHandler::handleInput(const yarp::os::Bottle &     input,
                                             const YarpString &           senderChannel,
                                             yarp::os::ConnectionWriter * replyMechanism,
                                             const size_t                 numBytes)
{
#if (! defined(ODL_ENABLE_LOGGING_))
# if MAC_OR_LINUX_
#  pragma unused(senderChannel,replyMechanism,numBytes)
# endif // MAC_OR_LINUX_
#endif // ! defined(ODL_ENABLE_LOGGING_)
    ODL_OBJENTER(); //####
    ODL_S2s("senderChannel = ", senderChannel, "got ", input.toString()); //####
    ODL_P1("replyMechanism = ", replyMechanism); //####
    ODL_I1("numBytes = ", numBytes); //####
    bool result = true;

    try
    {
        yarp::os::Bottle outBottle;

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

            if (aValue.isInt())
            {
                outBottle.addInt(aValue.asInt());
            }
            else if (aValue.isDouble())
            {
                outBottle.addInt(static_cast<int>(aValue.asDouble()));
            }
        }
        if ((0 < outBottle.size()) && _outChannel)
        {
            if (! _outChannel->write(outBottle))
            {
                ODL_LOG("(! _outChannel->write(message))"); //####
#if defined(MpM_StallOnSendProblem)
                Stall();
#endif // defined(MpM_StallOnSendProblem)
            }
        }
    }
    catch (...)
    {
        ODL_LOG("Exception caught"); //####
        throw;
    }
    ODL_OBJEXIT_B(result); //####
    return result;
} // TruncateFloatFilterInputHandler::handleInput
bool WorldRpcInterface::parseSimRTBottle(const std::string name, const yarp::os::Bottle& command, int& n, QMatrix4x4 &rt) {
	if ( (command.size()-n) != 6) {
		return false;
	}

	//position
	double px = command.get(n).asDouble(); n++;
	double py = command.get(n).asDouble(); n++;
	double pz = command.get(n).asDouble(); n++;
		
	// rotation
	double rx = command.get(n).asDouble(); n++;
	double ry = command.get(n).asDouble(); n++;
	double rz = command.get(n).asDouble(); n++;
		
	// special simulator rotation:
	QQuaternion qrx = QQuaternion::fromAxisAndAngle( QVector3D(1, 0, 0), -rz);
	QQuaternion qry = QQuaternion::fromAxisAndAngle( QVector3D(0, 1, 0), -rx);
	QQuaternion qrz = QQuaternion::fromAxisAndAngle( QVector3D(0, 0, 1), ry);
	rt.setToIdentity();
	rt.rotate(qry * (qrz*qrx));

	// cylinders are rotated 90 degrees on the z-axis with respect to iCubSIM:
	QRegExp rxtype("([^\\d]+)(?:\\s*\\d+)"); // check for: {[one or more non-numeric characters]: return as part 1} {[zero or more whitespace characters followed by one or more numeric characters]: do not return}
	int pos = rxtype.indexIn(QString(name.c_str()));
	if (pos > -1) {
		yarp::os::Value type(rxtype.cap(1).toStdString().c_str());
		int vtype = type.asVocab();
		if (vtype == VOCAB_CYL || vtype == VOCAB_SCYL) {
			rt.rotate( QQuaternion::fromAxisAndAngle( QVector3D(0, 0, 1), 90));
		}
	}
	
	// position translation
	rt(0, 3) = -(pz + 0.026);
	rt(1, 3) = -px;
	rt(2, 3) = py-0.5976;

	return true;
}
Beispiel #27
0
  virtual bool cmdQuery(yarp::os::Bottle& cmd, 
			yarp::os::Bottle& reply,
			yarp::os::Contact& remote) {
    reply.addString("old");
    string name = cmd.get(1).asString().c_str();    
    map<string,Entry>::iterator it = names.find(name);
    if (it==names.end()) {
      return true;
    }
    Entry& e = it->second;
    appendEntry(reply,e);
    return true;
  }
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");
}
Beispiel #29
0
bool AnalogServerHandler::_handleIAnalog(yarp::os::Bottle &cmd, yarp::os::Bottle &reply)
{
    yTrace();
    if (is==0)
      return false;

    int msgsize=cmd.size();

    int code=cmd.get(1).asVocab();
    switch (code)
    {
    case VOCAB_CALIBRATE:
      if (msgsize==2)
        is->calibrateSensor();
      else
      {
        //read Vector of values and pass to is->calibrate();
      }
      return true;
      break;
    case VOCAB_CALIBRATE_CHANNEL:
      if (msgsize==3)
      {
        int ch=cmd.get(2).asInt();
        is->calibrateChannel(ch);
      }
      if (msgsize==4)
      {
        int ch=cmd.get(2).asInt();
        double v=cmd.get(3).asDouble();
        is->calibrateChannel(ch, v);
      }

      return true;
      break;
    default:
      return false;
    }
}
Beispiel #30
0
std::string WireTwiddler::fromTemplate(const yarp::os::Bottle& msg) {
    string result = "";

    // assume we want to remove any meta-information

    int len = msg.size();

    int code = -1;
    for (int i=0; i<len; i++) {
        Value&v = msg.get(i);
        int icode = v.getCode();
        if (i==0) code = icode;
        if (icode!=code) code = -1;
    }
    string codeName = nameThatCode(code);
    if (code == -1) {
        result += "list ";
    } else {
        result += "vector ";
        result += codeName;
        result += " ";
    }
    result += NetType::toString(len).c_str();
    result += " ";
    for (int i=0; i<len; i++) {
        Value&v = msg.get(i);
        if (!v.isList()) {
            if (code == -1) {
                result += nameThatCode(v.getCode());
                result += " ";
            }
            result += "* ";
        } else {
            result += fromTemplate(*v.asList());
        }
    }
    return result;
}