Ceres smoother #2753

robotechvision · 2021-12-22T10:20:59Z

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	#2498
Primary OS tested on	Ubuntu
Robotic platform tested on	(hardware RTV Caster, gazebo simulation of RTV Caster)

Description of contribution in a few bullet points

Original Ceres smoother from Smac planner put into operational state by simplifying to AutoDiff and fixing errors
Added support for forward/reverse motion (e.g. Reeds-Shepp model)
Added cubic interpolator for costmap cost
Curvature cost simplified and generalized to be valid for segments with non-equal lengths
Added option to add extra costmap cost near cusps (forward/reverse direction changes) which are difficult to be smoothed properly using basic cost parameters
Added option to read costmap cost from different points of robot, which can be useful for arbitrary robot shapes (e.g. diff-drive with caster wheels has footprint offset backwards against rotation center)
Added option to downsample path prior to smoothing (for speedup) and upsample back to original (or even higher) density using cubic bezier
Detailed tests written to check for smoother attributes to be maintained when optimizing/modifying smoother in the future
Added TruncatePathLocal BT action to pick a section from the global plan so that smoothing can be performed with a good quality in a reasonable time (can be used also for other purposes)

Note: We will check for reviews soonest on 2022-01-10 so no need to hurry with them. Merry Christmas

Description of documentation updates required from your changes

Added new smoother plugin (CeresCostawareSmoother), it should be added to docs
Added new BT plugin (TruncatePathLocal), it should be added to docs

Future work that may be required in bullet points

Try to find an optimizer configuration which converges faster
Try to write down proper derivatives which can speed up optimization significantly, maintaining (or improving) smoothing quality (see tests)

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

…sion checker and footprint subscriber

and path collision checking

into smoother

… added

non-existing plugins

…dded into bringup

nav2_smoother.cpp test coverage

SteveMacenski

Overall, LGTM, I skimmed over [nav2_ceres_costaware_smoother/smoother.hpp](https://github.com/ros-planning/navigation2/pull/2753/files#diff-d237e6158151d23832965fb29cd112aa21aa3f09a966907ab7b1aca076a56a46) since alot of that logic is related to the cusp handling I requested some clarification regarding. Not worth trying to pull that apart without more context. It may be worth adding some more inline comments regarding this since its a bit different.

But otherwise, I went through the rest of the files and except where noted, this is really nice looking and not much I would ask for change -- mostly superficial.

Can you also mention in the readme a few examples of paths being smoothed with the costmap being shown & runtime / length metrics alongside that? Pictures / metrics tell a thousand words

nav2_ceres_costaware_smoother/include/nav2_ceres_costaware_smoother/smoother_cost_function.hpp

nav2_ceres_costaware_smoother/src/ceres_costaware_smoother.cpp

nav2_ceres_costaware_smoother/include/nav2_ceres_costaware_smoother/options.hpp

nav2_ceres_costaware_smoother/include/nav2_ceres_costaware_smoother/smoother.hpp

following commit to prevent git clutter)

SteveMacenski · 2022-03-29T22:36:42Z

I merged your docs PR, I'll fix the remaining items from the last review and add in the "simple" smoother plugin from #2875, which is just a port from the simple Smac smoother into this plugin form.

nav2_ceres_costaware_smoother/package.xml

nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/truncate_path_local_action.hpp

nav2_behavior_tree/plugins/action/truncate_path_local_action.cpp

nav2_ceres_costaware_smoother/include/nav2_ceres_costaware_smoother/utils.hpp

nav2_ceres_costaware_smoother/src/ceres_costaware_smoother.cpp

SteveMacenski · 2022-03-31T19:01:45Z

OK - we can do the BT node at the same time, reviewed that and went deeper into the code after you explained the cusp stuff to me

tests

SteveMacenski · 2022-04-04T23:29:54Z

Glancing over the open review comments, there aren't many really significant change requests - mostly just documentation / clarification in comments and a few little changes based on utilities we have in nav2_utils that would make it easier to understand / use the same methods for similar actions. Great work on improving this! I really wanted to get this with analytical derivatives, but I suppose as you found as well it was difficult to get that to work well.

I re-read the code again this afternoon and have no new comments to make. I think once these are done this is good to go (with docs, of course)

normalization

SteveMacenski · 2022-04-06T17:44:52Z

The remaining comments are w.r.t. documentation for the website / readme and the Truncate BT node. The smoother is A-OK with me now.

nav2_behavior_tree/plugins/action/truncate_path_local_action.cpp

nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/truncate_path_local_action.hpp

SteveMacenski

This has gotten pretty convoluted in the truncation node. I think it would be better if we handled the BT node in another PR so that we can ship the actual smoother work which is completed except for the documentation elements on navigation.ros.org. There are probably going to be a few more iterations required given the current state of this BT node but the smoother is approved.

If only all users wanted to do roughly-exact path tracking instead of dynamic behaviors... it would make all of our lives so much easier... 😆

nav2_behavior_tree/plugins/action/truncate_path_local_action.cpp

detection without pose orientations assigned

SteveMacenski

LGTM, I think at this point its just on documentation / adding in the diagrams / descriptions we've previously discussed in the still-open discussion points (you may need to expand the shortened view git has due to the number of comments on the PR). The check list on the top of the PR for maintainers goes through the documentation elements that be updated + the diagrams we previously discussed.

I did another run through of the smoother code and I have no new comments / requests. This all looks ready to go to me!

If you rebase to main / pull it in, CI should turn over with fewer errors.

SteveMacenski · 2022-04-28T18:21:48Z

Any update? I think its just about an hour of writing up the documentation around this for the website! Docs help other users and gives you a better platform for users to use this work if they can understand how it works / what dials they have to play with (more users = more people to find issues = more fixes without our explicit effort)

afrixs · 2022-05-02T07:11:23Z

Agreed, I'll get to it today or tomorrow

SteveMacenski · 2022-05-10T17:55:31Z

Thanks for your dedication and help in getting this in! It's much appreciated and one of the new crown jewels in Nav2! I'll make an announcement on Discourse today on its inclusion

afrixs · 2022-05-10T18:39:01Z

Thank you very much for your notes and experiences, I think we've together found a solution I'm satisfied with much more than with our original version.

* old version of smoother server with path truncation and without collision checker and footprint subscriber * Removed path truncation from smoother server, added footprint subscriber and path collision checking * smoother code styling fixes * Update nav2_tree_nodes.xml * added TruncatePathLocal BT action * truncate_path_local_action bt plugin code styling * WIP: PR ros-navigation#2569 fixes * smoother server changes as requested in PR ros-navigation#2569, tests added * cpplint fixes * added TruncatePathLocal into bt configs * added nav2_smoother to composed bringup * smoother server: improved test coverage, details fixed * removed smoother from composed bringup lifecycle nodes as it fails on non-existing plugins * removed smoother server default plugin since it's not merged yet, readded into bringup * nav2_smoother tests: added mocked class loader to improve nav2_smoother.cpp test coverage * nav2_smoother 100% test coverage * test_smoother_server cpplint fix * test_smoother_server added cleanup transition * nav2_smoother added default plugin type autoconfig test * added nav2_ceres_costaware_smoother * changed rtv_ to nav2_ * Update nav2_smoother/README.md * nav2_ceres_costaware_smoother cleanup * removed smoother_server_rclcpp_node from nav2_params.yaml, added smoother_server to other config YAML files * registered nav2_smoother component * registered nav2_smoother component * fixed header uniquity macros * fixed nav2_smoother_core to be shared * fixed nav2_smoother build * minor cleanup, test interface added * added smoothness, distance and curvature tests * added more tests * ceres smoother cleanup, minor optimizations * ceres smoother: fixed interpolator cleanup bug, added tests, more code cleanup * ceres smoother fixes and tests * truncate_path_local bt node: changed API to be more similar to other nav2 bt nodes, tests written nav2_ceres_costaware_smoother: changed ASSERT to EXPECT in tests * ceres smoother uncrustified, fixed linting errors * updated readme * ceres smoother: updated readme * fixed signed-unsigned comparison error * removing obsolete max_time parameter since max_time is specified in smooth action goal * ceres smoother: updated readme * Update options.hpp * by-passing path orientations approximation when skipping smac smoother * moved smac smoother by-pass directly to smooth method * fixed smac smoother tests * removed debug logger * Removed merge bug from nav2_smoother/package.xml * ceres smoother: added keep start/goal orientation parameters * ceres smoother: added tests for start/goal orientations * ceres smoother: resolved requested details (name will be changed in following commit to prevent git clutter) * unified point variable names, added tests for edge-cases * ceres smoother: added comments, made parameters more intuitive, improved tests * ceres smoother: adding coments and cost check point weights normalization * ceres smoother: renamed to constrained smoother * TruncatePathLocal BT node: cleaned up, added max_robot_pose_search_dist * TruncatePathLocal: improved readability, added comments * constrained smoother: added readme note about not supporting cusp detection without pose orientations assigned * constrained smoother: updated readme * Constrained smoother: fixed readme and comments * Constrained smoother ReadMe: added recommended usage note Co-authored-by: afrixs <matej.vargovcik@gmail.com> Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> Co-authored-by: Matej Vargovcik <vargovcik@robotechvision.com>

afrixs and others added 30 commits September 2, 2021 11:03

old version of smoother server with path truncation and without colli…

30a1bc7

…sion checker and footprint subscriber

Removed path truncation from smoother server, added footprint subscriber

0ebeffd

and path collision checking

Merge branch 'main' into smoother

4b30b93

smoother code styling fixes

ab08c88

Update nav2_tree_nodes.xml

e26601f

added TruncatePathLocal BT action

b999cec

truncate_path_local_action bt plugin code styling

ccc24a9

WIP: PR ros-navigation#2569 fixes

b60a6a9

Merge branch 'smoother' into ceres_smoother

2db99f5

Merge branch 'smoother' of https://github.com/robotechvision/navigation2

0a6d4f7

into smoother

smoother server changes as requested in PR ros-navigation#2569, tests…

45c8dc7

… added

Merge branch 'main' into smoother

4e4bf96

cpplint fixes

2155524

Merge branch 'smoother' into ceres_smoother

d6cf474

added TruncatePathLocal into bt configs

e272c4f

added nav2_smoother to composed bringup

8a45a26

smoother server: improved test coverage, details fixed

e51ffff

removed smoother from composed bringup lifecycle nodes as it fails on

647c8bf

non-existing plugins

removed smoother server default plugin since it's not merged yet, rea…

3412f5e

…dded into bringup

nav2_smoother tests: added mocked class loader to improve

9567180

nav2_smoother.cpp test coverage

nav2_smoother 100% test coverage

8956645

test_smoother_server cpplint fix

6261768

Merge branch 'smoother' into ceres_smoother

888ada8

test_smoother_server added cleanup transition

a97e2d5

nav2_smoother added default plugin type autoconfig test

d1df4c7

Merge branch 'smoother' into ceres_smoother

b769b31

added nav2_ceres_costaware_smoother

6967e05

changed rtv_ to nav2_

9735e80

Update nav2_smoother/README.md

f67f81d

nav2_ceres_costaware_smoother cleanup

7a7ffd8

SteveMacenski reviewed Mar 28, 2022

View reviewed changes

Matej Vargovcik added 2 commits March 29, 2022 14:30

ceres smoother: added tests for start/goal orientations

fdf483b

ceres smoother: resolved requested details (name will be changed in

bd4ec4c

following commit to prevent git clutter)

unified point variable names, added tests for edge-cases

9dc74c0

SteveMacenski reviewed Mar 31, 2022

View reviewed changes

nav2_ceres_costaware_smoother/package.xml Show resolved Hide resolved

SteveMacenski reviewed Mar 31, 2022

View reviewed changes

ceres smoother: added comments, made parameters more intuitive, improved

170d14f

tests

Matej Vargovcik added 2 commits April 6, 2022 08:13

ceres smoother: adding coments and cost check point weights

32089ee

normalization

ceres smoother: renamed to constrained smoother

7195563

SteveMacenski reviewed Apr 6, 2022

View reviewed changes

nav2_behavior_tree/plugins/action/truncate_path_local_action.cpp Outdated Show resolved Hide resolved

nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/truncate_path_local_action.hpp Outdated Show resolved Hide resolved

TruncatePathLocal BT node: cleaned up, added max_robot_pose_search_dist

87e0947

SteveMacenski requested changes Apr 7, 2022

View reviewed changes

Matej Vargovcik added 3 commits April 13, 2022 09:48

TruncatePathLocal: improved readability, added comments

47ca35a

constrained smoother: added readme note about not supporting cusp

b429f0a

detection without pose orientations assigned

constrained smoother: updated readme

de33e78

SteveMacenski approved these changes Apr 13, 2022

View reviewed changes

Matej Vargovcik and others added 3 commits May 5, 2022 09:59

Merge branch 'main' into ceres_smoother

bda0623

Constrained smoother: fixed readme and comments

5570e62

Constrained smoother ReadMe: added recommended usage note

84557b1

SteveMacenski merged commit af3ff35 into ros-navigation:main May 10, 2022

Ceres smoother #2753

Ceres smoother #2753

Uh oh!

Conversation

robotechvision commented Dec 22, 2021 • edited by SteveMacenski Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

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:

SteveMacenski left a comment • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

SteveMacenski commented Mar 29, 2022

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

SteveMacenski commented Mar 31, 2022

SteveMacenski commented Apr 4, 2022 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

SteveMacenski commented Apr 6, 2022

Uh oh!

Uh oh!

SteveMacenski left a comment • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Uh oh!

SteveMacenski left a comment • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

SteveMacenski commented Apr 28, 2022

afrixs commented May 2, 2022

SteveMacenski commented May 10, 2022 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

afrixs commented May 10, 2022

Labels

3 participants

robotechvision commented Dec 22, 2021 •

edited by SteveMacenski

Loading

SteveMacenski left a comment •

edited

Loading

SteveMacenski commented Apr 4, 2022 •

edited

Loading

SteveMacenski left a comment •

edited

Loading

SteveMacenski left a comment •

edited

Loading

SteveMacenski commented May 10, 2022 •

edited

Loading