bool Behavior::respond(const Bottle &command,Bottle &reply){
bool ok = false; // command executed successfully
switch (command.get(0).asVocab()) {
case VOCAB_QUIT:
reply.addVocab(VOCAB_OK);
ok = true;
OK_MSG = 1;
break;
case VOCAB_STOP:
reply.addVocab(VOCAB_OK);
ok = true;
next_state=IDLE;
OK_MSG = 1;
break;
case VOCAB_CONT:
reply.addVocab(VOCAB_OK);
ok = true;
next_state=ATTENTION;
OK_MSG = 1;
break;
default:
printf("VOCAB default\n ");
reply.addVocab(VOCAB_FAILED);
}
ok = RFModule::respond(command,reply); // will add message 'not recognized' if not recognized
return ok;
}
bool respond(const Bottle& command, Bottle& reply)
{
LockGuard lg(mutex);
int cmd=command.get(0).asVocab();
if (cmd==Vocab::encode("stop"))
{
state=idle;
reply.addVocab(Vocab::encode("ack"));
return true;
}
else if (cmd==Vocab::encode("start"))
{
if (command.size()>=5)
{
initRect.x=command.get(1).asInt();
initRect.y=command.get(2).asInt();
initRect.width=command.get(3).asInt();
initRect.height=command.get(4).asInt();
state=init;
reply.addVocab(Vocab::encode("ack"));
}
else
reply.addVocab(Vocab::encode("nack"));
return true;
}
else
return RFModule::respond(command,reply);
}
virtual bool respond(const Bottle &command, Bottle &reply)
{
bool ret;
if (command.size()!=0)
{
switch (command.get(0).asVocab())
{
case yarp::os::createVocab('h','e','l','p'):
{
cout << "Available commands:" << endl;
cout<<"Queue command:\n";
cout<<"[ctpq] [time] seconds [off] j [pos] (list)\n";
cout<<"New command, execute now (erase queue):\n";
cout<<"[ctpq] [time] seconds [off] j [pos] (list)\n";
cout<<"Load sequence from file:\n";
cout<<"[ctpf] filename\n";
reply.addVocab(Vocab::encode("ack"));
return true;
}
case VCTP_CMD_NOW:
{
ret=handlectpm(command, reply);
return ret;
}
case VCTP_CMD_QUEUE:
{
ret=handle_ctp_queue(command, reply);
return ret;
}
case VCTP_CMD_FILE:
{
ret=handle_ctp_file(command, reply);
return ret;
}
case VCTP_WAIT:
{
ret=handle_wait(command, reply);
}
default:
return RFModule::respond(command,reply);
}
}
else
{
reply.addVocab(Vocab::encode("nack"));
return false;
}
}
// rpcPort commands handler
bool respond(const Bottle & command,
Bottle & reply)
{
// This method is called when a command string is sent via RPC
// Get command string
string receivedCmd = command.get(0).asString().c_str();
reply.clear(); // Clear reply bottle
if (receivedCmd == "help")
{
reply.addVocab(Vocab::encode("many"));
reply.addString("Available commands are:");
reply.addString("help");
reply.addString("quit");
}
else if (receivedCmd == "quit")
{
reply.addString("Quitting.");
return false; //note also this
}
else
reply.addString("Invalid command, type [help] for a list of accepted commands.");
return true;
}
Contact NameServer::unregisterName(const ConstString& name) {
Contact prev = queryName(name);
if (prev.isValid()) {
if (prev.getPort()!=-1) {
NameRecord& rec = getNameRecord(prev.getRegName());
if (rec.isReusablePort()) {
HostRecord& host = getHostRecord(prev.getHost());
host.release(prev.getPort());
}
if (rec.isReusableIp()) {
if (rec.getAddress().getCarrier()=="mcast") {
mcastRecord.releaseAddress(rec.getAddress().getHost().c_str());
}
}
rec.clear();
tmpNames.release(name);
Bottle event;
event.addVocab(Vocab::encode("del"));
event.addString(name.c_str());
onEvent(event);
}
}
return queryName(name);
}
bool respond(const Bottle &command, Bottle &reply)
{
LockGuard lg(mutex);
int cmd=command.get(0).asVocab();
int ack=Vocab::encode("ack");
int nack=Vocab::encode("nack");
if (cmd==Vocab::encode("start"))
{
if (command.size()>=3)
{
actionTag=command.get(1).asString();
objectTag=command.get(2).asString();
gate=1;
if (command.size()>=4)
{
int g=command.get(3).asInt();
if (g>0)
gate=g;
}
reply.addVocab(ack);
}
else
reply.addVocab(nack);
return true;
}
else if (cmd==Vocab::encode("stop"))
{
actionTag="none";
objectTag="none";
gate=0;
reply.addVocab(ack);
return true;
}
else if (cmd==Vocab::encode("get"))
{
reply.addVocab(ack);
reply.addString(actionTag);
reply.addString(objectTag);
reply.addInt(gate);
return true;
}
else
return RFModule::respond(command,reply);
}
bool handle_ctp_file(const Bottle &cmd, Bottle &reply)
{
if (cmd.size()<2)
return false;
string fileName=rf->findFile(cmd.get(1).asString());
bool ret = velThread.go(fileName);
if (ret)
{
reply.addVocab(Vocab::encode("ack"));
}
else
{
reply.addVocab(Vocab::encode("nack"));
reply.addString("Unable to load file");
}
return ret;
}
bool respond(const Bottle& command, Bottle& reply)
{
reply.clear();
if (command.get(0).isInt())
{
if (command.get(0).asInt()==0)
{
fprintf(stderr,"Asking recalibration...\n");
if (inv_dyn)
{
inv_dyn->suspend();
inv_dyn->calibrateOffset();
inv_dyn->resume();
}
fprintf(stderr,"Recalibration complete.\n");
reply.addString("Recalibrated");
return true;
}
}
if (command.get(0).isString())
{
if (command.get(0).asString()=="help")
{
reply.addVocab(Vocab::encode("many"));
reply.addString("Available commands:");
reply.addString("calib all");
reply.addString("calib arms");
reply.addString("calib legs");
reply.addString("calib feet");
return true;
}
else if (command.get(0).asString()=="calib")
{
fprintf(stderr,"Asking recalibration...\n");
if (inv_dyn)
{
calib_enum calib_code=CALIB_ALL;
if (command.get(1).asString()=="all") calib_code=CALIB_ALL;
else if (command.get(1).asString()=="arms") calib_code=CALIB_ARMS;
else if (command.get(1).asString()=="legs") calib_code=CALIB_LEGS;
else if (command.get(1).asString()=="feet") calib_code=CALIB_FEET;
inv_dyn->suspend();
inv_dyn->calibrateOffset(calib_code);
inv_dyn->resume();
}
fprintf(stderr,"Recalibration complete.\n");
reply.addString("Recalibrated");
return true;
}
}
reply.addString("Unknown command");
return true;
}
int main(int argc, char *argv[]) {
if (argc<=1) {
printf("This is a very simple database\n");
printf("Call as: %s --name /database\n", argv[0]);
printf("Then you can test it by running:\n");
printf(" yarp rpc /database\n");
printf("And typing things like:\n");
printf(" set x 24\n");
printf(" get x\n");
printf(" get y\n");
printf(" rm x\n");
printf(" get x\n");
printf(" set \"my favorite numbers\" (5 10 16)\n");
printf(" get \"my favorite numbers\"\n");
}
Network yarp;
Property option;
option.fromCommand(argc,argv);
Property state;
Port port;
port.open(option.check("name",Value("/database")).asString());
while (true) {
Bottle cmd;
Bottle response;
port.read(cmd,true); // true -> will reply
Bottle tmp;
tmp.add(cmd.get(1));
std::string key = tmp.toString();
switch (Vocab::encode(cmd.get(0).toString())) {
case VOCAB_SET:
state.put(key,cmd.get(2));
break;
case VOCAB_GET:
break;
case VOCAB_REMOVE:
state.unput(key);
break;
}
Value& v = state.find(key);
response.addVocab(v.isNull()?VOCAB_NOT:VOCAB_IS);
response.add(cmd.get(1));
if (!v.isNull()) {
response.add(v);
}
port.reply(response);
}
return 0;
}
bool handle_wait(const Bottle &cmd, Bottle &reply)
{
cerr<<"Warning command not implemented yet"<<endl;
reply.addVocab(Vocab::encode("ack"));
return true;
//ActionItem *action;
//bool ret=parseWaitCmd(cmd, reply, &action);
//if (ret)
// posPort.queue(action);
}
bool handle_ctp_queue(const Bottle &cmd, Bottle &reply)
{
ActionItem *action;
bool ret=parsePosCmd(cmd, reply, &action);
if (ret)
{
posPort.queue(action);
reply.addVocab(Vocab::encode("ack"));
}
return ret;
}
bool respond(const Bottle &command, Bottle &reply)
{
int ack =Vocab::encode("ack");
int nack=Vocab::encode("nack");
if (command.size()>0)
{
if (command.get(0).asString() == "get")
{
if (command.get(1).asString() == "motionGain")
{
reply.addVocab(ack);
reply.addDouble(motionGain);
}
else
{
reply.addVocab(nack);
}
}
else if (command.get(0).asString() == "set")
{
if (command.get(1).asString() == "motionGain")
{
reply.addVocab(ack);
motionGain = command.get(2).asDouble();
reply.addDouble(motionGain);
}
else if (command.get(1).asString() == "behavior")
{
if (command.get(2).asString() == "avoidance")
{
reply.addVocab(ack);
motionGain = -1.0;
reply.addDouble(motionGain);
}
else if (command.get(2).asString() == "catching")
{
reply.addVocab(ack);
motionGain = 1.0;
reply.addDouble(motionGain);
}
else
{
reply.addVocab(nack);
}
}
else
{
reply.addVocab(nack);
}
}
}
return true;
}
bool respond(const Bottle &command, Bottle &reply)
{
string cmd=command.get(0).asString().c_str();
int ack=Vocab::encode("ack");
int nack=Vocab::encode("nack");
if (cmd=="clear")
{
LockGuard lg(mutex);
contour.clear();
floodPoints.clear();
go=flood3d=flood=false;
reply.addVocab(ack);
}
else if ((cmd=="go") || (cmd=="flood3d"))
{
if (portSFM.getOutputCount()==0)
reply.addVocab(nack);
else
{
LockGuard lg(mutex);
if (cmd=="go")
{
if (contour.size()>2)
{
flood=false;
go=true;
reply.addVocab(ack);
}
else
reply.addVocab(nack);
}
else if (cmd=="flood3d")
{
if (command.size()>=2)
spatial_distance=command.get(1).asDouble();
contour.clear();
floodPoints.clear();
flood=false;
flood3d=true;
reply.addVocab(ack);
}
}
}
else if (cmd=="flood")
{
if (command.size()>=2)
color_distance=command.get(1).asInt();
contour.clear();
floodPoints.clear();
flood=true;
reply.addVocab(ack);
}
else
RFModule::respond(command,reply);
return true;
}
bool respond(const Bottle &command, Bottle &reply)
{
/* This method is called when a command string is sent via RPC */
reply.clear(); // Clear reply bottle
/* Get command string */
string receivedCmd = command.get(0).asString().c_str();
int responseCode; //Will contain Vocab-encoded response
if (receivedCmd == "merge") {
int ok = MergePointclouds();
if (ok>=0) {
responseCode = Vocab::encode("ack");
} else {
fprintf(stdout,"Couldnt merge pointclouds. \n");
responseCode = Vocab::encode("nack");
return false;
}
reply.addVocab(responseCode);
return true;
} else if (receivedCmd == "save") {
saving = true;
responseCode = Vocab::encode("ack");
reply.addVocab(responseCode);
return true;
} else if (receivedCmd == "view") {
visualizing = true;
responseCode = Vocab::encode("ack");
reply.addVocab(responseCode);
return true;
}else if (receivedCmd == "help"){
reply.addVocab(Vocab::encode("many"));
reply.addString("Available commands are:");
reply.addString("merge - Merges all pointclouds on the path folder, or the new ones if 'cloud_merged' already exists.");
reply.addString("view - Activates visualization. (XXX visualizer does not close). Default off");
reply.addString("save - Activates saving the resulting merged point cloud.");
//reply.addString("verbose ON/OFF - Sets active the printouts of the program, for debugging or visualization.");
reply.addString("help - produces this help.");
reply.addString("quit - closes the module.");
responseCode = Vocab::encode("ack");
reply.addVocab(responseCode);
return true;
} else if (receivedCmd == "quit") {
responseCode = Vocab::encode("ack");
reply.addVocab(responseCode);
closing = true;
return true;
}
reply.addString("Invalid command, type [help] for a list of accepted commands.");
return true;
}
bool Module::basicRespond(const Bottle& command, Bottle& reply) {
switch (command.get(0).asVocab()) {
case VOCAB3('s','e','t'):
state.put(command.get(1).toString(),command.get(2));
reply.addVocab(Vocab::encode("ack"));
return true;
break;
case VOCAB3('g','e','t'):
reply.add(state.check(command.get(1).toString(),Value(0)));
return true;
break;
case VOCAB4('q','u','i','t'):
case VOCAB4('e','x','i','t'):
case VOCAB3('b','y','e'):
reply.addVocab(Vocab::encode("bye"));
stopFlag = true;
interruptModule();
return true;
default:
reply.add("command not recognized");
return false;
}
return false;
}
bool RFModule::basicRespond(const Bottle& command, Bottle& reply) {
switch (command.get(0).asVocab()) {
case VOCAB4('q','u','i','t'):
case VOCAB4('e','x','i','t'):
case VOCAB3('b','y','e'):
reply.addVocab(Vocab::encode("bye"));
stopModule(false); //calls interruptModule()
// interruptModule();
return true;
default:
reply.add("command not recognized");
return false;
}
return false;
}
bool respond(const Bottle& command, Bottle& reply)
{
reply.clear();
if (command.get(0).asString()=="help")
{
reply.addVocab(Vocab::encode("many"));
reply.addString("Available commands are:");
reply.addString("reset_odometry");
reply.addString("relocalize <x> <y> <theta>");
reply.addString("go <dir> <vel_lin> <vel_ang>");
return true;
}
else if (command.get(0).asString()=="reset_odometry")
{
if (control)
{
control->reset();
reply.addString("Odometry reset done.");
}
return true;
}
else if (command.get(0).asString() == "relocalize")
{
if (control)
{
control->reset(command.get(1).asDouble(), command.get(2).asDouble(), command.get(3).asDouble());
reply.addString("Odometry reset done.");
}
return true;
}
else if (command.get(0).asString() == "go")
{
m_command.m_command_lin_dir = command.get(1).asDouble();
m_command.m_command_lin_vel = command.get(2).asDouble();
m_command.m_command_ang_vel = command.get(3).asDouble();
m_command.m_command_time = yarp::os::Time::now();
reply.addString("ok");
return true;
}
reply.addString("Unknown command.");
return true;
}
bool respond(const Bottle &command, Bottle &reply)
{
int ack =Vocab::encode("ack");
int nack=Vocab::encode("nack");
if (command.size()>0)
{
switch (command.get(0).asVocab())
{
//-----------------
case VOCAB4('l','o','o','k'):
{
if (lookForFaces())
{
iarm -> waitMotionDone();
sendMessage(0,"Hello, would you like to take one of our samples?\n");
reply.addVocab(ack);
return true;
}
reply.addVocab(nack);
return true;
}
case VOCAB4('s','o','r','r'):
{
sendMessage(0,"Oh, sorry.\n");
reply.addVocab(ack);
return true;
}
case VOCAB4('t','h','a','n'):
{
sendMessage(0,"Thank you!\n");
reply.addVocab(ack);
return true;
}
case VOCAB4('h','e','r','e'):
{
sendMessage(0,"Here you go!\n");
reply.addVocab(ack);
return true;
}
case VOCAB4('h','o','m','e'):
{
if (goHome())
{
reply.addVocab(ack);
}
else
{
reply.addVocab(nack);
}
return true;
}
case VOCAB4('a','s','k','n'):
{
sendMessage(0,"Can you please give me another sample?\n");
reply.addVocab(ack);
return true;
}
//-----------------
default:
return RFModule::respond(command,reply);
}
}
reply.addVocab(nack);
return true;
}
bool respond(const Bottle &command, Bottle &reply)
{
int ack=Vocab::encode("ack");
int nack=Vocab::encode("nack");
if (command.size()>0)
{
switch (command.get(0).asVocab())
{
//-----------------
case VOCAB4('i','n','i','t'):
{
if (command.size()>=5)
{
mutex.lock();
tl.x=(float)command.get(1).asInt();
tl.y=(float)command.get(2).asInt();
br.x=(float)command.get(3).asInt();
br.y=(float)command.get(4).asInt();
initBoundingBox=true;
mutex.unlock();
reply.addVocab(ack);
}
else
reply.addVocab(nack);
return true;
}
//-----------------
case VOCAB4('s','t','a','r'):
{
mutex.lock();
doCMT=true;
mutex.unlock();
reply.addVocab(ack);
return true;
}
//-----------------
case VOCAB4('s','t','o','p'):
{
mutex.lock();
doCMT=false;
mutex.unlock();
reply.addVocab(ack);
return true;
}
//-----------------
default:
return RFModule::respond(command,reply);
}
}
reply.addVocab(nack);
return true;
}
bool asvGrabberModule::respond(const Bottle& command, Bottle& reply) {
bool ok = false;
bool rec = false; // is the command recognized?
string helpMessage = string(getName().c_str()) +
" commands are: \n" +
"help \n" +
"quit \n" +
"set thr <n> ... set the threshold \n" +
"(where <n> is an integer number) \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");
}
mutex.wait();
switch (command.get(0).asVocab()) {
case COMMAND_VOCAB_HELP:
rec = true;
{
reply.addString("many");
reply.addString("help");
reply.addString("");
reply.addString("set fn \t: general set command ");
reply.addString("");
reply.addString("");
reply.addString("");
reply.addString("set left pr <int> \t: setting the costant time between saccadic events (default 3000) ");
reply.addString("set left foll <double> \t: setting the coefficients to the default value ");
reply.addString("set left diff <double> \t: setting of linear combination coefficient (map1) ");
reply.addString("set left diffon<double> \t: setting of linear combination coefficient (map2) ");
reply.addString("set left puy <double> \t: setting of linear combination coefficient (map3) ");
reply.addString("set left refr <double> \t: setting of linear combination coefficient (map4) ");
reply.addString("set left req <double> \t: setting of linear combination coefficient (map5) ");
reply.addString("set left diffoff <double> \t: setting of linear combination coefficient (map6) ");
reply.addString("set left pux <double> \t: setting of linear combination coefficient (mapc1) ");
reply.addString("set left reqpd <double> \t: setting of linear combination coefficient (flow motion) ");
reply.addString("set left injgnd <double> \t: setting of linear combination coefficient (flow motion) ");
reply.addString("set left cas <double> \t: setting of linear combination coefficient (flow motion) ");
reply.addString("");
reply.addString("get fn \t: general get command ");
reply.addString("");
reply.addString("");
reply.addString("get left pr <double> \t: setting the costant time between saccadic events (default 3000) ");
reply.addString("get left foll <double> \t: setting the coefficients to the default value ");
reply.addString("get left diff <double> \t: setting of linear combination coefficient (map1) ");
reply.addString("get left diffon<double> \t: setting of linear combination coefficient (map2) ");
reply.addString("get left puy <double> \t: setting of linear combination coefficient (map3) ");
reply.addString("get left refr <double> \t: setting of linear combination coefficient (map4) ");
reply.addString("get left req <double> \t: setting of linear combination coefficient (map5) ");
reply.addString("get left diffoff <double> \t: setting of linear combination coefficient (map6) ");
reply.addString("get left pux <double> \t: setting of linear combination coefficient (mapc1) ");
reply.addString("get left reqpd <double> \t: setting of linear combination coefficient (flow motion) ");
reply.addString("get left injgnd <double> \t: setting of linear combination coefficient (flow motion) ");
reply.addString("get left cas <double> \t: setting of linear combination coefficient (flow motion) ");
reply.addString("");
reply.addString("");
reply.addString("prog fn \t: general prog command ");
reply.addString("");
reply.addString("");
reply.addString("prog left bias \t: asks to reprogram biases ");
reply.addString("prog right bias \t: asks to reprogram biases ");
reply.addString("dump on \t: asks to reprogram biases ");
reply.addString("dump off \t: asks to reprogram biases ");
ok = true;
}
break;
case COMMAND_VOCAB_SUSPEND:
rec = true;
{
D2Y->suspend();
ok = true;
}
break;
case COMMAND_VOCAB_RESUME:
rec = true;
{
D2Y->resume();
ok = true;
}
break;
case COMMAND_VOCAB_NAME:
rec = true;
{
// check and change filter name to pass on to the next filter
string fName(command.get(1).asString());
string subName;
Bottle subCommand;
int pos=1;
//int pos = fName.find_first_of(filter->getFilterName());
if (pos == 0) {
pos = fName.find_first_of('.');
if (pos > -1) { // there is a subfilter name
subName = fName.substr(pos + 1, fName.size()-1);
subCommand.add(command.get(0));
subCommand.add(Value(subName.c_str()));
}
for (int i = 2; i < command.size(); i++)
subCommand.add(command.get(i));
//ok = filter->respond(subCommand, reply);
}
else {
printf("filter name does not match top filter name ");
ok = false;
}
}
break;
case COMMAND_VOCAB_SET:
rec = true;
{
switch(command.get(1).asVocab()) {
case COMMAND_VOCAB_SYTH: {
double w = command.get(2).asDouble();
if(D2Y!=0)
D2Y->setSynThr(w);
ok = true;
}
break;
case COMMAND_VOCAB_SYTA: {
double w = command.get(2).asDouble();
if(D2Y!=0)
D2Y->setSynTau(w);
ok = true;
}
break;
case COMMAND_VOCAB_SYPA: {
double w = command.get(2).asDouble();
if(D2Y!=0)
D2Y->setSynPxlTau(w);
ok = true;
}
break;
case COMMAND_VOCAB_SYPH: {
double w = command.get(2).asDouble();
if(D2Y!=0)
D2Y->setSynPxlThr(w);
ok = true;
}
break;
case COMMAND_VOCAB_TPB: {
double w = command.get(2).asDouble();
if(D2Y!=0)
D2Y->setTestPbias(w);
ok = true;
}
break;
case COMMAND_VOCAB_CDR: {
double w = command.get(2).asDouble();
if(D2Y!=0)
D2Y->setCDRefr(w);
ok = true;
}
break;
case COMMAND_VOCAB_CDS: {
double w = command.get(2).asDouble();
if(D2Y!=0)
D2Y->setCDSf(w);
ok = true;
}
break;
case COMMAND_VOCAB_CDP: {
double w = command.get(2).asDouble();
if(D2Y!=0)
D2Y->setCDPr(w);
ok = true;
}
break;
case COMMAND_VOCAB_RPX: {
double w = command.get(2).asDouble();
if(D2Y!=0)
D2Y->setReqPuX(w);
ok = true;
}
break;
case COMMAND_VOCAB_RPY: {
double w = command.get(2).asDouble();
if(D2Y!=0)
D2Y->setReqPuY(w);
ok = true;
}
break;
case COMMAND_VOCAB_IFR: {
double w = command.get(2).asDouble();
if(D2Y!=0)
D2Y->setIFRf(w);
ok = true;
}
break;
case COMMAND_VOCAB_IFT: {
double w = command.get(2).asDouble();
if(D2Y!=0)
D2Y->setIFThr(w);
ok = true;
}
break;
case COMMAND_VOCAB_IFL: {
double w = command.get(2).asDouble();
if(D2Y!=0)
D2Y->setIFLk(w);
ok = true;
}
break;
case COMMAND_VOCAB_CDOF: {
double w = command.get(2).asDouble();
if(D2Y!=0)
D2Y->setCDOffThr(w);
ok = true;
}
break;
case COMMAND_VOCAB_SYPW: {
double w = command.get(2).asDouble();
if(D2Y!=0)
D2Y->setSynPxlW(w);
ok = true;
}
break;
case COMMAND_VOCAB_SYW: {
double w = command.get(2).asDouble();
if(D2Y!=0)
D2Y->setSynW(w);
ok = true;
}
break;
case COMMAND_VOCAB_CDON: {
double w = command.get(2).asDouble();
if(D2Y!=0)
D2Y->setCDOnThr(w);
ok = true;
}
break;
case COMMAND_VOCAB_CDD: {
double w = command.get(2).asDouble();
if(D2Y!=0)
D2Y->setCDDiff(w);
ok = true;
}
break;
case COMMAND_VOCAB_EMCH: {
double w = command.get(2).asDouble();
if(D2Y!=0)
D2Y->setEMCompH(w);
ok = true;
}
break;
case COMMAND_VOCAB_EMCT: {
double w = command.get(2).asDouble();
if(D2Y!=0)
D2Y->setEMCompT(w);
ok = true;
}
break;
case COMMAND_VOCAB_CDI: {
double w = command.get(2).asDouble();
if(D2Y!=0)
D2Y->setCDIoff(w);
ok = true;
}
break;
case COMMAND_VOCAB_CDRG: {
double w = command.get(2).asDouble();
if(D2Y!=0)
D2Y->setCDRGnd(w);
ok = true;
}
break;
case COMMAND_VOCAB_SELF: {
double w = command.get(2).asDouble();
if(D2Y!=0)
D2Y->setSelf(w);
ok = true;
}
break;
case COMMAND_VOCAB_FOLL: {
double w = command.get(2).asDouble();
if(D2Y!=0)
D2Y->setFollBias(w);
ok = true;
}
break;
case COMMAND_VOCAB_ARBP: {
double w = command.get(2).asDouble();
if(D2Y!=0)
D2Y->setArbPd(w);
ok = true;
}
break;
case COMMAND_VOCAB_EMVL: {
double w = command.get(2).asDouble();
if(D2Y!=0)
D2Y->setEMVrefL(w);
ok = true;
}
break;
case COMMAND_VOCAB_CDC: {
double w = command.get(2).asDouble();
if(D2Y!=0)
D2Y->setCDCas(w);
ok = true;
}
break;
case COMMAND_VOCAB_EMVH: {
double w = command.get(2).asDouble();
if(D2Y!=0)
D2Y->setEMVrefH(w);
ok = true;
}
break;
default: {
} break;
} // closing the inner switch
}
break; //closing the SET
case COMMAND_VOCAB_GET:
rec = true;
{
switch(command.get(1).asVocab()) {
case COMMAND_VOCAB_SYTH: {
double w = D2Y->getSynThr();
reply.addDouble(w);
printf (" %f \n", w);
ok = true;
}
break;
case COMMAND_VOCAB_SYTA: {
double w = D2Y->getSynTau();
reply.addDouble(w);
printf (" %f \n", w);
ok = true;
}
break;
case COMMAND_VOCAB_SYPA: {
double w = D2Y->getSynPxlTau();
reply.addDouble(w);
printf (" %f \n", w);
ok = true;
}
break;
case COMMAND_VOCAB_SYPH: {
double w = D2Y->getSynPxlThr();
reply.addDouble(w);
printf (" %f \n", w);
ok = true;
}
break;
case COMMAND_VOCAB_TPB: {
double w = D2Y->getTestPBias();
reply.addDouble(w);
printf (" %f \n", w);
ok = true;
}
break;
case COMMAND_VOCAB_CDR: {
double w = D2Y->getCDRefr();
printf (" %f \n", w);
reply.addDouble(w);
ok = true;
}
break;
case COMMAND_VOCAB_CDS: {
double w = D2Y->getCDSf();
printf (" %f \n", w);
reply.addDouble(w);
ok = true;
}
break;
case COMMAND_VOCAB_CDP: {
double w = D2Y->getCDPr();
printf (" %f \n", w);
reply.addDouble(w);
ok = true;
}
break;
case COMMAND_VOCAB_RPX: {
double w = D2Y->getReqPuX();
printf (" %f \n", w);
reply.addDouble(w);
ok = true;
}
break;
case COMMAND_VOCAB_RPY : {
double w = D2Y->getReqPuY();
printf (" %f \n", w);
reply.addDouble(w);
ok = true;
}
break;
case COMMAND_VOCAB_IFR : {
double w = D2Y->getIFRf();
printf (" %f \n", w);
reply.addDouble(w);
ok = true;
}
break;
case COMMAND_VOCAB_IFT : {
double w = D2Y->getIFThr();
printf (" %f \n", w);
reply.addDouble(w);
ok = true;
}
break;
case COMMAND_VOCAB_IFL : {
double w = D2Y->getIFLk();
printf (" %f \n", w);
reply.addDouble(w);
ok = true;
}
break;
case COMMAND_VOCAB_CDOF: {
double w = D2Y->getCDOffThr();
printf (" %f \n", w);
reply.addDouble(w);
ok = true;
}
break;
case COMMAND_VOCAB_SYPW: {
double w = D2Y->getSynPxlW();
printf (" %f \n", w);
reply.addDouble(w);
ok = true;
}
break;
case COMMAND_VOCAB_SYW: {
double w = D2Y->getSynW();
printf (" %f \n", w);
reply.addDouble(w);
ok = true;
}
break;
case COMMAND_VOCAB_CDON: {
double w = D2Y->getCDOnThr();
printf (" %f \n", w);
reply.addDouble(w);
ok = true;
}
break;
case COMMAND_VOCAB_CDD: {
double w = D2Y->getCDDiff();
printf (" %f \n", w);
reply.addDouble(w);
ok = true;
}
break;
case COMMAND_VOCAB_EMCH: {
double w = D2Y->getEMCompH();
printf (" %f \n", w);
reply.addDouble(w);
ok = true;
}
break;
case COMMAND_VOCAB_EMCT: {
double w = D2Y->getEMCompT();
printf (" %f \n", w);
reply.addDouble(w);
ok = true;
}
break;
case COMMAND_VOCAB_CDI: {
double w = D2Y->getCDIoff();
printf (" %f \n", w);
reply.addDouble(w);
ok = true;
}
break;
case COMMAND_VOCAB_CDRG: {
double w = D2Y->getCDRGnd();
printf (" %f \n", w);
reply.addDouble(w);
ok = true;
}
break;
case COMMAND_VOCAB_SELF: {
double w = D2Y->getSelf();
printf (" %f \n", w);
reply.addDouble(w);
ok = true;
}
break;
case COMMAND_VOCAB_FOLL: {
double w = D2Y->getFollBias();
printf (" %f \n", w);
reply.addDouble(w);
ok = true;
}
break;
case COMMAND_VOCAB_ARBP: {
double w = D2Y->getArbPd();
printf (" %f \n", w);
reply.addDouble(w);
ok = true;
}
break;
case COMMAND_VOCAB_EMVL: {
double w = D2Y->getEMVrefL();
printf (" %f \n", w);
reply.addDouble(w);
ok = true;
}
break;
case COMMAND_VOCAB_CDC : {
double w = D2Y->getCDCas();
printf (" %f \n", w);
reply.addDouble(w);
ok = true;
}
break;
case COMMAND_VOCAB_EMVH: {
double w = D2Y->getEMVrefH();
printf (" %f \n", w);
reply.addDouble(w);
ok = true;
}
break;
default: {
}
break;
}
}
break;
case COMMAND_VOCAB_PROG:
rec = true;
{
switch(command.get(1).asVocab()) {
case COMMAND_VOCAB_LEFT: {
printf("request of reprogrammming biases arrived \n");
D2Y->closeDevice();
Time::delay(1);
D2Y->setFromBinary(false);
D2Y->prepareBiases();
ok = true;
}
break;
case COMMAND_VOCAB_RIGHT: {
printf("request of reprogrammming biases arrived \n");
D2Y->closeDevice();
Time::delay(1);
D2Y->setFromBinary(false);
D2Y->prepareBiasesRight();
ok = true;
}
break;
default: {
}
break;
}
}
break;
case COMMAND_VOCAB_DUMP:
rec = true;
printf("recognised the command DUMP \n");
{
switch(command.get(1).asVocab()) {
case COMMAND_VOCAB_ON: {
printf("request of start dumping events arrived \n");
string filename = (string) command.get(2).asString();
if(strcmp(filename.c_str(),"")) {
D2Y->setDumpFile(filename);
D2Y->setDumpEvent(true);
printf("success in opening the dump file \n");
}
ok = true;
}
break;
case COMMAND_VOCAB_OFF: {
printf("request of stop dumping events arrived \n");
D2Y->setDumpEvent(false);
ok = true;
}
break;
}
}
break;
} //end of the outer switch
mutex.post();
if (!rec) {
ok = RFModule::respond(command,reply);
}
if (!ok) {
reply.clear();
reply.addVocab(COMMAND_VOCAB_FAILED);
}
else
reply.addVocab(COMMAND_VOCAB_OK);
return ok;
//return true;
}
bool RosNameSpace::writeToNameServer(PortWriter& cmd,
PortReader& reply,
const ContactStyle& style) {
DummyConnector con0;
cmd.write(con0.getWriter());
Bottle in;
in.read(con0.getReader());
ConstString key = in.get(0).asString();
ConstString arg1 = in.get(1).asString();
Bottle cmd2, cache;
if (key=="query") {
Contact c = queryName(arg1.c_str());
c.setName("");
Bottle reply2;
reply2.addString(arg1.c_str());
reply2.addString(c.toString().c_str());
DummyConnector con;
reply2.write(con.getWriter());
reply.read(con.getReader());
return true;
} else if (key=="list") {
cmd2.addString("getSystemState");
cmd2.addString("dummy_id");
if (!NetworkBase::write(getNameServerContact(), cmd2, cache, style)) {
fprintf(stderr, "Failed to contact ROS server\n");
return false;
}
Bottle out;
out.addVocab(Vocab::encode("many"));
Bottle *parts = cache.get(2).asList();
Property nodes;
Property topics;
Property services;
if (parts) {
for (int i=0; i<3; i++) {
Bottle *part = parts->get(i).asList();
if (!part) continue;
for (int j=0; j<part->size(); j++) {
Bottle *unit = part->get(j).asList();
if (!unit) continue;
ConstString stem = unit->get(0).asString();
Bottle *links = unit->get(1).asList();
if (!links) continue;
if (i<2) {
topics.put(stem, 1);
} else {
services.put(stem, 1);
}
for (int j=0; j<links->size(); j++) {
nodes.put(links->get(j).asString(), 1);
}
}
}
Property *props[3] = {&nodes, &topics, &services};
const char *title[3] = {"node", "topic", "service"};
for (int p=0; p<3; p++) {
Bottle blist;
blist.read(*props[p]);
for (int i=0; i<blist.size(); i++) {
ConstString name = blist.get(i).asList()->get(0).asString();
Bottle& info = out.addList();
info.addString(title[p]);
info.addString(name);
}
}
}
out.write(reply);
return true;
} else {
return false;
}
}
bool scriptModule::respond(const Bottle &command, Bottle &reply)
{
bool ret=true;
this->thread.mutex.wait();
if (command.size()!=0)
{
string cmdstring = command.get(0).asString().c_str();
{
if (cmdstring == "help")
{
cout << "Available commands:" << endl;
cout << "start" << endl;
cout << "stop" << endl;
cout << "reset" << endl;
reply.addVocab(Vocab::encode("ack"));
}
else if (cmdstring == "start")
{
if (this->thread.actions.current_action == 0)
this->thread.actions.current_status = ACTION_START;
else
this->thread.actions.current_status = ACTION_RUNNING;
reply.addVocab(Vocab::encode("ack"));
}
else if (cmdstring == "stop")
{
this->thread.actions.current_status = ACTION_IDLE;
reply.addVocab(Vocab::encode("ack"));
}
else if (cmdstring == "reset")
{
this->thread.actions.current_status = ACTION_IDLE;
this->thread.actions.current_action = 0;
reply.addVocab(Vocab::encode("ack"));
// Re-read sequence for torqueBalancing
string filenamePrefix;
if (rfCopy.check("torqueBalancingSequence")==true)
{
filenamePrefix = rfCopy.find("torqueBalancingSequence").asString().c_str();
// Overwrite the execute flag value. This option has higher priority and
// should simply stream trajectories use by the torqueBalancing module.
//!!!!: Temporarily put this flag to true so that constraints are also streamed, useful for icubWalkingIK
thread.enable_execute_joint_command = true;
if (!thread.actions.openTorqueBalancingSequence(filenamePrefix,rfCopy))
{
cout << "ERROR: Unable to parse torque balancing sequence" << endl;
return false;
}
}
this->thread.actions.openFile(filenamePrefix, rfCopy);
}
else
{
reply.addVocab(Vocab::encode("nack"));
ret = false;
}
}
}
else
{
reply.addVocab(Vocab::encode("nack"));
ret = false;
}
this->thread.mutex.post();
return ret;
}
bool respond(const Bottle &command, Bottle &reply)
{
int ack=Vocab::encode("ack");
int nack=Vocab::encode("nack");
if (command.size()>0)
{
switch (command.get(0).asVocab())
{
//-----------------
case VOCAB4('c','l','e','a'):
{
mutex.wait();
solver.clearItems();
solution=0.0;
mutex.post();
reply.addVocab(ack);
return true;
}
//-----------------
case VOCAB4('s','e','l','e'):
{
if (command.size()>=3)
{
string arm=command.get(1).asString().c_str();
string eye=command.get(2).asString().c_str();
if ((arm=="left") || (arm=="right"))
this->arm=arm;
if ((eye=="left") || (eye=="right"))
this->eye=eye;
if (this->arm=="left")
drvArmL.view(iarm);
else
drvArmR.view(iarm);
reply.addVocab(ack);
}
else
reply.addVocab(nack);
return true;
}
//-----------------
case VOCAB3('n','u','m'):
{
reply.addVocab(ack);
mutex.wait();
reply.addInt((int)solver.getNumItems());
mutex.post();
return true;
}
//-----------------
case VOCAB4('f','i','n','d'):
{
double error;
mutex.wait();
bool ok=solver.solve(solution,error);
mutex.post();
if (ok)
{
reply.addVocab(ack);
for (size_t i=0; i<solution.length(); i++)
reply.addDouble(solution[i]);
}
else
reply.addVocab(nack);
return true;
}
//-----------------
case VOCAB4('s','h','o','w'):
{
if (command.size()>=4)
{
solution[0]=command.get(1).asDouble();
solution[1]=command.get(2).asDouble();
solution[2]=command.get(3).asDouble();
reply.addVocab(ack);
}
else
reply.addVocab(nack);
return true;
}
//-----------------
case VOCAB3('t','i','p'):
{
if (tip.size()>=2)
{
reply.addVocab(ack);
reply.append(tip);
}
else
reply.addVocab(nack);
return true;
}
//-----------------
case VOCAB4('e','n','a','b'):
{
enabled=true;
reply.addVocab(ack);
return true;
}
//-----------------
case VOCAB4('d','i','s','a'):
{
enabled=false;
reply.addVocab(ack);
return true;
}
//-----------------
default:
return RFModule::respond(command,reply);
}
}
reply.addVocab(nack);
return true;
}
bool respond(const Bottle & command,
Bottle & reply)
{
// Get command string
string receivedCmd = command.get(0).asString().c_str();
bool f;
int responseCode; //Will contain Vocab-encoded response
reply.clear(); // Clear reply bottle
if (receivedCmd == "getPointClick")
{
if (! getPointClick())
{
//Encode response
responseCode = Vocab::encode("nack");
reply.addVocab(responseCode);
}
else
{
//Encode response
responseCode = Vocab::encode("ack");
reply.addVocab(responseCode);
Bottle& bCoords = reply.addList();
bCoords.addInt(coords2D(0));
bCoords.addInt(coords2D(1));
//bCoords.addDouble(coords3D(2));
}
return true;
} else if (receivedCmd == "getPointTrack")
{
tableHeight = command.get(1).asDouble();
if (!getPointTrack(tableHeight))
{
//Encode response
responseCode = Vocab::encode("nack");
reply.addVocab(responseCode);
}
else
{
//Encode response
printf("Sending out 3D point!!\n");
responseCode = Vocab::encode("ack");
reply.addVocab(responseCode);
Bottle& bCoords = reply.addList();
bCoords.addDouble(coords3D(0));
bCoords.addDouble(coords3D(1));
bCoords.addDouble(coords3D(2));
printf("Coords Bottle %s \n", bCoords.toString().c_str());
printf("sent reply %s \n", reply.toString().c_str());
}
return true;
} else if (receivedCmd == "getBox")
{
if (!getBox())
{
//Encode response
responseCode = Vocab::encode("nack");
reply.addVocab(responseCode);
}
else
{
//Encode response
responseCode = Vocab::encode("ack");
reply.addVocab(responseCode);
Bottle& bBox = reply.addList();
bBox.addInt(BBox.tl().x);
bBox.addInt(BBox.tl().y);
bBox.addInt(BBox.br().x);
bBox.addInt(BBox.br().y);
}
}
else if (receivedCmd == "help")
{
reply.addVocab(Vocab::encode("many"));
reply.addString("Available commands are:");
reply.addString("help");
reply.addString("quit");
reply.addString("getPointClick - reads a click and returns the 2D coordinates");
reply.addString("getPointTrack - Retrieves 2D coords and returns the 3D coordinates of the object tracked based on the table Height");
reply.addString("getBox - Crops the image based on user input and creates a template for the tracker with it");
return true;
}
else if (receivedCmd == "quit")
{
reply.addString("Quitting.");
return false;
} else{
reply.addString("Invalid command, type [help] for a list of accepted commands.");
}
return true;
}
bool respond(const Bottle& command, Bottle& reply)
{
stringstream temp;
string helpMessage = string(getName().c_str()) + " commands are: ";
reply.clear();
TorqueCtrlTestCommand com;
Bottle param;
if(!identifyCommand(command, com, param)){
reply.addString("Unknown command. Input 'help' to get a list of the available commands.");
return true;
}
switch( com ){
case quit: reply.addString("quitting"); return false;
case help:
reply.addVocab(Vocab::encode("many")); // print every string added to the bottle on a new line
reply.addString(helpMessage.c_str());
for(unsigned int i=0; i< SFC_COMMAND_COUNT; i++){
reply.addString( ("- "+TorqueCtrlTestCommand_s[i]+": "+TorqueCtrlTestCommand_desc[i]).c_str() );
}
return true;
case no_ctrl: contrThread->setCtrlMode(NO_CONTROL); break;
case set_ctrl:
{
if(param.size()<1 || !(param.get(0).isInt() || param.get(0).isDouble())){
reply.addString("Error: control law number missing or not an int.");
return true;
}
int cl = param.get(0).asInt();
if(cl<0 || cl>=CONTROL_MODE_SIZE){
reply.addString("Error: control law number out of range.");
return true;
}
contrThread->setCtrlMode((ControlMode)cl);
reply.addString(("Control law "+ControlMode_desc[cl]+"set.").c_str());
return true;
}
case get_ctrl:
{
int cl = (int)contrThread->getCtrlMode();
reply.addInt(cl);
reply.addString(ControlMode_desc[cl].c_str());
return true;
}
case get_kp:
reply.addDouble(contrThread->getKp());
return true;
case get_kd:
reply.addDouble(contrThread->getKd());
return true;
case get_ki:
reply.addDouble(contrThread->getKi());
return true;
case get_ktao:
reply.addDouble(contrThread->getKtao());
return true;
case get_kbemf:
reply.addDouble(contrThread->getKbemf());
return true;
case get_kstic:
reply.addDouble(contrThread->getKstic());
reply.addDouble(contrThread->getKcoulomb());
return true;
case get_kcp:
reply.addDouble(contrThread->getKcp());
return true;
case get_kcn:
reply.addDouble(contrThread->getKcn());
return true;
case get_kdither:
reply.addDouble(contrThread->getKdither());
return true;
case get_wdither:
reply.addDouble(contrThread->getWdither());
return true;
case get_est_thr:
reply.addDouble(contrThread->getEstThr());
return true;
case get_est_wind:
reply.addDouble(contrThread->getEstWind());
return true;
case get_taod:
{
Vector taod = contrThread->getTaod();
reply.addString(taod.toString(3).c_str());
return true;
}
case get_tao:
{
Vector tao = contrThread->getTao();
reply.addString(tao.toString(3).c_str());
return true;
}
case get_alpha:
reply.addDouble(contrThread->getAlpha());
reply.addDouble(contrThread->getAlphaStic());
return true;
case get_joint:
reply.addInt(contrThread->getJoint());
return true;
case get_pwm:
reply.addInt((int)contrThread->getPwmD());
return true;
case get_qd:
reply.addDouble(contrThread->getQd());
return true;
case get_traj_time:
reply.addDouble(contrThread->getTrajTime());
return true;
case get_body_part:
reply.addInt(contrThread->getBodyPart());
reply.addString(BodyPart_s[contrThread->getBodyPart()].c_str());
return true;
case get_impedance:
reply.addString("Desired inertia");
reply.addDouble(contrThread->getMd());
reply.addString("Real inertia");
reply.addDouble(contrThread->getM());
reply.addString("Damping");
reply.addDouble(contrThread->getB());
reply.addString("Stiffness");
reply.addDouble(contrThread->getK());
return true;
case set_kp:
{
try{
contrThread->setKp(param.get(0).asDouble());
reply.addString("kp set successfully.");
}catch(runtime_error &e){
reply.addString("set kp failed: ");
reply.addString(e.what());
}
return true;
}
case set_kd:
{
try{
contrThread->setKd(param.get(0).asDouble());
reply.addString("kd set successfully.");
}catch(runtime_error &e){
reply.addString("set kd failed: ");
reply.addString(e.what());
}
return true;
}
case set_ki:
{
try{
contrThread->setKi(param.get(0).asDouble());
reply.addString("ki set successfully.");
}catch(runtime_error &e){
reply.addString("set ki failed: ");
reply.addString(e.what());
}
return true;
}
case set_ktao:
{
try{
contrThread->setKtao(param.get(0).asDouble());
reply.addString("ktao set successfully.");
}catch(runtime_error &e){
reply.addString("set ktao failed: ");
reply.addString(e.what());
}
return true;
}
case set_kbemf:
{
try{
contrThread->setKbemf(param.get(0).asDouble());
reply.addString("kbemf set successfully.");
}catch(runtime_error &e){
reply.addString("set kbemf failed: ");
reply.addString(e.what());
}
return true;
}
case set_kstic:
{
try{
contrThread->setKstic(param.get(0).asDouble());
if(param.size()>1)
contrThread->setKcoulomb(param.get(1).asDouble());
reply.addString("kstic set successfully.");
}catch(runtime_error &e){
reply.addString("set kstic failed: ");
reply.addString(e.what());
}
return true;
}
case set_kcp:
{
try{
contrThread->setKcp(param.get(0).asDouble());
reply.addString("kcp set successfully.");
}catch(runtime_error &e){
reply.addString("set kcp failed: ");
reply.addString(e.what());
}
return true;
}
case set_kcn:
{
try{
contrThread->setKcn(param.get(0).asDouble());
reply.addString("kcn set successfully.");
}catch(runtime_error &e){
reply.addString("set kcn failed: ");
reply.addString(e.what());
}
return true;
}
case set_kdither:
{
try{
contrThread->setKdither(param.get(0).asDouble());
reply.addString("kdither set successfully.");
}catch(runtime_error &e){
reply.addString("set kdither failed: ");
reply.addString(e.what());
}
return true;
}
case set_wdither:
{
try{
contrThread->setWdither(param.get(0).asDouble());
reply.addString("wdith set successfully.");
}catch(runtime_error &e){
reply.addString("set wdith failed: ");
reply.addString(e.what());
}
return true;
}
case set_est_wind:
{
try{
contrThread->setEstWind(param.get(0).asInt());
reply.addString("est wind set successfully.");
}catch(runtime_error &e){
reply.addString("set est wind failed: ");
reply.addString(e.what());
}
return true;
}
case set_est_thr:
{
try{
contrThread->setEstThr(param.get(0).asDouble());
reply.addString("est thr set successfully.");
}catch(runtime_error &e){
reply.addString("set est thr failed: ");
reply.addString(e.what());
}
return true;
}
case set_taod:
{
try{
if(param.size()>1){
contrThread->setTaod(bottle2vector(param));
}else{
contrThread->setTaod(param.get(0).asDouble());
}
reply.addString("taod set successfully.");
}catch(runtime_error &e){
reply.addString("set taod failed: ");
reply.addString(e.what());
}
return true;
}
case set_alpha:
{
try{
contrThread->setAlpha(param.get(0).asDouble());
if(param.size()>1)
contrThread->setAlphaStic(param.get(1).asDouble());
reply.addString("alpha set successfully.");
}catch(runtime_error &e){
reply.addString("set alpha failed: ");
reply.addString(e.what());
}
return true;
}
case set_joint:
try{
contrThread->setJoint(param.get(0).asInt());
reply.addString("joint set successfully.");
}catch(runtime_error &e){
reply.addString("set joint failed: ");
reply.addString(e.what());
}
return true;
case set_pwm:
try{
contrThread->setPwmD(param.get(0).asDouble());
reply.addString("pwm set successfully.");
}catch(runtime_error &e){
reply.addString("set pwm failed: ");
reply.addString(e.what());
}
return true;
case set_qd:
try{
contrThread->setQd(param.get(0).asDouble());
reply.addString("qd set successfully.");
}catch(runtime_error &e){
reply.addString("set qd failed: ");
reply.addString(e.what());
}
return true;
case set_traj_time:
try{
contrThread->setTrajTime(param.get(0).asDouble());
reply.addString("traj time set successfully.");
}catch(runtime_error &e){
reply.addString("set traj time failed: ");
reply.addString(e.what());
}
return true;
case set_body_part:
try{
contrThread->setBodyPart((BodyPart)param.get(0).asInt());
reply.addString("body part set successfully.");
}catch(runtime_error &e){
reply.addString("set body part failed: ");
reply.addString(e.what());
}
return true;
case set_impedance:
try{
if(param.size()==2)
{
if(param.get(0).asInt()==0) contrThread->setMd(param.get(1).asDouble());
else if(param.get(0).asInt()==1) contrThread->setM(param.get(1).asDouble());
else if(param.get(0).asInt()==2) contrThread->setB(param.get(1).asDouble());
else if(param.get(0).asInt()==3) contrThread->setK(param.get(1).asDouble());
if(param.get(0).asInt()>3 || param.get(0).asInt()<0)
reply.addString("Error: first parameter has to be an int in [0, 3].");
else
reply.addString("Impedance value set.");
}
else if(param.size()==4)
{
contrThread->setMd(param.get(0).asDouble());
contrThread->setM(param.get(1).asDouble());
contrThread->setB(param.get(2).asDouble());
contrThread->setK(param.get(3).asDouble());
reply.addString("Impedance parameters set.");
}
else
reply.addString("Error: this method accepts either 2 or 4 parameters.");
return true;
}catch(runtime_error &e){
reply.addString("set impedance failed: ");
reply.addString(e.what());
}
case reset_pid:
{
contrThread->resetTorquePid();
break;
}
case sim_on: contrThread->setSimMode(true); break;
case sim_off: contrThread->setSimMode(false); break;
default: reply.addString("ERROR: This command is known but it is not managed in the code."); return true;
}
reply.addString( (TorqueCtrlTestCommand_s[com]+" command received.").c_str());
return true;
}
void CtrlThread::run() {
Bottle controlCmd;
Bottle simCmd;
//int objIndex = 0;
//int toolIndex = 0;
double toolPose = 0.0;
double toolDisp = 0.0;
double toolTilt = 0.0;
double objPosX = -0.17;
double objPosZ = 0.45;
NetInt32 code;
if( (*simToolLoaderCmdInputPort).read(controlCmd, true) ) {
code = Vocab::encode((controlCmd.get(0)).asString());
cout << (controlCmd.get(0)).asString() << endl;
switch(code) {
case VOCAB3('d','e','l'):
{
cout << "Clear world" <<endl;
//Clear the World:
simCmd = simWorld.deleteAll();
writeSim(simCmd);
//reply to the control manager
controlCmd.clear();
controlCmd.addVocab(code);
replyCmd(controlCmd);
}
break;
case VOCAB3('o','b','j'):
{
cout << "Create table and object (on the table)." <<endl;
//Clear the World:
simCmd = simWorld.deleteAll();
writeSim(simCmd);
//Create the table:
//simWorld.simTable->objSubIndex = 1; //needs to be automatic, each type of object has his own subIndex
simCmd = simWorld.simTable->makeObjectBottle(simWorld.objSubIndex, false);
writeSim(simCmd);
//---------------------------------------------------------------
//Create one object:
objIndex = controlCmd.get(1).asInt();
if ( objIndex==100 ) {
objIndex = rand() % simWorld.simObject.size() + 1;
}
Rand randG; // YARP random number generator
//simWorld.simObject[objIndex]->objSubIndex = 1; //needs to be automatic, each type of object has his own subIndex
simWorld.simObject[objIndex]->setObjectPosition(
threadTable.get(4).asDouble()+objPosX + randG.scalar(-0.04, 0.04), // Add a randomness of -+ 4 cm to the X position of the cube
threadTable.get(5).asDouble()+((threadTable.get(2).asDouble())/2) + 0.05, // Place the object 5 cmd above the table
objPosZ + randG.scalar(-0.04, 0.04)); // Add a randomness of -+ 4 cm to the X position of the cube
simCmd = simWorld.simObject[objIndex]->makeObjectBottle(simWorld.objSubIndex);
writeSim(simCmd);
//reply to the control manager the tool ID
controlCmd.clear();
controlCmd.addVocab(code);
controlCmd.addInt(objIndex);
replyCmd(controlCmd);
}
break;
case VOCAB4('m','o','v','e'):
{
cout << "Move object to original position on table." <<endl;
objIndex = controlCmd.get(1).asInt();
//---------------------------------------------------------------
//Create one object:
//simWorld.simObject[objIndex]->objSubIndex = 1; //needs to be automatic, each type of object has his own subIndex
if (simWorld.simObject[objIndex]->objSubIndex != 0)
{
Rand randG; // YARP random number generator
simWorld.simObject[objIndex]->setObjectPosition(
threadTable.get(4).asDouble()+objPosX + randG.scalar(-0.04, 0.04),
threadTable.get(5).asDouble()+((threadTable.get(2).asDouble())/2) + 0.05,
objPosZ + randG.scalar(-0.04, 0.04));
simCmd = simWorld.simObject[objIndex]->moveObjectBottle();
writeSim(simCmd);
simWorld.simObject[objIndex]->setObjectRotation(0, 0, 0);
simCmd = simWorld.simObject[objIndex]->rotateObjectBottle();
writeSim(simCmd);
//reply to the control manager the tool ID
controlCmd.clear();
controlCmd.addVocab(code);
controlCmd.addInt(objIndex);
replyCmd(controlCmd);
}else {
cout << "No object of type " << objIndex << " is present, so it can't be moved." <<endl;
controlCmd.clear();
controlCmd.addString("nack");
controlCmd.addInt(objIndex);
replyCmd(controlCmd);
}
}
break;
case VOCAB4('t','o','o','l'):
{
cout << "Create the tool in the hand, the objects on the table." <<endl;
//Clear the World:
simCmd = simWorld.deleteAll();
writeSim(simCmd);
//Create the table:
//simWorld.simTable->objSubIndex = 1; //needs to be automatic, each type of object has his own subIndex
simCmd = simWorld.simTable->makeObjectBottle(simWorld.objSubIndex, false);
writeSim(simCmd);
//------------------------------------------------------------------
//Create one tool in the hand:
toolIndex = controlCmd.get(1).asInt();
int numTools = simWorld.simObject.size();
if (toolIndex > numTools){
cout << "No tool with the given index. Max tool Index is :" << numTools<< endl;
break;
}
if ( toolIndex==100 ) {
toolIndex = rand() % simWorld.simObject.size() + 1;
}
//receive the orientation of the tool
toolPose = controlCmd.get(3).asDouble();
toolPose -= 90; // Add 90 degrees to the tool orientation to orient it to the front by default.
toolDisp = controlCmd.get(4).asDouble(); // Longitudinal displacement along the tool grasp, in cm
toolTilt = controlCmd.get(5).asDouble(); // Tilt of the tool wrt the Y axis. 0 means tool grasped along -Y axis, 90 along X axis
if (toolTilt > 90.0) { toolTilt = 90.0; }
if (toolTilt < 0.0) { toolTilt = 0.0; }
cout << "Requested to create tool " << toolIndex << " with orientation " << toolPose << " extension " << toolDisp << ", and tilt " << toolTilt << endl;
toolTilt = 90 - toolTilt; // Because rotation in SIM goes on oppoiste axis as on PCL visualizer.
/* Tool to hand transformation*/
// Transformation to express coordinate from the tool in the hand coordinate system.
Vector rot1(4), rot2(4), rot3(4);
// Orientation in all 3 axis in axis-angle notation
//--- this works on positions around the rest position (0 10 0 90 -10)
rot1[0] = 0.0; rot1[1] = 1.0; rot1[2] = 0.0; rot1[3] = M_PI/2; // introduce 90 degree rot on Y axis and perform it first, to change the tool orientation (along Z) to aling
rot2[0] = 0.0; rot2[1] = 0.0; rot2[2] = 1.0; rot2[3] = toolTilt*M_PI/180; // rotate 'tilt' aroudn the new X axis (hands -Z axis) to set the position of the tool w.r.t the hand
rot3[0] = 1.0; rot3[1] = 0.0; rot3[2] = 0.0; rot3[3] = M_PI/2 + toolPose*M_PI/180; //rotate around the tool axis (Z) to select the tool orientation.
//--- this works on positions around upper position (-40 100 50 70 -10)
//rot1[0] = 0.0; rot1[1] = 1.0; rot1[2] = 0.0; rot1[3] = M_PI/2; // introduce 90 degree rot on Y axis and perform it first, to change the tool orientation (along Z) to aling
//rot2[0] = 0.0; rot2[1] = 1.0; rot2[2] = 0.0; rot2[3] = 45*M_PI/180;
//rot3[0] = 1.0; rot3[1] = 0.0; rot3[2] = 0.0; rot3[3] = M_PI/2 + toolPose*M_PI/180;
cout << "Tool oriented "<< toolPose <<" deg wrt hand. " << endl;
Matrix Rrot1 = axis2dcm(rot1); //printf("Rotation in Y in hand coords:\n %s \n", Rrot1.toString().c_str());
Matrix Rrot2 = axis2dcm(rot2); //printf("Rotation in X in hand coords:\n %s \n", Rrot2.toString().c_str());
Matrix Rrot3 = axis2dcm(rot3); //printf("Rotation in Z in hand coords:\n %s \n", Rrot3.toString().c_str());
// rotate first around Y axis to orient tool along the hand's X axis
// then around the X axis to tilt the tool 45deg wrt the hand
// finally around the tool axis (Z) axis to tool-pose (orientation) of the tool effector
Matrix T2H = Rrot3 * Rrot2 * Rrot1; // Mutiply from right to left.
//Matrix T2H = Rrot2*Rrot1; // Mutiply from right to left.
//Matrix T2H(4,4);
//T2H.eye();
//printf("Hand to tool rotation matrix:\n %s \n", H2T.toString().c_str());
T2H(2,3) = 0.03; // This accounts for the traslation of 3 cm in the Z axis in the hand coord system.
T2H(1,3) = -toolDisp /100; // This accounts for the traslation of 'toolDisp' in the -Y axis in the hand coord system along the extended thumb).
printf("Tool to Hand transformation matrix (T2H):\n %s \n", T2H.toString().c_str());
Vector T2Hrpy = dcm2rpy(T2H); // from rot Matrix to roll pitch yaw
//Vector T2HrpyDeg = T2Hrpy *(180.0/M_PI);
//printf("Orientation of tool wrt to the hand, in degrees:\n %s \n",T2HrpyDeg.toString().c_str());
/* Hand to Robot transformation*/
// Transformation to express coordinates from the hand coordinate system in the robot coordinate system.
printf("getting Pose form icart... \n");
Vector posRobot, H2Raa, H2Rrpy, H2RrpyDeg;
icart->getPose(posRobot, H2Raa); // Get hand to robot rotation matrix from iCart.
Matrix H2R=axis2dcm(H2Raa); // from axis/angle to rotation matrix notation
H2Rrpy = dcm2rpy(H2R); // from rot Matrix to roll pitch yaw
//H2RrpyDeg = H2Rrpy *(180.0/M_PI);
//printf("Orientation of hand in robot coords, in degrees:\n %s \n",H2RrpyDeg.toString().c_str());
// Include translation
H2R(0,3)= posRobot[0];
H2R(1,3)= posRobot[1];
H2R(2,3)= posRobot[2];
printf("Hand to robot transformatoin matrix (H2R):\n %s \n", H2R.toString().c_str());
/* Robot to World transformation */
// Transformation to express coordinate from the robot coordinate system in the world coordinate system.
Matrix R2W(4,4);
// pose x-axis y-axis z-axis translation
R2W(0,0)= 0.0; R2W(0,1)= -1.0; R2W(0,2)= 0.0; R2W(0,3)= 0.0; // x-coordinate in root frame
R2W(1,0)= 0.0; R2W(1,1)= 0.0; R2W(1,2)= 1.0; R2W(1,3)= 0.5976; // y-coordinate "
R2W(2,0)=-1.0; R2W(2,1)= 0.0; R2W(2,2)= 0.0; R2W(2,3)= -0.026; // z-coordinate "
R2W(3,0)= 0.0; R2W(3,1)= 0.0; R2W(3,2)= 0.0; R2W(3,3)= 1.0; // Translation
printf("Robot to World Transf matrix is:\n %s \n", R2W.toString().c_str());
/* Transformation concatenation*/
Matrix T2R = H2R*T2H;
printf("Tool to Robot transformation matrix (T2R= H2R*T2H):\n %s \n",T2R.toString().c_str());
Matrix T2W = R2W*T2R; // trasnform rotated tool orientation to World frame
printf("Tool to World transformation matrix (T2W = R2W*T2R): \n %s \n", T2W.toString().c_str());
/* Decomposition of transformation matrix into rotation and translation */
yarp::sig::Vector posWorld(3); //Change coordinate frame from robot to world.
posWorld[0]= T2W(0,3);
posWorld[1]= T2W(1,3);
posWorld[2]= T2W(2,3);
Vector T2Wrpy = dcm2rpy(T2W); // from rot Matrix to roll pitch yaw
Vector T2WrpyDeg = T2Wrpy *(180.0/M_PI);
printf("Orientation of tool in world coords, in degrees:\n %s \n",T2WrpyDeg.toString().c_str());
/* create rotate and grab the tool */
simWorld.simObject[toolIndex]->setObjectPosition(posWorld[0],posWorld[1],posWorld[2]);//(0.23, 0.70, 0.20); //left arm end effector position
simCmd = simWorld.simObject[toolIndex]->makeObjectBottle(simWorld.objSubIndex);
writeSim(simCmd);
//simWorld.simObject[toolIndex]->setObjectRotation(70, 120, 30);
simWorld.simObject[toolIndex]->setObjectRotation(T2WrpyDeg[0],T2WrpyDeg[1],T2WrpyDeg[2]);//(-65, -5, 110);
simCmd = simWorld.simObject[toolIndex]->rotateObjectBottle();
writeSim(simCmd);
simCmd = simWorld.simObject[toolIndex]->grabObjectBottle(RIGHT); //right arm by default
writeSim(simCmd);
// Get and return the name of the loaded tool
string modelName = simWorld.simObject[toolIndex]->getObjName();
cout << endl << "Model " << modelName << " has been loaded." << endl;
//------------------------------------------------------------------
//Create one object:
objIndex = controlCmd.get(2).asInt();
if ( objIndex==100 ) {
objIndex = rand() % simWorld.simObject.size() + 1;
}
//simWorld.simObject[objIndex]->objSubIndex = 1; //needs to be automatic, each type of object has his own subIndex
simWorld.simObject[objIndex]->setObjectPosition(
threadTable.get(4).asDouble()+objPosX,
threadTable.get(5).asDouble()+((threadTable.get(2).asDouble())/2)+0.1,
objPosZ);
simCmd = simWorld.simObject[objIndex]->makeObjectBottle(simWorld.objSubIndex);
writeSim(simCmd);
//reply to the control manager the tool and object IDs
controlCmd.clear();
controlCmd.addVocab(code);
controlCmd.addInt(toolIndex);
controlCmd.addString(modelName);
controlCmd.addInt(objIndex);
replyCmd(controlCmd);
}
break;
case VOCAB3('r','o','t'):
{
cout << "Rotate the tool in the hand." <<endl;
//receive the orientation of the tool
toolPose = controlCmd.get(1).asDouble();
toolPose -= 90; // Add 90 degrees to the tool orientation to orient it to the front by default.
toolDisp = controlCmd.get(2).asDouble(); // Longitudinal displacement along the tool grasp, in cm
toolTilt = controlCmd.get(3).asDouble(); // Tilt of the tool wrt the Y axis. 0 means tool grasped along -Y axis, 90 along X axis
if (toolTilt > 90.0) { toolTilt = 90.0; }
if (toolTilt < 0.0) { toolTilt = 0.0; }
cout << "Requested to regrasp tool " << toolIndex << " with orientation " << toolPose << " extension " << toolDisp << ", and tilt " << toolTilt << endl;
toolTilt = 90 - toolTilt; // Because rotation in SIM goes on oppoiste axis as on PCL visualizer.
// Tool to hand transformation
// Transformation to express coordinate from the tool in the hand coordinate system.
Vector newRot1(4), newRot2(4), newRot3(4);
// Orientation in all 3 axis in axis-angle notation
//--- this works on positions around the rest position (0 10 0 90 -10)
newRot1[0] = 0.0; newRot1[1] = 1.0; newRot1[2] = 0.0; newRot1[3] = M_PI/2; // introduce 90 degree rot on Y axis and perform it first, to change the tool orientation (along Z) to aling
newRot2[0] = 0.0; newRot2[1] = 0.0; newRot2[2] = 1.0; newRot2[3] = toolTilt*M_PI/180; // rotate 45 aroudn the new X axis (hands -Z axis) to set the position of the tool w.r.t the hand
newRot3[0] = 1.0; newRot3[1] = 0.0; newRot3[2] = 0.0; newRot3[3] = M_PI/2 + toolPose*M_PI/180; //rotate around the tool axis (Z) to select the tool orientation.
//--- this works on positions around upper position (-40 100 50 70 -10)
//newRot1[0] = 0.0; newRot1[1] = 1.0; newRot1[2] = 0.0; newRot1[3] = M_PI/2; // introduce 90 degree rot on Y axis and perform it first, to change the tool orientation (along Z) to aling
//newRot2[0] = 0.0; newRot2[1] = 1.0; newRot2[2] = 0.0; newRot2[3] = 45*M_PI/180;
//newRot3[0] = 1.0; newRot3[1] = 0.0; newRot3[2] = 0.0; newRot3[3] = M_PI/2 + toolPose*M_PI/180;
cout << "Tool oriented " << toolPose << " deg wrt hand"<< endl;
Matrix newRrot1 = axis2dcm(newRot1); //printf("Rotation in Y in hand coords:\n %s \n", Rrot1.toString().c_str());
Matrix newRrot2 = axis2dcm(newRot2); //printf("Rotation in X in hand coords:\n %s \n", Rrot2.toString().c_str());
Matrix newRrot3 = axis2dcm(newRot3); //printf("Rotation in Z in hand coords:\n %s \n", Rrot3.toString().c_str());
// rotate first around Y axis to orient tool along the hand's X axis
// then around the X axis to tilt the tool 45deg wrt the hand
// finally around the tool axis (Z) axis to tool-pose (orientation) of the tool effector
Matrix newT2H = newRrot3 * newRrot2 * newRrot1; // Mutiply from right to left.
//printf("Hand to tool rotation matrix:\n %s \n", H2T.toString().c_str());
newT2H(2,3) = 0.03; // This accounts for the traslation of 3 cm in the Z axis in the hand coord system.
newT2H(1,3) = -toolDisp /100; // This accounts for the traslation of 'toolDisp' in the -Y axis in the hand coord system along the extended thumb).
printf("Tool to Hand transformatoin matrix (T2H):\n %s \n", newT2H.toString().c_str());
Vector newT2Hrpy = dcm2rpy(newT2H); // from rot Matrix to roll pitch yaw
//Vector newT2HrpyDeg = newT2Hrpy *(180.0/M_PI);
//printf("Orientation of tool wrt to the hand, in degrees:\n %s \n",newT2HrpyDeg.toString().c_str());
// Hand to Robot transformation
// Transformation to express coordinates from the hand coordinate system in the robot coordinate system.
printf("getting Pose form icart... \n");
Vector newPosRobot, newH2Raa, newH2Rrpy, newH2RrpyDeg;
icart->getPose(newPosRobot, newH2Raa); // Get hand to robot rotation matrix from iCart.
Matrix newH2R=axis2dcm(newH2Raa); // from axis/angle to rotation matrix notation
newH2Rrpy = dcm2rpy(newH2R); // from rot Matrix to roll pitch yaw
//newH2RrpyDeg = newH2Rrpy *(180.0/M_PI);
//printf("Orientation of hand in robot coords, in degrees:\n %s \n",newH2RrpyDeg.toString().c_str());
// Include translation
newH2R(0,3)= newPosRobot[0];
newH2R(1,3)= newPosRobot[1];
newH2R(2,3)= newPosRobot[2];
printf("Hand to robot transformatoin matrix (H2R):\n %s \n", newH2R.toString().c_str());
// Robot to World transformation
// Transformation to express coordinate from the robot coordinate system in the world coordinate system.
Matrix newR2W(4,4);
// pose x-axis y-axis z-axis translation
newR2W(0,0)= 0.0; newR2W(0,1)= -1.0; newR2W(0,2)= 0.0; newR2W(0,3)= 0.0; // x-coordinate in root frame
newR2W(1,0)= 0.0; newR2W(1,1)= 0.0; newR2W(1,2)= 1.0; newR2W(1,3)= 0.5976; // y-coordinate "
newR2W(2,0)=-1.0; newR2W(2,1)= 0.0; newR2W(2,2)= 0.0; newR2W(2,3)= -0.026; // z-coordinate "
newR2W(3,0)= 0.0; newR2W(3,1)= 0.0; newR2W(3,2)= 0.0; newR2W(3,3)= 1.0; // Translation
printf("Robot to World Transf matrix is:\n %s \n", newR2W.toString().c_str());
// Transformation concatenation
Matrix newT2R = newH2R*newT2H;
printf("Tool to Robot transformation matrix (T2R= H2R*T2H):\n %s \n",newT2R.toString().c_str());
Matrix newT2W = newR2W*newT2R; // trasnform rotated tool orientation to World frame
printf("Tool to World transformation matrix (T2W = R2W*T2R): \n %s \n", newT2W.toString().c_str());
// Decomposition of transformation matrix into rotation and translation
yarp::sig::Vector newPosWorld(3); //Change coordinate frame from robot to world.
newPosWorld[0]= newT2W(0,3);
newPosWorld[1]= newT2W(1,3);
newPosWorld[2]= newT2W(2,3);
Vector newT2Wrpy = dcm2rpy(newT2W); // from rot Matrix to roll pitch yaw
Vector newT2WrpyDeg = newT2Wrpy *(180.0/M_PI);
printf("Orientation of tool in world coords, in degrees:\n %s \n",newT2WrpyDeg.toString().c_str());
// Set new position and orientation
simWorld.simObject[toolIndex]->setObjectRotation(newT2WrpyDeg[0],newT2WrpyDeg[1],newT2WrpyDeg[2]);// new regrasp position
simWorld.simObject[toolIndex]->setObjectPosition(newPosWorld[0],newPosWorld[1],newPosWorld[2]); // new regrasp orientation
// Release grasp-magnet to allow regrasping
simCmd = simWorld.simObject[toolIndex]->releaseObjectBottle(RIGHT); //right arm by default
writeSim(simCmd);
// send new position command
simCmd = simWorld.simObject[toolIndex]->moveObjectBottle();
writeSim(simCmd);
// send new orientation command
simCmd = simWorld.simObject[toolIndex]->rotateObjectBottle();
writeSim(simCmd);
// Reactivate grasp-magnet to fix tool in place.
simCmd = simWorld.simObject[toolIndex]->grabObjectBottle(RIGHT); //right arm by default
writeSim(simCmd);
//reply to the control manager the tool and object IDs
controlCmd.clear();
controlCmd.addVocab(code);
controlCmd.addInt(toolIndex);
controlCmd.addDouble(toolPose+90); // +90 to cancel the 90 substracted before.
controlCmd.addDouble(toolDisp);
replyCmd(controlCmd);
}
break;
case VOCAB4('h','e','l','p'):
{
printf("Help request received. \n ");
controlCmd.clear();
controlCmd.addVocab(code);
controlCmd.addString("Available commands are:");
controlCmd.addString("help");
controlCmd.addString("delete - delete all object in sim worlds");
controlCmd.addString("obj (int) - creates an object on the table (0 = cube).");
controlCmd.addString("move objIdex(int) - moves object índex to original position on table (0 = cube).");
controlCmd.addString("tool toolIndex(int) objIndex(int) orient(int) pos(int) - creates tool 'toolIndex' at the robots hand with orientation 'orient' and displacement 'pos', and object 'objIndex' on the table.");
controlCmd.addString("rot orient(int) pos(int) - moved the tool in hand to displacement 'pos' (in cm) and rotation 'orient'. ");
replyCmd(controlCmd);
}
break;
}
}
}
virtual bool respond(const Bottle &command, Bottle &reply)
{
bool ret=true;
this->w_thread.mutex.wait();
if (command.size()!=0)
{
string cmdstring = command.get(0).asString().c_str();
{
if (cmdstring == "help")
{
cout << "Available commands:" << endl;
cout << "start" << endl;
cout << "stop" << endl;
cout << "reset" << endl;
cout << "clear" << endl;
cout << "add" << endl;
cout << "load" << endl;
cout << "forever" << endl;
cout << "list" << endl;
reply.addVocab(Vocab::encode("many"));
reply.addVocab(Vocab::encode("ack"));
reply.addString("Available commands:");
reply.addString("start");
reply.addString("stop");
reply.addString("reset");
reply.addString("clear");
reply.addString("add");
reply.addString("load");
reply.addString("forever");
reply.addString("list");
}
else if (cmdstring == "start")
{
if (this->w_thread.actions.current_action == 0)
this->w_thread.actions.current_status = ACTION_START;
else
this->w_thread.actions.current_status = ACTION_RUNNING;
this->w_thread.actions.forever = false;
this->b_thread.attachActions(&w_thread.actions);
if (this->b_thread.isRunning()==false) b_thread.start();
reply.addVocab(Vocab::encode("ack"));
}
else if (cmdstring == "forever")
{
if (this->w_thread.actions.current_action == 0)
this->w_thread.actions.current_status = ACTION_START;
else
this->w_thread.actions.current_status = ACTION_RUNNING;
w_thread.actions.forever = true;
reply.addVocab(Vocab::encode("ack"));
}
else if (cmdstring == "stop")
{
this->w_thread.actions.current_status = ACTION_STOP;
if (this->b_thread.isRunning()==true) b_thread.askToStop();
reply.addVocab(Vocab::encode("ack"));
}
else if (cmdstring == "clear")
{
this->w_thread.actions.clear();
reply.addVocab(Vocab::encode("ack"));
}
else if (cmdstring == "add")
{
if(!w_thread.actions.parseCommandLine(command.get(1).asString().c_str(), -1, robot.n_joints))
{
yError() << "Unable to parse command";
reply.addVocab(Vocab::encode("ERROR Unable to parse file"));
}
else
{
yInfo() << "Command added";
reply.addVocab(Vocab::encode("ack"));
}
}
else if (cmdstring == "load")
{
string filename = command.get(1).asString().c_str();
if (!w_thread.actions.openFile(filename, robot.n_joints))
{
yError() << "Unable to parse file";
reply.addVocab(Vocab::encode("ERROR Unable to parse file"));
}
else
{
yInfo() << "File opened";
reply.addVocab(Vocab::encode("ack"));
}
}
else if (cmdstring == "reset")
{
this->w_thread.actions.current_status = ACTION_RESET;
this->w_thread.actions.current_action = 0;
if (this->b_thread.isRunning()==true) b_thread.askToStop();
reply.addVocab(Vocab::encode("ack"));
}
else if (cmdstring == "list")
{
this->w_thread.actions.print();
reply.addVocab(Vocab::encode("ack"));
}
else
{
reply.addVocab(Vocab::encode("nack"));
ret = false;
}
}
}
else
{
reply.addVocab(Vocab::encode("nack"));
ret = false;
}
this->w_thread.mutex.post();
return ret;
}
bool targetFinderModule::respond(const Bottle& command, Bottle& reply) {
reply.clear();
bool ok = false;
bool rec = false; // is the command recognized?
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");
}
respondLock.wait();
switch (command.get(0).asVocab()) {
case COMMAND_VOCAB_HELP:
rec = true;
{
//reply.addString("many"); // what used to work
reply.addString("help");
reply.addString("commands are:");
reply.addString(" help : to get help");
reply.addString(" quit : to quit the module");
reply.addString(" ");
reply.addString(" ");
reply.addString(" sus ");
reply.addString(" sus ");
reply.addString(" ");
reply.addString(" ");
reply.addString(" ");
reply.addString(" ");
//reply.addString(helpMessage.c_str());
ok = true;
}
break;
case COMMAND_VOCAB_QUIT:
rec = true;
{
reply.addString("quitting");
ok = false;
}
break;
case COMMAND_VOCAB_SUSPEND:
{
reply.addString("suspending");
ok = true;
tf->suspend();
}
break;
case COMMAND_VOCAB_RESUME:
{
reply.addString("resuming");
ok = true;
tf->resume();
}
break;
default:
rec = false;
ok = false;
}
respondLock.post();
if (!rec){
ok = RFModule::respond(command,reply);
}
if (!ok) {
reply.clear();
reply.addVocab(COMMAND_VOCAB_FAILED);
}
else
reply.addVocab(COMMAND_VOCAB_OK);
return ok;
}
bool HapticDeviceWrapper::read(ConnectionReader &connection)
{
Bottle cmd;
if (!cmd.read(connection))
return false;
int tag=cmd.get(0).asVocab();
Bottle rep;
if (device!=NULL)
{
LockGuard lg(mutex);
if (tag==hapticdevice::set_transformation)
{
if (cmd.size()>=2)
{
if (Bottle *payload=cmd.get(1).asList())
{
Matrix T(payload->get(0).asInt(),
payload->get(1).asInt());
if (Bottle *vals=payload->get(2).asList())
{
for (int r=0; r<T.rows(); r++)
for (int c=0; c<T.cols(); c++)
T(r,c)=vals->get(T.rows()*r+c).asDouble();
if (device->setTransformation(T))
rep.addVocab(hapticdevice::ack);
else
rep.addVocab(hapticdevice::nack);
}
}
}
}
else if (tag==hapticdevice::get_transformation)
{
Matrix T;
if (device->getTransformation(T))
{
rep.addVocab(hapticdevice::ack);
rep.addList().read(T);
}
else
rep.addVocab(hapticdevice::nack);
}
else if (tag==hapticdevice::stop_feedback)
{
fdbck=0.0;
device->stopFeedback();
applyFdbck=false;
rep.addVocab(hapticdevice::ack);
}
else if (tag==hapticdevice::is_cartesian)
{
bool ret;
if (device->isCartesianForceModeEnabled(ret))
{
rep.addVocab(hapticdevice::ack);
rep.addInt(ret?1:0);
}
else
rep.addVocab(hapticdevice::nack);
}
else if (tag==hapticdevice::set_cartesian)
{
rep.addVocab(device->setCartesianForceMode()?
hapticdevice::ack:hapticdevice::nack);
}
else if (tag==hapticdevice::set_joint)
{
rep.addVocab(device->setJointTorqueMode()?
hapticdevice::ack:hapticdevice::nack);
}
else if (tag==hapticdevice::get_max)
{
Vector max;
if (device->getMaxFeedback(max))
{
rep.addVocab(hapticdevice::ack);
rep.addList().read(max);
}
else
rep.addVocab(hapticdevice::nack);
}
}
if (rep.size()==0)
rep.addVocab(hapticdevice::nack);
ConnectionWriter *writer=connection.getWriter();
if (writer!=NULL)
rep.write(*writer);
return true;
}
bool respond(const Bottle &command, Bottle &reply)
{
int ack =Vocab::encode("ack");
int nack=Vocab::encode("nack");
if (command.size()>0)
{
switch (command.get(0).asVocab())
{
case VOCAB4('s','a','v','e'):
{
int res=Vocab::encode("saved");
string fileName = vtRFThrd -> save();
if (fileName=="")
{
reply.addVocab(nack);
}
else
{
reply.addVocab(ack);
reply.addString(fileName.c_str());
}
reply.addVocab(res);
return true;
}
case VOCAB4('l','o','a','d'):
{
int res=Vocab::encode("loaded");
string fileName=vtRFThrd -> load();
if (fileName=="")
{
reply.addVocab(nack);
}
else
{
reply.addVocab(ack);
reply.addString(fileName.c_str());
}
reply.addVocab(res);
return true;
}
case VOCAB4('r','e','s','e'):
{
vtRFThrd -> resetParzenWindows();
reply.addVocab(ack);
return true;
}
case VOCAB4('s','t','o','p'):
{
vtRFThrd -> stopLearning();
reply.addVocab(ack);
return true;
}
case VOCAB4('r','e','s','t'):
{
vtRFThrd -> restoreLearning();
reply.addVocab(ack);
return true;
}
//-----------------
default:
return RFModule::respond(command,reply);
}
}
reply.addVocab(nack);
return true;
}