Draft: command streaming

[tcappellari-bdai]

Change Overview

Adds command streaming to the ros2_control package

Testing Done

• [x] Simple test: run ros2 launch spot_ros2_control spot_ros2_control.launch.py config_file:=spot_ros.yaml hardware_interface:=robot. This will start command streaming and simply forward the current state into the command. Check that the commands are being accepted correctly and that the robot doesn't move but the joints are active.

more complex test: try running the noarm_squat or wiggle_arm examples once we are sure the first simple test is working correctly. Make sure the joint angles in the command are in the correct order!!!

• [x] wiggle arm on a spot with arm
• [ ] noarm squat on a spot without an arm

[tcappellari-bdai]

• #458 👈
• dev/ros2_control

This stack of pull requests is managed by Graphite. Learn more about stacking.

Join @tcappellari-bdai and the rest of your teammates on Graphite

[khughes-bdai]

Getting this stream of errors when I test on robot currently

[ros2_control_node-1] [INFO] [1724175682.517103116] [SpotHardware]: Powered on!
[ros2_control_node-1] [INFO] [1724175682.565908258] [SpotHardware]: Robot Command Client successfully created!
[ros2_control_node-1] [ERROR] [1724175682.566138808] [SpotHardware]: Failed to activate joint control mode
[ros2_control_node-1] [INFO] [1724175682.566692128] [SpotHardware]: Robot Command Streaming Client successfully created!
[ros2_control_node-1] [INFO] [1724175682.566801602] [resource_manager]: Successful 'activate' of hardware 'SpotSystem'
...
[ros2_control_node-1] [ERROR] [1724175682.672456813] [SpotHardware]: Failed to send command: '3(Streaming for joint control failed): '
[ros2_control_node-1] [ERROR] [1724175682.672686174] [SpotHardware]: Failed to send command: '3(Streaming for joint control failed): '

looking into why this is failing. also, if the joint command mode fails to get activated, I think the hardware interface should also fail to activate.

[khughes-bdai]

Pushed some changes, but still getting this very non informative error

[ros2_control_node-1] [ERROR] [1724179006.507922729] [SpotHardware]: Failed to send command: '3(Streaming for joint control failed): '

EDIT: commands are successfully sent for about a few milliseconds, and I see the robot slightly move. The problem is that the joint command streaming client is somehow switching off shortly after initialization.

[tcappellari-bdai]

@mhidalgo-bdai should be ready for another pass now I'd like to change loading in the gains later but for now I'm ok with it. @khughes-bdai also tried your suggestion about the joint ordering but it didnt work out :crying_cat_face: (see above)