bool updateModule()
{
double verg = 0;
Bottle bin;
Bottle bout;
inport.read(bin);
if (reset_offset)
{
initial_offset[0] = bin.get(0).asDouble();
initial_offset[1] = bin.get(1).asDouble();
initial_offset[2] = bin.get(2).asDouble();
initial_offset[3] = bin.get(3).asDouble();
initial_offset[4] = bin.get(4).asDouble();
initial_offset[5] = bin.get(5).asDouble();
reset_offset = false;
}
double x_torque = fabs(bin.get(3).asDouble()) - initial_offset [3];
double y_force = fabs(bin.get(1).asDouble()) - initial_offset[1];
bout.addInt(2);
double j_torque = x_torque + y_force * 0.70; //Nm + N * m
bout.addDouble(j_torque); //j0
bout.addDouble(j_torque); //j1
bout.addDouble(j_torque); //j2
bout.addDouble(j_torque); //j3
bout.addDouble(j_torque); //j4
bout.addDouble(j_torque); //j5
outport.write(bout);
return true;
}
/**
* 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;
}
virtual bool close()
{
rpcPort.interrupt();
rpcPort.close();
//gotoThread->shutdown();
gotoThread->stop();
delete gotoThread;
gotoThread=NULL;
return true;
}
virtual bool close()
{
mHandlerPort.interrupt();
mHandlerPort.close();
//mNavThread->shutdown();
mNavThread->stop();
delete mNavThread;
mNavThread=NULL;
return true;
}
virtual bool threadInit() override
{
p.open("/readWriteTest_writer");
image.resize(100, 100);
image.zero();
return true;
}
virtual bool configure(yarp::os::ResourceFinder &rf)
{
yarp::os::Time::turboBoost();
mNavThread=new Navigator(&rf);
if (!mNavThread->start())
{
delete mNavThread;
return false;
}
std::string local=rf.check("local",yarp::os::Value("/ikartnav")).asString().c_str();
mHandlerPort.open(local.c_str());
attach(mHandlerPort);
//attachTerminal();
return true;
}
bool configure( yarp::os::ResourceFinder &rf )
{
std::string moduleName = rf.check("name",
yarp::os::Value("demoServerModule"),
"module name (string)").asString().c_str();
setName(moduleName.c_str());
std::string slash="/";
attach(cmdPort);
std::string cmdPortName= "/";
cmdPortName+= getName();
cmdPortName += "/cmd";
if (!cmdPort.open(cmdPortName.c_str())) {
std::cout << getName() << ": Unable to open port " << cmdPortName << std::endl;
return false;
}
return true;
}
virtual bool configure(yarp::os::ResourceFinder &rf)
{
yarp::os::Time::turboBoost();
Property p;
ConstString configFile = rf.findFile("from");
if (configFile!="") p.fromConfigFile(configFile.c_str());
gotoThread = new GotoThread(10,rf,p);
if (!gotoThread->start())
{
delete gotoThread;
return false;
}
rpcPort.open("/ikartGoto/rpc:i");
attach(rpcPort);
//attachTerminal();
return true;
}
/**
* Interrupts ports operations
* @return this method cannot fail, so it always returns true
*/
virtual bool interruptModule()
{
m_rpcPort.interrupt();
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 close()
{
cmdPort.close();
return true;
}
bool attach(yarp::os::Port& source)
{
attachedToPort=true;
source.setReader(*this);
return true;
}
virtual void run() override
{
p.write(image);
yarp::os::Time::delay(1);
}
virtual void threadRelease() override
{
p.close();
}
virtual bool close()
{
inport.close();
outport.close();
return true;
}
virtual bool configure(yarp::os::ResourceFinder &rf)
{
Property options;
options.fromString(rf.toString());
char robotName[255];
Bottle *jointsList=0;
std::string moduleName = "torqueObserver";
Time::turboBoost();
reset_offset = true;
for (int i=0; i<6; i++) initial_offset[i]=0.0;
/* options.put("device", "remote_controlboard");
if(options.check("robot"))
strncpy(robotName, options.find("robot").asString().c_str(),sizeof(robotName));
else
strncpy(robotName, "icub", sizeof(robotName));
if (options.check("name"))
{
moduleName = options.find("name").asString();
}
if(options.check("part"))
{
sprintf(partName, "%s", options.find("part").asString().c_str());
sprintf(tmp, "/%s/%s/%s/client", moduleName.c_str(), robotName, partName);
options.put("local",tmp);
sprintf(tmp, "/%s/%s", robotName, partName);
options.put("remote", tmp);
sprintf(tmp, "/%s/%s/rpc", moduleName.c_str(), partName);
rpc_port.open(tmp);
options.put("carrier", "tcp");
attach(rpc_port);
}
else
{
yError("Please specify part (e.g. --part head)\n");
return false;
}
//opening the device driver
if (!driver.open(options))
{
yError("Error opening device, check parameters\n");
return false;
}
bool ret = true;
idir = 0;
icmd = 0;
ret &= driver.view(idir);
ret &= driver.view(icmd);
*/
char pin[255];
sprintf(pin, "/%s/torque:i", moduleName.c_str());
inport.open(pin);
char pout[255];
sprintf(pout, "/%s/torque:o", moduleName.c_str());
outport.open(pout);
bool b1 = yarp::os::Network::connect("/icub/left_leg/analog:o",pin);
bool b2 = yarp::os::Network::connect(pout,"/icub/left_leg/analog:o");
if (!b1)
{
yWarning("failed input connection");
}
if (!b2)
{
yWarning("failed ouput connection");
}
return true;
}
virtual bool interruptModule()
{
mHandlerPort.interrupt();
return true;
}
