bool SpeechRecognizerModule::respond(const Bottle& cmd, Bottle& reply)
{
reply.addString("ACK");
string firstVocab = cmd.get(0).asString().c_str();
if (firstVocab == "tts")
{
string sentence = cmd.get(1).asString().c_str();
say(sentence);
reply.addString("OK");
}
else if (firstVocab == "RGM" || firstVocab == "rgm" )
{
string secondVocab = cmd.get(1).asString().c_str();
if (secondVocab=="vocabulory")
handleRGMCmd(cmd.tail().tail(), reply);
}
else if (firstVocab == "recog")
{
handleRecognitionCmd(cmd.tail(), reply);
}
else if (firstVocab == "asyncrecog")
{
handleAsyncRecognitionCmd(cmd.tail(), reply);
}
else if (firstVocab == "interrupt")
{
handleInterrupt(cmd.tail(), reply);
}
else
reply.addString("UNKNOWN");
return true;
}
bool TactileLearning::identifyCommand(Bottle commandBot, TactileLearningCommand &com, Bottle& params)
{
for(unsigned int i = 0; i < tactileLearningCommandSize; i++)
{
stringstream stream(TactileLearningCommandList[i]);
string word;
int j = 0;
bool found = true;
while(stream>>word)
{
if (commandBot.get(j).asString() != word.c_str())
{
found=false;
break;
}
j++;
}
if(found)
{
com = (TactileLearningCommand)i;
params = commandBot.tail();
for(int k = 1; k < j; k++) params = params.tail();
return true;
}
}
return false;
}
void RosNameSpace::run() {
int pct = 0;
do {
mutex.wait();
pct = pending.size();
mutex.post();
if (pct>0) {
mutex.wait();
Bottle *bot = pending.get(0).asList();
Bottle curr = *bot;
mutex.post();
dbg_printf("ROS connection begins: %s\n", curr.toString().c_str());
ContactStyle style;
style.admin = true;
style.carrier = "tcp";
Bottle cmd = curr.tail();
Contact contact = Contact::fromString(curr.get(0).asString());
contact.setName("");
Bottle reply;
NetworkBase::write(contact, cmd, reply, style);
dbg_printf("ROS connection ends: %s\n", curr.toString().c_str());
mutex.wait();
pending = pending.tail();
pct = pending.size();
mutex.post();
}
} while (pct>0);
}
bool TactileLearning::respond(const Bottle& command, Bottle& reply)
{
stringstream temp;
// reply contains a "command not recognized" by default
reply.clear();
TactileLearningCommand com;
Bottle params;
if(command.get(0).isInt())
{
// if first value is int then it is the id of the command
com = (TactileLearningCommand)command.get(0).asInt();
params = command.tail();
}
else if(!identifyCommand(command, com, params))
{
reply.addString("Unknown command. Input 'help' to get a list of the available commands.");
return true;
}
// external declarations
vector<Vector> referenceTrajectory;
vector<Vector> tactileState;
switch(com)
{
case get_optimal_trajectory:
reply.addString("Module should provide the optimal trajectory if computation is finished");
//thread->getState(tactileState);
//addToBottle(reply, tactileState);
return true;
case get_info:
reply.addString("Sorry but there is no info available for the moment!");
return true;
case get_help:
reply.addString("Sorry but there is no help available for the moment!");
return true;
case quit:
reply.addString("quitting...");
return false;
default:
reply.addString("ERROR: This command is known but it is not managed in the code.");
return true;
}
return true;
}
int Manager::processHumanCmd(const Bottle &cmd, Bottle &b)
{
int ret=Vocab::encode(cmd.get(0).asString().c_str());
b.clear();
if (cmd.size()>1)
{
if (cmd.get(1).isList())
b=*cmd.get(1).asList();
else
b=cmd.tail();
}
return ret;
}
bool BehaviorManager::respond(const Bottle& cmd, Bottle& reply)
{
yDebug() << "RPC received in BM";
yDebug() << cmd.toString();
reply.clear();
if (cmd.get(0).asString() == "help" )
{ string help = "\n";
help += " ['behavior_name'] : Triggers corresponding behavior \n";
reply.addString(help);
}
else if (cmd.get(0).asString() == "manual") {
for(auto& beh : behaviors) {
if (beh->behaviorName == "followingOrder") {
if (cmd.get(1).asString() == "on") {
yInfo() << "followingOrder behavior manual mode on";
dynamic_cast<FollowingOrder *>(beh)->manual = true;
} else if (cmd.get(1).asString() == "off") {
yInfo() << "followingOrder behavior manual mode off";
dynamic_cast<FollowingOrder *>(beh)->manual = false;
}
}
else if (beh->behaviorName == "recognitionOrder") {
if (cmd.get(1).asString() == "on") {
yInfo() << "recognitionOrder behavior manual mode on";
dynamic_cast<RecognitionOrder *>(beh)->manual = true;
} else if (cmd.get(1).asString() == "off") {
yInfo() << "recognitionOrder behavior manual mode off";
dynamic_cast<RecognitionOrder *>(beh)->manual = false;
}
}
}
reply.addString("ack");
} else if (cmd.get(0).asString() == "names" ) {
Bottle names;
for(auto& beh : behaviors) {
names.addString(beh->behaviorName);
}
reply.addList() = names;
} else if (cmd.get(0).asString() == "is_available" ) {
if (mut.tryLock()) {
mut.unlock();
reply.addInt(1);
} else {
reply.addInt(0);
}
}
else
{
bool behavior_triggered = false;
for(auto& beh : behaviors) {
if (cmd.get(0).asString() == beh->behaviorName) {
// Bottle args;
// args.clear();
// for (int a = 1; a < cmd.size(); a++)
// {
// args.add(&cmd.get(a));
// }
bool aux;
if (beh->from_sensation_port_name != "None") {
if (!Network::isConnected(beh->from_sensation_port_name, beh->sensation_port_in.getName())) {
aux =Network::connect(beh->from_sensation_port_name, beh->sensation_port_in.getName());
yInfo()<< "The sensations port for "<< beh->behaviorName <<" was not connected. \nReconnection status: " << aux;
}
}
if (beh->external_port_name != "None") {
if (!Network::isConnected(beh->rpc_out_port.getName(), beh->external_port_name)) {
aux = Network::connect(beh->rpc_out_port.getName(), beh->external_port_name);
yInfo()<< "The external port for "<< beh->behaviorName <<" was not connected. \nReconnection status: " << aux;
}
}
Bottle args;
if (cmd.size()>1){
args = cmd.tail();
}
yDebug() << "arguments are " << args.toString().c_str();
// beh->trigger(args);
behavior_triggered = beh->trigger(args);
// Add event into ABM
if (iCub->getABMClient()->Connect()) {
std::list<std::pair<std::string, std::string> > lArgument;
lArgument.push_back(std::pair<std::string, std::string>("iCub", "agent"));
lArgument.push_back(std::pair<std::string, std::string>(cmd.get(0).asString(), "predicate"));
lArgument.push_back(std::pair<std::string, std::string>("unknown_object", "object"));
lArgument.push_back(std::pair<std::string, std::string>("partner", "recipient"));
iCub->getABMClient()->sendActivity("action",
cmd.get(0).asString(),
"behavior", // expl: "pasar", "drives"...
lArgument,
true);
yInfo() << cmd.get(0).asString() + " behavior has been recorded in the ABM";
}
else{
yDebug() << "ABM not connected; no recording of action.";
}
}
}
if (behavior_triggered) {
reply.addString("ack");
} else {
reply.addString("nack");
yDebug()<< "Behavior ' " << cmd.get(0).asString() << " ' cant be triggered. \nSend 'names' to see a list of available behaviors. ";
}
}
yDebug() << "End of BehaviorManager::respond";
return true;
}
int main()
{
Network::init();
// open an rpc interface to receive user's commands
Port speaker;
speaker.setRpcMode(true);
int i=0;
while(!speaker.open("/IITdemo/rpc"))
{
if(i>=4) {cout << "Error in creating rpc port" << endl; Network::fini(); return -1;}
Time::delay(0.25);
i++;
}
// open a face interface to dialog with expressions
Port face;
while(!face.open("/IITdemo/face:o"))
{
if(i>=4) {cout << "Error in creating face port" << endl; speaker.close(); Network::fini(); return -1;}
Time::delay(0.25);
i++;
}
// open a planner instance that executes user's commands
Demo1Module * planner;
ResourceFinder rf;
rf.setVerbose(true); // print to a console conf info
rf.setDefaultConfigFile("C:/DARWIN/ConXML/conf/Demo1Configuration.ini");
rf.setDefaultContext("../conf"); // dove sono i file .ini
rf.configure("ICUB_ROOT",0,NULL);
planner = new Demo1Module(rf,20);
if(!planner->threadInit())
{
fprintf(stderr, "Error configuring module returning\n");
face.close();
return -1;
}
if(!planner->start())
{
planner->threadRelease();
fprintf(stderr, "Error configuring module returning\n");
face.close();
return -1;
}
// wait for the port to be connected
//send a welcome message:
Network::connect(face.getName().c_str(),"/icub/face/emotions/in");
Bottle faceCmd,test,reply;
test.clear();
faceCmd.append("set mou sur");
face.write(faceCmd,test);
Bottle welcome;
welcome.addString("--> Hello! Are you ready to play with me?");
speaker.read(test,true);
reply.append(welcome);
speaker.reply(reply);
test.clear();
speaker.read(test,true);
if (test.get(0).asString() == "no")
{
faceCmd.clear();
faceCmd.append("set mou sad");
face.write(faceCmd,test);
welcome.clear();
welcome.addString("--> Bye");
speaker.replyAndDrop(welcome);
planner->stop();
speaker.close();
face.close();
Network::fini();
return 0;
}
faceCmd.clear();
faceCmd.append("set all hap");
face.write(faceCmd,test);
// mettere icub in posizione relax
planner->Relax();
if(!planner->startPMP())
{
welcome.clear();
welcome.addString("--> Did you connect my ports?");
speaker.reply(welcome);
}
planner->enableTouch();
planner->suspend();
welcome.clear();
welcome.addString("--> What should I do?");
speaker.reply(welcome);
Bottle cmd;
while(true)
{
reply.clear();
speaker.read(cmd,true);
if (!planner->arePortsConnected() && cmd.get(0).asString() != "quit")
{
reply.append("--> Have you connected all my ports?");
speaker.reply(reply);
}
else
{
string first = cmd.get(0).asString().c_str();
if (first == "stop")
{
if(planner->stopMotion()) reply.addString("--> OK, I'm freezed");
else reply.addString("--> Sorry, I'm not freezed");
speaker.reply(reply);
}
else if(first == "save")
{
int done;
if(cmd.size() != 5 || !cmd.get(4).isString())
done = -1;
else
done = planner->add2map(cmd.get(4).asString().c_str(),cmd.get(1).asDouble(),cmd.get(2).asDouble(),cmd.get(3).asDouble());
switch(done)
{
case 1: reply.addString("--> Ok, I got it! But it was too low, I have saved it higher");break;
case 0: reply.addString("--> Ok, now I know this object!"); break;
case -1: reply.addString("--> Sorry, I can't understand");break;
case -2: reply.addString("--> Hey, I already knew this object!"); break;
}
speaker.reply(reply);
}
else if(first == "userRecord")
{
int done;
bool left = (cmd.get(1).asString() == "left");
// record using user input to stop
if (cmd.size() == 2)
{
if(!left)
done = planner->userRecordStart(false);
else
done = planner->userRecordStart();
}
if (done == 0)
{
reply.addString("--> Ok, you can guide me!");
speaker.reply(reply);
Bottle stop;
speaker.read(stop,true);
if(!left)
done = planner->userRecordStop(false);
else
done = planner->userRecordStop();
}
switch(done)
{
case 1:
reply.addString("--> Ok, I got it! But it was too low, I have saved it higher. How is this object called (label <name>)?");
break;
case 0:
reply.addString("--> Ok, I got it! How is this object called (label <name>)?");
break;
case -3: reply.addString("--> Sorry, I did not understand what you said");break;
case -4: reply.addString("--> Sorry, I could not stop my motion");break;
case -5: reply.addString("--> Sorry, I could not start recording");break;
case -6: reply.addString("--> Sorry, I don't know where my arm is");break;
}
}
else if(first == "record")
{
int done;
// record using user input to stop
if (cmd.size() == 2)
{
if(cmd.get(1).asString() == "right")
done = planner->Record(false);
else if(cmd.get(1).asString() == "left")
done = planner->Record();
else
done = -3;
}
else
done = planner->Record();
switch(done)
{
case 1:
// wait for the point to be recorded: if z<=SAFETY_Z, z = z+0.1
// once it has been recorded ask for the object label
planner->pointRecorded.wait();
Time::delay(0.5);
//planner->Relax();
planner->Prepare();
reply.addString("--> Ok, I got it! But it was too low, I have saved it higher. How is this object called (label <name>)?");
break;
case 0:
// wait for the point to be recorded
// once it has been recorded ask for the object label
planner->pointRecorded.wait();
Time::delay(5);
//planner->Relax();
planner->Prepare();
reply.addString("--> Ok, I got it! How is this object called (label <name>)?");
break;
case -3: reply.addString("--> Sorry, I did not understand what you said");break;
case -4: reply.addString("--> Sorry, I could not stop my motion");break;
case -5: reply.addString("--> Sorry, I could not start recording");break;
case -6: reply.addString("--> Sorry, I don't know where my arm is");break;
}
speaker.reply(reply);
}
else if (first == "label")
{
if (cmd.size() != 2)
reply.addString("--> Sorry, I did not understand the label");
else
{
int done = planner->Label(cmd.get(1).asString().c_str());
switch(done)
{
case 0: reply.addString("--> Ok, now I know this object!"); break;
case -1: reply.addString("--> Sorry, I don't have any point to label"); break;
case -2: reply.addString("--> Hey, I already knew this object!"); break;
}
}
speaker.reply(reply);
}
else if (first == "forget")
{
if (cmd.size() != 2)
reply.addString("--> Sorry, I did not understand the label");
else
{
int done = planner->Forget(cmd.get(1).asString().c_str());
switch(done)
{
case 0: reply.addString("--> Ok, object forgotten!"); break;
case -1: reply.addString("--> Sorry, I don't remember this object"); break;
}
}
speaker.reply(reply);
}
else if (first == "recall")
{
reply = planner->Recall();
if (reply.size() == 0)
reply.addString("--> Sorry, tabula rasa");
speaker.reply(reply);
}
else if (first == "relax")
{
int done = planner->Relax();
switch(done)
{
case 0: reply.addString("--> Ok, I relax!"); break;
case -1: reply.addString("--> Sorry, I can't relax"); break;
}
speaker.reply(reply);
}
else if (first == "prepare")
{
int done = planner->Prepare();
switch(done)
{
case 0: reply.addString("--> Ok, I'm ready!"); break;
case -1: reply.addString("--> Sorry, something went wrong"); break;
}
speaker.reply(reply);
}
// sequence reaching
else if (first == "reach")
{
if(cmd.size() == 1)
reply.addString("--> Sorry, I don't know what I should reach with my finger");
else
{
int done = planner->SequenceReach(cmd.tail());
switch(done)
{
case 0: reply.addString("--> Ok, I have reached the object!"); break;
case -1: reply.addString("--> Sorry, I don't know this object"); break;
case -2: reply.addString("--> Sorry, I don't know where my hands are"); break;
case -3: reply.addString("--> Sorry, I did not understand what you said"); break;
case -4: reply.addString("--> Sorry, my ports are not connected"); break;
case -5: reply.addString("--> Sorry, I don't know this command"); break;
case -8: reply.addString("--> Sorry, I couldn't reach the object"); break;
case -9: reply.addString("--> Sorry, my wrist orientation is wrong"); break;
}
}
speaker.reply(reply);
}
// finger reaching
else if (first == "reach1")
{
if(cmd.size() != 3 && cmd.size() != 2)
reply.addString("--> Sorry, I don't know what I should reach with my finger");
else
{
int done = planner->Prepare();
if(done != 0) reply.addString("--> Sorry, I'm lazy");
else if (cmd.size() == 3)
{
if (cmd.get(2).asString() == "top" || cmd.get(2).asString() == "side")
{
done = planner->FingerReach(cmd.get(1).asString().c_str(),"",cmd.get(2).asString().c_str(),false);
if (done == 0)
{
cout << "check reaching performance" << endl;
done = planner->checkReachingSuccess(cmd.get(1).asString().c_str());
}
}
else
done = -9;
}
else
{
done = planner->FingerReach(cmd.get(1).asString().c_str(),"","top",false);
if (done == 0)
{
cout << "check reaching performance" << endl;
done = planner->checkReachingSuccess(cmd.get(1).asString().c_str());
}
}
switch(done)
{
case 0:
reply.addString("--> Ok, I have reached the object!");
//done = planner->Lift(planner->activeChain);
//if (done !=0 )
//{reply.addString("But I could not lift my hand"); cout << "done: " << done << endl;break;}
done = planner->goFar(planner->activeChain);
if (done !=0 )
{reply.addString("But I could not go farer"); cout << "done: " << done << endl;}
planner->Relax();
break;
case -1: reply.addString("--> Sorry, I don't know this object"); break;
case -2: reply.addString("--> Sorry, I don't know where my hands are"); break;
case -3: reply.addString("--> Sorry, I did not understand what you said"); break;
case -4: reply.addString("--> Sorry, my ports are not connected"); break;
case -5: reply.addString("--> Sorry, I don't know this command"); break;
case -8: reply.addString("--> Sorry, I couldn't reach the object"); break;
case -9: reply.addString("--> Sorry, my wrist orientation is wrong"); break;
}
}
speaker.reply(reply);
}
// reach to grasp function
else if (first == "grasp")
{
if(cmd.size() != 2)
{
reply.addString("--> Sorry, I don't know what I should grasp");
}
else
{
cout << "prepare for grasping" << endl;
// this function will return 1 if no reaching is needed, 0 otherwise
int done = planner->prepare4Grasping(cmd.get(1).asString().c_str());
if (done == 0)
{
// check that motion is finished successfully
cout << "check reaching performance" << endl;
done = planner->checkReachingSuccess(cmd.get(1).asString().c_str());
}
if(done == 0 || done == 1)
{
cout << "grasp" << endl;
done = (planner->Grasp()).get(0).asInt();
}
if(done == 0)
{
done = planner->Lift(planner->activeChain,true);
// check if contact is mantained
if(done == 0)
{
done = planner->Release(planner->activeChain);
if (done == 0) // reinitialize
done = planner->Relax();
}
}
switch(done)
{
case 0:
case 1: reply.addString("--> Ok, I grasped the object!"); break;
case -1: reply.addString("--> Sorry, I don't know this object"); break;
case -2: reply.addString("--> Sorry, I don't know where my hands are"); break;
case -3: reply.addString("--> Sorry, I did not understand what you said"); break;
case -4: reply.addString("--> Sorry, my ports are not connected, I can't grasp"); break;
case -5: reply.addString("--> Sorry, I don't know this command"); break;
case -6: reply.addString("--> Sorry, I don't have this hand"); break;
case -7: reply.addString("--> Sorry, I couldn't grasp the object"); break;
case -8: reply.addString("--> Sorry, I couldn't reach the object"); break;
case -9: reply.addString("--> Hey, I have lost the object!"); break;
}
}
speaker.reply(reply);
}
else if (first == "release")
{
if(cmd.size() != 2)
reply.addString("--> Sorry, I don't know what hand to open");
else
{
int done = planner->Release(cmd.get(1).asString().c_str());
switch(done)
{
case 0: reply.addString("--> Ok, I released the object!"); break;
case -1: reply.addString("--> Sorry, I can't release the object"); break;
}
if (done == 0) planner->Relax();//planner->initIcubUp();
}
speaker.reply(reply);
}
else if (first == "lift")
{
if(cmd.size() != 2)
reply.addString("--> Sorry, I don't know what hand to lift");
else
{
int done = planner->Lift(cmd.get(1).asString().c_str());
switch(done)
{
case 0: reply.addString("--> Ok, I have lifted my hand!"); break;
case -2: reply.addString("--> Sorry, I don't know where my hands are"); break;
case -3: reply.addString("--> Sorry, I did not understand what you said"); break;
case -4: reply.addString("--> Sorry, my ports are not connected"); break;
case -5: reply.addString("--> Sorry, I don't know this command"); break;
case -6: reply.addString("--> Sorry, I don't have this hand"); break;
case -9: reply.addString("--> Hey, I have lost the object!"); break;
}
}
speaker.reply(reply);
}
else if (first == "carry")
{
if(cmd.size() != 2)
{
reply.addString("--> Sorry, I don't know what I should grasp");
}
else
{
}
speaker.reply(reply);
}
else if (first == "unstack")
{
int done = planner->Unstack();
switch(done)
{
case 0: reply.addString("--> Ok, I have recycled the object!"); break;
case -1: reply.addString("--> Sorry, I don't know this object"); break;
case 1 : reply.addString("--> Sorry, I don't know where the litter is (label = bin)"); break;
case -2: reply.addString("--> Sorry, I don't know where my hands are"); break;
case -3: reply.addString("--> Sorry, I did not understand what you said"); break;
case -4: reply.addString("--> Sorry, my ports are not connected"); break;
case -5: reply.addString("--> Sorry, I don't know this command"); break;
case -6: reply.addString("--> Sorry, I don't have this hand"); break;
case -7: reply.addString("--> Sorry, I couldn't grasp the object"); break;
case -8: reply.addString("--> Sorry, I couldn't reach the object"); break;
case -9: reply.addString("--> Sorry, my wrist orientation is wrong"); break;
case -10: reply.addString("--> Hey, I have lost the object!"); break;
case -11: reply.addString("--> Hey, I do not see a stack!"); break;
case -12: reply.addString("--> Sorry, points are not consistent!"); break;
case -13: reply.addString("--> Sorry, I can't see any object!"); break;
}
speaker.reply(reply);
planner->Prepare();
}
else if (first == "recycle")
{
if(cmd.size() != 2)
{
reply.addString("--> Sorry, I don't know what I should throw in the garbage");
}
else
{
int done = planner->Recycle(cmd.get(1).asString().c_str());
switch(done)
{
case 0: reply.addString("--> Ok, I have recycled the object!"); break;
case -1: reply.addString("--> Sorry, I don't know this object"); break;
case 1 : reply.addString("--> Sorry, I don't know where the litter is (label = bin)"); break;
case -2: reply.addString("--> Sorry, I don't know where my hands are"); break;
case -3: reply.addString("--> Sorry, I did not understand what you said"); break;
case -4: reply.addString("--> Sorry, my ports are not connected"); break;
case -5: reply.addString("--> Sorry, I don't know this command"); break;
case -6: reply.addString("--> Sorry, I don't have this hand"); break;
case -7: reply.addString("--> Sorry, I couldn't grasp the object"); break;
case -8: reply.addString("--> Sorry, I couldn't reach the object"); break;
case -9: reply.addString("--> Sorry, my wrist orientation is wrong"); break;
case -10: reply.addString("--> Hey, I have lost the object!"); break;
}
planner->Relax();
}
speaker.reply(reply);
}
else if (first == "geometry")
{
if(cmd.size() < 2)
reply.addString("--> Sorry, I don't understand");
if(cmd.get(1).asString() == "get")
reply.append(planner->getObjectGeometry());
else if (cmd.get(1).asString() == "set" && cmd.size() == 4)
{
planner->setObjectGeometry(cmd.get(2).asDouble(),cmd.get(3).asDouble());
reply.addString("--> OK, now I have new geometry values");
}
else
reply.addString("--> Sorry, I don't understand");
speaker.reply(reply);
}
else if (first == "bimanual")
{
if(cmd.size() < 2)
reply.addString("--> Sorry, I don't know what I should do with my hands");
else
{
int done;
if (cmd.get(1).asString() == "reach")
{
switch(cmd.size())
{
case 3: // use the geometrical info abt the object to reach it
done = planner->initBimanualReach(cmd.get(2).asString().c_str(),cmd.get(2).asString().c_str());
planner->BimanualRelease();
break;
case 4: // reach from top two different objects
done = planner->initBimanualReach(cmd.get(2).asString().c_str(),cmd.get(3).asString().c_str());
break;
}
// get the label of the object/s to reach: if one
}
else if (cmd.get(1).asString() == "grasp" && cmd.size() == 3)
{
done = planner->initBimanualReach(cmd.get(2).asString().c_str(),cmd.get(2).asString().c_str());
if (done == 0)
{
done = planner->checkBimanualReachingSuccess(cmd.get(2).asString().c_str(),cmd.get(2).asString().c_str());
if (done == 0)
{
done = planner->BimanualGrasp();
if (done == 0)
done = planner->BimanualLift(false);
}
}
Time::delay(1);
planner->BimanualRelease();
}
else if(cmd.get(1).asString() == "carry")
{
}
switch(done)
{
case 0: reply.addString("--> Ok, I have recycled the object!"); break;
case -1: reply.addString("--> Sorry, I don't know this object"); break;
case 1 : reply.addString("--> Sorry, I don't know where the litter is (label = bin)"); break;
case -2: reply.addString("--> Sorry, I don't know where my hands are"); break;
case -3: reply.addString("--> Sorry, I did not understand what you said"); break;
case -4: reply.addString("--> Sorry, my ports are not connected"); break;
case -5: reply.addString("--> Sorry, I don't know this command"); break;
case -6: reply.addString("--> Sorry, I don't have this hand"); break;
case -7: reply.addString("--> Sorry, I couldn't grasp the object"); break;
case -8: reply.addString("--> Sorry, I couldn't reach the object"); break;
case -9: reply.addString("--> Sorry, my wrist orientation is wrong"); break;
case -10: reply.addString("--> Hey, I have lost the object!"); break;
}
}
speaker.reply(reply);
}
else if (first == "quit")
{
planner->Relax();
//if(planner->isSuspended()) planner->resume();
planner->stop();
reply.addString("--> Bye");
speaker.replyAndDrop(reply);
break;
}
else
{
reply.addString("--> I did not understand what you said");
speaker.reply(reply);
}
}
cmd.clear();
reply.clear();
}
face.interrupt();
face.close();
speaker.close();
Network::fini();
return 0;
}
bool PMPthread::setVTGSparam(Bottle cmdBot, string side)
{
VTGSparam cmd;
if(cmdBot.size() < 2)
{
printf("Error: No value specified\n");
return false;
}
if(!identifyCmd(cmdBot,cmd))
{
printf("Error: Parameter not recognized\n");
return false;
}
VirtualTrajectoryGenerator * VTGS;
if (side == "right") VTGS = VTGS_r;
if (side == "left") VTGS = VTGS_l;
// remove the keyword from bottle, so as to have only parameter value inside
Bottle bot = cmdBot.tail();
Vector vtemp(9);
Matrix mtemp(3,3);
bool wasSuspended = isSuspended();
string active;
Vector deltaindex(3);
switch(cmd)
{
// VTGS parameters
// Before any change, suspend the thread. Probably not effective ...
// At the end of the switch the thread initial state will be restored
updateSem.wait();
if(!isSuspended()) suspend();
case target:
// set target as first in matrix
if(bot.size() == 3)
{
mtemp.resize(1,3);
mtemp.zero();
Bottle2Matrix(bot,mtemp);
VTGS->setTarget(mtemp);
cout << mtemp.toString() << endl;
updateSem.post();
break;
}
else if(bot.get(0).isInt() && (bot.size()-1)%3 == 0)
{
mtemp.resize(bot.get(0).asInt(),3);
mtemp.zero();
if (mtemp.rows() <= 3)
{
Bottle2Matrix(bot.tail(),mtemp);
VTGS->setTarget(mtemp);
cout << mtemp.toString() << endl;
}
else
{
printf("Error: parameter value size mismatch\n");
if(!wasSuspended) resume();
updateSem.post();
return false;
}
updateSem.post();
break;
}
// set target in tail: (FIFO mode)
/* else if(bot.size() == 3)
{
// new critical point coordinates: if at least one VTGS has a new target, then the thread flag
// is set to false: tha target saved has not yet been reached.
// the target should be set only when the thread is not running (is suspended).
VTGS->setTarget(Bottle2Vector(bot));
VTGS->numTargetPoints++;
//readyToStart = false;
updateSem.post();
break;
}
*/
else
{
updateSem.post();
return false;
}
case weights:
if(bot.get(0).isInt() && (bot.size()-1)%3 == 0)
{
mtemp.resize(bot.get(0).asInt(),3);
mtemp.zero();
if (mtemp.rows() <= 3)
{
Bottle2Matrix(bot.tail(),mtemp);
VTGS->setWeights(mtemp);
}
else
{
printf("Error: parameter value size mismatch\n");
if(!wasSuspended) resume();
updateSem.post();
return false;
}
updateSem.post();
break;
}
// set one weight vector in tail
else if(bot.size() == 3)
{
VTGS->setWeights(Bottle2Vector(bot));
updateSem.post();
break;
}
else if (bot.size() == VTGS->weights.rows()*VTGS->weights.cols())
{
Bottle2Matrix(bot,VTGS->weights);
updateSem.post();
break;
}
else
{
printf("Error: parameter value size mismatch\n");
if(!wasSuspended) resume();
updateSem.post();
return false;
}
break;
// tbg1 parameters
case T_init1:
VTGS->tbg1.setT_init(bot.get(0).asDouble());
break;
case T_dur1:
VTGS->tbg1.setT_dur(bot.get(0).asDouble());
break;
case SlopeRamp1:
VTGS->tbg1.setSlopeRamp(bot.get(0).asDouble());
updateSem.post();
break;
case alpha1:
VTGS->tbg1.setAlpha(bot.get(0).asDouble());
break;
// tbg2 parameters
case T_init2:
VTGS->tbg2.setT_init(bot.get(0).asDouble());
break;
case T_dur2:
VTGS->tbg2.setT_dur(bot.get(0).asDouble());
break;
case SlopeRamp2:
VTGS->tbg2.setSlopeRamp(bot.get(0).asDouble());
break;
case alpha2:
VTGS->tbg2.setAlpha(bot.get(0).asDouble());
break;
}
// check if hasTarget Flag can be set to true
string Achain = getActiveChain();
if (Achain == "right" && VTGS_r->hasTarget) hasTarget = true;
else if (Achain == "left" && VTGS_l->hasTarget) hasTarget = true;
else if (VTGS_r->hasTarget && VTGS_l->hasTarget) hasTarget = true;
// don't change previous thread state
if(!wasSuspended) resume();
updateSem.post();
return true;
}
bool PMPthread::setPMPparam(Bottle cmdBot)
{
// mettere tutti i printf in 1 stringa messaggio che viene letta dal modulo ogni volta che respond riceve un false
// così scrivo tutto da un unico programma, il modulo, ed elimino tutti i printf.
PMPparam cmd;
if(cmdBot.size() < 2)
{
printf("Error: No value specified\n");
return false;
}
if(!identifyCmd(cmdBot,cmd))
{
printf("Error: Parameter not recognized\n");
return false;
}
// remove the keyword from bottle, so as to have only parameter value inside
Bottle bot = cmdBot.tail();
Bottle test;
Bottle &list = test.addList();
bool wasSuspended = isSuspended();
switch(cmd)
{
// PMP parameters
updateSem.wait();
if(!isSuspended()) suspend();
case xr_target:
if(bot.size() != PMP->x_tgR.size())
{
printf("Error: parameter value size mismatch\n");
if(!wasSuspended) resume();
updateSem.post();
return false;
}
PMP->x_tgR = Bottle2Vector(bot);
break;
case xl_target:
if(bot.size() != PMP->x_tgL.size())
{
printf("Error: parameter value size mismatch\n");
if(!wasSuspended) resume();
updateSem.post();
return false;
}
PMP->x_tgL = Bottle2Vector(bot);
break;
case q_rightArm:
if(bot.size() != PMP->q_rightArm.size())
{
printf("Error: parameter value size mismatch\n");
if(!wasSuspended) resume();
updateSem.post();
return false;
}
PMP->set_q_rightArm(Bottle2Vector(bot));
break;
case q_leftArm:
if(bot.size() != PMP->q_leftArm.size())
{
printf("Error: parameter value size mismatch\n");
if(!wasSuspended) resume();
updateSem.post();
return false;
}
PMP->set_q_leftArm(Bottle2Vector(bot));
break;
case qr_initial:
if(bot.size() != PMP->q_0R.size())
{
printf("Error: parameter value size mismatch\n");
if(!wasSuspended) resume();
updateSem.post();
return false;
}
PMP->q_0R = Bottle2Vector(bot);
break;
case ql_initial:
if(bot.size() != PMP->q_0L.size())
{
printf("Error: parameter value size mismatch\n");
if(!wasSuspended) resume();
updateSem.post();
return false;
}
PMP->q_0L = Bottle2Vector(bot);
break;
case Kr_virt:
if(bot.size() != PMP->K_right.cols())
{
printf("Error: parameter value size mismatch\n");
if(!wasSuspended) resume();
updateSem.post();
return false;
}
PMP->set_K_right(Bottle2Vector(bot));
break;
case Kl_virt:
if(bot.size() != PMP->K_left.cols())
{
printf("Error: parameter value size mismatch\n");
if(!wasSuspended) resume();
updateSem.post();
return false;
}
PMP->set_K_left(Bottle2Vector(bot));
break;
case A_rightArm:
if(bot.size() != PMP->A_rightArm.cols())
{
printf("Error: parameter value size mismatch\n");
if(!wasSuspended) resume();
updateSem.post();
return false;
}
PMP->set_A_rightArm(Bottle2Vector(bot));
break;
case A_leftArm:
if(bot.size() != PMP->A_leftArm.cols())
{
printf("Error: parameter value size mismatch\n");
if(!wasSuspended) resume();
updateSem.post();
return false;
}
PMP->set_A_leftArm(Bottle2Vector(bot));
break;
case Kr_int:
if(bot.size() != PMP->Kint_right.cols())
{
printf("Error: parameter value size mismatch\n");
if(!wasSuspended) resume();
updateSem.post();
return false;
}
PMP->set_Kint_right(Bottle2Vector(bot));
break;
case Kl_int:
if(bot.size() != PMP->Kint_left.cols())
{
printf("Error: parameter value size mismatch\n");
if(!wasSuspended) resume();
updateSem.post();
return false;
}
PMP->set_Kint_left(Bottle2Vector(bot));
break;
case K_virt_bimanual:
if(bot.size() != PMP->K_biman.cols())
{
printf("Error: parameter value size mismatch\n");
if(!wasSuspended) resume();
updateSem.post();
return false;
}
PMP->set_K_biman(Bottle2Vector(bot));
break;
case A_bimanual:
if(bot.size() != PMP->A_biman.cols())
{
printf("Error: parameter value size mismatch\n");
if(!wasSuspended) resume();
updateSem.post();
return false;
}
PMP->set_A_biman(Bottle2Vector(bot));
break;
// tbg parameters
case T_init:
PMP->tbg.setT_init(bot.get(0).asDouble());
break;
case T_dur:
PMP->tbg.setT_dur(bot.get(0).asDouble());
break;
case SlopeRamp:
PMP->tbg.setSlopeRamp(bot.get(0).asDouble());
break;
case alpha:
PMP->tbg.setAlpha(bot.get(0).asDouble());
break;
// others
case wrist:
if(bot.size() != 2)
{
printf("Error: parameter value size mismatch\n");
if(!wasSuspended) resume();
updateSem.post();
return false;
}
if (bot.get(0).asString() == "right")
{
PMP->q_ref_right(7) = bot.get(1).asDouble()*CTRL_DEG2RAD;
list = Vector2Bottle(PMP->q_ref_right);
PMP->opt_right->put("q_ref_right",test.get(0));
cout << PMP->opt_right->toString()<< endl;
if(!wasSuspended) resume();
updateSem.post();
return true;
}
else if (bot.get(0).asString() == "left")
{
PMP->q_ref_left(7) = bot.get(1).asDouble()*CTRL_DEG2RAD;
list = Vector2Bottle(PMP->q_ref_left);
PMP->opt_left->put("q_ref_left",test.get(0));
if(!wasSuspended) resume();
updateSem.post();
return true;
}
else if (bot.get(0).asString() == "bimanual")
{
PMP->q_ref_right(7) = PMP->q_ref_left(7) = bot.get(1).asDouble()*CTRL_DEG2RAD;
list = Vector2Bottle(PMP->q_ref_right);
PMP->opt_right->put("q_ref_right",test.get(0));
list.clear();
list = Vector2Bottle(PMP->q_ref_left);
PMP->opt_left->put("q_ref_left",test.get(0));
if(!wasSuspended) resume();
updateSem.post();
return true;
}
else
{
printf("Error: unknown chain parameter value\n");
if(!wasSuspended) resume();
updateSem.post();
return false;
}
default:
printf("idle\n");
if(!wasSuspended) resume();
updateSem.post();
return true;
}
if(!wasSuspended) resume();
updateSem.post();
return true;
}
bool skinManager::respond(const Bottle& command, Bottle& reply) {
stringstream temp;
reply.clear();
SkinManagerCommand com;
Bottle params;
if(command.get(0).isInt()){
// if first value is int then it is the id of the command
com = (SkinManagerCommand)command.get(0).asInt();
params = command.tail();
}
else if(!identifyCommand(command, com, params)){
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((string(getName().c_str()) + " commands are: ").c_str());
for(unsigned int i=0; i< SkinManagerCommandSize; i++){
reply.addString( ("- "+SkinManagerCommandList[i]+": "+SkinManagerCommandDesc[i]).c_str() );
}
return true;
case calibrate:
myThread->calibrate();
break;
case get_touch_thr:
{
Vector touchThreshold = myThread->getTouchThreshold();
for(size_t i=0; i< touchThreshold.size(); i++)
reply.addDouble(touchThreshold[i]);
return true;
}
case set_binarization:
{
if(params.size()<1){
reply.addString("Binarization state missing! Specify either on or off.");
return true;
}
string value = params.get(0).asString().c_str();
if(value.compare("on")==0)
myThread->setBinarization(true);
else if(value.compare("off")==0)
myThread->setBinarization(false);
else{
reply.addString("Value not recognized.");
return true;
}
break;
}
case get_binarization:
if(myThread->getBinarization())
reply.addString("on");
else
reply.addString("off");
return true;
case set_smooth_filter:
{
if(params.size()<1){
reply.addString("Smooth filter state missing! Specify either on or off.");
return true;
}
string value = params.get(0).asString().c_str();
if(value.compare("on")==0)
myThread->setSmoothFilter(true);
else if(value.compare("off")==0)
myThread->setSmoothFilter(false);
else{
reply.addString("Value not recognized.");
return true;
}
break;
}
case get_smooth_filter:
if(myThread->getSmoothFilter())
reply.addString("on");
else
reply.addString("off");
return true;
case set_smooth_factor:
{
if(params.size()<1 || (!params.get(0).isDouble() && !params.get(0).isInt())){
reply.addString("New smooth factor value missing or not a number! Smooth factor not updated.");
return true;
}
stringstream temp;
if(myThread->setSmoothFactor((float)params.get(0).asDouble())){
temp<< "New smooth factor set: "<< params.get(0).asDouble();
}
else{
temp<< "ERROR in setting new smooth factor: "<< params.get(0).asDouble();
}
reply.addString( temp.str().c_str());
return true;
}
case get_smooth_factor:
reply.addDouble(myThread->getSmoothFactor());
return true;
case set_threshold:
{
if(params.size()<1 || (!params.get(0).isInt())){
reply.addString("New threshold value missing or not an integer! Threshold not updated.");
return true;
}
stringstream temp;
if(myThread->setAddThreshold(params.get(0).asInt())){
temp<< "New threshold set: "<< params.get(0).asInt();
}
else{
temp<< "ERROR in setting new threshold: "<< params.get(0).asInt();
}
reply.addString( temp.str().c_str());
return true;
}
case get_threshold:
reply.addInt(myThread->getAddThreshold());
return true;
case set_gain:
{
if(params.size()<1 || (!params.get(0).isDouble())){
reply.addString("New gain value missing or not a double! Gain not updated.");
return true;
}
stringstream temp;
if(myThread->setCompensationGain(params.get(0).asDouble())){
temp<< "New gain set: "<< params.get(0).asDouble();
}
else{
temp<< "ERROR in setting new gain: "<< params.get(0).asDouble();
}
reply.addString( temp.str().c_str());
return true;
}
case get_gain:
reply.addDouble(myThread->getCompensationGain());
return true;
case set_cont_gain:
{
if(params.size()<1 || (!params.get(0).isDouble())){
reply.addString("New gain value missing or not a double! Contact gain not updated.");
return true;
}
stringstream temp;
if(myThread->setContactCompensationGain(params.get(0).asDouble())){
temp<< "New contact gain set: "<< params.get(0).asDouble();
}
else{
temp<< "ERROR in setting new contact gain: "<< params.get(0).asDouble();
}
reply.addString( temp.str().c_str());
return true;
}
case get_cont_gain:
reply.addDouble(myThread->getContactCompensationGain());
return true;
case get_max_neigh_dist:
reply.addDouble(myThread->getMaxNeighborDistance());
return true;
case set_max_neigh_dist:
{
if(params.size()<1 || (!params.get(0).isDouble())){
reply.addString("New max neighbor distance value missing or not a double! Not updated.");
return true;
}
stringstream temp;
if(myThread->setMaxNeighborDistance(params.get(0).asDouble())){
temp<< "New max neighbor distance set: "<< params.get(0).asDouble();
}
else{
temp<< "ERROR in setting new max neighbor distance: "<< params.get(0).asDouble();
}
reply.addString( temp.str().c_str());
return true;
}
case get_skin_parts:
{
vector<SkinPart> spl = myThread->getSkinParts();
for(vector<SkinPart>::const_iterator it=spl.begin(); it!=spl.end(); it++){
reply.addInt(*it);
reply.addString(SkinPart_s[*it].c_str());
}
return true;
}
case enable_skin_part:
if(!(params.size()>0 && params.get(0).isInt())){
reply.addString(("ERROR: SkinPart is not specified. Params read are: "+string(params.toString().c_str())).c_str());
return true;
}
if(myThread->enableSkinPart((SkinPart)params.get(0).asInt()))
reply.addString("SkinPart enabled");
else
reply.addString("SkinPart not found");
return true;
case disable_skin_part:
if(!(params.size()>0 && params.get(0).isInt())){
reply.addString(("ERROR: SkinPart is not specified. Params read are: "+string(params.toString().c_str())).c_str());
return true;
}
if(myThread->disableSkinPart((SkinPart)params.get(0).asInt()))
reply.addString("SkinPart disabled");
else
reply.addString("SkinPart not found");
return true;
case is_skin_enabled:
if(!(params.size()>0 && params.get(0).isInt())){
reply.addString(("ERROR: SkinPart is not specified. Params read are: "+string(params.toString().c_str())).c_str());
return true;
}
if(myThread->isSkinEnabled((SkinPart)params.get(0).asInt()))
reply.addString("yes");
else
reply.addString("no");
return true;
case is_calibrating:
if(myThread->isCalibrating())
reply.addString("yes");
else
reply.addString("no");
return true;
case get_pose:
{
if(!(params.size()>0 && params.get(0).isInt())){
reply.addString(("ERROR: SkinPart is not specified. Params read are: "+string(params.toString().c_str())).c_str());
return true;
}
SkinPart sp = (SkinPart) params.get(0).asInt();
if(params.size()>1 && params.get(1).isInt()){
unsigned int taxelId = params.get(1).asInt();
Vector res = myThread->getTaxelPose(sp, taxelId);
if(res.size()>0)
addToBottle(reply, res);
else
reply.addString("No poses for the specified skin part");
}
else{
vector<Vector> res = myThread->getTaxelPoses(sp);
addToBottle(reply, res);
}
return true;
}
case set_pose:
{
if(!(params.size()>6 && params.get(0).isInt() )){
reply.addString(("ERROR: SkinPart is not specified. Params read are: "+string(params.toString().c_str())).c_str());
return true;
}
SkinPart sp = (SkinPart) params.get(0).asInt();
if(params.get(1).isInt()){
unsigned int taxelId = params.get(1).asInt();
Vector pose;
if(!bottleToVector(params.tail().tail(), pose)){
reply.addString("ERROR while reading the taxel pose");
return true;
}
if(myThread->setTaxelPose(sp, taxelId, pose))
reply.addInt(skin_manager_ok);
else{
reply.addInt(skin_manager_error);
reply.addString("ERROR: pose was not set");
}
}
else{
Vector poses;
if(!bottleToVector(params.tail(), poses)){
reply.addString("ERROR while reading the taxel poses");
return true;
}
if(myThread->setTaxelPoses(sp, poses))
reply.addInt(skin_manager_ok);
else{
reply.addInt(skin_manager_error);
reply.addString("ERROR: pose was not set");
}
}
return true;
}
case get_position:
{
if(!(params.size()>0 && params.get(0).isInt())){
reply.addString(("ERROR: SkinPart is not specified. Params read are: "+string(params.toString().c_str())).c_str());
return true;
}
SkinPart sp = (SkinPart) params.get(0).asInt();
if(params.size()>1 && params.get(1).isInt()){
unsigned int taxelId = params.get(1).asInt();
Vector res = myThread->getTaxelPosition(sp, taxelId);
if(res.size()>0)
addToBottle(reply, res);
else
reply.addString("No position for the specified skin part");
}
else{
vector<Vector> res = myThread->getTaxelPositions(sp);
if(res.empty())
reply.addString("Specified skin part has not been found.");
else
addToBottle(reply, res);
}
return true;
}
case set_position:
{
if(!(params.size()>3 && params.get(0).isInt() )){
reply.addString(("ERROR: SkinPart is not specified. Params read are: "+string(params.toString().c_str())).c_str());
return true;
}
SkinPart sp = (SkinPart) params.get(0).asInt();
if(params.get(1).isInt()){
unsigned int taxelId = params.get(1).asInt();
Vector pose;
if(!bottleToVector(params.tail().tail(), pose)){
reply.addString("ERROR while reading the taxel position");
return true;
}
if(myThread->setTaxelPosition(sp, taxelId, pose))
reply.addInt(skin_manager_ok);
else{
reply.addInt(skin_manager_error);
reply.addString("ERROR: position was not set");
}
}
else{
Vector poses;
if(!bottleToVector(params.tail(), poses)){
reply.addString("ERROR while reading the taxel positions");
return true;
}
if(myThread->setTaxelPositions(sp, poses))
reply.addInt(skin_manager_ok);
else{
reply.addInt(skin_manager_error);
reply.addString("ERROR: position was not set");
}
}
return true;
}
case get_confidence:
{
if(!(params.size()>0 && params.get(0).isInt())){
reply.addString(("ERROR: SkinPart is not specified. Params read are: "+string(params.toString().c_str())).c_str());
return true;
}
SkinPart sp = (SkinPart) params.get(0).asInt();
if(params.size()>1 && params.get(1).isInt()){
unsigned int taxelId = params.get(1).asInt();
double res = myThread->getPoseConfidence(sp, taxelId);
reply.addDouble(res);
}
else{
Vector res = myThread->getPoseConfidences(sp);
if(res.size()==0)
reply.addString("Specified skin part has not been found");
else
addToBottle(reply, res);
}
return true;
}
case get_info:
reply.append(myThread->getInfo());
return true;
default:
reply.addString("ERROR: This command is known but it is not managed in the code.");
return true;
}
reply.addString( (SkinManagerCommandList[com]+" command received.").c_str());
return true;
}
bool XmlRpcCarrier::write(Protocol& proto, SizedWriter& writer) {
//XmlRpc::setVerbosity(10);
StringOutputStream sos;
StringInputStream sis;
writer.write(sos);
sis.reset(sos.toString());
String header;
if (sender) {
header = NetType::readLine(sis);
}
String body = NetType::readLine(sis);
//printf("Asked to write: hdr %s body %s\n",
// header.c_str(), body.c_str());
Value v;
//printf("HEADER %s\n", header.c_str());
if (header[0]=='q') {
body = "yarp.quit";
// XMLRPC does not need a quit message, this should get stripped
return false;
}
Bottle *bot = v.asList();
//Bottle aux;
bot->fromString(body.c_str());
ConstString methodName;
if (sender) {
methodName = bot->get(0).toString();
*bot = bot->tail();
}
XmlRpcValue args;
if (bot->size()==1) {
toXmlRpcValue(bot->get(0),args);
} else {
toXmlRpcValue(v,args);
}
//printf("xmlrpc block to write is %s\n", args.toXml().c_str());
std::string req;
if (sender) {
const Address& addr = host.isValid()?host:proto.getStreams().getRemoteAddress();
XmlRpcClient c(addr.getName().c_str(),(addr.getPort()>0)?addr.getPort():80);
c.generateRequest(methodName.c_str(),args);
req = c.getRequest();
} else {
XmlRpcServerConnection c(0,NULL);
c.generateResponse(args.toXml());
req = c.getResponse();
}
int start = 0;
//printf("converts to %s\n", req.c_str());
if (sender) {
if (req.length()<8) {
fprintf(stderr, "XmlRpcCarrier fail, %s:%d\n", __FILE__, __LINE__);
return false;
}
for (int i=0; i<(int)req.length(); i++) {
if (req[i] == '\n') {
start++;
break;
}
start++;
}
if (!firstRound) {
Bytes b((char*)http.c_str(),http.length());
proto.os().write(b);
}
firstRound = false;
}
Bytes b((char*)req.c_str()+start,req.length()-start);
//printf("WRITING [%s]\n", req.c_str()+start);
proto.os().write(b);
return proto.os().isOk();
}
bool XmlRpcCarrier::write(ConnectionState& proto, SizedWriter& writer) {
StringOutputStream sos;
StringInputStream sis;
writer.write(sos);
sis.reset(sos.toString());
ConstString header;
if (sender) {
header = sis.readLine();
}
ConstString body = sis.readLine();
Value v;
if (header.length()>0 && header[0]=='q') {
body = "yarp.quit";
// XMLRPC does not need a quit message, this should get stripped
return false;
}
Bottle *bot = v.asList();
bot->fromString(body.c_str());
ConstString methodName;
if (sender) {
methodName = bot->get(0).toString();
*bot = bot->tail();
}
XmlRpcValue args;
if (bot->size()==1) {
toXmlRpcValue(bot->get(0),args);
} else {
toXmlRpcValue(v,args);
}
std::string req;
if (sender) {
const Contact& addr = host.isValid()?host:proto.getStreams().getRemoteAddress();
XmlRpcClient c(addr.getHost().c_str(),(addr.getPort()>0)?addr.getPort():80);
c.generateRequest(methodName.c_str(),args);
req = c.getRequest();
} else {
XmlRpcServerConnection c(0,NULL);
c.generateResponse(args.toXml());
req = c.getResponse();
}
int start = 0;
if (sender) {
if (req.length()<8) {
fprintf(stderr, "XmlRpcCarrier fail, %s:%d\n", __FILE__, __LINE__);
return false;
}
for (int i=0; i<(int)req.length(); i++) {
if (req[i] == '\n') {
start++;
break;
}
start++;
}
if (!firstRound) {
Bytes b((char*)http.c_str(),http.length());
proto.os().write(b);
}
firstRound = false;
}
Bytes b((char*)req.c_str()+start,req.length()-start);
proto.os().write(b);
return proto.os().isOk();
}
