From d9284f007bbd5747a48cb1aba0c6efcbdf307c0a Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Sun, 20 Aug 2023 10:30:40 +0900 Subject: [PATCH 1/4] feat(start_planner): support freespace pull out (#514) Signed-off-by: kosuke55 --- .../start_planner/start_planner.param.yaml | 36 +++++++++++++++++++ 1 file changed, 36 insertions(+) diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/start_planner/start_planner.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/start_planner/start_planner.param.yaml index 60f2927440..bea78664c6 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/start_planner/start_planner.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/start_planner/start_planner.param.yaml @@ -36,3 +36,39 @@ th_turn_signal_on_lateral_offset: 1.0 intersection_search_length: 30.0 length_ratio_for_turn_signal_deactivation_near_intersection: 0.5 + # freespace planner + freespace_planner: + enable_freespace_planner: true + end_pose_search_start_distance: 20.0 + end_pose_search_end_distance: 30.0 + end_pose_search_interval: 2.0 + freespace_planner_algorithm: "astar" # options: astar, rrtstar + velocity: 1.0 + vehicle_shape_margin: 1.0 + time_limit: 3000.0 + minimum_turning_radius: 5.0 + maximum_turning_radius: 5.0 + turning_radius_size: 1 + # search configs + search_configs: + theta_size: 144 + angle_goal_range: 6.0 + curve_weight: 1.2 + reverse_weight: 1.0 + lateral_goal_range: 0.5 + longitudinal_goal_range: 2.0 + # costmap configs + costmap_configs: + obstacle_threshold: 30 + # -- A* search Configurations -- + astar: + only_behind_solutions: false + use_back: false + distance_heuristic_weight: 1.0 + # -- RRT* search Configurations -- + rrtstar: + enable_update: true + use_informed_sampling: true + max_planning_time: 150.0 + neighbor_radius: 8.0 + margin: 1.0 From 5146002a24126974a44ad9df5e7559b7607615ea Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Tue, 22 Aug 2023 19:49:42 +0900 Subject: [PATCH 2/4] feat(goal_planner): add extra front margin for collision check considering stopping distance (#520) * feat(goal_planner): add extra front margin for collision check considering stopping distance Signed-off-by: kosuke55 * object_recognition_collision_check_margin: 0.6 Signed-off-by: kosuke55 * rename args and params Signed-off-by: kosuke55 * add comments Signed-off-by: kosuke55 --------- Signed-off-by: kosuke55 --- .../behavior_path_planner/goal_planner/goal_planner.param.yaml | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/goal_planner/goal_planner.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/goal_planner/goal_planner.param.yaml index eaa103bff7..cd2bc6168d 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/goal_planner/goal_planner.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/goal_planner/goal_planner.param.yaml @@ -31,7 +31,8 @@ # object recognition object_recognition: use_object_recognition: true - object_recognition_collision_check_margin: 1.0 + object_recognition_collision_check_margin: 0.6 # this should be larger than `surround_check_distance` of surround_obstacle_checker + object_recognition_collision_check_max_extra_stopping_margin: 1.0 # pull over pull_over: From b4262b63c2881cda1e6279b3971e9fb2a6065875 Mon Sep 17 00:00:00 2001 From: kosuke55 Date: Mon, 4 Sep 2023 21:38:51 +0900 Subject: [PATCH 3/4] update params Signed-off-by: kosuke55 --- .../behavior_path_planner/goal_planner/goal_planner.param.yaml | 2 +- .../start_planner/start_planner.param.yaml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/goal_planner/goal_planner.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/goal_planner/goal_planner.param.yaml index cd2bc6168d..fbf05e9828 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/goal_planner/goal_planner.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/goal_planner/goal_planner.param.yaml @@ -11,7 +11,7 @@ goal_search: search_priority: "efficient_path" # "efficient_path" or "close_goal" parking_policy: "left_side" # "left_side" or "right_side" - forward_goal_search_length: 20.0 + forward_goal_search_length: 30.0 backward_goal_search_length: 20.0 goal_search_interval: 2.0 longitudinal_margin: 3.0 diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/start_planner/start_planner.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/start_planner/start_planner.param.yaml index bea78664c6..313f80ca5c 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/start_planner/start_planner.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/start_planner/start_planner.param.yaml @@ -28,7 +28,7 @@ # search start pose backward enable_back: true search_priority: "efficient_path" # "efficient_path" or "short_back_distance" - max_back_distance: 30.0 + max_back_distance: 20.0 backward_search_resolution: 2.0 backward_path_update_duration: 3.0 ignore_distance_from_lane_end: 15.0 From 4dc92ece822cac10c5ff91d44dd53d4b1eef8053 Mon Sep 17 00:00:00 2001 From: kosuke55 Date: Mon, 4 Sep 2023 21:39:26 +0900 Subject: [PATCH 4/4] disable freespace start planner Signed-off-by: kosuke55 --- .../start_planner/start_planner.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/start_planner/start_planner.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/start_planner/start_planner.param.yaml index 313f80ca5c..e22be27ec5 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/start_planner/start_planner.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/start_planner/start_planner.param.yaml @@ -38,7 +38,7 @@ length_ratio_for_turn_signal_deactivation_near_intersection: 0.5 # freespace planner freespace_planner: - enable_freespace_planner: true + enable_freespace_planner: false end_pose_search_start_distance: 20.0 end_pose_search_end_distance: 30.0 end_pose_search_interval: 2.0