Cleanup handling of yarpDeviceName in robotinterface plugin

traversaro · traversaro · commit c30b6abf5ba5 · 2021-02-09T19:26:49.000+01:00
diff --git a/libraries/singleton/include/GazeboYarpPlugins/Handler.hh b/libraries/singleton/include/GazeboYarpPlugins/Handler.hh
@@ -100,32 +100,52 @@ 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& deviceName);
+    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.
      */
-    void getDevicesAsPolyDriverList(yarp::dev::PolyDriverList& list);
+    bool getDevicesAsPolyDriverList(const std::string& modelScopedName, yarp::dev::PolyDriverList& list);
     
 
     /** 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,29 +215,76 @@ 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;
 }
 
-void Handler::getDevicesAsPolyDriverList(yarp::dev::PolyDriverList& list)
+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)
 {
-    for(auto&& devicesMapElem: m_devicesMap) {
-        yDebug() << "DEBUG TO REMOVE: Add device to " << devicesMapElem.first;
-        list.push(devicesMapElem.second.object(), devicesMapElem.first.c_str());
+    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) {
+        yDebug() << "DEBUG TO REMOVE: Add device deviceDatabaseKey " << devicesMapElem.first
+                 << " modelScopedName "  << modelScopedName;
+
+        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());
+                yDebug() << " add  yarpDeviceName " << yarpDeviceName;
+            } 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();
+                return false;
+            }
+
+        }
+
     }
 
-    return;
+    return true;
 }
 
 
diff --git a/plugins/robotinterface/src/GazeboYarpRobotInterface.cc b/plugins/robotinterface/src/GazeboYarpRobotInterface.cc
@@ -85,18 +85,22 @@ void GazeboYarpRobotInterface::Load(physics::ModelPtr _parentModel, sdf::Element
 
     // Extract externalDriverList of  devices from the one that have been already opened in the Gazebo model by other gazebo_yarp plugins 
     yarp::dev::PolyDriverList externalDriverList;
-    GazeboYarpPlugins::Handler::getHandler()->getDevicesAsPolyDriverList(externalDriverList);
+    GazeboYarpPlugins::Handler::getHandler()->getDevicesAsPolyDriverList(_parentModel->GetScopedName(), externalDriverList);
 
     // Set external devices from the one that have been already opened in the Gazebo model by other gazebo_yarp plugins 
     bool ok = m_xmlRobotInterfaceResult.robot.setExternalDevices(externalDriverList);
     if (!ok) {
         yError() << "GazeboYarpRobotInterface : impossible to set external devices";
+        return;
     }
 
     // Start robotinterface 
     ok = m_xmlRobotInterfaceResult.robot.enterPhase(yarp::robotinterface::experimental::ActionPhaseStartup);
     if (!ok) {
         yError() << "GazeboYarpRobotInterface : impossible to start robotinterface";
+        m_xmlRobotInterfaceResult.robot.enterPhase(yarp::robotinterface::experimental::ActionPhaseInterrupt1);
+        m_xmlRobotInterfaceResult.robot.enterPhase(yarp::robotinterface::experimental::ActionPhaseShutdown);
+        return;
     }
 }
 
diff --git a/tests/robotinterface/RobotInterfaceTest.xml b/tests/robotinterface/RobotInterfaceTest.xml
@@ -11,8 +11,10 @@
       <param name="joints"> 1                        </param>
       <action phase="startup" level="5" type="attach">
         <paramlist name="networks">
-          <!-- This should match the name of the device in the .ini file loaded in the Gazebo plugin, prefied by the Gazebo model name -->
-          <elem name="FirstSetOfJoints">  pendulum_with_base::pendulum_aaa </elem>
+          <!-- 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 
+               for devices that are created via the robotinterface XML format. -->
+          <elem name="FirstSetOfJoints">  pendulum_controlboard_ </elem>
         </paramlist>
       </action>
       <action phase="shutdown" level="5" type="detach" />
