virtual bool threadInit()
{
// open a client interface to connect to the gaze server
// we suppose that:
// 1 - the iCub simulator is running;
// 2 - the gaze server iKinGazeCtrl is running and
// launched with the following options: "--from configSim.ini"
Property optGaze("(device gazecontrollerclient)");
optGaze.put("remote","/iKinGazeCtrl");
optGaze.put("local","/gaze_client");
if (!clientGaze.open(optGaze))
return false;
// open the view
clientGaze.view(igaze);
// latch the controller context in order to preserve
// it after closing the module
// the context contains the tracking mode, the neck limits and so on.
igaze->storeContext(&startup_context_id);
// set trajectory time:
igaze->setNeckTrajTime(0.8);
igaze->setEyesTrajTime(0.4);
// put the gaze in tracking mode, so that
// when the torso moves, the gaze controller
// will compensate for it
igaze->setTrackingMode(true);
// print out some info about the controller
Bottle info;
igaze->getInfo(info);
fprintf(stdout,"info = %s\n",info.toString().c_str());
Property optTorso("(device remote_controlboard)");
optTorso.put("remote","/icubSim/torso");
optTorso.put("local","/torso_client");
if (!clientTorso.open(optTorso))
return false;
// open the view
clientTorso.view(ienc);
clientTorso.view(ipos);
ipos->setRefSpeed(0,10.0);
fp.resize(3);
state=STATE_TRACK;
t=t0=t1=t2=t3=t4=Time::now();
return true;
}
virtual void threadRelease()
{
// we require an immediate stop
// before closing the client for safety reason
// (anyway it's already done internally in the
// destructor)
igaze->stopControl();
// it's a good rule to reinstate the tracking mode
// as it was
igaze->setTrackingMode(false);
delete clientGaze;
// delete clientTorso; //pramod
}
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;
}