Add gazebo_yarp_robotinterface plugin

[traversaro]

This PR implements a gazebo_yarp_robotinterface plugin as it was originally described in #391 . You can find an explanation of the capability of this plugin in the documentation added in https://github.com/robotology/gazebo-yarp-plugins/blob/2a4bdf123d1c87b84a9f79cf2978f61bc100503c/plugins/robotinterface/README.md .

This plugin can be helpful in the following use cases:

• For the iCub robot, spawn devices such as wholebodydynamics or cartesiancontrollerserver that on the real robot are normally spawned with the yarprobotinterface that also creates the devices that communicate with the internal fieldbus of the robot directly in the Gazebo simulation, without the need of running external yarprobotinterface instances as done in https://github.com/robotology/icub-gazebo-grasping-sandbox/blob/89ec286ced316b5a5bc63dc4eb702bd2fbe7e613/scripts/icub-grasp.xml#L18 or documented in https://github.com/robotology/whole-body-controllers/blame/v2.5/doc/How-to-run-controllers-with-gazebo-simulator.md#L21 (Provide simulated iCub models that also spawn wholebodydynamics and cartesiancontrollerserver devices icub-models#84)
• Attach new network wrappers for ROS or ROS2 to simulate model and sensors, without the need to manually modify the source code of each YARP Gazebo plugin.
• For the iCub robot, permit to cleanup the simulation of the distributed inertial sensors (see Cleanup distributed inertial sensors implementations #312 and Add the possibility to use multipleanalogsensorsclient and multipleanalogsensorsremapper also in Gazebo icub-models#28)

While the implementation of the plugin is complete and contained in this PR, for fully exploit this plugin it may be necessary to do further modification to other plugins as described in #534 and #535 . However, this improvements can be done in future PRs, as they are independent from the implementation of the gazebo_yarp_robotinterface .

If you want to test this branch with yarp-3.5, you can check the branch https://github.com/robotology/gazebo-yarp-plugins/tree/add-robotinterface-yarp-3.5 .

[randaz81]

I have the following question: what is the purpose of this piece of code, extracted from LaserSensor.cc ?

gazebo-yarp-plugins/plugins/lasersensor/src/LaserSensor.cc

Lines 101 to 112 in 6936859

	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))
	{
	yError()<<"GazeboYarpLaserSensor: failed setting deviceId(=" << deviceId << ")";
	return;
	}

I was able to attach the robotinterface wrapper by using the name specified by this deviceId parameter.
But then I noticed that this is the only plugin the implements such a parameter. For example, the depthCamera plugin does not perform any getHandler()->setDevice(). Thus, if my interpretation is correct and this is the correct way to perform the attach, then the other plugins such as depthCamera are incomplete and need to implement the same piece of code. What do you think?

[randaz81]

Additionally, in file ControlBoard.cc, I found the following:

GazeboYarpPlugins::Handler::getHandler()->setDevice(scopedDeviceName, newPoly.poly);

The variable scopedDeviceName seems to answer to your point:

Cleanup handling of devices names. At the moment they are prefixed with the Gazebo Model name, and that is not ideal if we want to re-use XML files in both the gazebo_yarp_robotinterface plugin, and an externally instantiated yarprobotinterface .

So it seems that in the controlboard plugin, the device name is not taken from an external parameter, but it is generated internally.

[traversaro]

I have the following question: what is the purpose of this piece of code, extracted from LaserSensor.cc ?

This was implemented in #419, I guess to support a similar user case of device created in one gazebo plugin and attached to a network server wrapper created in another gazebo plugin.

[traversaro]

However, I thought a bit about the naming aspect and I think a good solution could be:

• All Gazebo plugins take in input a variable yarpDeviceName that specifies the string that will be used then to attach to that device in the robotinterface XML file. The name of this parameter is chosen to mirror the similarly named element in the robotinterface XML elements, for example <device name="additional_controlboard_nws" type="controlboardwrapper2">. Some plugins will have this parameter or have a special case parameter (yarpDeviceNames as a list?) if they can instantiate multiple yarp devices from a single gazebo plugin. Plugins that currently do not register their device in the singleton need to be modified to read from yarpDeviceName and register their device.
• To avoid collisions, the device name are registered in the singleton with a scopedName that is constructed by concatenating the Gazebo model scoped name and the yarpDeviceName (see as done in

  gazebo-yarp-plugins/plugins/controlboard/src/ControlBoard.cc

  Line 53 in 6936859

  std::string scopedDeviceName = m_robotName + "::" + m_controlBoards[n]->key.c_str();

  ).
• As we want to pass the device name without any scope in the attach parameters, i.e. in :

      <action phase="startup" level="5" type="attach">
        <paramlist name="networks">
          <elem name="key_of_the_list">  yarp_device_name_used_to_found_the_device_in_the_list_of_devices </elem>
        </paramlist>
      </action>

the Handler::getDevicesAsPolyDriverList method (see https://github.com/robotology/gazebo-yarp-plugins/pull/532/files#diff-51f84011401d52ad0a51e43aa1a8d5b1d32ced0fdbb644993860b70444b3de4dR231) needs to be modified to only return only the device spawned by the model that it contains the robotinterface plugin and its submodels, and to remove the model-specific prefix to just obtain the yarp device name. If there is any naming conflict (i.e. two devices that have the same unscoped name) will result in an error.

The last point probably prevents some corner case uses (such as the use of robotinterface plugin in the parent model that contains two iCub models, for example) but until we have a concrete use case I would focus on implementing our main use case (i.e. a single iCub that runs custom devices in its "yarprobotinterface") easily and in a clean way.

[traversaro]

cc @PeterBowman I do not know if you may be interested in this (as you recently started using the gazebo-yarp-plugins and I think you are also implementing a motor interface for a robot using the yarprobotinterface), if you are and you like feel free to provide some feedback!

[PeterBowman]

Thank you, @traversaro! That is one interesting tool we once decided to set aside due to lack of documentation among others (empty doxy, roboticslab-uc3m/yarp-devices#237), but it's still on our radar now that we are exploring further integration with your codebase.

[traversaro]

Cleanup handling of devices names. At the moment they are prefixed with the Gazebo Model name, and that is not ideal if we want to re-use XML files in both the gazebo_yarp_robotinterface plugin, and an externally instantiated yarprobotinterface .

This point was fixed by c30b6ab . @randaz81 I also incorporated your other commits, and I prepare a yarp 3.5-compatible version of the branch in https://github.com/robotology/gazebo-yarp-plugins/tree/add-robotinterface-yarp-3.5 .

[traversaro]

The PR is now ready for review, the tests were failing due to fact that the device usage needs to be manually tracked as the plugins in Gazebo are not destructed/unloaded in reverse order, and that was fixed in 2a4bdf1 .

[traversaro]

As you can see by the linked issues, this plugin is an important piece of software that could be used to enable several convenient workflows. For this reason, I will tag a few people that may be affected by this or one of the related issues, and even if I do not list them as a reviewer, feel free to review the PR if you like: @drdanz @randaz81 @gabrielenava @Giulero @lrapetti @HosameldinMohamed @S-Dafarra @GiulioRomualdi @prashanthr05 @Nicogene @elandini84 @diegoferigo @nunoguedelha @Yeshasvitvs .

[traversaro]

Great, now that I fixed the newly added test, the already existing ControlBoardControlTest started failing, probably some timing-related bug that was already present.

[traversaro]

Great, now that I fixed the newly added test, the already existing ControlBoardControlTest started failing, probably some timing-related bug that was already present.

The test failures seems flaky and unrelated (see #536). Suppressing the test for now.

[prashanthr05]

Minor comments mainly regarding debug prints and documentation plus,

• Should we address the commented stray lines in DoubleLaser and LaserSensor source files?

[prashanthr05]

new line ;D

[traversaro]

I should have addressed all the comments @prashanthr05 .

[prashanthr05]

Thank you @traversaro. I have not personally tested the plugin, but overall it looks good to me.