[RPP] Add parameter to enable/disable collision detection (#3204)

* [RPP] Add parameter to enable/disable collision detection

* [RPP] Update README

nav2_regulated_pure_pursuit_controller/README.md

@@ -62,7 +62,8 @@ Note: The maximum allowed time to collision is thresholded by the lookahead poin
 | `use_velocity_scaled_lookahead_dist` | Whether to use the velocity scaled lookahead distances or constant `lookahead_distance` | 
 | `min_approach_linear_velocity` | The minimum velocity threshold to apply when approaching the goal | 
 | `approach_velocity_scaling_dist` | Integrated distance from end of transformed path at which to start applying velocity scaling. This defaults to the forward extent of the costmap minus one costmap cell length. | 
-| `max_allowed_time_to_collision_up_to_carrot` | The time to project a velocity command to check for collisions, limited to maximum distance of lookahead distance selected | 
+| `use_collision_detection` | Whether to enable collision detection. |
+| `max_allowed_time_to_collision_up_to_carrot` | The time to project a velocity command to check for collisions when `use_collision_detection` is `true`. It is limited to maximum distance of lookahead distance selected. |
 | `use_regulated_linear_velocity_scaling` | Whether to use the regulated features for curvature | 
 | `use_cost_regulated_linear_velocity_scaling` | Whether to use the regulated features for proximity to obstacles | 
 | `cost_scaling_dist` | The minimum distance from an obstacle to trigger the scaling of linear velocity, if `use_cost_regulated_linear_velocity_scaling` is enabled. The value set should be smaller or equal to the `inflation_radius` set in the inflation layer of costmap, since inflation is used to compute the distance from obstacles | 
@@ -111,6 +112,7 @@ controller_server:
  min_approach_linear_velocity: 0.05
  use_approach_linear_velocity_scaling: true
  approach_velocity_scaling_dist: 1.0
+ use_collision_detection: true
  max_allowed_time_to_collision_up_to_carrot: 1.0
  use_regulated_linear_velocity_scaling: true
  use_cost_regulated_linear_velocity_scaling: false

nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp

@@ -292,6 +292,7 @@ class RegulatedPurePursuitController : public nav2_core::Controller
  double approach_velocity_scaling_dist_;
  double control_duration_;
  double max_allowed_time_to_collision_up_to_carrot_;
+ bool use_collision_detection_;
  bool use_regulated_linear_velocity_scaling_;
  bool use_cost_regulated_linear_velocity_scaling_;
  double cost_scaling_dist_;

nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp

@@ -85,6 +85,9 @@ void RegulatedPurePursuitController::configure(
  declare_parameter_if_not_declared(
  node, plugin_name_ + ".max_allowed_time_to_collision_up_to_carrot",
  rclcpp::ParameterValue(1.0));
+ declare_parameter_if_not_declared(
+ node, plugin_name_ + ".use_collision_detection",
+ rclcpp::ParameterValue(true));
  declare_parameter_if_not_declared(
  node, plugin_name_ + ".use_regulated_linear_velocity_scaling", rclcpp::ParameterValue(true));
  declare_parameter_if_not_declared(
@@ -142,6 +145,9 @@ void RegulatedPurePursuitController::configure(
  node->get_parameter(
  plugin_name_ + ".max_allowed_time_to_collision_up_to_carrot",
  max_allowed_time_to_collision_up_to_carrot_);
+ node->get_parameter(
+ plugin_name_ + ".use_collision_detection",
+ use_collision_detection_);
  node->get_parameter(
  plugin_name_ + ".use_regulated_linear_velocity_scaling",
  use_regulated_linear_velocity_scaling_);
@@ -346,7 +352,7 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity
 
  // Collision checking on this velocity heading
  const double & carrot_dist = hypot(carrot_pose.pose.position.x, carrot_pose.pose.position.y);
- if (isCollisionImminent(pose, linear_vel, angular_vel, carrot_dist)) {
+ if (use_collision_detection_ && isCollisionImminent(pose, linear_vel, angular_vel, carrot_dist)) {
  throw nav2_core::PlannerException("RegulatedPurePursuitController detected collision ahead!");
  }