[Graceful Controller] Enhance approach goal logic

[Tacha-S]

---

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	#5926
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

• Implemented a mechanism to compare the direct final rotation trajectory against alternative spiral curve approaches when the direct path is close to obstacles.
• Add obstacle_cost_margin and final_rotation_search_step parameters
  • Introduced obstacle_cost_margin to define the range of cost values considered as "close to obstacles," which triggers the search for safer alternatives.
  • final_rotation_search_step to control the granularity of the search for alternative spiral approaches.

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 95.23810% with 3 lines in your changes missing coverage. Please review.

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

Files with missing lines	Coverage Δ
...e/nav2_graceful_controller/graceful_controller.hpp	100.00% <ø> (ø)
nav2_graceful_controller/src/parameter_handler.cpp	98.47% <100.00%> (+0.11%)	⬆️
...v2_graceful_controller/src/graceful_controller.cpp	89.51% <94.44%> (+2.63%)	⬆️

... and 12 files with indirect coverage changes

🚀 New features to boost your workflow:

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

[Tacha-S]

I will check test case, as it seems to be passing without actually testing the intended behavior.

[SteveMacenski]

This looks so much better, thanks for iterating!

[Tacha-S]

Sorry for the delay.
I have addressed the review comments in 943a9e2.

Additionally, during the safe approach to the goal, the optimal angle can change at each control cycle, which may cause chattering.
To mitigate this, once a safe path is found, we prioritize the path with that angle unless the global path is updated. (
d79ebfc)

[SteveMacenski]

Additionally, during the safe approach to the goal, the optimal angle can change at each control cycle, which may cause chattering.

Nice -- do you have a before and after video of that? I'm just curious mostly.

[SteveMacenski]

Otherwise LGTM. These are very pedantic

[Tacha-S]

This video shows the system behavior prior to the chattering mitigation. The black line indicates the trajectory generated by the graceful controller.

chattering_approach.webm

After implementing the mitigation, the response changes as shown below.
Since the global path is updated at 1 Hz, the approach angle is updated at that timing.

stable_approach.webm

[SteveMacenski]

Make sure to add a docs update for the configuration guide at docs.nav2.org

[SteveMacenski]

Also your test has failed:

/opt/overlay_ws/src/navigation2/nav2_graceful_controller/test/test_graceful_controller.cpp:1058
Expected: (std::abs(cmd_vel_risky.twist.angular.z)) > (1e-3), actual: 0 vs 0.001

/opt/overlay_ws/src/navigation2/nav2_graceful_controller/test/test_graceful_controller.cpp:1058
Expected: (std::abs(cmd_vel_risky.twist.angular.z)) > (1e-3), actual: 0 vs 0.001

[SteveMacenski]

Make sure to add a docs update for the configuration guide at docs.nav2.org

I think its good to go once we have that!

[SteveMacenski]

Merged! Thanks so much for following up and fixing this as well! Any interest in more graceful improvements (or elsewhere? would like to have your help where you have interest!) 😉