Feature/porting motion velocity smoother (#1653)

* Add motion_velocity_smoother (#1333)

* Refactor node and utilities

* Fix utilities

* Fix utilities

* Fixing...

* Runs with L2 smoother

* Use boost::optional

* Add Linf, JerkFiltered

* JerkFiltered

* Fix

* Fix awapi

* Fix bug

* Add config files

* Fix bug

* Fix bug and apply clang-format

* Remove unused variable

* Fix bug

* Change C-style cast to static_cast

* Add destructors

* Use smart pointers for members

* Add test

* Fix test code

* Tmp: add L2 norm of jerk in objective function

* Fix external velocity limit

* Fix interpolate in velocity controller and remove prevent move to close stop line

* add initial velocity and acceleration to the filter function

* Fix index calculation

* add new marge function

* handle edge case

* Tmp: skip osqp_interface build test

* Revert get_modified_package.sh

* Fix CI

* Fix version of osqp

* Fix

* Add design docment (JPN)

* Fix linear interpolation

* Refactor node and utilities

* Fix utilities

* Fix utilities

* Fixing...

* Runs with L2 smoother

* Use boost::optional

* Add Linf, JerkFiltered

* JerkFiltered

* Fix

* Fix awapi

* Fix bug

* Add config files

* Fix bug

* Fix bug and apply clang-format

* Remove unused variable

* Fix bug

* Change C-style cast to static_cast

* Add destructors

* Use smart pointers for members

* Add test

* Fix test code

* Tmp: add L2 norm of jerk in objective function

* Fix external velocity limit

* Fix interpolate in velocity controller and remove prevent move to close stop line

* add initial velocity and acceleration to the filter function

* Fix index calculation

* add new marge function

* handle edge case

* Tmp: skip osqp_interface build test

* Revert get_modified_package.sh

* Fix CI

* Fix version of osqp

* Fix

* Add design docment (JPN)

* Fix linear interpolation

* add new sample function

* Fix

* Revert velocity_controller

* Reset motion_velocity_optimizer

* Fix parameter setting

* Refactor and fix bugs

* Use autoware_utils

* Fix doc and bug

* Fix doc

* Fix external velocity limit and parametrize margin and jerk weight

* Fix typo

* Fix typo and remove old readme

* add stop point calculation and modify objective function

* Add parameter handling functions and add namespace

* Rename calcClosestTrajectoryPoint to calcInterpolatedTrajectoryPoint and remove unused function

* Fix applyExternalVelocityLimit and fix comments

* Fix variable name

* Fix variable name

* Fix yaml comment

* Add const

* Fix interpolaion

* Remove run() and change type of prev_closest_point_

* Update planning/scenario_planning/common/motion_velocity_smoother/src/smoother/smoother_base.cpp

Co-authored-by: tkimura4 <tomoya.kimura@tier4.jp>

* fix stop point error

* Fix linear interpolation and external velocity calculation

* Remove debug code

* Rename BaseParam

* Remove unused func and fix misc

* Fix package.xml, include, apply formatting

* Fix external velocity limit

* add debug calculation time

* modify calculation time debugger

* modify calculation time publisher

* rescale the calculation time

* Fix some problem

* Update planning/scenario_planning/common/motion_velocity_smoother/launch/motion_velocity_smoother.launch

Co-authored-by: tkimura4 <tomoya.kimura@tier4.jp>

Co-authored-by: yutaka <purewater0901@gmail.com>
Co-authored-by: tkimura4 <tomoya.kimura@tier4.jp>

* Add Resample Procedure after the optimization in motion_velocity_smoother (#1530)

* do resample after the optimization

* modify resample timing

* add 0 at the end of the resampeld output trajectory

* Manage parameters

* Fix format

* Fix format

* Devide resample function in other file

* Update default_motion_velocity_smoother.yaml

* Update default_motion_velocity_smoother.yaml

Co-authored-by: Fumiya Watanabe <rej55.g@gmail.com>

* Output debug trajectories in motion velocity smoother (#1533)

* add debug trajectory

* add rqt_multiplot setting files

* add calculation tiem visualization

* guard emtpy output (#1537)

* Motion velocity smoother doc (#1563)

* Fix doc

* Add english doc (tmp)

* Add english file

* Remove unused files

* Fix

* Fix typo

* Modify 1-size trajectory handling & warning messages (#1540)

* add new guard to delete unnecessary message

* change message

* add warning messages

* chnage message to warning

* chnage to throttle

* Fix jerk filter calculation (#1593)

* modify jerk filter calculation

* remove unnecessary code

* change the way of interp. of pose in smoother (#1600)

* Update smoother resampling (#1595)

* add new guard to delete unnecessary message

* change message

* add warning messages

* chnage message to warning

* chnage to throttle

* modify jerk filter calculation

* modify jerk filter calculation

* separate resampling code

* add new parameter

* add new parameter to dynamic reconfigure

* remove unnecessary code

* add sampling before optimization

* add const and blocker

* change access to at

* fix terminal length value

* add comments

* clean code

* add comment

* Fix/insert backward on reverse trajectory (#1602)

* remove unused code

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* move debug code

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* fix insert backward on reverse trajectory

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* Porting motion velocity smoother to ros2

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

* Fix launch and fix warning

* pre-commit fixes

* Remove unused argument

* Comment out unused parameter

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

* Add distance threshold in motion_velocity_smoother (#1659)

* Add distance threshold

* Remove parameters and use default value

* Fix for pre-commit

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

* Support Non-zero Stop Point Acceleration (#1651)

* Support non-zero stop point acceleration

* modify zero velocity id search function (#1690)

* modify zero velocity id search function

* Fix resample function and error handling

* Add comment

Co-authored-by: Fumiya Watanabe <rej55.g@gmail.com>

* Apply lint

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

* pre-commit fixes

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

Co-authored-by: Fumiya Watanabe <rej55.g@gmail.com>
Co-authored-by: yutaka <purewater0901@gmail.com>
Co-authored-by: tkimura4 <tomoya.kimura@tier4.jp>
Co-authored-by: purewater0901 <43805014+purewater0901@users.noreply.github.com>
Co-authored-by: Takamasa Horibe <horibe.takamasa@gmail.com>
Co-authored-by: pre-commit <pre-commit@example.com>

planning/motion_velocity_smoother/CMakeLists.txt

@@ -0,0 +1,61 @@
+cmake_minimum_required(VERSION 3.5)
+project(motion_velocity_smoother)
+
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+  set(CMAKE_CXX_STANDARD_REQUIRED ON)
+  set(CMAKE_CXX_EXTENSIONS OFF)
+endif()
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+find_package(ament_cmake_auto REQUIRED)
+ament_auto_find_build_dependencies()
+
+find_package(Boost REQUIRED)
+find_package(eigen3_cmake_module REQUIRED)
+find_package(Eigen3 REQUIRED)
+
+
+set(MOTION_VELOCITY_SMOOTHER_SRC
+  src/motion_velocity_smoother_node.cpp
+  src/trajectory_utils.cpp
+  src/linear_interpolation.cpp
+  src/resample.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
+)
+
+ament_auto_add_library(smoother SHARED
+  ${SMOOTHER_SRC}
+)
+
+ament_auto_add_library(motion_velocity_smoother_node SHARED
+  ${MOTION_VELOCITY_SMOOTHER_SRC}
+)
+
+target_link_libraries(motion_velocity_smoother_node
+  smoother
+)
+
+rclcpp_components_register_node(motion_velocity_smoother_node
+  PLUGIN "motion_velocity_smoother::MotionVelocitySmootherNode"
+  EXECUTABLE motion_velocity_smoother
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_auto_package(
+  INSTALL_TO_SHARE
+    launch
+    config
+)

planning/motion_velocity_smoother/config/JerkFiltered.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/motion_velocity_smoother/config/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/motion_velocity_smoother/config/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/motion_velocity_smoother/config/default_motion_velocity_smoother.param.yaml

@@ -0,0 +1,55 @@
+/**:
+  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]
+    stop_decel: 0.0         # deceleration at a stop point[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