MPPI: visualize trajectory per-critic + total costs by tonynajjar · Pull Request #6036 · ros-navigation/navigation2

tonynajjar · 2026-03-16T15:38:55Z

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	(add tickets here #1)
Primary OS tested on	(Ubuntu, MacOS, Windows)
Robotic platform tested on	(Steve's Robot, gazebo simulation of Tally, hardware turtlebot)
Does this PR contain AI generated software?	(No; Yes and it is marked inline in the code)
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

This pull request introduces enhancements to the visualization and introspection capabilities of the MPPI controller, allowing for per-critic cost breakdowns and improved candidate trajectory visualization. The main is enabling the visualization of trajectories colored by cost gradients.

Added per-critic cost storage and retrieval in CriticManager, including methods to enable/disable this feature and access critic names and cost breakdowns. This allows for introspection of individual critic contributions to trajectory scoring.
Modified Optimizer to expose total and per-critic costs, and to control per-critic cost storage for visualization purposes.
Extended TrajectoryVisualizer to support visualizing candidate trajectories colored by total and per-critic costs, including methods for cost normalization, color mapping, and checking for active subscribers.
Updated the controller logic to conditionally enable per-critic cost storage only when visualization subscribers are present, and to visualize candidate trajectories with cost breakdowns.

Description of documentation updates required from your changes

Description of how this change was tested

Future work that may be required in bullet points

Filter out outliers (e.g. trajectories in collision) that skew the visualization

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

Signed-off-by: Tony Najjar <tony.najjar@dexory.com>

tonynajjar · 2026-03-16T15:41:10Z

Filter out outliers (e.g. trajectories in collision) that skew the visualization

TBD if we do it in this PR or another. I want to keep this PR as simple as possible and focus on the piping of the data

Copilot

Pull request overview

This PR enhances Nav2’s MPPI controller introspection by exposing total and per-critic costs from the optimizer and adding a new candidate-trajectory visualization mode that colors trajectories by cost (total + per-critic layers).

Changes:

Added per-critic cost storage/access in CriticManager and plumbed it through Optimizer.
Extended TrajectoryVisualizer with a cost-colored candidate trajectory marker mode and subscriber detection.
Updated controller loop to enable per-critic cost storage only when visualization subscribers are present and to publish cost-layer visualizations.

Reviewed changes

Copilot reviewed 6 out of 6 changed files in this pull request and generated 7 comments.

Show a summary per file

File	Description
nav2_mppi_controller/src/trajectory_visualizer.cpp	Implements cost normalization, cost-to-color mapping, and LINE_STRIP marker generation for cost-colored candidate trajectories.
nav2_mppi_controller/include/nav2_mppi_controller/tools/trajectory_visualizer.hpp	Declares new visualization APIs (cost-colored add overload, subscriber check, helpers).
nav2_mppi_controller/src/critic_manager.cpp	Adds optional per-critic scoring bookkeeping and uses it to publish critic stats.
nav2_mppi_controller/include/nav2_mppi_controller/critic_manager.hpp	Exposes per-critic cost storage controls and getters; adjusts `evalTrajectoriesScores` signature.
nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp	Exposes total costs and per-critic costs from the optimizer; adds toggle for per-critic storage.
nav2_mppi_controller/src/controller.cpp	Enables per-critic storage only with active viz subscribers; switches visualization to cost-colored candidate trajectories.

You can also share your feedback on Copilot code review. Take the survey.

nav2_mppi_controller/include/nav2_mppi_controller/critic_manager.hpp

nav2_mppi_controller/src/critic_manager.cpp

nav2_mppi_controller/include/nav2_mppi_controller/critic_manager.hpp

nav2_mppi_controller/include/nav2_mppi_controller/tools/trajectory_visualizer.hpp

nav2_mppi_controller/src/trajectory_visualizer.cpp

SteveMacenski · 2026-03-16T17:11:03Z

Let me know with this and the other PRs on this topic what yo usee the path forward with when you're ready for me to review.

tonynajjar · 2026-03-16T17:13:50Z

@SteveMacenski maybe you missed it on slack:

I started again from scratch with the less intrusive approach and with as little "features" as possible to keep it simple to review. It's already very useful as is. I still have to fully go over it myself but you can already generally review and tell me if you find it less intrusive and a viable path forward

If this is viable, I'd close the other PR and ticket whatever improvement ideas we have in mind still.

P.S. I think this will be very useful for your MPPI debugging soon, so you can also give it a spin then and see what you would change/add

nav2_mppi_controller/src/controller.cpp

nav2_mppi_controller/include/nav2_mppi_controller/critic_manager.hpp

nav2_mppi_controller/include/nav2_mppi_controller/tools/trajectory_visualizer.hpp

nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp

SteveMacenski · 2026-03-16T17:40:38Z

What does the other PR contain that you think is core / needed that's missing here?

tonynajjar · 2026-03-16T17:57:15Z

What does the other PR contain that you think is core / needed that's missing here?

Nothing absolutely needed here, just more nice-to-have feature:

more metrics like standard deviation, influence ratio, etc...
visualization of some critics' furthest poses
filtering out of very high cost trajectories that skew the visualization

(maybe a couple more things, would need to recheck)

These can be follow-ups

Signed-off-by: Tony Najjar <tony.najjar@dexory.com>

SteveMacenski · 2026-03-16T18:05:33Z

standard deviation

I don't really see how I'd really use that intra-iteration TBH

influence ratio

That is interesting, we we make the layers normalized based on the total cost or something so the colors still in effect do that? To be honest though this as a trajectory-marker-color visualization isn't very helpful. That's more interesting for the published stats to put into charts. What - am I going to visualize N of these massive trajectory samples and be able to figure out what's going on with that mess for each namespace? Fat chance. That's a charting thing to see the influence ratio total for the iteration, not per-trajectory sample or per-point

But also maybe I'm misunderstanding what this is currently visualizing (N+1 total candidate replicas?) or how the coloring is applied.

visualization of some critics' furthest poses

This seems useful, but also as a separate PR

filtering out of very high cost trajectories that skew the visualization

Seems like this could be done here as well with the normalization

Signed-off-by: Tony Najjar <tony.najjar@dexory.com>

tonynajjar · 2026-03-16T18:26:04Z

I don't really see how I'd really use that intra-iteration TBH

It would tell how much a critic differentiates between trajectories: A high std dev means that critic is actively shaping the selection (some trajectories score much better than others). A std dev near zero means the critic scores all trajectories similarly and has little influence on the choice.

Seems like this could be done here as well with the normalization

Yeah I'll try it

Signed-off-by: Tony Najjar <tony.najjar@dexory.com>

This reverts commit c198a95.

tonynajjar · 2026-03-16T19:44:06Z

filtering out of very high cost trajectories that skew the visualization

I'm not finding a reliable way that doesn't use some fancy clustering algorithm. It's mainly for the CostCritic, because it's this piece-wise function that has "regular costs", costs + critical_cost, and outright collision_cost, so the costs are a bit all over the place.

One alternative is to track which trajectory is in collision and color it differently but it's pretty intrusive, I already know you won't like it.

SteveMacenski · 2026-03-16T19:51:23Z

One alternative is to track which trajectory is in collision and color it differently but it's pretty intrusive, I already know you won't like it.

isn't that what another months-ago PR was doing to fix this same issue? I think its totally sane. I think in-collision should be uniquely marked and if that's throwing off the normalization that makes total sense to me

tonynajjar · 2026-03-16T19:54:29Z

I think its totally sane. I think in-collision should be uniquely marked and if that's throwing off the normalization that makes total sense to me

Ah, then I misjudged your opinion - will do then

- Introduced `trajectories_in_collision` to `CriticData` for tracking trajectory collisions. - Updated `Optimizer`, `CostCritic`, and `ObstaclesCritic` to populate collision data. - Modified `TrajectoryVisualizer` to visualize collision status in trajectories. - Adjusted tests to accommodate changes in `CriticData` structure. Signed-off-by: Tony Najjar <tony.najjar@dexory.com>

Signed-off-by: Tony Najjar <tony.najjar@dexory.com>

tonynajjar · 2026-03-17T10:28:03Z

One alternative is to track which trajectory is in collision and color it differently

Done. The different coloring based on whether a trajectory is in collision only applies to the namespaces CostCritic, ObstacleCritic and Total Cost

…sualizer Signed-off-by: Tony Najjar <tony.najjar@dexory.com>

… support cost layer visualization Signed-off-by: Tony Najjar <tony.najjar@dexory.com>

tonynajjar · 2026-03-17T11:46:41Z

It's ready for review, the only point still subject to discussion is the visualize_cost_layer param

nav2_mppi_controller/include/nav2_mppi_controller/tools/trajectory_visualizer.hpp

nav2_mppi_controller/src/critics/cost_critic.cpp

nav2_mppi_controller/src/controller.cpp

nav2_mppi_controller/src/optimizer.cpp

nav2_mppi_controller/include/nav2_mppi_controller/critic_manager.hpp

nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp

Signed-off-by: Tony Najjar <tony.najjar@dexory.com>

…_collision in evalTrajectoriesScores and remove redundant resizing in critics Signed-off-by: Tony Najjar <tony.najjar@dexory.com>

…d methods Signed-off-by: Tony Najjar <tony.najjar@dexory.com>

…for conditional updates in score methods Signed-off-by: Tony Najjar <tony.najjar@dexory.com>

…c costs Signed-off-by: Tony Najjar <tony.najjar@dexory.com>

Copilot

Pull request overview

Enhances Nav2’s MPPI controller introspection by tracking per-critic trajectory costs and visualizing candidate trajectories with a cost-based color gradient (including collision highlighting), enabling easier debugging/tuning of critic contributions.

Changes:

Add per-critic cost breakdown storage/publishing in CriticManager and expose it via Optimizer.
Update TrajectoryVisualizer to color candidate trajectories by total/per-critic cost and highlight collisions.
Add visualize_cost_layer controller parameter to select which cost layer to visualize (total vs individual critic).

Reviewed changes

Copilot reviewed 16 out of 16 changed files in this pull request and generated 6 comments.

Show a summary per file

File	Description
nav2_system_tests/src/gps_navigation/nav2_no_map_params.yaml	Removes obsolete `publish_critics_stats` config entry.
nav2_mppi_controller/test/utils_test.cpp	Updates `CriticData` initializers for the new collision-tracking field.
nav2_mppi_controller/test/trajectory_visualizer_tests.cpp	Updates tests for new candidate visualization API and marker expectations.
nav2_mppi_controller/test/critics_tests.cpp	Updates `CriticData` initializers for the new collision-tracking field.
nav2_mppi_controller/test/critic_manager_test.cpp	Updates `CriticData` initializer for the new collision-tracking field.
nav2_mppi_controller/src/trajectory_visualizer.cpp	Implements cost-normalized, LINE_STRIP candidate trajectory visualization + collision coloring.
nav2_mppi_controller/src/critics/obstacles_critic.cpp	Records per-trajectory collision flags when enabled.
nav2_mppi_controller/src/critics/cost_critic.cpp	Records per-trajectory collision flags when enabled.
nav2_mppi_controller/src/critic_manager.cpp	Adds per-critic cost capture path and gates stats publishing on `visualize`.
nav2_mppi_controller/src/controller.cpp	Adds `visualize_cost_layer` selection logic for total vs per-critic visualization.
nav2_mppi_controller/include/nav2_mppi_controller/tools/trajectory_visualizer.hpp	Updates candidate trajectory visualization API and adds helpers.
nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp	Exposes total costs, per-critic costs, and collision flags to controller/visualization.
nav2_mppi_controller/include/nav2_mppi_controller/critic_manager.hpp	Adds per-critic cost accessor and internal storage.
nav2_mppi_controller/include/nav2_mppi_controller/critic_data.hpp	Adds `trajectories_in_collision` to share collision info between critics/visualization.
nav2_mppi_controller/include/nav2_mppi_controller/controller.hpp	Stores `visualize_cost_layer_` parameter.
nav2_mppi_controller/README.md	Updates docs to reflect stats publishing now tied to `visualize`.

Comments suppressed due to low confidence (1)

nav2_mppi_controller/src/critic_manager.cpp:55

visualize_ is retrieved as a dynamic parameter here (default ParameterType::Dynamic), but loadCritics() only creates/activates critics_effect_pub_ during configure. If a user toggles visualize from false->true at runtime, evalTrajectoriesScores() will start doing per-critic bookkeeping but critics_stats will never publish because the publisher was never created. Either make this parameter static (as publish_critics_stats was) or lazily create/activate/deactivate the publisher when visualize_ changes.

void CriticManager::getParams()
{
  auto node = parent_.lock();
  auto getParam = parameters_handler_->getParamGetter(name_);
  getParam(critic_names_, "critics", std::vector<std::string>{}, ParameterType::Static);
  getParam(visualize_, "visualize", false);
}

void CriticManager::loadCritics()
{
  if (!loader_) {
    loader_ = std::make_unique<pluginlib::ClassLoader<critics::CriticFunction>>(
      "nav2_mppi_controller", "mppi::critics::CriticFunction");
  }

  auto node = parent_.lock();
  if (visualize_) {
    critics_effect_pub_ = node->create_publisher<nav2_msgs::msg::CriticsStats>(
      "~/critics_stats");
    critics_effect_pub_->on_activate();
  }

You can also share your feedback on Copilot code review. Take the survey.

nav2_mppi_controller/src/controller.cpp

nav2_mppi_controller/src/trajectory_visualizer.cpp

nav2_mppi_controller/test/trajectory_visualizer_tests.cpp

nav2_mppi_controller/src/critic_manager.cpp

nav2_mppi_controller/README.md

…visualizer Signed-off-by: Tony Najjar <tony.najjar@dexory.com>

nav2_mppi_controller/src/critics/obstacles_critic.cpp

nav2_mppi_controller/src/trajectory_visualizer.cpp

nav2_mppi_controller/include/nav2_mppi_controller/controller.hpp

nav2_mppi_controller/src/controller.cpp

nav2_mppi_controller/src/critic_manager.cpp

Signed-off-by: Tony Najjar <tony.najjar@dexory.com>

…lues Signed-off-by: Tony Najjar <tony.najjar@dexory.com>

Signed-off-by: Tony Najjar <tony.najjar@dexory.com>

SteveMacenski · 2026-03-18T21:42:21Z

Remaining 2x open items are all that's left 👍

Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

tonynajjar · 2026-03-18T22:29:08Z

Can we have the flaky (or even constantly failing) tests disabled? I'd love to see a green check on my PRs to not have to scrutinize CI so often.

tonynajjar · 2026-03-18T22:31:21Z

Good to merge from my side. Still need a PR for the migration doc for removing publish_critics_stats, will do that tomorrow

tonynajjar · 2026-03-18T22:45:48Z

will do that tomorrow

I lied: ros-navigation/docs.nav2.org#890

tonynajjar added 2 commits March 16, 2026 15:24

Add per-critic cost coloring

8ecce26

Signed-off-by: Tony Najjar <tony.najjar@dexory.com>

apply fixes

8a75577

Signed-off-by: Tony Najjar <tony.najjar@dexory.com>

tonynajjar requested a review from Copilot March 16, 2026 15:38

Copilot started reviewing on behalf of tonynajjar March 16, 2026 15:39 View session

Copilot AI reviewed Mar 16, 2026

View reviewed changes

SteveMacenski reviewed Mar 16, 2026

View reviewed changes

Pr comments, simplify

e245184

Signed-off-by: Tony Najjar <tony.najjar@dexory.com>

fix test

18f0810

Signed-off-by: Tony Najjar <tony.najjar@dexory.com>

tonynajjar added 2 commits March 16, 2026 19:35

maxGapSplit

c198a95

Signed-off-by: Tony Najjar <tony.najjar@dexory.com>

Revert "maxGapSplit"

ec6a54c

This reverts commit c198a95.

tonynajjar added 2 commits March 17, 2026 10:00

replace publish_critics_stat with visualize

7348b60

Signed-off-by: Tony Najjar <tony.najjar@dexory.com>

tonynajjar added 2 commits March 17, 2026 10:28

Enhance collision coloring for per-critic cost layers in TrajectoryVi…

81efc18

…sualizer Signed-off-by: Tony Najjar <tony.najjar@dexory.com>

Add visualize_cost_layer parameter and update TrajectoryVisualizer to…

d6dac2d

… support cost layer visualization Signed-off-by: Tony Najjar <tony.najjar@dexory.com>

tonynajjar marked this pull request as ready for review March 17, 2026 11:43

SteveMacenski reviewed Mar 17, 2026

View reviewed changes

tonynajjar added 2 commits March 18, 2026 09:07

renaming

89780d5

Signed-off-by: Tony Najjar <tony.najjar@dexory.com>

Refactor collision handling in CriticData: initialize trajectories_in…

9d7ea87

…_collision in evalTrajectoriesScores and remove redundant resizing in critics Signed-off-by: Tony Najjar <tony.najjar@dexory.com>

tonynajjar added 3 commits March 18, 2026 09:17

Remove show_collisions parameter from TrajectoryVisualizer and relate…

bf54917

…d methods Signed-off-by: Tony Najjar <tony.najjar@dexory.com>

Refactor collision tracking in CriticData: use track_collisions flag …

202e7d4

…for conditional updates in score methods Signed-off-by: Tony Najjar <tony.najjar@dexory.com>

Refactor visualize method in MPPIController: simplify access to criti…

cbbd093

…c costs Signed-off-by: Tony Najjar <tony.najjar@dexory.com>

tonynajjar requested a review from Copilot March 18, 2026 09:36

Copilot started reviewing on behalf of tonynajjar March 18, 2026 09:36 View session

Copilot AI reviewed Mar 18, 2026

View reviewed changes

Add visualize_cost_layer parameter and improve collision handling in …

2d5701b

…visualizer Signed-off-by: Tony Najjar <tony.najjar@dexory.com>

tonynajjar requested a review from SteveMacenski March 18, 2026 09:48

SteveMacenski requested changes Mar 18, 2026

View reviewed changes

tonynajjar added 8 commits March 18, 2026 20:34

more readable cost fetching

ec0c825

Signed-off-by: Tony Najjar <tony.najjar@dexory.com>

fix

8629a7e

Signed-off-by: Tony Najjar <tony.najjar@dexory.com>

reduce the diff

f66e62a

Signed-off-by: Tony Najjar <tony.najjar@dexory.com>

reduce more diff

c67d8fd

Signed-off-by: Tony Najjar <tony.najjar@dexory.com>

store clock

840878c

Signed-off-by: Tony Najjar <tony.najjar@dexory.com>

improve cost normalization by using numeric limits for min and max va…

b9f792a

…lues Signed-off-by: Tony Najjar <tony.najjar@dexory.com>

rename visualize_cost_layer to critic_index_to_visualize for clarity

86a8401

Signed-off-by: Tony Najjar <tony.najjar@dexory.com>

refactor visualize method to use critic_index_to_visualize directly

c0f6496

Signed-off-by: Tony Najjar <tony.najjar@dexory.com>

tonynajjar and others added 2 commits March 18, 2026 22:43

Apply suggestion from @SteveMacenski

4b18c4d

Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

Update nav2_mppi_controller/src/critics/obstacles_critic.cpp

5904829

Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

tonynajjar requested a review from SteveMacenski March 18, 2026 21:45

SteveMacenski approved these changes Mar 19, 2026

View reviewed changes

SteveMacenski merged commit 6fd9ae3 into ros-navigation:main Mar 19, 2026
15 of 16 checks passed

tonynajjar mentioned this pull request Mar 19, 2026

Panav/feat/update mppi visualization #5975

Closed

adivardi mentioned this pull request Mar 19, 2026

Add MPPI visualization enwaytech/navigation2#9

Open

Pana1v mentioned this pull request Mar 22, 2026

Better Trajectory Visualizer for MPPI Controller #5800

Closed

Conversation

tonynajjar commented Mar 16, 2026

Basic Info

Description of contribution in a few bullet points

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:

Uh oh!

tonynajjar commented Mar 16, 2026

Uh oh!

Copilot AI left a comment

Choose a reason for hiding this comment

Pull request overview

Reviewed changes

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

SteveMacenski commented Mar 16, 2026

Uh oh!

tonynajjar commented Mar 16, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

SteveMacenski commented Mar 16, 2026

Uh oh!

tonynajjar commented Mar 16, 2026

Uh oh!

SteveMacenski commented Mar 16, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

tonynajjar commented Mar 16, 2026

Uh oh!

tonynajjar commented Mar 16, 2026

Uh oh!

SteveMacenski commented Mar 16, 2026

Uh oh!

tonynajjar commented Mar 16, 2026

Uh oh!

tonynajjar commented Mar 17, 2026

Uh oh!

tonynajjar commented Mar 17, 2026

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Copilot AI left a comment

Choose a reason for hiding this comment

Pull request overview

Reviewed changes

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

SteveMacenski commented Mar 18, 2026

Uh oh!

tonynajjar commented Mar 16, 2026 •

edited

Loading

SteveMacenski commented Mar 16, 2026 •

edited

Loading