MPPI: visualize trajectory per-critic + total costs

nav2_mppi_controller/README.md

@@ -56,10 +56,10 @@ This process is then repeated a number of times and returns a converged solution
  | temperature                | double | Default: 0.3. Selectiveness of trajectories by their costs (The closer this value to 0, the "more" we take in consideration controls with less cost), 0 mean use control with best cost, huge value will lead to just taking mean of all trajectories without cost consideration                                                   |
  | gamma                      | double | Default: 0.015. A trade-off between smoothness (high) and low energy (low). This is a complex parameter that likely won't need to be changed from the default of `0.1` which works well for a broad range of cases. See Section 3D-2 in "Information Theoretic Model Predictive Control: Theory and Applications to Autonomous Driving" for detailed information.       |
  | visualize                  | bool   | Default: false. Publish visualization of trajectories, which can slow down the controller significantly. Use only for debugging.                                                                                                                                       |
+ | critic_index_to_visualize  | int    | Default: 0. 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.                                                                                                        |
  | 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. |
  | open_loop        | bool    | Default false. Useful when using low accelerations and when wheel odometry's latency causes issues in initial state estimation. |
 
 #### Trajectory Visualizer
@@ -282,7 +282,7 @@ controller_server:
 | Topic                     | Type                             | Description                                                           |
 |---------------------------|----------------------------------|-----------------------------------------------------------------------|
 | `trajectories`            | `visualization_msgs/MarkerArray` | Randomly generated trajectories, including resulting control sequence |
-| `critics_stats`           | `nav2_msgs/CriticsStats`         | Statistics about each critic's performance (published when `publish_critics_stats` is enabled) |
+| `critics_stats`           | `nav2_msgs/CriticsStats`         | Statistics about each critic's performance (published when `visualize` is enabled) |
 
 ## Notes to Users
 
@@ -320,7 +320,7 @@ Once you have your obstacle avoidance behavior tuned and matched with an appropr
 
 ### 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 `visualize` 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:
 

nav2_mppi_controller/include/nav2_mppi_controller/controller.hpp

@@ -125,6 +125,7 @@ class MPPIController : public nav2_core::Controller
   TrajectoryVisualizer trajectory_visualizer_;
 
   bool visualize_;
+  int critic_index_to_visualize_;
   bool publish_optimal_trajectory_;
 };
 

nav2_mppi_controller/include/nav2_mppi_controller/critic_data.hpp

@@ -51,6 +51,7 @@ struct CriticData
   std::shared_ptr<MotionModel> motion_model;
   std::optional<std::vector<bool>> path_pts_valid;
   std::optional<size_t> furthest_reached_path_point;
+  std::vector<bool> trajectories_in_collision;
 };
 
 }  // namespace mppi

nav2_mppi_controller/include/nav2_mppi_controller/critic_manager.hpp

@@ -17,6 +17,7 @@
 
 #include <memory>
 #include <string>
+#include <utility>
 #include <vector>
 #include <pluginlib/class_loader.hpp>
 
@@ -69,7 +70,16 @@ class CriticManager
     * @brief Score trajectories by the set of loaded critic functions
     * @param CriticData Struct of necessary information to pass to the critic functions
     */
-  void evalTrajectoriesScores(CriticData & data) const;
+  void evalTrajectoriesScores(CriticData & data);
+
+  /**
+    * @brief Get stored per-critic costs from last evaluation
+    * @return Vector of (critic_name, cost_array) pairs
+    */
+  const std::vector<std::pair<std::string, Eigen::ArrayXf>> & getCriticCosts() const
+  {
+    return critic_costs_;
+  }
 
 protected:
   /**
@@ -98,7 +108,9 @@ class CriticManager
   Critics critics_;
 
   nav2::Publisher<nav2_msgs::msg::CriticsStats>::SharedPtr critics_effect_pub_;
-  bool publish_critics_stats_;
+  bool visualize_;
+  std::vector<std::pair<std::string, Eigen::ArrayXf>> critic_costs_;
+  rclcpp::Clock::SharedPtr clock_;
 
   rclcpp::Logger logger_{rclcpp::get_logger("MPPIController")};
 };

nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp

@@ -20,6 +20,8 @@
 #include <string>
 #include <memory>
 #include <tuple>
+#include <utility>
+#include <vector>
 
 #include "rclcpp_lifecycle/lifecycle_node.hpp"
 
@@ -118,6 +120,30 @@ class Optimizer
    */
   const models::ControlSequence & getOptimalControlSequence();
 
+  /**
+   * @brief Get the aggregated trajectory costs from last evaluation
+   * @return Array of costs per trajectory
+   */
+  const Eigen::ArrayXf & getCosts() const {return costs_;}
+
+  /**
+   * @brief Get per-critic cost breakdown from last evaluation
+   * @return Vector of (critic_name, cost_array) pairs
+   */
+  const std::vector<std::pair<std::string, Eigen::ArrayXf>> & getCriticCosts() const
+  {
+    return critic_manager_.getCriticCosts();
+  }
+
+  /**
+   * @brief Get per-trajectory collision flags from last evaluation
+   * @return Vector of bools, true if trajectory is in collision
+   */
+  const std::vector<bool> & getCollisionFlags() const
+  {
+    return critics_data_.trajectories_in_collision;
+  }
+
   /**
    * @brief Set the maximum speed based on the speed limits callback
    * @param speed_limit Limit of the speed for use
@@ -291,7 +317,7 @@ class Optimizer
   CriticData critics_data_ = {
     state_, generated_trajectories_, path_, goal_,
     costs_, settings_.model_dt, false, nullptr, nullptr,
-    std::nullopt, std::nullopt};  /// Caution, keep references
+    std::nullopt, std::nullopt, {}};  /// Caution, keep references
 
   rclcpp::Logger logger_{rclcpp::get_logger("MPPIController")};
 

nav2_mppi_controller/include/nav2_mppi_controller/tools/trajectory_visualizer.hpp

@@ -19,6 +19,8 @@
 
 #include <memory>
 #include <string>
+#include <utility>
+#include <vector>
 
 #include "nav_msgs/msg/path.hpp"
 #include "nav2_ros_common/lifecycle_node.hpp"
@@ -78,10 +80,17 @@ class TrajectoryVisualizer
     const builtin_interfaces::msg::Time & cmd_stamp);
 
   /**
-    * @brief Add candidate trajectories to visualize
+    * @brief Add candidate trajectories colored by cost gradient
     * @param trajectories Candidate trajectories
+    * @param costs Cost per trajectory for the selected layer
+    * @param collisions Per-trajectory collision flags
+    * @param stamp Timestamp for markers
     */
-  void add(const models::Trajectories & trajectories, const std::string & marker_namespace);
+  void add(
+    const models::Trajectories & trajectories,
+    const Eigen::ArrayXf & costs,
+    const std::vector<bool> & collisions,
+    const builtin_interfaces::msg::Time & stamp);
 
   /**
     * @brief Visualize the plan
@@ -94,6 +103,28 @@ class TrajectoryVisualizer
   void reset();
 
 protected:
+  /**
+    * @brief Create a LINE_STRIP marker for a single trajectory colored by normalized cost
+    * @param trajectory_idx Row index into the trajectories arrays
+    * @param trajectories Trajectory data
+    * @param normalized_cost Cost value in [0, 1] range
+    * @param in_collision Whether this trajectory is in collision
+    * @param stamp Timestamp
+    */
+  void addCostColoredTrajectory(
+    size_t trajectory_idx,
+    const models::Trajectories & trajectories,
+    float normalized_cost,
+    bool in_collision,
+    const builtin_interfaces::msg::Time & stamp);
+
+  /**
+    * @brief Convert a normalized cost [0,1] to a green->yellow->red color
+    * @param normalized Value in [0, 1]
+    * @return ColorRGBA
+    */
+  static std_msgs::msg::ColorRGBA costToColor(float normalized);
+
   std::string frame_id_;
   nav2::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr
     trajectories_publisher_;

nav2_mppi_controller/src/controller.cpp

@@ -37,6 +37,7 @@ void MPPIController::configure(
   // Get high-level controller parameters
   auto getParam = parameters_handler_->getParamGetter(name_);
   getParam(visualize_, "visualize", false);
+  getParam(critic_index_to_visualize_, "critic_index_to_visualize", 0);
 
   getParam(publish_optimal_trajectory_, "publish_optimal_trajectory", false);
 
@@ -138,7 +139,19 @@ void MPPIController::visualize(
   const builtin_interfaces::msg::Time & cmd_stamp,
   const Eigen::ArrayXXf & optimal_trajectory)
 {
-  trajectory_visualizer_.add(optimizer_.getGeneratedTrajectories(), "Candidate Trajectories");
+  const auto & critic_costs = optimizer_.getCriticCosts();
+  const Eigen::ArrayXf & costs =
+    (critic_index_to_visualize_ <= 0 ||
+    critic_index_to_visualize_ > static_cast<int>(critic_costs.size())) ?
+    optimizer_.getCosts() :
+    critic_costs[critic_index_to_visualize_ - 1].second;
+
+  trajectory_visualizer_.add(
+    optimizer_.getGeneratedTrajectories(),
+    costs,
+    optimizer_.getCollisionFlags(),
+    cmd_stamp);
+
   trajectory_visualizer_.add(optimal_trajectory, "Optimal Trajectory", cmd_stamp);
   trajectory_visualizer_.visualize();
 }

nav2_mppi_controller/src/critic_manager.cpp

@@ -26,6 +26,7 @@ void CriticManager::on_configure(
   name_ = name;
   auto node = parent_.lock();
   logger_ = node->get_logger();
+  clock_ = node->get_clock();
   parameters_handler_ = param_handler;
 
   getParams();
@@ -37,7 +38,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);
+  getParam(visualize_, "visualize", false);
 }
 
 void CriticManager::loadCritics()
@@ -48,7 +49,7 @@ void CriticManager::loadCritics()
   }
 
   auto node = parent_.lock();
-  if (publish_critics_stats_) {
+  if (visualize_) {
     critics_effect_pub_ = node->create_publisher<nav2_msgs::msg::CriticsStats>(
       "~/critics_stats");
     critics_effect_pub_->on_activate();
@@ -73,10 +74,13 @@ std::string CriticManager::getFullName(const std::string & name)
 }
 
 void CriticManager::evalTrajectoriesScores(
-  CriticData & data) const
+  CriticData & data)
 {
   std::unique_ptr<nav2_msgs::msg::CriticsStats> stats_msg;
-  if (publish_critics_stats_) {
+  if (visualize_) {
+    data.trajectories_in_collision.assign(data.costs.size(), false);
+    critic_costs_.clear();
+    critic_costs_.reserve(critics_.size());
     stats_msg = std::make_unique<nav2_msgs::msg::CriticsStats>();
     stats_msg->critics.reserve(critics_.size());
     stats_msg->changed.reserve(critics_.size());
@@ -90,28 +94,28 @@ void CriticManager::evalTrajectoriesScores(
 
     // Store costs before critic evaluation
     Eigen::ArrayXf costs_before;
-    if (publish_critics_stats_) {
+    if (visualize_) {
       costs_before = data.costs;
     }
 
     critics_[i]->score(data);
 
-    // Calculate statistics if publishing is enabled
-    if (publish_critics_stats_) {
+    // Calculate statistics if visualization is enabled
+    if (visualize_) {
       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);
+      critic_costs_.emplace_back(critic_names_[i], std::move(cost_diff));
     }
   }
 
   // Publish statistics if enabled
-  if (critics_effect_pub_) {
-    auto node = parent_.lock();
-    stats_msg->stamp = node->get_clock()->now();
+  if (visualize_ && critics_effect_pub_) {
+    stats_msg->stamp = clock_->now();
     critics_effect_pub_->publish(std::move(stats_msg));
   }
 }

nav2_mppi_controller/src/critics/cost_critic.cpp

@@ -158,6 +158,9 @@ void CostCritic::score(CriticData & data)
   repulsive_cost.setZero();
   bool all_trajectories_collide = true;
 
+  auto & collisions = data.trajectories_in_collision;
+  const bool track_collisions = !collisions.empty();
+
   int strided_traj_cols = floor((data.trajectories.x.cols() - 1) / trajectory_point_step_) + 1;
   int strided_traj_rows = data.trajectories.x.rows();
   int outer_stride = strided_traj_rows * trajectory_point_step_;
@@ -200,6 +203,7 @@ void CostCritic::score(CriticData & data)
       if (inCollision(pose_cost, Tx, Ty, traj_yaw(i, j))) {
         traj_cost = collision_cost_;
         trajectory_collide = true;
+        if (track_collisions) {collisions[i] = true;}
         break;
       }
 

nav2_mppi_controller/src/critics/obstacles_critic.cpp

@@ -141,6 +141,9 @@ void ObstaclesCritic::score(CriticData & data)
   const unsigned int batch_size = data.trajectories.x.rows();
   bool all_trajectories_collide = true;
 
+  auto & collisions = data.trajectories_in_collision;
+  const bool track_collisions = !collisions.empty();
+
   for (unsigned int i = 0; i != batch_size; i++) {
     bool trajectory_collide = false;
     float traj_cost = 0.0f;
@@ -178,6 +181,7 @@ void ObstaclesCritic::score(CriticData & data)
 
     if (!trajectory_collide) {all_trajectories_collide = false;}
     raw_cost(i) = trajectory_collide ? collision_cost_ : traj_cost;
+    if (trajectory_collide && track_collisions) {collisions[i] = true;}
   }
 
   // Normalize repulsive cost by trajectory length & lowest score to not overweight importance