Use integral path distance for MPPI Critics

[tonynajjar]

---

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	#5484
Primary OS tested on	Ubuntu
Robotic platform tested on	private sim
Does this PR contain AI generated software?	here and there, not marked (does it matter at this point?)
Was this PR description generated by AI software?	Out of respect for maintainers, AI for human-to-human communications are banned

---

Description of contribution in a few bullet points

• Add plan_length and plan_length_up_to_inversion and have them computed in the path_handler and passed to the critics data
• Use one of these paths (according enforce_path_inversion) in the critics

Description of documentation updates required from your changes

Description of how this change was tested

---

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

[tonynajjar]

@claude review the PR

[claude]

Claude encountered an error —— View job

Failed with exit code 128

---

I'll analyze this and get back to you.

[tonynajjar]

Tests probably still need fixing but let's start with functionality review first

[SteveMacenski]

I'll review, but you got some test failures which makes me suspicious that there are functional problems with this approach if you're getting 0s where you have have non-zeros.

[Copilot]

Pull Request Overview

This PR introduces integral path distance calculations for MPPI critics instead of relying on Euclidean distance calculations. The change aims to improve path following accuracy by using actual path length measurements from the robot's current position to the goal.

• Add plan_length and plan_length_up_to_inversion fields to the Path model and calculate them in the path handler
• Update all critics to use path distance instead of Euclidean distance for goal proximity checks
• Modify the optimizer interface to accept and pass through the calculated path lengths

Reviewed Changes

Copilot reviewed 19 out of 19 changed files in this pull request and generated 2 comments.

Show a summary per file

File	Description
nav2_mppi_controller/include/nav2_mppi_controller/models/path.hpp	Add plan length fields to Path struct
nav2_mppi_controller/include/nav2_mppi_controller/tools/path_handler.hpp	Add methods for path length calculation and retrieval
nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp	Update optimizer interface to accept plan lengths
nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp	Add utility function for getting critic goal path distance
nav2_mppi_controller/src/path_handler.cpp	Implement path length calculation logic
nav2_mppi_controller/src/optimizer.cpp	Update optimizer to use new plan length parameters
nav2_mppi_controller/src/controller.cpp	Pass calculated plan lengths to optimizer
nav2_mppi_controller/src/critics/*.cpp	Update critics to use path distance instead of Euclidean distance
nav2_mppi_controller/test/*.cpp	Update tests to work with new optimizer interface
nav2_mppi_controller/benchmark/optimizer_benchmark.cpp	Update benchmark to use new optimizer interface

---

_{Tip: Customize your code reviews with copilot-instructions.md. Create the file or learn how to get started.}

[SteveMacenski]

You have

I honestly have no idea what I started typing here

[codecov]

Codecov Report

❌ Patch coverage is 93.33333% with 1 line in your changes missing coverage. Please review.

Files with missing lines	Patch %	Lines
...v2_mppi_controller/src/critics/twirling_critic.cpp	75.00%	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% <ø> (ø)
...ller/include/nav2_mppi_controller/models/state.hpp	100.00% <ø> (ø)
... and 12 more

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

DCO sign off 😉 Otherwise LGTM if you don't have any concerns from your testing

[SteveMacenski]

Why is this one special to use the goal checker?

[tonynajjar]

I don't know, question right back at you; seems like it's been like this forever

[SteveMacenski]

Oh this was just wrapped into the util function, got it. I thought you introduced this, not refactoring it

[tonynajjar]

LGTM if you don't have any concerns from your testing

We haven't migrated to MPPI yet in production so the testing was limited to simulation. Would be great if OP @bektaskemal could also give it a test
But I think we can merge in the meantime

[SteveMacenski]

@bektaskemal please test and let us know how it goes! :-)

[sebatztian]

@tonynajjar for my clarity, this implementation only works as intended, as long as the threshold to consider is smaller than the pruning distance and local costmap bounds, right?

[tonynajjar]

this implementation only works as intended

Not sure what you mean by that, because what is intended is that the critic is activated based on which one of threshold_to_consider and the local_path_length is greater. if threshold_to_consider is huge then some critics will always be activated and some others not.

[sebatztian]

Okay, thanks for the clarification.
I tried to implement the same functionality as a Critic Plugin, where I could only use the pruned path, since the global path is not handed to the critics.
So I was interested in your solution, and wanted to compare.
I guess in theory we would like to check the actual remaining path length, but I am on the same page, that greater threshold distances make little sense.