From 466875bd83d7223ac7584a0b275aef5a6378fde4 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Wed, 18 Mar 2026 22:44:48 +0000 Subject: [PATCH 1/2] docs(mppi): enhance trajectory visualization with per-critic cost coloring and update parameters Signed-off-by: Tony Najjar --- configuration/packages/configuring-mppic.rst | 25 ++++++++++---------- migration/Kilted.rst | 8 +++++++ 2 files changed, 21 insertions(+), 12 deletions(-) diff --git a/configuration/packages/configuring-mppic.rst b/configuration/packages/configuring-mppic.rst index bcd4771cd2..62e98033fa 100644 --- a/configuration/packages/configuring-mppic.rst +++ b/configuration/packages/configuring-mppic.rst @@ -259,7 +259,18 @@ MPPI Parameters ============== =========================== Description - Whether to publish debugging trajectories for visualization. This can slow down the controller substantially (e.g. 1000 batches of 56 size every 30hz is a lot of data). + Whether to publish debugging trajectories for visualization and critic statistics. When enabled, candidate trajectories are colored by cost (green to red gradient, magenta for collisions) and a ``nav2_msgs::msg::CriticsStats`` message is published on the ``~/critics_stats`` topic. This can slow down the controller substantially (e.g. 1000 batches of 56 size every 30hz is a lot of data). + +:critic_index_to_visualize: + + ============== =========================== + Type Default + -------------- --------------------------- + int 0 + ============== =========================== + + Description + Selects which critic to visualize when ``visualize`` is true. ``0`` shows the total cost across all critics, ``1..N`` selects an individual critic by index (in the order listed in the ``critics`` parameter). :publish_optimal_trajectory: @@ -305,17 +316,6 @@ MPPI Parameters Description Whether to regenerate noises each iteration or use single noise distribution computed on initialization and reset. Practically, this is found to work fine since the trajectories are being sampled stochastically from a normal distribution and reduces compute jittering at run-time due to thread wake-ups to resample normal distribution. -:publish_critics_stats: - - ============== =========================== - Type Default - -------------- --------------------------- - bool false - ============== =========================== - - Description - Whether to publish statistics about each critic's performance. When enabled, publishes a ``nav2_msgs::msg::CriticsStats`` message containing critic names, whether they changed costs, and the sum of costs added by each critic for all trajectory samples. Useful for debugging and tuning critic behavior but should not be enabled for generic runtime use. - :open_loop: ============== =========================== @@ -1087,6 +1087,7 @@ Example gamma: 0.015 motion_model: "DiffDrive" visualize: false + critic_index_to_visualize: 0 reset_period: 1.0 # (only in Humble) regenerate_noises: false TrajectoryVisualizer: diff --git a/migration/Kilted.rst b/migration/Kilted.rst index 91bc07af8f..d3bd74ef37 100644 --- a/migration/Kilted.rst +++ b/migration/Kilted.rst @@ -860,3 +860,11 @@ See: - :ref:`configuring_collision_monitor_node` - :ref:`configuring_collision_detector_node` + +MPPI per-critic trajectory cost visualization +--------------------------------------------- + +`PR #6036 `_ enhances MPPI trajectory visualization with per-critic cost coloring. +When ``visualize`` is enabled, candidate trajectories are now rendered as cost-colored lines using a green-to-yellow-to-red gradient, with collision trajectories shown in magenta. +A new ``critic_index_to_visualize`` parameter (default ``0``) selects which critic's costs to display: ``0`` shows the total cost across all critics, while ``1..N`` selects an individual critic by index. +The ``publish_critics_stats`` parameter has been removed; critic statistics (``~/critics_stats`` topic) are now published automatically when ``visualize`` is enabled. From 9c0bb4fc0878b66e7e4611c32f84e0d094274512 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Thu, 19 Mar 2026 16:00:47 +0100 Subject: [PATCH 2/2] Update configuration/packages/configuring-mppic.rst Co-authored-by: Steve Macenski Signed-off-by: Tony Najjar --- configuration/packages/configuring-mppic.rst | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/configuration/packages/configuring-mppic.rst b/configuration/packages/configuring-mppic.rst index 62e98033fa..341f3d728a 100644 --- a/configuration/packages/configuring-mppic.rst +++ b/configuration/packages/configuring-mppic.rst @@ -270,7 +270,7 @@ MPPI Parameters ============== =========================== Description - Selects which critic to visualize when ``visualize`` is true. ``0`` shows the total cost across all critics, ``1..N`` selects an individual critic by index (in the order listed in the ``critics`` parameter). + Selects which critic to visualize the color-scheme of when ``visualize`` is true publishing Marker messages for visualization in rviz. ``0`` shows the total cost across all critics, ``1..N`` selects an individual critic by index (in the order listed in the ``critics`` parameter). :publish_optimal_trajectory: