Skip to content

Commit

Permalink
feat(start_planner): add skip_rear_vehicle_check parameter (#8863)
Browse files Browse the repository at this point in the history
Add the skip_rear_vehicle_check parameter to the start planner module configuration. This parameter allows disabling the rear vehicle check during collision detection. By default, the rear vehicle check is enabled.

Signed-off-by: Kyoichi Sugahara <[email protected]>
  • Loading branch information
kyoichi-sugahara authored Sep 12, 2024
1 parent 19894ae commit 9fa11de
Show file tree
Hide file tree
Showing 5 changed files with 42 additions and 21 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -349,25 +349,26 @@ PullOutPath --o PullOutPlannerBase

## General parameters for start_planner

| Name | Unit | Type | Description | Default value |
| :--------------------------------------------------------- | :---- | :----- | :------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------------- |
| th_arrived_distance | [m] | double | distance threshold for arrival of path termination | 1.0 |
| th_stopped_velocity | [m/s] | double | velocity threshold for arrival of path termination | 0.01 |
| th_stopped_time | [s] | double | time threshold for arrival of path termination | 1.0 |
| th_distance_to_middle_of_the_road | [m] | double | distance threshold to determine if the vehicle is on the middle of the road | 0.1 |
| collision_check_margins | [m] | double | Obstacle collision check margins list | [2.0, 1.0, 0.5, 0.1] |
| shift_collision_check_distance_from_end | [m] | double | collision check distance from end shift end pose | -10.0 |
| geometric_collision_check_distance_from_end | [m] | double | collision check distance from end geometric end pose | 0.0 |
| collision_check_margin_from_front_object | [m] | double | collision check margin from front object | 5.0 |
| extra_width_margin_for_rear_obstacle | [m] | double | extra width that is added to the perceived rear obstacle's width when deciding if the rear obstacle can overtake the ego vehicle while it is merging to a lane from the shoulder lane. | 0.5 |
| object_types_to_check_for_path_generation.check_car | - | bool | flag to check car for path generation | true |
| object_types_to_check_for_path_generation.check_truck | - | bool | flag to check truck for path generation | true |
| object_types_to_check_for_path_generation.check_bus | - | bool | flag to check bus for path generation | true |
| object_types_to_check_for_path_generation.check_bicycle | - | bool | flag to check bicycle for path generation | true |
| object_types_to_check_for_path_generation.check_motorcycle | - | bool | flag to check motorcycle for path generation | true |
| object_types_to_check_for_path_generation.check_pedestrian | - | bool | flag to check pedestrian for path generation | true |
| object_types_to_check_for_path_generation.check_unknown | - | bool | flag to check unknown for path generation | true |
| center_line_path_interval | [m] | double | reference center line path point interval | 1.0 |
| Name | Unit | Type | Description | Default value |
| :--------------------------------------------------------- | :---- | :----- | :------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------ | :------------------- |
| th_arrived_distance | [m] | double | distance threshold for arrival of path termination | 1.0 |
| th_stopped_velocity | [m/s] | double | velocity threshold for arrival of path termination | 0.01 |
| th_stopped_time | [s] | double | time threshold for arrival of path termination | 1.0 |
| th_distance_to_middle_of_the_road | [m] | double | distance threshold to determine if the vehicle is on the middle of the road | 0.1 |
| collision_check_margins | [m] | double | Obstacle collision check margins list | [2.0, 1.0, 0.5, 0.1] |
| shift_collision_check_distance_from_end | [m] | double | collision check distance from end shift end pose | -10.0 |
| geometric_collision_check_distance_from_end | [m] | double | collision check distance from end geometric end pose | 0.0 |
| collision_check_margin_from_front_object | [m] | double | collision check margin from front object | 5.0 |
| skip_rear_vehicle_check | - | bool | flag to skip rear vehicle check (rear vehicle check is performed to skip safety check and proceed with departure when the ego vehicle is obstructing the progress of a rear vehicle) | false |
| extra_width_margin_for_rear_obstacle | [m] | double | extra width that is added to the perceived rear obstacle's width when deciding if the rear obstacle can overtake the ego vehicle while it is merging to a lane from the shoulder lane | 0.5 |
| object_types_to_check_for_path_generation.check_car | - | bool | flag to check car for path generation | true |
| object_types_to_check_for_path_generation.check_truck | - | bool | flag to check truck for path generation | true |
| object_types_to_check_for_path_generation.check_bus | - | bool | flag to check bus for path generation | true |
| object_types_to_check_for_path_generation.check_bicycle | - | bool | flag to check bicycle for path generation | true |
| object_types_to_check_for_path_generation.check_motorcycle | - | bool | flag to check motorcycle for path generation | true |
| object_types_to_check_for_path_generation.check_pedestrian | - | bool | flag to check pedestrian for path generation | true |
| object_types_to_check_for_path_generation.check_unknown | - | bool | flag to check unknown for path generation | true |
| center_line_path_interval | [m] | double | reference center line path point interval | 1.0 |

### **Ego vehicle's velocity planning**

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -7,6 +7,7 @@
th_stopped_time: 1.0
collision_check_margins: [2.0, 1.0, 0.5, 0.1]
collision_check_margin_from_front_object: 5.0
skip_rear_vehicle_check: false
extra_width_margin_for_rear_obstacle: 0.5
th_moving_object_velocity: 1.0
object_types_to_check_for_path_generation:
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -102,6 +102,7 @@ struct StartPlannerParameters
double th_stopped_time{0.0};
double prepare_time_before_start{0.0};
double th_distance_to_middle_of_the_road{0.0};
bool skip_rear_vehicle_check{false};
double extra_width_margin_for_rear_obstacle{0.0};
std::vector<double> collision_check_margins{};
double collision_check_margin_from_front_object{0.0};
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -40,6 +40,7 @@ void StartPlannerModuleManager::init(rclcpp::Node * node)
p.prepare_time_before_start = node->declare_parameter<double>(ns + "prepare_time_before_start");
p.th_distance_to_middle_of_the_road =
node->declare_parameter<double>(ns + "th_distance_to_middle_of_the_road");
p.skip_rear_vehicle_check = node->declare_parameter<bool>(ns + "skip_rear_vehicle_check");
p.extra_width_margin_for_rear_obstacle =
node->declare_parameter<double>(ns + "extra_width_margin_for_rear_obstacle");
p.collision_check_margins =
Expand Down Expand Up @@ -357,6 +358,7 @@ void StartPlannerModuleManager::updateModuleParams(
updateParam<double>(parameters, ns + "prepare_time_before_start", p->prepare_time_before_start);
updateParam<double>(
parameters, ns + "th_distance_to_middle_of_the_road", p->th_distance_to_middle_of_the_road);
updateParam<bool>(parameters, ns + "skip_rear_vehicle_check", p->skip_rear_vehicle_check);
updateParam<double>(
parameters, ns + "extra_width_margin_for_rear_obstacle",
p->extra_width_margin_for_rear_obstacle);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -301,8 +301,24 @@ bool StartPlannerModule::receivedNewRoute() const

bool StartPlannerModule::requiresDynamicObjectsCollisionDetection() const
{
return parameters_->safety_check_params.enable_safety_check && status_.driving_forward &&
!isPreventingRearVehicleFromPassingThrough();
const auto & safety_params = parameters_->safety_check_params;
const auto & skip_rear_vehicle_check = parameters_->skip_rear_vehicle_check;

// Return false and do not perform collision detection if any of the following conditions are
// true:
// - Safety check is disabled.
// - The vehicle is not driving forward.
if (!safety_params.enable_safety_check || !status_.driving_forward) {
return false;
}

// Return true and always perform collision detection if the following condition is true:
// - Rear vehicle check is set to be skipped.
if (skip_rear_vehicle_check) {
return true;
}

return !isPreventingRearVehicleFromPassingThrough();
}

bool StartPlannerModule::noMovingObjectsAround() const
Expand Down

0 comments on commit 9fa11de

Please sign in to comment.