I’d like to share some recent work in NAV2 to enable using
TwistStamped for velocity control rather than
Traditionally, when NAV2 controllers send the control data to the robot, the form is
geometry_msgs/msg/Twist, which represents the linear and angular control commands to the robot. On typical ground robots that run the low-level controls on the same computer as the NAV2 stack, and always control in the same frame ID, this works great!
The community has been asking for
TwistStamped support in ROS 2 for a while because of two major reasons:
The header allows for controlling in different frame ID’s instead of making the assumption of always using “base_link” without requiring new topics for each control method or dubiously documented conventions
The timestamping allows for safer fault detection for stale control data, which is critical when an overloaded physical layer bus could cause stale controls data
Yea, I forgot, but Foxglove is much nicer when you timestamp your data
A more detailed discussion on Twist can be seen here:
And, the related ticket from NAV2:
TwistStamped won’t solve all of the problems mentioned above threads, but it does move in the right right direction!
NAV2 nodes that either subscribe to or publish
Twist now can optionally change to
TwistStamped through a parameter called
enable_stamped_cmd_vel. The default value is
false, which preserves existing behavior. In the future, the default may change when enough of the ROS ecosystem has migrated to
One proposed approach was to add a topic to every node that handles velocity control data, such that it would support both Twist and TwistStamped at the same time.
The implementation I chose in NAV2 creates subscribers or publishers dynamically when the node starts up depending on what is requested, which avoids duplication of topics, wasted resources, and extra DDS network traffic of having two parallel topics.
Other approaches, like this one, add a shim node, but another node adds overhead and latency in control data.
ros2_control implemented an approach similar to nav2, but duplicated the handling of Twist and TwistStamped across the codebase as needed.
nav2_util::TwistPublisher classes allow for a near-seamless migration with little code change.
The migration information is documented here: https://navigation.ros.org/migration/Iron.html?highlight=twiststamped#added-twiststamped-option-for-commands
For projects that hide their implementation details a little better than NAV2, it would be possible to get this working in
humble with significantly less effort than doing it from scratch.
Make sure when migrating not to accidentally drop timestamps along the code paths through your nodes, or re-stamp the time with an invalid timestamp!
ArduPilot implements a REP-147 compliant subscriber for velocity commands. This is all done over a single topic. Each topic in MicroROS consume precious RAM on the microcontroller, so this approach is great to save RAM!
TwistStamped allows controlling the drone in “base_link” (body) frame, but we also augment this to control in “map” frame. In practice, this means that a ROS developer that wants a fixed wing plane to fly in a circle can send the following data. This results in the vehicle conducting a coordinated turn to fly a circle, and the low-level autopilot automatically sets the roll angle, correcting for wind, changes in speed, etc. The vehicle completes a full turn in
desired_circle_period seconds, and the controller will do this with minimal side-slip if tuned well. (caveat - I’m still developing this feature; it’s more to show a useful example)
z: 2*PI / desired_circle_period
Conversely, if the ROS 2 developer wants to do the low level controls, feed these to the autopilot, but still let the autopilot handle speed (throttle), it would look like this:
x: 2 * PI / desired_circle_period * some_algorithm(current_speed, vehicle_dynamics_model)
y: 2 * PI / desired_circle_period * some_algorithm(current_speed, vehicle_dynamics_model)
z: 2 * PI / desired_circle_period * some_algorithm(current_speed, wind estimate, vehicle_dynamics_model)
Part of what made this effort difficult and put off for so long is it really needs ecosystem buy-in to be worth doing. Luckily, we’ve made a lot of headway. I’ll update this list as needed.
ROS 2 Control supports both TwistStamped and Twist: https://github.com/ros-controls/ros2_controllers/issues/82 and message data type for cmd_vel in diff_drive_controller · Issue #80 · ros-controls/ros2_controllers · GitHub and
Unstamped cmd_vel subscriber rebased by v-lopez · Pull Request #143 · ros-controls/ros2_controllers · GitHub