ros-navigation · SteveMacenski · Sep 20, 2025 · Aug 26, 2025 · Aug 26, 2025 · Aug 26, 2025
diff --git a/nav2_amcl/package.xml b/nav2_amcl/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav2_amcl</name>
-  <version>1.4.1</version>
+  <version>1.4.2</version>
   <description>
     <p>
       amcl is a probabilistic localization system for a robot moving in

diff --git a/nav2_behavior_tree/package.xml b/nav2_behavior_tree/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav2_behavior_tree</name>
-  <version>1.4.1</version>
+  <version>1.4.2</version>
   <description>Nav2 behavior tree wrappers, nodes, and utilities</description>
   <maintainer email="[email protected]">Michael Jeronimo</maintainer>
   <maintainer email="[email protected]">Carlos Orduno</maintainer>

diff --git a/nav2_behaviors/package.xml b/nav2_behaviors/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav2_behaviors</name>
-  <version>1.4.1</version>
+  <version>1.4.2</version>
   <description>Nav2 behavior server</description>
   <maintainer email="[email protected]">Carlos Orduno</maintainer>
   <maintainer email="[email protected]">Steve Macenski</maintainer>

diff --git a/nav2_bringup/package.xml b/nav2_bringup/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav2_bringup</name>
-  <version>1.4.1</version>
+  <version>1.4.2</version>
   <description>Bringup scripts and configurations for the Nav2 stack</description>
   <maintainer email="[email protected]">Michael Jeronimo</maintainer>
   <maintainer email="[email protected]">Steve Macenski</maintainer>

diff --git a/nav2_bt_navigator/package.xml b/nav2_bt_navigator/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav2_bt_navigator</name>
-  <version>1.4.1</version>
+  <version>1.4.2</version>
   <description>Nav2 BT Navigator Server</description>
   <maintainer email="[email protected]">Michael Jeronimo</maintainer>
   <license>Apache-2.0</license>

diff --git a/nav2_collision_monitor/package.xml b/nav2_collision_monitor/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav2_collision_monitor</name>
-  <version>1.4.1</version>
+  <version>1.4.2</version>
   <description>Collision Monitor</description>
   <maintainer email="[email protected]">Alexey Merzlyakov</maintainer>
   <maintainer email="[email protected]">Steve Macenski</maintainer>

diff --git a/nav2_common/package.xml b/nav2_common/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav2_common</name>
-  <version>1.4.1</version>
+  <version>1.4.2</version>
   <description>Common support functionality used throughout the navigation 2 stack</description>
   <maintainer email="[email protected]">Carl Delsey</maintainer>
   <license>Apache-2.0</license>

diff --git a/nav2_constrained_smoother/package.xml b/nav2_constrained_smoother/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav2_constrained_smoother</name>
-  <version>1.4.1</version>
+  <version>1.4.2</version>
   <description>Ceres constrained smoother</description>
   <maintainer email="[email protected]">Matej Vargovcik</maintainer>
   <maintainer email="[email protected]">Steve Macenski</maintainer>

diff --git a/nav2_controller/package.xml b/nav2_controller/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav2_controller</name>
-  <version>1.4.1</version>
+  <version>1.4.2</version>
   <description>Controller action interface</description>
   <maintainer email="[email protected]">Carl Delsey</maintainer>
   <license>Apache-2.0</license>

diff --git a/nav2_core/package.xml b/nav2_core/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav2_core</name>
-  <version>1.4.1</version>
+  <version>1.4.2</version>
   <description>A set of headers for plugins core to the Nav2 stack</description>
   <maintainer email="[email protected]">Steve Macenski</maintainer>
   <maintainer email="[email protected]">Carl Delsey</maintainer>

diff --git a/nav2_costmap_2d/package.xml b/nav2_costmap_2d/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format ="3">
   <name>nav2_costmap_2d</name>
-  <version>1.4.1</version>
+  <version>1.4.2</version>
   <description>
     This package provides an implementation of a 2D costmap that takes in sensor
     data from the world, builds a 2D or 3D occupancy grid of the data (depending

diff --git a/nav2_docking/opennav_docking/package.xml b/nav2_docking/opennav_docking/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>opennav_docking</name>
-  <version>1.4.1</version>
+  <version>1.4.2</version>
   <description>A Task Server for robot charger docking</description>
   <maintainer email="[email protected]">Steve Macenski</maintainer>
   <license>Apache-2.0</license>

diff --git a/nav2_docking/opennav_docking_bt/package.xml b/nav2_docking/opennav_docking_bt/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>opennav_docking_bt</name>
-  <version>1.4.1</version>
+  <version>1.4.2</version>
   <description>A set of BT nodes and XMLs for docking</description>
   <maintainer email="[email protected]">Steve Macenski</maintainer>
   <license>Apache-2.0</license>

diff --git a/nav2_docking/opennav_docking_core/package.xml b/nav2_docking/opennav_docking_core/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>opennav_docking_core</name>
-  <version>1.4.1</version>
+  <version>1.4.2</version>
   <description>A set of headers for plugins core to the opennav docking framework</description>
   <maintainer email="[email protected]">Steve Macenski</maintainer>
   <license>Apache-2.0</license>

diff --git a/nav2_dwb_controller/costmap_queue/package.xml b/nav2_dwb_controller/costmap_queue/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>costmap_queue</name>
-  <version>1.4.1</version>
+  <version>1.4.2</version>
   <description>The costmap_queue package</description>
   <maintainer email="[email protected]">David V. Lu!!</maintainer>
   <license>BSD-3-Clause</license>

diff --git a/nav2_dwb_controller/dwb_core/package.xml b/nav2_dwb_controller/dwb_core/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>dwb_core</name>
-  <version>1.4.1</version>
+  <version>1.4.2</version>
   <description>DWB core interfaces package</description>
   <maintainer email="[email protected]">Carl Delsey</maintainer>
   <license>BSD-3-Clause</license>

diff --git a/nav2_dwb_controller/dwb_critics/package.xml b/nav2_dwb_controller/dwb_critics/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>dwb_critics</name>
-  <version>1.4.1</version>
+  <version>1.4.2</version>
   <description>The dwb_critics package</description>
   <maintainer email="[email protected]">David V. Lu!!</maintainer>
   <license>BSD-3-Clause</license>

diff --git a/nav2_dwb_controller/dwb_msgs/package.xml b/nav2_dwb_controller/dwb_msgs/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>dwb_msgs</name>
-  <version>1.4.1</version>
+  <version>1.4.2</version>
   <description>Message/Service definitions specifically for the dwb_core</description>
   <maintainer email="[email protected]">David V. Lu!!</maintainer>
   <license>BSD-3-Clause</license>

diff --git a/nav2_dwb_controller/dwb_plugins/package.xml b/nav2_dwb_controller/dwb_plugins/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>dwb_plugins</name>
-  <version>1.4.1</version>
+  <version>1.4.2</version>
   <description>
       Standard implementations of the GoalChecker
       and TrajectoryGenerators for dwb_core

diff --git a/nav2_dwb_controller/nav2_dwb_controller/package.xml b/nav2_dwb_controller/nav2_dwb_controller/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav2_dwb_controller</name>
-  <version>1.4.1</version>
+  <version>1.4.2</version>
   <description>
     ROS2 controller (DWB) metapackage
   </description>

diff --git a/nav2_dwb_controller/nav_2d_msgs/package.xml b/nav2_dwb_controller/nav_2d_msgs/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav_2d_msgs</name>
-  <version>1.4.1</version>
+  <version>1.4.2</version>
   <description>Basic message types for two dimensional navigation, extending from geometry_msgs::Pose2D.</description>
   <maintainer email="[email protected]">David V. Lu!!</maintainer>
   <license>BSD-3-Clause</license>

diff --git a/nav2_dwb_controller/nav_2d_utils/package.xml b/nav2_dwb_controller/nav_2d_utils/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav_2d_utils</name>
-  <version>1.4.1</version>
+  <version>1.4.2</version>
   <description>A handful of useful utility functions for nav_2d packages.</description>
   <maintainer email="[email protected]">David V. Lu!!</maintainer>
   <license>BSD-3-Clause</license>

diff --git a/nav2_graceful_controller/package.xml b/nav2_graceful_controller/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav2_graceful_controller</name>
-  <version>1.4.1</version>
+  <version>1.4.2</version>
   <description>Graceful motion controller</description>
   <maintainer email="[email protected]">Alberto Tudela</maintainer>
   <license>Apache-2.0</license>

diff --git a/nav2_lifecycle_manager/package.xml b/nav2_lifecycle_manager/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav2_lifecycle_manager</name>
-  <version>1.4.1</version>
+  <version>1.4.2</version>
   <description>A controller/manager for the lifecycle nodes of the Navigation 2 system</description>
   <maintainer email="[email protected]">Michael Jeronimo</maintainer>
   <license>Apache-2.0</license>

diff --git a/nav2_loopback_sim/package.xml b/nav2_loopback_sim/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav2_loopback_sim</name>
-  <version>1.4.1</version>
+  <version>1.4.2</version>
   <description>A loopback simulator to replace physics simulation</description>
   <maintainer email="[email protected]">steve macenski</maintainer>
   <license>Apache-2.0</license>

diff --git a/nav2_map_server/package.xml b/nav2_map_server/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav2_map_server</name>
-  <version>1.4.1</version>
+  <version>1.4.2</version>
   <description>
     Refactored map server for ROS2 Navigation
   </description>

diff --git a/nav2_mppi_controller/CMakeLists.txt b/nav2_mppi_controller/CMakeLists.txt
@@ -7,6 +7,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)
@@ -56,6 +57,7 @@ target_link_libraries(mppi_controller PUBLIC
   nav2_core::nav2_core
   nav2_costmap_2d::layers
   nav2_costmap_2d::nav2_costmap_2d_core
+  ${nav2_msgs_TARGETS}
   ${nav_msgs_TARGETS}
   pluginlib::pluginlib
   rclcpp::rclcpp

diff --git a/nav2_mppi_controller/README.md b/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.
diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critic_manager.hpp b/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,11 @@ class CriticManager
   std::unique_ptr<pluginlib::ClassLoader<critics::CriticFunction>> loader_;
   Critics critics_;
 
+  rclcpp_lifecycle::LifecyclePublisher<nav2_msgs::msg::CriticsStats>::SharedPtr
+    critics_effect_pub_;
+
+  bool publish_critics_stats_;
+
   rclcpp::Logger logger_{rclcpp::get_logger("MPPIController")};
 };
 

diff --git a/nav2_mppi_controller/media/critics_stats.png b/nav2_mppi_controller/media/critics_stats.png
diff --git a/nav2_mppi_controller/package.xml b/nav2_mppi_controller/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav2_mppi_controller</name>
-  <version>1.4.1</version>
+  <version>1.4.2</version>
   <description>nav2_mppi_controller</description>
   <maintainer email="[email protected]">Steve Macenski</maintainer>
   <maintainer email="[email protected]">Aleksei Budyakov</maintainer>
@@ -12,11 +12,13 @@
   <buildtool_depend>ament_cmake_ros</buildtool_depend>
 
   <depend>angles</depend>
+  <depend>backward_ros</depend>
+  <depend>benchmark</depend>
   <depend>geometry_msgs</depend>
-  <depend>libomp-dev</depend>
   <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>