Styling and ordering of files refining comments

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

Author: silanus23

nav2_mppi_controller/include/nav2_mppi_controller/critics/path_align_critic.hpp

@@ -15,6 +15,7 @@
 #ifndef NAV2_MPPI_CONTROLLER__CRITICS__PATH_ALIGN_CRITIC_HPP_
 #define NAV2_MPPI_CONTROLLER__CRITICS__PATH_ALIGN_CRITIC_HPP_
 
+#include <algorithm>
 #include <vector>
 
 #include "nav2_mppi_controller/critic_function.hpp"
@@ -48,26 +49,54 @@ class PathAlignCritic : public CriticFunction
 
 protected:
   /**
-   * @brief Score trajectories using arc-length matching
+   * @brief Update cached path data when path changes
+   *
+   * Precomputes and caches path segment geometry including segment vectors,
+   * lengths, and cumulative arc-length distances. Only recomputes when the path
+   * actually changes (detected via size and key point checks).
+   *
+   * @param path Reference path to cache
+   * @param path_segments_count Number of segments to consider from path start
    */
-  void scoreArcLength(CriticData & data, std::vector<bool> & path_pts_valid);
+  void updatePathCache(const models::Path & path, size_t path_segments_count);
 
   /**
-   * @brief Score trajectories using geometric distance to path segments
+   * @brief Score trajectories using arc-length matching along path
+   *
+   * Integrates distance traveled along each trajectory and matches it to equivalent
+   * arc-length positions on the reference path. Penalizes Euclidean distance between
+   * trajectory points and their corresponding path points. Optionally includes
+   * orientation difference when use_path_orientations_ is enabled.
+   *
+   * @param data Critic data containing trajectories and path information
+   * @param path_pts_valid Validity flags for each path point (collision-free or not)
    */
-  void scoreGeometric(CriticData & data, std::vector<bool> & path_pts_valid);
+  void scoreArcLength(CriticData & data, std::vector<bool> & path_pts_valid);
 
   /**
-   * @brief Update cached path data when path changes
+   * @brief Score trajectories using geometric distance to path segments
+   *
+   * Samples points along each trajectory and computes their perpendicular distance
+   * to the nearest path segment. Uses a hint-based search within a configurable
+   * window for efficiency. Only scores points within lookahead_distance_ of the
+   * trajectory start.
+   *
+   * @param data Critic data containing trajectories and path information
+   * @param path_pts_valid Validity flags for each path point (collision-free or not)
    */
-  void updatePathCache(const models::Path & path, size_t path_segments_count);
+  void scoreGeometric(CriticData & data, std::vector<bool> & path_pts_valid);
 
   /**
    * @brief Compute minimum distance from point to any path segment
+   *
+   * Uses a search window around the hint index to find the nearest segment,
+   * exploiting path locality. The window is defined in arc-length distance
+   * along the path. Updates the hint for subsequent calls.
+   *
    * @param px Point x coordinate
    * @param py Point y coordinate
-   * @param closest_seg_idx In/out: hint for search, updated to closest segment
-   * @return Distance to closest segment
+   * @param closest_seg_idx [in/out] Search hint, updated to closest segment found
+   * @return Euclidean distance to closest segment
    */
   float computeMinDistanceToPath(float px, float py, Eigen::Index & closest_seg_idx);
 

nav2_mppi_controller/src/critics/path_align_critic.cpp

@@ -46,13 +46,13 @@ void PathAlignCritic::initialize()
 
   RCLCPP_INFO(
     logger_,
-    "PathAlignCritic instantiated with %d power, arc_length_weight: %f, geometric_weight: %f, mode: %s",
+    "PathAlignCritic instantiated with %d power, arc_length_weight: %f, "
+    "geometric_weight: %f, mode: %s",
     power_, arc_length_weight_, geometric_weight_, mode.c_str());
 }
 
 void PathAlignCritic::score(CriticData & data)
 {
-  // Skip scoring if disabled or path too short
   if (!enabled_ || data.state.local_path_length < threshold_to_consider_) {
     return;
   }
@@ -76,14 +76,12 @@ void PathAlignCritic::score(CriticData & data)
     if (invalid_ctr / static_cast<float>(path_segments_count) > max_path_occupancy_ratio_ &&
       invalid_ctr > 2.0f)
     {
-      return;  // Too many obstacles on path, don't score
+      return;
     }
   }
 
-  // Cache path segment geometry for efficient distance calculations
   updatePathCache(data.path, path_segments_count);
 
-  // Apply selected scoring mode(s)
   if (use_geometric_alignment_) {
     scoreGeometric(data, path_pts_valid);
   }
@@ -92,6 +90,63 @@ void PathAlignCritic::score(CriticData & data)
   }
 }
 
+void PathAlignCritic::updatePathCache(const models::Path & path, size_t path_segments_count)
+{
+  if (path_segments_count == 0) {
+    path_size_cache_ = 0;
+    return;
+  }
+
+  // path_segments_count is the number of segments, so number of points is segments + 1
+  const size_t path_points_count = path_segments_count + 1;
+
+  // Detect path changes cheaply using size and strategic point comparisons
+  const Eigen::Index path_points_idx = static_cast<Eigen::Index>(path_points_count);
+  const Eigen::Index mid_idx = path_points_idx / 2;
+  if (path_points_idx == static_cast<Eigen::Index>(path_size_cache_) &&
+    path_x_cache_.size() == path_points_idx &&
+    path.x(0) == path_x_cache_(0) &&
+    path.y(0) == path_y_cache_(0) &&
+    path.x(mid_idx) == path_x_cache_(mid_idx) &&
+    path.y(mid_idx) == path_y_cache_(mid_idx))
+  {
+    return;
+  }
+
+  path_size_cache_ = path_points_count;
+
+  if (static_cast<Eigen::Index>(path_x_cache_.size()) != path_points_idx) {
+    path_x_cache_.resize(path_points_idx);
+    path_y_cache_.resize(path_points_idx);
+  }
+
+  path_x_cache_.head(path_points_idx) = path.x.head(path_points_idx);
+  path_y_cache_.head(path_points_idx) = path.y.head(path_points_idx);
+
+  if (path_points_count < 2) {return;}
+
+  const Eigen::Index num_segments = path_points_idx - 1;
+
+  if (static_cast<Eigen::Index>(segment_lengths_.size()) != num_segments) {
+    segment_lengths_.resize(num_segments);
+    cumulative_distances_.resize(path_points_idx);
+    segment_dx_.resize(num_segments);
+    segment_dy_.resize(num_segments);
+    segment_len_sq_.resize(num_segments);
+    segment_inv_len_sq_.resize(num_segments);
+  }
+
+  cumulative_distances_(0) = 0.0;
+  for (Eigen::Index i = 0; i < num_segments; ++i) {
+    segment_dx_(i) = path.x(i + 1) - path.x(i);
+    segment_dy_(i) = path.y(i + 1) - path.y(i);
+    segment_len_sq_(i) = segment_dx_(i) * segment_dx_(i) + segment_dy_(i) * segment_dy_(i);
+    segment_lengths_(i) = std::sqrt(segment_len_sq_(i));
+    cumulative_distances_(i + 1) = cumulative_distances_(i) + segment_lengths_(i);
+    segment_inv_len_sq_(i) = (segment_len_sq_(i) > 1e-6f) ? (1.0f / segment_len_sq_(i)) : 0.0f;
+  }
+}
+
 void PathAlignCritic::scoreArcLength(CriticData & data, std::vector<bool> & path_pts_valid)
 {
   const size_t path_segments_count = *data.furthest_reached_path_point;
@@ -112,10 +167,10 @@ void PathAlignCritic::scoreArcLength(CriticData & data, std::vector<bool> & path
   unsigned int path_pt = 0u;
   float traj_integrated_distance = 0.0f;
 
+  // Sample trajectories at intervals to reduce computation while maintaining accuracy
   int strided_traj_rows = data.trajectories.x.rows();
   int strided_traj_cols = floor((data.trajectories.x.cols() - 1) / trajectory_point_step_) + 1;
   int outer_stride = strided_traj_rows * trajectory_point_step_;
-  // Get strided trajectory information
   const auto T_x = Eigen::Map<const Eigen::ArrayXXf, 0,
       Eigen::Stride<-1, -1>>(
     data.trajectories.x.data(),
@@ -145,12 +200,12 @@ void PathAlignCritic::scoreArcLength(CriticData & data, std::vector<bool> & path
       Tx_m1 = Tx;
       Ty_m1 = Ty;
       traj_integrated_distance += sqrtf(dx * dx + dy * dy);
+      // Find path point at equivalent arc-length distance
       path_pt = utils::findClosestPathPtBinary(
         cumulative_distances_.data(), path_segments_count,
         traj_integrated_distance);
 
-      // The nearest path point to align to needs to be not in collision, else
-      // let the obstacle critic take over in this region due to dynamic obstacles
+      // Skip invalid path points - let obstacle critic handle dynamic obstacles
       if (path_pts_valid[path_pt]) {
         const auto & pose = path[path_pt];
         dx = pose.x - Tx;
@@ -193,13 +248,12 @@ void PathAlignCritic::scoreGeometric(CriticData & data, std::vector<bool> & path
   Eigen::ArrayXi valid_sample_count(batch_size);
   valid_sample_count.setZero();
 
-  // Store closest segment index per trajectory for search efficiency
+  // Initialize search hints from robot's current position for efficiency
   if (closest_indices_.size() != batch_size) {
     closest_indices_.resize(batch_size);
     closest_indices_.setZero();
   }
 
-  // Reset hints to robot's current position on path to avoid stale hints from previous cycle
   const float robot_x = data.state.pose.pose.position.x;
   const float robot_y = data.state.pose.pose.position.y;
   const size_t path_segments_count = *data.furthest_reached_path_point;
@@ -208,7 +262,6 @@ void PathAlignCritic::scoreGeometric(CriticData & data, std::vector<bool> & path
   Eigen::Index current_closest_seg = 0;
   float min_dist_sq = std::numeric_limits<float>::max();
 
-  // Find initial hint by locating robot position on path
   for (Eigen::Index i = 0;
     i < num_segments && i < static_cast<Eigen::Index>(path_segments_count);
     ++i)
@@ -225,17 +278,14 @@ void PathAlignCritic::scoreGeometric(CriticData & data, std::vector<bool> & path
   const int effective_stride = std::max(1, std::min(trajectory_point_step_,
      static_cast<int>(traj_length)));
 
-  // Score each trajectory in the batch
   for (Eigen::Index traj_idx = 0; traj_idx < batch_size; ++traj_idx) {
     float accumulated_distance = 0.0f;
 
-    // Start from first trajectory point
     float prev_x = traj_x(traj_idx, 0);
     float prev_y = traj_y(traj_idx, 0);
 
     int sample_idx = 0;
 
-    // Sample trajectory points at regular intervals
     while (true) {
       const Eigen::Index traj_col = sample_idx * effective_stride;
       if (traj_col >= traj_length) {break;}
@@ -249,15 +299,13 @@ void PathAlignCritic::scoreGeometric(CriticData & data, std::vector<bool> & path
         const float dy = py - prev_y;
         accumulated_distance += std::sqrt(dx * dx + dy * dy);
 
-        // Stop scoring beyond lookahead distance
         if (accumulated_distance > lookahead_distance_) {break;}
       }
 
       // Use previous closest segment as hint for efficient search
       Eigen::Index closest_seg = closest_indices_(traj_idx);
       float min_dist = computeMinDistanceToPath(px, py, closest_seg);
 
-      // Only accumulate cost if the closest segment is valid
       if (closest_seg < static_cast<Eigen::Index>(path_segments_count) &&
         path_pts_valid[closest_seg])
       {
@@ -272,96 +320,32 @@ void PathAlignCritic::scoreGeometric(CriticData & data, std::vector<bool> & path
     }
   }
 
-  // Normalize by number of samples to get average deviation per sample
   for (Eigen::Index i = 0; i < batch_size; ++i) {
     if (valid_sample_count(i) > 0) {
       cost_array(i) /= static_cast<float>(valid_sample_count(i));
     }
   }
 
-  // Apply weight and power, then add to total costs
   if (power_ > 1u) {
     data.costs += (cost_array * geometric_weight_).pow(power_);
   } else {
     data.costs += cost_array * geometric_weight_;
   }
 }
 
-void PathAlignCritic::updatePathCache(const models::Path & path, size_t path_segments_count)
-{
-  // Handle empty path
-  if (path_segments_count == 0) {
-    path_size_cache_ = 0;
-    return;
-  }
-
-  // path_segments_count is the number of segments, so number of points is segments + 1
-  const size_t path_points_count = path_segments_count + 1;
-
-  // Only update cache if path changed by checking size, first, and middle positions
-  const Eigen::Index path_points_idx = static_cast<Eigen::Index>(path_points_count);
-  const Eigen::Index mid_idx = path_points_idx / 2;
-  if (path_points_idx == static_cast<Eigen::Index>(path_size_cache_) &&
-    path_x_cache_.size() == path_points_idx &&
-    path.x(0) == path_x_cache_(0) &&
-    path.y(0) == path_y_cache_(0) &&
-    path.x(mid_idx) == path_x_cache_(mid_idx) &&
-    path.y(mid_idx) == path_y_cache_(mid_idx))
-  {
-    return;
-  }
-
-  path_size_cache_ = path_points_count;
-
-  if (static_cast<Eigen::Index>(path_x_cache_.size()) != path_points_idx) {
-    path_x_cache_.resize(path_points_idx);
-    path_y_cache_.resize(path_points_idx);
-  }
-
-  // Cache path points
-  path_x_cache_.head(path_points_idx) = path.x.head(path_points_idx);
-  path_y_cache_.head(path_points_idx) = path.y.head(path_points_idx);
-
-  if (path_points_count < 2) {return;}
-
-  const Eigen::Index num_segments = path_points_idx - 1;
-
-  if (static_cast<Eigen::Index>(segment_lengths_.size()) != num_segments) {
-    segment_lengths_.resize(num_segments);
-    cumulative_distances_.resize(path_points_idx);
-    segment_dx_.resize(num_segments);
-    segment_dy_.resize(num_segments);
-    segment_len_sq_.resize(num_segments);
-    segment_inv_len_sq_.resize(num_segments);
-  }
-
-  // Precompute segment geometry for efficient distance calculations
-  cumulative_distances_(0) = 0.0;
-  for (Eigen::Index i = 0; i < num_segments; ++i) {
-    segment_dx_(i) = path.x(i + 1) - path.x(i);
-    segment_dy_(i) = path.y(i + 1) - path.y(i);
-    segment_len_sq_(i) = segment_dx_(i) * segment_dx_(i) + segment_dy_(i) * segment_dy_(i);
-    segment_lengths_(i) = std::sqrt(segment_len_sq_(i));
-    cumulative_distances_(i + 1) = cumulative_distances_(i) + segment_lengths_(i);
-    segment_inv_len_sq_(i) = (segment_len_sq_(i) > 1e-6f) ? (1.0f / segment_len_sq_(i)) : 0.0f;
-  }
-}
-
 // TODO(@silanus23): Not sure if this should be in util too
 float PathAlignCritic::computeMinDistanceToPath(float px, float py, Eigen::Index & closest_seg_idx)
 {
   const Eigen::Index num_segments = static_cast<Eigen::Index>(path_size_cache_) - 1;
   if (num_segments < 1) {return std::numeric_limits<float>::max();}
 
-  // Clamp hint index to valid range
   closest_seg_idx = std::clamp(closest_seg_idx, Eigen::Index(0), num_segments - 1);
 
-  // Define search window around hint using cumulative distances along path
+  // Limit search to window around hint (exploits path locality)
   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_);
 
-  // Use binary search to find start segment for search window
   const Eigen::Index start_idx = static_cast<Eigen::Index>(
     utils::findClosestPathPtBinary(
       cumulative_distances_.data(),
@@ -371,10 +355,9 @@ float PathAlignCritic::computeMinDistanceToPath(float px, float py, Eigen::Index
   float min_dist_sq = std::numeric_limits<float>::max();
   Eigen::Index best_seg = closest_seg_idx;
 
-  // Search only within window for efficiency
   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;}  // Skip degenerate segments
+    if (segment_len_sq_(seg_idx) < 1e-6f) {continue;}
 
     const float dist_sq = distSqToSegment(px, py, seg_idx);
 
@@ -384,7 +367,6 @@ float PathAlignCritic::computeMinDistanceToPath(float px, float py, Eigen::Index
     }
   }
 
-  // Update hint for next call
   closest_seg_idx = best_seg;
   return std::sqrt(min_dist_sq);
 }