New Graceful Motion Controller#4021
Conversation
Codecov ReportAttention:
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. |
|
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 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. |
|
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 |
|
A nice demo video highlighting this controller's value would be great too both for here & for the documentation |
|
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! |
nav2_graceful_motion_controller/include/nav2_graceful_motion_controller/ego_polar_coords.hpp
Outdated
Show resolved
Hide resolved
nav2_graceful_motion_controller/include/nav2_graceful_motion_controller/ego_polar_coords.hpp
Outdated
Show resolved
Hide resolved
nav2_graceful_motion_controller/include/nav2_graceful_motion_controller/smooth_control_law.hpp
Outdated
Show resolved
Hide resolved
nav2_graceful_motion_controller/src/graceful_motion_controller.cpp
Outdated
Show resolved
Hide resolved
nav2_graceful_motion_controller/src/graceful_motion_controller.cpp
Outdated
Show resolved
Hide resolved
nav2_graceful_motion_controller/src/graceful_motion_controller.cpp
Outdated
Show resolved
Hide resolved
nav2_graceful_motion_controller/src/graceful_motion_controller.cpp
Outdated
Show resolved
Hide resolved
|
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 |
|
@ajtudela, your PR has failed to build. Please check CI outputs and resolve issues. |
Signed-off-by: Alberto Tudela <ajtudela@gmail.com>
Signed-off-by: Alberto Tudela <ajtudela@gmail.com>
Signed-off-by: Alberto Tudela <ajtudela@gmail.com>
Signed-off-by: Alberto Tudela <ajtudela@gmail.com>
Signed-off-by: Alberto Tudela <ajtudela@gmail.com>
Signed-off-by: Alberto Tudela <ajtudela@gmail.com>
Signed-off-by: Alberto Tudela <ajtudela@gmail.com>
Signed-off-by: Alberto Tudela <ajtudela@gmail.com>
Signed-off-by: Alberto Tudela <ajtudela@gmail.com>
Signed-off-by: Alberto Tudela <ajtudela@gmail.com>
Signed-off-by: Alberto Tudela <ajtudela@gmail.com>
Signed-off-by: Alberto Tudela <ajtudela@gmail.com>
Signed-off-by: Alberto Tudela <ajtudela@gmail.com>
Signed-off-by: Alberto Tudela <ajtudela@gmail.com>
Signed-off-by: Alberto Tudela <ajtudela@gmail.com>
47a461c to
f745c7a
Compare
|
I think I fixed all the requested changes. |
|
@ajtudela, your PR has failed to build. Please check CI outputs and resolve issues. |
Signed-off-by: Alberto Tudela <ajtudela@gmail.com>
9afb4c4 to
2e230b0
Compare
Signed-off-by: Alberto Tudela <ajtudela@gmail.com>
|
Let us know when you're ready for another review! |
|
I'm ready. I made all the requested changes. |
|
@mikeferguson please re-review |
nav2_graceful_motion_controller/src/graceful_motion_controller.cpp
Outdated
Show resolved
Hide resolved
nav2_graceful_motion_controller/src/graceful_motion_controller.cpp
Outdated
Show resolved
Hide resolved
|
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 |
Signed-off-by: Alberto Tudela <ajtudela@gmail.com>
|
I'm ready. I made all the requested changes.
I agreed. I think I made most of the changes proposed by Fergs but I let the rest of that part to him. |
| } | ||
|
|
||
| // Limit the angular velocity to be proportional to the linear velocity | ||
| params_->v_angular_max = params_->v_linear_max * params_->rotation_scaling_factor; |
There was a problem hiding this comment.
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
|
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! |
Signed-off-by: Alberto Tudela <ajtudela@gmail.com>
|
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
left a comment
There was a problem hiding this comment.
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
nav2_graceful_motion_controller/include/nav2_graceful_motion_controller/parameter_handler.hpp
Show resolved
Hide resolved
Signed-off-by: Alberto Tudela <ajtudela@gmail.com>
|
Thanks @mikeferguson!! I made a PR for the docs here: 517 |
|
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.
@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! |
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.
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.
Yes, this is resolved |
|
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! |
|
No objections |
|
@ajtudela last-last thing, you have a test failure:
|
Signed-off-by: Alberto Tudela <ajtudela@gmail.com>
|
ups, fixed! |
* Initial commit Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Fix egopolar and add compute next pose Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Added simulated trajectory Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Added slowdown publisher Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Added first tests Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Added basic tests and smoth control Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Added initial rotation Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Added final rotation Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Added collision Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Improve last motion target Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Added new tests Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Set min velocity Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Added EOL Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Path test Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Add utils and minor fixes Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Update footprint calculation Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Added more tests Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Added nav2_controller to package test Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Improve comments Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Added backward motion Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Split in two libraries Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Improve rotation velocity Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Update documentation Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Minor fixes Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Revert collision_checker Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Pass costmap transform to simulate trajectory Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Retain old headers Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Update dyn parameters of control law Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Better comment in test Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Update setSpeedLimits with angular vel max Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Fix SpeedLimits Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Fixes in vel and collision Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Fix backward motion Signed-off-by: Alberto Tudela <ajtudela@gmail.com> --------- Signed-off-by: Alberto Tudela <ajtudela@gmail.com> Signed-off-by: enricosutera <enricosutera@outlook.com>
* Initial commit Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Fix egopolar and add compute next pose Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Added simulated trajectory Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Added slowdown publisher Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Added first tests Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Added basic tests and smoth control Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Added initial rotation Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Added final rotation Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Added collision Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Improve last motion target Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Added new tests Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Set min velocity Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Added EOL Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Path test Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Add utils and minor fixes Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Update footprint calculation Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Added more tests Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Added nav2_controller to package test Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Improve comments Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Added backward motion Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Split in two libraries Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Improve rotation velocity Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Update documentation Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Minor fixes Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Revert collision_checker Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Pass costmap transform to simulate trajectory Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Retain old headers Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Update dyn parameters of control law Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Better comment in test Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Update setSpeedLimits with angular vel max Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Fix SpeedLimits Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Fixes in vel and collision Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Fix backward motion Signed-off-by: Alberto Tudela <ajtudela@gmail.com> --------- Signed-off-by: Alberto Tudela <ajtudela@gmail.com>
* Initial commit Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Fix egopolar and add compute next pose Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Added simulated trajectory Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Added slowdown publisher Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Added first tests Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Added basic tests and smoth control Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Added initial rotation Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Added final rotation Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Added collision Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Improve last motion target Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Added new tests Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Set min velocity Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Added EOL Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Path test Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Add utils and minor fixes Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Update footprint calculation Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Added more tests Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Added nav2_controller to package test Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Improve comments Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Added backward motion Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Split in two libraries Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Improve rotation velocity Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Update documentation Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Minor fixes Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Revert collision_checker Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Pass costmap transform to simulate trajectory Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Retain old headers Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Update dyn parameters of control law Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Better comment in test Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Update setSpeedLimits with angular vel max Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Fix SpeedLimits Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Fixes in vel and collision Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Fix backward motion Signed-off-by: Alberto Tudela <ajtudela@gmail.com> --------- Signed-off-by: Alberto Tudela <ajtudela@gmail.com>
* Initial commit Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Fix egopolar and add compute next pose Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Added simulated trajectory Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Added slowdown publisher Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Added first tests Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Added basic tests and smoth control Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Added initial rotation Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Added final rotation Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Added collision Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Improve last motion target Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Added new tests Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Set min velocity Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Added EOL Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Path test Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Add utils and minor fixes Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Update footprint calculation Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Added more tests Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Added nav2_controller to package test Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Improve comments Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Added backward motion Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Split in two libraries Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Improve rotation velocity Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Update documentation Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Minor fixes Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Revert collision_checker Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Pass costmap transform to simulate trajectory Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Retain old headers Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Update dyn parameters of control law Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Better comment in test Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Update setSpeedLimits with angular vel max Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Fix SpeedLimits Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Fixes in vel and collision Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Fix backward motion Signed-off-by: Alberto Tudela <ajtudela@gmail.com> --------- Signed-off-by: Alberto Tudela <ajtudela@gmail.com> Signed-off-by: stevedanomodolor <stevedan.o.omodolor@gmail.com>
Basic Info
Description of contribution in a few bullet points
Description of documentation updates required from your changes
Future work that may be required in bullet points
For Maintainers: