virtual void threadRelease()
{
// we require an immediate stop
// before closing the client for safety reason
igaze->stopControl();
// it's a good rule to restore the controller
// context as it was before opening the module
igaze->restoreContext(startup_context_id);
clientGaze.close();
clientTorso.close();
}
bool close()
{
igaze->restoreContext(startup_context_gaze);
drvGaze.close();
iarm->restoreContext(startup_context_arm);
drvArm.close();
imgLPortIn.close();
imgRPortIn.close();
imgLPortOut.close();
imgRPortOut.close();
rpcPort.close();
return true;
}
virtual void threadRelease()
{
armRest();
gazeRest();
igaze->restoreContext(startup_gazeContext_id);
clientGaze.close();
if (arm!="none")
{
iarm->restoreContext(startup_armContext_id);
clientArm.close();
}
portImgIn.interrupt();
portImgOut.interrupt();
portTopDown.interrupt();
portSetFace.interrupt();
portImgIn.close();
portImgOut.close();
portTopDown.close();
portSetFace.close();
}
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 configure(yarp::os::ResourceFinder &rf)
{
Vector newDof, curDof;
cout<<"Configuring module!"<<endl;
moduleName=rf.check("name",Value("torsoModule")).asString().c_str();
robotName=rf.check("robot",Value("icub")).asString().c_str();
OPCName=rf.check("OPC",Value("memory")).asString().c_str();
period=rf.check("period",Value(0.2)).asDouble();
kp=rf.check("kp",Value(KP)).asDouble();
maxTorsoTrajTime=rf.check("torsoTime",Value(MAX_TORSO_TRAJ_TIME)).asDouble();
maxTorsoVelocity=rf.check("maxTorsoVelocity",Value(MAX_TORSO_VELOCITY)).asDouble();
maxArmTrajTime=rf.check("armTime",Value(MAX_ARM_TRAJ_TIME)).asDouble();
handlerPort.open(("/"+moduleName+"/rpc:i").c_str());
attach(handlerPort);
objectsPort.open(("/"+moduleName+"/OPC:io").c_str());
if(!objectsPort.addOutput(("/"+OPCName+"/rpc").c_str())){
cout<<"Error connecting to OPC client!"<<endl;
return false;
}
Property optionGaze;
optionGaze.put("device","gazecontrollerclient");
optionGaze.put("remote","/iKinGazeCtrl");
optionGaze.put("local",("/"+moduleName+"/gaze").c_str());
if(!clientGazeCtrl.open(optionGaze)){
cout<<"Error opening gaze client!"<<endl;
return false;
}
clientGazeCtrl.view(igaze);
igaze->restoreContext(0);
igaze->storeContext(&startupGazeContextID);
gazeHomePosition.push_back(GAZE_HOME_POS_X);
gazeHomePosition.push_back(GAZE_HOME_POS_Y);
gazeHomePosition.push_back(GAZE_HOME_POS_Z);
Property leftArmOption;
leftArmOption.put("device","cartesiancontrollerclient");
leftArmOption.put("remote",("/"+robotName+"/cartesianController/left_arm").c_str());
leftArmOption.put("local",("/"+moduleName+"/left_arm").c_str());
if(!clientArmLeft.open(leftArmOption)){
cout<<"Error opening left arm client!"<<endl;
return false;
}
clientArmLeft.view(icartLeft);
icartLeft->restoreContext(0);
icartLeft->storeContext(&startupArmLeftContextID);
leftArmHomePosition.push_back(LEFT_ARM_HOME_POS_X);
leftArmHomePosition.push_back(LEFT_ARM_HOME_POS_Y);
leftArmHomePosition.push_back(LEFT_ARM_HOME_POS_Z);
icartLeft->setTrajTime(maxArmTrajTime);
icartLeft->storeContext(¤tArmLeftContextID);
Property rightArmOption;
rightArmOption.put("device","cartesiancontrollerclient");
rightArmOption.put("remote",("/"+robotName+"/cartesianController/right_arm").c_str());
rightArmOption.put("local",("/"+moduleName+"/right_arm").c_str());
if(!clientArmRight.open(rightArmOption)){
cout<<"Error opening right arm client!"<<endl;
return false;
}
clientArmRight.view(icartRight);
icartRight->restoreContext(0);
icartRight->storeContext(&startupArmRightContextID);
rightArmHomePosition.push_back(RIGHT_ARM_HOME_POS_X);
rightArmHomePosition.push_back(RIGHT_ARM_HOME_POS_Y);
rightArmHomePosition.push_back(RIGHT_ARM_HOME_POS_Z);
icartRight->setTrajTime(maxArmTrajTime);
icartRight->storeContext(¤tArmRightContextID);
computeArmOr();
Property torsoOptions;
torsoOptions.put("device", "remote_controlboard");
torsoOptions.put("remote",("/"+robotName+"/torso").c_str());
torsoOptions.put("local",("/"+moduleName+"/torso").c_str());
if(!clientTorso.open(torsoOptions)){
cout<<"Error opening torso client!"<<endl;
return false;
}
clientTorso.view(itorsoVelocity);
clientTorso.view(itorsoMode);
torsoHomePosition.push_back(TORSO_HOME_POS_ROLL);
torsoHomePosition.push_back(TORSO_HOME_POS_PITCH);
torsoHomePosition.push_back(TORSO_HOME_POS_YAW);
torsoAcceleration.push_back(TORSO_ACCELERATION_ROLL);
torsoAcceleration.push_back(TORSO_ACCELERATION_PITCH);
torsoAcceleration.push_back(TORSO_ACCELERATION_YAW);
clientTorso.view(iTorsoEncoder);
waypoints.resize(3,3);
waypoints(0,0) = 0.0; waypoints(0,1) = -15.0; waypoints(0,2) = 10.0;
waypoints(1,0) = 0.0; waypoints(1,1) = 15.0; waypoints(1,2) = 10.0;
waypoints(2,0) = 0.0; waypoints(2,1) = 0.0; waypoints(2,2) = 20.0;
index = 0;
running = false;
/*Bottle bAdd, bReply;
bAdd.addVocab(Vocab::encode("add"));
Bottle &bTempAdd=bAdd.addList();
Bottle &bEntity=bTempAdd.addList();
bEntity.addString("entity"); bEntity.addString("action");
Bottle &bName=bTempAdd.addList();
bName.addString("name"); bName.addString("ball");
Bottle &bX= bTempAdd.addList();
bX.addString("position_3d");
Bottle &coord = bX.addList();
coord.addDouble(-0.05);
coord.addDouble(0.0);
coord.addDouble(0.2);
objectsPort.write(bAdd,bReply);
cout<<bReply.get(0).asVocab()<<endl;
*/
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;
}