[joint_state_controller/broadcaster] "Resorted" joint_names compared to URDF/xacro file

[destogl]

I just got reported that the joint_state_controller has resorted joint_names compared to the ros2_control-tag defined in the URDF file. They could be confusing and we should "debug" this if possible.

For this, I propose the following procedure:

1. Start an example 6D robot to test this:
   1. Checkout ros2_control_demos repository 6Dbot-example-for-tests branch (Add example 6Dbot for tests and debugging ros2_control_demos#74).
   2. Compile the package and start ros2 launch ros2_control_demo_robot 6Dbot_example.launch.py
   3. Echo /joint_states topics and see that the name field does not start with "joint1", but "joint2". Why? This has to be answered in this issue!
2. Check how the interfaces are ordered in the configuration incoming to the FakeSystem implementation. Check names of the joints in this line.
3. Check the order of the state_interfaces in the assign_interfaces method here.
4. Check the order here.
5. Finally, the output joint names are sorted as stored in the joint_names_ variable here.

If the joint_names are mixed, we have to find out exactly where to know if this is repairable or add additional parameters or parsing of URDF in the controller.

[bailaC]

I think, you are pointing at,

header:
  stamp:
    sec: 1617109375
    nanosec: 388138438
  frame_id: ''
name:
- joint2
- joint3
- joint6
- joint4
- joint1
- joint5
position:

I will take a look at this. Please feel free to assign me this issue.

[gavanderhoorn]

@destogl wrote:

If the joint_names are mixed, we have to find out exactly where to know if this is repairable

why?

This has been discussed in the past, and it makes everything much more robust if nodes/controllers/plugins/whatever don't/can't assume a certain order.

There are so many producers and consumers of (fi) JointState and similar messages that enforcing a certain order will be impossible, and different languages may have different sorting orders for maps/dictionaries/lists/associative_arrays/etc which make this even harder.

I'd suggest making the requirement for consumers to always re-order (ie: preprocess/validate user input) incoming information as they need it explicit instead of the implicit assumption we have now (in ros_control, but also in MoveIt and other frameworks which produce and consume these kinds of messages, services and actions).

[bailaC]

@destogl , please find the observation below,

While listing the hardware interfaces, printing the interface names at this line
File : /ros2_control/hardware_interface/src/fake_components/generic_system.cpp

$ ros2 control list_hardware_interfaces
command interfaces
        joint1/position [claimed]
        joint2/position [claimed]
        joint3/position [claimed]
        joint4/position [claimed]
        joint5/position [claimed]
        joint6/position [claimed]
        tcp_fts_sensor/fx [unclaimed]
        tcp_fts_sensor/fy [unclaimed]
        tcp_fts_sensor/fz [unclaimed]
        tcp_fts_sensor/tx [unclaimed]
        tcp_fts_sensor/ty [unclaimed]
        tcp_fts_sensor/tz [unclaimed]
state interfaces
         joint1/acceleration
         joint1/position
         joint1/velocity
         joint2/acceleration
         joint2/position
         joint2/velocity
         joint3/acceleration
         joint3/position
         joint3/velocity
         joint4/acceleration
         joint4/position
         joint4/velocity
         joint5/acceleration
         joint5/position
         joint5/velocity
         joint6/acceleration
         joint6/position
         joint6/velocity
         tcp_fts_sensor/fx
         tcp_fts_sensor/fy
         tcp_fts_sensor/fz
         tcp_fts_sensor/tx
         tcp_fts_sensor/ty
         tcp_fts_sensor/tz

So, it is clear that in generic_system.cpp file, everything is in order.

Found the output below while launch. Added log at line 123 and line 136
File : /ros2_controllers/joint_state_controller/src/joint_state_controller.cpp

[ros2_control_node-1] [INFO] [1617205559.318050832] [controller_manager]: Loading controller 'joint_state_controller'
[ros2_control_node-1] [INFO] [1617205559.346614319] [controller_manager]: Configuring controller 'joint_state_controller'
[ros2_control_node-1] JointStateController Line 123 : joint5
[ros2_control_node-1] JointStateController Line 123 : joint1
[ros2_control_node-1] JointStateController Line 123 : joint1
[ros2_control_node-1] JointStateController Line 123 : joint6
[ros2_control_node-1] JointStateController Line 123 : joint4
[ros2_control_node-1] JointStateController Line 123 : joint3
[ros2_control_node-1] JointStateController Line 123 : joint2
[ros2_control_node-1] JointStateController Line 123 : joint6
[ros2_control_node-1] JointStateController Line 123 : tcp_fts_sensor
[ros2_control_node-1] JointStateController Line 123 : joint3
[ros2_control_node-1] JointStateController Line 123 : joint5
[ros2_control_node-1] JointStateController Line 123 : joint2
[ros2_control_node-1] JointStateController Line 123 : joint4
[ros2_control_node-1] JointStateController Line 123 : joint4
[ros2_control_node-1] JointStateController Line 123 : joint1
[ros2_control_node-1] JointStateController Line 123 : tcp_fts_sensor
[ros2_control_node-1] JointStateController Line 123 : joint2
[ros2_control_node-1] JointStateController Line 123 : joint3
[ros2_control_node-1] JointStateController Line 123 : joint5
[ros2_control_node-1] JointStateController Line 123 : joint6
[ros2_control_node-1] JointStateController Line 123 : tcp_fts_sensor
[ros2_control_node-1] JointStateController Line 123 : tcp_fts_sensor
[ros2_control_node-1] JointStateController Line 123 : tcp_fts_sensor
[ros2_control_node-1] JointStateController Line 123 : tcp_fts_sensor

[ros2_control_node-1] JointStateController Line 138 : joint2
[ros2_control_node-1] JointStateController Line 138 : joint3
[ros2_control_node-1] JointStateController Line 138 : joint6
[ros2_control_node-1] JointStateController Line 138 : joint4
[ros2_control_node-1] JointStateController Line 138 : joint1
[ros2_control_node-1] JointStateController Line 138 : joint5

Observe the sequence of Line 138 is exactly same as the output of the /joint_states topic.

Note : The joint names are in order in ros2_control_demos/ros2_control_demo_robot/description/6Dbot/6Dbot_macro.ros2_control.xacro file.

[destogl]

@bailaC can you also check this in the assigned interfaces method? See the first comment, point Nr. 3

[bailaC]

Yes, sure, I missed that.

[bailaC]

Collecting the output from both command_interfaces_ and state_interfaces_ from here

[ros2_control_node-1] [INFO] [1617635704.310279983] [controller_manager]: Configuring controller 'joint_state_controller'
[ros2_control_node-1] ControllerInterface : state interfaces : tcp_fts_sensor
[ros2_control_node-1] ControllerInterface : state interfaces : tcp_fts_sensor
[ros2_control_node-1] ControllerInterface : state interfaces : tcp_fts_sensor
[ros2_control_node-1] ControllerInterface : state interfaces : tcp_fts_sensor
[ros2_control_node-1] ControllerInterface : state interfaces : joint6
[ros2_control_node-1] ControllerInterface : state interfaces : joint5
[ros2_control_node-1] ControllerInterface : state interfaces : joint3
[ros2_control_node-1] ControllerInterface : state interfaces : joint2
[ros2_control_node-1] ControllerInterface : state interfaces : tcp_fts_sensor
[ros2_control_node-1] ControllerInterface : state interfaces : joint1
[ros2_control_node-1] ControllerInterface : state interfaces : joint4
[ros2_control_node-1] ControllerInterface : state interfaces : joint4
[ros2_control_node-1] ControllerInterface : state interfaces : joint2
[ros2_control_node-1] ControllerInterface : state interfaces : joint5
[ros2_control_node-1] ControllerInterface : state interfaces : joint3
[ros2_control_node-1] ControllerInterface : state interfaces : tcp_fts_sensor
[ros2_control_node-1] ControllerInterface : state interfaces : joint6
[ros2_control_node-1] ControllerInterface : state interfaces : joint2
[ros2_control_node-1] ControllerInterface : state interfaces : joint3
[ros2_control_node-1] ControllerInterface : state interfaces : joint4
[ros2_control_node-1] ControllerInterface : state interfaces : joint6
[ros2_control_node-1] ControllerInterface : state interfaces : joint1
[ros2_control_node-1] ControllerInterface : state interfaces : joint1
[ros2_control_node-1] ControllerInterface : state interfaces : joint5


[ros2_control_node-1] ControllerInterface : command interfaces : joint1
[ros2_control_node-1] ControllerInterface : command interfaces : joint2
[ros2_control_node-1] ControllerInterface : command interfaces : joint3
[ros2_control_node-1] ControllerInterface : command interfaces : joint4
[ros2_control_node-1] ControllerInterface : command interfaces : joint5
[ros2_control_node-1] ControllerInterface : command interfaces : joint6

Everything is at one place now.

[destogl]

Can you make print also with "interface.get_name() + interface.get_interface_name()"

[bmagyar]

Heads up: I've just merged joint_state_broadcaster, please rebase if you have code depending on joint_state_controller.

[martijnhabers]

This is something I have seen before as well. Not sure if it's the same issue that is happening, but I believe that it has to do with something that happens here:

ros2_controllers/joint_state_broadcaster/src/joint_state_broadcaster.cpp

Lines 253 to 260 in 219a121

	for (const auto & name_ifv : name_if_value_mapping_)
	{
	const auto & interfaces_and_values = name_ifv.second;
	if (has_any_key(interfaces_and_values, {HW_IF_POSITION, HW_IF_VELOCITY, HW_IF_EFFORT}))
	{
	joint_names_.push_back(name_ifv.first);
	}
	}

By looping over an unordered map, the order of the joints in the /joint_states is not sorted. I'm not sure how it iterates over an unordered map, but in my project we have now tried changing it to a regular std::map

This leads to the /joint_states topic being sorted alphabetically by joint name

Maybe there's a more elegant way to implement this but this was a bit of a quick hack to get it working.

[christophfroehlich]

That is a good hint, it seems that you have found the culprit. The question for me is now, should it be in alphabetical order or in the order of definition in the parameters? If the latter is the case, then we have to use something like a std::vector instead.

[bmagyar]

Anything but the current setup would be an improvement.
I'd vote for alphabetical as it's much faster to get into the next release ;)

[christophfroehlich]

we have two configurations:

• selected joints defined in the parameter for the broadcaster
• all joints, we get the order from the CM with state_interfaces_ variable

for the first I'd say, use the same order as in the parameter.
for the second, no reordering?

[mhubii]

The question for me is now, should it be in alphabetical order or in the order of definition in the parameters?

Would it not make sense to put it in order of the kinematic chain (if possible)? This would simplify Jacobian computations and what not. Hope I am not missing something.

[antrom99]

i have a problem with joints name on the topic /joint_states , in particular i wish would :

name:
  - joint1
  - joint2
  - joint3
  - joint4
  - joint5
  - joint6
  - 

but i have:

header:
  stamp:
    sec: 1740586545
    nanosec: 900974358
  frame_id: ''
name:
  - joint2
  - joint3
  - joint1
  - joint4
  - joint5
  - joint6
position:
  - 0.0
  - -1.57
  - 0.0
  - 0.0
  - -1.57
  - 0.0
  

The order of links and joints in the axcro file robot.model.xacro is correct from base to last joint , in robot.ros2_control.xacro where declared che command and state interfaces the order of joints is joint1 to joint6 and in the file ros2_controllers.yaml i defined :

# This config file is used by ros2_control
controller_manager:
  ros__parameters:
    update_rate: 1000  # Hz

    smartsix_arm_trajectory_controller:
      type: joint_trajectory_controller/JointTrajectoryController


    joint_state_broadcaster:
      type: joint_state_broadcaster/JointStateBroadcaster

    smartsix_forward_position_controller:
      type: forward_command_controller/ForwardCommandController

smartsix_arm_trajectory_controller:
  ros__parameters:
    joints:
      - joint1
      - joint2
      - joint3
      - joint4
      - joint5
      - joint6
    command_interfaces:
      - position
    state_interfaces:
      - position
      - velocity

smartsix_forward_position_controller:
  ros__parameters:
    joints:
      - joint1
      - joint2
      - joint3
      - joint4
      - joint5
      - joint6
    interface_name: position

how can i fix this order on the topic /joint_states?

[bmagyar]

You'd need to tweak how the list of joint names are created here:
https://github.com/ros-controls/ros2_controllers/blob/master/joint_state_broadcaster/src/joint_state_broadcaster.cpp#L228

As it was said here before, you could take a new (optional) parameter for the joint state broadcaster to specify the order of joints but do put some thought into how you'd want to handle discrepancies in the number of available joints vs the list provided?

Also as @mhubii said, the kinematics order sounds very nice but then you'd need to grab the URDF and figure out what that order is. Also, kinematics from where to where? ;)

You can be fairly liberal with the implementation here, no need to worry about realtime as the function I referenced runs before the update() method would kick in.

[martijnhabers]

If you go here you can see what we changed in order to get it alphabetical. Essentially it was changing the type from an unordered_map to a map, as I believe they are sorted "by default" (not sure, was a while ago)

https://gitlab.com/project-march/libraries/control/ros2_controllers/-/commit/6668511de7a43bf17f84cb66d174dd509f3e68d6

in the joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp

std::unordered_map<std::string, std::unordered_map<std::string, double>> name_if_value_mapping_;
was changed to
std::map<std::string, std::unordered_map<std::string, double>> name_if_value_mapping_;

and in the joint_state_broadcaster/src/joint_state_broadcaster.cpp

double get_value(
  // removed this line
  const std::unordered_map<std::string, std::unordered_map<std::string, double>> & map,
  // added this line
  const std::map<std::string, std::unordered_map<std::string, double>> & map,
  const std::string & name, const std::string & interface_name)
{

Hope that's clear, and can help you fix it.

In the end we moved away from the Joint state broadcaster completely and made our own broadcaster as we had created a custom fork of the JSB and were sometimes sourcing the wrong one (without alphabetical sorting), leading to some interesting results.

should I make a pull request with this fix?

[mhubii]

Also, kinematics from where to where? ;)

That's a very good point, had only considered simple chains. Might there be room for another broadcaster then? Something along the lines of a serial chain joint state broadcaster?

[christophfroehlich]

Also, kinematics from where to where? ;)

That's a very good point, had only considered simple chains. Might there be room for another broadcaster then? Something along the lines of a serial chain joint state broadcaster?

if we change the unordered map, you always can give the joints manually as ROS parameter and the order will be preserved. What is your proposal with the kinematic chains, giving the start and end of the chain and use the URDF to look for the joints?

[bmagyar]

@martijnhabers In my experience it is always better to discuss implementation details on PRs ;)

[mhubii]

if we change the unordered map, you always can give the joints manually as ROS parameter and the order will be preserved. What is your proposal with the kinematic chains, giving the start and end of the chain and use the URDF to look for the joints?

well either ordered way is fine really. Extracting the order from the chain may offer a unique source and default initialization with robustness to prefixes at the cost of introducing complexity and entanglement.

[antrom99]

@martijnhabers

Do the PR pls

[saikishor]

IMO Introducing kinematics chains into JSB will just make the code complex. Better start with the ordering part for the defined joints parameter.

[saikishor]

I just checked the code, i think changing unorderd_map to map won't fix anything. What needs to be fixed is the order of names in the joint_names_ variable here:

https://github.com/ros-controls/ros2_controllers/blob/master/joint_state_broadcaster%2Fsrc%2Fjoint_state_broadcaster.cpp#L253-L267

This is what is used in the update function as well to fill in the data

[antrom99]

@martijnhabers

I have doing your change to file joint_state_broadcaster.cpp e joint_state_broadcaster.hpp and finally the joints name are in roder from 1 to 6.

Thanks for the tips and thanks also all everyone others !!!

[bmagyar]

@mhubii I think there is a way we could have the cake and eat it.
What if we had 2 cakes?

I mean, what if we had 2 JSB instances in a system? And by we I mean you, or anyone who wants to have it published in a specific order for some specific chain?
Here's the idea

• you have a JSB with no configuration, publishing every joint in whatever order it wants it on the /joint_states topic.
• you have a second JSB with a joints configuration argument with the list of exact joints in the exact order you want them, publishing on some other, potentially namespaced topic.

Would this work?

[mhubii]

TLDR;

• you have a second JSB with a joints configuration argument with the list of exact joints in the exact order you want

Would be very helpful indeed (this should ideally be supported (lbr-stack/lbr_fri_ros2_stack#178 (comment)), and this seems to be the conses here:

if we change the unordered map, you always can give the joints manually as ROS parameter

Better start with the ordering part for the defined joints parameter.

But one might consider an additional kinematic chain broadcaster in another issue. Now, instead of talking, I also agree with

@martijnhabers In my experience it is always better to discuss implementation details on PRs ;)

Details

I mean, what if we had 2 JSB instances in a system? And by we I mean you

yes would totally work of course :). Any solution that driver maintainers can support such that users of that driver don't run into that same pitfall over and over. It does happen a lot that users send commands and then the robot does funky stuff which is ultimately dangerous.

That's why I was thinking of leaving the JSB untouched

Might there be room for another broadcaster then?

I.e. provide an alternative maybe inherited JSB with access to chain. This doesn't seem much more complex than the admittance controller, and I am of course coming from a serial manipulator standpoint. Again, I see this argument

IMO Introducing kinematics chains into JSB will just make the code complex.

However, I don't think it will "just" make the code complex. It will provide benefits indeed as explained above. Let me make this clearer. Downstream I see a lot of this, and this does assume at least alphabetical sorting (and even worse, I don't see it)

  def _on_joint_state(self, joint_state: JointState) -> None:
      q = np.array(joint_state.position)[np.argsort(joint_state.name)]
      # dq = J(q)^(-1)@dx ...

Is having an argsort / re-sampling really less complex? You could of course now go ahead and have every maintainer have a variant of JSB

I have doing your change to file joint_state_broadcaster.cpp e joint_state_broadcaster.hpp and finally the joints name are in roder from 1 to 6.

but that defeats the purpose imho.

The alphabetical sorting assumption may already break for people building on top of drivers... You can of course now argue that I am using ros2_controllers the wrong way. But it is helpful to write up stuff using a forward command controller then turn it into a proper controller if needed.

[saikishor]

@antrom99 @mhubii @martijnhabers, Could you please give my changes in PR a shot at #1572 and see if they improve now?

Thank you

[saikishor]

Hello!

I can confirm that the fix proposed in #1572 is working