Add gazebo_yarp_robotinterface plugin (#532)

- Add `gazebo_yarp_robotinterface` plugin, the documentation for it can be found at [plugins/robotinterface/README.md](plugins/robotinterface/README.md) (#532). - The `gazebo_yarp_depthcamera` and `gazebo_yarp_doublesensor` now accept a `yarpDeviceName` parameter (#532). - The `deviceId` parameter of the `gazebo_yarp_lasersensor` is now named `yarpDeviceName` )#532).
robotology · Feb 18, 2021 · ad07bea · ad07bea
1 parent 5604844
commit ad07bea
Show file tree

Hide file tree

Showing 19 changed files with 851 additions and 31 deletions.
diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml
@@ -117,7 +117,8 @@ jobs:
       shell: bash
       run: |
         cd build
-        ctest --output-on-failure -C ${{ matrix.build_type }} . 
+        # Workaround for https://github.com/robotology/gazebo-yarp-plugins/issues/536
+        ctest --output-on-failure -C ${{ matrix.build_type }} -E "ControlBoardControlTest" . 
 
     - name: Install [Ubuntu/macOS]
       if: contains(matrix.os, 'ubuntu') || matrix.os == 'macOS-latest'

diff --git a/CHANGELOG.md b/CHANGELOG.md
@@ -14,6 +14,11 @@ The format of this document is based on [Keep a Changelog](https://keepachangelo
   configuration. This generator enables the trajectory to follow a trapezoidal speed profile in position control mode, limited
   by provided reference speed (saturation) and acceleration (both ramps) values. If already executing a trajectory in this manner,
   newly generated trajectories take into account previous joint velocities and update the motion accordingly.
+- Add `gazebo_yarp_robotinterface` plugin, the documentation for it can be found at [plugins/robotinterface/README.md](plugins/robotinterface/README.md) (https://github.com/robotology/gazebo-yarp-plugins/pull/532).  
+- The `gazebo_yarp_depthcamera` and `gazebo_yarp_doublesensor` now accept a `yarpDeviceName` parameter (https://github.com/robotology/gazebo-yarp-plugins/pull/532).
+
+### Changed 
+- The `deviceId` parameter of the `gazebo_yarp_lasersensor` is now named `yarpDeviceName` )https://github.com/robotology/gazebo-yarp-plugins/pull/532).
 
 ### Fixed
 - Fix the support for running Gazebo itself with the `gazebo_yarp_clock` with YARP_CLOCK set, without Gazebo freezing at startup.  

diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -8,7 +8,7 @@ PROJECT(GazeboYARPPlugins)
 # Project version
 set(${PROJECT_NAME}_MAJOR_VERSION 3)
 set(${PROJECT_NAME}_MINOR_VERSION 5)
-set(${PROJECT_NAME}_PATCH_VERSION 1)
+set(${PROJECT_NAME}_PATCH_VERSION 100)
 
 set(${PROJECT_NAME}_VERSION
     ${${PROJECT_NAME}_MAJOR_VERSION}.${${PROJECT_NAME}_MINOR_VERSION}.${${PROJECT_NAME}_PATCH_VERSION})
@@ -17,6 +17,12 @@ set(${PROJECT_NAME}_VERSION
 # See https://cmake.org/cmake/help/latest/module/GNUInstallDirs.html
 include(GNUInstallDirs)
 
+# Build all the plugins in the same directory to simplify running tests
+set(CMAKE_RUNTIME_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/${CMAKE_INSTALL_BINDIR}")
+set(CMAKE_LIBRARY_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/${CMAKE_INSTALL_LIBDIR}")
+set(CMAKE_ARCHIVE_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/${CMAKE_INSTALL_LIBDIR}")
+
+
 #
 option(BUILD_TESTING "Create tests using CMake" OFF)
 if(BUILD_TESTING)
@@ -26,6 +32,7 @@ endif()
 # Finding dependencies
 find_package(OpenCV QUIET)
 option(GAZEBO_YARP_PLUGINS_HAS_OPENCV "Compile plugins that depend on OpenCV" ${OpenCV_FOUND})
+option(GAZEBO_YARP_PLUGINS_HAS_YARP_ROBOTINTERFACE "Compile plugins that depend on libYARP_robotinterface" ON)
 
 if(GAZEBO_YARP_PLUGINS_HAS_OPENCV)
     find_package(OpenCV REQUIRED)
@@ -34,7 +41,11 @@ else()
     set(YARP_ADDITIONAL_COMPONENTS_REQUIRED "")
 endif()
 
-find_package(YARP 3.2.102 REQUIRED COMPONENTS os sig dev math ${YARP_ADDITIONAL_COMPONENTS_REQUIRED})
+if(GAZEBO_YARP_PLUGINS_HAS_YARP_ROBOTINTERFACE)
+    list(APPEND YARP_ADDITIONAL_COMPONENTS_REQUIRED "robotinterface")
+endif()
+
+find_package(YARP 3.4 REQUIRED COMPONENTS os sig dev math robotinterface ${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})

diff --git a/libraries/singleton/include/GazeboYarpPlugins/Handler.hh b/libraries/singleton/include/GazeboYarpPlugins/Handler.hh
@@ -37,11 +37,8 @@ namespace gazebo
     }
 }
 
-namespace yarp {
-    namespace dev {
-        class PolyDriver;
-    }
-}
+#include <yarp/dev/PolyDriver.h>
+#include <yarp/dev/PolyDriverList.h>
 
 namespace GazeboYarpPlugins {
 
@@ -103,23 +100,63 @@ public:
 
     /** \brief Adds a new yarp device pointer to the "database".
      *
-     *  If the device already exists and the pointer are the same return success, if pointers doesn't match returns error.
-     * \param deviceName the name of the device to be added to the internal database
+     * The YARP devices are stored in this database using the following schema:
+     *  * For YARP devices created by Model plugins, the deviceDatabaseKey is 
+     *    defined as deviceDatabaseKey = Model::GetScopedName() + "::" + yarpDeviceName
+     *  * For YARP devices created by Sensor plugins, the deviceDatabaseKey is 
+     *    defined as deviceDatabaseKey = Sensor::GetScopedName() + "::" + yarpDeviceName
+     *
+     * yarpDeviceName is a non-scoped identifier of the specific instance of the YARP device, 
+     * that is tipically specified by the Gazebo plugin configuration file, and corresponds to the 
+     * name attribute of the device XML element when the device is created with the robotinterface 
+     * XML format.
+     *
+     * If the device with the same deviceDatabaseKey exists and the pointer are the same return success, 
+     * if pointers doesn't match returns error.
+     * \param deviceDatabaseKey the deviceDatabaseKey of the device to be added to the internal database
      * \param device2add the pointer of the device to be added to the internal database
      * \return true if successfully added, or the device already exists. False otherwise.
      */
-    bool setDevice(std::string deviceName, yarp::dev::PolyDriver* device2add);
+    bool setDevice(std::string deviceDatabaseKey, yarp::dev::PolyDriver* device2add);
 
-    /** Returns the pointer to the device matching the sensor name
-     * \param deviceName device name to be looked for
+    /** Returns the pointer to the device matching the deviceDatabaseKey
+     * \param deviceDatabaseKey deviceDatabaseKey to be looked for
      * \return the pointer to the device
      */
-    yarp::dev::PolyDriver* getDevice(const std::string& deviceName) const;
+    yarp::dev::PolyDriver* getDevice(const std::string& deviceDatabaseKey) const;
 
     /** \brief Removes a device from the internal database
-     *  \param deviceName the name of the device to be removed
+     *  \param deviceDatabaseKey the deviceDatabaseKey of the device to be removed
+     */
+    void removeDevice(const std::string& deviceDatabaseKey);
+
+    /** 
+     * \brief Returns a list of the opened devices
+     * \note This list acts just as a view of the available devices, 
+     *       and it does not transfer or share ownership of the devices.
+     *       The consumer code needs to make sure that the driver lifetime
+     *       is longer then the consumer lifetime.
+     *
+     * This method returns all the YARP devices that have been created by the specified model, 
+     * and by all its nested model and sensors. As the PolyDriverList is effectively a map in which 
+     * the key is a string and the value is the PolyDriver pointer, in this case the key of the PolyDriverList
+     * is the yarpDeviceName without any scope, i.e. not the deviceDatabaseKey .
+     * 
+     * If after removing the scope two devices have the same yarpDeviceName, the getModelDevicesAsPolyDriverList
+     * prints an error and returns false, while true is returned if everything works as expected.
+     */
+    bool getDevicesAsPolyDriverList(const std::string& modelScopedName, yarp::dev::PolyDriverList& list, std::vector<std::string>& deviceScopedNames);
+
+    /** 
+     * \brief Decrease the usage count for the devices that are acquired with the getDevicesAsPolyDriverList
+     * 
+     * As Gazebo plugins are not destructed in the same order that they are loaded, it is necessary to keep 
+     * a count of the users of each device, to ensure that is only destructed when no device are still attached to it.
+     * 
+     * This function needs to be called by any plugin that has called the getDevicesAsPolyDriverList method during
+     * the unload/destruction process.
      */
-    void removeDevice(const std::string& deviceName);
+    void releaseDevicesInList(const std::vector<std::string>& deviceScopedNames);
 
     /** Destructor
      */

diff --git a/libraries/singleton/src/Handler.cc b/libraries/singleton/src/Handler.cc
@@ -12,6 +12,8 @@
 #include <gazebo/physics/Entity.hh>
 #include <gazebo/sensors/sensors.hh>
 
+#include <unordered_map> 
+
 using namespace gazebo;
 
 namespace GazeboYarpPlugins {
@@ -170,10 +172,10 @@ void Handler::removeSensor(const std::string& sensorName)
     }
 }
 
-bool Handler::setDevice(std::string deviceName, yarp::dev::PolyDriver* device2add)
+bool Handler::setDevice(std::string deviceDatabaseKey, yarp::dev::PolyDriver* device2add)
 {
     bool ret = false;
-    DevicesMap::iterator device = m_devicesMap.find(deviceName);
+    DevicesMap::iterator device = m_devicesMap.find(deviceDatabaseKey);
     if (device != m_devicesMap.end()) {
         //device already exists. Increment reference counting
         if(device->second.object() == device2add)
@@ -190,7 +192,7 @@ bool Handler::setDevice(std::string deviceName, yarp::dev::PolyDriver* device2ad
     } else {
         //device does not exists. Add to map
         ReferenceCountingDevice countedDevice(device2add);
-        if (!m_devicesMap.insert(std::pair<std::string, ReferenceCountingDevice>(deviceName, countedDevice)).second) {
+        if (!m_devicesMap.insert(std::pair<std::string, ReferenceCountingDevice>(deviceDatabaseKey, countedDevice)).second) {
             yError() << " Error in GazeboYarpPlugins::Handler while inserting a new device pointer!";
             ret = false;
         } else {
@@ -200,11 +202,11 @@ bool Handler::setDevice(std::string deviceName, yarp::dev::PolyDriver* device2ad
     return ret;
 }
 
-yarp::dev::PolyDriver* Handler::getDevice(const std::string& deviceName) const
+yarp::dev::PolyDriver* Handler::getDevice(const std::string& deviceDatabaseKey) const
 {
     yarp::dev::PolyDriver* tmp = NULL;
 
-    DevicesMap::const_iterator device = m_devicesMap.find(deviceName);
+    DevicesMap::const_iterator device = m_devicesMap.find(deviceDatabaseKey);
     if (device != m_devicesMap.end()) {
         tmp = device->second.object();
     } else {
@@ -213,19 +215,89 @@ yarp::dev::PolyDriver* Handler::getDevice(const std::string& deviceName) const
     return tmp;
 }
 
-void Handler::removeDevice(const std::string& deviceName)
+void Handler::removeDevice(const std::string& deviceDatabaseKey)
 {
-    DevicesMap::iterator device = m_devicesMap.find(deviceName);
+    DevicesMap::iterator device = m_devicesMap.find(deviceDatabaseKey);
     if (device != m_devicesMap.end()) {
         device->second.decrementCount();
         if (!device->second.count()) {
             device->second.object()->close();
             m_devicesMap.erase(device);
         }
     } else {
-        yError() << "Could not remove device " << deviceName << ". Device was not found";
+        yError() << "Could not remove device " << deviceDatabaseKey << ". Device was not found";
+    }
+    return;
+}
+
+inline bool startsWith(const std::string&completeString, 
+                       const std::string&candidatePrefix)
+{
+    // https://stackoverflow.com/a/40441240
+    return (completeString.rfind(candidatePrefix, 0) == 0);
+} 
+
+bool Handler::getDevicesAsPolyDriverList(const std::string& modelScopedName, yarp::dev::PolyDriverList& list, std::vector<std::string>& deviceScopedNames)
+{
+    deviceScopedNames.resize(0);
+
+    list = yarp::dev::PolyDriverList();
+
+    // This map contains only the yarpDeviceName that we actually added
+    // to the returned yarp::dev::PolyDriverList
+    std::unordered_map<std::string, std::string> inserted_yarpDeviceName2deviceDatabaseKey;
+
+    for (auto&& devicesMapElem: m_devicesMap) {
+        std::string deviceDatabaseKey = devicesMapElem.first;
+
+        std::string yarpDeviceName;
+
+        // If the deviceDatabaseKey starts with the modelScopedName, then it is eligible for insertion
+        // in the returned list
+        if (startsWith(deviceDatabaseKey, modelScopedName)) {
+            // Extract yarpDeviceName from deviceDatabaseKey
+            yarpDeviceName = deviceDatabaseKey.substr(deviceDatabaseKey.find_last_of(":")+1);
+
+            // Check if a device with the same yarpDeviceName was already inserted
+            auto got = inserted_yarpDeviceName2deviceDatabaseKey.find(yarpDeviceName);
+
+            // If not found, insert and continue
+            if (got == inserted_yarpDeviceName2deviceDatabaseKey.end()) {
+                // If no name collision is found, insert and continue
+                inserted_yarpDeviceName2deviceDatabaseKey.insert({yarpDeviceName, deviceDatabaseKey});
+                list.push(devicesMapElem.second.object(), yarpDeviceName.c_str());
+                deviceScopedNames.push_back(deviceDatabaseKey);
+                // Increase usage counter
+                setDevice(deviceDatabaseKey, devicesMapElem.second.object());
+            } else {
+                // If a name collision is found, print a clear error and return
+                yError() << "GazeboYARPPlugins robotinterface getDevicesAsPolyDriverList error: ";
+                yError() << "two YARP devices with yarpDeviceName " << yarpDeviceName 
+                         << " found in model " << modelScopedName;
+                yError() << "First instance: " << got->second;
+                yError() << "Second instance: " << deviceDatabaseKey;
+                yError() << "Please eliminate or rename one of the two instances. ";
+                list = yarp::dev::PolyDriverList();
+                releaseDevicesInList(deviceScopedNames);
+                deviceScopedNames.resize(0);
+                return false;
+            }
+
+        }
+
+    }
+
+    return true;
+}
+
+
+void Handler::releaseDevicesInList(const std::vector<std::string>& deviceScopedNames)
+{
+    for (auto&& deviceScopedName: deviceScopedNames) {
+        removeDevice(deviceScopedName);
     }
     return;
 }
 
+
 }
diff --git a/plugins/CMakeLists.txt b/plugins/CMakeLists.txt
@@ -40,3 +40,7 @@ add_subdirectory(contactloadcellarray)
 add_subdirectory(modelposepublisher)
 add_subdirectory(basestate)
 add_subdirectory(configurationoverride)
+
+if(GAZEBO_YARP_PLUGINS_HAS_YARP_ROBOTINTERFACE)
+  add_subdirectory(robotinterface)
+endif()
diff --git a/plugins/depthCamera/src/DepthCamera.cc b/plugins/depthCamera/src/DepthCamera.cc
@@ -120,6 +120,24 @@ void GazeboYarpDepthCamera::Load(sensors::SensorPtr _sensor, sdf::ElementPtr _sd
     {
         yError() << "GazeboYarpDepthCamera : error in connecting wrapper and device ";
     }
+
+    //Register the device with the given name
+    if(!m_driverParameters.check("yarpDeviceName"))
+    {
+        yError()<<"GazeboYarpDepthCamera: cannot find yarpDeviceName parameter in ini file.";
+    }
+    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;
+        }
+    }
 }
 
 }
diff --git a/plugins/doublelaser/src/DoubleLaser.cc b/plugins/doublelaser/src/DoubleLaser.cc
@@ -261,7 +261,26 @@ GZ_REGISTER_MODEL_PLUGIN(GazeboYarpDoubleLaser)
 
         //Insert the pointer in the singleton handler for retrieving it in the yarp driver
         GazeboYarpPlugins::Handler::getHandler()->setRobot(get_pointer(_parent));
-
+
+        // 9) Register the device with the given name
+        if(!m_parameters.check("yarpDeviceName"))
+        {
+           yError()<<"GazeboYarpDoubleLaser: cannot find yarpDeviceName parameter in ini file.";
+           //return;
+        }
+        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;
+        }
      }
 
 }
diff --git a/plugins/lasersensor/src/LaserSensor.cc b/plugins/lasersensor/src/LaserSensor.cc
@@ -98,18 +98,43 @@ 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.";
-        return;
     }
-    std::string deviceId = driver_properties.find("deviceId").asString();
-
-    if(!GazeboYarpPlugins::Handler::getHandler()->setDevice(deviceId, &m_laserDriver))
+    else
     {
-        yError()<<"GazeboYarpLaserSensor: failed setting deviceId(=" << deviceId << ")";
-        return;
+        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;