feat: add planning_launch package

launch/planning_launch/CMakeLists.txt

@@ -0,0 +1,23 @@
+cmake_minimum_required(VERSION 3.5)
+project(planning_launch)
+
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wno-unused-parameter -Wall -Wextra -Wpedantic)
+endif()
+
+find_package(ament_cmake_auto REQUIRED)
+ament_auto_find_build_dependencies()
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_auto_package(
+  INSTALL_TO_SHARE
+  launch
+  config
+)

launch/planning_launch/README.md

@@ -0,0 +1,16 @@
+# planning_launch
+
+## Structure
+
+![planning_launch](./planning_launch.drawio.svg)
+
+## Package Dependencies
+
+Please see `<exec_depend>` in `package.xml`.
+
+## Usage
+
+```xml
+<include file="$(find-pkg-share planning_launch)/launch/planning.launch.xml">
+</include>
+```

launch/planning_launch/config/scenario_planning/common/common.param.yaml

@@ -0,0 +1,15 @@
+/**:
+  ros__parameters:
+    # constraints param for normal driving
+    normal:
+      min_acc: -0.5         # min deceleration [m/ss]
+      max_acc: 1.0          # max acceleration [m/ss]
+      min_jerk: -0.5        # min jerk [m/sss]
+      max_jerk: 1.0         # max jerk [m/sss]
+
+    # constraints to be observed
+    limit:
+      min_acc: -2.5         # min deceleration limit [m/ss]
+      max_acc: 1.0          # max acceleration limit [m/ss]
+      min_jerk: -1.5        # min jerk limit [m/sss]
+      max_jerk: 1.5         # max jerk limit [m/sss]

launch/planning_launch/config/scenario_planning/common/motion_velocity_smoother/Analytical.param.yaml

@@ -0,0 +1,25 @@
+/**:
+  ros__parameters:
+    resample:
+      ds_resample: 0.1
+      num_resample: 1
+      delta_yaw_threshold: 0.785
+
+    latacc:
+      enable_constant_velocity_while_turning: false
+      constant_velocity_dist_threshold: 2.0
+
+    forward:
+      max_acc: 1.0
+      min_acc: -1.0
+      max_jerk: 0.3
+      min_jerk: -0.3
+      kp: 0.3
+
+    backward:
+      start_jerk: -0.1
+      min_jerk_mild_stop: -0.3
+      min_jerk: -1.5
+      min_acc_mild_stop: -1.0
+      min_acc: -2.5
+      span_jerk: -0.01

launch/planning_launch/config/scenario_planning/common/motion_velocity_smoother/JerkFiltered.param.yaml

@@ -0,0 +1,7 @@
+/**:
+  ros__parameters:
+    jerk_weight: 0.1         # weight for "smoothness" cost for jerk
+    over_v_weight: 10000.0   # weight for "over speed limit" cost
+    over_a_weight: 500.0     # weight for "over accel limit" cost
+    over_j_weight: 200.0     # weight for "over jerk limit" cost
+    jerk_filter_ds: 0.1      # resampling ds for jerk filter

launch/planning_launch/config/scenario_planning/common/motion_velocity_smoother/L2.param.yaml

@@ -0,0 +1,5 @@
+/**:
+  ros__parameters:
+    pseudo_jerk_weight: 100.0 # weight for "smoothness" cost
+    over_v_weight: 100000.0   # weight for "over speed limit" cost
+    over_a_weight: 1000.0     # weight for "over accel limit" cost

launch/planning_launch/config/scenario_planning/common/motion_velocity_smoother/Linf.param.yaml

@@ -0,0 +1,5 @@
+/**:
+  ros__parameters:
+    pseudo_jerk_weight: 200.0 # weight for "smoothness" cost
+    over_v_weight: 100000.0   # weight for "over speed limit" cost
+    over_a_weight: 5000.0     # weight for "over accel limit" cost

launch/planning_launch/config/scenario_planning/common/motion_velocity_smoother/motion_velocity_smoother.param.yaml

@@ -0,0 +1,50 @@
+/**:
+  ros__parameters:
+    # motion state constraints
+    max_velocity: 20.0      # max velocity limit [m/s]
+
+    # external velocity limit parameter
+    margin_to_insert_external_velocity_limit: 0.3   #  margin distance to insert external velocity limit [m]
+
+    # curve parameters
+    max_lateral_accel: 0.8             # max lateral acceleration limit [m/ss]
+    min_curve_velocity: 2.74           # min velocity at lateral acceleration limit [m/ss]
+    decel_distance_before_curve: 3.5   # slow speed distance before a curve for lateral acceleration limit
+    decel_distance_after_curve: 2.0    # slow speed distance after a curve for lateral acceleration limit
+
+    # engage & replan parameters
+    replan_vel_deviation: 5.53         # velocity deviation to replan initial velocity [m/s]
+    engage_velocity: 0.25              # engage velocity threshold [m/s] (if the trajectory velocity is higher than this value, use this velocity for engage vehicle speed)
+    engage_acceleration: 0.1           # engage acceleration [m/ss] (use this acceleration when engagement)
+    engage_exit_ratio: 0.5             # exit engage sequence to normal velocity planning when the velocity exceeds engage_exit_ratio x engage_velocity.
+    stop_dist_to_prohibit_engage: 0.5  # if the stop point is in this distance, the speed is set to 0 not to move the vehicle [m]
+
+    # stop velocity
+    stopping_velocity: 2.778  # change target velocity to this value before v=0 point [m/s]
+    stopping_distance: 0.0    # distance for the stopping_velocity [m]. 0 means the stopping velocity is not applied.
+
+    # path extraction parameters
+    extract_ahead_dist: 200.0         # forward trajectory distance used for planning [m]
+    extract_behind_dist: 5.0          # backward trajectory distance used for planning [m]
+    delta_yaw_threshold: 1.0472       # Allowed delta yaw between ego pose and trajectory pose [radian]
+
+    # resampling parameters for optimization
+    max_trajectory_length: 200.0        # max trajectory length for resampling [m]
+    min_trajectory_length: 150.0        # min trajectory length for resampling [m]
+    resample_time: 5.0                 # resample total time for dense sampling [s]
+    dense_dt: 0.1                       # resample time interval for dense sampling [s]
+    dense_min_interval_distance: 0.1    # minimum points-interval length for dense sampling [m]
+    sparse_dt: 0.5                      # resample time interval for sparse sampling [s]
+    sparse_min_interval_distance: 4.0   # minimum points-interval length for sparse sampling [m]
+
+    # resampling parameters for post process
+    post_max_trajectory_length: 300.0        # max trajectory length for resampling [m]
+    post_min_trajectory_length: 30.0         # min trajectory length for resampling [m]
+    post_resample_time: 10.0                  # resample total time for dense sampling [s]
+    post_dense_dt: 0.1                       # resample time interval for dense sampling [s]
+    post_dense_min_interval_distance: 0.1    # minimum points-interval length for dense sampling [m]
+    post_sparse_dt: 0.1                      # resample time interval for sparse sampling [s]
+    post_sparse_min_interval_distance: 1.0   # minimum points-interval length for sparse sampling [m]
+
+    # system
+    over_stop_velocity_warn_thr: 1.389  # used to check if the optimization exceeds the input velocity on the stop point

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

@@ -0,0 +1,41 @@
+# see AvoidanceParameters description in avoidance_module_data.hpp for description.
+/**:
+  ros__parameters:
+    avoidance:
+      resample_interval_for_planning: 0.3
+      resample_interval_for_output: 4.0
+      detection_area_right_expand_dist: 0.0
+      detection_area_left_expand_dist: 1.0
+
+      threshold_distance_object_is_on_center: 1.0 # [m]
+      threshold_speed_object_is_stopped: 1.0      # [m/s]
+      object_check_forward_distance: 150.0        # [m]
+      object_check_backward_distance: 2.0         # [m]
+      lateral_collision_margin: 2.0               # [m]
+
+      prepare_time: 2.0                           # [s]
+      min_prepare_distance: 1.0                   # [m]
+      min_avoidance_distance: 10.0                # [m]
+
+      min_nominal_avoidance_speed: 7.0  # [m/s]
+      min_sharp_avoidance_speed: 1.0    # [m/s]
+
+      max_right_shift_length: 5.0
+      max_left_shift_length: 5.0
+
+      nominal_lateral_jerk: 0.2   # [m/s3]
+      max_lateral_jerk: 1.0       # [m/s3]
+
+      object_hold_max_count: 20
+
+      # For prevention of large acceleration while avoidance
+      min_avoidance_speed_for_acc_prevention: 3.0  # [m/s]
+      max_avoidance_acceleration: 0.5  # [m/ss]
+
+      # for debug
+      publish_debug_marker: false
+      print_debug_info: false
+
+      # not enabled yet
+      longitudinal_collision_margin_min_distance: 0.0          # [m]
+      longitudinal_collision_margin_time: 0.0

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

@@ -0,0 +1,14 @@
+/**:
+  ros__parameters:
+    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
+    refine_goal_search_radius_range: 7.5
+    intersection_search_distance: 30.0

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

@@ -0,0 +1,20 @@
+/**:
+  ros__parameters:
+    lane_change:
+      min_stop_distance: 5.0
+      stop_time: 2.0
+      hysteresis_buffer_distance: 2.0
+      lane_change_prepare_duration: 4.0
+      lane_changing_duration: 8.0
+      lane_change_finish_judge_buffer: 3.0
+      minimum_lane_change_velocity: 5.6
+      prediction_duration: 8.0
+      prediction_time_resolution: 0.5
+      static_obstacle_velocity_thresh: 1.5
+      maximum_deceleration: 1.0
+      enable_abort_lane_change: false
+      enable_collision_check_at_prepare_phase: false
+      use_predicted_path_outside_lanelet: false
+      use_all_predicted_path: false
+      enable_blocked_by_obstacle: false
+      lane_change_search_distance: 30.0

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

@@ -0,0 +1,7 @@
+/**:
+  ros__parameters:
+    lane_following:
+      expand_drivable_area: false
+      right_bound_offset: 0.5
+      left_bound_offset: 0.5
+      lane_change_prepare_duration: 2.0

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

@@ -0,0 +1,25 @@
+/**:
+  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

launch/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

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

@@ -0,0 +1,8 @@
+/**:
+  ros__parameters:
+    side_shift:
+      min_distance_to_start_shifting: 5.0
+      time_to_start_shifting: 1.0
+      shifting_lateral_jerk: 0.2
+      min_shifting_distance: 5.0
+      min_shifting_speed: 5.56

launch/planning_launch/config/scenario_planning/lane_driving/behavior_planning/lane_change_planner/lane_change_planner.param.yaml

@@ -0,0 +1,22 @@
+/**:
+  ros__parameters:
+    min_stop_distance: 5.0
+    stop_time: 2.0
+    hysteresis_buffer_distance: 2.0
+    backward_path_length: 5.0
+    forward_path_length: 300.0
+    max_accel: -5.0
+    lane_change_prepare_duration: 4.0
+    lane_changing_duration: 8.0
+    backward_length_buffer_for_end_of_lane: 5.0
+    lane_change_finish_judge_buffer: 3.0
+    minimum_lane_change_length: 12.0
+    minimum_lane_change_velocity: 5.6
+    prediction_duration: 8.0
+    prediction_time_resolution: 0.5
+    drivable_area_resolution: 0.1
+    drivable_area_width: 100.0
+    drivable_area_height: 50.0
+    static_obstacle_velocity_thresh: 1.5
+    maximum_deceleration: 1.0
+    refine_goal_search_radius_range: 7.5