[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 (ros-controls/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.