NAV2 Support for TwistStamped Available (rolling, jazzy)

Navigation Stack
, ,
1

Twist Stamped for Velocity Commands in NAV2

Hello!

I’d like to share some recent work in NAV2 to enable using TwistStamped for velocity control rather than Twist.

Background

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:

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

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

  3. Yea, I forgot, but Foxglove is much nicer when you timestamp your data

A more detailed discussion on Twist can be seen here:

https://discourse.ros.org/t/is-twist-still-a-good-velocity-command-interface/13218/23
And, the related ticket from NAV2:
https://github.com/ros-planning/navigation2/issues/1594
TwistStamped won’t solve all of the problems mentioned above threads, but it does move in the right right direction!

How it works

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

The implementation of the class is in TwistPublisher and TwistSubscriber.

Comparison to other approaches

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.

The nav2_util::TwistSubscriber and nav2_util::TwistPublisher classes allow for a near-seamless migration with little code change.

Migration

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!

How this will be Used in Aerial Vehicles to Change Control Behavior

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)

header:
  frame_id: “map”
twist:
  linear:
    x: NaN
    y: NaN
    z: NaN
  angular:
    x: NaN
    y: NaN
    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:
linear:

header:
  frame_id: “body”
twist:
  linear:
    x: NaN
    y: NaN
    z: NaN
  angular:
    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)

Migration Status of Other Packages

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.

Credits

Many thanks to those involved in this effort, particularly @smac and @srmainwaring for all the feedback and testing.

2 Likes