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)
{
string robot=rf.check("robot",Value("icub")).asString().c_str();
string name=rf.check("name",Value("karmaToolFinder")).asString().c_str();
arm=rf.check("arm",Value("right")).asString().c_str();
eye=rf.check("eye",Value("left")).asString().c_str();
if ((arm!="left") && (arm!="right"))
{
printf("Invalid arm requested!\n");
return false;
}
if ((eye!="left") && (eye!="right"))
{
printf("Invalid eye requested!\n");
return false;
}
Property optionArmL("(device cartesiancontrollerclient)");
optionArmL.put("remote",("/"+robot+"/cartesianController/left_arm").c_str());
optionArmL.put("local",("/"+name+"/left_arm").c_str());
if (!drvArmL.open(optionArmL))
{
printf("Cartesian left_arm controller not available!\n");
terminate();
return false;
}
Property optionArmR("(device cartesiancontrollerclient)");
optionArmR.put("remote",("/"+robot+"/cartesianController/right_arm").c_str());
optionArmR.put("local",("/"+name+"/right_arm").c_str());
if (!drvArmR.open(optionArmR))
{
printf("Cartesian right_arm controller not available!\n");
terminate();
return false;
}
if (arm=="left")
drvArmL.view(iarm);
else
drvArmR.view(iarm);
Property optionGaze("(device gazecontrollerclient)");
optionGaze.put("remote","/iKinGazeCtrl");
optionGaze.put("local",("/"+name+"/gaze").c_str());
if (drvGaze.open(optionGaze))
drvGaze.view(igaze);
else
{
printf("Gaze controller not available!\n");
terminate();
return false;
}
Bottle info;
igaze->getInfo(info);
if (Bottle *pB=info.find(("camera_intrinsics_"+eye).c_str()).asList())
{
int cnt=0;
Prj.resize(3,4);
for (int r=0; r<Prj.rows(); r++)
for (int c=0; c<Prj.cols(); c++)
Prj(r,c)=pB->get(cnt++).asDouble();
}
else
{
printf("Camera intrinsic parameters not available!\n");
terminate();
return false;
}
imgInPort.open(("/"+name+"/img:i").c_str());
imgOutPort.open(("/"+name+"/img:o").c_str());
dataInPort.open(("/"+name+"/in").c_str());
logPort.open(("/"+name+"/log:o").c_str());
rpcPort.open(("/"+name+"/rpc").c_str());
attach(rpcPort);
Vector min(3),max(3);
min[0]=-1.0; max[0]=1.0;
min[1]=-1.0; max[1]=1.0;
min[2]=-1.0; max[2]=1.0;
solver.setBounds(min,max);
solution.resize(3,0.0);
enabled=false;
dataInPort.setReader(*this);
return true;
}