ROS2 to Dynamixel Bridge (`ros2_dynamixel_bridge`)

Hi everyone :coffee: :wave:

One of the by-products of the development of L3X-Z - 107-systems mixed electric/hydraulic hexapod - is a ROS2 to Dynamixel Bridge (ros2_dynamixel_bridge) heavily leveraging libdynamixelplusplus.

It’s GitHub home including build and usage instructions can be found here.

Upon startup the ROS node scans the connected network and automagically creates topics for each discovered Robotis servo.

Servos can be either controlled by setting a desired angular setpoint (rad) OR an angular velocity setpoint (rad/sec).

The discovery and configuration on L3X-Z (we’ve got 8 MX-28AR) looks like this:

[INFO] [ros2_dynamixel_bridge_node-1]: process started with pid [14871]
[ros2_dynamixel_bridge_node-1] [INFO] [1683915635.048548445] [l3xz.ros2_dynamixel_bridge]: configuring Dynamixel RS485 bus:
[ros2_dynamixel_bridge_node-1] 	Device:   /dev/serial/by-id/usb-FTDI_USB__-__Serial_Converter_FT4NNZ55-if00-port0
[ros2_dynamixel_bridge_node-1] 	Baudrate: 2000000
[ros2_dynamixel_bridge_node-1] [INFO] [1683915635.839335420] [l3xz.ros2_dynamixel_bridge]: detected Dynamixel MX-28AR: { 1 2 3 4 5 6 7 8 }.
[ros2_dynamixel_bridge_node-1] [INFO] [1683915635.839423475] [l3xz.ros2_dynamixel_bridge]: checking if all required servos are online ...
[ros2_dynamixel_bridge_node-1] [INFO] [1683915636.114801833] [l3xz.ros2_dynamixel_bridge]: initialize servo #1
[ros2_dynamixel_bridge_node-1] 	Init. Pos. = 129.99
[ros2_dynamixel_bridge_node-1] 	Init. Vel. = 0.00
[ros2_dynamixel_bridge_node-1] 	Pub:       = /dynamixel/servo_1/angle/actual
[ros2_dynamixel_bridge_node-1] 	Sub:       = /dynamixel/servo_1/angle/target
[ros2_dynamixel_bridge_node-1] 	Sub:       = /dynamixel/servo_1/angular_velocity/target
[ros2_dynamixel_bridge_node-1] 	Sub:       = /dynamixel/servo_1/mode/set
[ros2_dynamixel_bridge_node-1] [INFO] [1683915636.434833599] [l3xz.ros2_dynamixel_bridge]: initialize servo #2
[ros2_dynamixel_bridge_node-1] 	Init. Pos. = 208.04
[ros2_dynamixel_bridge_node-1] 	Init. Vel. = 0.00
[ros2_dynamixel_bridge_node-1] 	Pub:       = /dynamixel/servo_2/angle/actual
[ros2_dynamixel_bridge_node-1] 	Sub:       = /dynamixel/servo_2/angle/target
[ros2_dynamixel_bridge_node-1] 	Sub:       = /dynamixel/servo_2/angular_velocity/target
[ros2_dynamixel_bridge_node-1] 	Sub:       = /dynamixel/servo_2/mode/set
[ros2_dynamixel_bridge_node-1] [INFO] [1683915636.754749012] [l3xz.ros2_dynamixel_bridge]: initialize servo #3
[ros2_dynamixel_bridge_node-1] 	Init. Pos. = 235.81
[ros2_dynamixel_bridge_node-1] 	Init. Vel. = 0.00
[ros2_dynamixel_bridge_node-1] 	Pub:       = /dynamixel/servo_3/angle/actual
[ros2_dynamixel_bridge_node-1] 	Sub:       = /dynamixel/servo_3/angle/target
[ros2_dynamixel_bridge_node-1] 	Sub:       = /dynamixel/servo_3/angular_velocity/target
[ros2_dynamixel_bridge_node-1] 	Sub:       = /dynamixel/servo_3/mode/set
[ros2_dynamixel_bridge_node-1] [INFO] [1683915637.074768750] [l3xz.ros2_dynamixel_bridge]: initialize servo #4
[ros2_dynamixel_bridge_node-1] 	Init. Pos. = 157.94
[ros2_dynamixel_bridge_node-1] 	Init. Vel. = 0.00
[ros2_dynamixel_bridge_node-1] 	Pub:       = /dynamixel/servo_4/angle/actual
[ros2_dynamixel_bridge_node-1] 	Sub:       = /dynamixel/servo_4/angle/target
[ros2_dynamixel_bridge_node-1] 	Sub:       = /dynamixel/servo_4/angular_velocity/target
[ros2_dynamixel_bridge_node-1] 	Sub:       = /dynamixel/servo_4/mode/set
[ros2_dynamixel_bridge_node-1] [INFO] [1683915637.394745813] [l3xz.ros2_dynamixel_bridge]: initialize servo #5
[ros2_dynamixel_bridge_node-1] 	Init. Pos. = 135.53
[ros2_dynamixel_bridge_node-1] 	Init. Vel. = 0.00
[ros2_dynamixel_bridge_node-1] 	Pub:       = /dynamixel/servo_5/angle/actual
[ros2_dynamixel_bridge_node-1] 	Sub:       = /dynamixel/servo_5/angle/target
[ros2_dynamixel_bridge_node-1] 	Sub:       = /dynamixel/servo_5/angular_velocity/target
[ros2_dynamixel_bridge_node-1] 	Sub:       = /dynamixel/servo_5/mode/set
[ros2_dynamixel_bridge_node-1] [INFO] [1683915637.714666262] [l3xz.ros2_dynamixel_bridge]: initialize servo #6
[ros2_dynamixel_bridge_node-1] 	Init. Pos. = 211.64
[ros2_dynamixel_bridge_node-1] 	Init. Vel. = 0.00
[ros2_dynamixel_bridge_node-1] 	Pub:       = /dynamixel/servo_6/angle/actual
[ros2_dynamixel_bridge_node-1] 	Sub:       = /dynamixel/servo_6/angle/target
[ros2_dynamixel_bridge_node-1] 	Sub:       = /dynamixel/servo_6/angular_velocity/target
[ros2_dynamixel_bridge_node-1] 	Sub:       = /dynamixel/servo_6/mode/set
[ros2_dynamixel_bridge_node-1] [INFO] [1683915638.034643456] [l3xz.ros2_dynamixel_bridge]: initialize servo #7
[ros2_dynamixel_bridge_node-1] 	Init. Pos. = 184.66
[ros2_dynamixel_bridge_node-1] 	Init. Vel. = 0.00
[ros2_dynamixel_bridge_node-1] 	Pub:       = /dynamixel/servo_7/angle/actual
[ros2_dynamixel_bridge_node-1] 	Sub:       = /dynamixel/servo_7/angle/target
[ros2_dynamixel_bridge_node-1] 	Sub:       = /dynamixel/servo_7/angular_velocity/target
[ros2_dynamixel_bridge_node-1] 	Sub:       = /dynamixel/servo_7/mode/set
[ros2_dynamixel_bridge_node-1] [INFO] [1683915638.354619399] [l3xz.ros2_dynamixel_bridge]: initialize servo #8
[ros2_dynamixel_bridge_node-1] 	Init. Pos. = 3.69
[ros2_dynamixel_bridge_node-1] 	Init. Vel. = 0.00
[ros2_dynamixel_bridge_node-1] 	Pub:       = /dynamixel/servo_8/angle/actual
[ros2_dynamixel_bridge_node-1] 	Sub:       = /dynamixel/servo_8/angle/target
[ros2_dynamixel_bridge_node-1] 	Sub:       = /dynamixel/servo_8/angular_velocity/target
[ros2_dynamixel_bridge_node-1] 	Sub:       = /dynamixel/servo_8/mode/set
[ros2_dynamixel_bridge_node-1] [INFO] [1683915638.395367915] [l3xz.ros2_dynamixel_bridge]: ros2_dynamixel_bridge init complete.

Using ROS topic remapping these topics can be renamed to meaningful names for your target application, i.e.

$ ros2 topic list
/l3xz/head/pan/angle/actual
/l3xz/head/pan/angle/target
/l3xz/head/pan/angular_velocity/target
/l3xz/head/pan/mode/set
/l3xz/head/tilt/angle/actual
/l3xz/head/tilt/angle/target
/l3xz/head/tilt/angular_velocity/target
/l3xz/head/tilt/mode/set
/l3xz/leg/left_back/coxa/angle/actual
/l3xz/leg/left_back/coxa/angle/target
/l3xz/leg/left_back/coxa/angular_velocity/target
/l3xz/leg/left_back/coxa/mode/set
/l3xz/leg/left_front/coxa/angle/actual
/l3xz/leg/left_front/coxa/angle/target
/l3xz/leg/left_front/coxa/angular_velocity/target
/l3xz/leg/left_front/coxa/mode/set
/l3xz/leg/left_middle/coxa/angle/actual
/l3xz/leg/left_middle/coxa/angle/target
/l3xz/leg/left_middle/coxa/angular_velocity/target
/l3xz/leg/left_middle/coxa/mode/set
/l3xz/leg/right_back/coxa/angle/actual
/l3xz/leg/right_back/coxa/angle/target
/l3xz/leg/right_back/coxa/angular_velocity/target
/l3xz/leg/right_back/coxa/mode/set
/l3xz/leg/right_front/coxa/angle/actual
/l3xz/leg/right_front/coxa/angle/target
/l3xz/leg/right_front/coxa/angular_velocity/target
/l3xz/leg/right_front/coxa/mode/set
/l3xz/leg/right_middle/coxa/angle/actual
/l3xz/leg/right_middle/coxa/angle/target
/l3xz/leg/right_middle/coxa/angular_velocity/target
/l3xz/leg/right_middle/coxa/mode/set

Currently only MX-28AR are supported, as those are the servos of choice for L3X-Z. However, it would be a no-brainer to extent the package to other servos. Contributions are welcome :wink: .

Cheers, Alex