Merge pull request autowarefoundation#430 from tier4/feature/pull_out

add pull over/out module

planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/behavior_path_planner.param.yaml

@@ -3,7 +3,10 @@
     backward_path_length: 5.0
     forward_path_length: 300.0
     backward_length_buffer_for_end_of_lane: 5.0
+    backward_length_buffer_for_end_of_pull_over: 5.0
+    backward_length_buffer_for_end_of_pull_out: 5.0
     minimum_lane_change_length: 12.0
+    minimum_pull_over_length: 16.0
     drivable_area_resolution: 0.1
     drivable_area_width: 100.0
     drivable_area_height: 50.0

planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/pull_out/pull_out.param.yaml

@@ -0,0 +1,26 @@
+/**:
+  ros__parameters:
+    pull_out:
+      min_stop_distance: 2.0
+      stop_time: 0.0
+      hysteresis_buffer_distance: 1.0
+      pull_out_prepare_duration: 4.0
+      pull_out_duration: 2.0
+      pull_out_finish_judge_buffer: 1.0
+      minimum_pull_out_velocity: 2.0
+      prediction_duration: 30.0
+      prediction_time_resolution: 0.5
+      static_obstacle_velocity_thresh: 1.5
+      maximum_deceleration: 1.0
+      enable_collision_check_at_prepare_phase: false
+      use_predicted_path_outside_lanelet: false
+      use_all_predicted_path: false
+      use_dynamic_object: true
+      enable_blocked_by_obstacle: false
+      pull_out_search_distance: 30.0
+      before_pull_out_straight_distance: 5.0
+      after_pull_out_straight_distance: 5.0
+      maximum_lateral_jerk: 2.0
+      minimum_lateral_jerk: 0.5
+      deceleration_interval: 15.0
+

planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/pull_over/pull_over.param.yaml

@@ -0,0 +1,25 @@
+/**:
+  ros__parameters:
+    pull_over:
+      min_stop_distance: 5.0
+      stop_time: 2.0
+      hysteresis_buffer_distance: 2.0
+      pull_over_prepare_duration: 4.0
+      pull_over_duration: 2.0
+      pull_over_finish_judge_buffer: 3.0
+      minimum_pull_over_velocity: 3.0
+      prediction_duration: 30.0
+      prediction_time_resolution: 0.5
+      static_obstacle_velocity_thresh: 1.5
+      maximum_deceleration: 1.0
+      enable_collision_check_at_prepare_phase: false
+      use_predicted_path_outside_lanelet: false
+      use_all_predicted_path: false
+      enable_blocked_by_obstacle: false
+      pull_over_search_distance: 30.0
+      after_pull_over_straight_distance: 5.0
+      before_pull_over_straight_distance: 5.0
+      margin_from_boundary: 0.5
+      maximum_lateral_jerk: 2.0
+      minimum_lateral_jerk: 0.5
+      deceleration_interval: 15.0

planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py

@@ -82,6 +82,32 @@ def generate_launch_description():
     with open(lane_following_param_path, 'r') as f:
         lane_following_param = yaml.safe_load(f)['/**']['ros__parameters']
 
+    pull_over_param_path = os.path.join(
+        get_package_share_directory('planning_launch'),
+        'config',
+        'scenario_planning',
+        'lane_driving',
+        'behavior_planning',
+        'behavior_path_planner',
+        'pull_over',
+        'pull_over.param.yaml',
+    )
+    with open(pull_over_param_path, 'r') as f:
+        pull_over_param = yaml.safe_load(f)['/**']['ros__parameters']
+
+    pull_out_param_path = os.path.join(
+        get_package_share_directory('planning_launch'),
+        'config',
+        'scenario_planning',
+        'lane_driving',
+        'behavior_planning',
+        'behavior_path_planner',
+        'pull_out',
+        'pull_out.param.yaml',
+    )
+    with open(pull_out_param_path, 'r') as f:
+        pull_out_param = yaml.safe_load(f)['/**']['ros__parameters']
+
     behavior_path_planner_param_path = os.path.join(
         get_package_share_directory('planning_launch'),
         'config',
@@ -127,6 +153,8 @@ def generate_launch_description():
             avoidance_param,
             lane_change_param,
             lane_following_param,
+            pull_over_param,
+            pull_out_param,
             behavior_path_planner_param,
             {
                 'bt_tree_config_path':