From 21782c354a04fc5141b7c39a647e241d88521e8e Mon Sep 17 00:00:00 2001 From: Daniel Sanchez Date: Tue, 23 Jan 2024 16:54:47 +0900 Subject: [PATCH 01/11] add params for yield Signed-off-by: Daniel Sanchez --- .../obstacle_cruise_planner/obstacle_cruise_planner.param.yaml | 1 + 1 file changed, 1 insertion(+) diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml index 1ac32ba562..e139efc553 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml @@ -90,6 +90,7 @@ collision_time_margin : 4.0 # time threshold of collision between obstacle adn ego for cruise or stop [s] cruise: + allow_yield: true max_lat_margin: 1.0 # lateral margin between obstacle and trajectory band with ego's width outside_obstacle: obstacle_velocity_threshold : 3.5 # minimum velocity threshold of obstacles outside the trajectory to cruise or stop [m/s] From c9e0e388c74815acbf5fe82a91c1481f4ec225a1 Mon Sep 17 00:00:00 2001 From: Daniel Sanchez Date: Wed, 24 Jan 2024 08:45:09 +0900 Subject: [PATCH 02/11] param name change Signed-off-by: Daniel Sanchez --- .../behavior_path_planner/behavior_path_planner.param.yaml | 6 +++--- .../obstacle_cruise_planner.param.yaml | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/behavior_path_planner.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/behavior_path_planner.param.yaml index 8d93a95b37..e347d36d57 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/behavior_path_planner.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/behavior_path_planner.param.yaml @@ -6,10 +6,10 @@ traffic_light_signal_timeout: 1.0 planning_hz: 10.0 - backward_path_length: 5.0 + backward_path_length: 0.5 forward_path_length: 300.0 - backward_length_buffer_for_end_of_pull_over: 5.0 - backward_length_buffer_for_end_of_pull_out: 5.0 + backward_length_buffer_for_end_of_pull_over: 0.5 + backward_length_buffer_for_end_of_pull_out: 0.5 minimum_pull_over_length: 16.0 refine_goal_search_radius_range: 7.5 diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml index e139efc553..a052785fd4 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml @@ -90,7 +90,7 @@ collision_time_margin : 4.0 # time threshold of collision between obstacle adn ego for cruise or stop [s] cruise: - allow_yield: true + enable_yield: true max_lat_margin: 1.0 # lateral margin between obstacle and trajectory band with ego's width outside_obstacle: obstacle_velocity_threshold : 3.5 # minimum velocity threshold of obstacles outside the trajectory to cruise or stop [m/s] From 1263ff419047264bae89eb23f76e72cef255339c Mon Sep 17 00:00:00 2001 From: Daniel Sanchez Date: Wed, 24 Jan 2024 18:27:30 +0900 Subject: [PATCH 03/11] add params Signed-off-by: Daniel Sanchez --- .../obstacle_cruise_planner/obstacle_cruise_planner.param.yaml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml index a052785fd4..77fa4dc91f 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml @@ -91,6 +91,8 @@ cruise: enable_yield: true + yield_lat_distance_threshold: 4.0 # lateral margin between obstacle in neighbor lanes and trajectory band with ego's width for yielding + max_lat_dist_between_obstacles: 1.5 # lateral margin between moving obstacle in neighbor lanes and stopped obstacle in front of it max_lat_margin: 1.0 # lateral margin between obstacle and trajectory band with ego's width outside_obstacle: obstacle_velocity_threshold : 3.5 # minimum velocity threshold of obstacles outside the trajectory to cruise or stop [m/s] From c31c5a02c9e7b63eab9efbfb2f913148757c0418 Mon Sep 17 00:00:00 2001 From: Daniel Sanchez Date: Thu, 25 Jan 2024 13:18:54 +0900 Subject: [PATCH 04/11] refactoring Signed-off-by: Daniel Sanchez --- .../obstacle_cruise_planner.param.yaml | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml index 77fa4dc91f..65b310decb 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml @@ -90,15 +90,17 @@ collision_time_margin : 4.0 # time threshold of collision between obstacle adn ego for cruise or stop [s] cruise: - enable_yield: true - yield_lat_distance_threshold: 4.0 # lateral margin between obstacle in neighbor lanes and trajectory band with ego's width for yielding - max_lat_dist_between_obstacles: 1.5 # lateral margin between moving obstacle in neighbor lanes and stopped obstacle in front of it max_lat_margin: 1.0 # lateral margin between obstacle and trajectory band with ego's width outside_obstacle: obstacle_velocity_threshold : 3.5 # minimum velocity threshold of obstacles outside the trajectory to cruise or stop [m/s] ego_obstacle_overlap_time_threshold : 2.0 # time threshold to decide cut-in obstacle for cruise or stop [s] max_prediction_time_for_collision_check : 20.0 # prediction time to check collision between obstacle and ego - + yield: + enable_yield: true + lat_distance_threshold: 4.0 # lateral margin between obstacle in neighbor lanes and trajectory band with ego's width for yielding + max_lat_dist_between_obstacles: 1.5 # lateral margin between moving obstacle in neighbor lanes and stopped obstacle in front of it + max_moving_obstacle_long_distance: 50.0 # how far the blocking obstacle + max_long_distance_between obstacles: 50.0 # only consider to yield when the two obstacles are at most this distance apart slow_down: max_lat_margin: 1.1 # lateral margin between obstacle and trajectory band with ego's width lat_hysteresis_margin: 0.2 From 0c603710fc42439dbfea22049088172e597b583b Mon Sep 17 00:00:00 2001 From: Daniel Sanchez Date: Thu, 25 Jan 2024 14:28:45 +0900 Subject: [PATCH 05/11] fix typo, tuning Signed-off-by: Daniel Sanchez --- .../obstacle_cruise_planner.param.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml index 65b310decb..8bb999e7cb 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml @@ -98,9 +98,9 @@ yield: enable_yield: true lat_distance_threshold: 4.0 # lateral margin between obstacle in neighbor lanes and trajectory band with ego's width for yielding - max_lat_dist_between_obstacles: 1.5 # lateral margin between moving obstacle in neighbor lanes and stopped obstacle in front of it + max_lat_dist_between_obstacles: 2.0 # lateral margin between moving obstacle in neighbor lanes and stopped obstacle in front of it max_moving_obstacle_long_distance: 50.0 # how far the blocking obstacle - max_long_distance_between obstacles: 50.0 # only consider to yield when the two obstacles are at most this distance apart + max_long_distance_between_obstacles: 50.0 # only consider to yield when the two obstacles are at most this distance apart slow_down: max_lat_margin: 1.1 # lateral margin between obstacle and trajectory band with ego's width lat_hysteresis_margin: 0.2 From 171deb5a73ee8cde9752d23a90c4375613671f15 Mon Sep 17 00:00:00 2001 From: Daniel Sanchez Date: Thu, 25 Jan 2024 16:45:18 +0900 Subject: [PATCH 06/11] update parameters Signed-off-by: Daniel Sanchez --- .../obstacle_cruise_planner/obstacle_cruise_planner.param.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml index 8bb999e7cb..f858e4b9a5 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml @@ -99,7 +99,7 @@ enable_yield: true lat_distance_threshold: 4.0 # lateral margin between obstacle in neighbor lanes and trajectory band with ego's width for yielding max_lat_dist_between_obstacles: 2.0 # lateral margin between moving obstacle in neighbor lanes and stopped obstacle in front of it - max_moving_obstacle_long_distance: 50.0 # how far the blocking obstacle + max_obstacles_collision_time: 10.0 # how far the blocking obstacle max_long_distance_between_obstacles: 50.0 # only consider to yield when the two obstacles are at most this distance apart slow_down: max_lat_margin: 1.1 # lateral margin between obstacle and trajectory band with ego's width From b86373e9dacb9ab51c6568e757b11d8b9a579f75 Mon Sep 17 00:00:00 2001 From: Daniel Sanchez Date: Thu, 25 Jan 2024 16:46:57 +0900 Subject: [PATCH 07/11] delete unused param Signed-off-by: Daniel Sanchez --- .../obstacle_cruise_planner/obstacle_cruise_planner.param.yaml | 1 - 1 file changed, 1 deletion(-) diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml index f858e4b9a5..80323c2d73 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml @@ -100,7 +100,6 @@ lat_distance_threshold: 4.0 # lateral margin between obstacle in neighbor lanes and trajectory band with ego's width for yielding max_lat_dist_between_obstacles: 2.0 # lateral margin between moving obstacle in neighbor lanes and stopped obstacle in front of it max_obstacles_collision_time: 10.0 # how far the blocking obstacle - max_long_distance_between_obstacles: 50.0 # only consider to yield when the two obstacles are at most this distance apart slow_down: max_lat_margin: 1.1 # lateral margin between obstacle and trajectory band with ego's width lat_hysteresis_margin: 0.2 From ee43e66ca4700223a11780448ddbbb78cdb9a476 Mon Sep 17 00:00:00 2001 From: Daniel Sanchez Date: Mon, 29 Jan 2024 12:59:45 +0900 Subject: [PATCH 08/11] set cruise planner as default for testing Signed-off-by: Daniel Sanchez --- .../obstacle_cruise_planner.param.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml index 80323c2d73..f81f38a7be 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml @@ -97,8 +97,8 @@ max_prediction_time_for_collision_check : 20.0 # prediction time to check collision between obstacle and ego yield: enable_yield: true - lat_distance_threshold: 4.0 # lateral margin between obstacle in neighbor lanes and trajectory band with ego's width for yielding - max_lat_dist_between_obstacles: 2.0 # lateral margin between moving obstacle in neighbor lanes and stopped obstacle in front of it + lat_distance_threshold: 5.0 # lateral margin between obstacle in neighbor lanes and trajectory band with ego's width for yielding + max_lat_dist_between_obstacles: 2.5 # lateral margin between moving obstacle in neighbor lanes and stopped obstacle in front of it max_obstacles_collision_time: 10.0 # how far the blocking obstacle slow_down: max_lat_margin: 1.1 # lateral margin between obstacle and trajectory band with ego's width From 497b3091affc78d45bf986f8398767b582735d08 Mon Sep 17 00:00:00 2001 From: Daniel Sanchez Date: Tue, 30 Jan 2024 13:24:07 +0900 Subject: [PATCH 09/11] add param for stopped obj speed threshold Signed-off-by: Daniel Sanchez --- .../obstacle_cruise_planner/obstacle_cruise_planner.param.yaml | 1 + 1 file changed, 1 insertion(+) diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml index f81f38a7be..e9156441a4 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml @@ -100,6 +100,7 @@ lat_distance_threshold: 5.0 # lateral margin between obstacle in neighbor lanes and trajectory band with ego's width for yielding max_lat_dist_between_obstacles: 2.5 # lateral margin between moving obstacle in neighbor lanes and stopped obstacle in front of it max_obstacles_collision_time: 10.0 # how far the blocking obstacle + stopped_obstacle_velocity_threshold: 0.5 slow_down: max_lat_margin: 1.1 # lateral margin between obstacle and trajectory band with ego's width lat_hysteresis_margin: 0.2 From 651d755d602d29b0c194b68bff234a6595bdac41 Mon Sep 17 00:00:00 2001 From: Daniel Sanchez Date: Wed, 31 Jan 2024 09:38:48 +0900 Subject: [PATCH 10/11] change back param Signed-off-by: Daniel Sanchez --- .../behavior_path_planner/behavior_path_planner.param.yaml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/behavior_path_planner.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/behavior_path_planner.param.yaml index e347d36d57..8d93a95b37 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/behavior_path_planner.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/behavior_path_planner.param.yaml @@ -6,10 +6,10 @@ traffic_light_signal_timeout: 1.0 planning_hz: 10.0 - backward_path_length: 0.5 + backward_path_length: 5.0 forward_path_length: 300.0 - backward_length_buffer_for_end_of_pull_over: 0.5 - backward_length_buffer_for_end_of_pull_out: 0.5 + backward_length_buffer_for_end_of_pull_over: 5.0 + backward_length_buffer_for_end_of_pull_out: 5.0 minimum_pull_over_length: 16.0 refine_goal_search_radius_range: 7.5 From 843db4a9692ed0713febc5c4dc92de0184b7281d Mon Sep 17 00:00:00 2001 From: Daniel Sanchez Date: Wed, 31 Jan 2024 09:39:46 +0900 Subject: [PATCH 11/11] set default false Signed-off-by: Daniel Sanchez --- .../obstacle_cruise_planner/obstacle_cruise_planner.param.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml index e9156441a4..2ff87e9b01 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml @@ -96,7 +96,7 @@ ego_obstacle_overlap_time_threshold : 2.0 # time threshold to decide cut-in obstacle for cruise or stop [s] max_prediction_time_for_collision_check : 20.0 # prediction time to check collision between obstacle and ego yield: - enable_yield: true + enable_yield: false lat_distance_threshold: 5.0 # lateral margin between obstacle in neighbor lanes and trajectory band with ego's width for yielding max_lat_dist_between_obstacles: 2.5 # lateral margin between moving obstacle in neighbor lanes and stopped obstacle in front of it max_obstacles_collision_time: 10.0 # how far the blocking obstacle