Skip to content

[RPP] Add parameter to enable/disable collision detection #3204

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
merged 2 commits into from
Sep 21, 2022
Merged

[RPP] Add parameter to enable/disable collision detection #3204

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
4ba0c3b
[RPP] Add parameter to enable/disable collision detection
fantalukas Sep 8, 2022
d4fc42b
[RPP] Update README
fantalukas Sep 21, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 3 additions & 1 deletion nav2_regulated_pure_pursuit_controller/README.md
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -62,7 +62,8 @@ Note: The maximum allowed time to collision is thresholded by the lookahead poin
| `use_velocity_scaled_lookahead_dist` | Whether to use the velocity scaled lookahead distances or constant `lookahead_distance` |
| `min_approach_linear_velocity` | The minimum velocity threshold to apply when approaching the goal |
| `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. |
| `max_allowed_time_to_collision_up_to_carrot` | The time to project a velocity command to check for collisions, limited to maximum distance of lookahead distance selected |
| `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. |
| `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 Expand Up @@ -111,6 +112,7 @@ controller_server:
min_approach_linear_velocity: 0.05
use_approach_linear_velocity_scaling: true
approach_velocity_scaling_dist: 1.0
use_collision_detection: true
max_allowed_time_to_collision_up_to_carrot: 1.0
use_regulated_linear_velocity_scaling: true
use_cost_regulated_linear_velocity_scaling: false
Expand Down
1 change: 1 addition & 0 deletions ...ller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -292,6 +292,7 @@ class RegulatedPurePursuitController : public nav2_core::Controller
double approach_velocity_scaling_dist_;
double control_duration_;
double max_allowed_time_to_collision_up_to_carrot_;
bool use_collision_detection_;
bool use_regulated_linear_velocity_scaling_;
bool use_cost_regulated_linear_velocity_scaling_;
double cost_scaling_dist_;
Expand Down
8 changes: 7 additions & 1 deletion nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -85,6 +85,9 @@ void RegulatedPurePursuitController::configure(
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_ + ".use_collision_detection",
rclcpp::ParameterValue(true));
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 @@ -142,6 +145,9 @@ void RegulatedPurePursuitController::configure(
node->get_parameter(
plugin_name_ + ".max_allowed_time_to_collision_up_to_carrot",
max_allowed_time_to_collision_up_to_carrot_);
node->get_parameter(
plugin_name_ + ".use_collision_detection",
use_collision_detection_);
node->get_parameter(
plugin_name_ + ".use_regulated_linear_velocity_scaling",
use_regulated_linear_velocity_scaling_);
Expand Down Expand Up @@ -346,7 +352,7 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity

// Collision checking on this velocity heading
const double & carrot_dist = hypot(carrot_pose.pose.position.x, carrot_pose.pose.position.y);
if (isCollisionImminent(pose, linear_vel, angular_vel, carrot_dist)) {
if (use_collision_detection_ && isCollisionImminent(pose, linear_vel, angular_vel, carrot_dist)) {
throw nav2_core::PlannerException("RegulatedPurePursuitController detected collision ahead!");
}

Expand Down