Switch from Twist to TwistStamped for cmd_vel

[SteveMacenski]

Right now we use Twist which contains no time based information. This may be important for detecting old or out of order messages in async applications.

A survey should be conducted for supporting this across the navigation ecosystem to support:

• Navigation
• RL
• turtlebot drivers
• gazebo
• other common robot drivers
• rviz (?)

In the mean-time, may be worth having a deprecation warning in navigation controllers for the meantime to let users know its coming. It would be ideal to have most mainstream drivers support this before too many people are ported over and stuck into it.

Maybe rename to cmd_vel_stamped?

[Timple]

Copy-paste from discourse, probably better to have the discussion here:

How does twist_stamped help? It can be stamped upon generation, but maybe the navigation algorithm took very (unexpectedly) long and is thus based on old sensor information.
Or the message never arrives and you cannot check it’s timestamp -> you need a timeout detection mechanism on the receiving side to be safe.

Which problem does the stamped message tackle which cannot be caught with a timeout detection?

[SteveMacenski]

It can be stamped upon generation

Yes, so you know exactly when that command is supposed to be executed.

maybe the navigation algorithm took very (unexpectedly) long

That's an unrelated issue.

Or the message never arrives and you cannot check it’s timestamp

Sure, but when the new message comes and using timing relative to that. If you get an old message instead, you definitely don't want to arbitrarily execute it.

Timeout detection should already be in your system, frankly. But just because you receive a message in a given time window, does not guarantee that it was generated in that time window, or that they're in order, or that they're still valid. You need some knowledge of timing for that.

[Timple]

The inorder part is a good point. And indeed an inorder can messup another timeout system which is unaware of that. 👍

[Rayman]

Nice, I would like this switch to TwistStamped.

On our robots we always publish a TwistStamped in tandem with twist. For rosbag this is nice, but also when viewing a rqt_graph of the cmd_vel. If you have a stamp you have nice smooth graph, even though you could be on a wifi connection for example.

[SteveMacenski]

We will discuss this more after the Foxy release, we don't want to break anything before an LTS or make things harder for other parts of the ecosystem.

In the meantime, it would be helpful to compile a list of places that need to be updated to support this off hand I can think of:

• DWB
• TEB
• pure pursuit
• Spin and backup recovery
• Gazebo/ignition plugins, probably
• open-source robot drivers (kobuki, turtlebot3, create, rover, ?? [not sure how many other vendors even have a ros2 driver yet])

Please add more to this list if you know of anything!

[Rayman]

• robot_localization
• teleop_twist_joy
• teleop_twist_keyboard
• twist_mux
• yocs_velocity_smoother

[SteveMacenski]

This should be started after the Foxy release in a few weeks

[SteveMacenski]

CC @tfoote @Karsten1987 - maybe worth a conversation to divide and conquer for this in Galactic?

[Karsten1987]

CC @bmagyar fyi.

[tfoote]

@SteveMacenski the big thing that I wanted to bring up is that the Header on a Twist is not enough information to allow us to transform it directly there. If I remember correctly there's actually 3 reference frames that need to be captured. I'm going to need to dig through my notes. you can do things like require messages published assume that one or more of those frames are coincident. But that's not always intuitive. I'm going to have to dig deeper to find notes on this. But before we do a mass migration to a TwistStamped we should make sure that it's adaquately complete.

[SteveMacenski]

Thinking about this again - I would seriously consider us coming together and doing this leading up to the Galactic release. It would be better to have issues with some packages out of sync of this change outside of an LTS distribution. Galactic seems like a good target to change this for so we have a year to work out all the packages needing update before the next LTS

[Ryanf55]

Copying from the other ticket:

Feature request

Add optional support of TwistStamped to replace Twist publishers of velocity control data.

Feature description

I'm working with @pedro-fuoco on integrating NAV2 and ArduPilot for his Google Summer of Code project. More info here:
https://discuss.ardupilot.org/t/gsoc-2023-gps-denied-autonomous-exploration-with-ros-2/101121

Over the past 6 months, I've added MicroROS support to ArduPilot that removes the need to use MavROS, which added a layer of abstraction, complexity, and confusion for many ROS users. Now, ArduPilot directly supports standard ROS messages like sensor_msgs/msg/NavSatFix.

I've been following REP-147 for the velocity control, which we now have a draft working in ArduPilot here.

REP-147 says to use TwistStamped for velocity control, and I would like NAV2 to support that. ArduPilot supports controls in a few different frames already; the use of a frame_id will allow ArduPilot to select the correct frame to control about. Currently, NAV2 uses Twist for the control message, so I would like to request a way to support TwistStamped

Proposal

1. Add a new parameter to all nodes that publish velocity control data, something like is_vel_control_stamped, default to False, which preserves the current behavior
2. On the calls to create_publisher, that parameter will now change which messages are published.
3. NAV2 can hard code the frame as base_link in compliance with rep-105 because NAV2 always controls in the body frame.
4. For aerial users following REP-147 control recommendations, if using NAV2, they should set is_vel_control_stamped to True.
5. NAV2 can start timestamping the controls data when it's published as TwistStamped, which can be used by the low-level controller ArduPilot to discard stale data and enter a recovery behavior (Loiter, RTL, etc)

Implementation considerations

• Preserve existing behavior for NAV2, so that this feature can be merged into humble, which is the version of ROS 2 that ArduPilot has selected for its 4.5 release (Oct/Nov 2023) or so.

Note: I can do the work, I'd just like feedback on the proposal.

[SteveMacenski]

I have full support for moving to stamping the outputs - the major issue is just having the time to update all of the downstream things that consume it (which the other ticket goes over an initial top-of-mind list). I don't think this should be an option but the hard change over to this. But, I suppose we could make a velocity publisher wrapper in nav2_util that deals with that parameterization outside of the application code (and in a way we can remove the non-stamped support more gradually). I would prefer to move the ecosystem over in a concerted effort which would mean updating the Twist to TwistStamped on Nav2 consumers like the gazebo control plugins and such listed in the other ticket. This does really seem like a great GSOC project, but I assume your student is more focused on the ardupilot side? wink

Preserve existing behavior for NAV2, so that this feature can be merged into humble

Certainly if you want it backported to humble, it would need to be an option.

[Ryanf55]

I have full support for moving to stamping the outputs - the major issue is just having the time to update all of the downstream things that consume it (which the other ticket goes over an initial top-of-mind list). I don't think this should be an option but the hard change over to this. But, I suppose we could make a velocity publisher wrapper in nav2_util that deals with that parameterization outside of the application code (and in a way we can remove the non-stamped support more gradually). I would prefer to move the ecosystem over in a concerted effort which would mean updating the Twist to TwistStamped on Nav2 consumers like the gazebo control plugins and such listed in the other ticket. This does really seem like a great GSOC project, but I assume your student is more focused on the ardupilot side? wink

Preserve existing behavior for NAV2, so that this feature can be merged into humble

Certainly if you want it backported to humble, it would need to be an option.

We actually have two students that were accepted. One is working on the embedded MicroROS/Service/Action support + UDP transport (Arsh), the other (Pedro) is working on SLAM on what that ecosystem call the companion computer (Linux+ROS 2). I've taken the responsibility of supporting the ROS 2 interfaces for the SLAM by adding ROS 2 topics in the autopilot to replace MavLink.

Yes, Humble would be the target. Based on the past releases for ArduPilot, I don't expect the release timelines to line up between ROS 2 Iron ArduPilot 4.5. Iron will be EOL'd while ArduPilot 4.5 is still supported...

There are some other options for the ArduPilot's 4.5 release, none of which seem super attractive:

• Fork nav2 for ArduPilot and do the conversion - maintenance burden
• Write a little converter node that intercepts the data and timestamps and frame ID - complexity, lag, another failure point
• Change ArduPilot to use raw Twist, and then add topics for each frame - topics are expensive for flash+ram on the embedded side).
• Only supporting one frame of control from ROS 2 in the autopilot

Did I miss any alternatives?

[SteveMacenski]

Humble might be the target, but the PRs and work should be done in main (rolling) and it'll be backported as long as its possible to be backported to Humble. That way Iron, Jazzy, ... get it as well.

Converting the pub/sub to objects which handle the stamp / no stamp seems like the best option. However, to close this work out, we'd need to update ignition control plugins + stuff described above to use stamped as well so that moving forward we can use stamp everywhere. I doubt it would be very difficult nor time consuming, its just about doing the work of opening the PRs and harassing the maintainers (with my help) to merge the change as the new status quo. And/or potentially use the same stamped/no stamped pub/sub

Write a little converter node that intercepts the data and timestamps and frame ID - complexity, lag, another failure point

Also the wrong stamp

and then add topics for each frame ... Only supporting one frame of control from ROS 2 in the autopilot

Can't speak to any of that, we only have one frame we think about in mobile robots

[Ryanf55]

Humble might be the target, but the PRs and work should be done in main (rolling) and it'll be backported as long as its possible to be backported to Humble. That way Iron, Jazzy, ... get it as well.

Of course, not an issue.

Converting the pub/sub to objects which handle the stamp / no stamp seems like the best option. However, to close this work out, we'd need to update ignition control plugins + stuff described above to use stamped as well so that moving forward we can use stamp everywhere. I doubt it would be very difficult nor time consuming, its just about doing the work of opening the PRs and harassing the maintainers (with my help) to merge the change as the new status quo. And/or potentially use the same stamped/no stamped pub/sub

If it's not difficult nor time consuming, then I can do it, and you can vouch for me to merge the PR's all together.

Write a little converter node that intercepts the data and timestamps and frame ID - complexity, lag, another failure point

Also the wrong stamp

Yep

and then add topics for each frame ... Only supporting one frame of control from ROS 2 in the autopilot

Can't speak to any of that, we only have one frame we think about in mobile robots

Fair enough, but that might change with aerial. There's a bunch of different ways in ArduPilot, NAV2 could at least say which one it's using in the frame_id, even if it only currently supports one. For reference: two messages, four coordinate frames for control.
https://ardupilot.org/dev/docs/copter-commands-in-guided-mode.html#set-position-target-local-ned
https://ardupilot.org/dev/docs/copter-commands-in-guided-mode.html#set-position-target-global-int

[SteveMacenski]

It might take some time before we get folks to merge them all in, but if they're all open, that can allow me to instantiate the process to get it done and shouldn't require any further work from you

Are you referring to

Valid options are:

    MAV_FRAME_GLOBAL (0): alt is in meters above sea level

    MAV_FRAME_GLOBAL_INT (5): alt is in meters above sea level

    MAV_FRAME_GLOBAL_RELATIVE_ALT (3): alt is in meters above home

    MAV_FRAME_GLOBAL_RELATIVE_ALT_INT (6): alt is in meters above home

    MAV_FRAME_GLOBAL_TERRAIN_ALT (10): alt is in meters above terrain

    MAV_FRAME_GLOBAL_TERRAIN_ALT_INT (11): alt is in meters above terrain

If so, if there's a TF tree that translates between them, then it shouldn't matter which is being published, they should be always transferable

[Ryanf55]

It might take some time before we get folks to merge them all in, but if they're all open, that can allow me to instantiate the process to get it done and shouldn't require any further work from you

Are you referring to

Valid options are:

    MAV_FRAME_GLOBAL (0): alt is in meters above sea level

    MAV_FRAME_GLOBAL_INT (5): alt is in meters above sea level

    MAV_FRAME_GLOBAL_RELATIVE_ALT (3): alt is in meters above home

    MAV_FRAME_GLOBAL_RELATIVE_ALT_INT (6): alt is in meters above home

    MAV_FRAME_GLOBAL_TERRAIN_ALT (10): alt is in meters above terrain

    MAV_FRAME_GLOBAL_TERRAIN_ALT_INT (11): alt is in meters above terrain

If so, if there's a TF tree that translates between them, then it shouldn't matter which is being published, they should be always transferable

Yes, plus

MAV_FRAME_LOCAL_NED
MAV_FRAME_LOCAL_OFFSET_NED 
MAV_FRAME_BODY_NED 
MAV_FRAME_BODY_OFFSET_NED 

The trick will be how to translate these into the select few frame ID's supported in REP-105, or to allow more.

[SteveMacenski]

Are these a part of a TF tree so that 1 published can be transformed into any of the rest?

[Ryanf55]

Are these a part of a TF tree so that 1 published can be transformed into any of the rest?

Right now, we have a few of the REP-105 frames (map, odom, base_link) implemented, however with the TF broadcaster running on the companion computer, and the latencies in messages to offboard, I expect there are going to need to be some compromises on the autopilot to restrict control to only a few frames where the math is done manually on the embedded side.

[SteveMacenski]

if the transform exists on the main computer, then it can handle that transform and the embedded machine can publish at whatever frame is comfortable and doesn’t need to worry about it, right?

[Ryanf55]

if the transform exists on the main computer, then it can handle that transform and the embedded machine can publish at whatever frame is comfortable and doesn’t need to worry about it, right?

The TF tree doesn't support automatically intercepting messages on cmd_vel, transforming the control data, and republishing with a new frame ID. Did you have another idea on how to do it?

[Ryanf55]

For this PR, I'd be happy if we could just get in (optional) support for TwistStamped, with the current frames, and merge that.

[SteveMacenski]

Yeah agreed, but just mentioning that we can handle more to fully solve the problem.

The TF tree doesn't support automatically intercepting messages on cmd_vel, transforming the control data, and republishing with a new frame ID. Did you have another idea on how to do it?

We could have a shim node that takes in a message and converts to a particular frame of interest before republishing (that the drone embedded computer uses). Once such instance of that could be a twist stamped, but I'd probably template it or try to make it generic to any message with a header field just to future proof

[tfoote]

I opened ros2/common_interfaces#228 which I see as a blocker before we start promoting TwistStamped more generally.

[SteveMacenski]

This is a very good point, thanks for forwarding that on. One of the major objectives I'm looking for in a migration is less to do with the frame and more to do with having a stamp to know when it was sent for lower-level safety promises to know that you're not executing something too old or of anomalous frequency. DDS gives you Lifespan or Deadline (depending on what metric you're interested in) for that, but it doesn't hurt to check this in case those features aren't in use in a particular subscription application.

I know this is a different case for @Ryanf55 who wants frames considered and that's a timely comment. I'm sure he cares about timestamping for safety as much as I do as well.

For starting out, I don't think the fact that the transforms are missing is a huge problem for us. Obviously as the message changes for TwistStamped, we'll have to adapt, but I imagine that will coincide with a Twist update as well so we'd be in the same boat regardless. So unless you don't think Twist would also change, I think this isn't a blocker, but something to be keenly aware of

[SteveMacenski]

#4077 to be merged. Where are we at with GZ taking in twist stamped? We'll need to update #3634 once that's ready to take in that plugin

[SteveMacenski]

@azeey just a quick follow up as I'm working on the Jazzy release today. Is TwistStamped supported in Harmonic for Rolling? I can make that switch for Nav2 later this year if so!

[azeey]

Yes, TwistsStamped is available on Jazzy and Rolling. The bridge is geometry_msgs/msg/TwistStamped <-> gz.msgs.Twist

[SteveMacenski]

I'm going to let things from the jazzy release settle for a bit, but then I'll make that switch over before end of this year (unless someone pings me first about it)!

[AntoBrandi]

Hi @SteveMacenski is this planned to be backported to Humble or Iron at some point?

[Ryanf55]

Sorry, we had to break ABI of the public headers in NAV2 to get it implemented. It can't be backported to iron or humble without significant changes. Check the linked PR for specifics on why.

If you have time and want to investigate it, go for it. My plan from the beginning was to make it available on humble, but that's not how the implementation went. With some more thought, it may be possible.

[AntoBrandi]

ok thank you for the feedback, I'll try to dedicate some time to investigate it.

[SteveMacenski]

This will be done for Kilted and newer. We can't backport forcing folks to use a different message type for output or else it would break literally every system in the world instantly.