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