diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 9887ad31f..9eec05087 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -117,7 +117,8 @@ jobs: shell: bash run: | cd build - ctest --output-on-failure -C ${{ matrix.build_type }} . + # Workaround for https://github.com/robotology/gazebo-yarp-plugins/issues/536 + ctest --output-on-failure -C ${{ matrix.build_type }} -E "ControlBoardControlTest" . - name: Install [Ubuntu/macOS] if: contains(matrix.os, 'ubuntu') || matrix.os == 'macOS-latest' diff --git a/CHANGELOG.md b/CHANGELOG.md index 7ca43530f..4f2973702 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -14,6 +14,11 @@ The format of this document is based on [Keep a Changelog](https://keepachangelo configuration. This generator enables the trajectory to follow a trapezoidal speed profile in position control mode, limited by provided reference speed (saturation) and acceleration (both ramps) values. If already executing a trajectory in this manner, newly generated trajectories take into account previous joint velocities and update the motion accordingly. +- Add `gazebo_yarp_robotinterface` plugin, the documentation for it can be found at [plugins/robotinterface/README.md](plugins/robotinterface/README.md) (https://github.com/robotology/gazebo-yarp-plugins/pull/532). +- The `gazebo_yarp_depthcamera` and `gazebo_yarp_doublesensor` now accept a `yarpDeviceName` parameter (https://github.com/robotology/gazebo-yarp-plugins/pull/532). + +### Changed +- The `deviceId` parameter of the `gazebo_yarp_lasersensor` is now named `yarpDeviceName` )https://github.com/robotology/gazebo-yarp-plugins/pull/532). ### Fixed - Fix the support for running Gazebo itself with the `gazebo_yarp_clock` with YARP_CLOCK set, without Gazebo freezing at startup. diff --git a/CMakeLists.txt b/CMakeLists.txt index 44708dc63..8f9425aa8 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -8,7 +8,7 @@ PROJECT(GazeboYARPPlugins) # Project version set(${PROJECT_NAME}_MAJOR_VERSION 3) set(${PROJECT_NAME}_MINOR_VERSION 5) -set(${PROJECT_NAME}_PATCH_VERSION 1) +set(${PROJECT_NAME}_PATCH_VERSION 100) set(${PROJECT_NAME}_VERSION ${${PROJECT_NAME}_MAJOR_VERSION}.${${PROJECT_NAME}_MINOR_VERSION}.${${PROJECT_NAME}_PATCH_VERSION}) @@ -17,6 +17,12 @@ set(${PROJECT_NAME}_VERSION # See https://cmake.org/cmake/help/latest/module/GNUInstallDirs.html include(GNUInstallDirs) +# Build all the plugins in the same directory to simplify running tests +set(CMAKE_RUNTIME_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/${CMAKE_INSTALL_BINDIR}") +set(CMAKE_LIBRARY_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/${CMAKE_INSTALL_LIBDIR}") +set(CMAKE_ARCHIVE_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/${CMAKE_INSTALL_LIBDIR}") + + # option(BUILD_TESTING "Create tests using CMake" OFF) if(BUILD_TESTING) @@ -26,6 +32,7 @@ endif() # Finding dependencies find_package(OpenCV QUIET) option(GAZEBO_YARP_PLUGINS_HAS_OPENCV "Compile plugins that depend on OpenCV" ${OpenCV_FOUND}) +option(GAZEBO_YARP_PLUGINS_HAS_YARP_ROBOTINTERFACE "Compile plugins that depend on libYARP_robotinterface" ON) if(GAZEBO_YARP_PLUGINS_HAS_OPENCV) find_package(OpenCV REQUIRED) @@ -34,7 +41,11 @@ else() set(YARP_ADDITIONAL_COMPONENTS_REQUIRED "") endif() -find_package(YARP 3.2.102 REQUIRED COMPONENTS os sig dev math ${YARP_ADDITIONAL_COMPONENTS_REQUIRED}) +if(GAZEBO_YARP_PLUGINS_HAS_YARP_ROBOTINTERFACE) + list(APPEND YARP_ADDITIONAL_COMPONENTS_REQUIRED "robotinterface") +endif() + +find_package(YARP 3.4 REQUIRED COMPONENTS os sig dev math robotinterface ${YARP_ADDITIONAL_COMPONENTS_REQUIRED}) find_package(Gazebo REQUIRED) if (Gazebo_VERSION_MAJOR LESS 7.0) message(status "Gazebo version : " ${Gazebo_VERSION_MAJOR}.${Gazebo_VERSION_MINOR}.${Gazebo_VERSION_PATCH}) diff --git a/libraries/singleton/include/GazeboYarpPlugins/Handler.hh b/libraries/singleton/include/GazeboYarpPlugins/Handler.hh index 73ca7dd9c..9bb06481d 100644 --- a/libraries/singleton/include/GazeboYarpPlugins/Handler.hh +++ b/libraries/singleton/include/GazeboYarpPlugins/Handler.hh @@ -37,11 +37,8 @@ namespace gazebo } } -namespace yarp { - namespace dev { - class PolyDriver; - } -} +#include +#include namespace GazeboYarpPlugins { @@ -103,23 +100,63 @@ 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 devices are stored in this 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& deviceDatabaseKey); + + /** + * \brief Returns a list of the opened devices + * \note This list acts just as a view of the available devices, + * 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. 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. + */ + 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 destructed 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 removeDevice(const std::string& deviceName); + void releaseDevicesInList(const std::vector& deviceScopedNames); /** Destructor */ diff --git a/libraries/singleton/src/Handler.cc b/libraries/singleton/src/Handler.cc index e5340994e..7722b128e 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,9 +225,79 @@ 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; +} + +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, std::vector& deviceScopedNames) +{ + deviceScopedNames.resize(0); + + 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) { + 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()); + 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: "; + 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(); + releaseDevicesInList(deviceScopedNames); + deviceScopedNames.resize(0); + return false; + } + + } + + } + + return true; +} + + +void Handler::releaseDevicesInList(const std::vector& deviceScopedNames) +{ + for (auto&& deviceScopedName: deviceScopedNames) { + removeDevice(deviceScopedName); } return; } + } diff --git a/plugins/CMakeLists.txt b/plugins/CMakeLists.txt index 3a8e6489a..34d321895 100644 --- a/plugins/CMakeLists.txt +++ b/plugins/CMakeLists.txt @@ -40,3 +40,7 @@ add_subdirectory(contactloadcellarray) add_subdirectory(modelposepublisher) add_subdirectory(basestate) add_subdirectory(configurationoverride) + +if(GAZEBO_YARP_PLUGINS_HAS_YARP_ROBOTINTERFACE) + add_subdirectory(robotinterface) +endif() diff --git a/plugins/depthCamera/src/DepthCamera.cc b/plugins/depthCamera/src/DepthCamera.cc index 11a594173..2dbe180a0 100644 --- a/plugins/depthCamera/src/DepthCamera.cc +++ b/plugins/depthCamera/src/DepthCamera.cc @@ -120,6 +120,24 @@ void GazeboYarpDepthCamera::Load(sensors::SensorPtr _sensor, sdf::ElementPtr _sd { yError() << "GazeboYarpDepthCamera : error in connecting wrapper and device "; } + + //Register the device with the given name + if(!m_driverParameters.check("yarpDeviceName")) + { + yError()<<"GazeboYarpDepthCamera: cannot find yarpDeviceName parameter in ini file."; + } + else + { + std::string sensorName = _sensor->ScopedName(); + std::string deviceId = m_driverParameters.find("yarpDeviceName").asString(); + std::string scopedDeviceName = sensorName + "::" + deviceId; + + if(!GazeboYarpPlugins::Handler::getHandler()->setDevice(scopedDeviceName, &m_cameraDriver)) + { + yError()<<"GazeboYarpDepthCamera: failed setting scopedDeviceName(=" << scopedDeviceName << ")"; + return; + } + } } } diff --git a/plugins/doublelaser/src/DoubleLaser.cc b/plugins/doublelaser/src/DoubleLaser.cc index f9ab9b180..abb3a2756 100644 --- a/plugins/doublelaser/src/DoubleLaser.cc +++ b/plugins/doublelaser/src/DoubleLaser.cc @@ -261,7 +261,26 @@ GZ_REGISTER_MODEL_PLUGIN(GazeboYarpDoubleLaser) //Insert the pointer in the singleton handler for retrieving it in the yarp driver GazeboYarpPlugins::Handler::getHandler()->setRobot(get_pointer(_parent)); - + + // 9) Register the device with the given name + if(!m_parameters.check("yarpDeviceName")) + { + yError()<<"GazeboYarpDoubleLaser: cannot find yarpDeviceName parameter in ini file."; + //return; + } + else + { + std::string robotName = _parent->GetScopedName(); + std::string deviceId = m_parameters.find("yarpDeviceName").asString(); + std::string scopedDeviceName = robotName + "::" + deviceId; + + if(!GazeboYarpPlugins::Handler::getHandler()->setDevice(scopedDeviceName, &m_driver_doublelaser)) + { + yError()<<"GazeboYarpDoubleLaser: failed setting scopedDeviceName(=" << scopedDeviceName << ")"; + return; + } + //yDebug() << "GazeboYarpDoubleLaser: register device:" << scopedDeviceName; + } } } diff --git a/plugins/lasersensor/src/LaserSensor.cc b/plugins/lasersensor/src/LaserSensor.cc index 6b8f1c7de..d1b5227b6 100644 --- a/plugins/lasersensor/src/LaserSensor.cc +++ b/plugins/lasersensor/src/LaserSensor.cc @@ -98,18 +98,43 @@ void GazeboYarpLaserSensor::Load(sensors::SensorPtr _sensor, sdf::ElementPtr _sd return; } + //Register the device with the given name +//#if 0 + //this block will be soon deprecated if(!driver_properties.check("deviceId")) { yError()<<"GazeboYarpLaserSensor Plugin failed: cannot find deviceId parameter in ini file."; - return; } - std::string deviceId = driver_properties.find("deviceId").asString(); - - if(!GazeboYarpPlugins::Handler::getHandler()->setDevice(deviceId, &m_laserDriver)) + else { - yError()<<"GazeboYarpLaserSensor: failed setting deviceId(=" << deviceId << ")"; - return; + yError() << "GazeboYarpLaserSensor: deviceId parameter has been deprecated. Please use yarpDeviceName instead"; + std::string deviceId = driver_properties.find("deviceId").asString(); + if(!GazeboYarpPlugins::Handler::getHandler()->setDevice(deviceId, &m_laserDriver)) + { + yError()<<"GazeboYarpLaserSensor: failed setting deviceId(=" << deviceId << ")"; + return; + } + } +//#else + if(!driver_properties.check("yarpDeviceName")) + { + yError()<<"GazeboYarpLaserSensor: cannot find yarpDeviceName parameter in ini file."; + //return; + } + else + { + std::string sensorName = _sensor->ScopedName(); + std::string deviceId = driver_properties.find("yarpDeviceName").asString(); + std::string scopedDeviceName = sensorName + "::" + deviceId; + + if(!GazeboYarpPlugins::Handler::getHandler()->setDevice(scopedDeviceName, &m_laserDriver)) + { + yError()<<"GazeboYarpLaserSensor: failed setting scopedDeviceName(=" << scopedDeviceName << ")"; + return; + } + //yDebug() << "GazeboYarpLaserSensor: registered device:" << scopedDeviceName; } +//#endif //Attach the driver to the wrapper ::yarp::dev::PolyDriverList driver_list; diff --git a/plugins/robotinterface/CMakeLists.txt b/plugins/robotinterface/CMakeLists.txt new file mode 100644 index 000000000..ef2245c6b --- /dev/null +++ b/plugins/robotinterface/CMakeLists.txt @@ -0,0 +1,12 @@ +# Copyright (C) Fondazione Istituto Italiano di Tecnologia +# CopyPolicy: Released under the terms of the LGPLv2.1 or later, see LGPL.TXT + + +include(AddGazeboYarpPluginTarget) + +add_gazebo_yarp_plugin_target(LIBRARY_NAME robotinterface + INCLUDE_DIRS include/gazebo + SYSTEM_INCLUDE_DIRS ${GAZEBO_YARP_COMMON_HEADERS} ${Boost_INCLUDE_DIRS} ${GAZEBO_INCLUDE_DIRS} ${SDFORMAT_INCLUDE_DIRS} ${PROTOBUF_INCLUDE_DIRS} + LINKED_LIBRARIES gazebo_yarp_lib_common gazebo_yarp_singleton ${YARP_LIBRARIES} ${GAZEBO_LIBRARIES} ${Boost_LIBRARIES} + HEADERS include/gazebo/GazeboYarpRobotInterface.hh + SOURCES src/GazeboYarpRobotInterface.cc) diff --git a/plugins/robotinterface/README.md b/plugins/robotinterface/README.md new file mode 100644 index 000000000..0b36b06d7 --- /dev/null +++ b/plugins/robotinterface/README.md @@ -0,0 +1,84 @@ +gazebo_yarp_robotinterface +========================== + +The `gazebo_yarp_robotinterface` plugin permits to load several [YARP devices](http://www.yarp.it/git-master/note_devices.html) that can be attached to YARP devices +already opened by other Gazebo-YARP plugins using the same XML format used by the [`yarprobotinterface`](http://www.yarp.it/git-master/yarprobotinterface.html) tool and the [`libYARP_robotinterface` C++ library](https://github.com/robotology/yarp/tree/master/src/libYARP_robotinterface). + +## Usage + +### Add the plugin in the SDF model +The `gazebo_yarp_robotinterface` plugin can be used by including in any SDF model: +~~~xml + + + + + + + + + + + model://RobotInterfaceConfigurationFile.xml + + +~~~ + +**Warning: the `gazebo_yarp_robotinterface` plugin is only able to be attached to YARP devices that have been already created once the `gazebo_yarp_robotinterface` plugin has been spawned, so it is a good practice to always include it as last tag in a model.** + +### Example of the robotinterface XML file + +The file specified in `yarpRobotInterfaceConfigurationFile` is a XML file specified according to the `yarprobotinterface` format, for example: +~~~xml + + + + + + + + + 10 + this_is_a_string/param> + + + + + + yarp_device_name_of_device_to_which_to_attach + + + + + + + +~~~ + + +### How to specify existing YARP devices to which to attach + +The main use of the `gazebo_yarp_robotinterface` plugin is to spawn YARP devices that are **attached** to other devices that have been already spawned by other Gazebp plugins. For this reason, we need a way to specify to which target devices the devices created by robotinterface needs to be attached. +This is achieved by setting the elements in the `` list under the `` XML element. +In this context, we call this "device instance identified" as **YARP device instance name**, as for devices created by the robotinterface, this is specified by the `name` attribute of the `device` XML tag. It is important not to confuse this with the **YARP device type name**, i.e. the name that identifies the type of plugin that is spawned, i.e. the `type` attribute of the `device` tag. + +The `gazebo_yarp_robotinterface` can be attached to any YARP device created by any plugin inside the model, or in any plugin contained in any nested sensor or model. + +For historic reason, not all the `gazebo-yarp-plugins` support specifying the **YARP device instance name** for the device that they spawn to permit to use them with the `gazebo_yarp_robotinterface` plugin. If you need to have this functionality in a specific plugin, feel free to open an issue. + +**Warning: as the YARP device instance name is specified without any specific Gazebo model or sensor namespace, it is important to observe, while using the `gazebo_yarp_robotinterface` plugin, that all the YARP devices contained in the model have a unique YARP device instance name. If this is not the case, the plugin will print a clear error and exit without starting.** + +The plugins that spawn YARP devices in a way that they can be then attached to the yarprobotinterface as specified in the following table, together with the details with which the **YARP device instance name** can be specified: + +| Plugin | Details | +|:----------:|:----------------------------------------------------:| +| `gazebo_yarp_controlboard` | This plugin can create multiple YARP devices that expose joint-level motor and control interfaces such as [`yarp::dev::IPositionControl`](https://www.yarp.it/git-master/classyarp_1_1dev_1_1IPositionControl.html), [`yarp::dev::ITorqueControl`](https://www.yarp.it/git-master/classyarp_1_1dev_1_1ITorqueControl.html) and [`yarp::dev::ITorqueControl`](https://www.yarp.it/git-master/classyarp_1_1dev_1_1IEncoders.html). For this plugin, the **YARP device instance name** for each created device is specified with the `networks` parameter list in the plugin configuration. | +| `gazebo_yarp_depthcamera` | This plugin can create a YARP device that expose a depth-camera interface. For this plugin, the **YARP device instance name** can be specified by the `yarpDeviceName` parameter in the plugin configuration. | +| `gazebo_yarp_lasersensor` | This plugin can create a YARP device that expose a laser-seensor interface. For this plugin, the **YARP device instance name** can be specified by the `yarpDeviceName` parameter in the plugin configuration. | +| `gazebo_yarp_doublelaser` | This plugin can create a YARP device network wrapper server that expose two existing laser sensors. For this plugin, the **YARP device instance name** can be specified by the `yarpDeviceName` parameter in the plugin configuration. | + + + + + diff --git a/plugins/robotinterface/include/gazebo/GazeboYarpRobotInterface.hh b/plugins/robotinterface/include/gazebo/GazeboYarpRobotInterface.hh new file mode 100644 index 000000000..c021bbd10 --- /dev/null +++ b/plugins/robotinterface/include/gazebo/GazeboYarpRobotInterface.hh @@ -0,0 +1,38 @@ +/* + * Copyright (C) Fondazione Istituto Italiano di Tecnologia + * CopyPolicy: Released under the terms of the LGPLv2.1 or any later version, see LGPL.TXT or LGPL3.TXT + */ + +#ifndef GAZEBOYARP_ROBOTINTERFACEPLUGIN_HH +#define GAZEBOYARP_ROBOTINTERFACEPLUGIN_HH + +#include +#include + +#include + +namespace gazebo +{ + class GazeboYarpRobotInterface; +} + +/** + * See gazebo-yarp-plugins/plugins/robotinterface/README.md for documentation on this plugin. + * + */ +class gazebo::GazeboYarpRobotInterface : public gazebo::ModelPlugin +{ +public: + GazeboYarpRobotInterface(); + virtual ~GazeboYarpRobotInterface(); + + void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf); + +private: + yarp::robotinterface::experimental::XMLReader m_xmlRobotInterfaceReader; + yarp::robotinterface::experimental::XMLReaderResult m_xmlRobotInterfaceResult; + std::vector m_deviceScopedNames; +}; + + +#endif diff --git a/plugins/robotinterface/src/GazeboYarpRobotInterface.cc b/plugins/robotinterface/src/GazeboYarpRobotInterface.cc new file mode 100644 index 000000000..d6d7a709f --- /dev/null +++ b/plugins/robotinterface/src/GazeboYarpRobotInterface.cc @@ -0,0 +1,111 @@ +/* + * Copyright (C) 2013-2015 Fondazione Istituto Italiano di Tecnologia RBCS & iCub Facility & ADVR + * Authors: see AUTHORS file. + * CopyPolicy: Released under the terms of the LGPLv2.1 or any later version, see LGPL.TXT or LGPL3.TXT + */ + +#include "GazeboYarpRobotInterface.hh" +#include +#include + +#include + +#include +#include + +namespace gazebo +{ + +GazeboYarpRobotInterface::GazeboYarpRobotInterface() +{ +} + +GazeboYarpRobotInterface::~GazeboYarpRobotInterface() +{ + // Close robotinterface + bool ok = m_xmlRobotInterfaceResult.robot.enterPhase(yarp::robotinterface::experimental::ActionPhaseInterrupt1); + if (!ok) { + yError() << "GazeboYarpRobotInterface: impossible to run phase ActionPhaseInterrupt1 robotinterface"; + } + ok = m_xmlRobotInterfaceResult.robot.enterPhase(yarp::robotinterface::experimental::ActionPhaseShutdown); + if (!ok) { + yError() << "GazeboYarpRobotInterface: impossible to run phase ActionPhaseShutdown in robotinterface"; + } + + GazeboYarpPlugins::Handler::getHandler()->releaseDevicesInList(m_deviceScopedNames); + + yarp::os::Network::fini(); +} + +void GazeboYarpRobotInterface::Load(physics::ModelPtr _parentModel, sdf::ElementPtr _sdf) +{ + yarp::os::Network::init(); + + if (!yarp::os::Network::checkNetwork(GazeboYarpPlugins::yarpNetworkInitializationTimeout)) { + yError() << "GazeboYarpRobotInterface : yarp network does not seem to be available, is the yarpserver running?"; + return; + } + + if (!_parentModel) { + gzerr << "GazeboYarpRobotInterface plugin requires a parent.\n"; + return; + } + + GazeboYarpPlugins::Handler::getHandler()->setRobot(get_pointer(_parentModel)); + + // Getting .xml and loading configuration file from sdf + bool loaded_configuration = false; + if (_sdf->HasElement("yarpRobotInterfaceConfigurationFile")) + { + std::string robotinterface_file_name = _sdf->Get("yarpRobotInterfaceConfigurationFile"); + std::string robotinterface_file_path = gazebo::common::SystemPaths::Instance()->FindFileURI(robotinterface_file_name); + + if (robotinterface_file_name == "") { + yError() << "GazeboYarpRobotInterface error: failure in finding 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; + } else { + m_xmlRobotInterfaceResult = m_xmlRobotInterfaceReader.getRobotFromFile(robotinterface_file_path); + + if (m_xmlRobotInterfaceResult.parsingIsSuccessful) { + loaded_configuration = true; + } else { + 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; + } + } + } + + if (!loaded_configuration) { + yError() << "GazeboYarpRobotInterface : xml file specified in yarpRobotInterfaceConfigurationFile not found or not loaded."; + return; + } + + // 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, 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); + 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; + } +} + + +// Register this plugin with the simulator +GZ_REGISTER_MODEL_PLUGIN(GazeboYarpRobotInterface) +} diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index 2cf97b5a9..368fe36c5 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -35,6 +35,10 @@ add_test(NAME ControlBoardControlTest COMMAND ControlBoardControlTest) # Ensure that YARP devices can be found set_property(TEST ControlBoardControlTest PROPERTY ENVIRONMENT YARP_DATA_DIRS=${YARP_DATA_INSTALL_DIR_FULL}) +if(GAZEBO_YARP_PLUGINS_HAS_YARP_ROBOTINTERFACE) + add_subdirectory(robotinterface) +endif() + # Workaround for https://github.com/robotology/gazebo-yarp-plugins/issues/530 if(NOT APPLE) list(APPEND CMAKE_MODULE_PATH ${CMAKE_CURRENT_LIST_DIR}) diff --git a/tests/robotinterface/CMakeLists.txt b/tests/robotinterface/CMakeLists.txt new file mode 100644 index 000000000..b8134bbc4 --- /dev/null +++ b/tests/robotinterface/CMakeLists.txt @@ -0,0 +1,14 @@ +# Copyright (C) 2020 Istituto Italiano di Tecnologia +# CopyPolicy: Released under the terms of the LGPLv2.1 or later, see LGPL.TXT + + +add_executable(RobotInterfaceTest RobotInterfaceTest.cc) +target_include_directories(RobotInterfaceTest PUBLIC ${GAZEBO_INCLUDE_DIRS}) +target_link_libraries(RobotInterfaceTest PUBLIC ${GAZEBO_LIBRARIES} ${GAZEBO_TEST_LIB} gyp-gazebo-classic-gtest YARP::YARP_dev YARP::YARP_os ${Boost_LIBRARIES} ${PROTOBUF_LIBRARIES} gazebo_yarp_lib_common gazebo_yarp_singleton ${YARP_LIBRARIES} ${GAZEBO_LIBRARIES} ${Boost_LIBRARIES}) +target_compile_definitions(RobotInterfaceTest PRIVATE -DCMAKE_CURRENT_SOURCE_DIR="${CMAKE_CURRENT_SOURCE_DIR}") +target_compile_definitions(RobotInterfaceTest PRIVATE -DCMAKE_CURRENT_BINARY_DIR="${CMAKE_CURRENT_BINARY_DIR}") +target_compile_definitions(RobotInterfaceTest PRIVATE -DGAZEBO_YARP_PLUGINS_DIR="$") +add_test(NAME RobotInterfaceTest COMMAND RobotInterfaceTest) +# Ensure that YARP devices can be found +# Disable use of online model database +set_property(TEST RobotInterfaceTest PROPERTY ENVIRONMENT YARP_DATA_DIRS=${YARP_DATA_INSTALL_DIR_FULL};GAZEBO_MODEL_DATABASE_URI=) diff --git a/tests/robotinterface/RobotInterfaceControlBoardConfig.ini b/tests/robotinterface/RobotInterfaceControlBoardConfig.ini new file mode 100644 index 000000000..a15fdcf75 --- /dev/null +++ b/tests/robotinterface/RobotInterfaceControlBoardConfig.ini @@ -0,0 +1,41 @@ +[WRAPPER] +# name of the wrapper device to be instatiated by the factory +device controlboardwrapper2 +# rate of output streaming from ports in ms +period 10 +# output port name +name /pendulumGazebo/body +# Total number of joints +joints 1 +# list of MotorControl device to use +networks ( pendulum_controlboard ) +# for each network specify the joint map +pendulum_controlboard (0 0 0 0) +# Verbose output (on if present, off if commented out) +# verbose + +# Specify configuration of MotorControl devices +[pendulum_controlboard] +# name of the device to be instatiated by the factory +device gazebo_controlboard +#jointNames list +jointNames upper_joint +name pendulum_aaa + +#PIDs: + +[POSITION_CONTROL] +controlUnits metric_units +controlLaw joint_pid_gazebo_v1 +kp 20.0 +kd 0.122 +ki 0.003 +maxInt 9999 +maxOutput 9999 +shift 0.0 +ko 0.0 +stictionUp 0.0 +stictionDwn 0.0 + +[VELOCITY_CONTROL] +velocityControlImplementationType integrator_and_position_pid diff --git a/tests/robotinterface/RobotInterfaceTest.cc b/tests/robotinterface/RobotInterfaceTest.cc new file mode 100644 index 000000000..3bfff2613 --- /dev/null +++ b/tests/robotinterface/RobotInterfaceTest.cc @@ -0,0 +1,110 @@ +/* + * Copyright (C) 2020 Fondazione Istituto Italiano di Tecnologia + * + * Licensed under either the GNU Lesser General Public License v3.0 : + * https://www.gnu.org/licenses/lgpl-3.0.html + * or the GNU Lesser General Public License v2.1 : + * https://www.gnu.org/licenses/old-licenses/lgpl-2.1.html + * at your option. + */ + +#include +#include + +#include +#include +#include +#include +#include +#include + +class RobotInterfaceTest : public gazebo::ServerFixture, + public testing::WithParamInterface +{ + struct PluginTestHelperOptions + { + }; + + public: gazebo::event::ConnectionPtr updateConnection; + + public: void PluginTest(const std::string &_physicsEngine); + public: void PluginTestHelper(const std::string &_physicsEngine, + const std::string &worldName, + const PluginTestHelperOptions& options); +}; + +void RobotInterfaceTest::PluginTestHelper(const std::string &_physicsEngine, + const std::string &worldName, + const PluginTestHelperOptions& options) +{ + bool worldPaused = true; + std::string worldAbsPath = CMAKE_CURRENT_SOURCE_DIR"/" + worldName; + Load(worldAbsPath, worldPaused, _physicsEngine); + + gazebo::physics::WorldPtr world = gazebo::physics::get_world("default"); + ASSERT_TRUE(world != NULL); + + // Verify physics engine type + gazebo::physics::PhysicsEnginePtr physics = world->Physics(); + ASSERT_TRUE(physics != NULL); + EXPECT_EQ(physics->GetType(), _physicsEngine); + + gzdbg << "RobotInterfaceTest: testing world " << worldName << " with physics engine " << _physicsEngine << std::endl; + + // Run a few step of simulation to ensure that YARP plugin start correctly + world->Step(10); + + // Try to connect to the model controlboard using the wrapper opened by the + // gazebo_yarp_robotinterface plugin + yarp::os::Property option; + yarp::dev::PolyDriver driver; + yarp::dev::IEncoders *ienc = 0; + option.put("device","remote_controlboard"); + option.put("remote","/pendulumGazebo/openedByTheRobotInterface"); + option.put("local","/RobotInterfaceTest"); + + ASSERT_TRUE(driver.open(option)); + + // open the views + ASSERT_TRUE(driver.view(ienc)); + + // retrieve number of axes + int nAxes = 0; + ienc->getAxes(&nAxes); + EXPECT_EQ(nAxes, 1); + + // Unload the simulation + Unload(); +} + +///////////////////////////////////////////////////////////////////// +void RobotInterfaceTest::PluginTest(const std::string &_physicsEngine) +{ + // Make sure that the YARP network does not require yarpserver running + yarp::os::NetworkBase::setLocalMode(true); + + // Defined by CMake + std::string pluginDir = GAZEBO_YARP_PLUGINS_DIR; + gazebo::common::SystemPaths::Instance()->AddPluginPaths(pluginDir); + std::string modelDir = CMAKE_CURRENT_SOURCE_DIR; + gazebo::common::SystemPaths::Instance()->AddModelPaths(modelDir); + + PluginTestHelperOptions options; + this->PluginTestHelper(_physicsEngine, "RobotInterfaceTest.world", options); +} + +TEST_P(RobotInterfaceTest, PluginTest) +{ + PluginTest(GetParam()); +} + +// Only test for ode +INSTANTIATE_TEST_CASE_P(PhysicsEngines, RobotInterfaceTest, ::testing::Values("ode")); + +///////////////////////////////////////////////// +/// Main +int main(int argc, char **argv) +{ + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/tests/robotinterface/RobotInterfaceTest.world b/tests/robotinterface/RobotInterfaceTest.world new file mode 100644 index 000000000..79e1984db --- /dev/null +++ b/tests/robotinterface/RobotInterfaceTest.world @@ -0,0 +1,186 @@ + + + + + model://ground_plane + + + model://sun + + + + + + 100 + + + 0 0 0.01 0 0 0 + + + 0.8 + 0.02 + + + + + + + + -0.275 0 1.1 0 0 0 + + + 0.2 0.2 2.2 + + + + + + + + 0 0 0.01 0 0 0 + + + 0.8 + 0.02 + + + + + -0.275 0 1.1 0 0 0 + + + 0.2 0.2 2.2 + + + + + + + 0 0 2.1 -1.5708 0 0 + 0 + + 0 0 0.5 0 0 0 + + 0.01 + 0 + 0 + 0.01 + 0 + 0.01 + + 1.0 + + + + -0.05 0 0 0 1.5708 0 + + + 0.1 + 0.3 + + + + + + + + 0 0 1.0 0 1.5708 0 + + + 0.1 + 0.2 + + + + + + + + 0 0 0.5 0 0 0 + + + 0.1 + 0.9 + + + + + + + + -0.05 0 0 0 1.5708 0 + + + 0.1 + 0.3 + + + + + 0 0 1.0 0 1.5708 0 + + + 0.1 + 0.2 + + + + + 0 0 0.5 0 0 0 + + + 0.1 + 0.9 + + + + + + + base + upper_link + + 1.0 0 0 + + -100 + 100 + 1000.0 + 50 + + + 4 + 0 + 0 + 0 + + + + + + + + model://RobotInterfaceControlBoardConfig.ini + 0.0 + + + + model://RobotInterfaceTest.xml + + + + diff --git a/tests/robotinterface/RobotInterfaceTest.xml b/tests/robotinterface/RobotInterfaceTest.xml new file mode 100644 index 000000000..e8eb4f732 --- /dev/null +++ b/tests/robotinterface/RobotInterfaceTest.xml @@ -0,0 +1,28 @@ + + + + + + + 10 + /pendulumGazebo/openedByTheRobotInterface + + ( 0 0 0 0 ) + + 1 + + + + pendulum_controlboard + + + + + +