Add gazebo_yarp_robotinterface plugin

.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:/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'

CHANGELOG.md

@@ -14,6 +14,12 @@ 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:/robotology/gazebo-yarp-plugins/pull/532). 
+- The `gazebo_yarp_depthcamera` and `gazebo_yarp_doublesensor` now accept a `yarpDeviceName` parameter (https:/robotology/gazebo-yarp-plugins/pull/532).
+
+
+### Changed 
+- The `deviceId` parameter of the `gazebo_yarp_lasersensor` is now named `yarpDeviceName` )https:/robotology/gazebo-yarp-plugins/pull/532).
 
 ## [3.5.1] - 2020-10-05
 

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})

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 device are stored in these 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. the yarpDeviceName and 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 denstructed 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
  */

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;
 }
 
+
 }

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()

plugins/depthCamera/src/DepthCamera.cc

@@ -120,6 +120,25 @@ 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.";
+ //return;
+ }
+ 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;
+ }
+ }
 }
 
 }

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;
+ }
  }
 
 }