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).

[mergify]

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

[codecov]

Codecov Report

Attention: Patch coverage is 88.76404% with 10 lines in your changes missing coverage. Please review.

Files with missing lines	Patch %	Lines
...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% <ø> (ø)
...oller/include/nav2_mppi_controller/tools/utils.hpp	97.85% <100.00%> (+0.10%)	⬆️
... and 9 more

... and 7 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]

Pedantic stuff for code coverage and readability, this is good to merge after this.

thanks so much!

For docs: Updating the docs.nav2.org configuration guide page for MPPI with what you added to the readme would be fabulous

[SteveMacenski]

How about just goal (here and all other cases)?

[SteveMacenski]

I believe this works with just a geometry_msgs/Quaternion too

navigation2/nav2_behaviors/plugins/spin.cpp

Line 86 in 6e58fbe

prev_yaw_ = tf2::getYaw(current_pose.pose.orientation);

[SteveMacenski]

I think we can abstract this even more to a util:

geometry_msgs::msg::Pose getCriticGoal(data, enforce_path_inversion)
{
  if (enable_path_inversion) {
    return getLastPathPose(data.path);
  } else {
    return data.goal;
  }
}

that would make that function testable as a unit and we don't have all the missing coverage warnings in the individual critics for this block since we'd just call that one util in each to return the goal extracted

[BriceRenaudeau]

could be a good option 👍