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);
}
}
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;
}
/* Respond function */
bool opcManager::respond(const yarp::os::Bottle& bCommand, yarp::os::Bottle& bReply)
{
string helpMessage = string(getName().c_str()) +
" commands are: \n" +
"help \n" +
"connect + name\n" +
"quit \n";
bReply.clear();
string keyWord = bCommand.get(0).asString().c_str();
if (keyWord == "quit") {
bReply.addString("quitting");
return false;
}
else if (keyWord == "help") {
cout << helpMessage;
bReply.addString("ok");
}
else if (keyWord == "connect") {
bReply = connect(bCommand);
}
else if (keyWord == "updateBeliefs") {
bReply.addString("nack");
if (bCommand.size() == 2)
{
if (bCommand.get(1).isString())
{
bReply.addList() = updateBelief(bCommand.get(1).toString().c_str());
}
}
}
else if (keyWord == "synchronise")
{
bReply.addString("ack");
bReply.addList() = synchoniseOPCs();
}
else if (keyWord == "executeActivity")
{
bReply.addString("ack");
bReply.addList() = simulateActivity(bCommand);
}
else if (keyWord == "diffOPC")
{
bReply.addString("ack");
bReply.addList() = diffOPC();
}
return true;
}
bool 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;
}
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;
}
/**
* 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;
}
/**
* 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;
}
/* 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;
}
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;
}
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;
}
/*
* 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;
}
bool clearAppNames() {
apps.clear();
return true;
}
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;
}
// 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;
}
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;
}
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;
}
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;
}
/* 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;
}
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;
}
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());
}
}
