multicamera: Cleanup use of removeDevice

robotology · Mar 31, 2022 · f3f0fc4 · f3f0fc4
1 parent e9a8a30
commit f3f0fc4
Show file tree

Hide file tree

Showing 2 changed files with 8 additions and 1 deletion.
diff --git a/plugins/multicamera/include/gazebo/MultiCamera.hh b/plugins/multicamera/include/gazebo/MultiCamera.hh
@@ -50,6 +50,8 @@ namespace gazebo
         sensors::MultiCameraSensor *m_sensor;
 
         yarp::dev::IFrameGrabberImage*      iFrameGrabberImage;
+        bool m_deviceRegistered;
+        std::string m_scopedDeviceName;
     };
 }
 

diff --git a/plugins/multicamera/src/MultiCamera.cc b/plugins/multicamera/src/MultiCamera.cc
@@ -29,12 +29,16 @@ using GazeboYarpPlugins::GAZEBOMULTICAMERA;
 namespace gazebo {
 
 GazeboYarpMultiCamera::GazeboYarpMultiCamera() :
-        MultiCameraPlugin()
+        MultiCameraPlugin(), m_deviceRegistered(false)
 {
 }
 
 GazeboYarpMultiCamera::~GazeboYarpMultiCamera()
 {
+    if (m_deviceRegistered) {
+        GazeboYarpPlugins::Handler::getHandler()->removeDevice(m_scopedDeviceName);
+        m_deviceRegistered = false;
+    }
     this->m_cameraDriver.close();
     GazeboYarpPlugins::Handler::getHandler()->removeSensor(this->m_sensorName);
 }
@@ -110,6 +114,7 @@ void GazeboYarpMultiCamera::Load(sensors::SensorPtr _sensor, sdf::ElementPtr _sd
         yCError(GAZEBOMULTICAMERA)<<"GazeboYarpMultiCamera: failed setting scopedDeviceName(=" << scopedDeviceName << ")";
         return;
     }
+    m_deviceRegistered = true;
     yCInfo(GAZEBOMULTICAMERA) << "GazeboYarpMultiCamera: Register YARP device with instance name:" << scopedDeviceName;
 }