From 9fa11de61b02edf3cedc54663338bdfa290c2821 Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara Date: Thu, 12 Sep 2024 18:06:34 +0900 Subject: [PATCH] feat(start_planner): add skip_rear_vehicle_check parameter (#8863) 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 --- .../README.md | 39 ++++++++++--------- .../config/start_planner.param.yaml | 1 + .../data_structs.hpp | 1 + .../src/manager.cpp | 2 + .../src/start_planner_module.cpp | 20 +++++++++- 5 files changed, 42 insertions(+), 21 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/README.md b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/README.md index 5b68b021f99cb..931b70f8482a5 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/README.md +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/README.md @@ -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** diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/config/start_planner.param.yaml b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/config/start_planner.param.yaml index 443563a7626eb..f93800450a479 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/config/start_planner.param.yaml +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/config/start_planner.param.yaml @@ -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: diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/data_structs.hpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/data_structs.hpp index 5903a691d60e1..9c439e9b3b921 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/data_structs.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/data_structs.hpp @@ -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 collision_check_margins{}; double collision_check_margin_from_front_object{0.0}; diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/manager.cpp index 193f88d212842..f5739ce322248 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/manager.cpp @@ -40,6 +40,7 @@ void StartPlannerModuleManager::init(rclcpp::Node * node) p.prepare_time_before_start = node->declare_parameter(ns + "prepare_time_before_start"); p.th_distance_to_middle_of_the_road = node->declare_parameter(ns + "th_distance_to_middle_of_the_road"); + p.skip_rear_vehicle_check = node->declare_parameter(ns + "skip_rear_vehicle_check"); p.extra_width_margin_for_rear_obstacle = node->declare_parameter(ns + "extra_width_margin_for_rear_obstacle"); p.collision_check_margins = @@ -357,6 +358,7 @@ void StartPlannerModuleManager::updateModuleParams( updateParam(parameters, ns + "prepare_time_before_start", p->prepare_time_before_start); updateParam( parameters, ns + "th_distance_to_middle_of_the_road", p->th_distance_to_middle_of_the_road); + updateParam(parameters, ns + "skip_rear_vehicle_check", p->skip_rear_vehicle_check); updateParam( parameters, ns + "extra_width_margin_for_rear_obstacle", p->extra_width_margin_for_rear_obstacle); diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp index ec0db1e9c0f4b..4b3a76c8530dd 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp @@ -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