min_distance_to_obstacle

Author: Guillaume Doisy

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);