bool close()
{
handlerPort.close();
objectsPort.close();
igaze->stopControl();
igaze->restoreContext(startupGazeContextID);
igaze->deleteContext(startupGazeContextID);
igaze->deleteContext(currentGazeContextID);
if (clientGazeCtrl.isValid())
clientGazeCtrl.close();
icartLeft->stopControl();
icartLeft->restoreContext(startupArmLeftContextID);
icartLeft->deleteContext(startupArmLeftContextID);
icartLeft->deleteContext(currentArmLeftContextID);
if (clientArmLeft.isValid())
clientArmLeft.close();
icartRight->stopControl();
icartRight->restoreContext(startupArmRightContextID);
icartRight->deleteContext(startupArmRightContextID);
icartRight->deleteContext(currentArmRightContextID);
if (clientArmLeft.isValid())
clientArmLeft.close();
itorsoVelocity->stop();
if (clientTorso.isValid())
clientTorso.close();
return true;
}
bool respond(const Bottle& command, Bottle& reply)
{
if(command.size()==0)
{
reply.addString("No command received.");
return true;
}
switch(command.get(0).asVocab()){
case HOME:
if(command.size()>1)
switch(command.get(1).asVocab()){
case ARM:
int tempCxL,tempCxR;
icartLeft->storeContext(&tempCxL);
icartLeft->storeContext(&tempCxR);
icartLeft->restoreContext(currentArmLeftContextID);
icartLeft->restoreContext(currentArmRightContextID);
icartLeft->goToPositionSync(leftArmHomePosition);
icartRight->goToPositionSync(rightArmHomePosition);
icartRight->waitMotionDone(0.1,2);
icartLeft->waitMotionDone(0.1,2);
icartLeft->restoreContext(tempCxR);
icartLeft->restoreContext(tempCxL);
icartLeft->deleteContext(tempCxR);
icartLeft->deleteContext(tempCxL);
reply.addString("Arm home position reached.");
return true;
case TORSO:
exploreTorso(torsoHomePosition);
reply.addString("Torso home position reached.");
return true;
case GAZE:
int tempCx;
igaze->storeContext(&tempCx);
igaze->restoreContext(currentGazeContextID);
igaze->lookAtFixationPoint(gazeHomePosition);
igaze->waitMotionDone(0.1,2);
igaze->restoreContext(tempCx);
igaze->deleteContext(tempCx);
reply.addString("Gaze home position reached.");
return true;
default:
reply.addString("Wrong device for home position.");
return true;
}
else{
int tempCxL,tempCxR,tempCx;
icartLeft->storeContext(&tempCxL);
icartLeft->storeContext(&tempCxR);
icartLeft->restoreContext(currentArmLeftContextID);
icartLeft->restoreContext(currentArmRightContextID);
igaze->storeContext(&tempCx);
igaze->restoreContext(currentGazeContextID);
icartLeft->goToPositionSync(leftArmHomePosition);
icartRight->goToPositionSync(rightArmHomePosition);
igaze->lookAtFixationPoint(gazeHomePosition);
exploreTorso(torsoHomePosition);
igaze->waitMotionDone(0.1,2);
icartRight->waitMotionDone(0.1,2);
icartLeft->waitMotionDone(0.1,2);
icartLeft->restoreContext(tempCxR);
igaze->restoreContext(tempCx);
icartLeft->restoreContext(tempCxL);
icartLeft->deleteContext(tempCxR);
icartLeft->deleteContext(tempCxL);
igaze->deleteContext(tempCx);
reply.addString("Ok, moving to home position.");
return true;
}
case LOOK_AT:
if(command.size()==2){
Bottle bAsk,bReply, bGet;
bAsk.addVocab(Vocab::encode("ask"));
Bottle &bTempAsk=bAsk.addList().addList();
bTempAsk.addString("name");
bTempAsk.addString("==");
bTempAsk.addString(command.get(1).asString());
objectsPort.write(bAsk,bReply);
cout<<"first"<<endl;
if(bReply.size()==0 || bReply.get(0).asVocab()!=Vocab::encode("ack") || bReply.get(1).asList()->check("id")==false ||
bReply.get(1).asList()->find("id").asList()->size()==0){
reply.addVocab(Vocab::encode("nack"));
return true;
}
bGet.addVocab(Vocab::encode("get"));
Bottle &bTempGet=bGet.addList().addList();
bTempGet.addString("id");
bTempGet.addInt(bReply.get(1).asList()->find("id").asList()->get(0).asInt());
objectsPort.write(bGet,bReply);
cout<<"second"<<endl;
if(bReply.size()==0 || bReply.get(0).asVocab()!=Vocab::encode("ack") || bReply.get(1).asList()->check("position_3d")==false ||
bReply.get(1).asList()->find("position_3d").asList()->size()==0){
reply.addVocab(Vocab::encode("nack"));
return true;
}
cout<<"third"<<endl;
Vector gazePosition(3);
gazePosition[0] = bReply.get(1).asList()->find("position_3d").asList()->get(0).asDouble();
gazePosition[1] = bReply.get(1).asList()->find("position_3d").asList()->get(1).asDouble();
gazePosition[2] = bReply.get(1).asList()->find("position_3d").asList()->get(2).asDouble();
int tempCx;
igaze->storeContext(&tempCx);
igaze->restoreContext(currentGazeContextID);
igaze->lookAtFixationPoint(gazePosition);
igaze->waitMotionDone(0.2,3);
igaze->restoreContext(tempCx);
igaze->deleteContext(tempCx);
reply.addString("Gaze position reached.");
return true;
}
else if(command.size()==4){
Vector gazePosition(3);
int tempCx;
gazePosition[0] = command.get(1).asDouble();
gazePosition[1] = command.get(2).asDouble();
gazePosition[2] = command.get(3).asDouble();
igaze->storeContext(&tempCx);
igaze->restoreContext(currentGazeContextID);
igaze->lookAtFixationPoint(gazePosition);
igaze->waitMotionDone(0.2,3);
igaze->restoreContext(tempCx);
igaze->deleteContext(tempCx);
reply.addString("Gaze position reached.");
return true;
}
else{
reply.addString("Wrong number of parameters for lookAt.");
return true;
}
case GET:
if(command.size()==2){
Bottle bAsk,bReply, bGet;
bAsk.addVocab(Vocab::encode("ask"));
Bottle &bTempAsk=bAsk.addList().addList();
bTempAsk.addString("name");
bTempAsk.addString("==");
bTempAsk.addString(command.get(1).asString());
objectsPort.write(bAsk,bReply);
if(bReply.size()==0 || bReply.get(0).asVocab()!=Vocab::encode("ack") || bReply.get(1).asList()->check("id")==false ||
bReply.get(1).asList()->find("id").asList()->size()==0){
reply.addVocab(Vocab::encode("nack"));
return true;
}
bGet.addVocab(Vocab::encode("get"));
Bottle &bTempGet=bGet.addList().addList();
bTempGet.addString("id");
bTempGet.addInt(bReply.get(1).asList()->find("id").asList()->get(0).asInt());
objectsPort.write(bGet,bReply);
if(bReply.size()==0 || bReply.get(0).asVocab()!=Vocab::encode("ack") || bReply.get(1).asList()->check("position_2d_left")==false ||
bReply.get(1).asList()->find("position_2d_left").asList()->size()==0){
reply.addVocab(Vocab::encode("nack"));
return true;
}
Vector objPosition(4);
objPosition[0] = bReply.get(1).asList()->find("position_2d_left").asList()->get(0).asInt();
objPosition[1] = bReply.get(1).asList()->find("position_2d_left").asList()->get(1).asInt();
objPosition[2] = bReply.get(1).asList()->find("position_2d_left").asList()->get(2).asInt();
objPosition[3] = bReply.get(1).asList()->find("position_2d_left").asList()->get(3).asInt();
reply.addVocab(VOCAB3('a','c','k'));
Bottle &coord = reply.addList();
coord.addInt((int)(objPosition[0]+objPosition[2])/2);
coord.addInt((int)(objPosition[1]+objPosition[3])/2);
return true;
}
else{
reply.addString("Wrong number of parameters for get.");
return true;
}
case TRACK:
if(command.size()==3)
switch(command.get(1).asVocab()){
case ARM:
if (command.get(2).asString() == "on"){
int tempCxL,tempCxR;
//icartLeft->storeContext(&tempCxL);
//icartLeft->storeContext(&tempCxR);
//icartLeft->restoreContext(currentArmLeftContextID);
//icartLeft->restoreContext(currentArmRightContextID);
icartLeft->setTrackingMode(true);
icartRight->setTrackingMode(true);
icartRight->storeContext(¤tArmRightContextID);
icartLeft->storeContext(¤tArmLeftContextID);
//icartLeft->restoreContext(tempCxR);
// icartLeft->restoreContext(tempCxL);
// icartLeft->deleteContext(tempCxR);
// icartLeft->deleteContext(tempCxL);
reply.addString("Arm tracking mode enabled.");
}
else if (command.get(2).asString() == "off"){
int tempCxL,tempCxR;
//icartLeft->storeContext(&tempCxL);
//icartLeft->storeContext(&tempCxR);
//icartLeft->restoreContext(currentArmLeftContextID);
//icartLeft->restoreContext(currentArmRightContextID);
icartLeft->setTrackingMode(false);
icartRight->setTrackingMode(false);
icartRight->storeContext(¤tArmRightContextID);
icartLeft->storeContext(¤tArmLeftContextID);
//icartLeft->restoreContext(tempCxR);
// icartLeft->restoreContext(tempCxL);
// icartLeft->deleteContext(tempCxR);
// icartLeft->deleteContext(tempCxL);
reply.addString("Arm tracking mode disabled.");
}
else
reply.addString("Wrong parameter: on/off");
return true;
case GAZE:
if (command.get(2).asString() == "on"){
int tempCx;
//igaze->storeContext(&tempCx);
//igaze->restoreContext(currentGazeContextID);
igaze->setTrackingMode(true);
igaze->storeContext(¤tGazeContextID);
//igaze->restoreContext(tempCx);
//igaze->deleteContext(tempCx);
reply.addString("Gaze tracking mode enabled.");
}
else if (command.get(2).asString() == "off"){
int tempCx;
//igaze->storeContext(&tempCx);
//igaze->restoreContext(currentGazeContextID);
igaze->setTrackingMode(false);
igaze->storeContext(¤tGazeContextID);
//igaze->restoreContext(tempCx);
//igaze->deleteContext(tempCx);
reply.addString("Gaze tracking mode disabled.");
}
else
reply.addString("Wrong parameter for trac: on/off");
return true;
default:
reply.addString("Wrong device for tracking mode.");
return true;
}
else{
reply.addString("Missing parameters for track.");
return true;
}
case BLOCK:
if(command.size()>1)
switch(command.get(1).asVocab()){
case GAZE:
if(command.size()==3){
// int tempCx;
//igaze->storeContext(&tempCx);
//igaze->restoreContext(currentGazeContextID);
igaze->blockEyes(command.get(2).asDouble());
igaze->storeContext(¤tGazeContextID);
//igaze->restoreContext(tempCx);
//igaze->deleteContext(tempCx);
reply.addString("Gaze blocking mode enabled.");
}
else{
//int tempCx;
//igaze->storeContext(&tempCx);
//igaze->restoreContext(currentGazeContextID);
igaze->blockEyes(DEFAULT_VERGENCE);
igaze->storeContext(¤tGazeContextID);
//igaze->restoreContext(tempCx);
//igaze->deleteContext(tempCx);
reply.addString("Default vergence set.");
}
return true;
default:
reply.addString("Wrong device for blocking mode.");
return true;
}
else{
reply.addString("Missing parameters for block.");
return true;
}
case GOTO:
if(command.size()==4){
Vector torsoTarget(3);
torsoTarget[0] = command.get(1).asDouble();
torsoTarget[1] = command.get(2).asDouble();
torsoTarget[2] = command.get(3).asDouble();
exploreTorso(torsoTarget);
reply.addString("Torso position reached.");
}
else{
reply.addString("Missing parameters for goto.");
return true;
}
return true;
case RUN:
running = true;
reply.addString("Ready for exploration. next or stop?");
return true;
case NEXT:
if(running){
exploreTorso(waypoints.getRow(index));
index++;
if (index > 2){
running = false;
index = 0;
reply.addString("Waypoint reached. End of waypoints.");
}
else{
reply.addString("Waypoint reached. next or stop?");
}
}
else
reply.addString("Not running.");
return true;
case STOP:
index = 0;
running = false;
reply.addString("Run stopped. Index reset.");
return true;
default:
RFModule::respond(command,reply);
return true;
}
return true;
}