Add debug topic to visualize whether MPPI critic has an effect on costs

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/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/package.xml

@@ -18,6 +18,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/src/critic_manager.cpp

@@ -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,47 @@ std::string CriticManager::getFullName(const std::string & name)
 void CriticManager::evalTrajectoriesScores(
   CriticData & data) const
 {
-  for (const auto & critic : critics_) {
+  nav2_msgs::msg::CriticsStats stats_msg;
+
+  if (publish_critics_stats_) {
+    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_) {
+      std::string critic_name = critic_names_[i];
+      stats_msg.critics.push_back(critic_name);
+
+      // 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);
+
+      bool costs_changed = costs_sum != 0.0f;
+      stats_msg.changed.push_back(costs_changed);
+    }
+  }
+
+  // Publish statistics if enabled
+  if (publish_critics_stats_ && critics_effect_pub_) {
+    auto node = parent_.lock();
+    stats_msg.header.stamp = node->get_clock()->now();
+    critics_effect_pub_->publish(stats_msg);
   }
 }
 

nav2_msgs/CMakeLists.txt

@@ -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/msg/CriticsStats.msg

@@ -0,0 +1,5 @@
+# Critics statistics message
+std_msgs/Header header
+string[] critics
+bool[] changed
+float32[] costs_sum