[RPP] Prevent collision check premature termination with scaled_lookahead

nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/collision_checker.hpp

@@ -62,12 +62,13 @@ class CollisionChecker
    * @param linear_vel linear velocity to forward project
    * @param angular_vel angular velocity to forward project
    * @param carrot_dist Distance to the carrot for PP
+   * @param dist_to_path_end Distance used to regulate min_distance_to_obstacle
    * @return Whether collision is imminent
    */
   bool isCollisionImminent(
     const geometry_msgs::msg::PoseStamped &,
     const double &, const double &,
-    const double &);
+    const double &, const double &);
 
   /**
    * @brief checks for collision at projected pose

nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/parameter_handler.hpp

@@ -61,6 +61,7 @@ struct Parameters
   double max_robot_pose_search_dist;
   bool interpolate_curvature_after_goal;
   bool use_collision_detection;
+  bool use_path_aware_obstacle_distance;
   double transform_tolerance;
   bool stateful;
 };

nav2_regulated_pure_pursuit_controller/src/collision_checker.cpp

@@ -48,7 +48,7 @@ CollisionChecker::CollisionChecker(
 bool CollisionChecker::isCollisionImminent(
   const geometry_msgs::msg::PoseStamped & robot_pose,
   const double & linear_vel, const double & angular_vel,
-  const double & carrot_dist)
+  const double & carrot_dist, const double & dist_to_path_end)
 {
   // Note(stevemacenski): This may be a bit unusual, but the robot_pose is in
   // odom frame and the carrot_pose is in robot base frame. Just how the data comes to us
@@ -91,12 +91,27 @@ bool CollisionChecker::isCollisionImminent(
 
   // only forward simulate within time requested
   double max_allowed_time_to_collision_check = params_->max_allowed_time_to_collision_up_to_carrot;
+  double simulation_distance_limit = carrot_dist;
   if (params_->min_distance_to_obstacle > 0.0) {
     max_allowed_time_to_collision_check = std::max(
         params_->max_allowed_time_to_collision_up_to_carrot,
         params_->min_distance_to_obstacle / std::max(std::abs(linear_vel),
         params_->min_approach_linear_velocity)
     );
+    if (params_->use_velocity_scaled_lookahead_dist) {
+      double base_simulation_dist;
+      if (params_->use_path_aware_obstacle_distance) {
+        // prevents the checker from looking "through" the goal
+        // considering the safe distance until the path end
+        const double effective_min_dist = std::min(params_->min_distance_to_obstacle,
+            dist_to_path_end);
+        // the simulation distance should look at least as far as the effective safety distance
+        base_simulation_dist = std::max(carrot_dist, effective_min_dist);
+      } else {
+        base_simulation_dist = std::max(carrot_dist, params_->min_distance_to_obstacle);
+      }
+      simulation_distance_limit = std::min(base_simulation_dist, params_->max_lookahead_dist);
+    }
   }
   int i = 1;
   while (i * projection_time < max_allowed_time_to_collision_check) {
@@ -110,8 +125,11 @@ bool CollisionChecker::isCollisionImminent(
     theta += projection_time * angular_vel;
     curr_pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(theta);
 
-    // check if past carrot pose, where no longer a thoughtfully valid command
-    if (hypot(curr_pose.position.x - robot_xy.x, curr_pose.position.y - robot_xy.y) > carrot_dist) {
+    // Stop simulating if we've gone past the simulation distance limit
+    // (max: carrot or min distance)
+    if (hypot(curr_pose.position.x - robot_xy.x,
+        curr_pose.position.y - robot_xy.y) > simulation_distance_limit)
+    {
       break;
     }
 

nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp

@@ -104,6 +104,9 @@ ParameterHandler::ParameterHandler(
   declare_parameter_if_not_declared(
     node, plugin_name_ + ".use_collision_detection",
     rclcpp::ParameterValue(true));
+  declare_parameter_if_not_declared(
+    node, plugin_name_ + ".use_path_aware_obstacle_distance",
+    rclcpp::ParameterValue(false));
   declare_parameter_if_not_declared(
       node, plugin_name_ + ".stateful", rclcpp::ParameterValue(true));
 
@@ -189,6 +192,30 @@ ParameterHandler::ParameterHandler(
   node->get_parameter(
     plugin_name_ + ".use_collision_detection",
     params_.use_collision_detection);
+  if (params_.use_collision_detection &&
+    params_.min_distance_to_obstacle > params_.max_lookahead_dist)
+  {
+    RCLCPP_WARN(
+      logger_,
+      "min_distance_to_obstacle (%.02f) is greater than max_lookahead_dist (%.02f). "
+      "The collision check distance will be capped by max_lookahead_dist.",
+      params_.min_distance_to_obstacle, params_.max_lookahead_dist);
+  }
+  node->get_parameter(
+    plugin_name_ + ".use_path_aware_obstacle_distance",
+    params_.use_path_aware_obstacle_distance);
+  if (params_.use_path_aware_obstacle_distance && params_.min_distance_to_obstacle <= 0.0) {
+    RCLCPP_WARN(
+      logger_,
+      "For 'use_path_aware_obstacle_distance' to be enabled, 'min_distance_to_obstacle' "
+    "must be set to a positive value, but it is not. The path-aware logic will be ignored.");
+  }
+  if (params_.use_path_aware_obstacle_distance && params_.use_velocity_scaled_lookahead_dist) {
+    RCLCPP_WARN(
+      logger_,
+      "For 'use_path_aware_obstacle_distance' to be enabled, 'use_velocity_scaled_lookahead_dist' "
+    "must also be true. The path-aware logic will be ignored.");
+  }
   node->get_parameter(plugin_name_ + ".stateful", params_.stateful);
 
   if (params_.inflation_cost_scaling_factor <= 0.0) {
@@ -344,6 +371,8 @@ ParameterHandler::updateParametersCallback(
         params_.allow_reversing = parameter.as_bool();
       } else if (param_name == plugin_name_ + ".interpolate_curvature_after_goal") {
         params_.interpolate_curvature_after_goal = parameter.as_bool();
+      } else if (param_name == plugin_name_ + ".use_path_aware_obstacle_distance") {
+        params_.use_path_aware_obstacle_distance = parameter.as_bool();
       }
     }
   }

nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp

@@ -274,10 +274,13 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity
     angular_vel = linear_vel * regulation_curvature;
   }
 
+  const double dist_to_path_end =
+    nav2_util::geometry_utils::calculate_path_length(transformed_plan);
   // Collision checking on this velocity heading
   const double & carrot_dist = hypot(carrot_pose.pose.position.x, carrot_pose.pose.position.y);
   if (params_->use_collision_detection &&
-    collision_checker_->isCollisionImminent(pose, linear_vel, angular_vel, carrot_dist))
+    collision_checker_->isCollisionImminent(pose, linear_vel, angular_vel, carrot_dist,
+      dist_to_path_end))
   {
     throw nav2_core::NoValidControl("RegulatedPurePursuitController detected collision ahead!");
   }