Feature/porting motion velocity smoother (autowarefoundation#302)

* Launch motion_velocity_smoother (autowarefoundation#215)

* Launch motion_velocity_smoother

* Change params

* Fix parameter files

* Fix

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* Fix/smoother trajectory ds (autowarefoundation#249)

* Add parameter

* Fix

* Fix

* change launcher parameter (autowarefoundation#265)

* Feature/smoother resampling (autowarefoundation#269)

* change launcher parameter

* add new parameter

Co-authored-by: Fumiya Watanabe <rej55.g@gmail.com>
Co-authored-by: purewater0901 <43805014+purewater0901@users.noreply.github.com>

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

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

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

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

@@ -0,0 +1,54 @@
+/**:
+  ros__parameters:
+    # motion state constraints
+    max_velocity: 20.0      # max velocity limit [m/s]
+    max_accel: 1.0          # max acceleration limit [m/ss]
+    min_decel: -0.5         # min deceleration limit [m/ss]
+    max_jerk: 1.0             # default maximum jerk limit
+    min_jerk: -0.5            # default minimum jerk limit
+
+    # 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

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

@@ -4,7 +4,7 @@
     stop_time: 2.0
     hysteresis_buffer_distance: 2.0
     backward_path_length: 5.0
-    forward_path_length: 200.0
+    forward_path_length: 300.0
     max_accel: -5.0
     lane_change_prepare_duration: 4.0
     lane_changing_duration: 8.0

planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/obstacle_avoidance_planner.param.yaml

@@ -1,7 +1,7 @@
 /**:
   ros__parameters:
     # trajectory total/fixing length
-    trajectory_length: 200 # total trajectory length[m]
+    trajectory_length: 300 # total trajectory length[m]
     forward_fixing_distance: 5.0 # forward fixing length from base_link[m]
     backward_fixing_distance: 3.0 # backward fixing length from base_link[m]
 

planning_launch/launch/scenario_planning/scenario_planning.launch.xml

@@ -14,10 +14,13 @@
 
   <!-- velocity planning with max velocity, acceleration, jerk, stop point constraint -->
   <group>
-    <include file="$(find-pkg-share motion_velocity_optimizer)/launch/motion_velocity_optimizer.launch.xml">
-      <arg name="input_trajectory" value="/planning/scenario_planning/scenario_selector/trajectory"/>
-      <arg name="output_trajectory" value="/planning/scenario_planning/trajectory"/>
-      <arg name="param_path" value="$(find-pkg-share planning_launch)/config/scenario_planning/common/motion_velocity_optimizer/motion_velocity_optimizer.param.yaml"/>
+    <arg name="smoother_type" default="JerkFiltered" />
+    <set_remap from="~/input/trajectory" to="/planning/scenario_planning/scenario_selector/trajectory" />
+    <set_remap from="~/output/trajectory" to="/planning/scenario_planning/trajectory" />
+    <include file="$(find-pkg-share motion_velocity_smoother)/launch/motion_velocity_smoother.launch.xml">
+      <arg name="smoother_type" value="$(var smoother_type)" />
+      <arg name="param_path" value="$(find-pkg-share planning_launch)/config/scenario_planning/common/motion_velocity_smoother/motion_velocity_smoother.param.yaml"/>
+      <arg name="smoother_param_path" value="$(find-pkg-share planning_launch)/config/scenario_planning/common/motion_velocity_smoother/$(var smoother_type).param.yaml"/>
     </include>
 
   </group>