Add gzyarp::ConfigurationOverride plugin to permit to override initialConfiguration tag in gzyarp::ControlBoard plugins

README.md

@@ -34,7 +34,20 @@ If you want to build `gz-sim-yarp-plugins` directly from source, you can check t
 
 ## Usage
 
-Once the plugins are available, 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.
+The plugins available in the repo are listed in the following.
+
+|  `name` |  `filename` |   Documentation | Example |
+|:-------------:|:-----------------:|:---------------------:|:---------------:|
+| `gzyarp::ControlBoard` | `gz-sim-yarp-controlboard-system` | [Missing. If you need it please open an issue.](https://github.com/robotology/gz-sim-yarp-plugins/issues/new)  | [`tutorial/single_pendulum`](./tutorial/single_pendulum) |
+| `gzyarp::ConfigurationOverride` | `gz-sim-yarp-configurationoverride-system` |  [`configurationoverride/README.md`](./configurationoverride/README.md) | [`tutorial/single_pendulum`](./tutorial/single_pendulum) |
+| `gzyarp::ForceTorque` | `gz-sim-yarp-forcetorque-system` | [Missing. If you need it please open an issue.](https://github.com/robotology/gz-sim-yarp-plugins/issues/new)  | [`tutorial/forcetorque`](./tutorial/forcetorque) |
+| `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::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.
 
 ### Migrating from Gazebo Classic and `gazebo-yarp-plugins`
 
@@ -61,7 +74,7 @@ For example, the configuration file for an IMU plugin can be specified as:
 </plugin>
 ```
 
-This means that the `imu.ini` file location is relative to the `ergoCub` model path, that is automatically found by Gazebo through the `GZ_SIM_SYSTEM_PLUGIN_PATH` environment variable:
+This means that the `imu.ini` file location is relative to the `ergoCub` model path, that is automatically found by Gazebo through the `GZ_SIM_RESOURCE_PATH` environment variable:
 
 ```
 models

docs/how-to-migrate-from-gazebo-classic.md

@@ -47,6 +47,8 @@ In the table below are listed the `gz-sim-yarp-plugins` currently available: for
 | IMU             | `libgazebo_yarp_imu.so`             | `gz-sim-yarp-imu-system`            | `gzyarp::Imu`                  |
 | Laser           | `libgazebo_yarp_lasersensor.so`     | `gz-sim-yarp-laser-system`          | `gzyarp::Laser`                |
 | Robot interface | `libgazebo_yarp_robotinterface.so`  | `gz-sim-yarp-robotinterface-system` | `gzyarp::RobotInterface`       |
+| Configuration Override | `libgazebo_yarp_configurationoverride.so`  | `gz-sim-yarp-configurationoverride-system` | `gzyarp::ConfigurationOverride`       |
+
 
 #### URDF
 
@@ -67,7 +69,7 @@ If your robot description is in a URDF file, then you should add the `gz-sim-yar
 
 From this example, just update the `name` and `filename` attributes of the `<plugin>` element with the ones found in the table above.
 
-> [!NOTE]  
+> [!NOTE]
 > The `<sensor>` elements already present in the model do not need any modification.
 
 #### SDF
@@ -93,7 +95,7 @@ One of the main differences of Modern Gazebo with respect to Gazebo Classic is i
 
 For example, in order to be able to use a specific type of sensor with the `<sensor>` tag, a `gz-sim` plugin should be added. This can be done just once in the world SDF file containing the models needed for a simulation, or it can be also added to each model file (UDRF or SDF). The latter approach can be useful in all situations in which the robot is part of a library that will be used by different users that could not be aware they have to include such plugins in their world files, like for example [ergoCub](https://github.com/icub-tech-iit/ergocub-software).
 
-> [!NOTE]  
+> [!NOTE]
 > Please note that the `gz-sim` plugins are different from `gz-sim-yarp-plugins` and both are needed to make the sensor information available from YARP devices.
 
 Below are listed some examples related to sensors available with `gz-sim-yarp-plugins`, see the [official documentation](https://gazebosim.org/docs/harmonic/sensors) to learn more.
@@ -118,6 +120,45 @@ If the plugin is added to a URDF robot file, it should placed under `<robot>` an
 <plugin filename="gz-sim-sensors-system" name="gz::sim::systems::Sensors"/>
 ```
 
+#### Configuration Override
+
+In `gazebo-yarp-plugins` with Gazebo Classic, the `libgazebo_yarp_configurationoverride.so` plugin can be used to override parameters in nested plugins, for example:
+
+~~~xml
+    <plugin name='configuration_override' filename='libgazebo_yarp_configurationoverride.so'>
+      <yarpPluginConfigurationOverride plugin_name='controlboard_plugin'/>
+      <initialConfiguration>0.785398</initialConfiguration>
+    </plugin>
+    <include>
+      <uri>model://single_pendulum/model.sdf</uri>
+    </include>
+~~~
+
+the equivalent syntax with `gz-sim-yarp-plugins` is:
+
+~~~xml
+    <!-- This is an example of how to override the initial configuration -->
+    <plugin name='gzyarp::ConfigurationOverride' filename='gz-sim-yarp-configurationoverride-system'>
+      <!-- The yarpPluginConfigurationOverride device is used to override the configuration of
+           a given yarp device, whose name is specified via the yarpDeviceName attribute -->
+      <yarpPluginConfigurationOverride yarpDeviceName='controlboard_plugin_device'/>
+
+      <!-- If you want to override the initialConfiguration xml element, set it -->
+      <initialConfiguration>0.785398</initialConfiguration>
+    </plugin>
+
+    <include>
+      <uri>model://single_pendulum/model.sdf</uri>
+    </include>
+~~~
+
+Beside the change of the `name` and `filename` attributes of the `plugin` element, the main difference is how we identify
+the plugin of which we want to override the parameters: in `gazebo-yarp-plugins` we use the `name` element of the overriden plugin,
+that may be unique even if you have multiple controlboards. In `gz-sim-yarp-plugins`, the `name` element of each controlboard plugin **must**
+be `gzyarp::ControlBoard`, so it will not be unique among different controlboards. For this reason, to identify the plugin to override we
+use instead the `yarpDeviceName`, that is unique for each plugin in the same model, and that needs already to be defined to expose each
+plugins with Network Wrapper Servers instantiated by the `gzyarp::RobotInterface` plugin.
+
 ## Update of plugins configurations
 
 `gz-sim-yarp-plugins` usually need a configuration, that can be specified in two ways:
@@ -128,7 +169,7 @@ If the plugin is added to a URDF robot file, it should placed under `<robot>` an
 
 See the [YARP documentation](https://www.yarp.it/latest/yarp_config_files.html) to learn the syntax needed for the configuration.
 
-> [!NOTE]  
+> [!NOTE]
 > The configuration parameters needed by `gz-sim-yarp-plugins` are slightly different from the ones needed by `gazebo-yarp-plugins`.
 > If you plan to use the same configuration file for both `gazebo-yarp-plugins` and `gz-sim-yarp-plugins`, you can just append the new parameters to the existing ones in the same file.
 

libraries/device-registry/DeviceRegistry.cc

@@ -50,7 +50,7 @@ DeviceRegistry* DeviceRegistry::getHandler()
 }
 
 bool DeviceRegistry::getDevicesAsPolyDriverList(const gz::sim::EntityComponentManager& ecm,
-                                                const std::string& modelScopedName,
+                                                const std::string& parentEntityScopedName,
                                                 yarp::dev::PolyDriverList& list,
                                                 std::vector<std::string>& deviceScopedNames) const
 {
@@ -74,10 +74,10 @@ bool DeviceRegistry::getDevicesAsPolyDriverList(const gz::sim::EntityComponentMa
 
         for (auto&& [key, value] : devicesMap)
         {
-            std::string deviceModelScopedName = getModelScopedName(key);
+            std::string deviceParentEntityScopedName = getParentEntityScopedName(key);
             std::string yarpDeviceName = getYarpDeviceName(key);
 
-            if (deviceModelScopedName == modelScopedName)
+            if (deviceParentEntityScopedName == parentEntityScopedName)
             {
                 list.push(value, yarpDeviceName.c_str());
                 deviceScopedNames.push_back(key);
@@ -144,12 +144,12 @@ bool DeviceRegistry::setDevice(const gz::sim::Entity& entity,
                             "with the one already registered!";
                 yError() << " This should not happen, check the names are correct in your "
                             "config file. "
-                            "The device already in the map has model scoped name: "
-                         << getModelScopedName(device_it->first)
+                            "The device already in the map has parent entity scoped name: "
+                         << getParentEntityScopedName(device_it->first)
                          << " and yarp device name: " << getYarpDeviceName(device_it->first)
                          << ". "
-                            "The device you are trying to insert has model scoped name: "
-                         << getModelScopedName(deviceDatabaseKey)
+                            "The device you are trying to insert has parent entity scoped name: "
+                         << getParentEntityScopedName(deviceDatabaseKey)
                          << " and yarp device name: " << getYarpDeviceName(deviceDatabaseKey)
                          << ". "
                             "Fatal error.";
@@ -289,11 +289,11 @@ std::string DeviceRegistry::getYarpDeviceName(const std::string& deviceDatabaseK
     return yarpDeviceName;
 }
 
-std::string DeviceRegistry::getModelScopedName(const std::string& deviceDatabaseKey)
+std::string DeviceRegistry::getParentEntityScopedName(const std::string& deviceDatabaseKey)
 {
-    // Extract modelScopedName from deviceDatabaseKey
-    std::string modelScopedName = deviceDatabaseKey.substr(0, deviceDatabaseKey.find_last_of("/"));
-    return modelScopedName;
+    // Extract parentEntityScopedName from deviceDatabaseKey
+    std::string parentEntityScopedName = deviceDatabaseKey.substr(0, deviceDatabaseKey.find_last_of("/"));
+    return parentEntityScopedName;
 }
 
 DeviceRegistry::DeviceRegistry()
@@ -347,4 +347,55 @@ void DeviceRegistry::incrementNrOfGzSimYARPPluginsNotSuccessfullyLoaded(const gz
     }
 }
 
+bool DeviceRegistry::addConfigurationOverrideForYARPDevice(const gz::sim::EntityComponentManager& ecm,
+                                           const std::string& parentEntityScopedNameWhereConfigurationOverrideWasInserted,
+                                           const std::string& yarpDeviceName,
+                                           const std::string& configurationOverrideInstanceId,
+                                           std::unordered_map<std::string, std::string> overridenParameters)
+{
+    m_yarpDevicesOverridenParametersList.push_back({getGzInstanceId(ecm),
+                                                    parentEntityScopedNameWhereConfigurationOverrideWasInserted,
+                                                    yarpDeviceName,
+                                                    configurationOverrideInstanceId,
+                                                    overridenParameters});
+    return true;
+}
+
+bool DeviceRegistry::removeConfigurationOverrideForYARPDevice(const std::string& configurationOverrideInstanceId)
+{
+    m_yarpDevicesOverridenParametersList.erase(
+        std::remove_if(
+            m_yarpDevicesOverridenParametersList.begin(),
+            m_yarpDevicesOverridenParametersList.end(),
+            [&configurationOverrideInstanceId](const ConfigurationOverrideParameters& params) {
+                return params.configurationOverrideInstanceId == configurationOverrideInstanceId;
+            }),
+        m_yarpDevicesOverridenParametersList.end());
+    return true;
+}
+
+bool DeviceRegistry::getConfigurationOverrideForYARPDevice(const gz::sim::EntityComponentManager& ecm,
+                                           const std::string& parentEntityScopedNameWhereYARPDeviceWasInserted,
+                                           const std::string& yarpDeviceName,
+                                           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 matchin
+    // 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_yarpDevicesOverridenParametersList) {
+        if (params.gzInstanceId == getGzInstanceId(ecm) &&
+            parentEntityScopedNameWhereYARPDeviceWasInserted.rfind(params.parentEntityScopedNameWhereConfigurationOverrideWasInserted) == 0 &&
+            params.yarpDeviceName == yarpDeviceName) {
+            std::unordered_map<std::string, std::string> consideredOverridenParameters = params.overridenParameters;
+            overridenParameters.merge(consideredOverridenParameters);
+        }
+    }
+
+    return true;
+}
 } // namespace gzyarp

libraries/device-registry/DeviceRegistry.hh

@@ -109,32 +109,61 @@ public:
      */
     void incrementNrOfGzSimYARPPluginsNotSuccessfullyLoaded(const gz::sim::EntityComponentManager& ecm);
 
-private:
+    /**
+     * Add a configuration override for a given yarp device.
+     *
+     * This function is meant to be called only by the gzyarp::ConfigurationOverride plugin.
+     *
+     */
+    bool addConfigurationOverrideForYARPDevice(const gz::sim::EntityComponentManager& ecm,
+                                               const std::string& parentEntityScopedNameWhereConfigurationOverrideWasInserted,
+                                               const std::string& yarpDeviceName,
+                                               const std::string& configurationOverrideInstanceId,
+                                               std::unordered_map<std::string, std::string> overridenParameters);
+
+    /**
+     * Remove a configuration override for a given yarp device.
+     *
+     */
+    bool removeConfigurationOverrideForYARPDevice(const std::string& configurationOverrideInstanceId);
+
+    /**
+     * Get the configuration override for a given yarp device.
+     *
+     * This function is meant to be called by each plugin to override its configuration.
+     *
+     */
+    bool getConfigurationOverrideForYARPDevice(const gz::sim::EntityComponentManager& ecm,
+                                               const std::string& parentEntityScopedNameWhereYARPDeviceWasInserted,
+                                               const std::string& yarpDeviceName,
+                                               std::unordered_map<std::string, std::string>& overridenParameters) const;
+
     /**
      * Generate a unique device id for a given yarp device name and entity.
      *
      * The device id is a process-unique string that identifies a device in a given gz server.
      * Inside the process, a device is uniquely identified by the pair of the gz instance id and the device id.
      */
-    static std::string generateDeviceId(const gz::sim::Entity& entity,
-                                        const gz::sim::EntityComponentManager& ecm,
-                                        const std::string& yarpDeviceName);
+     static std::string generateDeviceId(const gz::sim::Entity& parentEntity,
+        const gz::sim::EntityComponentManager& ecm,
+        const std::string& yarpDeviceName);
 
     /**
      * Generate a unique instance id for a given gz server.
      */
     static std::string getGzInstanceId(const gz::sim::EntityComponentManager& ecm);
 
     /**
-     * Extract the yarpDeviceName from a device id, i.e. the third argument passed to generateDeviceId.
-     */
+    * Extract the yarpDeviceName from a device id, i.e. the third argument passed to generateDeviceId.
+    */
     static std::string getYarpDeviceName(const std::string& deviceId);
 
     /**
-     * Extract the scoped model name from a device id, i.e. the scoped name of the parent model of the plugin that created the device.
-     */
-    static std::string getModelScopedName(const std::string& deviceId);
+    * Extract the scoped name of the parent entity from a device id, i.e. the scoped name of the parent entity of the plugin that created the device.
+    */
+    static std::string getParentEntityScopedName(const std::string& deviceId);
 
+private:
     // Private constructor. The constructor is private to ensure that the class is a singleton.
     DeviceRegistry();
 
@@ -152,6 +181,33 @@ 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;
+
+
+    // Element that stores the parameters that will be overriden for a given yarp device
+    // A device will be affected by the override if:
+    // - gzInstanceId match
+    // - yarpDeviceName 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,
+        // 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
+        // Some keys have a special meaning:
+        // gzyarp-xml-element-initialConfiguration: override the content of a <initialConfiguration> tag
+        std::unordered_map<std::string, std::string> overridenParameters;
+    };
+
+    // 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;
 };
 
 } // namespace gzyarp

plugins/CMakeLists.txt

@@ -6,3 +6,4 @@ add_subdirectory(imu)
 add_subdirectory(basestate)
 add_subdirectory(clock)
 add_subdirectory(controlboard)
+add_subdirectory(configurationoverride)