MPPI PathAlignCritic: Add a minimum dist for occupancy check

nav2_mppi_controller/README.md

@@ -133,7 +133,8 @@ Uses inflated costmap cost directly to avoid obstacles
  | threshold_to_consider      | double | Default 0.5. Distance between robot and goal above which path align cost stops being considered                                    |
  | offset_from_furthest      | double | Default 20. Checks that the candidate trajectories are sufficiently far along their way tracking the path to apply the alignment critic. This ensures that path alignment is only considered when actually tracking the path, preventing awkward initialization motions preventing the robot from leaving the path to achieve the appropriate heading.  |
  | trajectory_point_step      | int | Default 4. Step of trajectory points to evaluate for path distance to reduce compute time. Between 1-10 is typically reasonable.   |
- | max_path_occupancy_ratio   | double | Default 0.07 (7%). Maximum proportion of the path that can be occupied before this critic is not considered to allow the obstacle and path follow critics to avoid obstacles while following the path's intent in presence of dynamic objects in the scene.  |
+ | max_path_occupancy_ratio   | double | Default 0.07 (7%). Maximum proportion of a path segment that can be occupied before this critic is not considered to allow the obstacle and path follow critics to avoid obstacles while following the path's intent in presence of dynamic objects in the scene.  |
+ | occupancy_check_min_distance   | double | Default 2.0. Min distance for the occupancy check's path segment. The segment length is ``min(occupancy_check_min_distance, distance_to_furthest_reached_path_point)`` |
  | use_path_orientations   | bool | Default false. Whether to consider path's orientations in path alignment, which can be useful when paired with feasible smac planners to incentivize directional changes only where/when the smac planner requests them. If you want the robot to deviate and invert directions where the controller sees fit, keep as false. If your plans do not contain orientation information (e.g. navfn), keep as false.  |
 
 #### Path Angle Critic
@@ -308,6 +309,8 @@ The same applies to the Path Follow and Align offsets from furthest. In the same
 
 The Path Follow critic cannot drive velocities greater than the projectable distance of that velocity on the available path on the rolling costmap. The Path Align critic `offset_from_furthest` represents the number of path points a trajectory passes through while tracking the path. If this is set either absurdly low (e.g. 5) it can trigger when a robot is simply trying to start path tracking causing some suboptimal behaviors and local minima while starting a task. If it is set absurdly high (e.g. 50) relative to the path resolution and costmap size, then the critic may never trigger or only do so when at full-speed. A balance here is wise. A selection of this value to be ~30% of the maximum velocity distance projected is good (e.g. if a planner produces points every 2.5cm, 60 can fit on the 1.5m local costmap radius. If the max speed is 0.5m/s with a 3s prediction time, then 20 points represents 33% of the maximum speed projected over the prediction horizon onto the path). When in doubt, `prediction_horizon_s * max_speed / path_resolution / 3.0` is a good baseline.
 
+If your robot's kinematics is very different than the defaults, you may need to tune ``PathAlignCritic.occupancy_check_min_distance``. This distance defines the min length of the path segment used to disable the ``PathAlignCritic``. A reasonable initial value should be the horizon's length at full speed. If the distance is too short, the robot may slow down when approaching an obstacle ahead and may even get too close to it and get stuck. If the distance is too big, the critic will be disabled too early, which may cause deviation from path (if for example, the reference point for PathFollowCritic is beyond a curve on the path).
+
 ### Obstacle, Inflation Layer, and Path Following
 
 There also exists a relationship between the costmap configurations and the Obstacle critic configurations. If the Obstacle critic is not well tuned with the costmap parameters (inflation radius, scale) it can cause the robot to wobble significantly as it attempts to take finitely lower-cost trajectories with a slightly lower cost in exchange for jerky motion. The default behavior was tuned for small AMRs (e.g. turtlebots or similar), so if using a larger robot, you may want to reduce the `repulsion_weight` in kind. It may also perform awkward maneuvers when in free-space to try to maximize time in a small pocket of 0-cost over a more natural motion which involves moving into some low-costed region. Finally, it may generally refuse to go into costed space at all when starting in a free 0-cost space if the gain is set disproportionately higher than the Path Follow scoring to encourage the robot to move along the path. This is due to the critic cost of staying in free space becoming more attractive than entering even lightly costed space in exchange for progression along the task.

nav2_mppi_controller/include/nav2_mppi_controller/critics/path_align_critic.hpp

@@ -48,6 +48,7 @@ class PathAlignCritic : public CriticFunction
   size_t offset_from_furthest_{0};
   int trajectory_point_step_{0};
   float threshold_to_consider_{0};
+  float min_distance_occupancy_check_{0};
   float max_path_occupancy_ratio_{0};
   bool use_path_orientations_{false};
   unsigned int power_{0};

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(min_distance_occupancy_check_, "min_distance_occupancy_check", 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);
@@ -45,29 +46,15 @@ void PathAlignCritic::score(CriticData & data)
 
   // Don't apply 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 (path_segments_count < offset_from_furthest_) {
+  const size_t furthest_reached_path_point = *data.furthest_reached_path_point;
+  if (furthest_reached_path_point < offset_from_furthest_) {
     return;
   }
 
-  // Don't apply when dynamic obstacles are blocking significant proportions of the local path
-  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) {
-      return;
-    }
-  }
-
-  const size_t batch_size = data.trajectories.x.rows();
-  Eigen::ArrayXf cost(data.costs.rows());
-  cost.setZero();
-
-  // Find integrated distance in the path
+  // Find integrated distance in the path and find the first path IDX further than
+  //  max(min_distance_occupancy_check_, furthest_reached_path_point)
+  const size_t path_segments_count = data.path.x.size() - 1;
+  size_t occupancy_check_distance_idx = 0;
   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;
@@ -80,6 +67,26 @@ void PathAlignCritic::score(CriticData & data)
     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);
+
+    if (path_integrated_distances[i] <= min_distance_occupancy_check_ ||
+      i < furthest_reached_path_point)
+    {
+      occupancy_check_distance_idx = (i + 1 < path_segments_count) ? i + 1 : i;
+    }
+  }
+
+  // Don't apply when dynamic obstacles are blocking significant proportions of the local path
+  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)
+    {
+      return;
+    }
   }
 
   // Finish populating the path vector
@@ -111,13 +118,21 @@ void PathAlignCritic::score(CriticData & data)
     Eigen::Stride<-1, -1>(outer_stride, 1));
   const auto traj_sampled_size = T_x.cols();
 
+  const size_t batch_size = data.trajectories.x.rows();
+  Eigen::ArrayXf cost(data.costs.rows());
+  cost.setZero();
+
   for (size_t t = 0; t < batch_size; ++t) {
     summed_path_dist = 0.0f;
     num_samples = 0u;
     traj_integrated_distance = 0.0f;
     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 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);