Develop ros rviz plugin for contact force visualization

[yeshasvitirupachuri]

Following the idea in https://github.com/dic-iit/lab-events-demos/issues/102#issuecomment-493367239 it is ideal to have a way to visualize the contact forces estimated by wholeBodyDynamics in rviz rather than using the legacy iCubGui tool.

• One way to achieve this is to make use of is http://wiki.ros.org/rviz_plugin_tutorials but I notice that the support is maintained till ros Fuerte http://docs.ros.org/fuerte/api/rviz_plugin_tutorials/html/ which is a bit outdated with the current versions of ROS.
• Alternatively, we can write a device or module that subscribes to /wholeBodyDynamics/dynContacts:o and publish wrench information through analog server similar to the way it is done in HDE https://github.com/robotology/human-dynamics-estimation/blob/master/conf/xml/Human.xml#L214

@traversaro

[traversaro]

• Alternatively, we can write a device or module that subscribes to /wholeBodyDynamics/dynContacts:o and publish wrench information through analog server similar to the way it is done in HDE https://github.com/robotology/human-dynamics-estimation/blob/master/conf/xml/Human.xml#L214

Note that in general this would required a RViz's Wrench plugin for each link of the model, including upper_leg, right_leg, etc etc.

[yeshasvitirupachuri]

this would required a RViz's Wrench plugin for each link of the model

I do not understand what you mean by rviz wrench plugin. I am not wrong, we can visualize wrench messages in rviz by connecting to the correct ros topics that carry the wrench information. The second method opens the ros topics and publishes the wrench information on them.

[traversaro]

I am not wrong, we can visualize wrench messages in rviz by connecting to the correct ros topics that carry the wrench information.

How do you specify the "correct ros topics" in RViz? By adding to the RViz a Wrench DisplayType (see http://wiki.ros.org/rviz/DisplayTypes/Wrench, that is an RViz plugin, if I get everything correctly) for each link. That is non-trivial amount of manual configuration necessary, especially if you need to set it up for a new robot.

[yeshasvitirupachuri]

That is non-trivial amount of manual configuration necessary

Yes but this is something we can do once and save it as a rviz configuration file http://wiki.ros.org/rviz/UserGuide#Configurations. That is what I mean by having the ros rviz visualization file along with the device or module.

How do you specify the "correct ros topics" in RViz?

We set the ros topics of all the links prior and save it in the rviz configuration file. The device or module will publish the ros topics relevant to the links that are in contact, which we can specify through the device or module configuration file. This is very similar to the case of visualizing the joint torque as temp messages in the case of HDE. The rviz configuration file is already set correctly to the ros topics corresponding to the each joint and HumanDynamicsPublisher publishers the ros topics https://github.com/robotology/human-dynamics-estimation/blob/master/conf/xml/Human.xml#L280

[traversaro]

Yes but this is something we can do once and save it as a rviz configuration file http://wiki.ros.org/rviz/UserGuide#Configurations. That is what I mean by having the ros rviz visualization file along with the device or module.

Yes, and that seems to be quite cumbersome and non-scalable to me. Writing repeated piece of information is exactly something that I prefer computers to do for me, to be honest. :D
Writing an RViz plugin can be extremely compact (see the Wrench display itself, that is less then 200 lines of code: https://github.com/ros-visualization/rviz/blob/070835c426b8982e304b38eb4a9c6eb221155d5f/src/rviz/default_plugin/wrench_display.cpp), and will have the advantage that the only two pieces of configuration necessary would be the YARP port name, and the robot model.

[traversaro]

One way to achieve this is to make use of is http://wiki.ros.org/rviz_plugin_tutorials but I notice that the support is maintained till ros Fuerte http://docs.ros.org/fuerte/api/rviz_plugin_tutorials/html/ which is a bit outdated with the current versions of ROS.

I am not sure what you mean by "support is maintained till ros Fuerte". The repo https://github.com/ros-visualization/visualization_tutorials is updated for Kinetic and Melodic, and contains also recent patches related to the ROSOnWindows project. Furthermore, the plugin interface (with some changes) is supported on ROS2's RViz as well (see https://github.com/ros2/rviz/blob/ros2/docs/plugin_development.md).

[yeshasvitirupachuri]

Writing an RViz plugin can be extremely compact

I agree with it but looking at the file https://github.com/ros-visualization/rviz/blob/070835c426b8982e304b38eb4a9c6eb221155d5f/src/rviz/default_plugin/wrench_display.cpp it has the same content of what rviz configuration file will contain.

will have the advantage that the only two pieces of configuration necessary would be the YARP port name, and the robot model.

I agree with this but what I am trying to say is that the explicit rviz plugin may not be necessary. If I understood it correctly, in the example of wrench display, processMessage() is the main method that will process the wrench stamped ros messages coming through a topic.

[yeshasvitirupachuri]

At least for the tool that will replace the legacy icubGui for estimated contact wrench visualization all we need to be do is to provide a ros topic that will publish the wrench messages. The rviz plugins like are wrench display and other plugins are simply used on the rviz side to subscribe to these topics and visualize the information present in them. Instead of launching rviz first, adding a robot model and then adding the wrench pluging to different links, we can simply create a rviz configuration file once and launch rviz using this configuration file.

[traversaro]

I agree with it but looking at the file https://github.com/ros-visualization/rviz/blob/070835c426b8982e304b38eb4a9c6eb221155d5f/src/rviz/default_plugin/wrench_display.cpp it has the same content of what rviz configuration file will contain.

Not really, the RViz plugin will be able to load the link informations from the URDF mode, so there will be no need to manually specify all the links to consider. Furthermore, consider that the RViz plugin will also contain the logic that otherwise you will need to put in the YARP's dynContactList --> ROS's Wrench topics bridge.

I agree with this but what I am trying to say is that the explicit rviz plugin may not be necessary.

I understand the two options mentioned in the first topic (either stock RViz + YARP <--> ROS bridge or RViz plugin). I simple believe that the RViz plugin is more maintainable, less error prone and much less computationally demanding (opening 25 TCP sockets just for visualizing the external forces of the iCub seems to be a bit of an overkill).

[yeshasvitirupachuri]

I simple believe that the RViz plugin is more maintainable, less error prone and much less computationally demanding

Ok that makes sense

[traversaro]

I just realized that there two additional feature supported by the dynContactList published by wholeBodyDynamics, and that are currently correctly handled in iCubGui, that will not be trivial to realize with the existing RViz/ROS Wrench DisplayType + custom bridge, i.e.:

• In a dynContactList, the origin/application/contact point of the force can change continuously, while using the ROS Wrench display type it is constrained to be in the frame origin
• In a dynContactList, it is possible that on a given link there are multiple external forces/torques, that would required multiple Wrench DisplayType on the same link.

See this video https://youtu.be/yQGXYGS0Ojo?t=67 for an example of both features.

[prashanthr05]

This is an interesting tool http://wiki.ros.org/xpp