diff --git a/planning/autoware_velocity_smoother/CHANGELOG.rst b/planning/autoware_velocity_smoother/CHANGELOG.rst new file mode 100644 index 0000000000..4655c9658e --- /dev/null +++ b/planning/autoware_velocity_smoother/CHANGELOG.rst @@ -0,0 +1,183 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package autoware_velocity_smoother +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.40.0 (2024-12-12) +------------------- +* Merge branch 'main' into release-0.40.0 +* Revert "chore(package.xml): bump version to 0.39.0 (`#9587 `_)" + This reverts commit c9f0f2688c57b0f657f5c1f28f036a970682e7f5. +* fix: fix ticket links in CHANGELOG.rst (`#9588 `_) +* chore(package.xml): bump version to 0.39.0 (`#9587 `_) + * chore(package.xml): bump version to 0.39.0 + * fix: fix ticket links in CHANGELOG.rst + * fix: remove unnecessary diff + --------- + Co-authored-by: Yutaka Kondo +* fix: fix ticket links in CHANGELOG.rst (`#9588 `_) +* fix(cpplint): include what you use - planning (`#9570 `_) +* feat(velocity_smoother, external_velocity_limit_selector): enable stronger acceleration when requested (`#9502 `_) + * change max acceleration and max jerk according to external velocity request + * modify external velocity limit selector + * fix external velocity limit selector + * fix format + --------- +* 0.39.0 +* update changelog +* Merge commit '6a1ddbd08bd' into release-0.39.0 +* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) +* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) +* chore(package.xml): bump version to 0.38.0 (`#9266 `_) (`#9284 `_) + * unify package.xml version to 0.37.0 + * remove system_monitor/CHANGELOG.rst + * add changelog + * 0.38.0 + --------- +* refactor(qp_interface): prefix package and namespace with autoware (`#9236 `_) +* Contributors: Esteve Fernandez, Fumiya Watanabe, Go Sakayori, M. Fatih Cırıt, Ryohsuke Mitsudome, Yutaka Kondo + +0.39.0 (2024-11-25) +------------------- +* Merge commit '6a1ddbd08bd' into release-0.39.0 +* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) +* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) +* chore(package.xml): bump version to 0.38.0 (`#9266 `_) (`#9284 `_) + * unify package.xml version to 0.37.0 + * remove system_monitor/CHANGELOG.rst + * add changelog + * 0.38.0 + --------- +* refactor(qp_interface): prefix package and namespace with autoware (`#9236 `_) +* Contributors: Esteve Fernandez, Yutaka Kondo + +0.38.0 (2024-11-08) +------------------- +* unify package.xml version to 0.37.0 +* refactor(osqp_interface): added autoware prefix to osqp_interface (`#8958 `_) +* refactor(autoware_interpolation): prefix package and namespace with autoware (`#8088 `_) + Co-authored-by: kosuke55 +* perf(motion_velocity_smoother): remove some heavy debug logging (`#8798 `_) + remove some heavy logging in the velocity smoother +* fix(autoware_velocity_smoother): fix unusedFunction (`#8649 `_) + fix:unusedFunction +* fix(autoware_velocity_smoother): fix variableScope (`#8442 `_) + fix:variableScope +* fix(autoware_velocity_smoother): fix unreadVariable (`#8364 `_) + fix:unreadVariable +* fix(autoware_velocity_smoother): fix passedByValue (`#8207 `_) + * fix:passedByValue + * fix:clang format + --------- +* perf(velocity_smoother): not resample debug_trajectories is not used (`#8030 `_) + * not resample debug_trajectories if not published + * update dependant packages + --------- +* perf(velocity_smoother): use ProxQP for faster optimization (`#8028 `_) + * perf(velocity_smoother): use ProxQP for faster optimization + * consider max_iteration + * disable warm start + * fix test + --------- +* fix(velocity_smoother, obstacle_cruise_planner ): float type of processing time was wrong (`#8161 `_) + fix(velocity_smoother): float type of processing time was wrong +* fix(autoware_velocity_smoother): fix constVariableReference (`#8060 `_) + fix:constVariableReference +* fix(autoware_velocity_smoother): fix constParameterReference (`#8043 `_) + fix:constParameterReference +* chore(velocity_smoother): add maintainer (`#8121 `_) + add maintainer +* refactor(autoware_universe_utils): changed the API to be more intuitive and added documentation (`#7443 `_) + * refactor(tier4_autoware_utils): Changed the API to be more intuitive and added documentation. + * use raw shared ptr in PollingPolicy::NEWEST + * update + * fix + * Update evaluator/autoware_control_evaluator/include/autoware/control_evaluator/control_evaluator_node.hpp + Co-authored-by: danielsanchezaran + --------- + Co-authored-by: danielsanchezaran +* fix(autoware_velocity_smoother): fix shadowVariablefix:shadowVariable (`#7950 `_) + fix:shadowVariable +* feat(velocity_smoother): add time_keeper (`#8026 `_) +* refactor(velocity_smoother): change method to take data for external velocity (`#7810 `_) + refactor external velocity +* fix(autoware_velocity_smoother): fix duplicateBreak warning (`#7699 `_) +* refactor(universe_utils/motion_utils)!: add autoware namespace (`#7594 `_) +* fix(autoware_velocity_smoother): fix unusedVariable warning (`#7585 `_) + fix unusedVariable warning +* refactor(motion_utils)!: add autoware prefix and include dir (`#7539 `_) + refactor(motion_utils): add autoware prefix and include dir +* feat(autoware_universe_utils)!: rename from tier4_autoware_utils (`#7538 `_) + Co-authored-by: kosuke55 +* refactor(velocity_smoother): rename to include/autoware/{package_name} (`#7533 `_) +* refactor(test_utils): move to common folder (`#7158 `_) + * Move autoware planning test manager to autoware namespace + * fix package share directory for behavior path planner + * renaming files and directory + * rename variables that has planning_test_utils in its name. + * use autoware namespace for test utils + * move folder to common + * update .pages file + * fix test error + * removed obstacle velocity limiter test artifact + * remove namespace from planning validator, it has using keyword + --------- +* refactor(vehicle_info_utils)!: prefix package and namespace with autoware (`#7353 `_) + * chore(autoware_vehicle_info_utils): rename header + * chore(bpp-common): vehicle info + * chore(path_optimizer): vehicle info + * chore(velocity_smoother): vehicle info + * chore(bvp-common): vehicle info + * chore(static_centerline_generator): vehicle info + * chore(obstacle_cruise_planner): vehicle info + * chore(obstacle_velocity_limiter): vehicle info + * chore(mission_planner): vehicle info + * chore(obstacle_stop_planner): vehicle info + * chore(planning_validator): vehicle info + * chore(surround_obstacle_checker): vehicle info + * chore(goal_planner): vehicle info + * chore(start_planner): vehicle info + * chore(control_performance_analysis): vehicle info + * chore(lane_departure_checker): vehicle info + * chore(predicted_path_checker): vehicle info + * chore(vehicle_cmd_gate): vehicle info + * chore(obstacle_collision_checker): vehicle info + * chore(operation_mode_transition_manager): vehicle info + * chore(mpc): vehicle info + * chore(control): vehicle info + * chore(common): vehicle info + * chore(perception): vehicle info + * chore(evaluator): vehicle info + * chore(freespace): vehicle info + * chore(planning): vehicle info + * chore(vehicle): vehicle info + * chore(simulator): vehicle info + * chore(launch): vehicle info + * chore(system): vehicle info + * chore(sensing): vehicle info + * fix(autoware_joy_controller): remove unused deps + --------- +* feat(autoware_velocity_smoother): use polling subscriber (`#7216 `_) + feat(motion_velocity_smoother): use polling subscriber +* refactor(path_optimizer, velocity_smoother)!: prefix package and namespace with autoware (`#7354 `_) + * chore(autoware_velocity_smoother): update namespace + * chore(autoware_path_optimizer): update namespace + --------- +* feat!: replace autoware_auto_msgs with autoware_msgs for planning modules (`#7246 `_) + Co-authored-by: Cynthia Liu + Co-authored-by: NorahXiong + Co-authored-by: beginningfan +* chore(autoware_velocity_smoother, autoware_path_optimizer): rename packages (`#7202 `_) + * chore(autoware_path_optimizer): rename package and namespace + * chore(autoware_static_centerline_generator): rename package and namespace + * chore: update module name + * chore(autoware_velocity_smoother): rename package and namespace + * chore(tier4_planning_launch): update module name + * chore: update module name + * fix: test + * fix: test + * fix: test + --------- +* Contributors: Esteve Fernandez, Fumiya Watanabe, Go Sakayori, Kosuke Takeuchi, Maxime CLEMENT, Ryohsuke Mitsudome, Ryuta Kambe, Satoshi OTA, Takayuki Murooka, Yukinari Hisaki, Yutaka Kondo, Zulfaqar Azmi, kobayu858 + +0.26.0 (2024-04-03) +------------------- diff --git a/planning/autoware_velocity_smoother/CMakeLists.txt b/planning/autoware_velocity_smoother/CMakeLists.txt new file mode 100644 index 0000000000..a5807329f7 --- /dev/null +++ b/planning/autoware_velocity_smoother/CMakeLists.txt @@ -0,0 +1,64 @@ +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/jerk_filtered_smoother.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 +) diff --git a/planning/autoware_velocity_smoother/README.ja.md b/planning/autoware_velocity_smoother/README.ja.md new file mode 100644 index 0000000000..977d3d5456 --- /dev/null +++ b/planning/autoware_velocity_smoother/README.ja.md @@ -0,0 +1,226 @@ +# Velocity Smoother + +## Purpose + +`autoware_velocity_smoother`は目標軌道上の各点における望ましい車速を計画して出力するモジュールである。 +このモジュールは、速度の最大化と乗り心地の良さを両立するために、事前に指定された制限速度、制限加速度および制限躍度の範囲で車速を計画する。 +加速度や躍度の制限を与えることは車速の変化を滑らかにすることに対応するため、このモジュールを`autoware_velocity_smoother`と呼んでいる。 + +## Inner-workings / Algorithms + +### Flow chart + +![motion_velocity_smoother_flow](./media/motion_velocity_smoother_flow.drawio.svg) + +#### Extract trajectory + +自車後輪軸中心位置に最も近い参照経路上の点に対し、`extract_behind_dist`だけ戻った点から`extract_ahead_dist`だけ進んだ点までの参照経路を抜き出す。 + +#### Apply external velocity limit + +モジュール外部から指定された速度制限を適用する。 +ここで扱う外部の速度制限は`/planning/scenario_planning/max_velocity`の topic で渡されるもので、地図上で設定された速度制限など、参照経路にすでに設定されている制限速度とは別である。 +外部から指定される速度制限は、パラメータで指定されている減速度および躍度の制限の範囲で減速可能な位置から速度制限を適用する。 + +#### Apply stop approaching velocity + +停止点に近づいたときの速度を設定する。障害物近傍まで近づく場合や、正着精度向上などの目的に用いる。 + +#### Apply lateral acceleration limit + +経路の曲率に応じて、指定された最大横加速度`max_lateral_accel`を超えない速度を制限速度として設定する。ただし、制限速度は`min_curve_velocity`を下回らないように設定する。 + +#### Resample trajectory + +指定された時間間隔で経路の点を再サンプルする。ただし、経路全体の長さは`min_trajectory_length`から`max_trajectory_length`の間となるように再サンプルを行い、点の間隔は`min_trajectory_interval_distance`より小さくならないようにする。 +現在車速で`resample_time`の間進む距離までは密にサンプリングし、それ以降は疎にサンプリングする。 +この方法でサンプリングすることで、低速時は密に、高速時は疎にサンプルされるため、停止精度と計算負荷軽減の両立を図っている。 + +#### Calculate initial state + +速度計画のための初期値を計算する。初期値は状況に応じて下表のように計算する。 + +| 状況 | 初期速度 | 初期加速度 | +| ------------------------ | ----------------- | --------------------- | +| 最初の計算時 | 現在車速 | 0.0 | +| 発進時 | `engage_velocity` | `engage_acceleration` | +| 現在車速と計画車速が乖離 | 現在車速 | 前回計画値 | +| 通常時 | 前回計画値 | 前回計画値 | + +#### Smooth velocity + +速度の計画を行う。速度計画のアルゴリズムは`JerkFiltered`のみサポートされており、コンフィグで指定する。 +最適化のソルバは OSQP[1]を利用する。 + +##### JerkFiltered + +速度の 2 乗（最小化で表すため負値で表現）、制限速度逸脱量の 2 乗、制限加度逸脱量の 2 乗、制限ジャーク逸脱量の 2 乗、ジャークの 2 乗の総和を最小化する。 + +#### Post process + +計画された軌道の後処理を行う。 + +- 停止点より先の速度を 0 に設定 +- 速度がパラメータで与えられる`max_velocity`以下となるように設定 +- 自車位置より手前の点における速度を設定 +- Trajectory の仕様に合わせてリサンプリング(`post resampling`) +- デバッグデータの出力 + +最適化の計算が終わったあと、次のノードに経路を渡す前に`post resampling`と呼ばれるリサンプリングを行う。ここで再度リサンプリングを行っている理由としては、最適化前で必要な経路間隔と後段のモジュールに渡す経路間隔が必ずしも一致していないからであり、その差を埋めるために再度サンプリングを行っている。そのため、`post resampling`では後段モジュールの経路仕様を確認してパラメータを決める必要がある。なお、最適化アルゴリズムの計算負荷が高く、最初のリサンプリングで経路間隔が後段モジュールの経路仕様より疎になっている場合、`post resampling`で経路を蜜にリサンプリングする。逆に最適化アルゴリズムの計算負荷が小さく、最初のリサンプリングで経路間隔が後段の経路仕様より蜜になっている場合は、`post resampling`で経路をその仕様に合わせて疎にリサンプリングする。 + +## Inputs / Outputs + +### Input + +| Name | Type | Description | +| ------------------------------------------ | ----------------------------------- | ----------------------------- | +| `~/input/trajectory` | `autoware_planning_msgs/Trajectory` | Reference trajectory | +| `/planning/scenario_planning/max_velocity` | `std_msgs/Float32` | External velocity limit [m/s] | +| `/localization/kinematic_state` | `nav_msgs/Odometry` | Current odometry | +| `/tf` | `tf2_msgs/TFMessage` | TF | +| `/tf_static` | `tf2_msgs/TFMessage` | TF static | + +### Output + +| Name | Type | Description | +| -------------------------------------------------- | ----------------------------------- | --------------------------------------------------------------------------------------------------------- | +| `~/output/trajectory` | `autoware_planning_msgs/Trajectory` | Modified trajectory | +| `/planning/scenario_planning/current_max_velocity` | `std_msgs/Float32` | Current external velocity limit [m/s] | +| `~/closest_velocity` | `std_msgs/Float32` | Planned velocity closest to ego base_link (for debug) | +| `~/closest_acceleration` | `std_msgs/Float32` | Planned acceleration closest to ego base_link (for debug) | +| `~/closest_jerk` | `std_msgs/Float32` | Planned jerk closest to ego base_link (for debug) | +| `~/debug/trajectory_raw` | `autoware_planning_msgs/Trajectory` | Extracted trajectory (for debug) | +| `~/debug/trajectory_external_velocity_limited` | `autoware_planning_msgs/Trajectory` | External velocity limited trajectory (for debug) | +| `~/debug/trajectory_lateral_acc_filtered` | `autoware_planning_msgs/Trajectory` | Lateral acceleration limit filtered trajectory (for debug) | +| `~/debug/trajectory_time_resampled` | `autoware_planning_msgs/Trajectory` | Time resampled trajectory (for debug) | +| `~/distance_to_stopline` | `std_msgs/Float32` | Distance to stop line from current ego pose (max 50 m) (for debug) | +| `~/stop_speed_exceeded` | `std_msgs/Bool` | It publishes `true` if planned velocity on the point which the maximum velocity is zero is over threshold | + +## Parameters + +### Constraint parameters + +| Name | Type | Description | Default value | +| :------------- | :------- | :--------------------------------------------- | :------------ | +| `max_velocity` | `double` | Max velocity limit [m/s] | 20.0 | +| `max_accel` | `double` | Max acceleration limit [m/ss] | 1.0 | +| `min_decel` | `double` | Min deceleration limit [m/ss] | -0.5 | +| `stop_decel` | `double` | Stop deceleration value at a stop point [m/ss] | 0.0 | +| `max_jerk` | `double` | Max jerk limit [m/sss] | 1.0 | +| `min_jerk` | `double` | Min jerk limit [m/sss] | -0.5 | + +### External velocity limit parameter + +| Name | Type | Description | Default value | +| :----------------------------------------- | :------- | :---------------------------------------------------- | :------------ | +| `margin_to_insert_external_velocity_limit` | `double` | margin distance to insert external velocity limit [m] | 0.3 | + +### Curve parameters + +| Name | Type | Description | Default value | +| :---------------------------- | :------- | :--------------------------------------------------------------------- | :------------ | +| `max_lateral_accel` | `double` | Max lateral acceleration limit [m/ss] | 0.5 | +| `min_curve_velocity` | `double` | Min velocity at lateral acceleration limit [m/ss] | 2.74 | +| `decel_distance_before_curve` | `double` | Distance to slowdown before a curve for lateral acceleration limit [m] | 3.5 | +| `decel_distance_after_curve` | `double` | Distance to slowdown after a curve for lateral acceleration limit [m] | 2.0 | + +### Engage & replan parameters + +| Name | Type | Description | Default value | +| :----------------------------- | :------- | :--------------------------------------------------------------------------------------------------------------------------------- | :------------ | +| `replan_vel_deviation` | `double` | Velocity deviation to replan initial velocity [m/s] | 5.53 | +| `engage_velocity` | `double` | Engage velocity threshold [m/s] (if the trajectory velocity is higher than this value, use this velocity for engage vehicle speed) | 0.25 | +| `engage_acceleration` | `double` | Engage acceleration [m/ss] (use this acceleration when engagement) | 0.1 | +| `engage_exit_ratio` | `double` | Exit engage sequence to normal velocity planning when the velocity exceeds engage_exit_ratio x engage_velocity. | 0.5 | +| `stop_dist_to_prohibit_engage` | `double` | If the stop point is in this distance, the speed is set to 0 not to move the vehicle [m] | 0.5 | + +### Stopping velocity parameters + +| Name | Type | Description | Default value | +| :------------------ | :------- | :------------------------------------------------------------------------------------ | :------------ | +| `stopping_velocity` | `double` | change target velocity to this value before v=0 point [m/s] | 2.778 | +| `stopping_distance` | `double` | distance for the stopping_velocity [m]. 0 means the stopping velocity is not applied. | 0.0 | + +### Extraction parameters + +| Name | Type | Description | Default value | +| :-------------------- | :------- | :-------------------------------------------------------------- | :------------ | +| `extract_ahead_dist` | `double` | Forward trajectory distance used for planning [m] | 200.0 | +| `extract_behind_dist` | `double` | backward trajectory distance used for planning [m] | 5.0 | +| `delta_yaw_threshold` | `double` | Allowed delta yaw between ego pose and trajectory pose [radian] | 1.0472 | + +### Resampling parameters + +| Name | Type | Description | Default value | +| :----------------------------- | :------- | :----------------------------------------------------- | :------------ | +| `max_trajectory_length` | `double` | Max trajectory length for resampling [m] | 200.0 | +| `min_trajectory_length` | `double` | Min trajectory length for resampling [m] | 30.0 | +| `resample_time` | `double` | Resample total time [s] | 10.0 | +| `dense_resample_dt` | `double` | resample time interval for dense sampling [s] | 0.1 | +| `dense_min_interval_distance` | `double` | minimum points-interval length for dense sampling [m] | 0.1 | +| `sparse_resample_dt` | `double` | resample time interval for sparse sampling [s] | 0.5 | +| `sparse_min_interval_distance` | `double` | minimum points-interval length for sparse sampling [m] | 4.0 | + +### Resampling parameters for post process + +| Name | Type | Description | Default value | +| :---------------------------------- | :------- | :----------------------------------------------------- | :------------ | +| `post_max_trajectory_length` | `double` | max trajectory length for resampling [m] | 300.0 | +| `post_min_trajectory_length` | `double` | min trajectory length for resampling [m] | 30.0 | +| `post_resample_time` | `double` | resample total time for dense sampling [s] | 10.0 | +| `post_dense_resample_dt` | `double` | resample time interval for dense sampling [s] | 0.1 | +| `post_dense_min_interval_distance` | `double` | minimum points-interval length for dense sampling [m] | 0.1 | +| `post_sparse_resample_dt` | `double` | resample time interval for sparse sampling [s] | 0.1 | +| `post_sparse_min_interval_distance` | `double` | minimum points-interval length for sparse sampling [m] | 1.0 | + +### Weights for optimization + +#### JerkFiltered + +| Name | Type | Description | Default value | +| :-------------- | :------- | :------------------------------------ | :------------ | +| `jerk_weight` | `double` | Weight for "smoothness" cost for jerk | 10.0 | +| `over_v_weight` | `double` | Weight for "over speed limit" cost | 100000.0 | +| `over_a_weight` | `double` | Weight for "over accel limit" cost | 5000.0 | +| `over_j_weight` | `double` | Weight for "over jerk limit" cost | 1000.0 | + +### Others + +| Name | Type | Description | Default value | +| :---------------------------- | :------- | :------------------------------------------------------------------------------------------------ | :------------ | +| `over_stop_velocity_warn_thr` | `double` | Threshold to judge that the optimized velocity exceeds the input velocity on the stop point [m/s] | 1.389 | + + + +## Assumptions / Known limits + +- 参照経路上の点には制限速度（停止点）が正しく設定されていることを仮定 +- 参照経路に設定されている制限速度を指定した減速度やジャークで達成不可能な場合、可能な範囲で速度、加速度、ジャークの逸脱量を抑えながら減速 +- 各逸脱量の重視の度合いはパラメータにより指定 + +## (Optional) Error detection and handling + +## (Optional) Performance characterization + +## (Optional) References/External links + +[1] B. Stellato, et al., "OSQP: an operator splitting solver for quadratic programs", Mathematical Programming Computation, 2020, [10.1007/s12532-020-00179-2](https://link.springer.com/article/10.1007/s12532-020-00179-2). + +[2] Y. Zhang, et al., "Toward a More Complete, Flexible, and Safer Speed Planning for Autonomous Driving via Convex Optimization", Sensors, vol. 18, no. 7, p. 2185, 2018, [10.3390/s18072185](https://doi.org/10.3390/s18072185) + +## (Optional) Future extensions / Unimplemented parts diff --git a/planning/autoware_velocity_smoother/README.md b/planning/autoware_velocity_smoother/README.md new file mode 100644 index 0000000000..5b9084de8f --- /dev/null +++ b/planning/autoware_velocity_smoother/README.md @@ -0,0 +1,251 @@ +# Velocity Smoother + +## Purpose + +`autoware_velocity_smoother` outputs a desired velocity profile on a reference trajectory. +This module plans a velocity profile within the limitations of the velocity, the acceleration and the jerk to realize both the maximization of velocity and the ride quality. +We call this module `autoware_velocity_smoother` because the limitations of the acceleration and the jerk means the smoothness of the velocity profile. + +## Inner-workings / Algorithms + +### Flow chart + +![motion_velocity_smoother_flow](./media/motion_velocity_smoother_flow.drawio.svg) + +#### Extract trajectory + +For the point on the reference trajectory closest to the center of the rear wheel axle of the vehicle, it extracts the reference path between `extract_behind_dist` behind and `extract_ahead_dist` ahead. + +#### Apply external velocity limit + +It applies the velocity limit input from the external of `autoware_velocity_smoother`. +Remark that the external velocity limit is different from the velocity limit already set on the map and the reference trajectory. +The external velocity is applied at the position that it is able to reach the velocity limit with the deceleration and the jerk constraints set as the parameter. + +#### Apply stop approaching velocity + +It applies the velocity limit near the stopping point. +This function is used to approach near the obstacle or improve the accuracy of stopping. + +#### Apply lateral acceleration limit + +It applies the velocity limit to decelerate at the curve. +It calculates the velocity limit from the curvature of the reference trajectory and the maximum lateral acceleration `max_lateral_accel`. +The velocity limit is set as not to fall under `min_curve_velocity`. + +Note: velocity limit that requests larger than `nominal.jerk` is not applied. In other words, even if a sharp curve is planned just in front of the ego, no deceleration is performed. + +#### Apply steering rate limit + +It calculates the desired steering angles of trajectory points. and it applies the steering rate limit. If the (`steering_angle_rate` > `max_steering_angle_rate`), it decreases the velocity of the trajectory point to acceptable velocity. + +#### Resample trajectory + +It resamples the points on the reference trajectory with designated time interval. +Note that the range of the length of the trajectory is set between `min_trajectory_length` and `max_trajectory_length`, and the distance between two points is longer than `min_trajectory_interval_distance`. +It samples densely up to the distance traveled between `resample_time` with the current velocity, then samples sparsely after that. +By sampling according to the velocity, both calculation load and accuracy are achieved since it samples finely at low velocity and coarsely at high velocity. + +#### Calculate initial state + +Calculate initial values for velocity planning. +The initial values are calculated according to the situation as shown in the following table. + +| Situation | Initial velocity | Initial acceleration | +| ------------------------------------------------------------- | ---------------------- | ---------------------- | +| First calculation | Current velocity | 0.0 | +| Engaging | `engage_velocity` | `engage_acceleration` | +| Deviate between the planned velocity and the current velocity | Current velocity | Previous planned value | +| Normal | Previous planned value | Previous planned value | + +#### Smooth velocity + +It plans the velocity. +Only `JerkFiltered` is supported as an algorithm of velocity planning, and it is set in the launch file. +In these algorithms, they use OSQP[1] as the solver of the optimization. + +##### JerkFiltered + +It minimizes the sum of the minus of the square of the velocity and the square of the violation of the velocity limit, the acceleration limit and the jerk limit. + +#### Post process + +It performs the post-process of the planned velocity. + +- Set zero velocity ahead of the stopping point +- Set maximum velocity given in the config named `max_velocity` +- Set velocity behind the current pose +- Resample trajectory (`post resampling`) +- Output debug data + +After the optimization, a resampling called `post resampling` is performed before passing the optimized trajectory to the next node. Since the required path interval from optimization may be different from the one for the next module, `post resampling` helps to fill this gap. Therefore, in `post resampling`, it is necessary to check the path specification of the following module to determine the parameters. Note that if the computational load of the optimization algorithm is high and the path interval is sparser than the path specification of the following module in the first resampling, `post resampling` would resample the trajectory densely. On the other hand, if the computational load of the optimization algorithm is small and the path interval is denser than the path specification of the following module in the first resampling, the path is sparsely resampled according to the specification of the following module. + +## Inputs / Outputs + +### Input + +| Name | Type | Description | +| ------------------------------------------ | ----------------------------------- | ----------------------------- | +| `~/input/trajectory` | `autoware_planning_msgs/Trajectory` | Reference trajectory | +| `/planning/scenario_planning/max_velocity` | `std_msgs/Float32` | External velocity limit [m/s] | +| `/localization/kinematic_state` | `nav_msgs/Odometry` | Current odometry | +| `/tf` | `tf2_msgs/TFMessage` | TF | +| `/tf_static` | `tf2_msgs/TFMessage` | TF static | + +### Output + +| Name | Type | Description | +| -------------------------------------------------- | ----------------------------------- | --------------------------------------------------------------------------------------------------------- | +| `~/output/trajectory` | `autoware_planning_msgs/Trajectory` | Modified trajectory | +| `/planning/scenario_planning/current_max_velocity` | `std_msgs/Float32` | Current external velocity limit [m/s] | +| `~/closest_velocity` | `std_msgs/Float32` | Planned velocity closest to ego base_link (for debug) | +| `~/closest_acceleration` | `std_msgs/Float32` | Planned acceleration closest to ego base_link (for debug) | +| `~/closest_jerk` | `std_msgs/Float32` | Planned jerk closest to ego base_link (for debug) | +| `~/debug/trajectory_raw` | `autoware_planning_msgs/Trajectory` | Extracted trajectory (for debug) | +| `~/debug/trajectory_external_velocity_limited` | `autoware_planning_msgs/Trajectory` | External velocity limited trajectory (for debug) | +| `~/debug/trajectory_lateral_acc_filtered` | `autoware_planning_msgs/Trajectory` | Lateral acceleration limit filtered trajectory (for debug) | +| `~/debug/trajectory_steering_rate_limited` | `autoware_planning_msgs/Trajectory` | Steering angle rate limit filtered trajectory (for debug) | +| `~/debug/trajectory_time_resampled` | `autoware_planning_msgs/Trajectory` | Time resampled trajectory (for debug) | +| `~/distance_to_stopline` | `std_msgs/Float32` | Distance to stop line from current ego pose (max 50 m) (for debug) | +| `~/stop_speed_exceeded` | `std_msgs/Bool` | It publishes `true` if planned velocity on the point which the maximum velocity is zero is over threshold | + +## Parameters + +### Constraint parameters + +| Name | Type | Description | Default value | +| :------------- | :------- | :--------------------------------------------- | :------------ | +| `max_velocity` | `double` | Max velocity limit [m/s] | 20.0 | +| `max_accel` | `double` | Max acceleration limit [m/ss] | 1.0 | +| `min_decel` | `double` | Min deceleration limit [m/ss] | -0.5 | +| `stop_decel` | `double` | Stop deceleration value at a stop point [m/ss] | 0.0 | +| `max_jerk` | `double` | Max jerk limit [m/sss] | 1.0 | +| `min_jerk` | `double` | Min jerk limit [m/sss] | -0.5 | + +### External velocity limit parameter + +| Name | Type | Description | Default value | +| :----------------------------------------- | :------- | :---------------------------------------------------- | :------------ | +| `margin_to_insert_external_velocity_limit` | `double` | margin distance to insert external velocity limit [m] | 0.3 | + +### Curve parameters + +| Name | Type | Description | Default value | +| :------------------------------------- | :------- | :----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ | +| `enable_lateral_acc_limit` | `bool` | To toggle the lateral acceleration filter on and off. You can switch it dynamically at runtime. | true | +| `max_lateral_accel` | `double` | Max lateral acceleration limit [m/ss] | 0.5 | +| `min_curve_velocity` | `double` | Min velocity at lateral acceleration limit [m/ss] | 2.74 | +| `decel_distance_before_curve` | `double` | Distance to slowdown before a curve for lateral acceleration limit [m] | 3.5 | +| `decel_distance_after_curve` | `double` | Distance to slowdown after a curve for lateral acceleration limit [m] | 2.0 | +| `min_decel_for_lateral_acc_lim_filter` | `double` | Deceleration limit to avoid sudden braking by the lateral acceleration filter [m/ss]. Strong limitation degrades the deceleration response to the appearance of sharp curves due to obstacle avoidance, etc. | -2.5 | + +### Engage & replan parameters + +| Name | Type | Description | Default value | +| :----------------------------- | :------- | :--------------------------------------------------------------------------------------------------------------------------------- | :------------ | +| `replan_vel_deviation` | `double` | Velocity deviation to replan initial velocity [m/s] | 5.53 | +| `engage_velocity` | `double` | Engage velocity threshold [m/s] (if the trajectory velocity is higher than this value, use this velocity for engage vehicle speed) | 0.25 | +| `engage_acceleration` | `double` | Engage acceleration [m/ss] (use this acceleration when engagement) | 0.1 | +| `engage_exit_ratio` | `double` | Exit engage sequence to normal velocity planning when the velocity exceeds engage_exit_ratio x engage_velocity. | 0.5 | +| `stop_dist_to_prohibit_engage` | `double` | If the stop point is in this distance, the speed is set to 0 not to move the vehicle [m] | 0.5 | + +### Stopping velocity parameters + +| Name | Type | Description | Default value | +| :------------------ | :------- | :------------------------------------------------------------------------------------ | :------------ | +| `stopping_velocity` | `double` | change target velocity to this value before v=0 point [m/s] | 2.778 | +| `stopping_distance` | `double` | distance for the stopping_velocity [m]. 0 means the stopping velocity is not applied. | 0.0 | + +### Extraction parameters + +| Name | Type | Description | Default value | +| :-------------------- | :------- | :-------------------------------------------------------------- | :------------ | +| `extract_ahead_dist` | `double` | Forward trajectory distance used for planning [m] | 200.0 | +| `extract_behind_dist` | `double` | backward trajectory distance used for planning [m] | 5.0 | +| `delta_yaw_threshold` | `double` | Allowed delta yaw between ego pose and trajectory pose [radian] | 1.0472 | + +### Resampling parameters + +| Name | Type | Description | Default value | +| :----------------------------- | :------- | :----------------------------------------------------- | :------------ | +| `max_trajectory_length` | `double` | Max trajectory length for resampling [m] | 200.0 | +| `min_trajectory_length` | `double` | Min trajectory length for resampling [m] | 30.0 | +| `resample_time` | `double` | Resample total time [s] | 10.0 | +| `dense_dt` | `double` | resample time interval for dense sampling [s] | 0.1 | +| `dense_min_interval_distance` | `double` | minimum points-interval length for dense sampling [m] | 0.1 | +| `sparse_dt` | `double` | resample time interval for sparse sampling [s] | 0.5 | +| `sparse_min_interval_distance` | `double` | minimum points-interval length for sparse sampling [m] | 4.0 | + +### Resampling parameters for post process + +| Name | Type | Description | Default value | +| :---------------------------------- | :------- | :----------------------------------------------------- | :------------ | +| `post_max_trajectory_length` | `double` | max trajectory length for resampling [m] | 300.0 | +| `post_min_trajectory_length` | `double` | min trajectory length for resampling [m] | 30.0 | +| `post_resample_time` | `double` | resample total time for dense sampling [s] | 10.0 | +| `post_dense_dt` | `double` | resample time interval for dense sampling [s] | 0.1 | +| `post_dense_min_interval_distance` | `double` | minimum points-interval length for dense sampling [m] | 0.1 | +| `post_sparse_dt` | `double` | resample time interval for sparse sampling [s] | 0.1 | +| `post_sparse_min_interval_distance` | `double` | minimum points-interval length for sparse sampling [m] | 1.0 | + +### Limit steering angle rate parameters + +| Name | Type | Description | Default value | +| :------------------------------- | :------- | :------------------------------------------------------------------------------------ | :------------ | +| `enable_steering_rate_limit` | `bool` | To toggle the steer rate filter on and off. You can switch it dynamically at runtime. | true | +| `max_steering_angle_rate` | `double` | Maximum steering angle rate [degree/s] | 40.0 | +| `resample_ds` | `double` | Distance between trajectory points [m] | 0.1 | +| `curvature_threshold` | `double` | If curvature > curvature_threshold, steeringRateLimit is triggered [1/m] | 0.02 | +| `curvature_calculation_distance` | `double` | Distance of points while curvature is calculating [m] | 1.0 | + +### Weights for optimization + +#### JerkFiltered + +| Name | Type | Description | Default value | +| :-------------- | :------- | :------------------------------------ | :------------ | +| `jerk_weight` | `double` | Weight for "smoothness" cost for jerk | 10.0 | +| `over_v_weight` | `double` | Weight for "over speed limit" cost | 100000.0 | +| `over_a_weight` | `double` | Weight for "over accel limit" cost | 5000.0 | +| `over_j_weight` | `double` | Weight for "over jerk limit" cost | 1000.0 | + +### Others + +| Name | Type | Description | Default value | +| :---------------------------- | :------- | :------------------------------------------------------------------------------------------------ | :------------ | +| `over_stop_velocity_warn_thr` | `double` | Threshold to judge that the optimized velocity exceeds the input velocity on the stop point [m/s] | 1.389 | + + + +## Assumptions / Known limits + +- Assume that the velocity limit or the stopping point is properly set at the point on the reference trajectory +- If the velocity limit set in the reference path cannot be achieved by the designated constraints of the deceleration and the jerk, decelerate while suppressing the velocity, the acceleration and the jerk deviation as much as possible +- The importance of the deviations is set in the config file + +## (Optional) Error detection and handling + +## (Optional) Performance characterization + +## (Optional) References/External links + +[1] B. Stellato, et al., "OSQP: an operator splitting solver for quadratic programs", Mathematical Programming Computation, 2020, [10.1007/s12532-020-00179-2](https://link.springer.com/article/10.1007/s12532-020-00179-2). + +[2] Y. Zhang, et al., "Toward a More Complete, Flexible, and Safer Speed Planning for Autonomous Driving via Convex Optimization", Sensors, vol. 18, no. 7, p. 2185, 2018, [10.3390/s18072185](https://doi.org/10.3390/s18072185) + +## (Optional) Future extensions / Unimplemented parts diff --git a/planning/autoware_velocity_smoother/config/JerkFiltered.param.yaml b/planning/autoware_velocity_smoother/config/JerkFiltered.param.yaml new file mode 100644 index 0000000000..78c088b084 --- /dev/null +++ b/planning/autoware_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 diff --git a/planning/autoware_velocity_smoother/config/default_common.param.yaml b/planning/autoware_velocity_smoother/config/default_common.param.yaml new file mode 100644 index 0000000000..be0667c158 --- /dev/null +++ b/planning/autoware_velocity_smoother/config/default_common.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] diff --git a/planning/autoware_velocity_smoother/config/default_velocity_smoother.param.yaml b/planning/autoware_velocity_smoother/config/default_velocity_smoother.param.yaml new file mode 100644 index 0000000000..21ad85137e --- /dev/null +++ b/planning/autoware_velocity_smoother/config/default_velocity_smoother.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. diff --git a/planning/autoware_velocity_smoother/config/rqt_multiplot.xml b/planning/autoware_velocity_smoother/config/rqt_multiplot.xml new file mode 100644 index 0000000000..694f18c0d9 --- /dev/null +++ b/planning/autoware_velocity_smoother/config/rqt_multiplot.xml @@ -0,0 +1,317 @@ + + + + #babdb6 + #000000 + false + false + + + + + + + Untitled Axis + 0 + true + + + Untitled Axis + 0 + true + + + + + + + + + 1 + + 1000 + 0 + 0 + -1000 + 0 + + /planning/scenario_planning/velocity_smoother/closest_velocity + std_msgs/Float32 + + + data + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /planning/scenario_planning/velocity_smoother/closest_velocity + std_msgs/Float32 + + + + #000000 + 1 + + + 1000 + 10 + 0 + + + 100 + Optimum Velocity + + + + + + 1 + + 1000 + 0 + 0 + -1000 + 0 + + /planning/scenario_planning/velocity_smoother/closest_merged_velocity + std_msgs/Float32 + + + data + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /planning/scenario_planning/velocity_smoother/closest_merged_velocity + std_msgs/Float32 + + + + #73d216 + 1 + + + 1000 + 10 + 0 + + + 100 + Jerk Filtered Velocity + + + + + + 1 + + 1000 + 0 + 0 + -1000 + 0 + + /planning/scenario_planning/velocity_smoother/closest_max_velocity + std_msgs/Float32 + + + data + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /planning/scenario_planning/velocity_smoother/closest_max_velocity + std_msgs/Float32 + + + + #75507b + 1 + + + 1000 + 10 + 0 + + + 100 + Maximum Velocity + + + + + + 1 + + 1000 + 0 + 0 + -1000 + 0 + + /planning/scenario_planning/velocity_smoother/closest_acceleration + std_msgs/Float32 + + + data + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /planning/scenario_planning/velocity_smoother/closest_acceleration + std_msgs/Float32 + + + + #ef2929 + 1 + + + 1000 + 10 + 0 + + + 100 + Acceleration + + + + true + + 30 + Velocity + + + + + + + + Untitled Axis + 0 + true + + + Untitled Axis + 0 + true + + + + + + + + + 1 + + 1000 + 0 + 0 + -1000 + 0 + + /planning/scenario_planning/velocity_smoother/calculation_time + std_msgs/Float32 + + + data + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /planning/scenario_planning/velocity_smoother/calculation_time + std_msgs/Float32 + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + Calculation Time + + + + true + + 30 + Calculation Time + + + + false +
+ diff --git a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/node.hpp b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/node.hpp new file mode 100644 index 0000000000..a3763efb42 --- /dev/null +++ b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/node.hpp @@ -0,0 +1,290 @@ +// Copyright 2021 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE__VELOCITY_SMOOTHER__NODE_HPP_ +#define AUTOWARE__VELOCITY_SMOOTHER__NODE_HPP_ + +#include "autoware/motion_utils/trajectory/conversion.hpp" +#include "autoware/motion_utils/trajectory/trajectory.hpp" +#include "autoware/osqp_interface/osqp_interface.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/math/unit_conversion.hpp" +#include "autoware/universe_utils/ros/logger_level_configure.hpp" +#include "autoware/universe_utils/ros/polling_subscriber.hpp" +#include "autoware/universe_utils/ros/self_pose_listener.hpp" +#include "autoware/universe_utils/system/stop_watch.hpp" +#include "autoware/universe_utils/system/time_keeper.hpp" +#include "autoware/velocity_smoother/resample.hpp" +#include "autoware/velocity_smoother/smoother/jerk_filtered_smoother.hpp" +#include "autoware/velocity_smoother/smoother/smoother_base.hpp" +#include "autoware/velocity_smoother/trajectory_utils.hpp" +#include "rclcpp/rclcpp.hpp" +#include "tf2/utils.h" +#include "tf2_ros/transform_listener.h" + +#include + +#include "autoware_adapi_v1_msgs/msg/operation_mode_state.hpp" +#include "autoware_planning_msgs/msg/trajectory.hpp" +#include "autoware_planning_msgs/msg/trajectory_point.hpp" +#include "geometry_msgs/msg/accel_with_covariance_stamped.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "tier4_debug_msgs/msg/float32_stamped.hpp" // temporary +#include "tier4_debug_msgs/msg/float64_stamped.hpp" // temporary +#include "tier4_planning_msgs/msg/stop_speed_exceeded.hpp" // temporary +#include "tier4_planning_msgs/msg/velocity_limit.hpp" // temporary +#include "visualization_msgs/msg/marker_array.hpp" + +#include +#include +#include +#include +#include +#include + +namespace autoware::velocity_smoother +{ +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; +using TrajectoryPoints = std::vector; +using autoware_adapi_v1_msgs::msg::OperationModeState; +using geometry_msgs::msg::AccelWithCovarianceStamped; +using geometry_msgs::msg::Pose; +using geometry_msgs::msg::PoseStamped; +using nav_msgs::msg::Odometry; +using tier4_debug_msgs::msg::Float32Stamped; // temporary +using tier4_debug_msgs::msg::Float64Stamped; // temporary +using tier4_planning_msgs::msg::StopSpeedExceeded; // temporary +using tier4_planning_msgs::msg::VelocityLimit; // temporary +using visualization_msgs::msg::MarkerArray; + +struct Motion +{ + double vel = 0.0; + double acc = 0.0; + + Motion() {} + Motion(const double v, const double a) : vel(v), acc(a) {} +}; + +class VelocitySmootherNode : public rclcpp::Node +{ +public: + explicit VelocitySmootherNode(const rclcpp::NodeOptions & node_options); + +private: + rclcpp::Publisher::SharedPtr pub_trajectory_; + rclcpp::Publisher::SharedPtr pub_virtual_wall_; + rclcpp::Publisher::SharedPtr pub_over_stop_velocity_; + rclcpp::Subscription::SharedPtr sub_current_trajectory_; + autoware::universe_utils::InterProcessPollingSubscriber sub_current_odometry_{ + this, "/localization/kinematic_state"}; + autoware::universe_utils::InterProcessPollingSubscriber + sub_current_acceleration_{this, "~/input/acceleration"}; + autoware::universe_utils::InterProcessPollingSubscriber< + VelocityLimit, universe_utils::polling_policy::Newest> + sub_external_velocity_limit_{this, "~/input/external_velocity_limit_mps"}; + autoware::universe_utils::InterProcessPollingSubscriber sub_operation_mode_{ + this, "~/input/operation_mode_state"}; + + Odometry::ConstSharedPtr current_odometry_ptr_; // current odometry + AccelWithCovarianceStamped::ConstSharedPtr current_acceleration_ptr_; + VelocityLimit::ConstSharedPtr external_velocity_limit_ptr_{ + nullptr}; // external velocity limit message + Trajectory::ConstSharedPtr base_traj_raw_ptr_; // current base_waypoints + double max_velocity_with_deceleration_; // maximum velocity with deceleration + // for external velocity limit + double wheelbase_; // wheelbase + double base_link2front_; // base_link to front + + TrajectoryPoints prev_output_; // previously published trajectory + + // previous trajectory point closest to ego vehicle + boost::optional prev_closest_point_{}; + boost::optional current_closest_point_from_prev_output_{}; + + bool is_reverse_; + + // check if the vehicle is under control of the planning module + OperationModeState operation_mode_; + + enum class AlgorithmType { + INVALID = 0, + JERK_FILTERED = 1, + }; + + enum class InitializeType { + EGO_VELOCITY = 0, + LARGE_DEVIATION_REPLAN = 1, + ENGAGING = 2, + NORMAL = 3, + }; + + struct Param + { + bool enable_lateral_acc_limit; + bool enable_steering_rate_limit; + + double max_velocity; // max velocity [m/s] + double margin_to_insert_external_velocity_limit; // for external velocity limit [m] + double replan_vel_deviation; // if speed error exceeds this [m/s], + // replan from current velocity + double engage_velocity; // use this speed when start moving [m/s] + double engage_acceleration; // use this acceleration when start moving [m/ss] + double engage_exit_ratio; // exit engage sequence + // when the speed exceeds ratio x engage_vel. + double stopping_velocity; // change target velocity to this value before v=0 point. + double stopping_distance; // distance for the stopping_velocity + double extract_ahead_dist; // forward waypoints distance from current position [m] + double extract_behind_dist; // backward waypoints distance from current position [m] + double stop_dist_to_prohibit_engage; // prevent to move toward close stop point + double ego_nearest_dist_threshold; // for ego's closest index calculation + double ego_nearest_yaw_threshold; // for ego's closest index calculation + + resampling::ResampleParam post_resample_param; + AlgorithmType algorithm_type; // Option : JerkFiltered + + bool plan_from_ego_speed_on_manual_mode = true; + } node_param_{}; + + struct AccelerationRequest + { + bool request{false}; + double max_acceleration{0.0}; + double max_jerk{0.0}; + }; + struct ExternalVelocityLimit + { + double velocity{0.0}; // current external_velocity_limit + double dist{0.0}; // distance to set external velocity limit + AccelerationRequest acceleration_request; + std::string sender{""}; + }; + ExternalVelocityLimit + external_velocity_limit_; // velocity and distance constraint of external velocity limit + + std::shared_ptr smoother_; + + bool publish_debug_trajs_; // publish planned trajectories + + double over_stop_velocity_warn_thr_; // threshold to publish over velocity warn + + mutable rclcpp::Clock::SharedPtr clock_; + + void setupSmoother(const double wheelbase); + + // parameter update + OnSetParametersCallbackHandle::SharedPtr set_param_res_; + rcl_interfaces::msg::SetParametersResult onParameter( + const std::vector & parameters); + + // topic callback + void onCurrentTrajectory(const Trajectory::ConstSharedPtr msg); + + void calcExternalVelocityLimit(); + + // publish methods + void publishTrajectory(const TrajectoryPoints & traj) const; + + void publishStopDistance(const TrajectoryPoints & trajectory) const; + + // non-const methods + void publishClosestState(const TrajectoryPoints & trajectory); + + void updatePrevValues(const TrajectoryPoints & final_result); + + // const methods + bool checkData() const; + + void updateDataForExternalVelocityLimit(); + + AlgorithmType getAlgorithmType(const std::string & algorithm_name) const; + + TrajectoryPoints calcTrajectoryVelocity(const TrajectoryPoints & traj_input) const; + + bool smoothVelocity( + const TrajectoryPoints & input, const size_t input_closest, + TrajectoryPoints & traj_smoothed) const; + + std::pair calcInitialMotion( + const TrajectoryPoints & input_traj, const size_t input_closest) const; + + void applyExternalVelocityLimit(TrajectoryPoints & traj) const; + + void insertBehindVelocity( + const size_t output_closest, const InitializeType type, TrajectoryPoints & output) const; + + void applyStopApproachingVelocity(TrajectoryPoints & traj) const; + + void overwriteStopPoint(const TrajectoryPoints & input, TrajectoryPoints & output) const; + + double calcTravelDistance() const; + + bool isEngageStatus(const double target_vel) const; + + void publishDebugTrajectories(const std::vector & debug_trajectories) const; + + void publishClosestVelocity( + const TrajectoryPoints & trajectory, const Pose & current_pose, + const rclcpp::Publisher::SharedPtr pub) const; + + Trajectory toTrajectoryMsg( + const TrajectoryPoints & points, const std_msgs::msg::Header * header = nullptr) const; + + TrajectoryPoint calcProjectedTrajectoryPoint( + const TrajectoryPoints & trajectory, const Pose & pose) const; + TrajectoryPoint calcProjectedTrajectoryPointFromEgo(const TrajectoryPoints & trajectory) const; + + // parameter handling + void initCommonParam(); + + // debug + autoware::universe_utils::StopWatch stop_watch_; + std::shared_ptr prev_time_; + double prev_acc_; + rclcpp::Publisher::SharedPtr pub_dist_to_stopline_; + rclcpp::Publisher::SharedPtr pub_trajectory_raw_; + rclcpp::Publisher::SharedPtr pub_velocity_limit_; + rclcpp::Publisher::SharedPtr pub_trajectory_vel_lim_; + rclcpp::Publisher::SharedPtr pub_trajectory_latacc_filtered_; + rclcpp::Publisher::SharedPtr pub_trajectory_steering_rate_limited_; + rclcpp::Publisher::SharedPtr pub_trajectory_resampled_; + rclcpp::Publisher::SharedPtr debug_closest_velocity_; + rclcpp::Publisher::SharedPtr debug_closest_acc_; + rclcpp::Publisher::SharedPtr debug_closest_jerk_; + rclcpp::Publisher::SharedPtr debug_calculation_time_; + rclcpp::Publisher::SharedPtr debug_closest_max_velocity_; + rclcpp::Publisher::SharedPtr + debug_processing_time_detail_; + + // For Jerk Filtered Algorithm Debug + rclcpp::Publisher::SharedPtr pub_forward_filtered_trajectory_; + rclcpp::Publisher::SharedPtr pub_backward_filtered_trajectory_; + rclcpp::Publisher::SharedPtr pub_merged_filtered_trajectory_; + rclcpp::Publisher::SharedPtr pub_closest_merged_velocity_; + + // helper functions + size_t findNearestIndexFromEgo(const TrajectoryPoints & points) const; + bool isReverse(const TrajectoryPoints & points) const; + void flipVelocity(TrajectoryPoints & points) const; + void publishStopWatchTime(); + + std::unique_ptr logger_configure_; + std::unique_ptr published_time_publisher_; + + mutable std::shared_ptr time_keeper_{nullptr}; +}; +} // namespace autoware::velocity_smoother + +#endif // AUTOWARE__VELOCITY_SMOOTHER__NODE_HPP_ diff --git a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/resample.hpp b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/resample.hpp new file mode 100644 index 0000000000..40081e1c79 --- /dev/null +++ b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/resample.hpp @@ -0,0 +1,53 @@ +// Copyright 2021 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE__VELOCITY_SMOOTHER__RESAMPLE_HPP_ +#define AUTOWARE__VELOCITY_SMOOTHER__RESAMPLE_HPP_ + +#include "autoware_planning_msgs/msg/trajectory_point.hpp" +#include + +#include + +namespace autoware::velocity_smoother +{ +namespace resampling +{ +using autoware_planning_msgs::msg::TrajectoryPoint; +using TrajectoryPoints = std::vector; + +struct ResampleParam +{ + double max_trajectory_length; // max length of the objective trajectory for resample + double min_trajectory_length; // min length of the objective trajectory for resample + double resample_time; // max time to calculate trajectory length + double dense_resample_dt; // resample time interval for dense sampling [s] + double dense_min_interval_distance; // minimum points-interval length for dense sampling [m] + double sparse_resample_dt; // resample time interval for sparse sampling [s] + double sparse_min_interval_distance; // minimum points-interval length for sparse sampling [m] +}; + +TrajectoryPoints resampleTrajectory( + const TrajectoryPoints & input, const double v_current, + const geometry_msgs::msg::Pose & current_pose, const double nearest_dist_threshold, + const double nearest_yaw_threshold, const ResampleParam & param, const bool use_zoh_for_v = true); + +TrajectoryPoints resampleTrajectory( + const TrajectoryPoints & input, const geometry_msgs::msg::Pose & current_pose, + const double nearest_dist_threshold, const double nearest_yaw_threshold, + const ResampleParam & param, const double nominal_ds, const bool use_zoh_for_v = true); +} // namespace resampling +} // namespace autoware::velocity_smoother + +#endif // AUTOWARE__VELOCITY_SMOOTHER__RESAMPLE_HPP_ diff --git a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/jerk_filtered_smoother.hpp b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/jerk_filtered_smoother.hpp new file mode 100644 index 0000000000..6ddba29bf0 --- /dev/null +++ b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/jerk_filtered_smoother.hpp @@ -0,0 +1,78 @@ +// Copyright 2021 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE__VELOCITY_SMOOTHER__SMOOTHER__JERK_FILTERED_SMOOTHER_HPP_ +#define AUTOWARE__VELOCITY_SMOOTHER__SMOOTHER__JERK_FILTERED_SMOOTHER_HPP_ + +#include "autoware/motion_utils/trajectory/trajectory.hpp" +#include "autoware/qp_interface/qp_interface.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/system/time_keeper.hpp" +#include "autoware/velocity_smoother/smoother/smoother_base.hpp" + +#include "autoware_planning_msgs/msg/trajectory_point.hpp" + +#include "boost/optional.hpp" + +#include +#include + +namespace autoware::velocity_smoother +{ +class JerkFilteredSmoother : public SmootherBase +{ +public: + struct Param + { + double jerk_weight; + double over_v_weight; + double over_a_weight; + double over_j_weight; + double jerk_filter_ds; + }; + + explicit JerkFilteredSmoother( + rclcpp::Node & node, const std::shared_ptr time_keeper); + + bool apply( + const double initial_vel, const double initial_acc, const TrajectoryPoints & input, + TrajectoryPoints & output, std::vector & debug_trajectories, + const bool publish_debug_trajs) override; + + TrajectoryPoints resampleTrajectory( + const TrajectoryPoints & input, [[maybe_unused]] const double v0, + const geometry_msgs::msg::Pose & current_pose, const double nearest_dist_threshold, + const double nearest_yaw_threshold) const override; + + void setParam(const Param & param); + Param getParam() const; + +private: + Param smoother_param_; + std::shared_ptr qp_interface_; + rclcpp::Logger logger_{rclcpp::get_logger("smoother").get_child("jerk_filtered_smoother")}; + + TrajectoryPoints forwardJerkFilter( + const double v0, const double a0, const double a_max, const double a_stop, const double j_max, + const TrajectoryPoints & input) const; + TrajectoryPoints backwardJerkFilter( + const double v0, const double a0, const double a_min, const double a_stop, const double j_min, + const TrajectoryPoints & input) const; + TrajectoryPoints mergeFilteredTrajectory( + const double v0, const double a0, const double a_min, const double j_min, + const TrajectoryPoints & forward_filtered, const TrajectoryPoints & backward_filtered) const; +}; +} // namespace autoware::velocity_smoother + +#endif // AUTOWARE__VELOCITY_SMOOTHER__SMOOTHER__JERK_FILTERED_SMOOTHER_HPP_ diff --git a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/smoother_base.hpp b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/smoother_base.hpp new file mode 100644 index 0000000000..d51431383f --- /dev/null +++ b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/smoother_base.hpp @@ -0,0 +1,98 @@ +// Copyright 2021 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE__VELOCITY_SMOOTHER__SMOOTHER__SMOOTHER_BASE_HPP_ +#define AUTOWARE__VELOCITY_SMOOTHER__SMOOTHER__SMOOTHER_BASE_HPP_ + +#include "autoware/universe_utils/system/time_keeper.hpp" +#include "autoware/velocity_smoother/resample.hpp" +#include "rclcpp/rclcpp.hpp" + +#include "autoware_planning_msgs/msg/trajectory_point.hpp" + +#include +#include +#include + +namespace autoware::velocity_smoother +{ +using autoware_planning_msgs::msg::TrajectoryPoint; +using TrajectoryPoints = std::vector; + +class SmootherBase +{ +public: + struct BaseParam + { + double max_accel; // max acceleration in planning [m/s2] > 0 + double min_decel; // min deceleration in planning [m/s2] < 0 + double stop_decel; // deceleration at a stop point [m/s2] <= 0 + double max_jerk; + double min_jerk; + double max_lateral_accel; // max lateral acceleration [m/ss] > 0 + double min_decel_for_lateral_acc_lim_filter; // deceleration limit applied in the lateral + // acceleration filter to avoid sudden braking. + double min_curve_velocity; // min velocity at curve [m/s] + double decel_distance_before_curve; // distance before slow down for lateral acc at a curve + double decel_distance_after_curve; // distance after slow down for lateral acc at a curve + double max_steering_angle_rate; // max steering angle rate [degree/s] + double wheel_base; // wheel base [m] + double sample_ds; // distance between trajectory points [m] + double curvature_threshold; // look-up distance of Trajectory point for calculation of steering + // angle limit [m] + double curvature_calculation_distance; // threshold steering degree limit to trigger + // steeringRateLimit [degree] + resampling::ResampleParam resample_param; + }; + + explicit SmootherBase( + rclcpp::Node & node, const std::shared_ptr time_keeper); + virtual ~SmootherBase() = default; + virtual bool apply( + const double initial_vel, const double initial_acc, const TrajectoryPoints & input, + TrajectoryPoints & output, std::vector & debug_trajectories, + const bool publish_debug_trajs) = 0; + + virtual TrajectoryPoints resampleTrajectory( + const TrajectoryPoints & input, const double v0, const geometry_msgs::msg::Pose & current_pose, + const double nearest_dist_threshold, const double nearest_yaw_threshold) const = 0; + + virtual TrajectoryPoints applyLateralAccelerationFilter( + const TrajectoryPoints & input, [[maybe_unused]] const double v0 = 0.0, + [[maybe_unused]] const double a0 = 0.0, [[maybe_unused]] const bool enable_smooth_limit = false, + const bool use_resampling = true, const double input_points_interval = 1.0) const; + + TrajectoryPoints applySteeringRateLimit( + const TrajectoryPoints & input, const bool use_resampling = true, + const double input_points_interval = 1.0) const; + + double getMaxAccel() const; + double getMinDecel() const; + double getMaxJerk() const; + double getMinJerk() const; + + void setWheelBase(const double wheel_base); + void setMaxAccel(const double max_acceleration); + void setMaxJerk(const double max_jerk); + + void setParam(const BaseParam & param); + BaseParam getBaseParam() const; + +protected: + BaseParam base_param_; + mutable std::shared_ptr time_keeper_{nullptr}; +}; +} // namespace autoware::velocity_smoother + +#endif // AUTOWARE__VELOCITY_SMOOTHER__SMOOTHER__SMOOTHER_BASE_HPP_ diff --git a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/trajectory_utils.hpp b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/trajectory_utils.hpp new file mode 100644 index 0000000000..d20f82538e --- /dev/null +++ b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/trajectory_utils.hpp @@ -0,0 +1,71 @@ +// Copyright 2021 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE__VELOCITY_SMOOTHER__TRAJECTORY_UTILS_HPP_ +#define AUTOWARE__VELOCITY_SMOOTHER__TRAJECTORY_UTILS_HPP_ + +#include "autoware_planning_msgs/msg/trajectory_point.hpp" +#include "geometry_msgs/msg/pose.hpp" + +#include +#include +#include +#include + +namespace autoware::velocity_smoother::trajectory_utils +{ +using autoware_planning_msgs::msg::TrajectoryPoint; +using TrajectoryPoints = std::vector; +using geometry_msgs::msg::Pose; + +TrajectoryPoint calcInterpolatedTrajectoryPoint( + const TrajectoryPoints & trajectory, const Pose & target_pose, const size_t seg_idx); + +TrajectoryPoints extractPathAroundIndex( + const TrajectoryPoints & trajectory, const size_t index, const double & ahead_length, + const double & behind_length); + +std::vector calcArclengthArray(const TrajectoryPoints & trajectory); + +std::vector calcTrajectoryIntervalDistance(const TrajectoryPoints & trajectory); + +std::vector calcTrajectoryCurvatureFrom3Points( + const TrajectoryPoints & trajectory, size_t idx_dist); + +void applyMaximumVelocityLimit( + const size_t from, const size_t to, const double max_vel, TrajectoryPoints & trajectory); + +std::optional searchZeroVelocityIdx(const TrajectoryPoints & trajectory); + +bool calcStopDistWithJerkConstraints( + const double v0, const double a0, const double jerk_acc, const double jerk_dec, + const double min_acc, const double target_vel, std::map & jerk_profile, + double & stop_dist); + +bool isValidStopDist( + const double v_end, const double a_end, const double v_target, const double a_target, + const double v_margin, const double a_margin); + +std::optional> updateStateWithJerkConstraint( + const double v0, const double a0, const std::map & jerk_profile, const double t); + +std::vector calcVelocityProfileWithConstantJerkAndAccelerationLimit( + const TrajectoryPoints & trajectory, const double v0, const double a0, const double jerk, + const double acc_max, const double acc_min); + +double calcStopDistance(const TrajectoryPoints & trajectory, const size_t closest); + +} // namespace autoware::velocity_smoother::trajectory_utils + +#endif // AUTOWARE__VELOCITY_SMOOTHER__TRAJECTORY_UTILS_HPP_ diff --git a/planning/autoware_velocity_smoother/launch/velocity_smoother.launch.xml b/planning/autoware_velocity_smoother/launch/velocity_smoother.launch.xml new file mode 100644 index 0000000000..18b4dbe8e7 --- /dev/null +++ b/planning/autoware_velocity_smoother/launch/velocity_smoother.launch.xml @@ -0,0 +1,34 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/planning/autoware_velocity_smoother/media/motion_velocity_smoother_flow.drawio.svg b/planning/autoware_velocity_smoother/media/motion_velocity_smoother_flow.drawio.svg new file mode 100644 index 0000000000..529de678b1 --- /dev/null +++ b/planning/autoware_velocity_smoother/media/motion_velocity_smoother_flow.drawio.svg @@ -0,0 +1,1622 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Extract trajectory + + + + + Extract trajectory + + + + + + + + + + + + Apply external velocity limit + + + + + Apply external velocity limit + + + + + + + + + + + + Apply lateral acceleration limit + + + + + Apply lateral acceleration limit + + + + + + + + + + + + Resample trajectory + + + + + Resample trajectory + + + + + + + + + + + + Calculate initial state + + + + + Calculate initial state + + + + + + + + + + + + Smooth velocity + + + + + Smooth velocity + + + + + + + + + + + + Postprocess + + + + + Postprocess + + + + + Reference trajectory + + + + + + Output trajectory + + + + + + + + + onCurrentTrajectory + + + + + onCurrentTrajectory + + + + + + + + + + + + Calculate travel distance + + + + + Calculate travel distance + + + + + + + Apply steering rate limit + + + + + + + + + + + Apply stop approaching velocity + + + + + Apply stop approaching velocity + + + + + + + + + + + + + + + + + + + + extract_behind_dist + + + + + extract_behind_dist + + + + + + + + + + extract_ahead_dist + + + + + extract_ahead_dist + + + + + + + + + + + Extract trajectory + + + + + Extract trajecto... + + + + + + + + + + Apply external velocity limit + + + + + Apply external velocity li... + + + + + + + + + + + + + + default velocity limit + + + + + default velocity lim... + + + + + + + + + + external velocity limit + + + + + external velocity li... + + + + + + + + + + + + + + velocity + + + + + velocity + + + + + + + + + + distance + + + + + distance + + + + + + + + + + + + + applied velocity limit + + + + + applied velocity lim... + + + + + + + + + + + + distance required for deceleration under jerk constraints + + + + + distance required for decelera... + + + + + + + + + + current pose + + + + + current p... + + + + + + + + + + Apply stopping velocity limit + + + + + Apply stopping velocity li... + + + + + + + + + + + + + + + + + + + stopping velocity + + + + + stopping velocity + + + + + + + + + + + + + + velocity + + + + + velocity + + + + + + + + + + + + + applied velocity limit + + + + + applied velocity lim... + + + + + + + + + + + stopping + + distance + + + + + stopping... + + + + + + + + + + current pose + + + + + current p... + + + + + + + + + + + + + + velocity limit with external velocity limit + + + + + velocity limit with external veloci... + + + + + + + + + + + + + + + + + + Apply lateral acceleration limit + + + + + Apply lateral acceleration lim... + + + + + + + + + + + + + + + velocity + + + + + velocity + + + + + + + + + + + + applied velocity limit + + + + + applied velocity lim... + + + + + + + + + + current pose + + + + + current p... + + + + + + + + + + velocity limit with stopping velocity + + + + + velocity limit with stopping veloci... + + + + + + + + + + + + + + + ego car + + + + + ego car + + + + + + + + + + + + + + + + + + + distance + + + + + distance + + + + + + + + + + distance + + + + + distance + + + + + + + + + + curvature + + + + + curvature + + + + + + + + + + distance + + + + + distance + + + + + + + + + + + + trajectory + + + + + trajectory + + + + + + + + + + + + Resample trajectory + + + + + Resample trajectory + + + + + + + + + + + + + + + velocity + + + + + velocity + + + + + + + + + + current pose + + + + + current p... + + + + + + + + + + distance + + + + + distance + + + + + + + + + + + + + + + + + + + + + + + + + + + resample time * current velocity + + + + + resample time * current veloc... + + + + + + + + + + + + dense sampling + + + + + dense sampling + + + + + + + + + + sparse sampling + + + + + sparse sampling + + + + + + + + + + + + + + + + Smooth velocity + + + + + Smooth velocity + + + + + + + + + + + + + + + + + + + + + + + + Apply steering rate limit + + + + + Apply steering rate limit + + + + + + + + + + + + + + + velocity + + + + + velocity + + + + + + + + + + + + applied velocity limit + + + + + applied velocity lim... + + + + + + + + + + current pose + + + + + current p... + + + + + + + + + + velocity limit with lateral acceleration limit + + + + + + velocity with lateral acceleration limit + + + + + + + + + + + + + + + + ego car + + + + + ego car + + + + + + + + + + + + + + + + + + + distance + + + + + distance + + + + + + + + + + curvature + + + + + curvature + + + + + + + + + + distance + + + + + distance + + + + + + + + + + + + trajectory + + + + + trajectory + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + smoothed velocity + + + + + smoothed velocity + + + + + + + + + + velocity limit + + + + + velocity limit + + + + + + Viewer does not support full SVG 1.1 + + + + + + + + + + + + + + + + + + + + + + + + + + velocity + + + + + velocity + + + + + + + + + + + current pose + + + + + current p... + + + + + + + + + + distance + + + + + distance + + + diff --git a/planning/autoware_velocity_smoother/package.xml b/planning/autoware_velocity_smoother/package.xml new file mode 100644 index 0000000000..96462602d8 --- /dev/null +++ b/planning/autoware_velocity_smoother/package.xml @@ -0,0 +1,47 @@ + + + + autoware_velocity_smoother + 0.40.0 + The autoware_velocity_smoother package + + Fumiya Watanabe + Takamasa Horibe + Makoto Kurihara + Satoshi Ota + Go Sakayori + Apache License 2.0 + + Takamasa Horibe + Fumiya Watanabe + Yutaka Shimizu + Makoto Kurihara + + ament_cmake_auto + autoware_cmake + eigen3_cmake_module + + autoware_interpolation + autoware_motion_utils + autoware_osqp_interface + autoware_planning_msgs + autoware_planning_test_manager + autoware_qp_interface + autoware_universe_utils + autoware_vehicle_info_utils + geometry_msgs + nav_msgs + rclcpp + tf2 + tf2_ros + tier4_debug_msgs + tier4_planning_msgs + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/planning/autoware_velocity_smoother/src/node.cpp b/planning/autoware_velocity_smoother/src/node.cpp new file mode 100644 index 0000000000..4113b2e6bd --- /dev/null +++ b/planning/autoware_velocity_smoother/src/node.cpp @@ -0,0 +1,1090 @@ +// Copyright 2021 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware/velocity_smoother/node.hpp" + +#include "autoware/motion_utils/marker/marker_helper.hpp" +#include "autoware/universe_utils/ros/update_param.hpp" +#include "autoware/velocity_smoother/smoother/jerk_filtered_smoother.hpp" + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// clang-format on +namespace autoware::velocity_smoother +{ +VelocitySmootherNode::VelocitySmootherNode(const rclcpp::NodeOptions & node_options) +: Node("velocity_smoother", node_options) +{ + using std::placeholders::_1; + + // set common params + const auto vehicle_info = autoware::vehicle_info_utils::VehicleInfoUtils(*this).getVehicleInfo(); + wheelbase_ = vehicle_info.wheel_base_m; + base_link2front_ = vehicle_info.max_longitudinal_offset_m; + initCommonParam(); + over_stop_velocity_warn_thr_ = declare_parameter("over_stop_velocity_warn_thr"); + + // create time_keeper and its publisher + // NOTE: This has to be called before setupSmoother to pass the time_keeper to the smoother. + debug_processing_time_detail_ = create_publisher( + "~/debug/processing_time_detail_ms", 1); + time_keeper_ = + std::make_shared(debug_processing_time_detail_); + + // create smoother + setupSmoother(wheelbase_); + + // publishers, subscribers + pub_trajectory_ = create_publisher("~/output/trajectory", 1); + pub_virtual_wall_ = create_publisher("~/virtual_wall", 1); + pub_velocity_limit_ = create_publisher( + "~/output/current_velocity_limit_mps", rclcpp::QoS{1}.transient_local()); + pub_dist_to_stopline_ = create_publisher("~/distance_to_stopline", 1); + pub_over_stop_velocity_ = create_publisher("~/stop_speed_exceeded", 1); + sub_current_trajectory_ = create_subscription( + "~/input/trajectory", 1, std::bind(&VelocitySmootherNode::onCurrentTrajectory, this, _1)); + + // parameter update + set_param_res_ = + this->add_on_set_parameters_callback(std::bind(&VelocitySmootherNode::onParameter, this, _1)); + + // debug + publish_debug_trajs_ = declare_parameter("publish_debug_trajs"); + debug_closest_velocity_ = create_publisher("~/closest_velocity", 1); + debug_closest_acc_ = create_publisher("~/closest_acceleration", 1); + debug_closest_jerk_ = create_publisher("~/closest_jerk", 1); + debug_closest_max_velocity_ = create_publisher("~/closest_max_velocity", 1); + debug_calculation_time_ = create_publisher("~/debug/processing_time_ms", 1); + pub_trajectory_raw_ = create_publisher("~/debug/trajectory_raw", 1); + pub_trajectory_vel_lim_ = + create_publisher("~/debug/trajectory_external_velocity_limited", 1); + pub_trajectory_latacc_filtered_ = + create_publisher("~/debug/trajectory_lateral_acc_filtered", 1); + pub_trajectory_steering_rate_limited_ = + create_publisher("~/debug/trajectory_steering_rate_limited", 1); + pub_trajectory_resampled_ = create_publisher("~/debug/trajectory_time_resampled", 1); + + external_velocity_limit_.velocity = node_param_.max_velocity; + max_velocity_with_deceleration_ = node_param_.max_velocity; + + // publish default max velocity + VelocityLimit max_vel_msg{}; + max_vel_msg.stamp = this->now(); + max_vel_msg.max_velocity = node_param_.max_velocity; + pub_velocity_limit_->publish(max_vel_msg); + + clock_ = get_clock(); + + logger_configure_ = std::make_unique(this); + published_time_publisher_ = + std::make_unique(this); +} + +void VelocitySmootherNode::setupSmoother(const double wheelbase) +{ + switch (node_param_.algorithm_type) { + case AlgorithmType::JERK_FILTERED: { + smoother_ = std::make_shared(*this, time_keeper_); + + // Set Publisher for jerk filtered algorithm + pub_forward_filtered_trajectory_ = + create_publisher("~/debug/forward_filtered_trajectory", 1); + pub_backward_filtered_trajectory_ = + create_publisher("~/debug/backward_filtered_trajectory", 1); + pub_merged_filtered_trajectory_ = + create_publisher("~/debug/merged_filtered_trajectory", 1); + pub_closest_merged_velocity_ = + create_publisher("~/closest_merged_velocity", 1); + break; + } + default: + throw std::domain_error("[VelocitySmootherNode] invalid algorithm"); + } + + smoother_->setWheelBase(wheelbase); +} + +rcl_interfaces::msg::SetParametersResult VelocitySmootherNode::onParameter( + const std::vector & parameters) +{ + auto update_param = [&](const std::string & name, double & v) { + auto it = std::find_if( + parameters.cbegin(), parameters.cend(), + [&name](const rclcpp::Parameter & parameter) { return parameter.get_name() == name; }); + if (it != parameters.cend()) { + v = it->as_double(); + return true; + } + return false; + }; + + // TODO(Horibe): temporally. replace with template. + auto update_param_bool = [&](const std::string & name, bool & v) { + auto it = std::find_if( + parameters.cbegin(), parameters.cend(), + [&name](const rclcpp::Parameter & parameter) { return parameter.get_name() == name; }); + if (it != parameters.cend()) { + v = it->as_bool(); + return true; + } + return false; + }; + + { + auto & p = node_param_; + update_param_bool("enable_lateral_acc_limit", p.enable_lateral_acc_limit); + update_param_bool("enable_steering_rate_limit", p.enable_steering_rate_limit); + + update_param("max_vel", p.max_velocity); + update_param( + "margin_to_insert_external_velocity_limit", p.margin_to_insert_external_velocity_limit); + update_param("replan_vel_deviation", p.replan_vel_deviation); + update_param("engage_velocity", p.engage_velocity); + update_param("engage_acceleration", p.engage_acceleration); + update_param("engage_exit_ratio", p.engage_exit_ratio); + update_param("stopping_velocity", p.stopping_velocity); + update_param("stopping_distance", p.stopping_distance); + update_param("extract_ahead_dist", p.extract_ahead_dist); + update_param("extract_behind_dist", p.extract_behind_dist); + update_param("stop_dist_to_prohibit_engage", p.stop_dist_to_prohibit_engage); + update_param("ego_nearest_dist_threshold", p.ego_nearest_dist_threshold); + update_param("ego_nearest_yaw_threshold", p.ego_nearest_yaw_threshold); + update_param_bool("plan_from_ego_speed_on_manual_mode", p.plan_from_ego_speed_on_manual_mode); + } + + { + auto p = smoother_->getBaseParam(); + update_param("normal.max_acc", p.max_accel); + update_param("normal.min_acc", p.min_decel); + update_param("stop_decel", p.stop_decel); + update_param("normal.max_jerk", p.max_jerk); + update_param("normal.min_jerk", p.min_jerk); + update_param("max_lateral_accel", p.max_lateral_accel); + update_param("min_curve_velocity", p.min_curve_velocity); + update_param("decel_distance_before_curve", p.decel_distance_before_curve); + update_param("decel_distance_after_curve", p.decel_distance_after_curve); + update_param("max_trajectory_length", p.resample_param.max_trajectory_length); + update_param("min_trajectory_length", p.resample_param.min_trajectory_length); + update_param("resample_time", p.resample_param.resample_time); + update_param("dense_resample_dt", p.resample_param.dense_resample_dt); + update_param("min_interval_distance", p.resample_param.dense_min_interval_distance); + update_param("sparse_resample_dt", p.resample_param.sparse_resample_dt); + update_param("sparse_min_interval_distance", p.resample_param.sparse_min_interval_distance); + update_param("resample_ds", p.sample_ds); + update_param("curvature_threshold", p.curvature_threshold); + update_param("max_steering_angle_rate", p.max_steering_angle_rate); + update_param("curvature_calculation_distance", p.curvature_calculation_distance); + smoother_->setParam(p); + } + + switch (node_param_.algorithm_type) { + case AlgorithmType::JERK_FILTERED: { + auto p = std::dynamic_pointer_cast(smoother_)->getParam(); + update_param("jerk_weight", p.jerk_weight); + update_param("over_v_weight", p.over_v_weight); + update_param("over_a_weight", p.over_a_weight); + update_param("over_j_weight", p.over_j_weight); + update_param("jerk_filter_ds", p.jerk_filter_ds); + std::dynamic_pointer_cast(smoother_)->setParam(p); + break; + } + default: + throw std::domain_error("[VelocitySmootherNode] invalid algorithm"); + } + + rcl_interfaces::msg::SetParametersResult result{}; + result.successful = true; + result.reason = "success"; + return result; +} + +void VelocitySmootherNode::initCommonParam() +{ + auto & p = node_param_; + p.enable_lateral_acc_limit = declare_parameter("enable_lateral_acc_limit"); + p.enable_steering_rate_limit = declare_parameter("enable_steering_rate_limit"); + + p.max_velocity = declare_parameter("max_vel"); + p.margin_to_insert_external_velocity_limit = + declare_parameter("margin_to_insert_external_velocity_limit"); + p.replan_vel_deviation = declare_parameter("replan_vel_deviation"); + p.engage_velocity = declare_parameter("engage_velocity"); + p.engage_acceleration = declare_parameter("engage_acceleration"); + p.engage_exit_ratio = declare_parameter("engage_exit_ratio"); + p.engage_exit_ratio = std::min(std::max(p.engage_exit_ratio, 0.0), 1.0); + p.stopping_velocity = declare_parameter("stopping_velocity"); + p.stopping_distance = declare_parameter("stopping_distance"); + p.extract_ahead_dist = declare_parameter("extract_ahead_dist"); + p.extract_behind_dist = declare_parameter("extract_behind_dist"); + p.stop_dist_to_prohibit_engage = declare_parameter("stop_dist_to_prohibit_engage"); + p.ego_nearest_dist_threshold = declare_parameter("ego_nearest_dist_threshold"); + p.ego_nearest_yaw_threshold = declare_parameter("ego_nearest_yaw_threshold"); + p.post_resample_param.max_trajectory_length = + declare_parameter("post_max_trajectory_length"); + p.post_resample_param.min_trajectory_length = + declare_parameter("post_min_trajectory_length"); + p.post_resample_param.resample_time = declare_parameter("post_resample_time"); + p.post_resample_param.dense_resample_dt = declare_parameter("post_dense_resample_dt"); + p.post_resample_param.dense_min_interval_distance = + declare_parameter("post_dense_min_interval_distance"); + p.post_resample_param.sparse_resample_dt = declare_parameter("post_sparse_resample_dt"); + p.post_resample_param.sparse_min_interval_distance = + declare_parameter("post_sparse_min_interval_distance"); + p.algorithm_type = getAlgorithmType(declare_parameter("algorithm_type")); + + p.plan_from_ego_speed_on_manual_mode = + declare_parameter("plan_from_ego_speed_on_manual_mode"); +} + +void VelocitySmootherNode::publishTrajectory(const TrajectoryPoints & trajectory) const +{ + Trajectory publishing_trajectory = autoware::motion_utils::convertToTrajectory(trajectory); + publishing_trajectory.header = base_traj_raw_ptr_->header; + pub_trajectory_->publish(publishing_trajectory); + published_time_publisher_->publish_if_subscribed( + pub_trajectory_, publishing_trajectory.header.stamp); +} + +void VelocitySmootherNode::calcExternalVelocityLimit() +{ + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + + if (!external_velocity_limit_ptr_) { + external_velocity_limit_.acceleration_request.request = false; + return; + } + + // sender + external_velocity_limit_.sender = external_velocity_limit_ptr_->sender; + + // on the first time, apply directly + if (prev_output_.empty() || !current_closest_point_from_prev_output_) { + external_velocity_limit_.velocity = external_velocity_limit_ptr_->max_velocity; + pub_velocity_limit_->publish(*external_velocity_limit_ptr_); + return; + } + + constexpr double eps = 1.0E-04; + const double margin = node_param_.margin_to_insert_external_velocity_limit; + + // Set distance as zero if ego vehicle is stopped and external velocity limit is zero + if ( + std::fabs(current_odometry_ptr_->twist.twist.linear.x) < eps && + external_velocity_limit_.velocity < eps) { + external_velocity_limit_.dist = 0.0; + } + + const auto base_max_acceleration = get_parameter("normal.max_acc").as_double(); + const auto acceleration_request = + external_velocity_limit_ptr_->use_constraints && + base_max_acceleration < external_velocity_limit_ptr_->constraints.max_acceleration; + if ( + acceleration_request && + current_odometry_ptr_->twist.twist.linear.x < external_velocity_limit_ptr_->max_velocity) { + external_velocity_limit_.acceleration_request.request = true; + external_velocity_limit_.acceleration_request.max_acceleration = + external_velocity_limit_ptr_->constraints.max_acceleration; + external_velocity_limit_.acceleration_request.max_jerk = + external_velocity_limit_ptr_->constraints.max_jerk; + } + + // calculate distance and maximum velocity + // to decelerate to external velocity limit with jerk and acceleration + // constraints. + // if external velocity limit decreases + if ( + std::fabs((external_velocity_limit_.velocity - external_velocity_limit_ptr_->max_velocity)) > + eps) { + const double v0 = current_closest_point_from_prev_output_->longitudinal_velocity_mps; + const double a0 = current_closest_point_from_prev_output_->acceleration_mps2; + + if (isEngageStatus(v0)) { + max_velocity_with_deceleration_ = external_velocity_limit_ptr_->max_velocity; + external_velocity_limit_.dist = 0.0; + } else { + const auto & cstr = external_velocity_limit_ptr_->constraints; + const auto a_min = external_velocity_limit_ptr_->use_constraints ? cstr.min_acceleration + : smoother_->getMinDecel(); + const auto j_max = + external_velocity_limit_ptr_->use_constraints ? cstr.max_jerk : smoother_->getMaxJerk(); + const auto j_min = + external_velocity_limit_ptr_->use_constraints ? cstr.min_jerk : smoother_->getMinJerk(); + + // If the closest acceleration is positive, velocity will increase + // until the acceleration becomes zero + // So we set the maximum increased velocity as the velocity limit + if (a0 > 0) { + max_velocity_with_deceleration_ = v0 - 0.5 * a0 * a0 / j_min; + } else { + max_velocity_with_deceleration_ = v0; + } + + if (external_velocity_limit_ptr_->max_velocity < max_velocity_with_deceleration_) { + // TODO(mkuri) If v0 < external_velocity_limit_ptr_->max_velocity < + // max_velocity_with_deceleration_ meets, stronger jerk than expected may be applied to + // external velocity limit. + if (v0 < external_velocity_limit_ptr_->max_velocity) { + RCLCPP_WARN( + get_logger(), + "Stronger jerk than expected may be applied to external velocity limit in this " + "condition."); + } + + double stop_dist = 0.0; + std::map jerk_profile; + if (!trajectory_utils::calcStopDistWithJerkConstraints( + v0, a0, j_max, j_min, a_min, external_velocity_limit_ptr_->max_velocity, jerk_profile, + stop_dist)) { + RCLCPP_WARN(get_logger(), "Stop distance calculation failed!"); + } + external_velocity_limit_.dist = stop_dist + margin; + } else { + max_velocity_with_deceleration_ = external_velocity_limit_ptr_->max_velocity; + external_velocity_limit_.dist = 0.0; + } + } + } + + external_velocity_limit_.velocity = external_velocity_limit_ptr_->max_velocity; + pub_velocity_limit_->publish(*external_velocity_limit_ptr_); + + return; +} + +bool VelocitySmootherNode::checkData() const +{ + if (!current_odometry_ptr_ || !base_traj_raw_ptr_ || !current_acceleration_ptr_) { + RCLCPP_DEBUG( + get_logger(), "wait topics : current_vel = %d, base_traj = %d, acceleration = %d", + (bool)current_odometry_ptr_, (bool)base_traj_raw_ptr_, (bool)current_acceleration_ptr_); + return false; + } + if (base_traj_raw_ptr_->points.size() < 2) { + RCLCPP_ERROR(get_logger(), "input trajectory size must > 1. Skip computation."); + return false; + } + + return true; +} + +void VelocitySmootherNode::onCurrentTrajectory(const Trajectory::ConstSharedPtr msg) +{ + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + + RCLCPP_DEBUG(get_logger(), "========================= run start ========================="); + stop_watch_.tic(); + + base_traj_raw_ptr_ = msg; + + // receive data + current_odometry_ptr_ = sub_current_odometry_.takeData(); + current_acceleration_ptr_ = sub_current_acceleration_.takeData(); + external_velocity_limit_ptr_ = sub_external_velocity_limit_.takeData(); + const auto operation_mode_ptr = sub_operation_mode_.takeData(); + if (operation_mode_ptr) { + operation_mode_ = *operation_mode_ptr; + } + + // guard + if (!checkData()) { + return; + } + + // calculate trajectory velocity + auto input_points = autoware::motion_utils::convertToTrajectoryPointArray(*base_traj_raw_ptr_); + + // guard for invalid trajectory + input_points = autoware::motion_utils::removeOverlapPoints(input_points); + if (input_points.size() < 2) { + RCLCPP_ERROR(get_logger(), "No enough points in trajectory after overlap points removal"); + return; + } + + // Set 0 at the end of the trajectory + input_points.back().longitudinal_velocity_mps = 0.0; + + // calculate prev closest point + if (!prev_output_.empty()) { + current_closest_point_from_prev_output_ = calcProjectedTrajectoryPointFromEgo(prev_output_); + } + + // calculate distance to insert external velocity limit + calcExternalVelocityLimit(); + updateDataForExternalVelocityLimit(); + + // For negative velocity handling, multiple -1 to velocity if it is for reverse. + // NOTE: this process must be in the beginning of the process + is_reverse_ = isReverse(input_points); + if (is_reverse_) { + flipVelocity(input_points); + } + + const auto output = calcTrajectoryVelocity(input_points); + if (output.empty()) { + RCLCPP_WARN(get_logger(), "Output Point is empty"); + return; + } + + // Note that output velocity is resampled by linear interpolation + auto output_resampled = resampling::resampleTrajectory( + output, current_odometry_ptr_->twist.twist.linear.x, current_odometry_ptr_->pose.pose, + node_param_.ego_nearest_dist_threshold, node_param_.ego_nearest_yaw_threshold, + node_param_.post_resample_param, false); + + // Set 0 at the end of the trajectory + if (!output_resampled.empty()) { + output_resampled.back().longitudinal_velocity_mps = 0.0; + } + + // update previous step infomation + updatePrevValues(output); + + // for reverse velocity + // NOTE: this process must be in the end of the process + if (is_reverse_) { + flipVelocity(output_resampled); + } + + // publish message + publishTrajectory(output_resampled); + + // publish debug message + publishStopDistance(output); + publishClosestState(output); + + // Publish Calculation Time + publishStopWatchTime(); + RCLCPP_DEBUG(get_logger(), "========================== run() end ==========================\n\n"); +} + +void VelocitySmootherNode::updateDataForExternalVelocityLimit() +{ + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + + if (prev_output_.empty()) { + return; + } + + // calculate distance to insert external velocity limit + const double travel_dist = calcTravelDistance(); + external_velocity_limit_.dist = std::max(external_velocity_limit_.dist - travel_dist, 0.0); + RCLCPP_DEBUG( + get_logger(), "run: travel_dist = %f, external_velocity_limit_dist_ = %f", travel_dist, + external_velocity_limit_.dist); + + return; +} + +TrajectoryPoints VelocitySmootherNode::calcTrajectoryVelocity( + const TrajectoryPoints & traj_input) const +{ + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + + TrajectoryPoints output{}; // velocity is optimized by qp solver + + // Extract trajectory around self-position with desired forward-backward length + const size_t input_closest = findNearestIndexFromEgo(traj_input); + + auto traj_extracted = trajectory_utils::extractPathAroundIndex( + traj_input, input_closest, node_param_.extract_ahead_dist, node_param_.extract_behind_dist); + if (traj_extracted.empty()) { + RCLCPP_WARN(get_logger(), "Fail to extract the path from the input trajectory"); + return prev_output_; + } + + // Debug + if (publish_debug_trajs_) { + auto tmp = traj_extracted; + if (is_reverse_) flipVelocity(tmp); + pub_trajectory_raw_->publish(toTrajectoryMsg(tmp)); + } + + // Apply external velocity limit + applyExternalVelocityLimit(traj_extracted); + + // Change trajectory velocity to zero when current_velocity == 0 & stop_dist is close + const size_t traj_extracted_closest = findNearestIndexFromEgo(traj_extracted); + + // Apply velocity to approach stop point + applyStopApproachingVelocity(traj_extracted); + + // Debug + if (publish_debug_trajs_) { + auto tmp = traj_extracted; + if (is_reverse_) flipVelocity(tmp); + pub_trajectory_vel_lim_->publish(toTrajectoryMsg(traj_extracted)); + } + + // Smoothing velocity + if (!smoothVelocity(traj_extracted, traj_extracted_closest, output)) { + return prev_output_; + } + + return output; +} + +bool VelocitySmootherNode::smoothVelocity( + const TrajectoryPoints & input, const size_t input_closest, + TrajectoryPoints & traj_smoothed) const +{ + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + + if (input.empty()) { + return false; // cannot apply smoothing + } + + // Calculate initial motion for smoothing + const auto [initial_motion, type] = calcInitialMotion(input, input_closest); + + // Lateral acceleration limit + constexpr bool enable_smooth_limit = true; + constexpr bool use_resampling = true; + const auto traj_lateral_acc_filtered = + node_param_.enable_lateral_acc_limit + ? smoother_->applyLateralAccelerationFilter( + input, initial_motion.vel, initial_motion.acc, enable_smooth_limit, use_resampling) + : input; + + // Steering angle rate limit (Note: set use_resample = false since it is resampled above) + const auto traj_steering_rate_limited = + node_param_.enable_steering_rate_limit + ? smoother_->applySteeringRateLimit(traj_lateral_acc_filtered, false) + : traj_lateral_acc_filtered; + + // Resample trajectory with ego-velocity based interval distance + auto traj_resampled = smoother_->resampleTrajectory( + traj_steering_rate_limited, current_odometry_ptr_->twist.twist.linear.x, + current_odometry_ptr_->pose.pose, node_param_.ego_nearest_dist_threshold, + node_param_.ego_nearest_yaw_threshold); + + const size_t traj_resampled_closest = findNearestIndexFromEgo(traj_resampled); + + // Set 0[m/s] in the terminal point + if (!traj_resampled.empty()) { + traj_resampled.back().longitudinal_velocity_mps = 0.0; + } + + // Publish Closest Resample Trajectory Velocity + publishClosestVelocity( + traj_resampled, current_odometry_ptr_->pose.pose, debug_closest_max_velocity_); + + // Clip trajectory from closest point + TrajectoryPoints clipped; + clipped.insert( + clipped.end(), traj_resampled.begin() + traj_resampled_closest, traj_resampled.end()); + + // Set maximum acceleration before applying smoother. Depends on acceleration request from + // external velocity limit + const double smoother_max_acceleration = + external_velocity_limit_.acceleration_request.request + ? external_velocity_limit_.acceleration_request.max_acceleration + : get_parameter("normal.max_acc").as_double(); + const double smoother_max_jerk = external_velocity_limit_.acceleration_request.request + ? external_velocity_limit_.acceleration_request.max_jerk + : get_parameter("normal.max_jerk").as_double(); + smoother_->setMaxAccel(smoother_max_acceleration); + smoother_->setMaxJerk(smoother_max_jerk); + + std::vector debug_trajectories; + if (!smoother_->apply( + initial_motion.vel, initial_motion.acc, clipped, traj_smoothed, debug_trajectories, + publish_debug_trajs_)) { + RCLCPP_WARN(get_logger(), "Fail to solve optimization."); + } + + // Set 0 velocity after input-stop-point + overwriteStopPoint(clipped, traj_smoothed); + + traj_smoothed.insert( + traj_smoothed.begin(), traj_resampled.begin(), traj_resampled.begin() + traj_resampled_closest); + + // For the endpoint of the trajectory + if (!traj_smoothed.empty()) { + traj_smoothed.back().longitudinal_velocity_mps = 0.0; + } + + // Max velocity filter for safety + trajectory_utils::applyMaximumVelocityLimit( + traj_resampled_closest, traj_smoothed.size(), node_param_.max_velocity, traj_smoothed); + + // Insert behind velocity for output's consistency + insertBehindVelocity(traj_resampled_closest, type, traj_smoothed); + + RCLCPP_DEBUG(get_logger(), "smoothVelocity : traj_smoothed.size() = %lu", traj_smoothed.size()); + if (publish_debug_trajs_) { + { + auto tmp = traj_lateral_acc_filtered; + if (is_reverse_) flipVelocity(tmp); + pub_trajectory_latacc_filtered_->publish(toTrajectoryMsg(tmp)); + } + { + auto tmp = traj_resampled; + if (is_reverse_) flipVelocity(tmp); + pub_trajectory_resampled_->publish(toTrajectoryMsg(tmp)); + } + { + auto tmp = traj_steering_rate_limited; + if (is_reverse_) flipVelocity(tmp); + pub_trajectory_steering_rate_limited_->publish(toTrajectoryMsg(tmp)); + } + + for (auto & debug_trajectory : debug_trajectories) { + debug_trajectory.insert( + debug_trajectory.begin(), traj_resampled.begin(), + traj_resampled.begin() + traj_resampled_closest); + for (size_t i = 0; i < traj_resampled_closest; ++i) { + debug_trajectory.at(i).longitudinal_velocity_mps = + debug_trajectory.at(traj_resampled_closest).longitudinal_velocity_mps; + } + } + publishDebugTrajectories(debug_trajectories); + } + + return true; +} + +void VelocitySmootherNode::insertBehindVelocity( + const size_t output_closest, const InitializeType type, TrajectoryPoints & output) const +{ + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + + const bool keep_closest_vel_for_behind = + (type == InitializeType::EGO_VELOCITY || type == InitializeType::LARGE_DEVIATION_REPLAN || + type == InitializeType::ENGAGING); + + for (size_t i = output_closest - 1; i < output.size(); --i) { + if (keep_closest_vel_for_behind) { + output.at(i).longitudinal_velocity_mps = output.at(output_closest).longitudinal_velocity_mps; + output.at(i).acceleration_mps2 = output.at(output_closest).acceleration_mps2; + } else { + // TODO(planning/control team) deal with overlapped lanes with the same direction + const size_t seg_idx = [&]() { + // with distance and yaw thresholds + const auto opt_nearest_seg_idx = autoware::motion_utils::findNearestSegmentIndex( + prev_output_, output.at(i).pose, node_param_.ego_nearest_dist_threshold, + node_param_.ego_nearest_yaw_threshold); + if (opt_nearest_seg_idx) { + return opt_nearest_seg_idx.value(); + } + + // with distance threshold + const auto opt_second_nearest_seg_idx = autoware::motion_utils::findNearestSegmentIndex( + prev_output_, output.at(i).pose, node_param_.ego_nearest_dist_threshold); + if (opt_second_nearest_seg_idx) { + return opt_second_nearest_seg_idx.value(); + } + + return autoware::motion_utils::findNearestSegmentIndex( + prev_output_, output.at(i).pose.position); + }(); + const auto prev_output_point = + trajectory_utils::calcInterpolatedTrajectoryPoint(prev_output_, output.at(i).pose, seg_idx); + + // output should be always positive: TODO(Horibe) think better way + output.at(i).longitudinal_velocity_mps = + std::abs(prev_output_point.longitudinal_velocity_mps); + output.at(i).acceleration_mps2 = prev_output_point.acceleration_mps2; + } + } +} + +void VelocitySmootherNode::publishStopDistance(const TrajectoryPoints & trajectory) const +{ + const size_t closest = findNearestIndexFromEgo(trajectory); + + // stop distance calculation + const double stop_dist_lim{50.0}; + double stop_dist{stop_dist_lim}; + const auto stop_idx{autoware::motion_utils::searchZeroVelocityIndex(trajectory)}; + if (stop_idx) { + stop_dist = autoware::motion_utils::calcSignedArcLength(trajectory, closest, *stop_idx); + } else { + stop_dist = closest > 0 ? stop_dist : -stop_dist; + } + Float32Stamped dist_to_stopline{}; + dist_to_stopline.stamp = this->now(); + dist_to_stopline.data = std::clamp(stop_dist, -stop_dist_lim, stop_dist_lim); + pub_dist_to_stopline_->publish(dist_to_stopline); +} + +std::pair VelocitySmootherNode::calcInitialMotion( + const TrajectoryPoints & input_traj, const size_t input_closest) const +{ + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + + const double vehicle_speed = std::fabs(current_odometry_ptr_->twist.twist.linear.x); + const double vehicle_acceleration = current_acceleration_ptr_->accel.accel.linear.x; + const double target_vel = std::fabs(input_traj.at(input_closest).longitudinal_velocity_mps); + + // first time + if (!current_closest_point_from_prev_output_) { + Motion initial_motion = {vehicle_speed, 0.0}; + return {initial_motion, InitializeType::EGO_VELOCITY}; + } + + // when velocity tracking deviation is large + const double desired_vel = current_closest_point_from_prev_output_->longitudinal_velocity_mps; + const double desired_acc = current_closest_point_from_prev_output_->acceleration_mps2; + const double vel_error = vehicle_speed - std::fabs(desired_vel); + + if (std::fabs(vel_error) > node_param_.replan_vel_deviation) { + RCLCPP_DEBUG( + get_logger(), + "calcInitialMotion : Large deviation error for speed control. Use current speed for " + "initial value, desired_vel = %f, vehicle_speed = %f, vel_error = %f, error_thr = %f", + desired_vel, vehicle_speed, vel_error, node_param_.replan_vel_deviation); + Motion initial_motion = {vehicle_speed, desired_acc}; // TODO(Horibe): use current acc + return {initial_motion, InitializeType::LARGE_DEVIATION_REPLAN}; + } + + // if current vehicle velocity is low && base_desired speed is high, + // use engage_velocity for engage vehicle + const double engage_vel_thr = node_param_.engage_velocity * node_param_.engage_exit_ratio; + if (vehicle_speed < engage_vel_thr) { + if (target_vel >= node_param_.engage_velocity) { + const double stop_dist = trajectory_utils::calcStopDistance(input_traj, input_closest); + if (stop_dist > node_param_.stop_dist_to_prohibit_engage) { + RCLCPP_DEBUG( + get_logger(), + "calcInitialMotion : vehicle speed is low (%.3f), and desired speed is high (%.3f). Use " + "engage speed (%.3f) until vehicle speed reaches engage_vel_thr (%.3f). stop_dist = %.3f", + vehicle_speed, target_vel, node_param_.engage_velocity, engage_vel_thr, stop_dist); + const double engage_acceleration = + external_velocity_limit_.acceleration_request.request + ? external_velocity_limit_.acceleration_request.max_acceleration + : node_param_.engage_acceleration; + Motion initial_motion = {node_param_.engage_velocity, engage_acceleration}; + return {initial_motion, InitializeType::ENGAGING}; + } + RCLCPP_DEBUG( + get_logger(), "calcInitialMotion : stop point is close (%.3f[m]). no engage.", stop_dist); + } else if (target_vel > 0.0) { + RCLCPP_WARN_THROTTLE( + get_logger(), *clock_, 3000, + "calcInitialMotion : target velocity(%.3f[m/s]) is lower than engage velocity(%.3f[m/s]). ", + target_vel, node_param_.engage_velocity); + } + } + + // If the control mode is not AUTONOMOUS (vehicle is not under control of the planning module), + // use ego velocity/acceleration in the planning for smooth transition from MANUAL to AUTONOMOUS. + if (node_param_.plan_from_ego_speed_on_manual_mode) { // could be false for debug purpose + const bool is_in_autonomous_control = operation_mode_.is_autoware_control_enabled && + (operation_mode_.mode == OperationModeState::AUTONOMOUS || + operation_mode_.mode == OperationModeState::STOP); + if (!is_in_autonomous_control) { + RCLCPP_INFO_THROTTLE( + get_logger(), *clock_, 10000, "Not in autonomous control. Plan from ego velocity."); + // We should plan from the current vehicle speed, but if the initial value is greater than the + // velocity limit, the current planning algorithm decelerates with a very high deceleration. + // To avoid this, we set the initial value of the vehicle speed to be below the speed limit. + const auto p = smoother_->getBaseParam(); + const auto v0 = std::min(target_vel, vehicle_speed); + const auto a0 = std::clamp(vehicle_acceleration, p.min_decel, p.max_accel); + const Motion initial_motion = {v0, a0}; + return {initial_motion, InitializeType::EGO_VELOCITY}; + } + } + + // normal update: use closest in current_closest_point_from_prev_output + Motion initial_motion = {desired_vel, desired_acc}; + RCLCPP_DEBUG( + get_logger(), + "calcInitialMotion : normal update. v0 = %f, a0 = %f, vehicle_speed = %f, target_vel = %f", + initial_motion.vel, initial_motion.acc, vehicle_speed, target_vel); + return {initial_motion, InitializeType::NORMAL}; +} + +void VelocitySmootherNode::overwriteStopPoint( + const TrajectoryPoints & input, TrajectoryPoints & output) const +{ + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + + const auto stop_idx = autoware::motion_utils::searchZeroVelocityIndex(input); + if (!stop_idx) { + return; + } + + // Get Closest Point from Output + // TODO(planning/control team) deal with overlapped lanes with the same directions + const auto nearest_output_point_idx = autoware::motion_utils::findNearestIndex( + output, input.at(*stop_idx).pose, node_param_.ego_nearest_dist_threshold, + node_param_.ego_nearest_yaw_threshold); + + // check over velocity + bool is_stop_velocity_exceeded{false}; + double input_stop_vel{}; + double output_stop_vel{}; + if (nearest_output_point_idx) { + double optimized_stop_point_vel = + output.at(*nearest_output_point_idx).longitudinal_velocity_mps; + is_stop_velocity_exceeded = (optimized_stop_point_vel > over_stop_velocity_warn_thr_); + input_stop_vel = input.at(*stop_idx).longitudinal_velocity_mps; + output_stop_vel = output.at(*nearest_output_point_idx).longitudinal_velocity_mps; + trajectory_utils::applyMaximumVelocityLimit( + *nearest_output_point_idx, output.size(), 0.0, output); + RCLCPP_DEBUG( + get_logger(), + "replan : input_stop_idx = %lu, stop velocity : input = %f, output = %f, thr = %f", + *nearest_output_point_idx, input_stop_vel, output_stop_vel, over_stop_velocity_warn_thr_); + } else { + input_stop_vel = -1.0; + output_stop_vel = -1.0; + RCLCPP_DEBUG( + get_logger(), + "replan : input_stop_idx = -1, stop velocity : input = %f, output = %f, thr = %f", + input_stop_vel, output_stop_vel, over_stop_velocity_warn_thr_); + } + + { + StopSpeedExceeded msg{}; + msg.stamp = this->now(); + msg.stop_speed_exceeded = is_stop_velocity_exceeded; + pub_over_stop_velocity_->publish(msg); + } +} + +void VelocitySmootherNode::applyExternalVelocityLimit(TrajectoryPoints & traj) const +{ + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + + if (traj.size() < 1) { + return; + } + + trajectory_utils::applyMaximumVelocityLimit( + 0, traj.size(), max_velocity_with_deceleration_, traj); + + // insert the point at the distance of external velocity limit + const auto & current_pose = current_odometry_ptr_->pose.pose; + const size_t closest_seg_idx = + autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + traj, current_pose, node_param_.ego_nearest_dist_threshold, + node_param_.ego_nearest_yaw_threshold); + const auto inserted_index = + autoware::motion_utils::insertTargetPoint(closest_seg_idx, external_velocity_limit_.dist, traj); + if (!inserted_index) { + traj.back().longitudinal_velocity_mps = std::min( + traj.back().longitudinal_velocity_mps, static_cast(external_velocity_limit_.velocity)); + return; + } + + // apply external velocity limit from the inserted point + trajectory_utils::applyMaximumVelocityLimit( + *inserted_index, traj.size(), external_velocity_limit_.velocity, traj); + + // create virtual wall + if (std::abs(external_velocity_limit_.velocity) < 1e-3) { + const auto virtual_wall_marker = autoware::motion_utils::createStopVirtualWallMarker( + traj.at(*inserted_index).pose, external_velocity_limit_.sender, this->now(), 0, + base_link2front_); + pub_virtual_wall_->publish(virtual_wall_marker); + } + + RCLCPP_DEBUG( + get_logger(), "externalVelocityLimit : limit_vel = %.3f", external_velocity_limit_.velocity); +} + +void VelocitySmootherNode::applyStopApproachingVelocity(TrajectoryPoints & traj) const +{ + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + + const auto stop_idx = autoware::motion_utils::searchZeroVelocityIndex(traj); + if (!stop_idx) { + return; // no stop point. + } + double distance_sum = 0.0; + for (size_t i = *stop_idx - 1; i < traj.size(); --i) { // search backward + distance_sum += autoware::universe_utils::calcDistance2d(traj.at(i), traj.at(i + 1)); + if (distance_sum > node_param_.stopping_distance) { + break; + } + if (traj.at(i).longitudinal_velocity_mps > node_param_.stopping_velocity) { + traj.at(i).longitudinal_velocity_mps = node_param_.stopping_velocity; + } + } +} + +void VelocitySmootherNode::publishDebugTrajectories( + const std::vector & debug_trajectories) const +{ + auto debug_trajectories_tmp = debug_trajectories; + if (node_param_.algorithm_type == AlgorithmType::JERK_FILTERED) { + if (debug_trajectories_tmp.size() != 3) { + RCLCPP_DEBUG(get_logger(), "Size of the debug trajectories is incorrect"); + return; + } + if (is_reverse_) { + flipVelocity(debug_trajectories_tmp.at(0)); + flipVelocity(debug_trajectories_tmp.at(1)); + flipVelocity(debug_trajectories_tmp.at(2)); + } + pub_forward_filtered_trajectory_->publish(toTrajectoryMsg(debug_trajectories_tmp.at(0))); + pub_backward_filtered_trajectory_->publish(toTrajectoryMsg(debug_trajectories_tmp.at(1))); + pub_merged_filtered_trajectory_->publish(toTrajectoryMsg(debug_trajectories_tmp.at(2))); + publishClosestVelocity( + debug_trajectories_tmp.at(2), current_odometry_ptr_->pose.pose, pub_closest_merged_velocity_); + } +} + +void VelocitySmootherNode::publishClosestVelocity( + const TrajectoryPoints & trajectory, const Pose & current_pose, + const rclcpp::Publisher::SharedPtr pub) const +{ + const auto closest_point = calcProjectedTrajectoryPoint(trajectory, current_pose); + + Float32Stamped vel_data{}; + vel_data.stamp = this->now(); + vel_data.data = std::max(closest_point.longitudinal_velocity_mps, static_cast(0.0)); + pub->publish(vel_data); +} + +void VelocitySmootherNode::publishClosestState(const TrajectoryPoints & trajectory) +{ + const auto closest_point = calcProjectedTrajectoryPointFromEgo(trajectory); + + auto publishFloat = [=](const double data, const auto pub) { + Float32Stamped msg{}; + msg.stamp = this->now(); + msg.data = data; + pub->publish(msg); + return; + }; + + const double curr_vel{closest_point.longitudinal_velocity_mps}; + const double curr_acc{closest_point.acceleration_mps2}; + if (!prev_time_) { + prev_time_ = std::make_shared(this->now()); + prev_acc_ = curr_acc; + return; + } + + // Calculate jerk + rclcpp::Time curr_time{this->now()}; + double dt = (curr_time - *prev_time_).seconds(); + double curr_jerk = (curr_acc - prev_acc_) / dt; + + // Publish data + publishFloat(curr_vel, debug_closest_velocity_); + publishFloat(curr_acc, debug_closest_acc_); + publishFloat(curr_jerk, debug_closest_jerk_); + + // Update + prev_acc_ = curr_acc; + *prev_time_ = curr_time; +} + +void VelocitySmootherNode::updatePrevValues(const TrajectoryPoints & final_result) +{ + prev_output_ = final_result; + prev_closest_point_ = calcProjectedTrajectoryPointFromEgo(final_result); +} + +VelocitySmootherNode::AlgorithmType VelocitySmootherNode::getAlgorithmType( + const std::string & algorithm_name) const +{ + if (algorithm_name == "JerkFiltered") { + return AlgorithmType::JERK_FILTERED; + } + + throw std::domain_error("[VelocitySmootherNode] undesired algorithm is selected."); +} + +double VelocitySmootherNode::calcTravelDistance() const +{ + const auto closest_point = calcProjectedTrajectoryPointFromEgo(prev_output_); + + if (prev_closest_point_) { + const double travel_dist = + autoware::universe_utils::calcDistance2d(*prev_closest_point_, closest_point); + return travel_dist; + } + + return 0.0; +} + +bool VelocitySmootherNode::isEngageStatus(const double target_vel) const +{ + const double vehicle_speed = std::fabs(current_odometry_ptr_->twist.twist.linear.x); + const double engage_vel_thr = node_param_.engage_velocity * node_param_.engage_exit_ratio; + return vehicle_speed < engage_vel_thr && target_vel >= node_param_.engage_velocity; +} + +Trajectory VelocitySmootherNode::toTrajectoryMsg( + const TrajectoryPoints & points, const std_msgs::msg::Header * header) const +{ + auto trajectory = autoware::motion_utils::convertToTrajectory(points); + trajectory.header = header ? *header : base_traj_raw_ptr_->header; + return trajectory; +} + +size_t VelocitySmootherNode::findNearestIndexFromEgo(const TrajectoryPoints & points) const +{ + return autoware::motion_utils::findFirstNearestIndexWithSoftConstraints( + points, current_odometry_ptr_->pose.pose, node_param_.ego_nearest_dist_threshold, + node_param_.ego_nearest_yaw_threshold); +} + +bool VelocitySmootherNode::isReverse(const TrajectoryPoints & points) const +{ + if (points.empty()) return true; + + return std::any_of( + points.begin(), points.end(), [](const auto & pt) { return pt.longitudinal_velocity_mps < 0; }); +} +void VelocitySmootherNode::flipVelocity(TrajectoryPoints & points) const +{ + for (auto & pt : points) { + pt.longitudinal_velocity_mps *= -1.0; + } +} + +void VelocitySmootherNode::publishStopWatchTime() +{ + Float64Stamped calculation_time_data{}; + calculation_time_data.stamp = this->now(); + calculation_time_data.data = stop_watch_.toc(); + debug_calculation_time_->publish(calculation_time_data); +} + +TrajectoryPoint VelocitySmootherNode::calcProjectedTrajectoryPoint( + const TrajectoryPoints & trajectory, const Pose & pose) const +{ + const size_t current_seg_idx = + autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + trajectory, pose, node_param_.ego_nearest_dist_threshold, + node_param_.ego_nearest_yaw_threshold); + return trajectory_utils::calcInterpolatedTrajectoryPoint(trajectory, pose, current_seg_idx); +} + +TrajectoryPoint VelocitySmootherNode::calcProjectedTrajectoryPointFromEgo( + const TrajectoryPoints & trajectory) const +{ + return calcProjectedTrajectoryPoint(trajectory, current_odometry_ptr_->pose.pose); +} + +} // namespace autoware::velocity_smoother + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::velocity_smoother::VelocitySmootherNode) diff --git a/planning/autoware_velocity_smoother/src/resample.cpp b/planning/autoware_velocity_smoother/src/resample.cpp new file mode 100644 index 0000000000..f69ad5a592 --- /dev/null +++ b/planning/autoware_velocity_smoother/src/resample.cpp @@ -0,0 +1,269 @@ +// Copyright 2021 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware/velocity_smoother/resample.hpp" + +#include "autoware/motion_utils/resample/resample.hpp" +#include "autoware/motion_utils/trajectory/conversion.hpp" +#include "autoware/motion_utils/trajectory/trajectory.hpp" +#include "autoware/velocity_smoother/trajectory_utils.hpp" + +#include +#include + +namespace autoware::velocity_smoother +{ +namespace resampling +{ +TrajectoryPoints resampleTrajectory( + const TrajectoryPoints & input, const double v_current, + const geometry_msgs::msg::Pose & current_pose, const double nearest_dist_threshold, + const double nearest_yaw_threshold, const ResampleParam & param, const bool use_zoh_for_v) +{ + // Arc length from the initial point to the closest point + const size_t current_seg_idx = + autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + input, current_pose, nearest_dist_threshold, nearest_yaw_threshold); + const double negative_front_arclength_value = autoware::motion_utils::calcSignedArcLength( + input, current_pose.position, current_seg_idx, input.at(0).pose.position, 0); + const auto front_arclength_value = std::fabs(negative_front_arclength_value); + + const auto dist_to_closest_stop_point = + autoware::motion_utils::calcDistanceToForwardStopPoint(input, current_pose); + + // Get the resample size from the closest point + const double trajectory_length = autoware::motion_utils::calcArcLength(input); + const double Nt = param.resample_time / std::max(param.dense_resample_dt, 0.001); + const double ds_nominal = + std::max(v_current * param.dense_resample_dt, param.dense_min_interval_distance); + const double Ns = param.min_trajectory_length / std::max(ds_nominal, 0.001); + const double N = std::max(Nt, Ns); + std::vector out_arclength; + + // Step1. Resample front trajectory + constexpr double front_ds = 0.1; + for (double ds = 0.0; ds <= front_arclength_value; ds += front_ds) { + out_arclength.push_back(ds); + } + if (std::fabs(out_arclength.back() - front_arclength_value) < 1e-3) { + out_arclength.back() = front_arclength_value; + } else { + out_arclength.push_back(front_arclength_value); + } + + // Step2. Resample behind trajectory + double dist_i = 0.0; + bool is_zero_point_included = false; + bool is_endpoint_included = false; + for (size_t i = 1; static_cast(i) <= N; ++i) { + double ds = ds_nominal; + if (i > Nt) { + // if the planning time is not enough to see the desired distance, + // change the interval distance to see far. + ds = std::max(param.sparse_min_interval_distance, param.sparse_resample_dt * v_current); + } + + dist_i += ds; + + // Check if the distance is longer than max_trajectory_length + if (dist_i > param.max_trajectory_length) { + break; // distance is over max. + } + + // Check if the distance is longer than input arclength + if (dist_i + front_arclength_value >= trajectory_length) { + is_endpoint_included = true; // distance is over input endpoint. + break; + } + + // Check if the distance is longer than minimum_trajectory_length + if (i > Nt && dist_i >= param.min_trajectory_length) { + if ( + std::fabs(out_arclength.back() - (param.min_trajectory_length + front_arclength_value)) < + 1e-3) { + out_arclength.back() = param.min_trajectory_length + front_arclength_value; + } else { + out_arclength.push_back(param.min_trajectory_length + front_arclength_value); + } + break; + } + + // Handle Close Stop Point + if ( + dist_to_closest_stop_point && dist_i > *dist_to_closest_stop_point && + !is_zero_point_included) { + if (std::fabs(dist_i - *dist_to_closest_stop_point) > 1e-3) { + // dist_i is much bigger than zero_vel_arclength_value + if ( + !out_arclength.empty() && + std::fabs(out_arclength.back() - (*dist_to_closest_stop_point + front_arclength_value)) < + 1e-3) { + out_arclength.back() = *dist_to_closest_stop_point + front_arclength_value; + } else { + out_arclength.push_back(*dist_to_closest_stop_point + front_arclength_value); + } + } else { + // dist_i is close to the zero_vel_arclength_value + dist_i = *dist_to_closest_stop_point; + } + + is_zero_point_included = true; + } + + out_arclength.push_back(dist_i + front_arclength_value); + } + + if (input.size() < 2 || out_arclength.size() < 2 || trajectory_length < out_arclength.back()) { + return input; + } + + const auto output_traj = autoware::motion_utils::resampleTrajectory( + autoware::motion_utils::convertToTrajectory(input), out_arclength, false, true, use_zoh_for_v); + auto output = autoware::motion_utils::convertToTrajectoryPointArray(output_traj); + + // add end point directly to consider the endpoint velocity. + if (is_endpoint_included) { + constexpr double ep_dist = 1.0E-3; + if (autoware::universe_utils::calcDistance2d(output.back(), input.back()) < ep_dist) { + output.back() = input.back(); + } else { + output.push_back(input.back()); + } + } + + return output; +} + +TrajectoryPoints resampleTrajectory( + const TrajectoryPoints & input, const geometry_msgs::msg::Pose & current_pose, + const double nearest_dist_threshold, const double nearest_yaw_threshold, + const ResampleParam & param, const double nominal_ds, const bool use_zoh_for_v) +{ + // input arclength + const double trajectory_length = autoware::motion_utils::calcArcLength(input); + const auto dist_to_closest_stop_point = + autoware::motion_utils::calcDistanceToForwardStopPoint(input, current_pose); + + // distance to stop point + double stop_arclength_value = param.max_trajectory_length; + if (dist_to_closest_stop_point) { + stop_arclength_value = std::min(*dist_to_closest_stop_point, stop_arclength_value); + } + if (param.min_trajectory_length < stop_arclength_value) { + stop_arclength_value = param.min_trajectory_length; + } + + // Do dense resampling before the stop line(3[m] ahead of the stop line) + const double start_stop_arclength_value = std::max(stop_arclength_value - 3.0, 0.0); + + std::vector out_arclength; + + // Step1. Resample front trajectory + // Arc length from the initial point to the closest point + const size_t current_seg_idx = + autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + input, current_pose, nearest_dist_threshold, nearest_yaw_threshold); + const double negative_front_arclength_value = autoware::motion_utils::calcSignedArcLength( + input, current_pose.position, current_seg_idx, input.at(0).pose.position, + static_cast(0)); + const auto front_arclength_value = std::fabs(negative_front_arclength_value); + for (double s = 0.0; s <= front_arclength_value; s += nominal_ds) { + out_arclength.push_back(s); + } + if (std::fabs(out_arclength.back() - front_arclength_value) < 1e-3) { + out_arclength.back() = front_arclength_value; + } else { + out_arclength.push_back(front_arclength_value); + } + + // Step2. Resample behind trajectory + double dist_i{0.0}; + bool is_zero_point_included{false}; + bool is_endpoint_included{false}; + while (rclcpp::ok()) { + double ds = nominal_ds; + if (start_stop_arclength_value <= dist_i && dist_i <= stop_arclength_value) { + // Dense sampling before the stop point + ds = 0.01; + } + dist_i += ds; + + // Check if the distance is longer than max_trajectory_length + if (dist_i > param.max_trajectory_length) { + break; // distance is over max. + } + + // Check if the distance is longer than input arclength + if (dist_i + front_arclength_value >= trajectory_length) { + is_endpoint_included = true; // distance is over input endpoint. + break; + } + + // Check if the distance is longer than minimum_trajectory_length + if (dist_i >= param.min_trajectory_length) { + if ( + std::fabs(out_arclength.back() - (param.min_trajectory_length + front_arclength_value)) < + 1e-3) { + out_arclength.back() = param.min_trajectory_length + front_arclength_value; + } else { + out_arclength.push_back(param.min_trajectory_length + front_arclength_value); + } + break; + } + + // Handle Close Stop Point + if (dist_i > stop_arclength_value && !is_zero_point_included) { + if (std::fabs(dist_i - stop_arclength_value) > 1e-3) { + // dist_i is much bigger than zero_vel_arclength_value + if ( + !out_arclength.empty() && + std::fabs(out_arclength.back() - (stop_arclength_value + front_arclength_value)) < 1e-3) { + out_arclength.back() = stop_arclength_value + front_arclength_value; + } else { + out_arclength.push_back(stop_arclength_value + front_arclength_value); + } + } else { + // dist_i is close to the zero_vel_arclength_value + dist_i = stop_arclength_value; + } + + is_zero_point_included = true; + } + + out_arclength.push_back(dist_i + front_arclength_value); + } + + if (input.size() < 2 || out_arclength.size() < 2 || trajectory_length < out_arclength.back()) { + return input; + } + + const auto output_traj = autoware::motion_utils::resampleTrajectory( + autoware::motion_utils::convertToTrajectory(input), out_arclength, false, true, use_zoh_for_v); + auto output = autoware::motion_utils::convertToTrajectoryPointArray(output_traj); + + // add end point directly to consider the endpoint velocity. + if (is_endpoint_included) { + constexpr double ep_dist = 1.0E-3; + if (autoware::universe_utils::calcDistance2d(output.back(), input.back()) < ep_dist) { + output.back() = input.back(); + } else { + output.push_back(input.back()); + } + } + + return output; +} + +} // namespace resampling +} // namespace autoware::velocity_smoother diff --git a/planning/autoware_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp b/planning/autoware_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp new file mode 100644 index 0000000000..0f61171a3b --- /dev/null +++ b/planning/autoware_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp @@ -0,0 +1,495 @@ +// Copyright 2021 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware/velocity_smoother/smoother/jerk_filtered_smoother.hpp" + +#include "autoware/qp_interface/proxqp_interface.hpp" +#include "autoware/velocity_smoother/trajectory_utils.hpp" + +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#define VERBOSE_TRAJECTORY_VELOCITY false + +namespace autoware::velocity_smoother +{ +JerkFilteredSmoother::JerkFilteredSmoother( + rclcpp::Node & node, const std::shared_ptr time_keeper) +: SmootherBase(node, time_keeper) +{ + auto & p = smoother_param_; + p.jerk_weight = node.declare_parameter("jerk_weight"); + p.over_v_weight = node.declare_parameter("over_v_weight"); + p.over_a_weight = node.declare_parameter("over_a_weight"); + p.over_j_weight = node.declare_parameter("over_j_weight"); + p.jerk_filter_ds = node.declare_parameter("jerk_filter_ds"); + + qp_interface_ = + std::make_shared(false, 20000, 1.0e-8, 1.0e-6, false); +} + +void JerkFilteredSmoother::setParam(const Param & smoother_param) +{ + smoother_param_ = smoother_param; +} + +JerkFilteredSmoother::Param JerkFilteredSmoother::getParam() const +{ + return smoother_param_; +} + +bool JerkFilteredSmoother::apply( + const double v0, const double a0, const TrajectoryPoints & input, TrajectoryPoints & output, + std::vector & debug_trajectories, const bool publish_debug_trajs) +{ + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + + output = input; + + if (input.empty()) { + RCLCPP_WARN(logger_, "Input TrajectoryPoints to the jerk filtered optimization is empty."); + return false; + } + + if (input.size() == 1) { + // No need to do optimization + output.front().longitudinal_velocity_mps = v0; + output.front().acceleration_mps2 = a0; + debug_trajectories.resize(3); + debug_trajectories[0] = output; + debug_trajectories[1] = output; + debug_trajectories[2] = output; + return true; + } + + const auto ts = std::chrono::system_clock::now(); + + const double a_max = base_param_.max_accel; + const double a_min = base_param_.min_decel; + const double a_stop_accel = 0.0; + const double a_stop_decel = base_param_.stop_decel; + const double j_max = base_param_.max_jerk; + const double j_min = base_param_.min_jerk; + const double over_j_weight = smoother_param_.over_j_weight; + const double over_v_weight = smoother_param_.over_v_weight; + const double over_a_weight = smoother_param_.over_a_weight; + + // jerk filter + const auto forward_filtered = + forwardJerkFilter(v0, std::max(a0, a_min), a_max, a_stop_accel, j_max, input); + const auto backward_filtered = backwardJerkFilter( + input.back().longitudinal_velocity_mps, a_stop_decel, a_min, a_stop_decel, j_min, input); + const auto filtered = + mergeFilteredTrajectory(v0, a0, a_min, j_min, forward_filtered, backward_filtered); + + // Resample TrajectoryPoints for Optimization + // TODO(planning/control team) deal with overlapped lanes with the same direction + const auto initial_traj_pose = filtered.front().pose; + + const auto resample = [&](const auto & trajectory) { + autoware::universe_utils::ScopedTimeTrack st("resample", *time_keeper_); + + return resampling::resampleTrajectory( + trajectory, v0, initial_traj_pose, std::numeric_limits::max(), + std::numeric_limits::max(), base_param_.resample_param); + }; + + auto opt_resampled_trajectory = resample(filtered); + + // Set debug trajectories + if (publish_debug_trajs) { + debug_trajectories.resize(3); + debug_trajectories[0] = resample(forward_filtered); + debug_trajectories[1] = resample(backward_filtered); + debug_trajectories[2] = resample(filtered); + } + + // Ensure terminal velocity is zero + opt_resampled_trajectory.back().longitudinal_velocity_mps = 0.0; + + // If Resampled Size is too small, we don't do optimization + if (opt_resampled_trajectory.size() == 1) { + // No need to do optimization + output.front().longitudinal_velocity_mps = v0; + output.front().acceleration_mps2 = a0; + if (publish_debug_trajs) { + debug_trajectories.resize(3); + debug_trajectories[0] = output; + debug_trajectories[1] = output; + debug_trajectories[2] = output; + } + return true; + } + + // to avoid getting 0 as a stop point, search zero velocity index from 1. + // the size of the resampled trajectory must not be less than 2. + const auto zero_vel_id = autoware::motion_utils::searchZeroVelocityIndex( + opt_resampled_trajectory, 1, opt_resampled_trajectory.size()); + + if (!zero_vel_id) { + RCLCPP_WARN(logger_, "opt_resampled_trajectory must have stop point."); + return false; + } + + // Clip trajectory from 0 to zero_vel_id (the size becomes zero_vel_id_ + 1) + const size_t N = *zero_vel_id + 1; + + output = opt_resampled_trajectory; + + const std::vector interval_dist_arr = + trajectory_utils::calcTrajectoryIntervalDistance(opt_resampled_trajectory); + + std::vector v_max_arr(N, 0.0); + for (size_t i = 0; i < N; ++i) { + v_max_arr.at(i) = opt_resampled_trajectory.at(i).longitudinal_velocity_mps; + } + + time_keeper_->start_track("initOptimization"); + /* + * x = [ + * b[0], b[1], ..., b[N], : 0~N + * a[0], a[1], .... a[N], : N~2N + * delta[0], ..., delta[N], : 2N~3N + * sigma[0], sigma[1], ...., sigma[N], : 3N~4N + * gamma[0], gamma[1], ..., gamma[N] : 4N~5N + * ] + * + * b[i] : velocity^2 + * delta : 0 < b[i]-delta[i] < max_vel[i]*max_vel[i] + * sigma : a_min < a[i] - sigma[i] < a_max + * gamma : jerk_min < pseudo_jerk[i] * ref_vel[i] - gamma[i] < jerk_max + */ + const uint32_t IDX_B0 = 0; + const uint32_t IDX_A0 = N; + const uint32_t IDX_DELTA0 = 2 * N; + const uint32_t IDX_SIGMA0 = 3 * N; + const uint32_t IDX_GAMMA0 = 4 * N; + + const uint32_t l_variables = 5 * N; + const uint32_t l_constraints = 4 * N + 1; + + // the matrix size depends on constraint numbers. + Eigen::MatrixXd A = Eigen::MatrixXd::Zero(l_constraints, l_variables); + + std::vector lower_bound(l_constraints, 0.0); + std::vector upper_bound(l_constraints, 0.0); + + Eigen::MatrixXd P = Eigen::MatrixXd::Zero(l_variables, l_variables); + std::vector q(l_variables, 0.0); + + /**************************************************************/ + /**************************************************************/ + /**************** design objective function *******************/ + /**************************************************************/ + /**************************************************************/ + + // jerk: d(ai)/ds * v_ref -> minimize weight * ((a1 - a0) / ds * v_ref)^2 * ds + const double smooth_weight = smoother_param_.jerk_weight; + for (size_t i = 0; i < N - 1; ++i) { + const double ref_vel = 0.5 * (v_max_arr.at(i) + v_max_arr.at(i + 1)); + const double interval_dist = std::max(interval_dist_arr.at(i), 0.0001); + const double w_x_ds_inv = (1.0 / interval_dist) * ref_vel; + P(IDX_A0 + i, IDX_A0 + i) += smooth_weight * w_x_ds_inv * w_x_ds_inv * interval_dist; + P(IDX_A0 + i, IDX_A0 + i + 1) -= smooth_weight * w_x_ds_inv * w_x_ds_inv * interval_dist; + P(IDX_A0 + i + 1, IDX_A0 + i) -= smooth_weight * w_x_ds_inv * w_x_ds_inv * interval_dist; + P(IDX_A0 + i + 1, IDX_A0 + i + 1) += smooth_weight * w_x_ds_inv * w_x_ds_inv * interval_dist; + } + + // |v_max_i^2 - b_i|/v_max^2 -> minimize (-bi) * ds / v_max^2 + for (size_t i = 0; i < N; ++i) { + if (v_max_arr.at(i) > 0.01) { + // Note that if v_max[i] is too small, we did not minimize the corresponding -b[i] + double v_weight_term = -1.0 / (v_max_arr.at(i) * v_max_arr.at(i)); + if (i < N - 1) { + v_weight_term *= std::max(interval_dist_arr.at(i), 0.0001); + } + q.at(IDX_B0 + i) += v_weight_term; + } + P(IDX_DELTA0 + i, IDX_DELTA0 + i) += over_v_weight; // over velocity cost + P(IDX_SIGMA0 + i, IDX_SIGMA0 + i) += over_a_weight; // over acceleration cost + P(IDX_GAMMA0 + i, IDX_GAMMA0 + i) += over_j_weight; // over jerk cost + } + + /**************************************************************/ + /**************************************************************/ + /**************** design constraint matrix ********************/ + /**************************************************************/ + /**************************************************************/ + + /* + NOTE: The delta allows b to be negative. This is actually invalid because the definition is b=v^2. + But mathematically, the strict b>0 constraint may make the problem infeasible, such as the case of + v=0 & a<0. To avoid the infeasibility, we allow b<0. The negative b is dealt as b=0 when it is + converted to v with sqrt. If the weight of delta^2 is large (the value of delta is very small), + b is almost 0, and is not a big problem. + */ + + size_t constr_idx = 0; + + // Soft Constraint Velocity Limit: 0 < b - delta < v_max^2 + for (size_t i = 0; i < N; ++i, ++constr_idx) { + A(constr_idx, IDX_B0 + i) = 1.0; // b_i + A(constr_idx, IDX_DELTA0 + i) = -1.0; // -delta_i + upper_bound[constr_idx] = v_max_arr.at(i) * v_max_arr.at(i); + lower_bound[constr_idx] = 0.0; + } + + // Soft Constraint Acceleration Limit: a_min < a - sigma < a_max + for (size_t i = 0; i < N; ++i, ++constr_idx) { + A(constr_idx, IDX_A0 + i) = 1.0; // a_i + A(constr_idx, IDX_SIGMA0 + i) = -1.0; // -sigma_i + + constexpr double stop_vel = 1e-3; + if (v_max_arr.at(i) < stop_vel) { + // Stop Point + upper_bound[constr_idx] = a_stop_decel; + lower_bound[constr_idx] = a_stop_decel; + } else { + upper_bound[constr_idx] = a_max; + lower_bound[constr_idx] = a_min; + } + } + + // Soft Constraint Jerk Limit: jerk_min < pseudo_jerk[i] * ref_vel[i] - gamma[i] < jerk_max + // -> jerk_min * ds < (a[i+1] - a[i]) * ref_vel[i] - gamma[i] * ds < jerk_max * ds + for (size_t i = 0; i < N - 1; ++i, ++constr_idx) { + const double ref_vel = 0.5 * (v_max_arr.at(i) + v_max_arr.at(i + 1)); + const double ds = interval_dist_arr.at(i); + A(constr_idx, IDX_A0 + i) = -ref_vel; // -a[i] * ref_vel + A(constr_idx, IDX_A0 + i + 1) = ref_vel; // a[i+1] * ref_vel + A(constr_idx, IDX_GAMMA0 + i) = -ds; // -gamma[i] * ds + upper_bound[constr_idx] = j_max * ds; // jerk_max * ds + lower_bound[constr_idx] = j_min * ds; // jerk_min * ds + } + + // b' = 2a ... (b(i+1) - b(i)) / ds = 2a(i) + for (size_t i = 0; i < N - 1; ++i, ++constr_idx) { + A(constr_idx, IDX_B0 + i) = -1.0; // b(i) + A(constr_idx, IDX_B0 + i + 1) = 1.0; // b(i+1) + A(constr_idx, IDX_A0 + i) = -2.0 * interval_dist_arr.at(i); // a(i) * ds + upper_bound[constr_idx] = 0.0; + lower_bound[constr_idx] = 0.0; + } + + // initial condition + { + A(constr_idx, IDX_B0) = 1.0; // b0 + upper_bound[constr_idx] = v0 * v0; + lower_bound[constr_idx] = v0 * v0; + ++constr_idx; + + A(constr_idx, IDX_A0) = 1.0; // a0 + upper_bound[constr_idx] = a0; + lower_bound[constr_idx] = a0; + ++constr_idx; + } + time_keeper_->end_track("initOptimization"); + + // execute optimization + time_keeper_->start_track("optimize"); + const auto optval = qp_interface_->optimize(P, A, q, lower_bound, upper_bound); + time_keeper_->end_track("optimize"); + if (!qp_interface_->isSolved()) { + RCLCPP_WARN(logger_, "optimization failed : %s", qp_interface_->getStatus().c_str()); + return false; + } + + const auto has_nan = + std::any_of(optval.begin(), optval.end(), [](const auto v) { return std::isnan(v); }); + if (has_nan) { + RCLCPP_WARN(logger_, "optimization failed: result contains NaN values"); + return false; + } + + const auto tf1 = std::chrono::system_clock::now(); + const double dt_ms1 = + std::chrono::duration_cast(tf1 - ts).count() * 1.0e-6; + RCLCPP_DEBUG(logger_, "optimization time = %f [ms]", dt_ms1); + + // get velocity & acceleration + for (size_t i = 0; i < N; ++i) { + double b = optval.at(IDX_B0 + i); + output.at(i).longitudinal_velocity_mps = std::sqrt(std::max(b, 0.0)); + output.at(i).acceleration_mps2 = optval.at(IDX_A0 + i); + } + for (size_t i = N; i < output.size(); ++i) { + output.at(i).longitudinal_velocity_mps = 0.0; + output.at(i).acceleration_mps2 = a_stop_decel; + } + + if (VERBOSE_TRAJECTORY_VELOCITY) { + const auto s_output = trajectory_utils::calcArclengthArray(output); + + std::cerr << "\n\n" << std::endl; + for (size_t i = 0; i < N; ++i) { + const auto v_opt = output.at(i).longitudinal_velocity_mps; + const auto a_opt = output.at(i).acceleration_mps2; + const auto ds = i < interval_dist_arr.size() ? interval_dist_arr.at(i) : 0.0; + const auto v_rs = i < opt_resampled_trajectory.size() + ? opt_resampled_trajectory.at(i).longitudinal_velocity_mps + : 0.0; + RCLCPP_INFO( + logger_, "i = %4lu | s: %5f | ds: %5f | rs: %9f | op_v: %10f | op_a: %10f |", i, + s_output.at(i), ds, v_rs, v_opt, a_opt); + } + } + + return true; +} + +TrajectoryPoints JerkFilteredSmoother::forwardJerkFilter( + const double v0, const double a0, const double a_max, const double a_start, const double j_max, + const TrajectoryPoints & input) const +{ + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + + auto applyLimits = [&input, &a_start](double & v, double & a, size_t i) { + double v_lim = input.at(i).longitudinal_velocity_mps; + static constexpr double ep = 1.0e-5; + if (v > v_lim + ep) { + v = v_lim; + a = 0.0; + + if (v_lim < 1e-3 && i < input.size() - 1) { + double next_v_lim = input.at(i + 1).longitudinal_velocity_mps; + if (next_v_lim >= 1e-3) { + a = a_start; // start from stop velocity + } + } + } + + if (v < 0.0) { + v = a = 0.0; + } + }; + + auto output = input; + + double current_vel = v0; + double current_acc = a0; + applyLimits(current_vel, current_acc, 0); + + output.front().longitudinal_velocity_mps = current_vel; + output.front().acceleration_mps2 = current_acc; + for (size_t i = 1; i < input.size(); ++i) { + const double ds = autoware::universe_utils::calcDistance2d(input.at(i), input.at(i - 1)); + const double max_dt = std::pow(6.0 * ds / j_max, 1.0 / 3.0); // assuming v0 = a0 = 0. + const double dt = std::min(ds / std::max(current_vel, 1.0e-6), max_dt); + + if (current_acc + j_max * dt >= a_max) { + const double tmp_jerk = std::min((a_max - current_acc) / dt, j_max); + current_vel = current_vel + current_acc * dt + 0.5 * tmp_jerk * dt * dt; + current_acc = a_max; + } else { + current_vel = current_vel + current_acc * dt + 0.5 * j_max * dt * dt; + current_acc = current_acc + j_max * dt; + } + applyLimits(current_vel, current_acc, i); + output.at(i).longitudinal_velocity_mps = current_vel; + output.at(i).acceleration_mps2 = current_acc; + } + return output; +} + +TrajectoryPoints JerkFilteredSmoother::backwardJerkFilter( + const double v0, const double a0, const double a_min, const double a_stop, const double j_min, + const TrajectoryPoints & input) const +{ + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + + auto input_rev = input; + std::reverse(input_rev.begin(), input_rev.end()); + auto filtered = forwardJerkFilter( + v0, std::fabs(a0), std::fabs(a_min), std::fabs(a_stop), std::fabs(j_min), input_rev); + std::reverse(filtered.begin(), filtered.end()); + for (size_t i = 0; i < filtered.size(); ++i) { + filtered.at(i).acceleration_mps2 *= -1.0; // Deceleration + } + return filtered; +} + +TrajectoryPoints JerkFilteredSmoother::mergeFilteredTrajectory( + const double v0, const double a0, const double a_min, const double j_min, + const TrajectoryPoints & forward_filtered, const TrajectoryPoints & backward_filtered) const +{ + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + + TrajectoryPoints merged; + merged = forward_filtered; + + auto getVx = [](const TrajectoryPoints & trajectory, int i) { + return trajectory.at(i).longitudinal_velocity_mps; + }; + + size_t i = 0; + + if (getVx(backward_filtered, 0) < v0) { + double current_vel = v0; + double current_acc = a0; + while (getVx(backward_filtered, i) < current_vel && i < merged.size() - 1) { + merged.at(i).longitudinal_velocity_mps = current_vel; + merged.at(i).acceleration_mps2 = current_acc; + + const double ds = autoware::universe_utils::calcDistance2d( + forward_filtered.at(i + 1), forward_filtered.at(i)); + const double max_dt = + std::pow(6.0 * ds / std::fabs(j_min), 1.0 / 3.0); // assuming v0 = a0 = 0. + const double dt = std::min(ds / std::max(current_vel, 1.0e-6), max_dt); + + if (current_acc + j_min * dt < a_min) { + const double tmp_jerk = std::max((a_min - current_acc) / dt, j_min); + current_vel = current_vel + current_acc * dt + 0.5 * tmp_jerk * dt * dt; + current_acc = std::max(current_acc + tmp_jerk * dt, a_min); + } else { + current_vel = current_vel + current_acc * dt + 0.5 * j_min * dt * dt; + current_acc = current_acc + j_min * dt; + } + + if (current_vel > getVx(forward_filtered, i)) { + current_vel = getVx(forward_filtered, i); + } + ++i; + } + } + + // take smaller velocity point + for (; i < merged.size(); ++i) { + merged.at(i) = (getVx(forward_filtered, i) < getVx(backward_filtered, i)) + ? forward_filtered.at(i) + : backward_filtered.at(i); + } + return merged; +} + +TrajectoryPoints JerkFilteredSmoother::resampleTrajectory( + const TrajectoryPoints & input, [[maybe_unused]] const double v0, + const geometry_msgs::msg::Pose & current_pose, const double nearest_dist_threshold, + const double nearest_yaw_threshold) const +{ + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + + return resampling::resampleTrajectory( + input, current_pose, nearest_dist_threshold, nearest_yaw_threshold, base_param_.resample_param, + smoother_param_.jerk_filter_ds); +} + +} // namespace autoware::velocity_smoother diff --git a/planning/autoware_velocity_smoother/src/smoother/smoother_base.cpp b/planning/autoware_velocity_smoother/src/smoother/smoother_base.cpp new file mode 100644 index 0000000000..e4271a9c38 --- /dev/null +++ b/planning/autoware_velocity_smoother/src/smoother/smoother_base.cpp @@ -0,0 +1,292 @@ +// Copyright 2021 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware/velocity_smoother/smoother/smoother_base.hpp" + +#include "autoware/motion_utils/resample/resample.hpp" +#include "autoware/motion_utils/trajectory/conversion.hpp" +#include "autoware/motion_utils/trajectory/trajectory.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/math/unit_conversion.hpp" +#include "autoware/velocity_smoother/resample.hpp" +#include "autoware/velocity_smoother/trajectory_utils.hpp" + +#include +#include +#include +#include +#include + +namespace autoware::velocity_smoother +{ + +namespace +{ +TrajectoryPoints applyPreProcess( + const TrajectoryPoints & input, const double interval, const bool use_resampling) +{ + using autoware::motion_utils::calcArcLength; + using autoware::motion_utils::convertToTrajectory; + using autoware::motion_utils::convertToTrajectoryPointArray; + using autoware::motion_utils::resampleTrajectory; + + if (!use_resampling) { + return input; + } + + TrajectoryPoints output; + std::vector arc_length; + + // since the resampling takes a long time, omit the resampling when it is not requested + const auto traj_length = calcArcLength(input); + for (double s = 0; s < traj_length; s += interval) { + arc_length.push_back(s); + } + + const auto points = resampleTrajectory(convertToTrajectory(input), arc_length); + output = convertToTrajectoryPointArray(points); + output.back() = input.back(); // keep the final speed. + + return output; +} +} // namespace + +SmootherBase::SmootherBase( + rclcpp::Node & node, const std::shared_ptr time_keeper) +: time_keeper_(time_keeper) +{ + auto & p = base_param_; + p.max_accel = node.declare_parameter("normal.max_acc"); + p.min_decel = node.declare_parameter("normal.min_acc"); + p.stop_decel = node.declare_parameter("stop_decel"); + p.max_jerk = node.declare_parameter("normal.max_jerk"); + p.min_jerk = node.declare_parameter("normal.min_jerk"); + p.max_lateral_accel = node.declare_parameter("max_lateral_accel"); + p.min_decel_for_lateral_acc_lim_filter = + node.declare_parameter("min_decel_for_lateral_acc_lim_filter"); + p.sample_ds = node.declare_parameter("resample_ds"); + p.curvature_threshold = node.declare_parameter("curvature_threshold"); + p.max_steering_angle_rate = node.declare_parameter("max_steering_angle_rate"); + p.curvature_calculation_distance = + node.declare_parameter("curvature_calculation_distance"); + p.decel_distance_before_curve = node.declare_parameter("decel_distance_before_curve"); + p.decel_distance_after_curve = node.declare_parameter("decel_distance_after_curve"); + p.min_curve_velocity = node.declare_parameter("min_curve_velocity"); + p.resample_param.max_trajectory_length = node.declare_parameter("max_trajectory_length"); + p.resample_param.min_trajectory_length = node.declare_parameter("min_trajectory_length"); + p.resample_param.resample_time = node.declare_parameter("resample_time"); + p.resample_param.dense_resample_dt = node.declare_parameter("dense_resample_dt"); + p.resample_param.dense_min_interval_distance = + node.declare_parameter("dense_min_interval_distance"); + p.resample_param.sparse_resample_dt = node.declare_parameter("sparse_resample_dt"); + p.resample_param.sparse_min_interval_distance = + node.declare_parameter("sparse_min_interval_distance"); +} + +void SmootherBase::setWheelBase(const double wheel_base) +{ + base_param_.wheel_base = wheel_base; +} + +void SmootherBase::setMaxAccel(const double max_acceleration) +{ + base_param_.max_accel = max_acceleration; +} + +void SmootherBase::setMaxJerk(const double max_jerk) +{ + base_param_.max_jerk = max_jerk; +} + +void SmootherBase::setParam(const BaseParam & param) +{ + base_param_ = param; +} + +SmootherBase::BaseParam SmootherBase::getBaseParam() const +{ + return base_param_; +} + +double SmootherBase::getMaxAccel() const +{ + return base_param_.max_accel; +} + +double SmootherBase::getMinDecel() const +{ + return base_param_.min_decel; +} + +double SmootherBase::getMaxJerk() const +{ + return base_param_.max_jerk; +} + +double SmootherBase::getMinJerk() const +{ + return base_param_.min_jerk; +} + +TrajectoryPoints SmootherBase::applyLateralAccelerationFilter( + const TrajectoryPoints & input, [[maybe_unused]] const double v0, + [[maybe_unused]] const double a0, [[maybe_unused]] const bool enable_smooth_limit, + const bool use_resampling, const double input_points_interval) const +{ + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + + if (input.size() < 3) { + return input; // cannot calculate lateral acc. do nothing. + } + + // Interpolate with constant interval distance for lateral acceleration calculation. + TrajectoryPoints output; + const double points_interval = + use_resampling ? base_param_.sample_ds : input_points_interval; // [m] + // since the resampling takes a long time, omit the resampling when it is not requested + if (use_resampling) { + std::vector out_arclength; + const auto traj_length = autoware::motion_utils::calcArcLength(input); + for (double s = 0; s < traj_length; s += points_interval) { + out_arclength.push_back(s); + } + const auto output_traj = autoware::motion_utils::resampleTrajectory( + autoware::motion_utils::convertToTrajectory(input), out_arclength); + output = autoware::motion_utils::convertToTrajectoryPointArray(output_traj); + output.back() = input.back(); // keep the final speed. + } else { + output = input; + } + + const size_t idx_dist = static_cast( + std::max(static_cast((base_param_.curvature_calculation_distance) / points_interval), 1)); + + // Calculate curvature assuming the trajectory points interval is constant + const auto curvature_v = trajectory_utils::calcTrajectoryCurvatureFrom3Points(output, idx_dist); + + // Decrease speed according to lateral G + const size_t before_decel_index = + static_cast(std::round(base_param_.decel_distance_before_curve / points_interval)); + const size_t after_decel_index = + static_cast(std::round(base_param_.decel_distance_after_curve / points_interval)); + const double max_lateral_accel_abs = std::fabs(base_param_.max_lateral_accel); + + const auto latacc_min_vel_arr = + enable_smooth_limit ? trajectory_utils::calcVelocityProfileWithConstantJerkAndAccelerationLimit( + output, v0, a0, base_param_.min_jerk, base_param_.max_accel, + base_param_.min_decel_for_lateral_acc_lim_filter) + : std::vector{}; + + for (size_t i = 0; i < output.size(); ++i) { + double curvature = 0.0; + const size_t start = i > after_decel_index ? i - after_decel_index : 0; + const size_t end = std::min(output.size(), i + before_decel_index + 1); + for (size_t j = start; j < end; ++j) { + if (j >= curvature_v.size()) return output; + curvature = std::max(curvature, std::fabs(curvature_v.at(j))); + } + double v_curvature_max = std::sqrt(max_lateral_accel_abs / std::max(curvature, 1.0E-5)); + v_curvature_max = std::max(v_curvature_max, base_param_.min_curve_velocity); + + if (enable_smooth_limit) { + if (i >= latacc_min_vel_arr.size()) return output; + v_curvature_max = std::max(v_curvature_max, latacc_min_vel_arr.at(i)); + } + if (output.at(i).longitudinal_velocity_mps > v_curvature_max) { + output.at(i).longitudinal_velocity_mps = v_curvature_max; + } + } + return output; +} + +TrajectoryPoints SmootherBase::applySteeringRateLimit( + const TrajectoryPoints & input, const bool use_resampling, + const double input_points_interval) const +{ + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + + if (input.size() < 3) { + return input; // cannot calculate the desired velocity. do nothing. + } + + // Interpolate with constant interval distance for lateral acceleration calculation. + const double points_interval = use_resampling ? base_param_.sample_ds : input_points_interval; + + auto output = applyPreProcess(input, points_interval, use_resampling); + + const size_t idx_dist = static_cast( + std::max(static_cast((base_param_.curvature_calculation_distance) / points_interval), 1)); + + // Step1. Calculate curvature assuming the trajectory points interval is constant. + const auto curvature_v = trajectory_utils::calcTrajectoryCurvatureFrom3Points(output, idx_dist); + + // Step2. Calculate steer rate for each trajectory point. + std::vector steer_rate_arr(output.size(), 0.0); + for (size_t i = 0; i < output.size() - 1; i++) { + // velocity + const auto & v_front = output.at(i + 1).longitudinal_velocity_mps; + const auto & v_back = output.at(i).longitudinal_velocity_mps; + // steer + auto & steer_front = output.at(i + 1).front_wheel_angle_rad; + auto & steer_back = output.at(i).front_wheel_angle_rad; + + // calculate the just 2 steering angle + steer_front = std::atan(base_param_.wheel_base * curvature_v.at(i + 1)); + steer_back = std::atan(base_param_.wheel_base * curvature_v.at(i)); + + const auto mean_vel = 0.5 * (v_front + v_back); + const auto dt = std::max(points_interval / mean_vel, std::numeric_limits::epsilon()); + const auto steering_diff = std::fabs(steer_front - steer_back); + + steer_rate_arr.at(i) = steering_diff / dt; + } + + steer_rate_arr.back() = steer_rate_arr.at((output.size() - 2)); + + // Step3. Remove noise by mean filter. + for (size_t i = 1; i < steer_rate_arr.size() - 1; i++) { + steer_rate_arr.at(i) = + (steer_rate_arr.at(i - 1) + steer_rate_arr.at(i) + steer_rate_arr.at(i + 1)) / 3.0; + } + + // Step4. Limit velocity by steer rate. + for (size_t i = 0; i < output.size() - 1; i++) { + if (fabs(curvature_v.at(i)) < base_param_.curvature_threshold) { + continue; + } + + const auto steer_rate = steer_rate_arr.at(i); + if (steer_rate < autoware::universe_utils::deg2rad(base_param_.max_steering_angle_rate)) { + continue; + } + + const auto mean_vel = + (output.at(i).longitudinal_velocity_mps + output.at(i + 1).longitudinal_velocity_mps) / 2.0; + const auto target_mean_vel = + mean_vel * + (autoware::universe_utils::deg2rad(base_param_.max_steering_angle_rate) / steer_rate); + + for (size_t k = 0; k < 2; k++) { + auto & velocity = output.at(i + k).longitudinal_velocity_mps; + const float target_velocity = std::max( + base_param_.min_curve_velocity, + std::min(target_mean_vel, velocity * (target_mean_vel / mean_vel))); + velocity = std::min(velocity, target_velocity); + } + } + + return output; +} + +} // namespace autoware::velocity_smoother diff --git a/planning/autoware_velocity_smoother/src/trajectory_utils.cpp b/planning/autoware_velocity_smoother/src/trajectory_utils.cpp new file mode 100644 index 0000000000..cf031601cf --- /dev/null +++ b/planning/autoware_velocity_smoother/src/trajectory_utils.cpp @@ -0,0 +1,475 @@ +// Copyright 2021 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware/velocity_smoother/trajectory_utils.hpp" + +#include "autoware/interpolation/linear_interpolation.hpp" +#include "autoware/motion_utils/trajectory/trajectory.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" + +#include +#include +#include +#include +#include +#include + +namespace autoware::velocity_smoother +{ +using geometry_msgs::msg::Point; +namespace trajectory_utils +{ +inline void convertEulerAngleToMonotonic(std::vector & a) +{ + for (unsigned int i = 1; i < a.size(); ++i) { + const double da = a[i] - a[i - 1]; + a[i] = a[i - 1] + autoware::universe_utils::normalizeRadian(da); + } +} + +inline tf2::Vector3 getTransVector3(const Pose & from, const Pose & to) +{ + double dx = to.position.x - from.position.x; + double dy = to.position.y - from.position.y; + double dz = to.position.z - from.position.z; + return tf2::Vector3(dx, dy, dz); +} + +inline double integ_x(double x0, double v0, double a0, double j0, double t) +{ + return x0 + v0 * t + 0.5 * a0 * t * t + (1.0 / 6.0) * j0 * t * t * t; +} + +inline double integ_v(double v0, double a0, double j0, double t) +{ + return v0 + a0 * t + 0.5 * j0 * t * t; +} + +inline double integ_a(double a0, double j0, double t) +{ + return a0 + j0 * t; +} + +TrajectoryPoint calcInterpolatedTrajectoryPoint( + const TrajectoryPoints & trajectory, const Pose & target_pose, const size_t seg_idx) +{ + TrajectoryPoint traj_p{}; + traj_p.pose = target_pose; + + if (trajectory.empty()) { + traj_p.longitudinal_velocity_mps = 0.0; + traj_p.acceleration_mps2 = 0.0; + return traj_p; + } + + if (trajectory.size() == 1) { + traj_p.longitudinal_velocity_mps = trajectory.at(0).longitudinal_velocity_mps; + traj_p.acceleration_mps2 = trajectory.at(0).acceleration_mps2; + return traj_p; + } + + auto v1 = getTransVector3(trajectory.at(seg_idx).pose, trajectory.at(seg_idx + 1).pose); + auto v2 = getTransVector3(trajectory.at(seg_idx).pose, target_pose); + // calc internal proportion + const double prop{std::max(0.0, std::min(1.0, v1.dot(v2) / v1.length2()))}; + + { + const auto & seg_pt = trajectory.at(seg_idx); + const auto & next_pt = trajectory.at(seg_idx + 1); + traj_p.pose = autoware::universe_utils::calcInterpolatedPose(seg_pt.pose, next_pt.pose, prop); + traj_p.longitudinal_velocity_mps = autoware::interpolation::lerp( + seg_pt.longitudinal_velocity_mps, next_pt.longitudinal_velocity_mps, prop); + traj_p.acceleration_mps2 = + autoware::interpolation::lerp(seg_pt.acceleration_mps2, next_pt.acceleration_mps2, prop); + } + + return traj_p; +} + +TrajectoryPoints extractPathAroundIndex( + const TrajectoryPoints & trajectory, const size_t index, const double & ahead_length, + const double & behind_length) +{ + if (trajectory.size() == 0 || trajectory.size() - 1 < index) { + return {}; + } + + // calc ahead distance + size_t ahead_index{trajectory.size() - 1}; + { + double dist_sum = 0.0; + for (size_t i = index; i < trajectory.size() - 1; ++i) { + dist_sum += autoware::universe_utils::calcDistance2d(trajectory.at(i), trajectory.at(i + 1)); + if (dist_sum > ahead_length) { + ahead_index = i + 1; + break; + } + } + } + + // calc behind distance + size_t behind_index{0}; + { + double dist_sum{0.0}; + for (size_t i = index; i != 0; --i) { + dist_sum += autoware::universe_utils::calcDistance2d(trajectory.at(i), trajectory[i - 1]); + if (dist_sum > behind_length) { + behind_index = i - 1; + break; + } + } + } + + // extract trajectory + TrajectoryPoints extracted_traj{}; + for (size_t i = behind_index; i < ahead_index + 1; ++i) { + extracted_traj.push_back(trajectory.at(i)); + } + + return extracted_traj; +} + +std::vector calcArclengthArray(const TrajectoryPoints & trajectory) +{ + if (trajectory.empty()) { + return {}; + } + + std::vector arclength(trajectory.size()); + double dist = 0.0; + arclength.front() = dist; + for (unsigned int i = 1; i < trajectory.size(); ++i) { + const TrajectoryPoint tp = trajectory.at(i); + const TrajectoryPoint tp_prev = trajectory.at(i - 1); + dist += autoware::universe_utils::calcDistance2d(tp.pose, tp_prev.pose); + arclength.at(i) = dist; + } + return arclength; +} + +std::vector calcTrajectoryIntervalDistance(const TrajectoryPoints & trajectory) +{ + std::vector intervals; + for (unsigned int i = 1; i < trajectory.size(); ++i) { + const TrajectoryPoint tp = trajectory.at(i); + const TrajectoryPoint tp_prev = trajectory.at(i - 1); + const double dist = autoware::universe_utils::calcDistance2d(tp.pose, tp_prev.pose); + intervals.push_back(dist); + } + return intervals; +} + +std::vector calcTrajectoryCurvatureFrom3Points( + const TrajectoryPoints & trajectory, size_t idx_dist) +{ + using autoware::universe_utils::calcCurvature; + using autoware::universe_utils::getPoint; + + if (trajectory.size() < 3) { + const std::vector k_arr(trajectory.size(), 0.0); + return k_arr; + } + + // if the idx size is not enough, change the idx_dist + const auto max_idx_dist = static_cast(std::floor((trajectory.size() - 1) / 2.0)); + idx_dist = std::max(1ul, std::min(idx_dist, max_idx_dist)); + + if (idx_dist < 1) { + throw std::logic_error("idx_dist less than 1 is not expected"); + } + + // calculate curvature by circle fitting from three points + std::vector k_arr(trajectory.size(), 0.0); + + for (size_t i = 1; i + 1 < trajectory.size(); i++) { + double curvature = 0.0; + const auto p0 = getPoint(trajectory.at(i - std::min(idx_dist, i))); + const auto p1 = getPoint(trajectory.at(i)); + const auto p2 = getPoint(trajectory.at(i + std::min(idx_dist, trajectory.size() - 1 - i))); + try { + curvature = calcCurvature(p0, p1, p2); + } catch (std::exception const & e) { + // ...code that handles the error... + RCLCPP_WARN( + rclcpp::get_logger("autoware_velocity_smoother").get_child("trajectory_utils"), "%s", + e.what()); + if (i > 1) { + curvature = k_arr.at(i - 1); // previous curvature + } else { + curvature = 0.0; + } + } + k_arr.at(i) = curvature; + } + // copy curvatures for the last and first points; + k_arr.at(0) = k_arr.at(1); + k_arr.back() = k_arr.at((trajectory.size() - 2)); + + return k_arr; +} + +void applyMaximumVelocityLimit( + const size_t begin, const size_t end, const double max_vel, TrajectoryPoints & trajectory) +{ + for (size_t idx = begin; idx < end; ++idx) { + if (trajectory.at(idx).longitudinal_velocity_mps > max_vel) { + trajectory.at(idx).longitudinal_velocity_mps = max_vel; + } + } +} + +bool calcStopDistWithJerkConstraints( + const double v0, const double a0, const double jerk_acc, const double jerk_dec, + const double min_acc, const double target_vel, std::map & jerk_profile, + double & stop_dist) +{ + auto calculateTime = [](const double a0, const double target_acc, const double jerk) { + return (target_acc - a0) / jerk; + }; + enum class AccelerationType { TRAPEZOID = 0, TRIANGLE = 1, LINEAR = 2 } type; + + jerk_profile.clear(); + + constexpr double t_threshold = 1e-06; // Threshold for truncating value to 0 + constexpr double a_target = 0.0; // [m/s^2] + + // Calculate time interval with constant acceleration (min_acc) + double t_min; + { + const double v_diff_jerk_dec = 0.5 * calculateTime(a0, 0.0, jerk_dec) * a0 + + 0.5 * calculateTime(0.0, min_acc, jerk_dec) * min_acc; + const double v_diff_jerk_acc = 0.5 * calculateTime(min_acc, 0.0, jerk_acc) * min_acc; + const double v_error = target_vel - v0; + // From v_error = v_diff_jerk_dec + min_acc * t_min + v_diff_jerk_acc + t_min = (v_error - v_diff_jerk_dec - v_diff_jerk_acc) / min_acc; + } + + if (t_min > 0) { + // Necessary to increase deceleration to min_acc + type = AccelerationType::TRAPEZOID; + } else { + const double v_error = target_vel - v0; + const double v_diff_decel = 0.5 * a0 * calculateTime(a0, 0.0, jerk_acc); + if (v_diff_decel > v_error || a0 > 0.0) { + // Necessary to increase deceleration case + type = AccelerationType::TRIANGLE; + } else { + type = AccelerationType::LINEAR; + } + } + + double x, v, a, j; + + switch (type) { + case AccelerationType::TRAPEZOID: { + // Calculate time + double t1 = calculateTime(a0, min_acc, jerk_dec); + double t2 = t_min; + double t3 = calculateTime(min_acc, 0.0, jerk_acc); + if (t1 < t_threshold) { + t1 = 0; + } + if (t2 < t_threshold) { + t2 = 0; + } + if (t3 < t_threshold) { + t3 = 0; + } + + // Set jerk profile + jerk_profile.insert(std::make_pair(jerk_dec, t1)); + jerk_profile.insert(std::make_pair(0.0, t2)); + jerk_profile.insert(std::make_pair(jerk_acc, t3)); + + // Calculate state = (x, v, a, j) at t = t1 + t2 + t3 + const auto state = updateStateWithJerkConstraint(v0, a0, jerk_profile, t1 + t2 + t3); + if (!state) { + return false; + } + std::tie(x, v, a, j) = *state; + break; + } + + case AccelerationType::TRIANGLE: { + /* + v_error = v_diff_from_a0_to_zero + v_diff_from_zero_to_a1 + v_diff_from_a1_to_zero, + where v_diff_from_zero_to_a1 = 1/2 * ((a1 - 0.0) / jerk_dec) * a1, v_diff_from_a1_to_zero + = 1/2 * ((0.0 - a1) / jerk_acc) * a1. Thus a1^2 = (v_error - v_diff_from_a0_to_zero) * 2 + * 1.0 / (1.0 / jerk_dec - 1.0 / jerk_acc). + */ + const double v_error = target_vel - v0; + const double v_diff_from_a0_to_zero = 0.5 * calculateTime(a0, 0.0, jerk_dec) * a0; + const double a1_square = + (v_error - v_diff_from_a0_to_zero) * 2 * 1.0 / (1.0 / jerk_dec - 1.0 / jerk_acc); + const double a1 = -std::sqrt(a1_square); + + // Calculate time + double t1 = calculateTime(a0, a1, jerk_dec); + double t2 = calculateTime(a1, 0.0, jerk_acc); + if (t1 < t_threshold) { + t1 = 0; + } + if (t2 < t_threshold) { + t2 = 0; + } + + // Set jerk profile + jerk_profile.insert(std::make_pair(jerk_dec, t1)); + jerk_profile.insert(std::make_pair(jerk_acc, t2)); + + // Calculate state = (x, v, a, j) at t = t1 + t2 + const auto state = updateStateWithJerkConstraint(v0, a0, jerk_profile, t1 + t2); + if (!state) { + return false; + } + std::tie(x, v, a, j) = *state; + break; + } + + case AccelerationType::LINEAR: { + // Calculate time + const double jerk = (0.0 - a0 * a0) / (2 * (target_vel - v0)); + double t1 = calculateTime(a0, 0.0, jerk); + if (t1 < 0) { + RCLCPP_WARN( + rclcpp::get_logger("autoware_velocity_smoother").get_child("trajectory_utils"), + "t1 < 0. unexpected condition."); + return false; + } else if (t1 < t_threshold) { + t1 = 0; + } + + // Set jerk profile + jerk_profile.insert(std::make_pair(jerk, t1)); + + // Calculate state = (x, v, a, j) at t = t1 + t2 + const auto state = updateStateWithJerkConstraint(v0, a0, jerk_profile, t1); + if (!state) { + return false; + } + std::tie(x, v, a, j) = *state; + break; + } + } + + constexpr double v_margin = 0.3; // [m/s] + constexpr double a_margin = 0.1; // [m/s^2] + stop_dist = x; + if (!isValidStopDist(v, a, target_vel, a_target, v_margin, a_margin)) { + RCLCPP_WARN_STREAM( + rclcpp::get_logger("autoware_velocity_smoother").get_child("trajectory_utils"), + "valid error. type = " << static_cast(type)); + return false; + } + return true; +} + +bool isValidStopDist( + const double v_end, const double a_end, const double v_target, const double a_target, + const double v_margin, const double a_margin) +{ + const double v_min = v_target - std::abs(v_margin); + const double v_max = v_target + std::abs(v_margin); + const double a_min = a_target - std::abs(a_margin); + const double a_max = a_target + std::abs(a_margin); + if (v_end < v_min || v_max < v_end) { + RCLCPP_WARN_STREAM( + rclcpp::get_logger("autoware_velocity_smoother").get_child("trajectory_utils"), + "valid check error! v_target = " << v_target << ", v_end = " << v_end); + return false; + } + if (a_end < a_min || a_max < a_end) { + RCLCPP_WARN_STREAM( + rclcpp::get_logger("autoware_velocity_smoother").get_child("trajectory_utils"), + "valid check error! a_target = " << a_target << ", a_end = " << a_end); + return false; + } + return true; +} + +std::optional> updateStateWithJerkConstraint( + const double v0, const double a0, const std::map & jerk_profile, const double t) +{ + // constexpr double eps = 1.0E-05; + double a{a0}; + double v{v0}; + double x{0.0}; + double t_sum{0.0}; + + for (const auto & it : jerk_profile) { + double j = it.first; + t_sum += it.second; + if (t > t_sum) { + x = integ_x(x, v, a, j, it.second); + v = integ_v(v, a, j, it.second); + a = integ_a(a, j, it.second); + } else { + const double dt = t - (t_sum - it.second); + x = integ_x(x, v, a, j, dt); + v = integ_v(v, a, j, dt); + a = integ_a(a, j, dt); + return std::make_tuple(x, v, a, j); + } + } + + RCLCPP_WARN_STREAM( + rclcpp::get_logger("autoware_velocity_smoother").get_child("trajectory_utils"), + "Invalid jerk profile"); + return std::nullopt; +} + +std::vector calcVelocityProfileWithConstantJerkAndAccelerationLimit( + const TrajectoryPoints & trajectory, const double v0, const double a0, const double jerk, + const double acc_max, const double acc_min) +{ + if (trajectory.empty()) return {}; + + std::vector velocities(trajectory.size()); + velocities.at(0) = v0; + auto curr_v = v0; + auto curr_a = a0; + + const auto intervals = calcTrajectoryIntervalDistance(trajectory); + + if (intervals.size() + 1 != trajectory.size()) { + throw std::logic_error("interval calculation result has unexpected array size."); + } + + for (size_t i = 0; i < intervals.size(); ++i) { + const auto t = intervals.at(i) / std::max(velocities.at(i), 1.0e-5); + curr_v = integ_v(curr_v, curr_a, jerk, t); + velocities.at(i + 1) = curr_v; + curr_a = std::clamp(integ_a(curr_a, jerk, t), acc_min, acc_max); + } + + return velocities; +} + +double calcStopDistance(const TrajectoryPoints & trajectory, const size_t closest) +{ + const auto idx = autoware::motion_utils::searchZeroVelocityIndex(trajectory); + + if (!idx) { + return std::numeric_limits::max(); // stop point is located far away + } + + // TODO(Horibe): use arc length distance + const double stop_dist = + autoware::universe_utils::calcDistance2d(trajectory.at(*idx), trajectory.at(closest)); + + return stop_dist; +} + +} // namespace trajectory_utils +} // namespace autoware::velocity_smoother diff --git a/planning/autoware_velocity_smoother/test/test_smoother_functions.cpp b/planning/autoware_velocity_smoother/test/test_smoother_functions.cpp new file mode 100644 index 0000000000..5daf332fb0 --- /dev/null +++ b/planning/autoware_velocity_smoother/test/test_smoother_functions.cpp @@ -0,0 +1,63 @@ +// Copyright 2022 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware/velocity_smoother/trajectory_utils.hpp" + +#include + +#include + +using autoware::velocity_smoother::trajectory_utils::TrajectoryPoints; +using autoware_planning_msgs::msg::TrajectoryPoint; + +TrajectoryPoints genStraightTrajectory(const size_t size) +{ + double x = 0.0; + double dx = 1.0; + geometry_msgs::msg::Quaternion o; + o.x = 0.0; + o.y = 0.0; + o.z = 0.0; + o.w = 1.0; + + TrajectoryPoints tps; + TrajectoryPoint p; + for (size_t i = 0; i < size; ++i) { + p.pose.position.x = x; + p.pose.orientation = o; + x += dx; + tps.push_back(p); + } + return tps; +} + +TEST(TestTrajectoryUtils, CalcTrajectoryCurvatureFrom3Points) +{ + // the output curvature vector should have the same size of the input trajectory. + const auto checkOutputSize = [](const size_t trajectory_size, const size_t idx_dist) { + const auto trajectory_points = genStraightTrajectory(trajectory_size); + const auto curvatures = + autoware::velocity_smoother::trajectory_utils::calcTrajectoryCurvatureFrom3Points( + trajectory_points, idx_dist); + EXPECT_EQ(curvatures.size(), trajectory_size) << ", idx_dist = " << idx_dist; + }; + + const auto trajectory_size_arr = {0, 1, 2, 3, 4, 5, 10, 100}; + const auto idx_dist_arr = {0, 1, 2, 3, 4, 5, 10, 100}; + for (const auto trajectory_size : trajectory_size_arr) { + for (const auto idx_dist : idx_dist_arr) { + checkOutputSize(trajectory_size, idx_dist); + } + } +} diff --git a/planning/autoware_velocity_smoother/test/test_velocity_smoother_node_interface.cpp b/planning/autoware_velocity_smoother/test/test_velocity_smoother_node_interface.cpp new file mode 100644 index 0000000000..b590af1b3e --- /dev/null +++ b/planning/autoware_velocity_smoother/test/test_velocity_smoother_node_interface.cpp @@ -0,0 +1,104 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware/velocity_smoother/node.hpp" + +#include +#include +#include + +#include + +#include +#include + +using autoware::planning_test_manager::PlanningInterfaceTestManager; +using autoware::velocity_smoother::VelocitySmootherNode; + +std::shared_ptr generateTestManager() +{ + auto test_manager = std::make_shared(); + test_manager->setTrajectorySubscriber("velocity_smoother/output/trajectory"); + test_manager->setTrajectoryInputTopicName("velocity_smoother/input/trajectory"); + test_manager->setOdometryTopicName("/localization/kinematic_state"); + return test_manager; +} + +std::shared_ptr generateNode() +{ + auto node_options = rclcpp::NodeOptions{}; + node_options.append_parameter_override("algorithm_type", "JerkFiltered"); + node_options.append_parameter_override("publish_debug_trajs", false); + const auto autoware_test_utils_dir = + ament_index_cpp::get_package_share_directory("autoware_test_utils"); + const auto velocity_smoother_dir = + ament_index_cpp::get_package_share_directory("autoware_velocity_smoother"); + node_options.arguments( + {"--ros-args", "--params-file", autoware_test_utils_dir + "/config/test_common.param.yaml", + "--params-file", autoware_test_utils_dir + "/config/test_nearest_search.param.yaml", + "--params-file", autoware_test_utils_dir + "/config/test_vehicle_info.param.yaml", + "--params-file", velocity_smoother_dir + "/config/default_velocity_smoother.param.yaml", + "--params-file", velocity_smoother_dir + "/config/default_common.param.yaml", "--params-file", + velocity_smoother_dir + "/config/JerkFiltered.param.yaml"}); + + return std::make_shared(node_options); +} + +void publishMandatoryTopics( + std::shared_ptr test_manager, + rclcpp::Node::SharedPtr test_target_node) +{ + // publish necessary topics from test_manager + test_manager->publishOdometry(test_target_node, "/localization/kinematic_state"); + test_manager->publishMaxVelocity( + test_target_node, "velocity_smoother/input/external_velocity_limit_mps"); + test_manager->publishOperationModeState( + test_target_node, "velocity_smoother/input/operation_mode_state"); + test_manager->publishAcceleration(test_target_node, "velocity_smoother/input/acceleration"); +} + +TEST(PlanningModuleInterfaceTest, testPlanningInterfaceWithVariousTrajectoryInput) +{ + rclcpp::init(0, nullptr); + auto test_manager = generateTestManager(); + auto test_target_node = generateNode(); + + publishMandatoryTopics(test_manager, test_target_node); + + // test for normal trajectory + ASSERT_NO_THROW(test_manager->testWithNominalTrajectory(test_target_node)); + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + // test for trajectory with empty/one point/overlapping point + ASSERT_NO_THROW(test_manager->testWithAbnormalTrajectory(test_target_node)); + + rclcpp::shutdown(); +} + +TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) +{ + rclcpp::init(0, nullptr); + auto test_manager = generateTestManager(); + auto test_target_node = generateNode(); + + publishMandatoryTopics(test_manager, test_target_node); + + // test for normal trajectory + ASSERT_NO_THROW(test_manager->testWithNominalTrajectory(test_target_node)); + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + ASSERT_NO_THROW(test_manager->testTrajectoryWithInvalidEgoPose(test_target_node)); + + rclcpp::shutdown(); +}