Bug report

Required Info:

• Operating System:
  • ubuntu 24.04
• Computer:
  • 13th Gen Intel NUC
• ROS2 Version:
  • Jazzy
• Version or commit hash:
  • main repository
• DDS implementation:
  • Fast-RTPS

Steps to reproduce issue

The MPPI controller's GoalCritic behavior, modified in PR #4822, causes the robot to ignore local path inversions and focus solely on the global goal.

1. Set up a Nav2 environment with the MPPI controller.
2. Create a path with an inversion point (e.g., in a narrow aisle requiring a 180-degree turn).
3. Send a goal pose with an orientation opposite to the robot's current direction.
4. Observe the robot bypassing the inversion point and moving directly to the final goal.

Expected behavior

The robot should respect path inversions, navigating to intermediary goals (e.g., inversion points) when required, before approaching the final goal.

Actual behavior

The robot to ignore local path inversions and focus solely on the global goal.

Additional information

Proposed Solution:
Default to using the "real" (global) goal for GoalCritic scoring and other critics.

If a path inversion is detected and inversions are considered (based on a parameter), use the intermediary goal (end of the pruned path or inversion point).