robotinterface: Close as soon as one external device is closing

CHANGELOG.md

@@ -11,6 +11,9 @@ The format of this document is based on [Keep a Changelog](https://keepachangelo
 ### Changed
 - Migrate the example models under the tutorial directory to avoid the use of implicit network wrapper servers, and use the `gazebo_yarp_robotinterface` plugin to spawn their network wrapper servers (https://github.com/robotology/gazebo-yarp-plugins/pull/615 and https://github.com/robotology/gazebo-yarp-plugins/pull/616). 
 
+### Fixed 
+- It is now possible to remove and add again to the simulation models that use `gazebo_yarp_robotinterface` without any crash (https://github.com/robotology/gazebo-yarp-plugins/pull/618, https://github.com/robotology/gazebo-yarp-plugins/pull/619).
+
 ### Fixed
 - Fixed value returned by getDeviceStatus method in `gazebo_yarp_laser` plugin (https://github.com/robotology/gazebo-yarp-plugins/pull/617).
 

libraries/singleton/include/GazeboYarpPlugins/Handler.hh

@@ -40,6 +40,8 @@ namespace gazebo
 #include <yarp/dev/PolyDriver.h>
 #include <yarp/dev/PolyDriverList.h>
 
+#include <gazebo/common/Events.hh>
+
 namespace GazeboYarpPlugins {
 
 /** \class Handler
@@ -162,6 +164,15 @@ public:
      */
     void releaseDevicesInList(const std::vector<std::string>& deviceScopedNames);
 
+    /**
+     * Add a callback when a device is completly removed, i.e. removeDevice is called and the usage counter goes to 0.
+     */
+    template<typename T>
+    gazebo::event::ConnectionPtr ConnectDeviceCompletlyRemoved(T _subscriber)
+    {
+      return m_deviceCompletlyRemoved.Connect(_subscriber);
+    }
+
     /** Destructor
      */
     ~Handler();
@@ -203,6 +214,8 @@ private:
 
     bool findRobotName(sdf::ElementPtr sdf, std::string* robotName);
 
+    gazebo::event::EventT<void (std::string)> m_deviceCompletlyRemoved;
+
 };
 
 }

libraries/singleton/src/Handler.cc

@@ -217,6 +217,7 @@ void Handler::removeDevice(const std::string& deviceDatabaseKey)
     if (device != m_devicesMap.end()) {
         device->second.decrementCount();
         if (!device->second.count()) {
+            m_deviceCompletlyRemoved(deviceDatabaseKey);
             device->second.object()->close();
             m_devicesMap.erase(device);
         }

plugins/robotinterface/include/gazebo/GazeboYarpRobotInterface.hh

@@ -27,11 +27,15 @@ public:
     virtual ~GazeboYarpRobotInterface();
 
     void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf);
+    void CloseRobotInterface();
+    void OnDeviceCompletlyRemoved(std::string scopedDeviceName);
 
 private:
     yarp::robotinterface::XMLReader m_xmlRobotInterfaceReader;
     yarp::robotinterface::XMLReaderResult m_xmlRobotInterfaceResult;
     std::vector<std::string> m_deviceScopedNames;
+    gazebo::event::ConnectionPtr m_connection;
+    bool m_robotInterfaceCorrectlyStarted;
 };
 
 

plugins/robotinterface/src/GazeboYarpRobotInterface.cc

@@ -17,21 +17,43 @@
 namespace gazebo
 {
 
-GazeboYarpRobotInterface::GazeboYarpRobotInterface()
+GazeboYarpRobotInterface::GazeboYarpRobotInterface(): m_robotInterfaceCorrectlyStarted(false)
 {
+
 }
 
-GazeboYarpRobotInterface::~GazeboYarpRobotInterface()
+void GazeboYarpRobotInterface::CloseRobotInterface()
 {
-    // Close robotinterface
-    bool ok = m_xmlRobotInterfaceResult.robot.enterPhase(yarp::robotinterface::ActionPhaseInterrupt1);
-    if (!ok) {
-        yError() << "GazeboYarpRobotInterface: impossible to run phase ActionPhaseInterrupt1 robotinterface";
+    if(m_robotInterfaceCorrectlyStarted) {
+        // Close robotinterface
+        bool ok = m_xmlRobotInterfaceResult.robot.enterPhase(yarp::robotinterface::ActionPhaseInterrupt1);
+        if (!ok) {
+            yError() << "GazeboYarpRobotInterface: impossible to run phase ActionPhaseInterrupt1 robotinterface";
+        }
+        ok = m_xmlRobotInterfaceResult.robot.enterPhase(yarp::robotinterface::ActionPhaseShutdown);
+        if (!ok) {
+            yError() << "GazeboYarpRobotInterface: impossible  to run phase ActionPhaseShutdown in robotinterface";
+        }
+        m_connection.reset();
+        m_robotInterfaceCorrectlyStarted = false;
     }
-    ok = m_xmlRobotInterfaceResult.robot.enterPhase(yarp::robotinterface::ActionPhaseShutdown);
-    if (!ok) {
-        yError() << "GazeboYarpRobotInterface: impossible  to run phase ActionPhaseShutdown in robotinterface";
+}
+
+void GazeboYarpRobotInterface::OnDeviceCompletlyRemoved(std::string deletedDeviceScopedName)
+{
+    // Check if scopedDeviceName is among the one passed to this instance of gazebo_yarp_robotinterface
+    // If yes, close the robotinterface to avoid crashes due to access to a device that is being deleted
+    for (auto&& usedDeviceScopedName: m_deviceScopedNames) {
+        if (deletedDeviceScopedName == usedDeviceScopedName) {
+            CloseRobotInterface();
+        }
     }
+    return;
+}
+
+GazeboYarpRobotInterface::~GazeboYarpRobotInterface()
+{
+    CloseRobotInterface();
 
     yarp::os::Network::fini();
 }
@@ -71,7 +93,7 @@ void GazeboYarpRobotInterface::Load(physics::ModelPtr _parentModel, sdf::Element
                 yError() << "GazeboYarpRobotInterface error: failure in parsing robotinterface configuration for model" << _parentModel->GetName() << "\n"
                       << "GazeboYarpRobotInterface error: yarpRobotInterfaceConfigurationFile : " << robotinterface_file_name << "\n"
                       << "GazeboYarpRobotInterface error: yarpRobotInterfaceConfigurationFile absolute path : " << robotinterface_file_path;
-                loaded_configuration = false; 
+                loaded_configuration = false;
             }
         }
     }
@@ -93,14 +115,21 @@ void GazeboYarpRobotInterface::Load(physics::ModelPtr _parentModel, sdf::Element
         return;
     }
 
-    // Start robotinterface 
+    // Start robotinterface
     ok = m_xmlRobotInterfaceResult.robot.enterPhase(yarp::robotinterface::ActionPhaseStartup);
     if (!ok) {
         yError() << "GazeboYarpRobotInterface : impossible to start robotinterface";
         m_xmlRobotInterfaceResult.robot.enterPhase(yarp::robotinterface::ActionPhaseInterrupt1);
         m_xmlRobotInterfaceResult.robot.enterPhase(yarp::robotinterface::ActionPhaseShutdown);
         return;
     }
+
+    m_robotInterfaceCorrectlyStarted = true;
+    // If the robotinterface started correctly, add a callback to ensure that it is closed as
+    // soon that an external device passed to it is deleted
+    m_connection =
+      GazeboYarpPlugins::Handler::getHandler()->ConnectDeviceCompletlyRemoved(
+        boost::bind(&GazeboYarpRobotInterface::OnDeviceCompletlyRemoved, this, boost::placeholders::_1));
 }