Add debug topic to visualize whether MPPI critic has an effect on costs

[tonynajjar]

---

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

• Change the score function to return a bool (true if costs changed)
• Add debug topic

Visualize result:

Description of documentation updates required from your changes

Description of how this change was tested

Send navigation goals and checked that the values in the topic make sense according to threshold_to_consider

---

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]

Open points:

• Make a parameter to enable (disabled by default)
• Choose best ROS msg; standard or custom? If custom then I propose:

string[] critics
bool[] changed

• Is there more relevant debug info we could publish?

[codecov]

Codecov Report

✅ All modified and coverable lines are covered by tests.

Files with missing lines	Coverage Δ
...er/include/nav2_mppi_controller/critic_manager.hpp	100.00% <ø> (ø)
nav2_mppi_controller/src/critic_manager.cpp	100.00% <100.00%> (ø)

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

If just a bool array, how are you labeling / visualizing that?

Yeah, I think having the label + score alignment is useful. I suppose even publishing the trajectory scores for each critic would be a useful visualization -- that would replace the bool with actual numbers though (0 means no affect).

Another visualization could be coloring the samples in the candidate_trajectories topic by the accumulated costs of all critics to see how they they penalized by various values. DWB does this I believe. That isn't related to what you're doing yet, but we could use the final scores to adjust the colors of the trajectory markers for each trajectory on some normalized scale.

[tonynajjar]

If just a bool array, how are you labeling / visualizing that?

I added the labels manually for that screenshot

I suppose even publishing the trajectory scores for each critic would be a useful visualization

I think I'm misunderstanding something. You mean publish all the scores of all the trajectories? (nb traj x nb citics ?). Is there one useful score per metric for all the trajectories that would be useful to publish? e.g. mean, sum, etc...?

Another visualization could be coloring the samples in the candidate_trajectories topic by the accumulated costs of all critics to see how they they penalized by various values

Yes also thought about it. I'll implement it if I find myself needing it

[SteveMacenski]

I think I'm misunderstanding something. You mean publish all the scores of all the trajectories? (nb traj x nb citics ?). Is there one useful score per metric for all the trajectories that would be useful to publish? e.g. mean, sum, etc...?

The critics have a single float score for each trajectory. So you could publish the float for each trajectory for each critic, yes. I think what I was suggesting was mostly the sum total of contribution each critic has in an iteration (so it would be 1 float for each critic) which would show you which are the most influential at a give time & which aren't enabled at all due to threshold to consider or dynamic parameters. But each critic score for each trajectory could be useful too, I think that's better suited for the trajectory candidate visualizer. I don't know how to display 2000x trajectories (or tell the chart what these even comprise of as the trajectory's shape/direction/etc) meaningfully with each of ~5-10 critics that would be useful and not take 30 minutes of analysis per timestep to draw meaningful conclusions. That seems like a firehose where it isn't (?) necessary. What I'm suggesting is more expanding on your bool to a float of the total score it applies -- which I actually do find to be a useful metric sometimes I print out while tuning.

It might be useful to vibe code a python script / create a RQT graph config or something for tools/ that can listen to that topic and plot them over time, either way, since none of these are things rviz or RQT are going to do easily. You'll have to make something to chat it anyway, may as well reduce the burden at the next company or for the next person (and so the labels are built in ;-) ).

[tonynajjar]

Got it. For this PR I will stick to only publishing sum but in a way that is extensible. I changed the approach completely to not change the critics. Consider the PR a draft, no need to nitpick now, can you give your review on the general approach before I spend more time into it?

[SteveMacenski]

I'm curious about the perf hit, but I don't think it should be that bad.

Something to visualize this in tools/ would be good to pair in this PR so this can be immediately useful to someone

[SteveMacenski]

unique ptr for efficiency reasons

[tonynajjar]

interesting, can you say why more specifically? The object is relatively small, has a short lifetime and doesn't need to be transferred or copied

[SteveMacenski]

When you publish a raw message in ROS 2, it gets copied into a unique pointer by the middleware. If you allocate the unique pointer up front and then std::move() into the middleware, then it doesn't have to do that message-sized copy. Plus, if IPC or composition is used, it can also be directly handled which may or may not be entirely zero copy depending on the situation (1-to-1 or 1-to-N).

[tonynajjar]

When you publish a raw message in ROS 2, it gets copied into a unique pointer by the middleware

Okay thanks. It feels like an intermediate-level thing that I should have known. How did you get to know that? Because even the official ROS tutorials don't use a pointer there

[SteveMacenski]

Experience and benchmarking :-)

[tonynajjar]

Something to visualize this in tools/ would be good to pair in this PR so this can be immediately useful to someone

Like what? They can use Foxglove or Plotjuggler. Whatever you have in mind, I can't spend much more time on that debug PR

[tonynajjar]

I'm curious about the perf hit, but I don't think it should be that bad.

The additional time taken when publish_critics_stats is true maxes out at 0.1 ms so we're fine. (false by default anyway)

[SteveMacenski]

Something to visualize this in tools/ would be good to pair in this PR so this can be immediately useful to someone

Like the plotjuggler config you've shown and a 3-5 sentence readme about how to launch with MPPI's publish_critics_stats: true to visualize the critic scores ;-) Nothing fancy, just the tooling you yourself are using so that it can be easily done by the next person

The additional time taken when publish_critics_stats is true maxes out at 0.1 ms so we're fine. (false by default anyway)

Cool cool

[tonynajjar]

Like the plotjuggler config you've shown

These were Foxglove screenshots, I don't use plotjuggler (sorry Davide if you ever read this).

I added the param in the readme and in the docs. I also added a small section explaining more. I wouldn't really add a Foxglove layout config, I think it's more confusing than it helps because the config depends on the order of the critics in their nav2 config - I couldn't find a way to fetch the label from the message.

[mergify]

@tonynajjar, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

[SteveMacenski]

@tonynajjar set the param to true https://github.com/ros-navigation/navigation2/blob/main/nav2_system_tests/src/gps_navigation/nav2_no_map_params.yaml#L57 to at least exercise it in CI