Merge pull request #584 from GrmanRodriguez/feat/portForceTorqueRobot…

…Interface

Port gazebo_forcetorque for being used with the gazebo_robotinterface plugin

CHANGELOG.md

@@ -6,6 +6,8 @@ The format of this document is based on [Keep a Changelog](https://keepachangelo
 ## [Unreleased]
 
 ### Added
+- The `gazebo_yarp_forcetorque` plugin now handle the `yarpDeviceName` parameter.
+- The `gazebo_yarp_forcetorque` plugin can be now opened without implicit/explicit wrapper.
 - The `gazebo_imu` plugin now handle the `yarpDeviceName` parameter.
 - The `gazebo_imu` plugin can be now opened without implicit/explicit wrapper.
 

plugins/forcetorque/src/ForceTorque.cc

@@ -52,52 +52,55 @@ void GazeboYarpForceTorque::Load(sensors::SensorPtr _sensor, sdf::ElementPtr _sd
 
     _sensor->SetActive(true);
 
+    std::string netWrapper {""};
+
+    #ifndef GAZEBO_YARP_PLUGINS_DISABLE_IMPLICIT_NETWORK_WRAPPERS
+    netWrapper = "analogServer";
+    #endif // GAZEBO_YARP_PLUGINS_DISABLE_IMPLICIT_NETWORK_WRAPPERS
+
     // Add my gazebo device driver to the factory.
     ::yarp::dev::Drivers::factory().add(new ::yarp::dev::DriverCreatorOf< ::yarp::dev::GazeboYarpForceTorqueDriver>
-                                      ("gazebo_forcetorque", "analogServer", "GazeboYarpForceTorqueDriver"));
-
-    //Getting .ini configuration file from sdf
-    ::yarp::os::Property wrapper_properties;
+                                      ("gazebo_forcetorque", netWrapper.c_str(), "GazeboYarpForceTorqueDriver"));
+
     ::yarp::os::Property driver_properties;
     bool configuration_loaded = GazeboYarpPlugins::loadConfigSensorPlugin(_sensor,_sdf,driver_properties);
 
     if (!configuration_loaded) {
         return;
     };
 
-
+    #ifndef GAZEBO_YARP_PLUGINS_DISABLE_IMPLICIT_NETWORK_WRAPPERS
     ///< \todo TODO handle in a better way the parameters that are for the wrapper and the one that are for driver
-    wrapper_properties = driver_properties;
-
-    if( !configuration_loaded )
-    {
-        return;
-    }
+    ::yarp::os::Property wrapper_properties = driver_properties;
+    #endif // GAZEBO_YARP_PLUGINS_DISABLE_IMPLICIT_NETWORK_WRAPPERS
+
     m_sensorName = _sensor->ScopedName();
 
     //Insert the pointer in the singleton handler for retriving it in the yarp driver
     GazeboYarpPlugins::Handler::getHandler()->setSensor(_sensor.get());
 
     driver_properties.put(YarpForceTorqueScopedName.c_str(), m_sensorName.c_str());
 
+    #ifndef GAZEBO_YARP_PLUGINS_DISABLE_IMPLICIT_NETWORK_WRAPPERS
     //Open the wrapper
     //Force the wrapper to be of type "analogServer" (it make sense? probably no)
     wrapper_properties.put("device","analogServer");
-    if( m_forcetorqueWrapper.open(wrapper_properties) ) {
-    } else {
+    if( !m_forcetorqueWrapper.open(wrapper_properties) ) {
         yError()<<"GazeboYarpForceTorque Plugin failed: error in opening yarp driver wrapper";
         return;
     }
+    #endif // GAZEBO_YARP_PLUGINS_DISABLE_IMPLICIT_NETWORK_WRAPPERS
 
     //Open the driver
     //Force the device to be of type "gazebo_forcetorque" (it make sense? probably yes)
     driver_properties.put("device","gazebo_forcetorque");
-    if( m_forceTorqueDriver.open(driver_properties) ) {
-    } else {
+    if( !m_forceTorqueDriver.open(driver_properties) ) {
         yError()<<"GazeboYarpForceTorque Plugin failed: error in opening yarp driver";
         return;
     }
-
+
+    std::string scopedDeviceName;
+    #ifndef GAZEBO_YARP_PLUGINS_DISABLE_IMPLICIT_NETWORK_WRAPPERS
     //Attach the driver to the wrapper
     ::yarp::dev::PolyDriverList driver_list;
 
@@ -108,11 +111,33 @@ void GazeboYarpForceTorque::Load(sensors::SensorPtr _sensor, sdf::ElementPtr _sd
 
     driver_list.push(&m_forceTorqueDriver,"dummy");
 
-    if( m_iWrap->attachAll(driver_list) ) {
-    } else {
+    if( !m_iWrap->attachAll(driver_list) ) {
         yError() << "GazeboYarpForceTorque : error in connecting wrapper and device ";
     }
 
+    if(!driver_properties.check("yarpDeviceName"))
+    {
+        scopedDeviceName = m_sensorName + "::" + driverList[0]->key;
+    }
+    else
+    {
+        scopedDeviceName = m_sensorName + "::" + driver_properties.find("yarpDeviceName").asString();
+    }
+    #else
+    if(!driver_properties.check("yarpDeviceName"))
+    {
+        yError() << "GazeboYarpForceTorque : missing yarpDeviceName parameter for device" << m_sensorName;
+        return;
+    }
+    scopedDeviceName = m_sensorName + "::" + driver_properties.find("yarpDeviceName").asString();
+    #endif // GAZEBO_YARP_PLUGINS_DISABLE_IMPLICIT_NETWORK_WRAPPERS
+
+    if(!GazeboYarpPlugins::Handler::getHandler()->setDevice(scopedDeviceName, &m_forceTorqueDriver))
+    {
+        yError()<<"GazeboYarpForceTorque: failed setting scopedDeviceName(=" << scopedDeviceName << ")";
+        return;
+    }
+    yInfo() << "Registered YARP device with instance name:" << scopedDeviceName;
 }
 
 }

plugins/robotinterface/README.md

@@ -77,12 +77,13 @@ For all the following plugins, the **YARP device instance name** can be specifie
 
 If the `GAZEBO_YARP_PLUGINS_DISABLE_IMPLICIT_NETWORK_WRAPPERS` option is set to `OFF` (default value), for the `gazebo_yarp_controlboard` if the `yarpDeviceName` parameter is not specified, for legacy reason the **YARP device instance name** for each created device can also be specified with the `networks` parameter list in the plugin configuration of `yarpDeviceName`. If instead the `GAZEBO_YARP_PLUGINS_DISABLE_IMPLICIT_NETWORK_WRAPPERS` option is set to `ON`, the `gazebo_yarp_controlboard` behaves like the rest of the plugins and requires to take the **YARP device instance name** from the `yarpDeviceName` parameter. The same behaviour applies to the plugins listed in the table below.
 
-Furthermore if the `GAZEBO_YARP_PLUGINS_DISABLE_IMPLICIT_NETWORK_WRAPPERS` option is set to `ON` then the plugins `gazebo_yarp_multicamera`, `gazebo_yarp_lasersensor`, `gazebo_yarp_controlboard`, `gazebo_yarp_depthCamera` and `gazebo_yarp_imu` doesn't create the implicit wrapper device but is left to the user to attach it via the robotinterface.
+Furthermore if the `GAZEBO_YARP_PLUGINS_DISABLE_IMPLICIT_NETWORK_WRAPPERS` option is set to `ON` then the plugins `gazebo_yarp_multicamera`, `gazebo_yarp_lasersensor`, `gazebo_yarp_controlboard`, `gazebo_yarp_depthCamera`, `gazebo_yarp_forcetorque` and `gazebo_yarp_imu` doesn't create the implicit wrapper device but is left to the user to attach it via the robotinterface.
 
 | Plugin                     | Details                                              |
 |:--------------------------:|:----------------------------------------------------:|
 | `gazebo_yarp_controlboard` | This plugin can create multiple YARP devices that expose joint-level motor and control interfaces such as [`yarp::dev::IPositionControl`](https://www.yarp.it/git-master/classyarp_1_1dev_1_1IPositionControl.html), [`yarp::dev::ITorqueControl`](https://www.yarp.it/git-master/classyarp_1_1dev_1_1ITorqueControl.html) and [`yarp::dev::ITorqueControl`](https://www.yarp.it/git-master/classyarp_1_1dev_1_1IEncoders.html). |
-| `gazebo_yarp_depthcamera`  | This plugin can create a YARP device that expose a depth-camera interface. |
-| `gazebo_yarp_lasersensor`  | This plugin can create a YARP device that expose a laser-seensor interface. |
-| `gazebo_yarp_multicamera`  | This plugin can create a YARP device that expose a multicamera interface. |
-| `gazebo_yarp_imu`  | This plugin can create a YARP device that expose a imu interface. |
+| `gazebo_yarp_depthcamera`  | This plugin can create a YARP device that exposes a depth-camera interface. |
+| `gazebo_yarp_lasersensor`  | This plugin can create a YARP device that exposes a laser-seensor interface. |
+| `gazebo_yarp_multicamera`  | This plugin can create a YARP device that exposes a multicamera interface. |
+| `gazebo_yarp_imu`  | This plugin can create a YARP device that exposes a imu interface. |
+| `gazebo_yarp_forcetorque`  | This plugin can create a YARP device that exposes a force-torque interface. |