bool configure(ResourceFinder &rf)
{
if(rf.check("period"))
period = rf.find("period").asDouble();
else
period = 0.3;
return detector->open(rf);
}
bool configure(ResourceFinder &rf)
{
ConstString cascade;
ConstString nestedCascade;
if(!rf.check("cascade") /*|| !rf.check("nested-cascade")*/)
{
fprintf(stderr, "Could not find the cascade file. \n");
return false;
}
detector->strCascade = rf.getContextPath() + "/" + rf.find("cascade").asString();
printf("cascade: %s\n", detector->strCascade.c_str());
//detector->strNestedCascade= rf.find("nested-cascade").asString();
return detector->open(rf);
}
bool configure(ResourceFinder &rf)
{
printf(" Starting Configure\n");
this->rf=&rf;
//****************** PARAMETERS ******************
robot = rf.check("robot",Value("icub")).asString().c_str();
name = rf.check("name",Value("demoINNOROBO")).asString().c_str();
arm = rf.check("arm",Value("right_arm")).asString().c_str();
verbosity = rf.check("verbosity",Value(0)).asInt();
rate = rf.check("rate",Value(0.5)).asDouble();
if (arm!="right_arm" && arm!="left_arm")
{
printMessage(0,"ERROR: arm was set to %s, putting it to right_arm\n",arm.c_str());
arm="right_arm";
}
printMessage(1,"Parameters correctly acquired\n");
printMessage(1,"Robot: %s \tName: %s \tArm: %s\n",robot.c_str(),name.c_str(),arm.c_str());
printMessage(1,"Verbosity: %i \t Rate: %g \n",verbosity,rate);
//****************** DETECTOR ******************
certainty = rf.check("certainty", Value(10.0)).asInt();
strCascade = rf.check("cascade", Value("haarcascade_frontalface_alt.xml")).asString().c_str();
detectorL=new Detector();
detectorL->open(certainty,strCascade);
detectorR=new Detector();
detectorR->open(certainty,strCascade);
printMessage(1,"Detectors opened\n");
//****************** DRIVERS ******************
Property optionArm("(device remote_controlboard)");
optionArm.put("remote",("/"+robot+"/"+arm).c_str());
optionArm.put("local",("/"+name+"/"+arm).c_str());
if (!drvArm.open(optionArm))
{
printf("Position controller not available!\n");
return false;
}
Property optionCart("(device cartesiancontrollerclient)");
optionCart.put("remote",("/"+robot+"/cartesianController/"+arm).c_str());
optionCart.put("local",("/"+name+"/cart/").c_str());
if (!drvCart.open(optionCart))
{
printf("Cartesian controller not available!\n");
// return false;
}
Property optionGaze("(device gazecontrollerclient)");
optionGaze.put("remote","/iKinGazeCtrl");
optionGaze.put("local",("/"+name+"/gaze").c_str());
if (!drvGaze.open(optionGaze))
{
printf("Gaze controller not available!\n");
// return false;
}
// quitting conditions
bool andArm=drvArm.isValid()==drvCart.isValid()==drvGaze.isValid();
if (!andArm)
{
printMessage(0,"Something wrong occured while configuring drivers... quitting!\n");
return false;
}
drvArm.view(iencs);
drvArm.view(iposs);
drvArm.view(ictrl);
drvArm.view(iimp);
drvCart.view(iarm);
drvGaze.view(igaze);
iencs->getAxes(&nEncs);
iimp->setImpedance(0, 0.4, 0.03);
iimp->setImpedance(1, 0.35, 0.03);
iimp->setImpedance(2, 0.35, 0.03);
iimp->setImpedance(3, 0.2, 0.02);
iimp->setImpedance(4, 0.2, 0.00);
ictrl -> setImpedancePositionMode(0);
ictrl -> setImpedancePositionMode(1);
ictrl -> setImpedancePositionMode(2);
ictrl -> setImpedancePositionMode(3);
ictrl -> setImpedancePositionMode(2);
ictrl -> setImpedancePositionMode(4);
igaze -> storeContext(&contextGaze);
igaze -> setSaccadesStatus(false);
igaze -> setNeckTrajTime(0.75);
igaze -> setEyesTrajTime(0.5);
iarm -> storeContext(&contextCart);
printMessage(1,"Drivers opened\n");
//****************** PORTS ******************
imagePortInR -> open(("/"+name+"/imageR:i").c_str());
imagePortInL -> open(("/"+name+"/imageL:i").c_str());
imagePortOutR.open(("/"+name+"/imageR:o").c_str());
imagePortOutL.open(("/"+name+"/imageL:o").c_str());
portOutInfo.open(("/"+name+"/info:o").c_str());
if (robot=="icub")
{
Network::connect("/icub/camcalib/left/out",("/"+name+"/imageL:i").c_str());
Network::connect("/icub/camcalib/right/out",("/"+name+"/imageR:i").c_str());
}
else
{
Network::connect("/icubSim/cam/left",("/"+name+"/imageL:i").c_str());
Network::connect("/icubSim/cam/right",("/"+name+"/imageR:i").c_str());
}
Network::connect(("/"+name+"/imageL:o").c_str(),"/demoINN/left");
Network::connect(("/"+name+"/imageR:o").c_str(),"/demoINN/right");
Network::connect(("/"+name+"/info:o").c_str(),"/iSpeak");
rpcClnt.open(("/"+name+"/rpc:o").c_str());
rpcSrvr.open(("/"+name+"/rpc:i").c_str());
attach(rpcSrvr);
printMessage(0,"Configure Finished!\n");
return true;
}
bool configure(ResourceFinder &rf)
{
return detector->open(rf);
}
