Add footprint expansion to graceful controller

[Tacha-S]

---

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	#4865
Primary OS tested on	Ubuntu
Robotic platform tested on	hardware my robot
Does this PR contain AI generated software?	No
Was this PR description generated by AI software?	No

---

Description of contribution in a few bullet points

• I added footprint expansion to graceful controller
• I also added a new parameter final_rotation_tolerance to the prefer_final_rotation feature.

Description of documentation updates required from your changes

• Added new parameter, so need to add that to default configs and documentation page

Description of how this change was tested

I was tested in a real environment that included open spaces, narrow corridors, and corners.

---

Future work that may be required in bullet points

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
• Should this be backported to current distributions? If so, tag with backport-*.

[codecov]

Codecov Report

❌ Patch coverage is 93.93939% with 2 lines in your changes missing coverage. Please review.

Files with missing lines	Patch %	Lines
...v2_graceful_controller/src/graceful_controller.cpp	90.47%	2 Missing ⚠️

Files with missing lines	Coverage Δ
...e/nav2_graceful_controller/graceful_controller.hpp	100.00% <ø> (ø)
nav2_graceful_controller/src/parameter_handler.cpp	98.36% <100.00%> (+0.17%)	⬆️
...v2_graceful_controller/src/graceful_controller.cpp	86.88% <90.47%> (+1.94%)	⬆️

... and 20 files with indirect coverage changes

🚀 New features to boost your workflow:

• ❄️ Test Analytics: Detect flaky tests, report on failures, and find test suite problems.

[SteveMacenski]

It looks like this has some severe conflicts that need to be adjusted before I can review. The GitHub UX won't even let me see the conflicts.

You may be better off reopening on a new branch perhaps (but check first that its not easy)

[Tacha-S]

@SteveMacenski It seemed to conflict test codes with #5446, so I rebased and fixed it.

[SteveMacenski]

@mikeferguson would you be open to review this PR for footprint expansion based on velocity in the graceful controller?

[SteveMacenski]

Overall, looks straight forward and solid to me from a first look!

[SteveMacenski]

@Tacha-S sorry for the delay - small requests are made :- )

[SteveMacenski]

I think what's left is documentation. The new parameters need to be included in the graceful controller's configuration guide + entry into the migration guide to highlight this new feature!

[SteveMacenski]

Should we set this as default behavior on? If so, what is a reasonable value to set this as?

[Tacha-S]

Yes, I think it would be fine to enable it by default.

When I tested it, I used a value of 1.0,
which allows the footprint to expand up to twice its original size.
This may depend on the robot’s size and speed, but in general, the footprint_scaling_factor of 1.0 seems reasonable as a default value.

[SteveMacenski]

Twice the size seems a bit much, no? Twice the size over what velocity - I guess the scale of speed is relevant.

[Tacha-S]

That’s a fair point.

If the goal is to account for positional instability at higher speeds, then expanding the footprint up to twice its size even at maximum velocity may indeed be excessive.
I would also agree with a configuration where most robots only need an expansion of up to about 50%, or even less (around 20–30%), to be sufficiently safe.

For reference, my initial setting was based on the example shown toward the end of the parameter list in the graceful_controller README.

[SteveMacenski]

Lets do 25%, I think that's a reasonable trade off by default!

Thanks for iterating. I think update the defaults to use this + in the configuration guide mention that 1.0 means double (and that this can be thought of as the % increase w.r.t. full velocity). That's useful user tuning context to share

[SteveMacenski]

Is final orientation tolerance just the tolerance for the goal orientation? If so, then we should use the value from the goal checker.

Can you explain this block of code and why its useful here for documentation?

Is this in another controller(s)?

[Tacha-S]

This is slightly different from the goal-reaching condition.
The final_rotation_tolerance represents the allowable angular difference used to decide whether the approach to the goal can be replaced with a straight-line trajectory.
This is useful for preventing the local planner from independently deciding to move straight toward the goal even when an avoidance path has already been generated near the goal.
In other words, final_rotation_tolerance helps ensure that the planner respects the computed avoidance behavior in the vicinity of the goal rather than oversimplifying the motion.

[SteveMacenski]

Its common though for prefer_final_rotation to be used with algorithms like NavFn. These have the path to the goal and then the goal orientation itself be discontinuous to the path. It just drives to the goal using gradient descent and then sets the goal orientation at the goal pose for an in-place rotation or DWB/MPPI/Graceful to deal with the details. This would make it so that if the approach orientation vs goal orientation is too high, it bypasses the feature -- which I think are decoupled problems/features.

The prefer_final_rotation is the parameter to make it so that the robot will drive directly to the pose and then rotate in place to the final orientation smoothly rather than try to bake it into the curve. I think whether the orientation is set to be continuous from the path leading up to it or discontinuous, the intent of the parameter is to go directly to the pose regardless of the difference.

Is this an issue you were noticing with shortcutting corners at the end of the path? I think that is a consequence of setting prefer_final_rotation = true.

I agree that I would like to find a way to potentially bypass it in the case that we're really shortcutting a corner, I'm just not sure that this is the right way. I suppose we could compare on approach to the goal the path generated from the direct-approach vs he spiral curve and use some comparison function to decide which to use in that situation. For example, looking at length vs cost, or checking if the cost is very close to max cost but the curve one is much safer. Then, we use the safer one for that iteration. Then once we get past the corner, we would be able to go back to the straight line behavior. This would involve some surgery but would be relatively easy for me to review.

If you agree and think that we can do this as a follow up PR, I think this would make sense to be something we chat about (and a very good next contribution!) and implement separate of this work so we can get this PR in. I think its very useful and I know I've been leaving you hanging for some time -- so I want to give you a win 😄 I dont believe (?) this is critical for the footprint expansion part of the work, right?

[Tacha-S]

I agree.
For this PR, it makes sense to focus only on the footprint expansion and keep the scope limited.

I’ve reverted the changes related to shortcutting behavior, and I’ve also updated the default value of the scaling factor.

For the shortcutting behavior, I’ll follow up with a separate PR and propose an improvement along the lines you suggested (e.g., comparing direct-approach vs curved paths near the goal and selecting the safer option when appropriate).

[SteveMacenski]

I think just now the docs for the new parameters and migration guide to reference this new feature and we're good to go!

[SteveMacenski]

@Tacha-S pull in main to make CI pass. I want to check the code coverage metrics before merging

[SteveMacenski]

Thanks so much for this feature addition, I'm sure it'll be useful for many Nav2 users!

Any interest in other improvements to Graceful from the ticket (or otherwise) or to other parts of the stack? I'd be happy to help find other places to contribute :-)