Cleanup use of the GazeboYarpPlugins::Handler::getHandler() object to actually remove a device once is deleted

libraries/singleton/src/Handler.cc

@@ -271,8 +271,6 @@ bool Handler::getDevicesAsPolyDriverList(const std::string& modelScopedName, yar
                 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: ";
@@ -282,7 +280,6 @@ 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;
             }

plugins/camera/include/gazebo/Camera.hh

@@ -55,6 +55,8 @@ namespace gazebo
         yarp::dev::PolyDriver m_cameraDriver;
         std::string m_sensorName;
         sensors::CameraSensor *m_sensor;
+        bool m_deviceRegistered;
+        std::string m_scopedDeviceName;
 
         yarp::dev::IFrameGrabberImage*      iFrameGrabberImage;
     };

plugins/camera/src/Camera.cc

@@ -21,12 +21,16 @@ GZ_REGISTER_SENSOR_PLUGIN(gazebo::GazeboYarpCamera)
 
 namespace gazebo {
 
-GazeboYarpCamera::GazeboYarpCamera() : CameraPlugin(), m_yarp()
+GazeboYarpCamera::GazeboYarpCamera() : CameraPlugin(), m_yarp(), m_deviceRegistered(false)
 {
 }
 
 GazeboYarpCamera::~GazeboYarpCamera()
 {
+    if (m_deviceRegistered) {
+        GazeboYarpPlugins::Handler::getHandler()->removeDevice(m_scopedDeviceName);
+        m_deviceRegistered = false;
+    }
     m_cameraDriver.close();
     GazeboYarpPlugins::Handler::getHandler()->removeSensor(m_sensorName);
 }
@@ -96,22 +100,22 @@ void GazeboYarpCamera::Load(sensors::SensorPtr _sensor, sdf::ElementPtr _sdf)
     }
 
     // Register the device with the given name
-    std::string scopedDeviceName;
     if(!m_parameters.check("yarpDeviceName"))
     {
-        scopedDeviceName = m_sensorName + "::" "camera";
+        m_scopedDeviceName = m_sensorName + "::" "camera";
     }
     else
     {
-        scopedDeviceName = m_sensorName + "::" + m_parameters.find("yarpDeviceName").asString();
+        m_scopedDeviceName = m_sensorName + "::" + m_parameters.find("yarpDeviceName").asString();
     }
 
-    if(!GazeboYarpPlugins::Handler::getHandler()->setDevice(scopedDeviceName, &m_cameraDriver))
+    if(!GazeboYarpPlugins::Handler::getHandler()->setDevice(m_scopedDeviceName, &m_cameraDriver))
     {
-        yError()<<"GazeboYarpCamera: failed setting scopedDeviceName(=" << scopedDeviceName << ")";
+        yError()<<"GazeboYarpCamera: failed setting scopedDeviceName(=" << m_scopedDeviceName << ")";
         return;
     }
-    yInfo() << "GazeboYarpCamera: Register YARP device with instance name:" << scopedDeviceName;
+    m_deviceRegistered = true;
+    yInfo() << "GazeboYarpCamera: Register YARP device with instance name:" << m_scopedDeviceName;
 
 }
 

plugins/controlboard/include/gazebo/ControlBoard.hh

@@ -59,6 +59,7 @@ private:
     bool m_useVirtAnalogSensor = false;
     #endif
     yarp::dev::PolyDriver m_controlboardDriver;
+    bool m_deviceRegistered;
     std::string m_scopedDeviceName;
     std::string m_yarpDeviceName;
 

plugins/controlboard/src/ControlBoard.cc

@@ -54,12 +54,20 @@ GZ_REGISTER_MODEL_PLUGIN(GazeboYarpControlBoard)
             m_virtAnalogSensorWrapper.close();
         }
 
-        for (int n = 0; n < m_controlBoards.size(); n++) {
-            std::string scopedDeviceName = m_robotName + "::" + m_controlBoards[n]->key.c_str();
-            GazeboYarpPlugins::Handler::getHandler()->removeDevice(scopedDeviceName);
+        if (m_deviceRegistered) {
+            if (m_parameters.check("disableImplicitNetworkWrapper")) {
+                GazeboYarpPlugins::Handler::getHandler()->removeDevice(m_scopedDeviceName);
+            } else {
+                for (int n = 0; n < m_controlBoards.size(); n++) {
+                    std::string scopedDeviceName = m_robotName + "::" + m_controlBoards[n]->key.c_str();
+                    GazeboYarpPlugins::Handler::getHandler()->removeDevice(scopedDeviceName);
+                }
+            }
         }
         #else
-        GazeboYarpPlugins::Handler::getHandler()->removeDevice(m_scopedDeviceName);
+        if (m_deviceRegistered) {
+            GazeboYarpPlugins::Handler::getHandler()->removeDevice(m_scopedDeviceName);
+        }
         #endif
 
         GazeboYarpPlugins::Handler::getHandler()->removeRobot(m_robotName);
@@ -191,6 +199,7 @@ GZ_REGISTER_MODEL_PLUGIN(GazeboYarpControlBoard)
                     else
                 {
                     scopedDeviceName = m_robotName + "::" + m_parameters.find("yarpDeviceName").asString();
+                    m_scopedDeviceName = scopedDeviceName;
                 }
 
                 newPoly.poly = GazeboYarpPlugins::Handler::getHandler()->getDevice(scopedDeviceName);
@@ -239,8 +248,8 @@ GZ_REGISTER_MODEL_PLUGIN(GazeboYarpControlBoard)
                     yCError(GAZEBOCONTROLBOARD) << "failed setting scopedDeviceName(=" << scopedDeviceName << ")";
                     return;
                 }
+                m_deviceRegistered = true;
                 yCInfo(GAZEBOCONTROLBOARD) << "Registered YARP device with instance name:" << scopedDeviceName;
-
                 m_controlBoards.push(newPoly);
             }
 
@@ -315,6 +324,7 @@ GZ_REGISTER_MODEL_PLUGIN(GazeboYarpControlBoard)
                 yCError(GAZEBOCONTROLBOARD) << "failed setting scopedDeviceName(=" << m_scopedDeviceName << ")";
                 return;
             }
+            m_deviceRegistered = true;
             yCInfo(GAZEBOCONTROLBOARD) << "Registered YARP device with instance name:" << m_scopedDeviceName;
         }
         #else
@@ -350,6 +360,7 @@ GZ_REGISTER_MODEL_PLUGIN(GazeboYarpControlBoard)
             yCError(GAZEBOCONTROLBOARD) << "failed setting scopedDeviceName(=" << m_scopedDeviceName << ")";
             return;
         }
+        m_deviceRegistered = true;
         yCInfo(GAZEBOCONTROLBOARD) << "Registered YARP device with instance name:" << m_scopedDeviceName;
         #endif
     }

plugins/depthCamera/include/gazebo/DepthCamera.hh

@@ -63,7 +63,8 @@ namespace gazebo
         yarp::dev::PolyDriver m_cameraDriver;
         std::string m_sensorName;
         sensors::DepthCameraSensor *m_sensor;
-
+        bool m_deviceRegistered;
+        std::string m_scopedDeviceName;
         #ifndef GAZEBO_YARP_PLUGINS_DISABLE_IMPLICIT_NETWORK_WRAPPERS
         yarp::dev::PolyDriver m_cameraWrapper;
         yarp::dev::IMultipleWrapper* m_iWrap;

plugins/depthCamera/src/DepthCamera.cc

@@ -26,12 +26,16 @@ GZ_REGISTER_SENSOR_PLUGIN(gazebo::GazeboYarpDepthCamera)
 
 namespace gazebo {
 
-GazeboYarpDepthCamera::GazeboYarpDepthCamera() : DepthCameraPlugin(), m_yarp()
+GazeboYarpDepthCamera::GazeboYarpDepthCamera() : DepthCameraPlugin(), m_yarp(), m_deviceRegistered(false)
 {
 }
 
 GazeboYarpDepthCamera::~GazeboYarpDepthCamera()
 {
+    if (m_deviceRegistered) {
+        GazeboYarpPlugins::Handler::getHandler()->removeDevice(m_scopedDeviceName);
+        m_deviceRegistered = false;
+    }
     m_cameraDriver.close();
     GazeboYarpPlugins::Handler::getHandler()->removeSensor(m_sensorName);
 }
@@ -149,32 +153,31 @@ void GazeboYarpDepthCamera::Load(sensors::SensorPtr _sensor, sdf::ElementPtr _sd
     #endif
 
     //Register the device with the given name
-    std::string scopedDeviceName;
-
     #ifndef GAZEBO_YARP_PLUGINS_DISABLE_IMPLICIT_NETWORK_WRAPPERS
     if(!m_driverParameters.check("yarpDeviceName"))
     {
-        scopedDeviceName = m_sensorName + "::" + driver_list[0]->key;
+        m_scopedDeviceName = m_sensorName + "::" + driver_list[0]->key;
     }
     else
     {
-        scopedDeviceName = m_sensorName + "::" + m_driverParameters.find("yarpDeviceName").asString();
+        m_scopedDeviceName = m_sensorName + "::" + m_driverParameters.find("yarpDeviceName").asString();
     }
     #else
     if(!m_driverParameters.check("yarpDeviceName"))
     {
         yCError(GAZEBODEPTH) << "missing yarpDeviceName parameter";
         return;
     }
-    scopedDeviceName = m_sensorName + "::" + m_driverParameters.find("yarpDeviceName").asString();
+    m_scopedDeviceName = m_sensorName + "::" + m_driverParameters.find("yarpDeviceName").asString();
     #endif
 
-    if(!GazeboYarpPlugins::Handler::getHandler()->setDevice(scopedDeviceName, &m_cameraDriver))
+    if(!GazeboYarpPlugins::Handler::getHandler()->setDevice(m_scopedDeviceName, &m_cameraDriver))
     {
-        yCError(GAZEBODEPTH)<<"GazeboYarpDepthCamera: failed setting scopedDeviceName(=" << scopedDeviceName << ")";
+        yCError(GAZEBODEPTH)<<"GazeboYarpDepthCamera: failed setting scopedDeviceName(=" << m_scopedDeviceName << ")";
         return;
     }
-    yCInfo(GAZEBODEPTH) << "Registered YARP device with instance name:" << scopedDeviceName;
+    m_deviceRegistered = true;
+    yCInfo(GAZEBODEPTH) << "Registered YARP device with instance name:" << m_scopedDeviceName;
 }
 
 } // namespace gazebo

plugins/forcetorque/include/gazebo/ForceTorque.hh

@@ -57,15 +57,17 @@ namespace gazebo
     public:
         GazeboYarpForceTorque();
         virtual ~GazeboYarpForceTorque();
-        
+
         virtual void Load(sensors::SensorPtr _sensor, sdf::ElementPtr _sdf);
 
     private:
         yarp::dev::PolyDriver m_forcetorqueWrapper;
         yarp::dev::IMultipleWrapper* m_iWrap;
         yarp::dev::PolyDriver m_forceTorqueDriver;
-        
+
         std::string m_sensorName;
+        bool m_deviceRegistered;
+        std::string m_scopedDeviceName;
     };
 }
 

plugins/forcetorque/src/ForceTorque.cc

@@ -23,12 +23,16 @@ GZ_REGISTER_SENSOR_PLUGIN(gazebo::GazeboYarpForceTorque)
 
 namespace gazebo {
 
-GazeboYarpForceTorque::GazeboYarpForceTorque() : SensorPlugin(), m_iWrap(0)
+GazeboYarpForceTorque::GazeboYarpForceTorque() : SensorPlugin(), m_iWrap(0), m_deviceRegistered(false)
 {
 }
 
 GazeboYarpForceTorque::~GazeboYarpForceTorque()
 {
+    if (m_deviceRegistered) {
+        GazeboYarpPlugins::Handler::getHandler()->removeDevice(m_scopedDeviceName);
+        m_deviceRegistered = false;
+    }
     if(m_iWrap) { m_iWrap->detachAll(); m_iWrap = 0; }
     if( m_forcetorqueWrapper.isValid() ) m_forcetorqueWrapper.close();
     if( m_forceTorqueDriver.isValid() ) m_forceTorqueDriver.close();
@@ -61,7 +65,7 @@ void GazeboYarpForceTorque::Load(sensors::SensorPtr _sensor, sdf::ElementPtr _sd
     // Add my gazebo device driver to the factory.
     ::yarp::dev::Drivers::factory().add(new ::yarp::dev::DriverCreatorOf< ::yarp::dev::GazeboYarpForceTorqueDriver>
                                       ("gazebo_forcetorque", netWrapper.c_str(), "GazeboYarpForceTorqueDriver"));
-    
+
     ::yarp::os::Property driver_properties;
     bool configuration_loaded = GazeboYarpPlugins::loadConfigSensorPlugin(_sensor,_sdf,driver_properties);
 
@@ -73,7 +77,7 @@ void GazeboYarpForceTorque::Load(sensors::SensorPtr _sensor, sdf::ElementPtr _sd
     ///< \todo TODO handle in a better way the parameters that are for the wrapper and the one that are for driver
     ::yarp::os::Property wrapper_properties = driver_properties;
     #endif // GAZEBO_YARP_PLUGINS_DISABLE_IMPLICIT_NETWORK_WRAPPERS
-    
+
     m_sensorName = _sensor->ScopedName();
 
     //Insert the pointer in the singleton handler for retriving it in the yarp driver
@@ -106,8 +110,7 @@ void GazeboYarpForceTorque::Load(sensors::SensorPtr _sensor, sdf::ElementPtr _sd
         yError()<<"GazeboYarpForceTorque Plugin failed: error in opening yarp driver";
         return;
     }
-
-    std::string scopedDeviceName;
+
     #ifndef GAZEBO_YARP_PLUGINS_DISABLE_IMPLICIT_NETWORK_WRAPPERS
     if (!disable_wrapper) {
         //Attach the driver to the wrapper
@@ -123,17 +126,17 @@ void GazeboYarpForceTorque::Load(sensors::SensorPtr _sensor, sdf::ElementPtr _sd
         if( !m_iWrap->attachAll(driver_list) ) {
             yError() << "GazeboYarpForceTorque : error in connecting wrapper and device ";
         }
-        
+
         if(!driver_properties.check("yarpDeviceName"))
         {
-            scopedDeviceName = m_sensorName + "::" + driver_list[0]->key;
+            m_scopedDeviceName = m_sensorName + "::" + driver_list[0]->key;
         }
         else
         {
-            scopedDeviceName = m_sensorName + "::" + driver_properties.find("yarpDeviceName").asString();
+            m_scopedDeviceName = m_sensorName + "::" + driver_properties.find("yarpDeviceName").asString();
         }
     } else {
-        scopedDeviceName = m_sensorName + "::" + driver_properties.find("yarpDeviceName").asString();
+        m_scopedDeviceName = m_sensorName + "::" + driver_properties.find("yarpDeviceName").asString();
     }
 
 
@@ -143,15 +146,16 @@ void GazeboYarpForceTorque::Load(sensors::SensorPtr _sensor, sdf::ElementPtr _sd
         yError() << "GazeboYarpForceTorque : missing yarpDeviceName parameter for device" << m_sensorName;
         return;
     }
-    scopedDeviceName = m_sensorName + "::" + driver_properties.find("yarpDeviceName").asString();
+    m_scopedDeviceName = m_sensorName + "::" + driver_properties.find("yarpDeviceName").asString();
     #endif // GAZEBO_YARP_PLUGINS_DISABLE_IMPLICIT_NETWORK_WRAPPERS
 
-    if(!GazeboYarpPlugins::Handler::getHandler()->setDevice(scopedDeviceName, &m_forceTorqueDriver))
+    if(!GazeboYarpPlugins::Handler::getHandler()->setDevice(m_scopedDeviceName, &m_forceTorqueDriver))
     {
-        yError()<<"GazeboYarpForceTorque: failed setting scopedDeviceName(=" << scopedDeviceName << ")";
+        yError()<<"GazeboYarpForceTorque: failed setting scopedDeviceName(=" << m_scopedDeviceName << ")";
         return;
     }
-    yInfo() << "Registered YARP device with instance name:" << scopedDeviceName;
+    m_deviceRegistered = true;
+    yInfo() << "Registered YARP device with instance name:" << m_scopedDeviceName;
 }
 
 }

plugins/imu/include/gazebo/IMU.hh

@@ -67,13 +67,16 @@ namespace gazebo
         void Load(sensors::SensorPtr _sensor, sdf::ElementPtr _sdf) override;
 
     private:
-        yarp::os::Property m_parameters; 
+        yarp::os::Property m_parameters;
         yarp::dev::PolyDriver m_imuDriver;
         yarp::dev::PolyDriver m_MASWrapper;
         yarp::dev::PolyDriver m_AdditionalWrapper; // for the legacy wrapper ServerInertial
         yarp::dev::IMultipleWrapper* m_iWrap{nullptr};
         yarp::dev::IMultipleWrapper* m_iWrapAdditional{nullptr};
         std::string m_scopedSensorName;
+        bool m_deviceRegistered;
+        std::string m_scopedDeviceName;
+
     };
 }
 

plugins/imu/src/IMU.cc

@@ -22,12 +22,16 @@ GZ_REGISTER_SENSOR_PLUGIN(gazebo::GazeboYarpIMU)
 
 namespace gazebo {
 
-GazeboYarpIMU::GazeboYarpIMU() : SensorPlugin()
+GazeboYarpIMU::GazeboYarpIMU() : SensorPlugin(), m_deviceRegistered(false)
 {
 }
 
 GazeboYarpIMU::~GazeboYarpIMU()
 {
+    if (m_deviceRegistered) {
+        GazeboYarpPlugins::Handler::getHandler()->removeDevice(m_scopedDeviceName);
+        m_deviceRegistered = false;
+    }
     if(m_iWrap) {
         m_iWrap->detachAll();
         m_iWrap = nullptr;
@@ -155,7 +159,6 @@ void GazeboYarpIMU::Load(sensors::SensorPtr _sensor, sdf::ElementPtr _sdf)
     }
 
     //Register the device with the given name
-    std::string scopedDeviceName;
     #ifndef GAZEBO_YARP_PLUGINS_DISABLE_IMPLICIT_NETWORK_WRAPPERS
     ::yarp::dev::PolyDriverList driverList;
     if (!disable_wrapper) 
@@ -174,28 +177,29 @@ void GazeboYarpIMU::Load(sensors::SensorPtr _sensor, sdf::ElementPtr _sdf)
 
     if(!m_parameters.check("yarpDeviceName"))
     {
-        scopedDeviceName = m_scopedSensorName + "::" + driverList[0]->key;
+        m_scopedDeviceName = m_scopedSensorName + "::" + driverList[0]->key;
     }
     else
     {
-        scopedDeviceName = m_scopedSensorName + "::" + m_parameters.find("yarpDeviceName").asString();
+        m_scopedDeviceName = m_scopedSensorName + "::" + m_parameters.find("yarpDeviceName").asString();
     }
     #else
     if(!m_parameters.check("yarpDeviceName"))
     {
         yError() << "GazeboYarpIMU : missing yarpDeviceName parameter for device" << m_scopedSensorName;
         return;
     }
-    scopedDeviceName = m_scopedSensorName + "::" + m_parameters.find("yarpDeviceName").asString();
+    m_scopedDeviceName = m_scopedSensorName + "::" + m_parameters.find("yarpDeviceName").asString();
     #endif // GAZEBO_YARP_PLUGINS_DISABLE_IMPLICIT_NETWORK_WRAPPERS
 
 
-    if(!GazeboYarpPlugins::Handler::getHandler()->setDevice(scopedDeviceName, &m_imuDriver))
+    if(!GazeboYarpPlugins::Handler::getHandler()->setDevice(m_scopedDeviceName, &m_imuDriver))
     {
-        yError()<<"GazeboYarpIMU: failed setting scopedDeviceName(=" << scopedDeviceName << ")";
+        yError()<<"GazeboYarpIMU: failed setting scopedDeviceName(=" << m_scopedDeviceName << ")";
         return;
     }
-    yInfo() << "Registered YARP device with instance name:" << scopedDeviceName;
+    m_deviceRegistered = true;
+    yInfo() << "Registered YARP device with instance name:" << m_scopedDeviceName;
 }
 
 }