diff --git a/CHANGELOG.md b/CHANGELOG.md index 5003c8345..503baa32c 100644 --- a/CHANGELOG.md +++ b/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). diff --git a/libraries/singleton/include/GazeboYarpPlugins/Handler.hh b/libraries/singleton/include/GazeboYarpPlugins/Handler.hh index e584e9392..08ad7a6a6 100644 --- a/libraries/singleton/include/GazeboYarpPlugins/Handler.hh +++ b/libraries/singleton/include/GazeboYarpPlugins/Handler.hh @@ -40,6 +40,8 @@ namespace gazebo #include #include +#include + namespace GazeboYarpPlugins { /** \class Handler @@ -162,6 +164,15 @@ public: */ void releaseDevicesInList(const std::vector& deviceScopedNames); + /** + * Add a callback when a device is completly removed, i.e. removeDevice is called and the usage counter goes to 0. + */ + template + 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 m_deviceCompletlyRemoved; + }; } diff --git a/libraries/singleton/src/Handler.cc b/libraries/singleton/src/Handler.cc index 10a7b0ff5..e432e5a46 100644 --- a/libraries/singleton/src/Handler.cc +++ b/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); } diff --git a/plugins/robotinterface/include/gazebo/GazeboYarpRobotInterface.hh b/plugins/robotinterface/include/gazebo/GazeboYarpRobotInterface.hh index 13cb6483b..bdf6599d3 100644 --- a/plugins/robotinterface/include/gazebo/GazeboYarpRobotInterface.hh +++ b/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 m_deviceScopedNames; + gazebo::event::ConnectionPtr m_connection; + bool m_robotInterfaceCorrectlyStarted; }; diff --git a/plugins/robotinterface/src/GazeboYarpRobotInterface.cc b/plugins/robotinterface/src/GazeboYarpRobotInterface.cc index f1b7089da..c97c2bccf 100644 --- a/plugins/robotinterface/src/GazeboYarpRobotInterface.cc +++ b/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,7 +115,7 @@ 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"; @@ -101,6 +123,13 @@ void GazeboYarpRobotInterface::Load(physics::ModelPtr _parentModel, sdf::Element 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)); }