[Jazzy] Optimize MPPI visualization to skip processing when no subscribers

[SatArw]

---

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	Issue #5801
Primary OS tested on	Ubuntu 24.04.3 LTS (via nav2_dev Docker container)
Robotic platform tested on	Gazebo Simulation (TurtleBot3 Waffle)
Does this PR contain AI generated software?	No
Was this PR description generated by AI software?	No

---

Description of contribution in a few bullet points

• Updated both overloaded instances of TrajectoryVisualizer::add to check subscription counts before executing the marker generation loops.
  • Topics involved: Checks get_subscription_count() on the /trajectories (MarkerArray) and /optimal_trajectory (Path) publishers.
• When visualize is set to true:
  • 0 Subscribers (Both topics): The method returns immediately. No markers or paths are calculated.
  • Subscriber for /optimal_trajectory only: Generates and publishes the visualization data for only the optimal trajectory.
  • Subscriber for /trajectories: Generates the visualization data for all the trajectories (including the optimal_trajectory).
• Reduces controller_server CPU usage.

Description of documentation updates required from your changes

• Since these changes are for the jazzy branch and the documentation only tracks main (rolling), I don't think we need to update any documentation pages.

Description of how this change was tested

• Unit Testing:
  • Ran the nav2_mppi_controller GTest suite locally. Passed 17/17 (100%) tests.
  • Verified that the logic changes did not break existing functionality or style guidelines. (passed ament_uncrustify and cpplint).
• Performance Testing:
  • Used a non-composable launch configuration to run the controller_server as a standalone process.
  • Isolated the specific PID of the controller_server to track its CPU usage exclusively
  • Executed a NavigateThroughPoses action for ~30 seconds across 6 subscription cases and 2 base cases.
• Results:
  • Before: Enabling visualization caused a constant CPU spike even with 0 subscribers.
  • After: CPU spikes only when there are subscribers to /trajectories

Cases

Navigation Task

Average CPU Load Summary

CPU Usage Timeline (Before vs After)

---

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

[mergify]

@SatArw, all pull requests must be targeted towards the main development branch.
Once merged into main, it is possible to backport to @jazzy, but it must be in main
to have these changes reflected into new distributions.

[codecov]

Codecov Report

✅ All modified and coverable lines are covered by tests.

Files with missing lines	Coverage Δ
nav2_mppi_controller/src/trajectory_visualizer.cpp	100.00% <100.00%> (ø)

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

[mini-1235]

Conceptually, I agree with this PR, but I'll leave it to @SteveMacenski in case he has other preferences

[SteveMacenski]

Please submit against the main branch so that it is applied in rolling and all other new branches

[SatArw]

Sure, I will submit these changes as a new PR against the main branch.

1. Do I need to conduct the tests across multiple cases and report the 'Average CPU Load' and 'CPU Usage Timeline' again?
2. To request for backporting these changes to Jazzy, do I need to perform any additional steps?

I'm new to this, so I'd appreciate these clarifications. Thank you.

[mini-1235]

Do I need to conduct the tests across multiple cases and report the 'Average CPU Load' and 'CPU Usage Timeline' again?

Typically we require more comprehensive testing before submitting a PR. However, since you have already tested this on the Jazzy branch and the changes are relatively simple, I think it's fine not to rerun everything.

To request for backporting these changes to Jazzy, do I need to perform any additional steps?

You can leave this PR open. Once the PR targeting main is merged, we'll merge this one for Jazzy too :)

[SteveMacenski]

Ill leave for Maurice to review / approve before I merge