Add TwistStamped support via TwistPublisher and TwistSubscriber

[Ryanf55]

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	#1594
Primary OS tested on	Ubuntu 22.04
Robotic platform tested on	CI

---

Description of contribution in a few bullet points

• Add a new util class for publishing either Twist or TwistStamped called TwistPublisher
• Add a new util class for subscribing to either Twist or TwistStamped called TwistSubscriber
• Add a new parameter for selecting to stamp the twist data to each util
• Consume TwistPublisher in nav2_controller, among other nodes
• Consume TwistSubscriber in nav2_velocityy_smoother, among other nodes
• Implements the approach described here: 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).

Description of documentation updates required from your changes

• Would need to document the new optional parameter behavior in the normal docs on top of the README.

Future work that may be required in bullet points

• Related PR's in gazebo, etc, to convert the ecosystem to TwistStamped

Test Instructions

colcon test --packages-select nav2_util --event-handlers=console_cohesion+ --ctest-args " -R" " test_twist"

Quick iteration

colcon build --packages-select nav2_util && colcon test --packages-select nav2_util --event-handlers=console_cohesion+ --ctest-args " -R" " test_twist"

For Maintainers:

• Check that any new parameters added are updated in navigation.ros.org
• Check that any significant change is added to the migration guide
• Check that any new features OR changes to existing behaviors are reflected in the tuning guide
• Check that any new functions have Doxygen added
• Check that any new features have test coverage
• Check that any new plugins is added to the plugins page
• If BT Node, Additionally: add to BT's XML index of nodes for groot, BT package's readme table, and BT library lists

[codecov]

Codecov Report

Attention: 9 lines in your changes are missing coverage. Please review.

Comparison is base (e2c8555) 90.15% compared to head (03ec283) 90.31%.

Files	Patch %	Lines
nav2_behaviors/plugins/assisted_teleop.cpp	78.57%	3 Missing ⚠️
...2_collision_monitor/src/collision_monitor_node.cpp	88.88%	2 Missing ⚠️
nav2_util/include/nav2_util/twist_publisher.hpp	92.30%	2 Missing ⚠️
nav2_velocity_smoother/src/velocity_smoother.cpp	93.93%	2 Missing ⚠️

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #3775      +/-   ##
==========================================
+ Coverage   90.15%   90.31%   +0.15%     
==========================================
  Files         415      417       +2     
  Lines       18593    18646      +53     
==========================================
+ Hits        16763    16840      +77     
+ Misses       1830     1806      -24     

☔ View full report in Codecov by Sentry.
📢 Have feedback on the report? Share it here.

[SteveMacenski]

Generally LGTM other than missing test coverage + documentation updates on the new param / feature

[SteveMacenski]

I think just unit testing is the main blocker here

[Ryanf55]

I think just unit testing is the main blocker here

What would you like to see as far as testing goes?

• Unit test with gtest
• Running the nav2 stack with this parameter set in system_testing?
• Something else

[SteveMacenski]

Add unit tests for this new object to make sure its working properly e.g.

• Check when you ask it to pub with stamp it does
• Check when you ask it to pub with non-stamp it does
• Check the different APIs you've created work / don't crash when called correctly
• Check that get sub count works properly in getting the right stamp vs non-stamp

^ the last 2 are obvious, but we don't know how its going to be modified over time. Also, its dereferencing pointers that will crash if the logic is ever wrong since the other publisher isn't allocated. So exercising the code in CI promises it won't crash applications later.

Just basic tests with gtest like the other utils - nothing nuts. System testing with a full bringup is unnecessary since its being used in the controller server which then would be tested by the system tests suite

[Ryanf55]

Add unit tests for this new object to make sure its working properly e.g.

• Check when you ask it to pub with stamp it does
• Check when you ask it to pub with non-stamp it does
• Check the different APIs you've created work / don't crash when called correctly
• Check that get sub count works properly in getting the right stamp vs non-stamp

^ the last 2 are obvious, but we don't know how its going to be modified over time. Also, its dereferencing pointers that will crash if the logic is ever wrong since the other publisher isn't allocated. So exercising the code in CI promises it won't crash applications later.

Just basic tests with gtest like the other utils - nothing nuts.

Sounds good, can do!

[Ryanf55]

I've added much more comprehensive testing. Currently, CI is failing a test:
https://app.circleci.com/pipelines/github/ros-planning/navigation2/9938/workflows/f5cb1d0d-0c04-48cb-9a51-1a7771b7b613/jobs/31717?invite=true#step-110-9217_47

    [ RUN      ] UtilsTests.SmootherTest
    /opt/overlay_ws/src/navigation2/nav2_mppi_controller/test/utils_test.cpp:381: Failure
    Expected: (smoothed_val) < (original_val), actual: 6.3252e+25 vs 11.9772
    [  FAILED  ] UtilsTests.SmootherTest (0 ms)

I'm currently looking to see if my PR causes that failure. I also did a rebase since you had mentioned fixing some flaky tests recently and I branched before that.

As far as all the other uses of Twist in NAV2, I;ve learned some nodes subscribe and republish Twist such as the velocity smoother. In order to have this backward-compatible support for TwistStamped in the entire stack, I expect I'll need to create a mirrored TwistSubscriber just like the TwistPublisher. Please let me know if you want TwistSubscriber added to this PR, or if we could do it in a follow-up.

[SteveMacenski]

I think this PR so that we have a clean migration over to it. Just merging this alone could break users that enable that param and don't have the ability to enable that new param with the velocity smoother and collision monitor.

I looked over the codecov results and the code and this is good to me now, so consider the publisher approved!

[mergify]

@Ryanf55, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

[Ryanf55]

I was curious if you had any ideas on the TwistStamped subscribers how to deal with the callbacks needing to have a certain signature. For example: AssistedTeleop::teleopVelocityCallback

My thought is to add overrides for all node callbacks to also work with TwistStamped, and convert all internal variables to TwistStamped typed. Then, once the ecosystem (gazebo, etc) is moved over to TwistStamped, the pure Twist callbacks can be deprected on rolling and removed in J-turtle.

[SteveMacenski]

I was curious if you had any ideas on the TwistStamped subscribers how to deal with the callbacks needing to have a certain signature. For example: AssistedTeleop::teleopVelocityCallback

That sample is pretty easy: the twist subscriber has some getTwist method for when it needs to be gotten and otherwise can just be a subobject of this behavior. There may also need to be a reset() to set the twist to default (or just have a bool for received_ that gets reset to mark validness).

Others may be more complex. You may want to enumerate them and we can discuss them individually so you don't waste time

My thought is to add overrides for all node callbacks to also work with TwistStamped, and convert all internal variables to TwistStamped typed. Then, once the ecosystem (gazebo, etc) is moved over to TwistStamped, the pure Twist callbacks can be deprected on rolling and removed in J-turtle.

I don't fully follow. Like have both a stamped and non-stamped callbacks, each registered, and the stamped just passes the msg.twist to the non-stamped version that implements the logic? Or only actually create_subscription based on the twist stamped-ness so only one's active on the network

[Ryanf55]

There may also need to be a

Goal: Only subscribe with either Twist or TwistStamped, not both to eliminate overhead.

Based on your feedback, it's leading me to have the TwistSubscriber class constructor take in both signatures (Twist and TwistStamped) of the subscriber callback. It will bind whichever the node is configured for. The problem with that approach, assuming the callbacks are overrides is that the template arg deduction in bind fails.

/home/ryan/Dev/ros2_ws/src/navigation2/nav2_collision_monitor/src/collision_monitor_node.cpp: In member function ‘virtual nav2_util::CallbackReturn nav2_collision_monitor::CollisionMonitor::on_configure(const rclcpp_lifecycle::State&)’:
/home/ryan/Dev/ros2_ws/src/navigation2/nav2_collision_monitor/src/collision_monitor_node.cpp:68:14: error: no matching function for call to ‘bind(<unresolved overloaded function type>, nav2_collision_monitor::CollisionMonitor*, const std::_Placeholder<1>&)’
   68 |     std::bind(&CollisionMonitor::cmdVelInCallback, this, std::placeholders::_1));
      |     ~~~~~~~~~^~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
In file included from /usr/include/c++/11/pstl/glue_algorithm_defs.h:13,
                 from /usr/include/c++/11/algorithm:74,
                 from /opt/ros/rolling/include/rclcpp/rclcpp/executor.hpp:18,
                 from /opt/ros/rolling/include/rclcpp/rclcpp/executors/multi_threaded_executor.hpp:25,
                 from /opt/ros/rolling/include/rclcpp/rclcpp/executors.hpp:21,
                 from /opt/ros/rolling/include/rclcpp/rclcpp/rclcpp.hpp:167,
                 from /home/ryan/Dev/ros2_ws/src/navigation2/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp:22,
                 from /home/ryan/Dev/ros2_ws/src/navigation2/nav2_collision_monitor/src/collision_monitor_node.cpp:15:
/usr/include/c++/11/functional:789:5: note: candidate: ‘template<class _Func, class ... _BoundArgs> typename std::_Bind_helper<std::__is_socketlike<_Func>::value, _Func, _BoundArgs ...>::type std::bind(_Func&&, _BoundArgs&& ...)’
  789 |     bind(_Func&& __f, _BoundArgs&&... __args)
      |     ^~~~
/usr/include/c++/11/functional:789:5: note:   template argument deduction/substitution failed:
/home/ryan/Dev/ros2_ws/src/navigation2/nav2_collision_monitor/src/collision_monitor_node.cpp:68:14: note:   couldn’t deduce template parameter ‘_Func’
   68 |     std::bind(&CollisionMonitor::cmdVelInCallback, this, std::placeholders::_1));
      |     ~~~~~~~~~^~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
In file included from /usr/include/c++/11/pstl/glue_algorithm_defs.h:13,
                 from /usr/include/c++/11/algorithm:74,
                 from /opt/ros/rolling/include/rclcpp/rclcpp/executor.hpp:18,
                 from /opt/ros/rolling/include/rclcpp/rclcpp/executors/multi_threaded_executor.hpp:25,
                 from /opt/ros/rolling/include/rclcpp/rclcpp/executors.hpp:21,
                 from /opt/ros/rolling/include/rclcpp/rclcpp/rclcpp.hpp:167,
                 from /home/ryan/Dev/ros2_ws/src/navigation2/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp:22,
                 from /home/ryan/Dev/ros2_ws/src/navigation2/nav2_collision_monitor/src/collision_monitor_node.cpp:15:
/usr/include/c++/11/functional:813:5: note: candidate: ‘template<class _Result, class _Func, class ... _BoundArgs> typename std::_Bindres_helper<_Result, _Func, _BoundArgs>::type std::bind(_Func&&, _BoundArgs&& ...)’
  813 |     bind(_Func&& __f, _BoundArgs&&... __args)
      |     ^~~~
/usr/include/c++/11/functional:813:5: note:   template argument deduction/substitution failed:
/home/ryan/Dev/ros2_ws/src/navigation2/nav2_collision_monitor/src/collision_monitor_node.cpp:68:14: note:   couldn’t deduce template parameter ‘_Result’
   68 |     std::bind(&CollisionMonitor::cmdVelInCallback, this, std::placeholders::_1));
      |     ~~~~~~~~~^~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
gmake[2]: *** [CMakeFiles/collision_monitor_core.dir/build.make:76: CMakeFiles/collision_monitor_core.dir/src/collision_monitor_node.cpp.o] Error 1
gmake[1]: *** [CMakeFiles/Makefile2:186: CMakeFiles/collision_monitor_core.dir/all] Error 2
gmake: *** [Makefile:146: all] Error 2
---
Failed   <<< nav2_collision_monitor [2.51s, exited with code 2]

For now, I'll just name the callbacks differently (easy) or switch to lambdas (harder).

[mergify]

This pull request is in conflict. Could you fix it @Ryanf55?

[Ryanf55]

Hi @SteveMacenski,

I've gotten all the features working. I'd like to start iterating on getting to 100% code coverage, or at least no decrease in code coverage such that you will be happy merging.

I know that CI runs a tool that shows you a diff of your PR against the target branch, colorizing with respect to changes in code coverage. Is there any way to use it locally so I can iterate faster than waiting for the build and test workflow in CI to run?

Example: https://app.codecov.io/github/ros-planning/navigation2/commit/98af3b9a28d9180ba22b17573f01cccaf7cbe04b

[Ryanf55]

Looks like the latest build has flake8 failures in CI are totally unrelated to my changes. I do not know what I should do to resolve those:
https://app.circleci.com/pipelines/github/ros-planning/navigation2/10324/workflows/e2990a21-f6d6-428b-8b0c-8029562638b1/jobs/32684?invite=true#step-110-7208_26

[SteveMacenski]

Overall, LGTM. One thing I'd ask is to review the README files of changed packages + Nav2 documentation to update the use of Twist or TwistStamped. I know its annoying to do docs, but you should be able to get through skimming each of the pages and updating that within 15 minutes and would do a world to help users down the line if they didn't know about this to have hints in the docs easily

[mergify]

This pull request is in conflict. Could you fix it @Ryanf55?

[SteveMacenski]

I fixed the merge conflict

[SteveMacenski]

You lost me a bit with the 2 callbacks. I suppose previous-me reviewing this didn't catch the reason why I thought having the getMsg() functions might be nice.

What do you do if there is a mismatch? Or what if there is no Stamped callback? It seems weird to force a user to define both when they only want to implement one or the other.

Thought 1: TwistStampedCallback has a default argument so that it doesn't need to be defined. Then we have a default twist stamped callback that just strips the stamp and calls the Twist callback so that a Twist user can subscribe to Twist/TwistStamped and get the same result.

Thought 2: Have a second constructor if TwistStamped is the only valid option, so that you don't have to define a bogus Twist callback that won't be used. Or, we could do a similar thing with default argument that results in a default callback for Twist that just throws an exception as unimplemented for that use-case.

The desire I have is not to have to define 2 callbacks, when I only need / want 1, and the other is bogus just to fit the API. There may be applications where both are acceptable, so having a constructor such as this makes sense -- but I think we should have the other options to specify one or the other alone (or even still the same constructor but default arguments so not required to implement)

[Ryanf55]

I've implemented the support for the Thought 2 with a second constructor.

[SteveMacenski]

Any update here?

[Ryanf55]

Any update here?

Yea: I got the TwistSubscriber alternative constructor (TwistStamped only) as we discussed working, and tested.
00ba082#diff-6c65cfa2155d600fefba7d3dd82f4f71ea27ef7221ab349934ec811fc8b5759eR123

Converting back to std::move everywhere has compilation errors about trying to call a deleted function. I spent a while chasing the ROS 2 design docs, older github issues regarding std::move and the API, but was not successful. I could use help on that. If you have time to work with me today, that would be excellent.

Otherwise, I just need to move the markdown docs to rst once we're happy with the behavior and it should be ready.

[SteveMacenski]

All of the open requests from the last review are also still relevant (but all small). I'll handle the re-pointer-ification once everything else is handled.

The least of it, but CI is complaining about a small number of linting issues. Usual bits on configuration guide for new parameter + migration guide for capability documentation

[mergify]

This pull request is in conflict. Could you fix it @Ryanf55?

[SteveMacenski]

I handled the conflict

[Ryanf55]

All of the open requests from the last review are also still relevant (but all small). I'll handle the re-pointer-ification once everything else is handled.

The least of it, but CI is complaining about a small number of linting issues. Usual bits on configuration guide for new parameter + migration guide for capability documentation

Thanks. Almost ready. Just working on the docs now in navigation.ros.org.

Here's the resources on publishing unique_ptr I tried following. Sadly, none of them led me to a resolution with std::move.
https://design.ros2.org/articles/intraprocess_communications.html

https://herbsutter.com/2013/06/05/gotw-91-solution-smart-pointer-parameters/

If you need to reproduce the failure:
See commit 2f3d8d4 on my branch 1594-twist-stamped-publisher-with-move.

According to the ROS design docs, this should work:

  void publish(std::unique_ptr<geometry_msgs::msg::TwistStamped> velocity)

However, I get this compilation error:

Starting >>> nav2_util
--- stderr: nav2_util                             
/home/ryan/Dev/nav2_ws/src/navigation2/nav2_util/test/test_twist_publisher.cpp: In member function ‘virtual void TwistPublisher_StampedWithMove_Test::TestBody()’:
/home/ryan/Dev/nav2_ws/src/navigation2/nav2_util/test/test_twist_publisher.cpp:128:25: error: use of deleted function ‘std::unique_ptr<_Tp, _Dp>::unique_ptr(const std::unique_ptr<_Tp, _Dp>&) [with _Tp = geometry_msgs::msg::TwistStamped_<std::allocator<void> >; _Dp = std::default_delete<geometry_msgs::msg::TwistStamped_<std::allocator<void> > >]’
 128 |   vel_publisher->publish(std::move(pub_msg_ptr));
     |   ~~~~~~~~~~~~~~~~~~~~~~^~~~~~~~~~~~~~~~~~~~~~~~
In file included from /usr/include/c++/11/memory:76,
                from /home/ryan/Dev/nav2_ws/src/navigation2/nav2_util/test/test_twist_publisher.cpp:15:
/usr/include/c++/11/bits/unique_ptr.h:468:7: note: declared here
 468 |       unique_ptr(const unique_ptr&) = delete;
     |       ^~~~~~~~~~
In file included from /home/ryan/Dev/nav2_ws/src/navigation2/nav2_util/test/test_twist_publisher.cpp:18:
/home/ryan/Dev/nav2_ws/src/navigation2/nav2_util/include/nav2_util/twist_publisher.hpp:112:66: note:   initializing argument 1 of ‘void nav2_util::TwistPublisher::publish(std::unique_ptr<geometry_msgs::msg::TwistStamped_<std::allocator<void> >, std::default_delete<geometry_msgs::msg::TwistStamped_<std::allocator<void> > > >)’
 112 |   void publish(std::unique_ptr<geometry_msgs::msg::TwistStamped> velocity)
     |                ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~^~~~~~~~
gmake[2]: *** [test/CMakeFiles/test_twist_publisher.dir/build.make:76: test/CMakeFiles/test_twist_publisher.dir/test_twist_publisher.cpp.o] Error 1
gmake[1]: *** [CMakeFiles/Makefile2:577: test/CMakeFiles/test_twist_publisher.dir/all] Error 2
gmake: *** [Makefile:146: all] Error 2
---
Failed   <<< nav2_util [4.22s, exited with code 2]

[mergify]

@Ryanf55, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

[SteveMacenski]

@Ryanf55 I will follow up in a few moments with a PR to supersede this one with some changes. Please review :-)

[Ryanf55]

Feel free to re-open this one. I just pushed a fix that @srmainwaring uncovered during testing and also cherry-pickd your changes in from #4031.