Fix early destruction of device

Author: traversaro

libraries/singleton/include/GazeboYarpPlugins/Handler.hh

@@ -145,8 +145,18 @@ public:
      * 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);
+    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 releaseDevicesInList(const std::vector<std::string>& deviceScopedNames);
 
     /** Destructor
      */

libraries/singleton/src/Handler.cc

@@ -237,18 +237,17 @@ inline bool startsWith(const std::string&completeString,
     return (completeString.rfind(candidatePrefix, 0) == 0);
 } 
 
-bool Handler::getDevicesAsPolyDriverList(const std::string& modelScopedName, yarp::dev::PolyDriverList& list)
+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) {
-        yDebug() << "DEBUG TO REMOVE: Add device deviceDatabaseKey " << devicesMapElem.first
-                 << " modelScopedName "  << modelScopedName;
-
         std::string deviceDatabaseKey = devicesMapElem.first;
 
         std::string yarpDeviceName;
@@ -267,7 +266,9 @@ bool Handler::getDevicesAsPolyDriverList(const std::string& modelScopedName, yar
                 // 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;
+                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: ";
@@ -277,6 +278,8 @@ bool Handler::getDevicesAsPolyDriverList(const std::string& modelScopedName, yar
                 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;
             }
 
@@ -288,4 +291,13 @@ bool Handler::getDevicesAsPolyDriverList(const std::string& modelScopedName, yar
 }
 
 
+void Handler::releaseDevicesInList(const std::vector<std::string>& deviceScopedNames)
+{
+    for (auto&& deviceScopedName: deviceScopedNames) {
+        removeDevice(deviceScopedName);
+    }
+    return;
+}
+
+
 }

plugins/robotinterface/include/gazebo/GazeboYarpRobotInterface.hh

@@ -31,6 +31,7 @@ public:
 private:
     yarp::robotinterface::experimental::XMLReader m_xmlRobotInterfaceReader;
     yarp::robotinterface::experimental::XMLReaderResult m_xmlRobotInterfaceResult;
+    std::vector<std::string> m_deviceScopedNames;
 };
 
 

plugins/robotinterface/src/GazeboYarpRobotInterface.cc

@@ -18,7 +18,6 @@ namespace gazebo
 
 GazeboYarpRobotInterface::GazeboYarpRobotInterface()
 {
-    std::cerr << "GazeboYarpRobotInterface constructor" << std::endl;
 }
 
 GazeboYarpRobotInterface::~GazeboYarpRobotInterface()
@@ -33,6 +32,8 @@ GazeboYarpRobotInterface::~GazeboYarpRobotInterface()
         yError() << "GazeboYarpRobotInterface: impossible  to run phase ActionPhaseShutdown in robotinterface";
     }
 
+    GazeboYarpPlugins::Handler::getHandler()->releaseDevicesInList(m_deviceScopedNames);
+
     yarp::os::Network::fini();
 }
 
@@ -85,7 +86,7 @@ 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(_parentModel->GetScopedName(), externalDriverList);
+    GazeboYarpPlugins::Handler::getHandler()->getDevicesAsPolyDriverList(_parentModel->GetScopedName(), externalDriverList, m_deviceScopedNames);
 
     // 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);