Ensure all arrays in the JointState command message have the same size by srmainwaring · Pull Request #101 · ros-controls/topic_based_hardware_interfaces

srmainwaring · 2026-03-02T21:41:21Z

JointState messages require the position, velocity and effort arrays are all the same size or empty in order to uniquely associate each joint name with the correct state. The current behaviour is to only push_back commands on the supported interfaces which can result in position, velocity and effort arrays of different sizes. This prevents the joint name from being uniquely associated with its corresponding command.

Example

This example is adapted from the ackermann_drive_example in gz_ros2_control_demos and uses the following control block.

  <ros2_control name="ackermann_joint_state_system" type="system">
    <hardware>
      <plugin>joint_state_topic_hardware_interface/JointStateTopicSystem</plugin>
      <param name="joint_commands_topic">/robot_joint_commands</param>
      <param name="joint_states_topic">/robot_joint_states</param>
    </hardware>
    <joint name="rear_left_wheel_joint">
      <command_interface name="velocity" />
      <state_interface name="velocity" />
      <state_interface name="position" />
      <state_interface name="effort" />
    </joint>
    <joint name="rear_right_wheel_joint">
      <command_interface name="velocity" />
      <state_interface name="position" />
      <state_interface name="velocity" />
      <state_interface name="effort" />
    </joint>
    <joint name="left_wheel_steering_joint">
      <command_interface name="position" />
      <state_interface name="position" />
      <state_interface name="velocity" />
      <state_interface name="effort" />
    </joint>
    <joint name="right_wheel_steering_joint">
      <command_interface name="position" />
      <state_interface name="position" />
      <state_interface name="velocity" />
      <state_interface name="effort" />
    </joint>
    <joint name="front_left_wheel_joint">
      <state_interface name="position" />
      <state_interface name="velocity" />
      <state_interface name="effort" />
    </joint>
    <joint name="front_right_wheel_joint">
      <state_interface name="position" />
      <state_interface name="velocity" />
      <state_interface name="effort" />
    </joint>
  </ros2_control>

Publish a twist to /ackermann_steering_controller/reference and inspect the output from the ackermann controller:

Before

ros2 topic echo -f /robot_joint_commands --once
header:
  stamp:
    sec: 31
    nanosec: 778000000
  frame_id: ''
name:
- rear_left_wheel_joint
- rear_right_wheel_joint
- left_wheel_steering_joint
- right_wheel_steering_joint
- front_left_wheel_joint
- front_right_wheel_joint
position:
- 0.0
- 0.0
velocity:
- 3.3330395022246466
- 3.3336271644420203
effort: []
---

After

ros2 topic echo -f /robot_joint_commands --once
header:
  stamp:
    sec: 1928
    nanosec: 134000000
  frame_id: ''
name:
- rear_left_wheel_joint
- rear_right_wheel_joint
- left_wheel_steering_joint
- right_wheel_steering_joint
- front_left_wheel_joint
- front_right_wheel_joint
position:
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
velocity:
- 3.3332697821011488
- 3.333396884565518
- 0.0
- 0.0
- 0.0
- 0.0
effort:
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
---

It may be better to initialise the command arrays with NaN to allow consumers of the message to differentiate between a command with value zero from one that is unset. The JointState message notes do not provide guidance on this.

The JointState messages requires the position, velocity and effort arrays are all the same size or empty in order to uniquely associate each joint name with the correct state. Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com>

saikishor · 2026-03-03T06:25:44Z

joint_state_topic_hardware_interface/src/joint_state_topic_hardware_interface.cpp

+  joint_state.name.resize(joints.size());
+  joint_state.position.resize(joints.size(), 0.0);
+  joint_state.velocity.resize(joints.size(), 0.0);
+  joint_state.effort.resize(joints.size(), 0.0);


Suggested change

joint_state.effort.resize(joints.size(), 0.0);

joint_state.effort.resize(joints.size(), std::numerical_limits::quiet_NaN());

Changed in f52a21c

saikishor · 2026-03-03T06:26:37Z

joint_state_topic_hardware_interface/src/joint_state_topic_hardware_interface.cpp

+  // https://docs.ros.org/en/api/sensor_msgs/html/msg/JointState.html
+  joint_state.name.resize(joints.size());
+  joint_state.position.resize(joints.size(), 0.0);
+  joint_state.velocity.resize(joints.size(), 0.0);


I'm not sure if we should set zeros here as default

Changed to NaN. Example after change now outputs:

ros2 topic echo -f /robot_joint_commands --once header: stamp: sec: 31 nanosec: 833000000 frame_id: '' name: - rear_left_wheel_joint - rear_right_wheel_joint - left_wheel_steering_joint - right_wheel_steering_joint - front_left_wheel_joint - front_right_wheel_joint position: - .nan - .nan - 0.0 - 0.0 - .nan - .nan velocity: - 3.3330395907534287 - 3.3336270759132383 - .nan - .nan - .nan - .nan effort: - .nan - .nan - .nan - .nan - .nan - .nan ---

saikishor

Can you add a test that the message has elements with proper size?

Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com>

srmainwaring · 2026-03-03T09:55:16Z

Can you add a test that the message has elements with proper size?

Sure. At present the two tests are for robots that only have a position command interface, and a joint ordering such that this issue is not exposed. It will need a separate test, for something like an ackermann steering controlled vehicle that mixes different command interfaces.

saikishor · 2026-03-03T11:57:03Z

Can you add a test that the message has elements with proper size?

Sure. At present the two tests are for robots that only have a position command interface, and a joint ordering such that this issue is not exposed. It will need a separate test, for something like an ackermann steering controlled vehicle that mixes different command interfaces.

For now, just extend the test of the existing one by checking that the elements are of same size and are filled with NaNs for instance.

srmainwaring · 2026-03-03T19:49:27Z

For now, just extend the test of the existing one by checking that the elements are of same size and are filled with NaNs for instance.

Added in 13c65f3

There is an additional test for an ackermann steering controller in this branch: https://github.com/srmainwaring/topic_based_hardware_interfaces/tree/prs/joint-state-vector-sizes-with-ackermann-test which can be added if required. It is a stronger test of the indexing than the current tests as it includes both position and velocity command interfaces.

Could you please approve the workflow execution. Tests pass locally.

Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com>

MarqRazz · 2026-03-05T18:04:40Z

JointState messages require the position, velocity and effort arrays are all the same size or empty in order to uniquely associate each joint name with the correct state. The current behaviour is to only push_back commands on the supported interfaces which can result in position, velocity and effort arrays of different sizes. This prevents the joint name from being uniquely associated with its corresponding command.

The tool was specifically design this way to be compatible with different simulators. For example Isaac sim does not allow you to specify position and velocity commands for a singe joint at the same time (the position command conflicts with the velocity and it doesn't know which one to ignore). You should not be creating hardware interface groups that require different command interface types (position, velocity, effort, ...). For example most mobile bases require a velocity interface to the wheels and the manipulator requires position interfaces to its joints. This means you have to create two hardware interfaces, one for the base and a second for the manipulator.

If your hardware supports switching between position and velocity control then you need to be sure you only claim one at at time otherwise the receiving system has no way of knowing which command values in the JointState message are in control.

MarqRazz · 2026-03-05T18:22:16Z

If you wanted to do a hardware interface that supported mixed command interface types you would need to guarantee that all commanding controllers were smart enough to only set the interface they are allowed to. For example if you were using the JTC and mixing Position and Velocity control something would need to set the remaining interface values for each joint to NaN and the receiving software would need to support ignoring NaN and selecting the correct command.

srmainwaring · 2026-03-05T19:56:29Z

@MarqRazz, the proposed change is not to allow each joint to have multiple command types within a single hardware interface, but for a controller and hardware interface to allow each joint to be associated with the command type it requires: some joints may be position controlled, others velocity.

I don't think this is incompatible with Isaac Sim or other simulators. Certainly Gazebo supports such controllers and the gz_ros_control package provide examples.

As an example, ros_controllers provides an ackermann_steering controller, a controller for a single mobile base, which expects a hardware interface that provides position control for steering joints and velocity control for traction joints. As it stands topic_based_hardware_interface cannot provide a valid command output for this type of controller and the reason for that is that it does not respect the semantic requirements described in the JointState message type.

I don't think that these changes will break existing usage, but would be happy to check if there are examples the maintainers would like examined.

If you wanted to do a hardware interface that supported mixed command interface types you would need to guarantee that all commanding controllers were smart enough to only set the interface they are allowed to.

I think this is expected behaviour given the JointState message definition. That's what I see from the ackermann_steering controller at least.

MarqRazz · 2026-03-05T21:26:55Z

I don't think this is incompatible with Isaac Sim or other simulators.

I have tired to control a mobile base with velocity controlled wheels and arm joints with position and wasn't able to make it work in Isaac sim which is what drove the code to be implemented this way. I haven't had a chance to play much with Isaac lately so it will take me some time to get a valid environment working again.

I'm not saying it's impossible to do it in Isaac sim but changes are likely required to the simulator to support ignoring commands with values of NaN. Right now the simulator won't run if you send mixed interface types (position & velocity) and I have not tired sending joint commands of NaN to the Isaac Articulation controller.

For my robot I command the mobile base on the topic /base_joint_commands and the arms are on their own topic/hardware interface.

You can still support the ackermann_steering controller but will have to split the hardware interface into position and velocity controlled joints as shown below.

  <ros2_control name="ackermann_steering_system" type="system">
    <hardware>
      <plugin>joint_state_topic_hardware_interface/JointStateTopicSystem</plugin>
      <param name="joint_commands_topic">/robot_position_commands</param>
      <param name="joint_states_topic">/robot_joint_states</param>
    </hardware>
    <joint name="left_wheel_steering_joint">
      <command_interface name="position" />
      <state_interface name="position" />
      <state_interface name="velocity" />
      <state_interface name="effort" />
    </joint>
    <joint name="right_wheel_steering_joint">
      <command_interface name="position" />
      <state_interface name="position" />
      <state_interface name="velocity" />
      <state_interface name="effort" />
    </joint>
    <joint name="front_left_wheel_joint">
      <state_interface name="position" />
      <state_interface name="velocity" />
      <state_interface name="effort" />
    </joint>
    <joint name="front_right_wheel_joint">
      <state_interface name="position" />
      <state_interface name="velocity" />
      <state_interface name="effort" />
    </joint>
  </ros2_control>

  <ros2_control name="ackermann_propulsion_system" type="system">
    <hardware>
      <plugin>joint_state_topic_hardware_interface/JointStateTopicSystem</plugin>
      <param name="joint_commands_topic">/robot_velocity_commands</param>
      <param name="joint_states_topic">/robot_joint_states</param>
    </hardware>
    <joint name="rear_left_wheel_joint">
      <command_interface name="velocity" />
      <state_interface name="velocity" />
      <state_interface name="position" />
      <state_interface name="effort" />
    </joint>
    <joint name="rear_right_wheel_joint">
      <command_interface name="velocity" />
      <state_interface name="position" />
      <state_interface name="velocity" />
      <state_interface name="effort" />
    </joint>
  </ros2_control>

MarqRazz · 2026-03-05T23:03:08Z

joint_state_topic_hardware_interface/src/joint_state_topic_hardware_interface.cpp

  sensor_msgs::msg::JointState joint_state;
+
+  // The JointState message definition requires that all arrays in the message
+  // should have the same size, or be empty.


In this library we opted for them to be empty.

github-actions bot requested review from Juliaj, aprotyas, bmagyar, mhubii and saikishor March 2, 2026 21:46

saikishor reviewed Mar 3, 2026

View reviewed changes

Initialise arrays in the JointState message to quiet NaN

f52a21c

Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com>

Add tests for position, velocity and effort command array sizes

13c65f3

Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com>

srmainwaring force-pushed the prs/joint-state-vector-sizes branch from 89d4f2f to 13c65f3 Compare March 3, 2026 19:53

srmainwaring requested a review from saikishor March 5, 2026 08:28

MarqRazz reviewed Mar 5, 2026

View reviewed changes

srmainwaring mentioned this pull request Mar 11, 2026

Gazebo and MoveIt brukg/SO-100-arm#12

Open

	joint_state.effort.resize(joints.size(), 0.0);
	joint_state.effort.resize(joints.size(), std::numerical_limits::quiet_NaN());

Conversation

srmainwaring commented Mar 2, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Example

Uh oh!

saikishor Mar 3, 2026

Choose a reason for hiding this comment

Uh oh!

srmainwaring Mar 3, 2026

Choose a reason for hiding this comment

Uh oh!

saikishor Mar 3, 2026

Choose a reason for hiding this comment

Uh oh!

srmainwaring Mar 3, 2026

Choose a reason for hiding this comment

Uh oh!

saikishor left a comment • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

srmainwaring commented Mar 3, 2026

Uh oh!

saikishor commented Mar 3, 2026

Uh oh!

srmainwaring commented Mar 3, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

MarqRazz commented Mar 5, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

MarqRazz commented Mar 5, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

srmainwaring commented Mar 5, 2026

Uh oh!

MarqRazz commented Mar 5, 2026

Uh oh!

MarqRazz Mar 5, 2026

Choose a reason for hiding this comment

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

3 participants

srmainwaring commented Mar 2, 2026 •

edited

Loading

saikishor left a comment •

edited

Loading

srmainwaring commented Mar 3, 2026 •

edited

Loading

MarqRazz commented Mar 5, 2026 •

edited

Loading

MarqRazz commented Mar 5, 2026 •

edited

Loading