fix MPPI goal critic inversion (#5088)

[brayanpa]

---

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	(#5088)
Primary OS tested on	(Ubuntu 24.04.2 LTS)
Robotic platform tested on	(gazebo simulation, Gary Robot Hardware)
Does this PR contain AI generated software?	(No)

---

Description of testing performed

colcon testing

Description of contribution in a few bullet points

• Added logic to the GoalCritic in the MPPI controller to check the enforce_path_inversion parameter.
• When enforce_path_inversion is set to true, the end of the path is used instead of the goal pose to compute costs.
• This resolves cases where the robot would get stuck near the goal in an incorrect orientation due to undetected path inversion, especially when using planners that generate feasible paths.

Description of how this change was tested

Tested in simulation and on a real robot across multiple maps and scenarios where feasible planners produced paths that passed near the goal before performing an inversion to reach it with the correct orientation.

Previously, the GoalCritic would interpret this proximity as having reached the goal, preventing further progress and causing the robot to get stuck.

With the new logic using the path endpoint instead of the goal pose when enforce_path_inversion is true, the robot was able to continue following the path and reach the goal correctly.

All test cases confirmed that the robot no longer gets stuck near the goal due to orientation mismatches caused by undetected path inversions.

---

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

[mergify]

@brayanpa, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

[mergify]

@brayanpa, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

[SteveMacenski]

I think this should be using the name_ getter instead. This is a goal critic plugin specific parameterization

[SteveMacenski]

Should withinPositionGoalTolerance use the back of the path as well in this case @BriceRenaudeau? Possibly need to be considered for other critic plugins as well

[brayanpa]

I think it would be good to add it to the goal_angle_critic. Let me know if you think it should be added to any others as well.

Regarding adding it to withinPositionGoalTolerance, I think the best approach would be to create a parameter in each critic plugin to choose whether to use the existing withinPositionGoalTolerance logic or a new one that considers path inversions. What do you think?

[SteveMacenski]

@brayanpa I think you're probably right that we should have enforce_path_inversion be a plugin-level parameter that is used in the individual plugins as-needed.

I think Goal and GoalAngle are obvious ones that need to look at that and use the goal vs local goal for both is computation, but also its within tolerance check.

The others are Cost, Obstacles, Twirl, Path Align, Path Follow, Path Angle, and Prefer Forward.

The Path Align/Follow/Angle have natural hand-offs from the Goal and GoalAngle critics, so anything that the Path critics sees the Goal critics also need to see. If intermediary goals for path inversions are near obstacles like the goal, I think the Cost and Obstacles critics probably should have it too. At that point, lets just make it universal for consistency.

So, I think the first thing we should do in each critics' score method is to check on the inversion parameter and store the data.goal or data.path(-1) for the goal to check. Then that should be used for withinPositionGoalTolerance and for the computation of the critic value where necessary.

A small change to the README and parameter guide should be made to make it clear this is used by both the path handler for enforcing path inversions and the critics' behaviors that these are treated as local goals to achieve.

[brayanpa]

I have added the enforce_path_inversion parameter to all critics and modified the goal between the end of the path and the original goal accordingly in the necessary critics. In the tests I performed, it is working very well. The robot is able to handle path inversion correctly, and when enforce_path_inversion is set to false, it behaves just like before.

I'll be on the lookout in case any further changes are needed, or if you think any additional clarification is required in the README or documentation.

[mergify]

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

[codecov]

Codecov Report

Attention: Patch coverage is 76.40449% with 21 lines in your changes missing coverage. Please review.

Files with missing lines	Patch %	Lines
...oller/include/nav2_mppi_controller/tools/utils.hpp	0.00%	11 Missing ⚠️
...v2_mppi_controller/src/critics/twirling_critic.cpp	77.77%	2 Missing ⚠️
nav2_mppi_controller/src/critics/cost_critic.cpp	83.33%	1 Missing ⚠️
..._mppi_controller/src/critics/goal_angle_critic.cpp	90.00%	1 Missing ⚠️
nav2_mppi_controller/src/critics/goal_critic.cpp	90.90%	1 Missing ⚠️
...2_mppi_controller/src/critics/obstacles_critic.cpp	83.33%	1 Missing ⚠️
..._mppi_controller/src/critics/path_align_critic.cpp	88.88%	1 Missing ⚠️
..._mppi_controller/src/critics/path_angle_critic.cpp	87.50%	1 Missing ⚠️
...mppi_controller/src/critics/path_follow_critic.cpp	90.00%	1 Missing ⚠️
...i_controller/src/critics/prefer_forward_critic.cpp	88.88%	1 Missing ⚠️

Files with missing lines	Coverage Δ
...clude/nav2_mppi_controller/critics/cost_critic.hpp	81.25% <ø> (ø)
...nav2_mppi_controller/critics/goal_angle_critic.hpp	100.00% <ø> (ø)
...clude/nav2_mppi_controller/critics/goal_critic.hpp	100.00% <ø> (ø)
.../nav2_mppi_controller/critics/obstacles_critic.hpp	100.00% <ø> (ø)
...nav2_mppi_controller/critics/path_align_critic.hpp	100.00% <ø> (ø)
...nav2_mppi_controller/critics/path_angle_critic.hpp	100.00% <ø> (ø)
...av2_mppi_controller/critics/path_follow_critic.hpp	100.00% <ø> (ø)
..._mppi_controller/critics/prefer_forward_critic.hpp	100.00% <ø> (ø)
...e/nav2_mppi_controller/critics/twirling_critic.hpp	100.00% <ø> (ø)
nav2_mppi_controller/src/critics/cost_critic.cpp	85.52% <83.33%> (-0.39%)	⬇️
... and 9 more

... and 4 files with indirect coverage changes

🚀 New features to boost your workflow:

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