bool configure(ResourceFinder &rf)
{
rf1=rf;
if(algorithm==1)
loc5=new UnscentedParticleFilter();
else
loc5=new ScalingSeries();
loc5->configure(rf1);
state=0;
meas_given=1;
if(loc5->measurementsString!=1)
{
namePorts = rf.check("namePorts", Value("visual-localization"), "Getting module name").asString();
portIn.open("/"+namePorts+"/pnt:i");
rpcOut.open("/"+namePorts+"/toolFeat:rpc");
meas_given=0;
}
cout<<"meas_given "<<meas_given<<endl;
error_thres=loc5->error_thres;
cout<<"thresconf "<<error_thres<<endl;
rpcPort.open(("/"+namePorts+"/rpc").c_str());
attach(rpcPort);
delete loc5;
return true;
}
/*
* Configure function. Receive a previously initialized
* resource finder object. Use it to configure your module.
* Open port and attach it to message handler.
*/
bool configure(yarp::os::ResourceFinder &rf)
{
count=0;
handlerPort.open("/myModule");
attach(handlerPort);
return true;
}
bool configure(ResourceFinder &rf)
{
iPort.open("/tracker:i");
oPort.open("/tracker:o");
rpcPort.open("/tracker:rpc");
attach(rpcPort);
state=idle;
return true;
}
bool configure(ResourceFinder &rf)
{
string name=rf.check("name",Value("cmt")).asString().c_str();
imgInPort.open(("/"+name+"/img:i").c_str());
imgOutPort.open(("/"+name+"/img:o").c_str());
dataOutPort.open(("/"+name+"/data:o").c_str());
rpcPort.open(("/"+name+"/rpc").c_str());
attach(rpcPort);
initBoundingBox=doCMT=false;
return true;
}
bool configure(ResourceFinder &rf)
{
// request high resolution scheduling
Time::turboBoost();
speaker.configure(rf);
if (!speaker.start())
return false;
string name=rf.find("name").asString().c_str();
rpc.open(("/"+name+"/rpc").c_str());
attach(rpc);
return true;
}
bool configure(ResourceFinder &rf)
{
interrupting=false;
closing=false;
this->rf=&rf;
Bottle &bGeneral=rf.findGroup("general");
if (bGeneral.isNull())
{
yError("group [general] is missing!");
return false;
}
string name=bGeneral.check("name",Value("fingersTuner")).asString().c_str();
setName(name.c_str());
if (Bottle *bParts=bGeneral.find("relevantParts").asList())
{
for (int i=0; (i<bParts->size()) && !interrupting; i++)
{
string part=bParts->get(i).asString().c_str();
tuners[part]=new Tuner;
Tuner *tuner=tuners[part];
if (!tuner->configure(rf,part))
{
dispose();
return false;
}
tuner->sync(Value("*"));
}
}
else
{
yError("\"relevantParts\" option is missing!");
return false;
}
rpcPort.open(("/"+name+"/rpc").c_str());
attach(rpcPort);
// request high resolution scheduling
Time::turboBoost();
return true;
}
bool configure(yarp::os::ResourceFinder &rf)
{
path = rf.find("clouds_path").asString();
printf("Path: %s",path.c_str());
handlerPort.open("/mergeModule");
attach(handlerPort);
/* Module rpc parameters */
visualizing = false;
saving = true;
closing = false;
/*Init variables*/
initF = true;
filesScanned = 0;
cout<<"Configuring done."<<endl;
return true;
}
bool configure(ResourceFinder &rf)
{
agentName=rf.check("agent-name",Value("partner")).asString();
period=rf.check("period",Value(0.05)).asDouble();
if (!opc.connect("OPC"))
{
yError()<<"OPC seems unavailabe!";
return false;
}
dumpPort.open("/actionRecogDataDumper/data/dump:o");
rpcPort.open("/actionRecogDataDumper/rpc");
attach(rpcPort);
actionTag="none";
objectTag="none";
gate=0;
return true;
}
bool configure(ResourceFinder &rf)
{
portDispIn.open("/3d-points/disp:i");
portDispOut.open("/3d-points/disp:o");
portImgIn.open("/3d-points/img:i");
portOutPoints.open("/3d-points/pnt:o");
portContour.open("/3d-points/contour:i");
portSFM.open("/3d-points/SFM:rpc");
portRpc.open("/3d-points/rpc");
portContour.setReader(*this);
attach(portRpc);
homeContextPath=rf.getHomeContextPath().c_str();
downsampling=std::max(1,rf.check("downsampling",Value(1)).asInt());
spatial_distance=rf.check("spatial_distance",Value(0.004)).asDouble();
color_distance=rf.check("color_distance",Value(6)).asInt();
go=flood3d=flood=false;
return true;
}
int main(int argc, char *argv[]) {
Network yarp;
RpcServer server;
server.promiseType(Type::byNameOnWire("rospy_tutorials/AddTwoInts"));
if (!server.open("/add_two_ints@/yarp_add_int_server")) {
fprintf(stderr,"Failed to open port\n");
return 1;
}
while (true) {
Bottle msg, reply;
if (!server.read(msg,true)) continue;
int x = msg.get(0).asInt32();
int y = msg.get(1).asInt32();
int sum = x + y;
reply.addInt32(sum);
printf("Got %d + %d, answering %d\n", x, y, sum);
server.reply(reply);
}
return 0;
}
bool configure(ResourceFinder &rf)
{
name = "visuoTactileRF";
robot = "icub";
modality = "1D";
verbosity = 0; // verbosity
rate = 20; // rate of the vtRFThread
//******************************************************
//********************** CONFIGS ***********************
//******************* NAME ******************
if (rf.check("name"))
{
name = rf.find("name").asString();
yInfo("Module name set to %s", name.c_str());
}
else yInfo("Module name set to default, i.e. %s", name.c_str());
setName(name.c_str());
//******************* ROBOT ******************
if (rf.check("robot"))
{
robot = rf.find("robot").asString();
yInfo("Robot is %s", robot.c_str());
}
else yInfo("Could not find robot option in the config file; using %s as default",robot.c_str());
//***************** MODALITY *****************
if (rf.check("modality"))
{
modality = rf.find("modality").asString();
yInfo("modality is %s", modality.c_str());
}
else yInfo("Could not find modality option in the config file; using %s as default",modality.c_str());
//******************* VERBOSE ******************
if (rf.check("verbosity"))
{
verbosity = rf.find("verbosity").asInt();
yInfo("vtRFThread verbosity set to %i", verbosity);
}
else yInfo("Could not find verbosity option in config file; using %i as default",verbosity);
//****************** rate ******************
if (rf.check("rate"))
{
rate = rf.find("rate").asInt();
yInfo("vtRFThread working at %i ms", rate);
}
else yInfo("Could not find rate in the config file; using %i ms as default",rate);
//************* skinTaxels' Resource finder **************
ResourceFinder skinRF;
skinRF.setVerbose(false);
skinRF.setDefaultContext("skinGui"); //overridden by --context parameter
skinRF.setDefaultConfigFile("skinManForearms.ini"); //overridden by --from parameter
skinRF.configure(0,NULL);
vector<string> filenames;
int partNum=4;
Bottle &skinEventsConf = skinRF.findGroup("SKIN_EVENTS");
if(!skinEventsConf.isNull())
{
yInfo("SKIN_EVENTS section found\n");
if(skinEventsConf.check("skinParts"))
{
Bottle* skinPartList = skinEventsConf.find("skinParts").asList();
partNum=skinPartList->size();
}
if(skinEventsConf.check("taxelPositionFiles"))
{
Bottle *taxelPosFiles = skinEventsConf.find("taxelPositionFiles").asList();
if (rf.check("leftHand") || rf.check("leftForeArm") || rf.check("rightHand") || rf.check("rightForeArm"))
{
if (rf.check("leftHand"))
{
string taxelPosFile = taxelPosFiles->get(0).asString().c_str();
string filePath(skinRF.findFile(taxelPosFile.c_str()));
if (filePath!="")
{
yInfo("[visuoTactileRF] filePath [%i] %s\n",0,filePath.c_str());
filenames.push_back(filePath);
}
}
if (rf.check("leftForeArm"))
{
string taxelPosFile = taxelPosFiles->get(1).asString().c_str();
string filePath(skinRF.findFile(taxelPosFile.c_str()));
if (filePath!="")
{
yInfo("[visuoTactileRF] filePath [%i] %s\n",1,filePath.c_str());
filenames.push_back(filePath);
}
}
if (rf.check("rightHand"))
{
string taxelPosFile = taxelPosFiles->get(2).asString().c_str();
string filePath(skinRF.findFile(taxelPosFile.c_str()));
if (filePath!="")
{
yInfo("[visuoTactileRF] filePath [%i] %s\n",2,filePath.c_str());
filenames.push_back(filePath);
}
}
if (rf.check("rightForeArm"))
{
string taxelPosFile = taxelPosFiles->get(3).asString().c_str();
string filePath(skinRF.findFile(taxelPosFile.c_str()));
if (filePath!="")
{
yInfo("[visuoTactileRF] filePath [%i] %s\n",3,filePath.c_str());
filenames.push_back(filePath);
}
}
}
else
{
for(int i=0;i<partNum;i++) // all of the skinparts
{
string taxelPosFile = taxelPosFiles->get(i).asString().c_str();
string filePath(skinRF.findFile(taxelPosFile.c_str()));
if (filePath!="")
{
yInfo("[visuoTactileRF] filePath [%i] %s\n",i,filePath.c_str());
filenames.push_back(filePath);
}
}
}
}
}
else
{
yError(" No skin's configuration files found.");
return 0;
}
//*************** eyes' Resource finder ****************
ResourceFinder gazeRF;
gazeRF.setVerbose(verbosity!=0);
gazeRF.setDefaultContext("iKinGazeCtrl");
robot=="icub"?gazeRF.setDefaultConfigFile("config.ini"):gazeRF.setDefaultConfigFile("configSim.ini");
gazeRF.configure(0,NULL);
double head_version=gazeRF.check("head_version",Value(1.0)).asDouble();
ResourceFinder eyeAlignRF;
Bottle &camerasGroup = gazeRF.findGroup("cameras");
if(!camerasGroup.isNull())
{
eyeAlignRF.setVerbose(verbosity!=0);
camerasGroup.check("context")?
eyeAlignRF.setDefaultContext(camerasGroup.find("context").asString().c_str()):
eyeAlignRF.setDefaultContext(gazeRF.getContext().c_str());
eyeAlignRF.setDefaultConfigFile(camerasGroup.find("file").asString().c_str());
eyeAlignRF.configure(0,NULL);
}
else
{
yWarning("Did not find camera calibration group into iKinGazeCtrl ResourceFinder!");
}
//******************************************************
//*********************** THREAD **********************
if( filenames.size() > 0 )
{
vtRFThrd = new vtRFThread(rate, name, robot, modality, verbosity, rf,
filenames, head_version, eyeAlignRF);
if (!vtRFThrd -> start())
{
delete vtRFThrd;
vtRFThrd = 0;
yError("vtRFThread wasn't instantiated!!");
return false;
}
yInfo("VISUO TACTILE RECEPTIVE FIELDS: vtRFThread instantiated...");
}
else {
vtRFThrd = 0;
yError("vtRFThread wasn't instantiated. No filenames have been given!");
return false;
}
//******************************************************
//************************ PORTS ***********************
rpcClnt.open(("/"+name+"/rpc:o").c_str());
rpcSrvr.open(("/"+name+"/rpc:i").c_str());
attach(rpcSrvr);
return true;
}
//********************************************
bool configure(ResourceFinder &rf)
{
if (rf.check("noLeftArm") && rf.check("noRightArm"))
{
yError("[demoAvoidance] no arm has been selected. Closing.");
return false;
}
useLeftArm = false;
useRightArm = false;
string name=rf.check("name",Value("avoidance")).asString().c_str();
string robot=rf.check("robot",Value("icub")).asString().c_str();
motionGain = -1.0;
if (rf.check("catching"))
{
motionGain = 1.0;
}
if (motionGain!=-1.0)
{
yWarning("[demoAvoidance] motionGain set to catching");
}
bool autoConnect=rf.check("autoConnect");
if (autoConnect)
{
yWarning("[demoAvoidance] Autoconnect mode set to ON");
}
bool stiff=rf.check("stiff");
if (stiff)
{
yInfo("[demoAvoidance] Stiff Mode enabled.");
}
Matrix R(4,4);
R(0,0)=-1.0; R(2,1)=-1.0; R(1,2)=-1.0; R(3,3)=1.0;
if (!rf.check("noLeftArm"))
{
useLeftArm=true;
data["left"]=Data();
data["left"].home_x[0]=-0.30;
data["left"].home_x[1]=-0.20;
data["left"].home_x[2]=+0.05;
data["left"].home_o=dcm2axis(R);
Property optionCartL;
optionCartL.put("device","cartesiancontrollerclient");
optionCartL.put("remote","/"+robot+"/cartesianController/left_arm");
optionCartL.put("local",("/"+name+"/cart/left_arm").c_str());
if (!driverCartL.open(optionCartL))
{
close();
return false;
}
Property optionJointL;
optionJointL.put("device","remote_controlboard");
optionJointL.put("remote","/"+robot+"/left_arm");
optionJointL.put("local",("/"+name+"/joint/left_arm").c_str());
if (!driverJointL.open(optionJointL))
{
close();
return false;
}
driverCartL.view(data["left"].iarm);
data["left"].iarm->storeContext(&contextL);
Vector dof;
data["left"].iarm->getDOF(dof);
dof=0.0; dof[3]=dof[4]=dof[5]=dof[6]=1.0;
data["left"].iarm->setDOF(dof,dof);
data["left"].iarm->setTrajTime(0.9);
data["left"].iarm->goToPoseSync(data["left"].home_x,data["left"].home_o);
data["left"].iarm->waitMotionDone();
IInteractionMode *imode; driverJointL.view(imode);
IImpedanceControl *iimp; driverJointL.view(iimp);
if (!stiff)
{
imode->setInteractionMode(0,VOCAB_IM_COMPLIANT); iimp->setImpedance(0,0.4,0.03);
imode->setInteractionMode(1,VOCAB_IM_COMPLIANT); iimp->setImpedance(1,0.4,0.03);
imode->setInteractionMode(2,VOCAB_IM_COMPLIANT); iimp->setImpedance(2,0.4,0.03);
imode->setInteractionMode(3,VOCAB_IM_COMPLIANT); iimp->setImpedance(3,0.2,0.01);
imode->setInteractionMode(4,VOCAB_IM_COMPLIANT); iimp->setImpedance(4,0.2,0.0);
}
}
if (!rf.check("noRightArm"))
{
useRightArm = true;
data["right"]=Data();
data["right"].home_x[0]=-0.30;
data["right"].home_x[1]=+0.20;
data["right"].home_x[2]=+0.05;
data["right"].home_o=dcm2axis(R);
Property optionCartR;
optionCartR.put("device","cartesiancontrollerclient");
optionCartR.put("remote","/"+robot+"/cartesianController/right_arm");
optionCartR.put("local",("/"+name+"/cart/right_arm").c_str());
if (!driverCartR.open(optionCartR))
{
close();
return false;
}
Property optionJointR;
optionJointR.put("device","remote_controlboard");
optionJointR.put("remote","/"+robot+"/right_arm");
optionJointR.put("local",("/"+name+"/joint/right_arm").c_str());
if (!driverJointR.open(optionJointR))
{
close();
return false;
}
driverCartR.view(data["right"].iarm);
data["right"].iarm->storeContext(&contextR);
Vector dof;
data["right"].iarm->getDOF(dof);
dof=0.0; dof[3]=dof[4]=dof[5]=dof[6]=1.0;
data["right"].iarm->setDOF(dof,dof);
data["right"].iarm->setTrajTime(0.9);
data["right"].iarm->goToPoseSync(data["right"].home_x,data["right"].home_o);
data["right"].iarm->waitMotionDone();
IInteractionMode *imode; driverJointR.view(imode);
IImpedanceControl *iimp; driverJointR.view(iimp);
if (!stiff)
{
imode->setInteractionMode(0,VOCAB_IM_COMPLIANT); iimp->setImpedance(0,0.4,0.03);
imode->setInteractionMode(1,VOCAB_IM_COMPLIANT); iimp->setImpedance(1,0.4,0.03);
imode->setInteractionMode(2,VOCAB_IM_COMPLIANT); iimp->setImpedance(2,0.4,0.03);
imode->setInteractionMode(3,VOCAB_IM_COMPLIANT); iimp->setImpedance(3,0.2,0.01);
imode->setInteractionMode(4,VOCAB_IM_COMPLIANT); iimp->setImpedance(4,0.2,0.0);
}
}
dataPort.open(("/"+name+"/data:i").c_str());
dataPort.setReader(*this);
if (autoConnect)
{
Network::connect("/visuoTactileRF/skin_events:o",("/"+name+"/data:i").c_str());
}
rpcSrvr.open(("/"+name+"/rpc:i").c_str());
attach(rpcSrvr);
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;
}
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 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(ResourceFinder &rf)
{
Time::turboBoost();
string name=rf.check("name",Value("yarpMinJerk")).asString().c_str();
string robot=rf.check("robot",Value("icubSim")).asString().c_str();
string part=rf.check("part",Value("left_arm")).asString().c_str();
joint=rf.check("joint",Value(3)).asInt();
compliant=rf.check("compliant");
Property option;
option.put("device","remote_controlboard");
option.put("remote",("/"+robot+"/"+part).c_str());
option.put("local",("/"+name+"/"+part).c_str());
if (!drv.open(option))
return false;
drv.view(imod);
drv.view(iint);
drv.view(ienc);
drv.view(ivel);
imod->setControlMode(joint,VOCAB_CM_VELOCITY);
if (compliant)
iint->setInteractionMode(joint,VOCAB_IM_COMPLIANT);
IControlLimits *ilim; drv.view(ilim);
double min_joint,max_joint;
ilim->getLimits(joint,&min_joint,&max_joint);
double enc;
while (!ienc->getEncoder(joint,&enc))
Time::delay(0.1);
Controller_P.Plant_IC=enc;
Controller_P.Plant_Max=max_joint;
Controller_P.Plant_Min=min_joint;
Controller_P.AutoCompensator_ThresHystMax=0.5;
Controller_P.AutoCompensator_ThresHystMin=0.1;
yInfo("enc=%g in [%g, %g] deg",enc,min_joint,max_joint);
// Pack model data into RTM
Controller_M->ModelData.defaultParam = &Controller_P;
Controller_M->ModelData.blockIO = &Controller_B;
Controller_M->ModelData.dwork = &Controller_DW;
// Initialize model
Controller_initialize(Controller_M, &Controller_U_reference,
&Controller_U_compensator_state,
&Controller_U_plant_output,
&Controller_Y_controller_output,
&Controller_Y_controller_reference,
&Controller_Y_plant_reference,
&Controller_Y_error_statistics,
&Controller_Y_enable_compensation);
Controller_U_reference=enc;
Controller_U_compensator_state=CompensatorState::Off;
Controller_U_plant_output=enc;
dataIn.open(("/"+name+"/data:i").c_str());
dataOut.open(("/"+name+"/data:o").c_str());
rpc.open(("/"+name+"/rpc").c_str());
attach(rpc);
return true;
}
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;
}
int main(int argc, char * argv[])
{
Network yarp;
cout << "Server name is : " << yarp.getNameServerName() << endl;
if (!Network::checkNetwork())
{
cout << "yarp network is not available!" << endl;
return 0;
}
ResourceFinder rf;
rf.setVerbose(true);
rf.setDefaultContext("visualSystem");
rf.setDefaultConfigFile("visualSystem.ini"); //overridden by --from parameter
rf.configure(argc, argv);
//Get the parameters of the model
cvz::core::CvzStack stack;
int retinaX = rf.check("retinaW",Value(3)).asInt();
int retinaY = rf.check("retinaH", Value(3)).asInt();
int foveaX = rf.check("foveaW", Value(3)).asInt();
int foveaY = rf.check("foveaH", Value(3)).asInt();
/*************************************************/
//Add V1
configureV1Retina(&stack, retinaX, retinaY);
configureV1Fovea(&stack, foveaX, foveaY);
/*************************************************/
//Add v2
stringstream configV2;
configV2
<< "type" << '\t' << cvz::core::TYPE_MMCM << endl
<< "name" << '\t' << "v2" << endl
<< "width" << '\t' << V2_W << endl
<< "height" << '\t' << V2_H << endl
<< "layers" << '\t' << V2_L << endl
<< "sigmaFactor" << '\t' << V2_SIGMA_FACTOR << endl
<< "learningRate" << '\t' << V2_LEARNING << endl << endl;
//Add the proprioception
configV2
<< "[modality_0]" << endl
<< "name" << '\t' << "gaze" << endl
<< "type" << '\t' << "yarpVector" << endl
<< "size" << '\t' << 3 << endl << endl;
//Add the v1Retina
configV2
<< "[modality_1]" << endl
<< "name" << '\t' << "v1Retina" << endl
<< "type" << '\t' << "yarpVector" << endl
<< "size" << '\t' << TOPDOWN_SIZE << endl << endl;
configV2
<< "[modality_2]" << endl
<< "name" << '\t' << "v1Fovea" << endl
<< "type" << '\t' << "yarpVector" << endl
<< "size" << '\t' << TOPDOWN_SIZE << endl << endl;
Property propV2;
propV2.fromConfig(configV2.str().c_str());
stack.addCvzFromProperty(propV2,false, "v2");
//Add the head proprioception
stack.addCvzFromConfigFile(std::string("icub_head.ini"),false, "gaze");
//Connect the head to v2 -- I know this is non plausible
stack.connectModalities("/gaze/v1", "/v2/gaze");
stack.connectModalities("/v1Retina/out", "/v2/v1Retina");
stack.connectModalities("/v1Fovea/out", "/v2/v1Fovea");
//Make sure the graph is completed and all the connections are established
stack.connectModalities();
stack.configure(rf);
RpcServer* ticker = NULL;
StackTicker* tickerProcessor = NULL;
//Start the RPC wrapper for commands
bool isTickBased = rf.check("tickBased");
StackRpcWrapper* rpc = new StackRpcWrapper(&stack, retinaX, retinaY, foveaX, foveaY, isTickBased);
rpc->open("/visualSystem/rpc");
rpc->useCallback();
rpc->start();
if (isTickBased)
{
ticker = new RpcServer();
ticker->open("/visualSystem/ticker:i");
//Start the ticker to synchronize with the arrival of data
tickerProcessor = new StackTicker(rpc);
ticker->setReader(*tickerProcessor);
}
stack.runModule();
rpc->stop();
rpc->close();
delete rpc;
if (ticker != NULL)
{
ticker->close();
delete ticker;
delete tickerProcessor;
}
return 0;
}
