Пример #1
0
bool yarp::dev::ServerInertial::getInertial(yarp::os::Bottle &bot)
{
    if (IMU==NULL)
    {
        return false;
    }
    else
    {
        int nchannels;
        IMU->getChannels (&nchannels);

        yarp::sig::Vector indata(nchannels);
        bool worked(false);

        worked=IMU->read(indata);
        if (worked)
        {
            bot.clear();

            // Euler+accel+gyro+magn orientation values
            for (int i = 0; i < nchannels; i++)
                bot.addDouble (indata[i]);
        }
        else
        {
            bot.clear(); //dummy info.
        }

        return(worked);
    }
}
Пример #2
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;
}
Пример #3
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;
}
Пример #4
0
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;
	
	
}
Пример #5
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;
 }
Пример #6
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;
}
Пример #7
0
 /**
 * Parser for user command received from the RPC port
 * @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
 */
 virtual bool respond(const yarp::os::Bottle& command,yarp::os::Bottle& reply) 
 {
     yarp::os::LockGuard lock(m_mutex);
     reply.clear(); 
     //parser for VOCAB  commands
     if (command.get(0).isVocab())
     {
         if(command.get(0).asVocab() == VOCAB_INAVIGATION && command.get(1).isVocab())
         {
             parse_respond_vocab(command,reply);
         }
         else
         {
             yError() << "Invalid vocab received";
             reply.addVocab(VOCAB_ERR);
         }
     }
     //parser for string commands
     else if (command.get(0).isString())
     {
         if (command.get(0).asString()=="help")
         {
             reply.addVocab(Vocab::encode("many"));
             reply.addString("Available commands are:");
             reply.addString("getLoc");
             reply.addString("initLoc <map_name> <x> <y> <angle in degrees>");
         }
         else if (command.get(0).isString())
         {
             parse_respond_string(command, reply);
         }
     }
     //unknown/invalid command received
     else
     {
         yError() << "Invalid command type";
         reply.addVocab(VOCAB_ERR);
     }
     return true;
 }
Пример #8
0
/* Respond function */
bool babbler::respond(const yarp::os::Bottle& bCommand, yarp::os::Bottle& bReply)
{

    std::string helpMessage = std::string(getName().c_str()) +
        " commands are: \n" +
        "help \n" +
        "F + (int)Frequency\n" +
        "RF \n" +
        "quit \n";

    bReply.clear();
    std::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 == "F") {
        if (bCommand.size() == 2)
        {
            if (bCommand.get(1).isInt())
            {
                setNewFrequency(bCommand.get(1).asInt());
                bReply.addInt(f);
            }
        }
    }
    else if (keyWord == "RF") {    
                bReply.addInt(newRandomFrequency());
    }

    return true;
}
Пример #9
0
  virtual bool apply(yarp::os::Bottle& cmd, 
		     yarp::os::Bottle& reply, 
		     yarp::os::Bottle& event,
		     yarp::os::Contact& remote) {
    bool ok = false;
    mutex.wait();
    printf(" + %s\n", cmd.toString().c_str());
    reply.clear();
    ConstString tag = cmd.get(0).asString();
    if (tag=="register") {
      ok = cmdRegister(cmd,reply,remote);
    } else if (tag=="unregister") {
      ok = cmdUnregister(cmd,reply,remote);
    } else if (tag=="query") {
      ok = cmdQuery(cmd,reply,remote);
    } else if (tag=="list") {
      ok = cmdList(cmd,reply,remote);
    } else {
      reply.addString("old");
      reply.addString("I have no idea what you are talking about");
    }
    mutex.post();
    return ok;
  }
Пример #10
0
 virtual bool respond(const yarp::os::Bottle& command, 
                      yarp::os::Bottle& reply) {
     mutex.wait();
     switch (command.get(0).asVocab()) {
     case VOCAB3('a','d','d'):
         {
             string name = command.get(1).asString().c_str();
             if (name!="") {
                 removeName(name.c_str());
                 Entry entry;
                 entry.name = name;
                 q.push_back(entry);
                 reply.clear();
                 reply.add(Value::makeVocab("add"));
                 reply.addString(name.c_str());
                 addQueue(reply);
             }
         }
         break;
     case VOCAB3('d','e','l'):
         {
             if (command.get(1).isInt()) {
                 int idx = command.get(1).asInt();
                 bool acted = removeName(idx);
                 if (acted) {
                     reply.clear();
                     reply.add(Value::makeVocab("del"));
                     reply.addInt(idx);
                 } else {
                     reply.clear();
                     reply.add(Value::makeVocab("no"));
                     reply.addInt(idx);
                 }
                 addQueue(reply);
             } else {
                 string name = command.get(1).asString().c_str();
                 if (name!="") {
                     bool acted = removeName(name.c_str());
                     if (acted) {
                         reply.clear();
                         reply.add(Value::makeVocab("del"));
                         reply.addString(name.c_str());
                     } else {
                         reply.clear();
                         reply.add(Value::makeVocab("no"));
                         reply.addString(name.c_str());
                     }
                     addQueue(reply);
                 }
             } 
         }
         break;
     case VOCAB4('l','i','s','t'):
         {
             reply.clear();
             addQueue(reply);
         }
         break;
     default:
         updateHelp();
         mutex.post();
         return DeviceResponder::respond(command,reply);
     }
     mutex.post();
     printf("%s\n", reply.toString().c_str());
     return true;
 }
Пример #11
0
/*
* 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();	
//			if( userSetTargetName == "" ) {
//				reply.addString("using latest object in the world as target!");
//			} else {
//				reply.addString(userSetTargetName.c_str());					
//				reply.addString(" is now the target!");	
//			}
//
//
//		} else 
//			
//		if( command.get(1).asString() == "offset" ) {
//			if(reach) {
//				if( command.get(2).isDouble() ) {
//					reach->setOffset( command.get(2).asDouble() );
//					reply.clear();
//					reply.addString("OK");			
//				} else {
//					reply.clear();
//					reply.addString("ERROR: Could not parse double! the syntax should be: set offset <double>");	
//				}
//			} else {
//				reply.clear();
//				reply.addString("ReachingWorker not ready yet!");	
//			}
//		} else 
//		
//		if( command.get(1).asString() == "policy" ) {
//			if(reach) {
//				
//				if( command.get(2).asString() == "above" ) {
//					reach->setPolicy( ReachingWorker::FROM_ABOVE | ReachingWorker::STRAIGHT );
//					reply.clear();
//					reply.addString("OK");
//					
//				} else if( command.get(2).asString() == "below" ) {
//					reach->setPolicy( ReachingWorker::FROM_BELOW | ReachingWorker::STRAIGHT );
//					reply.clear();
//					reply.addString("OK");	
//					
//				} else if( command.get(2).asString() == "left" ) {
//					reach->setPolicy( ReachingWorker::FROM_LEFT  | ReachingWorker::STRAIGHT );
//					reply.clear();
//					reply.addString("OK");	
//					
//				} else if( command.get(2).asString() == "right" ) {
//					reach->setPolicy( ReachingWorker::FROM_RIGHT | ReachingWorker::STRAIGHT );
//					reply.clear();
//					reply.addString("OK");	
//					
//				} else {
//					reply.clear();
//					reply.addString("ERROR: Could not parse policy! the syntax should be: set policy <above|below|left|right>");	
//				}
//			} else {
//				reply.clear();
//				reply.addString("ReachingWorker not ready yet!");	
//			}
//		} else {
//			
//			// not a correct command?!
//			reply.clear();
//			reply.addString("Could not parse command try set [tgt|policy|offset]!");	
//		}

	return true;
}
Пример #12
0
 bool clearAppNames() {
     apps.clear();
     return true;
 }
Пример #13
0
    virtual bool respond(const yarp::os::Bottle &command, yarp::os::Bottle &reply)
    {
        reply.clear(); 
        yDebug("receiving command from port\n");
        int index = 0;
        int cmdSize = command.size();
    
        while(cmdSize>0)
            {
                switch(command.get(index).asVocab())
                {
                    case  VOCAB4('s','u','s','p'):
                        reply.addVocab(Vocab::encode("ack"));
                        vc->halt();
                        cmdSize--;
                        index++;
                    break;
                    case VOCAB3('r','u','n'):
                        reply.addVocab(Vocab::encode("ack"));
                        vc->go();
                        cmdSize--;
                        index++;
                    break;
                    //this set current position reference
                    case VOCAB3('s','e','t'):
                        if (command.size()>=3)
                            {
                                int i=command.get(index+1).asInt();
                                double pos=command.get(index+2).asDouble();
                                vc->setRef(i, pos);
                                index +=3;
                                cmdSize-=3;
                            }
                        else
                            {
                                cmdSize--;
                                index++;
                                yError("Invalid set message, ignoring\n");
                            }
                        reply.addVocab(Vocab::encode("ack"));
                    break;
                    //this set maximum velocity (limiter)
                    case VOCAB4('s','v','e','l'):
                        if(command.size()>=3)
                            {
                                int i=command.get(index+1).asInt();
                                double vel = command.get(index+2).asDouble();
                                vc->setVel(i,vel);
                                index += 3;
                                cmdSize-=3;;
                                reply.addVocab(Vocab::encode("ack"));
                            }
                        else
                            {
                                cmdSize--;
                                index++;
                                yError("Invalid set vel message, ignoring\n");
                                reply.addVocab(Vocab::encode("fail"));
                            }
                    break;
                    case VOCAB4('g','a','i','n'):
                        if(command.size()>=3)
                            {
                                int i=command.get(index+1).asInt();
                                double gain = command.get(index+2).asDouble();
                                vc->setGain(i,gain);
                                index+=3;
                                cmdSize-=3;
                                reply.addVocab(Vocab::encode("ack"));
                            }
                        else
                            {
                                cmdSize--;
                                index++;
                                yError("Invalid set gain message, ignoring\n");
                                reply.addVocab(Vocab::encode("fail"));
                            }
                    break;
                    case VOCAB4('h','e','l','p'):
                        fprintf(stdout,"VelocityControl module, valid commands are:\n");
                        fprintf(stdout,"-   [susp]         suspend the controller (command zero velocity)\n");
                        fprintf(stdout,"-   [run]          start (and resume after being suspended) the controller\n");
                        fprintf(stdout,"-   [quit]         quit the module (exit)\n");
                        fprintf(stdout,"-   [set]  <j> <p> move joint j to p (degrees)\n");
                        fprintf(stdout,"-   [svel] <j> <v> set maximum speed for joint j to v (deg/sec)\n");
                        fprintf(stdout,"-   [gain] <j> <k> set P gain for joint j to k\n");
                        fprintf(stdout,"-   [help] to get this help\n");
                        fprintf(stdout,"\n typical commands:\n gain 0 10\n svel 0 10\n run\n set 0 x\n\n");
                        
                        reply.addVocab(Vocab::encode("many"));
                        reply.addVocab(Vocab::encode("ack"));
                        reply.addString("VelocityControl module, valid commands are:");
                        reply.addString("-   [susp]         suspend the controller (command zero velocity)");
                        reply.addString("-   [run]          start (and resume after being suspended) the controller");
                        reply.addString("-   [quit]         quit the module (exit)");
                        reply.addString("-   [set]  <j> <p> move joint j to p (degrees)");
                        reply.addString("-   [svel] <j> <v> set maximum speed for joint j to v (deg/sec)");
                        reply.addString("-   [gain] <j> <k> set P gain for joint j to k");
                        reply.addString("-   [help] to get this help");
                        reply.addString("\n typical commands:\n gain 0 10\n svel 0 10\n run\n set 0 x\n\n");
                    break;
                    default:
                        yError("Invalid command, ignoring\n");
                        reply.addVocab(Vocab::encode("fail"));
                        //cmdSize--;
                        //index++;
                        //return respond(command, reply); // call default
                    break;
                }
                return true;
            }

        return false;
    }
Пример #14
0
// Callback handler for RPC commands (?)
bool RpcMsgHandler::respond(const yarp::os::Bottle& cmd, yarp::os::Bottle& reply)
{
    bool ok = true;
    bool commandUnderstood = true; // is the command recognized?

    if (caller->verbose())
        printf("command received: %s\n", cmd.toString().c_str());

    int code = cmd.get(0).asVocab();

    switch (code)
    {
        case VOCAB_GET:
        {
            double dtmp  = 0.0;
            double dtmp2 = 0.0;
            reply.addVocab(VOCAB_IS);
            reply.add(cmd.get(1));

            switch(cmd.get(1).asVocab())
            {
                case VOCAB_AXES:
                {
                    int axes;
                    caller->getAxes(axes);
                    reply.addInt(axes);
                }
                break;

                case VOCAB_DEBUG_PARAMETER:
                {
                    int j     = cmd.get(2).asInt();
                    int index = cmd.get(3).asInt();
                    ok = caller->getDebugParameter(j, index, &dtmp);
                    reply.addInt(j);
                    reply.addInt(index);
                    reply.addDouble(dtmp);
                }
                break;

                case VOCAB_GENERIC_PARAMETER:
                {
                    int j     = cmd.get(2).asInt();
                    int param = cmd.get(3).asInt();
                    ok = caller->getParameter(j, param, &dtmp);
                    reply.addInt(j);
                    reply.addInt(param);
                    reply.addDouble(dtmp);
                }
                break;

                default:
                {
                    commandUnderstood = false;
                    std::cout << "Debug Interface 1: command not understood! received " << cmd.toString().c_str() << std::endl;
                }
                break;
            }
        }
        break;      // case VOCAB_GET

        case VOCAB_SET:
        {
            switch(cmd.get(1).asVocab())
            {
                case VOCAB_GENERIC_PARAMETER:
                {
                    int j     = cmd.get(2).asInt();
                    int param = cmd.get(3).asInt();
                    double val   = cmd.get(4).asDouble();
                    ok = caller->setParameter(j, param, val);
                }
                break;

                case VOCAB_DEBUG_PARAMETER:
                {
                    int j     = cmd.get(2).asInt();
                    int index = cmd.get(3).asInt();
                    double val   = cmd.get(4).asDouble();
                    ok = caller->setDebugParameter(j, index, val);
                }
                break;

                default:
                {
                    commandUnderstood = false;
                    std::cout << "Debug Interface 2: command not understood! received " << cmd.toString().c_str() << std::endl;
                }
                break;
            }
        }
        break;      // case VOCAB_SET

        default:
        {
            commandUnderstood = false;
            std::cout << "Debug Interface 3: command not understood! received " << cmd.toString().c_str() << std::endl;
        }
        break;

    } //switch code

    if (!commandUnderstood)
    {
        ok = DeviceResponder::respond(cmd,reply);
    }


    if (!ok)
    {
        // failed thus send only a VOCAB back.
        reply.clear();
        reply.addVocab(VOCAB_FAILED);
    }
    else
        reply.addVocab(VOCAB_OK);

    return ok;
}
Пример #15
0
    virtual bool respond(const yarp::os::Bottle& command,yarp::os::Bottle& reply) 
    {
        reply.clear(); 

        gotoThread->mutex.wait();
        if (command.get(0).asString()=="quit")
        {
            gotoThread->mutex.post();
            return false;
        }

        else if (command.get(0).asString()=="help")
        {
            reply.addVocab(Vocab::encode("many"));
            reply.addString("Available commands are:");
            reply.addString("gotoAbs <x> <y> <angle>");
            reply.addString("gotoRel <x> <y> <angle>");
            reply.addString("stop");
            reply.addString("pause");
            reply.addString("resume");
            reply.addString("quit");
            reply.addString("set linear_tol <m>");
            reply.addString("set linear_ang <deg>");
            reply.addString("set max_lin_speed <m/s>");
            reply.addString("set max_ang_speed <deg/s>");
            reply.addString("set min_lin_speed <m/s>");
            reply.addString("set min_ang_speed <deg/s>");
            reply.addString("set obstacle_stop");
            reply.addString("set obstacle_avoidance");
        }

        else if (command.get(0).asString()=="gotoAbs")
        {
            yarp::sig::Vector v;
            v.push_back(command.get(1).asDouble());
            v.push_back(command.get(2).asDouble());
            if (command.size()==4) v.push_back(command.get(3).asDouble());
            gotoThread->setNewAbsTarget(v);
            reply.addString("new absolute target received");
        }

        else if (command.get(0).asString()=="gotoRel")
        {
            yarp::sig::Vector v;
            v.push_back(command.get(1).asDouble());
            v.push_back(command.get(2).asDouble());
            if (command.size()==4) v.push_back(command.get(3).asDouble());
            gotoThread->setNewRelTarget(v);
            reply.addString("new relative target received");
        }
        else if (command.get(0).asString()=="set")
        {
            if (command.get(1).asString()=="linear_tol")
            {
                gotoThread->goal_tolerance_lin=command.get(2).asDouble();
                reply.addString("linear_tol set.");
            }
            else if (command.get(1).asString()=="angular_tol")
            {
                gotoThread->goal_tolerance_ang=command.get(2).asDouble();
                reply.addString("angular_tol set.");
            }
            else if (command.get(1).asString()=="max_lin_speed")
            {
                gotoThread->max_lin_speed=command.get(2).asDouble();
                reply.addString("max_lin_speed set.");
            }
            else if (command.get(1).asString()=="max_ang_speed")
            {
                gotoThread->max_ang_speed=command.get(2).asDouble();
                reply.addString("max_ang_speed set.");
            }
            else if (command.get(1).asString()=="min_lin_speed")
            {
                gotoThread->min_lin_speed=command.get(2).asDouble();
                reply.addString("min_lin_speed set.");
            }
            else if (command.get(1).asString()=="min_ang_speed")
            {
                gotoThread->min_ang_speed=command.get(2).asDouble();
                reply.addString("min_ang_speed set.");
            }
            else if (command.get(1).asString()=="obstacle_avoidance")
            {
                if (gotoThread->enable_obstacles_avoidance)
                    {
                        reply.addString("enable_obstacles_avoidance=false");
                        gotoThread->enable_obstacles_avoidance=false;
                    }
                else
                    {
                        gotoThread->enable_obstacles_avoidance=true;
                        reply.addString("enable_obstacles_avoidance=true");
                    }
            }
            else if (command.get(1).asString()=="obstacle_stop")
            {
                if (gotoThread->enable_obstacles_avoidance)
                    {
                        reply.addString("enable_obstacle_stop=false");
                        gotoThread->enable_obstacles_emergency_stop=false;
                    }
                else
                    {
                        gotoThread->enable_obstacles_emergency_stop=true;
                        reply.addString("enable_obstacle_stop=true");
                    }
            }
            else
            {
                reply.addString("Unknown set.");
            }
        }
        else if (command.get(0).asString()=="get")
        {
            if (command.get(1).asString()=="navigation_status")
            {
                string s = gotoThread->getNavigationStatus();
                reply.addString(s.c_str());
            }
            else
            {
                reply.addString("Unknown get.");
            }
        }
        else if (command.get(0).asString()=="stop")
        {
            gotoThread->stopMovement();
            reply.addString("Stopping movement.");
        }
        else if (command.get(0).asString()=="pause")
        {
            double time = -1;
            if (command.size() > 1)
                time = command.get(1).asDouble();
            gotoThread->pauseMovement(time);
            reply.addString("Pausing.");
        }
        else if (command.get(0).asString()=="resume")
        {
            gotoThread->resumeMovement();
            reply.addString("Resuming.");
        }
        else
        {
            reply.addString("Unknown command.");
        }

        gotoThread->mutex.post();
        return true;
    }
Пример #16
0
bool Implement_DepthVisualParams_Parser::respond(const yarp::os::Bottle& cmd, yarp::os::Bottle& response)
{
    bool ret = false;
    response.clear();
    if(!iDepthVisual)
    {
        yError() << "Depth Visual parameter Parser has not been correctly configured. IDepthVisualParams interface is not valid";
        response.addVocab(VOCAB_FAILED);
        return false;
    }

    int code = cmd.get(0).asVocab();
    if(code != VOCAB_DEPTH_VISUAL_PARAMS)
    {
        yError() << "Depth Visual Params Parser received a command not belonging to this interface. Required interface was " << yarp::os::Vocab::decode(code);
        response.addVocab(VOCAB_FAILED);
        return false;
    }

    switch (cmd.get(1).asVocab())
    {
        case VOCAB_GET:
        {
            switch(cmd.get(2).asVocab())
            {
                case VOCAB_HEIGHT:
                {
                    response.addVocab(VOCAB_DEPTH_VISUAL_PARAMS);
                    response.addVocab(VOCAB_HEIGHT);
                    response.addVocab(VOCAB_IS);
                    response.addInt(iDepthVisual->getDepthHeight());
                }
                break;

                case VOCAB_WIDTH:
                {
                    response.addVocab(VOCAB_DEPTH_VISUAL_PARAMS);
                    response.addVocab(VOCAB_WIDTH);
                    response.addVocab(VOCAB_IS);
                    response.addInt(iDepthVisual->getDepthWidth());
                }
                break;

                case VOCAB_FOV:
                {
                    double hFov, vFov;
                    ret = iDepthVisual->getDepthFOV(hFov, vFov);
                    if(ret)
                    {
                        response.addVocab(VOCAB_DEPTH_VISUAL_PARAMS);
                        response.addVocab(VOCAB_FOV);
                        response.addVocab(VOCAB_IS);
                        response.addDouble(hFov);
                        response.addDouble(vFov);
                    }
                    else
                        response.addVocab(VOCAB_FAILED);
                }
                break;

                case VOCAB_INTRINSIC_PARAM:
                {
                    yarp::os::Property params;
                    ret = iDepthVisual->getDepthIntrinsicParam(params);
                    if(ret)
                    {
                        yarp::os::Bottle params_b;
                        response.addVocab(VOCAB_DEPTH_VISUAL_PARAMS);
                        response.addVocab(VOCAB_INTRINSIC_PARAM);
                        response.addVocab(VOCAB_IS);
                        Property::copyPortable(params, params_b);  // will it really work??
                        response.append(params_b);
                    }
                    else
                    {
                        response.addVocab(VOCAB_FAILED);
                    }
                }
                break;

                case VOCAB_ACCURACY:
                {
                    response.addVocab(VOCAB_DEPTH_VISUAL_PARAMS);
                    response.addVocab(VOCAB_ACCURACY);
                    response.addVocab(VOCAB_IS);
                    response.addDouble(iDepthVisual->getDepthAccuracy());
                }
                break;

                case VOCAB_CLIP_PLANES:
                {
                    double nearPlane, farPlane;
                    iDepthVisual->getDepthClipPlanes(nearPlane, farPlane);
                    response.addVocab(VOCAB_DEPTH_VISUAL_PARAMS);
                    response.addVocab(VOCAB_CLIP_PLANES);
                    response.addVocab(VOCAB_IS);
                    response.addDouble(nearPlane);
                    response.addDouble(farPlane);
                }
                break;

                case VOCAB_MIRROR:
                {
                    bool mirror;
                    ret = iDepthVisual->getDepthMirroring(mirror);
                    if(ret)
                    {
                        response.addVocab(VOCAB_DEPTH_VISUAL_PARAMS);
                        response.addVocab(VOCAB_MIRROR);
                        response.addVocab(VOCAB_IS);
                        response.addInt(mirror);
                    }
                    else
                        response.addVocab(VOCAB_FAILED);
                }
                break;

                default:
                {
                    yError() << "Depth Visual Parameter interface parser received am unknown GET command. Command is " << cmd.toString();
                    response.addVocab(VOCAB_FAILED);
                    ret = false;
                }
                break;
            }
        }
        break;

        case VOCAB_SET:
        {
            switch(cmd.get(2).asVocab())
            {
                case VOCAB_RESOLUTION:
                {
                    ret = iDepthVisual->setDepthResolution(cmd.get(3).asInt(), cmd.get(4).asInt());
                    response.addVocab(VOCAB_DEPTH_VISUAL_PARAMS);
                    response.addVocab(VOCAB_SET);
                    response.addInt(ret);
                }
                break;

                case VOCAB_FOV:
                {
                    ret = iDepthVisual->setDepthFOV(cmd.get(3).asDouble(), cmd.get(4).asDouble());
                    response.addVocab(VOCAB_DEPTH_VISUAL_PARAMS);
                    response.addVocab(VOCAB_SET);
                    response.addInt(ret);
                }
                break;

                case VOCAB_ACCURACY:
                {
                    ret = iDepthVisual->setDepthAccuracy(cmd.get(3).asDouble());
                    response.addVocab(VOCAB_DEPTH_VISUAL_PARAMS);
                    response.addVocab(VOCAB_SET);
                    response.addInt(ret);
                }
                break;

                case VOCAB_CLIP_PLANES:
                {
                    ret = iDepthVisual->setDepthClipPlanes(cmd.get(3).asDouble(), cmd.get(4).asDouble());
                    response.addVocab(VOCAB_DEPTH_VISUAL_PARAMS);
                    response.addVocab(VOCAB_SET);
                    response.addInt(ret);
                }
                break;

                case VOCAB_INTRINSIC_PARAM:
                {
                    yarp::os::Property params;
                    ret = iDepthVisual->getDepthIntrinsicParam(params);
                    if(ret)
                    {
                        yarp::os::Bottle params_b;
                        response.addVocab(VOCAB_DEPTH_VISUAL_PARAMS);
                        response.addVocab(VOCAB_INTRINSIC_PARAM);
                        response.addVocab(VOCAB_IS);
                        Property::copyPortable(params, params_b);  // will it really work??
                        response.append(params_b);
                    }
                    else
                        response.addVocab(VOCAB_FAILED);
                }
                break;

                case VOCAB_MIRROR:
                {
                    ret = iDepthVisual->setDepthMirroring(cmd.get(3).asBool());
                    response.addVocab(VOCAB_DEPTH_VISUAL_PARAMS);
                    response.addVocab(VOCAB_SET);
                    response.addInt(ret);
                }
                break;

                default:
                {
                    yError() << "Rgb Visual Parameter interface parser received am unknown SET command. Command is " << cmd.toString();
                    response.addVocab(VOCAB_FAILED);
                    ret = false;
                }
                break;
            }
        }
        break;

        default:
        {
            yError() << "Rgb Visual parameter interface Parser received a malformed request. Command should either be 'set' or 'get', received " << cmd.toString();
            response.addVocab(VOCAB_FAILED);
            ret = false;
        }
        break;
    }
    return ret;
}
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;
}
Пример #18
0
bool FrameGrabberControls2_Parser::respond(const yarp::os::Bottle& cmd, yarp::os::Bottle& response)
{
    bool ok = false;
    int action = cmd.get(1).asVocab();
    int param  = cmd.get(2).asVocab();

//     yTrace() << "cmd received\n\t" << cmd.toString().c_str();


    if(!fgCtrl2)
    {
        yError() << " Selected camera device has no IFrameGrabberControl2 interface";
        return false;
    }

    response.clear();

    switch (action)
    {
        case VOCAB_HAS:
        {
            response.addVocab(VOCAB_FRAMEGRABBER_CONTROL2);
            response.addVocab(VOCAB_HAS);
            response.addVocab(VOCAB_FEATURE);
            response.addInt(param);

            switch (param)
            {
                case VOCAB_FEATURE:
                {
                    bool _hasFeat;
                    ok = fgCtrl2->hasFeature(cmd.get(3).asInt(), &_hasFeat);
                    response.addInt(_hasFeat);
                } break;

                case VOCAB_ONOFF:
                {
                    bool _hasOnOff;
                    ok = fgCtrl2->hasOnOff(cmd.get(3).asInt(), &_hasOnOff);
                    response.addInt(_hasOnOff);
                } break;

                case VOCAB_AUTO:
                {
                    bool _hasAuto;
                    ok = fgCtrl2->hasAuto(cmd.get(3).asInt(), &_hasAuto);
                    response.addInt(_hasAuto);
                } break;

                case VOCAB_MANUAL:
                {
                    bool _hasManual;
                    ok = fgCtrl2->hasManual(cmd.get(3).asInt(), &_hasManual);
                    response.addInt(_hasManual);
                } break;

                case VOCAB_ONEPUSH:
                {
                    bool _hasOnePush;
                    ok = fgCtrl2->hasOnePush(cmd.get(3).asInt(), &_hasOnePush);
                    response.addInt(_hasOnePush);
                } break;

                default:
                {
                    yError() << "Unknown command 'HAS " << Vocab::decode(param) << "' received on IFrameGrabber2 interface";
                    response.clear();
                    ok = false;
                } break;
            } break; // end switch (param)

        } break; // end VOCAB_HAS

        case VOCAB_SET:
        {
            switch (param)
            {
                case VOCAB_FEATURE:
                {
                    ok = fgCtrl2->setFeature(cmd.get(3).asInt(), cmd.get(4).asDouble());
                } break;

                case VOCAB_FEATURE2:
                {
                    ok = fgCtrl2->setFeature(cmd.get(3).asInt(), cmd.get(4).asDouble(), cmd.get(5).asDouble());
                } break;

                case VOCAB_ACTIVE:
                {
                    ok = fgCtrl2->setActive(cmd.get(3).asInt(), cmd.get(4).asInt());
                } break;

                case VOCAB_MODE:
                {
                    ok = fgCtrl2->setMode(cmd.get(3).asInt(), (FeatureMode) cmd.get(4).asInt());
                } break;

                case VOCAB_ONEPUSH:
                {
                    ok = fgCtrl2->setOnePush(cmd.get(3).asInt());
                } break;

                default:
                {
                    yError() << "Unknown command 'SET " << Vocab::decode(param) << "' received on IFrameGrabber2 interface";
                    response.clear();
                    ok = false;
                }
            } break; // end switch (param)

        } break; // end VOCAB_SET

        case VOCAB_GET:
        {
            response.addVocab(VOCAB_FRAMEGRABBER_CONTROL2);
            response.addVocab(param);
            response.addVocab(VOCAB_IS);
            switch (param)
            {
                case VOCAB_CAMERA_DESCRIPTION:
                {
                    CameraDescriptor camera;
                    ok = fgCtrl2->getCameraDescription(&camera);
                    response.addInt(camera.busType);
                    response.addString(camera.deviceDescription);
                    yDebug() << "Response is " << response.toString();
                } break;

                case VOCAB_FEATURE:
                {
                    double value;
                    ok = fgCtrl2->getFeature(cmd.get(3).asInt(), &value);
                    response.addDouble(value);
                } break;

                case VOCAB_FEATURE2:
                {
                    double value1, value2;
                    ok = fgCtrl2->getFeature(cmd.get(3).asInt(), &value1, &value2);
                    response.addDouble(value1);
                    response.addDouble(value2);
                } break;

                case VOCAB_ACTIVE:
                {
                    bool _isActive;
                    ok = fgCtrl2->getActive(cmd.get(3).asInt(), &_isActive);
                    response.addInt(_isActive);
                } break;

                case VOCAB_MODE:
                {
                    FeatureMode _mode;
                    ok = fgCtrl2->getMode(cmd.get(3).asInt(), &_mode);
                    response.addInt(_mode);
                } break;

                default:
                {
                    yError() << "Unknown command 'GET " << Vocab::decode(param) << "' received on IFrameGrabber2 interface";
                    response.clear();
                    ok = false;
                }

            } break; // end switch (param)

        } break; // end VOCAB_GET
    }
//     yTrace() << "response is\n\t" << response.toString().c_str();
    return ok;
}
Пример #19
0
/* 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");
        }

        else {
            bReply.addString("ack");
            bReply.addList() = printPDDLContextualKnowledgeDomain();
        }
    }



    // PRINT PDDL PROBLEM

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

        if (bCommand.size() != 2) {
            bReply.addString("nack");
            bReply.addString("Usage : printPDDLContextualKnowledgeProblem ( (condition1) (condition2) )");
        }

        else {

            if (!bCommand.get(1).isList()) {
                bReply.addString("nack");
                bReply.addString("Usage : printPDDLContextualKnowledgeProblem ( (condition1) (condition2) )");
            }

            bReply.addString("ack");
            Bottle bGoal;
            bGoal = *bCommand.get(1).asList();
            bReply.addList() = printPDDLContextualKnowledgeProblem(bGoal);
        }
    }

    // HELP

    else if (bCommand.get(0).asString() == "help") {
        bReply.addString("ack");
        bReply.addString("commands are: findActivity <actionName> <begin|end|both> [<colums,to,select>]| findSharedPlan <sharedPlanName> <begin|end|both> [<colums,to,select>] | help | sqlQueryTest | quit ");
    }


    // ASK LAST ACTION

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


    // ASK LAST ACTIVITY

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


    // ADD LAST ACTIVITY

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

    //TEST : to be removed when done
    else if (bCommand.get(0).asString() == "findPossibleSharedPlan") {
        bReply.addString("ack");
        bReply.addList() = findPossibleSharedPlan(bCommand.get(1).asInt(), bCommand.get(2).asInt());
    }

    //TEST : to be removed when done
    else if (bCommand.get(0).asString() == "availableSharedPlan") {
        bReply.addString("ack");
        bReply.addList() = availableSharedPlan(bCommand.get(1).asInt(), bCommand.get(2).asInt());
    }


    //TEST : to be removed when done
    else if (bCommand.get(0).asString() == "findAllSentence") {
        bReply.addString("ack");
        bReply.addList() = findAllSentence();
    }

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



    // DISCRIMINATE ACTION

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


    // IMAGINE INSTANCE

    else if (bCommand.get(0).asString() == "imagine") {
        bReply.addString("ack");
        bReply.addList() = imagineOPC(atoi(bCommand.get(1).toString().c_str()));
    }


    // GET ACTION 
    else if (bCommand.get(0).asString() == "getAction") {
        bReply.addString("ack");
        bReply.addList() = askActionFromId(atoi(bCommand.get(1).toString().c_str()));
    }


    // QUERY BEHAVIOR

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


    // SAVE KNOWLEDGE

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


    // RESET KNOWLEDGE

    else if (bCommand.get(0).asString() == "resetKnowledge") {
        bReply.addString("ack");
        if (bCommand.size() == 2)
        {
            bReply.addList() = resetKnowledge(atoi(bCommand.get(1).toString().c_str()));
            //bReply.addList() = retroReasoning(atoi(bCommand.get(1).toString().c_str()));
        }
        else
        {
            bReply.addList() = resetKnowledge();
            //  bReply.addList() = retroReasoning();
        }
    }
    // RETRO REASONING


    else if (bCommand.get(0).asString() == "retroReasoning") {
        bReply.addString("ack");
        if (bCommand.size() == 2)
        {
            bReply.addList() = retroReasoning(atoi(bCommand.get(1).toString().c_str()));
        }
        else
        {
            bReply.addList() = retroReasoning();
        }
    }

    else if (bCommand.get(0).asString() == "level3") {
        bReply.addString("ack");
        if (bCommand.size() == 2)
        {
            bReply.addList() = level3Reasoning(atoi(bCommand.get(1).toString().c_str()));
        }
        else
        {
            bReply.addList() = level3Reasoning();
        }
    }

    // DISCRIMINATE UNKNOWN ACTIONS

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


    // CONNECT OPC

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


    // EXECUTE ACTION

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

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

    // EXECUTE ACTIVITY

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

    // EXECUTE ACTIVITY

    else if (bCommand.get(0).asString() == "whatIs") {
        bReply.addString("ack");
        if (bCommand.size() == 2)
        {
            bReply.addList() = whatIs(bCommand.get(1).asString());
        }
        else
        {
            yWarning("Error in abmReasoning::respond wrong size of input | (whatIs input)");
            bReply.addString("Error in abmReasoning::respond wrong size of input | (whatIs input)");
        }
    }

    else if (bCommand.get(0).asString() == "howTo") {
        bReply.addString("ack");
        if (bCommand.size() == 4)
        {
            bReply.addList() = howTo(bCommand.get(1).asString(), bCommand.get(2).asString(), bCommand.get(3).asString());
        }
        else
        {
            bReply.addString("Error in abmReasoning::howTo wrong size of input | (howTo sAgent sPredicate sObject)");
            yWarning("Error in abmReasoning::howTo wrong size of input | (howTo sAgent sPredicate sObject)");
        }
    }


    // EXECUTE SHARED PLAN

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

    // GRAMMAR

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

    // WORD KNOWLEDGE

    else if (bCommand.get(0).asString() == "askWordKnowledge") {
        bReply.addString("ack");

        // Get Context

        bReply.addList() = askWordKnowledge(bCommand);
    }


    // UPDATE LOCATION

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

    else if (bCommand.get(0).asString() == "displayContextual") {
        bReply.addString("ack");
        if (bCommand.size() == 4)
        {
            displayContextual(bCommand.get(1).toString(), bCommand.get(2).toString(), bCommand.get(3).toString());
        }
        else
        {

            yInfo() << " displaying " << listContextualKnowledge.size() << " contextual knowledge";
            vector<string> pastCK;
            for (vector<contextualKnowledge>::iterator itCK = listContextualKnowledge.begin();
                itCK != listContextualKnowledge.end();
                itCK++)
            {
                bool passed = false;
                for (vector<string>::iterator itVS = pastCK.begin();
                    itVS != pastCK.end();
                    itVS++){
                    if (itCK->sName == *itVS) passed = true;
                }
                if (!passed){
                    whatIs(itCK->sName);
                    pastCK.push_back(itCK->sName);
                }
                //                displayContextual(itCK->sName, itCK->sArgument, itCK->sType);
            }
        }
    }



    // UPDATE OBJECT LOCATION

    else if (bCommand.get(0).asString() == "updateObjectLocation") {
        bReply.addString("ack");
        bReply.addList() = updateOpcObjectLocation(bCommand.get(1).toString().c_str());
    }

    //no command recognize
    else {
        bReply.addString("nack");
        bReply.addString("wrong commands. Available are : findActivity <actionName> <begin|end|both> [<colums,to,select>] | findSharedPlan <sharedPlanName> <begin|end|both> [<colums,to,select>] | help | sqlQueryTest | quit ");
    }

    yInfo() << "\t" << "reply : " << bReply.toString().c_str() << "\n";
    handlerPort.reply(bReply);

    return true;
}
bool GazeboYarpFakeControlBoardDriver::getRemoteVariable(std::string key, yarp::os::Bottle& val)
{
    val.clear();
    yWarning("getRemoteVariable(): Unknown variable %s", key.c_str());
    return false;
}
Пример #21
0
bool NameServiceOnTriples::apply(yarp::os::Bottle& cmd,
                                 yarp::os::Bottle& reply,
                                 yarp::os::Bottle& event,
                                 const yarp::os::Contact& remote) {
    ConstString key = cmd.get(0).toString();
    ConstString prefix = " * ";

    access.wait();
    if (key=="register") {
        lastRegister = cmd.get(1).asString().c_str();
    } else if (key=="set") {
        if (cmd.get(1).asString()==lastRegister.c_str()) {
            prefix = "   + ";
        }
    } else {
        lastRegister = "";
    }
    if (!silent) {
        printf("%s%s\n",
               prefix.c_str(),
               cmd.toString().c_str());
    }
    access.post();

    TripleSource& mem = *db;
    //mem.begin();
    mem.reset();
    reply.clear();
    NameTripleState act(cmd,reply,event,remote,mem);

    if (cmd.check("format")) {
        if (cmd.find("format")=="json") {
            act.bottleMode = true;
        }
    }

    if (key == "NAME_SERVER") {
        cmd = cmd.tail();
        key = cmd.get(0).asString();
    }
    if (key == "bot") {
        act.bottleMode = true;
        cmd = cmd.tail();
        key = cmd.get(0).asString();
    }

    if (key=="register") {
        return cmdRegister(act);
    } else if (key=="unregister") {
        return cmdUnregister(act);
    } else if (key=="query") {
        return cmdQuery(act);
    } else if (key=="list") {
        return cmdList(act);
    } else if (key=="set") {
        return cmdSet(act);
    } else if (key=="get") {
        return cmdGet(act);
    } else if (key=="check") {
        return cmdCheck(act);
    } else if (key=="route") {
        return cmdRoute(act);
    } else if (key=="gc") {
        return cmdGc(act);
    } else if (key=="help") {
        return cmdHelp(act);
    } else {
        // not understood
        act.reply.addString("old");
    }
    //mem.end();

    return true;
}
Пример #22
0
void WorldRpcInterface::make( const yarp::os::Bottle& command, yarp::os::Bottle& reply, int& n )
{
	int geom = command.get(n).asVocab();
	n++;
	 
	try {
		KinematicModel::CompositeObject* composite = new KinematicModel::CompositeObject( model->OBSTACLE(), model->FIELD() );
		KinematicModel::PrimitiveObject* primitive;
		if ( geom == VOCAB_SPH || geom == VOCAB_SSPH ) {
			
			double r = command.get(n).asDouble(); n++; // radius 
			double px = command.get(n).asDouble(); n++; // x position
			double py = command.get(n).asDouble(); n++; // y position  
			double pz = command.get(n).asDouble(); n++; // z position
			
			primitive = new KinematicModel::Sphere( r );
			composite->appendPrimitive( primitive );
			composite->setPosition( QVector3D(px,py,pz) );
            
            QString name;
			if ( geom == VOCAB_SPH ) { name = "sph" + QString::number(++s); }
			else if ( geom == VOCAB_SSPH ) { name = "ssph" + QString::number(++ss); }
            composite->setName(name);
            //printf("%s\n",name.toLocal8Bit().data());
			reply.addString( name.toLocal8Bit().data() );
			
		} else if ( geom == VOCAB_CYL || geom == VOCAB_SCYL ) {
			
			double r = command.get(n).asDouble(); n++; // radius
			double h = command.get(n).asDouble(); n++; // height 
			double px = command.get(n).asDouble(); n++; // x position
			double py = command.get(n).asDouble(); n++; // y position  
			double pz = command.get(n).asDouble(); n++; // z position
			
			//if ( geom == VOCAB_CYL ) { world->newCylinder( r, h, QVector3D(px,py,pz) ); }
			//else if ( geom == VOCAB_SCYL ) { world->newSCylinder( r, h, QVector3D(px,py,pz) ); }
			
			primitive = new KinematicModel::Cylinder( r, h );
			composite->appendPrimitive( primitive );
			composite->setPosition( QVector3D(px,py,pz) );
			
			QString name;
            if ( geom == VOCAB_CYL ) { name = "cyl" + QString::number(++c); }
			else if ( geom == VOCAB_SCYL ) { name = "scyl" + QString::number(++sc); }
            composite->setName(name);
			reply.addString( name.toLocal8Bit().data() );
			
		} else if ( geom == VOCAB_BOX || geom == VOCAB_SBOX ) {
			
			double x = command.get(n).asDouble(); n++;  // x size
			double y = command.get(n).asDouble(); n++;  // y size  
			double z = command.get(n).asDouble(); n++;  // z size
			double px = command.get(n).asDouble(); n++; // x position
			double py = command.get(n).asDouble(); n++; // y position  
			double pz = command.get(n).asDouble(); n++; // z position

			//if ( geom == VOCAB_BOX ) { world->newBox( QVector3D(x,y,z), QVector3D(px,py,pz) ); }
			//else if ( geom == VOCAB_SBOX ) { world->newSBox( QVector3D(x,y,z), QVector3D(px,py,pz) ); }
			
			primitive = new KinematicModel::Box( QVector3D(x,y,z) );
			composite->appendPrimitive( primitive );
			composite->setPosition( QVector3D(px,py,pz) );
            
            QString name;
            if ( geom == VOCAB_BOX ) { name = "box" + QString::number(++b); }
			else if ( geom == VOCAB_SBOX ) { name = "sbox" + QString::number(++sb); }
            composite->setName(name);
			reply.addString( name.toLocal8Bit().data() );
		}
		else { reply.addString("MK ERROR: Unknown geometry... use '(s)sph', '(s)cyl', or '(s)box'"); }
		
		model->appendObject( composite );
	}
	catch (std::exception& e)
	{
		reply.clear();
		reply.addString(e.what());
	}
}