From 487becf5eb73d37aa5bcea990d666e85cb82bde1 Mon Sep 17 00:00:00 2001 From: Silvio Date: Tue, 28 Feb 2017 18:36:41 +0100 Subject: [PATCH] Fix deletion of model in Gazebo 8 Streamlining of the disconnection event: simply resetting the Connection shared pointer is enough to disconnect an event since at least Gazebo 2. So the code was cleaned up from all the call to the Disconnect* functions are all the unnecessary check if a connection pointer is valid before resetting it. Fix https://github.com/robotology/gazebo-yarp-plugins/issues/282 . --- plugins/camera/src/CameraDriver.cpp | 3 +-- plugins/clock/src/Clock.cc | 17 ++++---------- .../src/ControlBoardDriverDeviceDriver.cpp | 6 +---- plugins/depthCamera/src/DepthCameraDriver.cpp | 23 ++++--------------- .../externalwrench/src/ApplyExternalWrench.cc | 2 +- plugins/forcetorque/src/ForceTorqueDriver.cpp | 5 +--- plugins/imu/src/IMUDriver.cpp | 5 +--- .../jointsensors/src/JointSensorsDriver.cpp | 2 +- plugins/lasersensor/src/LaserSensorDriver.cpp | 7 ++---- .../maissensor/src/MaisSensorDeviceDriver.cpp | 6 +---- plugins/multicamera/src/MultiCameraDriver.cpp | 8 ++----- plugins/showmodelcom/src/ShowModelCoM.cc | 3 ++- plugins/videotexture/src/VideoTexture.cc | 2 +- 13 files changed, 22 insertions(+), 67 deletions(-) diff --git a/plugins/camera/src/CameraDriver.cpp b/plugins/camera/src/CameraDriver.cpp index 7821ea95a..ace976577 100644 --- a/plugins/camera/src/CameraDriver.cpp +++ b/plugins/camera/src/CameraDriver.cpp @@ -148,8 +148,7 @@ bool GazeboYarpCameraDriver::close() { if (this->m_updateConnection.get()) { - m_camera->DisconnectNewImageFrame(this->m_updateConnection); - this->m_updateConnection = gazebo::event::ConnectionPtr(); + this->m_updateConnection.reset(); m_parentSensor = NULL; } diff --git a/plugins/clock/src/Clock.cc b/plugins/clock/src/Clock.cc index 6b81b96e6..2499e66b8 100644 --- a/plugins/clock/src/Clock.cc +++ b/plugins/clock/src/Clock.cc @@ -40,15 +40,9 @@ namespace gazebo void GazeboYarpClock::cleanup() { - if (m_worldCreatedEvent.get()) { - gazebo::event::Events::DisconnectWorldCreated(m_worldCreatedEvent); - m_worldCreatedEvent = gazebo::event::ConnectionPtr(); //resetting the pointer to NULL. I don't know if boost does it automatically with the above call - } - if (m_timeUpdateEvent.get()) { - gazebo::event::Events::DisconnectWorldUpdateBegin(m_timeUpdateEvent); - m_timeUpdateEvent = gazebo::event::ConnectionPtr(); - } - + m_worldCreatedEvent.reset(); + m_timeUpdateEvent.reset(); + if (m_clockPort) { m_clockPort->close(); delete m_clockPort; m_clockPort = 0; @@ -99,10 +93,7 @@ namespace gazebo void GazeboYarpClock::gazeboYarpClockLoad(std::string world_name) { - if (m_worldCreatedEvent.get()) { - gazebo::event::Events::DisconnectWorldCreated(m_worldCreatedEvent); - m_worldCreatedEvent = gazebo::event::ConnectionPtr(); - } + m_worldCreatedEvent.reset(); //Create ports m_clockPort = new yarp::os::BufferedPort(); diff --git a/plugins/controlboard/src/ControlBoardDriverDeviceDriver.cpp b/plugins/controlboard/src/ControlBoardDriverDeviceDriver.cpp index 42e6d4142..80b3a9961 100644 --- a/plugins/controlboard/src/ControlBoardDriverDeviceDriver.cpp +++ b/plugins/controlboard/src/ControlBoardDriverDeviceDriver.cpp @@ -33,11 +33,7 @@ bool GazeboYarpControlBoardDriver::open(yarp::os::Searchable& config) bool GazeboYarpControlBoardDriver::close() { - //unbinding events - if (this->m_updateConnection.get()) { - gazebo::event::Events::DisconnectWorldUpdateBegin (this->m_updateConnection); - this->m_updateConnection = gazebo::event::ConnectionPtr(); - } + this->m_updateConnection.reset(); delete [] m_controlMode; delete [] m_interactionMode; diff --git a/plugins/depthCamera/src/DepthCameraDriver.cpp b/plugins/depthCamera/src/DepthCameraDriver.cpp index fa735018a..33bab2291 100644 --- a/plugins/depthCamera/src/DepthCameraDriver.cpp +++ b/plugins/depthCamera/src/DepthCameraDriver.cpp @@ -107,36 +107,21 @@ bool GazeboYarpDepthCameraDriver::open(yarp::os::Searchable &config) m_colorFrameMutex.post(); - - //Connect the driver to the gazebo simulation auto imageConnectionBind = boost::bind(&GazeboYarpDepthCameraDriver::OnNewImageFrame, this, _1, _2, _3, _4, _5); auto depthConnectionBind = boost::bind(&GazeboYarpDepthCameraDriver::OnNewDepthFrame, this, _1, _2, _3, _4, _5); this->m_updateImageFrame_Connection = m_depthCameraPtr->ConnectNewImageFrame(imageConnectionBind); this->m_updateDepthFrame_Connection = m_depthCameraPtr->ConnectNewDepthFrame(depthConnectionBind); + return true; } bool GazeboYarpDepthCameraDriver::close() { - if (this->m_updateImageFrame_Connection.get()) - { - m_depthCameraPtr->DisconnectNewImageFrame(this->m_updateImageFrame_Connection); - this->m_updateImageFrame_Connection = gazebo::event::ConnectionPtr(); - } - - if (this->m_updateRGBPointCloud_Connection.get()) - { - m_depthCameraPtr->DisconnectNewImageFrame(this->m_updateRGBPointCloud_Connection); - this->m_updateRGBPointCloud_Connection = gazebo::event::ConnectionPtr(); - } - - if (this->m_updateDepthFrame_Connection.get()) - { - m_depthCameraPtr->DisconnectNewImageFrame(this->m_updateDepthFrame_Connection); - this->m_updateDepthFrame_Connection = gazebo::event::ConnectionPtr(); - } + this->m_updateImageFrame_Connection.reset(); + this->m_updateRGBPointCloud_Connection.reset(); + this->m_updateDepthFrame_Connection.reset(); m_depthCameraSensorPtr = NULL; diff --git a/plugins/externalwrench/src/ApplyExternalWrench.cc b/plugins/externalwrench/src/ApplyExternalWrench.cc index 2844d81c8..eba60fc43 100644 --- a/plugins/externalwrench/src/ApplyExternalWrench.cc +++ b/plugins/externalwrench/src/ApplyExternalWrench.cc @@ -22,7 +22,7 @@ ApplyExternalWrench::ApplyExternalWrench() ApplyExternalWrench::~ApplyExternalWrench() { m_rpcThread.stop(); - gazebo::event::Events::DisconnectWorldUpdateBegin ( this->m_updateConnection ); + this->m_updateConnection.reset(); } void ApplyExternalWrench::UpdateChild() diff --git a/plugins/forcetorque/src/ForceTorqueDriver.cpp b/plugins/forcetorque/src/ForceTorqueDriver.cpp index 643db2b2e..8c607eaae 100644 --- a/plugins/forcetorque/src/ForceTorqueDriver.cpp +++ b/plugins/forcetorque/src/ForceTorqueDriver.cpp @@ -86,10 +86,7 @@ bool GazeboYarpForceTorqueDriver::open(yarp::os::Searchable& config) bool GazeboYarpForceTorqueDriver::close() { - if (this->m_updateConnection.get()) { - gazebo::event::Events::DisconnectWorldUpdateBegin(this->m_updateConnection); - this->m_updateConnection = gazebo::event::ConnectionPtr(); - } + this->m_updateConnection.reset(); return true; } diff --git a/plugins/imu/src/IMUDriver.cpp b/plugins/imu/src/IMUDriver.cpp index a3cb56320..85be8750a 100644 --- a/plugins/imu/src/IMUDriver.cpp +++ b/plugins/imu/src/IMUDriver.cpp @@ -95,10 +95,7 @@ bool GazeboYarpIMUDriver::open(yarp::os::Searchable& config) bool GazeboYarpIMUDriver::close() { - if (this->m_updateConnection.get()) { - gazebo::event::Events::DisconnectWorldUpdateBegin(this->m_updateConnection); - this->m_updateConnection = gazebo::event::ConnectionPtr(); - } + this->m_updateConnection.reset(); return true; } diff --git a/plugins/jointsensors/src/JointSensorsDriver.cpp b/plugins/jointsensors/src/JointSensorsDriver.cpp index d880ef553..19aceb449 100644 --- a/plugins/jointsensors/src/JointSensorsDriver.cpp +++ b/plugins/jointsensors/src/JointSensorsDriver.cpp @@ -181,7 +181,7 @@ bool GazeboYarpJointSensorsDriver::setJointSensorsType(yarp::os::Property & plug bool GazeboYarpJointSensorsDriver::close() { - gazebo::event::Events::DisconnectWorldUpdateBegin(this->updateConnection); + this->updateConnection.reset(); return true; } diff --git a/plugins/lasersensor/src/LaserSensorDriver.cpp b/plugins/lasersensor/src/LaserSensorDriver.cpp index 5d403f507..056c61151 100644 --- a/plugins/lasersensor/src/LaserSensorDriver.cpp +++ b/plugins/lasersensor/src/LaserSensorDriver.cpp @@ -144,11 +144,8 @@ bool GazeboYarpLaserSensorDriver::open(yarp::os::Searchable& config) bool GazeboYarpLaserSensorDriver::close() { - if (this->m_updateConnection.get()) - { - gazebo::event::Events::DisconnectWorldUpdateBegin(this->m_updateConnection); - this->m_updateConnection = gazebo::event::ConnectionPtr(); - } + this->m_updateConnection.reset(); + return true; } diff --git a/plugins/maissensor/src/MaisSensorDeviceDriver.cpp b/plugins/maissensor/src/MaisSensorDeviceDriver.cpp index 582790987..299348e14 100644 --- a/plugins/maissensor/src/MaisSensorDeviceDriver.cpp +++ b/plugins/maissensor/src/MaisSensorDeviceDriver.cpp @@ -40,11 +40,7 @@ bool GazeboYarpMaisSensorDriver::open(yarp::os::Searchable& config) bool GazeboYarpMaisSensorDriver::close() { - //unbinding events - if (this->m_updateConnection.get()) { - gazebo::event::Events::DisconnectWorldUpdateBegin (this->m_updateConnection); - this->m_updateConnection = gazebo::event::ConnectionPtr(); - } + this->m_updateConnection.reset(); return true; } diff --git a/plugins/multicamera/src/MultiCameraDriver.cpp b/plugins/multicamera/src/MultiCameraDriver.cpp index 12861c79e..4285cdc40 100644 --- a/plugins/multicamera/src/MultiCameraDriver.cpp +++ b/plugins/multicamera/src/MultiCameraDriver.cpp @@ -145,12 +145,8 @@ bool yarp::dev::GazeboYarpMultiCameraDriver::open(yarp::os::Searchable& config) bool yarp::dev::GazeboYarpMultiCameraDriver::close() { for (unsigned int i = 0; i < m_camera_count; ++i) { - if (this->m_updateConnection[i].get()) - { - m_camera[i]->DisconnectNewImageFrame(this->m_updateConnection[i]); - this->m_updateConnection[i] = gazebo::event::ConnectionPtr(); - m_parentSensor = NULL; - } + this->m_updateConnection[i].reset(); + m_parentSensor = NULL; delete[] m_imageBuffer[i]; m_imageBuffer[i] = 0; delete m_dataMutex[i]; diff --git a/plugins/showmodelcom/src/ShowModelCoM.cc b/plugins/showmodelcom/src/ShowModelCoM.cc index 11a36fcd3..3414d5a12 100644 --- a/plugins/showmodelcom/src/ShowModelCoM.cc +++ b/plugins/showmodelcom/src/ShowModelCoM.cc @@ -40,7 +40,8 @@ namespace gazebo delete m_comOutputPort; m_comOutputPort = 0; } - gazebo::event::Events::DisconnectWorldUpdateBegin(this->m_updateConnection); + + m_updateConnection.reset(); yarp::os::Network::fini(); } diff --git a/plugins/videotexture/src/VideoTexture.cc b/plugins/videotexture/src/VideoTexture.cc index 3dac52442..e79ac0fb9 100644 --- a/plugins/videotexture/src/VideoTexture.cc +++ b/plugins/videotexture/src/VideoTexture.cc @@ -25,7 +25,7 @@ namespace gazebo VideoTexture::~VideoTexture() { - event::Events::DisconnectPreRender(m_connection); + m_connection.reset(); if (m_network) { delete m_network;