Removed onUpdate event callbacks when device driver closes

include/gazebo_yarp_plugins/ControlBoard.hh

@@ -22,9 +22,6 @@
 
 namespace gazebo
 {
-
-
-
 /// \class GazeboYarpControlBoard
 /// Gazebo Plugin emulating the yarp controlBoard device in Gazebo.
 ///.

include/gazebo_yarp_plugins/ControlBoardDriver.h

@@ -24,7 +24,6 @@
 #include <gazebo/gazebo.hh>
 #include <gazebo/physics/physics.hh>
 #include <gazebo/transport/transport.hh>
-
 #pragma GCC diagnostic pop
 
 #define toRad(X) (X*M_PI/180.0)
@@ -51,15 +50,14 @@ class yarp::dev::GazeboYarpControlBoardDriver :
     public yarp::os::RateThread
 {
 public:
-    GazeboYarpControlBoardDriver() : RateThread(20)
-    {}
+    GazeboYarpControlBoardDriver();
 
-    ~GazeboYarpControlBoardDriver(){}
+    ~GazeboYarpControlBoardDriver();
 
     /**
      * Gazebo stuff
      */
-    void gazebo_init();
+    bool gazebo_init();
     void onUpdate(const gazebo::common::UpdateInfo & /*_info*/);
 
     /**
@@ -68,7 +66,7 @@ class yarp::dev::GazeboYarpControlBoardDriver :
 
     //DEVICE DRIVER
     virtual bool open(yarp::os::Searchable& config);    
-    virtual bool close(); //NOT IMPLEMENTED
+    virtual bool close();
     //THREAD (inside comanDeviceDriver.cpp)
     virtual void run();
     virtual bool threadInit();

src/gazebo_plugins/ControlBoard.cc

@@ -28,6 +28,7 @@ GZ_REGISTER_MODEL_PLUGIN(GazeboYarpControlBoard)
 
     GazeboYarpControlBoard::~GazeboYarpControlBoard()
     {
+        _controlBoard.close();
         _wrapper.close();
         std::cout<<"Goodbye!"<<std::endl;
     }
@@ -98,7 +99,7 @@ GZ_REGISTER_MODEL_PLUGIN(GazeboYarpControlBoard)
         {
             printf("GazeboYarpControlBoard ERROR, net list to attach to was not found, exiting\n");
             _wrapper.close();
-            _controlBoard.close();
+            // _controlBoard.close();
             return;
         }
 

src/gazebo_plugins/ForceTorque.cc

@@ -36,6 +36,7 @@ void GazeboYarpForceTorque::Init()
 GazeboYarpForceTorque::~GazeboYarpForceTorque()
 {
     std::cout<<"*** GazeboYarpForceTorque closing ***"<<std::endl;
+    _forcetorque_driver.close();
 }
 
 void GazeboYarpForceTorque::Load(sensors::SensorPtr _sensor, sdf::ElementPtr _sdf)

src/gazebo_plugins/IMU.cc

@@ -36,6 +36,7 @@ void GazeboYarpIMU::Init()
 GazeboYarpIMU::~GazeboYarpIMU()
 {
     std::cout<<"*** GazeboYarpIMU closing ***"<<std::endl;
+    _imu_driver.close();
 }
 
 void GazeboYarpIMU::Load(sensors::SensorPtr _sensor, sdf::ElementPtr _sdf)
@@ -49,8 +50,6 @@ void GazeboYarpIMU::Load(sensors::SensorPtr _sensor, sdf::ElementPtr _sdf)
 
     _sensor->SetActive(true);
 
-
-
     // Add my gazebo device driver to the factory.
     yarp::dev::Drivers::factory().add(new yarp::dev::DriverCreatorOf<yarp::dev::GazeboYarpIMUDriver>
                                       ("gazebo_imu", "inertial", "GazeboYarpIMUDriver"));

src/yarp_drivers/ControlBoardDriver.cpp

@@ -22,11 +22,18 @@ using namespace yarp::sig::draw;
 using namespace yarp::sig::file;
 using namespace yarp::dev;
 
-void GazeboYarpControlBoardDriver::gazebo_init()
+GazeboYarpControlBoardDriver::GazeboYarpControlBoardDriver(): RateThread(20)
+{}
+
+GazeboYarpControlBoardDriver::~GazeboYarpControlBoardDriver() {}
+
+bool GazeboYarpControlBoardDriver::gazebo_init()
 {
     //_robot = gazebo_pointer_wrapper::getModel();
-    std::cout<<"if this message is the last one you read, _robot has not been set"<<std::endl;
+    // std::cout<<"if this message is the last one you read, _robot has not been set"<<std::endl;
+    //assert is a NOP in release mode. We should change the error handling either with an exception or something else
     assert ( _robot );
+    if (!_robot) return false;
 
     std::cout<<"Robot Name: "<<_robot->GetName() <<std::endl;
     std::cout<<"# Joints: "<<_robot->GetJoints().size() <<std::endl;
@@ -77,6 +84,7 @@ void GazeboYarpControlBoardDriver::gazebo_init()
 
     this->updateConnection = gazebo::event::Events::ConnectWorldUpdateBegin (
                                  boost::bind ( &GazeboYarpControlBoardDriver::onUpdate, this, _1 ) );
+
     gazebo_node_ptr = gazebo::transport::NodePtr ( new gazebo::transport::Node );
     gazebo_node_ptr->Init ( this->_robot->GetWorld()->GetName() );
     jointCmdPub = gazebo_node_ptr->Advertise<gazebo::msgs::JointCmd>
@@ -112,6 +120,7 @@ void GazeboYarpControlBoardDriver::gazebo_init()
             _robot->GetJoint(joint_name)->SetAngle(0,a);
         }
     }
+    return true;
 }
 
 void GazeboYarpControlBoardDriver::onUpdate ( const gazebo::common::UpdateInfo & /*_info*/ )

src/yarp_drivers/ControlBoardDriverDeviceDriver.cpp

@@ -28,14 +28,16 @@ bool GazeboYarpControlBoardDriver::open(yarp::os::Searchable& config)
         return false;
     }
 
-    gazebo_init();
-    return RateThread::start();
+    return gazebo_init() && RateThread::start();
 }
 
 
 
-bool GazeboYarpControlBoardDriver::close() //NOT IMPLEMENTED
+bool GazeboYarpControlBoardDriver::close()
 {
+    //unbinding events
+    gazebo::event::Events::DisconnectWorldUpdateBegin (this->updateConnection);
+
     delete [] control_mode;
     delete [] motion_done;
     return true;

src/yarp_drivers/ForceTorqueDriver.cpp

@@ -13,13 +13,13 @@ using namespace yarp::dev;
 
 GazeboYarpForceTorqueDriver::GazeboYarpForceTorqueDriver()
 {
-    return;
+
 }
 
 
 GazeboYarpForceTorqueDriver::~GazeboYarpForceTorqueDriver()
 {
-    return;
+
 }
 
 
@@ -88,6 +88,7 @@ bool GazeboYarpForceTorqueDriver::open(yarp::os::Searchable& config)
 
 bool GazeboYarpForceTorqueDriver::close()
 {
+    gazebo::event::Events::DisconnectWorldUpdateBegin(this->updateConnection);
     return true;
 }
 

src/yarp_drivers/IMUDriver.cpp

@@ -14,13 +14,12 @@ using namespace yarp::dev;
 
 GazeboYarpIMUDriver::GazeboYarpIMUDriver()
 {
-    return;
 }
 
 
 GazeboYarpIMUDriver::~GazeboYarpIMUDriver()
 {
-    return;
+
 }
 
 
@@ -100,6 +99,7 @@ bool GazeboYarpIMUDriver::open(yarp::os::Searchable& config)
 
 bool GazeboYarpIMUDriver::close()
 {
+    gazebo::event::Events::DisconnectWorldUpdateBegin(this->updateConnection);
     return true;
 }