Publishing iCub joints on ROS TF

[cedricgoubard]

Hello,
I am trying to use ROS TF to keep track of iCub, iKart and other sensors' sets of coordinates. So far, I managed to publish the robot state using several URDF files (the official 2_5 with visuomanip, a former version and a custom one for our iCubLondon01 that was developed by previous lab members).

The only thing I am now missing is the joints state. I found a repository containing some C++ code to manually publish those, but it was designed with an older URDF, so I would have to manually review all the joints to make it work.

On the other hand I found this PR and this file which lead me to believe that the joints publication may already be available.

Do you have any advice on how to proceed?

Thank you for your help!

[traversaro]

Hi @cedricgoubard,
the piece of software that can be used to publish the joint position information exposed by a YARP device for YARP <= 3.4 (so for any robotology distro <= 2021.08) is the controlboardwrapper2 (see http://www.yarp.it/v3.4/classControlBoardWrapper.html#details). This is exactly the same device used to publish joint information on YARP ports, but that can be configured also to publish on ROS topics via the ROS group. For an example of configuration file configured for this goal, see https://github.com/robotology/robots-configuration/blob/v1.21.0/iCubLondon01/wrappers/motorControl/icub_ros_wrapper.xml . See also robotology/robots-configuration#117 for a related issue.

However, notice that controlboardwrapper2 device is deprecated in the new YARP 3.5 relese (that will be use in robotology distros from >= 2021.11), and its functionality is going to be replaced by middleware-specific devices. In particular controlBoard_nws_yarp for YARP , controlBoard_nws_ros for ROS(1) and controlBoard_nws_ros2 for ROS2 (still unreleased work in progress). See http://www.yarp.it/latest/nws_and_nwc_architecture.html and robotology/yarp#2441 for architecture details on the changes in YARP 3.5 .

[cedricgoubard]

See this reply below for more details

[cedricgoubard]

Hi @traversaro,

Thank you for answering so quickly! I'll start by using controlboardwrapper2 for now, and move on to controlBoard_nws_ros when we update our system.

I tried using controlboardwrapper2, but I don't think I am doing it right:

➜  robotology-superbuild git:(master) cd build/install/share/ICUBcontrib/robots/iCubLondon01/wrappers/motorControl 
➜  motorControl git:(master) ls
head_mc_wrapper.xml   left_arm_mc_wrapper.xml  right_arm_mc_wrapper.xml  torso_mc_wrapper.xml
icub_ros_wrapper.xml  left_leg_mc_wrapper.xml  right_leg_mc_wrapper.xml
➜  motorControl git:(master) yarpdev --device controlboardwrapper2 --from icub_ros_wrapper.xml  --subdevice icub_all_joints_mc_wrapper 
[ERROR] |yarp.devices.ControlBoardWrapper| ************************************************************************************
* controlboardwrapper2 missing mandatory parameter 'name' for port name, usage is: *
*     name:    full port prefix name with leading '/',  e.g.  /robotName/part/     *
************************************************************************************
[ERROR] |yarp.devices.ControlBoardWrapper| 'portName' was not correctly set, check you r configuration file
[ERROR] |yarp.dev.PolyDriver| Driver <controlboardwrapper2> was found but could not open
[DEBUG] |yarp.dev.Drivers| ===============================================================
[DEBUG] |yarp.dev.Drivers| == Options checked by device:
[DEBUG] |yarp.dev.Drivers| ==
[DEBUG] |yarp.dev.Drivers| device=controlboardwrapper2
[DEBUG] |yarp.dev.Drivers| ==
[DEBUG] |yarp.dev.Drivers| ===============================================================
[ERROR] |yarp.dev.Drivers| yarpdev: ***ERROR*** device not available.
[INFO] |yarp.dev.Drivers| Suggestions:
[INFO] |yarp.dev.Drivers| + Do "yarpdev --list" to see list of supported devices.

From what I understand, the xml file is not used for some reason.

Here is the output from yarpdev --list (which does not include the icub_all_joints_mc_wrapper device declared in the xml file):

➜  motorControl git:(master) yarpdev --list | grep controlboardwrapper2
[WARNING] |yarp.dev.Drivers| Wrong library name for plugin readonlyremotecontrolboard
[INFO] |yarp.dev.Drivers| Device "controlboardremapper", available on request (found in /home/cedric/icub/robotology-superbuild/build/install/lib/yarp/yarp_controlboardremapper.so library), wrapped by "controlboardwrapper2".
[INFO] |yarp.dev.Drivers| Device "controlboardwrapper2", available on request (found in /home/cedric/icub/robotology-superbuild/build/install/lib/yarp/yarp_controlboardwrapper2.so library), is a network wrapper.
[INFO] |yarp.dev.Drivers| Device "fakeMotionControl", available on request (found in /home/cedric/icub/robotology-superbuild/build/install/lib/yarp/yarp_fakeMotionControl.so library), wrapped by "controlboardwrapper2".
[INFO] |yarp.dev.Drivers| Device "fakeMotor", available on request (found in /home/cedric/icub/robotology-superbuild/build/install/lib/yarp/yarp_fakeMotor.so library), wrapped by "controlboardwrapper2".
[INFO] |yarp.dev.Drivers| Device "remote_controlboard", available on request (found in /home/cedric/icub/robotology-superbuild/build/install/lib/yarp/yarp_remote_controlboard.so library), wrapped by "controlboardwrapper2".
[INFO] |yarp.dev.Drivers| Device "remotecontrolboardremapper", available on request (found in /home/cedric/icub/robotology-superbuild/build/install/lib/yarp/yarp_controlboardremapper.so library), wrapped by "controlboardwrapper2".
[INFO] |yarp.dev.Drivers| Device "test_motor", available on request (found in /home/cedric/icub/robotology-superbuild/build/install/lib/yarp/yarp_fakeMotor.so library), wrapped by "controlboardwrapper2".

Do you have any suggestions?

[cedricgoubard]

Hi @traversaro,
Adding the line you mentioned to our icub_ikart.xml file (referenced by yarprobotinterface.ini) solved the problem! I have not checked whether all the joints are properly published, but I think I can mark your reply as answer.

It may be interesting to add your explanation somewhere in the documentation? Maybe:

• here; even though this is a YARP page and the wrapper is specific for iCub, it is the first place I found
• or create a new page in iCub's documentation

For now, the first result for iCub ROS is this page, which is probably not up to date.

[traversaro]

I think that indeed the best place for this documentation may be iCub documentation at https://github.com/icub-tech-iit/documentation (that is rendered at https://icub-tech-iit.github.io/documentation/), perhaps under the iCub SW section. What do you think @pattacini ?

If you like @cedricgoubard you can also go forward and port the content of the answer in a documentation page yourself, and open a PR to https://github.com/icub-tech-iit/documentation . Once the documentation is in place, we can add a warning header and a redirect to it from http://wiki.icub.org/wiki/UPMC_iCub_project, if that pages pops out if you search for "iCub ROS" on google.

[cedricgoubard]

I'd be happy to do the PR! I'll just wait for the confirmation from @pattacini.

[pattacini]

Hi @traversaro, @cedricgoubard

It's a good idea 👍🏻
I didn't follow all the discussion, so my mere concern is that the new page should also contain some backdrop and give some general context/info on the publishing of iCub Joints to TF and not only the way to solve the specific issue.

If this is the intention, green light from me 👍🏻

[cedricgoubard]

A first version of the PR is ready here: icub-tech-iit/documentation#173, let me know what you think 😄