feat(autoware_velocity_smoother): port the package from Autoware Universe (#299)

Signed-off-by: Ryohsuke Mitsudome <ryohsuke.mitsudome@tier4.jp>

Author: mitsudome-r

planning/autoware_velocity_smoother/CMakeLists.txt

@@ -0,0 +1,68 @@
+cmake_minimum_required(VERSION 3.14)
+project(autoware_velocity_smoother)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+find_package(Boost REQUIRED)
+find_package(eigen3_cmake_module REQUIRED)
+find_package(Eigen3 REQUIRED)
+
+include_directories(
+  SYSTEM
+    ${EIGEN3_INCLUDE_DIR}
+)
+
+set(MOTION_VELOCITY_SMOOTHER_SRC
+  src/node.cpp
+)
+
+set(SMOOTHER_SRC
+  src/smoother/smoother_base.cpp
+  src/smoother/l2_pseudo_jerk_smoother.cpp
+  src/smoother/linf_pseudo_jerk_smoother.cpp
+  src/smoother/jerk_filtered_smoother.cpp
+  src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp
+  src/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.cpp
+  src/trajectory_utils.cpp
+  src/resample.cpp
+)
+
+ament_auto_add_library(smoother SHARED
+  ${SMOOTHER_SRC}
+)
+
+ament_auto_add_library(${PROJECT_NAME}_node SHARED
+  ${MOTION_VELOCITY_SMOOTHER_SRC}
+)
+
+target_link_libraries(${PROJECT_NAME}_node
+  smoother
+)
+
+rclcpp_components_register_node(${PROJECT_NAME}_node
+  PLUGIN "autoware::velocity_smoother::VelocitySmootherNode"
+  EXECUTABLE velocity_smoother_node
+)
+
+if(BUILD_TESTING)
+  ament_add_ros_isolated_gmock(test_smoother_functions
+  test/test_smoother_functions.cpp
+  )
+  target_link_libraries(test_smoother_functions
+  smoother
+  )
+  ament_add_ros_isolated_gtest(test_${PROJECT_NAME}
+    test/test_velocity_smoother_node_interface.cpp
+  )
+  target_link_libraries(test_${PROJECT_NAME}
+    ${PROJECT_NAME}_node
+  )
+endif()
+
+
+ament_auto_package(
+  INSTALL_TO_SHARE
+    launch
+    config
+)

planning/autoware_velocity_smoother/README.md

@@ -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

planning/autoware_velocity_smoother/config/Analytical.param.yaml

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

planning/autoware_velocity_smoother/config/JerkFiltered.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/autoware_velocity_smoother/config/L2.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/autoware_velocity_smoother/config/Linf.param.yaml

@@ -0,0 +1,17 @@
+/**:
+  ros__parameters:
+    # constraints param for normal driving
+    max_vel: 11.1           # max velocity limit [m/s]
+
+    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]

planning/autoware_velocity_smoother/config/default_common.param.yaml

@@ -0,0 +1,63 @@
+/**:
+  ros__parameters:
+    # motion state constraints
+    max_velocity: 20.0      # max velocity limit [m/s]
+    stop_decel: 0.0         # deceleration at a stop point[m/ss]
+
+    # external velocity limit parameter
+    margin_to_insert_external_velocity_limit: 0.3   #  margin distance to insert external velocity limit [m]
+
+    # ---- curve parameters  ----
+    # - common parameters -
+    curvature_calculation_distance: 5.0         # distance of points while curvature is calculating for the steer rate and lateral acceleration limit [m]
+    # - lateral acceleration limit parameters -
+    enable_lateral_acc_limit: true     # To toggle the lateral acc filter on and off. You can switch it dynamically at runtime.
+    max_lateral_accel: 0.8             # max lateral acceleration limit [m/ss]
+    min_curve_velocity: 2.74           # min velocity at lateral acceleration limit and steering angle rate limit [m/s]
+    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
+    min_decel_for_lateral_acc_lim_filter: -2.5  # deceleration limit applied in the lateral acceleration filter to avoid sudden braking [m/ss]
+    # - steering angle rate limit parameters -
+    enable_steering_rate_limit: true         # To toggle the steer rate filter on and off. You can switch it dynamically at runtime.
+    max_steering_angle_rate: 40.0            # maximum steering angle rate [degree/s]
+    resample_ds: 0.1                         # distance between trajectory points [m]
+    curvature_threshold: 0.02                # if curvature > curvature_threshold, steeringRateLimit is triggered [1/m]
+
+    # 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: 2.0                  # resample total time for dense sampling [s]
+    dense_resample_dt: 0.2              # resample time interval for dense sampling [s]
+    dense_min_interval_distance: 0.1    # minimum points-interval length for dense sampling [m]
+    sparse_resample_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_resample_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_resample_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
+
+    plan_from_ego_speed_on_manual_mode: true  # planning is done from ego velocity/acceleration on MANUAL mode. This should be true for smooth transition from MANUAL to AUTONOMOUS, but could be false for debugging.