Use SensorDataQoS() when subscribing to joint_states

[smith-doug]

Description

It looks like the joint_states topic can/will be published as rclcpp::SensorDataQoS() in Humble. The current_state_monitor does not use SensorDataQoS(), and will not be able to subscribe to joint_states if it's published as sensor data. I am working with a new in-development robot driver that publishes joint_states as sensor data.

More info: https://answers.ros.org/question/399325/qos-settings-for-joint_states-in-humble/

I don't know if the correct behavior is already defined in a REP somewhere. I would recommend defaulting to sensor data and making it configurable, like in robot_state_publisher.

Your environment

• ROS Distro: galactic
• OS Version: Ubuntu 20.04
• Binary and Source (galactic and main)

Steps to reproduce

Publish joint_states with a SensorDataQoS publisher and try to get the robot's current state with the planning_scene_monitor/current_state_monitor.

Expected behaviour

joint_states should be received and current_state_monitor should work.

Actual behaviour

joint_states is never received and the planning_scene_monitor times out.

[tylerjw]

To respond to this with an extra data point. All of the robots we are currently using with MoveIt2 use the joint_state_broadcaster from ros2_controllers which uses SystemDefaultsQoS

Here is the line where it creates the publisher: https://github.com/ros-controls/ros2_controllers/blob/7acf29c191ea44870c7327436a4579e06261fde9/joint_state_broadcaster/src/joint_state_broadcaster.cpp#L156

I don't really understand why you would want to use one or the other or what robot_state_publisher is and why you would use it over ros2_control.

What are the implications of SensorDataQoS vs SystemDefaultQoS? Is there a reason one is more appropriate for joint_states topics than the other? If so we would need to coordinate also changing ros2_control's use of this topic.

[gavanderhoorn]

I don't really understand [..] what robot_state_publisher is and why you would use it over ros2_control.

RSP performs FK based on incoming JointState messages and the URDF, to produce the relevant TF messages on /tf and /tf_static.

ros2_control does not have anything equivalent.

[nbbrooks]

Thanks for bringing this up, Doug!

I made a lightning talk on my DDS/QOS learnings for the February MoveIt Community meeting. You can see the full recording here.

Here is a table with some ideas I've had around out of box QOS profiles that could be helpful. Items in green are proposed new QOS (I renamed "sensor data" to "low frequency data").

In practice, I think it is common to use "best effort" for high frequency data, such as joint states. I have not had time to benchmark anything on hardware though so I do not know if there is a substantial impact on performance changing to it, nor if this interesting edge case is encountered.

[nbbrooks]

It looks like the joint_states topic can/will be published as rclcpp::SensorDataQoS() in Humble

Could you provide a link to this? I've only seen this RSP PR, which just changed its subscription to use SensorData by default.

If the official recommendation in Humble is to publish SensorData QOS for joint states, we should also be coordinate with ros2_controller so they publish accordingly as well. Right now its joint state publisher (which is largely what MoveIt subscribes to) uses system default as well.

[smith-doug]

I updated my post on ROS Answers with the feedback I got during a call with the ROS2 TSC.

I interpreted them changing the subscription type in a ros_base package to mean that "publishers and subscribers should use best_effort reliability for joint_states". I was also working with a new experimental robot driver that published joint_states as best_effort.

In reality, their decision was more like "Let's make robot_state_publisher's subscriber use best_effort, since that can receive reliable and best_effort messages, and make it reconfigurable." There isn't any official guideline for publishers of this topic. I do think that their use of best_effort subscriber in a ros_base package is a fair enough guideline to do the same.

I'm not an authority on the subject, but I don't think you have to change the publisher in ros2_controller. It would be nice if it was configurable. The burden falls on the subscriber to make sure it has a compatible reliability.

A best_effort joint_states subscriber in moveit2 should be able to receive both reliable and best_effort messages fine. Making it configurable like RSP did would satisfy people that are worried about only receiving from reliable publishers.

[nbbrooks]

Understood, so this is about future proofing CSM by making it subscribe using "Best Effort", since Best Effort subscribers can receive from both "Best Effort" and "Reliable" publishers. Whether joint state publishers should switch from Reliable to Best Effort is a separate topic (but we should at least make sure they are configurable from the launch files).

[AndyZe]

It looks like the joint_states topic can/will be published as rclcpp::SensorDataQoS() in Humble. The current_state_monitor does not use SensorDataQoS(), and will not be able to subscribe to joint_states if it's published as sensor data.

I don't believe that's true. I believe SensorDataQoS and SystemDefaultQoS are compatible, per the table here. Either way, I support this idea.