diff --git a/libraries/device-registry/DeviceRegistry.cc b/libraries/device-registry/DeviceRegistry.cc index 0fb9365..2aaf75b 100644 --- a/libraries/device-registry/DeviceRegistry.cc +++ b/libraries/device-registry/DeviceRegistry.cc @@ -1,9 +1,10 @@ #include -#include #include #include #include + +#include #include #include #include @@ -242,6 +243,7 @@ bool DeviceRegistry::removeDevice(const gz::sim::EntityComponentManager& ecm, return false; } + m_deviceRemovedEvent(deviceDatabaseKey); m_devicesMap.at(gzInstanceId).erase(deviceDatabaseKey); } } diff --git a/libraries/device-registry/DeviceRegistry.hh b/libraries/device-registry/DeviceRegistry.hh index bf03ae3..d86e0dc 100644 --- a/libraries/device-registry/DeviceRegistry.hh +++ b/libraries/device-registry/DeviceRegistry.hh @@ -1,6 +1,9 @@ #pragma once +#include +#include #include + #include #include #include @@ -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 + gz::common::ConnectionPtr connectDeviceRemoved(T _subscriber) + { + return m_deviceRemovedEvent.Connect(_subscriber); + } + std::vector getDevicesKeys(const gz::sim::EntityComponentManager& ecm) const; private: @@ -54,7 +66,9 @@ private: static DeviceRegistry* s_handle; static std::mutex& mutex(); std::unordered_map> - 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 m_deviceRemovedEvent; }; } // namespace gzyarp diff --git a/plugins/robotinterface/RobotInterface.cc b/plugins/robotinterface/RobotInterface.cc index 2177d93..2d7ab64 100644 --- a/plugins/robotinterface/RobotInterface.cc +++ b/plugins/robotinterface/RobotInterface.cc @@ -1,11 +1,14 @@ #include #include +#include #include #include #include #include +#include +#include #include #include #include @@ -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& _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 m_deviceScopedNames; + gz::common::ConnectionPtr m_connection; bool m_robotInterfaceCorrectlyStarted; bool loadYarpRobotInterfaceConfigurationFile(const std::shared_ptr& _sdf,