From 2a4bdf123d1c87b84a9f79cf2978f61bc100503c Mon Sep 17 00:00:00 2001 From: Silvio Traversaro Date: Thu, 11 Feb 2021 11:06:02 +0100 Subject: [PATCH] Fix early destruction of device --- .../include/GazeboYarpPlugins/Handler.hh | 12 +++++++++- libraries/singleton/src/Handler.cc | 22 ++++++++++++++----- .../gazebo/GazeboYarpRobotInterface.hh | 1 + .../src/GazeboYarpRobotInterface.cc | 5 +++-- 4 files changed, 32 insertions(+), 8 deletions(-) diff --git a/libraries/singleton/include/GazeboYarpPlugins/Handler.hh b/libraries/singleton/include/GazeboYarpPlugins/Handler.hh index 32e12300c..1ee599ebe 100644 --- a/libraries/singleton/include/GazeboYarpPlugins/Handler.hh +++ b/libraries/singleton/include/GazeboYarpPlugins/Handler.hh @@ -145,8 +145,18 @@ public: * If after removing the scope two devices have the same yarpDeviceName, the getModelDevicesAsPolyDriverList * prints an error and returns false, while true is returned if everything works as expected. */ - bool getDevicesAsPolyDriverList(const std::string& modelScopedName, yarp::dev::PolyDriverList& list); + bool getDevicesAsPolyDriverList(const std::string& modelScopedName, yarp::dev::PolyDriverList& list, std::vector& deviceScopedNames); + /** + * \brief Decrease the usage count for the devices that are acquired with the getDevicesAsPolyDriverList + * + * As Gazebo plugins are not denstructed in the same order that they are loaded, it is necessary to keep + * a count of the users of each device, to ensure that is only destructed when no device are still attached to it. + * + * This function needs to be called by any plugin that has called the getDevicesAsPolyDriverList method during + * the unload/destruction process. + */ + void releaseDevicesInList(const std::vector& deviceScopedNames); /** Destructor */ diff --git a/libraries/singleton/src/Handler.cc b/libraries/singleton/src/Handler.cc index 3e950eb41..7722b128e 100644 --- a/libraries/singleton/src/Handler.cc +++ b/libraries/singleton/src/Handler.cc @@ -237,8 +237,10 @@ inline bool startsWith(const std::string&completeString, return (completeString.rfind(candidatePrefix, 0) == 0); } -bool Handler::getDevicesAsPolyDriverList(const std::string& modelScopedName, yarp::dev::PolyDriverList& list) +bool Handler::getDevicesAsPolyDriverList(const std::string& modelScopedName, yarp::dev::PolyDriverList& list, std::vector& deviceScopedNames) { + deviceScopedNames.resize(0); + list = yarp::dev::PolyDriverList(); // This map contains only the yarpDeviceName that we actually added @@ -246,9 +248,6 @@ bool Handler::getDevicesAsPolyDriverList(const std::string& modelScopedName, yar std::unordered_map inserted_yarpDeviceName2deviceDatabaseKey; for (auto&& devicesMapElem: m_devicesMap) { - yDebug() << "DEBUG TO REMOVE: Add device deviceDatabaseKey " << devicesMapElem.first - << " modelScopedName " << modelScopedName; - std::string deviceDatabaseKey = devicesMapElem.first; std::string yarpDeviceName; @@ -267,7 +266,9 @@ bool Handler::getDevicesAsPolyDriverList(const std::string& modelScopedName, yar // If no name collision is found, insert and continue inserted_yarpDeviceName2deviceDatabaseKey.insert({yarpDeviceName, deviceDatabaseKey}); list.push(devicesMapElem.second.object(), yarpDeviceName.c_str()); - yDebug() << " add yarpDeviceName " << yarpDeviceName; + deviceScopedNames.push_back(deviceDatabaseKey); + // Increase usage counter + setDevice(deviceDatabaseKey, devicesMapElem.second.object()); } else { // If a name collision is found, print a clear error and return yError() << "GazeboYARPPlugins robotinterface getDevicesAsPolyDriverList error: "; @@ -277,6 +278,8 @@ bool Handler::getDevicesAsPolyDriverList(const std::string& modelScopedName, yar yError() << "Second instance: " << deviceDatabaseKey; yError() << "Please eliminate or rename one of the two instances. "; list = yarp::dev::PolyDriverList(); + releaseDevicesInList(deviceScopedNames); + deviceScopedNames.resize(0); return false; } @@ -288,4 +291,13 @@ bool Handler::getDevicesAsPolyDriverList(const std::string& modelScopedName, yar } +void Handler::releaseDevicesInList(const std::vector& deviceScopedNames) +{ + for (auto&& deviceScopedName: deviceScopedNames) { + removeDevice(deviceScopedName); + } + return; +} + + } diff --git a/plugins/robotinterface/include/gazebo/GazeboYarpRobotInterface.hh b/plugins/robotinterface/include/gazebo/GazeboYarpRobotInterface.hh index c00264fbf..c021bbd10 100644 --- a/plugins/robotinterface/include/gazebo/GazeboYarpRobotInterface.hh +++ b/plugins/robotinterface/include/gazebo/GazeboYarpRobotInterface.hh @@ -31,6 +31,7 @@ public: private: yarp::robotinterface::experimental::XMLReader m_xmlRobotInterfaceReader; yarp::robotinterface::experimental::XMLReaderResult m_xmlRobotInterfaceResult; + std::vector m_deviceScopedNames; }; diff --git a/plugins/robotinterface/src/GazeboYarpRobotInterface.cc b/plugins/robotinterface/src/GazeboYarpRobotInterface.cc index 5604dfd8c..6b0e5500a 100644 --- a/plugins/robotinterface/src/GazeboYarpRobotInterface.cc +++ b/plugins/robotinterface/src/GazeboYarpRobotInterface.cc @@ -18,7 +18,6 @@ namespace gazebo GazeboYarpRobotInterface::GazeboYarpRobotInterface() { - std::cerr << "GazeboYarpRobotInterface constructor" << std::endl; } GazeboYarpRobotInterface::~GazeboYarpRobotInterface() @@ -33,6 +32,8 @@ GazeboYarpRobotInterface::~GazeboYarpRobotInterface() yError() << "GazeboYarpRobotInterface: impossible to run phase ActionPhaseShutdown in robotinterface"; } + GazeboYarpPlugins::Handler::getHandler()->releaseDevicesInList(m_deviceScopedNames); + yarp::os::Network::fini(); } @@ -85,7 +86,7 @@ void GazeboYarpRobotInterface::Load(physics::ModelPtr _parentModel, sdf::Element // Extract externalDriverList of devices from the one that have been already opened in the Gazebo model by other gazebo_yarp plugins yarp::dev::PolyDriverList externalDriverList; - GazeboYarpPlugins::Handler::getHandler()->getDevicesAsPolyDriverList(_parentModel->GetScopedName(), externalDriverList); + GazeboYarpPlugins::Handler::getHandler()->getDevicesAsPolyDriverList(_parentModel->GetScopedName(), externalDriverList, m_deviceScopedNames); // Set external devices from the one that have been already opened in the Gazebo model by other gazebo_yarp plugins bool ok = m_xmlRobotInterfaceResult.robot.setExternalDevices(externalDriverList);