From f5b91211d6e06e0e5cdb6b1e9fe048a7a30c8da0 Mon Sep 17 00:00:00 2001
From: Adi Vardi <adi.vardi@enway.ai>
Date: Fri, 13 Feb 2026 12:26:55 +0100
Subject: [PATCH 01/32] improve comment a bit

---
 nav2_mppi_controller/src/critics/path_align_critic.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/nav2_mppi_controller/src/critics/path_align_critic.cpp b/nav2_mppi_controller/src/critics/path_align_critic.cpp
index 021c7170798..120e0c100b6 100644
--- a/nav2_mppi_controller/src/critics/path_align_critic.cpp
+++ b/nav2_mppi_controller/src/critics/path_align_critic.cpp
@@ -94,7 +94,7 @@ void PathAlignCritic::score(CriticData & data)
   Eigen::ArrayXf cost(data.costs.rows());
   cost.setZero();
 
-  // Find integrated distance in the path
+  // Find integrated arc-length distance along the path = total dist traveled along the path to each path point
   std::vector<float> path_integrated_distances(path_segments_count, 0.0f);
   std::vector<utils::Pose2D> path(path_segments_count);
   float dx = 0.0f, dy = 0.0f;

From 34c8540615d38a2558266bd8e1f88a2f4bea4c6b Mon Sep 17 00:00:00 2001
From: Adi Vardi <adi.vardi@enway.ai>
Date: Wed, 21 Jan 2026 15:41:54 +0100
Subject: [PATCH 02/32] Use pruned path length for path occupancy check

---
 .../src/critics/path_align_critic.cpp         | 20 ++++++++++---------
 1 file changed, 11 insertions(+), 9 deletions(-)

diff --git a/nav2_mppi_controller/src/critics/path_align_critic.cpp b/nav2_mppi_controller/src/critics/path_align_critic.cpp
index 120e0c100b6..4a7f502ba7e 100644
--- a/nav2_mppi_controller/src/critics/path_align_critic.cpp
+++ b/nav2_mppi_controller/src/critics/path_align_critic.cpp
@@ -53,11 +53,16 @@ void PathAlignCritic::score(CriticData & data)
     return;
   }
 
-  // Don't apply when first getting bearing w.r.t. the path
+  // Only apply critic when trajectories reach far enough along the way path.
+  // This ensures that path alignment is only considered when actually tracking the path (e.g. not driving very slow
+  // or when first getting bearing w.r.t. the path)
   utils::setPathFurthestPointIfNotSet(data);
-  // Up to furthest only, closest path point is always 0 from path handler
-  const size_t path_segments_count = *data.furthest_reached_path_point;
-  float path_segments_flt = static_cast<float>(path_segments_count);
+  if (*data.furthest_reached_path_point < offset_from_furthest_) {
+    return;
+  }
+
+  // use the whole local path as reference path
+  const size_t path_segments_count = data.path.x.size() - 1;
 
   // Visualize target pose if enabled
   if (visualize_furthest_point_ && path_segments_count > 0 &&
@@ -75,17 +80,14 @@ void PathAlignCritic::score(CriticData & data)
     furthest_point_pub_->publish(std::move(furthest_point));
   }
 
-  if (path_segments_count < offset_from_furthest_) {
-    return;
-  }
-
   // Don't apply when dynamic obstacles are blocking significant proportions of the local path
+  const float path_segments_count_flt = static_cast<float>(path_segments_count);
   utils::setPathCostsIfNotSet(data, costmap_ros_);
   std::vector<bool> & path_pts_valid = *data.path_pts_valid;
   float invalid_ctr = 0.0f;
   for (size_t i = 0; i < path_segments_count; i++) {
     if (!path_pts_valid[i]) {invalid_ctr += 1.0f;}
-    if (invalid_ctr / path_segments_flt > max_path_occupancy_ratio_ && invalid_ctr > 2.0f) {
+    if (invalid_ctr / path_segments_count_flt > max_path_occupancy_ratio_ && invalid_ctr > 2.0f) {
       return;
     }
   }

From 6ac069935cb4ee253343f9a3755beab3bfb701af Mon Sep 17 00:00:00 2001
From: Adi Vardi <adi.vardi@enway.ai>
Date: Fri, 13 Feb 2026 16:47:16 +0100
Subject: [PATCH 03/32] [clean] use a fixed distance for occupancy check

---
 .../src/critics/path_align_critic.cpp         | 80 ++++++++++++-------
 1 file changed, 51 insertions(+), 29 deletions(-)

diff --git a/nav2_mppi_controller/src/critics/path_align_critic.cpp b/nav2_mppi_controller/src/critics/path_align_critic.cpp
index 4a7f502ba7e..a9481e1fd1c 100644
--- a/nav2_mppi_controller/src/critics/path_align_critic.cpp
+++ b/nav2_mppi_controller/src/critics/path_align_critic.cpp
@@ -41,6 +41,15 @@ void PathAlignCritic::initialize()
     }
   }
 
+  // TODO
+  // if (visualize_occupancy_check_point_) {
+  //   auto node = parent_.lock();
+  //   if (node) {
+  //     occupancy_check_point_pub_ = node->create_publisher<geometry_msgs::msg::PoseStamped>(
+  //       "PathAlignCritic/occupancy_check_point", 1);
+  //     occupancy_check_point_pub_->on_activate();
+  //   }
+  // }
   RCLCPP_INFO(
     logger_,
     "ReferenceTrajectoryCritic instantiated with %d power and %f weight",
@@ -61,56 +70,69 @@ void PathAlignCritic::score(CriticData & data)
     return;
   }
 
-  // use the whole local path as reference path
-  const size_t path_segments_count = data.path.x.size() - 1;
+  const size_t batch_size = data.trajectories.x.rows();
+  Eigen::ArrayXf cost(data.costs.rows());
+  cost.setZero();
+
+  const size_t path_size = data.path.x.size();
+
+  constexpr float occupancy_check_distance = 5.0; // TODO make into a parameter
+                                                  // TODO what if smaller than furthest_reached_path_point
+  size_t occupancy_check_distance_idx = path_size; // initialize to max, in case the whole path is within the occupancy_check_distance
+
+  // Find integrated arc-length distance along the path = total dist traveled along the path to each path point
+  // TODO path_segments_count can be reduced, but should be at least max(occupancy_check_distance_idx, furthest_reached_path_point)
+  //      but we don't have occupancy_check_distance_idx before the next loop... so maybe we can break the loop when reached
+  const size_t path_segments_count = path_size;
+  std::vector<float> path_integrated_distances(path_segments_count, 0.0f);
+  std::vector<utils::Pose2D> path(path_segments_count);
+  float dx = 0.0f, dy = 0.0f;
+  for (unsigned int i = 1; i != path_segments_count; i++) {
+    auto & pose = path[i - 1];
+    pose.x = data.path.x(i - 1);
+    pose.y = data.path.y(i - 1);
+    pose.theta = data.path.yaws(i - 1);
+
+    dx = data.path.x(i) - pose.x;
+    dy = data.path.y(i) - pose.y;
+    path_integrated_distances[i] = path_integrated_distances[i - 1] + sqrtf(dx * dx + dy * dy);
+
+    // find the first path point that is further along the path than the occupancy_check_distance
+    if (path_integrated_distances[i] > occupancy_check_distance && occupancy_check_distance_idx == path_size) {
+      occupancy_check_distance_idx = i;
+    }
+  }
 
   // Visualize target pose if enabled
-  if (visualize_furthest_point_ && path_segments_count > 0 &&
+  // TODO separate publisher for furthest_point_pub_ (published before <offset_from_furthest_ check, like upstream)
+  // TODO and a publisher for occupancy_check_distance point
+  if (visualize_furthest_point_ && occupancy_check_distance_idx > 0 &&
     furthest_point_pub_->get_subscription_count() > 0)
   {
     auto furthest_point = std::make_unique<geometry_msgs::msg::PoseStamped>();
     furthest_point->header.frame_id = costmap_ros_->getGlobalFrameID();
     furthest_point->header.stamp = clock_->now();
-    furthest_point->pose.position.x = data.path.x(path_segments_count);
-    furthest_point->pose.position.y = data.path.y(path_segments_count);
+    furthest_point->pose.position.x = data.path.x(occupancy_check_distance_idx);
+    furthest_point->pose.position.y = data.path.y(occupancy_check_distance_idx);
     furthest_point->pose.position.z = 0.0;
     tf2::Quaternion quat;
-    quat.setRPY(0.0, 0.0, data.path.yaws(path_segments_count));
+    quat.setRPY(0.0, 0.0, data.path.yaws(occupancy_check_distance_idx));
     furthest_point->pose.orientation = tf2::toMsg(quat);
     furthest_point_pub_->publish(std::move(furthest_point));
   }
 
-  // Don't apply when dynamic obstacles are blocking significant proportions of the local path
-  const float path_segments_count_flt = static_cast<float>(path_segments_count);
+  // Don't apply when dynamic obstacles are blocking significant proportions of the path up to occupancy_check_distance
+  const float occupancy_check_distance_idx_flt = static_cast<float>(occupancy_check_distance_idx);
   utils::setPathCostsIfNotSet(data, costmap_ros_);
   std::vector<bool> & path_pts_valid = *data.path_pts_valid;
   float invalid_ctr = 0.0f;
-  for (size_t i = 0; i < path_segments_count; i++) {
+  for (size_t i = 0; i < occupancy_check_distance_idx; i++) {
     if (!path_pts_valid[i]) {invalid_ctr += 1.0f;}
-    if (invalid_ctr / path_segments_count_flt > max_path_occupancy_ratio_ && invalid_ctr > 2.0f) {
+    if (invalid_ctr / occupancy_check_distance_idx_flt > max_path_occupancy_ratio_ && invalid_ctr > 2.0f) {
       return;
     }
   }
 
-  const size_t batch_size = data.trajectories.x.rows();
-  Eigen::ArrayXf cost(data.costs.rows());
-  cost.setZero();
-
-  // Find integrated arc-length distance along the path = total dist traveled along the path to each path point
-  std::vector<float> path_integrated_distances(path_segments_count, 0.0f);
-  std::vector<utils::Pose2D> path(path_segments_count);
-  float dx = 0.0f, dy = 0.0f;
-  for (unsigned int i = 1; i != path_segments_count; i++) {
-    auto & pose = path[i - 1];
-    pose.x = data.path.x(i - 1);
-    pose.y = data.path.y(i - 1);
-    pose.theta = data.path.yaws(i - 1);
-
-    dx = data.path.x(i) - pose.x;
-    dy = data.path.y(i) - pose.y;
-    path_integrated_distances[i] = path_integrated_distances[i - 1] + sqrtf(dx * dx + dy * dy);
-  }
-
   // Finish populating the path vector
   auto & final_pose = path[path_segments_count - 1];
   final_pose.x = data.path.x(path_segments_count - 1);

From 78bec1422b4152586bda0f0391409cb676fcda74 Mon Sep 17 00:00:00 2001
From: Adi Vardi <adi.vardi@enway.ai>
Date: Mon, 16 Feb 2026 08:42:23 +0100
Subject: [PATCH 04/32] parameterize & vis occupancy check distance

---
 .../critics/path_align_critic.hpp             |  4 +
 .../src/critics/path_align_critic.cpp         | 83 +++++++++++--------
 2 files changed, 54 insertions(+), 33 deletions(-)

diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_align_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_align_critic.hpp
index 6cc33a509a5..727ad269cbb 100644
--- a/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_align_critic.hpp
+++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_align_critic.hpp
@@ -51,6 +51,7 @@ class PathAlignCritic : public CriticFunction
   size_t offset_from_furthest_{0};
   int trajectory_point_step_{0};
   float threshold_to_consider_{0};
+  float occupancy_check_min_distance_{0};
   float max_path_occupancy_ratio_{0};
   bool use_path_orientations_{false};
   unsigned int power_{0};
@@ -58,6 +59,9 @@ class PathAlignCritic : public CriticFunction
 
   bool visualize_furthest_point_{false};
   nav2::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr furthest_point_pub_;
+
+  bool visualize_occupancy_check_distance_{false};
+  nav2::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr occupancy_check_dist_pub_;
 };
 
 }  // namespace mppi::critics
diff --git a/nav2_mppi_controller/src/critics/path_align_critic.cpp b/nav2_mppi_controller/src/critics/path_align_critic.cpp
index a9481e1fd1c..42f0a1c5938 100644
--- a/nav2_mppi_controller/src/critics/path_align_critic.cpp
+++ b/nav2_mppi_controller/src/critics/path_align_critic.cpp
@@ -23,6 +23,7 @@ void PathAlignCritic::initialize()
   auto getParam = parameters_handler_->getParamGetter(name_);
   getParam(power_, "cost_power", 1);
   getParam(weight_, "cost_weight", 10.0f);
+  getParam(occupancy_check_min_distance_, "occupancy_check_min_distance", 2.0f);
   getParam(max_path_occupancy_ratio_, "max_path_occupancy_ratio", 0.07f);
   getParam(offset_from_furthest_, "offset_from_furthest", 20);
   getParam(trajectory_point_step_, "trajectory_point_step", 4);
@@ -41,15 +42,17 @@ void PathAlignCritic::initialize()
     }
   }
 
-  // TODO
-  // if (visualize_occupancy_check_point_) {
-  //   auto node = parent_.lock();
-  //   if (node) {
-  //     occupancy_check_point_pub_ = node->create_publisher<geometry_msgs::msg::PoseStamped>(
-  //       "PathAlignCritic/occupancy_check_point", 1);
-  //     occupancy_check_point_pub_->on_activate();
-  //   }
-  // }
+  getParam(visualize_occupancy_check_distance_, "visualize_occupancy_check_distance", false);
+
+  if (visualize_occupancy_check_distance_) {
+    auto node = parent_.lock();
+    if (node) {
+      occupancy_check_dist_pub_ = node->create_publisher<geometry_msgs::msg::PoseStamped>(
+        "PathAlignCritic/occupancy_check_end_point", 1);
+      occupancy_check_dist_pub_->on_activate();
+    }
+  }
+
   RCLCPP_INFO(
     logger_,
     "ReferenceTrajectoryCritic instantiated with %d power and %f weight",
@@ -66,6 +69,24 @@ void PathAlignCritic::score(CriticData & data)
   // This ensures that path alignment is only considered when actually tracking the path (e.g. not driving very slow
   // or when first getting bearing w.r.t. the path)
   utils::setPathFurthestPointIfNotSet(data);
+
+  const auto now = clock_->now();
+  // Visualize furthest reached pose if enabled
+  if (visualize_furthest_point_ && *data.furthest_reached_path_point > 0 &&
+    furthest_point_pub_->get_subscription_count() > 0)
+  {
+    auto furthest_point = std::make_unique<geometry_msgs::msg::PoseStamped>();
+    furthest_point->header.frame_id = costmap_ros_->getGlobalFrameID();
+    furthest_point->header.stamp = now;
+    furthest_point->pose.position.x = data.path.x(*data.furthest_reached_path_point);
+    furthest_point->pose.position.y = data.path.y(*data.furthest_reached_path_point);
+    furthest_point->pose.position.z = 0.0;
+    tf2::Quaternion quat;
+    quat.setRPY(0.0, 0.0, data.path.yaws(*data.furthest_reached_path_point));
+    furthest_point->pose.orientation = tf2::toMsg(quat);
+    furthest_point_pub_->publish(std::move(furthest_point));
+  }
+
   if (*data.furthest_reached_path_point < offset_from_furthest_) {
     return;
   }
@@ -74,16 +95,13 @@ void PathAlignCritic::score(CriticData & data)
   Eigen::ArrayXf cost(data.costs.rows());
   cost.setZero();
 
-  const size_t path_size = data.path.x.size();
-
-  constexpr float occupancy_check_distance = 5.0; // TODO make into a parameter
-                                                  // TODO what if smaller than furthest_reached_path_point
-  size_t occupancy_check_distance_idx = path_size; // initialize to max, in case the whole path is within the occupancy_check_distance
-
   // Find integrated arc-length distance along the path = total dist traveled along the path to each path point
-  // TODO path_segments_count can be reduced, but should be at least max(occupancy_check_distance_idx, furthest_reached_path_point)
+  // TODO the integration doesn't have to be until the end of the path but should be at least
+  //      max(occupancy_check_distance_idx, furthest_reached_path_point)
   //      but we don't have occupancy_check_distance_idx before the next loop... so maybe we can break the loop when reached
-  const size_t path_segments_count = path_size;
+  const size_t path_segments_count = data.path.x.size() - 1;
+  // initialize the occupancy check id to max, in case the entire path is within the distance
+  size_t occupancy_check_distance_idx = path_segments_count;
   std::vector<float> path_integrated_distances(path_segments_count, 0.0f);
   std::vector<utils::Pose2D> path(path_segments_count);
   float dx = 0.0f, dy = 0.0f;
@@ -97,31 +115,30 @@ void PathAlignCritic::score(CriticData & data)
     dy = data.path.y(i) - pose.y;
     path_integrated_distances[i] = path_integrated_distances[i - 1] + sqrtf(dx * dx + dy * dy);
 
-    // find the first path point that is further along the path than the occupancy_check_distance
-    if (path_integrated_distances[i] > occupancy_check_distance && occupancy_check_distance_idx == path_size) {
+    // find the first path point that is further along the path than the occupancy_check_min_distance_
+    // TODO what if smaller than furthest_reached_path_point
+    if (path_integrated_distances[i] > occupancy_check_min_distance_ && occupancy_check_distance_idx == path_segments_count) {
       occupancy_check_distance_idx = i;
     }
   }
 
-  // Visualize target pose if enabled
-  // TODO separate publisher for furthest_point_pub_ (published before <offset_from_furthest_ check, like upstream)
-  // TODO and a publisher for occupancy_check_distance point
-  if (visualize_furthest_point_ && occupancy_check_distance_idx > 0 &&
-    furthest_point_pub_->get_subscription_count() > 0)
+  // Visualize occupancy check distance if enabled
+  if (visualize_occupancy_check_distance_ &&
+    occupancy_check_dist_pub_->get_subscription_count() > 0)
   {
-    auto furthest_point = std::make_unique<geometry_msgs::msg::PoseStamped>();
-    furthest_point->header.frame_id = costmap_ros_->getGlobalFrameID();
-    furthest_point->header.stamp = clock_->now();
-    furthest_point->pose.position.x = data.path.x(occupancy_check_distance_idx);
-    furthest_point->pose.position.y = data.path.y(occupancy_check_distance_idx);
-    furthest_point->pose.position.z = 0.0;
+    auto occupancy_check_point = std::make_unique<geometry_msgs::msg::PoseStamped>();
+    occupancy_check_point->header.frame_id = costmap_ros_->getGlobalFrameID();
+    occupancy_check_point->header.stamp = now;
+    occupancy_check_point->pose.position.x = data.path.x(occupancy_check_distance_idx);
+    occupancy_check_point->pose.position.y = data.path.y(occupancy_check_distance_idx);
+    occupancy_check_point->pose.position.z = 0.0;
     tf2::Quaternion quat;
     quat.setRPY(0.0, 0.0, data.path.yaws(occupancy_check_distance_idx));
-    furthest_point->pose.orientation = tf2::toMsg(quat);
-    furthest_point_pub_->publish(std::move(furthest_point));
+    occupancy_check_point->pose.orientation = tf2::toMsg(quat);
+    occupancy_check_dist_pub_->publish(std::move(occupancy_check_point));
   }
 
-  // Don't apply when dynamic obstacles are blocking significant proportions of the path up to occupancy_check_distance
+  // Don't apply when dynamic obstacles are blocking significant proportions of the path up to occupancy_check_min_distance_
   const float occupancy_check_distance_idx_flt = static_cast<float>(occupancy_check_distance_idx);
   utils::setPathCostsIfNotSet(data, costmap_ros_);
   std::vector<bool> & path_pts_valid = *data.path_pts_valid;

From 9d3f149e7daa4bf48ed79489f737d29200853ff4 Mon Sep 17 00:00:00 2001
From: Adi Vardi <adi.vardi@enway.ai>
Date: Mon, 16 Feb 2026 10:58:19 +0100
Subject: [PATCH 05/32] Use max(distance, furthest_reached) for occupancy check

---
 nav2_mppi_controller/src/critics/path_align_critic.cpp | 10 ++++------
 1 file changed, 4 insertions(+), 6 deletions(-)

diff --git a/nav2_mppi_controller/src/critics/path_align_critic.cpp b/nav2_mppi_controller/src/critics/path_align_critic.cpp
index 42f0a1c5938..4068be5da1f 100644
--- a/nav2_mppi_controller/src/critics/path_align_critic.cpp
+++ b/nav2_mppi_controller/src/critics/path_align_critic.cpp
@@ -96,9 +96,6 @@ void PathAlignCritic::score(CriticData & data)
   cost.setZero();
 
   // Find integrated arc-length distance along the path = total dist traveled along the path to each path point
-  // TODO the integration doesn't have to be until the end of the path but should be at least
-  //      max(occupancy_check_distance_idx, furthest_reached_path_point)
-  //      but we don't have occupancy_check_distance_idx before the next loop... so maybe we can break the loop when reached
   const size_t path_segments_count = data.path.x.size() - 1;
   // initialize the occupancy check id to max, in case the entire path is within the distance
   size_t occupancy_check_distance_idx = path_segments_count;
@@ -115,9 +112,10 @@ void PathAlignCritic::score(CriticData & data)
     dy = data.path.y(i) - pose.y;
     path_integrated_distances[i] = path_integrated_distances[i - 1] + sqrtf(dx * dx + dy * dy);
 
-    // find the first path point that is further along the path than the occupancy_check_min_distance_
-    // TODO what if smaller than furthest_reached_path_point
-    if (path_integrated_distances[i] > occupancy_check_min_distance_ && occupancy_check_distance_idx == path_segments_count) {
+    // find the first path point that is further along the path than the occupancy_check_min_distance_ and
+    if (occupancy_check_distance_idx == path_segments_count &&
+        path_integrated_distances[i] > occupancy_check_min_distance_ &&
+        i >= *data.furthest_reached_path_point) {
       occupancy_check_distance_idx = i;
     }
   }

From 530223476557e3987c9529fe63b8f8db14ebe2ee Mon Sep 17 00:00:00 2001
From: Adi Vardi <adi.vardi@enway.ai>
Date: Wed, 21 Jan 2026 16:12:33 +0100
Subject: [PATCH 06/32] First version of direction change critic

---
 nav2_mppi_controller/CMakeLists.txt           |  1 +
 nav2_mppi_controller/critics.xml              |  4 ++
 .../critics/direction_change_critic.hpp       | 52 ++++++++++++++
 .../src/critics/direction_change_critic.cpp   | 70 +++++++++++++++++++
 4 files changed, 127 insertions(+)
 create mode 100644 nav2_mppi_controller/include/nav2_mppi_controller/critics/direction_change_critic.hpp
 create mode 100644 nav2_mppi_controller/src/critics/direction_change_critic.cpp

diff --git a/nav2_mppi_controller/CMakeLists.txt b/nav2_mppi_controller/CMakeLists.txt
index b1babe00a41..81f445c6cf1 100644
--- a/nav2_mppi_controller/CMakeLists.txt
+++ b/nav2_mppi_controller/CMakeLists.txt
@@ -83,6 +83,7 @@ add_library(mppi_critics SHARED
   src/critics/prefer_forward_critic.cpp
   src/critics/twirling_critic.cpp
   src/critics/velocity_deadband_critic.cpp
+  src/critics/direction_change_critic.cpp
 )
 if(CMAKE_CXX_COMPILER_ID STREQUAL "Clang" AND APPLE)
     # Apple Clang: use C++20 and optimization, omit -fconcepts
diff --git a/nav2_mppi_controller/critics.xml b/nav2_mppi_controller/critics.xml
index deae2c11cb9..bba4f9d7375 100644
--- a/nav2_mppi_controller/critics.xml
+++ b/nav2_mppi_controller/critics.xml
@@ -45,5 +45,9 @@
       <description>mppi critic for restricting command velocities in deadband range</description>
     </class>
 
+    <class type="mppi::critics::DirectionChangeCritic" base_class_type="mppi::critics::CriticFunction">
+      <description>mppi critic for penalizing changes in driving direction</description>
+    </class>
+
   </library>
 </class_libraries>
diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/direction_change_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/direction_change_critic.hpp
new file mode 100644
index 00000000000..caf77dcc6da
--- /dev/null
+++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/direction_change_critic.hpp
@@ -0,0 +1,52 @@
+// Copyright (c) 2024 Enway GmbH, Adi Vardi
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef NAV2_MPPI_CONTROLLER__CRITICS__DIRECTION_CHANGE_CRITIC_HPP_
+#define NAV2_MPPI_CONTROLLER__CRITICS__DIRECTION_CHANGE_CRITIC_HPP_
+
+#include "nav2_mppi_controller/critic_function.hpp"
+#include "nav2_mppi_controller/tools/utils.hpp"
+
+namespace mppi::critics
+{
+
+/**
+ * @class mppi::critics::DirectionChangeCritic
+ * @brief Critic objective function for penalizing changes in driving direction.
+ */
+class DirectionChangeCritic : public CriticFunction
+{
+public:
+  /**
+    * @brief Initialize critic
+    */
+  void initialize() override;
+
+  /**
+   * @brief Evaluate cost related to changing driving direction
+   *
+   * @param costs [out] add goal angle cost values to this tensor
+   */
+  void score(CriticData & data) override;
+
+protected:
+  unsigned int power_{0};
+  float weight_{0};
+  float threshold_to_consider_{0};
+  bool enforce_path_inversion_{false};
+};
+
+}  // namespace mppi::critics
+
+#endif  // NAV2_MPPI_CONTROLLER__CRITICS__DIRECTION_CHANGE_CRITIC_HPP_
diff --git a/nav2_mppi_controller/src/critics/direction_change_critic.cpp b/nav2_mppi_controller/src/critics/direction_change_critic.cpp
new file mode 100644
index 00000000000..67d05b53618
--- /dev/null
+++ b/nav2_mppi_controller/src/critics/direction_change_critic.cpp
@@ -0,0 +1,70 @@
+// Copyright (c) 2024 Enway GmbH, Adi Vardi
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "nav2_mppi_controller/critics/direction_change_critic.hpp"
+
+#include <Eigen/Dense>
+
+namespace mppi::critics
+{
+
+void DirectionChangeCritic::initialize()
+{
+  auto getParentParam = parameters_handler_->getParamGetter(parent_name_);
+  getParentParam(enforce_path_inversion_, "enforce_path_inversion", false);
+
+  auto getParam = parameters_handler_->getParamGetter(name_);
+  getParam(power_, "cost_power", 1);
+  getParam(weight_, "cost_weight", 5.0f);
+  getParam(
+    threshold_to_consider_,
+    "threshold_to_consider", 0.5f);
+
+  RCLCPP_INFO(
+    logger_, "DirectionChangeCritic instantiated with %d power and %f weight.", power_, weight_);
+}
+
+void DirectionChangeCritic::score(CriticData & data)
+{
+  if (!enabled_) {
+    return;
+  }
+
+  geometry_msgs::msg::Pose goal = utils::getCriticGoal(data, enforce_path_inversion_);
+
+  if (utils::withinPositionGoalTolerance(
+      threshold_to_consider_, data.state.pose.pose, goal))
+  {
+    return;
+  }
+
+  constexpr size_t penalize_up_to_idx = 2;
+  if (power_ > 1u) {
+    data.costs += (
+      // only penalize the first penalize_up_to_idx elements
+      (data.state.vx.leftCols(penalize_up_to_idx).unaryExpr([&](const float & x){return std::sign(x *
+        data.state.speed.linear.x);})).rowwise().sum() * weight_).pow(power_);
+  } else {
+    data.costs += (data.state.vx.leftCols(penalize_up_to_idx).unaryExpr([&](const float & x){
+      return std::sign(x * data.state.speed.linear.x);})).rowwise().sum() * weight_;
+  }
+}
+
+}  // namespace mppi::critics
+
+#include <pluginlib/class_list_macros.hpp>
+
+PLUGINLIB_EXPORT_CLASS(
+  mppi::critics::DirectionChangeCritic,
+  mppi::critics::CriticFunction)

From fe6468b66f9131b63dd755b5d686885e84f3d983 Mon Sep 17 00:00:00 2001
From: Adi Vardi <adi.vardi@enway.ai>
Date: Wed, 21 Jan 2026 16:41:15 +0100
Subject: [PATCH 07/32] Refactor direction change critic

---
 .../src/critics/direction_change_critic.cpp    | 18 ++++++++++++------
 1 file changed, 12 insertions(+), 6 deletions(-)

diff --git a/nav2_mppi_controller/src/critics/direction_change_critic.cpp b/nav2_mppi_controller/src/critics/direction_change_critic.cpp
index 67d05b53618..f10e6d29cd0 100644
--- a/nav2_mppi_controller/src/critics/direction_change_critic.cpp
+++ b/nav2_mppi_controller/src/critics/direction_change_critic.cpp
@@ -49,15 +49,21 @@ void DirectionChangeCritic::score(CriticData & data)
     return;
   }
 
+  // Penalize the magnitude of velocity difference when crossing zero (direction change)
+  // Calculate |vx - current_speed| only where signs differ, otherwise 0
+
   constexpr size_t penalize_up_to_idx = 2;
+  const float current_speed = data.state.speed.linear.x;
+  // Process in-place using Eigen views to avoid allocations
+  auto vx_view = data.state.vx.leftCols(penalize_up_to_idx);
+
+  // TODO also penalize change direction in wz (and vy for holonomic case) . maybe add a flag to enable/disable wz
   if (power_ > 1u) {
-    data.costs += (
-      // only penalize the first penalize_up_to_idx elements
-      (data.state.vx.leftCols(penalize_up_to_idx).unaryExpr([&](const float & x){return std::sign(x *
-        data.state.speed.linear.x);})).rowwise().sum() * weight_).pow(power_);
+    data.costs += ((vx_view * current_speed < 0.0f).select(
+      (vx_view - current_speed).abs(), 0.0f).rowwise().sum() * weight_).pow(power_);
   } else {
-    data.costs += (data.state.vx.leftCols(penalize_up_to_idx).unaryExpr([&](const float & x){
-      return std::sign(x * data.state.speed.linear.x);})).rowwise().sum() * weight_;
+    data.costs += (vx_view * current_speed < 0.0f).select(
+      (vx_view - current_speed).abs(), 0.0f).rowwise().sum() * weight_;
   }
 }
 

From 680eed1a37b2320685a93ea3de0ab5445c0dbf3c Mon Sep 17 00:00:00 2001
From: Adi Vardi <adi.vardi@enway.ai>
Date: Mon, 16 Feb 2026 15:02:17 +0100
Subject: [PATCH 08/32] Update DirectionChangeCritic after rebase

---
 .../critics/direction_change_critic.hpp                   | 3 +--
 .../src/critics/direction_change_critic.cpp               | 8 +-------
 2 files changed, 2 insertions(+), 9 deletions(-)

diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/direction_change_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/direction_change_critic.hpp
index caf77dcc6da..c823c564226 100644
--- a/nav2_mppi_controller/include/nav2_mppi_controller/critics/direction_change_critic.hpp
+++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/direction_change_critic.hpp
@@ -36,7 +36,7 @@ class DirectionChangeCritic : public CriticFunction
   /**
    * @brief Evaluate cost related to changing driving direction
    *
-   * @param costs [out] add goal angle cost values to this tensor
+   * @param costs [out] add cost values to this tensor
    */
   void score(CriticData & data) override;
 
@@ -44,7 +44,6 @@ class DirectionChangeCritic : public CriticFunction
   unsigned int power_{0};
   float weight_{0};
   float threshold_to_consider_{0};
-  bool enforce_path_inversion_{false};
 };
 
 }  // namespace mppi::critics
diff --git a/nav2_mppi_controller/src/critics/direction_change_critic.cpp b/nav2_mppi_controller/src/critics/direction_change_critic.cpp
index f10e6d29cd0..839d1b1d141 100644
--- a/nav2_mppi_controller/src/critics/direction_change_critic.cpp
+++ b/nav2_mppi_controller/src/critics/direction_change_critic.cpp
@@ -22,8 +22,6 @@ namespace mppi::critics
 void DirectionChangeCritic::initialize()
 {
   auto getParentParam = parameters_handler_->getParamGetter(parent_name_);
-  getParentParam(enforce_path_inversion_, "enforce_path_inversion", false);
-
   auto getParam = parameters_handler_->getParamGetter(name_);
   getParam(power_, "cost_power", 1);
   getParam(weight_, "cost_weight", 5.0f);
@@ -41,11 +39,7 @@ void DirectionChangeCritic::score(CriticData & data)
     return;
   }
 
-  geometry_msgs::msg::Pose goal = utils::getCriticGoal(data, enforce_path_inversion_);
-
-  if (utils::withinPositionGoalTolerance(
-      threshold_to_consider_, data.state.pose.pose, goal))
-  {
+  if (data.state.local_path_length < threshold_to_consider_) {
     return;
   }
 

From 3daf03540c17571416a34fc8ee13cebf3d94d2bf Mon Sep 17 00:00:00 2001
From: Adi Vardi <adi.vardi@enway.ai>
Date: Mon, 16 Feb 2026 16:04:37 +0100
Subject: [PATCH 09/32] DirectionChangeCritic: test use robot speed or
 last_cmd_published

---
 .../include/nav2_mppi_controller/models/state.hpp            | 1 +
 nav2_mppi_controller/src/critics/direction_change_critic.cpp | 1 +
 nav2_mppi_controller/src/optimizer.cpp                       | 5 +++++
 3 files changed, 7 insertions(+)

diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/models/state.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/models/state.hpp
index 50f9d400a91..bcbe12f7ba7 100644
--- a/nav2_mppi_controller/include/nav2_mppi_controller/models/state.hpp
+++ b/nav2_mppi_controller/include/nav2_mppi_controller/models/state.hpp
@@ -40,6 +40,7 @@ struct State
 
   geometry_msgs::msg::PoseStamped pose;
   geometry_msgs::msg::Twist speed;
+  geometry_msgs::msg::Twist robot_speed;
   float local_path_length;
 
   /**
diff --git a/nav2_mppi_controller/src/critics/direction_change_critic.cpp b/nav2_mppi_controller/src/critics/direction_change_critic.cpp
index 839d1b1d141..2b6abe5568d 100644
--- a/nav2_mppi_controller/src/critics/direction_change_critic.cpp
+++ b/nav2_mppi_controller/src/critics/direction_change_critic.cpp
@@ -48,6 +48,7 @@ void DirectionChangeCritic::score(CriticData & data)
 
   constexpr size_t penalize_up_to_idx = 2;
   const float current_speed = data.state.speed.linear.x;
+  std::cout << "critic speed: " << current_speed << std::endl;
   // Process in-place using Eigen views to avoid allocations
   auto vx_view = data.state.vx.leftCols(penalize_up_to_idx);
 
diff --git a/nav2_mppi_controller/src/optimizer.cpp b/nav2_mppi_controller/src/optimizer.cpp
index b4e13147bbb..3f96c6bba5a 100644
--- a/nav2_mppi_controller/src/optimizer.cpp
+++ b/nav2_mppi_controller/src/optimizer.cpp
@@ -297,6 +297,11 @@ void Optimizer::prepare(
 {
   state_.pose = robot_pose;
   state_.speed = settings_.open_loop ? last_command_vel_ : robot_speed;
+  state_.robot_speed = robot_speed;
+  std::cout << "last_command_vel_.linear.x: " << last_command_vel_.linear.x << std::endl;
+  std::cout << "robot_speed.linear.x: " << robot_speed.linear.x << std::endl;
+  std::cout << "state_.speed.linear.x: " << state_.speed.linear.x << std::endl;
+
   state_.local_path_length = nav2_util::geometry_utils::calculate_path_length(plan);
   path_ = utils::toTensor(plan);
   costs_.setZero(settings_.batch_size);

From f564a5c7dc076a8849df5a7eefe23069384fd54d Mon Sep 17 00:00:00 2001
From: Adi Vardi <adi.vardi@enway.ai>
Date: Tue, 17 Feb 2026 10:29:53 +0100
Subject: [PATCH 10/32] DirectionChangeCritic: use robot speed from feedback

Use robot_speed from feedback, as it better represents the actual direction of motion.
This results in more consistent critic application and longer motions in each direction
---
 .../include/nav2_mppi_controller/models/state.hpp            | 4 ++--
 nav2_mppi_controller/src/critics/direction_change_critic.cpp | 5 ++---
 nav2_mppi_controller/src/optimizer.cpp                       | 3 ---
 3 files changed, 4 insertions(+), 8 deletions(-)

diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/models/state.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/models/state.hpp
index bcbe12f7ba7..af2d2cb84bb 100644
--- a/nav2_mppi_controller/include/nav2_mppi_controller/models/state.hpp
+++ b/nav2_mppi_controller/include/nav2_mppi_controller/models/state.hpp
@@ -39,8 +39,8 @@ struct State
   Eigen::ArrayXXf cwz;
 
   geometry_msgs::msg::PoseStamped pose;
-  geometry_msgs::msg::Twist speed;
-  geometry_msgs::msg::Twist robot_speed;
+  geometry_msgs::msg::Twist speed;  // current speed or last command published, depends on open_loop setting
+  geometry_msgs::msg::Twist robot_speed;  // current speed from odometry
   float local_path_length;
 
   /**
diff --git a/nav2_mppi_controller/src/critics/direction_change_critic.cpp b/nav2_mppi_controller/src/critics/direction_change_critic.cpp
index 2b6abe5568d..52b0a4b744f 100644
--- a/nav2_mppi_controller/src/critics/direction_change_critic.cpp
+++ b/nav2_mppi_controller/src/critics/direction_change_critic.cpp
@@ -45,10 +45,9 @@ void DirectionChangeCritic::score(CriticData & data)
 
   // Penalize the magnitude of velocity difference when crossing zero (direction change)
   // Calculate |vx - current_speed| only where signs differ, otherwise 0
-
+  // Use robot_speed from feedback, as it better represents the actual direction of motion
   constexpr size_t penalize_up_to_idx = 2;
-  const float current_speed = data.state.speed.linear.x;
-  std::cout << "critic speed: " << current_speed << std::endl;
+  const float current_speed = data.state.robot_speed.linear.x;
   // Process in-place using Eigen views to avoid allocations
   auto vx_view = data.state.vx.leftCols(penalize_up_to_idx);
 
diff --git a/nav2_mppi_controller/src/optimizer.cpp b/nav2_mppi_controller/src/optimizer.cpp
index 3f96c6bba5a..c5e1402b45b 100644
--- a/nav2_mppi_controller/src/optimizer.cpp
+++ b/nav2_mppi_controller/src/optimizer.cpp
@@ -298,9 +298,6 @@ void Optimizer::prepare(
   state_.pose = robot_pose;
   state_.speed = settings_.open_loop ? last_command_vel_ : robot_speed;
   state_.robot_speed = robot_speed;
-  std::cout << "last_command_vel_.linear.x: " << last_command_vel_.linear.x << std::endl;
-  std::cout << "robot_speed.linear.x: " << robot_speed.linear.x << std::endl;
-  std::cout << "state_.speed.linear.x: " << state_.speed.linear.x << std::endl;
 
   state_.local_path_length = nav2_util::geometry_utils::calculate_path_length(plan);
   path_ = utils::toTensor(plan);

From 8f875aa3885a8910c50b9a95f999d7fef845e056 Mon Sep 17 00:00:00 2001
From: Adi Vardi <adi.vardi@enway.ai>
Date: Tue, 17 Feb 2026 10:42:26 +0100
Subject: [PATCH 11/32] DirectionChangeCritic: rm todo to penalize wz

Not sure if useful
---
 nav2_mppi_controller/src/critics/direction_change_critic.cpp | 1 -
 1 file changed, 1 deletion(-)

diff --git a/nav2_mppi_controller/src/critics/direction_change_critic.cpp b/nav2_mppi_controller/src/critics/direction_change_critic.cpp
index 52b0a4b744f..e51d9f9b3c8 100644
--- a/nav2_mppi_controller/src/critics/direction_change_critic.cpp
+++ b/nav2_mppi_controller/src/critics/direction_change_critic.cpp
@@ -51,7 +51,6 @@ void DirectionChangeCritic::score(CriticData & data)
   // Process in-place using Eigen views to avoid allocations
   auto vx_view = data.state.vx.leftCols(penalize_up_to_idx);
 
-  // TODO also penalize change direction in wz (and vy for holonomic case) . maybe add a flag to enable/disable wz
   if (power_ > 1u) {
     data.costs += ((vx_view * current_speed < 0.0f).select(
       (vx_view - current_speed).abs(), 0.0f).rowwise().sum() * weight_).pow(power_);

From 6659ba87de1a76be0109b76aaf3099bf0e73b89b Mon Sep 17 00:00:00 2001
From: Adi Vardi <adi.vardi@enway.ai>
Date: Wed, 18 Mar 2026 10:13:26 +0100
Subject: [PATCH 12/32] fix comment

---
 nav2_mppi_controller/src/critics/path_align_critic.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/nav2_mppi_controller/src/critics/path_align_critic.cpp b/nav2_mppi_controller/src/critics/path_align_critic.cpp
index 4068be5da1f..ec3bfeafd78 100644
--- a/nav2_mppi_controller/src/critics/path_align_critic.cpp
+++ b/nav2_mppi_controller/src/critics/path_align_critic.cpp
@@ -112,7 +112,7 @@ void PathAlignCritic::score(CriticData & data)
     dy = data.path.y(i) - pose.y;
     path_integrated_distances[i] = path_integrated_distances[i - 1] + sqrtf(dx * dx + dy * dy);
 
-    // find the first path point that is further along the path than the occupancy_check_min_distance_ and
+    // find the first path point that is further than  max(occupancy_check_min_distance_, furthest_reached_path_point)
     if (occupancy_check_distance_idx == path_segments_count &&
         path_integrated_distances[i] > occupancy_check_min_distance_ &&
         i >= *data.furthest_reached_path_point) {

From 809d679952e730c72ec7475062404177b5560572 Mon Sep 17 00:00:00 2001
From: Adi Vardi <adi.vardi@enway.ai>
Date: Fri, 20 Mar 2026 14:54:03 +0100
Subject: [PATCH 13/32] break long lines

---
 .../nav2_mppi_controller/models/state.hpp       |  3 ++-
 .../src/critics/path_align_critic.cpp           | 17 +++++++++++------
 2 files changed, 13 insertions(+), 7 deletions(-)

diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/models/state.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/models/state.hpp
index af2d2cb84bb..fb8b10d808e 100644
--- a/nav2_mppi_controller/include/nav2_mppi_controller/models/state.hpp
+++ b/nav2_mppi_controller/include/nav2_mppi_controller/models/state.hpp
@@ -39,7 +39,8 @@ struct State
   Eigen::ArrayXXf cwz;
 
   geometry_msgs::msg::PoseStamped pose;
-  geometry_msgs::msg::Twist speed;  // current speed or last command published, depends on open_loop setting
+  // current speed or last command published, depends on open_loop setting
+  geometry_msgs::msg::Twist speed;
   geometry_msgs::msg::Twist robot_speed;  // current speed from odometry
   float local_path_length;
 
diff --git a/nav2_mppi_controller/src/critics/path_align_critic.cpp b/nav2_mppi_controller/src/critics/path_align_critic.cpp
index ec3bfeafd78..32eaf0967e1 100644
--- a/nav2_mppi_controller/src/critics/path_align_critic.cpp
+++ b/nav2_mppi_controller/src/critics/path_align_critic.cpp
@@ -66,8 +66,8 @@ void PathAlignCritic::score(CriticData & data)
   }
 
   // Only apply critic when trajectories reach far enough along the way path.
-  // This ensures that path alignment is only considered when actually tracking the path (e.g. not driving very slow
-  // or when first getting bearing w.r.t. the path)
+  // This ensures that path alignment is only considered when actually tracking the path
+  // (e.g. not driving very slow or when first getting bearing w.r.t. the path)
   utils::setPathFurthestPointIfNotSet(data);
 
   const auto now = clock_->now();
@@ -95,7 +95,9 @@ void PathAlignCritic::score(CriticData & data)
   Eigen::ArrayXf cost(data.costs.rows());
   cost.setZero();
 
-  // Find integrated arc-length distance along the path = total dist traveled along the path to each path point
+  // Find integrated arc-length distance along the path = total dist traveled along the path to each
+  // path point loop until end of path, to guarantee don't truncate long trajectories when furthest
+  // reached path is small (e.g. when all traj curve away from the path)
   const size_t path_segments_count = data.path.x.size() - 1;
   // initialize the occupancy check id to max, in case the entire path is within the distance
   size_t occupancy_check_distance_idx = path_segments_count;
@@ -112,7 +114,8 @@ void PathAlignCritic::score(CriticData & data)
     dy = data.path.y(i) - pose.y;
     path_integrated_distances[i] = path_integrated_distances[i - 1] + sqrtf(dx * dx + dy * dy);
 
-    // find the first path point that is further than  max(occupancy_check_min_distance_, furthest_reached_path_point)
+    // find the first path point that is further than
+    //  max(occupancy_check_min_distance_, furthest_reached_path_point)
     if (occupancy_check_distance_idx == path_segments_count &&
         path_integrated_distances[i] > occupancy_check_min_distance_ &&
         i >= *data.furthest_reached_path_point) {
@@ -136,14 +139,16 @@ void PathAlignCritic::score(CriticData & data)
     occupancy_check_dist_pub_->publish(std::move(occupancy_check_point));
   }
 
-  // Don't apply when dynamic obstacles are blocking significant proportions of the path up to occupancy_check_min_distance_
+  // Don't apply when dynamic obstacles are blocking significant proportions of the path
+  // up to occupancy_check_min_distance_
   const float occupancy_check_distance_idx_flt = static_cast<float>(occupancy_check_distance_idx);
   utils::setPathCostsIfNotSet(data, costmap_ros_);
   std::vector<bool> & path_pts_valid = *data.path_pts_valid;
   float invalid_ctr = 0.0f;
   for (size_t i = 0; i < occupancy_check_distance_idx; i++) {
     if (!path_pts_valid[i]) {invalid_ctr += 1.0f;}
-    if (invalid_ctr / occupancy_check_distance_idx_flt > max_path_occupancy_ratio_ && invalid_ctr > 2.0f) {
+    if (invalid_ctr / occupancy_check_distance_idx_flt > max_path_occupancy_ratio_ &&
+        invalid_ctr > 2.0f) {
       return;
     }
   }

From afd3190e65abb9915ead489ccf82f1190e2900e0 Mon Sep 17 00:00:00 2001
From: Adi Vardi <adi.vardi@enway.ai>
Date: Fri, 20 Mar 2026 15:00:51 +0100
Subject: [PATCH 14/32] fix line indentations

---
 .../src/critics/direction_change_critic.cpp               | 2 +-
 nav2_mppi_controller/src/critics/path_align_critic.cpp    | 8 +++++---
 2 files changed, 6 insertions(+), 4 deletions(-)

diff --git a/nav2_mppi_controller/src/critics/direction_change_critic.cpp b/nav2_mppi_controller/src/critics/direction_change_critic.cpp
index e51d9f9b3c8..f7d5f55ddee 100644
--- a/nav2_mppi_controller/src/critics/direction_change_critic.cpp
+++ b/nav2_mppi_controller/src/critics/direction_change_critic.cpp
@@ -53,7 +53,7 @@ void DirectionChangeCritic::score(CriticData & data)
 
   if (power_ > 1u) {
     data.costs += ((vx_view * current_speed < 0.0f).select(
-      (vx_view - current_speed).abs(), 0.0f).rowwise().sum() * weight_).pow(power_);
+        (vx_view - current_speed).abs(), 0.0f).rowwise().sum() * weight_).pow(power_);
   } else {
     data.costs += (vx_view * current_speed < 0.0f).select(
       (vx_view - current_speed).abs(), 0.0f).rowwise().sum() * weight_;
diff --git a/nav2_mppi_controller/src/critics/path_align_critic.cpp b/nav2_mppi_controller/src/critics/path_align_critic.cpp
index 32eaf0967e1..60b8ac9e475 100644
--- a/nav2_mppi_controller/src/critics/path_align_critic.cpp
+++ b/nav2_mppi_controller/src/critics/path_align_critic.cpp
@@ -117,8 +117,9 @@ void PathAlignCritic::score(CriticData & data)
     // find the first path point that is further than
     //  max(occupancy_check_min_distance_, furthest_reached_path_point)
     if (occupancy_check_distance_idx == path_segments_count &&
-        path_integrated_distances[i] > occupancy_check_min_distance_ &&
-        i >= *data.furthest_reached_path_point) {
+      path_integrated_distances[i] > occupancy_check_min_distance_ &&
+      i >= *data.furthest_reached_path_point)
+    {
       occupancy_check_distance_idx = i;
     }
   }
@@ -148,7 +149,8 @@ void PathAlignCritic::score(CriticData & data)
   for (size_t i = 0; i < occupancy_check_distance_idx; i++) {
     if (!path_pts_valid[i]) {invalid_ctr += 1.0f;}
     if (invalid_ctr / occupancy_check_distance_idx_flt > max_path_occupancy_ratio_ &&
-        invalid_ctr > 2.0f) {
+      invalid_ctr > 2.0f)
+    {
       return;
     }
   }

From 240676c0a03657c3cebfa4f24509bf6b2bd98aaa Mon Sep 17 00:00:00 2001
From: Adi Vardi <adi.vardi@enway.ai>
Date: Wed, 18 Feb 2026 13:38:44 +0100
Subject: [PATCH 15/32] Add comments to PathAlign

After looking into adding break early when computing the path's arc-length
---
 nav2_mppi_controller/src/critics/path_align_critic.cpp | 9 +++++++--
 1 file changed, 7 insertions(+), 2 deletions(-)

diff --git a/nav2_mppi_controller/src/critics/path_align_critic.cpp b/nav2_mppi_controller/src/critics/path_align_critic.cpp
index 60b8ac9e475..5fef32d498c 100644
--- a/nav2_mppi_controller/src/critics/path_align_critic.cpp
+++ b/nav2_mppi_controller/src/critics/path_align_critic.cpp
@@ -96,8 +96,9 @@ void PathAlignCritic::score(CriticData & data)
   cost.setZero();
 
   // Find integrated arc-length distance along the path = total dist traveled along the path to each
-  // path point loop until end of path, to guarantee don't truncate long trajectories when furthest
-  // reached path is small (e.g. when all traj curve away from the path)
+  // path point
+  // loop until end of path, to guarantee don't truncate long trajectories when
+  // furthest_reached_path_point is small (e.g. when all trajectories curve away from the path)
   const size_t path_segments_count = data.path.x.size() - 1;
   // initialize the occupancy check id to max, in case the entire path is within the distance
   size_t occupancy_check_distance_idx = path_segments_count;
@@ -191,6 +192,10 @@ void PathAlignCritic::score(CriticData & data)
     path_pt = 0u;
     float Tx_m1 = T_x(t, 0);
     float Ty_m1 = T_y(t, 0);
+    // At each (strided) traj point, find the path point whose integrated arc-length distance along
+    // the path is closest to the trajectory point's integrated distance along the trajectory.
+    // if that path point is not in collision, compute the Euclidean distance between the matching
+    // path pt & traj pt the total cost is the average of those distances across the trajectory
     for (int p = 1; p < traj_sampled_size; p++) {
       const float Tx = T_x(t, p);
       const float Ty = T_y(t, p);

From b6a9d58e70352853e662a5749dc9cde8724764c6 Mon Sep 17 00:00:00 2001
From: Adi Vardi <adi.vardi@enway.ai>
Date: Fri, 27 Mar 2026 13:48:18 +0100
Subject: [PATCH 16/32] add namespace critics to critics topics

---
 nav2_mppi_controller/src/critics/path_align_critic.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/nav2_mppi_controller/src/critics/path_align_critic.cpp b/nav2_mppi_controller/src/critics/path_align_critic.cpp
index 5fef32d498c..2c2cb97edb2 100644
--- a/nav2_mppi_controller/src/critics/path_align_critic.cpp
+++ b/nav2_mppi_controller/src/critics/path_align_critic.cpp
@@ -48,7 +48,7 @@ void PathAlignCritic::initialize()
     auto node = parent_.lock();
     if (node) {
       occupancy_check_dist_pub_ = node->create_publisher<geometry_msgs::msg::PoseStamped>(
-        "PathAlignCritic/occupancy_check_end_point", 1);
+          "/critics/PathAlignCritic/occupancy_check_end_point", 1);
       occupancy_check_dist_pub_->on_activate();
     }
   }

From 59c0ffe98e5ec1894112d9e38039a82168e616d8 Mon Sep 17 00:00:00 2001
From: Adi Vardi <adi.vardi@enway.ai>
Date: Tue, 17 Feb 2026 16:48:16 +0100
Subject: [PATCH 17/32] Add timing prints

---
 nav2_mppi_controller/src/controller.cpp     |  2 +-
 nav2_mppi_controller/src/critic_manager.cpp | 52 +++++++++++++++++++++
 nav2_mppi_controller/src/optimizer.cpp      | 37 +++++++++++++++
 3 files changed, 90 insertions(+), 1 deletion(-)

diff --git a/nav2_mppi_controller/src/controller.cpp b/nav2_mppi_controller/src/controller.cpp
index 7eea0416929..3ad07fd2bca 100644
--- a/nav2_mppi_controller/src/controller.cpp
+++ b/nav2_mppi_controller/src/controller.cpp
@@ -17,7 +17,7 @@
 #include "nav2_mppi_controller/controller.hpp"
 #include "nav2_mppi_controller/tools/utils.hpp"
 
-// #define BENCHMARK_TESTING
+#define BENCHMARK_TESTING
 
 namespace nav2_mppi_controller
 {
diff --git a/nav2_mppi_controller/src/critic_manager.cpp b/nav2_mppi_controller/src/critic_manager.cpp
index c66ef5f2cfd..82ed57815d5 100644
--- a/nav2_mppi_controller/src/critic_manager.cpp
+++ b/nav2_mppi_controller/src/critic_manager.cpp
@@ -12,6 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
+#include <chrono>
+
 #include "nav2_mppi_controller/critic_manager.hpp"
 
 namespace mppi
@@ -97,6 +99,23 @@ void CriticManager::evalTrajectoriesScores(
     data.individual_critics_cost->reserve(critics_.size());
   }
 
+  // Averaging accumulators (static for debug profiling)
+  static size_t eval_count = 0;
+  static std::vector<double> critic_time_accum;
+  static double eval_total_accum = 0.0;
+  constexpr size_t kAvgWindow = 50;
+
+  if (critic_time_accum.size() != critics_.size()) {
+    critic_time_accum.assign(critics_.size(), 0.0);
+    eval_count = 0;
+    eval_total_accum = 0.0;
+  }
+
+  constexpr double kCriticsThresholdUs = 33000.0;  // 30ms
+  std::vector<double> call_critic_times(critics_.size(), 0.0);
+
+  auto eval_start = std::chrono::steady_clock::now();
+
   for (size_t i = 0; i < critics_.size(); ++i) {
     if (data.fail_flag) {
       break;
@@ -108,7 +127,13 @@ void CriticManager::evalTrajectoriesScores(
       costs_before = data.costs;
     }
 
+    auto critic_start = std::chrono::steady_clock::now();
     critics_[i]->score(data);
+    auto critic_end = std::chrono::steady_clock::now();
+    double critic_us =
+      std::chrono::duration<double, std::micro>(critic_end - critic_start).count();
+    critic_time_accum[i] += critic_us;
+    call_critic_times[i] = critic_us;
 
     // Calculate cost contribution from this critic
     if (visualize_per_critic_costs_ || publish_critics_stats_) {
@@ -130,6 +155,33 @@ void CriticManager::evalTrajectoriesScores(
     }
   }
 
+  auto eval_end = std::chrono::steady_clock::now();
+  double eval_this_us =
+    std::chrono::duration<double, std::micro>(eval_end - eval_start).count();
+  eval_total_accum += eval_this_us;
+  eval_count++;
+
+  // Print if this evaluation exceeded threshold
+  if (eval_this_us > kCriticsThresholdUs) {
+    std::cout << "[SLOW critics] " << eval_this_us / 1000.0 << "ms:";
+    for (size_t i = 0; i < critics_.size(); ++i) {
+      std::cout << " " << critic_names_[i] << "=" << call_critic_times[i] << "us";
+    }
+    std::cout << std::endl;
+  }
+
+  if (eval_count >= kAvgWindow) {
+    std::cout << "--- Critics avg over " << kAvgWindow << " evals (total="
+      << eval_total_accum / kAvgWindow << " us) ---" << std::endl;
+    for (size_t i = 0; i < critics_.size(); ++i) {
+      std::cout << "  " << critic_names_[i] << ": "
+        << critic_time_accum[i] / kAvgWindow << " us" << std::endl;
+    }
+    std::fill(critic_time_accum.begin(), critic_time_accum.end(), 0.0);
+    eval_total_accum = 0.0;
+    eval_count = 0;
+  }
+
   // Publish statistics if enabled
   if (critics_effect_pub_) {
     auto node = parent_.lock();
diff --git a/nav2_mppi_controller/src/optimizer.cpp b/nav2_mppi_controller/src/optimizer.cpp
index c5e1402b45b..75d4dbadece 100644
--- a/nav2_mppi_controller/src/optimizer.cpp
+++ b/nav2_mppi_controller/src/optimizer.cpp
@@ -261,11 +261,48 @@ std::tuple<geometry_msgs::msg::TwistStamped, Eigen::ArrayXXf> Optimizer::evalCon
 
 void Optimizer::optimize()
 {
+  // Averaging accumulators (static for debug profiling)
+  static size_t opt_count = 0;
+  static double gen_accum = 0.0, eval_accum = 0.0, update_accum = 0.0;
+  constexpr size_t kAvgWindow = 50;
+
+  constexpr double kThresholdUs = 50000.0;  // 40ms
+  double call_gen = 0.0, call_eval = 0.0, call_update = 0.0;
+
   for (size_t i = 0; i < settings_.iteration_count; ++i) {
+    auto t0 = std::chrono::steady_clock::now();
     generateNoisedTrajectories();
+    auto t1 = std::chrono::steady_clock::now();
     critic_manager_.evalTrajectoriesScores(critics_data_);
+    auto t2 = std::chrono::steady_clock::now();
     updateControlSequence();
+    auto t3 = std::chrono::steady_clock::now();
     generated_trajectories_.costs = costs_;
+    call_gen += std::chrono::duration<double, std::micro>(t1 - t0).count();
+    call_eval += std::chrono::duration<double, std::micro>(t2 - t1).count();
+    call_update += std::chrono::duration<double, std::micro>(t3 - t2).count();
+  }
+
+  gen_accum += call_gen;
+  eval_accum += call_eval;
+  update_accum += call_update;
+  opt_count++;
+
+  // Print if this iteration exceeded threshold
+  double call_total = call_gen + call_eval + call_update;
+  if (call_total > kThresholdUs) {
+    std::cout << "[SLOW optimize] " << call_total / 1000.0 << "ms: generateNoised="
+      << call_gen << "us evalCritics=" << call_eval << "us updateCtrl="
+      << call_update << "us" << std::endl;
+  }
+
+  if (opt_count >= kAvgWindow) {
+    std::cout << "optimize avg(" << kAvgWindow << "): generateNoised="
+      << gen_accum / kAvgWindow << "us evalCritics="
+      << eval_accum / kAvgWindow << "us updateCtrl="
+      << update_accum / kAvgWindow << "us" << std::endl;
+    gen_accum = eval_accum = update_accum = 0.0;
+    opt_count = 0;
   }
 }
 

From a515e522c793d8362b8be5bbaf5156ae69c38a04 Mon Sep 17 00:00:00 2001
From: Adi Vardi <adi.vardi@enway.ai>
Date: Fri, 20 Feb 2026 14:28:24 +0100
Subject: [PATCH 18/32] fix typo

---
 .../include/nav2_mppi_controller/critics/cost_critic.hpp        | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/cost_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/cost_critic.hpp
index c285a14b664..1b6776a27c1 100644
--- a/nav2_mppi_controller/include/nav2_mppi_controller/critics/cost_critic.hpp
+++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/cost_critic.hpp
@@ -58,7 +58,7 @@ class CostCritic : public CriticFunction
     */
   inline bool inCollision(float cost, float x, float y, float theta)
   {
-    // If consider_footprint_ check footprint scort for collision
+    // If consider_footprint_ check footprint score for collision
     float score_cost = cost;
     if (consider_footprint_ &&
       (cost >= possible_collision_cost_ || possible_collision_cost_ < 1.0f))

From 12855528a0c0b349657cab80d679a22f1283bc2d Mon Sep 17 00:00:00 2001
From: Adi Vardi <adi.vardi@enway.ai>
Date: Fri, 20 Feb 2026 14:29:24 +0100
Subject: [PATCH 19/32] [tmp] add metrics to costCritic

---
 nav2_mppi_controller/src/critic_manager.cpp   |  2 +-
 .../src/critics/cost_critic.cpp               | 64 +++++++++++++++++++
 nav2_mppi_controller/src/optimizer.cpp        |  2 +-
 3 files changed, 66 insertions(+), 2 deletions(-)

diff --git a/nav2_mppi_controller/src/critic_manager.cpp b/nav2_mppi_controller/src/critic_manager.cpp
index 82ed57815d5..ebe9ba40db6 100644
--- a/nav2_mppi_controller/src/critic_manager.cpp
+++ b/nav2_mppi_controller/src/critic_manager.cpp
@@ -103,7 +103,7 @@ void CriticManager::evalTrajectoriesScores(
   static size_t eval_count = 0;
   static std::vector<double> critic_time_accum;
   static double eval_total_accum = 0.0;
-  constexpr size_t kAvgWindow = 50;
+  constexpr size_t kAvgWindow = 200;
 
   if (critic_time_accum.size() != critics_.size()) {
     critic_time_accum.assign(critics_.size(), 0.0);
diff --git a/nav2_mppi_controller/src/critics/cost_critic.cpp b/nav2_mppi_controller/src/critics/cost_critic.cpp
index 4a0320efc26..7f02897f4fa 100644
--- a/nav2_mppi_controller/src/critics/cost_critic.cpp
+++ b/nav2_mppi_controller/src/critics/cost_critic.cpp
@@ -13,7 +13,9 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
+#include <chrono>
 #include <cmath>
+#include <iostream>
 #include "nav2_mppi_controller/critics/cost_critic.hpp"
 #include "nav2_costmap_2d/inflation_layer_interface.hpp"
 #include "nav2_core/controller_exceptions.hpp"
@@ -175,6 +177,13 @@ void CostCritic::score(CriticData & data)
     data.trajectories.yaws.data(), strided_traj_rows, strided_traj_cols,
     Eigen::Stride<-1, -1>(outer_stride, 1));
 
+  // Debug profiling counters
+  size_t footprint_checks = 0;
+  size_t point_lookups = 0;
+  size_t free_skips = 0;
+  size_t collisions = 0;
+  auto score_start = std::chrono::steady_clock::now();
+
   for (int i = 0; i < strided_traj_rows; ++i) {
     bool trajectory_collide = false;
     float pose_cost = 0.0f;
@@ -192,14 +201,24 @@ void CostCritic::score(CriticData & data)
         pose_cost = 255.0f;  // NO_INFORMATION in float
       } else {
         pose_cost = static_cast<float>(costmap->getCost(getIndex(x_i, y_i)));
+        point_lookups++;
         if (pose_cost < 1.0f) {
+          free_skips++;
           continue;  // In free space
         }
       }
 
+      // Count footprint checks (when inCollision will call footprintCostAtPose)
+      if (consider_footprint_ &&
+        (pose_cost >= possible_collision_cost_ || possible_collision_cost_ < 1.0f))
+      {
+        footprint_checks++;
+      }
+
       if (inCollision(pose_cost, Tx, Ty, traj_yaw(i, j))) {
         traj_cost = collision_cost_;
         trajectory_collide = true;
+        collisions++;
         break;
       }
 
@@ -216,6 +235,51 @@ void CostCritic::score(CriticData & data)
     all_trajectories_collide &= trajectory_collide;
   }
 
+  auto score_end = std::chrono::steady_clock::now();
+  double score_us = std::chrono::duration<double, std::micro>(score_end - score_start).count();
+
+  // Averaging accumulators
+  static size_t score_count = 0;
+  static double score_us_accum = 0.0;
+  static size_t fp_checks_accum = 0;
+  static size_t point_lookups_accum = 0;
+  static size_t free_skips_accum = 0;
+  static size_t collisions_accum = 0;
+  constexpr size_t kAvgWindow = 20;
+  constexpr double kSlowThresholdUs = 30000.0;
+
+  score_us_accum += score_us;
+  fp_checks_accum += footprint_checks;
+  point_lookups_accum += point_lookups;
+  free_skips_accum += free_skips;
+  collisions_accum += collisions;
+  score_count++;
+
+  // Print per-call if slow
+  if (score_us > kSlowThresholdUs) {
+    std::cout << "[SLOW CostCritic] " << score_us << "us"
+      << " fp_checks=" << footprint_checks
+      << " lookups=" << point_lookups
+      << " free=" << free_skips
+      << " collisions=" << collisions << std::endl;
+  }
+
+  if (score_count >= kAvgWindow) {
+    std::cout << "CostCritic avg(" << kAvgWindow << "): "
+      << score_us_accum / kAvgWindow << "us"
+      << " fp_checks=" << fp_checks_accum / kAvgWindow
+      << " lookups=" << point_lookups_accum / kAvgWindow
+      << " free=" << free_skips_accum / kAvgWindow
+      << " collisions=" << collisions_accum / kAvgWindow
+      << " possible_collision_cost=" << possible_collision_cost_
+      // << " circumscribed_cost_=" << circumscribed_cost_
+      // << " circumscribed_radius_=" << circumscribed_radius_
+      << std::endl;
+    score_us_accum = 0.0;
+    fp_checks_accum = point_lookups_accum = free_skips_accum = collisions_accum = 0;
+    score_count = 0;
+  }
+
   if (power_ > 1u) {
     data.costs += (repulsive_cost *
       (weight_ / static_cast<float>(strided_traj_cols))).pow(power_);
diff --git a/nav2_mppi_controller/src/optimizer.cpp b/nav2_mppi_controller/src/optimizer.cpp
index 75d4dbadece..f50f6f1c914 100644
--- a/nav2_mppi_controller/src/optimizer.cpp
+++ b/nav2_mppi_controller/src/optimizer.cpp
@@ -264,7 +264,7 @@ void Optimizer::optimize()
   // Averaging accumulators (static for debug profiling)
   static size_t opt_count = 0;
   static double gen_accum = 0.0, eval_accum = 0.0, update_accum = 0.0;
-  constexpr size_t kAvgWindow = 50;
+  constexpr size_t kAvgWindow = 200;
 
   constexpr double kThresholdUs = 50000.0;  // 40ms
   double call_gen = 0.0, call_eval = 0.0, call_update = 0.0;

From 124dd761965580641ae3d6e933dd009cb25c6e93 Mon Sep 17 00:00:00 2001
From: Adi Vardi <adi.vardi@enway.ai>
Date: Fri, 20 Feb 2026 14:36:33 +0100
Subject: [PATCH 20/32] CostCritic: get collision only once

---
 .../include/nav2_mppi_controller/critics/cost_critic.hpp     | 5 +++--
 nav2_mppi_controller/src/critics/cost_critic.cpp             | 4 +++-
 2 files changed, 6 insertions(+), 3 deletions(-)

diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/cost_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/cost_critic.hpp
index 1b6776a27c1..dd466a7f1ba 100644
--- a/nav2_mppi_controller/include/nav2_mppi_controller/critics/cost_critic.hpp
+++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/cost_critic.hpp
@@ -56,7 +56,8 @@ class CostCritic : public CriticFunction
     * @param theta theta of pose
     * @return bool if in collision
     */
-  inline bool inCollision(float cost, float x, float y, float theta)
+  inline bool inCollision(float cost, float x, float y, float theta,
+    const nav2_costmap_2d::Footprint & footprint)
   {
     // If consider_footprint_ check footprint score for collision
     float score_cost = cost;
@@ -65,7 +66,7 @@ class CostCritic : public CriticFunction
     {
       score_cost = static_cast<float>(collision_checker_.footprintCostAtPose(
           static_cast<double>(x), static_cast<double>(y), static_cast<double>(theta),
-          costmap_ros_->getRobotFootprint()));
+          footprint));
     }
 
     switch (static_cast<unsigned char>(score_cost)) {
diff --git a/nav2_mppi_controller/src/critics/cost_critic.cpp b/nav2_mppi_controller/src/critics/cost_critic.cpp
index 7f02897f4fa..2e340dc30d1 100644
--- a/nav2_mppi_controller/src/critics/cost_critic.cpp
+++ b/nav2_mppi_controller/src/critics/cost_critic.cpp
@@ -145,9 +145,11 @@ void CostCritic::score(CriticData & data)
   size_x_ = costmap->getSizeInCellsX();
   size_y_ = costmap->getSizeInCellsY();
 
+  nav2_costmap_2d::Footprint footprint;
   if (consider_footprint_) {
     // footprint may have changed since initialization if user has dynamic footprints
     possible_collision_cost_ = findCircumscribedCost(costmap_ros_);
+    footprint = costmap_ros_->getRobotFootprint();
   }
 
   // If near the goal, don't apply the preferential term since the goal is near obstacles
@@ -215,7 +217,7 @@ void CostCritic::score(CriticData & data)
         footprint_checks++;
       }
 
-      if (inCollision(pose_cost, Tx, Ty, traj_yaw(i, j))) {
+      if (inCollision(pose_cost, Tx, Ty, traj_yaw(i, j), footprint)) {
         traj_cost = collision_cost_;
         trajectory_collide = true;
         collisions++;

From c08ec449f9abc2eb0b006e2efe4107e256c40fb0 Mon Sep 17 00:00:00 2001
From: Adi Vardi <adi.vardi@enway.ai>
Date: Fri, 20 Feb 2026 15:38:33 +0100
Subject: [PATCH 21/32] [tmp] cleaner count footprint checks

---
 .../nav2_mppi_controller/critics/cost_critic.hpp    |  1 +
 nav2_mppi_controller/src/critics/cost_critic.cpp    | 13 +++----------
 2 files changed, 4 insertions(+), 10 deletions(-)

diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/cost_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/cost_critic.hpp
index dd466a7f1ba..d5e25102c48 100644
--- a/nav2_mppi_controller/include/nav2_mppi_controller/critics/cost_critic.hpp
+++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/cost_critic.hpp
@@ -145,6 +145,7 @@ class CostCritic : public CriticFunction
   std::string inflation_layer_name_;
 
   unsigned int power_{0};
+  size_t footprint_checks_;
 };
 
 }  // namespace mppi::critics
diff --git a/nav2_mppi_controller/src/critics/cost_critic.cpp b/nav2_mppi_controller/src/critics/cost_critic.cpp
index 2e340dc30d1..9c19c4632bb 100644
--- a/nav2_mppi_controller/src/critics/cost_critic.cpp
+++ b/nav2_mppi_controller/src/critics/cost_critic.cpp
@@ -180,7 +180,7 @@ void CostCritic::score(CriticData & data)
     Eigen::Stride<-1, -1>(outer_stride, 1));
 
   // Debug profiling counters
-  size_t footprint_checks = 0;
+  footprint_checks_ = 0;
   size_t point_lookups = 0;
   size_t free_skips = 0;
   size_t collisions = 0;
@@ -210,13 +210,6 @@ void CostCritic::score(CriticData & data)
         }
       }
 
-      // Count footprint checks (when inCollision will call footprintCostAtPose)
-      if (consider_footprint_ &&
-        (pose_cost >= possible_collision_cost_ || possible_collision_cost_ < 1.0f))
-      {
-        footprint_checks++;
-      }
-
       if (inCollision(pose_cost, Tx, Ty, traj_yaw(i, j), footprint)) {
         traj_cost = collision_cost_;
         trajectory_collide = true;
@@ -251,7 +244,7 @@ void CostCritic::score(CriticData & data)
   constexpr double kSlowThresholdUs = 30000.0;
 
   score_us_accum += score_us;
-  fp_checks_accum += footprint_checks;
+  fp_checks_accum += footprint_checks_;
   point_lookups_accum += point_lookups;
   free_skips_accum += free_skips;
   collisions_accum += collisions;
@@ -260,7 +253,7 @@ void CostCritic::score(CriticData & data)
   // Print per-call if slow
   if (score_us > kSlowThresholdUs) {
     std::cout << "[SLOW CostCritic] " << score_us << "us"
-      << " fp_checks=" << footprint_checks
+      << " fp_checks=" << footprint_checks_
       << " lookups=" << point_lookups
       << " free=" << free_skips
       << " collisions=" << collisions << std::endl;

From 2137d3fc96c8537e8d063da7ce4fce5a46cc64e8 Mon Sep 17 00:00:00 2001
From: Adi Vardi <adi.vardi@enway.ai>
Date: Fri, 20 Feb 2026 15:38:53 +0100
Subject: [PATCH 22/32] early exit if center is in collision

---
 .../include/nav2_mppi_controller/critics/cost_critic.hpp    | 6 ++++++
 1 file changed, 6 insertions(+)

diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/cost_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/cost_critic.hpp
index d5e25102c48..d88818d2a88 100644
--- a/nav2_mppi_controller/include/nav2_mppi_controller/critics/cost_critic.hpp
+++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/cost_critic.hpp
@@ -59,6 +59,12 @@ class CostCritic : public CriticFunction
   inline bool inCollision(float cost, float x, float y, float theta,
     const nav2_costmap_2d::Footprint & footprint)
   {
+    // if the center cost guarantees a collision, return before doing an expensive footprint check
+    if (cost == nav2_costmap_2d::LETHAL_OBSTACLE ||
+      cost == nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE) {
+      return true;
+    }
+
     // If consider_footprint_ check footprint score for collision
     float score_cost = cost;
     if (consider_footprint_ &&

From c78e04f2d69feb51e62294779f0c125256881716 Mon Sep 17 00:00:00 2001
From: Adi Vardi <adi.vardi@enway.ai>
Date: Fri, 20 Feb 2026 16:08:20 +0100
Subject: [PATCH 23/32] CostCritic: check collisions backwards

---
 nav2_mppi_controller/src/critics/cost_critic.cpp | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/nav2_mppi_controller/src/critics/cost_critic.cpp b/nav2_mppi_controller/src/critics/cost_critic.cpp
index 9c19c4632bb..cb80584e2fb 100644
--- a/nav2_mppi_controller/src/critics/cost_critic.cpp
+++ b/nav2_mppi_controller/src/critics/cost_critic.cpp
@@ -191,7 +191,8 @@ void CostCritic::score(CriticData & data)
     float pose_cost = 0.0f;
     float & traj_cost = repulsive_cost(i);
 
-    for (int j = 0; j < strided_traj_cols; j++) {
+    // iterate over the trajectory backwards, as collisions are more likely towards the end of the trajectory
+    for (int j = strided_traj_cols - 1; j >=0; j--) {
       float Tx = traj_x(i, j);
       float Ty = traj_y(i, j);
       unsigned int x_i = 0u, y_i = 0u;

From 109d2cad43285d7627adecf8ed1fe247cf2e8bb2 Mon Sep 17 00:00:00 2001
From: Adi Vardi <adi.vardi@enway.ai>
Date: Tue, 24 Feb 2026 10:20:25 +0100
Subject: [PATCH 24/32] Clamp exponential arguments

The clamp (min(80 * temperature)) caps all exp() arguments at -80, where the value is already ~1e-35, slightly above float32's smallest fraction of ~1.2e-38.
This eliminates potential underflows &= slow paths in the exp implementation.
This reduces computation by ~6x, from 2000-300us to ~400us
---
 nav2_mppi_controller/src/optimizer.cpp | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/nav2_mppi_controller/src/optimizer.cpp b/nav2_mppi_controller/src/optimizer.cpp
index f50f6f1c914..83b8288d2a2 100644
--- a/nav2_mppi_controller/src/optimizer.cpp
+++ b/nav2_mppi_controller/src/optimizer.cpp
@@ -585,7 +585,8 @@ void Optimizer::updateControlSequence()
 
   auto costs_normalized = costs_ - costs_.minCoeff();
   const float inv_temp = 1.0f / s.temperature;
-  auto softmaxes = (-inv_temp * costs_normalized).exp().eval();
+  constexpr float kMaxExponent = 80.0f;
+  auto softmaxes = ((-inv_temp * costs_normalized).cwiseMax(-kMaxExponent)).exp().eval();
   softmaxes /= softmaxes.sum();
 
   auto softmax_mat = softmaxes.matrix();

From 6d7992cb8e998f2acbbf47300963297bf2939297 Mon Sep 17 00:00:00 2001
From: Adi Vardi <adi.vardi@enway.ai>
Date: Tue, 24 Feb 2026 11:16:04 +0100
Subject: [PATCH 25/32] Add noalias to matrix multiplication

Let's Eigen know there is no aliasing, very small optimization
---
 nav2_mppi_controller/src/optimizer.cpp | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/nav2_mppi_controller/src/optimizer.cpp b/nav2_mppi_controller/src/optimizer.cpp
index 83b8288d2a2..8738a918269 100644
--- a/nav2_mppi_controller/src/optimizer.cpp
+++ b/nav2_mppi_controller/src/optimizer.cpp
@@ -590,11 +590,11 @@ void Optimizer::updateControlSequence()
   softmaxes /= softmaxes.sum();
 
   auto softmax_mat = softmaxes.matrix();
-  control_sequence_.vx = state_.cvx.transpose().matrix() * softmax_mat;
-  control_sequence_.wz = state_.cwz.transpose().matrix() * softmax_mat;
+  control_sequence_.vx.matrix().noalias() = state_.cvx.transpose().matrix() * softmax_mat;
+  control_sequence_.wz.matrix().noalias() = state_.cwz.transpose().matrix() * softmax_mat;
 
   if (is_holo) {
-    control_sequence_.vy = state_.cvy.transpose().matrix() * softmax_mat;
+    control_sequence_.vy.matrix().noalias() = state_.cvy.transpose().matrix() * softmax_mat;
   }
 
   utils::savitskyGolayFilter(control_sequence_, control_history_, settings_);

From 3e230ba8c7930e5044f8e40a4d90345e898b68cb Mon Sep 17 00:00:00 2001
From: Adi Vardi <adi.vardi@enway.ai>
Date: Tue, 24 Feb 2026 12:03:55 +0100
Subject: [PATCH 26/32] [print] fix missing fp checks value

---
 .../include/nav2_mppi_controller/critics/cost_critic.hpp         | 1 +
 1 file changed, 1 insertion(+)

diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/cost_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/cost_critic.hpp
index d88818d2a88..70ce5d2c7ac 100644
--- a/nav2_mppi_controller/include/nav2_mppi_controller/critics/cost_critic.hpp
+++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/cost_critic.hpp
@@ -73,6 +73,7 @@ class CostCritic : public CriticFunction
       score_cost = static_cast<float>(collision_checker_.footprintCostAtPose(
           static_cast<double>(x), static_cast<double>(y), static_cast<double>(theta),
           footprint));
+      footprint_checks_++;
     }
 
     switch (static_cast<unsigned char>(score_cost)) {

From 7cdcc1fa6519ec557f98dd6f068230a361fdb9a7 Mon Sep 17 00:00:00 2001
From: Adi Vardi <adi.vardi@enway.ai>
Date: Tue, 24 Feb 2026 14:27:12 +0100
Subject: [PATCH 27/32] [tmp] rm metrics from CostCritic

---
 .../critics/cost_critic.hpp                   |  2 -
 nav2_mppi_controller/src/controller.cpp       |  2 +-
 .../src/critics/cost_critic.cpp               | 57 -------------------
 3 files changed, 1 insertion(+), 60 deletions(-)

diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/cost_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/cost_critic.hpp
index 70ce5d2c7ac..2c523e7502b 100644
--- a/nav2_mppi_controller/include/nav2_mppi_controller/critics/cost_critic.hpp
+++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/cost_critic.hpp
@@ -73,7 +73,6 @@ class CostCritic : public CriticFunction
       score_cost = static_cast<float>(collision_checker_.footprintCostAtPose(
           static_cast<double>(x), static_cast<double>(y), static_cast<double>(theta),
           footprint));
-      footprint_checks_++;
     }
 
     switch (static_cast<unsigned char>(score_cost)) {
@@ -152,7 +151,6 @@ class CostCritic : public CriticFunction
   std::string inflation_layer_name_;
 
   unsigned int power_{0};
-  size_t footprint_checks_;
 };
 
 }  // namespace mppi::critics
diff --git a/nav2_mppi_controller/src/controller.cpp b/nav2_mppi_controller/src/controller.cpp
index 3ad07fd2bca..7eea0416929 100644
--- a/nav2_mppi_controller/src/controller.cpp
+++ b/nav2_mppi_controller/src/controller.cpp
@@ -17,7 +17,7 @@
 #include "nav2_mppi_controller/controller.hpp"
 #include "nav2_mppi_controller/tools/utils.hpp"
 
-#define BENCHMARK_TESTING
+// #define BENCHMARK_TESTING
 
 namespace nav2_mppi_controller
 {
diff --git a/nav2_mppi_controller/src/critics/cost_critic.cpp b/nav2_mppi_controller/src/critics/cost_critic.cpp
index cb80584e2fb..f64c5392ee6 100644
--- a/nav2_mppi_controller/src/critics/cost_critic.cpp
+++ b/nav2_mppi_controller/src/critics/cost_critic.cpp
@@ -13,9 +13,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include <chrono>
 #include <cmath>
-#include <iostream>
 #include "nav2_mppi_controller/critics/cost_critic.hpp"
 #include "nav2_costmap_2d/inflation_layer_interface.hpp"
 #include "nav2_core/controller_exceptions.hpp"
@@ -179,13 +177,6 @@ void CostCritic::score(CriticData & data)
     data.trajectories.yaws.data(), strided_traj_rows, strided_traj_cols,
     Eigen::Stride<-1, -1>(outer_stride, 1));
 
-  // Debug profiling counters
-  footprint_checks_ = 0;
-  size_t point_lookups = 0;
-  size_t free_skips = 0;
-  size_t collisions = 0;
-  auto score_start = std::chrono::steady_clock::now();
-
   for (int i = 0; i < strided_traj_rows; ++i) {
     bool trajectory_collide = false;
     float pose_cost = 0.0f;
@@ -204,9 +195,7 @@ void CostCritic::score(CriticData & data)
         pose_cost = 255.0f;  // NO_INFORMATION in float
       } else {
         pose_cost = static_cast<float>(costmap->getCost(getIndex(x_i, y_i)));
-        point_lookups++;
         if (pose_cost < 1.0f) {
-          free_skips++;
           continue;  // In free space
         }
       }
@@ -214,7 +203,6 @@ void CostCritic::score(CriticData & data)
       if (inCollision(pose_cost, Tx, Ty, traj_yaw(i, j), footprint)) {
         traj_cost = collision_cost_;
         trajectory_collide = true;
-        collisions++;
         break;
       }
 
@@ -231,51 +219,6 @@ void CostCritic::score(CriticData & data)
     all_trajectories_collide &= trajectory_collide;
   }
 
-  auto score_end = std::chrono::steady_clock::now();
-  double score_us = std::chrono::duration<double, std::micro>(score_end - score_start).count();
-
-  // Averaging accumulators
-  static size_t score_count = 0;
-  static double score_us_accum = 0.0;
-  static size_t fp_checks_accum = 0;
-  static size_t point_lookups_accum = 0;
-  static size_t free_skips_accum = 0;
-  static size_t collisions_accum = 0;
-  constexpr size_t kAvgWindow = 20;
-  constexpr double kSlowThresholdUs = 30000.0;
-
-  score_us_accum += score_us;
-  fp_checks_accum += footprint_checks_;
-  point_lookups_accum += point_lookups;
-  free_skips_accum += free_skips;
-  collisions_accum += collisions;
-  score_count++;
-
-  // Print per-call if slow
-  if (score_us > kSlowThresholdUs) {
-    std::cout << "[SLOW CostCritic] " << score_us << "us"
-      << " fp_checks=" << footprint_checks_
-      << " lookups=" << point_lookups
-      << " free=" << free_skips
-      << " collisions=" << collisions << std::endl;
-  }
-
-  if (score_count >= kAvgWindow) {
-    std::cout << "CostCritic avg(" << kAvgWindow << "): "
-      << score_us_accum / kAvgWindow << "us"
-      << " fp_checks=" << fp_checks_accum / kAvgWindow
-      << " lookups=" << point_lookups_accum / kAvgWindow
-      << " free=" << free_skips_accum / kAvgWindow
-      << " collisions=" << collisions_accum / kAvgWindow
-      << " possible_collision_cost=" << possible_collision_cost_
-      // << " circumscribed_cost_=" << circumscribed_cost_
-      // << " circumscribed_radius_=" << circumscribed_radius_
-      << std::endl;
-    score_us_accum = 0.0;
-    fp_checks_accum = point_lookups_accum = free_skips_accum = collisions_accum = 0;
-    score_count = 0;
-  }
-
   if (power_ > 1u) {
     data.costs += (repulsive_cost *
       (weight_ / static_cast<float>(strided_traj_cols))).pow(power_);

From 8eb3b515564efef2e04a97acad226402416e9328 Mon Sep 17 00:00:00 2001
From: Adi Vardi <adi.vardi@enway.ai>
Date: Tue, 24 Feb 2026 14:46:34 +0100
Subject: [PATCH 28/32] CostCritic: Add OpenMP with 4 threads

Reduces computation time from 18-20ms to 5ms
---
 nav2_mppi_controller/CMakeLists.txt              | 4 ++++
 nav2_mppi_controller/src/critics/cost_critic.cpp | 7 +++++++
 2 files changed, 11 insertions(+)

diff --git a/nav2_mppi_controller/CMakeLists.txt b/nav2_mppi_controller/CMakeLists.txt
index 81f445c6cf1..d2c1e21622c 100644
--- a/nav2_mppi_controller/CMakeLists.txt
+++ b/nav2_mppi_controller/CMakeLists.txt
@@ -22,6 +22,8 @@ find_package(visualization_msgs REQUIRED)
 find_package(nav2_ros_common REQUIRED)
 find_package(Eigen3 REQUIRED)
 
+find_package(OpenMP REQUIRED)
+
 include_directories(
   include
   ${EIGEN3_INCLUDE_DIR}
@@ -69,6 +71,7 @@ target_link_libraries(mppi_controller PUBLIC
   tf2_geometry_msgs::tf2_geometry_msgs
   tf2_ros::tf2_ros
   ${visualization_msgs_TARGETS}
+  Eigen3::Eigen
 )
 
 add_library(mppi_critics SHARED
@@ -114,6 +117,7 @@ target_link_libraries(mppi_critics PUBLIC
 )
 target_link_libraries(mppi_critics PRIVATE
   pluginlib::pluginlib
+  OpenMP::OpenMP_CXX
 )
 
 add_library(mppi_trajectory_validators SHARED
diff --git a/nav2_mppi_controller/src/critics/cost_critic.cpp b/nav2_mppi_controller/src/critics/cost_critic.cpp
index f64c5392ee6..e9158c506d3 100644
--- a/nav2_mppi_controller/src/critics/cost_critic.cpp
+++ b/nav2_mppi_controller/src/critics/cost_critic.cpp
@@ -177,6 +177,13 @@ void CostCritic::score(CriticData & data)
     data.trajectories.yaws.data(), strided_traj_rows, strided_traj_cols,
     Eigen::Stride<-1, -1>(outer_stride, 1));
 
+#pragma omp parallel for \
+  default(none) \
+  shared(repulsive_cost, costmap, near_goal, footprint, \
+    strided_traj_rows, strided_traj_cols, traj_x, traj_y, traj_yaw) \
+  reduction(&&:all_trajectories_collide) \
+  schedule(dynamic) \
+  num_threads(4)
   for (int i = 0; i < strided_traj_rows; ++i) {
     bool trajectory_collide = false;
     float pose_cost = 0.0f;

From 6b33b0d6c5b63f410a7ad5175f90bef30d96da7f Mon Sep 17 00:00:00 2001
From: Adi Vardi <adi.vardi@enway.ai>
Date: Tue, 24 Feb 2026 15:37:47 +0100
Subject: [PATCH 29/32] Add flag to disable/enable openMP & a param to set no.
 of threads

---
 nav2_mppi_controller/CMakeLists.txt           | 14 +++++-
 .../critics/cost_critic.hpp                   | 14 ++++++
 .../src/critics/cost_critic.cpp               | 44 ++++++++++++++++---
 3 files changed, 63 insertions(+), 9 deletions(-)

diff --git a/nav2_mppi_controller/CMakeLists.txt b/nav2_mppi_controller/CMakeLists.txt
index d2c1e21622c..1363dd13530 100644
--- a/nav2_mppi_controller/CMakeLists.txt
+++ b/nav2_mppi_controller/CMakeLists.txt
@@ -22,7 +22,13 @@ find_package(visualization_msgs REQUIRED)
 find_package(nav2_ros_common REQUIRED)
 find_package(Eigen3 REQUIRED)
 
-find_package(OpenMP REQUIRED)
+option(ENABLE_OPENMP "Enable OpenMP parallelization" OFF)
+if(ENABLE_OPENMP)
+  find_package(OpenMP REQUIRED)
+  message(STATUS "OpenMP ENABLED for nav2_mppi_controller (${OpenMP_CXX_VERSION})")
+else()
+  message(STATUS "OpenMP DISABLED for nav2_mppi_controller")
+endif()
 
 include_directories(
   include
@@ -117,8 +123,12 @@ target_link_libraries(mppi_critics PUBLIC
 )
 target_link_libraries(mppi_critics PRIVATE
   pluginlib::pluginlib
-  OpenMP::OpenMP_CXX
 )
+if(ENABLE_OPENMP)
+  target_link_libraries(mppi_critics PRIVATE OpenMP::OpenMP_CXX)
+else()
+  target_compile_options(mppi_critics PRIVATE -Wno-unknown-pragmas)
+endif()
 
 add_library(mppi_trajectory_validators SHARED
   src/trajectory_validators/optimal_trajectory_validator.cpp
diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/cost_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/cost_critic.hpp
index 2c523e7502b..bc320752368 100644
--- a/nav2_mppi_controller/include/nav2_mppi_controller/critics/cost_critic.hpp
+++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/cost_critic.hpp
@@ -18,6 +18,10 @@
 #include <memory>
 #include <string>
 
+#ifdef _OPENMP
+#include <omp.h>
+#endif
+
 #include "nav2_costmap_2d/footprint_collision_checker.hpp"
 #include "nav2_costmap_2d/inflation_layer.hpp"
 
@@ -96,6 +100,15 @@ class CostCritic : public CriticFunction
     */
   inline float findCircumscribedCost(std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap);
 
+  /**
+   * @brief Determine the number of threads to use for OpenMP parallelization.
+   *        If num_threads_ > 0, use that value directly.
+   *        If num_threads_ == -1 (auto), use half the available cores (memory-bound heuristic).
+   *        If OpenMP is disabled at build time, always returns 1.
+   * @return Number of threads to use
+   */
+  int getOptimalThreadCount();
+
   /**
     * @brief An implementation of worldToMap fully using floats
     * @param wx Float world X coord
@@ -150,6 +163,7 @@ class CostCritic : public CriticFunction
   float near_goal_distance_;
   std::string inflation_layer_name_;
 
+  int num_threads_{-1};
   unsigned int power_{0};
 };
 
diff --git a/nav2_mppi_controller/src/critics/cost_critic.cpp b/nav2_mppi_controller/src/critics/cost_critic.cpp
index e9158c506d3..4d48ccfe819 100644
--- a/nav2_mppi_controller/src/critics/cost_critic.cpp
+++ b/nav2_mppi_controller/src/critics/cost_critic.cpp
@@ -13,6 +13,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
+#include <algorithm>
 #include <cmath>
 #include "nav2_mppi_controller/critics/cost_critic.hpp"
 #include "nav2_costmap_2d/inflation_layer_interface.hpp"
@@ -33,6 +34,7 @@ void CostCritic::initialize()
   getParam(collision_cost_, "collision_cost", 1000000.0f);
   getParam(near_goal_distance_, "near_goal_distance", 0.5f);
   getParam(inflation_layer_name_, "inflation_layer_name", std::string(""));
+  getParam(num_threads_, "num_threads", -1);
   getParam(trajectory_point_step_, "trajectory_point_step", 2);
 
   // Normalized by cost value to put in same regime as other weights
@@ -128,6 +130,32 @@ float CostCritic::findCircumscribedCost(
   return circumscribed_cost_;
 }
 
+int CostCritic::getOptimalThreadCount()
+{
+#ifdef _OPENMP
+  if (num_threads_ > 0) {
+    RCLCPP_INFO_ONCE(
+      logger_,
+      "OpenMP: Using configured num_threads: %d",
+      num_threads_);
+    return num_threads_;
+  }
+  // Auto (-1): use half of available cores — costmap lookups are memory-bound,
+  // so more threads hit diminishing returns and increase cache contention.
+  // omp_get_max_threads() respects OMP_NUM_THREADS if set.
+  const int cpu_cores = omp_get_max_threads();
+  const int optimal = std::max(1, cpu_cores / 2);
+  RCLCPP_INFO_ONCE(
+    logger_,
+    "OpenMP: %d cores available, using %d threads (auto)",
+    cpu_cores, optimal);
+
+  return std::max(1, optimal);
+#else
+  return 1;
+#endif
+}
+
 void CostCritic::score(CriticData & data)
 {
   if (!enabled_) {
@@ -177,13 +205,15 @@ void CostCritic::score(CriticData & data)
     data.trajectories.yaws.data(), strided_traj_rows, strided_traj_cols,
     Eigen::Stride<-1, -1>(outer_stride, 1));
 
-#pragma omp parallel for \
-  default(none) \
-  shared(repulsive_cost, costmap, near_goal, footprint, \
-    strided_traj_rows, strided_traj_cols, traj_x, traj_y, traj_yaw) \
-  reduction(&&:all_trajectories_collide) \
-  schedule(dynamic) \
-  num_threads(4)
+#ifdef _OPENMP
+  #pragma omp parallel for \
+    default(none) \
+    shared(repulsive_cost, costmap, near_goal, footprint, \
+      strided_traj_rows, strided_traj_cols, traj_x, traj_y, traj_yaw) \
+    reduction(&&:all_trajectories_collide) \
+    schedule(dynamic) \
+    num_threads(getOptimalThreadCount())
+#endif
   for (int i = 0; i < strided_traj_rows; ++i) {
     bool trajectory_collide = false;
     float pose_cost = 0.0f;

From 9d9d810c74908c1400c8d305892f9242b92b6e47 Mon Sep 17 00:00:00 2001
From: Adi Vardi <adi.vardi@enway.ai>
Date: Tue, 24 Feb 2026 16:09:19 +0100
Subject: [PATCH 30/32] Revert "Add timing prints"

This reverts commit 9d721b966e67f7db64829905132f555390f0d7ed.
---
 nav2_mppi_controller/src/critic_manager.cpp | 52 ---------------------
 nav2_mppi_controller/src/optimizer.cpp      | 37 ---------------
 2 files changed, 89 deletions(-)

diff --git a/nav2_mppi_controller/src/critic_manager.cpp b/nav2_mppi_controller/src/critic_manager.cpp
index ebe9ba40db6..c66ef5f2cfd 100644
--- a/nav2_mppi_controller/src/critic_manager.cpp
+++ b/nav2_mppi_controller/src/critic_manager.cpp
@@ -12,8 +12,6 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include <chrono>
-
 #include "nav2_mppi_controller/critic_manager.hpp"
 
 namespace mppi
@@ -99,23 +97,6 @@ void CriticManager::evalTrajectoriesScores(
     data.individual_critics_cost->reserve(critics_.size());
   }
 
-  // Averaging accumulators (static for debug profiling)
-  static size_t eval_count = 0;
-  static std::vector<double> critic_time_accum;
-  static double eval_total_accum = 0.0;
-  constexpr size_t kAvgWindow = 200;
-
-  if (critic_time_accum.size() != critics_.size()) {
-    critic_time_accum.assign(critics_.size(), 0.0);
-    eval_count = 0;
-    eval_total_accum = 0.0;
-  }
-
-  constexpr double kCriticsThresholdUs = 33000.0;  // 30ms
-  std::vector<double> call_critic_times(critics_.size(), 0.0);
-
-  auto eval_start = std::chrono::steady_clock::now();
-
   for (size_t i = 0; i < critics_.size(); ++i) {
     if (data.fail_flag) {
       break;
@@ -127,13 +108,7 @@ void CriticManager::evalTrajectoriesScores(
       costs_before = data.costs;
     }
 
-    auto critic_start = std::chrono::steady_clock::now();
     critics_[i]->score(data);
-    auto critic_end = std::chrono::steady_clock::now();
-    double critic_us =
-      std::chrono::duration<double, std::micro>(critic_end - critic_start).count();
-    critic_time_accum[i] += critic_us;
-    call_critic_times[i] = critic_us;
 
     // Calculate cost contribution from this critic
     if (visualize_per_critic_costs_ || publish_critics_stats_) {
@@ -155,33 +130,6 @@ void CriticManager::evalTrajectoriesScores(
     }
   }
 
-  auto eval_end = std::chrono::steady_clock::now();
-  double eval_this_us =
-    std::chrono::duration<double, std::micro>(eval_end - eval_start).count();
-  eval_total_accum += eval_this_us;
-  eval_count++;
-
-  // Print if this evaluation exceeded threshold
-  if (eval_this_us > kCriticsThresholdUs) {
-    std::cout << "[SLOW critics] " << eval_this_us / 1000.0 << "ms:";
-    for (size_t i = 0; i < critics_.size(); ++i) {
-      std::cout << " " << critic_names_[i] << "=" << call_critic_times[i] << "us";
-    }
-    std::cout << std::endl;
-  }
-
-  if (eval_count >= kAvgWindow) {
-    std::cout << "--- Critics avg over " << kAvgWindow << " evals (total="
-      << eval_total_accum / kAvgWindow << " us) ---" << std::endl;
-    for (size_t i = 0; i < critics_.size(); ++i) {
-      std::cout << "  " << critic_names_[i] << ": "
-        << critic_time_accum[i] / kAvgWindow << " us" << std::endl;
-    }
-    std::fill(critic_time_accum.begin(), critic_time_accum.end(), 0.0);
-    eval_total_accum = 0.0;
-    eval_count = 0;
-  }
-
   // Publish statistics if enabled
   if (critics_effect_pub_) {
     auto node = parent_.lock();
diff --git a/nav2_mppi_controller/src/optimizer.cpp b/nav2_mppi_controller/src/optimizer.cpp
index 8738a918269..1b3782cfddf 100644
--- a/nav2_mppi_controller/src/optimizer.cpp
+++ b/nav2_mppi_controller/src/optimizer.cpp
@@ -261,48 +261,11 @@ std::tuple<geometry_msgs::msg::TwistStamped, Eigen::ArrayXXf> Optimizer::evalCon
 
 void Optimizer::optimize()
 {
-  // Averaging accumulators (static for debug profiling)
-  static size_t opt_count = 0;
-  static double gen_accum = 0.0, eval_accum = 0.0, update_accum = 0.0;
-  constexpr size_t kAvgWindow = 200;
-
-  constexpr double kThresholdUs = 50000.0;  // 40ms
-  double call_gen = 0.0, call_eval = 0.0, call_update = 0.0;
-
   for (size_t i = 0; i < settings_.iteration_count; ++i) {
-    auto t0 = std::chrono::steady_clock::now();
     generateNoisedTrajectories();
-    auto t1 = std::chrono::steady_clock::now();
     critic_manager_.evalTrajectoriesScores(critics_data_);
-    auto t2 = std::chrono::steady_clock::now();
     updateControlSequence();
-    auto t3 = std::chrono::steady_clock::now();
     generated_trajectories_.costs = costs_;
-    call_gen += std::chrono::duration<double, std::micro>(t1 - t0).count();
-    call_eval += std::chrono::duration<double, std::micro>(t2 - t1).count();
-    call_update += std::chrono::duration<double, std::micro>(t3 - t2).count();
-  }
-
-  gen_accum += call_gen;
-  eval_accum += call_eval;
-  update_accum += call_update;
-  opt_count++;
-
-  // Print if this iteration exceeded threshold
-  double call_total = call_gen + call_eval + call_update;
-  if (call_total > kThresholdUs) {
-    std::cout << "[SLOW optimize] " << call_total / 1000.0 << "ms: generateNoised="
-      << call_gen << "us evalCritics=" << call_eval << "us updateCtrl="
-      << call_update << "us" << std::endl;
-  }
-
-  if (opt_count >= kAvgWindow) {
-    std::cout << "optimize avg(" << kAvgWindow << "): generateNoised="
-      << gen_accum / kAvgWindow << "us evalCritics="
-      << eval_accum / kAvgWindow << "us updateCtrl="
-      << update_accum / kAvgWindow << "us" << std::endl;
-    gen_accum = eval_accum = update_accum = 0.0;
-    opt_count = 0;
   }
 }
 

From 333a28f758a92ed1b5931cf0828de01a029796ef Mon Sep 17 00:00:00 2001
From: Adi Vardi <adi.vardi@enway.ai>
Date: Mon, 23 Mar 2026 11:48:09 +0100
Subject: [PATCH 31/32] break long line

---
 nav2_mppi_controller/src/critics/cost_critic.cpp | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/nav2_mppi_controller/src/critics/cost_critic.cpp b/nav2_mppi_controller/src/critics/cost_critic.cpp
index 4d48ccfe819..df886b23b0b 100644
--- a/nav2_mppi_controller/src/critics/cost_critic.cpp
+++ b/nav2_mppi_controller/src/critics/cost_critic.cpp
@@ -219,7 +219,8 @@ void CostCritic::score(CriticData & data)
     float pose_cost = 0.0f;
     float & traj_cost = repulsive_cost(i);
 
-    // iterate over the trajectory backwards, as collisions are more likely towards the end of the trajectory
+    // iterate over the trajectory backwards, as collisions are more likely towards the end of
+    // the trajectory
     for (int j = strided_traj_cols - 1; j >=0; j--) {
       float Tx = traj_x(i, j);
       float Ty = traj_y(i, j);

From 04855875261233c7496a02910c74523232d45ec7 Mon Sep 17 00:00:00 2001
From: Adi Vardi <adi.vardi@enway.ai>
Date: Wed, 8 Apr 2026 15:56:24 +0200
Subject: [PATCH 32/32] update license year

---
 .../nav2_mppi_controller/critics/direction_change_critic.hpp    | 2 +-
 nav2_mppi_controller/src/critics/direction_change_critic.cpp    | 2 +-
 2 files changed, 2 insertions(+), 2 deletions(-)

diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/direction_change_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/direction_change_critic.hpp
index c823c564226..b481baa96e2 100644
--- a/nav2_mppi_controller/include/nav2_mppi_controller/critics/direction_change_critic.hpp
+++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/direction_change_critic.hpp
@@ -1,4 +1,4 @@
-// Copyright (c) 2024 Enway GmbH, Adi Vardi
+// Copyright (c) 2026 Enway GmbH, Adi Vardi
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
diff --git a/nav2_mppi_controller/src/critics/direction_change_critic.cpp b/nav2_mppi_controller/src/critics/direction_change_critic.cpp
index f7d5f55ddee..fdbaffc903f 100644
--- a/nav2_mppi_controller/src/critics/direction_change_critic.cpp
+++ b/nav2_mppi_controller/src/critics/direction_change_critic.cpp
@@ -1,4 +1,4 @@
-// Copyright (c) 2024 Enway GmbH, Adi Vardi
+// Copyright (c) 2026 Enway GmbH, Adi Vardi
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.