Add debug topic to visualize whether MPPI critic has an effect on costs (#5485)

* Publish criticsStats

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

* linting

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

* change header to stamp

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

* make unique_pointer

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

* typo

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

* Add readme

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

* add to readme

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

* fixes

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

---------

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

Author: tonynajjar

nav2_mppi_controller/CMakeLists.txt

@@ -8,6 +8,7 @@ find_package(geometry_msgs REQUIRED)
 find_package(nav2_common REQUIRED)
 find_package(nav2_core REQUIRED)
 find_package(nav2_costmap_2d REQUIRED)
+find_package(nav2_msgs REQUIRED)
 find_package(nav2_util REQUIRED)
 find_package(nav_msgs REQUIRED)
 find_package(pluginlib REQUIRED)
@@ -59,6 +60,7 @@ target_link_libraries(mppi_controller PUBLIC
   nav2_ros_common::nav2_ros_common
   nav2_costmap_2d::layers
   nav2_costmap_2d::nav2_costmap_2d_core
+  ${nav2_msgs_TARGETS}
   ${nav_msgs_TARGETS}
   pluginlib::pluginlib
   rclcpp::rclcpp

nav2_mppi_controller/README.md

@@ -59,6 +59,7 @@ This process is then repeated a number of times and returns a converged solution
  | retry_attempt_limit        | int    | Default 1. Number of attempts to find feasible trajectory on failure for soft-resets before reporting failure.                                                                                                                                                                                                       |
  | regenerate_noises          | bool   | Default false. 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_optimal_trajectory | bool   | Publishes the full optimal trajectory sequence each control iteration for downstream  control systems, collision checkers, etc to have context beyond the next timestep. |
+ | publish_critics_stats      | bool   | Default false. 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. Useful for debugging and tuning critic behavior. |
 
 
 #### Trajectory Visualizer
@@ -293,6 +294,7 @@ controller_server:
 |---------------------------|----------------------------------|-----------------------------------------------------------------------|
 | `trajectories`            | `visualization_msgs/MarkerArray` | Randomly generated trajectories, including resulting control sequence |
 | `transformed_global_plan` | `nav_msgs/Path`                  | Part of global plan considered by local planner                       |
+| `critics_stats`           | `nav2_msgs/CriticsStats`         | Statistics about each critic's performance (published when `publish_critics_stats` is enabled) |
 
 ## Notes to Users
 
@@ -328,6 +330,26 @@ As you increase or decrease your weights on the Obstacle, you may notice the afo
 
 Once you have your obstacle avoidance behavior tuned and matched with an appropriate path following penalty, tune the Path Align critic to align with the path. If you design exact-path-alignment behavior, its possible to skip the obstacle critic step as highly tuning the system to follow the path will give it less ability to deviate to avoid obstacles (though it'll slow and stop). Tuning the critic weight for the Obstacle critic high will do the job to avoid near-collisions but the repulsion weight is largely unnecessary to you. For others wanting more dynamic behavior, it _can_ be beneficial to slowly lower the weight on the obstacle critic to give the path alignment critic some more room to work. If your path was generated with a cost-aware planner (like all provided by Nav2) and providing paths sufficiently far from obstacles for your satisfaction, the impact of a slightly reduced Obstacle critic with a Path Alignment critic will do you well. Not over-weighting the path align critic will allow the robot to  deviate from the path to get around dynamic obstacles in the scene or other obstacles not previous considered during path planning. It is subjective as to the best behavior for your application, but it has been shown that MPPI can be an exact path tracker and/or avoid dynamic obstacles very fluidly and everywhere in between. The defaults provided are in the generally right regime for a balanced initial trade-off.
 
+### Critic costs debugging
+
+The `publish_critics_stats` parameter enables publishing of statistics about each critic's performance, which can be visualized using tools like PlotJuggler or Foxglove to analyze and debug critic behavior.
+
+The published `nav2_msgs::msg::CriticsStats` message contains the following fields:
+
+- **stamp**: Timestamp of when the statistics were computed
+- **critics**: Array of critic names that were evaluated (e.g., "ConstraintCritic", "GoalCritic", "ObstaclesCritic")
+- **changed**: Boolean array indicating whether each critic modified the trajectory costs. `true` means the critic added non-zero costs, `false` means it had no effect
+- **costs_sum**: Array of the total cost contribution from each critic. This represents the sum of all costs added by that specific critic across all trajectory candidates
+
+This data is invaluable for understanding:
+- Which critics are actively influencing trajectory selection
+- The relative impact of each critic on the final trajectory choice
+- Whether critics are working as expected or if parameter tuning is needed (e.g. `threshold_to_consider`)
+
+More detailed statistics could be added in the future.
+
+![critics_stats](media/critics_stats.png)
+
 ### MFMA and AVX2 Optimizations
 
 This MPPI is made possible to run on CPU-only by using a very well optimized implementation that rely on CPU vectorization through AVX2 and MFMA. All even remotely modern computers support this (2013+), but if using a very old computer you may not be able to use the plugin. Note that MPC is computationally heavy to begin with, so computers circa-2013 even if it were to have those compiler flags available probably wouldn't run it at a satisfactory rate anyway.

nav2_mppi_controller/include/nav2_mppi_controller/critic_manager.hpp

@@ -22,6 +22,7 @@
 
 #include "geometry_msgs/msg/twist.hpp"
 #include "geometry_msgs/msg/twist_stamped.hpp"
+#include "nav2_msgs/msg/critics_stats.hpp"
 
 #include "nav2_costmap_2d/costmap_2d_ros.hpp"
 #include "rclcpp_lifecycle/lifecycle_node.hpp"
@@ -96,6 +97,9 @@ class CriticManager
   std::unique_ptr<pluginlib::ClassLoader<critics::CriticFunction>> loader_;
   Critics critics_;
 
+  nav2::Publisher<nav2_msgs::msg::CriticsStats>::SharedPtr critics_effect_pub_;
+  bool publish_critics_stats_;
+
   rclcpp::Logger logger_{rclcpp::get_logger("MPPIController")};
 };
 

nav2_mppi_controller/media/critics_stats.png

@@ -17,6 +17,7 @@
   <depend>nav2_common</depend>
   <depend>nav2_core</depend>
   <depend>nav2_costmap_2d</depend>
+  <depend>nav2_msgs</depend>
   <depend>nav2_util</depend>
   <depend>nav_msgs</depend>
   <depend>pluginlib</depend>

nav2_mppi_controller/package.xml

@@ -37,6 +37,7 @@ void CriticManager::getParams()
   auto node = parent_.lock();
   auto getParam = parameters_handler_->getParamGetter(name_);
   getParam(critic_names_, "critics", std::vector<std::string>{}, ParameterType::Static);
+  getParam(publish_critics_stats_, "publish_critics_stats", false, ParameterType::Static);
 }
 
 void CriticManager::loadCritics()
@@ -46,6 +47,13 @@ void CriticManager::loadCritics()
       "nav2_mppi_controller", "mppi::critics::CriticFunction");
   }
 
+  auto node = parent_.lock();
+  if (publish_critics_stats_) {
+    critics_effect_pub_ = node->create_publisher<nav2_msgs::msg::CriticsStats>(
+    "~/critics_stats");
+    critics_effect_pub_->on_activate();
+  }
+
   critics_.clear();
   for (auto name : critic_names_) {
     std::string fullname = getFullName(name);
@@ -67,11 +75,44 @@ std::string CriticManager::getFullName(const std::string & name)
 void CriticManager::evalTrajectoriesScores(
   CriticData & data) const
 {
-  for (const auto & critic : critics_) {
+  std::unique_ptr<nav2_msgs::msg::CriticsStats> stats_msg;
+  if (publish_critics_stats_) {
+    stats_msg = std::make_unique<nav2_msgs::msg::CriticsStats>();
+    stats_msg->critics.reserve(critics_.size());
+    stats_msg->changed.reserve(critics_.size());
+    stats_msg->costs_sum.reserve(critics_.size());
+  }
+
+  for (size_t i = 0; i < critics_.size(); ++i) {
     if (data.fail_flag) {
       break;
     }
-    critic->score(data);
+
+    // Store costs before critic evaluation
+    Eigen::ArrayXf costs_before;
+    if (publish_critics_stats_) {
+      costs_before = data.costs;
+    }
+
+    critics_[i]->score(data);
+
+    // Calculate statistics if publishing is enabled
+    if (publish_critics_stats_) {
+      stats_msg->critics.push_back(critic_names_[i]);
+
+      // Calculate sum of costs added by this individual critic
+      Eigen::ArrayXf cost_diff = data.costs - costs_before;
+      float costs_sum = cost_diff.sum();
+      stats_msg->costs_sum.push_back(costs_sum);
+      stats_msg->changed.push_back(costs_sum != 0.0f);
+    }
+  }
+
+  // Publish statistics if enabled
+  if (critics_effect_pub_) {
+    auto node = parent_.lock();
+    stats_msg->stamp = node->get_clock()->now();
+    critics_effect_pub_->publish(std::move(stats_msg));
   }
 }
 

nav2_mppi_controller/src/critic_manager.cpp

@@ -30,6 +30,7 @@ rosidl_generate_interfaces(${PROJECT_NAME}
   "msg/VoxelGrid.msg"
   "msg/BehaviorTreeStatusChange.msg"
   "msg/BehaviorTreeLog.msg"
+  "msg/CriticsStats.msg"
   "msg/Particle.msg"
   "msg/ParticleCloud.msg"
   "msg/WaypointStatus.msg"

nav2_msgs/CMakeLists.txt

@@ -0,0 +1,5 @@
+# Critics statistics message
+builtin_interfaces/Time stamp
+string[] critics
+bool[] changed
+float32[] costs_sum

nav2_msgs/msg/CriticsStats.msg

@@ -76,6 +76,7 @@ controller_server:
       temperature: 0.3
       gamma: 0.015
       motion_model: "DiffDrive"
+      publish_critics_stats: true
       visualize: true
       regenerate_noises: true
       TrajectoryVisualizer: