From b3e2d9e7653ed6415ec6859355d24e5ed0bf62ae Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Thu, 7 Mar 2024 10:17:50 +0900 Subject: [PATCH 1/4] feat(avoidance): change lateral margin based on if it's parked vehicle (#894) * feat(avoidance): change lateral margin based on if it's parked vehicle Signed-off-by: satoshi-ota * fix(AbLC): update values Signed-off-by: satoshi-ota --------- Signed-off-by: satoshi-ota --- .../avoidance/avoidance.param.yaml | 48 ++++++++++++------- .../avoidance_by_lane_change.param.yaml | 48 ++++++++++++------- 2 files changed, 64 insertions(+), 32 deletions(-) diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml index a40bb39001..a0b55581b8 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml @@ -31,80 +31,96 @@ execute_num: 1 # [-] moving_speed_threshold: 1.0 # [m/s] moving_time_threshold: 2.0 # [s] + lateral_margin: + soft_margin: 0.7 # [m] + hard_margin: 0.3 # [m] + hard_margin_for_parked_vehicle: 0.3 # [m] max_expand_ratio: 0.0 # [-] envelope_buffer_margin: 0.5 # [m] - avoid_margin_lateral: 0.7 # [m] - safety_buffer_lateral: 0.3 # [m] safety_buffer_longitudinal: 0.0 # [m] use_conservative_buffer_longitudinal: true # [-] When set to true, the base_link2front is added to the longitudinal buffer before avoidance. truck: execute_num: 1 moving_speed_threshold: 1.0 # 3.6km/h moving_time_threshold: 2.0 + lateral_margin: + soft_margin: 0.9 # [m] + hard_margin: 0.1 # [m] + hard_margin_for_parked_vehicle: 0.1 # [m] max_expand_ratio: 0.0 envelope_buffer_margin: 0.5 - avoid_margin_lateral: 0.9 - safety_buffer_lateral: 0.1 safety_buffer_longitudinal: 0.0 use_conservative_buffer_longitudinal: true bus: execute_num: 1 moving_speed_threshold: 1.0 # 3.6km/h moving_time_threshold: 2.0 + lateral_margin: + soft_margin: 0.9 # [m] + hard_margin: 0.1 # [m] + hard_margin_for_parked_vehicle: 0.1 # [m] max_expand_ratio: 0.0 envelope_buffer_margin: 0.5 - avoid_margin_lateral: 0.9 - safety_buffer_lateral: 0.1 safety_buffer_longitudinal: 0.0 use_conservative_buffer_longitudinal: true trailer: execute_num: 1 moving_speed_threshold: 1.0 # 3.6km/h moving_time_threshold: 2.0 + lateral_margin: + soft_margin: 0.9 # [m] + hard_margin: 0.1 # [m] + hard_margin_for_parked_vehicle: 0.1 # [m] max_expand_ratio: 0.0 envelope_buffer_margin: 0.5 - avoid_margin_lateral: 0.9 - safety_buffer_lateral: 0.1 safety_buffer_longitudinal: 0.0 use_conservative_buffer_longitudinal: true unknown: execute_num: 1 moving_speed_threshold: 0.28 # 1.0km/h moving_time_threshold: 1.0 + lateral_margin: + soft_margin: 0.7 # [m] + hard_margin: -0.2 # [m] + hard_margin_for_parked_vehicle: -0.2 # [m] max_expand_ratio: 0.0 envelope_buffer_margin: 0.1 - avoid_margin_lateral: 0.7 - safety_buffer_lateral: -0.2 safety_buffer_longitudinal: 0.0 use_conservative_buffer_longitudinal: true bicycle: execute_num: 1 moving_speed_threshold: 0.28 # 1.0km/h moving_time_threshold: 1.0 + lateral_margin: + soft_margin: 0.7 # [m] + hard_margin: 0.5 # [m] + hard_margin_for_parked_vehicle: 0.5 # [m] max_expand_ratio: 0.0 envelope_buffer_margin: 0.5 - avoid_margin_lateral: 0.7 - safety_buffer_lateral: 0.5 safety_buffer_longitudinal: 1.0 use_conservative_buffer_longitudinal: true motorcycle: execute_num: 1 moving_speed_threshold: 1.0 # 3.6km/h moving_time_threshold: 1.0 + lateral_margin: + soft_margin: 0.7 # [m] + hard_margin: 0.3 # [m] + hard_margin_for_parked_vehicle: 0.3 # [m] max_expand_ratio: 0.0 envelope_buffer_margin: 0.5 - avoid_margin_lateral: 0.7 - safety_buffer_lateral: 0.3 safety_buffer_longitudinal: 1.0 use_conservative_buffer_longitudinal: true pedestrian: execute_num: 1 moving_speed_threshold: 0.28 # 1.0km/h moving_time_threshold: 1.0 + lateral_margin: + soft_margin: 0.7 # [m] + hard_margin: 0.5 # [m] + hard_margin_for_parked_vehicle: 0.5 # [m] max_expand_ratio: 0.0 envelope_buffer_margin: 0.5 - avoid_margin_lateral: 0.7 - safety_buffer_lateral: 0.5 safety_buffer_longitudinal: 1.0 use_conservative_buffer_longitudinal: true lower_distance_for_polygon_expansion: 30.0 # [m] diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance_by_lane_change/avoidance_by_lane_change.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance_by_lane_change/avoidance_by_lane_change.param.yaml index a5c1cbf23a..ea772d67fd 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance_by_lane_change/avoidance_by_lane_change.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance_by_lane_change/avoidance_by_lane_change.param.yaml @@ -12,32 +12,40 @@ moving_time_threshold: 1.0 # [s] max_expand_ratio: 0.0 # [-] envelope_buffer_margin: 0.3 # [m] - avoid_margin_lateral: 0.0 # [m] - safety_buffer_lateral: 0.0 # [m] + lateral_margin: + soft_margin: 0.0 # [m] + hard_margin: 0.0 # [m] + hard_margin_for_parked_vehicle: 0.0 # [m] truck: execute_num: 2 moving_speed_threshold: 1.0 # 3.6km/h moving_time_threshold: 1.0 max_expand_ratio: 0.0 envelope_buffer_margin: 0.3 - avoid_margin_lateral: 0.0 - safety_buffer_lateral: 0.0 + lateral_margin: + soft_margin: 0.0 # [m] + hard_margin: 0.0 # [m] + hard_margin_for_parked_vehicle: 0.0 # [m] bus: execute_num: 2 moving_speed_threshold: 1.0 # 3.6km/h moving_time_threshold: 1.0 max_expand_ratio: 0.0 envelope_buffer_margin: 0.3 - avoid_margin_lateral: 0.0 - safety_buffer_lateral: 0.0 + lateral_margin: + soft_margin: 0.0 # [m] + hard_margin: 0.0 # [m] + hard_margin_for_parked_vehicle: 0.0 # [m] trailer: execute_num: 2 moving_speed_threshold: 1.0 # 3.6km/h moving_time_threshold: 1.0 max_expand_ratio: 0.0 envelope_buffer_margin: 0.3 - avoid_margin_lateral: 0.0 - safety_buffer_lateral: 0.0 + lateral_margin: + soft_margin: 0.0 # [m] + hard_margin: 0.0 # [m] + hard_margin_for_parked_vehicle: 0.0 # [m] unknown: is_target: false execute_num: 1 @@ -45,32 +53,40 @@ moving_time_threshold: 1.0 max_expand_ratio: 0.0 envelope_buffer_margin: 0.3 - avoid_margin_lateral: 0.0 - safety_buffer_lateral: 0.0 + lateral_margin: + soft_margin: 0.0 # [m] + hard_margin: 0.0 # [m] + hard_margin_for_parked_vehicle: 0.0 # [m] bicycle: execute_num: 2 moving_speed_threshold: 0.28 # 1.0km/h moving_time_threshold: 1.0 max_expand_ratio: 0.0 envelope_buffer_margin: 0.8 - avoid_margin_lateral: 0.0 - safety_buffer_lateral: 1.0 + lateral_margin: + soft_margin: 0.0 # [m] + hard_margin: 1.0 # [m] + hard_margin_for_parked_vehicle: 1.0 # [m] motorcycle: execute_num: 2 moving_speed_threshold: 1.0 # 3.6km/h moving_time_threshold: 1.0 max_expand_ratio: 0.0 envelope_buffer_margin: 0.8 - avoid_margin_lateral: 0.0 - safety_buffer_lateral: 1.0 + lateral_margin: + soft_margin: 0.0 # [m] + hard_margin: 1.0 # [m] + hard_margin_for_parked_vehicle: 1.0 # [m] pedestrian: execute_num: 2 moving_speed_threshold: 0.28 # 1.0km/h moving_time_threshold: 1.0 max_expand_ratio: 0.0 envelope_buffer_margin: 0.8 - avoid_margin_lateral: 0.0 - safety_buffer_lateral: 1.0 + lateral_margin: + soft_margin: 0.0 # [m] + hard_margin: 1.0 # [m] + hard_margin_for_parked_vehicle: 1.0 # [m] lower_distance_for_polygon_expansion: 0.0 # [m] upper_distance_for_polygon_expansion: 1.0 # [m] From da094bb278ace90dc452d89cedc744a3e179c531 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Mon, 11 Mar 2024 16:52:37 +0900 Subject: [PATCH 2/4] fix(avoidance): tune lateral margin params (#913) Signed-off-by: satoshi-ota --- .../avoidance/avoidance.param.yaml | 24 +++++++++---------- 1 file changed, 12 insertions(+), 12 deletions(-) diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml index a0b55581b8..4f685f4304 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml @@ -32,9 +32,9 @@ moving_speed_threshold: 1.0 # [m/s] moving_time_threshold: 2.0 # [s] lateral_margin: - soft_margin: 0.7 # [m] - hard_margin: 0.3 # [m] - hard_margin_for_parked_vehicle: 0.3 # [m] + soft_margin: 0.3 # [m] + hard_margin: 0.2 # [m] + hard_margin_for_parked_vehicle: 0.7 # [m] max_expand_ratio: 0.0 # [-] envelope_buffer_margin: 0.5 # [m] safety_buffer_longitudinal: 0.0 # [m] @@ -44,9 +44,9 @@ moving_speed_threshold: 1.0 # 3.6km/h moving_time_threshold: 2.0 lateral_margin: - soft_margin: 0.9 # [m] - hard_margin: 0.1 # [m] - hard_margin_for_parked_vehicle: 0.1 # [m] + soft_margin: 0.3 # [m] + hard_margin: 0.2 # [m] + hard_margin_for_parked_vehicle: 0.7 # [m] max_expand_ratio: 0.0 envelope_buffer_margin: 0.5 safety_buffer_longitudinal: 0.0 @@ -56,9 +56,9 @@ moving_speed_threshold: 1.0 # 3.6km/h moving_time_threshold: 2.0 lateral_margin: - soft_margin: 0.9 # [m] - hard_margin: 0.1 # [m] - hard_margin_for_parked_vehicle: 0.1 # [m] + soft_margin: 0.3 # [m] + hard_margin: 0.2 # [m] + hard_margin_for_parked_vehicle: 0.7 # [m] max_expand_ratio: 0.0 envelope_buffer_margin: 0.5 safety_buffer_longitudinal: 0.0 @@ -68,9 +68,9 @@ moving_speed_threshold: 1.0 # 3.6km/h moving_time_threshold: 2.0 lateral_margin: - soft_margin: 0.9 # [m] - hard_margin: 0.1 # [m] - hard_margin_for_parked_vehicle: 0.1 # [m] + soft_margin: 0.3 # [m] + hard_margin: 0.2 # [m] + hard_margin_for_parked_vehicle: 0.7 # [m] max_expand_ratio: 0.0 envelope_buffer_margin: 0.5 safety_buffer_longitudinal: 0.0 From 81060f71387254509de3e9219fca72daeabf9942 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Tue, 12 Mar 2024 08:47:06 +0900 Subject: [PATCH 3/4] fix(avoidance): tune safety check params (#914) Signed-off-by: satoshi-ota --- .../behavior_path_planner/avoidance/avoidance.param.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml index 4f685f4304..d5258c5250 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml @@ -194,7 +194,7 @@ check_all_predicted_path: false # [-] safety_check_backward_distance: 100.0 # [m] hysteresis_factor_expand_rate: 1.5 # [-] - hysteresis_factor_safe_count: 10 # [-] + hysteresis_factor_safe_count: 3 # [-] # predicted path parameters min_velocity: 1.38 # [m/s] max_velocity: 50.0 # [m/s] @@ -209,7 +209,7 @@ rear_vehicle_safety_time_margin: 1.0 # [s] lateral_distance_max_threshold: 2.0 # [m] longitudinal_distance_min_threshold: 3.0 # [m] - longitudinal_velocity_delta_time: 0.8 # [s] + longitudinal_velocity_delta_time: 0.0 # [s] # For avoidance maneuver avoidance: From c4aa60000a0a4d09dbf56ab4a08281d131d2b23b Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Fri, 15 Mar 2024 11:01:02 +0900 Subject: [PATCH 4/4] refactor(avoidance): rename param (#923) Signed-off-by: satoshi-ota --- .../behavior_path_planner/avoidance/avoidance.param.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml index d5258c5250..f8b5a5fb5a 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml @@ -156,7 +156,7 @@ backward_distance: 10.0 # [m] # params for avoidance of vehicle type objects that are ambiguous as to whether they are parked. - force_avoidance: + avoidance_for_ambiguous_vehicle: enable: true # [-] time_threshold: 3.0 # [s] distance_threshold: 1.0 # [m]