New Graceful Motion Controller

[ajtudela]

---

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	#1975
Primary OS tested on	Ubuntu 22.04
Robotic platform tested on	Ignition Gazebo
Does this PR contain AI generated software?	No

---

Description of contribution in a few bullet points

• Added a new version of the graceful controller based on the smooth control law with similar settings to the existing controllers.
• I think I need to add more tests to increase the test coverage.

Description of documentation updates required from your changes

• New entry in configuration page: https://navigation.ros.org/configuration/index.html
• New entry in plugins: https://navigation.ros.org/plugins/index.html#controllers

---

Future work that may be required in bullet points

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: 13 lines in your changes are missing coverage. Please review.

Comparison is base (7872bfa) 90.34% compared to head (dc4f154) 90.48%.
Report is 17 commits behind head on main.

❗ Current head dc4f154 differs from pull request most recent head e90e26e. Consider uploading reports for the commit e90e26e to get more accurate results

Files	Patch %	Lines
...tion_controller/src/graceful_motion_controller.cpp	91.80%	10 Missing ⚠️
...v2_graceful_motion_controller/src/path_handler.cpp	94.28%	2 Missing ⚠️
...aceful_motion_controller/src/parameter_handler.cpp	98.85%	1 Missing ⚠️

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #4021      +/-   ##
==========================================
+ Coverage   90.34%   90.48%   +0.14%     
==========================================
  Files         415      424       +9     
  Lines       18583    18892     +309     
==========================================
+ Hits        16788    17095     +307     
- Misses       1795     1797       +2     

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

[SteveMacenski]

Yes, more test coverage is definitely needed: https://app.codecov.io/gh/ros-planning/navigation2/pull/4021/tree/nav2_graceful_motion_controller/src. We accept new packages at 90%+ test coverage, though you should aim for as much as is reasonably possible (e.g. missing a stray line due to an edge case within an impossible edge case is -- like not being able to lock the node's shared pointer 🤷)

You also have a few linting issues that CI will probably bring up, but you can find with ament_cpplint and ament_uncrustify

I'm pleasantly surprised by the quality of this PR so this shouldn't be a huge effort for me to review even though its a big update. I did a once over on obvious things in the meantime since I don't have the cycles to give it a deep review right now, but I did want to give you feedback on tests / things you can do immediately in the meantime.

It would also be good for @mikeferguson to review this one as well given his experience. He might be able to find errors faster than I and have feedback on good bells and whistles to add that are missing.

[ajtudela]

Thanks for the compliment, Steve. Sorry, to make this PR later than I though but I've been very busy this past months and I preferred to upload the code as complete as possible.

In the meantime, I'll fix the linting issues and I'll working in the new tests to increase the coverage

[SteveMacenski]

A nice demo video highlighting this controller's value would be great too both for here & for the documentation

[SteveMacenski]

Hi! I'm really excited for this work and its very timely - a docking effort will begin shortly which can definitely leverage this core work. any updates on your side? I know we're just now back from the holidays so no worries!

[ajtudela]

Hi Steve, sorry for not making the video, I'm still on Christmas holidays until Monday. I did new tests on the last day before the holidays, but I left a few until we reach 90%+ coverage. So, it's almost done.

I'll review your changes; they don't seem very difficult to do.

I'm thrilled to work on the docking implementation. #1975

[mergify]

@ajtudela, 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).

[ajtudela]

I think I fixed all the requested changes.

[mergify]

@ajtudela, 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]

Let us know when you're ready for another review!

[ajtudela]

I'm ready. I made all the requested changes.

[SteveMacenski]

@mikeferguson please re-review

[SteveMacenski]

But overall the code looks really good! I'm leaning on Fergs for a details algorithm review and subtle details from his expertise. He still has several of his original comments unaddressed

[ajtudela]

I'm ready. I made all the requested changes.

But overall the code looks really good! I'm leaning on Fergs for a details algorithm review and subtle details from his expertise. He still has several of his original comments unaddressed

I agreed. I think I made most of the changes proposed by Fergs but I let the rest of that part to him.

[SteveMacenski]

Why would either of these values appear in this expression? It should be something like the proportion of max linear velocity reduction times the original max angular velocity to scale it down proportionately to the requested linear velocity reduction. Try checking out other implementations

[SteveMacenski]

OK on the Fergs items. We discussed in the Working Group meeting today that we can merge in once you finish up these last couple details with the dynamic parameters and speed limits. The remaining items @mikeferguson will enumerate here, then we can merge this (thanks so much!) and he can pick those up in a follow on PR!

[SteveMacenski]

Looks good to me! Fergs?

The last bits are just documentation! Adding the new plugin to the Configuration Guide with the parameters and a few sentences up top about what it is & announcing it on the Migration Guide and in the Navigation Plugins table.

[mikeferguson]

We can almost merge this - remaining issues:

• The v_linear_min issue I just raised (I found this during some other testing)
• Add the name of the updated paper to the README
• Address the collision checking concerns that Steve raised

Things that I will do in a follow on PR:

• Revamp the controller body to "search" for a motion target (I can re-license and merge the stuff from my controller because I personally hold the copyright on that)
• Likely adjust some parameter defaults based on experience with running this kind of controller on a number of robots

[ajtudela]

Thanks @mikeferguson!!
I fixed the minor issues with the velocity and Readme and I think the collision cheking is also fixed.

I made a PR for the docs here: 517

[SteveMacenski]

One small typo on the documentation, but otherwise LGTM!

@mikeferguson are these comments still pending and/or items you plan to address? Just want to make sure we don't leave anything hanging. I went through the code in detail on the ROS2-y things, but left the main control law to your review. It sounds like the finding of the carrot is going to change alot, I had some gripes with that, but no need to so anything if that's getting redone anyway.

I believe this will be wrong when the robot is "reversing" (which is a case not handled by the paper, but which occurs in the real world)

I think there is a subtle bug here if current.orientation isn't always 0 (I think we get that right now because you transform the plan into the local frame before proceeding, right?

I would recommend you actually use the angle_to_target rather than a fixed -1/+1 - and then have a scaling parameter - otherwise this almost sure to overshoot (you'll be going max_angular_v as you cross the goal, and you still need to decelerate)

@ajtudela Overall, really great work on all of this. Definitely great code quality, functionality, and understanding of the important trade offs, I'm very impressed! I'd welcome your help anytime on any subjects your find interest in! 😄

[ajtudela]

@ajtudela Overall, really great work on all of this. Definitely great code quality, functionality, and understanding of the important trade offs, I'm very impressed! I'd welcome your help anytime on any subjects your find interest in! 😄

Thank you very much! It was hard at first, especially because of the tests, I'm not used to doing them. But after so many commits I've gotten the hang of it and I'm starting to love them. :D

Of course. Next project: docking in nav2!

[mikeferguson]

I believe this will be wrong when the robot is "reversing" (which is a case not handled by the paper, but which occurs in the real world)

As close as I can tell without manually testing, yes this is now resolved. I can fix any remaining issues when doing the carrot overhaul.

I think there is a subtle bug here if current.orientation isn't always 0 (I think we get that right now because you transform the plan into the local frame before proceeding, right?

It's not an issue for the codebase as-is - and it does make the EpiPolar code more generic as currently implemented. I only see it as a potential trap for someone who comes in and modifies this code without realizing the assumption. I figured I would leave the comment unresolved there in case anyone someday comes back through this because they didn't realize those assumptions.

I would recommend you actually use the angle_to_target rather than a fixed -1/+1 - and then have a scaling parameter - otherwise this almost sure to overshoot (you'll be going max_angular_v as you cross the goal, and you still need to decelerate)

Yes, this is resolved

[SteveMacenski]

Great! Any objections to a merge @mikeferguson ?

@ajtudela this is a great contribution as to part of Docking. Currently, Fergs and I are working on such a thing and the vast majority of it is done, in part thanks to your work! That should be released in the coming month or two depending on the testing hiccups. Unfortunately, it is part of a project funded by a company, so I can't share the WIP code with you for your help (that was sponsored after we chatted about this; sorry!) so there's not much I can actually get your help on besides this for the docking framework. My apologies, but this was a great step that has made it into the final docking framework and a meaningful contribution there! To be honest, the rest of the Docking work is alot of software engineering and less algorithmic, you did the fun algorithmic parts :-)

I'd be more than happy to have your help on other projects however!

[mikeferguson]

No objections

[SteveMacenski]

@ajtudela last-last thing, you have a test failure:

/opt/overlay_ws/src/navigation2/nav2_graceful_motion_controller/test/test_graceful_motion_controller.cpp:1119
Expected: (cmd_vel.twist.linear.x) < (0.0), actual: 0.1 vs 0/opt/overlay_ws/src/navigation2/nav2_graceful_motion_controller/test/test_graceful_motion_controller.cpp:1120
Expected: (cmd_vel.twist.angular.z) < (0.0), actual: 0.293018 vs 0

[ajtudela]

ups, fixed!