Final optimizations:

Stoppin after finding close enough segment
Adaptive search window
Tuning parameters.

Signed-off-by: silanus23 <berkantali23@outlook.com>

Author: silanus23

nav2_bringup/params/nav2_params.yaml

@@ -215,10 +215,10 @@ controller_server:
         threshold_to_consider: 0.5
         offset_from_furthest: 20
         use_path_orientations: false
-        use_geometric_alignment: true
-        search_window: 1.0
+        use_geometric_alignment: false
+        search_window: 0.3
         score_arc_length: true
-        lookahead_distance: 2.0
+        lookahead_distance: 0.7
       PathFollowCritic:
         enabled: true
         cost_power: 1

nav2_mppi_controller/include/nav2_mppi_controller/critics/path_align_critic.hpp

@@ -114,42 +114,36 @@ class PathAlignCritic : public CriticFunction
    * @note Returns squared distance to avoid sqrt() computation
    * @note Handles degenerate (zero-length) segments
    */
-  inline float distSqToSegment(float px, float py, Eigen::Index seg_idx)
-  {
-    const float x1 = path_x_cache_(seg_idx);
-    const float y1 = path_y_cache_(seg_idx);
-    const float dx = segment_dx_(seg_idx);
-    const float dy = segment_dy_(seg_idx);
-    const float inv_len_sq = segment_inv_len_sq_(seg_idx);
-
-    if (inv_len_sq < 1e-6f) {
-      // Degenerate segment, return distance to point
-      const float dpx = px - x1;
-      const float dpy = py - y1;
-      return dpx * dpx + dpy * dpy;
-    }
-
-    // Project point onto segment, clamped to [0,1]
-    const float t = std::max(0.0f, std::min(1.0f, ((px - x1) * dx + (py - y1) * dy) * inv_len_sq));
-
-    const float closest_x = x1 + t * dx;
-    const float closest_y = y1 + t * dy;
-
-    const float dist_x = px - closest_x;
-    const float dist_y = py - closest_y;
-
-    return dist_x * dist_x + dist_y * dist_y;
+inline float distSqToSegment(float px, float py, Eigen::Index seg_idx) const
+{
+  const float dpx = px - path_x_cache_(seg_idx);
+  const float dpy = py - path_y_cache_(seg_idx);
+  
+  const float inv_len_sq = segment_inv_len_sq_(seg_idx);
+  if (inv_len_sq < 1e-6f) [[unlikely]] {
+    return dpx * dpx + dpy * dpy;
   }
 
+  const float dx = segment_dx_(seg_idx);
+  const float dy = segment_dy_(seg_idx);
+  const float t = std::clamp((dpx * dx + dpy * dy) * inv_len_sq, 0.0f, 1.0f);
+  
+  const float dist_x = dpx - t * dx;
+  const float dist_y = dpy - t * dy;
+
+  return dist_x * dist_x + dist_y * dist_y;
+}
+
   size_t offset_from_furthest_{0};
   int trajectory_point_step_{0};
   float threshold_to_consider_{0};
   float max_path_occupancy_ratio_{0};
-  bool use_geometric_alignment_{false};
-  bool score_arc_length_{true};
-  double search_window_{2.0};
   bool use_path_orientations_{false};
-  float lookahead_distance_{5.0};
+  bool score_arc_length_{true};
+  bool use_geometric_alignment_;
+  double search_window_;
+  float lookahead_distance_;
+  float early_termination_distance_;
 
   unsigned int power_{0};
   float weight_{0};

nav2_mppi_controller/src/critics/path_align_critic.cpp

@@ -31,9 +31,10 @@ void PathAlignCritic::initialize()
   getParam(threshold_to_consider_, "threshold_to_consider", 0.5f);
   getParam(use_path_orientations_, "use_path_orientations", false);
   getParam(use_geometric_alignment_, "use_geometric_alignment", false);
-  getParam(search_window_, "search_window", 0.5);
   getParam(score_arc_length_, "score_arc_length", true);
-  getParam(lookahead_distance_, "lookahead_distance", 1.0f);
+  getParam(search_window_, "search_window", 0.3);
+  getParam(lookahead_distance_, "lookahead_distance", 0.7f);
+  getParam(early_termination_distance_, "early_termination_distance", 0.025f);
 
   if (!score_arc_length_ && !use_geometric_alignment_) {
     RCLCPP_WARN(logger_,
@@ -341,29 +342,44 @@ float PathAlignCritic::computeMinDistanceToPath(float px, float py, Eigen::Index
 
   closest_seg_idx = std::clamp(closest_seg_idx, Eigen::Index(0), num_segments - 1);
 
-  // Limit search to window around hint (exploits path locality)
+  float min_dist_sq = distSqToSegment(px, py, closest_seg_idx);
+  Eigen::Index best_seg = closest_seg_idx;
+
+  // Early termination if already very close
+  const float good_enough_dist_sq = early_termination_distance_ * early_termination_distance_;
+  if (min_dist_sq < good_enough_dist_sq) {
+    return std::sqrt(min_dist_sq);
+  }
+  
+  // Adaptive search window - smaller if hint is good
+  const float adaptive_window = (min_dist_sq < 0.01f) ? search_window_ * 0.5f : search_window_;
+  
   const float hint_distance = cumulative_distances_(closest_seg_idx);
-  const float start_distance = std::max(0.0f, hint_distance - static_cast<float>(search_window_));
-  const float end_distance = hint_distance + static_cast<float>(search_window_);
+  const float start_distance = std::max(0.0f, hint_distance - adaptive_window);
+  const float end_distance = hint_distance + adaptive_window;
 
   const Eigen::Index start_idx = static_cast<Eigen::Index>(
     utils::findClosestPathPtBinary(
       cumulative_distances_.data(),
       path_size_cache_,
       start_distance));
 
-  float min_dist_sq = std::numeric_limits<float>::max();
-  Eigen::Index best_seg = closest_seg_idx;
-
   for (Eigen::Index seg_idx = start_idx; seg_idx < num_segments; ++seg_idx) {
     if (cumulative_distances_(seg_idx) > end_distance) {break;}
     if (segment_len_sq_(seg_idx) < 1e-6f) {continue;}
+    if (seg_idx == closest_seg_idx) {continue;} // Already checked
 
     const float dist_sq = distSqToSegment(px, py, seg_idx);
 
     if (dist_sq < min_dist_sq) {
       min_dist_sq = dist_sq;
       best_seg = seg_idx;
+      
+      // Early exit if found very close match
+      if (min_dist_sq < good_enough_dist_sq) {
+        closest_seg_idx = best_seg;
+        return std::sqrt(min_dist_sq);
+      }
     }
   }