Better management of RPP path pruning when cusp points are detected

[andreacelani]

---

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	(#4757)
Primary OS tested on	(Ubuntu 22.04.5)
Robotic platform tested on	(custom Gazebo simulation with Ackermann model)
Does this PR contain AI generated software?	(No)

---

Description of contribution in a few bullet points

the contribution make RPP controller follows better the path given by planner. There is no skip of path points even with tight maneuvers. This apply to all models (diff drive, ackermann and omni)

fixed RPP.webm

Description of documentation updates required from your changes

Added the following parameters. They are the same found in MPPI controller

• inversion_xy_tolerance
• inversion_yaw_tolerance

the parameters are useful to terminate an inversion and start the following the next portion of the path

---

Future work that may be required in bullet points

original RPP coding style has been follow (one single file cpp file). MPPI has more structured organization with a dedicated path_handler.cpp file -->

For Maintainers:

• [ ] Check that any new parameters added are updated in docs.nav2.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

[mergify[bot]]

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

[mergify[bot]]

@andreacelani, all pull requests must be targeted towards the main development branch. Once merged into main, it is possible to backport to @humble, but it must be in main to have these changes reflected into new distributions.

[SteveMacenski]

@andreacelani any update?

[andreacelani]

@SteveMacenski sorry I had not time, heavy period at work! :sweat_smile: I'm moving to main and add tests to new methods

[SteveMacenski]

Thank you @andreacelani :heart:

Just checking in :-)

[andreacelani]

@SteveMacenski I trying to switch to Rolling but my ROS2 is humble. Any advice/workaround to build the main (rolling) while in humble? Or should I install everything in a VM? Thank you

[SteveMacenski]

Use a docker container, typically. OSRF has Jazzy / Rolling docker images that you can mount your workspace into. I have some docs on it if you need help setting up https://docs.nav2.org/tutorials/docs/docker_dev.html

[SteveMacenski]

@andreacelani I'm hoping this is something you can get done this month, are you able to?

[andreacelani]

I hope so, I'm really slowly but I'm working on it. Now I'm fighting with PoseStampedArray message that you can find in Nav2 rolling but not in Ros2 rolling docker image. I'll get out of it 😅

[SteveMacenski]

What I did was clone geometry2 into my workspace, then colcon build --packages-up-to geometry_msgs --parallel-workers 1 and then source the workspace, finally colcon build the full space and you should be good to go (its annoying, but getting that built and sourced first makes later things use the workspace version of the package on the next re-build)

[mikeferguson]

I wasn't using the docker images - but I found I had to delete the build/install folders after adding common_interfaces to the workspace - my hypothesis is that there was some left over Cmake generated files that were still pointing to /opt/ros for geometry_msgs stuff (kept getting errors with no header found for PoseStampedArray). Once I cleaned out the build/install folders, build ran fine (even doing it all at once, with unlimited workers)

[SteveMacenski]

Indeed, I would also expect that, sorry for not mentioning that :frowning:

[mini-1235]

I have been working on this issue the past few days to push https://github.com/ros-navigation/navigation2/pull/5446 forward, I think I am able to see a before-and-after difference here.

Should I:

• Open a new PR targeting main to add this in RPP?
• Add this feature in #5446 directly?

[SteveMacenski]

If the PRs aren't directly attached to one another, a different PR would be good to keep things small and sharp. Thanks!

Are there other controllers that should have it updated too? I think MPPI has this already, so DWB, Graceful?

[mini-1235]

If the PRs aren't directly attached to one another, a different PR would be good to keep things small and sharp. Thanks!

To add this feature in RPP, we will need to add a couple parameters. Like MPPI, I think we can refactor the parameter handler in RPP, and pass it to the path handler? Or even move the parameter handler to nav2_util, so we can have a nav2_util::ParameterHandler object in every controller?

Are there other controllers that should have it updated too? I think MPPI has this already, so DWB, Graceful?

I think so. But, to be honest, I haven’t worked with DWB or Graceful before, so I would appreciate your advice on how best to approach those

[SteveMacenski]

@mini-1235 yeah that sounds good! Lets start with RPP but if we find that they are exactly the same, then abstracting them might be useful, especially if we look at DWB and Graceful and find that we could use them there too.

I haven’t worked with DWB or Graceful before, so I would appreciate your advice on how best to approach those

I'd look at their respective path handlers (https://github.com/ros-navigation/navigation2/blob/main/nav2_graceful_controller/src/path_handler.cpp) and see if this would mold well into it or its doing anything unique / unique inputs or outputs that need to be considered. I'm guessing the answer is 'no'

[SteveMacenski]

Closing this for now due to lack of response from the original author. Some work is happening in Main that includes this feature so it is being addressed another way. If the author would like to implement the review comments, happy to re-review and continue from there!