/**
* Terminates module execution and performs cleanup
* @return this method cannot fail, so it always returns true
*/
virtual bool close()
{
if (m_ptf.isValid())
{
m_ptf.close();
}
if (m_pmap.isValid())
{
m_pmap.close();
}
if (m_rosPublisher_occupancyGrid.asPort().isOpen())
{
m_rosPublisher_occupancyGrid.interrupt();
m_rosPublisher_occupancyGrid.close();
}
if (m_rosPublisher_initial_pose.asPort().isOpen())
{
m_rosPublisher_initial_pose.interrupt();
m_rosPublisher_initial_pose.close();
}
if (m_rosNode)
{
delete m_rosNode;
m_rosNode = 0;
}
m_rpcPort.interrupt();
m_rpcPort.close();
return true;
}
bool configure(ResourceFinder &rf)
{
printf("Configuring...\n");
name = rf.check("name", Value("objectFinder"), "Getting module name").asString();
setName(name.c_str());
camera = rf.check("camera", Value("left"), "Selecting the camera").asString().c_str();
robot = rf.check("robot", Value("icub"), "Choosing a robot").asString().c_str();
tableHeight = rf.check("tableHeight", Value(-0.10)).asDouble(); // Height of the table in the robots coord frame
// Open ports
printf("Opening ports after prefix \n" );
bool ret= true;
ret = rpcPort.open(("/"+name+"/rpc:i").c_str()); //rpc client to interact with the objectFinder
ret = ret && imInPort.open(("/"+name+"/img:i").c_str()); // port to receive images
ret = ret && coordsInPort.open(("/"+name+"/coords:i").c_str()); // port to receive yarpview coordinates
ret = ret && trackInPort.open(("/"+name+"/track:i").c_str()); // port to receive tracker coordinates
ret = ret && coordsOutPort.open(("/"+name+"/coords:o").c_str()); // port to send object coordinates
ret = ret && imOutPort.open(("/"+name+"/imgOut:o").c_str()); // port to send croped image for template
ret = ret && tempOutPort.open(("/"+name+"/crop:o").c_str()); // port to send croped image for template
if (!ret){
printf("Problems opening ports\n");
return false;
}
printf("Ports opened\n");
// Attach rpcPort to the respond() method
attach(rpcPort);
// Gaze Controller Interface
Property optGaze("(device gazecontrollerclient)");
optGaze.put("remote","/iKinGazeCtrl");
optGaze.put("local","/"+name+"/gaze_client");
if (!clientGaze.open(optGaze))
return false;
clientGaze.view(igaze);
running = true;
return true;
}
bool configure(ResourceFinder &rf)
{
int verbosity=rf.check("verbosity",Value(0)).asInt();
string remote=rf.check("remote",Value("d4c_server")).asString().c_str();
string local=rf.check("local",Value("d4c_client")).asString().c_str();
string robot=rf.check("robot",Value("icub")).asString().c_str();
string part=rf.check("part",Value("left_arm")).asString().c_str();
arm=(part=="left_arm"?"left":"right");
Property options;
options.put("verbosity",verbosity);
options.put("remote",("/"+remote).c_str());
options.put("local",("/"+local).c_str());
init=true;
closing=false;
Property optCtrl;
optCtrl.put("device","cartesiancontrollerclient");
optCtrl.put("remote",("/"+robot+"/cartesianController/"+part).c_str());
optCtrl.put("local",("/"+local+"/cartesian").c_str());
if (dCtrl.open(optCtrl))
dCtrl.view(iCtrl);
else
return false;
iCtrl->storeContext(&store_context_id);
Vector dof;
iCtrl->getDOF(dof);
Vector newDof=dof;
newDof[0]=1.0;
newDof[2]=1.0;
iCtrl->setDOF(newDof,dof);
iCtrl->setLimits(7,-70.0,70.0);
return client.open(options);
}
bool close()
{
if (!closing)
{
client.disableControl();
client.disableField();
iCtrl->restoreContext(store_context_id);
dCtrl.close();
client.clearItems();
}
client.close();
return true;
}
/**
* Performs module configuration, parsing user options stored in the resource finder.
* Available options are described in main module documentation.
* @return true if the module was successfully configured and opened, false otherwise.
*/
virtual bool configure(yarp::os::ResourceFinder &rf)
{
yarp::os::Time::turboBoost();
m_rpcPort.open("/"+m_module_name+"/rpc");
attach(m_rpcPort);
//attachTerminal();
m_rf = rf;
//configuration file checking
Bottle general_group = m_rf.findGroup("GENERAL");
if (general_group.isNull())
{
yError() << "Missing GENERAL group!";
return false;
}
Bottle initial_group = m_rf.findGroup("INITIAL_POS");
if (initial_group.isNull())
{
yError() << "Missing INITIAL_POS group!";
return false;
}
Bottle localization_group = m_rf.findGroup("LOCALIZATION");
if (localization_group.isNull())
{
yError() << "Missing LOCALIZATION group!";
return false;
}
Bottle ros_group = m_rf.findGroup("ROS");
Bottle tf_group = m_rf.findGroup("TF");
if (tf_group.isNull())
{
yError() << "Missing TF group!";
return false;
}
Bottle odometry_group = m_rf.findGroup("ODOMETRY");
if (odometry_group.isNull())
{
yError() << "Missing ODOMETRY group!";
return false;
}
Bottle map_group = m_rf.findGroup("MAP");
if (map_group.isNull())
{
yError() << "Missing MAP group!";
return false;
}
yDebug() << map_group.toString();
//general group
if (general_group.check("module_name") == false)
{
yError() << "Missing `module_name` in [GENERAL] group";
return false;
}
m_module_name = general_group.find("module_name").asString();
if (general_group.check("enable_ros") == false)
{
yError() << "Missing `ros_enable` in [GENERAL] group";
return false;
}
m_ros_enabled = (general_group.find("enable_ros").asInt()==1);
//ROS group
if (m_ros_enabled)
{
m_rosNode = new yarp::os::Node("/"+m_module_name);
//initialize an occupancy grid publisher (every time the localization is re-initialized, the map is published too)
if (ros_group.check ("occupancygrid_topic"))
{
m_topic_occupancyGrid = ros_group.find ("occupancygrid_topic").asString();
if (!m_rosPublisher_occupancyGrid.topic(m_topic_occupancyGrid))
{
if (m_rosNode)
{
delete m_rosNode;
m_rosNode=0;
}
yError() << "localizationModule: unable to publish data on " << m_topic_occupancyGrid << " topic, check your yarp-ROS network configuration";
return false;
}
}
//initialize an initial pose publisher
if (ros_group.check ("initialpose_topic"))
{
m_topic_initial_pose = ros_group.find ("initialpose_topic").asString();
{
if (!m_rosPublisher_initial_pose.topic(m_topic_initial_pose))
{
if (m_rosNode)
{
delete m_rosNode;
m_rosNode=0;
}
yError() << "localizationModule: unable to publish data on " << m_topic_initial_pose << " topic, check your yarp-ROS network configuration";
return false;
}
}
}
}
//localization group
if (localization_group.check("use_localization_from_odometry_port")) { m_use_localization_from_odometry_port = (localization_group.find("use_localization_from_odometry_port").asInt() == 1); }
if (localization_group.check("use_localization_from_tf")) { m_use_localization_from_tf = (localization_group.find("use_localization_from_tf").asInt() == 1); }
if (m_use_localization_from_odometry_port == true && m_use_localization_from_tf == true)
{
yError() << "`use_localization_from_tf` and `use_localization_from_odometry_port` cannot be true simultaneously!";
return false;
}
//map server group
yDebug() << map_group.toString();
if (map_group.check("connect_to_yarp_mapserver") == false)
{
yError() << "Missing `connect_to_yarp_mapserver` in [MAP] group";
return false;
}
m_use_map_server= (map_group.find("connect_to_yarp_mapserver").asInt()==1);
//tf group
if (tf_group.check("map_frame_id") == false)
{
yError() << "Missing `map_frame_id` in [TF] group";
return false;
}
if (tf_group.check("robot_frame_id") == false)
{
yError() << "Missing `robot_frame_id` in [TF] group";
return false;
}
m_frame_map_id = tf_group.find("map_frame_id").asString();
m_frame_robot_id = tf_group.find("robot_frame_id").asString();
//odometry group
if (odometry_group.check("odometry_broadcast_port") == false)
{
yError() << "Missing `odometry_port` in [ODOMETRY] group";
return false;
}
m_port_broadcast_odometry_name = odometry_group.find("odometry_broadcast_port").asString();
//device driver opening and/or connections
if (m_use_localization_from_odometry_port)
{
//opens a YARP port to receive odometry data
std::string odom_portname = "/" + m_module_name + "/odometry:i";
bool b1 = m_port_odometry_input.open(odom_portname.c_str());
bool b2 = yarp::os::Network::sync(odom_portname.c_str(),false);
bool b3 = yarp::os::Network::connect(m_port_broadcast_odometry_name.c_str(), odom_portname.c_str());
if (b1 == false || b2 ==false || b3==false)
{
yError() << "Unable to initialize odometry port connection from " << m_port_broadcast_odometry_name.c_str()<< "to:" << odom_portname.c_str();
return false;
}
}
if (m_use_localization_from_tf)
{
//opens a client to receive localization data from transformServer
Property options;
options.put("device", "transformClient");
options.put("local", "/"+m_module_name + "/TfClient");
options.put("remote", "/transformServer");
if (m_ptf.open(options) == false)
{
yError() << "Unable to open transform client";
return false;
}
m_ptf.view(m_iTf);
if (m_ptf.isValid() == false || m_iTf == 0)
{
yError() << "Unable to view iTransform interface";
return false;
}
}
if (m_use_map_server)
{
//opens a client to send/received data from mapServer
Property map_options;
map_options.put("device", "map2DClient");
map_options.put("local", "/" +m_module_name); //This is just a prefix. map2DClient will complete the port name.
map_options.put("remote", "/mapServer");
if (m_pmap.open(map_options) == false)
{
yWarning() << "Unable to open mapClient";
}
else
{
yInfo() << "Opened mapClient";
m_pmap.view(m_iMap);
if (m_pmap.isValid() == false || m_iMap == 0)
{
yError() << "Unable to view map interface";
return false;
}
}
}
//initial location initialization
if (initial_group.check("initial_x")) { m_initial_loc.x = initial_group.find("initial_x").asDouble(); }
else { yError() << "missing initial_x param"; return false; }
if (initial_group.check("initial_y")) { m_initial_loc.y = initial_group.find("initial_y").asDouble(); }
else { yError() << "missing initial_y param"; return false; }
if (initial_group.check("initial_theta")) { m_initial_loc.theta = initial_group.find("initial_theta").asDouble(); }
else { yError() << "missing initial_theta param"; return false; }
if (initial_group.check("initial_map")) { m_initial_loc.map_id = initial_group.find("initial_map").asString(); }
else { yError() << "missing initial_map param"; return false; }
this->initializeLocalization(m_initial_loc);
return true;
}
bool updateModule()
{
if (init)
{
Vector x(3),o;
x[0]=-0.2;
x[1]=(arm=="left"?-0.05:0.05);
x[2]=0.1;
Matrix R(3,3); R=0.0;
R(0,0)=-1.0; R(2,1)=-1.0; R(1,2)=-1.0;
o0=dcm2axis(R);
iCtrl->goToPoseSync(x,o0,2.0);
iCtrl->waitMotionDone();
iCtrl->getPose(x,o);
iCtrl->setTrajTime(1.0);
iCtrl->setInTargetTol(1e-3);
xTg=x;
xTg[0]-=0.2;
dist=norm(xTg-x);
Value centerTg; centerTg.fromString(("("+string(xTg.toString().c_str())+")").c_str());
Value radiusTg; radiusTg.fromString("(0.01 0.01 0.01)");
Property targetOpt;
targetOpt.put("type","target_msd");
targetOpt.put("active","on");
targetOpt.put("K",2.0);
targetOpt.put("D",2.5);
targetOpt.put("name","target");
targetOpt.put("center",centerTg);
targetOpt.put("radius",radiusTg);
client.addItem(targetOpt,target);
Vector xOb=x;
xOb[0]=(x[0]+xTg[0])/2.0;
xOb[1]+=(arm=="left"?0.05:-0.05);
Value centerOb; centerOb.fromString(("("+string(xOb.toString().c_str())+")").c_str());
Value radiusOb; radiusOb.fromString("(0.1 0.1 0.1)");
Property obstacleOpt;
obstacleOpt.put("type","obstacle_gaussian");
obstacleOpt.put("active","on");
obstacleOpt.put("G",5.0);
obstacleOpt.put("name","obstacle");
obstacleOpt.put("center",centerOb);
obstacleOpt.put("radius",radiusOb);
obstacleOpt.put("cut_tails","on");
client.addItem(obstacleOpt,obstacle);
client.setActiveIF(arm);
client.setPointStateToTool();
client.enableControl();
client.enableField();
init=false;
return true;
}
else
{
Vector x,o,xdot,odot;
client.getPointState(x,o,xdot,odot);
double d1=norm(xTg-x);
Vector xee,oee;
iCtrl->getPose(xee,oee);
double d2=norm(x-xee);
Vector zero(4); zero=0.0;
Vector o1=zero;
o1[0]=(arm=="left"?-1.0:1.0); o1[3]=M_PI/2.0*(1.0-d1/dist);
o=dcm2axis(axis2dcm(o1)*axis2dcm(o0));
client.setPointOrientation(o,zero);
fprintf(stdout,"|xTg-x|=%g [m], |x-xee|=%g[m]\n",d1,d2);
if ((d1<2e-3) && (d2<2e-3))
{
client.disableControl();
client.disableField();
fprintf(stdout,"job accomplished\n");
iCtrl->restoreContext(store_context_id);
dCtrl.close();
client.clearItems();
closing=true;
return false;
}
else
return true;
}
}
