bool threadInit()
{
string name=rf.find("name").asString().c_str();
double period=rf.find("period").asDouble();
setRate(int(1000.0*period));
// open a client interface to connect to the gaze server
// we suppose that:
// 1 - the iCub simulator (icubSim) is running
// 2 - the gaze server iKinGazeCtrl is running and
// launched with the following options:
// --robot icubSim --context cameraCalibration/conf --config icubSimEyes.ini
Property optGaze("(device gazecontrollerclient)");
optGaze.put("remote","/iKinGazeCtrl");
optGaze.put("local",("/"+name+"/gaze").c_str());
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);
port.open(("/"+name+"/target:i").c_str());
return true;
}
virtual bool threadInit()
{
string name=rf.check("name",Value("faceTracker")).asString().c_str();
string robot=rf.check("robot",Value("icub")).asString().c_str();
int period=rf.check("period",Value(50)).asInt();
eye=rf.check("eye",Value("left")).asString().c_str();
arm=rf.check("arm",Value("right")).asString().c_str();
eyeDist=rf.check("eyeDist",Value(1.0)).asDouble();
holdoff=rf.check("holdoff",Value(3.0)).asDouble();
Property optGaze("(device gazecontrollerclient)");
optGaze.put("remote","/iKinGazeCtrl");
optGaze.put("local",("/"+name+"/gaze_client").c_str());
if (!clientGaze.open(optGaze))
return false;
clientGaze.view(igaze);
igaze->storeContext(&startup_gazeContext_id);
igaze->blockNeckRoll(0.0);
if (arm!="none")
{
Property optArm("(device cartesiancontrollerclient)");
optArm.put("remote",("/"+robot+"/cartesianController/"+arm+"_arm").c_str());
optArm.put("local",("/"+name+"/arm_client").c_str());
if (!clientArm.open(optArm))
return false;
clientArm.view(iarm);
iarm->storeContext(&startup_armContext_id);
}
portImgIn.open(("/"+name+"/img:i").c_str());
portImgOut.open(("/"+name+"/img:o").c_str());
portTopDown.open(("/"+name+"/topdown:i").c_str());
portSetFace.open(("/"+name+"/setFace:rpc").c_str());
if (!fd.init(rf.findFile("descriptor").c_str()))
{
fprintf(stdout,"Cannot load descriptor!\n");
return false;
}
Rand::init();
resetState();
armCmdState=0;
queuedFaceExprFlag=false;
setRate(period);
cvSetNumThreads(1);
t0=Time::now();
return true;
}
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;
}
bool configure(ResourceFinder &rf)
{
std::string name = "MakeItRoll";
// open a client interface to connect to the gaze server
Property optGaze("(device gazecontrollerclient)");
optGaze.put("remote","/iKinGazeCtrl");
optGaze.put("local",("/"+name+"/gaze").c_str());
if (!drvGaze.open(optGaze))
return false;
// open the view
drvGaze.view(igaze);
if(!igaze)
return false;
// 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_gaze);
// set trajectory time:
igaze->setNeckTrajTime(0.8);
igaze->setEyesTrajTime(0.4);
// open a client interface to connect to the arm cartesian control server
Property optArm("(device cartesiancontrollerclient)");
optArm.put("remote","/icubSim/cartesianController/right_arm");
optArm.put("local",("/"+name+"/cartesian/right_arm").c_str());
if (!drvArm.open(optArm))
return false;
// open the view
drvArm.view(iarm);
// latch the controller context in order to preserve
// it after closing the module
iarm->storeContext(&startup_context_arm);
// set trajectory time
iarm->setTrajTime(1.0);
// get the torso dofs
Vector newDof, curDof;
iarm->getDOF(curDof);
newDof=curDof;
// enable the torso yaw and pitch
// disable the torso roll
newDof[0]=1;
newDof[1]=0;
newDof[2]=1;
// impose some restriction on the torso pitch
int axis=0; // pitch joint
double min, max;
// we keep the lower limit
iarm->getLimits(axis, &min, &max);
iarm->setLimits(axis, min, MAX_TORSO_PITCH);
iarm->setDOF(newDof,curDof);
// Opening the required streaming and rpc ports
imgLPortIn.open("/MakeItRoll/imgL:i");
imgRPortIn.open("/MakeItRoll/imgR:i");
imgLPortOut.open("/MakeItRoll/imgL:o");
imgRPortOut.open("/MakeItRoll/imgR:o");
rpcPort.open("/MakeItRoll/service");
attach(rpcPort);
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;
}