feat(behavior_path_sampling_planner): add sampling based planner to b…

…ehavior path planner (autowarefoundation#810) * Add sampling based planner params Signed-off-by: Daniel Sanchez <[email protected]> * update keep_last param Signed-off-by: Daniel Sanchez <[email protected]> * change priority of sampling based planner Signed-off-by: Daniel Sanchez <[email protected]> * Set parameters for frenet planner Signed-off-by: Daniel Sanchez <[email protected]> * changes for testing Signed-off-by: Daniel Sanchez <[email protected]> * change curvature weight for testing Signed-off-by: Daniel Sanchez <[email protected]> * tuning params Signed-off-by: Daniel Sanchez <[email protected]> * tuning Signed-off-by: Daniel Sanchez <[email protected]> * for integ w/ other modules Signed-off-by: Daniel Sanchez <[email protected]> * add support for soft constraints weight reconfig Signed-off-by: Daniel Sanchez <[email protected]> * rebase Signed-off-by: Daniel Sanchez <[email protected]> * temp Signed-off-by: Daniel Sanchez <[email protected]> * update default params Signed-off-by: Daniel Sanchez <[email protected]> * Tune params Signed-off-by: Daniel Sanchez <[email protected]> * Set defaults back to normal Signed-off-by: Daniel Sanchez <[email protected]> * fix name of ablc Signed-off-by: Daniel Sanchez <[email protected]> * formatting fix Signed-off-by: Daniel Sanchez <[email protected]> * set verbose to false Signed-off-by: Daniel Sanchez <[email protected]> --------- Signed-off-by: Daniel Sanchez <[email protected]> Signed-off-by: Daniel Sanchez <[email protected]>
tier4 · Feb 6, 2024 · 4e94d65 · 4e94d65
1 parent 4357093
commit 4e94d65
Show file tree

Hide file tree

Showing 4 changed files with 53 additions and 0 deletions.
diff --git 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/...ving/behavior_planning/behavior_path_planner/sampling_planner/sampling_planner.param.yaml b/...ving/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/...ning/lane_driving/behavior_planning/behavior_path_planner/scene_module_manager.param.yaml b/...ning/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
@@ -27,6 +27,7 @@
     <arg name="behavior_path_planner_side_shift_module_param_path" value="$(var behavior_path_config_path)/side_shift/side_shift.param.yaml"/>
     <arg name="behavior_path_planner_avoidance_module_param_path" value="$(var behavior_path_config_path)/avoidance/avoidance.param.yaml"/>
     <arg name="behavior_path_planner_avoidance_by_lc_module_param_path" value="$(var behavior_path_config_path)/avoidance_by_lane_change/avoidance_by_lane_change.param.yaml"/>
+    <arg name="behavior_path_planner_sampling_planner_module_param_path" value="$(var behavior_path_config_path)/sampling_planner/sampling_planner.param.yaml"/>
     <arg name="behavior_path_planner_dynamic_avoidance_module_param_path" value="$(var behavior_path_config_path)/dynamic_avoidance/dynamic_avoidance.param.yaml"/>
     <arg name="behavior_path_planner_lane_change_module_param_path" value="$(var behavior_path_config_path)/lane_change/lane_change.param.yaml"/>
     <arg name="behavior_path_planner_goal_planner_module_param_path" value="$(var behavior_path_config_path)/goal_planner/goal_planner.param.yaml"/>