Permit to set enable_tags and disable_tags in gzyarp::RobotInterface robotinterface

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)

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.

libraries/common/ConfigurationHelpers.hh

@@ -20,11 +20,11 @@ public:
     static bool loadRobotInterfaceConfiguration(const std::shared_ptr<const sdf::Element>& 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);
 

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<std::string, std::string> 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<std::string, std::string> 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,15 +405,44 @@ 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<std::string, std::string> consideredOverridenParameters = params.overridenParameters;
             overridenParameters.merge(consideredOverridenParameters);
         }
     }
 
     return true;
 }
+
+bool DeviceRegistry::getConfigurationOverrideForYARPRobotInterface(const gz::sim::EntityComponentManager& ecm,
+    const std::string& parentEntityScopedNameWhereYARPRobotInterfaceWasInserted,
+    const std::string& yarpRobotInterfaceName,
+    std::unordered_map<std::string, std::string>& 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<std::string, std::string> consideredOverridenParameters = params.overridenParameters;
+            overridenParameters.merge(consideredOverridenParameters);
+        }
+    }
+
+    return true;
+}
+
+
 } // namespace gzyarp

libraries/device-registry/DeviceRegistry.hh

@@ -122,10 +122,22 @@ public:
                                                std::unordered_map<std::string, std::string> 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<std::string, std::string> 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<std::string, std::string>& 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<std::string, std::string>& overridenParameters) const;
+
+
     /**
      * Generate a unique device id for a given yarp device name and entity.
      *
@@ -182,21 +207,27 @@ private:
     // Number of gz-sim-yarp-plugins YARP devices not loaded correctly for a given ecm
     std::unordered_map<std::string, std::size_t> 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
         std::string gzInstanceId;
         // 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<ConfigurationOverrideParameters> m_yarpDevicesOverridenParametersList;
+    // they specify the parameters that will be overriden for a given yarp plugin (device or robotinterface)
+    std::vector<ConfigurationOverrideParameters> m_yarpPluginsOverridenParametersList;
 };
 
 } // namespace gzyarp

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,37 +73,89 @@ 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<std::string>("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
             std::stringstream ss;
             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<std::string>("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<std::string>("yarpRobotInterfaceEnableTags");
+                }
+
+                if (sdfClone->HasElement("yarpRobotInterfaceDisableTags"))
+                {
+                    overridenParameters["gzyarp-xml-element-yarpRobotInterfaceDisableTags"] = sdfClone->Get<std::string>("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);
         }