Handle yarpDeviceName in controlboard and multicamera plugins

CHANGELOG.md

@@ -6,11 +6,12 @@ The format of this document is based on [Keep a Changelog](https://keepachangelo
 ## [Unreleased]
 
 ### Added
-- The multicamera plugin now implements the `yarp::dev::IRgbVisualParams` interface.
+- The multicamera plugin now implements the `yarp::dev::IRgbVisualParams` interface (https://github.com/robotology/gazebo-yarp-plugins/pull/558).
+- The controlboard and multicamera plugins now handle the `yarpDeviceName` parameter (https://github.com/robotology/gazebo-yarp-plugins/pull/559).
 
 ### Fixed
-- Fixed the getRgbIntrinsicParam method in the depthCamera plugin when the distortion is not set.
-- The property returned by `getRgbIntrinsicParam()`, now contains `rectificationMatrix` instead of `rectificationMatrix` (See also https://github.com/robotology/yarp/pull/2593).
+- Fixed the getRgbIntrinsicParam method in the depthCamera plugin when the distortion is not set (https://github.com/robotology/gazebo-yarp-plugins/pull/558).
+- The property returned by `getRgbIntrinsicParam()`, now contains `rectificationMatrix` instead of `rectificationMatrix` (https://github.com/robotology/gazebo-yarp-plugins/pull/558, see also https://github.com/robotology/yarp/pull/2593).
 
 ## [3.6.1] - 2021-05-19
 

CMakeLists.txt

@@ -45,7 +45,7 @@ if(GAZEBO_YARP_PLUGINS_HAS_YARP_ROBOTINTERFACE)
     list(APPEND YARP_ADDITIONAL_COMPONENTS_REQUIRED "robotinterface")
 endif()
 
-find_package(YARP 3.4.101 REQUIRED COMPONENTS os sig dev math ${YARP_ADDITIONAL_COMPONENTS_REQUIRED})
+find_package(YARP 3.4.101 REQUIRED COMPONENTS os sig dev math idl_tools ${YARP_ADDITIONAL_COMPONENTS_REQUIRED})
 find_package(Gazebo REQUIRED)
 if (Gazebo_VERSION_MAJOR LESS 7.0)
     message(status "Gazebo version : " ${Gazebo_VERSION_MAJOR}.${Gazebo_VERSION_MINOR}.${Gazebo_VERSION_PATCH})

plugins/controlboard/src/ControlBoard.cc

@@ -159,7 +159,15 @@ GZ_REGISTER_MODEL_PLUGIN(GazeboYarpControlBoard)
                 virt_group.append(yarp::os::Bottle(net));                    
             }
 
-            std::string scopedDeviceName = m_robotName + "::" + newPoly.key.c_str();
+            std::string scopedDeviceName;
+            if(!m_parameters.check("yarpDeviceName"))
+            {
+                scopedDeviceName = m_robotName + "::" + newPoly.key.c_str();
+            }
+            else
+            {
+                scopedDeviceName = m_robotName + "::" + m_parameters.find("yarpDeviceName").asString();
+            }
             newPoly.poly = GazeboYarpPlugins::Handler::getHandler()->getDevice(scopedDeviceName);
             if( newPoly.poly != NULL)
             {
@@ -186,7 +194,7 @@ GZ_REGISTER_MODEL_PLUGIN(GazeboYarpControlBoard)
                     //yDebug()<<configuration_s;
                 }
 
-                 newPoly.poly = new yarp::dev::PolyDriver;
+                newPoly.poly = new yarp::dev::PolyDriver;
                 if(! newPoly.poly->open(m_parameters) || ! newPoly.poly->isValid())
                 {
                     yError() << "GazeboYarpControlBoard : controlBoard <" << newPoly.key << "> did not open.";
@@ -198,7 +206,15 @@ GZ_REGISTER_MODEL_PLUGIN(GazeboYarpControlBoard)
                     return;
                 }
             }
-            GazeboYarpPlugins::Handler::getHandler()->setDevice(scopedDeviceName, newPoly.poly);
+
+            //Register the device with the given name
+            if(!GazeboYarpPlugins::Handler::getHandler()->setDevice(scopedDeviceName, newPoly.poly))
+            {
+                yError() << "GazeboYarpControlBoard: failed setting scopedDeviceName(=" << scopedDeviceName << ")";
+                return;
+            }
+            yInfo() << "GazeboYarpControlBoard: Registered YARP device with instance name:" << scopedDeviceName;
+
             m_controlBoards.push(newPoly);
         }
 
@@ -241,5 +257,4 @@ GZ_REGISTER_MODEL_PLUGIN(GazeboYarpControlBoard)
             return;
         }
     }
-
-}
+} // namespace gazebo

plugins/depthCamera/src/DepthCamera.cc

@@ -114,30 +114,30 @@ void GazeboYarpDepthCamera::Load(sensors::SensorPtr _sensor, sdf::ElementPtr _sd
         return;
     }
 
-    driver_list.push(&m_cameraDriver,"dummy");
+    driver_list.push(&m_cameraDriver, "depthcamera");
 
     if(!m_iWrap->attachAll(driver_list) )
     {
         yError() << "GazeboYarpDepthCamera : error in connecting wrapper and device ";
     }
 
     //Register the device with the given name
+    std::string scopedDeviceName;
     if(!m_driverParameters.check("yarpDeviceName"))
     {
-        yError()<<"GazeboYarpDepthCamera: cannot find yarpDeviceName parameter in ini file.";
+        scopedDeviceName = m_sensorName + "::" + driver_list[0]->key;
     }
     else
     {
-        std::string sensorName = _sensor->ScopedName();
-        std::string deviceId = m_driverParameters.find("yarpDeviceName").asString();
-        std::string scopedDeviceName = sensorName + "::" + deviceId; 
-
-        if(!GazeboYarpPlugins::Handler::getHandler()->setDevice(scopedDeviceName, &m_cameraDriver))
-        {
-            yError()<<"GazeboYarpDepthCamera: failed setting scopedDeviceName(=" << scopedDeviceName << ")";
-            return;
-        }
+        scopedDeviceName = m_sensorName + "::" + m_driverParameters.find("yarpDeviceName").asString();
     }
-}
 
+    if(!GazeboYarpPlugins::Handler::getHandler()->setDevice(scopedDeviceName, &m_cameraDriver))
+    {
+        yError()<<"GazeboYarpDepthCamera: failed setting scopedDeviceName(=" << scopedDeviceName << ")";
+        return;
+    }
+    yInfo() << "Registered YARP device with instance name:" << scopedDeviceName;
 }
+
+} // namespace gazebo

plugins/doublelaser/src/DoubleLaser.cc

@@ -249,6 +249,7 @@ GZ_REGISTER_MODEL_PLUGIN(GazeboYarpDoubleLaser)
         yarp::dev::PolyDriverList listofdoubellaser; //it will contain only double laser
         yarp::dev::PolyDriverDescriptor doublelaser_desc;
         doublelaser_desc.poly = &m_driver_doublelaser;
+        doublelaser_desc.key = "doublelaser";
 
         listofdoubellaser.push(doublelaser_desc);
 
@@ -263,24 +264,22 @@ GZ_REGISTER_MODEL_PLUGIN(GazeboYarpDoubleLaser)
         GazeboYarpPlugins::Handler::getHandler()->setRobot(get_pointer(_parent));
 
         // 9) Register the device with the given name
+        std::string robotName = _parent->GetScopedName();
+        std::string scopedDeviceName;
         if(!m_parameters.check("yarpDeviceName"))
         {
-           yError()<<"GazeboYarpDoubleLaser: cannot find yarpDeviceName parameter in ini file.";
-           //return;
+            scopedDeviceName = robotName + "::" + doublelaser_desc.key;
         }
         else
         {
-            std::string robotName = _parent->GetScopedName();
-            std::string deviceId = m_parameters.find("yarpDeviceName").asString();
-            std::string scopedDeviceName = robotName + "::" + deviceId; 
-
-            if(!GazeboYarpPlugins::Handler::getHandler()->setDevice(scopedDeviceName, &m_driver_doublelaser))
-            {
-               yError()<<"GazeboYarpDoubleLaser: failed setting scopedDeviceName(=" << scopedDeviceName << ")";
-               return;
-            }
-            //yDebug() << "GazeboYarpDoubleLaser: register device:" << scopedDeviceName;
+            scopedDeviceName = robotName + "::" + m_parameters.find("yarpDeviceName").asString();
         }
-     }
 
-}
+        if(!GazeboYarpPlugins::Handler::getHandler()->setDevice(scopedDeviceName, &m_driver_doublelaser))
+        {
+            yError()<<"GazeboYarpDoubleLaser: failed setting scopedDeviceName(=" << scopedDeviceName << ")";
+            return;
+        }
+        yInfo() << "Registered YARP device with instance name:" << scopedDeviceName;
+    }
+} // namespace gazebo

plugins/lasersensor/src/LaserSensor.cc

@@ -98,44 +98,6 @@ void GazeboYarpLaserSensor::Load(sensors::SensorPtr _sensor, sdf::ElementPtr _sd
         return;
     }
 
-    //Register the device with the given name
-//#if 0
-    //this block will be soon deprecated
-    if(!driver_properties.check("deviceId"))
-    {
-        yError()<<"GazeboYarpLaserSensor Plugin failed: cannot find deviceId parameter in ini file.";
-    }
-    else
-    {
-        yError() << "GazeboYarpLaserSensor: deviceId parameter has been deprecated. Please use yarpDeviceName instead";
-        std::string deviceId = driver_properties.find("deviceId").asString();
-        if(!GazeboYarpPlugins::Handler::getHandler()->setDevice(deviceId, &m_laserDriver))
-        {
-           yError()<<"GazeboYarpLaserSensor: failed setting deviceId(=" << deviceId << ")";
-           return;
-        }
-   }
-//#else
-    if(!driver_properties.check("yarpDeviceName"))
-    {
-       yError()<<"GazeboYarpLaserSensor: cannot find yarpDeviceName parameter in ini file.";
-       //return;
-    }
-    else
-    {
-        std::string sensorName = _sensor->ScopedName();
-        std::string deviceId = driver_properties.find("yarpDeviceName").asString();
-        std::string scopedDeviceName = sensorName + "::" + deviceId; 
-
-        if(!GazeboYarpPlugins::Handler::getHandler()->setDevice(scopedDeviceName, &m_laserDriver))
-        {
-           yError()<<"GazeboYarpLaserSensor: failed setting scopedDeviceName(=" << scopedDeviceName << ")";
-           return;
-        }
-        //yDebug() << "GazeboYarpLaserSensor: registered device:" << scopedDeviceName;
-    }
-//#endif
-
     //Attach the driver to the wrapper
     ::yarp::dev::PolyDriverList driver_list;
 
@@ -145,15 +107,38 @@ void GazeboYarpLaserSensor::Load(sensors::SensorPtr _sensor, sdf::ElementPtr _sd
         return;
     }
 
-    driver_list.push(&m_laserDriver,"dummy");
+    driver_list.push(&m_laserDriver, "lasersensor");
 
     if( m_iWrap->attachAll(driver_list) ) {
     } else
     {
         yError() << "GazeboYarpLaserSensor : error in connecting wrapper and device " ;
     }
 
-}
+    //Register the device with the given name
+    std::string sensorName = _sensor->ScopedName();
+    std::string scopedDeviceName;
+    if(driver_properties.check("deviceId"))
+    {
+        yWarning() << "GazeboYarpLaserSensor: deviceId parameter has been deprecated. Please use yarpDeviceName instead";
+        scopedDeviceName = sensorName + "::" + driver_properties.find("deviceId").asString();
+    }
+    else if(!driver_properties.check("yarpDeviceName"))
+    {
+        scopedDeviceName = sensorName + "::" + driver_list[0]->key;
+    }
+    else
+    {
+        scopedDeviceName = sensorName + "::" + driver_properties.find("yarpDeviceName").asString();
+    }
+
 
+    if(!GazeboYarpPlugins::Handler::getHandler()->setDevice(scopedDeviceName, &m_laserDriver))
+    {
+        yError()<<"GazeboYarpLaserSensor: failed setting scopedDeviceName(=" << scopedDeviceName << ")";
+        return;
+    }
+    yInfo() << "Registered YARP device with instance name:" << scopedDeviceName;
 }
 
+} // namespace gazebo

plugins/multicamera/src/MultiCamera.cc

@@ -81,6 +81,25 @@ void GazeboYarpMultiCamera::Load(sensors::SensorPtr _sensor, sdf::ElementPtr _sd
         yError() << "Unable to get the iFrameGrabberImage interface from the device";
         return;
     }
+
+
+    //Register the device with the given name
+    std::string scopedDeviceName;
+    if(!m_parameters.check("yarpDeviceName"))
+    {
+        scopedDeviceName = m_sensorName + "::" "multicamera";
+    }
+    else
+    {
+        scopedDeviceName = m_sensorName + "::" + m_parameters.find("yarpDeviceName").asString();
+    }
+
+    if(!GazeboYarpPlugins::Handler::getHandler()->setDevice(scopedDeviceName, &m_cameraDriver))
+    {
+        yError()<<"GazeboYarpMultiCamera: failed setting scopedDeviceName(=" << scopedDeviceName << ")";
+        return;
+    }
+    yInfo() << "GazeboYarpMultiCamera: Register YARP device with instance name:" << scopedDeviceName;
 }
 
 } // namespace gazebo

plugins/robotinterface/README.md

@@ -42,12 +42,15 @@ The file specified in `yarpRobotInterfaceConfigurationFile` is a XML file specif
       <param name="string_param_example">this_is_a_string/param>
       <!-- To specify the existing YARP devices to which the newly spawned device needs to attach -->
       <action phase="startup" level="5" type="attach">
-        <paramlist name="networks">
           <!-- This should match the yarpDeviceName in the .ini file loaded in the Gazebo plugin. -->
-          <!-- The yarpDeviceName plays the same role of the name attribute of the device XML element 
+          <!-- The yarpDeviceName plays the same role of the name attribute of the device XML element
                for devices that are created via the robotinterface XML format. -->
-          <elem name="list_key_example"> yarp_device_name_of_device_to_which_to_attach </elem>
-        </paramlist>
+        <param name="device">yarp_device_name_of_device_to_which_to_attach</param>
+        <!-- Multiple devices can be attached by passing the "networks" list parameter instead, i.e.
+        <paramlist name="networks">
+          <elem name="list_key1_example"> yarp_device_name_of_first_device_to_which_to_attach </elem>
+          <elem name="list_key2_example"> yarp_device_name_of_second_device_to_which_to_attach </elem>
+        </paramlist>-->
       </action>
       <!-- If you added an attach action, always specify the detach action during the shutdown phase -->
       <action phase="shutdown" level="5" type="detach" />
@@ -59,8 +62,8 @@ The file specified in `yarpRobotInterfaceConfigurationFile` is a XML file specif
 
 ### How to specify existing YARP devices to which to attach
 
-The main use of the `gazebo_yarp_robotinterface` plugin is to spawn YARP devices that are **attached** to other devices that have been already spawned by other Gazebp plugins. For this reason, we need a way to specify to which target devices the devices created by robotinterface needs  to be attached.
-This is achieved by setting the elements in the `<paramlist name="networks">` list under the `<action phase="startup" level="5" type="attach">` XML element. 
+The main use of the `gazebo_yarp_robotinterface` plugin is to spawn YARP devices that are **attached** to other devices that have been already spawned by other Gazebo plugins. For this reason, we need a way to specify to which target devices the devices created by robotinterface needs to be attached.
+This is achieved by setting the elements in the `<param name="device">` or in the `<paramlist name="networks">` list under the `<action phase="startup" level="5" type="attach">` XML element.
 In this context, we call this "device instance identified" as **YARP device instance name**, as for devices created by the robotinterface, this is specified by the `name` attribute of the `device` XML tag. It is important not to confuse this with the **YARP device type name**, i.e. the name that identifies the type of plugin that is spawned, i.e. the `type` attribute of the `device` tag.
 
 The `gazebo_yarp_robotinterface` can be attached to any YARP device created by any plugin inside the model, or in any plugin  contained in any nested sensor or model. 
@@ -69,16 +72,14 @@ For historic reason, not all the `gazebo-yarp-plugins` support specifying the **
 
 **Warning: as the YARP device instance name is specified without any specific Gazebo model or sensor namespace, it is important to observe, while using the `gazebo_yarp_robotinterface` plugin, that all the YARP devices contained in the model have a unique YARP device instance name. If this is not the case, the plugin will print a clear error and exit without starting.**
 
-The plugins that spawn YARP devices in a way that they can be then attached to the yarprobotinterface as specified in the following table, together with the details with which the **YARP device instance name** can be specified:
-
-| 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). For this plugin, the **YARP device instance name** for each created device is specified with the `networks` parameter list in the plugin configuration. |
-| `gazebo_yarp_depthcamera` | This plugin can create a YARP device that expose a depth-camera interface. For this plugin, the **YARP device instance name** can be specified by the `yarpDeviceName` parameter in the plugin configuration. |
-| `gazebo_yarp_lasersensor` | This plugin can create a YARP device that expose a laser-seensor interface. For this plugin, the **YARP device instance name** can be specified by the `yarpDeviceName` parameter in the plugin configuration. |
-| `gazebo_yarp_doublelaser` | This plugin can create a YARP device network wrapper server that expose two existing laser sensors. For this plugin, the **YARP device instance name** can be specified by the `yarpDeviceName` parameter in the plugin configuration. |
-
-
-
-
-
+The plugins that spawn YARP devices in a way that they can be then attached to the yarprobotinterface as specified in the following table.
+For all the following plugins, the **YARP device instance name** can be specified by the `yarpDeviceName` parameter in the plugin configuration.
+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.
+
+| 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_doublelaser`  | This plugin can create a YARP device network wrapper server that expose two existing laser sensors. |
+| `gazebo_yarp_multicamera`  | This plugin can create a YARP device that expose a multicamera interface. |