From 2356788869a45447b03d8d36f9ad0f6ebc21497f Mon Sep 17 00:00:00 2001 From: Tomoya Kimura Date: Mon, 6 Dec 2021 19:21:04 +0900 Subject: [PATCH] feat: add motion_velocity_smoother package (#48) * Feature/porting motion velocity smoother (#1653) * Add motion_velocity_smoother (#1333) * Refactor node and utilities * Fix utilities * Fix utilities * Fixing... * Runs with L2 smoother * Use boost::optional * Add Linf, JerkFiltered * JerkFiltered * Fix * Fix awapi * Fix bug * Add config files * Fix bug * Fix bug and apply clang-format * Remove unused variable * Fix bug * Change C-style cast to static_cast * Add destructors * Use smart pointers for members * Add test * Fix test code * Tmp: add L2 norm of jerk in objective function * Fix external velocity limit * Fix interpolate in velocity controller and remove prevent move to close stop line * add initial velocity and acceleration to the filter function * Fix index calculation * add new marge function * handle edge case * Tmp: skip osqp_interface build test * Revert get_modified_package.sh * Fix CI * Fix version of osqp * Fix * Add design docment (JPN) * Fix linear interpolation * Refactor node and utilities * Fix utilities * Fix utilities * Fixing... * Runs with L2 smoother * Use boost::optional * Add Linf, JerkFiltered * JerkFiltered * Fix * Fix awapi * Fix bug * Add config files * Fix bug * Fix bug and apply clang-format * Remove unused variable * Fix bug * Change C-style cast to static_cast * Add destructors * Use smart pointers for members * Add test * Fix test code * Tmp: add L2 norm of jerk in objective function * Fix external velocity limit * Fix interpolate in velocity controller and remove prevent move to close stop line * add initial velocity and acceleration to the filter function * Fix index calculation * add new marge function * handle edge case * Tmp: skip osqp_interface build test * Revert get_modified_package.sh * Fix CI * Fix version of osqp * Fix * Add design docment (JPN) * Fix linear interpolation * add new sample function * Fix * Revert velocity_controller * Reset motion_velocity_optimizer * Fix parameter setting * Refactor and fix bugs * Use autoware_utils * Fix doc and bug * Fix doc * Fix external velocity limit and parametrize margin and jerk weight * Fix typo * Fix typo and remove old readme * add stop point calculation and modify objective function * Add parameter handling functions and add namespace * Rename calcClosestTrajectoryPoint to calcInterpolatedTrajectoryPoint and remove unused function * Fix applyExternalVelocityLimit and fix comments * Fix variable name * Fix variable name * Fix yaml comment * Add const * Fix interpolaion * Remove run() and change type of prev_closest_point_ * Update planning/scenario_planning/common/motion_velocity_smoother/src/smoother/smoother_base.cpp Co-authored-by: tkimura4 * fix stop point error * Fix linear interpolation and external velocity calculation * Remove debug code * Rename BaseParam * Remove unused func and fix misc * Fix package.xml, include, apply formatting * Fix external velocity limit * add debug calculation time * modify calculation time debugger * modify calculation time publisher * rescale the calculation time * Fix some problem * Update planning/scenario_planning/common/motion_velocity_smoother/launch/motion_velocity_smoother.launch Co-authored-by: tkimura4 Co-authored-by: yutaka Co-authored-by: tkimura4 * Add Resample Procedure after the optimization in motion_velocity_smoother (#1530) * do resample after the optimization * modify resample timing * add 0 at the end of the resampeld output trajectory * Manage parameters * Fix format * Fix format * Devide resample function in other file * Update default_motion_velocity_smoother.yaml * Update default_motion_velocity_smoother.yaml Co-authored-by: Fumiya Watanabe * Output debug trajectories in motion velocity smoother (#1533) * add debug trajectory * add rqt_multiplot setting files * add calculation tiem visualization * guard emtpy output (#1537) * Motion velocity smoother doc (#1563) * Fix doc * Add english doc (tmp) * Add english file * Remove unused files * Fix * Fix typo * Modify 1-size trajectory handling & warning messages (#1540) * add new guard to delete unnecessary message * change message * add warning messages * chnage message to warning * chnage to throttle * Fix jerk filter calculation (#1593) * modify jerk filter calculation * remove unnecessary code * change the way of interp. of pose in smoother (#1600) * Update smoother resampling (#1595) * add new guard to delete unnecessary message * change message * add warning messages * chnage message to warning * chnage to throttle * modify jerk filter calculation * modify jerk filter calculation * separate resampling code * add new parameter * add new parameter to dynamic reconfigure * remove unnecessary code * add sampling before optimization * add const and blocker * change access to at * fix terminal length value * add comments * clean code * add comment * Fix/insert backward on reverse trajectory (#1602) * remove unused code Signed-off-by: Takamasa Horibe * move debug code Signed-off-by: Takamasa Horibe * fix insert backward on reverse trajectory Signed-off-by: Takamasa Horibe * Porting motion velocity smoother to ros2 Signed-off-by: wep21 * Fix launch and fix warning * pre-commit fixes * Remove unused argument * Comment out unused parameter Signed-off-by: wep21 * Add distance threshold in motion_velocity_smoother (#1659) * Add distance threshold * Remove parameters and use default value * Fix for pre-commit Signed-off-by: wep21 * Support Non-zero Stop Point Acceleration (#1651) * Support non-zero stop point acceleration * modify zero velocity id search function (#1690) * modify zero velocity id search function * Fix resample function and error handling * Add comment Co-authored-by: Fumiya Watanabe * Apply lint Signed-off-by: wep21 * pre-commit fixes Signed-off-by: wep21 Co-authored-by: Fumiya Watanabe Co-authored-by: yutaka Co-authored-by: tkimura4 Co-authored-by: purewater0901 <43805014+purewater0901@users.noreply.github.com> Co-authored-by: Takamasa Horibe Co-authored-by: pre-commit * Feature/add trajectory_visualizer and closest_velocity_checker to motion_velocity_smoother (#1820) * add trajectory_visualizer and closest_velocity_optimizer to motion_velocity_smoother. * Fix CMakeLists Co-authored-by: Fumiya Watanabe * Feature/port analytical smoother (#1896) * Port analytical jerk constrained smoother Signed-off-by: Makoto Kurihara * Fix parameters format Signed-off-by: Makoto Kurihara * Resolve warnings and remove unnecessary processing Signed-off-by: Makoto Kurihara * Change default parameters Signed-off-by: Makoto Kurihara * Change slash to dot for declare_parameter Signed-off-by: Makoto Kurihara * Fix debug messages Signed-off-by: Makoto Kurihara * Change pragma once to ifndef and endif Signed-off-by: Makoto Kurihara * Add #include vector, utility and string Signed-off-by: Makoto Kurihara * Remove extra white spaces Signed-off-by: Makoto Kurihara * Fix acceleration calculation at type 3 Signed-off-by: Makoto Kurihara * Apply clang-format Signed-off-by: Makoto Kurihara * Change copyright format Signed-off-by: Makoto Kurihara * Fix to be less than 100 characters per line Signed-off-by: Makoto Kurihara * Implement and remove TODO Signed-off-by: Makoto Kurihara * Add include to src Signed-off-by: Makoto Kurihara * Use static_cast Signed-off-by: Makoto Kurihara * Follow uncrustify Signed-off-by: Makoto Kurihara * Disable uncrustify at include guard Signed-off-by: Makoto Kurihara * Disable uncrustify at include Signed-off-by: Makoto Kurihara * Follow uncrustify Signed-off-by: Makoto Kurihara * Fix include order error Signed-off-by: Makoto Kurihara * Fix typo Signed-off-by: Makoto Kurihara * Change index type to size_t Signed-off-by: Makoto Kurihara * Fix size_t Signed-off-by: Makoto Kurihara * Add const Signed-off-by: Makoto Kurihara * Fix parameter namespace signs from slash to dot and add latacc one Signed-off-by: Makoto Kurihara * fix for createQuaternionFromRPY/Yaw (#2154) * fix stop dist calc bug (#2152) * use interpolation::slerp (#2161) * Add post resampling explanations (#2155) * Update motion_velocity_smoother-design.ja.md * Update motion_velocity_smoother-design.md * Feature/use external jerk/acc constraints (#2130) * use external jerk/acc constraints * update VelocityLimit.msg * Feature/add external velocity limit selector (#2217) * add external velocity limit selector * add README.md * improve document * apply document template * update external velocity limit selector & use commot params * update smoother & use commot params * update launch file * Fix/smoother params (#2239) * Update velocity debug script (add vehicle engage check) (#2267) * add vehicle engage Signed-off-by: Takamasa Horibe * fix format Signed-off-by: Takamasa Horibe * [motion_velocity_smoother] fix trajectory size guard condition (#2297) * change condition for input trajectory validation chack Signed-off-by: Takamasa Horibe * organize guard code into function Signed-off-by: Takamasa Horibe * Change formatter to clang-format and black (#2332) * Revert "Temporarily comment out pre-commit hooks" This reverts commit 748e9cdb145ce12f8b520bcbd97f5ff899fc28a3. * Replace ament_lint_common with autoware_lint_common Signed-off-by: Kenji Miyake * Remove ament_cmake_uncrustify and ament_clang_format Signed-off-by: Kenji Miyake * Apply Black Signed-off-by: Kenji Miyake * Apply clang-format Signed-off-by: Kenji Miyake * Fix build errors Signed-off-by: Kenji Miyake * Fix for cpplint * Fix include double quotes to angle brackets Signed-off-by: Kenji Miyake * Apply clang-format Signed-off-by: Kenji Miyake * Fix build errors Signed-off-by: Kenji Miyake * Add COLCON_IGNORE (#500) Signed-off-by: Kenji Miyake * port motion velocity smoother (#495) * Use autoware_auto_msgs * Fix bugs and apply clang-format * Remove COLCON_IGNORE * Rename TrajectoryPointArray to TrajectoryPoints, use conversion functions in autoware_utils * Apply clang-format * Use odometry * Fix document * rename to README.md (#550) * rename to README.md * dealt with new auto_msgs format Co-authored-by: Takayuki Murooka * update iv_msgs -> auto_msgs in planning readme (#576) * update iv_msgs -> auto_msgs in planning readme * minor change * some fix * some fix Co-authored-by: Takayuki Murooka * Back port .auto control packages (#571) * Implement Lateral and Longitudinal Control Muxer * [#570] Porting wf_simulator * [#1189] Deactivate flaky test in 'trajectory_follower_nodes' * [#1189] Fix flacky test in 'trajectory_follower_nodes/latlon_muxer' * [#1057] Add osqp_interface package * [#1057] Add library code for MPC-based lateral control * [#1271] Use std::abs instead of abs * [#1057] Implement Lateral Controller for Cargo ODD * [#1246] Resolve "Test case names currently use snake_case but should be CamelCase" * [#1325] Deactivate flaky smoke test in 'trajectory_follower_nodes' * [#1058] Add library code of longitudinal controller * Fix build error for trajectory follower Signed-off-by: wep21 * Fix build error for trajectory follower nodes Signed-off-by: wep21 * [#1272] Add AckermannControlCommand support to simple_planning_simulator * [#1058] Add Longitudinal Controller node * [#1058] Rename velocity_controller -> longitudinal_controller * [#1058] Update CMakeLists.txt for the longitudinal_controller_node * [#1058] Add smoke test python launch file * [#1058] Use LowPassFilter1d from trajectory_follower * [#1058] Use autoware_auto_msgs * [#1058] Changes for .auto (debug msg tmp fix, common func, tf listener) * [#1058] Remove unused parameters * [#1058] Fix ros test * [#1058] Rm default params from declare_parameters + use autoware types * [#1058] Use default param file to setup NodeOptions in the ros test * [#1058] Fix docstring * [#1058] Replace receiving a Twist with a VehicleKinematicState * [#1058] Change class variables format to m_ prefix * [#1058] Fix plugin name of LongitudinalController in CMakeLists.txt * [#1058] Fix copyright dates * [#1058] Reorder includes * [#1058] Add some tests (~89% coverage without disabling flaky tests) * [#1058] Add more tests (90+% coverage without disabling flaky tests) * [#1058] Use Float32MultiArrayDiagnostic message for debug and slope * [#1058] Calculate wheel_base value from vehicle parameters * [#1058] Cleanup redundant logger setting in tests * [#1058] Set ROS_DOMAIN_ID when running tests to prevent CI failures * [#1058] Remove TF listener and use published vehicle state instead * [#1058] Change smoke tests to use autoware_testing * [#1058] Add plotjuggler cfg for both lateral and longitudinal control * [#1058] Improve design documents * [#1058] Disable flaky test * [#1058] Properly transform vehicle state in longitudinal node * [#1058] Fix TF buffer of lateral controller * [#1058] Tuning of lateral controller for LGSVL * [#1058] Fix formating * [#1058] Fix /tf_static sub to be transient_local * [#1058] Fix yaw recalculation of reverse trajs in the lateral controller * modify trajectory_follower for galactic build Signed-off-by: Takamasa Horibe * [#1379] Update trajectory_follower * [#1379] Update simple_planning_simulator * [#1379] Update trajectory_follower_nodes * apply trajectory msg modification in control Signed-off-by: Takamasa Horibe * move directory Signed-off-by: Takamasa Horibe * remote control/trajectory_follower level dorectpry Signed-off-by: Takamasa Horibe * remove .iv trajectory follower Signed-off-by: Takamasa Horibe * use .auto trajectory_follower Signed-off-by: Takamasa Horibe * remove .iv simple_planning_simulator & osqp_interface Signed-off-by: Takamasa Horibe * use .iv simple_planning_simulator & osqp_interface Signed-off-by: Takamasa Horibe * add tmp_autoware_auto_dependencies Signed-off-by: Takamasa Horibe * tmporally add autoware_auto_msgs Signed-off-by: Takamasa Horibe * apply .auto message split Signed-off-by: Takamasa Horibe * fix build depend Signed-off-by: Takamasa Horibe * fix packages using osqp * fix autoware_auto_geometry * ignore lint of some packages * ignore ament_lint of some packages * ignore lint/pre-commit of trajectory_follower_nodes * disable unit tests of some packages Co-authored-by: Maxime CLEMENT Co-authored-by: Joshua Whitley Co-authored-by: Igor Bogoslavskyi Co-authored-by: MIURA Yasuyuki Co-authored-by: wep21 Co-authored-by: tomoya.kimura * [planning module]fix bug (#643) * fix topic name * remove angular z from LinearInterpolation in motion_velocity_smoother * FIx vehicle status topic name/type (#658) * shift -> gear_status * twist -> velocity_status * port closest velocity checker (#672) Signed-off-by: Takamasa Horibe * port trajectory visualizer (#682) Signed-off-by: Takamasa Horibe * add header for debug trajectories (#681) Signed-off-by: Takamasa Horibe * Sync .auto branch with the latest branch in internal repository (#691) * add trajectory point offset in rviz plugin (#2270) * sync rc rc/v0.23.0 (#2258) * fix interpolation for insert point (#2228) * fix interpolation for insert point * to prev interpolation pkg * Revert "to prev interpolation pkg" This reverts commit 9eb145b5d36e297186015fb17c267ccd5b3c21ef. Co-authored-by: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com> Co-authored-by: taikitanaka * fix topic name (#2266) Signed-off-by: Takamasa Horibe * Add namespace to diag for dual_return_filter (#2269) * Add a function to make 'geometry_msgs::msg::TransformStamped' (#2250) * Add a function to make 'geometry_msgs::msg::TransformStamped' * Add 'child_frame_id' as an argument of 'pose2transform' * Simplify marker scale initialization (#2286) * Fix/crosswalk polygon (#2279) * extend crosswalk polygon * improve readability * fix polygon shape * Add warning when decel distance calculation fails (#2289) Signed-off-by: Makoto Kurihara * [motion_velocity_smoother] ignore debug print (#2292) * cosmetic change Signed-off-by: Takamasa Horibe * cahnge severity from WARN to DEBUG for debug info Signed-off-by: Takamasa Horibe * use util for stop_watch Signed-off-by: Takamasa Horibe * fix map based prediction (#2200) * fix map based prediction * fix format * change map based prediction * fix spells * fix spells in comments * fix for cpplint * fix some problems * fix format and code for clang-tidy * fix space for cpplint * Update Readme.md * Update perception/object_recognition/prediction/map_based_prediction/Readme.md * Update perception/object_recognition/prediction/map_based_prediction/Readme.md * Update perception/object_recognition/prediction/map_based_prediction/Readme.md * Update perception/object_recognition/prediction/map_based_prediction/Readme.md * Update perception/object_recognition/prediction/map_based_prediction/Readme.md * Update perception/object_recognition/prediction/map_based_prediction/Readme.md * fix vector access method * fix readme format * add parameter * Update perception/object_recognition/prediction/map_based_prediction/Readme.md Co-authored-by: Kazuki Miyahara * Update perception/object_recognition/prediction/map_based_prediction/Readme.md Co-authored-by: Kazuki Miyahara * Update perception/object_recognition/prediction/map_based_prediction/Readme.md Co-authored-by: Kazuki Miyahara * Update Readme.md * Update perception/object_recognition/prediction/map_based_prediction/Readme.md Co-authored-by: Kazuki Miyahara Co-authored-by: tkimura4 Co-authored-by: Kazuki Miyahara * remove failure condition for 0 velocity trajectory (#2295) Signed-off-by: Takamasa Horibe * [mpc_follower] remove stop distance condition from stopState decision (#1916) * [mpc_follower] remove stop distance condition from stopState decision Signed-off-by: Takamasa Horibe * add invalid index handling Signed-off-by: Takamasa Horibe * Move the debug marker initialization part to another file (#2288) * Move the debug marker initialization part to 'debug.cpp' * Make 'isLocalOptimalSolutionOscillation' independent from 'NDTScanMatcher' (#2300) * Remove an unused function 'getTransform' (#2301) * Simplify iteration of initial poses (#2310) * Make a transform object const (#2311) * Represent poses in 'std::vector' instead of 'geometry_msgs::msg::PoseArray' (#2312) * Feature/no stopping area (#2163) * add no stopping area module to behavior velocity planner * apply utils * add polygon interpolation module order stopline around area is considered * devide jpass udge with stop line polygon * update docs * rename file name * update to latest * minor change for marker * update license Co-authored-by: Yukihiro Saito * update license Co-authored-by: Yukihiro Saito * update license Co-authored-by: Yukihiro Saito * update license Co-authored-by: Yukihiro Saito * minor fix * add parameter tuning at experiment * update readme * format doc * apply comments * add exception gurd * cosmetic change * fix ament * fix typo and remove for statement * & to " " * better ns * return pass judge param * add missing stoppable condition * add clear pass judge and stoppable flag * add comment * precommit fix * cpplint Co-authored-by: Yukihiro Saito * sync rc rc/v0.23.0 (#2281) * Fix side shift planner (#2171) (#2172) * add print debug Signed-off-by: TakaHoribe * remove forward shift points when adding new point Signed-off-by: TakaHoribe * remove debug print Signed-off-by: TakaHoribe * format Signed-off-by: TakaHoribe * Fix remove threshold Co-authored-by: Fumiya Watanabe Co-authored-by: Takamasa Horibe * Fix/pull out and pull over (#2175) * delete unnecessary check * fix condition of starting pull out * Add emergency status API (#2174) (#2182) * Fix/mpc reset prev result (#2185) (#2195) * reset prev result * clean code * reset only raw_steer_cmd * Update control/mpc_follower/src/mpc_follower_core.cpp Co-authored-by: Takamasa Horibe Co-authored-by: Takamasa Horibe Co-authored-by: Takamasa Horibe * [hotfix] 1 path point exception after resampling (#2204) * fix 1 path point exception after resampling Signed-off-by: TakaHoribe * Apply suggestions from code review * Apply suggestions from code review Co-authored-by: Takamasa Horibe Co-authored-by: tkimura4 * [hotfix] Fix lane ids (#2211) * Fix lane ids * Prevent acceleration on avoidance (#2214) * prevent acceleration on avoidance Signed-off-by: TakaHoribe * fix param name Signed-off-by: TakaHoribe * parametrize avoidance acc Signed-off-by: Takamasa Horibe * change param name Signed-off-by: Takamasa Horibe * fix typo Signed-off-by: Takamasa Horibe * Fix qos in roi cluster fusion (#2218) * fix confidence (#2220) * too high confidence (#2229) * Fix/obstacle stop 0.23.0 (#2232) * fix unexpected slow down in sharp curves (#2181) * Fix/insert implementation (#2186) Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> * [hotfix] Remove exception in avoidance module (#2233) * Remove exception * Fix clock * Remove blank line * Update traffic light state if ref stop point is ahead of previous one (#2197) Signed-off-by: wep21 * fix interpolation for insert point (#2228) * fix interpolation for insert point * to prev interpolation pkg * fix index (#2265) * turn signal calculation (#2280) * add turn signal funtion in path shifter * add ros parameters Co-authored-by: Fumiya Watanabe Co-authored-by: Takamasa Horibe Co-authored-by: tkimura4 Co-authored-by: Takagi, Isamu <43976882+isamu-takagi@users.noreply.github.com> Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> Co-authored-by: Yukihiro Saito Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Co-authored-by: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com> Co-authored-by: Sugatyon <32741405+Sugatyon@users.noreply.github.com> * [behavior_path_planner] fix sudden path change around ego (#2305) (#2318) * fix return-from-ego shift point generation logic Signed-off-by: Takamasa Horibe * change param for trimSimilarGradShiftPoint Signed-off-by: Takamasa Horibe * add comment for issue Signed-off-by: Takamasa Horibe * update comment Signed-off-by: Takamasa Horibe * replace code with function (logic has not changed) Signed-off-by: Takamasa Horibe * move func to cpp Signed-off-by: Takamasa Horibe * add comment for issue Signed-off-by: Takamasa Horibe * fix typo Signed-off-by: Takamasa Horibe * Update planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp Co-authored-by: Kazuki Miyahara * Update planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp Co-authored-by: Kazuki Miyahara Co-authored-by: Kazuki Miyahara Co-authored-by: Takamasa Horibe Co-authored-by: Kazuki Miyahara * Add functions to make stamped scalar messages (#2317) * Fix/object yaw in intersection module (#2294) * fix object orientation * fix function name * add guard (#2321) * reduce cost (double to float) (#2298) * Add detail collision check (#2274) * Add detail collision check Signed-off-by: wep21 * Remove unused function Signed-off-by: wep21 * Fix arc length Signed-off-by: wep21 * Seperate time margin Signed-off-by: wep21 * Fix parameter name Signed-off-by: wep21 * Update Readme Signed-off-by: wep21 * Address review: Add comment for TimeDistanceArray Signed-off-by: wep21 * Run pre-commit Signed-off-by: wep21 * Fix cpplint Signed-off-by: wep21 * Add return for empty polygon Signed-off-by: wep21 * update CenterPoint (#2222) * update to model trained by mmdet3d * add vizualizer (debug) * for multi-frame inputs * chagne config * use autoware_utils::pi * project specific model and param * rename vfe -> encoder * rename general to common * update download link * update * fix * rename model_name * change training toolbox link * chage lint package * fix test error * commit suggestion * Feature/lane change detection (#2331) * add old information deleter * fix access bug * change to deque * update obstacle buffer * fix some bugs * add lane change detector * make a update lanelet function * fix code style * parameterize essential values * Update perception/object_recognition/prediction/map_based_prediction/src/map_based_prediction_ros.cpp Co-authored-by: Kazuki Miyahara * fix slash position * remove unnecessary lines * fix format * fix format * change to new enum * fix format * fix typo and add guard * change funciton name * add lane change description Co-authored-by: Kazuki Miyahara * Add Planning Evaluator (#2293) * Add prototype planning evaluator Produced data for dist between points, curvature, and relative angle * Cleanup the code to make adding metrics easier * Add remaining basic metrics (length, duration, vel, accel, jerk) * Add motion_evaluator to evaluate the actual ego motion + code cleanup * Add deviation metrics * Add naive stability metric * Handle invalid stat (TODO: fix the output file formatting) * Add parameter file and cleanup * Add basic obstacle metric (TTC not yet implemented) and fix output file format * Add basic time to collision * Add lateral-distance based stability metric * Add check (at init time) that metrics' maps are complete * Publish metrics as ParamaterDeclaration msg (for openscenario) * Use lookahead and start from ego_pose when calculating stability metrics * Code cleanup * Fix lint * Add tests * Fix bug with Frechet dist and the last traj point * Finish implementing tests * Fix lint * Code cleanup * Update README.md * Remove unused metric * Change msg type of published metrics to DiagnosticArray * fix format to fix pre-commit check Signed-off-by: Takamasa Horibe * fix yaml format to fix pre-commit check Signed-off-by: Takamasa Horibe * fix yaml format Signed-off-by: Takamasa Horibe * apply clang-format Signed-off-by: Takamasa Horibe * apply clang-format Signed-off-by: Takamasa Horibe * Update planning/planning_diagnostics/planning_evaluator/include/planning_evaluator/planning_evaluator_node.hpp * Update planning/planning_diagnostics/planning_evaluator/test/test_planning_evaluator_node.cpp * Update planning/planning_diagnostics/planning_evaluator/test/test_planning_evaluator_node.cpp * change lint format to autoware_lint_common Signed-off-by: Takamasa Horibe Co-authored-by: Takamasa Horibe * Add keep braking function at driving state (#2346) * Add keep braking function at driving state Signed-off-by: Makoto Kurihara * Remove debug messages Signed-off-by: Makoto Kurihara * Fix format Signed-off-by: Makoto Kurihara * Change diag_updater's pediod from default to 0.1sec (#2348) * add cross judgement and common signal function (#2319) * merge branch turn_signal_common * add turn signal function in signal decider * add cross judge in path_utilities and delete from turn_signal_decider * remove original signal calculation in lane change * omit substitution * replace turn signal decider in pull over function * modify cross judge logic * replace turn signal decider in avoidance * add readme of turn signal * update * delete print debug * update * delete lane change decider in path shifter * delete blank line * fix indent * fix typo * fix typo * decrease nest * run pre commit * Add 0 limit at forward jerk velocity filter (#2340) Signed-off-by: Makoto Kurihara * add time offset param to point cloud concatenation (#2303) * add offset param * clang-format Co-authored-by: Akihito OHSATO * Feature/add doc for keep braking function at driving state (#2366) * Add the description of brake keeping Signed-off-by: Makoto Kurihara * Add the english document Signed-off-by: Makoto Kurihara * Improve description Signed-off-by: Makoto Kurihara * Add english description Signed-off-by: Makoto Kurihara * Fix include files (#2339) Signed-off-by: Kenji Miyake * fix behavior intersection module * fix behavior no stopping area module * fix planning_evaluator * fix motion_velocity_smoother * rename variable * Revert "[mpc_follower] remove stop distance condition from stopState decision (#1916)" This reverts commit ff4f0b5a844d1f835f1b93bd3b36a76747b0cd02. * Revert "Add keep braking function at driving state (#2346)" This reverts commit f0478187db4c28bf6092c198723dcc5ec11a9c70. * Revert "Feature/add doc for keep braking function at driving state (#2366)" This reverts commit 66de2f3924a479049fce2d5c5c6b579cacbd3e49. * Fix orientation availability in centerpoint Signed-off-by: wep21 * fix test_trajectory.cpp * add target link libraries * Use .auto msg in test code for planniing evaluator Signed-off-by: wep21 * fix include Signed-off-by: wep21 Co-authored-by: Takayuki Murooka Co-authored-by: autoware-iv-sync-ci[bot] <87871706+autoware-iv-sync-ci[bot]@users.noreply.github.com> Co-authored-by: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com> Co-authored-by: taikitanaka Co-authored-by: Takamasa Horibe Co-authored-by: Takeshi Miura <57553950+1222-takeshi@users.noreply.github.com> Co-authored-by: Takeshi Ishita Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Co-authored-by: Makoto Kurihara Co-authored-by: purewater0901 <43805014+purewater0901@users.noreply.github.com> Co-authored-by: tkimura4 Co-authored-by: Kazuki Miyahara Co-authored-by: Yukihiro Saito Co-authored-by: Fumiya Watanabe Co-authored-by: Takagi, Isamu <43976882+isamu-takagi@users.noreply.github.com> Co-authored-by: Sugatyon <32741405+Sugatyon@users.noreply.github.com> Co-authored-by: s-murakami-esol <81723883+s-murakami-esol@users.noreply.github.com> Co-authored-by: Yusuke Muramatsu Co-authored-by: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> Co-authored-by: Shunsuke Miura <37187849+miursh@users.noreply.github.com> Co-authored-by: Shinnosuke Hirakawa <8327162+0x126@users.noreply.github.com> Co-authored-by: Akihito OHSATO Co-authored-by: Kenji Miyake * fix osqp link (#700) * Update twist topic name (#736) Signed-off-by: wep21 * rename readme.ja * add default parameter file Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> Co-authored-by: Fumiya Watanabe Co-authored-by: yutaka Co-authored-by: purewater0901 <43805014+purewater0901@users.noreply.github.com> Co-authored-by: Takamasa Horibe Co-authored-by: pre-commit Co-authored-by: td12321 <85976747+td12321@users.noreply.github.com> Co-authored-by: Makoto Kurihara Co-authored-by: Takayuki Murooka Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> Co-authored-by: Takayuki Murooka Co-authored-by: Maxime CLEMENT Co-authored-by: Joshua Whitley Co-authored-by: Igor Bogoslavskyi Co-authored-by: MIURA Yasuyuki Co-authored-by: wep21 Co-authored-by: autoware-iv-sync-ci[bot] <87871706+autoware-iv-sync-ci[bot]@users.noreply.github.com> Co-authored-by: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com> Co-authored-by: taikitanaka Co-authored-by: Takeshi Miura <57553950+1222-takeshi@users.noreply.github.com> Co-authored-by: Takeshi Ishita Co-authored-by: Kazuki Miyahara Co-authored-by: Yukihiro Saito Co-authored-by: Takagi, Isamu <43976882+isamu-takagi@users.noreply.github.com> Co-authored-by: Sugatyon <32741405+Sugatyon@users.noreply.github.com> Co-authored-by: s-murakami-esol <81723883+s-murakami-esol@users.noreply.github.com> Co-authored-by: Yusuke Muramatsu Co-authored-by: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> Co-authored-by: Shunsuke Miura <37187849+miursh@users.noreply.github.com> Co-authored-by: Shinnosuke Hirakawa <8327162+0x126@users.noreply.github.com> Co-authored-by: Akihito OHSATO Co-authored-by: Kenji Miyake Co-authored-by: Kosuke Murakami --- .../motion_velocity_smoother/CMakeLists.txt | 69 ++ .../motion_velocity_smoother/README.ja.md | 250 ++++ planning/motion_velocity_smoother/README.md | 256 +++++ .../config/Analytical.param.yaml | 25 + .../config/JerkFiltered.param.yaml | 7 + .../config/L2.param.yaml | 5 + .../config/Linf.param.yaml | 5 + .../config/default_common.param.yaml | 15 + ...efault_motion_velocity_smoother.param.yaml | 51 + .../config/rqt_multiplot.xml | 317 ++++++ .../linear_interpolation.hpp | 34 + .../motion_velocity_smoother_node.hpp | 234 ++++ .../motion_velocity_smoother/resample.hpp | 55 + .../analytical_jerk_constrained_smoother.hpp | 117 ++ .../velocity_planning_utils.hpp | 59 + .../smoother/jerk_filtered_smoother.hpp | 71 ++ .../smoother/l2_pseudo_jerk_smoother.hpp | 59 + .../smoother/linf_pseudo_jerk_smoother.hpp | 59 + .../smoother/smoother_base.hpp | 76 ++ .../trajectory_utils.hpp | 91 ++ .../motion_velocity_smoother.launch.xml | 28 + .../motion_velocity_smoother_flow.drawio.svg | 3 + planning/motion_velocity_smoother/package.xml | 34 + .../scripts/closest_velocity_checker.py | 332 ++++++ .../scripts/trajectory_visualizer.py | 656 +++++++++++ .../src/linear_interpolation.cpp | 101 ++ .../src/motion_velocity_smoother_node.cpp | 1012 +++++++++++++++++ .../motion_velocity_smoother/src/resample.cpp | 266 +++++ .../analytical_jerk_constrained_smoother.cpp | 633 +++++++++++ .../velocity_planning_utils.cpp | 342 ++++++ .../src/smoother/jerk_filtered_smoother.cpp | 466 ++++++++ .../src/smoother/l2_pseudo_jerk_smoother.cpp | 224 ++++ .../smoother/linf_pseudo_jerk_smoother.cpp | 236 ++++ .../src/smoother/smoother_base.cpp | 96 ++ .../src/trajectory_utils.cpp | 643 +++++++++++ 35 files changed, 6927 insertions(+) create mode 100644 planning/motion_velocity_smoother/CMakeLists.txt create mode 100644 planning/motion_velocity_smoother/README.ja.md create mode 100644 planning/motion_velocity_smoother/README.md create mode 100644 planning/motion_velocity_smoother/config/Analytical.param.yaml create mode 100644 planning/motion_velocity_smoother/config/JerkFiltered.param.yaml create mode 100644 planning/motion_velocity_smoother/config/L2.param.yaml create mode 100644 planning/motion_velocity_smoother/config/Linf.param.yaml create mode 100644 planning/motion_velocity_smoother/config/default_common.param.yaml create mode 100644 planning/motion_velocity_smoother/config/default_motion_velocity_smoother.param.yaml create mode 100644 planning/motion_velocity_smoother/config/rqt_multiplot.xml create mode 100644 planning/motion_velocity_smoother/include/motion_velocity_smoother/linear_interpolation.hpp create mode 100644 planning/motion_velocity_smoother/include/motion_velocity_smoother/motion_velocity_smoother_node.hpp create mode 100644 planning/motion_velocity_smoother/include/motion_velocity_smoother/resample.hpp create mode 100644 planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp create mode 100644 planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.hpp create mode 100644 planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/jerk_filtered_smoother.hpp create mode 100644 planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp create mode 100644 planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp create mode 100644 planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/smoother_base.hpp create mode 100644 planning/motion_velocity_smoother/include/motion_velocity_smoother/trajectory_utils.hpp create mode 100644 planning/motion_velocity_smoother/launch/motion_velocity_smoother.launch.xml create mode 100644 planning/motion_velocity_smoother/media/motion_velocity_smoother_flow.drawio.svg create mode 100644 planning/motion_velocity_smoother/package.xml create mode 100755 planning/motion_velocity_smoother/scripts/closest_velocity_checker.py create mode 100755 planning/motion_velocity_smoother/scripts/trajectory_visualizer.py create mode 100644 planning/motion_velocity_smoother/src/linear_interpolation.cpp create mode 100644 planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp create mode 100644 planning/motion_velocity_smoother/src/resample.cpp create mode 100644 planning/motion_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp create mode 100644 planning/motion_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.cpp create mode 100644 planning/motion_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp create mode 100644 planning/motion_velocity_smoother/src/smoother/l2_pseudo_jerk_smoother.cpp create mode 100644 planning/motion_velocity_smoother/src/smoother/linf_pseudo_jerk_smoother.cpp create mode 100644 planning/motion_velocity_smoother/src/smoother/smoother_base.cpp create mode 100644 planning/motion_velocity_smoother/src/trajectory_utils.cpp diff --git a/planning/motion_velocity_smoother/CMakeLists.txt b/planning/motion_velocity_smoother/CMakeLists.txt new file mode 100644 index 0000000000000..27f840cf38c5f --- /dev/null +++ b/planning/motion_velocity_smoother/CMakeLists.txt @@ -0,0 +1,69 @@ +cmake_minimum_required(VERSION 3.5) +project(motion_velocity_smoother) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) + set(CMAKE_CXX_STANDARD_REQUIRED ON) + set(CMAKE_CXX_EXTENSIONS OFF) +endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +find_package(Boost REQUIRED) +find_package(eigen3_cmake_module REQUIRED) +find_package(Eigen3 REQUIRED) + + +set(MOTION_VELOCITY_SMOOTHER_SRC + src/motion_velocity_smoother_node.cpp + src/trajectory_utils.cpp + src/linear_interpolation.cpp + src/resample.cpp +) + +set(SMOOTHER_SRC + src/smoother/smoother_base.cpp + src/smoother/l2_pseudo_jerk_smoother.cpp + src/smoother/linf_pseudo_jerk_smoother.cpp + src/smoother/jerk_filtered_smoother.cpp + src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp + src/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.cpp +) + +ament_auto_add_library(smoother SHARED + ${SMOOTHER_SRC} +) + +ament_auto_add_library(motion_velocity_smoother_node SHARED + ${MOTION_VELOCITY_SMOOTHER_SRC} +) + +target_link_libraries(motion_velocity_smoother_node + smoother +) + +rclcpp_components_register_node(motion_velocity_smoother_node + PLUGIN "motion_velocity_smoother::MotionVelocitySmootherNode" + EXECUTABLE motion_velocity_smoother +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_auto_package( + INSTALL_TO_SHARE + launch + config +) + +install(PROGRAMS + scripts/trajectory_visualizer.py + scripts/closest_velocity_checker.py + DESTINATION lib/${PROJECT_NAME} +) diff --git a/planning/motion_velocity_smoother/README.ja.md b/planning/motion_velocity_smoother/README.ja.md new file mode 100644 index 0000000000000..e59b7087d20f3 --- /dev/null +++ b/planning/motion_velocity_smoother/README.ja.md @@ -0,0 +1,250 @@ +# Motion Velocity Smoother + +## Purpose + +`motion_velocity_smoother`は目標軌道上の各点における望ましい車速を計画して出力するモジュールである。 +このモジュールは、速度の最大化と乗り心地の良さを両立するために、事前に指定された制限速度、制限加速度および制限躍度の範囲で車速を計画する。 +加速度や躍度の制限を与えることは車速の変化を滑らかにすることに対応するため、このモジュールを`motion_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`, `L2`, `Linf`の 3 種類のうちからコンフィグで指定する。 +最適化のソルバは OSQP[1]を利用する。 + +##### JerkFiltered + +速度の 2 乗（最小化で表すため負値で表現）、制限速度逸脱量の 2 乗、制限加度逸脱量の 2 乗、制限ジャーク逸脱量の 2 乗、ジャークの 2 乗の総和を最小化する。 + +##### L2 + +速度の 2 乗（最小化で表すため負値で表現）、制限速度逸脱量の 2 乗、制限加度逸脱量の 2 乗、疑似ジャーク[2]の 2 乗の総和を最小化する。 + +##### Linf + +速度の 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_auto_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_auto_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_auto_planning_msgs/Trajectory` | Extracted trajectory (for debug) | +| `~/debug/trajectory_external_velocity_limited` | `autoware_auto_planning_msgs/Trajectory` | External velocity limited trajectory (for debug) | +| `~/debug/trajectory_lateral_acc_filtered` | `autoware_auto_planning_msgs/Trajectory` | Lateral acceleration limit filtered trajectory (for debug) | +| `~/debug/trajectory_time_resampled` | `autoware_auto_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_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 | + +### 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 | + +#### L2 + +| Name | Type | Description | Default value | +| :------------------- | :------- | :--------------------------------- | :------------ | +| `pseudo_jerk_weight` | `double` | Weight for "smoothness" cost | 100.0 | +| `over_v_weight` | `double` | Weight for "over speed limit" cost | 100000.0 | +| `over_a_weight` | `double` | Weight for "over accel limit" cost | 1000.0 | + +#### Linf + +| Name | Type | Description | Default value | +| :------------------- | :------- | :--------------------------------- | :------------ | +| `pseudo_jerk_weight` | `double` | Weight for "smoothness" cost | 100.0 | +| `over_v_weight` | `double` | Weight for "over speed limit" cost | 100000.0 | +| `over_a_weight` | `double` | Weight for "over accel 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/motion_velocity_smoother/README.md b/planning/motion_velocity_smoother/README.md new file mode 100644 index 0000000000000..0254b53fc7eb0 --- /dev/null +++ b/planning/motion_velocity_smoother/README.md @@ -0,0 +1,256 @@ +# Motion Velocity Smoother + +## Purpose + +`motion_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 `motion_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 `motion_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 calculate 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`. + +#### 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. +The algorithm of velocity planning is chosen from `JerkFiltered`, `L2` and `Linf`, 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. + +##### L2 + +It minimizes the sum of the minus of the square of the velocity, the square of the the pseudo-jerk[2] and the square of the violation of the velocity limit and the acceleration limit. + +##### Linf + +It minimizes the sum of the minus of the square of the velocity, the maximum absolute value of the the pseudo-jerk[2] and the square of the violation of the velocity limit and the acceleration 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_auto_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_auto_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_auto_planning_msgs/Trajectory` | Extracted trajectory (for debug) | +| `~/debug/trajectory_external_velocity_limited` | `autoware_auto_planning_msgs/Trajectory` | External velocity limited trajectory (for debug) | +| `~/debug/trajectory_lateral_acc_filtered` | `autoware_auto_planning_msgs/Trajectory` | Lateral acceleration limit filtered trajectory (for debug) | +| `~/debug/trajectory_time_resampled` | `autoware_auto_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_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 | + +### 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 | + +#### L2 + +| Name | Type | Description | Default value | +| :------------------- | :------- | :--------------------------------- | :------------ | +| `pseudo_jerk_weight` | `double` | Weight for "smoothness" cost | 100.0 | +| `over_v_weight` | `double` | Weight for "over speed limit" cost | 100000.0 | +| `over_a_weight` | `double` | Weight for "over accel limit" cost | 1000.0 | + +#### Linf + +| Name | Type | Description | Default value | +| :------------------- | :------- | :--------------------------------- | :------------ | +| `pseudo_jerk_weight` | `double` | Weight for "smoothness" cost | 100.0 | +| `over_v_weight` | `double` | Weight for "over speed limit" cost | 100000.0 | +| `over_a_weight` | `double` | Weight for "over accel 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/motion_velocity_smoother/config/Analytical.param.yaml b/planning/motion_velocity_smoother/config/Analytical.param.yaml new file mode 100644 index 0000000000000..329714e3d371e --- /dev/null +++ b/planning/motion_velocity_smoother/config/Analytical.param.yaml @@ -0,0 +1,25 @@ +/**: + ros__parameters: + resample: + ds_resample: 0.1 + num_resample: 1 + delta_yaw_threshold: 0.785 + + latacc: + enable_constant_velocity_while_turning: false + constant_velocity_dist_threshold: 2.0 + + forward: + max_acc: 1.0 + min_acc: -1.0 + max_jerk: 0.3 + min_jerk: -0.3 + kp: 0.3 + + backward: + start_jerk: -0.1 + min_jerk_mild_stop: -0.3 + min_jerk: -1.5 + min_acc_mild_stop: -1.0 + min_acc: -2.5 + span_jerk: -0.01 diff --git a/planning/motion_velocity_smoother/config/JerkFiltered.param.yaml b/planning/motion_velocity_smoother/config/JerkFiltered.param.yaml new file mode 100644 index 0000000000000..78c088b0843a2 --- /dev/null +++ b/planning/motion_velocity_smoother/config/JerkFiltered.param.yaml @@ -0,0 +1,7 @@ +/**: + ros__parameters: + jerk_weight: 10.0 # weight for "smoothness" cost for jerk + over_v_weight: 100000.0 # weight for "over speed limit" cost + over_a_weight: 5000.0 # weight for "over accel limit" cost + over_j_weight: 2000.0 # weight for "over jerk limit" cost + jerk_filter_ds: 0.1 # resampling ds for jerk filter diff --git a/planning/motion_velocity_smoother/config/L2.param.yaml b/planning/motion_velocity_smoother/config/L2.param.yaml new file mode 100644 index 0000000000000..c993978204f10 --- /dev/null +++ b/planning/motion_velocity_smoother/config/L2.param.yaml @@ -0,0 +1,5 @@ +/**: + ros__parameters: + pseudo_jerk_weight: 100.0 # weight for "smoothness" cost + over_v_weight: 100000.0 # weight for "over speed limit" cost + over_a_weight: 1000.0 # weight for "over accel limit" cost diff --git a/planning/motion_velocity_smoother/config/Linf.param.yaml b/planning/motion_velocity_smoother/config/Linf.param.yaml new file mode 100644 index 0000000000000..ec38010621cc9 --- /dev/null +++ b/planning/motion_velocity_smoother/config/Linf.param.yaml @@ -0,0 +1,5 @@ +/**: + ros__parameters: + pseudo_jerk_weight: 200.0 # weight for "smoothness" cost + over_v_weight: 100000.0 # weight for "over speed limit" cost + over_a_weight: 5000.0 # weight for "over accel limit" cost diff --git a/planning/motion_velocity_smoother/config/default_common.param.yaml b/planning/motion_velocity_smoother/config/default_common.param.yaml new file mode 100644 index 0000000000000..a23570a5fc791 --- /dev/null +++ b/planning/motion_velocity_smoother/config/default_common.param.yaml @@ -0,0 +1,15 @@ +/**: + ros__parameters: + # constraints param for normal driving + normal: + min_acc: -0.5 # min deceleration [m/ss] + max_acc: 1.0 # max acceleration [m/ss] + min_jerk: -0.5 # min jerk [m/sss] + max_jerk: 1.0 # max jerk [m/sss] + + # constraints to be observed + limit: + min_acc: -2.5 # min deceleration limit [m/ss] + max_acc: 1.0 # max acceleration limit [m/ss] + min_jerk: -1.5 # min jerk limit [m/sss] + max_jerk: 1.5 # max jerk limit [m/sss] diff --git a/planning/motion_velocity_smoother/config/default_motion_velocity_smoother.param.yaml b/planning/motion_velocity_smoother/config/default_motion_velocity_smoother.param.yaml new file mode 100644 index 0000000000000..904a80d6f4ffd --- /dev/null +++ b/planning/motion_velocity_smoother/config/default_motion_velocity_smoother.param.yaml @@ -0,0 +1,51 @@ +/**: + 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 + max_lateral_accel: 0.8 # max lateral acceleration limit [m/ss] + min_curve_velocity: 2.74 # min velocity at lateral acceleration limit [m/ss] + decel_distance_before_curve: 3.5 # slow speed distance before a curve for lateral acceleration limit + decel_distance_after_curve: 2.0 # slow speed distance after a curve for lateral acceleration limit + + # engage & replan parameters + replan_vel_deviation: 5.53 # velocity deviation to replan initial velocity [m/s] + engage_velocity: 0.25 # engage velocity threshold [m/s] (if the trajectory velocity is higher than this value, use this velocity for engage vehicle speed) + engage_acceleration: 0.1 # engage acceleration [m/ss] (use this acceleration when engagement) + engage_exit_ratio: 0.5 # exit engage sequence to normal velocity planning when the velocity exceeds engage_exit_ratio x engage_velocity. + stop_dist_to_prohibit_engage: 0.5 # if the stop point is in this distance, the speed is set to 0 not to move the vehicle [m] + + # stop velocity + stopping_velocity: 2.778 # change target velocity to this value before v=0 point [m/s] + stopping_distance: 0.0 # distance for the stopping_velocity [m]. 0 means the stopping velocity is not applied. + + # path extraction parameters + extract_ahead_dist: 200.0 # forward trajectory distance used for planning [m] + extract_behind_dist: 5.0 # backward trajectory distance used for planning [m] + delta_yaw_threshold: 1.0472 # Allowed delta yaw between ego pose and trajectory pose [radian] + + # resampling parameters for optimization + max_trajectory_length: 200.0 # max trajectory length for resampling [m] + min_trajectory_length: 150.0 # min trajectory length for resampling [m] + resample_time: 5.0 # resample total time for dense sampling [s] + dense_dt: 0.1 # resample time interval for dense sampling [s] + dense_min_interval_distance: 0.1 # minimum points-interval length for dense sampling [m] + sparse_dt: 0.5 # resample time interval for sparse sampling [s] + sparse_min_interval_distance: 4.0 # minimum points-interval length for sparse sampling [m] + + # resampling parameters for post process + post_max_trajectory_length: 300.0 # max trajectory length for resampling [m] + post_min_trajectory_length: 30.0 # min trajectory length for resampling [m] + post_resample_time: 10.0 # resample total time for dense sampling [s] + post_dense_dt: 0.1 # resample time interval for dense sampling [s] + post_dense_min_interval_distance: 0.1 # minimum points-interval length for dense sampling [m] + post_sparse_dt: 0.1 # resample time interval for sparse sampling [s] + post_sparse_min_interval_distance: 1.0 # minimum points-interval length for sparse sampling [m] + + # system + over_stop_velocity_warn_thr: 1.389 # used to check if the optimization exceeds the input velocity on the stop point diff --git a/planning/motion_velocity_smoother/config/rqt_multiplot.xml b/planning/motion_velocity_smoother/config/rqt_multiplot.xml new file mode 100644 index 0000000000000..ad0ff4b68251e --- /dev/null +++ b/planning/motion_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/motion_velocity_smoother/closest_velocity + std_msgs/Float32 + + + data + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /planning/scenario_planning/motion_velocity_smoother/closest_velocity + std_msgs/Float32 + + + + #000000 + 1 + + + 1000 + 10 + 0 + + + 100 + Optimum Velocity + + + + + + 1 + + 1000 + 0 + 0 + -1000 + 0 + + /planning/scenario_planning/motion_velocity_smoother/closest_merged_velocity + std_msgs/Float32 + + + data + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /planning/scenario_planning/motion_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/motion_velocity_smoother/closest_max_velocity + std_msgs/Float32 + + + data + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /planning/scenario_planning/motion_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/motion_velocity_smoother/closest_acceleration + std_msgs/Float32 + + + data + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /planning/scenario_planning/motion_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/motion_velocity_smoother/calculation_time + std_msgs/Float32 + + + data + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /planning/scenario_planning/motion_velocity_smoother/calculation_time + std_msgs/Float32 + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + Calculation Time + + + + true + + 30 + Calculation Time + + + + false +
+ diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/linear_interpolation.hpp b/planning/motion_velocity_smoother/include/motion_velocity_smoother/linear_interpolation.hpp new file mode 100644 index 0000000000000..10c0bca1ddddb --- /dev/null +++ b/planning/motion_velocity_smoother/include/motion_velocity_smoother/linear_interpolation.hpp @@ -0,0 +1,34 @@ +// 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 MOTION_VELOCITY_SMOOTHER__LINEAR_INTERPOLATION_HPP_ +#define MOTION_VELOCITY_SMOOTHER__LINEAR_INTERPOLATION_HPP_ + +#include + +#include +#include +#include + +namespace motion_velocity_smoother +{ +namespace linear_interpolation +{ +boost::optional> interpolate( + const std::vector & base_index, const std::vector & base_value, + const std::vector & return_index); +} +} // namespace motion_velocity_smoother + +#endif // MOTION_VELOCITY_SMOOTHER__LINEAR_INTERPOLATION_HPP_ diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/motion_velocity_smoother_node.hpp b/planning/motion_velocity_smoother/include/motion_velocity_smoother/motion_velocity_smoother_node.hpp new file mode 100644 index 0000000000000..b4b50655f17b1 --- /dev/null +++ b/planning/motion_velocity_smoother/include/motion_velocity_smoother/motion_velocity_smoother_node.hpp @@ -0,0 +1,234 @@ +// 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 MOTION_VELOCITY_SMOOTHER__MOTION_VELOCITY_SMOOTHER_NODE_HPP_ +#define MOTION_VELOCITY_SMOOTHER__MOTION_VELOCITY_SMOOTHER_NODE_HPP_ + +#include "autoware_utils/geometry/geometry.hpp" +#include "autoware_utils/math/unit_conversion.hpp" +#include "autoware_utils/ros/self_pose_listener.hpp" +#include "autoware_utils/system/stop_watch.hpp" +#include "autoware_utils/trajectory/tmp_conversion.hpp" +#include "autoware_utils/trajectory/trajectory.hpp" +#include "motion_velocity_smoother/resample.hpp" +#include "motion_velocity_smoother/smoother/jerk_filtered_smoother.hpp" +#include "motion_velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp" +#include "motion_velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp" +#include "osqp_interface/osqp_interface.hpp" +#include "rclcpp/rclcpp.hpp" +#include "tf2/utils.h" +#include "tf2_ros/transform_listener.h" + +#include "autoware_auto_planning_msgs/msg/trajectory.hpp" +#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" +#include "autoware_debug_msgs/msg/float32_stamped.hpp" // temporary +#include "autoware_planning_msgs/msg/stop_speed_exceeded.hpp" // temporary +#include "autoware_planning_msgs/msg/velocity_limit.hpp" // temporary +#include "nav_msgs/msg/odometry.hpp" + +#include +#include +#include +#include +#include + +// *INDENT-OFF* +#include "motion_velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp" +// *INDENT-ON* +#include "motion_velocity_smoother/smoother/smoother_base.hpp" +#include "motion_velocity_smoother/trajectory_utils.hpp" + +namespace motion_velocity_smoother +{ +using autoware_auto_planning_msgs::msg::Trajectory; +using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using TrajectoryPoints = std::vector; +using autoware_debug_msgs::msg::Float32Stamped; // temporary +using autoware_planning_msgs::msg::StopSpeedExceeded; // temporary +using autoware_planning_msgs::msg::VelocityLimit; // temporary +using geometry_msgs::msg::Pose; +using geometry_msgs::msg::PoseStamped; +using nav_msgs::msg::Odometry; + +class MotionVelocitySmootherNode : public rclcpp::Node +{ +public: + explicit MotionVelocitySmootherNode(const rclcpp::NodeOptions & node_options); + +private: + rclcpp::Publisher::SharedPtr + pub_trajectory_; //!< @brief publisher for output trajectory + rclcpp::Publisher::SharedPtr + pub_over_stop_velocity_; //!< @brief publisher for over stop velocity warning + rclcpp::Subscription::SharedPtr + sub_current_odometry_; //!< @brief subscriber for current velocity + rclcpp::Subscription::SharedPtr + sub_current_trajectory_; //!< @brief subscriber for reference trajectory + rclcpp::Subscription::SharedPtr + sub_external_velocity_limit_; //!< @brief subscriber for external velocity limit + + PoseStamped::ConstSharedPtr current_pose_ptr_; // current vehicle pose + Odometry::ConstSharedPtr current_odometry_ptr_; // current odometry + Trajectory::ConstSharedPtr base_traj_raw_ptr_; // current base_waypoints + double external_velocity_limit_; // current external_velocity_limit + double max_velocity_with_deceleration_; // maximum velocity with deceleration + // for external velocity limit + double external_velocity_limit_dist_{0.0}; // distance to set external velocity limit + + TrajectoryPoints prev_output_; // previously published trajectory + boost::optional prev_closest_point_{}; // previous trajectory point + // closest to ego vehicle + + autoware_utils::SelfPoseListener self_pose_listener_{this}; + + enum class AlgorithmType { + INVALID = 0, + JERK_FILTERED = 1, + L2 = 2, + LINF = 3, + ANALYTICAL = 4, + }; + + enum class InitializeType { + INIT = 0, + LARGE_DEVIATION_REPLAN = 1, + ENGAGING = 2, + NORMAL = 3, + }; + + struct Param + { + 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 delta_yaw_threshold; // for closest index calculation + resampling::ResampleParam post_resample_param; + AlgorithmType algorithm_type; // Option : JerkFiltered, Linf, L2 + } node_param_{}; + + SmootherBase::BaseParam base_param_{}; + JerkFilteredSmoother::Param jerk_filtered_smoother_param_{}; + L2PseudoJerkSmoother::Param l2_pseudo_jerk_smoother_param_{}; + LinfPseudoJerkSmoother::Param linf_pseudo_jerk_smoother_param_{}; + AnalyticalJerkConstrainedSmoother::Param analytical_jerk_constrained_smoother_param_{}; + + std::shared_ptr smoother_; + + bool publish_debug_trajs_; // publish planned trajectories + + double over_stop_velocity_warn_thr_; // threshold to publish over velocity warn + + // parameter update + OnSetParametersCallbackHandle::SharedPtr set_param_res_; + rcl_interfaces::msg::SetParametersResult onParameter( + const std::vector & parameters); + + // topic callback + void onCurrentOdometry(const Odometry::ConstSharedPtr msg); + + void onCurrentTrajectory(const Trajectory::ConstSharedPtr msg); + + void onExternalVelocityLimit(const VelocityLimit::ConstSharedPtr msg); + + // publish methods + void publishTrajectory(const TrajectoryPoints & traj) const; + + void publishStopDistance(const TrajectoryPoints & trajectory, const size_t closest) const; + + // non-const methods + void publishClosestState(const TrajectoryPoint & closest_point); + + // const methods + bool checkData() const; + + AlgorithmType getAlgorithmType(const std::string & algorithm_name) const; + + TrajectoryPoints calcTrajectoryVelocity(const TrajectoryPoints & input) const; + + bool smoothVelocity(const TrajectoryPoints & input, TrajectoryPoints & traj_smoothed) const; + + std::tuple calcInitialMotion( + const TrajectoryPoints & input_traj, const size_t input_closest, + const TrajectoryPoints & prev_traj) 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) const; + + // parameter handling + void initCommonParam(); + + void initSmootherBaseParam(); + + void initJerkFilteredSmootherParam(); + + void initL2PseudoJerkSmootherParam(); + + void initLinfPseudoJerkSmootherParam(); + + void initAnalyticalJerkConstrainedSmootherParam(); + + // debug + autoware_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_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_; + + // 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_; +}; +} // namespace motion_velocity_smoother + +#endif // MOTION_VELOCITY_SMOOTHER__MOTION_VELOCITY_SMOOTHER_NODE_HPP_ diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/resample.hpp b/planning/motion_velocity_smoother/include/motion_velocity_smoother/resample.hpp new file mode 100644 index 0000000000000..cd13a081e94a2 --- /dev/null +++ b/planning/motion_velocity_smoother/include/motion_velocity_smoother/resample.hpp @@ -0,0 +1,55 @@ +// 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 MOTION_VELOCITY_SMOOTHER__RESAMPLE_HPP_ +#define MOTION_VELOCITY_SMOOTHER__RESAMPLE_HPP_ + +#include "autoware_utils/trajectory/trajectory.hpp" +#include "motion_velocity_smoother/trajectory_utils.hpp" + +#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" + +#include "boost/optional.hpp" + +#include + +namespace motion_velocity_smoother +{ +namespace resampling +{ +using autoware_auto_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] +}; + +boost::optional resampleTrajectory( + const TrajectoryPoints & input, const double v_current, const size_t closest_id, + const ResampleParam & param); + +boost::optional resampleTrajectory( + const TrajectoryPoints & input, const size_t closest_id, const ResampleParam & param, + const double nominal_ds); +} // namespace resampling +} // namespace motion_velocity_smoother + +#endif // MOTION_VELOCITY_SMOOTHER__RESAMPLE_HPP_ diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp b/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp new file mode 100644 index 0000000000000..412427f0277f9 --- /dev/null +++ b/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp @@ -0,0 +1,117 @@ +// 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. + +// *INDENT-OFF* +#ifndef MOTION_VELOCITY_SMOOTHER__SMOOTHER__ANALYTICAL_JERK_CONSTRAINED_SMOOTHER__ANALYTICAL_JERK_CONSTRAINED_SMOOTHER_HPP_ // NOLINT +#define MOTION_VELOCITY_SMOOTHER__SMOOTHER__ANALYTICAL_JERK_CONSTRAINED_SMOOTHER__ANALYTICAL_JERK_CONSTRAINED_SMOOTHER_HPP_ // NOLINT +// *INDENT-ON* + +#include "autoware_utils/trajectory/trajectory.hpp" +#include "rclcpp/rclcpp.hpp" +#include "tf2/utils.h" + +#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" +#include "geometry_msgs/msg/pose.hpp" + +#include +#include +#include + +// *INDENT-OFF* +#include "motion_velocity_smoother/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.hpp" +// *INDENT-ON* +#include "motion_velocity_smoother/smoother/smoother_base.hpp" + +namespace motion_velocity_smoother +{ +class AnalyticalJerkConstrainedSmoother : public SmootherBase +{ +public: + struct Param + { + struct + { + double ds_resample; + double num_resample; + double delta_yaw_threshold; + } resample; + struct + { + bool enable_constant_velocity_while_turning; + double constant_velocity_dist_threshold; + } latacc; + struct + { + double max_acc; + double min_acc; + double max_jerk; + double min_jerk; + double kp; + } forward; + struct + { + double start_jerk; + double min_jerk_mild_stop; + double min_jerk; + double min_acc_mild_stop; + double min_acc; + double span_jerk; + } backward; + }; + + explicit AnalyticalJerkConstrainedSmoother(const Param & p); + + bool apply( + const double initial_vel, const double initial_acc, const TrajectoryPoints & input, + TrajectoryPoints & output, std::vector & debug_trajectories) override; + + boost::optional resampleTrajectory( + const TrajectoryPoints & input, const double v_current, const int closest_id) const override; + + boost::optional applyLateralAccelerationFilter( + const TrajectoryPoints & input) const override; + + void setParam(const Param & param); + +private: + Param smoother_param_; + rclcpp::Logger logger_{ + rclcpp::get_logger("smoother").get_child("analytical_jerk_constrained_smoother")}; + + bool searchDecelTargetIndices( + const TrajectoryPoints & trajectory, const size_t closest_index, + std::vector> & decel_target_indices) const; + bool applyForwardJerkFilter( + const TrajectoryPoints & base_trajectory, const size_t start_index, const double initial_vel, + const double initial_acc, const Param & params, TrajectoryPoints & output_trajectory) const; + bool applyBackwardDecelFilter( + const std::vector & start_indices, const size_t decel_target_index, + const double decel_target_vel, const Param & params, + TrajectoryPoints & output_trajectory) const; + bool calcEnoughDistForDecel( + const TrajectoryPoints & trajectory, const size_t start_index, const double decel_target_vel, + const double planning_jerk, const Param & params, const std::vector & dist_to_target, + bool & is_enough_dist, int & type, std::vector & times, double & stop_dist) const; + bool applyDecelVelocityFilter( + const size_t decel_start_index, const double decel_target_vel, const double planning_jerk, + const Param & params, const int type, const std::vector & times, + TrajectoryPoints & output_trajectory) const; + + // debug + std::string strTimes(const std::vector & times) const; + std::string strStartIndices(const std::vector & start_indices) const; +}; +} // namespace motion_velocity_smoother + +#endif diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.hpp b/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.hpp new file mode 100644 index 0000000000000..6c0156257cd96 --- /dev/null +++ b/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.hpp @@ -0,0 +1,59 @@ +// 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. + +// *INDENT-OFF* +#ifndef MOTION_VELOCITY_SMOOTHER__SMOOTHER__ANALYTICAL_JERK_CONSTRAINED_SMOOTHER__VELOCITY_PLANNING_UTILS_HPP_ // NOLINT +#define MOTION_VELOCITY_SMOOTHER__SMOOTHER__ANALYTICAL_JERK_CONSTRAINED_SMOOTHER__VELOCITY_PLANNING_UTILS_HPP_ // NOLINT +// *INDENT-ON* + +#include "autoware_utils/trajectory/trajectory.hpp" +#include "motion_velocity_smoother/linear_interpolation.hpp" +#include "rclcpp/rclcpp.hpp" + +#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" + +#include +#include +#include +#include + +namespace motion_velocity_smoother +{ +namespace analytical_velocity_planning_utils +{ +using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using TrajectoryPoints = std::vector; + +bool calcStopDistWithJerkAndAccConstraints( + const double v0, const double a0, const double jerk_acc, const double jerk_dec, + const double min_acc, const double target_vel, int & type, std::vector & times, + double & stop_dist); +bool validCheckCalcStopDist( + const double v_end, const double a_end, const double v_target, const double a_target, + const double v_margin, const double a_margin); +bool calcStopVelocityWithConstantJerkAccLimit( + const double v0, const double a0, const double jerk_acc, const double jerk_dec, + const double min_acc, const double decel_target_vel, const int type, + const std::vector & times, const size_t start_index, + TrajectoryPoints & output_trajectory); +void updateStopVelocityStatus( + double v0, double a0, double jerk_acc, double jerk_dec, int type, std::vector times, + double t, double & x, double & v, double & a, double & j); +double integ_x(double x0, double v0, double a0, double j0, double t); +double integ_v(double v0, double a0, double j0, double t); +double integ_a(double a0, double j0, double t); +} // namespace analytical_velocity_planning_utils +} // namespace motion_velocity_smoother + +#endif diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/jerk_filtered_smoother.hpp b/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/jerk_filtered_smoother.hpp new file mode 100644 index 0000000000000..c3e4f16cc344b --- /dev/null +++ b/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/jerk_filtered_smoother.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 MOTION_VELOCITY_SMOOTHER__SMOOTHER__JERK_FILTERED_SMOOTHER_HPP_ +#define MOTION_VELOCITY_SMOOTHER__SMOOTHER__JERK_FILTERED_SMOOTHER_HPP_ + +#include "autoware_utils/geometry/geometry.hpp" +#include "autoware_utils/trajectory/trajectory.hpp" +#include "motion_velocity_smoother/smoother/smoother_base.hpp" +#include "osqp_interface/osqp_interface.hpp" + +#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" + +#include "boost/optional.hpp" + +#include + +namespace motion_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(const Param & p); + + bool apply( + const double initial_vel, const double initial_acc, const TrajectoryPoints & input, + TrajectoryPoints & output, std::vector & debug_trajectories) override; + + boost::optional resampleTrajectory( + const TrajectoryPoints & input, const double v_current, const int closest_id) const override; + + void setParam(const Param & param); + +private: + Param smoother_param_; + autoware::common::osqp::OSQPInterface qp_solver_; + 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 motion_velocity_smoother + +#endif // MOTION_VELOCITY_SMOOTHER__SMOOTHER__JERK_FILTERED_SMOOTHER_HPP_ diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp b/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp new file mode 100644 index 0000000000000..fd203dee78a67 --- /dev/null +++ b/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp @@ -0,0 +1,59 @@ +// 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 MOTION_VELOCITY_SMOOTHER__SMOOTHER__L2_PSEUDO_JERK_SMOOTHER_HPP_ +#define MOTION_VELOCITY_SMOOTHER__SMOOTHER__L2_PSEUDO_JERK_SMOOTHER_HPP_ + +#include "autoware_utils/geometry/geometry.hpp" +#include "autoware_utils/trajectory/trajectory.hpp" +#include "motion_velocity_smoother/smoother/smoother_base.hpp" +#include "osqp_interface/osqp_interface.hpp" + +#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" + +#include "boost/optional.hpp" + +#include + +namespace motion_velocity_smoother +{ +class L2PseudoJerkSmoother : public SmootherBase +{ +public: + struct Param + { + double pseudo_jerk_weight; + double over_v_weight; + double over_a_weight; + }; + + explicit L2PseudoJerkSmoother(const Param & smoother_param); + + bool apply( + const double initial_vel, const double initial_acc, const TrajectoryPoints & input, + TrajectoryPoints & output, std::vector & debug_trajectories) override; + + boost::optional resampleTrajectory( + const TrajectoryPoints & input, const double v_current, const int closest_id) const override; + + void setParam(const Param & smoother_param); + +private: + Param smoother_param_; + autoware::common::osqp::OSQPInterface qp_solver_; + rclcpp::Logger logger_{rclcpp::get_logger("smoother").get_child("l2_pseudo_jerk_smoother")}; +}; +} // namespace motion_velocity_smoother + +#endif // MOTION_VELOCITY_SMOOTHER__SMOOTHER__L2_PSEUDO_JERK_SMOOTHER_HPP_ diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp b/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp new file mode 100644 index 0000000000000..89e641d183751 --- /dev/null +++ b/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp @@ -0,0 +1,59 @@ +// 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 MOTION_VELOCITY_SMOOTHER__SMOOTHER__LINF_PSEUDO_JERK_SMOOTHER_HPP_ +#define MOTION_VELOCITY_SMOOTHER__SMOOTHER__LINF_PSEUDO_JERK_SMOOTHER_HPP_ + +#include "autoware_utils/geometry/geometry.hpp" +#include "autoware_utils/trajectory/trajectory.hpp" +#include "motion_velocity_smoother/smoother/smoother_base.hpp" +#include "osqp_interface/osqp_interface.hpp" + +#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" + +#include "boost/optional.hpp" + +#include + +namespace motion_velocity_smoother +{ +class LinfPseudoJerkSmoother : public SmootherBase +{ +public: + struct Param + { + double pseudo_jerk_weight; + double over_v_weight; + double over_a_weight; + }; + + explicit LinfPseudoJerkSmoother(const Param & smoother_param); + + bool apply( + const double initial_vel, const double initial_acc, const TrajectoryPoints & input, + TrajectoryPoints & output, std::vector & debug_trajectories) override; + + boost::optional resampleTrajectory( + const TrajectoryPoints & input, const double v_current, const int closest_id) const override; + + void setParam(const Param & smoother_param); + +private: + Param smoother_param_; + autoware::common::osqp::OSQPInterface qp_solver_; + rclcpp::Logger logger_{rclcpp::get_logger("smoother").get_child("linf_pseudo_jerk_smoother")}; +}; +} // namespace motion_velocity_smoother + +#endif // MOTION_VELOCITY_SMOOTHER__SMOOTHER__LINF_PSEUDO_JERK_SMOOTHER_HPP_ diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/smoother_base.hpp b/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/smoother_base.hpp new file mode 100644 index 0000000000000..c14bdc0c21ad5 --- /dev/null +++ b/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/smoother_base.hpp @@ -0,0 +1,76 @@ +// 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 MOTION_VELOCITY_SMOOTHER__SMOOTHER__SMOOTHER_BASE_HPP_ +#define MOTION_VELOCITY_SMOOTHER__SMOOTHER__SMOOTHER_BASE_HPP_ + +#include "autoware_utils/geometry/geometry.hpp" +#include "autoware_utils/trajectory/trajectory.hpp" +#include "motion_velocity_smoother/resample.hpp" +#include "motion_velocity_smoother/trajectory_utils.hpp" +#include "rclcpp/rclcpp.hpp" + +#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" + +#include "boost/optional.hpp" + +#include +#include + +namespace motion_velocity_smoother +{ +using autoware_auto_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_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 + resampling::ResampleParam resample_param; + }; + + virtual ~SmootherBase() = default; + virtual bool apply( + const double initial_vel, const double initial_acc, const TrajectoryPoints & input, + TrajectoryPoints & output, std::vector & debug_trajectories) = 0; + + virtual boost::optional resampleTrajectory( + const TrajectoryPoints & input, const double v_current, const int closest_id) const = 0; + + virtual boost::optional applyLateralAccelerationFilter( + const TrajectoryPoints & input) const; + + double getMaxAccel() const; + double getMinDecel() const; + double getMaxJerk() const; + double getMinJerk() const; + + void setParam(const BaseParam & param); + +protected: + BaseParam base_param_; +}; +} // namespace motion_velocity_smoother + +#endif // MOTION_VELOCITY_SMOOTHER__SMOOTHER__SMOOTHER_BASE_HPP_ diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/trajectory_utils.hpp b/planning/motion_velocity_smoother/include/motion_velocity_smoother/trajectory_utils.hpp new file mode 100644 index 0000000000000..aea9fed6641a9 --- /dev/null +++ b/planning/motion_velocity_smoother/include/motion_velocity_smoother/trajectory_utils.hpp @@ -0,0 +1,91 @@ +// 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 MOTION_VELOCITY_SMOOTHER__TRAJECTORY_UTILS_HPP_ +#define MOTION_VELOCITY_SMOOTHER__TRAJECTORY_UTILS_HPP_ + +#include "autoware_utils/geometry/geometry.hpp" +#include "autoware_utils/trajectory/trajectory.hpp" + +#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" +#include "geometry_msgs/msg/pose.hpp" + +#include "boost/optional.hpp" + +#include +#include +#include +#include +#include + +namespace motion_velocity_smoother +{ +namespace trajectory_utils +{ +using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using TrajectoryPoints = std::vector; +using geometry_msgs::msg::Pose; + +TrajectoryPoint calcInterpolatedTrajectoryPoint( + const TrajectoryPoints & trajectory, const Pose & target_pose); + +boost::optional extractPathAroundIndex( + const TrajectoryPoints & trajectory, const size_t index, const double & ahead_length, + const double & behind_length); + +double calcArcLength(const TrajectoryPoints & trajectory, const int idx1, const int idx2); + +std::vector calcArclengthArray(const TrajectoryPoints & trajectory); + +std::vector calcTrajectoryIntervalDistance(const TrajectoryPoints & trajectory); + +boost::optional> calcTrajectoryCurvatureFrom3Points( + const TrajectoryPoints & trajectory, const size_t & idx_dist); + +void setZeroVelocity(TrajectoryPoints & trajectory); + +double getMaxVelocity(const TrajectoryPoints & trajectory); + +double getMaxAbsVelocity(const TrajectoryPoints & trajectory); + +void applyMaximumVelocityLimit( + const size_t from, const size_t to, const double max_vel, TrajectoryPoints & trajectory); + +boost::optional searchZeroVelocityIdx(const TrajectoryPoints & trajectory); + +boost::optional applyLinearInterpolation( + const std::vector & base_index, const TrajectoryPoints & base_trajectory, + const std::vector & out_index, const bool use_spline_for_pose = false); + +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); + +boost::optional applyDecelFilterWithJerkConstraint( + const TrajectoryPoints & input, const size_t start_index, const double v0, const double a0, + const double min_acc, const double decel_target_vel, + const std::map & jerk_profile); + +boost::optional> updateStateWithJerkConstraint( + const double v0, const double a0, const std::map & jerk_profile, const double t); + +} // namespace trajectory_utils +} // namespace motion_velocity_smoother + +#endif // MOTION_VELOCITY_SMOOTHER__TRAJECTORY_UTILS_HPP_ diff --git a/planning/motion_velocity_smoother/launch/motion_velocity_smoother.launch.xml b/planning/motion_velocity_smoother/launch/motion_velocity_smoother.launch.xml new file mode 100644 index 0000000000000..9c5768fccfad3 --- /dev/null +++ b/planning/motion_velocity_smoother/launch/motion_velocity_smoother.launch.xml @@ -0,0 +1,28 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/planning/motion_velocity_smoother/media/motion_velocity_smoother_flow.drawio.svg b/planning/motion_velocity_smoother/media/motion_velocity_smoother_flow.drawio.svg new file mode 100644 index 0000000000000..4d8ffd9eb8ae2 --- /dev/null +++ b/planning/motion_velocity_smoother/media/motion_velocity_smoother_flow.drawio.svg @@ -0,0 +1,3 @@ + + +
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
PostprocessReference trajectoryOutput trajectory
onCurrentTrajectory
onCurrentTrajectory
Calculate travel distance
Calculate travel distance
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
Resample trajectory
Resample trajectory
trajectory
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
velocity
velocity
smoothed velocity
smoothed velocity
velocity limit
velocity limit
distance
distanceViewer does not support full SVG 1.1 \ No newline at end of file diff --git a/planning/motion_velocity_smoother/package.xml b/planning/motion_velocity_smoother/package.xml new file mode 100644 index 0000000000000..43e958c205a37 --- /dev/null +++ b/planning/motion_velocity_smoother/package.xml @@ -0,0 +1,34 @@ + + + motion_velocity_smoother + 0.1.0 + The motion_velocity_smoother package + + Takamasa Horibe + Fumiya Watanabe + Yutaka Shimizu + + Apache License 2.0 + + ament_cmake_auto + eigen3_cmake_module + + autoware_auto_planning_msgs + autoware_debug_msgs + autoware_planning_msgs + autoware_utils + geometry_msgs + interpolation + libboost-dev + nav_msgs + osqp_interface + tf2 + tf2_ros + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/planning/motion_velocity_smoother/scripts/closest_velocity_checker.py b/planning/motion_velocity_smoother/scripts/closest_velocity_checker.py new file mode 100755 index 0000000000000..fa904db0b22f9 --- /dev/null +++ b/planning/motion_velocity_smoother/scripts/closest_velocity_checker.py @@ -0,0 +1,332 @@ +#!/usr/bin/env python3 + +# Copyright 2020 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. + +import time + +from autoware_auto_control_msgs.msg import AckermannControlCommand +from autoware_auto_planning_msgs.msg import Path +from autoware_auto_planning_msgs.msg import PathWithLaneId +from autoware_auto_planning_msgs.msg import Trajectory +from autoware_auto_vehicle_msgs.msg import Engage +from autoware_auto_vehicle_msgs.msg import VelocityReport +from autoware_debug_msgs.msg import Float32MultiArrayStamped +from autoware_planning_msgs.msg import VelocityLimit +from geometry_msgs.msg import Pose +from nav_msgs.msg import Odometry +import numpy as np +import rclpy +from rclpy.node import Node +from tf2_ros import LookupException +from tf2_ros.buffer import Buffer +from tf2_ros.transform_listener import TransformListener + +REF_LINK = "map" +SELF_LINK = "base_link" + +LANE_CHANGE = 0 +BEHAVIOR_VELOCITY = 1 +OBSTACLE_AVOID = 2 +OBSTACLE_STOP = 3 +LAT_ACC = 4 +VELOCITY_OPTIMIZE = 5 +CONTROL_CMD = 6 +VEHICLE_CMD = 7 +CONTROL_CMD_ACC = 8 +VEHICLE_CMD_ACC = 9 +DATA_NUM = 10 + + +class VelocityChecker(Node): + def __init__(self): + super().__init__("velocity_checker") + + self.autoware_engage = None + self.vehicle_engage = None + self.external_v_lim = np.nan + self.localization_twist_vx = np.nan + self.vehicle_twist_vx = np.nan + self.self_pose = Pose() + self.data_arr = [np.nan] * DATA_NUM + self.count = 0 + + self.tf_buffer = Buffer() + self.tf_listener = TransformListener(self.tf_buffer, self) + + # planning path and trajectories + profile = rclpy.qos.QoSProfile(depth=1) + lane_drv = "/planning/scenario_planning/lane_driving" + scenario = "/planning/scenario_planning" + self.sub0 = self.create_subscription( + PathWithLaneId, + lane_drv + "/behavior_planning/path_with_lane_id", + self.CallBackBehaviorPathWLid, + 1, + ) + self.sub1 = self.create_subscription( + Path, lane_drv + "/behavior_planning/path", self.CallBackBehaviorPath, 1 + ) + self.sub2 = self.create_subscription( + Trajectory, + lane_drv + "/motion_planning/obstacle_avoidance_planner/trajectory", + self.CallBackAvoidTrajectory, + 1, + ) + self.sub3 = self.create_subscription( + Trajectory, lane_drv + "/trajectory", self.CallBackLaneDriveTrajectory, 1 + ) + self.sub4 = self.create_subscription( + Trajectory, + scenario + "/motion_velocity_smoother/debug/trajectory_lateral_acc_filtered", + # TODO: change to following string after fixing bug of autoware + # '/motion_velocity_optimizer/debug/trajectory_lateral_acc_filtered', + self.CallBackLataccTrajectory, + 1, + ) + self.sub5 = self.create_subscription( + Trajectory, scenario + "/trajectory", self.CallBackScenarioTrajectory, 1 + ) + + # control commands + self.sub6 = self.create_subscription( + AckermannControlCommand, + "/control/trajectory_follower/control_cmd", + self.CallBackControlCmd, + 1, + ) + self.sub7 = self.create_subscription( + AckermannControlCommand, "/control/command/control_cmd", self.CallBackVehicleCmd, 1 + ) + + # others related to velocity + self.sub8 = self.create_subscription( + Engage, "/autoware/engage", self.CallBackAwEngage, profile + ) + self.sub12 = self.create_subscription( + Engage, "/vehicle/engage", self.CallBackVehicleEngage, profile + ) + self.sub9 = self.create_subscription( + VelocityLimit, + "/planning/scenario_planning/current_max_velocity", + self.CallBackExternalVelLim, + profile, + ) + + # self twist + self.sub10 = self.create_subscription( + Odometry, "/localization/kinematic_state", self.CallBackLocalizationTwist, 1 + ) + self.sub11 = self.create_subscription( + VelocityReport, "/vehicle/status/velocity_status", self.CallBackVehicleTwist, 1 + ) + + # publish data + self.pub_v_arr = self.create_publisher(Float32MultiArrayStamped, "closest_speeds", 1) + + time.sleep(1.0) # wait for ready to publish/subscribe + + # for publish traffic signal image + self.create_timer(0.1, self.timerCallback) + + def printInfo(self): + self.count = self.count % 30 + if self.count == 0: + self.get_logger().info("") + self.get_logger().info( + "| Map Limit | Behavior | Obs Avoid | Obs Stop | External Lim | LatAcc Filtered " + "| Optimized | Control VelCmd | Control AccCmd | Vehicle VelCmd | Vehicle AccCmd " + "| AW Engage | VC Engage | Localization Vel | Vehicle Vel | [km/h]" + ) # noqa: E501 + mps2kmph = 3.6 + vel_map_lim = self.data_arr[LANE_CHANGE] * mps2kmph + vel_behavior = self.data_arr[BEHAVIOR_VELOCITY] * mps2kmph + vel_obs_avoid = self.data_arr[OBSTACLE_AVOID] * mps2kmph + vel_obs_stop = self.data_arr[OBSTACLE_STOP] * mps2kmph + vel_external_lim = self.external_v_lim * mps2kmph + vel_latacc_filtered = self.data_arr[LAT_ACC] * mps2kmph + vel_optimized = self.data_arr[VELOCITY_OPTIMIZE] * mps2kmph + vel_ctrl_cmd = self.data_arr[CONTROL_CMD] * mps2kmph + acc_ctrl_cmd = self.data_arr[CONTROL_CMD_ACC] + vel_vehicle_cmd = self.data_arr[VEHICLE_CMD] * mps2kmph + acc_vehicle_cmd = self.data_arr[VEHICLE_CMD_ACC] + vel_localization = self.localization_twist_vx * mps2kmph + vel_vehicle = self.vehicle_twist_vx * mps2kmph + engage = ( + "None" + if self.autoware_engage is None + else ("True" if self.autoware_engage is True else "False") + ) + vehicle_engage = ( + "None" + if self.vehicle_engage is None + else ("True" if self.vehicle_engage is True else "False") + ) + self.get_logger().info( + "| {0: 9.2f} | {1: 8.2f} | {2: 9.2f} | {3: 8.2f} | {4: 12.2f} " + "| {5: 15.2f} | {6: 9.2f} | {7: 14.2f} | {8: 14.2f} | {9: 14.2f} | {10: 14.2f} " + "| {11:>9s} | {12:>9s} | {13: 16.2f} | {14: 11.2f} |".format( # noqa: E501 + vel_map_lim, + vel_behavior, + vel_obs_avoid, + vel_obs_stop, + vel_external_lim, + vel_latacc_filtered, + vel_optimized, + vel_ctrl_cmd, + acc_ctrl_cmd, + vel_vehicle_cmd, + acc_vehicle_cmd, + engage, + vehicle_engage, + vel_localization, + vel_vehicle, + ) + ) + self.count += 1 + + def timerCallback(self): + # self.get_logger().info('timer called') + self.updatePose(REF_LINK, SELF_LINK) + self.pub_v_arr.publish(Float32MultiArrayStamped(data=self.data_arr)) + self.printInfo() + + def CallBackAwEngage(self, msg): + self.autoware_engage = msg.engage + + def CallBackVehicleEngage(self, msg): + self.vehicle_engage = msg.engage + + def CallBackExternalVelLim(self, msg): + self.external_v_lim = msg.max_velocity + + def CallBackLocalizationTwist(self, msg): + self.localization_twist_vx = msg.twist.twist.linear.x + + def CallBackVehicleTwist(self, msg): + self.vehicle_twist_vx = msg.longitudinal_velocity + + def CallBackBehaviorPathWLid(self, msg): + # self.get_logger().info('LANE_CHANGE called') + closest = self.calcClosestPathWLid(msg) + self.data_arr[LANE_CHANGE] = msg.points[closest].point.longitudinal_velocity_mps + return + + def CallBackBehaviorPath(self, msg): + # self.get_logger().info('BEHAVIOR_VELOCITY called') + closest = self.calcClosestPath(msg) + self.data_arr[BEHAVIOR_VELOCITY] = msg.points[closest].longitudinal_velocity_mps + return + + def CallBackAvoidTrajectory(self, msg): + # self.get_logger().info('OBSTACLE_AVOID called') + closest = self.calcClosestTrajectory(msg) + self.data_arr[OBSTACLE_AVOID] = msg.points[closest].longitudinal_velocity_mps + return + + def CallBackLaneDriveTrajectory(self, msg): + # self.get_logger().info('OBSTACLE_STOP called') + closest = self.calcClosestTrajectory(msg) + self.data_arr[OBSTACLE_STOP] = msg.points[closest].longitudinal_velocity_mps + return + + def CallBackLataccTrajectory(self, msg): + # self.get_logger().info('LAT_ACC called') + closest = self.calcClosestTrajectory(msg) + self.data_arr[LAT_ACC] = msg.points[closest].longitudinal_velocity_mps + return + + def CallBackScenarioTrajectory(self, msg): + # self.get_logger().info('VELOCITY_OPTIMIZE called') + closest = self.calcClosestTrajectory(msg) + self.data_arr[VELOCITY_OPTIMIZE] = msg.points[closest].longitudinal_velocity_mps + return + + def CallBackControlCmd(self, msg): + # self.get_logger().info('CONTROL_CMD called') + self.data_arr[CONTROL_CMD] = msg.longitudinal.speed + self.data_arr[CONTROL_CMD_ACC] = msg.longitudinal.acceleration + return + + def CallBackVehicleCmd(self, msg): + # self.get_logger().info('VEHICLE_CMD called') + self.data_arr[VEHICLE_CMD] = msg.longitudinal.speed + self.data_arr[VEHICLE_CMD_ACC] = msg.longitudinal.acceleration + return + + def calcClosestPath(self, path): + closest = -1 + min_dist_squared = 1.0e10 + for i in range(0, len(path.points)): + dist_sq = self.calcSquaredDist2d(self.self_pose, path.points[i].pose) + if dist_sq < min_dist_squared: + min_dist_squared = dist_sq + closest = i + return closest + + def calcClosestPathWLid(self, path): + closest = -1 + min_dist_squared = 1.0e10 + for i in range(0, len(path.points)): + dist_sq = self.calcSquaredDist2d(self.self_pose, path.points[i].point.pose) + if dist_sq < min_dist_squared: + min_dist_squared = dist_sq + closest = i + return closest + + def calcClosestTrajectory(self, path): + closest = -1 + min_dist_squared = 1.0e10 + for i in range(0, len(path.points)): + dist_sq = self.calcSquaredDist2d(self.self_pose, path.points[i].pose) + if dist_sq < min_dist_squared: + min_dist_squared = dist_sq + closest = i + return closest + + def calcSquaredDist2d(self, p1, p2): + dx = p1.position.x - p2.position.x + dy = p1.position.y - p2.position.y + return dx * dx + dy * dy + + def updatePose(self, from_link, to_link): + try: + tf = self.tf_buffer.lookup_transform(from_link, to_link, rclpy.time.Time()) + self.self_pose.position.x = tf.transform.translation.x + self.self_pose.position.y = tf.transform.translation.y + self.self_pose.position.z = tf.transform.translation.z + self.self_pose.orientation.x = tf.transform.rotation.x + self.self_pose.orientation.y = tf.transform.rotation.y + self.self_pose.orientation.z = tf.transform.rotation.z + self.self_pose.orientation.w = tf.transform.rotation.w + return + except LookupException as e: + self.get_logger().warn("No required transformation found: `{}`".format(str(e))) + return + + +def main(args=None): + try: + rclpy.init(args=args) + node = VelocityChecker() + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/planning/motion_velocity_smoother/scripts/trajectory_visualizer.py b/planning/motion_velocity_smoother/scripts/trajectory_visualizer.py new file mode 100755 index 0000000000000..cdddaf483c196 --- /dev/null +++ b/planning/motion_velocity_smoother/scripts/trajectory_visualizer.py @@ -0,0 +1,656 @@ +#!/usr/bin/env python3 + +# Copyright 2020 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. + +import argparse + +from autoware_auto_planning_msgs.msg import Path +from autoware_auto_planning_msgs.msg import PathWithLaneId +from autoware_auto_planning_msgs.msg import Trajectory +from autoware_auto_vehicle_msgs.msg import VelocityReport +from geometry_msgs.msg import Pose +from matplotlib import animation +import matplotlib.pyplot as plt +import message_filters +from nav_msgs.msg import Odometry +import numpy as np +import rclpy +from rclpy.node import Node +import tf2_ros +from tf2_ros.buffer import Buffer +from tf2_ros.transform_listener import TransformListener + +parser = argparse.ArgumentParser() +parser.add_argument("-l", "--length", help="max arclength in plot") +parser.add_argument( + "-t", + "--type", + help="Options VEL(default): show velocity only, VEL_ACC_JERK: show vel & acc & jerk", +) + +args = parser.parse_args() + +if args.length is None: + PLOT_MAX_ARCLENGTH = 200 +else: + PLOT_MAX_ARCLENGTH = int(args.length) +print("max arclength = " + str(PLOT_MAX_ARCLENGTH)) + +if args.type is None: + PLOT_TYPE = "VEL" +elif args.type == "VEL": + PLOT_TYPE = "VEL" +elif args.type == "VEL_ACC_JERK": + PLOT_TYPE = "VEL_ACC_JERK" +else: + print("invalid type. set default VEL.") + PLOT_TYPE = "VEL" +print("plot type = " + PLOT_TYPE) + +PATH_ORIGIN_FRAME = "map" +SELF_POSE_FRAME = "base_link" + + +class TrajectoryVisualizer(Node): + def __init__(self): + + super().__init__("trajectory_visualizer") + + self.fig = plt.figure() + + self.max_vel = 0.0 + self.min_vel = 0.0 + self.min_accel = 0.0 + self.max_accel = 0.0 + self.min_jerk = 0.0 + self.max_jerk = 0.0 + + # update flag + self.update_ex_vel_lim = False + self.update_lat_acc_fil = False + self.update_traj_raw = False + self.update_traj_resample = False + self.update_traj_final = False + self.update_lanechange_path = False + self.update_behavior_path = False + self.update_traj_ob_avoid = False + self.update_traj_ob_stop = False + + self.tf_buffer = Buffer(node=self) + self.tf_listener = TransformListener(self.tf_buffer, self, spin_thread=True) + + self.self_pose = Pose() + self.self_pose_received = False + self.localization_vx = 0.0 + self.vehicle_vx = 0.0 + + self.trajectory_external_velocity_limited = Trajectory() + self.trajectory_lateral_acc_filtered = Trajectory() + self.trajectory_raw = Trajectory() + self.trajectory_time_resampled = Trajectory() + self.trajectory_final = Trajectory() + + self.lane_change_path = PathWithLaneId() + self.behavior_path = Path() + self.obstacle_avoid_traj = Trajectory() + self.obstacle_stop_traj = Trajectory() + + self.plotted = [False] * 9 + self.sub_localization_twist = self.create_subscription( + Odometry, "/localization/kinematic_state", self.CallbackLocalizationTwist, 1 + ) + self.sub_vehicle_twist = self.create_subscription( + VelocityReport, "/vehicle/status/velocity_status", self.CallbackVehicleTwist, 1 + ) + + # BUFFER_SIZE = 65536*100 + optimizer_debug = "/planning/scenario_planning/motion_velocity_smoother/debug/" + self.sub1 = message_filters.Subscriber( + self, Trajectory, optimizer_debug + "trajectory_external_velocity_limited" + ) + self.sub2 = message_filters.Subscriber( + self, Trajectory, optimizer_debug + "trajectory_lateral_acc_filtered" + ) + self.sub3 = message_filters.Subscriber(self, Trajectory, optimizer_debug + "trajectory_raw") + self.sub4 = message_filters.Subscriber( + self, Trajectory, optimizer_debug + "trajectory_time_resampled" + ) + self.sub5 = message_filters.Subscriber( + self, Trajectory, "/planning/scenario_planning/trajectory" + ) + + lane_driving = "/planning/scenario_planning/lane_driving" + self.sub6 = message_filters.Subscriber( + self, PathWithLaneId, lane_driving + "/behavior_planning/path_with_lane_id" + ) + self.sub7 = message_filters.Subscriber(self, Path, lane_driving + "/behavior_planning/path") + self.sub8 = message_filters.Subscriber( + self, + Trajectory, + lane_driving + "/motion_planning/obstacle_avoidance_planner/trajectory", + ) + self.sub9 = message_filters.Subscriber(self, Trajectory, lane_driving + "/trajectory") + + self.ts1 = message_filters.ApproximateTimeSynchronizer( + [self.sub1, self.sub2, self.sub3, self.sub4, self.sub5], 30, 0.5 + ) + self.ts1.registerCallback(self.CallbackMotionVelOptTraj) + + self.ts2 = message_filters.ApproximateTimeSynchronizer( + [self.sub6, self.sub7, self.sub8, self.sub9], 30, 1, 0 + ) + self.ts2.registerCallback(self.CallBackLaneDrivingTraj) + + # main process + if PLOT_TYPE == "VEL_ACC_JERK": + self.ani = animation.FuncAnimation( + self.fig, self.plotTrajectory, interval=100, blit=True + ) + self.setPlotTrajectory() + else: + self.ani = animation.FuncAnimation( + self.fig, self.plotTrajectoryVelocity, interval=100, blit=True + ) + self.setPlotTrajectoryVelocity() + + plt.show() + + return + + def test(self): + self.updatePose("map", "base_link") + + def CallbackLocalizationTwist(self, cmd): + self.localization_vx = cmd.twist.twist.linear.x + + def CallbackVehicleTwist(self, cmd): + self.vehicle_vx = cmd.longitudinal_velocity + + def CallbackMotionVelOptTraj(self, cmd1, cmd2, cmd3, cmd4, cmd5): + print("CallbackMotionVelOptTraj called") + self.CallBackTrajExVelLim(cmd1) + self.CallBackTrajLatAccFiltered(cmd2) + self.CallBackTrajRaw(cmd3) + self.CallBackTrajTimeResampled(cmd4) + self.CallBackTrajFinal(cmd5) + + def CallBackTrajExVelLim(self, cmd): + self.trajectory_external_velocity_limited = cmd + self.update_ex_vel_lim = True + + def CallBackTrajLatAccFiltered(self, cmd): + self.trajectory_lateral_acc_filtered = cmd + self.update_lat_acc_fil = True + + def CallBackTrajRaw(self, cmd): + self.trajectory_raw = cmd + self.update_traj_raw = True + + def CallBackTrajTimeResampled(self, cmd): + self.trajectory_time_resampled = cmd + self.update_traj_resample = True + + def CallBackTrajFinal(self, cmd): + self.trajectory_final = cmd + self.update_traj_final = True + + def CallBackLaneDrivingTraj(self, cmd6, cmd7, cmd8, cmd9): + print("CallBackLaneDrivingTraj called") + self.CallBackLaneChangePath(cmd6) + self.CallBackBehaviorPath(cmd7) + self.CallbackObstacleAvoidTraj(cmd8) + self.CallbackObstacleStopTraj(cmd9) + + def CallBackLaneChangePath(self, cmd): + self.lane_change_path = cmd + self.update_lanechange_path = True + + def CallBackBehaviorPath(self, cmd): + self.behavior_path = cmd + self.update_behavior_path = True + + def CallbackObstacleAvoidTraj(self, cmd): + self.obstacle_avoid_traj = cmd + self.update_traj_ob_avoid = True + + def CallbackObstacleStopTraj(self, cmd): + self.obstacle_stop_traj = cmd + self.update_traj_ob_stop = True + + def setPlotTrajectoryVelocity(self): + self.ax1 = plt.subplot(1, 1, 1) # row, col, index(= 0: + x_closest = x[closest] + self.im10.set_data(x_closest, self.localization_vx) + self.im11.set_data(x_closest, self.vehicle_vx) + + # change y-range + self.ax1.set_ylim([self.min_vel - 1.0, self.max_vel + 1.0]) + + return ( + self.im1, + self.im2, + self.im3, + self.im4, + self.im5, + self.im6, + self.im7, + self.im8, + self.im9, + self.im10, + self.im11, + ) + + def CalcArcLength(self, traj): + s_arr = [] + ds = 0.0 + s_sum = 0.0 + + if len(traj.points) > 0: + s_arr.append(s_sum) + + for i in range(1, len(traj.points)): + p0 = traj.points[i - 1] + p1 = traj.points[i] + dx = p1.pose.position.x - p0.pose.position.x + dy = p1.pose.position.y - p0.pose.position.y + ds = np.sqrt(dx ** 2 + dy ** 2) + s_sum += ds + s_arr.append(s_sum) + return s_arr + + def CalcArcLengthPathWLid(self, traj): + s_arr = [] + ds = 0.0 + s_sum = 0.0 + + if len(traj.points) > 0: + s_arr.append(s_sum) + + for i in range(1, len(traj.points)): + p0 = traj.points[i - 1].point + p1 = traj.points[i].point + dx = p1.pose.position.x - p0.pose.position.x + dy = p1.pose.position.y - p0.pose.position.y + ds = np.sqrt(dx ** 2 + dy ** 2) + s_sum += ds + s_arr.append(s_sum) + return s_arr + + def CalcArcLengthPath(self, traj): + s_arr = [] + ds = 0.0 + s_sum = 0.0 + + if len(traj.points) > 0: + s_arr.append(s_sum) + + for i in range(1, len(traj.points)): + p0 = traj.points[i - 1] + p1 = traj.points[i] + dx = p1.pose.position.x - p0.pose.position.x + dy = p1.pose.position.y - p0.pose.position.y + ds = np.sqrt(dx ** 2 + dy ** 2) + s_sum += ds + s_arr.append(s_sum) + return s_arr + + def ToVelList(self, traj): + v_list = [] + for p in traj.points: + v_list.append(p.longitudinal_velocity_mps) + return v_list + + def ToVelListPathWLid(self, traj): + v_list = [] + for p in traj.points: + v_list.append(p.point.longitudinal_velocity_mps) + return v_list + + def ToVelListPath(self, traj): + v_list = [] + for p in traj.points: + v_list.append(p.longitudinal_velocity_mps) + return v_list + + def CalcAcceleration(self, traj): + a_arr = [] + for i in range(1, len(traj.points) - 1): + p0 = traj.points[i - 1] + p1 = traj.points[i] + v0 = p0.longitudinal_velocity_mps + v1 = p1.longitudinal_velocity_mps + v = 0.5 * (v1 + v0) + dx = p1.pose.position.x - p0.pose.position.x + dy = p1.pose.position.y - p0.pose.position.y + ds = np.sqrt(dx ** 2 + dy ** 2) + dt = ds / max(abs(v), 0.001) + a = (v1 - v0) / dt + a_arr.append(a) + if len(traj.points) > 0: + a_arr.append(0) + a_arr.append(0) + return a_arr + + def CalcJerk(self, traj): + j_arr = [] + for i in range(1, len(traj.points) - 2): + p0 = traj.points[i - 1] + p1 = traj.points[i] + p2 = traj.points[i + 1] + v0 = p0.longitudinal_velocity_mps + v1 = p1.longitudinal_velocity_mps + v2 = p2.longitudinal_velocity_mps + + dx0 = p1.pose.position.x - p0.pose.position.x + dy0 = p1.pose.position.y - p0.pose.position.y + ds0 = np.sqrt(dx0 ** 2 + dy0 ** 2) + + dx1 = p2.pose.position.x - p1.pose.position.x + dy1 = p2.pose.position.y - p1.pose.position.y + ds1 = np.sqrt(dx1 ** 2 + dy1 ** 2) + + dt0 = ds0 / max(abs(0.5 * (v1 + v0)), 0.001) + dt1 = ds1 / max(abs(0.5 * (v2 + v1)), 0.001) + + a0 = (v1 - v0) / max(dt0, 0.001) + a1 = (v2 - v1) / max(dt1, 0.001) + j = (a1 - a0) / max(dt1, 0.001) + j_arr.append(j) + if len(traj.points) > 0: + j_arr.append(0) + j_arr.append(0) + j_arr.append(0) + return j_arr + + def setPlotTrajectory(self): + self.ax1 = plt.subplot(3, 1, 1) # row, col, index( + +namespace motion_velocity_smoother +{ +/* + * linear interpolation + */ +namespace linear_interpolation +{ +boost::optional> interpolate( + const std::vector & sample_x, const std::vector & sample_value, + const std::vector & query_x) +{ + std::vector query_value; + auto isIncrease = [](const std::vector & x) { + if (x.empty()) { + return false; + } + for (size_t i = 0; i < x.size() - 1; ++i) { + if (x.at(i) > x.at(i + 1)) { + return false; + } + } + return true; + }; + + if (sample_x.empty() || sample_value.empty() || query_x.empty()) { + printf( + "[interpolate] some vector size is zero: sample_x.size() = %lu, sample_value.size() = %lu, " + "query_x.size() = %lu\n", + sample_x.size(), sample_value.size(), query_x.size()); + return {}; + } + + // check if inputs are valid + if ( + !isIncrease(sample_x) || !isIncrease(query_x) || query_x.front() < sample_x.front() || + sample_x.back() < query_x.back() || sample_x.size() != sample_value.size()) { + std::cerr << "[isIncrease] bad index, return false" << std::endl; + const bool b1 = !isIncrease(sample_x); + const bool b2 = !isIncrease(query_x); + const bool b3 = query_x.front() < sample_x.front(); + const bool b4 = sample_x.back() < query_x.back(); + const bool b5 = sample_x.size() != sample_value.size(); + printf("%d, %d, %d, %d, %d\n", b1, b2, b3, b4, b5); + printf("sample_x = [%f, %f]\n", sample_x.front(), sample_x.back()); + printf("query_x = [%f, %f]\n", query_x.front(), query_x.back()); + printf( + "sample_x.size() = %lu, sample_value.size() = %lu\n", sample_x.size(), sample_value.size()); + return {}; + } + + // calculate linear interpolation + int i = 0; + for (const auto idx : query_x) { + if (sample_x.at(i) == idx) { + query_value.push_back(sample_value.at(i)); + continue; + } + while (sample_x.at(i) < idx) { + ++i; + } + if (i <= 0 || static_cast(sample_x.size()) - 1 < i) { + std::cerr << "? something wrong. skip this idx." << std::endl; + continue; + } + + const double dist_base_idx = sample_x.at(i) - sample_x.at(i - 1); + const double dist_to_forward = sample_x.at(i) - idx; + const double dist_to_backward = idx - sample_x.at(i - 1); + if (dist_to_forward < 0.0 || dist_to_backward < 0.0) { + std::cerr << "?? something wrong. skip this idx. sample_x.at(i - 1) = " << sample_x.at(i - 1) + << ", idx = " << idx << ", sample_x.at(i) = " << sample_x.at(i) << std::endl; + continue; + } + + const double value = + (dist_to_backward * sample_value.at(i) + dist_to_forward * sample_value.at(i - 1)) / + dist_base_idx; + query_value.push_back(value); + } + return query_value; +} +} // namespace linear_interpolation +} // namespace motion_velocity_smoother diff --git a/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp b/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp new file mode 100644 index 0000000000000..c51a6ec640f9a --- /dev/null +++ b/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp @@ -0,0 +1,1012 @@ +// 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 "motion_velocity_smoother/motion_velocity_smoother_node.hpp" + +#include "autoware_utils/ros/update_param.hpp" +#include "motion_velocity_smoother/smoother/jerk_filtered_smoother.hpp" +#include "motion_velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp" +#include "motion_velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include + +// clang-format on +namespace motion_velocity_smoother +{ +MotionVelocitySmootherNode::MotionVelocitySmootherNode(const rclcpp::NodeOptions & node_options) +: Node("motion_velocity_smoother", node_options) +{ + using std::placeholders::_1; + + // set common params + initCommonParam(); + over_stop_velocity_warn_thr_ = + declare_parameter("over_stop_velocity_warn_thr", autoware_utils::kmph2mps(5.0)); + + // create smoother + initSmootherBaseParam(); + switch (node_param_.algorithm_type) { + case AlgorithmType::JERK_FILTERED: { + initJerkFilteredSmootherParam(); + smoother_ = std::make_shared(jerk_filtered_smoother_param_); + smoother_->setParam(base_param_); + + // 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; + } + case AlgorithmType::L2: { + initL2PseudoJerkSmootherParam(); + smoother_ = std::make_shared(l2_pseudo_jerk_smoother_param_); + smoother_->setParam(base_param_); + break; + } + case AlgorithmType::LINF: { + initLinfPseudoJerkSmootherParam(); + smoother_ = std::make_shared(linf_pseudo_jerk_smoother_param_); + smoother_->setParam(base_param_); + break; + } + case AlgorithmType::ANALYTICAL: { + initAnalyticalJerkConstrainedSmootherParam(); + smoother_ = std::make_shared( + analytical_jerk_constrained_smoother_param_); + smoother_->setParam(base_param_); + break; + } + default: + throw std::domain_error("[MotionVelocitySmootherNode] invalid algorithm"); + } + + // publishers, subscribers + pub_trajectory_ = create_publisher("~/output/trajectory", 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(&MotionVelocitySmootherNode::onCurrentTrajectory, this, _1)); + sub_current_odometry_ = create_subscription( + "/localization/kinematic_state", 1, + std::bind(&MotionVelocitySmootherNode::onCurrentOdometry, this, _1)); + sub_external_velocity_limit_ = create_subscription( + "~/input/external_velocity_limit_mps", 1, + std::bind(&MotionVelocitySmootherNode::onExternalVelocityLimit, this, _1)); + + // parameter update + set_param_res_ = this->add_on_set_parameters_callback( + std::bind(&MotionVelocitySmootherNode::onParameter, this, _1)); + + // debug + publish_debug_trajs_ = declare_parameter("publish_debug_trajs", false); + 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("~/calculation_time", 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_resampled_ = create_publisher("~/debug/trajectory_time_resampled", 1); + + // Wait for first self pose + self_pose_listener_.waitForFirstPose(); + + external_velocity_limit_ = 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); +} + +rcl_interfaces::msg::SetParametersResult MotionVelocitySmootherNode::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; + }; + { + auto & p = node_param_; + update_param("max_velocity", 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("delta_yaw_threshold", p.delta_yaw_threshold); + } + + { + auto & p = base_param_; + 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); + smoother_->setParam(p); + } + + switch (node_param_.algorithm_type) { + case AlgorithmType::JERK_FILTERED: { + auto & p = jerk_filtered_smoother_param_; + 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; + } + case AlgorithmType::L2: { + auto & p = l2_pseudo_jerk_smoother_param_; + update_param("pseudo_jerk_weight", p.pseudo_jerk_weight); + update_param("over_v_weight", p.over_v_weight); + update_param("over_a_weight", p.over_a_weight); + std::dynamic_pointer_cast(smoother_)->setParam(p); + break; + } + case AlgorithmType::LINF: { + auto & p = linf_pseudo_jerk_smoother_param_; + update_param("pseudo_jerk_weight", p.pseudo_jerk_weight); + update_param("over_v_weight", p.over_v_weight); + update_param("over_a_weight", p.over_a_weight); + std::dynamic_pointer_cast(smoother_)->setParam(p); + break; + } + case AlgorithmType::ANALYTICAL: { + auto & p = analytical_jerk_constrained_smoother_param_; + update_param("resample.ds_resample", p.resample.ds_resample); + update_param("resample.num_resample", p.resample.num_resample); + update_param("resample.delta_yaw_threshold", p.resample.delta_yaw_threshold); + update_param( + "latacc.constant_velocity_dist_threshold", p.latacc.constant_velocity_dist_threshold); + update_param("forward.max_acc", p.forward.max_acc); + update_param("forward.min_acc", p.forward.min_acc); + update_param("forward.max_jerk", p.forward.max_jerk); + update_param("forward.min_jerk", p.forward.min_jerk); + update_param("forward.kp", p.forward.kp); + update_param("backward.start_jerk", p.backward.start_jerk); + update_param("backward.min_jerk_mild_stop", p.backward.min_jerk_mild_stop); + update_param("backward.min_jerk", p.backward.min_jerk); + update_param("backward.min_acc_mild_stop", p.backward.min_acc_mild_stop); + update_param("backward.min_acc", p.backward.min_acc); + update_param("backward.span_jerk", p.backward.span_jerk); + std::dynamic_pointer_cast(smoother_)->setParam(p); + break; + } + default: + throw std::domain_error("[MotionVelocitySmootherNode] invalid algorithm"); + } + + rcl_interfaces::msg::SetParametersResult result{}; + result.successful = true; + result.reason = "success"; + return result; +} + +void MotionVelocitySmootherNode::initCommonParam() +{ + auto & p = node_param_; + p.max_velocity = declare_parameter("max_velocity", 20.0); // 72.0 kmph + p.margin_to_insert_external_velocity_limit = + declare_parameter("margin_to_insert_external_velocity_limit", 0.3); + p.replan_vel_deviation = declare_parameter("replan_vel_deviation", 3.0); + p.engage_velocity = declare_parameter("engage_velocity", 0.3); + p.engage_acceleration = declare_parameter("engage_acceleration", 0.1); + p.engage_exit_ratio = declare_parameter("engage_exit_ratio", 0.5); + p.engage_exit_ratio = std::min(std::max(p.engage_exit_ratio, 0.0), 1.0); + p.stopping_velocity = declare_parameter("stopping_velocity", autoware_utils::kmph2mps(10.0)); + p.stopping_distance = declare_parameter("stopping_distance", 0.0); + p.extract_ahead_dist = declare_parameter("extract_ahead_dist", 200.0); + p.extract_behind_dist = declare_parameter("extract_behind_dist", 3.0); + p.stop_dist_to_prohibit_engage = declare_parameter("stop_dist_to_prohibit_engage", 1.5); + p.delta_yaw_threshold = declare_parameter("delta_yaw_threshold", M_PI / 3.0); + p.post_resample_param.max_trajectory_length = + declare_parameter("post_max_trajectory_length", 300.0); + p.post_resample_param.min_trajectory_length = + declare_parameter("post_min_trajectory_length", 30.0); + p.post_resample_param.resample_time = declare_parameter("post_resample_time", 10.0); + p.post_resample_param.dense_resample_dt = declare_parameter("post_dense_resample_dt", 0.1); + p.post_resample_param.dense_min_interval_distance = + declare_parameter("post_dense_min_interval_distance", 0.1); + p.post_resample_param.sparse_resample_dt = declare_parameter("post_sparse_resample_dt", 0.1); + p.post_resample_param.sparse_min_interval_distance = + declare_parameter("post_sparse_min_interval_distance", 1.0); + p.algorithm_type = getAlgorithmType(declare_parameter("algorithm_type", "JerkFiltered")); +} + +void MotionVelocitySmootherNode::initSmootherBaseParam() +{ + auto & p = base_param_; + p.max_accel = declare_parameter("normal.max_acc", 2.0); // 0.11G + p.min_decel = declare_parameter("normal.min_acc", -3.0); // -0.2G + p.stop_decel = declare_parameter("stop_decel", 0.0); + p.max_jerk = declare_parameter("normal.max_jerk", 0.3); + p.min_jerk = declare_parameter("normal.min_jerk", -0.1); + p.max_lateral_accel = declare_parameter("max_lateral_accel", 0.2); + p.decel_distance_before_curve = declare_parameter("decel_distance_before_curve", 3.5); + p.decel_distance_after_curve = declare_parameter("decel_distance_after_curve", 0.0); + p.min_curve_velocity = declare_parameter("min_curve_velocity", 1.38); + p.resample_param.max_trajectory_length = declare_parameter("max_trajectory_length", 200.0); + p.resample_param.min_trajectory_length = declare_parameter("min_trajectory_length", 30.0); + p.resample_param.resample_time = declare_parameter("resample_time", 10.0); + p.resample_param.dense_resample_dt = declare_parameter("dense_resample_dt", 0.1); + p.resample_param.dense_min_interval_distance = + declare_parameter("dense_min_interval_distance", 0.1); + p.resample_param.sparse_resample_dt = declare_parameter("sparse_resample_dt", 0.5); + p.resample_param.sparse_min_interval_distance = + declare_parameter("sparse_min_interval_distance", 4.0); +} + +void MotionVelocitySmootherNode::initJerkFilteredSmootherParam() +{ + auto & p = jerk_filtered_smoother_param_; + p.jerk_weight = declare_parameter("jerk_weight", 10.0); + p.over_v_weight = declare_parameter("over_v_weight", 100000.0); + p.over_a_weight = declare_parameter("over_a_weight", 5000.0); + p.over_j_weight = declare_parameter("over_j_weight", 2000.0); + p.jerk_filter_ds = declare_parameter("jerk_filter_ds", 0.1); +} + +void MotionVelocitySmootherNode::initL2PseudoJerkSmootherParam() +{ + auto & p = l2_pseudo_jerk_smoother_param_; + p.pseudo_jerk_weight = declare_parameter("pseudo_jerk_weight", 100.0); + p.over_v_weight = declare_parameter("over_v_weight", 100000.0); + p.over_a_weight = declare_parameter("over_a_weight", 1000.0); +} + +void MotionVelocitySmootherNode::initLinfPseudoJerkSmootherParam() +{ + auto & p = linf_pseudo_jerk_smoother_param_; + p.pseudo_jerk_weight = declare_parameter("pseudo_jerk_weight", 200.0); + p.over_v_weight = declare_parameter("over_v_weight", 100000.0); + p.over_a_weight = declare_parameter("over_a_weight", 5000.0); +} + +void MotionVelocitySmootherNode::initAnalyticalJerkConstrainedSmootherParam() +{ + auto & p = analytical_jerk_constrained_smoother_param_; + p.resample.ds_resample = declare_parameter("resample.ds_resample", 0.1); + p.resample.num_resample = declare_parameter("resample.num_resample", 1); + p.resample.delta_yaw_threshold = declare_parameter("resample.delta_yaw_threshold", 0.785); + + p.latacc.enable_constant_velocity_while_turning = + declare_parameter("latacc.enable_constant_velocity_while_turning", false); + p.latacc.constant_velocity_dist_threshold = + declare_parameter("latacc.constant_velocity_dist_threshold", 2.0); + + p.forward.max_acc = declare_parameter("forward.max_acc", 1.0); + p.forward.min_acc = declare_parameter("forward.min_acc", -1.0); + p.forward.max_jerk = declare_parameter("forward.max_jerk", 0.3); + p.forward.min_jerk = declare_parameter("forward.min_jerk", -0.3); + p.forward.kp = declare_parameter("forward.kp", 0.3); + + p.backward.start_jerk = declare_parameter("backward.start_jerk", -0.1); + p.backward.min_jerk_mild_stop = declare_parameter("backward.min_jerk_mild_stop", -0.3); + p.backward.min_jerk = declare_parameter("backward.min_jerk", -1.5); + p.backward.min_acc_mild_stop = declare_parameter("backward.min_acc_mild_stop", -1.0); + p.backward.min_acc = declare_parameter("backward.min_acc", -2.5); + p.backward.span_jerk = declare_parameter("backward.span_jerk", -0.01); +} + +void MotionVelocitySmootherNode::publishTrajectory(const TrajectoryPoints & trajectory) const +{ + Trajectory publishing_trajectory = autoware_utils::convertToTrajectory(trajectory); + publishing_trajectory.header = base_traj_raw_ptr_->header; + pub_trajectory_->publish(publishing_trajectory); +} + +void MotionVelocitySmootherNode::onCurrentOdometry(const Odometry::ConstSharedPtr msg) +{ + current_odometry_ptr_ = msg; +} + +void MotionVelocitySmootherNode::onExternalVelocityLimit(const VelocityLimit::ConstSharedPtr msg) +{ + constexpr double eps = 1.0E-04; + const double margin = node_param_.margin_to_insert_external_velocity_limit; + + // calculate distance and maximum velocity + // to decelerate to external velocity limit with jerk and acceleration + // constraints + if (!prev_output_.empty()) { + // if external velocity limit decreases + if ((external_velocity_limit_ - msg->max_velocity) > eps) { + if (prev_closest_point_) { + const double v0 = prev_closest_point_->longitudinal_velocity_mps; + const double a0 = prev_closest_point_->acceleration_mps2; + + if (isEngageStatus(v0)) { + max_velocity_with_deceleration_ = external_velocity_limit_; + external_velocity_limit_dist_ = 0.0; + } else { + const double a_min = + msg->use_constraints ? msg->constraints.min_acceleration : smoother_->getMinDecel(); + const double j_max = + msg->use_constraints ? msg->constraints.max_jerk : smoother_->getMaxJerk(); + const double j_min = + msg->use_constraints ? msg->constraints.min_jerk : smoother_->getMinJerk(); + double stop_dist; + std::map jerk_profile; + if (!trajectory_utils::calcStopDistWithJerkConstraints( + v0, a0, j_max, j_min, a_min, msg->max_velocity, jerk_profile, stop_dist)) { + RCLCPP_WARN(get_logger(), "Stop distance calculation is failed!"); + } + external_velocity_limit_dist_ = stop_dist + margin; + // 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 (max_velocity_with_deceleration_ < msg->max_velocity) { + max_velocity_with_deceleration_ = msg->max_velocity; + external_velocity_limit_dist_ = 0.0; + } + } + } + // if external velocity limit increases + } else if ((msg->max_velocity - external_velocity_limit_) > eps) { + max_velocity_with_deceleration_ = msg->max_velocity; + } + } + + external_velocity_limit_ = msg->max_velocity; + pub_velocity_limit_->publish(*msg); +} + +bool MotionVelocitySmootherNode::checkData() const +{ + if (!current_pose_ptr_ || !current_odometry_ptr_ || !base_traj_raw_ptr_) { + RCLCPP_DEBUG( + get_logger(), "wait topics : current_pose = %d, current_vel = %d, base_traj = %d", + (bool)current_pose_ptr_, (bool)current_odometry_ptr_, (bool)base_traj_raw_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 MotionVelocitySmootherNode::onCurrentTrajectory(const Trajectory::ConstSharedPtr msg) +{ + base_traj_raw_ptr_ = msg; + + stop_watch_.tic(); + RCLCPP_DEBUG(get_logger(), "========================= run start ========================="); + + current_pose_ptr_ = self_pose_listener_.getCurrentPose(); + + // guard + if (!checkData()) { + return; + } + + // calculate distance to insert external velocity limit + if (!prev_output_.empty()) { + const double travel_dist = calcTravelDistance(); + external_velocity_limit_dist_ -= travel_dist; + external_velocity_limit_dist_ = std::max(external_velocity_limit_dist_, 0.0); + RCLCPP_DEBUG( + get_logger(), "run: travel_dist = %f, external_velocity_limit_dist_ = %f", travel_dist, + external_velocity_limit_dist_); + } + + // calculate trajectory velocity + TrajectoryPoints output = + calcTrajectoryVelocity(autoware_utils::convertToTrajectoryPointArray(*base_traj_raw_ptr_)); + if (output.empty()) { + RCLCPP_WARN(get_logger(), "Output Point is empty"); + return; + } + + // Get the nearest point + const auto output_closest_idx = autoware_utils::findNearestIndex( + output, current_pose_ptr_->pose, std::numeric_limits::max(), + node_param_.delta_yaw_threshold); + const auto output_closest_point = + trajectory_utils::calcInterpolatedTrajectoryPoint(output, current_pose_ptr_->pose); + if (!output_closest_idx) { + RCLCPP_WARN_THROTTLE( + get_logger(), *get_clock(), 5000, "Cannot find closest waypoint for output trajectory"); + return; + } + + // Resample the optimized trajectory + auto output_resampled = resampling::resampleTrajectory( + output, current_odometry_ptr_->twist.twist.linear.x, *output_closest_idx, + node_param_.post_resample_param); + if (!output_resampled) { + RCLCPP_WARN(get_logger(), "Failed to get the resampled output trajectory"); + return; + } + + // Set 0 at the end of the trajectory + if (!output_resampled->empty()) { + output_resampled->back().longitudinal_velocity_mps = 0.0; + } + + // publish message + publishTrajectory(*output_resampled); + + // publish debug message + publishStopDistance(output, *output_closest_idx); + publishClosestState(output_closest_point); + + prev_output_ = output; + prev_closest_point_ = output_closest_point; + + // Publish Calculation Time + Float32Stamped calculation_time_data{}; + calculation_time_data.stamp = this->now(); + calculation_time_data.data = stop_watch_.toc(); + debug_calculation_time_->publish(calculation_time_data); + RCLCPP_DEBUG(get_logger(), "run: calculation time = %f [ms]", calculation_time_data.data); + RCLCPP_DEBUG(get_logger(), "========================== run() end ==========================\n\n"); +} + +TrajectoryPoints MotionVelocitySmootherNode::calcTrajectoryVelocity( + const TrajectoryPoints & traj_input) const +{ + TrajectoryPoints output{}; // velocity is optimized by qp solver + + // Extract trajectory around self-position with desired forward-backward length + const auto input_closest = autoware_utils::findNearestIndex( + traj_input, current_pose_ptr_->pose, std::numeric_limits::max(), + node_param_.delta_yaw_threshold); + if (!input_closest) { + auto clock{rclcpp::Clock{RCL_ROS_TIME}}; + RCLCPP_WARN_THROTTLE( + get_logger(), clock, 5000, "Cannot find the closest point from input trajectory"); + return prev_output_; + } + + auto traj_extracted = trajectory_utils::extractPathAroundIndex( + traj_input, *input_closest, node_param_.extract_ahead_dist, node_param_.extract_behind_dist); + if (!traj_extracted) { + RCLCPP_WARN(get_logger(), "Fail to extract the path from the input trajectory"); + return prev_output_; + } + + // Smoother can not handle negative velocity, + // so multiple -1 to velocity if any trajectory points have reverse + // velocity + const bool is_reverse = std::any_of( + traj_extracted->begin(), traj_extracted->end(), + [](auto & pt) { return pt.longitudinal_velocity_mps < 0; }); + if (is_reverse) { + for (auto & pt : *traj_extracted) { + pt.longitudinal_velocity_mps *= -1.0; + } + } + + // Debug + if (publish_debug_trajs_) { + pub_trajectory_raw_->publish(toTrajectoryMsg(*traj_extracted, base_traj_raw_ptr_->header)); + } + + // Apply external velocity limit + applyExternalVelocityLimit(*traj_extracted); + + // Change trajectory velocity to zero when current_velocity == 0 & stop_dist is close + const auto traj_extracted_closest = autoware_utils::findNearestIndex( + *traj_extracted, current_pose_ptr_->pose, std::numeric_limits::max(), + node_param_.delta_yaw_threshold); + if (!traj_extracted_closest) { + RCLCPP_WARN(get_logger(), "Cannot find the closest point from extracted trajectory"); + return prev_output_; + } + + // Apply velocity to approach stop point + applyStopApproachingVelocity(*traj_extracted); + + // Debug + if (publish_debug_trajs_) { + pub_trajectory_vel_lim_->publish(toTrajectoryMsg(*traj_extracted, base_traj_raw_ptr_->header)); + } + + // Smoothing velocity + if (!smoothVelocity(*traj_extracted, output)) { + return prev_output_; + } + + // for reverse velocity + if (is_reverse) { + for (auto & pt : output) { + pt.longitudinal_velocity_mps *= -1.0; + } + } + + return output; +} + +bool MotionVelocitySmootherNode::smoothVelocity( + const TrajectoryPoints & input, TrajectoryPoints & traj_smoothed) const +{ + // Lateral acceleration limit + const auto traj_lateral_acc_filtered = smoother_->applyLateralAccelerationFilter(input); + if (!traj_lateral_acc_filtered) { + return false; + } + + // Resample trajectory with ego-velocity based interval distance + const auto traj_pre_resampled_closest = autoware_utils::findNearestIndex( + *traj_lateral_acc_filtered, current_pose_ptr_->pose, std::numeric_limits::max(), + node_param_.delta_yaw_threshold); + auto traj_resampled = smoother_->resampleTrajectory( + *traj_lateral_acc_filtered, current_odometry_ptr_->twist.twist.linear.x, + *traj_pre_resampled_closest); + if (!traj_resampled) { + RCLCPP_WARN(get_logger(), "Fail to do resampling before the optimization"); + return false; + } + + // 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_pose_ptr_->pose, debug_closest_max_velocity_); + + // Calculate initial motion for smoothing + double initial_vel{}; + double initial_acc{}; + InitializeType type{}; + const auto traj_resampled_closest = autoware_utils::findNearestIndex( + *traj_resampled, current_pose_ptr_->pose, std::numeric_limits::max(), + node_param_.delta_yaw_threshold); + if (!traj_resampled_closest) { + RCLCPP_WARN(get_logger(), "Cannot find closest waypoint for resampled trajectory"); + return false; + } + std::tie(initial_vel, initial_acc, type) = + calcInitialMotion(*traj_resampled, *traj_resampled_closest, prev_output_); + + // Clip trajectory from closest point + TrajectoryPoints clipped; + clipped.insert( + clipped.end(), traj_resampled->begin() + *traj_resampled_closest, traj_resampled->end()); + + std::vector debug_trajectories; + if (!smoother_->apply(initial_vel, initial_acc, clipped, traj_smoothed, debug_trajectories)) { + RCLCPP_WARN(get_logger(), "Fail to solve optimization."); + } + + traj_smoothed.insert( + traj_smoothed.begin(), traj_resampled->begin(), + traj_resampled->begin() + *traj_resampled_closest); + + // Set 0 velocity after input-stop-point + overwriteStopPoint(*traj_resampled, traj_smoothed); + + // 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_) { + pub_trajectory_latacc_filtered_->publish( + toTrajectoryMsg(*traj_lateral_acc_filtered, base_traj_raw_ptr_->header)); + pub_trajectory_resampled_->publish( + toTrajectoryMsg(*traj_resampled, base_traj_raw_ptr_->header)); + + if (!debug_trajectories.empty()) { + 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 MotionVelocitySmootherNode::insertBehindVelocity( + const size_t output_closest, const InitializeType type, TrajectoryPoints & output) const +{ + const bool keep_closest_vel_for_behind = + (type == InitializeType::INIT || 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 { + const auto prev_output_point = + trajectory_utils::calcInterpolatedTrajectoryPoint(prev_output_, output.at(i).pose); + + // 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 MotionVelocitySmootherNode::publishStopDistance( + const TrajectoryPoints & trajectory, const size_t closest) const +{ + // stop distance calculation + const double stop_dist_lim{50.0}; + double stop_dist{stop_dist_lim}; + const auto stop_idx{autoware_utils::searchZeroVelocityIndex(trajectory)}; + if (stop_idx) { + stop_dist = trajectory_utils::calcArcLength(trajectory, closest, *stop_idx); + stop_dist = closest > *stop_idx ? stop_dist : -stop_dist; + } else { + stop_dist = closest > 0 ? stop_dist : -stop_dist; + } + Float32Stamped dist_to_stopline{}; + dist_to_stopline.stamp = this->now(); + dist_to_stopline.data = std::max(-stop_dist_lim, std::min(stop_dist_lim, stop_dist)); + pub_dist_to_stopline_->publish(dist_to_stopline); +} + +std::tuple +MotionVelocitySmootherNode::calcInitialMotion( + const TrajectoryPoints & input_traj, const size_t input_closest, + const TrajectoryPoints & prev_traj) const +{ + const double vehicle_speed{std::fabs(current_odometry_ptr_->twist.twist.linear.x)}; + const double target_vel{std::fabs(input_traj.at(input_closest).longitudinal_velocity_mps)}; + + double initial_vel{}; + double initial_acc{}; + InitializeType type{}; + + // first time + if (prev_traj.empty()) { + initial_vel = vehicle_speed; + initial_acc = 0.0; + type = InitializeType::INIT; + return std::make_tuple(initial_vel, initial_acc, type); + } + + const auto prev_output_closest_point = + trajectory_utils::calcInterpolatedTrajectoryPoint(prev_traj, input_traj.at(input_closest).pose); + + // when velocity tracking deviation is large + const double desired_vel{prev_output_closest_point.longitudinal_velocity_mps}; + const double vel_error{vehicle_speed - std::fabs(desired_vel)}; + if (std::fabs(vel_error) > node_param_.replan_vel_deviation) { + type = InitializeType::LARGE_DEVIATION_REPLAN; + initial_vel = vehicle_speed; // use current vehicle speed + initial_acc = prev_output_closest_point.acceleration_mps2; + 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); + return std::make_tuple(initial_vel, initial_acc, type); + } + + // 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 auto idx = autoware_utils::searchZeroVelocityIndex(input_traj); + const double stop_dist = + idx ? autoware_utils::calcDistance2d(input_traj.at(*idx), input_traj.at(input_closest)) + : 0.0; + if (!idx || stop_dist > node_param_.stop_dist_to_prohibit_engage) { + type = InitializeType::ENGAGING; + initial_vel = node_param_.engage_velocity; + initial_acc = node_param_.engage_acceleration; + 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); + return std::make_tuple(initial_vel, initial_acc, type); + } else { + RCLCPP_DEBUG( + get_logger(), "calcInitialMotion : stop point is close (%.3f[m]). no engage.", stop_dist); + } + } else if (target_vel > 0.0) { + auto clock{rclcpp::Clock{RCL_ROS_TIME}}; + 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); + } + } + + // normal update: use closest in prev_output + type = InitializeType::NORMAL; + initial_vel = prev_output_closest_point.longitudinal_velocity_mps; + initial_acc = prev_output_closest_point.acceleration_mps2; + RCLCPP_DEBUG( + get_logger(), + "calcInitialMotion : normal update. v0 = %f, a0 = %f, vehicle_speed = %f, target_vel = %f", + initial_vel, initial_acc, vehicle_speed, target_vel); + return std::make_tuple(initial_vel, initial_acc, type); +} + +void MotionVelocitySmootherNode::overwriteStopPoint( + const TrajectoryPoints & input, TrajectoryPoints & output) const +{ + const auto stop_idx = autoware_utils::searchZeroVelocityIndex(input); + if (!stop_idx) { + return; + } + + // Get Closest Point from Output + const auto nearest_output_point_idx = autoware_utils::findNearestIndex( + output, input.at(*stop_idx).pose, std::numeric_limits::max(), + node_param_.delta_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(*nearest_output_point_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 MotionVelocitySmootherNode::applyExternalVelocityLimit(TrajectoryPoints & traj) const +{ + if (traj.size() < 1) { + return; + } + + trajectory_utils::applyMaximumVelocityLimit( + 0, traj.size(), max_velocity_with_deceleration_, traj); + + const auto closest_idx = autoware_utils::findNearestIndex( + traj, current_pose_ptr_->pose, std::numeric_limits::max(), + node_param_.delta_yaw_threshold); + if (!closest_idx) { + return; + } + + double dist = 0.0; + for (size_t idx = *closest_idx; idx < traj.size() - 1; ++idx) { + dist += autoware_utils::calcDistance2d(traj.at(idx), traj.at(idx + 1)); + if (dist > external_velocity_limit_dist_) { + trajectory_utils::applyMaximumVelocityLimit( + idx + 1, traj.size(), external_velocity_limit_, traj); + return; + } + } + traj.back().longitudinal_velocity_mps = + std::min(traj.back().longitudinal_velocity_mps, static_cast(external_velocity_limit_)); + RCLCPP_DEBUG(get_logger(), "externalVelocityLimit : limit_vel = %.3f", external_velocity_limit_); +} + +void MotionVelocitySmootherNode::applyStopApproachingVelocity(TrajectoryPoints & traj) const +{ + const auto stop_idx = autoware_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_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 MotionVelocitySmootherNode::publishDebugTrajectories( + const std::vector & debug_trajectories) const +{ + if (node_param_.algorithm_type == AlgorithmType::JERK_FILTERED) { + if (debug_trajectories.size() != 3) { + RCLCPP_DEBUG(get_logger(), "Size of the debug trajectories is incorrect"); + return; + } + const auto & h = base_traj_raw_ptr_->header; + pub_forward_filtered_trajectory_->publish(toTrajectoryMsg(debug_trajectories.at(0), h)); + pub_backward_filtered_trajectory_->publish(toTrajectoryMsg(debug_trajectories.at(1), h)); + pub_merged_filtered_trajectory_->publish(toTrajectoryMsg(debug_trajectories.at(2), h)); + publishClosestVelocity( + debug_trajectories.at(2), current_pose_ptr_->pose, pub_closest_merged_velocity_); + } +} + +void MotionVelocitySmootherNode::publishClosestVelocity( + const TrajectoryPoints & trajectory, const Pose & current_pose, + const rclcpp::Publisher::SharedPtr pub) const +{ + const auto closest_point = + trajectory_utils::calcInterpolatedTrajectoryPoint(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 MotionVelocitySmootherNode::publishClosestState(const TrajectoryPoint & closest_point) +{ + auto publishFloat = [=]( + const double data, const rclcpp::Publisher::SharedPtr 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; +} + +MotionVelocitySmootherNode::AlgorithmType MotionVelocitySmootherNode::getAlgorithmType( + const std::string & algorithm_name) const +{ + if (algorithm_name == "JerkFiltered") { + return AlgorithmType::JERK_FILTERED; + } + if (algorithm_name == "L2") { + return AlgorithmType::L2; + } + if (algorithm_name == "Linf") { + return AlgorithmType::LINF; + } + if (algorithm_name == "Analytical") { + return AlgorithmType::ANALYTICAL; + } + + throw std::domain_error("[MotionVelocitySmootherNode] undesired algorithm is selected."); + return AlgorithmType::INVALID; +} + +double MotionVelocitySmootherNode::calcTravelDistance() const +{ + const auto closest_point = + trajectory_utils::calcInterpolatedTrajectoryPoint(prev_output_, current_pose_ptr_->pose); + + if (prev_closest_point_) { + const double travel_dist = autoware_utils::calcDistance2d(*prev_closest_point_, closest_point); + return travel_dist; + } + + return 0.0; +} + +bool MotionVelocitySmootherNode::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 MotionVelocitySmootherNode::toTrajectoryMsg( + const TrajectoryPoints & points, const std_msgs::msg::Header & header) const +{ + auto trajectory = autoware_utils::convertToTrajectory(points); + trajectory.header = header; + return trajectory; +} + +} // namespace motion_velocity_smoother + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(motion_velocity_smoother::MotionVelocitySmootherNode) diff --git a/planning/motion_velocity_smoother/src/resample.cpp b/planning/motion_velocity_smoother/src/resample.cpp new file mode 100644 index 0000000000000..aa70a3ec99eb5 --- /dev/null +++ b/planning/motion_velocity_smoother/src/resample.cpp @@ -0,0 +1,266 @@ +// 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 "motion_velocity_smoother/resample.hpp" + +#include +#include + +namespace motion_velocity_smoother +{ +namespace resampling +{ +boost::optional resampleTrajectory( + const TrajectoryPoints & input, const double v_current, const size_t closest_id, + const ResampleParam & param) +{ + // Arc length from the initial point to the closest point + const double front_arclength_value = trajectory_utils::calcArcLength(input, 0, closest_id); + + // Get the nearest point where velocity is zero + auto zero_vel_id = autoware_utils::searchZeroVelocityIndex(input, closest_id, input.size()); + // Arc length from the closest point to the point where velocity is zero + double zero_vel_arclength_value = param.max_trajectory_length; + if (zero_vel_id) { + zero_vel_arclength_value = std::min( + zero_vel_arclength_value, + autoware_utils::calcSignedArcLength(input, closest_id, *zero_vel_id)); + } + + // Get the resample size from the closest point + const std::vector in_arclength = trajectory_utils::calcArclengthArray(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 >= in_arclength.back()) { + 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_i > zero_vel_arclength_value && !is_zero_point_included) { + if (std::fabs(dist_i - zero_vel_arclength_value) > 1e-3) { + // dist_i is much bigger than zero_vel_arclength_value + if ( + !out_arclength.empty() && + std::fabs(out_arclength.back() - (zero_vel_arclength_value + front_arclength_value)) < + 1e-3) { + out_arclength.back() = zero_vel_arclength_value + front_arclength_value; + } else { + out_arclength.push_back(zero_vel_arclength_value + front_arclength_value); + } + } else { + // dist_i is close to the zero_vel_arclength_value + dist_i = zero_vel_arclength_value; + } + + is_zero_point_included = true; + } + + out_arclength.push_back(dist_i + front_arclength_value); + } + + auto output = + trajectory_utils::applyLinearInterpolation(in_arclength, input, out_arclength, true); + if (!output) { + RCLCPP_WARN( + rclcpp::get_logger("motion_velocity_smoother").get_child("resample"), + "fail trajectory interpolation. size : in_arclength = %lu, " + "input = %lu, out_arclength = %lu", + in_arclength.size(), input.size(), out_arclength.size()); + return boost::none; + } + + // add end point directly to consider the endpoint velocity. + if (is_endpoint_included) { + constexpr double ep_dist = 1.0E-3; + if (autoware_utils::calcDistance2d(output->back(), input.back()) < ep_dist) { + output->back() = input.back(); + } else { + output->push_back(input.back()); + } + } + + return output; +} + +boost::optional resampleTrajectory( + const TrajectoryPoints & input, const size_t closest_id, const ResampleParam & param, + const double nominal_ds) +{ + // input arclength + std::vector in_arclength = trajectory_utils::calcArclengthArray(input); + + // Get the nearest point where velocity is zero + // to avoid getting closest_id as a stop point, search zero velocity index from closest_id + 1. + auto stop_id = autoware_utils::searchZeroVelocityIndex(input, closest_id + 1, input.size()); + // Arc length from the closest point to the point where velocity is zero + double stop_arclength_value = param.max_trajectory_length; + if (stop_id) { + stop_arclength_value = std::min( + stop_arclength_value, autoware_utils::calcSignedArcLength(input, closest_id, *stop_id)); + } + + // Do dense resampling before the stop line(3[m] ahead of the stop line) + if (param.min_trajectory_length < stop_arclength_value) { + stop_arclength_value = param.min_trajectory_length; + } + if (in_arclength.back() < stop_arclength_value) { + stop_arclength_value = in_arclength.back(); + } + 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 double front_arclength_value = trajectory_utils::calcArcLength(input, 0, closest_id); + 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 (true) { + 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 >= in_arclength.back()) { + 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); + } + + auto output = trajectory_utils::applyLinearInterpolation(in_arclength, input, out_arclength); + if (!output) { + RCLCPP_WARN( + rclcpp::get_logger("motion_velocity_smoother").get_child("resample"), + "fail trajectory interpolation. size : in_arclength = %lu, " + "input = %lu, out_arclength = %lu", + in_arclength.size(), input.size(), out_arclength.size()); + return boost::none; + } + + // add end point directly to consider the endpoint velocity. + if (is_endpoint_included) { + constexpr double ep_dist = 1.0E-3; + if (autoware_utils::calcDistance2d(output->back(), input.back()) < ep_dist) { + output->back() = input.back(); + } else { + output->push_back(input.back()); + } + } + + return output; +} + +} // namespace resampling +} // namespace motion_velocity_smoother diff --git a/planning/motion_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp b/planning/motion_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp new file mode 100644 index 0000000000000..da0c630a05c55 --- /dev/null +++ b/planning/motion_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp @@ -0,0 +1,633 @@ +// 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 +#include +#include +#include +#include + +// *INDENT-OFF* +#include "motion_velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp" +// *INDENT-ON* + +namespace +{ +using TrajectoryPoints = std::vector; + +geometry_msgs::msg::Pose lerpByPose( + const geometry_msgs::msg::Pose & p1, const geometry_msgs::msg::Pose & p2, const double t) +{ + tf2::Transform tf_transform1, tf_transform2; + tf2::fromMsg(p1, tf_transform1); + tf2::fromMsg(p2, tf_transform2); + const auto & tf_point = tf2::lerp(tf_transform1.getOrigin(), tf_transform2.getOrigin(), t); + const auto & tf_quaternion = + tf2::slerp(tf_transform1.getRotation(), tf_transform2.getRotation(), t); + + geometry_msgs::msg::Pose pose; + pose.position.x = tf_point.getX(); + pose.position.y = tf_point.getY(); + pose.position.z = tf_point.getZ(); + pose.orientation = tf2::toMsg(tf_quaternion); + return pose; +} + +bool applyMaxVelocity( + const double max_velocity, const size_t start_index, const size_t end_index, + TrajectoryPoints & output_trajectory) +{ + if (end_index < start_index || output_trajectory.size() < end_index) { + return false; + } + + for (size_t i = start_index; i <= end_index; ++i) { + output_trajectory.at(i).longitudinal_velocity_mps = + std::min(output_trajectory.at(i).longitudinal_velocity_mps, static_cast(max_velocity)); + output_trajectory.at(i).acceleration_mps2 = 0.0; + } + return true; +} + +} // namespace + +namespace motion_velocity_smoother +{ +AnalyticalJerkConstrainedSmoother::AnalyticalJerkConstrainedSmoother(const Param & smoother_param) +: smoother_param_(smoother_param) +{ +} +void AnalyticalJerkConstrainedSmoother::setParam(const Param & smoother_param) +{ + smoother_param_ = smoother_param; +} + +bool AnalyticalJerkConstrainedSmoother::apply( + const double initial_vel, const double initial_acc, const TrajectoryPoints & input, + TrajectoryPoints & output, [[maybe_unused]] std::vector & debug_trajectories) +{ + RCLCPP_DEBUG(logger_, "-------------------- Start --------------------"); + + // guard + if (input.empty()) { + RCLCPP_DEBUG(logger_, "Fail. input trajectory is empty"); + return false; + } + + // intput trajectory is cropped, so closest_index = 0 + const size_t closest_index = 0; + + // Find deceleration targets + if (input.size() == 1) { + RCLCPP_DEBUG( + logger_, + "Input trajectory size is too short. Cannot find decel targets and " + "return v0, a0"); + output = input; + output.front().longitudinal_velocity_mps = initial_vel; + output.front().acceleration_mps2 = initial_acc; + return true; + } + std::vector> decel_target_indices; + searchDecelTargetIndices(input, closest_index, decel_target_indices); + RCLCPP_DEBUG(logger_, "Num deceleration targets: %zd", decel_target_indices.size()); + for (auto & index : decel_target_indices) { + RCLCPP_DEBUG( + logger_, "Target deceleration index: %ld, target velocity: %f", index.first, index.second); + } + + // Apply filters according to deceleration targets + TrajectoryPoints reference_trajectory = input; + TrajectoryPoints filtered_trajectory = input; + for (size_t i = 0; i < decel_target_indices.size(); ++i) { + size_t fwd_start_index; + double fwd_start_vel; + double fwd_start_acc; + if (i == 0) { + fwd_start_index = closest_index; + fwd_start_vel = initial_vel; + fwd_start_acc = initial_acc; + } else { + fwd_start_index = decel_target_indices.at(i - 1).first; + fwd_start_vel = filtered_trajectory.at(fwd_start_index).longitudinal_velocity_mps; + fwd_start_acc = filtered_trajectory.at(fwd_start_index).acceleration_mps2; + } + + RCLCPP_DEBUG(logger_, "Apply forward jerk filter from: %ld", fwd_start_index); + applyForwardJerkFilter( + reference_trajectory, fwd_start_index, fwd_start_vel, fwd_start_acc, smoother_param_, + filtered_trajectory); + + size_t bwd_start_index = closest_index; + double bwd_start_vel = initial_vel; + double bwd_start_acc = initial_acc; + for (int j = i; j >= 0; --j) { + if (j == 0) { + bwd_start_index = closest_index; + bwd_start_vel = initial_vel; + bwd_start_acc = initial_acc; + break; + } + if (decel_target_indices.at(j - 1).second < decel_target_indices.at(j).second) { + bwd_start_index = decel_target_indices.at(j - 1).first; + bwd_start_vel = filtered_trajectory.at(bwd_start_index).longitudinal_velocity_mps; + bwd_start_acc = filtered_trajectory.at(bwd_start_index).acceleration_mps2; + break; + } + } + std::vector start_indices; + if (bwd_start_index != fwd_start_index) { + start_indices.push_back(bwd_start_index); + start_indices.push_back(fwd_start_index); + } else { + start_indices.push_back(bwd_start_index); + } + + const size_t decel_target_index = decel_target_indices.at(i).first; + const double decel_target_vel = decel_target_indices.at(i).second; + RCLCPP_DEBUG( + logger_, "Apply backward decel filter from: %s, to: %ld (%f)", + strStartIndices(start_indices).c_str(), decel_target_index, decel_target_vel); + if (!applyBackwardDecelFilter( + start_indices, decel_target_index, decel_target_vel, smoother_param_, + filtered_trajectory)) { + RCLCPP_DEBUG( + logger_, + "Failed to apply backward decel filter, so apply max velocity filter. max velocity = %f, " + "start_index = %s, end_index = %zd", + decel_target_vel, strStartIndices(start_indices).c_str(), filtered_trajectory.size() - 1); + + const double ep = 0.001; + if (std::abs(decel_target_vel) < ep) { + applyMaxVelocity(0.0, bwd_start_index, filtered_trajectory.size() - 1, filtered_trajectory); + output = filtered_trajectory; + RCLCPP_DEBUG(logger_, "-------------------- Finish --------------------"); + return true; + } + applyMaxVelocity(decel_target_vel, bwd_start_index, decel_target_index, reference_trajectory); + RCLCPP_DEBUG(logger_, "Apply forward jerk filter from: %ld", bwd_start_index); + applyForwardJerkFilter( + reference_trajectory, bwd_start_index, bwd_start_vel, bwd_start_acc, smoother_param_, + filtered_trajectory); + } + } + + size_t start_index; + double start_vel; + double start_acc; + if (decel_target_indices.empty() == true) { + start_index = closest_index; + start_vel = initial_vel; + start_acc = initial_acc; + } else { + start_index = decel_target_indices.back().first; + start_vel = filtered_trajectory.at(start_index).longitudinal_velocity_mps; + start_acc = filtered_trajectory.at(start_index).acceleration_mps2; + } + RCLCPP_DEBUG(logger_, "Apply forward jerk filter from: %ld", start_index); + applyForwardJerkFilter( + reference_trajectory, start_index, start_vel, start_acc, smoother_param_, filtered_trajectory); + + output = filtered_trajectory; + + RCLCPP_DEBUG(logger_, "-------------------- Finish --------------------"); + return true; +} + +boost::optional AnalyticalJerkConstrainedSmoother::resampleTrajectory( + const TrajectoryPoints & input, [[maybe_unused]] const double v_current, + [[maybe_unused]] const int closest_id) const +{ + TrajectoryPoints output; + if (input.empty()) { + RCLCPP_WARN(logger_, "Input trajectory is empty"); + return {}; + } + + const double ds = 1.0 / static_cast(smoother_param_.resample.num_resample); + + for (size_t i = 0; i < input.size() - 1; ++i) { + double s = 0.0; + const auto tp0 = input.at(i); + const auto tp1 = input.at(i + 1); + + const double dist_thr = 0.001; // 1mm + const double dist_tp0_tp1 = autoware_utils::calcDistance2d(tp0, tp1); + if (std::fabs(dist_tp0_tp1) < dist_thr) { + output.push_back(input.at(i)); + continue; + } + + for (size_t j = 0; j < smoother_param_.resample.num_resample; ++j) { + auto tp = input.at(i); + + tp.pose = lerpByPose(tp0.pose, tp1.pose, s); + tp.longitudinal_velocity_mps = tp0.longitudinal_velocity_mps; + tp.heading_rate_rps = (1.0 - s) * tp0.heading_rate_rps + s * tp1.heading_rate_rps; + tp.acceleration_mps2 = tp0.acceleration_mps2; + // tp.accel.angular.z = (1.0 - s) * tp0.accel.angular.z + s * tp1.accel.angular.z; + + output.push_back(tp); + + s += ds; + } + } + + output.push_back(input.back()); + + return boost::optional(output); +} + +boost::optional AnalyticalJerkConstrainedSmoother::applyLateralAccelerationFilter( + const TrajectoryPoints & input) const +{ + if (input.empty()) { + return boost::none; + } + + if (input.size() < 3) { + return boost::optional(input); // cannot calculate lateral acc. do nothing. + } + + // Interpolate with constant interval distance for lateral acceleration calculation. + constexpr double points_interval = 0.1; // [m] + std::vector out_arclength; + const std::vector in_arclength = trajectory_utils::calcArclengthArray(input); + for (double s = 0; s < in_arclength.back(); s += points_interval) { + out_arclength.push_back(s); + } + auto output = trajectory_utils::applyLinearInterpolation(in_arclength, input, out_arclength); + if (!output) { + RCLCPP_WARN(logger_, "Interpolation failed at lateral acceleration filter."); + return boost::none; + } + output->back() = input.back(); // keep the final speed. + + constexpr double curvature_calc_dist = 5.0; // [m] calc curvature with 5m away points + const size_t idx_dist = + static_cast(std::max(static_cast((curvature_calc_dist) / points_interval), 1)); + + // Calculate curvature assuming the trajectory points interval is constant + const auto curvature_v = trajectory_utils::calcTrajectoryCurvatureFrom3Points(*output, idx_dist); + if (!curvature_v) { + return boost::optional(input); + } + + // 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); + + std::vector filtered_points; + for (size_t i = 0; i < output->size(); ++i) { + double curvature = 0.0; + const size_t start = i > before_decel_index ? i - before_decel_index : 0; + const size_t end = std::min(output->size(), i + after_decel_index); + for (size_t j = start; j < end; ++j) { + 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 (output->at(i).longitudinal_velocity_mps > v_curvature_max) { + output->at(i).longitudinal_velocity_mps = v_curvature_max; + filtered_points.push_back(i); + } + } + + // Keep constant velocity while turning + const double dist_threshold = smoother_param_.latacc.constant_velocity_dist_threshold; + std::vector> latacc_filtered_ranges; + size_t start_index = 0; + size_t end_index = 0; + bool is_updated = false; + double min_latacc_velocity; + for (size_t i = 0; i < filtered_points.size(); ++i) { + const size_t index = filtered_points.at(i); + + if (is_updated == false) { + start_index = index; + end_index = index; + min_latacc_velocity = output->at(index).longitudinal_velocity_mps; + is_updated = true; + continue; + } + + if (autoware_utils::calcDistance2d(output->at(end_index), output->at(index)) < dist_threshold) { + end_index = index; + min_latacc_velocity = std::min( + static_cast(output->at(index).longitudinal_velocity_mps), min_latacc_velocity); + } else { + latacc_filtered_ranges.emplace_back(start_index, end_index, min_latacc_velocity); + start_index = index; + end_index = index; + min_latacc_velocity = output->at(index).longitudinal_velocity_mps; + } + } + if (is_updated) { + latacc_filtered_ranges.emplace_back(start_index, end_index, min_latacc_velocity); + } + + for (size_t i = 0; i < output->size(); ++i) { + for (const auto & lat_acc_filtered_range : latacc_filtered_ranges) { + const size_t start_index = std::get<0>(lat_acc_filtered_range); + const size_t end_index = std::get<1>(lat_acc_filtered_range); + const double min_latacc_velocity = std::get<2>(lat_acc_filtered_range); + + if ( + start_index <= i && i <= end_index && + smoother_param_.latacc.enable_constant_velocity_while_turning) { + output->at(i).longitudinal_velocity_mps = min_latacc_velocity; + break; + } + } + } + + return output; +} + +bool AnalyticalJerkConstrainedSmoother::searchDecelTargetIndices( + const TrajectoryPoints & trajectory, const size_t closest_index, + std::vector> & decel_target_indices) const +{ + const double ep = -0.00001; + const size_t start_index = std::max(1, closest_index); + std::vector> tmp_indices; + for (size_t i = start_index; i < trajectory.size() - 1; ++i) { + const double dv_before = + trajectory.at(i).longitudinal_velocity_mps - trajectory.at(i - 1).longitudinal_velocity_mps; + const double dv_after = + trajectory.at(i + 1).longitudinal_velocity_mps - trajectory.at(i).longitudinal_velocity_mps; + if (dv_before < ep && dv_after > ep) { + tmp_indices.emplace_back(i, trajectory.at(i).longitudinal_velocity_mps); + } + } + + const unsigned int i = trajectory.size() - 1; + const double dv_before = + trajectory.at(i).longitudinal_velocity_mps - trajectory.at(i - 1).longitudinal_velocity_mps; + if (dv_before < ep) { + tmp_indices.emplace_back(i, trajectory.at(i).longitudinal_velocity_mps); + } + + if (!tmp_indices.empty()) { + for (unsigned int i = 0; i < tmp_indices.size() - 1; ++i) { + const size_t index_err = 10; + if ( + (tmp_indices.at(i + 1).first - tmp_indices.at(i).first < index_err) && + (tmp_indices.at(i + 1).second < tmp_indices.at(i).second)) { + continue; + } + + decel_target_indices.emplace_back(tmp_indices.at(i).first, tmp_indices.at(i).second); + } + } + if (!tmp_indices.empty()) { + decel_target_indices.emplace_back(tmp_indices.back().first, tmp_indices.back().second); + } + return true; +} + +bool AnalyticalJerkConstrainedSmoother::applyForwardJerkFilter( + const TrajectoryPoints & base_trajectory, const size_t start_index, const double initial_vel, + const double initial_acc, const Param & params, TrajectoryPoints & output_trajectory) const +{ + output_trajectory.at(start_index).longitudinal_velocity_mps = initial_vel; + output_trajectory.at(start_index).acceleration_mps2 = initial_acc; + + for (size_t i = start_index + 1; i < base_trajectory.size(); ++i) { + const double prev_vel = output_trajectory.at(i - 1).longitudinal_velocity_mps; + const double ds = + autoware_utils::calcDistance2d(base_trajectory.at(i - 1), base_trajectory.at(i)); + const double dt = ds / std::max(prev_vel, 1.0); + + const double prev_acc = output_trajectory.at(i - 1).acceleration_mps2; + const double curr_vel = std::max(prev_vel + prev_acc * dt, 0.0); + + const double error_vel = base_trajectory.at(i).longitudinal_velocity_mps - curr_vel; + const double fb_acc = params.forward.kp * error_vel; + const double limited_acc = + std::max(params.forward.min_acc, std::min(params.forward.max_acc, fb_acc)); + const double fb_jerk = (limited_acc - prev_acc) / dt; + const double limited_jerk = + std::max(params.forward.min_jerk, std::min(params.forward.max_jerk, fb_jerk)); + + const double curr_acc = prev_acc + limited_jerk * dt; + + output_trajectory.at(i).longitudinal_velocity_mps = curr_vel; + output_trajectory.at(i).acceleration_mps2 = curr_acc; + } + + return true; +} + +bool AnalyticalJerkConstrainedSmoother::applyBackwardDecelFilter( + const std::vector & start_indices, const size_t decel_target_index, + const double decel_target_vel, const Param & params, TrajectoryPoints & output_trajectory) const +{ + const double ep = 0.001; + + double output_planning_jerk = -100.0; + size_t output_start_index = 0; + std::vector output_dist_to_target; + int output_type; + std::vector output_times; + + for (size_t start_index : start_indices) { + double dist = 0.0; + std::vector dist_to_target(output_trajectory.size(), 0); + dist_to_target.at(decel_target_index) = dist; + for (size_t i = start_index; i < decel_target_index; ++i) { + if (output_trajectory.at(i).longitudinal_velocity_mps >= decel_target_vel) { + start_index = i; + break; + } + } + for (size_t i = decel_target_index; i > start_index; --i) { + dist += autoware_utils::calcDistance2d(output_trajectory.at(i - 1), output_trajectory.at(i)); + dist_to_target.at(i - 1) = dist; + } + + RCLCPP_DEBUG(logger_, "Check enough dist to decel. start_index: %ld", start_index); + double planning_jerk; + int type; + std::vector times; + double stop_dist; + bool is_enough_dist = false; + for (planning_jerk = params.backward.start_jerk; planning_jerk > params.backward.min_jerk - ep; + planning_jerk += params.backward.span_jerk) { + if (calcEnoughDistForDecel( + output_trajectory, start_index, decel_target_vel, planning_jerk, params, dist_to_target, + is_enough_dist, type, times, stop_dist)) { + break; + } + } + + if (!is_enough_dist) { + RCLCPP_DEBUG(logger_, "Distance is not enough for decel with all jerk condition"); + continue; + } + + if (planning_jerk >= output_planning_jerk) { + output_planning_jerk = planning_jerk; + output_start_index = start_index; + output_dist_to_target = dist_to_target; + output_type = type; + output_times = times; + RCLCPP_DEBUG( + logger_, "Update planning jerk: %f, start_index: %ld", planning_jerk, start_index); + } + } + + if (output_planning_jerk == -100.0) { + RCLCPP_DEBUG( + logger_, + "Distance is not enough for decel with all jerk and start index " + "condition"); + return false; + } + + RCLCPP_DEBUG(logger_, "Search decel start index"); + size_t decel_start_index = output_start_index; + bool is_enough_dist = false; + double stop_dist; + if (output_planning_jerk == params.backward.start_jerk) { + for (size_t i = decel_target_index - 1; i >= output_start_index; --i) { + if (calcEnoughDistForDecel( + output_trajectory, i, decel_target_vel, output_planning_jerk, params, + output_dist_to_target, is_enough_dist, output_type, output_times, stop_dist)) { + decel_start_index = i; + break; + } + } + } + + RCLCPP_DEBUG( + logger_, + "Apply filter. decel_start_index: %ld, target_vel: %f, " + "planning_jerk: %f, type: %d, times: %s", + decel_start_index, decel_target_vel, output_planning_jerk, output_type, + strTimes(output_times).c_str()); + if (!applyDecelVelocityFilter( + decel_start_index, decel_target_vel, output_planning_jerk, params, output_type, + output_times, output_trajectory)) { + RCLCPP_DEBUG( + logger_, + "[applyDecelVelocityFilter] dist is enough, but fail to plan backward decel velocity"); + return false; + } + + return true; +} + +bool AnalyticalJerkConstrainedSmoother::calcEnoughDistForDecel( + const TrajectoryPoints & trajectory, const size_t start_index, const double decel_target_vel, + const double planning_jerk, const Param & params, const std::vector & dist_to_target, + bool & is_enough_dist, int & type, std::vector & times, double & stop_dist) const +{ + const double v0 = trajectory.at(start_index).longitudinal_velocity_mps; + const double a0 = trajectory.at(start_index).acceleration_mps2; + const double jerk_acc = std::abs(planning_jerk); + const double jerk_dec = planning_jerk; + // *INDENT-OFF* + auto calcMinAcc = [¶ms](const double planning_jerk) { + if (planning_jerk < params.backward.min_jerk_mild_stop) { + return params.backward.min_acc; + } + return params.backward.min_acc_mild_stop; + }; + // *INDENT-ON* + const double min_acc = calcMinAcc(planning_jerk); + type = 0; + times.clear(); + stop_dist = 0.0; + + if (!analytical_velocity_planning_utils::calcStopDistWithJerkAndAccConstraints( + v0, a0, jerk_acc, jerk_dec, min_acc, decel_target_vel, type, times, stop_dist)) { + return false; + } + + const double allowed_dist = dist_to_target.at(start_index); + if (0.0 <= stop_dist && stop_dist <= allowed_dist) { + RCLCPP_DEBUG( + logger_, + "Distance is enough. v0: %f, a0: %f, jerk: %f, stop_dist: %f, " + "allowed_dist: %f", + v0, a0, planning_jerk, stop_dist, allowed_dist); + is_enough_dist = true; + return true; + } + RCLCPP_DEBUG( + logger_, + "Distance is not enough. v0: %f, a0: %f, jerk: %f, stop_dist: %f, " + "allowed_dist: %f", + v0, a0, planning_jerk, stop_dist, allowed_dist); + return false; +} + +bool AnalyticalJerkConstrainedSmoother::applyDecelVelocityFilter( + const size_t decel_start_index, const double decel_target_vel, const double planning_jerk, + const Param & params, const int type, const std::vector & times, + TrajectoryPoints & output_trajectory) const +{ + const double v0 = output_trajectory.at(decel_start_index).longitudinal_velocity_mps; + const double a0 = output_trajectory.at(decel_start_index).acceleration_mps2; + const double jerk_acc = std::abs(planning_jerk); + const double jerk_dec = planning_jerk; + // *INDENT-OFF* + auto calcMinAcc = [¶ms](const double planning_jerk) { + if (planning_jerk < params.backward.min_jerk_mild_stop) { + return params.backward.min_acc; + } + return params.backward.min_acc_mild_stop; + }; + // *INDENT-ON* + const double min_acc = calcMinAcc(planning_jerk); + + if (!analytical_velocity_planning_utils::calcStopVelocityWithConstantJerkAccLimit( + v0, a0, jerk_acc, jerk_dec, min_acc, decel_target_vel, type, times, decel_start_index, + output_trajectory)) { + return false; + } + + return true; +} + +std::string AnalyticalJerkConstrainedSmoother::strTimes(const std::vector & times) const +{ + std::stringstream ss; + unsigned int i = 0; + for (double time : times) { + ss << "time[" << i << "] = " << time << ", "; + i++; + } + return ss.str(); +} + +std::string AnalyticalJerkConstrainedSmoother::strStartIndices( + const std::vector & start_indices) const +{ + std::stringstream ss; + for (size_t i = 0; i < start_indices.size(); ++i) { + if (i != (start_indices.size() - 1)) { + ss << start_indices.at(i) << ", "; + } else { + ss << start_indices.at(i); + } + } + return ss.str(); +} + +} // namespace motion_velocity_smoother diff --git a/planning/motion_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.cpp b/planning/motion_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.cpp new file mode 100644 index 0000000000000..4d9da44ab8fbe --- /dev/null +++ b/planning/motion_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.cpp @@ -0,0 +1,342 @@ +// 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 +#include + +// *INDENT-OFF* +#include "motion_velocity_smoother/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.hpp" +// *INDENT-ON* + +namespace motion_velocity_smoother +{ +namespace analytical_velocity_planning_utils +{ +bool calcStopDistWithJerkAndAccConstraints( + const double v0, const double a0, const double jerk_acc, const double jerk_dec, + const double min_acc, const double target_vel, int & type, std::vector & times, + double & stop_dist) +{ + const double t_min = + (target_vel - v0 - 0.5 * (0 - a0) / jerk_dec * a0 - 0.5 * min_acc / jerk_dec * min_acc - + 0.5 * (0 - min_acc) / jerk_acc * min_acc) / + min_acc; + + if (t_min > 0) { + double t1 = (min_acc - a0) / jerk_dec; + if (t1 < 0.01) { + t1 = 0; + } + + const double a1 = a0 + jerk_dec * t1; + const double v1 = v0 + a0 * t1 + 0.5 * jerk_dec * t1 * t1; + const double x1 = v0 * t1 + 0.5 * a0 * t1 * t1 + (1.0 / 6.0) * jerk_dec * t1 * t1 * t1; + + double t2 = t_min; + if (t2 < 0.01) { + t2 = 0; + } + + const double a2 = a1; + const double v2 = v1 + a1 * t2; + const double x2 = x1 + v1 * t2 + 0.5 * a1 * t2 * t2; + + double t3 = (0 - min_acc) / jerk_acc; + if (t3 < 0.01) { + t3 = 0; + } + + const double a3 = a2 + jerk_acc * t3; + const double v3 = v2 + a2 * t3 + 0.5 * jerk_acc * t3 * t3; + const double x3 = x2 + v2 * t3 + 0.5 * a2 * t3 * t3 + (1.0 / 6.0) * jerk_acc * t3 * t3 * t3; + + const double a_target = 0.0; + const double v_margin = 0.3; // [m/s] + const double a_margin = 0.1; // [m/s^2] + if (!validCheckCalcStopDist(v3, a3, target_vel, a_target, v_margin, a_margin)) { + RCLCPP_DEBUG(rclcpp::get_logger("velocity_planning_utils"), "Valid check error. type = 1"); + return false; + } + + type = 1; + times.push_back(t1); + times.push_back(t2); + times.push_back(t3); + stop_dist = x3; + } else { + const double is_decel_needed = 0.5 * (0 - a0) / jerk_acc * a0 - (target_vel - v0); + if (is_decel_needed > 0 || a0 > 0) { + const double a1_square = (target_vel - v0 - 0.5 * (0 - a0) / jerk_dec * a0) * + (2 * jerk_acc * jerk_dec / (jerk_acc - jerk_dec)); + const double a1 = -std::sqrt(a1_square); + + double t1 = (a1 - a0) / jerk_dec; + if (t1 < 0.01) { + t1 = 0; + } + + const double v1 = v0 + a0 * t1 + 0.5 * jerk_dec * t1 * t1; + const double x1 = v0 * t1 + 0.5 * a0 * t1 * t1 + (1.0 / 6.0) * jerk_dec * t1 * t1 * t1; + + double t2 = (0 - a1) / jerk_acc; + if (t2 < 0.01) { + t2 = 0; + } + + const double a2 = a1 + jerk_acc * t2; + const double v2 = v1 + a1 * t2 + 0.5 * jerk_acc * t2 * t2; + const double x2 = x1 + v1 * t2 + 0.5 * a1 * t2 * t2 + (1.0 / 6.0) * jerk_acc * t2 * t2 * t2; + + const double a_target = 0.0; + const double v_margin = 0.3; + const double a_margin = 0.1; + if (!validCheckCalcStopDist(v2, a2, target_vel, a_target, v_margin, a_margin)) { + RCLCPP_DEBUG(rclcpp::get_logger("velocity_planning_utils"), "Valid check error. type = 2"); + return false; + } + + type = 2; + times.push_back(t1); + times.push_back(t2); + stop_dist = x2; + } else { + double t1 = (0 - a0) / jerk_acc; + if (t1 < 0) { + RCLCPP_DEBUG( + rclcpp::get_logger("velocity_planning_utils"), "t1 < 0. unexpected condition."); + return false; + } + if (t1 < 0.01) { + t1 = 0; + } + + const double a1 = a0 + jerk_acc * t1; + const double v1 = v0 + a0 * t1 + 0.5 * jerk_acc * t1 * t1; + const double x1 = v0 * t1 + 0.5 * a0 * t1 * t1 + (1.0 / 6.0) * jerk_acc * t1 * t1 * t1; + + const double a_target = 0.0; + const double v_margin = 0.3; + const double a_margin = 0.1; + if (!validCheckCalcStopDist(v1, a1, target_vel, a_target, v_margin, a_margin)) { + RCLCPP_DEBUG(rclcpp::get_logger("velocity_planning_utils"), "Valid check error. type = 3"); + return false; + } + + type = 3; + times.push_back(t1); + stop_dist = x1; + } + } + return true; +} + +bool validCheckCalcStopDist( + 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_DEBUG( + rclcpp::get_logger("velocity_planning_utils"), "Valid check error! v_target = %f, v_end = %f", + v_target, v_end); + return false; + } + if (a_end < a_min || a_max < a_end) { + RCLCPP_DEBUG( + rclcpp::get_logger("velocity_planning_utils"), "Valid check error! a_target = %f, a_end = %f", + a_target, a_end); + return false; + } + return true; +} + +bool calcStopVelocityWithConstantJerkAccLimit( + const double v0, const double a0, const double jerk_acc, const double jerk_dec, + const double min_acc, const double decel_target_vel, const int type, + const std::vector & times, const size_t start_index, TrajectoryPoints & output_trajectory) +{ + const double t_total = std::accumulate(times.begin(), times.end(), 0.0); + std::vector ts, xs, vs, as, js; + const double dt = 0.1; + double x = 0.0; + double v = 0.0; + double a = 0.0; + double j = 0.0; + + for (double t = 0.0; t < t_total; t += dt) { + updateStopVelocityStatus(v0, a0, jerk_acc, jerk_dec, type, times, t, x, v, a, j); + if (v > 0.0) { + a = std::max(a, min_acc); + ts.push_back(t); + xs.push_back(x); + vs.push_back(v); + as.push_back(a); + js.push_back(j); + } + } + updateStopVelocityStatus(v0, a0, jerk_acc, jerk_dec, type, times, t_total, x, v, a, j); + if (v > 0.0 && !xs.empty() && xs.back() < x) { + a = std::max(a, min_acc); + ts.push_back(t_total); + xs.push_back(x); + vs.push_back(v); + as.push_back(a); + js.push_back(j); + } + + // for debug + std::stringstream ss; + for (unsigned int i = 0; i < ts.size(); ++i) { + ss << "t: " << ts.at(i) << ", x: " << xs.at(i) << ", v: " << vs.at(i) << ", a: " << as.at(i) + << ", j: " << js.at(i) << std::endl; + } + RCLCPP_DEBUG( + rclcpp::get_logger("velocity_planning_utils"), "Calculate stop velocity. %s", ss.str().c_str()); + + const double a_target = 0.0; + const double v_margin = 0.3; + const double a_margin = 0.1; + if (!validCheckCalcStopDist(v, a, decel_target_vel, a_target, v_margin, a_margin)) { + return false; + } + + if (xs.empty()) { + for (size_t i = start_index; i < output_trajectory.size(); ++i) { + output_trajectory.at(i).longitudinal_velocity_mps = decel_target_vel; + output_trajectory.at(i).acceleration_mps2 = 0.0; + } + return true; + } + + double dist = 0.0; + std::vector dists; + dists.push_back(dist); + for (size_t i = start_index; i < output_trajectory.size() - 1; ++i) { + dist += autoware_utils::calcDistance2d(output_trajectory.at(i), output_trajectory.at(i + 1)); + if (dist > xs.back()) { + break; + } + dists.push_back(dist); + } + + const auto vel_at_wp = linear_interpolation::interpolate(xs, vs, dists); + const auto acc_at_wp = linear_interpolation::interpolate(xs, as, dists); + const auto jerk_at_wp = linear_interpolation::interpolate(xs, js, dists); + if (!vel_at_wp || !acc_at_wp || !jerk_at_wp) { + RCLCPP_DEBUG(rclcpp::get_logger("velocity_planning_utils"), "Interpolation error"); + return false; + } + + // for debug + std::stringstream ssi; + for (unsigned int i = 0; i < dists.size(); ++i) { + ssi << "d: " << dists.at(i) << ", v: " << vel_at_wp->at(i) << ", a: " << acc_at_wp->at(i) + << ", j: " << jerk_at_wp->at(i) << std::endl; + } + RCLCPP_DEBUG( + rclcpp::get_logger("velocity_planning_utils"), "Interpolated = %s", ssi.str().c_str()); + + for (size_t i = 0; i < vel_at_wp->size(); ++i) { + output_trajectory.at(start_index + i).longitudinal_velocity_mps = vel_at_wp->at(i); + output_trajectory.at(start_index + i).acceleration_mps2 = acc_at_wp->at(i); + } + for (size_t i = start_index + vel_at_wp->size(); i < output_trajectory.size(); ++i) { + output_trajectory.at(i).longitudinal_velocity_mps = decel_target_vel; + output_trajectory.at(i).acceleration_mps2 = 0.0; + } + + return true; +} + +void updateStopVelocityStatus( + double v0, double a0, double jerk_acc, double jerk_dec, int type, std::vector times, + double t, double & x, double & v, double & a, double & j) +{ + if (type == 1) { + if (0 <= t && t < times.at(0)) { + j = jerk_dec; + a = integ_a(a0, j, t); + v = integ_v(v0, a0, j, t); + x = integ_x(0, v0, a0, j, t); + } else if (times.at(0) <= t && t < times.at(0) + times.at(1)) { + const double t1 = times.at(0); + const double a1 = integ_a(a0, jerk_dec, t1); + const double v1 = integ_v(v0, a0, jerk_dec, t1); + const double x1 = integ_x(0, v0, a0, jerk_dec, t1); + + const double dt = t - t1; + j = 0; + a = integ_a(a1, j, dt); + v = integ_v(v1, a1, j, dt); + x = integ_x(x1, v1, a1, j, dt); + } else if (times.at(0) + times.at(1) <= t && t <= times.at(0) + times.at(1) + times.at(2)) { + const double t1 = times.at(0); + const double a1 = integ_a(a0, jerk_dec, t1); + const double v1 = integ_v(v0, a0, jerk_dec, t1); + const double x1 = integ_x(0, v0, a0, jerk_dec, t1); + + const double t2 = times.at(1); + const double a2 = integ_a(a1, 0, t2); + const double v2 = integ_v(v1, a1, 0, t2); + const double x2 = integ_x(x1, v1, a1, 0, t2); + + const double dt = t - (t1 + t2); + j = jerk_acc; + a = integ_a(a2, j, dt); + v = integ_v(v2, a2, j, dt); + x = integ_x(x2, v2, a2, j, dt); + } + } else if (type == 2) { + if (0 <= t && t < times.at(0)) { + j = jerk_dec; + a = integ_a(a0, j, t); + v = integ_v(v0, a0, j, t); + x = integ_x(0, v0, a0, j, t); + } else if (times.at(0) <= t && t <= times.at(0) + times.at(1)) { + const double t1 = times.at(0); + const double a1 = integ_a(a0, jerk_dec, t1); + const double v1 = integ_v(v0, a0, jerk_dec, t1); + const double x1 = integ_x(0, v0, a0, jerk_dec, t1); + + const double dt = t - t1; + j = jerk_acc; + a = integ_a(a1, j, dt); + v = integ_v(v1, a1, j, dt); + x = integ_x(x1, v1, a1, j, dt); + } + } else if (type == 3) { + if (0 <= t && t <= times.at(0)) { + j = jerk_acc; + a = integ_a(a0, j, t); + v = integ_v(v0, a0, j, t); + x = integ_x(0, v0, a0, j, t); + } + } else { + } +} + +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; +} + +double integ_v(double v0, double a0, double j0, double t) { return v0 + a0 * t + 0.5 * j0 * t * t; } + +double integ_a(double a0, double j0, double t) { return a0 + j0 * t; } + +} // namespace analytical_velocity_planning_utils +} // namespace motion_velocity_smoother diff --git a/planning/motion_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp b/planning/motion_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp new file mode 100644 index 0000000000000..596df3d26f6f1 --- /dev/null +++ b/planning/motion_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp @@ -0,0 +1,466 @@ +// 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 "motion_velocity_smoother/smoother/jerk_filtered_smoother.hpp" + +#include "eigen3/Eigen/Core" +#include "motion_velocity_smoother/trajectory_utils.hpp" + +#include +#include +#include +#include +#include +#include + +namespace motion_velocity_smoother +{ +JerkFilteredSmoother::JerkFilteredSmoother(const Param & smoother_param) +: smoother_param_{smoother_param} +{ + qp_solver_.updateMaxIter(20000); + qp_solver_.updateRhoInterval(0); // 0 means automatic + qp_solver_.updateEpsRel(1.0e-4); // def: 1.0e-4 + qp_solver_.updateEpsAbs(1.0e-8); // def: 1.0e-4 + qp_solver_.updateVerbose(false); +} + +void JerkFilteredSmoother::setParam(const Param & smoother_param) +{ + smoother_param_ = smoother_param; +} + +bool JerkFilteredSmoother::apply( + const double v0, const double a0, const TrajectoryPoints & input, TrajectoryPoints & output, + std::vector & debug_trajectories) +{ + 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; + } + + bool TMP_SHOW_DEBUG_INFO = false; + + 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); + + // Set debug trajectories + debug_trajectories.resize(3); + debug_trajectories[0] = forward_filtered; + debug_trajectories[1] = backward_filtered; + debug_trajectories[2] = filtered; + + // Resample TrajectoryPoints for Optimization + auto opt_resampled_trajectory = + resampling::resampleTrajectory(filtered, v0, 0, base_param_.resample_param); + + if (!opt_resampled_trajectory) { + RCLCPP_WARN(logger_, "Resample failed!"); + return false; + } + // 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; + 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_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; + } + + /* + * 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 + constexpr double ZERO_VEL_THR_FOR_DT_CALC = 0.3; + const double smooth_weight = smoother_param_.jerk_weight; + for (size_t i = 0; i < N - 1; ++i) { + const double ref_vel = v_max_arr.at(i); + 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; + } + + for (size_t i = 0; i < N; ++i) { + const double v_max = std::max(v_max_arr.at(i), 0.1); + q.at(IDX_B0 + i) = + -1.0 / (v_max * v_max); // |v_max_i^2 - b_i|/v_max^2 -> minimize (-bi) * ds / v_max^2 + if (i < N - 1) { + q.at(IDX_B0 + i) *= std::max(interval_dist_arr.at(i), 0.0001); + } + 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 = std::max(v_max_arr.at(i), ZERO_VEL_THR_FOR_DT_CALC); + 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; + } + + // execute optimization + const auto result = qp_solver_.optimize(P, A, q, lower_bound, upper_bound); + const std::vector optval = std::get<0>(result); + + 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; + } + + const int status_val = std::get<3>(result); + if (status_val != 1) { + RCLCPP_ERROR(logger_, "optimization failed : %s", qp_solver_.getStatusMessage().c_str()); + } + + if (TMP_SHOW_DEBUG_INFO) { + // jerk calculation + std::vector p_jerk, jerk_converted, jerk_ideal; + for (size_t i = 0; i < input.size() - 1; ++i) { + double v_ref0 = filtered.at(i).longitudinal_velocity_mps; + double v0 = output.at(i).longitudinal_velocity_mps; + double a0 = output.at(i).acceleration_mps2; + double a1 = output.at(i + 1).acceleration_mps2; + double ds = interval_dist_arr.at(i); + p_jerk.push_back((a1 - a0) / ds); + jerk_converted.push_back((a1 - a0) / ds * v_ref0); + jerk_ideal.push_back((a1 - a0) / ds * v0); + } + p_jerk.push_back(0.0); + jerk_converted.push_back(0.0); + jerk_ideal.push_back(0.0); + + // print + size_t i = 0; + auto getVx = [&i](const TrajectoryPoints & trajectory) { + return trajectory.at(i).longitudinal_velocity_mps; + }; + auto getAx = [&i](const TrajectoryPoints & trajectory) { + return trajectory.at(i).acceleration_mps2; + }; + printf("v0 = %.3f, a0 = %.3f\n", v0, a0); + for (; i < input.size(); ++i) { + double gamma = optval.at(IDX_GAMMA0 + i); + printf( + "i = %lu, input: %.3f, filt_f: (%.3f, %.3f), filt_b: (%.3f, %.3f), filt_fb: (%.3f, %.3f), " + "opt: (%.3f, %f), p_jerk = %.3f, p_jerk*v_ref: %.3f (min: %.3f, mac: %.3f), p_jerk*v_opt " + "(to " + "check): %.3f, gamma: %.3f\n", + i, getVx(input), getVx(forward_filtered), getAx(forward_filtered), getVx(backward_filtered), + getAx(backward_filtered), getVx(filtered), getAx(filtered), getVx(output), getAx(output), + p_jerk.at(i), jerk_converted.at(i), j_min, j_max, jerk_ideal.at(i), gamma); + } + printf("\n"); + } + + 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 +{ + 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_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 +{ + 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 +{ + 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 && current_vel <= getVx(forward_filtered, i) && + i < merged.size() - 1) { + merged.at(i).longitudinal_velocity_mps = current_vel; + merged.at(i).acceleration_mps2 = current_acc; + + const double ds = + autoware_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; + } + ++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; +} + +boost::optional JerkFilteredSmoother::resampleTrajectory( + const TrajectoryPoints & input, const double /*v_current*/, const int closest_id) const +{ + return resampling::resampleTrajectory( + input, closest_id, base_param_.resample_param, smoother_param_.jerk_filter_ds); +} + +} // namespace motion_velocity_smoother diff --git a/planning/motion_velocity_smoother/src/smoother/l2_pseudo_jerk_smoother.cpp b/planning/motion_velocity_smoother/src/smoother/l2_pseudo_jerk_smoother.cpp new file mode 100644 index 0000000000000..2e7bafe1bbfa6 --- /dev/null +++ b/planning/motion_velocity_smoother/src/smoother/l2_pseudo_jerk_smoother.cpp @@ -0,0 +1,224 @@ +// 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 "motion_velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp" + +#include "eigen3/Eigen/Core" +#include "motion_velocity_smoother/trajectory_utils.hpp" + +#include +#include +#include +#include + +namespace motion_velocity_smoother +{ +L2PseudoJerkSmoother::L2PseudoJerkSmoother(const Param & smoother_param) +: smoother_param_{smoother_param} +{ + qp_solver_.updateMaxIter(4000); + qp_solver_.updateRhoInterval(0); // 0 means automatic + qp_solver_.updateEpsRel(1.0e-4); // def: 1.0e-4 + qp_solver_.updateEpsAbs(1.0e-4); // def: 1.0e-4 + qp_solver_.updateVerbose(false); +} + +void L2PseudoJerkSmoother::setParam(const Param & smoother_param) +{ + smoother_param_ = smoother_param; +} + +bool L2PseudoJerkSmoother::apply( + const double initial_vel, const double initial_acc, const TrajectoryPoints & input, + TrajectoryPoints & output, std::vector & debug_trajectories) +{ + debug_trajectories.clear(); + + const auto ts = std::chrono::system_clock::now(); + + output = input; + + if (std::fabs(input.front().longitudinal_velocity_mps) < 0.1) { + RCLCPP_DEBUG(logger_, "closest v_max < 0.1. assume vehicle stopped. return."); + return false; + } + + const unsigned int N = input.size(); + + if (N < 2) { + RCLCPP_WARN(logger_, "trajectory length is not enough."); + return false; + } + + const std::vector interval_dist_arr = + trajectory_utils::calcTrajectoryIntervalDistance(input); + + std::vector v_max(N, 0.0); + for (unsigned int i = 0; i < N; ++i) { + v_max.at(i) = input.at(i).longitudinal_velocity_mps; + } + /* + * x = [b0, b1, ..., bN, | a0, a1, ..., aN, | delta0, delta1, ..., deltaN, | sigma0, sigma1, ..., + * sigmaN] in R^{4N} b: velocity^2 a: acceleration delta: 0 < bi < v_max^2 + delta sigma: a_min < + * ai - sigma < a_max + */ + + const uint32_t l_variables = 4 * N; + const uint32_t l_constraints = 3 * N + 1; + + Eigen::MatrixXd A = Eigen::MatrixXd::Zero( + l_constraints, l_variables); // the matrix size depends on constraint numbers. + + 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); + + const double a_max = base_param_.max_accel; + const double a_min = base_param_.min_decel; + const double smooth_weight = smoother_param_.pseudo_jerk_weight; + const double over_v_weight = smoother_param_.over_v_weight; + const double over_a_weight = smoother_param_.over_a_weight; + + // design objective function + for (unsigned int i = 0; i < N; ++i) { // bi + q[i] = -1.0; // |v_max^2 - b| -> minimize (-bi) + } + + // pseudo jerk: d(ai)/ds -> minimize weight * (a1 - a0)^2 + for (unsigned int i = N; i < 2 * N - 1; ++i) { + unsigned int j = i - N; + const double w_x_ds_inv = smooth_weight * (1.0 / std::max(interval_dist_arr.at(j), 0.0001)); + P(i, i) += w_x_ds_inv * w_x_ds_inv; + P(i, i + 1) -= w_x_ds_inv * w_x_ds_inv; + P(i + 1, i) -= w_x_ds_inv * w_x_ds_inv; + P(i + 1, i + 1) += w_x_ds_inv * w_x_ds_inv; + } + + for (unsigned int i = 2 * N; i < 3 * N; ++i) { // over velocity cost + P(i, i) += over_v_weight; + } + + for (unsigned int i = 3 * N; i < 4 * N; ++i) { // over acceleration cost + P(i, i) += over_a_weight; + } + + /* design constraint matrix + 0 < b - delta < v_max^2 + 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. + */ + for (unsigned int i = 0; i < N; ++i) { + const int j = 2 * N + i; + A(i, i) = 1.0; // b_i + A(i, j) = -1.0; // -delta_i + upper_bound[i] = v_max[i] * v_max[i]; + lower_bound[i] = 0.0; + } + + // a_min < a - sigma < a_max + for (unsigned int i = N; i < 2 * N; ++i) { + const int j = 2 * N + i; + A(i, i) = 1.0; // a_i + A(i, j) = -1.0; // -sigma_i + if (i != N && v_max[i - N] < std::numeric_limits::epsilon()) { + upper_bound[i] = 0.0; + lower_bound[i] = 0.0; + } else { + upper_bound[i] = a_max; + lower_bound[i] = a_min; + } + } + + // b' = 2a ... (b(i+1) - b(i)) / ds = 2a(i) + for (unsigned int i = 2 * N; i < 3 * N - 1; ++i) { + const unsigned int j = i - 2 * N; + const double ds_inv = 1.0 / std::max(interval_dist_arr.at(j), 0.0001); + A(i, j) = -ds_inv; // b(i) + A(i, j + 1) = ds_inv; // b(i+1) + A(i, j + N) = -2.0; // a(i) + upper_bound[i] = 0.0; + lower_bound[i] = 0.0; + } + + // initial condition + const double v0 = initial_vel; + { + const unsigned int i = 3 * N - 1; + A(i, 0) = 1.0; // b0 + upper_bound[i] = v0 * v0; + lower_bound[i] = v0 * v0; + + A(i + 1, N) = 1.0; // a0 + upper_bound[i + 1] = initial_acc; + lower_bound[i + 1] = initial_acc; + } + + const auto tf1 = std::chrono::system_clock::now(); + const double dt_ms1 = + std::chrono::duration_cast(tf1 - ts).count() * 1.0e-6; + + // execute optimization + const auto ts2 = std::chrono::system_clock::now(); + const auto result = qp_solver_.optimize(P, A, q, lower_bound, upper_bound); + + // [b0, b1, ..., bN, | a0, a1, ..., aN, | + // delta0, delta1, ..., deltaN, | sigma0, sigma1, ..., sigmaN] + const std::vector optval = std::get<0>(result); + + for (unsigned int i = 0; i < N; ++i) { + double v = optval.at(i); + // std::cout << "[smoother] v[" << i << "] : " << std::sqrt(std::max(v, 0.0)) << + // ", v_max[" << i << "] : " << v_max[i] << std::endl; + output.at(i).longitudinal_velocity_mps = std::sqrt(std::max(v, 0.0)); + output.at(i).acceleration_mps2 = optval.at(i + N); + } + for (unsigned int i = N; i < output.size(); ++i) { + output.at(i).longitudinal_velocity_mps = 0.0; + output.at(i).acceleration_mps2 = 0.0; + } + + // -- to check the all optimization variables -- + // ROS_DEBUG("[after optimize Linf] idx, vel, acc, over_vel, over_acc "); + // for (unsigned int i = 0; i < N; ++i) { + // ROS_DEBUG( + // "i = %d, v: %f, v_max: %f a: %f, b: %f, delta: %f, sigma: %f\n", + // i, std::sqrt(optval.at(i)), + // v_max[i], optval.at(i + N), optval.at(i), optval.at(i + 2 * N), optval.at(i + 3 * N)); + // } + + const int status_val = std::get<3>(result); + if (status_val != 1) { + RCLCPP_WARN(logger_, "optimization failed : %s", qp_solver_.getStatusMessage().c_str()); + } + + const auto tf2 = std::chrono::system_clock::now(); + const double dt_ms2 = + std::chrono::duration_cast(tf2 - ts2).count() * 1.0e-6; + RCLCPP_DEBUG(logger_, "init time = %f [ms], optimization time = %f [ms]", dt_ms1, dt_ms2); + + return true; +} + +boost::optional L2PseudoJerkSmoother::resampleTrajectory( + const TrajectoryPoints & input, const double v_current, const int closest_id) const +{ + return resampling::resampleTrajectory(input, v_current, closest_id, base_param_.resample_param); +} + +} // namespace motion_velocity_smoother diff --git a/planning/motion_velocity_smoother/src/smoother/linf_pseudo_jerk_smoother.cpp b/planning/motion_velocity_smoother/src/smoother/linf_pseudo_jerk_smoother.cpp new file mode 100644 index 0000000000000..fceb9432787f7 --- /dev/null +++ b/planning/motion_velocity_smoother/src/smoother/linf_pseudo_jerk_smoother.cpp @@ -0,0 +1,236 @@ +// 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 "motion_velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp" + +#include "eigen3/Eigen/Core" +#include "motion_velocity_smoother/trajectory_utils.hpp" + +#include +#include +#include +#include + +namespace motion_velocity_smoother +{ +LinfPseudoJerkSmoother::LinfPseudoJerkSmoother(const Param & smoother_param) +: smoother_param_(smoother_param) +{ + qp_solver_.updateMaxIter(20000); + qp_solver_.updateRhoInterval(5000); + qp_solver_.updateEpsRel(1.0e-4); // def: 1.0e-4 + qp_solver_.updateEpsAbs(1.0e-8); // def: 1.0e-4 + qp_solver_.updateVerbose(false); +} + +void LinfPseudoJerkSmoother::setParam(const Param & smoother_param) +{ + smoother_param_ = smoother_param; +} + +bool LinfPseudoJerkSmoother::apply( + const double initial_vel, const double initial_acc, const TrajectoryPoints & input, + TrajectoryPoints & output, std::vector & debug_trajectories) +{ + debug_trajectories.clear(); + + const auto ts = std::chrono::system_clock::now(); + + output = input; + + if (std::fabs(input.front().longitudinal_velocity_mps) < 0.1) { + RCLCPP_DEBUG( + logger_, + "closest v_max < 0.1, keep stopping. " + "return."); + return false; + } + + const size_t N{input.size()}; + + if (N < 2) { + return false; + } + + std::vector interval_dist_arr = trajectory_utils::calcTrajectoryIntervalDistance(input); + + std::vector v_max(N, 0.0); + for (size_t i = 0; i < N; ++i) { + v_max.at(i) = input.at(i).longitudinal_velocity_mps; + } + + /* + * x = [b0, b1, ..., bN, | a0, a1, ..., aN, | delta0, delta1, ..., deltaN, | sigma0, sigma1, ..., + * sigmaN, | psi] in R^{4N+1} b: velocity^2 a: acceleration delta: 0 < bi < v_max^2 + delta sigma: + * a_min < ai - sigma < a_max psi: a'*curr_v - psi < 0, - a'*curr_v - psi < 0 (<=> |a'|*curr_v < + * psi) + */ + const size_t l_variables{4 * N + 1}; + const size_t l_constraints{3 * N + 1 + 2 * (N - 1)}; + + Eigen::MatrixXd A = Eigen::MatrixXd::Zero( + l_constraints, l_variables); // the matrix size depends on constraint numbers. + + 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); + + const double a_max{base_param_.max_accel}; + const double a_min{base_param_.min_decel}; + const double smooth_weight{smoother_param_.pseudo_jerk_weight}; + const double over_v_weight{smoother_param_.over_v_weight}; + const double over_a_weight{smoother_param_.over_a_weight}; + + // design objective function + for (unsigned int i = 0; i < N; ++i) { // bi + q[i] = -1.0; // |v_max^2 - b| -> minimize (-bi) + } + + for (unsigned int i = 2 * N; i < 3 * N; ++i) { // over velocity cost + P(i, i) += over_v_weight; + } + + for (unsigned int i = 3 * N; i < 4 * N; ++i) { // over acceleration cost + P(i, i) += over_a_weight; + } + + // pseudo jerk (Linf): minimize psi, subject to |a'|*curr_v < psi + q[4 * N] = smooth_weight; + + /* design constraint matrix + 0 < b - delta < v_max^2 + 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. + */ + for (unsigned int i = 0; i < N; ++i) { + const int j = 2 * N + i; + A(i, i) = 1.0; // b_i + A(i, j) = -1.0; // -delta_i + upper_bound[i] = v_max[i] * v_max[i]; + lower_bound[i] = 0.0; + } + + // a_min < a - sigma < a_max + for (unsigned int i = N; i < 2 * N; ++i) { + const int j = 2 * N + i; + A(i, i) = 1.0; // a_i + A(i, j) = -1.0; // -sigma_i + if (i != N && v_max[i - N] < std::numeric_limits::epsilon()) { + upper_bound[i] = 0.0; + lower_bound[i] = 0.0; + } else { + upper_bound[i] = a_max; + lower_bound[i] = a_min; + } + } + + // b' = 2a + for (unsigned int i = 2 * N; i < 3 * N - 1; ++i) { + const unsigned int j = i - 2 * N; + const double ds_inv = 1.0 / std::max(interval_dist_arr.at(j), 0.0001); + A(i, j) = -ds_inv; + A(i, j + 1) = ds_inv; + A(i, j + N) = -2.0; + upper_bound[i] = 0.0; + lower_bound[i] = 0.0; + } + + // initial condition + const double v0 = initial_vel; + { + const unsigned int i = 3 * N - 1; + A(i, 0) = 1.0; // b0 + upper_bound[i] = v0 * v0; + lower_bound[i] = v0 * v0; + + A(i + 1, N) = 1.0; // a0 + upper_bound[i + 1] = initial_acc; + lower_bound[i + 1] = initial_acc; + } + + // constraint for slack variable (a[i+1] - a[i] <= psi[i], a[i] - a[i+1] <= psi[i]) + for (unsigned int i = 3 * N + 1; i < 4 * N; ++i) { + const unsigned int ia = i - (3 * N + 1) + N; + const unsigned int ip = 4 * N; + const unsigned int j = i - (3 * N + 1); + const double ds_inv = 1.0 / std::max(interval_dist_arr.at(j), 0.0001); + + A(i, ia) = -ds_inv; + A(i, ia + 1) = ds_inv; + A(i, ip) = -1; + lower_bound[i] = -OSQP_INFTY; + upper_bound[i] = 0; + + A(i + N - 1, ia) = ds_inv; + A(i + N - 1, ia + 1) = -ds_inv; + A(i + N - 1, ip) = -1; + lower_bound[i + N - 1] = -OSQP_INFTY; + upper_bound[i + N - 1] = 0; + } + + const auto tf1 = std::chrono::system_clock::now(); + const double dt_ms1 = + std::chrono::duration_cast(tf1 - ts).count() * 1.0e-6; + + // execute optimization + const auto ts2 = std::chrono::system_clock::now(); + const auto result = qp_solver_.optimize(P, A, q, lower_bound, upper_bound); + + // [b0, b1, ..., bN, | a0, a1, ..., aN, | + // delta0, delta1, ..., deltaN, | sigma0, sigma1, ..., sigmaN] + const std::vector optval = std::get<0>(result); + + /* get velocity & acceleration */ + for (unsigned int i = 0; i < N; ++i) { + double v = optval.at(i); + output.at(i).longitudinal_velocity_mps = std::sqrt(std::max(v, 0.0)); + output.at(i).acceleration_mps2 = optval.at(i + N); + } + for (unsigned int i = N; i < output.size(); ++i) { + output.at(i).longitudinal_velocity_mps = 0.0; + output.at(i).acceleration_mps2 = 0.0; + } + + // -- to check the all optimization variables -- + // ROS_DEBUG("[after optimize Linf] idx, vel, acc, over_vel, over_acc "); + // for (unsigned int i = 0; i < N; ++i) { + // ROS_DEBUG( + // "i = %d, v: %f, v_max: %f a: %f, b: %f, delta: %f, sigma: %f", i, std::sqrt(optval.at(i)), + // v_max[i], optval.at(i + N), optval.at(i), optval.at(i + 2 * N), optval.at(i + 3 * N)); + // } + + const int status_val = std::get<3>(result); + if (status_val != 1) { + RCLCPP_WARN(logger_, "optimization failed : %s", qp_solver_.getStatusMessage().c_str()); + } + + const auto tf2 = std::chrono::system_clock::now(); + const double dt_ms2 = + std::chrono::duration_cast(tf2 - ts2).count() * 1.0e-6; + RCLCPP_DEBUG(logger_, "init time = %f [ms], optimization time = %f [ms]", dt_ms1, dt_ms2); + return true; +} + +boost::optional LinfPseudoJerkSmoother::resampleTrajectory( + const TrajectoryPoints & input, const double v_current, const int closest_id) const +{ + return resampling::resampleTrajectory(input, v_current, closest_id, base_param_.resample_param); +} + +} // namespace motion_velocity_smoother diff --git a/planning/motion_velocity_smoother/src/smoother/smoother_base.cpp b/planning/motion_velocity_smoother/src/smoother/smoother_base.cpp new file mode 100644 index 0000000000000..adaaa71d68a09 --- /dev/null +++ b/planning/motion_velocity_smoother/src/smoother/smoother_base.cpp @@ -0,0 +1,96 @@ +// 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 "motion_velocity_smoother/smoother/smoother_base.hpp" + +#include "motion_velocity_smoother/resample.hpp" +#include "motion_velocity_smoother/trajectory_utils.hpp" + +#include +#include +#include + +namespace motion_velocity_smoother +{ +void SmootherBase::setParam(const BaseParam & param) { base_param_ = 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; } + +boost::optional SmootherBase::applyLateralAccelerationFilter( + const TrajectoryPoints & input) const +{ + if (input.empty()) { + return boost::none; + } + + if (input.size() < 3) { + return boost::optional(input); // cannot calculate lateral acc. do nothing. + } + + // Interpolate with constant interval distance for lateral acceleration calculation. + constexpr double points_interval = 0.1; // [m] + std::vector out_arclength; + const std::vector in_arclength = trajectory_utils::calcArclengthArray(input); + for (double s = 0; s < in_arclength.back(); s += points_interval) { + out_arclength.push_back(s); + } + auto output = trajectory_utils::applyLinearInterpolation(in_arclength, input, out_arclength); + if (!output) { + RCLCPP_WARN( + rclcpp::get_logger("smoother").get_child("smoother_base"), + "interpolation failed at lateral acceleration filter."); + return boost::none; + } + output->back() = input.back(); // keep the final speed. + + constexpr double curvature_calc_dist = 5.0; // [m] calc curvature with 5m away points + const size_t idx_dist = + static_cast(std::max(static_cast((curvature_calc_dist) / points_interval), 1)); + + // Calculate curvature assuming the trajectory points interval is constant + const auto curvature_v = trajectory_utils::calcTrajectoryCurvatureFrom3Points(*output, idx_dist); + if (!curvature_v) { + return boost::optional(input); + } + + // 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); + + for (size_t i = 0; i < output->size(); ++i) { + double curvature = 0.0; + const size_t start = i > before_decel_index ? i - before_decel_index : 0; + const size_t end = std::min(output->size(), i + after_decel_index); + for (size_t j = start; j < end; ++j) { + 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 (output->at(i).longitudinal_velocity_mps > v_curvature_max) { + output->at(i).longitudinal_velocity_mps = v_curvature_max; + } + } + return output; +} + +} // namespace motion_velocity_smoother diff --git a/planning/motion_velocity_smoother/src/trajectory_utils.cpp b/planning/motion_velocity_smoother/src/trajectory_utils.cpp new file mode 100644 index 0000000000000..8f0618c39b574 --- /dev/null +++ b/planning/motion_velocity_smoother/src/trajectory_utils.cpp @@ -0,0 +1,643 @@ +// 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 "motion_velocity_smoother/trajectory_utils.hpp" + +#include "interpolation/spline_interpolation.hpp" +#include "motion_velocity_smoother/linear_interpolation.hpp" + +#include +#include +#include +#include +#include +#include + +namespace motion_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_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) +{ + 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; + } + + const size_t segment_idx = + autoware_utils::findNearestSegmentIndex(trajectory, target_pose.position); + + auto v1 = getTransVector3(trajectory.at(segment_idx).pose, trajectory.at(segment_idx + 1).pose); + auto v2 = getTransVector3(trajectory.at(segment_idx).pose, target_pose); + // calc internal proportion + const double prop{std::max(0.0, std::min(1.0, v1.dot(v2) / v1.length2()))}; + + auto interpolate = [&prop](double x1, double x2) { return prop * x1 + (1.0 - prop) * x2; }; + + { + const auto & seg_pt = trajectory.at(segment_idx); + const auto & next_pt = trajectory.at(segment_idx + 1); + traj_p.longitudinal_velocity_mps = + interpolate(next_pt.longitudinal_velocity_mps, seg_pt.longitudinal_velocity_mps); + traj_p.acceleration_mps2 = interpolate(next_pt.acceleration_mps2, seg_pt.acceleration_mps2); + traj_p.pose.position.x = interpolate(next_pt.pose.position.x, seg_pt.pose.position.x); + traj_p.pose.position.y = interpolate(next_pt.pose.position.y, seg_pt.pose.position.y); + traj_p.pose.position.z = interpolate(next_pt.pose.position.z, seg_pt.pose.position.z); + } + + return traj_p; +} + +boost::optional 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_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_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 boost::optional(extracted_traj); +} + +double calcArcLength(const TrajectoryPoints & path, const int idx1, const int idx2) +{ + if (idx1 == idx2) { // zero distance + return 0.0; + } + + if ( + idx1 < 0 || idx2 < 0 || static_cast(path.size()) - 1 < idx1 || + static_cast(path.size()) - 1 < idx2) { + RCLCPP_ERROR( + rclcpp::get_logger("motion_velocity_smoother").get_child("trajectory_utils"), + "invalid index"); + return 0.0; + } + + const int idx_from = std::min(idx1, idx2); + const int idx_to = std::max(idx1, idx2); + double dist_sum = 0.0; + for (int i = idx_from; i < idx_to; ++i) { + dist_sum += autoware_utils::calcDistance2d(path.at(i), path.at(i + 1)); + } + return dist_sum; +} + +std::vector calcArclengthArray(const TrajectoryPoints & trajectory) +{ + std::vector arclength; + double dist = 0.0; + arclength.clear(); + arclength.push_back(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_utils::calcDistance2d(tp.pose, tp_prev.pose); + arclength.push_back(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_utils::calcDistance2d(tp.pose, tp_prev.pose); + intervals.push_back(dist); + } + return intervals; +} + +boost::optional> calcTrajectoryCurvatureFrom3Points( + const TrajectoryPoints & trajectory, const size_t & idx_dist) +{ + std::vector k_arr; + if (trajectory.size() < 2 * idx_dist + 1) { + RCLCPP_DEBUG( + rclcpp::get_logger("motion_velocity_smoother").get_child("trajectory_utils"), + "cannot calc curvature idx_dist = %lu, trajectory.size() = %lu", idx_dist, trajectory.size()); + return {}; + } + + // calculate curvature by circle fitting from three points + Point p1{}; + Point p2{}; + Point p3{}; + for (size_t i = idx_dist; i < trajectory.size() - idx_dist; ++i) { + p1.x = trajectory.at(i - idx_dist).pose.position.x; + p2.x = trajectory.at(i).pose.position.x; + p3.x = trajectory.at(i + idx_dist).pose.position.x; + p1.y = trajectory.at(i - idx_dist).pose.position.y; + p2.y = trajectory.at(i).pose.position.y; + p3.y = trajectory.at(i + idx_dist).pose.position.y; + double den = std::max( + autoware_utils::calcDistance2d(p1, p2) * autoware_utils::calcDistance2d(p2, p3) * + autoware_utils::calcDistance2d(p3, p1), + 0.0001); + double curvature = 2.0 * ((p2.x - p1.x) * (p3.y - p1.y) - (p2.y - p1.y) * (p3.x - p1.x)) / den; + k_arr.push_back(curvature); + } + + // for debug + if (k_arr.empty()) { + RCLCPP_ERROR( + rclcpp::get_logger("motion_velocity_smoother").get_child("trajectory_utils"), + "k_arr.size() = 0, something wrong. pls check."); + return {}; + } + + // first and last curvature is copied from next value + for (size_t i = 0; i < idx_dist; ++i) { + k_arr.insert(k_arr.begin(), k_arr.front()); + k_arr.push_back(k_arr.back()); + } + return boost::optional>(k_arr); +} + +void setZeroVelocity(TrajectoryPoints & trajectory) +{ + for (auto & tp : trajectory) { + tp.longitudinal_velocity_mps = 0.0; + } +} + +double getMaxVelocity(const TrajectoryPoints & trajectory) +{ + double max_vel = 0.0; + for (auto & tp : trajectory) { + if (tp.longitudinal_velocity_mps > max_vel) { + max_vel = tp.longitudinal_velocity_mps; + } + } + return max_vel; +} + +double getMaxAbsVelocity(const TrajectoryPoints & trajectory) +{ + double max_vel = 0.0; + for (auto & tp : trajectory) { + double abs_vel = std::fabs(tp.longitudinal_velocity_mps); + if (abs_vel > max_vel) { + max_vel = abs_vel; + } + } + return max_vel; +} + +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; + } + } +} + +boost::optional applyLinearInterpolation( + const std::vector & base_index, const TrajectoryPoints & base_trajectory, + const std::vector & out_index, const bool use_spline_for_pose) +{ + std::vector px, py, pz, pyaw, tlx, taz, alx; + for (const auto & p : base_trajectory) { + px.push_back(p.pose.position.x); + py.push_back(p.pose.position.y); + pz.push_back(p.pose.position.z); + pyaw.push_back(tf2::getYaw(p.pose.orientation)); + tlx.push_back(p.longitudinal_velocity_mps); + taz.push_back(p.heading_rate_rps); + alx.push_back(p.acceleration_mps2); + } + + convertEulerAngleToMonotonic(pyaw); + + boost::optional> px_p, py_p, pz_p, pyaw_p; + if (use_spline_for_pose) { + px_p = interpolation::slerp(base_index, px, out_index); + py_p = interpolation::slerp(base_index, py, out_index); + pz_p = interpolation::slerp(base_index, pz, out_index); + pyaw_p = interpolation::slerp(base_index, pyaw, out_index); + } else { + px_p = linear_interpolation::interpolate(base_index, px, out_index); + py_p = linear_interpolation::interpolate(base_index, py, out_index); + pz_p = linear_interpolation::interpolate(base_index, pz, out_index); + pyaw_p = linear_interpolation::interpolate(base_index, pyaw, out_index); + } + const auto tlx_p = linear_interpolation::interpolate(base_index, tlx, out_index); + const auto taz_p = linear_interpolation::interpolate(base_index, taz, out_index); + const auto alx_p = linear_interpolation::interpolate(base_index, alx, out_index); + + if (!px_p || !py_p || !pz_p || !pyaw_p || !tlx_p || !taz_p || !alx_p) { + RCLCPP_WARN( + rclcpp::get_logger("motion_velocity_smoother").get_child("trajectory_utils"), + "interpolation error!!"); + return {}; + } + + TrajectoryPoints out_trajectory; + TrajectoryPoint point; + for (unsigned int i = 0; i < out_index.size(); ++i) { + point.pose.position.x = px_p->at(i); + point.pose.position.y = py_p->at(i); + point.pose.position.z = pz_p->at(i); + point.pose.orientation = autoware_utils::createQuaternionFromYaw(pyaw_p->at(i)); + + point.longitudinal_velocity_mps = tlx_p->at(i); + point.heading_rate_rps = taz_p->at(i); + point.acceleration_mps2 = alx_p->at(i); + out_trajectory.push_back(point); + } + return boost::optional(out_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) +{ + 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; + std::tuple state; + + 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("motion_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("motion_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("motion_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("motion_velocity_smoother").get_child("trajectory_utils"), + "valid check error! a_target = " << a_target << ", a_end = " << a_end); + return false; + } + return true; +} + +boost::optional applyDecelFilterWithJerkConstraint( + const TrajectoryPoints & input, const size_t start_index, const double v0, const double a0, + const double min_acc, const double decel_target_vel, + const std::map & jerk_profile) +{ + double t_total{0.0}; + for (auto & it : jerk_profile) { + t_total += it.second; + } + + std::vector ts; + std::vector xs; + std::vector vs; + std::vector as; + std::vector js; + const double dt{0.1}; + double x{0.0}; + double v{0.0}; + double a{0.0}; + double j{0.0}; + + for (double t = 0.0; t < t_total; t += dt) { + // Calculate state = (x, v, a, j) at t + const auto state = updateStateWithJerkConstraint(v0, a0, jerk_profile, t_total); + if (!state) { + return {}; + } + std::tie(x, v, a, j) = *state; + if (v > 0.0) { + a = std::max(a, min_acc); + ts.push_back(t); + xs.push_back(x); + vs.push_back(v); + as.push_back(a); + js.push_back(j); + } + } + // Calculate state = (x, v, a, j) at t_total + const auto state = updateStateWithJerkConstraint(v0, a0, jerk_profile, t_total); + if (!state) { + return {}; + } + std::tie(x, v, a, j) = *state; + if (v > 0.0 && !xs.empty() && xs.back() < x) { + a = std::max(a, min_acc); + ts.push_back(t_total); + xs.push_back(x); + vs.push_back(v); + as.push_back(a); + js.push_back(j); + } + + const double a_target{0.0}; + const double v_margin{0.3}; + const double a_margin{0.1}; + if (!isValidStopDist(v, a, decel_target_vel, a_target, v_margin, a_margin)) { + RCLCPP_WARN_STREAM( + rclcpp::get_logger("motion_velocity_smoother").get_child("trajectory_utils"), + "validation check error"); + return {}; + } + + TrajectoryPoints output_trajectory{input}; + + if (xs.empty()) { + output_trajectory.at(start_index).longitudinal_velocity_mps = decel_target_vel; + output_trajectory.at(start_index).acceleration_mps2 = 0.0; + for (unsigned int i = start_index + 1; i < output_trajectory.size(); ++i) { + output_trajectory.at(i).longitudinal_velocity_mps = decel_target_vel; + output_trajectory.at(i).acceleration_mps2 = 0.0; + } + return output_trajectory; + } + + const std::vector distance_all = calcArclengthArray(output_trajectory); + const auto it_end = std::find_if( + distance_all.begin(), distance_all.end(), [&xs](double x) { return x > xs.back(); }); + const std::vector distance{distance_all.begin() + start_index, it_end}; + + const auto vel_at_wp = linear_interpolation::interpolate(xs, vs, distance); + const auto acc_at_wp = linear_interpolation::interpolate(xs, as, distance); + + if (!vel_at_wp || !acc_at_wp) { + RCLCPP_WARN_STREAM( + rclcpp::get_logger("motion_velocity_smoother").get_child("trajectory_utils"), + "interpolation error"); + return {}; + } + + for (unsigned int i = 0; i < vel_at_wp->size(); ++i) { + output_trajectory.at(start_index + i).longitudinal_velocity_mps = vel_at_wp->at(i); + output_trajectory.at(start_index + i).acceleration_mps2 = acc_at_wp->at(i); + } + for (unsigned int i = start_index + vel_at_wp->size(); i < output_trajectory.size(); ++i) { + output_trajectory.at(i).longitudinal_velocity_mps = decel_target_vel; + output_trajectory.at(i).acceleration_mps2 = 0.0; + } + + return output_trajectory; +} + +boost::optional> updateStateWithJerkConstraint( + const double v0, const double a0, const std::map & jerk_profile, const double t) +{ + // constexpr double eps = 1.0E-05; + double j{0.0}; + double a{a0}; + double v{v0}; + double x{0.0}; + double t_sum{0.0}; + + for (auto & it : jerk_profile) { + 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); + const auto state = std::make_tuple(x, v, a, j); + return boost::optional>(state); + } + } + + RCLCPP_WARN_STREAM( + rclcpp::get_logger("motion_velocity_smoother").get_child("trajectory_utils"), + "Invalid jerk profile"); + return {}; +} + +} // namespace trajectory_utils +} // namespace motion_velocity_smoother