diff --git a/nav2_regulated_pure_pursuit_controller/README.md b/nav2_regulated_pure_pursuit_controller/README.md index acdd0eb1ed9..e02725fe035 100644 --- a/nav2_regulated_pure_pursuit_controller/README.md +++ b/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 | diff --git a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/parameter_handler.hpp b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/parameter_handler.hpp index b033289622f..efad25ef56c 100644 --- a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/parameter_handler.hpp +++ b/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; diff --git a/nav2_regulated_pure_pursuit_controller/src/collision_checker.cpp b/nav2_regulated_pure_pursuit_controller/src/collision_checker.cpp index ac654fcab76..f1091d2f956 100644 --- a/nav2_regulated_pure_pursuit_controller/src/collision_checker.cpp +++ b/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) { diff --git a/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp b/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp index 42876c248e7..804ae123591 100644 --- a/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp +++ b/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") { diff --git a/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp b/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp index 38cc195bc61..a41b9b85693 100644 --- a/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp +++ b/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);