diff --git a/CMakeLists.txt b/CMakeLists.txt index 247097e..8c30564 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 3.22) project(gz-sim-yarp-plugins LANGUAGES CXX C - VERSION 0.3.1) + VERSION 0.4.0) find_package(YARP REQUIRED COMPONENTS robotinterface os) find_package(YCM REQUIRED) diff --git a/README.md b/README.md index 5813766..5ad0bee 100644 --- a/README.md +++ b/README.md @@ -44,7 +44,7 @@ The plugins available in the repo are listed in the following. | `gzyarp::Imu` | `gz-sim-yarp-imu-system` | [Missing. If you need it please open an issue.](https://github.com/robotology/gz-sim-yarp-plugins/issues/new) | [`tutorial/forcetorque`](./tutorial/imu) | | `gzyarp::Camera` | `gz-sim-yarp-camera-system` | [Missing. If you need it please open an issue.](https://github.com/robotology/gz-sim-yarp-plugins/issues/new) | [`tutorial/camera`](./tutorial/imu) | | `gzyarp::BaseState` | `gz-sim-yarp-basestate-system` | [Missing. If you need it please open an issue.](https://github.com/robotology/gz-sim-yarp-plugins/issues/new) | [`tutorial/basestate`](./tutorial/basestate) | -| `gzyarp::RobotInterface` | `gz-sim-yarp-robotinterface-system` | [Missing. If you need it please open an issue.](https://github.com/robotology/gz-sim-yarp-plugins/issues/new) | All tutorials in [`tutorial`](./tutorial) make use of the `gzyarp::RobotInterface` plugin. | +| `gzyarp::RobotInterface` | `gz-sim-yarp-robotinterface-system` | [`robotinterface/README.md`](./robotinterface/README.md) | All tutorials in [`tutorial`](./tutorial) make use of the `gzyarp::RobotInterface` plugin. | | `gzyarp::Clock` | `gz-sim-yarp-clock-system` | [Missing. If you need it please open an issue.](https://github.com/robotology/gz-sim-yarp-plugins/issues/new) | [`tutorial/clock`](./tutorial/clock) | You can see how to use the different plugins by looking in the directories contained in the [tutorial](tutorial/) folder of this repo. Each directory is an example, and contains a README that shows how to run that example. diff --git a/libraries/common/ConfigurationHelpers.hh b/libraries/common/ConfigurationHelpers.hh index 0777ebe..e739c11 100644 --- a/libraries/common/ConfigurationHelpers.hh +++ b/libraries/common/ConfigurationHelpers.hh @@ -20,11 +20,11 @@ public: static bool loadRobotInterfaceConfiguration(const std::shared_ptr& sdf, yarp::robotinterface::XMLReaderResult& result); + static bool findFile(const std::string& filename, std::string& filepath); + private: static bool isURI(const std::string& filepath); - static bool findFile(const std::string& filename, std::string& filepath); - static bool loadYarpConfigurationFile(const std::string& yarpConfigurationFile, yarp::os::Property& config); diff --git a/libraries/device-registry/DeviceRegistry.cc b/libraries/device-registry/DeviceRegistry.cc index e9a0032..d8ec9dc 100644 --- a/libraries/device-registry/DeviceRegistry.cc +++ b/libraries/device-registry/DeviceRegistry.cc @@ -65,9 +65,10 @@ bool DeviceRegistry::getDevicesAsPolyDriverList(const gz::sim::EntityComponentMa if (auto gzInstance_it = m_devicesMap.find(gzInstanceId); gzInstance_it == m_devicesMap.end()) { - yError() << "Error in gzyarp::DeviceRegistry::getDevicesAsPolyDriverList: gz instance " - "not found"; - return false; + // If no instances is found, it probably means that getDevicesAsPolyDriverList is called + // for a gz instance that has not created any device yet, so just returning an empty list + // is the correct way to handle this case + return true; } auto& devicesMap = m_devicesMap.at(gzInstanceId); @@ -353,24 +354,41 @@ bool DeviceRegistry::addConfigurationOverrideForYARPDevice(const gz::sim::Entity const std::string& configurationOverrideInstanceId, std::unordered_map overridenParameters) { - m_yarpDevicesOverridenParametersList.push_back({getGzInstanceId(ecm), + m_yarpPluginsOverridenParametersList.push_back({getGzInstanceId(ecm), parentEntityScopedNameWhereConfigurationOverrideWasInserted, + OverrideType::YARP_DEVICE, yarpDeviceName, configurationOverrideInstanceId, overridenParameters}); return true; } -bool DeviceRegistry::removeConfigurationOverrideForYARPDevice(const std::string& configurationOverrideInstanceId) +bool DeviceRegistry::addConfigurationOverrideForYARPRobotInterface(const gz::sim::EntityComponentManager& ecm, + const std::string& parentEntityScopedNameWhereConfigurationOverrideWasInserted, + const std::string& yarpRobotInterfaceName, + const std::string& configurationOverrideInstanceId, + std::unordered_map overridenParameters) { - m_yarpDevicesOverridenParametersList.erase( + m_yarpPluginsOverridenParametersList.push_back({getGzInstanceId(ecm), + parentEntityScopedNameWhereConfigurationOverrideWasInserted, + OverrideType::YARP_ROBOT_INTERFACE, + yarpRobotInterfaceName, + configurationOverrideInstanceId, + overridenParameters}); + return true; +} + + +bool DeviceRegistry::removeConfigurationOverrideForYARPPlugin(const std::string& configurationOverrideInstanceId) +{ + m_yarpPluginsOverridenParametersList.erase( std::remove_if( - m_yarpDevicesOverridenParametersList.begin(), - m_yarpDevicesOverridenParametersList.end(), + m_yarpPluginsOverridenParametersList.begin(), + m_yarpPluginsOverridenParametersList.end(), [&configurationOverrideInstanceId](const ConfigurationOverrideParameters& params) { return params.configurationOverrideInstanceId == configurationOverrideInstanceId; }), - m_yarpDevicesOverridenParametersList.end()); + m_yarpPluginsOverridenParametersList.end()); return true; } @@ -387,10 +405,11 @@ bool DeviceRegistry::getConfigurationOverrideForYARPDevice(const gz::sim::Entity // is that the model scoped name of the place where the configuration override was inserted is a prefix of the // model scoped name of the device, to ensure that the override is applied only to the devices that are descendent // of the model where the configuration override was inserted - for (auto&& params : m_yarpDevicesOverridenParametersList) { + for (auto&& params : m_yarpPluginsOverridenParametersList) { if (params.gzInstanceId == getGzInstanceId(ecm) && parentEntityScopedNameWhereYARPDeviceWasInserted.rfind(params.parentEntityScopedNameWhereConfigurationOverrideWasInserted) == 0 && - params.yarpDeviceName == yarpDeviceName) { + params.overrideType == OverrideType::YARP_DEVICE && + (params.yarpPluginIdentifier == yarpDeviceName || params.yarpPluginIdentifier == "all")) { std::unordered_map consideredOverridenParameters = params.overridenParameters; overridenParameters.merge(consideredOverridenParameters); } @@ -398,4 +417,32 @@ bool DeviceRegistry::getConfigurationOverrideForYARPDevice(const gz::sim::Entity return true; } + +bool DeviceRegistry::getConfigurationOverrideForYARPRobotInterface(const gz::sim::EntityComponentManager& ecm, + const std::string& parentEntityScopedNameWhereYARPRobotInterfaceWasInserted, + const std::string& yarpRobotInterfaceName, + std::unordered_map& overridenParameters) const +{ + overridenParameters.clear(); + // We go through all the overriden parameters and add to the returned overridenParameters all the matching + // elements of the list. Note that earlier elements in the list have higher priority, to provide the possibility + // of overriding a parameter that was already overriden in a nested configuration override. + // Note that the condition for the overriden to be consider (beside matching gzInstanceId and yarpDeviceName) + // is that the model scoped name of the place where the configuration override was inserted is a prefix of the + // model scoped name of the device, to ensure that the override is applied only to the devices that are descendent + // of the model where the configuration override was inserted + for (auto&& params : m_yarpPluginsOverridenParametersList) { + if (params.gzInstanceId == getGzInstanceId(ecm) && + parentEntityScopedNameWhereYARPRobotInterfaceWasInserted.rfind(params.parentEntityScopedNameWhereConfigurationOverrideWasInserted) == 0 && + params.overrideType == OverrideType::YARP_ROBOT_INTERFACE && + (params.yarpPluginIdentifier == yarpRobotInterfaceName || params.yarpPluginIdentifier == "all")) { + std::unordered_map consideredOverridenParameters = params.overridenParameters; + overridenParameters.merge(consideredOverridenParameters); + } + } + + return true; +} + + } // namespace gzyarp diff --git a/libraries/device-registry/DeviceRegistry.hh b/libraries/device-registry/DeviceRegistry.hh index d3c30cd..cc76b40 100644 --- a/libraries/device-registry/DeviceRegistry.hh +++ b/libraries/device-registry/DeviceRegistry.hh @@ -122,10 +122,22 @@ public: std::unordered_map overridenParameters); /** - * Remove a configuration override for a given yarp device. + * Add a configuration override for a given yarp RobotInterface. + * + * This function is meant to be called only by the gzyarp::ConfigurationOverride plugin. * */ - bool removeConfigurationOverrideForYARPDevice(const std::string& configurationOverrideInstanceId); + bool addConfigurationOverrideForYARPRobotInterface(const gz::sim::EntityComponentManager& ecm, + const std::string& parentEntityScopedNameWhereConfigurationOverrideWasInserted, + const std::string& yarpRobotInterfaceName, + const std::string& configurationOverrideInstanceId, + std::unordered_map overridenParameters); + + /** + * Remove a configuration override for a gz-sim-yarp-plugin. + * + */ + bool removeConfigurationOverrideForYARPPlugin(const std::string& configurationOverrideInstanceId); /** * Get the configuration override for a given yarp device. @@ -138,6 +150,19 @@ public: const std::string& yarpDeviceName, std::unordered_map& overridenParameters) const; + /** + * Get the configuration override for a given yarpRobotInterfaceName. + * + * This function is meant to be called by the gz-yarp-robotinterface plugin to override its configuration. + * + * At the moment only the `all` yarpRobotInterfaceName special value (to represent all the robotinterface in the nested models) is supported. + */ + bool getConfigurationOverrideForYARPRobotInterface(const gz::sim::EntityComponentManager& ecm, + const std::string& parentEntityScopedNameWhereYARPRobotInterfaceWasInserted, + const std::string& yarpRobotInterfaceName, + std::unordered_map& overridenParameters) const; + + /** * Generate a unique device id for a given yarp device name and entity. * @@ -182,11 +207,15 @@ private: // Number of gz-sim-yarp-plugins YARP devices not loaded correctly for a given ecm std::unordered_map m_nrOfGzSimYARPPluginsNotSuccessfullyLoaded; + enum OverrideType { + YARP_DEVICE, + YARP_ROBOT_INTERFACE + }; - // Element that stores the parameters that will be overriden for a given yarp device - // A device will be affected by the override if: + // Element that stores the parameters that will be overriden for a given yarp device or robotinterface + // A plugin will be affected by the override if: // - gzInstanceId match - // - yarpDeviceName match + // - (overrideType,yarpPluginIdentifier) match // - the parentEntityScopedNameWhereConfigurationOverrideWasInserted is a prefix of the parent entity scoped name of the device` struct ConfigurationOverrideParameters { // Id that identifies the gz instance where the configuration override plugin was inserted @@ -194,9 +223,11 @@ private: // Scoped name of the parent entity where the condiguration override plugin was inserted, // used to understand if a given yarp device is affected by the override std::string parentEntityScopedNameWhereConfigurationOverrideWasInserted; - // yarpDeviceName of the yarp device that will have its parameters overriden - std::string yarpDeviceName; - // configurationOverrideInstanceId is a unique id for each element in the m_yarpDevicesOverridenParametersList, + // Specify if this is a override for a device or a robotinterface + OverrideType overrideType; + // This is yarpDeviceName if overrideType==YARP_DEVICE or yarpRobotInterfaceName if overrideType==YARP_ROBOT_INTERFACE + std::string yarpPluginIdentifier; + // configurationOverrideInstanceId is a unique id for each element in the m_yarpPluginsOverridenParametersList, // it is used to remove a configuration override when the corresponding plugin is destroyed std::string configurationOverrideInstanceId; // Map of the parameters that will be overriden @@ -206,8 +237,8 @@ private: }; // This is a list of parameters specified by the configuration override plugin, - // they specify the parameters that will be overriden for a given yarp device - std::vector m_yarpDevicesOverridenParametersList; + // they specify the parameters that will be overriden for a given yarp plugin (device or robotinterface) + std::vector m_yarpPluginsOverridenParametersList; }; } // namespace gzyarp diff --git a/plugins/configurationoverride/ConfigurationOverride.cc b/plugins/configurationoverride/ConfigurationOverride.cc index a1935fa..c85d974 100644 --- a/plugins/configurationoverride/ConfigurationOverride.cc +++ b/plugins/configurationoverride/ConfigurationOverride.cc @@ -46,7 +46,7 @@ class ConfigurationOverride : public System, public ISystemConfigure, public ISy // and will find the overrides specified by the destroyed gzyarp::ConfigurationOverride plugin if (m_overrideInserted) { - DeviceRegistry::getHandler()->removeConfigurationOverrideForYARPDevice(m_configurationOverrideInstanceId); + DeviceRegistry::getHandler()->removeConfigurationOverrideForYARPPlugin(m_configurationOverrideInstanceId); m_overrideInserted = false; } } @@ -73,18 +73,23 @@ class ConfigurationOverride : public System, public ISystemConfigure, public ISy } sdf::ElementPtr yarpPluginConfigurationOverrideElem = sdfClone->GetElement("yarpPluginConfigurationOverride"); - if (!yarpPluginConfigurationOverrideElem->HasAttribute("yarpDeviceName")) + + // There are two possible way of specifying an override: + // * yarpDeviceName attribute, to specify that the override is related to a gz-sim-yarp-plugin that instantiates a YARP device + // * yarpRobotInterfaceName attribute, to specify that the override is related to a gz-sim-robotinterface-plugin (at the moment only the `all` special value is supported) + if (!yarpPluginConfigurationOverrideElem->HasAttribute("yarpDeviceName") && !yarpPluginConfigurationOverrideElem->HasAttribute("yarpRobotInterfaceName") ) { yError() << "Error in gzyarp::ConfigurationOverride::Configure: " - "missing yarpDeviceName attribute of yarpPluginConfigurationOverride element"; + "missing yarpDeviceName or yarpRobotInterfaceName attribute of yarpPluginConfigurationOverride element"; return; } - std::string yarpDeviceName = yarpPluginConfigurationOverrideElem->GetAttribute("yarpDeviceName")->GetAsString(); - - if (sdfClone->HasElement("initialConfiguration")) + // Only one of yarpDeviceName or yarpRobotInterfaceName can be specified + if (yarpPluginConfigurationOverrideElem->HasAttribute("yarpDeviceName") && yarpPluginConfigurationOverrideElem->HasAttribute("yarpRobotInterfaceName")) { - overridenParameters["gzyarp-xml-element-initialConfiguration"] = sdfClone->Get("initialConfiguration"); + yError() << "Error in gzyarp::ConfigurationOverride::Configure: " + "both yarpDeviceName and yarpRobotInterfaceName attributes of yarpPluginConfigurationOverride element are specified, while only one of the two is supported"; + return; } // Define unique key to identify the configuration override and then remove it @@ -92,18 +97,65 @@ class ConfigurationOverride : public System, public ISystemConfigure, public ISy ss << this; m_configurationOverrideInstanceId = DeviceRegistry::getGzInstanceId(_ecm) + "_" + ss.str(); - if (!DeviceRegistry::getHandler()->addConfigurationOverrideForYARPDevice( - _ecm, - scopedName(_entity, _ecm, "/"), - yarpDeviceName, - m_configurationOverrideInstanceId, - overridenParameters)) + // This code is used if yarpDeviceName is specified + if (yarpPluginConfigurationOverrideElem->HasAttribute("yarpDeviceName")) { - yError() << "Error in gzyarp::ConfigurationOverride::Configure: " - "addConfigurationOverrideForYARPDevice failed"; - return; + std::string yarpDeviceName = yarpPluginConfigurationOverrideElem->GetAttribute("yarpDeviceName")->GetAsString(); + + if (sdfClone->HasElement("initialConfiguration")) + { + overridenParameters["gzyarp-xml-element-initialConfiguration"] = sdfClone->Get("initialConfiguration"); + } + + if (!DeviceRegistry::getHandler()->addConfigurationOverrideForYARPDevice( + _ecm, + scopedName(_entity, _ecm, "/"), + yarpDeviceName, + m_configurationOverrideInstanceId, + overridenParameters)) + { + yError() << "Error in gzyarp::ConfigurationOverride::Configure: " + "addConfigurationOverrideForYARPDevice failed"; + return; + } } + // This code is used if yarpRobotInterfaceName is specified + if (yarpPluginConfigurationOverrideElem->HasAttribute("yarpRobotInterfaceName")) + { + std::string yarpRobotInterfaceName = yarpPluginConfigurationOverrideElem->GetAttribute("yarpRobotInterfaceName")->GetAsString(); + + if (yarpRobotInterfaceName != "all") + { + yError() << "Error in gzyarp::ConfigurationOverride::Configure: " + "yarpRobotInterfaceName attribute of yarpPluginConfigurationOverride element must be 'all'"; + return; + } + + if (sdfClone->HasElement("yarpRobotInterfaceEnableTags")) + { + overridenParameters["gzyarp-xml-element-yarpRobotInterfaceEnableTags"] = sdfClone->Get("yarpRobotInterfaceEnableTags"); + } + + if (sdfClone->HasElement("yarpRobotInterfaceDisableTags")) + { + overridenParameters["gzyarp-xml-element-yarpRobotInterfaceDisableTags"] = sdfClone->Get("yarpRobotInterfaceDisableTags"); + } + + if (!DeviceRegistry::getHandler()->addConfigurationOverrideForYARPRobotInterface( + _ecm, + scopedName(_entity, _ecm, "/"), + yarpRobotInterfaceName, + m_configurationOverrideInstanceId, + overridenParameters)) + { + yError() << "Error in gzyarp::ConfigurationOverride::Configure: " + "addConfigurationOverrideForYARPRobotInterface failed"; + return; + } + } + + m_overrideInserted = true; configureHelper.setConfigureIsSuccessful(true); } diff --git a/plugins/configurationoverride/README.md b/plugins/configurationoverride/README.md index c0771e0..1c1e65b 100644 --- a/plugins/configurationoverride/README.md +++ b/plugins/configurationoverride/README.md @@ -10,13 +10,6 @@ and that occurs in `xml` after the insertion of the ConfigurationOverride plugin Each ConfigurationOverride plugin points to another plugin using a unique identifier, and can override the values of its configuration parameters. -The parameters and plugins currently supported by the `gzyarp::ConfigurationOverride` plugin are: - -| Parameter SDF Element name | Overrided Plugin | -|:------------------------------:|:---------------------:| -| `initialConfiguration` | `gzyarp::ControlBoard` | - -Any other parameter is not overriden. The plan for the future is to support more parameters in the `gzyarp::ConfigurationOverride`, if there is some specific parameter that you would like to override via the `gzyarp::ConfigurationOverride`, please [open an issue](https://github.com/robotology/gz-sim-yarp-plugins/issues/new). ### Usage @@ -41,8 +34,27 @@ The relevant snippet is: ``` -The original `initialConfiguration` of the `controlboard_plugin_device` was `0.0`, while via the `gzyarp::ConfigurationOverride` plugin we override its value to be `0.785398` rad (i.e. 45 degrees).] +The original `initialConfiguration` of the `controlboard_plugin_device` was `0.0`, while via the `gzyarp::ConfigurationOverride` plugin we override its value to be `0.785398` rad (i.e. 45 degrees). Note that the `` element is child of another parent ``. This is required as Gazebo Harmonic (`gz-sim8`) there are some inconsistency related to the scoped name of the entity passed to plugin inserted under the `world` element. If you are using Gazebo Ionic (`gz-sim9`) or later, you can also insert directly the `` as a child of the `` element. + +Another tipical example of `gzyarp::ConfigurationOverride` plugin is to set the enable and disable tags of a `gzyarp::RobotInteface` plugin, you can read more about that in [`gzyarp::RobotInterface` documentation](../robotinterface/README.md). + +### Reference documentation of `plugin` child XML elements + +| XML element | Documentation | +|:------------:|:-----------------------:| +| `yarpPluginConfigurationOverride` | The attributes of this element specify the plugin of which we will override the parameters. Possible attributes are `yarpDeviceName`, that specifies the `yarpDeviceName='@yarpDeviceName@'` instantiated by the plugin to override, or `yarpRobotInterfaceName='@yarpRobotInterfaceName@'` to override the parameters of `gzyarp::RobotInterface` plugins named `yarpRobotInterfaceName`. In both cases, you can pass `all` to override the parameters of all YARP devices or YARP RobotInterface plugins in all nested models. | + +The parameters and plugins currently supported by the `gzyarp::ConfigurationOverride` plugin are the following: + +| Parameter SDF Element name | Overrided Plugin | +|:--------------------------------:|:---------------------:| +| `initialConfiguration` | `gzyarp::ControlBoard` | +| `yarpRobotInterfaceEnableTags` | `gzyarp::RobotInterface` | +| `yarpRobotInterfaceDisableTags` | `gzyarp::RobotInterface` | + + +Any other parameter is not overriden. The plan for the future is to support more parameters in the `gzyarp::ConfigurationOverride`, if there is some specific parameter that you would like to override via the `gzyarp::ConfigurationOverride`, please [open an issue](https://github.com/robotology/gz-sim-yarp-plugins/issues/new). diff --git a/plugins/controlboard/src/ControlBoard.cpp b/plugins/controlboard/src/ControlBoard.cpp index 7db0819..af59b80 100644 --- a/plugins/controlboard/src/ControlBoard.cpp +++ b/plugins/controlboard/src/ControlBoard.cpp @@ -107,9 +107,6 @@ void ControlBoard::Configure(const Entity& _entity, std::string yarpDeviceName = m_pluginParameters.find("yarpDeviceName").asString(); m_robotScopedName = gz::sim::scopedName(_entity, _ecm, "/"); - yDebug() << "gz-sim-yarp-controlboard-system : robot scoped name: " << m_robotScopedName; - yDebug() << "gz-sim-yarp-controlboard-system : yarpDeviceName: " << yarpDeviceName; - m_modelEntity = _entity; m_pluginParameters.put(yarp::dev::gzyarp::YarpControlBoardScopedName.c_str(), diff --git a/plugins/robotinterface/README.md b/plugins/robotinterface/README.md new file mode 100644 index 0000000..0cf5b30 --- /dev/null +++ b/plugins/robotinterface/README.md @@ -0,0 +1,67 @@ +### gzyarp::RobotInterface Plugin + + +| `name` | `filename` | +|:-------------:|:------------------:| +| `gzyarp::RobotInterface` | `gz-sim-yarp-robotinterface-system` | + +The `gzyarp::RobotInterface` plugin is used to instantiate arbitrary YARP devices as part of a `gz-sim` simulation, using the [YARP RobotInterface library](https://www.yarp.it/latest/group__robointerface__all.html). + +Particularly, tipically the YARP devices that simulate hardware devices are instantiated via dedicated `gz-sim-yarp-*` `gz-sim` plugins, while to launch any other YARP device, for example [Network Wrapper Servers](https://yarp.it/latest/group__nws__and__nwc__architecture.html) to expose YARP devices functionalities in dedicated middlewares like YARP ports or ROS2 topics, you need to use the `gzyarp::RobotInterface` plugin. + +### Usage + +Add the `gzyarp::RobotInterface` plugin to the model SDF after all the plugin declarations that instantiated YARP devices that you want to use. + +An example of its usage can be seen in the [`tutorial/single_pendulum/single_pendulum_world.sdf`](../../tutorial/single_pendulum/model.sdf) + +A more complete example of the usage is: + +```xml + + + model://single_pendulum/conf/gazebo_controlboard.ini + + + + + + model://single_pendulum/conf/single_pendulum_nws.xml + + + + (enable_tag1 enable_tag2) + + + + (disable_tagA) + + +``` + +The `yarpRobotInterfaceEnableTags` and `yarpRobotInterfaceDisableTags` can also be overriden via the `gzyarp::ConfigurationOverride` plugin, +for example if `model://model_with_robotinterface_inside` is a gz-sim model with a `gzyarp::RobotInterface` plugin inside, you can specify the `enable_tags` or `disable_tags` to specify with: + +```xml + + + + + (enable_tag1) + (disable_tagA disable_tagB) + + + model://model_with_robotinterface_inside + + +``` + +### Reference documentation of `plugin` child XML elements + +| XML element | Documentation | +|:------------:|:-----------------------:| +| `yarpRobotInterfaceConfigurationFile` | The [YARP robotinterface XML file](https://www.yarp.it/latest/group__yarp__robotinterface__xml__config__files.html) that specifies the YARP devices that will be launched by the `gzyarp::RobotInterface` plugin. | +| `yarpRobotInterfaceEnableTags` | This element can be used to pass "enable" tags to the libYARP_robotinterface instance. | +| `yarpRobotInterfaceDisableTags` | This element can be used to pass "disable" tags to the libYARP_robotinterface instance. | \ No newline at end of file diff --git a/plugins/robotinterface/RobotInterface.cc b/plugins/robotinterface/RobotInterface.cc index 8fae2f2..7512c01 100644 --- a/plugins/robotinterface/RobotInterface.cc +++ b/plugins/robotinterface/RobotInterface.cc @@ -92,7 +92,7 @@ class RobotInterface : public System, public ISystemConfigure gzyarp::PluginConfigureHelper configureHelper(_ecm); - if (!loadYarpRobotInterfaceConfigurationFile(_sdf, _ecm, model)) + if (!loadYarpRobotInterfaceConfigurationFile(_sdf, _ecm, model, _entity)) { yError("gz-sim-yarp-robotinterface-system : Error loading robotinterface configuration " "file"); @@ -100,7 +100,6 @@ class RobotInterface : public System, public ISystemConfigure } yarp::dev::PolyDriverList externalDriverList; - DeviceRegistry::getHandler()->getDevicesAsPolyDriverList(_ecm, scopedName(model.Entity(), _ecm), externalDriverList, @@ -137,10 +136,12 @@ class RobotInterface : public System, public ISystemConfigure std::vector m_deviceScopedNames; gz::common::ConnectionPtr m_connection; bool m_robotInterfaceCorrectlyStarted; + std::string m_yarpRobotInterfaceName; + std::string m_parentEntityScopedName; bool loadYarpRobotInterfaceConfigurationFile(const std::shared_ptr& _sdf, const EntityComponentManager& _ecm, - const Model& model) + const Model& model, const Entity& _entity) { if (!_sdf->HasElement("yarpRobotInterfaceConfigurationFile")) { @@ -150,13 +151,78 @@ class RobotInterface : public System, public ISystemConfigure return false; } - if (!ConfigurationHelpers::loadRobotInterfaceConfiguration(_sdf, m_xmlRobotInterfaceResult)) + if (!_sdf->HasElement("yarpRobotInterfaceConfigurationFile")) { - yError() << "gz-sim-yarp-robotinterface-system :" - "yarpRobotInterfaceConfigurationFile not found"; + yError() << "yarpRobotInterfaceConfigurationFile element not found"; + return false; + } + + auto robotinterface_file_name = _sdf->Get("yarpRobotInterfaceConfigurationFile"); + std::string filepath; + if (!ConfigurationHelpers::findFile(robotinterface_file_name, filepath)) + { + yError() << "Error while finding yarpRobotInterfaceConfigurationFile: " + << robotinterface_file_name; return false; } + yarp::robotinterface::XMLReader xmlRobotInterfaceReader; + + // We read the file once just to read the name of the robotinterface, so that we can process the override + m_xmlRobotInterfaceResult = xmlRobotInterfaceReader.getRobotFromFile(filepath); + m_yarpRobotInterfaceName = m_xmlRobotInterfaceResult.robot.name(); + + // Now we read the enable and disable tags specified in the SDF, and if available the overrides + std::string enableTags = ""; + std::string disableTags = ""; + if (_sdf->HasElement("yarpRobotInterfaceEnableTags")) + { + enableTags = _sdf->Get("yarpRobotInterfaceEnableTags"); + } + if (_sdf->HasElement("yarpRobotInterfaceDisableTags")) + { + disableTags = _sdf->Get("yarpRobotInterfaceDisableTags"); + } + + // Then check if there is any override for the enable and disable tags elements + m_parentEntityScopedName = gz::sim::scopedName(_entity, _ecm, "/"); + std::unordered_map overridenParameters; + DeviceRegistry::getHandler()->getConfigurationOverrideForYARPRobotInterface( + _ecm, m_parentEntityScopedName, m_yarpRobotInterfaceName, overridenParameters); + if (overridenParameters.find("gzyarp-xml-element-yarpRobotInterfaceEnableTags") != overridenParameters.end()) + { + enableTags = overridenParameters["gzyarp-xml-element-yarpRobotInterfaceEnableTags"]; + } + + if (overridenParameters.find("gzyarp-xml-element-yarpRobotInterfaceDisableTags") != overridenParameters.end()) + { + disableTags = overridenParameters["gzyarp-xml-element-yarpRobotInterfaceDisableTags"]; + } + + yarp::os::Property config; + if (!enableTags.empty()) + { + yarp::os::Bottle bot; + bot.fromString(enableTags); + config.put("enable_tags", bot.get(0)); + } + if (!disableTags.empty()) + { + yarp::os::Bottle bot; + bot.fromString(disableTags); + config.put("disable_tags", bot.get(0)); + } + + m_xmlRobotInterfaceResult = xmlRobotInterfaceReader.getRobotFromFile(filepath, config); + + if (!m_xmlRobotInterfaceResult.parsingIsSuccessful) + { + yError() << "Failure in parsing yarpRobotInterfaceConfigurationFile: " + << robotinterface_file_name; + return false; + } + + return true; } }; diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index 4a4536c..ef3cbb0 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -16,6 +16,7 @@ add_subdirectory(camera) add_subdirectory(controlboard) add_subdirectory(commons) add_subdirectory(test-helpers) +add_subdirectory(robotinterface) list(APPEND CMAKE_MODULE_PATH ${CMAKE_CURRENT_LIST_DIR}) include(Fetchtiny-process-library) diff --git a/tests/robotinterface/CMakeLists.txt b/tests/robotinterface/CMakeLists.txt new file mode 100644 index 0000000..58fa4a1 --- /dev/null +++ b/tests/robotinterface/CMakeLists.txt @@ -0,0 +1,16 @@ +add_executable(RobotInterfaceTest RobotInterfaceTest.cc) +target_link_libraries(RobotInterfaceTest + GTest::gtest_main + gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER} + gz-plugin${GZ_PLUGIN_VER}::gz-plugin${GZ_PLUGIN_VER} + YARP::YARP_dev +) +add_test(NAME RobotInterfaceTest + COMMAND RobotInterfaceTest) + +set_tests_properties(RobotInterfaceTest PROPERTIES + ENVIRONMENT_MODIFICATION "GZ_SIM_SYSTEM_PLUGIN_PATH=path_list_append:$;GZ_SIM_RESOURCE_PATH=path_list_append:${CMAKE_CURRENT_SOURCE_DIR}") + +target_compile_definitions(RobotInterfaceTest + PRIVATE CMAKE_CURRENT_SOURCE_DIR="${CMAKE_CURRENT_SOURCE_DIR}" +) diff --git a/tests/robotinterface/RobotInterfaceTest.cc b/tests/robotinterface/RobotInterfaceTest.cc new file mode 100644 index 0000000..5f37a87 --- /dev/null +++ b/tests/robotinterface/RobotInterfaceTest.cc @@ -0,0 +1,114 @@ +#include + +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include + +// This global flags are set by devices to easily check which devices were loaded +std::atomic g_device1WasLoaded = false; +std::atomic g_device2WasLoaded = false; + +void resetLoadedFlags() +{ + g_device1WasLoaded = false; + g_device2WasLoaded = false; +} + +class gsypRobotInterfaceTestDevice1 : public yarp::dev::DeviceDriver +{ +public: + gsypRobotInterfaceTestDevice1() = default; + ~gsypRobotInterfaceTestDevice1() override = default; + + bool open(yarp::os::Searchable&) override + { + g_device1WasLoaded = true; + return true; + } + + bool close() override + { + return true; + } +}; + +class gsypRobotInterfaceTestDevice2 : public yarp::dev::DeviceDriver +{ +public: + gsypRobotInterfaceTestDevice2() = default; + ~gsypRobotInterfaceTestDevice2() override = default; + + bool open(yarp::os::Searchable&) override + { + g_device2WasLoaded = true; + return true; + } + + bool close() override + { + return true; + } +}; + + + +TEST(RobotInterfaceTest, CheckEnableAndDisableTags) +{ + yarp::os::NetworkBase::setLocalMode(true); + + // Add devices used for tests + ::yarp::dev::Drivers::factory().add( + new ::yarp::dev::DriverCreatorOf("gsypRobotInterfaceTestDevice1", + "", + "")); + ::yarp::dev::Drivers::factory().add( + new ::yarp::dev::DriverCreatorOf("gsypRobotInterfaceTestDevice2", + "", + "")); + + resetLoadedFlags(); + auto modelPath = std::filesystem::path(CMAKE_CURRENT_SOURCE_DIR) / "robotinterface_with_default_tags.sdf"; + std::cerr << "Testing world " << modelPath << std::endl; + gz::sim::TestFixture fixtureDefault(modelPath.string()); + fixtureDefault.Finalize(); + EXPECT_FALSE(g_device1WasLoaded); + EXPECT_FALSE(g_device2WasLoaded); + + resetLoadedFlags(); + modelPath = std::filesystem::path(CMAKE_CURRENT_SOURCE_DIR) / "robotinterface_with_enable_device1.sdf"; + std::cerr << "Testing world " << modelPath << std::endl; + gz::sim::TestFixture fixtureEnableDevice1(modelPath.string()); + fixtureEnableDevice1.Finalize(); + EXPECT_TRUE(g_device1WasLoaded); + EXPECT_FALSE(g_device2WasLoaded); + + resetLoadedFlags(); + modelPath = std::filesystem::path(CMAKE_CURRENT_SOURCE_DIR) / "robotinterface_with_enable_device1_and_device2.sdf"; + std::cerr << "Testing world " << modelPath << std::endl; + gz::sim::TestFixture fixtureEnableDevice1andDevice2(modelPath.string()); + fixtureEnableDevice1andDevice2.Finalize(); + EXPECT_TRUE(g_device1WasLoaded); + EXPECT_TRUE(g_device2WasLoaded); + + resetLoadedFlags(); + modelPath = std::filesystem::path(CMAKE_CURRENT_SOURCE_DIR) / "robotinterface_with_enable_device1_and_device2_disable_device1.sdf"; + std::cerr << "Testing world " << modelPath << std::endl; + gz::sim::TestFixture fixtureEnableDevice1andDevice2DisableDevice1(modelPath.string()); + fixtureEnableDevice1andDevice2DisableDevice1.Finalize(); + EXPECT_FALSE(g_device1WasLoaded); + EXPECT_TRUE(g_device2WasLoaded); +} + diff --git a/tests/robotinterface/robotinterface_config.xml b/tests/robotinterface/robotinterface_config.xml new file mode 100644 index 0000000..0978bab --- /dev/null +++ b/tests/robotinterface/robotinterface_config.xml @@ -0,0 +1,9 @@ + + + + + + + + + diff --git a/tests/robotinterface/robotinterface_config_device1.xml b/tests/robotinterface/robotinterface_config_device1.xml new file mode 100644 index 0000000..ff2c686 --- /dev/null +++ b/tests/robotinterface/robotinterface_config_device1.xml @@ -0,0 +1,4 @@ + + + + \ No newline at end of file diff --git a/tests/robotinterface/robotinterface_config_device2.xml b/tests/robotinterface/robotinterface_config_device2.xml new file mode 100644 index 0000000..aeaef75 --- /dev/null +++ b/tests/robotinterface/robotinterface_config_device2.xml @@ -0,0 +1,4 @@ + + + + \ No newline at end of file diff --git a/tests/robotinterface/robotinterface_with_default_tags.sdf b/tests/robotinterface/robotinterface_with_default_tags.sdf new file mode 100644 index 0000000..48d95d3 --- /dev/null +++ b/tests/robotinterface/robotinterface_with_default_tags.sdf @@ -0,0 +1,28 @@ + + + + + + + world + base_link + + + + 0 0 0 0 0 0 + 100 + + 1 + 0 + 0 + 1 + 0 + 1 + + + + + model://robotinterface_config.xml + + + diff --git a/tests/robotinterface/robotinterface_with_enable_device1.sdf b/tests/robotinterface/robotinterface_with_enable_device1.sdf new file mode 100644 index 0000000..c756fb2 --- /dev/null +++ b/tests/robotinterface/robotinterface_with_enable_device1.sdf @@ -0,0 +1,16 @@ + + + + + + + + + (enable_device1) + + + model://robotinterface_with_default_tags.sdf + + + + diff --git a/tests/robotinterface/robotinterface_with_enable_device1_and_device2.sdf b/tests/robotinterface/robotinterface_with_enable_device1_and_device2.sdf new file mode 100644 index 0000000..b0872bc --- /dev/null +++ b/tests/robotinterface/robotinterface_with_enable_device1_and_device2.sdf @@ -0,0 +1,16 @@ + + + + + + + + + (enable_device1 enable_device2) + + + model://robotinterface_with_default_tags.sdf + + + + diff --git a/tests/robotinterface/robotinterface_with_enable_device1_and_device2_disable_device1.sdf b/tests/robotinterface/robotinterface_with_enable_device1_and_device2_disable_device1.sdf new file mode 100644 index 0000000..11135a6 --- /dev/null +++ b/tests/robotinterface/robotinterface_with_enable_device1_and_device2_disable_device1.sdf @@ -0,0 +1,17 @@ + + + + + + + + + (enable_device1 enable_device2) + (disable_device1) + + + model://robotinterface_with_default_tags.sdf + + + +