Skip to content

Commit 2356788

Browse files
tkimura4wep21rej55purewater0901TakaHoribe
authored
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 <tomoya.kimura@tier4.jp> * fix stop point error * Fix linear interpolation and external velocity calculation * Remove debug code * Rename BaseParam * Remove unused func and fix misc * Fix package.xml, include, apply formatting * Fix external velocity limit * add debug calculation time * modify calculation time debugger * modify calculation time publisher * rescale the calculation time * Fix some problem * Update planning/scenario_planning/common/motion_velocity_smoother/launch/motion_velocity_smoother.launch Co-authored-by: tkimura4 <tomoya.kimura@tier4.jp> Co-authored-by: yutaka <purewater0901@gmail.com> Co-authored-by: tkimura4 <tomoya.kimura@tier4.jp> * Add Resample Procedure after the optimization in motion_velocity_smoother (#1530) * do resample after the optimization * modify resample timing * add 0 at the end of the resampeld output trajectory * Manage parameters * Fix format * Fix format * Devide resample function in other file * Update default_motion_velocity_smoother.yaml * Update default_motion_velocity_smoother.yaml Co-authored-by: Fumiya Watanabe <rej55.g@gmail.com> * Output debug trajectories in motion velocity smoother (#1533) * add debug trajectory * add rqt_multiplot setting files * add calculation tiem visualization * guard emtpy output (#1537) * Motion velocity smoother doc (#1563) * Fix doc * Add english doc (tmp) * Add english file * Remove unused files * Fix * Fix typo * Modify 1-size trajectory handling & warning messages (#1540) * add new guard to delete unnecessary message * change message * add warning messages * chnage message to warning * chnage to throttle * Fix jerk filter calculation (#1593) * modify jerk filter calculation * remove unnecessary code * change the way of interp. of pose in smoother (#1600) * Update smoother resampling (#1595) * add new guard to delete unnecessary message * change message * add warning messages * chnage message to warning * chnage to throttle * modify jerk filter calculation * modify jerk filter calculation * separate resampling code * add new parameter * add new parameter to dynamic reconfigure * remove unnecessary code * add sampling before optimization * add const and blocker * change access to at * fix terminal length value * add comments * clean code * add comment * Fix/insert backward on reverse trajectory (#1602) * remove unused code Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com> * move debug code Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com> * fix insert backward on reverse trajectory Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com> * Porting motion velocity smoother to ros2 Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * Fix launch and fix warning * pre-commit fixes * Remove unused argument * Comment out unused parameter Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * Add distance threshold in motion_velocity_smoother (#1659) * Add distance threshold * Remove parameters and use default value * Fix for pre-commit Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * Support Non-zero Stop Point Acceleration (#1651) * Support non-zero stop point acceleration * modify zero velocity id search function (#1690) * modify zero velocity id search function * Fix resample function and error handling * Add comment Co-authored-by: Fumiya Watanabe <rej55.g@gmail.com> * Apply lint Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * pre-commit fixes Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> Co-authored-by: Fumiya Watanabe <rej55.g@gmail.com> Co-authored-by: yutaka <purewater0901@gmail.com> Co-authored-by: tkimura4 <tomoya.kimura@tier4.jp> Co-authored-by: purewater0901 <43805014+purewater0901@users.noreply.github.com> Co-authored-by: Takamasa Horibe <horibe.takamasa@gmail.com> Co-authored-by: pre-commit <pre-commit@example.com> * 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 <rej55.g@gmail.com> * Feature/port analytical smoother (#1896) * Port analytical jerk constrained smoother Signed-off-by: Makoto Kurihara <mkuri8m@gmail.com> * Fix parameters format Signed-off-by: Makoto Kurihara <mkuri8m@gmail.com> * Resolve warnings and remove unnecessary processing Signed-off-by: Makoto Kurihara <mkuri8m@gmail.com> * Change default parameters Signed-off-by: Makoto Kurihara <mkuri8m@gmail.com> * Change slash to dot for declare_parameter Signed-off-by: Makoto Kurihara <mkuri8m@gmail.com> * Fix debug messages Signed-off-by: Makoto Kurihara <mkuri8m@gmail.com> * Change pragma once to ifndef and endif Signed-off-by: Makoto Kurihara <mkuri8m@gmail.com> * Add #include vector, utility and string Signed-off-by: Makoto Kurihara <mkuri8m@gmail.com> * Remove extra white spaces Signed-off-by: Makoto Kurihara <mkuri8m@gmail.com> * Fix acceleration calculation at type 3 Signed-off-by: Makoto Kurihara <mkuri8m@gmail.com> * Apply clang-format Signed-off-by: Makoto Kurihara <mkuri8m@gmail.com> * Change copyright format Signed-off-by: Makoto Kurihara <mkuri8m@gmail.com> * Fix to be less than 100 characters per line Signed-off-by: Makoto Kurihara <mkuri8m@gmail.com> * Implement and remove TODO Signed-off-by: Makoto Kurihara <mkuri8m@gmail.com> * Add include to src Signed-off-by: Makoto Kurihara <mkuri8m@gmail.com> * Use static_cast Signed-off-by: Makoto Kurihara <mkuri8m@gmail.com> * Follow uncrustify Signed-off-by: Makoto Kurihara <mkuri8m@gmail.com> * Disable uncrustify at include guard Signed-off-by: Makoto Kurihara <mkuri8m@gmail.com> * Disable uncrustify at include Signed-off-by: Makoto Kurihara <mkuri8m@gmail.com> * Follow uncrustify Signed-off-by: Makoto Kurihara <mkuri8m@gmail.com> * Fix include order error Signed-off-by: Makoto Kurihara <mkuri8m@gmail.com> * Fix typo Signed-off-by: Makoto Kurihara <mkuri8m@gmail.com> * Change index type to size_t Signed-off-by: Makoto Kurihara <mkuri8m@gmail.com> * Fix size_t Signed-off-by: Makoto Kurihara <mkuri8m@gmail.com> * Add const Signed-off-by: Makoto Kurihara <mkuri8m@gmail.com> * Fix parameter namespace signs from slash to dot and add latacc one Signed-off-by: Makoto Kurihara <mkuri8m@gmail.com> * 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 <horibe.takamasa@gmail.com> * fix format Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com> * [motion_velocity_smoother] fix trajectory size guard condition (#2297) * change condition for input trajectory validation chack Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com> * organize guard code into function Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com> * 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 <kenji.miyake@tier4.jp> * Remove ament_cmake_uncrustify and ament_clang_format Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Apply Black Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Apply clang-format Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Fix build errors Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Fix for cpplint * Fix include double quotes to angle brackets Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Apply clang-format Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Fix build errors Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Add COLCON_IGNORE (#500) Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * 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 <takayuki.murooka@tier4.jp> * 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 <takayuki.murooka@tier4.jp> * 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 <border_goldenmarket@yahoo.co.jp> * Fix build error for trajectory follower nodes Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * [#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 <horibe.takamasa@gmail.com> * [#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 <horibe.takamasa@gmail.com> * move directory Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com> * remote control/trajectory_follower level dorectpry Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com> * remove .iv trajectory follower Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com> * use .auto trajectory_follower Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com> * remove .iv simple_planning_simulator & osqp_interface Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com> * use .iv simple_planning_simulator & osqp_interface Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com> * add tmp_autoware_auto_dependencies Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com> * tmporally add autoware_auto_msgs Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com> * apply .auto message split Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com> * fix build depend Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com> * 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 <maxime.clement@tier4.jp> Co-authored-by: Joshua Whitley <josh.whitley@autoware.org> Co-authored-by: Igor Bogoslavskyi <igor.bogoslavskyi@gmail.com> Co-authored-by: MIURA Yasuyuki <kokosabu@gmail.com> Co-authored-by: wep21 <border_goldenmarket@yahoo.co.jp> Co-authored-by: tomoya.kimura <tomoya.kimura@tier4.jp> * [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 <horibe.takamasa@gmail.com> * port trajectory visualizer (#682) Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com> * add header for debug trajectories (#681) Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com> * 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 <ttatcoder@outlook.jp> * fix topic name (#2266) Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com> * 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 <mkuri8m@gmail.com> * [motion_velocity_smoother] ignore debug print (#2292) * cosmetic change Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com> * cahnge severity from WARN to DEBUG for debug info Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com> * use util for stop_watch Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com> * 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 <kmiya@outlook.com> * Update perception/object_recognition/prediction/map_based_prediction/Readme.md Co-authored-by: Kazuki Miyahara <kmiya@outlook.com> * Update perception/object_recognition/prediction/map_based_prediction/Readme.md Co-authored-by: Kazuki Miyahara <kmiya@outlook.com> * Update Readme.md * Update perception/object_recognition/prediction/map_based_prediction/Readme.md Co-authored-by: Kazuki Miyahara <kmiya@outlook.com> Co-authored-by: tkimura4 <tomoya.kimura@tier4.jp> Co-authored-by: Kazuki Miyahara <kmiya@outlook.com> * remove failure condition for 0 velocity trajectory (#2295) Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com> * [mpc_follower] remove stop distance condition from stopState decision (#1916) * [mpc_follower] remove stop distance condition from stopState decision Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com> * add invalid index handling Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com> * 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 <yukky.saito@gmail.com> * update license Co-authored-by: Yukihiro Saito <yukky.saito@gmail.com> * update license Co-authored-by: Yukihiro Saito <yukky.saito@gmail.com> * update license Co-authored-by: Yukihiro Saito <yukky.saito@gmail.com> * 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 <yukky.saito@gmail.com> * sync rc rc/v0.23.0 (#2281) * Fix side shift planner (#2171) (#2172) * add print debug Signed-off-by: TakaHoribe <horibe.takamasa@gmail.com> * remove forward shift points when adding new point Signed-off-by: TakaHoribe <horibe.takamasa@gmail.com> * remove debug print Signed-off-by: TakaHoribe <horibe.takamasa@gmail.com> * format Signed-off-by: TakaHoribe <horibe.takamasa@gmail.com> * Fix remove threshold Co-authored-by: Fumiya Watanabe <rej55.g@gmail.com> Co-authored-by: Takamasa Horibe <horibe.takamasa@gmail.com> * 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 <horibe.takamasa@gmail.com> Co-authored-by: Takamasa Horibe <horibe.takamasa@gmail.com> Co-authored-by: Takamasa Horibe <horibe.takamasa@gmail.com> * [hotfix] 1 path point exception after resampling (#2204) * fix 1 path point exception after resampling Signed-off-by: TakaHoribe <horibe.takamasa@gmail.com> * Apply suggestions from code review * Apply suggestions from code review Co-authored-by: Takamasa Horibe <horibe.takamasa@gmail.com> Co-authored-by: tkimura4 <tomoya.kimura@tier4.jp> * [hotfix] Fix lane ids (#2211) * Fix lane ids * Prevent acceleration on avoidance (#2214) * prevent acceleration on avoidance Signed-off-by: TakaHoribe <horibe.takamasa@gmail.com> * fix param name Signed-off-by: TakaHoribe <horibe.takamasa@gmail.com> * parametrize avoidance acc Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com> * change param name Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com> * fix typo Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com> * 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 <border_goldenmarket@yahoo.co.jp> * 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 <rej55.g@gmail.com> Co-authored-by: Takamasa Horibe <horibe.takamasa@gmail.com> Co-authored-by: tkimura4 <tomoya.kimura@tier4.jp> 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 <yukky.saito@gmail.com> 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 <horibe.takamasa@gmail.com> * change param for trimSimilarGradShiftPoint Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com> * add comment for issue Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com> * update comment Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com> * replace code with function (logic has not changed) Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com> * move func to cpp Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com> * add comment for issue Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com> * fix typo Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com> * Update planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp Co-authored-by: Kazuki Miyahara <kmiya@outlook.com> * Update planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp Co-authored-by: Kazuki Miyahara <kmiya@outlook.com> Co-authored-by: Kazuki Miyahara <kmiya@outlook.com> Co-authored-by: Takamasa Horibe <horibe.takamasa@gmail.com> Co-authored-by: Kazuki Miyahara <kmiya@outlook.com> * 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 <border_goldenmarket@yahoo.co.jp> * Remove unused function Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * Fix arc length Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * Seperate time margin Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * Fix parameter name Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * Update Readme Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * Address review: Add comment for TimeDistanceArray Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * Run pre-commit Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * Fix cpplint Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * Add return for empty polygon Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * 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 <kmiya@outlook.com> * 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 <kmiya@outlook.com> * 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 <horibe.takamasa@gmail.com> * fix yaml format to fix pre-commit check Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com> * fix yaml format Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com> * apply clang-format Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com> * apply clang-format Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com> * 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 <horibe.takamasa@gmail.com> Co-authored-by: Takamasa Horibe <horibe.takamasa@gmail.com> * Add keep braking function at driving state (#2346) * Add keep braking function at driving state Signed-off-by: Makoto Kurihara <mkuri8m@gmail.com> * Remove debug messages Signed-off-by: Makoto Kurihara <mkuri8m@gmail.com> * Fix format Signed-off-by: Makoto Kurihara <mkuri8m@gmail.com> * 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 <mkuri8m@gmail.com> * add time offset param to point cloud concatenation (#2303) * add offset param * clang-format Co-authored-by: Akihito OHSATO <aohsato@gmail.com> * Feature/add doc for keep braking function at driving state (#2366) * Add the description of brake keeping Signed-off-by: Makoto Kurihara <mkuri8m@gmail.com> * Add the english document Signed-off-by: Makoto Kurihara <mkuri8m@gmail.com> * Improve description Signed-off-by: Makoto Kurihara <mkuri8m@gmail.com> * Add english description Signed-off-by: Makoto Kurihara <mkuri8m@gmail.com> * Fix include files (#2339) Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * 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 <border_goldenmarket@yahoo.co.jp> * fix test_trajectory.cpp * add target link libraries * Use .auto msg in test code for planniing evaluator Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * fix include Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> Co-authored-by: Takayuki Murooka <takayuki5168@gmail.com> 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 <ttatcoder@outlook.jp> Co-authored-by: Takamasa Horibe <horibe.takamasa@gmail.com> Co-authored-by: Takeshi Miura <57553950+1222-takeshi@users.noreply.github.com> Co-authored-by: Takeshi Ishita <ishitah.takeshi@gmail.com> Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Co-authored-by: Makoto Kurihara <mkuri8m@gmail.com> Co-authored-by: purewater0901 <43805014+purewater0901@users.noreply.github.com> Co-authored-by: tkimura4 <tomoya.kimura@tier4.jp> Co-authored-by: Kazuki Miyahara <kmiya@outlook.com> Co-authored-by: Yukihiro Saito <yukky.saito@gmail.com> Co-authored-by: Fumiya Watanabe <rej55.g@gmail.com> 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 <yukke42@users.noreply.github.com> 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 <aohsato@gmail.com> Co-authored-by: Kenji Miyake <kenji.miyake@tier4.jp> * fix osqp link (#700) * Update twist topic name (#736) Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * rename readme.ja * add default parameter file Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> Co-authored-by: Fumiya Watanabe <rej55.g@gmail.com> Co-authored-by: yutaka <purewater0901@gmail.com> Co-authored-by: purewater0901 <43805014+purewater0901@users.noreply.github.com> Co-authored-by: Takamasa Horibe <horibe.takamasa@gmail.com> Co-authored-by: pre-commit <pre-commit@example.com> Co-authored-by: td12321 <85976747+td12321@users.noreply.github.com> Co-authored-by: Makoto Kurihara <mkuri8m@gmail.com> Co-authored-by: Takayuki Murooka <takayuki5168@gmail.com> 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 <takayuki.murooka@tier4.jp> Co-authored-by: Maxime CLEMENT <maxime.clement@tier4.jp> Co-authored-by: Joshua Whitley <josh.whitley@autoware.org> Co-authored-by: Igor Bogoslavskyi <igor.bogoslavskyi@gmail.com> Co-authored-by: MIURA Yasuyuki <kokosabu@gmail.com> Co-authored-by: wep21 <border_goldenmarket@yahoo.co.jp> 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 <ttatcoder@outlook.jp> Co-authored-by: Takeshi Miura <57553950+1222-takeshi@users.noreply.github.com> Co-authored-by: Takeshi Ishita <ishitah.takeshi@gmail.com> Co-authored-by: Kazuki Miyahara <kmiya@outlook.com> Co-authored-by: Yukihiro Saito <yukky.saito@gmail.com> 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 <yukke42@users.noreply.github.com> 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 <aohsato@gmail.com> Co-authored-by: Kenji Miyake <kenji.miyake@tier4.jp> Co-authored-by: Kosuke Murakami <kosuke.murakami@tier4.jp>
1 parent 30346d3 commit 2356788

35 files changed

+6927
-0
lines changed
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,69 @@
1+
cmake_minimum_required(VERSION 3.5)
2+
project(motion_velocity_smoother)
3+
4+
if(NOT CMAKE_CXX_STANDARD)
5+
set(CMAKE_CXX_STANDARD 14)
6+
set(CMAKE_CXX_STANDARD_REQUIRED ON)
7+
set(CMAKE_CXX_EXTENSIONS OFF)
8+
endif()
9+
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
10+
add_compile_options(-Wall -Wextra -Wpedantic)
11+
endif()
12+
13+
find_package(ament_cmake_auto REQUIRED)
14+
ament_auto_find_build_dependencies()
15+
16+
find_package(Boost REQUIRED)
17+
find_package(eigen3_cmake_module REQUIRED)
18+
find_package(Eigen3 REQUIRED)
19+
20+
21+
set(MOTION_VELOCITY_SMOOTHER_SRC
22+
src/motion_velocity_smoother_node.cpp
23+
src/trajectory_utils.cpp
24+
src/linear_interpolation.cpp
25+
src/resample.cpp
26+
)
27+
28+
set(SMOOTHER_SRC
29+
src/smoother/smoother_base.cpp
30+
src/smoother/l2_pseudo_jerk_smoother.cpp
31+
src/smoother/linf_pseudo_jerk_smoother.cpp
32+
src/smoother/jerk_filtered_smoother.cpp
33+
src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp
34+
src/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.cpp
35+
)
36+
37+
ament_auto_add_library(smoother SHARED
38+
${SMOOTHER_SRC}
39+
)
40+
41+
ament_auto_add_library(motion_velocity_smoother_node SHARED
42+
${MOTION_VELOCITY_SMOOTHER_SRC}
43+
)
44+
45+
target_link_libraries(motion_velocity_smoother_node
46+
smoother
47+
)
48+
49+
rclcpp_components_register_node(motion_velocity_smoother_node
50+
PLUGIN "motion_velocity_smoother::MotionVelocitySmootherNode"
51+
EXECUTABLE motion_velocity_smoother
52+
)
53+
54+
if(BUILD_TESTING)
55+
find_package(ament_lint_auto REQUIRED)
56+
ament_lint_auto_find_test_dependencies()
57+
endif()
58+
59+
ament_auto_package(
60+
INSTALL_TO_SHARE
61+
launch
62+
config
63+
)
64+
65+
install(PROGRAMS
66+
scripts/trajectory_visualizer.py
67+
scripts/closest_velocity_checker.py
68+
DESTINATION lib/${PROJECT_NAME}
69+
)

planning/motion_velocity_smoother/README.ja.md

+250
Large diffs are not rendered by default.

planning/motion_velocity_smoother/README.md

+256
Large diffs are not rendered by default.
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,25 @@
1+
/**:
2+
ros__parameters:
3+
resample:
4+
ds_resample: 0.1
5+
num_resample: 1
6+
delta_yaw_threshold: 0.785
7+
8+
latacc:
9+
enable_constant_velocity_while_turning: false
10+
constant_velocity_dist_threshold: 2.0
11+
12+
forward:
13+
max_acc: 1.0
14+
min_acc: -1.0
15+
max_jerk: 0.3
16+
min_jerk: -0.3
17+
kp: 0.3
18+
19+
backward:
20+
start_jerk: -0.1
21+
min_jerk_mild_stop: -0.3
22+
min_jerk: -1.5
23+
min_acc_mild_stop: -1.0
24+
min_acc: -2.5
25+
span_jerk: -0.01
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,7 @@
1+
/**:
2+
ros__parameters:
3+
jerk_weight: 10.0 # weight for "smoothness" cost for jerk
4+
over_v_weight: 100000.0 # weight for "over speed limit" cost
5+
over_a_weight: 5000.0 # weight for "over accel limit" cost
6+
over_j_weight: 2000.0 # weight for "over jerk limit" cost
7+
jerk_filter_ds: 0.1 # resampling ds for jerk filter
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,5 @@
1+
/**:
2+
ros__parameters:
3+
pseudo_jerk_weight: 100.0 # weight for "smoothness" cost
4+
over_v_weight: 100000.0 # weight for "over speed limit" cost
5+
over_a_weight: 1000.0 # weight for "over accel limit" cost
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,5 @@
1+
/**:
2+
ros__parameters:
3+
pseudo_jerk_weight: 200.0 # weight for "smoothness" cost
4+
over_v_weight: 100000.0 # weight for "over speed limit" cost
5+
over_a_weight: 5000.0 # weight for "over accel limit" cost
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,15 @@
1+
/**:
2+
ros__parameters:
3+
# constraints param for normal driving
4+
normal:
5+
min_acc: -0.5 # min deceleration [m/ss]
6+
max_acc: 1.0 # max acceleration [m/ss]
7+
min_jerk: -0.5 # min jerk [m/sss]
8+
max_jerk: 1.0 # max jerk [m/sss]
9+
10+
# constraints to be observed
11+
limit:
12+
min_acc: -2.5 # min deceleration limit [m/ss]
13+
max_acc: 1.0 # max acceleration limit [m/ss]
14+
min_jerk: -1.5 # min jerk limit [m/sss]
15+
max_jerk: 1.5 # max jerk limit [m/sss]
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,51 @@
1+
/**:
2+
ros__parameters:
3+
# motion state constraints
4+
max_velocity: 20.0 # max velocity limit [m/s]
5+
stop_decel: 0.0 # deceleration at a stop point[m/ss]
6+
7+
# external velocity limit parameter
8+
margin_to_insert_external_velocity_limit: 0.3 # margin distance to insert external velocity limit [m]
9+
10+
# curve parameters
11+
max_lateral_accel: 0.8 # max lateral acceleration limit [m/ss]
12+
min_curve_velocity: 2.74 # min velocity at lateral acceleration limit [m/ss]
13+
decel_distance_before_curve: 3.5 # slow speed distance before a curve for lateral acceleration limit
14+
decel_distance_after_curve: 2.0 # slow speed distance after a curve for lateral acceleration limit
15+
16+
# engage & replan parameters
17+
replan_vel_deviation: 5.53 # velocity deviation to replan initial velocity [m/s]
18+
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)
19+
engage_acceleration: 0.1 # engage acceleration [m/ss] (use this acceleration when engagement)
20+
engage_exit_ratio: 0.5 # exit engage sequence to normal velocity planning when the velocity exceeds engage_exit_ratio x engage_velocity.
21+
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]
22+
23+
# stop velocity
24+
stopping_velocity: 2.778 # change target velocity to this value before v=0 point [m/s]
25+
stopping_distance: 0.0 # distance for the stopping_velocity [m]. 0 means the stopping velocity is not applied.
26+
27+
# path extraction parameters
28+
extract_ahead_dist: 200.0 # forward trajectory distance used for planning [m]
29+
extract_behind_dist: 5.0 # backward trajectory distance used for planning [m]
30+
delta_yaw_threshold: 1.0472 # Allowed delta yaw between ego pose and trajectory pose [radian]
31+
32+
# resampling parameters for optimization
33+
max_trajectory_length: 200.0 # max trajectory length for resampling [m]
34+
min_trajectory_length: 150.0 # min trajectory length for resampling [m]
35+
resample_time: 5.0 # resample total time for dense sampling [s]
36+
dense_dt: 0.1 # resample time interval for dense sampling [s]
37+
dense_min_interval_distance: 0.1 # minimum points-interval length for dense sampling [m]
38+
sparse_dt: 0.5 # resample time interval for sparse sampling [s]
39+
sparse_min_interval_distance: 4.0 # minimum points-interval length for sparse sampling [m]
40+
41+
# resampling parameters for post process
42+
post_max_trajectory_length: 300.0 # max trajectory length for resampling [m]
43+
post_min_trajectory_length: 30.0 # min trajectory length for resampling [m]
44+
post_resample_time: 10.0 # resample total time for dense sampling [s]
45+
post_dense_dt: 0.1 # resample time interval for dense sampling [s]
46+
post_dense_min_interval_distance: 0.1 # minimum points-interval length for dense sampling [m]
47+
post_sparse_dt: 0.1 # resample time interval for sparse sampling [s]
48+
post_sparse_min_interval_distance: 1.0 # minimum points-interval length for sparse sampling [m]
49+
50+
# system
51+
over_stop_velocity_warn_thr: 1.389 # used to check if the optimization exceeds the input velocity on the stop point

0 commit comments

Comments
 (0)