Skip to content

Commit

Permalink
min_distance_to_obstacle
Browse files Browse the repository at this point in the history
  • Loading branch information
Guillaume Doisy committed Jul 18, 2024
1 parent 14e1e84 commit 1714665
Show file tree
Hide file tree
Showing 5 changed files with 18 additions and 1 deletion.
1 change: 1 addition & 0 deletions nav2_regulated_pure_pursuit_controller/README.md
Original file line number Diff line number Diff line change
Expand Up @@ -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 |
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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) {
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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(
Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -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") {
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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),
Expand Down Expand Up @@ -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);
Expand Down

0 comments on commit 1714665

Please sign in to comment.