Add min_distance_to_obstacle parameter to RPP

nav2_regulated_pure_pursuit_controller/README.md

@@ -77,6 +77,7 @@ Note: The maximum allowed time to collision is thresholded by the lookahead poin
 | `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. | 
 | `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. |
+| `min_distance_to_obstacle` | The shorter distance at which the robot is allowed to be from an obstacle along its trajectory. Set <= 0.0 to disable. 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 | 

nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/parameter_handler.hpp

@@ -42,6 +42,7 @@ struct Parameters
  double min_approach_linear_velocity;
  double approach_velocity_scaling_dist;
  double max_allowed_time_to_collision_up_to_carrot;
+ double min_distance_to_obstacle;
  bool use_regulated_linear_velocity_scaling;
  bool use_cost_regulated_linear_velocity_scaling;
  double cost_scaling_dist;

nav2_regulated_pure_pursuit_controller/src/collision_checker.cpp

@@ -90,16 +90,21 @@ bool CollisionChecker::isCollisionImminent(
  curr_pose.x = robot_pose.pose.position.x;
  curr_pose.y = robot_pose.pose.position.y;
  curr_pose.theta = tf2::getYaw(robot_pose.pose.orientation);
+ double accumulated_distance = 0.0;
 
  // only forward simulate within time requested
  int i = 1;
- while (i * projection_time < params_->max_allowed_time_to_collision_up_to_carrot) {
+ while (i * projection_time < params_->max_allowed_time_to_collision_up_to_carrot ||
+ (accumulated_distance < params_->min_distance_to_obstacle &&
+ projection_time * linear_vel > 0.01))
+ {
  i++;
 
  // apply velocity at curr_pose over distance
  curr_pose.x += projection_time * (linear_vel * cos(curr_pose.theta));
  curr_pose.y += projection_time * (linear_vel * sin(curr_pose.theta));
  curr_pose.theta += projection_time * angular_vel;
+ accumulated_distance += projection_time * linear_vel;
 
  // check if past carrot pose, where no longer a thoughtfully valid command
  if (hypot(curr_pose.x - robot_xy.x, curr_pose.y - robot_xy.y) > carrot_dist) {

nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp

@@ -60,6 +60,9 @@ ParameterHandler::ParameterHandler(
  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_ + ".min_distance_to_obstacle",
+ rclcpp::ParameterValue(-1.0));
  declare_parameter_if_not_declared(
  node, plugin_name_ + ".use_regulated_linear_velocity_scaling", rclcpp::ParameterValue(true));
  declare_parameter_if_not_declared(
@@ -128,6 +131,9 @@ ParameterHandler::ParameterHandler(
  node->get_parameter(
  plugin_name_ + ".max_allowed_time_to_collision_up_to_carrot",
  params_.max_allowed_time_to_collision_up_to_carrot);
+ node->get_parameter(
+ plugin_name_ + ".min_distance_to_obstacle",
+ params_.min_distance_to_obstacle);
  node->get_parameter(
  plugin_name_ + ".use_regulated_linear_velocity_scaling",
  params_.use_regulated_linear_velocity_scaling);
@@ -233,6 +239,8 @@ ParameterHandler::dynamicParametersCallback(
  params_.curvature_lookahead_dist = parameter.as_double();
  } else if (name == plugin_name_ + ".max_allowed_time_to_collision_up_to_carrot") {
  params_.max_allowed_time_to_collision_up_to_carrot = parameter.as_double();
+ } else if (name == plugin_name_ + ".min_distance_to_obstacle") {
+ params_.min_distance_to_obstacle = parameter.as_double();
  } else if (name == plugin_name_ + ".cost_scaling_dist") {
  params_.cost_scaling_dist = parameter.as_double();
  } else if (name == plugin_name_ + ".cost_scaling_gain") {

nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp

@@ -701,6 +701,7 @@ TEST(RegulatedPurePursuitTest, testDynamicParameter)
  rclcpp::Parameter("test.rotate_to_heading_angular_vel", 18.0),
  rclcpp::Parameter("test.min_approach_linear_velocity", 1.0),
  rclcpp::Parameter("test.max_allowed_time_to_collision_up_to_carrot", 2.0),
+ rclcpp::Parameter("test.min_distance_to_obstacle", 2.0),
  rclcpp::Parameter("test.cost_scaling_dist", 2.0),
  rclcpp::Parameter("test.cost_scaling_gain", 4.0),
  rclcpp::Parameter("test.regulated_linear_scaling_min_radius", 10.0),
@@ -729,6 +730,7 @@ TEST(RegulatedPurePursuitTest, testDynamicParameter)
  EXPECT_EQ(
  node->get_parameter(
  "test.max_allowed_time_to_collision_up_to_carrot").as_double(), 2.0);
+ EXPECT_EQ(node->get_parameter("test.min_distance_to_obstacle").as_double(), 2.0);
  EXPECT_EQ(node->get_parameter("test.cost_scaling_dist").as_double(), 2.0);
  EXPECT_EQ(node->get_parameter("test.cost_scaling_gain").as_double(), 4.0);
  EXPECT_EQ(node->get_parameter("test.regulated_linear_scaling_min_radius").as_double(), 10.0);