From 4e94d652916525fccc1a9d505b5db2594767d9f5 Mon Sep 17 00:00:00 2001 From: danielsanchezaran Date: Tue, 6 Feb 2024 13:40:08 +0900 Subject: [PATCH] feat(behavior_path_sampling_planner): add sampling based planner to behavior path planner (#810) * Add sampling based planner params Signed-off-by: Daniel Sanchez * update keep_last param Signed-off-by: Daniel Sanchez * change priority of sampling based planner Signed-off-by: Daniel Sanchez * Set parameters for frenet planner Signed-off-by: Daniel Sanchez * changes for testing Signed-off-by: Daniel Sanchez * change curvature weight for testing Signed-off-by: Daniel Sanchez * tuning params Signed-off-by: Daniel Sanchez * tuning Signed-off-by: Daniel Sanchez * for integ w/ other modules Signed-off-by: Daniel Sanchez * add support for soft constraints weight reconfig Signed-off-by: Daniel Sanchez * rebase Signed-off-by: Daniel Sanchez * temp Signed-off-by: Daniel Sanchez * update default params Signed-off-by: Daniel Sanchez * Tune params Signed-off-by: Daniel Sanchez * Set defaults back to normal Signed-off-by: Daniel Sanchez * fix name of ablc Signed-off-by: Daniel Sanchez * formatting fix Signed-off-by: Daniel Sanchez * set verbose to false Signed-off-by: Daniel Sanchez --------- Signed-off-by: Daniel Sanchez Signed-off-by: Daniel Sanchez --- .../planning/preset/default_preset.yaml | 3 ++ .../sampling_planner.param.yaml | 40 +++++++++++++++++++ .../scene_module_manager.param.yaml | 9 +++++ .../tier4_planning_component.launch.xml | 1 + 4 files changed, 53 insertions(+) create mode 100644 autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/sampling_planner/sampling_planner.param.yaml diff --git a/autoware_launch/config/planning/preset/default_preset.yaml b/autoware_launch/config/planning/preset/default_preset.yaml index 3512d722ec..69328a9b79 100644 --- a/autoware_launch/config/planning/preset/default_preset.yaml +++ b/autoware_launch/config/planning/preset/default_preset.yaml @@ -9,6 +9,9 @@ launch: - arg: name: launch_dynamic_avoidance_module default: "false" + - arg: + name: launch_sampling_planner_module + default: "false" # Warning, experimental module, use only in simulations - arg: name: launch_lane_change_right_module default: "true" diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/sampling_planner/sampling_planner.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/sampling_planner/sampling_planner.param.yaml new file mode 100644 index 0000000000..9a6b30c219 --- /dev/null +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/sampling_planner/sampling_planner.param.yaml @@ -0,0 +1,40 @@ +/**: + ros__parameters: + common: + output_delta_arc_length: 0.5 # [m] delta arc length for output trajectory + + debug: + enable_calculation_time_info: false # flag to print calculation times + id: 0 # id of the candidate paths for which to print/show details (e.g., footprint in rviz) + + preprocessing: + force_zero_initial_deviation: True # if true, initial planning starts from the reference path + force_zero_initial_heading: True # if true, initial planning starts with a heading aligned with the reference path + smooth_reference_trajectory: False # if true, the reference trajectory is smoothed before being used for planning + constraints: + hard: + max_curvature: 3.0 # [m⁻¹] maximum curvature of a sampled path + min_curvature: -3.0 # [m⁻¹] minimum curvature of a sampled path + soft: + lateral_deviation_weight: 1.0 # cost weight for lateral deviation between the end of a sampled path and the reference path + length_weight: 1.0 # cost weight for the length of a sampled path + curvature_weight: 2000.0 # cost weight for the curvature of a sampled path + weights: [0.5,1.0,20.0] + sampling: + enable_frenet: True + enable_bezier: False + resolution: 0.5 # [m] target distance between sampled path points + previous_path_reuse_points_nb: 2 # number of points reused from the previously generated path (0:no reuse, 1:replan from end of prev path, 2: end and mid of prev path, etc) + target_lengths: [20.0, 40.0] # [m] target lengths of the sampled paths + nb_target_lateral_positions: 0 # number of lateral positions to use for sampling (in addition to 0.0 and the current lateral deviation) + target_lateral_positions: [-4.5,-2.5, 0.0, 2.5,4.5] # manual values that are only used if nb_target_lateral_positions = 0 + frenet: # target values for the sampled "lateral deviation over longitudinal position" polynomial + target_lateral_velocities: [-0.5, 0.0, 0.5] + target_lateral_accelerations: [0.0] + # bezier: + # nb_k: 3 # number of sampled curvature values + # mk_min: 0.0 # minimum curvature value + # mk_max: 10.0 # maximum curvature value + # nb_t: 5 # number of sampled acceleration values + # mt_min: 0.3 # minimum acceleration value + # mt_max: 1.7 # maximum acceleration value diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/scene_module_manager.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/scene_module_manager.param.yaml index 9853e220bc..8d4fe057b4 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/scene_module_manager.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/scene_module_manager.param.yaml @@ -82,3 +82,12 @@ keep_last: true priority: 7 max_module_size: 1 + + sampling_planner: + enable_module: true + enable_rtc: false + enable_simultaneous_execution_as_approved_module: false + enable_simultaneous_execution_as_candidate_module: false + keep_last: false + priority: 16 + max_module_size: 1 diff --git a/autoware_launch/launch/components/tier4_planning_component.launch.xml b/autoware_launch/launch/components/tier4_planning_component.launch.xml index 4e65c30c02..cbc2349a6e 100644 --- a/autoware_launch/launch/components/tier4_planning_component.launch.xml +++ b/autoware_launch/launch/components/tier4_planning_component.launch.xml @@ -27,6 +27,7 @@ +