Added proper wrapper detaching for solving issue #58

robotology · Apr 1, 2014 · fa39668 · fa39668
1 parent fb8b11d
commit fa39668
Show file tree

Hide file tree

Showing 5 changed files with 10 additions and 6 deletions.
diff --git a/include/gazebo_yarp_plugins/ForceTorque.hh b/include/gazebo_yarp_plugins/ForceTorque.hh
@@ -10,6 +10,7 @@
 
 #include <gazebo/gazebo.hh>
 #include <yarp/os/Network.h>
+#include <yarp/dev/Wrapper.h>
 #include <yarp/dev/PolyDriver.h>
 #include <string>
 
@@ -31,6 +32,7 @@ namespace gazebo
         sensors::ForceTorqueSensor* parentSensor;
         yarp::os::Network _yarp;
         yarp::dev::PolyDriver _forcetorque_wrapper;
+        yarp::dev::IMultipleWrapper *_iWrap;
         yarp::dev::PolyDriver _forcetorque_driver;
 
         std::string _sensorName;

diff --git a/include/gazebo_yarp_plugins/JointSensors.hh b/include/gazebo_yarp_plugins/JointSensors.hh
@@ -38,6 +38,7 @@ private:
 
     yarp::os::Network _yarp;
     yarp::dev::PolyDriver _jointsensors_wrapper;
+    yarp::dev::IMultipleWrapper *_iWrap;
     yarp::dev::PolyDriver _jointsensors_driver;
     yarp::os::Property _parameters;
 

diff --git a/src/gazebo_plugins/ControlBoard.cc b/src/gazebo_plugins/ControlBoard.cc
@@ -29,6 +29,7 @@ GZ_REGISTER_MODEL_PLUGIN(GazeboYarpControlBoard)
 
     GazeboYarpControlBoard::~GazeboYarpControlBoard()
     {
+        _iWrap->detachAll();
         _wrapper.close();
         _controlBoard.close();
         GazeboYarpPlugins::Handler::getHandler()->removeRobot(_robotName);

diff --git a/src/gazebo_plugins/ForceTorque.cc b/src/gazebo_plugins/ForceTorque.cc
@@ -35,6 +35,7 @@ void GazeboYarpForceTorque::Init()
 GazeboYarpForceTorque::~GazeboYarpForceTorque()
 {
     std::cout<<"*** GazeboYarpForceTorque closing ***"<<std::endl;
+    _iWrap->detachAll();
     _forcetorque_wrapper.close();
     _forcetorque_driver.close();
     GazeboYarpPlugins::Handler::getHandler()->removeSensor(_sensorName);
@@ -108,17 +109,16 @@ void GazeboYarpForceTorque::Load(sensors::SensorPtr _sensor, sdf::ElementPtr _sd
     }
 
     //Attach the driver to the wrapper
-    ::yarp::dev::IMultipleWrapper * iWrap;
     ::yarp::dev::PolyDriverList driver_list;
 
-    if( !_forcetorque_wrapper.view(iWrap) ) {
+    if( !_forcetorque_wrapper.view(_iWrap) ) {
         std::cerr << "GazeboYarpForceTorque : error in loading wrapper" << std::endl;
         return;
     }
 
     driver_list.push(&_forcetorque_driver,"dummy");
 
-    if( iWrap->attachAll(driver_list) ) {
+    if( _iWrap->attachAll(driver_list) ) {
         std::cerr << "GazeboYarpForceTorque : wrapper was connected with driver " << std::endl;
     } else {
         std::cerr << "GazeboYarpForceTorque : error in connecting wrapper and device " << std::endl;

diff --git a/src/gazebo_plugins/JointSensors.cc b/src/gazebo_plugins/JointSensors.cc
@@ -34,6 +34,7 @@ void GazeboYarpJointSensors::Init()
 GazeboYarpJointSensors::~GazeboYarpJointSensors()
 {
     std::cout<<"*** GazeboYarpJointSensors closing ***"<<std::endl;
+    _iWrap->detachAll();
     _jointsensors_wrapper.close();
     _jointsensors_driver.close();
     GazeboYarpPlugins::Handler::getHandler()->removeRobot(_robotName);
@@ -105,17 +106,16 @@ void GazeboYarpJointSensors::Load(physics::ModelPtr _parent, sdf::ElementPtr _sd
     }
 
     //Attach the driver to the wrapper
-    ::yarp::dev::IMultipleWrapper * iWrap;
     ::yarp::dev::PolyDriverList driver_list;
 
-    if( !_jointsensors_wrapper.view(iWrap) ) {
+    if( !_jointsensors_wrapper.view(_iWrap) ) {
         std::cerr << "GazeboYarpJointSensors : error in loading wrapper" << std::endl;
         return;
     }
 
     driver_list.push(&_jointsensors_driver,"dummy");
 
-    if( iWrap->attachAll(driver_list) ) {
+    if( _iWrap->attachAll(driver_list) ) {
         std::cerr << "GazeboYarpJointSensors : wrapper was connected with driver " << std::endl;
     } else {
         std::cerr << "GazeboYarpJointSensors : error in connecting wrapper and device " << std::endl;