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
I replaced the color gradient indicative of time_steps to an elevation gradient (the higher the point, the bigger the timestep)
I replaced points with lines for clearer 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)

Description of documentation updates required from your changes

Added new parameter, so need to add that to default configs and documentation page

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]>

tonynajjar · 2025-10-23T11:23:50Z

nav2_mppi_controller/include/nav2_mppi_controller/critics/path_align_critic.hpp

  float weight_{0};
+
+  bool visualize_{false};
+  nav2::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr target_pose_pub_;


Maybe this is better as a Marker sphere.

Currently:

If we add the orientation, I think that would help. If we don't add that, then yes a sphere could make sense

tonynajjar · 2025-10-23T11:25:01Z

nav2_mppi_controller/src/trajectory_visualizer.cpp

-      auto color = utils::createColor(0, green_component, blue_component, 1);
-      auto marker = utils::createMarker(
-        marker_id_++, pose, scale, color, frame_id_, marker_namespace);
+  // Use 10th and 90th percentile for robust color mapping


I found this percentile separation to be the best for my case

If we do the eigen normalization, we can promise normalization from 0-1 always. I think that would be the best?

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

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

SteveMacenski · 2025-10-23T18:12:37Z

nav2_mppi_controller/src/critics/path_align_critic.cpp

+    auto node = parent_.lock();
+    if (node) {
+      target_pose_pub_ = node->create_publisher<geometry_msgs::msg::PoseStamped>(
+        "/PathAlignCritic/furthest_reached_path_point", 1);


Remove the first /, which will mess with multirobot / namespacing

SteveMacenski · 2025-10-23T18:12:51Z

nav2_mppi_controller/src/critics/path_align_critic.cpp

  float path_segments_flt = static_cast<float>(path_segments_count);
+
+  // Visualize target pose if enabled
+  if (visualize_ && path_segments_count > 0) {


Check if there are any subscriptions

SteveMacenski · 2025-10-23T18:13:56Z

nav2_mppi_controller/src/critics/path_align_critic.cpp

+
+  // Visualize target pose if enabled
+  if (visualize_ && path_segments_count > 0) {
+    auto node = parent_.lock();


You could store the clock here:

navigation2/nav2_mppi_controller/include/nav2_mppi_controller/critic_function.hpp

Line 73 in 5202339

logger_ = parent_.lock()->get_logger();

and then all critics can access it as a shaerd resource, removing the need for the locking / node

SteveMacenski · 2025-10-23T18:14:07Z

nav2_mppi_controller/src/critics/path_align_critic.cpp

+    target_pose.pose.position.y = data.path.y(path_segments_count);
+    target_pose.pose.position.z = 0.0;
+    target_pose.pose.orientation.w = 1.0;
+    target_pose_pub_->publish(target_pose);


Unique pointer + std::move

SteveMacenski · 2025-10-23T18:14:19Z

nav2_mppi_controller/src/critics/path_align_critic.cpp

+    target_pose.pose.position.x = data.path.x(path_segments_count);
+    target_pose.pose.position.y = data.path.y(path_segments_count);
+    target_pose.pose.position.z = 0.0;
+    target_pose.pose.orientation.w = 1.0;


Why not set the orientation?

SteveMacenski · 2025-10-23T18:20:05Z

nav2_mppi_controller/src/trajectory_visualizer.cpp

+  // Use 10th and 90th percentile for robust color mapping
+  size_t idx_10th = static_cast<size_t>(sorted_costs.size() * 0.1);
+  size_t idx_90th = static_cast<size_t>(sorted_costs.size() * 0.9);


Can we use something eigen-y for this instead? sorting is expensive and eigen operations are vectorized. Finding the min/max cost and normalizing that should be easy in Eigen & there should be examples of that in the Optimizer when we normalize costs for scoring. Then you wouldn't need to manually normalize in the for loop.

SteveMacenski · 2025-10-23T18:24:00Z

nav2_mppi_controller/src/trajectory_visualizer.cpp

+    // Color scheme with smooth gradient:
+    // Green (0.0) -> Yellow-Green (0.25) -> Yellow (0.5) -> Orange (0.75) -> Red (1.0)
+    // Very high outlier costs (>95th percentile) will be clamped to red
+    blue_component = 0.0f;
+
+    if (normalized_cost < 0.5f) {
+      // Transition from Green to Yellow (0.0 - 0.5)
+      float t = normalized_cost * 2.0f;  // Scale to [0, 1]
+      red_component = t;
+      green_component = 1.0f;
+    } else {
+      // Transition from Yellow to Red (0.5 - 1.0)
+      float t = (normalized_cost - 0.5f) * 2.0f;  // Scale to [0, 1]
+      red_component = 1.0f;
+      green_component = 1.0f - t;


I'm not really sensitive to the particular color policy, but I'd like this to be architecturally a lambda or method that is utils::createColor given the input of normalized_cost. That would move alot of this code out of this method and make it more streamline and clear where the color picking policy is like the other parts of this file.

SteveMacenski · 2025-10-23T18:25:45Z

nav2_mppi_controller/src/trajectory_visualizer.cpp

+      point.x = trajectories.x(i, j);
+      point.y = trajectories.y(i, j);
+      // Increment z by time_step_elevation_ for each time step
+      if (time_step_elevation_ > 0.0f) {


I replaced the color gradient indicative of time_steps to an elevation gradient (the higher the point, the bigger the timestep)

How much does this really help? I think this is pretty confusing as some may see this as the trajectory itself actually going into the air. Especially when doing first-time roobt setup where you have TF issues or you're doing SE2 navigation where the Z dimension is real, I think this can cause confusion.

Is this super important to include or just something you added as a 'nice to have'

Is this super important to include or just something you added as a 'nice to have'

Not super important for me, I just didn't want to lose what we already have (the timestep gradient) and that was just one way to represent it. I'm fine with removing. btw default is no elevation

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

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

Open

8 tasks

fix test

5202339

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

SteveMacenski requested changes Oct 23, 2025

View reviewed changes

fixes

88a4adc

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

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

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!

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!

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!

adivardi commented Oct 23, 2025

Uh oh!

tonynajjar commented Oct 23, 2025

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!

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 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!

tonynajjar commented Oct 23, 2025

Uh oh!

SteveMacenski commented Oct 23, 2025

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

3 participants

SteveMacenski Oct 23, 2025 •

edited

Loading

tonynajjar Oct 23, 2025 •

edited

Loading