[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.