Proposal: Support TwistStamped Velocity Control for REP-147 (Aerial) applications

[Ryanf55]

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]

This is already an open ticket: #1594. 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? 😉

I think we should continue this conversation in the other ticket since its already open and covers the same topics. Feel free to copy+paste your comment / response over there to continue chatting.

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]

This is already an open ticket: #1594. 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? 😉

I think we should continue this conversation in the other ticket since its already open and covers the same topics. Feel free to copy+paste your comment / response over there to continue chatting.

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 MicroROS/Service/Action support + UDP transport (Arsh), the other (Pedro) is working on SLAM. I've taken the responsibility of supporting the ROS 2 interfaces for the SLAM interface in the autopilot which replaces 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. We can continue the specifics of the migration in the other ticket.

[SteveMacenski]

Please respond in the other ticket, I'm going to close this one.