From 039ad4bca08cd8e063e1df597ce3513c3ca8f892 Mon Sep 17 00:00:00 2001 From: Silvio Date: Mon, 12 Aug 2024 19:38:16 +0200 Subject: [PATCH 1/3] Fix robotinterface plugin crashes by closing it as soon as one of its devices is destroyed --- libraries/device-registry/DeviceRegistry.cc | 1 + libraries/device-registry/DeviceRegistry.hh | 18 +++++++++++++++++- plugins/robotinterface/RobotInterface.cc | 20 ++++++++++++++++++++ 3 files changed, 38 insertions(+), 1 deletion(-) diff --git a/libraries/device-registry/DeviceRegistry.cc b/libraries/device-registry/DeviceRegistry.cc index 0fb93659..515f4dfa 100644 --- a/libraries/device-registry/DeviceRegistry.cc +++ b/libraries/device-registry/DeviceRegistry.cc @@ -242,6 +242,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 bf03ae3f..6e16ed7c 100644 --- a/libraries/device-registry/DeviceRegistry.hh +++ b/libraries/device-registry/DeviceRegistry.hh @@ -1,6 +1,8 @@ #pragma once +#include #include + #include #include #include @@ -37,6 +39,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 +65,12 @@ 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 + + // Number of gz-sim-yarp-plugins YARP devices not loaded correctly for a given ecm + std::unordered_map m_nrOfGzSimYARPPluginsNotSuccessfullyLoaded; + // 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 2177d936..9d72a233 100644 --- a/plugins/robotinterface/RobotInterface.cc +++ b/plugins/robotinterface/RobotInterface.cc @@ -6,6 +6,7 @@ #include #include +#include #include #include #include @@ -62,10 +63,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 +119,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, From b07fd2897149d680b8b9f9db2eaba36446c848eb Mon Sep 17 00:00:00 2001 From: Silvio Traversaro Date: Mon, 12 Aug 2024 19:43:00 +0200 Subject: [PATCH 2/3] Update DeviceRegistry.hh --- libraries/device-registry/DeviceRegistry.hh | 3 --- 1 file changed, 3 deletions(-) diff --git a/libraries/device-registry/DeviceRegistry.hh b/libraries/device-registry/DeviceRegistry.hh index 6e16ed7c..6a1990f5 100644 --- a/libraries/device-registry/DeviceRegistry.hh +++ b/libraries/device-registry/DeviceRegistry.hh @@ -66,9 +66,6 @@ private: static std::mutex& mutex(); std::unordered_map> m_devicesMap; // map of known yarp devices Updated upstream - - // Number of gz-sim-yarp-plugins YARP devices not loaded correctly for a given ecm - std::unordered_map m_nrOfGzSimYARPPluginsNotSuccessfullyLoaded; // Event for when a device is removed gz::common::EventT m_deviceRemovedEvent; }; From c09a36c650fd012ab085ee844c8feab9076779a6 Mon Sep 17 00:00:00 2001 From: Alessandro Croci Date: Tue, 13 Aug 2024 15:50:00 +0200 Subject: [PATCH 3/3] Add missing includes --- libraries/device-registry/DeviceRegistry.cc | 3 ++- libraries/device-registry/DeviceRegistry.hh | 1 + plugins/robotinterface/RobotInterface.cc | 2 ++ 3 files changed, 5 insertions(+), 1 deletion(-) diff --git a/libraries/device-registry/DeviceRegistry.cc b/libraries/device-registry/DeviceRegistry.cc index 515f4dfa..2aaf75b4 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 diff --git a/libraries/device-registry/DeviceRegistry.hh b/libraries/device-registry/DeviceRegistry.hh index 6a1990f5..d86e0dc7 100644 --- a/libraries/device-registry/DeviceRegistry.hh +++ b/libraries/device-registry/DeviceRegistry.hh @@ -1,6 +1,7 @@ #pragma once #include +#include #include #include diff --git a/plugins/robotinterface/RobotInterface.cc b/plugins/robotinterface/RobotInterface.cc index 9d72a233..2d7ab644 100644 --- a/plugins/robotinterface/RobotInterface.cc +++ b/plugins/robotinterface/RobotInterface.cc @@ -1,12 +1,14 @@ #include #include +#include #include #include #include #include #include +#include #include #include #include