From 0ad0b7fbecf75628b058b145cb46e55a273c7377 Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Fri, 2 Aug 2024 17:24:57 +0900 Subject: [PATCH] feat(lane_change): use different rss param to deal with parked vehicle (#8316) * different rss value for parked vehicle Signed-off-by: Muhammad Zulfaqar Azmi * Documentation and config file update Signed-off-by: Zulfaqar Azmi --------- Signed-off-by: Muhammad Zulfaqar Azmi Signed-off-by: Zulfaqar Azmi Signed-off-by: Batuhan Beytekin --- .../README.md | 12 ++++++++++++ .../config/lane_change.param.yaml | 8 ++++++++ .../utils/data_structs.hpp | 1 + .../src/manager.cpp | 17 +++++++++++++++++ .../src/scene.cpp | 7 ++++++- 5 files changed, 44 insertions(+), 1 deletion(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md index 967e8352b830a..e42e6e2d47738 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md @@ -773,6 +773,18 @@ The following parameters are used to judge lane change completion. | `safety_check.execution.longitudinal_distance_min_threshold` | [m] | double | The longitudinal distance threshold that is used to determine whether longitudinal distance between two object is enough and whether lane change is safe. | 3.0 | | `safety_check.cancel.longitudinal_velocity_delta_time` | [m] | double | The time multiplier that is used to compute the actual gap between vehicle at each predicted points (not RSS distance) | 0.8 | +#### safety constraints specifically for stopped or parked vehicles + +| Name | Unit | Type | Description | Default value | +| :-------------------------------------------------------- | ------- | ------ | -------------------------------------------------------------------------------------------------------------------------------------------------------------- | ------------- | +| `safety_check.parked.expected_front_deceleration` | [m/s^2] | double | The front object's maximum deceleration when the front vehicle perform sudden braking. (\*1) | -1.0 | +| `safety_check.parked.expected_rear_deceleration` | [m/s^2] | double | The rear object's maximum deceleration when the rear vehicle perform sudden braking. (\*1) | -2.0 | +| `safety_check.parked.rear_vehicle_reaction_time` | [s] | double | The reaction time of the rear vehicle driver which starts from the driver noticing the sudden braking of the front vehicle until the driver step on the brake. | 1.0 | +| `safety_check.parked.rear_vehicle_safety_time_margin` | [s] | double | The time buffer for the rear vehicle to come into complete stop when its driver perform sudden braking. | 0.8 | +| `safety_check.parked.lateral_distance_max_threshold` | [m] | double | The lateral distance threshold that is used to determine whether lateral distance between two object is enough and whether lane change is safe. | 1.0 | +| `safety_check.parked.longitudinal_distance_min_threshold` | [m] | double | The longitudinal distance threshold that is used to determine whether longitudinal distance between two object is enough and whether lane change is safe. | 3.0 | +| `safety_check.parked.longitudinal_velocity_delta_time` | [m] | double | The time multiplier that is used to compute the actual gap between vehicle at each predicted points (not RSS distance) | 0.8 | + ##### safety constraints to cancel lane change path | Name | Unit | Type | Description | Default value | diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml index 77cf9b7b11cbe..7a9c466fec260 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml @@ -38,6 +38,14 @@ lateral_distance_max_threshold: 2.0 longitudinal_distance_min_threshold: 3.0 longitudinal_velocity_delta_time: 0.8 + parked: + expected_front_deceleration: -1.0 + expected_rear_deceleration: -2.0 + rear_vehicle_reaction_time: 1.0 + rear_vehicle_safety_time_margin: 0.8 + lateral_distance_max_threshold: 1.0 + longitudinal_distance_min_threshold: 3.0 + longitudinal_velocity_delta_time: 0.0 cancel: expected_front_deceleration: -1.0 expected_rear_deceleration: -2.0 diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp index 696cf4bd58d32..7962c878d7d64 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp @@ -151,6 +151,7 @@ struct Parameters bool allow_loose_check_for_cancel{true}; double collision_check_yaw_diff_threshold{3.1416}; utils::path_safety_checker::RSSparams rss_params{}; + utils::path_safety_checker::RSSparams rss_params_for_parked{}; utils::path_safety_checker::RSSparams rss_params_for_abort{}; utils::path_safety_checker::RSSparams rss_params_for_stuck{}; diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp index 16c4058bb3911..f3f371ec02a9b 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp @@ -122,6 +122,23 @@ void LaneChangeModuleManager::initParams(rclcpp::Node * node) p.rss_params.lateral_distance_max_threshold = getOrDeclareParameter( *node, parameter("safety_check.execution.lateral_distance_max_threshold")); + p.rss_params_for_parked.longitudinal_distance_min_threshold = getOrDeclareParameter( + *node, parameter("safety_check.parked.longitudinal_distance_min_threshold")); + p.rss_params_for_parked.longitudinal_distance_min_threshold = getOrDeclareParameter( + *node, parameter("safety_check.parked.longitudinal_distance_min_threshold")); + p.rss_params_for_parked.longitudinal_velocity_delta_time = getOrDeclareParameter( + *node, parameter("safety_check.parked.longitudinal_velocity_delta_time")); + p.rss_params_for_parked.front_vehicle_deceleration = getOrDeclareParameter( + *node, parameter("safety_check.parked.expected_front_deceleration")); + p.rss_params_for_parked.rear_vehicle_deceleration = getOrDeclareParameter( + *node, parameter("safety_check.parked.expected_rear_deceleration")); + p.rss_params_for_parked.rear_vehicle_reaction_time = getOrDeclareParameter( + *node, parameter("safety_check.parked.rear_vehicle_reaction_time")); + p.rss_params_for_parked.rear_vehicle_safety_time_margin = getOrDeclareParameter( + *node, parameter("safety_check.parked.rear_vehicle_safety_time_margin")); + p.rss_params_for_parked.lateral_distance_max_threshold = getOrDeclareParameter( + *node, parameter("safety_check.parked.lateral_distance_max_threshold")); + p.rss_params_for_abort.longitudinal_distance_min_threshold = getOrDeclareParameter( *node, parameter("safety_check.cancel.longitudinal_distance_min_threshold")); p.rss_params_for_abort.longitudinal_velocity_delta_time = getOrDeclareParameter( diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp index 5ae01c555b6f2..022cf2c9be7bd 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp @@ -2082,9 +2082,14 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe( const auto obj_predicted_paths = utils::path_safety_checker::getPredictedPathFromObj( obj, lane_change_parameters_->use_all_predicted_path); auto is_safe = true; + const auto selected_rss_param = + (obj.initial_twist.twist.linear.x <= + lane_change_parameters_->prepare_segment_ignore_object_velocity_thresh) + ? lane_change_parameters_->rss_params_for_parked + : rss_params; for (const auto & obj_path : obj_predicted_paths) { const auto collided_polygons = utils::path_safety_checker::getCollidedPolygons( - path, ego_predicted_path, obj, obj_path, common_parameters, rss_params, 1.0, + path, ego_predicted_path, obj, obj_path, common_parameters, selected_rss_param, 1.0, get_max_velocity_for_safety_check(), collision_check_yaw_diff_threshold, current_debug_data.second);