MPPI cost-based visualization #5643

tonynajjar · 2025-10-23T11:22:19Z

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	None
Primary OS tested on	Ubuntu
Robotic platform tested on	Custom Simulation
Does this PR contain AI generated software?	No
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

I added cost-based visualization for the trajectories, both total cost and per-critic cost under Marker namespaces

Total Cost

CostCritic

PathFollowCritic

I replaced points with lines for clearer continuity visualization
I added the visualization of the furthest_reached_path_point to the Path critics (this revealed a bug which I will fix in another PR)
I added the visualization of the footprints along the optimal trajectory

Refactored visualization to have everything inside the trajectory_visualizer and many flags to disable/enable different visualizations

Description of documentation updates required from your changes

New, deleted and moved params so docs definitely need updating

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

Signed-off-by: Tony Najjar <[email protected]>

nav2_mppi_controller/include/nav2_mppi_controller/critics/path_align_critic.hpp

nav2_mppi_controller/src/trajectory_visualizer.cpp

tonynajjar · 2025-10-23T11:26:07Z

nav2_mppi_controller/src/trajectory_visualizer.cpp

-      const float j_flt = static_cast<float>(j);
-      float blue_component = 1.0f - j_flt / shape_1;
-      float green_component = j_flt / shape_1;
+  // Use percentile-based normalization to handle outliers


It's important because the CostCritic creates high-cost outliers which increases the range of the color gradient to a point where it's just green or red

We could also skip trajectories marked as collision from the normalization process. Only have the scale normalization apply to non-collision trajectories. Have in-collision trajectories be straight up red-only (which makes sense to a casual viewer as well)

We could also skip trajectories marked as collision from the normalization process

I thought about that but how would you determine that? Just based off the cost? or would you add a in_collision field to Trajectories

Did you end up addressing this point? I see trajectory visualizations below that have collision ones as magenta

Signed-off-by: Tony Najjar <[email protected]>

adivardi · 2025-10-23T15:29:55Z

I only had a quick look as I am traveling. I think keeping the optimal trajectory visualized and distinguished is important, as it's the most useful part.
Ignore this if I just missed it looking on my phone

tonynajjar · 2025-10-23T15:56:18Z

I only had a quick look as I am traveling. I think keeping the optimal trajectory visualized and distinguished is important, as it's the most useful part. Ignore this if I just missed it looking on my phone

I mean its still there but hard to see when all the trajectories are shown.

The trajectories can be disabled though as they have a separate namespace:

if that's not enough we can put the optimal trajectory at a different Z-elevation

nav2_mppi_controller/src/critics/path_align_critic.cpp

nav2_mppi_controller/src/trajectory_visualizer.cpp

tonynajjar · 2025-10-23T18:30:22Z

Thanks for the review, will polish it tomorrow

SteveMacenski · 2025-10-23T18:52:59Z

I'll be on a plane to singapore, so it might be a minute before I can give it another looksee given ROSCon.

Signed-off-by: Tony Najjar <[email protected]>

tonynajjar · 2025-10-27T16:01:50Z

nav2_mppi_controller/src/trajectory_visualizer.cpp

+  if (publish_optimal_path_) {
+    optimal_path_pub_ = node->create_publisher<nav_msgs::msg::Path>("~/optimal_path");
+  }


We already have the optimal path published under a namespace in ~/candidate_trajectories, do you see the need to publish it on a separate topic as well?

Yes, please leave this :-)

Gladly but just fmi, why the duplicate visualization? If we were to keep one though, I'd keep this one and remove the one namespaced under ~/candidate_trajectories because that topic is very heavy and if I just want the optimal_trajectory I'd rather not get the extra baggage with it

Signed-off-by: Tony Najjar <[email protected]>

tonynajjar · 2025-10-27T16:21:31Z

@SteveMacenski I addressed many of your comments but additionally made many more changes. Can you give it a general look? There are probably a bunch of nitpicks left.

For now I dropped visualizing in-collision trajectories with another color because I think that would require adding an additional vector in CriticsData that tracks which trajectories are in-collision. Would you be open to that? Another option would be to somewhat infer from the cost if there was a collision

tonynajjar · 2025-10-28T14:11:15Z

For now I dropped visualizing in-collision trajectories with another color because I think that would require adding an additional vector in CriticsData that tracks which trajectories are in-collision. Would you be open to that?

This is what it would look like (magenta is collision):

status quo (in-collision cost overpowering the range):

adivardi · 2025-11-03T08:44:05Z

I only had a quick look as I am traveling. I think keeping the optimal trajectory visualized and distinguished is important, as it's the most useful part. Ignore this if I just missed it looking on my phone

I mean its still there but hard to see when all the trajectories are shown.

if that's not enough we can put the optimal trajectory at a different Z-elevation

Oh, I didn't see it. It probably looks clearer when reducing the number of steps.

Signed-off-by: Tony Najjar <[email protected]>

codecov · 2025-11-17T09:33:12Z

Codecov Report

❌ Patch coverage is 53.48837% with 100 lines in your changes missing coverage. Please review.

Files with missing lines	Patch %	Lines
nav2_mppi_controller/src/trajectory_visualizer.cpp	63.63%	52 Missing ⚠️
..._mppi_controller/src/critics/path_align_critic.cpp	18.75%	13 Missing ⚠️
..._mppi_controller/src/critics/path_angle_critic.cpp	20.00%	12 Missing ⚠️
...mppi_controller/src/critics/path_follow_critic.cpp	20.00%	12 Missing ⚠️
nav2_mppi_controller/src/critic_manager.cpp	37.50%	10 Missing ⚠️
...troller/include/nav2_mppi_controller/optimizer.hpp	80.00%	1 Missing ⚠️

Files with missing lines	Coverage Δ
...roller/include/nav2_mppi_controller/controller.hpp	`100.00% <ø> (ø)`
...r/include/nav2_mppi_controller/critic_function.hpp	`100.00% <100.00%> (ø)`
...er/include/nav2_mppi_controller/critic_manager.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% <ø> (ø)`
...v2_mppi_controller/tools/trajectory_visualizer.hpp	`100.00% <ø> (ø)`
nav2_mppi_controller/src/controller.cpp	`100.00% <100.00%> (+11.53%)`	⬆️
...troller/include/nav2_mppi_controller/optimizer.hpp	`85.71% <80.00%> (-14.29%)`	⬇️
nav2_mppi_controller/src/critic_manager.cpp	`63.63% <37.50%> (-36.37%)`	⬇️
... and 4 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

This is just alot of changes which is making this hard for me to review. I don't want to flood you with stuff so I'm going to keep many of these items higher level

SteveMacenski · 2025-11-19T19:04:02Z

nav2_mppi_controller/src/trajectory_visualizer.cpp

+  for (Eigen::Index i = 0; i < costs.size(); ++i) {
+    min_cost = std::min(min_cost, costs(i));
+    max_cost = std::max(max_cost, costs(i));
+  }


float min_val = arr.minCoeff(); float max_val = arr.maxCoeff();

would do the same in a vectorized format

SteveMacenski · 2025-11-19T19:05:40Z

nav2_mppi_controller/src/trajectory_visualizer.cpp

+      blue_component = 0.5f;
+    } else {
+      // Normal gradient for trajectories
+      float normalized_cost = (costs(i) - min_cost) / cost_range;


https://github.com/ros-navigation/navigation2/blob/main/nav2_mppi_controller/src/optimizer.cpp#L511 this can also be used to normalized in vectorized way that is more efficient than doing this in a loop. Use this before we get looping so we can just get float normalized_cost = normalized_costs(i);

You can even do the / cost_range too

SteveMacenski · 2025-11-19T19:07:27Z

nav2_mppi_controller/src/trajectory_visualizer.cpp

-      const float j_flt = static_cast<float>(j);
-      float blue_component = 1.0f - j_flt / shape_1;
-      float green_component = j_flt / shape_1;
+  // Use percentile-based normalization to handle outliers


Did you end up addressing this point? I see trajectory visualizations below that have collision ones as magenta

SteveMacenski · 2025-11-19T19:09:13Z

nav2_mppi_controller/src/trajectory_visualizer.cpp

+  if (publish_optimal_path_) {
+    optimal_path_pub_ = node->create_publisher<nav_msgs::msg::Path>("~/optimal_path");
+  }


Yes, please leave this :-)

SteveMacenski · 2025-11-19T19:10:41Z

nav2_bringup/params/nav2_params.yaml

-      publish_optimal_trajectory: true
      regenerate_noises: true
-      TrajectoryVisualizer:
+      Visualization:


What happened to visualize? Also why change the namespace from TrajectoryVisualizer for these? I don't want to make more changes than strictly required, it makes the migration path for users harder if there's no tangible benefit. The namespace here seems like an unnecessary change

What happened to visualize?

I replaced it with more granular configurations because some topics e.g. candidate_trajectories are much heavier than others and for my use case I'd like to have the optimal_path always published and rosbag-recorded, even in production, but not the candidate trajectories.

Alternatively we could have the catch-all visualize parameter and for those who would like to have a subset of visualizations activated in production, visualize would be true but only the visualizations with an active subscription would be created and published. But that's a little more... risky

What strategy would you follow?

Also why change the namespace from TrajectoryVisualizer

Since it now includes params not directly related to the candidate trajectories, e.g. publish_transformed_path, I thought I'd rename it to something more general but if it creates unnecessary migration effort for everyone, I can revert to TrajectoryVisualizer

SteveMacenski · 2025-11-19T19:13:37Z

nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp

+   */
+  const std::vector<std::pair<std::string, Eigen::ArrayXf>> & getCriticCosts() const
+  {
+    if (critics_data_.individual_critics_cost) {


When would this not be the case?

SteveMacenski · 2025-11-19T19:13:55Z

nav2_mppi_controller/include/nav2_mppi_controller/tools/trajectory_visualizer.hpp

+    const models::Trajectories & trajectories,
+    const Eigen::ArrayXf & total_costs,
+    const std::vector<std::pair<std::string, Eigen::ArrayXf>> & individual_critics_cost = {},
+    const builtin_interfaces::msg::Time & cmd_stamp = builtin_interfaces::msg::Time());


There should be no default value for time, that should be real at all times (har-de-har-har)

SteveMacenski · 2025-11-19T19:16:11Z

nav2_mppi_controller/src/critics/path_align_critic.cpp

    threshold_to_consider_,
    "threshold_to_consider", 0.5f);
  getParam(use_path_orientations_, "use_path_orientations", false);
+  getParam(visualize_furthest_point_, "visualize_furthest_point", false);


If we have a catch all "debug_visualizations" can this just be part of that instead of introducing new parameters for each critic?

Or, can each critic simply have a "visualize" setting so its global for any visualizations that a given critic might have rather than enabling each individually (should there be later > 1)?

Here I can propose to simplify things by publishing the pose at data.furthest_reached_path_point in the trajectory_visualizer. The upside is simplification in code and parameters, the downside is that it's less accurate because it doesn't reflect exactly the pose used in each critic because it's slightly changed, e.g. in PathAngle there is offset_from_furthest_. But I can personally live with that

SteveMacenski · 2025-11-19T19:17:40Z

nav2_mppi_controller/include/nav2_mppi_controller/tools/trajectory_visualizer.hpp

+    * @param ns Namespace for the markers
+    * @param cmd_stamp Timestamp for the markers
+    */
+  void createTrajectoryMarkers(


Can't these be free utils functions like the others before? I don't think these require class members and make them more easily unit testable

SteveMacenski · 2025-11-19T19:19:34Z

nav2_mppi_controller/src/trajectory_visualizer.cpp

  }
 }

 void TrajectoryVisualizer::add(


I think we have some major unused functions now if we're adding this / other createXYZ methods

tonynajjar added 2 commits October 23, 2025 13:01

Enrich MPPI visualizations

2d429c6

Signed-off-by: Tony Najjar <[email protected]>

lint

f0f3333

Signed-off-by: Tony Najjar <[email protected]>

tonynajjar commented Oct 23, 2025

View reviewed changes

nav2_mppi_controller/include/nav2_mppi_controller/critics/path_align_critic.hpp Outdated Show resolved Hide resolved

tonynajjar commented Oct 23, 2025

View reviewed changes

nav2_mppi_controller/src/trajectory_visualizer.cpp Outdated Show resolved Hide resolved

tonynajjar commented Oct 23, 2025

View reviewed changes

fix var name

cecc6ea

Signed-off-by: Tony Najjar <[email protected]>

tonynajjar mentioned this pull request Oct 23, 2025

[mppi] color trajectories by cost #5637

Closed

8 tasks

fix test

5202339

Signed-off-by: Tony Najjar <[email protected]>

SteveMacenski requested changes Oct 23, 2025

View reviewed changes

tonynajjar added 11 commits October 24, 2025 10:44

fixes

88a4adc

Signed-off-by: Tony Najjar <[email protected]>

visualization

8ab4228

Signed-off-by: Tony Najjar <[email protected]>

lint

d1092e7

Signed-off-by: Tony Najjar <[email protected]>

Add optimal footprint visualization

081e54d

Signed-off-by: Tony Najjar <[email protected]>

lint

b9f522e

Signed-off-by: Tony Najjar <[email protected]>

Add optimal footprint visualization

9c23e57

Signed-off-by: Tony Najjar <[email protected]>

.

1890dd9

Signed-off-by: Tony Najjar <[email protected]>

lint

b400b90

Signed-off-by: Tony Najjar <[email protected]>

rename visualize

a3611d4

Signed-off-by: Tony Najjar <[email protected]>

small fixes

41b4c35

Signed-off-by: Tony Najjar <[email protected]>

add optimal traj

0f57a9c

Signed-off-by: Tony Najjar <[email protected]>

tonynajjar commented Oct 27, 2025

View reviewed changes

tonynajjar added 2 commits October 27, 2025 17:07

updated params

70e7162

Signed-off-by: Tony Najjar <[email protected]>

set params to false

a88f5dc

Signed-off-by: Tony Najjar <[email protected]>

tonynajjar requested a review from SteveMacenski October 28, 2025 09:17

tonynajjar mentioned this pull request Nov 5, 2025

[MPPI] Cost Critic does not penalize for rear near-collision trajectories in a narrow spot for non-circular robot #5668

Open

tonynajjar and others added 4 commits November 10, 2025 15:21

fix unused var

badadf1

Signed-off-by: Tony Najjar <[email protected]>

Update nav2_params.yaml

303e0cb

Signed-off-by: Tony Najjar <[email protected]>

fix test

aa608c0

Signed-off-by: Tony Najjar <[email protected]>

lint

bb1792c

Signed-off-by: Tony Najjar <[email protected]>

SteveMacenski requested changes Nov 19, 2025

View reviewed changes

MPPI cost-based visualization #5643

Are you sure you want to change the base?

MPPI cost-based visualization #5643

Conversation

tonynajjar commented Oct 23, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

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!

Uh oh!

Uh oh!

Choose a reason for hiding this comment

Uh oh!

SteveMacenski Oct 23, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

adivardi commented Oct 23, 2025

Uh oh!

tonynajjar commented Oct 23, 2025

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

tonynajjar commented Oct 23, 2025

Uh oh!

SteveMacenski commented Oct 23, 2025

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

tonynajjar Nov 20, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

tonynajjar commented Oct 27, 2025

Uh oh!

tonynajjar commented Oct 28, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

adivardi commented Nov 3, 2025

Uh oh!

codecov bot commented Nov 17, 2025

Codecov Report

Uh oh!

SteveMacenski left a comment

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

tonynajjar Nov 20, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

tonynajjar commented Oct 23, 2025 •

edited

Loading

SteveMacenski Oct 23, 2025 •

edited

Loading

tonynajjar Nov 20, 2025 •

edited

Loading

tonynajjar commented Oct 28, 2025 •

edited

Loading

tonynajjar Nov 20, 2025 •

edited

Loading