Fix robotinterface plugin crashes by closing it as soon as one of its devices is destroyed

libraries/device-registry/DeviceRegistry.cc

@@ -1,9 +1,10 @@
 #include <DeviceRegistry.hh>
 
-#include <cstddef>
 #include <gz/sim/Entity.hh>
 #include <gz/sim/EntityComponentManager.hh>
 #include <gz/sim/Util.hh>
+
+#include <cstddef>
 #include <iostream>
 #include <mutex>
 #include <ostream>
@@ -242,6 +243,7 @@ bool DeviceRegistry::removeDevice(const gz::sim::EntityComponentManager& ecm,
                 return false;
             }
 
+            m_deviceRemovedEvent(deviceDatabaseKey);
             m_devicesMap.at(gzInstanceId).erase(deviceDatabaseKey);
         }
     }

libraries/device-registry/DeviceRegistry.hh

@@ -1,6 +1,9 @@
 #pragma once
 
+#include <gz/common/Event.hh>
+#include <gz/common/events/Types.hh>
 #include <gz/sim/Entity.hh>
+
 #include <mutex>
 #include <string>
 #include <unordered_map>
@@ -37,6 +40,15 @@ public:
     bool
     removeDevice(const gz::sim::EntityComponentManager& ecm, const std::string& deviceDatabaseKey);
 
+    /**
+     * Add a callback when a device is removed, i.e. removeDevice is called.
+     */
+    template<typename T>
+    gz::common::ConnectionPtr connectDeviceRemoved(T _subscriber)
+    {
+        return m_deviceRemovedEvent.Connect(_subscriber);
+    }
+
     std::vector<std::string> getDevicesKeys(const gz::sim::EntityComponentManager& ecm) const;
 
 private:
@@ -54,7 +66,9 @@ private:
     static DeviceRegistry* s_handle;
     static std::mutex& mutex();
     std::unordered_map<std::string, std::unordered_map<std::string, yarp::dev::PolyDriver*>>
-        m_devicesMap; // map of known yarp devices
+        m_devicesMap; // map of known yarp devices Updated upstream
+    // Event for when a device is removed
+    gz::common::EventT<void (std::string)> m_deviceRemovedEvent;
 };
 
 } // namespace gzyarp

plugins/robotinterface/RobotInterface.cc

@@ -1,11 +1,14 @@
 #include <ConfigurationHelpers.hh>
 #include <DeviceRegistry.hh>
 
+#include <functional>
 #include <memory>
 #include <sdf/Element.hh>
 #include <string>
 #include <vector>
 
+#include <gz/common/Event.hh>
+#include <gz/common/events/Types.hh>
 #include <gz/plugin/Register.hh>
 #include <gz/sim/Entity.hh>
 #include <gz/sim/EntityComponentManager.hh>
@@ -62,10 +65,23 @@ class RobotInterface : public System, public ISystemConfigure
                 yError() << "gz-sim-yarp-robotinterface-system: impossible  to run phase "
                             "ActionPhaseShutdown in robotinterface";
             }
+            m_connection.reset();
             m_robotInterfaceCorrectlyStarted = false;
         }
     }
 
+    void OnDeviceRemoved(std::string removeDeviceRegistryDatabaseKey)
+    {
+        // Check if deviceRegistryDatabaseKey is among the one passed to this instance of gz_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 (removeDeviceRegistryDatabaseKey == usedDeviceScopedName) {
+                CloseRobotInterface();
+            }
+        }
+        return;
+    }
+
     virtual void Configure(const Entity& _entity,
                            const std::shared_ptr<const sdf::Element>& _sdf,
                            EntityComponentManager& _ecm,
@@ -105,11 +121,17 @@ class RobotInterface : public System, public ISystemConfigure
             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 =
+            DeviceRegistry::getHandler()->connectDeviceRemoved(
+                std::bind(&RobotInterface::OnDeviceRemoved, this, std::placeholders::_1));
     }
 
 private:
     yarp::robotinterface::XMLReaderResult m_xmlRobotInterfaceResult;
     std::vector<std::string> m_deviceScopedNames;
+    gz::common::ConnectionPtr m_connection;
     bool m_robotInterfaceCorrectlyStarted;
 
     bool loadYarpRobotInterfaceConfigurationFile(const std::shared_ptr<const sdf::Element>& _sdf,