MPPI GoalCritic bug

[redvinaa]

Bug report

Required Info:

• Operating System:
• ROS2 Version:
• Version or commit hash:
  • main (16abf9b)
• DDS implementation:

Steps to reproduce issue

Publish critic scores and see when each of the critics is active. I've made a branch to do this, but I only have a dirty and forked branch on humble: https://github.com/EnjoyRobotics/navigation2/tree/humble-enjoy-mppi-critic-pub. (I also checked, and this is not fixed on main as of now.) Otherwise it can probably be easily checked by putting some prints in the critics code. In terms of the parameters, threshold_to_consider should be ~1-1.5 (assuming local costmap size is 3m), and everything else can stay default.

Expected behavior

MPPI GoalCritic should activate only when close to global goal.

Actual behavior

All critics have access only to the CriticData. In the GoalCritic (specifically the utils::withinPositionGoalTolerance), the goal is calculated from the last point of the CriticData.path. But this path is already the pruned one (pruning is by integrated distance), and if the path is very wavy, its end can end up close enough to the robot pose to trigger the GoalCritic's theshold_to_consider (and also of others', for example of PathFollow). I propose adding the global goal pose to the CriticData.

[SteveMacenski]

Sure thing! I support that change :-) Can you submit a PR against main and humble? I can backport to Jazzy from main once merged