Add min_distance_to_obstacle parameter to RPP (#4543)

* min_distance_to_obstacle

Signed-off-by: Guillaume Doisy <[email protected]>

* suggestion to time base and combine

Signed-off-by: Guillaume Doisy <[email protected]>

* typo

Signed-off-by: Guillaume Doisy <[email protected]>

* use min_approach_linear_velocity

Signed-off-by: Guillaume Doisy <[email protected]>

---------

Signed-off-by: Guillaume Doisy <[email protected]>
Co-authored-by: Guillaume Doisy <[email protected]>

Author: 2 people

nav2_regulated_pure_pursuit_controller/README.md

@@ -91,6 +91,7 @@ Note: The maximum allowed time to collision is thresholded by the lookahead poin
 | `max_angular_accel` | Maximum allowable angular acceleration while rotating to heading, if enabled |
 | `max_robot_pose_search_dist` | Maximum integrated distance along the path to bound the search for the closest pose to the robot. This is set by default to the maximum costmap extent, so it shouldn't be set manually unless there are loops within the local costmap. |
 | `interpolate_curvature_after_goal` | Needs use_fixed_curvature_lookahead to be true. Interpolate a carrot after the goal dedicated to the curvature calculation (to avoid oscillations at the end of the path) |
+| `min_distance_to_obstacle` | The shortest 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. |
 
 Example fully-described XML with default parameter values:
 

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

@@ -92,8 +92,16 @@ bool CollisionChecker::isCollisionImminent(
   curr_pose.theta = tf2::getYaw(robot_pose.pose.orientation);
 
   // only forward simulate within time requested
+  double max_allowed_time_to_collision_check = params_->max_allowed_time_to_collision_up_to_carrot;
+  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)
+    );
+  }
   int i = 1;
-  while (i * projection_time < params_->max_allowed_time_to_collision_up_to_carrot) {
+  while (i * projection_time < max_allowed_time_to_collision_check) {
     i++;
 
     // apply velocity at curr_pose over distance

nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp

@@ -61,6 +61,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(
@@ -131,6 +134,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);
@@ -278,6 +284,8 @@ ParameterHandler::updateParametersCallback(
         params_.curvature_lookahead_dist = parameter.as_double();
       } else if (param_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 (param_name == plugin_name_ + ".min_distance_to_obstacle") {
+        params_.min_distance_to_obstacle = parameter.as_double();
       } else if (param_name == plugin_name_ + ".cost_scaling_dist") {
         params_.cost_scaling_dist = parameter.as_double();
       } else if (param_name == plugin_name_ + ".cost_scaling_gain") {

nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp

@@ -720,6 +720,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),
@@ -749,6 +750,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);