diff --git a/libraries/singleton/include/GazeboYarpPlugins/Handler.hh b/libraries/singleton/include/GazeboYarpPlugins/Handler.hh index 6aabf5250..32e12300c 100644 --- a/libraries/singleton/include/GazeboYarpPlugins/Handler.hh +++ b/libraries/singleton/include/GazeboYarpPlugins/Handler.hh @@ -100,23 +100,35 @@ public: /** \brief Adds a new yarp device pointer to the "database". * - * If the device already exists and the pointer are the same return success, if pointers doesn't match returns error. - * \param deviceName the name of the device to be added to the internal database + * The YARP device are stored in these database using the following schema: + * * For YARP devices created by Model plugins, the deviceDatabaseKey is + * defined as deviceDatabaseKey = Model::GetScopedName() + "::" + yarpDeviceName + * * For YARP devices created by Sensor plugins, the deviceDatabaseKey is + * defined as deviceDatabaseKey = Sensor::GetScopedName() + "::" + yarpDeviceName + * + * yarpDeviceName is a non-scoped identifier of the specific instance of the YARP device, + * that is tipically specified by the Gazebo plugin configuration file, and corresponds to the + * name attribute of the device XML element when the device is created with the robotinterface + * XML format. + * + * If the device with the same deviceDatabaseKey exists and the pointer are the same return success, + * if pointers doesn't match returns error. + * \param deviceDatabaseKey the deviceDatabaseKey of the device to be added to the internal database * \param device2add the pointer of the device to be added to the internal database * \return true if successfully added, or the device already exists. False otherwise. */ - bool setDevice(std::string deviceName, yarp::dev::PolyDriver* device2add); + bool setDevice(std::string deviceDatabaseKey, yarp::dev::PolyDriver* device2add); - /** Returns the pointer to the device matching the sensor name - * \param deviceName device name to be looked for + /** Returns the pointer to the device matching the deviceDatabaseKey + * \param deviceDatabaseKey deviceDatabaseKey to be looked for * \return the pointer to the device */ - yarp::dev::PolyDriver* getDevice(const std::string& deviceName) const; + yarp::dev::PolyDriver* getDevice(const std::string& deviceDatabaseKey) const; /** \brief Removes a device from the internal database - * \param deviceName the name of the device to be removed + * \param deviceDatabaseKey the deviceDatabaseKey of the device to be removed */ - void removeDevice(const std::string& deviceName); + void removeDevice(const std::string& deviceDatabaseKey); /** * \brief Returns a list of the opened devices @@ -124,8 +136,16 @@ public: * and it does not transfer or share ownership of the devices. * The consumer code needs to make sure that the driver lifetime * is longer then the consumer lifetime. + * + * This method returns all the YARP devices that have been created by the specified model, + * and by all its nested model and sensors. As the PolyDriverList is effectively a map in which + * the key is a string and the value is the PolyDriver pointer, in this case the key of the PolyDriverList + * is the yarpDeviceName without any scope, i.e. the yarpDeviceName and not the deviceDatabaseKey . + * + * 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. */ - void getDevicesAsPolyDriverList(yarp::dev::PolyDriverList& list); + bool getDevicesAsPolyDriverList(const std::string& modelScopedName, yarp::dev::PolyDriverList& list); /** Destructor diff --git a/libraries/singleton/src/Handler.cc b/libraries/singleton/src/Handler.cc index f53735896..3e950eb41 100644 --- a/libraries/singleton/src/Handler.cc +++ b/libraries/singleton/src/Handler.cc @@ -12,6 +12,8 @@ #include #include +#include + using namespace gazebo; namespace GazeboYarpPlugins { @@ -170,10 +172,10 @@ void Handler::removeSensor(const std::string& sensorName) } } -bool Handler::setDevice(std::string deviceName, yarp::dev::PolyDriver* device2add) +bool Handler::setDevice(std::string deviceDatabaseKey, yarp::dev::PolyDriver* device2add) { bool ret = false; - DevicesMap::iterator device = m_devicesMap.find(deviceName); + DevicesMap::iterator device = m_devicesMap.find(deviceDatabaseKey); if (device != m_devicesMap.end()) { //device already exists. Increment reference counting if(device->second.object() == device2add) @@ -190,7 +192,7 @@ bool Handler::setDevice(std::string deviceName, yarp::dev::PolyDriver* device2ad } else { //device does not exists. Add to map ReferenceCountingDevice countedDevice(device2add); - if (!m_devicesMap.insert(std::pair(deviceName, countedDevice)).second) { + if (!m_devicesMap.insert(std::pair(deviceDatabaseKey, countedDevice)).second) { yError() << " Error in GazeboYarpPlugins::Handler while inserting a new device pointer!"; ret = false; } else { @@ -200,11 +202,11 @@ bool Handler::setDevice(std::string deviceName, yarp::dev::PolyDriver* device2ad return ret; } -yarp::dev::PolyDriver* Handler::getDevice(const std::string& deviceName) const +yarp::dev::PolyDriver* Handler::getDevice(const std::string& deviceDatabaseKey) const { yarp::dev::PolyDriver* tmp = NULL; - DevicesMap::const_iterator device = m_devicesMap.find(deviceName); + DevicesMap::const_iterator device = m_devicesMap.find(deviceDatabaseKey); if (device != m_devicesMap.end()) { tmp = device->second.object(); } else { @@ -213,9 +215,9 @@ yarp::dev::PolyDriver* Handler::getDevice(const std::string& deviceName) const return tmp; } -void Handler::removeDevice(const std::string& deviceName) +void Handler::removeDevice(const std::string& deviceDatabaseKey) { - DevicesMap::iterator device = m_devicesMap.find(deviceName); + DevicesMap::iterator device = m_devicesMap.find(deviceDatabaseKey); if (device != m_devicesMap.end()) { device->second.decrementCount(); if (!device->second.count()) { @@ -223,19 +225,66 @@ void Handler::removeDevice(const std::string& deviceName) m_devicesMap.erase(device); } } else { - yError() << "Could not remove device " << deviceName << ". Device was not found"; + yError() << "Could not remove device " << deviceDatabaseKey << ". Device was not found"; } return; } -void Handler::getDevicesAsPolyDriverList(yarp::dev::PolyDriverList& list) +inline bool startsWith(const std::string&completeString, + const std::string&candidatePrefix) +{ + // https://stackoverflow.com/a/40441240 + return (completeString.rfind(candidatePrefix, 0) == 0); +} + +bool Handler::getDevicesAsPolyDriverList(const std::string& modelScopedName, yarp::dev::PolyDriverList& list) { - for(auto&& devicesMapElem: m_devicesMap) { - yDebug() << "DEBUG TO REMOVE: Add device to " << devicesMapElem.first; - list.push(devicesMapElem.second.object(), devicesMapElem.first.c_str()); + list = yarp::dev::PolyDriverList(); + + // This map contains only the yarpDeviceName that we actually added + // to the returned yarp::dev::PolyDriverList + 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; + + // If the deviceDatabaseKey starts with the modelScopedName, then it is eligible for insertion + // in the returned list + if (startsWith(deviceDatabaseKey, modelScopedName)) { + // Extract yarpDeviceName from deviceDatabaseKey + yarpDeviceName = deviceDatabaseKey.substr(deviceDatabaseKey.find_last_of(":")+1); + + // Check if a device with the same yarpDeviceName was already inserted + auto got = inserted_yarpDeviceName2deviceDatabaseKey.find(yarpDeviceName); + + // If not found, insert and continue + if (got == inserted_yarpDeviceName2deviceDatabaseKey.end()) { + // 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; + } else { + // If a name collision is found, print a clear error and return + yError() << "GazeboYARPPlugins robotinterface getDevicesAsPolyDriverList error: "; + yError() << "two YARP devices with yarpDeviceName " << yarpDeviceName + << " found in model " << modelScopedName; + yError() << "First instance: " << got->second; + yError() << "Second instance: " << deviceDatabaseKey; + yError() << "Please eliminate or rename one of the two instances. "; + list = yarp::dev::PolyDriverList(); + return false; + } + + } + } - return; + return true; } diff --git a/plugins/robotinterface/src/GazeboYarpRobotInterface.cc b/plugins/robotinterface/src/GazeboYarpRobotInterface.cc index e90de0c50..5604dfd8c 100644 --- a/plugins/robotinterface/src/GazeboYarpRobotInterface.cc +++ b/plugins/robotinterface/src/GazeboYarpRobotInterface.cc @@ -85,18 +85,22 @@ 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(externalDriverList); + GazeboYarpPlugins::Handler::getHandler()->getDevicesAsPolyDriverList(_parentModel->GetScopedName(), externalDriverList); // 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); if (!ok) { yError() << "GazeboYarpRobotInterface : impossible to set external devices"; + return; } // Start robotinterface ok = m_xmlRobotInterfaceResult.robot.enterPhase(yarp::robotinterface::experimental::ActionPhaseStartup); if (!ok) { yError() << "GazeboYarpRobotInterface : impossible to start robotinterface"; + m_xmlRobotInterfaceResult.robot.enterPhase(yarp::robotinterface::experimental::ActionPhaseInterrupt1); + m_xmlRobotInterfaceResult.robot.enterPhase(yarp::robotinterface::experimental::ActionPhaseShutdown); + return; } } diff --git a/tests/robotinterface/RobotInterfaceTest.xml b/tests/robotinterface/RobotInterfaceTest.xml index 4eb862ff1..c3cd68d72 100644 --- a/tests/robotinterface/RobotInterfaceTest.xml +++ b/tests/robotinterface/RobotInterfaceTest.xml @@ -11,8 +11,10 @@ 1 - - pendulum_with_base::pendulum_aaa + + + pendulum_controlboard_