Add MPPI visualization

[adivardi]

Add MPPI visualization based on ros-navigation#5643

Note: This is a cherry-pick of a PR which was stopped mid-progress. It works wel but may not be the cleanest.

Note 2: Recentely, another vis PR was merged, but using a slightly different method and missing the visualization of furthest_reached point for critics. Considering the current code is ok for us, and the merged PR is missing the most useful feature, I decided to use this version, and reconsider in the next rebase on upstream,

[gemini-code-assist]

Summary of Changes

Hello, I'm Gemini Code Assist¹! I'm currently reviewing this pull request and will post my feedback shortly. In the meantime, here's a summary to help you and other reviewers quickly get up to speed!

This pull request significantly upgrades the visualization capabilities of the MPPI controller. It provides granular control over what aspects of the controller's operation are visualized, from individual trajectory costs to the robot's footprint along the chosen path. This enhancement aims to offer developers and users a clearer understanding of the controller's behavior and performance, facilitating debugging and tuning.

Highlights

• Enhanced MPPI Visualization: Introduced a comprehensive visualization system for the MPPI controller, allowing detailed inspection of candidate trajectories, their total costs, and individual critic contributions.
• Per-Critic Cost Visualization: Added the ability to visualize the cost contribution of each individual critic function, providing deeper insights into the decision-making process.
• Furthest Reached Path Point Visualization: Implemented visualization for the 'furthest reached path point' for PathAlign, PathAngle, and PathFollow critics, aiding in understanding path progression and alignment.
• Optimal Footprint Visualization: Enabled the visualization of the robot's footprint along the optimal trajectory, which can be downsampled for performance.
• Refactored Visualization Logic: Consolidated and refactored all visualization-related logic into a dedicated TrajectoryVisualizer class, improving modularity and maintainability.
• New Configuration Parameters: Added new parameters under a Visualization namespace in nav2_params.yaml to control various aspects of the new visualization features dynamically.

Using Gemini Code Assist

The full guide for Gemini Code Assist can be found on our documentation page, here are some quick tips.

Invoking Gemini

You can request assistance from Gemini at any point by creating a comment using either /gemini <command> or @gemini-code-assist <command>. Below is a summary of the supported commands on the current page.

Feature	Command	Description
Code Review	/gemini review	Performs a code review for the current pull request in its current state.
Pull Request Summary	/gemini summary	Provides a summary of the current pull request in its current state.
Comment	@gemini-code-assist	Responds in comments when explicitly tagged, both in pull request comments and review comments.
Help	/gemini help	Displays a list of available commands.

Customization

To customize Gemini Code Assist for GitHub experience, repository maintainers can create a configuration file and/or provide a custom code review style guide (such as PEP-8 for Python) by creating and adding files to a .gemini/ folder in the base of the repository. Detailed instructions can be found here.

Limitations & Feedback

Gemini Code Assist may make mistakes. Please leave feedback on any instances where its feedback is incorrect or counter productive. You can react with 👍 and 👎 on @gemini-code-assist comments. If you're interested in giving your feedback about your experience with Gemini Code Assist for GitHub and other Google products, sign up here.

Footnotes

1. Review the Privacy Notices, Generative AI Prohibited Use Policy, Terms of Service, and learn how to configure Gemini Code Assist in GitHub here. Gemini can make mistakes, so double check it and use code with caution. ↩

[gemini-code-assist]

Code Review

This pull request refactors the MPPI controller's visualization system by introducing a new Visualization parameter namespace in nav2_params.yaml and consolidating all visualization logic into a dedicated TrajectoryVisualizer class. The changes include adding new parameters for publishing various trajectory data (total costs, individual critic costs, optimal footprints, optimal trajectory messages, and optimal paths) and removing older, less granular visualization flags. The MPPIController now uses a single visualize call on the TrajectoryVisualizer, passing all necessary data. CriticData is extended to store individual critic costs, and CriticManager is updated to populate these costs when visualize_per_critic_costs is enabled. Additionally, individual critics (PathAlignCritic, PathAngleCritic, PathFollowCritic) gain the ability to visualize their furthest reached path points. A review comment highlights a bug where a new test, VisPathRepub, was added for publish_transformed_path, but the corresponding publishing logic was not implemented, causing the test to fail.

[adivardi]

self review