From 357dfae6a6e3bf5caced5ac1b00cb7a09fed2876 Mon Sep 17 00:00:00 2001 From: Tomoya Kimura Date: Mon, 6 Dec 2021 18:41:32 +0900 Subject: [PATCH] feat: add obstacle_stop_planner packages (#93) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * release v0.4.0 * Feature/stop reason (#712) * add stop reason msg * add mock of stop resaon publisher * change namespace of stop reason * update stop reason msg * add toRosPoint * implement stop reason publisher of blind stop * implement stop reason publisher of crosswalk * implement stop reason publisher of intersection * implement stop reason publisher of stop line * implement stop reason publisher of trafficlight * implement stop reason publisher of detection area * fix bug * remove unnecessary process * add remained stop factor * clean code * fix bug * not punlish stop reason if array size is 0 * add stop reason to stuck object in intersection * add stop factor of obstacle stop planner * add stop reason of surround_obstacle checker * Apply review Signed-off-by: Kenji Miyake * fix message type * delete unused message from cmake * remove stopReasonStamped * change topic name of stop reasons Co-authored-by: Kenji Miyake * Fix/stop reason (#724) * input stop reason of traffic light * add comment * add empty traffic light handling * change calculation method of traffic light position * avoid 0 position output * add debug values of adaptive cruise control (#742) * add debug values of adaptive cruise control * add rqt config file * cosmetic change (#738) Signed-off-by: Yukihiro Saito * Fix/acc same pointcloud (#743) * add handling with same pointcloud * clean code * add obstacle_stop_planner.yaml (#766) * add stop_planner.yaml * change file name * add explanation of the parameters * fix wrong start index (#745) Signed-off-by: Yukihiro Saito * Feature/smooth adaptive cruise (#789) * change param * do not insert max velocity near the ego vehicle position * Change min_slow_down_vel to 3kmph (#796) Signed-off-by: Kenji Miyake * Remove duplicated param (#797) Signed-off-by: Kenji Miyake * Feature/lowpass adaptive cruise control (#802) * add lowpass filter * update xml * change the method of insert velocity * change parameter * change default lowpass gain in acc (#820) * change default deceleration in acc * change lowpass gain * Reduce calc cost of slow down (#826) * Cosmetic change Signed-off-by: wep21 * Fix rough search range Signed-off-by: wep21 * Add circle inside/outside judgement Signed-off-by: wep21 * Apply clang Signed-off-by: wep21 * fix uninitialized variables (#816) * Revert "Reduce calc cost of slow down (#826)" (#832) This reverts commit 845ab7b1b90f8817c8fcb12880b5af6f78c2b183. * Reduce calc cost stop planner (#833) * Reduce calc cost of slow down (#826) * Cosmetic change Signed-off-by: wep21 * Fix rough search range Signed-off-by: wep21 * Add circle inside/outside judgement Signed-off-by: wep21 * Apply clang Signed-off-by: wep21 * Parameterize step length Signed-off-by: wep21 * Fix search range Signed-off-by: wep21 * remove ROS1 packages temporarily Signed-off-by: mitsudome-r * Revert "remove ROS1 packages temporarily" This reverts commit dae63c141d17c2c7d0402ca2b9c5c1f008101631. Signed-off-by: mitsudome-r * add COLCON_IGNORE to ros1 packages Signed-off-by: mitsudome-r * Rename launch files to launch.xml (#28) * Port obstacle stop planner (#86) * Port to ROS2 Signed-off-by: Servando German Serrano * Use autoware_debug_msgs Signed-off-by: Servando German Serrano * Fix variable name Signed-off-by: Servando German Serrano * Fix launch files (#122) * [surround_obstacle_checker] add parameter and arguments to launch file Signed-off-by: mitsudome-r * [obstacle_stop_planner] modify launch file to remap trajectory from argument Signed-off-by: mitsudome-r * [obstacle_avoidance_planner] modify launch file to remap topics from arguments Signed-off-by: mitsudome-r * [motion_velocity_optimizer] modify launch file to enable remapping from argument Signed-off-by: mitsudome-r * Convert calls of Duration to Duration::from_seconds where appropriate (#131) * Adjust copyright notice on 532 out of 699 source files (#143) * Use quotes for includes where appropriate (#144) * Use quotes for includes where appropriate * Fix lint tests * Make tests pass hopefully * Run uncrustify on the entire Pilot.Auto codebase (#151) * Run uncrustify on the entire Pilot.Auto codebase * Exclude open PRs * Added linters to obstacle_stop_planner (#154) * Added linters to obstacle_stop_planner * Remove use of deprecated ConstPtr * Remove use of deprecated ::Ptr * Removed unused variables * Fix sign mismatches * Fix non-void returns * Fix cpplint error * Fix sign mismatching Co-authored-by: Jilada Eccleston * Ros2 v0.8.0 obstacle stop planner (#300) * rename before v0.8.0 update * Update default parameters of slow_down_planner (#846) Signed-off-by: Kenji Miyake * Fix missing break (#855) Signed-off-by: wep21 * fix slow down param (#871) * Fix typos in planning modules (#866) * fix typos in planning * fix corresponding typos in planning * revert fixed typos temporarily due to its impact on launchers * enable to change acc_param file from arg (#997) * Feature/refactor obstacle stop planner (#1000) * devide functions * add StopPoint and SlowDownPoint structure * apply clang-format 6.0 * Feature/update adaptive cruise control (#995) * avoid upper velocity chattering * add option to use rough velocity estimation * fix typo * apply format * fix comment * change min velocity by PID (#1008) * Feature/obstacle stop after goal margin (#957) * devide functions * add StopPoint and SlowDownPoint structure * apply clang-format 6.0 * add extend trajectory method * change extend_distance default value to 0.0 * does not insert when insert point over output_msg points size * fix acc limit (#1015) * Feature/compensate lidar delay to acc (#1032) * add compensation to calc dist * change compensation way * delete debug output * separate emerngecy stop param (#1033) * improve accuracy of insert stop pose (#1034) * add error handling (#1037) * expand stop param (#1068) * add param * add awapi topic Co-authored-by: tomoya.kimura * fix default param (#1071) * Add expand_stop_range to obstacle_stop_planner.yaml (#1084) Signed-off-by: Kenji Miyake * Revert "rename before v0.8.0 update" This reverts commit 395c529ee1a854db35ee4f4d517da2dfb401914c. * fix clock & restore include guard * fix code style * fix node * fix params * nest acc params Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> Co-authored-by: Taichi Higashide Co-authored-by: Kazuki Miyahara Co-authored-by: tkimura4 Co-authored-by: Yukihiro Saito * Ros2 v0.8.0 remove std msgs awapi (#348) * [autoware_vehicle_msgs] add BatteryStatus msg Signed-off-by: mitsudome-r * [autoware_planning_msgs] add ExpandStopRange and StopSpeedExceeded messages Signed-off-by: mitsudome-r * [autoware_api_msgs] add DoorControlCommand, StopCommand, and VelocityLimit messages Signed-off-by: mitsudome-r * remove std_msgs related to autoware_awaiv_adapter node Signed-off-by: mitsudome-r * apply ament_uncrustify Signed-off-by: mitsudome-r * fix build failure Signed-off-by: mitsudome-r * fix test failures Signed-off-by: mitsudome-r * address review commends Signed-off-by: mitsudome-r * Rename ROS-related .yaml to .param.yaml (#352) * Rename ROS-related .yaml to .param.yaml Signed-off-by: Kenji Miyake * Remove prefix 'default_' of yaml files Signed-off-by: Kenji Miyake * Rename vehicle_info.yaml to vehicle_info.param.yaml Signed-off-by: Kenji Miyake * Rename diagnostic_aggregator's param files Signed-off-by: Kenji Miyake * Fix overlooked parameters Signed-off-by: Kenji Miyake * fix insert slow down point bug (#1159) (#376) * fix slow down insert slow down point bug * refactor code Co-authored-by: Taichi Higashide * fix implement miss (#409) * Sensor data qos (#407) * Use sensor data qos for pointcloud preprocessor Signed-off-by: Autoware * Use sensor data qos for pointcloud Signed-off-by: Autoware * Fix lint Signed-off-by: wep21 * Use sensor data qos for livox tag filter and vector map filter Signed-off-by: wep21 * Fix lint Signed-off-by: wep21 Co-authored-by: Autoware * Ros2 fix topic name part1 (#408) * Fix topic name of lane_departure_checker debug Signed-off-by: Takagi, Isamu * Fix topic name of mpc_follower debug Signed-off-by: Takagi, Isamu * Fix topic name of velocity_controller debug Signed-off-by: Takagi, Isamu * Fix topic name of motion_velocity_optimizer debug Signed-off-by: Takagi, Isamu * Fix topic name of lane_change_planner debug Signed-off-by: Takagi, Isamu * Fix topic name of behavior_velocity_planner debug Signed-off-by: Takagi, Isamu * Fix topic name of obstacle_avoidance_planner debug Signed-off-by: Takagi, Isamu * Fix topic name of behavior_velocity_planner Signed-off-by: Takagi, Isamu * Fix topic name of motion_velocity_optimizer Signed-off-by: Takagi, Isamu * Fix topic name of lane_departure_checker Signed-off-by: Takagi, Isamu * Fix topic name of mpc_follower Signed-off-by: Takagi, Isamu * Fix topic name of behavior_velocity_planner Signed-off-by: Takagi, Isamu * Fix topic name of velocity_controller Signed-off-by: Takagi, Isamu * Fix topic name of lane_change_planner Signed-off-by: Takagi, Isamu * Fix topic name of obstacle_avoidance_planner Signed-off-by: Takagi, Isamu * Fix topic name of obstacle_stop_planner Signed-off-by: Takagi, Isamu * Fix topic name of costmap_generator Signed-off-by: Takagi, Isamu * Fix topic name of freespace_planner Signed-off-by: Takagi, Isamu * Fix topic name of surround_obstacle_checker Signed-off-by: Takagi, Isamu * Fix topic name of costmap_generator Signed-off-by: Takagi, Isamu * Fix topic name of emergency_handler Signed-off-by: Takagi, Isamu * Fix lint errors Signed-off-by: Takagi, Isamu * Fix typo Signed-off-by: Takagi, Isamu * add use_sim-time option (#454) * Fix rolling build errors (#1225) * Add missing include files Signed-off-by: Kenji Miyake * Replace rclcpp::Duration Signed-off-by: Kenji Miyake * Use reference for exceptions Signed-off-by: Kenji Miyake * Use from_seconds Signed-off-by: Kenji Miyake * Sync public repo (#1228) * [simple_planning_simulator] add readme (#424) * add readme of simple_planning_simulator Signed-off-by: Takamasa Horibe * Update simulator/simple_planning_simulator/README.md * set transit_margin_time to intersect. planner (#460) * Fix pose2twist (#462) Signed-off-by: Takagi, Isamu * Ros2 vehicle info param server (#447) * add vehicle_info_param_server * update vehicle info * apply format * fix bug * skip unnecessary search * delete vehicle param file * fix bug * Ros2 fix topic name part2 (#425) * Fix topic name of traffic_light_classifier Signed-off-by: Takagi, Isamu * Fix topic name of traffic_light_visualization Signed-off-by: Takagi, Isamu * Fix topic name of traffic_light_ssd_fine_detector Signed-off-by: Takagi, Isamu * Fix topic name of traffic_light_map_based_detector Signed-off-by: Takagi, Isamu * Fix lint traffic_light_recognition Signed-off-by: Takagi, Isamu * Fix lint traffic_light_ssd_fine_detector Signed-off-by: Takagi, Isamu * Fix lint traffic_light_classifier Signed-off-by: Takagi, Isamu * Fix lint traffic_light_classifier Signed-off-by: Takagi, Isamu * Fix lint traffic_light_ssd_fine_detector Signed-off-by: Takagi, Isamu * Fix issues in hdd_reader (#466) * Fix some issues detected by Coverity Scan and Clang-Tidy * Update launch command * Add more `close(new_sock)` * Simplify the definitions of struct * fix: re-construct laneletMapLayer for reindex RTree (#463) * Rviz overlay render fix (#461) * Moved painiting in SteeringAngle plugin to update() Signed-off-by: Adam Dabrowski * super class now back to MFD Signed-off-by: Adam Dabrowski * uncrustified Signed-off-by: Adam Dabrowski * acquire data in mutex Signed-off-by: Adam Dabrowski * back to RTD as superclass Signed-off-by: Adam Dabrowski * Rviz overlay render in update (#465) * Moved painiting in SteeringAngle plugin to update() Signed-off-by: Adam Dabrowski * super class now back to MFD Signed-off-by: Adam Dabrowski * uncrustified Signed-off-by: Adam Dabrowski * acquire data in mutex Signed-off-by: Adam Dabrowski * removed unnecessary includes and some dead code Signed-off-by: Adam Dabrowski * Adepted remaining vehicle plugin classes to render-in-update concept. Returned to MFD superclass Signed-off-by: Adam Dabrowski * restored RTD superclass Signed-off-by: Adam Dabrowski Co-authored-by: Takamasa Horibe Co-authored-by: tkimura4 Co-authored-by: Takagi, Isamu <43976882+isamu-takagi@users.noreply.github.com> Co-authored-by: Kazuki Miyahara Co-authored-by: Makoto Tokunaga Co-authored-by: Adam Dąbrowski * Unify Apache-2.0 license name (#1242) * Make planning modules components (#1263) Signed-off-by: wep21 * fix miss spell (#1268) * Remove use_sim_time for set_parameter (#1260) Signed-off-by: wep21 * Refactor vehicle info util (#1305) * Update license Signed-off-by: Kenji Miyake * Refactor vehicle_info_util Signed-off-by: Kenji Miyake * Rename and split files Signed-off-by: Kenji Miyake * Fix interfaces Signed-off-by: Kenji Miyake * Fix bug and add error handling Signed-off-by: Kenji Miyake * Add "// namespace" Signed-off-by: Kenji Miyake * Add missing include Signed-off-by: Kenji Miyake * add config file of plot juggler (#1328) * Add pre-commit (#1560) * add pre-commit * add pre-commit-config * add additional settings for private repository * use default pre-commit-config * update pre-commit setting * Ignore whitespace for line breaks in markdown * Update .github/workflows/pre-commit.yml Co-authored-by: Kazuki Miyahara * exclude svg * remove pretty-format-json * add double-quote-string-fixer * consider COLCON_IGNORE file when seaching modified package * format file * pre-commit fixes * Update pre-commit.yml * Update .pre-commit-config.yaml Co-authored-by: Kazuki Miyahara Co-authored-by: pre-commit Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> * Fix -Wunused-parameter (#1836) * Fix -Wunused-parameter Signed-off-by: Kenji Miyake * Fix mistake Signed-off-by: Kenji Miyake * fix spell * Fix lint issues Signed-off-by: Kenji Miyake * Ignore flake8 warnings Signed-off-by: Kenji Miyake Co-authored-by: Hiroki OTA * Fix compiler warnings (#1837) * Fix -Wunused-private-field Signed-off-by: Kenji Miyake * Fix -Wunused-variable Signed-off-by: Kenji Miyake * Fix -Wformat-security Signed-off-by: Kenji Miyake * Fix -Winvalid-constexpr Signed-off-by: Kenji Miyake * Fix -Wdelete-non-abstract-non-virtual-dtor Signed-off-by: Kenji Miyake * Fix -Wdelete-abstract-non-virtual-dtor Signed-off-by: Kenji Miyake * Fix -Winconsistent-missing-override Signed-off-by: Kenji Miyake * Fix -Wrange-loop-construct Signed-off-by: Kenji Miyake * Fix "invalid application of 'sizeof' to an incomplete type" Signed-off-by: Kenji Miyake * Ignore -Wgnu-anonymous-struct and -Wnested-anon-types Signed-off-by: Kenji Miyake * Fix lint Signed-off-by: Kenji Miyake * Ignore -Wno-deprecated-declarations in CUDA-related packages Signed-off-by: Kenji Miyake * Fix mistake Signed-off-by: Kenji Miyake * Fix -Wunused-parameter Signed-off-by: Kenji Miyake * suppress warnings for planning (#1893) * add Werror * add maybe_unused * fix estm -> estimate * fix some typos (#1941) * fix some typos * fix typo * Fix typo Signed-off-by: Kenji Miyake Co-authored-by: Kenji Miyake * fix calc_dist in acc (#1945) * fix calc_dist in acc * use baselink2front * remove unused variable * fix * add sort-package-xml hook in pre-commit (#1881) * add sort xml hook in pre-commit * change retval to exit_status * rename * add prettier plugin-xml * use early return * add license note * add tier4 license * restore prettier * change license order * move local hooks to public repo * move prettier-xml to pre-commit-hooks-ros * update version for bug-fix * apply pre-commit * Feature/changeable slow down margin (#1546) (#1726) * Feature/changeable slow down margin (#1546) * start slow down at SlowDownStart * add publisher for debug * update slow down planner implementation * delete unnecessary publisher * change function name * apply clang-format * refactoring * refactoring & apply clang-format * add virtual wall marker Co-authored-by: satoshi-ota * Fix typo Signed-off-by: wep21 * Apply uncrustify Signed-off-by: wep21 * Apply cpplint Signed-off-by: wep21 * Fix package.xml Signed-off-by: wep21 * Remove unused variable Signed-off-by: wep21 Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Co-authored-by: satoshi-ota * Fix/not appropriate velocity overwrite (#1750) * fix not appropriate velocity overwrite * apply format * cleanup/wrap planner params by using struct (#1788) * wrap planner params by using struct * move bg alias & add address operator * improve readability of loading parameters * remove unsuitable vehicle_info alias * remove unsuitable slow_down/stop planner param alias * add struct NodeParam * move using define * change param order Co-authored-by: satoshi-ota * improvement/replace several lines with autoware_utils func (#1783) * clean up * remove redundant blank line * LINE_STRIP -> LINE_LIST Co-authored-by: satoshi-ota * Refactor/obstacle stop planner (#1811) * use autoware_utils getRPY() * use point alias & rename variables * use TrajectoryPoint instead of Eigen::Vector2d * split insertSlowDownSection() into two part * use trajectory point alias * change variables order * fix build error * use std::numeric_limits::epsilon() & small change * add address operator * fix typo * apply format * update TrajectoryPoint insert condition * use std::hypot * change alias name * move using alias & remove unnesessary include * remove an unnecessary variable * improve readability * undo the insert min dist * remane variables & improve readability * fix redundant variables name * tmp : remove scope * remove unnecessary alias * rename alias to improve readability * p_end -> p_back * undo partial rename * undo calc order modification * undo split insertSlowDownSection() into two parts * prevent TrajectoryPoint overlapping * remove unnecessary file * undo logic modification * initialize struct * fix constraints * fix indent * improve struct initialize * remove redundant address operator * initialize TrajectoryPoint * guard invalid index * remove std::min Co-authored-by: satoshi-ota * Fix/obstacle search radius in obstacle stop planner (#1823) * guard activation both of slow down and stop by a collision obstacle * fix obstacle search radius bug * fix indent * rename variables * not deactivate slow down planning * initialize index * remove .swp Co-authored-by: satoshi-ota * set initial value of lowpass filter in acc (#1880) * set initial value of lowpass filter in acc * add space * Feature/add obstacle stop doc (#1890) * add base doc. * add svg files * add flow chart * add roles * fix svg * add discription of flowchart * add figure to readme * fix typo * add plantuml * fix typo * upper velocity -> target velocity * add svg * add more dicription of stop/slow down planner * add adaptive cruise document * fix typo * apply markdown lint * fix format * fix format * fix svg * fix typo * fix format * Update planning/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/README.md Co-authored-by: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com> * Update planning/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/README.md Co-authored-by: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com> * add adaptive cruise description * fix deceleration range * fix description of slow down planner Co-authored-by: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com> * add maybe unused (#1908) * prevent chattering in adaptive cruise (#1954) * fix typo * update readme * add description * add two threhold of obstacle velocity * fix calc_dist in acc (#1945) * fix calc_dist in acc * use baselink2front * remove unused variable * fix * remove unused arg (#1971) * improve/commonize calc insert point function (#1825) * add insert function * fix typo * remove unnecessary address operator * use common function * index small change * rename variables * fix loop condition * fix loop condition no use static_cast * remove .swp * update insert inplementation * fix dist_remain plus/minus * improve readability Co-authored-by: satoshi-ota * Fix package.xml (#2056) Signed-off-by: Kenji Miyake * fix obstacle stop (#2083) (#2086) Co-authored-by: tkimura4 * Feature/keep slow down speed until end point of slow down section (#1985) * modify logic: not escape slow down until slow down section end * refactor: add findNearestFrontIndex() * remove unused argument & function * improve readability * add utility func * modify logic condition * fix for createQuaternionFromRPY/Yaw (#2154) * fix unexpected slow down in sharp curves (#2181) * Fix/insert implementation (#2186) * Feature/consider jerk by using external velocity limit (#2158) * consider jerk and acc constraint in slow down * use external velocity limit * consider constraints * add publisher for debug * publish jerk/acc constraints * add undershoot measures * add plotjuggle config (slow down planner) * fix unexpected speed up * update undershoot guard * bool initialize * support new VelocityLimit.msg * smplify velocity limit logic * improve readability * clean up debug publisher * use planner data * add publishDebugData() * clean up trajectory preprocess * add bad index detection * split a huge process into two parts * fix typo(threshould -> threshold) * use limit selector * add comment * update plotjuggler config * update member variables Co-authored-by: satoshi-ota * Fix/use common planning param (#2246) * use common planning param * jerk_min_slow_down -> slow_down_min_jerk * fix velocity threshold (#2264) * Fix/trajectory preprocess (#2285) * fix preprocess * add index guard * 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 * Auto/obstacle stop planner (#531) * delete COLCON_IGNORE * port obstacle stop planner * run pre commit * add nav_msgs * type conversion * [apply_predicted_obj_type] adapt to autoware auto msgs (#564) * fix obj shape * fix obj shape * fix goal pose * rename topic name twist -> odometry (#568) 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 * Auto/fix obstacle stop planner (#678) * change trajectory to trajectory point in node * update * update * delete comment * fix invalidContainerReference * substitute header to output Co-authored-by: tomoya.kimura * mix readme * add common param Co-authored-by: mitsudome-r Co-authored-by: Kenji Miyake Co-authored-by: Yukihiro Saito Co-authored-by: Taichi Higashide Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> Co-authored-by: Kazuki Miyahara Co-authored-by: Nikolai Morin Co-authored-by: Servando <43142004+sgermanserrano@users.noreply.github.com> Co-authored-by: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> Co-authored-by: Esteve Fernandez Co-authored-by: Jilada Eccleston Co-authored-by: Shinnosuke Hirakawa <8327162+0x126@users.noreply.github.com> Co-authored-by: Autoware Co-authored-by: Takagi, Isamu <43976882+isamu-takagi@users.noreply.github.com> Co-authored-by: Takamasa Horibe Co-authored-by: Makoto Tokunaga Co-authored-by: Adam Dąbrowski Co-authored-by: Yusuke FUJII Co-authored-by: Keisuke Shima <19993104+KeisukeShima@users.noreply.github.com> Co-authored-by: pre-commit Co-authored-by: Hiroki OTA Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Co-authored-by: satoshi-ota Co-authored-by: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com> Co-authored-by: autoware-iv-sync-ci[bot] <87871706+autoware-iv-sync-ci[bot]@users.noreply.github.com> Co-authored-by: Takayuki Murooka Co-authored-by: Sugatyon <32741405+Sugatyon@users.noreply.github.com> Co-authored-by: Takayuki Murooka --- planning/obstacle_stop_planner/CMakeLists.txt | 51 + planning/obstacle_stop_planner/README.md | 282 ++++ .../config/adaptive_cruise_control.param.yaml | 40 + .../config/common.param.yaml | 15 + .../config/obstacle_stop_planner.param.yaml | 25 + .../config/plot_juggler_adaptive_cruise.xml | 133 ++ .../config/plot_juggler_slow_down.xml | 93 ++ .../config/rqt_multiplot_adaptive_cruise.xml | 669 ++++++++ .../docs/adaptive_cruise.drawio.svg | 181 ++ .../docs/insert_decel_velocity.drawio.svg | 269 +++ .../docs/insert_velocity.drawio.svg | 208 +++ .../docs/point_cloud.drawio.svg | 174 ++ .../docs/point_cloud_decel.drawio.svg | 175 ++ .../docs/vehicle_shape.drawio.svg | 225 +++ .../docs/vehicle_shape_decel.drawio.svg | 376 +++++ .../docs/velocity_limitation.drawio.svg | 270 +++ .../adaptive_cruise_control.hpp | 225 +++ .../obstacle_stop_planner/debug_marker.hpp | 133 ++ .../include/obstacle_stop_planner/node.hpp | 291 ++++ .../launch/obstacle_stop_planner.launch.xml | 33 + planning/obstacle_stop_planner/package.xml | 41 + .../src/adaptive_cruise_control.cpp | 696 ++++++++ .../src/debug_marker.cpp | 359 ++++ planning/obstacle_stop_planner/src/node.cpp | 1463 +++++++++++++++++ 24 files changed, 6427 insertions(+) create mode 100644 planning/obstacle_stop_planner/CMakeLists.txt create mode 100644 planning/obstacle_stop_planner/README.md create mode 100644 planning/obstacle_stop_planner/config/adaptive_cruise_control.param.yaml create mode 100644 planning/obstacle_stop_planner/config/common.param.yaml create mode 100644 planning/obstacle_stop_planner/config/obstacle_stop_planner.param.yaml create mode 100644 planning/obstacle_stop_planner/config/plot_juggler_adaptive_cruise.xml create mode 100644 planning/obstacle_stop_planner/config/plot_juggler_slow_down.xml create mode 100644 planning/obstacle_stop_planner/config/rqt_multiplot_adaptive_cruise.xml create mode 100644 planning/obstacle_stop_planner/docs/adaptive_cruise.drawio.svg create mode 100644 planning/obstacle_stop_planner/docs/insert_decel_velocity.drawio.svg create mode 100644 planning/obstacle_stop_planner/docs/insert_velocity.drawio.svg create mode 100644 planning/obstacle_stop_planner/docs/point_cloud.drawio.svg create mode 100644 planning/obstacle_stop_planner/docs/point_cloud_decel.drawio.svg create mode 100644 planning/obstacle_stop_planner/docs/vehicle_shape.drawio.svg create mode 100644 planning/obstacle_stop_planner/docs/vehicle_shape_decel.drawio.svg create mode 100644 planning/obstacle_stop_planner/docs/velocity_limitation.drawio.svg create mode 100644 planning/obstacle_stop_planner/include/obstacle_stop_planner/adaptive_cruise_control.hpp create mode 100644 planning/obstacle_stop_planner/include/obstacle_stop_planner/debug_marker.hpp create mode 100644 planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp create mode 100644 planning/obstacle_stop_planner/launch/obstacle_stop_planner.launch.xml create mode 100644 planning/obstacle_stop_planner/package.xml create mode 100644 planning/obstacle_stop_planner/src/adaptive_cruise_control.cpp create mode 100644 planning/obstacle_stop_planner/src/debug_marker.cpp create mode 100644 planning/obstacle_stop_planner/src/node.cpp diff --git a/planning/obstacle_stop_planner/CMakeLists.txt b/planning/obstacle_stop_planner/CMakeLists.txt new file mode 100644 index 0000000000000..d31efe651281f --- /dev/null +++ b/planning/obstacle_stop_planner/CMakeLists.txt @@ -0,0 +1,51 @@ +cmake_minimum_required(VERSION 3.5) +project(obstacle_stop_planner) + +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 -Werror) +endif() + +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +find_package(Eigen3 REQUIRED) +find_package(OpenCV REQUIRED) +find_package(PCL REQUIRED) + +ament_auto_add_library(obstacle_stop_planner SHARED + src/debug_marker.cpp + src/node.cpp + src/adaptive_cruise_control.cpp +) + +target_include_directories(obstacle_stop_planner + PUBLIC + ${OpenCV_INCLUDE_DIRS} + ${PCL_INCLUDE_DIRS} +) + +target_link_libraries(obstacle_stop_planner + ${OpenCV_LIBRARIES} + ${PCL_LIBRARIES} +) + +rclcpp_components_register_node(obstacle_stop_planner + PLUGIN "motion_planning::ObstacleStopPlannerNode" + EXECUTABLE obstacle_stop_planner_node +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_auto_package( + INSTALL_TO_SHARE + config + launch +) diff --git a/planning/obstacle_stop_planner/README.md b/planning/obstacle_stop_planner/README.md new file mode 100644 index 0000000000000..4d2502b7b7cc8 --- /dev/null +++ b/planning/obstacle_stop_planner/README.md @@ -0,0 +1,282 @@ +# Obstacle Stop Planner + +## Overview + +`obstacle_stop_planner` has following modules + +- Obstacle Stop Planner + - inserts a stop point in trajectory when there is a static point cloud on the trajectory. +- Slow Down Planner + - inserts a deceleration section in trajectory when there is a point cloud near the trajectory. +- Adaptive Cruise Controller (ACC) + - embeds target velocity in trajectory when there is a dynamic point cloud on the trajectory. + +When the stop point that has 0 velocity is inserted, the point is inserted in front of the target point cloud by the distance of `baselink to front` + `stop margin`. The `baselink to front` means the distance between `base_link`(center of rear-wheel axis) and front of the car. `stop margin` is determined by the parameters described below. + +![insert_stop_velocity](./docs/insert_velocity.drawio.svg) + +When the deceleration section is inserted, the start point of the section is inserted in front of the target point cloud by the distance of `baselink to front` + `slow down forward margin`. the end point of the section is inserted behind the target point cloud by the distance of `slow down backward margin` + `baselink to rear`. The `baselink to rear` means the distance between `base_link` and rear of the car. The velocities of points in the deceleration section are modified to the deceleration velocity. `slow down backward margin` and `slow down forward margin` are determined by the parameters described below. + +![insert_stop_velocity](./docs/insert_decel_velocity.drawio.svg) + +## Input topics + +| Name | Type | Description | +| --------------------------- | ----------------------------------------------- | ------------------- | +| `~/input/pointcloud` | sensor_msgs::PointCloud2 | obstacle pointcloud | +| `~/input/trajectory` | autoware_auto_planning_msgs::Trajectory | trajectory | +| `~/input/vector_map` | autoware_auto_mapping_msgs::HADMapBin | vector map | +| `~/input/odometry` | nav_msgs::Odometry | vehicle velocity | +| `~/input/dynamic_objects` | autoware_auto_perception_msgs::PredictedObjects | dynamic objects | +| `~/input/expand_stop_range` | autoware_planning_msgs::msg::ExpandStopRange | expand stop range | + +## Output topics + +| Name | Type | Description | +| ---------------------- | --------------------------------------- | -------------------------------------- | +| `~output/trajectory` | autoware_auto_planning_msgs::Trajectory | trajectory to be followed | +| `~output/stop_reasons` | autoware_planning_msgs::StopReasonArray | reasons that cause the vehicle to stop | + +## Modules + +### Obstacle Stop Planner + +#### Role + +`Obstacle Stop Planner` module inserts a stop point in trajectory when there is a static point cloud on the trajectory. This module does not work when `Adaptive Cruise Controller` works. + +| Parameter | Type | Description | +| --------------------------------------- | ------ | ----------------------------------------------------------------------------- | +| `stop_planner.stop_margin` | double | stop margin distance from obstacle on the path [m] | +| `stop_planner.min_behavior_stop_margin` | double | stop margin distance when any other stop point is inserted in stop margin [m] | +| `stop_planner.step_length` | double | step length for pointcloud search range [m] | +| `stop_planner.extend_distance` | double | extend trajectory to consider after goal obstacle in the extend_distance [m] | +| `stop_planner.expand_stop_range` | double | margin of vehicle footprint [m] | + +#### Flowchart + +```plantuml +@startuml +title insertStopPoint +start + + +:callback trajectory; + +:trim trajectory from self-pose; + +:decimate trajectory; + +:generate detection area; + +if ( search obstacle pointcloud in detection area?) then (yes(find)) +else (no(not find)) + stop +endif + +if ( insert target velocity by adaptive cruise module?) then (yes) + stop +else (no) +endif + +:insert stop point; + + +stop +@enduml +``` + +First, this module cut off the trajectory behind the car and decimates the points of trajectory for reducing computational costs. + +Then, a detection area is generated by the decimated trajectory as following figure. The detection area means the area through which the vehicle-body passes. + +![vehicle_shape](./docs/vehicle_shape.drawio.svg) + +The module searches the obstacle pointcloud within detection area. When the pointcloud is found, `Adaptive Cruise Controller` modules starts to work. only when `Adaptive Cruise Controller` modules does not insert target velocity, the stop point is inserted to the trajectory. The stop point means the point with 0 velocity. + +![pointcloud](./docs/point_cloud.drawio.svg) + +### Slow Down Planner + +#### Role + +`Slow Down Planner` module inserts a deceleration point in trajectory when there is a point cloud near the trajectory. + +| Parameter | Type | Description | +| --------------------------------------------- | ------ | ---------------------------------------------------------------------------------------------- | +| `slow_down_planner.slow_down_forward_margin` | double | margin distance from slow down point to vehicle front [m] | +| `slow_down_planner.slow_down_backward_margin` | double | margin distance from slow down point to vehicle rear [m] | +| `slow_down_planner.expand_slow_down_range` | double | offset from vehicle side edge for expanding the search area of the surrounding point cloud [m] | +| `slow_down_planner.max_slow_down_vel` | double | max slow down velocity [m/s] | +| `slow_down_planner.min_slow_down_vel` | double | min slow down velocity [m/s] | + +#### Flowchart + +```plantuml +@startuml +title insertDecelerationPoint +start + + +:callback trajectory; + +:trim trajectory from self-pose; + +:decimate trajectory; + +:generate detection area; + +if ( search obstacle pointcloud in detection area?) then (yes(find)) +else (no(not find)) + stop +endif + +:insert deceleration point; + + +stop +@enduml +``` + +First, this module cut off the trajectory behind the car and decimates the points of trajectory for reducing computational costs. ( This is the same process as that of `Obstacle Stop planner` module. ) + +Then, a detection area is generated by the decimated trajectory as following figure. The detection area in this module is the extended area of the detection area used in `Obstacle Stop Planner` module. The distance to be extended depends on the above parameter `expand_slow_down_range`. + +![vehicle_shape_decel](./docs/vehicle_shape_decel.drawio.svg) + +The module searches the obstacle pointcloud within detection area. When the pointcloud is found, the deceleration point is inserted to the trajectory. + +![pointcloud_decel](./docs/point_cloud_decel.drawio.svg) + +The deceleration point means the point with low velocity; the value of the velocity $v_{target}$ is determined as follows. + +$v_{target} = v_{min} + \frac{l_{ld} - l_{vw}/2}{l_{er}} (v_{max} - v_{min} )$ + +- $v_{min}$ is minimum target value of `Slow Down Planner` module. The value of $v_{min}$ depends on the parameter `min_slow_down_vel`. +- $v_{max}$ is maximum target value of `Slow Down Planner` module. The value of $v_{max}$ depends on the parameter `max_slow_down_vel`. +- $l_{ld}$ is the lateral deviation of the target pointcloud. +- $l_{vw}$ is the vehicle width. +- $l_{er}$ is the expand range of detection area. The value of $l_{er}$ depends on the parameter `expand_slow_down_range` + +The above method means that the smaller the lateral deviation of the pointcloud, the lower the velocity of the deceleration point. + + + + + +### Adaptive Cruise Controller + +#### Role + +`Adaptive Cruise Controller` module embeds maximum velocity in trajectory when there is a dynamic point cloud on the trajectory. The value of maximum velocity depends on the own velocity, the velocity of the point cloud ( = velocity of the front car), and the distance to the point cloud (= distance to the front car). + +| Parameter | Type | Description | +| ---------------------------------------------------------------- | ------ | --------------------------------------------------------------------------------------------------------------- | +| `adaptive_cruise_control.use_object_to_estimate_vel` | bool | use dynamic objects for estimating object velocity or not | +| `adaptive_cruise_control.use_pcl_to_estimate_vel` | bool | use raw pointclouds for estimating object velocity or not | +| `adaptive_cruise_control.consider_obj_velocity` | bool | consider forward vehicle velocity to calculate target velocity in adaptive cruise or not | +| `adaptive_cruise_control.obstacle_velocity_thresh_to_start_acc` | double | start adaptive cruise control when the velocity of the forward obstacle exceeds this value [m/s] | +| `adaptive_cruise_control.obstacle_velocity_thresh_to_stop_acc` | double | stop acc when the velocity of the forward obstacle falls below this value [m/s] | +| `adaptive_cruise_control.emergency_stop_acceleration` | double | supposed minimum acceleration (deceleration) in emergency stop [m/ss] | +| `adaptive_cruise_control.emergency_stop_idling_time` | double | supposed idling time to start emergency stop [s] | +| `adaptive_cruise_control.min_dist_stop` | double | minimum distance of emergency stop [m] | +| `adaptive_cruise_control.obstacle_emergency_stop_acceleration` | double | supposed minimum acceleration (deceleration) in emergency stop [m/ss] | +| `adaptive_cruise_control.max_standard_acceleration` | double | supposed maximum acceleration in active cruise control [m/ss] | +| `adaptive_cruise_control.min_standard_acceleration` | double | supposed minimum acceleration (deceleration) in active cruise control [m/ss] | +| `adaptive_cruise_control.standard_idling_time` | double | supposed idling time to react object in active cruise control [s] | +| `adaptive_cruise_control.min_dist_standard` | double | minimum distance in active cruise control [m] | +| `adaptive_cruise_control.obstacle_min_standard_acceleration` | double | supposed minimum acceleration of forward obstacle [m/ss] | +| `adaptive_cruise_control.margin_rate_to_change_vel` | double | rate of margin distance to insert target velocity [-] | +| `adaptive_cruise_control.use_time_compensation_to_calc_distance` | bool | use time-compensation to calculate distance to forward vehicle | +| `adaptive_cruise_control.p_coefficient_positive` | double | coefficient P in PID control (used when target dist -current_dist >=0) [-] | +| `adaptive_cruise_control.p_coefficient_negative` | double | coefficient P in PID control (used when target dist -current_dist <0) [-] | +| `adaptive_cruise_control.d_coefficient_positive` | double | coefficient D in PID control (used when delta_dist >=0) [-] | +| `adaptive_cruise_control.d_coefficient_negative` | double | coefficient D in PID control (used when delta_dist <0) [-] | +| `adaptive_cruise_control.object_polygon_length_margin` | double | The distance to extend the polygon length the object in pointcloud-object matching [m] | +| `adaptive_cruise_control.object_polygon_width_margin` | double | The distance to extend the polygon width the object in pointcloud-object matching [m] | +| `adaptive_cruise_control.valid_estimated_vel_diff_time` | double | Maximum time difference treated as continuous points in speed estimation using a point cloud [s] | +| `adaptive_cruise_control.valid_vel_que_time` | double | Time width of information used for speed estimation in speed estimation using a point cloud [s] | +| `adaptive_cruise_control.valid_estimated_vel_max` | double | Maximum value of valid speed estimation results in speed estimation using a point cloud [m/s] | +| `adaptive_cruise_control.valid_estimated_vel_min` | double | Minimum value of valid speed estimation results in speed estimation using a point cloud [m/s] | +| `adaptive_cruise_control.thresh_vel_to_stop` | double | Embed a stop line if the maximum speed calculated by ACC is lower than this speed [m/s] | +| `adaptive_cruise_control.lowpass_gain_of_upper_velocity` | double | Lowpass-gain of target velocity | +| `adaptive_cruise_control.use_rough_velocity_estimation:` | bool | Use rough estimated velocity if the velocity estimation is failed | +| `adaptive_cruise_control.rough_velocity_rate` | double | In the rough velocity estimation, the velocity of front car is estimated as self current velocity \* this value | + +#### Flowchart + +```plantuml +@startuml +title insertTargetVelocity +start + +:get obstacle pointcloud in detection area; + +if (estimate velocity of target pointcloud?) then (yes(success to estimate)) +else (no(fail to estimate)) + stop +endif + + +if (the velocity of the pointcloud is fast enough?) then (yes) +else (no) + stop +endif + +if (calculate distance to the pointcloud from self-position?) then (yes) +else (no) + stop +endif + +:calculate target velocity to insert; + +if (the target velocity is fast enough?) then (yes) +else (no) + stop +endif + +:embed target velocity; + +stop +@enduml +``` + +This module works only when the obstacle pointcloud is found in the detection area of the `Obstacle stop planner` module. +At first, the velocity of the pointcloud is estimated. The velocity estimation uses the velocity information of dynamic objects, or the distance to the point cloud found in the previous step. + +Only when the estimation is succeeded and the estimated velocity exceeds the value of `obstacle_stop_velocity_thresh_*`, the distance to the pointcloud from self-position is calculated. For prevent chattering in the mode transition, `obstacle_velocity_thresh_to_start_acc` is used for the threshold to start adaptive cruise, and `obstacle_velocity_thresh_to_stop_acc` is used for the threshold to stop adaptive cruise. When the calculated distance value exceeds the emergency distance $d\_{emergency}$ calculated by emergency_stop parameters, target velocity to insert is calculated. + +The emergency distance $d\_{emergency}$ is calculated as follows. + +$d_{emergency} = d_{margin_{emergency}} + t_{idling_{emergency}} \cdot v_{ego} + (-\frac{v_{ego}^2}{2 \cdot a_{ego_{emergency}}}) - (-\frac{v_{obj}^2}{2 \cdot a_{obj_{emergency}}})$ + +- $d_{margin_{emergency}}$ is a minimum margin to the obstacle pointcloud. The value of $d_{margin_{emergency}}$ depends on the parameter `min_dist_stop` +- $t_{idling_{emergency}}$ is a supposed idling time. The value of $t_{idling_{emergency}}$ depends on the parameter `emergency_stop_idling_time` +- $v_{ego}$ is a current velocity of own vehicle +- $a_{ego_{_{emergency}}}$ is a minimum acceleration (maximum deceleration) of own vehicle. The value of $a_{ego_{_{emergency}}}$ depends on the parameter `emergency_stop_acceleration` +- $v_{obj}$ is a current velocity of obstacle pointcloud. +- $a_{obj_{_{emergency}}}$ is a supposed minimum acceleration of obstacle pointcloud. The value of $a_{obj_{_{emergency}}}$ depends on the parameter `obstacle_emergency_stop_acceleration` +- \*Above $X_{_{emergency}}$ parameters are used only in emergency situation. + +The target velocity is determined to keep the distance to the obstacle pointcloud from own vehicle at the standard distance $d\_{standard}$ calculated as following. Therefore, if the distance +to the obstacle pointcloud is longer than standard distance, The target velocity becomes higher than the current velocity, and vice versa. For keeping the distance, a PID controller is used. + +$d_{standard} = d_{margin_{standard}} + t_{idling_{standard}} \cdot v_{ego} + (-\frac{v_{ego}^2}{2 \cdot a_{ego_{standard}}}) - (-\frac{v_{obj}^2}{2 \cdot a_{obj_{standard}}})$ + +- $d_{margin_{standard}}$ is a minimum margin to the obstacle pointcloud. The value of $d_{margin_{standard}}$ depends on the parameter `min_dist_stop` +- $t_{idling_{standard}}$ is a supposed idling time. The value of $t_{idling_{standard}}$ depends on the parameter `standard_stop_idling_time` +- $v_{ego}$ is a current velocity of own vehicle +- $a_{ego_{_{standard}}}$ is a minimum acceleration (maximum deceleration) of own vehicle. The value of $a_{ego_{_{standard}}}$ depends on the parameter `min_standard_acceleration` +- $v_{obj}$ is a current velocity of obstacle pointcloud. +- $a_{obj_{_{standard}}}$ is a supposed minimum acceleration of obstacle pointcloud. The value of $a_{obj_{_{standard}}}$ depends on the parameter `obstacle_min_standard_acceleration` +- \*Above $X_{_{standard}}$ parameters are used only in non-emergency situation. + +![adaptive_cruise](./docs/adaptive_cruise.drawio.svg) + +If the target velocity exceeds the value of `thresh_vel_to_stop`, the target velocity is embedded in the trajectory. + +## Known Limits + +- It is strongly depends on velocity planning module whether or not it moves according to the target speed embedded by `Adaptive Cruise Controller` module. If the velocity planning module is updated, please take care of the vehicle's behavior as much as possible and always be ready for overriding. + +- The velocity estimation algorithm in `Adaptive Cruise Controller` is depend on object tracking module. Please note that if the object-tracking fails or the tracking result is incorrect, it the possibility that the vehicle behaves dangerously. diff --git a/planning/obstacle_stop_planner/config/adaptive_cruise_control.param.yaml b/planning/obstacle_stop_planner/config/adaptive_cruise_control.param.yaml new file mode 100644 index 0000000000000..691be53a7470f --- /dev/null +++ b/planning/obstacle_stop_planner/config/adaptive_cruise_control.param.yaml @@ -0,0 +1,40 @@ +/**: + ros__parameters: + adaptive_cruise_control: + # Adaptive Cruise Control (ACC) config + use_object_to_estimate_vel: true # use tracking objects for estimating object velocity or not + use_pcl_to_estimate_vel: true # use pcl for estimating object velocity or not + consider_obj_velocity: true # consider forward vehicle velocity to ACC or not + + # general parameter for ACC + obstacle_velocity_thresh_to_start_acc: 1.5 # start adaptive cruise control when the velocity of the forward obstacle exceeds this value [m/s] + obstacle_velocity_thresh_to_stop_acc: 1.0 # stop adaptive cruise control when the velocity of the forward obstacle falls below this value [m/s] + emergency_stop_acceleration: -5.0 # supposed minimum acceleration (deceleration) in emergency stop [m/ss] + emergency_stop_idling_time: 0.5 # supposed idling time to start emergency stop [s] + min_dist_stop: 4.0 # minimum distance of emergency stop [m] + obstacle_emergency_stop_acceleration: -5.0 # supposed minimum acceleration (deceleration) in emergency stop [m/ss] + max_standard_acceleration: 0.5 # supposed maximum acceleration in active cruise control [m/ss] + min_standard_acceleration: -1.0 # supposed minimum acceleration (deceleration) in active cruise control + standard_idling_time: 0.5 # supposed idling time to react object in active cruise control [s] + min_dist_standard: 4.0 # minimum distance in active cruise control [m] + obstacle_min_standard_acceleration: -1.5 # supposed minimum acceleration of forward obstacle [m/ss] + margin_rate_to_change_vel: 0.3 # margin to insert upper velocity [-] + use_time_compensation_to_calc_distance: true # use time-compensation to calculate distance to forward vehicle + + # pid parameter for ACC + p_coefficient_positive: 0.1 # coefficient P in PID control (used when target dist -current_dist >=0) [-] + p_coefficient_negative: 0.3 # coefficient P in PID control (used when target dist -current_dist <0) [-] + d_coefficient_positive: 0.0 # coefficient D in PID control (used when delta_dist >=0) [-] + d_coefficient_negative: 0.2 # coefficient D in PID control (used when delta_dist <0) [-] + + # parameter for object velocity estimation + object_polygon_length_margin: 2.0 # The distance to extend the polygon length the object in pointcloud-object matching [m] + object_polygon_width_margin: 0.5 # The distance to extend the polygon width the object in pointcloud-object matching [m] + valid_estimated_vel_diff_time: 1.0 # Maximum time difference treated as continuous points in speed estimation using a point cloud [s] + valid_vel_que_time: 0.5 # Time width of information used for speed estimation in speed estimation using a point cloud [s] + valid_estimated_vel_max: 20.0 # Maximum value of valid speed estimation results in speed estimation using a point cloud [m/s] + valid_estimated_vel_min: -20.0 # Minimum value of valid speed estimation results in speed estimation using a point cloud [m/s] + thresh_vel_to_stop: 1.5 # Embed a stop line if the maximum speed calculated by ACC is lower than this speed [m/s] + lowpass_gain_of_upper_velocity: 0.75 # Lowpass-gain of upper velocity + use_rough_velocity_estimation: false # Use rough estimated velocity if the velocity estimation is failed (#### If this parameter is true, the vehicle may collide with the front car. Be careful. ####) + rough_velocity_rate: 0.9 # In the rough velocity estimation, the velocity of front car is estimated as self current velocity * this value diff --git a/planning/obstacle_stop_planner/config/common.param.yaml b/planning/obstacle_stop_planner/config/common.param.yaml new file mode 100644 index 0000000000000..a23570a5fc791 --- /dev/null +++ b/planning/obstacle_stop_planner/config/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/obstacle_stop_planner/config/obstacle_stop_planner.param.yaml b/planning/obstacle_stop_planner/config/obstacle_stop_planner.param.yaml new file mode 100644 index 0000000000000..b9bada482694b --- /dev/null +++ b/planning/obstacle_stop_planner/config/obstacle_stop_planner.param.yaml @@ -0,0 +1,25 @@ +/**: + ros__parameters: + stop_planner: + stop_margin: 5.0 # stop margin distance from obstacle on the path [m] + min_behavior_stop_margin: 2.0 # stop margin distance when any other stop point is inserted in stop margin [m] + step_length: 1.0 # step length for pointcloud search range [m] + extend_distance: 0.0 # extend trajectory to consider after goal obstacle in the extend_distance + expand_stop_range: 0.0 # margin of vehicle footprint [m] + + slow_down_planner: + # slow down planner parameters + forward_margin: 5.0 # margin distance from slow down point to vehicle front [m] + backward_margin: 5.0 # margin distance from slow down point to vehicle rear [m] + expand_slow_down_range: 1.0 # offset from vehicle side edge for expanding the search area of the surrounding point cloud [m] + max_slow_down_vel: 1.38 # max slow down velocity [m/s] + min_slow_down_vel: 0.28 # min slow down velocity [m/s] + + # slow down constraint parameters + consider_constraints: False # set "True", if no decel plan found under jerk/dec constrains, relax target slow down vel + forward_margin_min: 1.0 # min margin for relaxing slow down margin [m/s] + forward_margin_span: -0.1 # fineness param for relaxing slow down margin [m/s] + jerk_min_slow_down: -0.3 # min slow down jerk constraint [m/sss] + jerk_span: -0.01 # fineness param for planning deceleration jerk [m/sss] + jerk_start: -0.1 # init jerk used for deceleration planning [m/sss] + slow_down_vel: 1.38 # target slow down velocity [m/s] diff --git a/planning/obstacle_stop_planner/config/plot_juggler_adaptive_cruise.xml b/planning/obstacle_stop_planner/config/plot_juggler_adaptive_cruise.xml new file mode 100644 index 0000000000000..0c66f8aa47c8e --- /dev/null +++ b/planning/obstacle_stop_planner/config/plot_juggler_adaptive_cruise.xml @@ -0,0 +1,133 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/planning/obstacle_stop_planner/config/plot_juggler_slow_down.xml b/planning/obstacle_stop_planner/config/plot_juggler_slow_down.xml new file mode 100644 index 0000000000000..f4480787d9711 --- /dev/null +++ b/planning/obstacle_stop_planner/config/plot_juggler_slow_down.xml @@ -0,0 +1,93 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/planning/obstacle_stop_planner/config/rqt_multiplot_adaptive_cruise.xml b/planning/obstacle_stop_planner/config/rqt_multiplot_adaptive_cruise.xml new file mode 100644 index 0000000000000..b3c3b4945a555 --- /dev/null +++ b/planning/obstacle_stop_planner/config/rqt_multiplot_adaptive_cruise.xml @@ -0,0 +1,669 @@ + + + + #babdb6 + #000000 + false + false + + + + + + + Untitled Axis + 0 + true + + + Untitled Axis + 0 + true + + + + + + + + + 1 + + 1000 + 0 + 0 + -1000 + 0 + + /planning/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/debug_values + std_msgs/Float32MultiArray + + + data/0 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /planning/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/debug_values + std_msgs/Float32MultiArray + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 1000 + Estimated Object Velocity(from pcl) + + + + + + 1 + + 1000 + 0 + 0 + -1000 + 0 + + /planning/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/debug_values + std_msgs/Float32MultiArray + + + data/1 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /planning/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/debug_values + std_msgs/Float32MultiArray + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 1000 + Estimated Object Velocity(from dynamic object) + + + + + + 1 + + 1000 + 0 + 0 + -1000 + 0 + + /planning/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/debug_values + std_msgs/Float32MultiArray + + + data/2 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /planning/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/debug_values + std_msgs/Float32MultiArray + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 1000 + Estimated Object Velocity(final) + + + + true + + 30 + Estimated Object Velocity + + + + + + Untitled Axis + 0 + true + + + Untitled Axis + 0 + true + + + + + + + + + 1 + + 1000 + 0 + 0 + -1000 + 0 + + /planning/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/debug_values + std_msgs/Float32MultiArray + + + data/4 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /planning/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/debug_values + std_msgs/Float32MultiArray + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 1000 + Current Velocity + + + + + + 1 + + 1000 + 0 + 0 + -1000 + 0 + + /planning/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/debug_values + std_msgs/Float32MultiArray + + + data/8 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /planning/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/debug_values + std_msgs/Float32MultiArray + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 1000 + Raw Upper Velocity + + + + + + 1 + + 1000 + 0 + 0 + -1000 + 0 + + /planning/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/debug_values + std_msgs/Float32MultiArray + + + data/9 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /planning/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/debug_values + std_msgs/Float32MultiArray + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 1000 + Upper Velocity + + + + + + 1 + + 1000 + 0 + 0 + -1000 + 0 + + /control/trajectory_follower/velocity_controller/debug_values + std_msgs/Float32MultiArray + + + data/2 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /control/trajectory_follower/velocity_controller/debug_values + std_msgs/Float32MultiArray + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 1000 + Target Velocity + + + + + + 1 + + 1000 + 0 + 0 + -1000 + 0 + + /control/trajectory_follower/velocity_controller/debug_values + std_msgs/Float32MultiArray + + + data/4 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /control/trajectory_follower/velocity_controller/debug_values + std_msgs/Float32MultiArray + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 1000 + Closest Velocity + + + + true + + 30 + Ego Velocity + + + + + + + + Untitled Axis + 0 + true + + + Untitled Axis + 0 + true + + + + + + + + + 1 + + 1000 + 0 + 0 + -1000 + 0 + + /planning/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/debug_values + std_msgs/Float32MultiArray + + + data/3 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /planning/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/debug_values + std_msgs/Float32MultiArray + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 1000 + Distance to Forward Object + + + + true + + 30 + Distance + + + + + + Untitled Axis + 0 + true + + + Untitled Axis + 0 + true + + + + + + + + + 1 + + 1000 + 0 + 0 + -1000 + 0 + + /control/trajectory_follower/velocity_controller/debug_values + std_msgs/Float32MultiArray + + + data/25 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /control/trajectory_follower/velocity_controller/debug_values + std_msgs/Float32MultiArray + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 1000 + Current Accel + + + + + + 1 + + 1000 + 0 + 0 + -1000 + 0 + + /control/trajectory_follower/velocity_controller/debug_values + std_msgs/Float32MultiArray + + + data/3 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /control/trajectory_follower/velocity_controller/debug_values + std_msgs/Float32MultiArray + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 1000 + Target Accel + + + + true + + 30 + Accel + + + + false +
+ diff --git a/planning/obstacle_stop_planner/docs/adaptive_cruise.drawio.svg b/planning/obstacle_stop_planner/docs/adaptive_cruise.drawio.svg new file mode 100644 index 0000000000000..a13a31e820294 --- /dev/null +++ b/planning/obstacle_stop_planner/docs/adaptive_cruise.drawio.svg @@ -0,0 +1,181 @@ + + + + + + + + + + + + +
+
+
+ Ego vehicle +
+
+
+ + + Ego vehicle + + + + + + + +
+
+
+ (Front vehicle) +
+
+
+ + + (Front vehicle) + + + + + + + +
+
+
+ Obstacle Pointcloud +
+
+
+ + + Obstacle Pointcloud + + + + + + + + + + + + + + + + + + + + + +
+
+
+ distance to obstacle pointcloud +
+
+
+ + + distance to obstacle pointcloud + + + + + + + + + +
+
+
+ Emergency Distance +
+
+
+ + + Emergency Distance + + + + + + + +
+
+
+ Standard Distance +
+
+
+ + + Standard Distance + + + + + + + +
+
+
+ + When the distance to obstacle pointcloud is shorter than standard distance, +
+ target velocity becomes higher than current velocity and vice versa + +
+
+
+ + + When the distance to obstacle pointcloud is shorter than s... + + + + + + + +
+
+
+ + When the distance to obstacle pointcloud is shorter than emergency distance, +
+ Adaptive Cruise Controller modules does not works and +
+ a stop point is inserted by Obstacle Stop Planner module. +
+ +
+
+
+ + + When the distance to obstacle pointcloud is shorter than eme... + + + + + + + + + Viewer does not support full SVG 1.1 + + + + \ No newline at end of file diff --git a/planning/obstacle_stop_planner/docs/insert_decel_velocity.drawio.svg b/planning/obstacle_stop_planner/docs/insert_decel_velocity.drawio.svg new file mode 100644 index 0000000000000..80cb8c2ed5016 --- /dev/null +++ b/planning/obstacle_stop_planner/docs/insert_decel_velocity.drawio.svg @@ -0,0 +1,269 @@ + + + + + + + +
+
+
+ +

+ + backward + + + margin + +

+ +
+
+
+ + + backward margin + + + + + + + + + + + +
+
+
+ +

+ + + deceleration point + + +

+ +
+
+
+ + + deceleration point + + + + + + + + + + + +
+
+
+ +

+ + + v + + +

+ +
+
+
+ + + v + + + + + + + +
+
+
+ +

+ + + x + + +

+ +
+
+
+ + + x + + + + + + + +
+
+
+ +

+ + + baselink to front + + +

+ +
+
+
+ + + baselink to front + + + + + + + + + + + + + +
+
+
+ +

+ + + output velocity + + +

+ +
+
+
+ + + output velocity + + + + + + + + +
+
+
+ +

+ + + reference velocity + + +

+ +
+
+
+ + + reference velocity + + + + + + + + +
+
+
+ +

+ + forward margin + +

+ +
+
+
+ + + forward margin + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ +

+ + + baselink to rear + + +

+ +
+
+
+ + + baselink to rear + + + + + + + + + Viewer does not support full SVG 1.1 + + + + \ No newline at end of file diff --git a/planning/obstacle_stop_planner/docs/insert_velocity.drawio.svg b/planning/obstacle_stop_planner/docs/insert_velocity.drawio.svg new file mode 100644 index 0000000000000..bba4ad8a79115 --- /dev/null +++ b/planning/obstacle_stop_planner/docs/insert_velocity.drawio.svg @@ -0,0 +1,208 @@ + + + + + + + + + + + +
+
+
+ +

+ + + stop point + + +

+ +
+
+
+ + + stop point + + + + + + + + + + + + +
+
+
+ +

+ + + v + + +

+ +
+
+
+ + + v + + + + + + + +
+
+
+ +

+ + + x + + +

+ +
+
+
+ + + x + + + + + + + +
+
+
+ +

+ + + baselink to front + + +

+ +
+
+
+ + + baselink to front + + + + + + + + + + + + + + + +
+
+
+ +

+ + + output velocity + + +

+ +
+
+
+ + + output velocity + + + + + + + + +
+
+
+ +

+ + + reference velocity + + +

+ +
+
+
+ + + reference velocity + + + + + + + + +
+
+
+ +

+ + stop margin + +

+ +
+
+
+ + + stop margin + + + + + + + + + + + + Viewer does not support full SVG 1.1 + + + + \ No newline at end of file diff --git a/planning/obstacle_stop_planner/docs/point_cloud.drawio.svg b/planning/obstacle_stop_planner/docs/point_cloud.drawio.svg new file mode 100644 index 0000000000000..b7f2e2c5bcf45 --- /dev/null +++ b/planning/obstacle_stop_planner/docs/point_cloud.drawio.svg @@ -0,0 +1,174 @@ + + + + + + + +
+
+
+ + + + +
+
+
+ + + + + + + + + +
+
+
+ + + + +
+
+
+ + + + + + + + + +
+
+
+ + + + +
+
+
+ + + + + + + + + +
+
+
+ + + + +
+
+
+ + + + + + + + + +
+
+
+ + + + +
+
+
+ + + + + + + + + +
+
+
+ + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + +
+
+
+ obstacle point cloud +
+
+
+ + + obstacle point cloud + + + + + + + +
+
+
+ stop point +
+
+
+ + + stop point + + + + + + + + + Viewer does not support full SVG 1.1 + + + + \ No newline at end of file diff --git a/planning/obstacle_stop_planner/docs/point_cloud_decel.drawio.svg b/planning/obstacle_stop_planner/docs/point_cloud_decel.drawio.svg new file mode 100644 index 0000000000000..e0bd3001607b6 --- /dev/null +++ b/planning/obstacle_stop_planner/docs/point_cloud_decel.drawio.svg @@ -0,0 +1,175 @@ + + + + + + + +
+
+
+ + + + +
+
+
+ + + + + + + + + +
+
+
+ + + + +
+
+
+ + + + + + + + + +
+
+
+ + + + +
+
+
+ + + + + + + + + +
+
+
+ + + + +
+
+
+ + + + + + + + + +
+
+
+ + + + +
+
+
+ + + + + + + + + +
+
+
+ + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + +
+
+
+ obstacle point cloud +
+
+
+ + + obstacle point cloud + + + + + + + +
+
+
+ deceleration point +
+
+
+ + + deceleration point + + + + + + + + + + + Viewer does not support full SVG 1.1 + + + + \ No newline at end of file diff --git a/planning/obstacle_stop_planner/docs/vehicle_shape.drawio.svg b/planning/obstacle_stop_planner/docs/vehicle_shape.drawio.svg new file mode 100644 index 0000000000000..677e43bed4448 --- /dev/null +++ b/planning/obstacle_stop_planner/docs/vehicle_shape.drawio.svg @@ -0,0 +1,225 @@ + + + + + + + +
+
+
+ + + + +
+
+
+ + + + + + + + + +
+
+
+ + + + +
+
+
+ + + + + + + + + +
+
+
+ + + + +
+
+
+ + + + + + + + + +
+
+
+ + + + +
+
+
+ + + + + + + + + +
+
+
+ + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ Reference Trajectory +
+
+
+ + + Reference Trajectory + + + + + + +
+
+
+ Detection Area for obstacle stop +
+
+
+ + + Detection Area for obstacle st... + + + + + + +
+
+
+ Vehicle Shape +
+
+
+ + + Vehicle Shape + + + + + + + + + Viewer does not support full SVG 1.1 + + + + \ No newline at end of file diff --git a/planning/obstacle_stop_planner/docs/vehicle_shape_decel.drawio.svg b/planning/obstacle_stop_planner/docs/vehicle_shape_decel.drawio.svg new file mode 100644 index 0000000000000..9847751a317e4 --- /dev/null +++ b/planning/obstacle_stop_planner/docs/vehicle_shape_decel.drawio.svg @@ -0,0 +1,376 @@ + + + + + + + +
+
+
+ + + + +
+
+
+ + + + + + + + + +
+
+
+ + + + +
+
+
+ + + + + + + + + +
+
+
+ + + + +
+
+
+ + + + + + + + + +
+
+
+ + + + +
+
+
+ + + + + + + + + +
+
+
+ + + + +
+
+
+ + + + + + + + + +
+
+
+ + + + +
+
+
+ + + + + + + + + +
+
+
+ + + + +
+
+
+ + + + + + + + + +
+
+
+ + + + +
+
+
+ + + + + + + + + +
+
+
+ + + + +
+
+
+ + + + + + + + + +
+
+
+ + + + +
+
+
+ + + + + + + + + +
+
+
+ + + + +
+
+
+ + + + + + + + + +
+
+
+ + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ Reference Trajectory +
+
+
+ + + Reference Trajectory + + + + + + +
+
+
+ (Detection Area for obstacle stop) +
+
+
+ + + (Detection Area for obstacle st... + + + + + + +
+
+
+ Vehicle Shape +
+
+
+ + + Vehicle Shape + + + + + + + +
+
+
+ Detection Area for slow down +
+
+
+ + + Detection Area for slow down + + + + + + + + + + +
+
+
+ expand slow down range +
+
+
+ + + expand slow down range + + + + + + + + + Viewer does not support full SVG 1.1 + + + + \ No newline at end of file diff --git a/planning/obstacle_stop_planner/docs/velocity_limitation.drawio.svg b/planning/obstacle_stop_planner/docs/velocity_limitation.drawio.svg new file mode 100644 index 0000000000000..e63c63d1ee1da --- /dev/null +++ b/planning/obstacle_stop_planner/docs/velocity_limitation.drawio.svg @@ -0,0 +1,270 @@ + + + + + + + + + + + +
+
+
+ +

+ + + deceleration point + + +

+ +
+
+
+ + + deceleration point + + + + + + + + + + + + +
+
+
+ +

+ + + v + + +

+ +
+
+
+ + + v + + + + + + + +
+
+
+ +

+ + + x + + +

+ +
+
+
+ + + x + + + + + + + +
+
+
+ +

+ + + baselink to front + + +

+ +
+
+
+ + + baselink to front + + + + + + + + + + + + + + +
+
+
+ +

+ + + output velocity + + +

+ +
+
+
+ + + output velocity + + + + + + + + +
+
+
+ +

+ + + reference velocity + + +

+ +
+
+
+ + + reference velocity + + + + + + + + +
+
+
+ +

+ + stop margin + +

+ +
+
+
+ + + stop margin + + + + + + + + + + + +
+
+
+ v +
+
+
+ + + v + + + + + + +
+
+
+ + target + +
+
+
+ + + targ... + + + + + + + + +
+
+
+ +

+ + + velocity limitation + + +

+ +
+
+
+ + + velocity limitation + + + + + + + + + + Viewer does not support full SVG 1.1 + + + + \ No newline at end of file diff --git a/planning/obstacle_stop_planner/include/obstacle_stop_planner/adaptive_cruise_control.hpp b/planning/obstacle_stop_planner/include/obstacle_stop_planner/adaptive_cruise_control.hpp new file mode 100644 index 0000000000000..cc554fe831a7a --- /dev/null +++ b/planning/obstacle_stop_planner/include/obstacle_stop_planner/adaptive_cruise_control.hpp @@ -0,0 +1,225 @@ +// 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. + +#ifndef OBSTACLE_STOP_PLANNER__ADAPTIVE_CRUISE_CONTROL_HPP_ +#define OBSTACLE_STOP_PLANNER__ADAPTIVE_CRUISE_CONTROL_HPP_ + +#include + +#include +#include +#include +#include + +#include +#include +#include + +#include + +namespace motion_planning +{ +using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using TrajectoryPoints = std::vector; +class AdaptiveCruiseController +{ +public: + AdaptiveCruiseController( + rclcpp::Node * node, const double vehicle_width, const double vehicle_length, + const double baselink2front); + + void insertAdaptiveCruiseVelocity( + const TrajectoryPoints & trajectory, const int nearest_collision_point_idx, + const geometry_msgs::msg::Pose self_pose, const pcl::PointXYZ & nearest_collision_point, + const rclcpp::Time nearest_collision_point_time, + const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr object_ptr, + const nav_msgs::msg::Odometry::ConstSharedPtr current_velocity_ptr, bool * need_to_stop, + TrajectoryPoints * output_trajectory, const std_msgs::msg::Header trajectory_header); + +private: + rclcpp::Publisher::SharedPtr pub_debug_; + + rclcpp::Node * node_; + /* + * Parameter + */ + double vehicle_width_; + double vehicle_length_; + double baselink2front_; + + rclcpp::Time prev_collision_point_time_; + pcl::PointXYZ prev_collision_point_; + double prev_target_vehicle_time_ = 0.0; + double prev_target_vehicle_dist_ = 0.0; + double prev_target_velocity_ = 0.0; + bool prev_collision_point_valid_ = false; + bool prev_obstacle_velocity_judge_to_start_acc_ = false; + std::vector est_vel_que_; + double prev_upper_velocity_ = 0.0; + + struct Param + { + //!< @brief use tracking objects for estimating object velocity or not + bool use_object_to_est_vel; + + //!< @brief use pcl for estimating object velocity or not + bool use_pcl_to_est_vel; + + //!< @brief consider forward vehicle velocity to self upper velocity or not + bool consider_obj_velocity; + + //!< @brief The distance to extend the polygon length the object in pointcloud-object matching + double object_polygon_length_margin; + + //!< @brief The distance to extend the polygon width the object in pointcloud-object matching + double object_polygon_width_margin; + + //!< @brief Maximum time difference treated as continuous points in speed estimation using a + // point cloud + double valid_est_vel_diff_time; + + //!< @brief Time width of information used for speed estimation in speed estimation using a + // point cloud + double valid_vel_que_time; + + //!< @brief Maximum value of valid speed estimation results in speed estimation using a point + // cloud + double valid_est_vel_max; + + //!< @brief Minimum value of valid speed estimation results in speed estimation using a point + // cloud + double valid_est_vel_min; + + //!< @brief Embed a stop line if the maximum speed calculated by ACC is lower than this speed + double thresh_vel_to_stop; + + /* parameter for acc */ + //!< @brief start acc when the velocity of the forward obstacle exceeds this value + double obstacle_velocity_thresh_to_start_acc; + + //!< @brief stop acc when the velocity of the forward obstacle falls below this value + double obstacle_velocity_thresh_to_stop_acc; + + //!< @brief supposed minimum acceleration in emergency stop + double emergency_stop_acceleration; + + //!< @brief supposed minimum acceleration of forward vehicle in emergency stop + double obstacle_emergency_stop_acceleration; + + //!< @brief supposed idling time to start emergency stop + double emergency_stop_idling_time; + + //!< @brief minimum distance of emergency stop + double min_dist_stop; + + //!< @brief supposed maximum acceleration in active cruise control + double max_standard_acceleration; + + //!< @brief supposed minimum acceleration(deceleration) in active cruise control + double min_standard_acceleration; + + //!< @brief supposed idling time to react object in active cruise control + double standard_idling_time; + + //!< @brief minimum distance in active cruise control + double min_dist_standard; + + //!< @brief supposed minimum acceleration of forward obstacle + double obstacle_min_standard_acceleration; + + //!< @brief margin to insert upper velocity + double margin_rate_to_change_vel; + + //!< @brief use time-compensation to calculate distance to forward vehicle + bool use_time_compensation_to_dist; + + //!< @brief gain of lowpass filter of upper velocity + double lowpass_gain_; + + //!< @brief when failed to estimate velocity, use rough velocity estimation or not + bool use_rough_est_vel; + + //!< @brief in rough velocity estimation, front car velocity is + //!< estimated as self current velocity * this value + double rough_velocity_rate; + + /* parameter for pid used in acc */ + //!< @brief coefficient P in PID control (used when target dist -current_dist >=0) + double p_coeff_pos; + + //!< @brief coefficient P in PID control (used when target dist -current_dist <0) + double p_coeff_neg; + + //!< @brief coefficient D in PID control (used when delta_dist >=0) + double d_coeff_pos; + + //!< @brief coefficient D in PID control (used when delta_dist <0) + double d_coeff_neg; + + static constexpr double d_coeff_valid_time = 1.0; + static constexpr double d_coeff_valid_diff_vel = 20.0; + static constexpr double d_max_vel_norm = 3.0; + }; + Param param_; + + double getMedianVel(const std::vector vel_que); + double lowpass_filter(const double current_value, const double prev_value, const double gain); + void calcDistanceToNearestPointOnPath( + const TrajectoryPoints & trajectory, const int nearest_point_idx, + const geometry_msgs::msg::Pose & self_pose, const pcl::PointXYZ & nearest_collision_point, + const rclcpp::Time & nearest_collision_point_time, double * distance, + const std_msgs::msg::Header & trajectory_header); + double calcTrajYaw(const TrajectoryPoints & trajectory, const int collision_point_idx); + bool estimatePointVelocityFromObject( + const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr object_ptr, + const double traj_yaw, const pcl::PointXYZ & nearest_collision_point, double * velocity); + bool estimatePointVelocityFromPcl( + const double traj_yaw, const pcl::PointXYZ & nearest_collision_point, + const rclcpp::Time & nearest_collision_point_time, double * velocity); + double estimateRoughPointVelocity(double current_vel); + bool isObstacleVelocityHigh(const double obj_vel); + double calcUpperVelocity(const double dist_to_col, const double obj_vel, const double self_vel); + double calcThreshDistToForwardObstacle(const double current_vel, const double obj_vel); + double calcBaseDistToForwardObstacle(const double current_vel, const double obj_vel); + double calcTargetVelocity_P(const double target_dist, const double current_dist); + double calcTargetVelocity_I(const double target_dist, const double current_dist); + double calcTargetVelocity_D(const double target_dist, const double current_dist); + double calcTargetVelocityByPID( + const double current_vel, const double current_dist, const double obj_vel); + + void insertMaxVelocityToPath( + const geometry_msgs::msg::Pose self_pose, const double current_vel, const double target_vel, + const double dist_to_collision_point, TrajectoryPoints * output_trajectory); + void registerQueToVelocity(const double vel, const rclcpp::Time & vel_time); + + /* Debug */ + mutable autoware_debug_msgs::msg::Float32MultiArrayStamped debug_values_; + enum DBGVAL { + ESTIMATED_VEL_PCL = 0, + ESTIMATED_VEL_OBJ = 1, + ESTIMATED_VEL_FINAL = 2, + FORWARD_OBJ_DISTANCE = 3, + CURRENT_VEL = 4, + UPPER_VEL_P = 5, + UPPER_VEL_I = 6, + UPPER_VEL_D = 7, + UPPER_VEL_RAW = 8, + UPPER_VEL = 9 + }; + static constexpr unsigned int num_debug_values_ = 10; +}; + +} // namespace motion_planning + +#endif // OBSTACLE_STOP_PLANNER__ADAPTIVE_CRUISE_CONTROL_HPP_ diff --git a/planning/obstacle_stop_planner/include/obstacle_stop_planner/debug_marker.hpp b/planning/obstacle_stop_planner/include/obstacle_stop_planner/debug_marker.hpp new file mode 100644 index 0000000000000..3af378e103df8 --- /dev/null +++ b/planning/obstacle_stop_planner/include/obstacle_stop_planner/debug_marker.hpp @@ -0,0 +1,133 @@ +// 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. +#ifndef OBSTACLE_STOP_PLANNER__DEBUG_MARKER_HPP_ +#define OBSTACLE_STOP_PLANNER__DEBUG_MARKER_HPP_ + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#define EIGEN_MPL2_ONLY +#include +#include +namespace motion_planning +{ +using autoware_debug_msgs::msg::Float32MultiArrayStamped; + +enum class PolygonType : int8_t { Vehicle = 0, Collision, SlowDownRange, SlowDown }; + +enum class PointType : int8_t { Stop = 0, SlowDown }; + +enum class PoseType : int8_t { Stop = 0, SlowDownStart, SlowDownEnd }; + +class DebugValues +{ +public: + enum class TYPE { + CURRENT_VEL = 0, + CURRENT_ACC = 1, + CURRENT_FORWARD_MARGIN = 2, + OBSTACLE_DISTANCE = 3, + FLAG_FIND_COLLISION_OBSTACLE = 4, + FLAG_FIND_SLOW_DOWN_OBSTACLE = 5, + FLAG_ADAPTIVE_CRUISE = 6, + FLAG_EXTERNAL = 7, + SIZE + }; + + /** + * @brief get the index corresponding to the given value TYPE + * @param [in] type the TYPE enum for which to get the index + * @return index of the type + */ + int getValuesIdx(const TYPE type) const { return static_cast(type); } + /** + * @brief get all the debug values as an std::array + * @return array of all debug values + */ + std::array(TYPE::SIZE)> getValues() const { return values_; } + /** + * @brief set the given type to the given value + * @param [in] type TYPE of the value + * @param [in] value value to set + */ + void setValues(const TYPE type, const double val) { values_.at(static_cast(type)) = val; } + /** + * @brief set the given type to the given value + * @param [in] type index of the type + * @param [in] value value to set + */ + void setValues(const int type, const double val) { values_.at(type) = val; } + +private: + static constexpr int num_debug_values_ = static_cast(TYPE::SIZE); + std::array(TYPE::SIZE)> values_; +}; + +class ObstacleStopPlannerDebugNode +{ +public: + explicit ObstacleStopPlannerDebugNode(rclcpp::Node * node, const double base_link2front); + ~ObstacleStopPlannerDebugNode() {} + bool pushPolygon( + const std::vector & polygon, const double z, const PolygonType & type); + bool pushPolygon(const std::vector & polygon, const PolygonType & type); + bool pushPose(const geometry_msgs::msg::Pose & pose, const PoseType & type); + bool pushObstaclePoint(const geometry_msgs::msg::Point & obstacle_point, const PointType & type); + bool pushObstaclePoint(const pcl::PointXYZ & obstacle_point, const PointType & type); + visualization_msgs::msg::MarkerArray makeVisualizationMarker(); + autoware_planning_msgs::msg::StopReasonArray makeStopReasonArray(); + + void setDebugValues(const DebugValues::TYPE type, const double val) + { + debug_values_.setValues(type, val); + } + void publish(); + +private: + rclcpp::Publisher::SharedPtr debug_viz_pub_; + rclcpp::Publisher::SharedPtr stop_reason_pub_; + rclcpp::Publisher::SharedPtr pub_debug_values_; + rclcpp::Node * node_; + double base_link2front_; + + std::shared_ptr stop_pose_ptr_; + std::shared_ptr slow_down_start_pose_ptr_; + std::shared_ptr slow_down_end_pose_ptr_; + std::shared_ptr stop_obstacle_point_ptr_; + std::shared_ptr slow_down_obstacle_point_ptr_; + std::vector> vehicle_polygons_; + std::vector> slow_down_range_polygons_; + std::vector> collision_polygons_; + std::vector> slow_down_polygons_; + + DebugValues debug_values_; +}; + +} // namespace motion_planning + +#endif // OBSTACLE_STOP_PLANNER__DEBUG_MARKER_HPP_ diff --git a/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp b/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp new file mode 100644 index 0000000000000..6ca9e4b217cb6 --- /dev/null +++ b/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp @@ -0,0 +1,291 @@ +// Copyright 2019 Autoware Foundation +// +// 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 OBSTACLE_STOP_PLANNER__NODE_HPP_ +#define OBSTACLE_STOP_PLANNER__NODE_HPP_ + +#include "obstacle_stop_planner/adaptive_cruise_control.hpp" +#include "obstacle_stop_planner/debug_marker.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +namespace motion_planning +{ +namespace bg = boost::geometry; +using autoware_auto_perception_msgs::msg::PredictedObjects; +using autoware_auto_planning_msgs::msg::Trajectory; +using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using TrajectoryPoints = std::vector; +using autoware_debug_msgs::msg::BoolStamped; +using autoware_debug_msgs::msg::Float32MultiArrayStamped; +using autoware_debug_msgs::msg::Float32Stamped; +using autoware_planning_msgs::msg::ExpandStopRange; +using autoware_planning_msgs::msg::VelocityLimit; +using autoware_planning_msgs::msg::VelocityLimitClearCommand; +using autoware_utils::Point2d; +using autoware_utils::Polygon2d; +using vehicle_info_util::VehicleInfo; + +struct StopPoint +{ + TrajectoryPoint point{}; + size_t index; +}; + +struct SlowDownSection +{ + TrajectoryPoint start_point{}; + TrajectoryPoint end_point{}; + size_t slow_down_start_idx; + size_t slow_down_end_idx; + double velocity; +}; + +class ObstacleStopPlannerNode : public rclcpp::Node +{ +public: + explicit ObstacleStopPlannerNode(const rclcpp::NodeOptions & node_options); + + struct NodeParam + { + bool enable_slow_down; // set True, slow down for obstacle beside the path + double max_velocity; // max velocity [m/s] + double lowpass_gain; // smoothing calculated current acceleration [-] + double hunting_threshold; // keep slow down or stop state if obstacle vanished [s] + }; + + struct StopParam + { + double stop_margin; // stop margin distance from obstacle on the path [m] + double min_behavior_stop_margin; // margin distance, any other stop point is inserted [m] + double expand_stop_range; // margin of vehicle footprint [m] + double extend_distance; // trajectory extend_distance [m] + double step_length; // step length for pointcloud search range [m] + double stop_search_radius; // search radius for obstacle point cloud [m] + }; + + struct SlowDownParam + { + double normal_min_jerk; // min jerk limit for mild stop [m/sss] + double normal_min_acc; // min deceleration limit for mild stop [m/ss] + double limit_min_jerk; // min jerk limit [m/sss] + double limit_min_acc; // min deceleration limit [m/ss] + double forward_margin; // slow down margin(vehicle front -> obstacle) [m] + double backward_margin; // slow down margin(obstacle vehicle rear) [m] + double expand_slow_down_range; // lateral range of detection area [m] + double max_slow_down_vel; // maximum speed in slow down section [m/s] + double min_slow_down_vel; // minimum velocity in slow down section [m/s] + bool consider_constraints; // set "True", decel point is planned under jerk/dec constraints + double slow_down_vel; // target slow down velocity [m/s] + double forward_margin_min; // min margin for relaxing slow down margin [m/s] + double forward_margin_span; // fineness param for relaxing slow down margin [m/s] + double slow_down_min_jerk; // min slow down jerk constraint [m/sss] + double jerk_start; // init jerk used for deceleration planning [m/sss] + double jerk_span; // fineness param for planning deceleration jerk [m/sss] + double vel_threshold_reset_velocity_limit_; // velocity threshold, + // check complete deceleration [m/s] + double dec_threshold_reset_velocity_limit_; // acceleration threshold, + // check complete deceleration [m/ss] + double slow_down_search_radius; // search radius for slow down obstacle point cloud [m] + }; + + struct PlannerData + { + diagnostic_msgs::msg::DiagnosticStatus stop_reason_diag{}; + + geometry_msgs::msg::Pose current_pose{}; + + pcl::PointXYZ nearest_collision_point; + pcl::PointXYZ nearest_slow_down_point; + pcl::PointXYZ lateral_nearest_slow_down_point; + rclcpp::Time nearest_collision_point_time{}; + double lateral_deviation{0.0}; + + size_t trajectory_trim_index{}; + size_t decimate_trajectory_collision_index{}; + size_t decimate_trajectory_slow_down_index{}; + std::map decimate_trajectory_index_map{}; // key: decimate index + // value: original index + + bool found_collision_points{false}; + bool found_slow_down_points{false}; + bool stop_require{false}; + bool slow_down_require{false}; + bool enable_adaptive_cruise{false}; + }; + +private: + /* + * ROS + */ + // publisher and subscriber + rclcpp::Subscription::SharedPtr path_sub_; + rclcpp::Subscription::SharedPtr obstacle_pointcloud_sub_; + rclcpp::Subscription::SharedPtr current_velocity_sub_; + rclcpp::Subscription::SharedPtr dynamic_object_sub_; + rclcpp::Subscription::SharedPtr expand_stop_range_sub_; + rclcpp::Publisher::SharedPtr path_pub_; + rclcpp::Publisher::SharedPtr stop_reason_diag_pub_; + rclcpp::Publisher::SharedPtr pub_clear_velocity_limit_; + rclcpp::Publisher::SharedPtr pub_velocity_limit_; + + std::unique_ptr acc_controller_; + std::shared_ptr debug_ptr_; + std::shared_ptr lpf_acc_{nullptr}; + boost::optional latest_slow_down_section_{}; + tf2_ros::Buffer tf_buffer_{get_clock()}; + tf2_ros::TransformListener tf_listener_{tf_buffer_}; + sensor_msgs::msg::PointCloud2::SharedPtr obstacle_ros_pointcloud_ptr_{nullptr}; + PredictedObjects::ConstSharedPtr object_ptr_{nullptr}; + rclcpp::Time last_detection_time_; + + nav_msgs::msg::Odometry::ConstSharedPtr current_velocity_ptr_{nullptr}; + nav_msgs::msg::Odometry::ConstSharedPtr prev_velocity_ptr_{nullptr}; + double current_acc_{0.0}; + + bool set_velocity_limit_{false}; + + VehicleInfo vehicle_info_; + NodeParam node_param_; + StopParam stop_param_; + SlowDownParam slow_down_param_; + + /* + * Callback + */ + void obstaclePointcloudCallback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr input_msg); + void pathCallback(const Trajectory::ConstSharedPtr input_msg); + void dynamicObjectCallback(const PredictedObjects::ConstSharedPtr input_msg); + void currentVelocityCallback(const nav_msgs::msg::Odometry::ConstSharedPtr input_msg); + void externalExpandStopRangeCallback(const ExpandStopRange::ConstSharedPtr input_msg); + +private: + bool withinPolygon( + const std::vector & cv_polygon, const double radius, const Point2d & prev_point, + const Point2d & next_point, pcl::PointCloud::Ptr candidate_points_ptr, + pcl::PointCloud::Ptr within_points_ptr); + + bool convexHull( + const std::vector & pointcloud, std::vector & polygon_points); + + void searchObstacle( + const TrajectoryPoints & decimate_trajectory, TrajectoryPoints & output, + PlannerData & planner_data, const std_msgs::msg::Header & trajectory_header); + + void insertVelocity( + TrajectoryPoints & trajectory, PlannerData & planner_data, + const std_msgs::msg::Header & trajectory_header); + + TrajectoryPoints decimateTrajectory( + const TrajectoryPoints & input, const double step_length, std::map & index_map); + + TrajectoryPoints trimTrajectoryWithIndexFromSelfPose( + const TrajectoryPoints & input, const geometry_msgs::msg::Pose & self_pose, size_t & index); + + bool searchPointcloudNearTrajectory( + const TrajectoryPoints & trajectory, + const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_points_ptr, + pcl::PointCloud::Ptr output_points_ptr, + const std_msgs::msg::Header & trajectory_header); + + void createOneStepPolygon( + const geometry_msgs::msg::Pose & base_step_pose, + const geometry_msgs::msg::Pose & next_step_pose, std::vector & polygon, + const double expand_width = 0.0); + + bool getSelfPose( + const std_msgs::msg::Header & header, const tf2_ros::Buffer & tf_buffer, + geometry_msgs::msg::Pose & self_pose); + + void getNearestPoint( + const pcl::PointCloud & pointcloud, const geometry_msgs::msg::Pose & base_pose, + pcl::PointXYZ * nearest_collision_point, rclcpp::Time * nearest_collision_point_time); + + void getLateralNearestPoint( + const pcl::PointCloud & pointcloud, const geometry_msgs::msg::Pose & base_pose, + pcl::PointXYZ * lateral_nearest_point, double * deviation); + + geometry_msgs::msg::Pose getVehicleCenterFromBase(const geometry_msgs::msg::Pose & base_pose); + + void insertStopPoint( + const StopPoint & stop_point, TrajectoryPoints & output, + diagnostic_msgs::msg::DiagnosticStatus & stop_reason_diag); + + StopPoint searchInsertPoint( + const int idx, const TrajectoryPoints & base_trajectory, const double dist_remain); + + StopPoint createTargetPoint( + const int idx, const double margin, const TrajectoryPoints & base_trajectory, + const double dist_remain); + + SlowDownSection createSlowDownSection( + const int idx, const TrajectoryPoints & base_trajectory, const double lateral_deviation, + const double dist_remain, const double dist_vehicle_to_obstacle); + + SlowDownSection createSlowDownSectionFromMargin( + const int idx, const TrajectoryPoints & base_trajectory, const double forward_margin, + const double backward_margin, const double velocity); + + void insertSlowDownSection(const SlowDownSection & slow_down_section, TrajectoryPoints & output); + + TrajectoryPoints extendTrajectory(const TrajectoryPoints & input, const double extend_distance); + + TrajectoryPoint getExtendTrajectoryPoint( + double extend_distance, const TrajectoryPoint & goal_point); + + void setExternalVelocityLimit(); + + void resetExternalVelocityLimit(); + + void publishDebugData(const PlannerData & planner_data); +}; +} // namespace motion_planning + +#endif // OBSTACLE_STOP_PLANNER__NODE_HPP_ diff --git a/planning/obstacle_stop_planner/launch/obstacle_stop_planner.launch.xml b/planning/obstacle_stop_planner/launch/obstacle_stop_planner.launch.xml new file mode 100644 index 0000000000000..10e1df8795206 --- /dev/null +++ b/planning/obstacle_stop_planner/launch/obstacle_stop_planner.launch.xml @@ -0,0 +1,33 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/planning/obstacle_stop_planner/package.xml b/planning/obstacle_stop_planner/package.xml new file mode 100644 index 0000000000000..49888a2c7ae91 --- /dev/null +++ b/planning/obstacle_stop_planner/package.xml @@ -0,0 +1,41 @@ + + + obstacle_stop_planner + 0.1.0 + The obstacle_stop_planner package + + Yukihiro Saito + + Apache License 2.0 + + ament_cmake_auto + + autoware_auto_perception_msgs + autoware_auto_planning_msgs + autoware_debug_msgs + autoware_planning_msgs + autoware_utils + diagnostic_msgs + nav_msgs + pcl_conversions + pcl_ros + rclcpp + rclcpp_components + sensor_msgs + signal_processing + std_msgs + tf2 + tf2_eigen + tf2_geometry_msgs + tf2_ros + vehicle_info_util + visualization_msgs + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + + diff --git a/planning/obstacle_stop_planner/src/adaptive_cruise_control.cpp b/planning/obstacle_stop_planner/src/adaptive_cruise_control.cpp new file mode 100644 index 0000000000000..ef552a0b6501d --- /dev/null +++ b/planning/obstacle_stop_planner/src/adaptive_cruise_control.cpp @@ -0,0 +1,696 @@ +// 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. + +#include "obstacle_stop_planner/adaptive_cruise_control.hpp" + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +namespace bg = boost::geometry; +using Point = bg::model::d2::point_xy; +using Polygon = bg::model::polygon; +using Line = bg::model::linestring; + +namespace +{ +Point convertPointRosToBoost(const geometry_msgs::msg::Point & point) +{ + const Point point2d(point.x, point.y); + return point2d; +} + +geometry_msgs::msg::Vector3 rpyFromQuat(const geometry_msgs::msg::Quaternion & q) +{ + tf2::Quaternion quat(q.x, q.y, q.z, q.w); + tf2::Matrix3x3 mat(quat); + double roll, pitch, yaw; + mat.getRPY(roll, pitch, yaw); + geometry_msgs::msg::Vector3 rpy; + rpy.x = roll; + rpy.y = pitch; + rpy.z = yaw; + return rpy; +} + +Polygon getPolygon( + const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Vector3 & size, + const double center_offset, const double l_margin = 0.0, const double w_margin = 0.0) +{ + Polygon obj_poly; + geometry_msgs::msg::Vector3 obj_rpy = rpyFromQuat(pose.orientation); + + double l = size.x * std::cos(obj_rpy.y) + l_margin; + double w = size.y * std::cos(obj_rpy.x) + w_margin; + double co = center_offset; + bg::exterior_ring(obj_poly) = boost::assign::list_of(l / 2.0 + co, w / 2.0)( + -l / 2.0 + co, w / 2.0)(-l / 2.0 + co, -w / 2.0)(l / 2.0 + co, -w / 2.0)(l / 2.0 + co, w / 2.0); + + // rotate polygon + bg::strategy::transform::rotate_transformer rotate( + -obj_rpy.z); // original:clockwise rotation + Polygon rotate_obj_poly; + bg::transform(obj_poly, rotate_obj_poly, rotate); + // translate polygon + bg::strategy::transform::translate_transformer translate( + pose.position.x, pose.position.y); + Polygon translate_obj_poly; + bg::transform(rotate_obj_poly, translate_obj_poly, translate); + return translate_obj_poly; +} + +double getDistanceFromTwoPoint( + const geometry_msgs::msg::Point & point1, const geometry_msgs::msg::Point & point2) +{ + const double dx = point1.x - point2.x; + const double dy = point1.y - point2.y; + const double dist = std::hypot(dx, dy); + return dist; +} + +[[maybe_unused]] double normalizeRadian( + const double rad, const double min_rad = -boost::math::constants::pi(), + const double max_rad = boost::math::constants::pi()) +{ + const auto value = std::fmod(rad, 2 * boost::math::constants::pi()); + if (min_rad < value && value <= max_rad) { + return value; + } else { + return value - std::copysign(2 * boost::math::constants::pi(), value); + } +} + +constexpr double sign(const double value) +{ + if (value > 0) { + return 1.0; + } else if (value < 0) { + return -1.0; + } else { + return 0.0; + } +} +} // namespace + +namespace motion_planning +{ +AdaptiveCruiseController::AdaptiveCruiseController( + rclcpp::Node * node, const double vehicle_width, const double vehicle_length, + const double baselink2front) +: node_(node), + vehicle_width_(vehicle_width), + vehicle_length_(vehicle_length), + baselink2front_(baselink2front) +{ + // get parameter + std::string acc_ns = "adaptive_cruise_control."; + + /* config */ + param_.use_object_to_est_vel = + node_->declare_parameter(acc_ns + "use_object_to_estimate_vel", true); + param_.use_pcl_to_est_vel = node_->declare_parameter(acc_ns + "use_pcl_to_estimate_vel", true); + param_.consider_obj_velocity = node_->declare_parameter(acc_ns + "consider_obj_velocity", true); + + /* parameter for acc */ + param_.obstacle_velocity_thresh_to_start_acc = + node_->declare_parameter(acc_ns + "obstacle_velocity_thresh_to_start_acc", 1.5); + param_.obstacle_velocity_thresh_to_stop_acc = + node_->declare_parameter(acc_ns + "obstacle_velocity_thresh_to_stop_acc", 1.0); + param_.emergency_stop_acceleration = + node_->declare_parameter(acc_ns + "emergency_stop_acceleration", -3.5); + param_.obstacle_emergency_stop_acceleration = + node_->declare_parameter(acc_ns + "obstacle_emergency_stop_acceleration", -5.0); + param_.emergency_stop_idling_time = + node_->declare_parameter(acc_ns + "emergency_stop_idling_time", 0.5); + param_.min_dist_stop = node_->declare_parameter(acc_ns + "min_dist_stop", 4.0); + param_.max_standard_acceleration = + node_->declare_parameter(acc_ns + "max_standard_acceleration", 0.5); + param_.min_standard_acceleration = + node_->declare_parameter(acc_ns + "min_standard_acceleration", -1.0); + param_.standard_idling_time = node_->declare_parameter(acc_ns + "standard_idling_time", 0.5); + param_.min_dist_standard = node_->declare_parameter(acc_ns + "min_dist_standard", 4.0); + param_.obstacle_min_standard_acceleration = + node_->declare_parameter(acc_ns + "obstacle_min_standard_acceleration", -1.5); + param_.margin_rate_to_change_vel = + node_->declare_parameter(acc_ns + "margin_rate_to_change_vel", 0.3); + param_.use_time_compensation_to_dist = + node_->declare_parameter(acc_ns + "use_time_compensation_to_calc_distance", true); + param_.lowpass_gain_ = node_->declare_parameter(acc_ns + "lowpass_gain_of_upper_velocity", 0.6); + + /* parameter for pid in acc */ + param_.p_coeff_pos = node_->declare_parameter(acc_ns + "p_coefficient_positive", 0.1); + param_.p_coeff_neg = node_->declare_parameter(acc_ns + "p_coefficient_negative", 0.3); + param_.d_coeff_pos = node_->declare_parameter(acc_ns + "d_coefficient_positive", 0.0); + param_.d_coeff_neg = node_->declare_parameter(acc_ns + "d_coefficient_negative", 0.1); + + /* parameter for speed estimation of obstacle */ + param_.object_polygon_length_margin = + node_->declare_parameter(acc_ns + "object_polygon_length_margin", 2.0); + param_.object_polygon_width_margin = + node_->declare_parameter(acc_ns + "object_polygon_width_margin", 0.5); + param_.valid_est_vel_diff_time = + node_->declare_parameter(acc_ns + "valid_estimated_vel_diff_time", 1.0); + param_.valid_vel_que_time = node_->declare_parameter(acc_ns + "valid_vel_que_time", 0.5); + param_.valid_est_vel_max = node_->declare_parameter(acc_ns + "valid_estimated_vel_max", 20.0); + param_.valid_est_vel_min = node_->declare_parameter(acc_ns + "valid_estimated_vel_min", -20.0); + param_.thresh_vel_to_stop = node_->declare_parameter(acc_ns + "thresh_vel_to_stop", 0.5); + param_.use_rough_est_vel = + node_->declare_parameter(acc_ns + "use_rough_velocity_estimation", false); + param_.rough_velocity_rate = node_->declare_parameter(acc_ns + "rough_velocity_rate", 0.9); + + /* publisher */ + pub_debug_ = node_->create_publisher( + "~/debug_values", 1); +} + +void AdaptiveCruiseController::insertAdaptiveCruiseVelocity( + const TrajectoryPoints & trajectory, const int nearest_collision_point_idx, + const geometry_msgs::msg::Pose self_pose, const pcl::PointXYZ & nearest_collision_point, + const rclcpp::Time nearest_collision_point_time, + const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr object_ptr, + const nav_msgs::msg::Odometry::ConstSharedPtr current_velocity_ptr, bool * need_to_stop, + TrajectoryPoints * output_trajectory, const std_msgs::msg::Header trajectory_header) +{ + debug_values_.data.clear(); + debug_values_.data.resize(num_debug_values_, 0.0); + + const double current_velocity = current_velocity_ptr->twist.twist.linear.x; + double col_point_distance; + double point_velocity; + bool success_estimate_vel = false; + /* + * calc distance to collision point + */ + calcDistanceToNearestPointOnPath( + trajectory, nearest_collision_point_idx, self_pose, nearest_collision_point, + nearest_collision_point_time, &col_point_distance, trajectory_header); + + /* + * calc yaw of trajectory at collision point + */ + const double traj_yaw = calcTrajYaw(trajectory, nearest_collision_point_idx); + + /* + * estimate velocity of collision point + */ + if (param_.use_pcl_to_est_vel) { + if (estimatePointVelocityFromPcl( + traj_yaw, nearest_collision_point, nearest_collision_point_time, &point_velocity)) { + success_estimate_vel = true; + } + } + + if (param_.use_object_to_est_vel) { + if (estimatePointVelocityFromObject( + object_ptr, traj_yaw, nearest_collision_point, &point_velocity)) { + success_estimate_vel = true; + } + } + + if (param_.use_rough_est_vel && !success_estimate_vel) { + point_velocity = estimateRoughPointVelocity(current_velocity); + success_estimate_vel = true; + } + + if (!success_estimate_vel) { + // if failed to estimate velocity, need to stop + RCLCPP_DEBUG_THROTTLE( + node_->get_logger(), *node_->get_clock(), std::chrono::milliseconds(1000).count(), + "Failed to estimate velocity of forward vehicle. Insert stop line."); + *need_to_stop = true; + prev_upper_velocity_ = current_velocity; // reset prev_upper_velocity + prev_target_velocity_ = 0.0; + pub_debug_->publish(debug_values_); + return; + } + + // calculate max(target) velocity of self + const double upper_velocity = + calcUpperVelocity(col_point_distance, point_velocity, current_velocity); + pub_debug_->publish(debug_values_); + + if (upper_velocity <= param_.thresh_vel_to_stop) { + // if upper velocity is too low, need to stop + RCLCPP_DEBUG_THROTTLE( + node_->get_logger(), *node_->get_clock(), std::chrono::milliseconds(1000).count(), + "Upper velocity is too low. Insert stop line."); + *need_to_stop = true; + return; + } + + /* + * insert max velocity + */ + insertMaxVelocityToPath( + self_pose, current_velocity, upper_velocity, col_point_distance, output_trajectory); + *need_to_stop = false; +} + +void AdaptiveCruiseController::calcDistanceToNearestPointOnPath( + const TrajectoryPoints & trajectory, const int nearest_point_idx, + const geometry_msgs::msg::Pose & self_pose, const pcl::PointXYZ & nearest_collision_point, + const rclcpp::Time & nearest_collision_point_time, double * distance, + const std_msgs::msg::Header & trajectory_header) +{ + if (trajectory.size() == 0) { + RCLCPP_DEBUG_THROTTLE( + node_->get_logger(), *node_->get_clock(), std::chrono::milliseconds(1000).count(), + "input path is too short(size=0)"); + *distance = 0; + return; + } + + // get self polygon + geometry_msgs::msg::Vector3 self_size; + self_size.x = vehicle_length_; + self_size.y = vehicle_width_; + double self_offset = baselink2front_ - vehicle_length_ / 2.0; + Polygon self_poly = getPolygon(self_pose, self_size, self_offset); + + // get nearest point + Point nearest_point2d(nearest_collision_point.x, nearest_collision_point.y); + + if (nearest_point_idx <= 2) { + // if too nearest collision point, return direct distance + *distance = boost::geometry::distance(self_poly, nearest_point2d); + debug_values_.data.at(DBGVAL::FORWARD_OBJ_DISTANCE) = *distance; + return; + } + + /* get total distance to collision point */ + double dist_to_point = 0; + // get distance from self to next nearest point + dist_to_point += boost::geometry::distance( + convertPointRosToBoost(self_pose.position), + convertPointRosToBoost(trajectory.at(1).pose.position)); + + // add distance from next self-nearest-point(=idx:0) to prev point of nearest_point_idx + for (int i = 1; i < nearest_point_idx - 1; i++) { + dist_to_point += + getDistanceFromTwoPoint(trajectory.at(i).pose.position, trajectory.at(i + 1).pose.position); + } + + // add distance from nearest_collision_point to prev point of nearest_point_idx + dist_to_point += boost::geometry::distance( + nearest_point2d, convertPointRosToBoost(trajectory.at(nearest_point_idx - 1).pose.position)); + + // subtract base_link to front + dist_to_point -= baselink2front_; + + // time compensation + if (param_.use_time_compensation_to_dist) { + const rclcpp::Time base_time = trajectory_header.stamp; + double delay_time = (base_time - nearest_collision_point_time).seconds(); + dist_to_point += prev_target_velocity_ * delay_time; + } + + *distance = std::max(0.0, dist_to_point); + debug_values_.data.at(DBGVAL::FORWARD_OBJ_DISTANCE) = *distance; +} + +double AdaptiveCruiseController::calcTrajYaw( + const TrajectoryPoints & trajectory, const int collision_point_idx) +{ + return tf2::getYaw(trajectory.at(collision_point_idx).pose.orientation); +} + +bool AdaptiveCruiseController::estimatePointVelocityFromObject( + const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr object_ptr, + const double traj_yaw, const pcl::PointXYZ & nearest_collision_point, double * velocity) +{ + geometry_msgs::msg::Point nearest_collision_p_ros; + nearest_collision_p_ros.x = nearest_collision_point.x; + nearest_collision_p_ros.y = nearest_collision_point.y; + nearest_collision_p_ros.z = nearest_collision_point.z; + + /* get object velocity, and current yaw */ + bool get_obj = false; + double obj_vel; + double obj_yaw; + const Point collision_point_2d = convertPointRosToBoost(nearest_collision_p_ros); + for (const auto & obj : object_ptr->objects) { + const Polygon obj_poly = getPolygon( + obj.kinematics.initial_pose_with_covariance.pose, obj.shape.dimensions, 0.0, + param_.object_polygon_length_margin, param_.object_polygon_width_margin); + if (boost::geometry::distance(obj_poly, collision_point_2d) <= 0) { + obj_vel = obj.kinematics.initial_twist_with_covariance.twist.linear.x; + obj_yaw = tf2::getYaw(obj.kinematics.initial_pose_with_covariance.pose.orientation); + get_obj = true; + break; + } + } + + if (get_obj) { + *velocity = obj_vel * std::cos(obj_yaw - traj_yaw); + debug_values_.data.at(DBGVAL::ESTIMATED_VEL_OBJ) = *velocity; + return true; + } else { + return false; + } +} + +bool AdaptiveCruiseController::estimatePointVelocityFromPcl( + const double traj_yaw, const pcl::PointXYZ & nearest_collision_point, + const rclcpp::Time & nearest_collision_point_time, double * velocity) +{ + geometry_msgs::msg::Point nearest_collision_p_ros; + nearest_collision_p_ros.x = nearest_collision_point.x; + nearest_collision_p_ros.y = nearest_collision_point.y; + nearest_collision_p_ros.z = nearest_collision_point.z; + + /* estimate velocity */ + const double p_dt = nearest_collision_point_time.seconds() - prev_collision_point_time_.seconds(); + + // if get same pointcloud with previous step, + // skip estimate process + if (std::fabs(p_dt) > std::numeric_limits::epsilon()) { + // valid time check + if (p_dt < 0 || param_.valid_est_vel_diff_time < p_dt) { + prev_collision_point_time_ = nearest_collision_point_time; + prev_collision_point_ = nearest_collision_point; + prev_collision_point_valid_ = true; + return false; + } + const double p_dx = nearest_collision_point.x - prev_collision_point_.x; + const double p_dy = nearest_collision_point.y - prev_collision_point_.y; + const double p_dist = std::hypot(p_dx, p_dy); + const double p_yaw = std::atan2(p_dy, p_dx); + const double p_vel = p_dist / p_dt; + const double est_velocity = p_vel * std::cos(p_yaw - traj_yaw); + // valid velocity check + if (est_velocity <= param_.valid_est_vel_min || param_.valid_est_vel_max <= est_velocity) { + prev_collision_point_time_ = nearest_collision_point_time; + prev_collision_point_ = nearest_collision_point; + prev_collision_point_valid_ = true; + est_vel_que_.clear(); + return false; + } + + // append new velocity and remove old velocity from que + registerQueToVelocity(est_velocity, nearest_collision_point_time); + } + + // calc average(median) velocity from que + *velocity = getMedianVel(est_vel_que_); + debug_values_.data.at(DBGVAL::ESTIMATED_VEL_PCL) = *velocity; + + prev_collision_point_time_ = nearest_collision_point_time; + prev_collision_point_ = nearest_collision_point; + prev_target_velocity_ = *velocity; + prev_collision_point_valid_ = true; + return true; +} + +double AdaptiveCruiseController::estimateRoughPointVelocity(double current_vel) +{ + const double p_dt = node_->now().seconds() - prev_collision_point_time_.seconds(); + if (param_.valid_est_vel_diff_time >= p_dt) { + // use previous estimated velocity + return prev_target_velocity_; + } + + // use current velocity * rough velocity rate + return current_vel * param_.rough_velocity_rate; +} + +bool AdaptiveCruiseController::isObstacleVelocityHigh(const double obj_vel) +{ + bool is_high = false; + if (prev_obstacle_velocity_judge_to_start_acc_) { + is_high = obj_vel > param_.obstacle_velocity_thresh_to_stop_acc; + } else { + is_high = obj_vel > param_.obstacle_velocity_thresh_to_start_acc; + } + prev_obstacle_velocity_judge_to_start_acc_ = is_high; + return is_high; +} + +double AdaptiveCruiseController::calcUpperVelocity( + const double dist_to_col, const double obj_vel, const double self_vel) +{ + debug_values_.data.at(DBGVAL::ESTIMATED_VEL_FINAL) = obj_vel; + if (!isObstacleVelocityHigh(obj_vel)) { + // stop acc by low-velocity obstacle + RCLCPP_DEBUG_THROTTLE( + node_->get_logger(), *node_->get_clock(), std::chrono::milliseconds(1000).count(), + "The velocity of forward vehicle is too low. Insert stop line."); + prev_upper_velocity_ = self_vel; // reset prev_upper_velocity + return 0.0; + } + + const double thresh_dist = calcThreshDistToForwardObstacle(self_vel, obj_vel); + if (thresh_dist >= dist_to_col) { + // emergency stop + RCLCPP_DEBUG_THROTTLE( + node_->get_logger(), *node_->get_clock(), std::chrono::milliseconds(1000).count(), + "Forward vehicle is too close. Insert stop line."); + prev_upper_velocity_ = self_vel; // reset prev_upper_velocity + return 0.0; + } + + const double upper_velocity = + std::max(1e-01, calcTargetVelocityByPID(self_vel, dist_to_col, obj_vel)); + const double lowpass_upper_velocity = + lowpass_filter(upper_velocity, prev_upper_velocity_, param_.lowpass_gain_); + prev_upper_velocity_ = lowpass_upper_velocity; + debug_values_.data.at(DBGVAL::UPPER_VEL) = lowpass_upper_velocity; + return lowpass_upper_velocity; +} + +double AdaptiveCruiseController::calcThreshDistToForwardObstacle( + const double current_vel, const double obj_vel) +{ + const double current_vel_min = std::max(1.0, std::fabs(current_vel)); + const double obj_vel_min = std::max(0.0, obj_vel); + const double minimum_distance = param_.min_dist_stop; + const double idling_distance = current_vel_min * param_.emergency_stop_idling_time; + const double braking_distance = + (-1.0 * current_vel_min * current_vel_min) / (2.0 * param_.emergency_stop_acceleration); + const double obj_braking_distance = + (-1.0 * obj_vel_min * obj_vel_min) / (2.0 * param_.obstacle_emergency_stop_acceleration); + + return minimum_distance + std::max( + 0.0, idling_distance + braking_distance - + obj_braking_distance * param_.consider_obj_velocity); +} + +double AdaptiveCruiseController::calcBaseDistToForwardObstacle( + const double current_vel, const double obj_vel) +{ + const double obj_vel_min = std::max(0.0, obj_vel); + const double minimum_distance = param_.min_dist_standard; + const double idling_distance = current_vel * param_.standard_idling_time; + const double braking_distance = + (-1.0 * current_vel * current_vel) / (2.0 * param_.min_standard_acceleration); + const double obj_braking_distance = + (-1.0 * obj_vel_min * obj_vel_min) / (2.0 * param_.obstacle_min_standard_acceleration); + return minimum_distance + std::max( + 0.0, idling_distance + braking_distance - + obj_braking_distance * param_.consider_obj_velocity); +} + +double AdaptiveCruiseController::calcTargetVelocity_P( + const double target_dist, const double current_dist) +{ + const double diff_dist = current_dist - target_dist; + double add_vel_p; + if (diff_dist >= 0) { + add_vel_p = diff_dist * param_.p_coeff_pos; + } else { + add_vel_p = diff_dist * param_.p_coeff_neg; + } + return add_vel_p; +} + +double AdaptiveCruiseController::calcTargetVelocity_I( + [[maybe_unused]] const double target_dist, [[maybe_unused]] const double current_dist) +{ + // not implemented + return 0.0; +} + +double AdaptiveCruiseController::calcTargetVelocity_D( + const double target_dist, const double current_dist) +{ + if (node_->now().seconds() - prev_target_vehicle_time_ >= param_.d_coeff_valid_time) { + // invalid time(prev is too old) + return 0.0; + } + + double diff_vel = (target_dist - prev_target_vehicle_dist_) / + (node_->now().seconds() - prev_target_vehicle_time_); + + if (std::fabs(diff_vel) >= param_.d_coeff_valid_diff_vel) { + // invalid(discontinuous) diff_vel + return 0.0; + } + + double add_vel_d = 0; + + add_vel_d = diff_vel; + if (add_vel_d >= 0) { + diff_vel *= param_.d_coeff_pos; + } + if (add_vel_d < 0) { + diff_vel *= param_.d_coeff_neg; + } + add_vel_d = boost::algorithm::clamp(add_vel_d, -param_.d_max_vel_norm, param_.d_max_vel_norm); + + // add buffer + prev_target_vehicle_dist_ = current_dist; + prev_target_vehicle_time_ = node_->now().seconds(); + + return add_vel_d; +} + +double AdaptiveCruiseController::calcTargetVelocityByPID( + const double current_vel, const double current_dist, const double obj_vel) +{ + const double target_dist = calcBaseDistToForwardObstacle(current_vel, obj_vel); + RCLCPP_DEBUG_STREAM(node_->get_logger(), "[adaptive cruise control] target_dist" << target_dist); + + const double add_vel_p = calcTargetVelocity_P(target_dist, current_dist); + //** I is not implemented ** + const double add_vel_i = calcTargetVelocity_I(target_dist, current_dist); + const double add_vel_d = calcTargetVelocity_D(target_dist, current_dist); + + double target_vel = current_vel + add_vel_p + add_vel_i + add_vel_d; + debug_values_.data.at(DBGVAL::CURRENT_VEL) = current_vel; + debug_values_.data.at(DBGVAL::UPPER_VEL_P) = add_vel_p; + debug_values_.data.at(DBGVAL::UPPER_VEL_I) = add_vel_i; + debug_values_.data.at(DBGVAL::UPPER_VEL_D) = add_vel_d; + debug_values_.data.at(DBGVAL::UPPER_VEL_RAW) = target_vel; + return target_vel; +} + +void AdaptiveCruiseController::insertMaxVelocityToPath( + const geometry_msgs::msg::Pose self_pose, const double current_vel, const double target_vel, + const double dist_to_collision_point, TrajectoryPoints * output_trajectory) +{ + // plus distance from self to next nearest point + double dist = dist_to_collision_point; + double dist_to_first_point = 0.0; + if (output_trajectory->size() > 1) { + dist_to_first_point = boost::geometry::distance( + convertPointRosToBoost(self_pose.position), + convertPointRosToBoost(output_trajectory->at(1).pose.position)); + } + dist += dist_to_first_point; + + double margin_to_insert = dist_to_collision_point * param_.margin_rate_to_change_vel; + // accel = (v_after^2 - v_before^2 ) / 2x + double target_acc = (std::pow(target_vel, 2) - std::pow(current_vel, 2)) / (2 * margin_to_insert); + + const double clipped_acc = boost::algorithm::clamp( + target_acc, param_.min_standard_acceleration, param_.max_standard_acceleration); + double pre_vel = current_vel; + double total_dist = 0.0; + for (size_t i = 1; i < output_trajectory->size(); i++) { + // calc velocity of each point by gradient deceleration + const auto current_p = output_trajectory->at(i); + const auto prev_p = output_trajectory->at(i - 1); + const double p_dist = getDistanceFromTwoPoint(current_p.pose.position, prev_p.pose.position); + total_dist += p_dist; + if (current_p.longitudinal_velocity_mps > target_vel && total_dist >= 0) { + double next_pre_vel; + if (std::fabs(clipped_acc) < 1e-05) { + next_pre_vel = pre_vel; + } else { + // v_after = sqrt (2x*accel + v_before^2) + next_pre_vel = std::sqrt(2 * p_dist * clipped_acc + std::pow(pre_vel, 2)); + } + if (target_acc >= 0) { + next_pre_vel = std::min(next_pre_vel, target_vel); + } else { + next_pre_vel = std::max(next_pre_vel, target_vel); + } + + if (total_dist >= margin_to_insert) { + const double max_velocity = std::max(target_vel, next_pre_vel); + if (output_trajectory->at(i).longitudinal_velocity_mps > max_velocity) { + output_trajectory->at(i).longitudinal_velocity_mps = max_velocity; + } + } + pre_vel = next_pre_vel; + } + } +} + +void AdaptiveCruiseController::registerQueToVelocity( + const double vel, const rclcpp::Time & vel_time) +{ + // remove old msg from que + std::vector delete_idxs; + for (size_t i = 0; i < est_vel_que_.size(); i++) { + if (node_->now().seconds() - est_vel_que_.at(i).header.stamp.sec > param_.valid_vel_que_time) { + delete_idxs.push_back(i); + } + } + for (int delete_idx = delete_idxs.size() - 1; 0 <= delete_idx; --delete_idx) { + est_vel_que_.erase(est_vel_que_.begin() + delete_idxs.at(delete_idx)); + } + + // append new que + nav_msgs::msg::Odometry new_vel; + new_vel.header.stamp = vel_time; + new_vel.twist.twist.linear.x = vel; + est_vel_que_.emplace_back(new_vel); +} + +double AdaptiveCruiseController::getMedianVel(const std::vector vel_que) +{ + if (vel_que.size() == 0) { + RCLCPP_WARN_STREAM(node_->get_logger(), "size of vel que is 0. Something has wrong."); + return 0.0; + } + + std::vector raw_vel_que; + for (const auto vel : vel_que) { + raw_vel_que.emplace_back(vel.twist.twist.linear.x); + } + + double med_vel; + if (raw_vel_que.size() % 2 == 0) { + size_t med1 = (raw_vel_que.size()) / 2 - 1; + size_t med2 = (raw_vel_que.size()) / 2; + std::nth_element(raw_vel_que.begin(), raw_vel_que.begin() + med1, raw_vel_que.end()); + const double vel1 = raw_vel_que[med1]; + std::nth_element(raw_vel_que.begin(), raw_vel_que.begin() + med2, raw_vel_que.end()); + const double vel2 = raw_vel_que[med2]; + med_vel = (vel1 + vel2) / 2; + } else { + size_t med = (raw_vel_que.size() - 1) / 2; + std::nth_element(raw_vel_que.begin(), raw_vel_que.begin() + med, raw_vel_que.end()); + med_vel = raw_vel_que[med]; + } + + return med_vel; +} + +double AdaptiveCruiseController::lowpass_filter( + const double current_value, const double prev_value, const double gain) +{ + return gain * prev_value + (1.0 - gain) * current_value; +} + +} // namespace motion_planning diff --git a/planning/obstacle_stop_planner/src/debug_marker.cpp b/planning/obstacle_stop_planner/src/debug_marker.cpp new file mode 100644 index 0000000000000..cbb10a304820b --- /dev/null +++ b/planning/obstacle_stop_planner/src/debug_marker.cpp @@ -0,0 +1,359 @@ +// 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. + +#include "obstacle_stop_planner/debug_marker.hpp" + +#include + +#include + +#include +#include + +using autoware_utils::appendMarkerArray; +using autoware_utils::calcOffsetPose; +using autoware_utils::createDefaultMarker; +using autoware_utils::createMarkerColor; +using autoware_utils::createMarkerOrientation; +using autoware_utils::createMarkerScale; +using autoware_utils::createPoint; +using autoware_utils::createSlowDownVirtualWallMarker; +using autoware_utils::createStopVirtualWallMarker; + +namespace motion_planning +{ +ObstacleStopPlannerDebugNode::ObstacleStopPlannerDebugNode( + rclcpp::Node * node, const double base_link2front) +: node_(node), base_link2front_(base_link2front) +{ + debug_viz_pub_ = + node_->create_publisher("~/debug/marker", 1); + stop_reason_pub_ = node_->create_publisher( + "~/output/stop_reasons", 1); + pub_debug_values_ = node_->create_publisher("~/debug/debug_values", 1); +} + +bool ObstacleStopPlannerDebugNode::pushPolygon( + const std::vector & polygon, const double z, const PolygonType & type) +{ + std::vector eigen_polygon; + for (const auto & point : polygon) { + Eigen::Vector3d eigen_point; + eigen_point << point.x, point.y, z; + eigen_polygon.push_back(eigen_point); + } + return pushPolygon(eigen_polygon, type); +} + +bool ObstacleStopPlannerDebugNode::pushPolygon( + const std::vector & polygon, const PolygonType & type) +{ + switch (type) { + case PolygonType::Vehicle: + if (!polygon.empty()) { + vehicle_polygons_.push_back(polygon); + } + return true; + case PolygonType::Collision: + if (!polygon.empty()) { + collision_polygons_.push_back(polygon); + } + return true; + case PolygonType::SlowDownRange: + if (!polygon.empty()) { + slow_down_range_polygons_.push_back(polygon); + } + return true; + case PolygonType::SlowDown: + if (!polygon.empty()) { + slow_down_polygons_.push_back(polygon); + } + return true; + default: + return false; + } +} + +bool ObstacleStopPlannerDebugNode::pushPose( + const geometry_msgs::msg::Pose & pose, const PoseType & type) +{ + switch (type) { + case PoseType::Stop: + stop_pose_ptr_ = std::make_shared(pose); + return true; + case PoseType::SlowDownStart: + slow_down_start_pose_ptr_ = std::make_shared(pose); + return true; + case PoseType::SlowDownEnd: + slow_down_end_pose_ptr_ = std::make_shared(pose); + return true; + default: + return false; + } +} + +bool ObstacleStopPlannerDebugNode::pushObstaclePoint( + const geometry_msgs::msg::Point & obstacle_point, const PointType & type) +{ + switch (type) { + case PointType::Stop: + stop_obstacle_point_ptr_ = std::make_shared(obstacle_point); + return true; + case PointType::SlowDown: + slow_down_obstacle_point_ptr_ = std::make_shared(obstacle_point); + return true; + default: + return false; + } +} + +bool ObstacleStopPlannerDebugNode::pushObstaclePoint( + const pcl::PointXYZ & obstacle_point, const PointType & type) +{ + geometry_msgs::msg::Point ros_point; + ros_point.x = obstacle_point.x; + ros_point.y = obstacle_point.y; + ros_point.z = obstacle_point.z; + return pushObstaclePoint(ros_point, type); +} + +void ObstacleStopPlannerDebugNode::publish() +{ + /* publish debug marker for rviz */ + const auto visualization_msg = makeVisualizationMarker(); + debug_viz_pub_->publish(visualization_msg); + + /* publish stop reason for autoware api */ + const auto stop_reason_msg = makeStopReasonArray(); + stop_reason_pub_->publish(stop_reason_msg); + + // publish debug values + autoware_debug_msgs::msg::Float32MultiArrayStamped debug_msg{}; + debug_msg.stamp = node_->now(); + for (const auto & v : debug_values_.getValues()) { + debug_msg.data.push_back(v); + } + pub_debug_values_->publish(debug_msg); + + /* reset variables */ + vehicle_polygons_.clear(); + collision_polygons_.clear(); + slow_down_range_polygons_.clear(); + slow_down_polygons_.clear(); + stop_pose_ptr_ = nullptr; + slow_down_start_pose_ptr_ = nullptr; + slow_down_end_pose_ptr_ = nullptr; + stop_obstacle_point_ptr_ = nullptr; + slow_down_obstacle_point_ptr_ = nullptr; +} + +visualization_msgs::msg::MarkerArray ObstacleStopPlannerDebugNode::makeVisualizationMarker() +{ + visualization_msgs::msg::MarkerArray msg; + rclcpp::Time current_time = node_->now(); + + // polygon + if (!vehicle_polygons_.empty()) { + auto marker = createDefaultMarker( + "map", current_time, "detection_polygons", 0, visualization_msgs::msg::Marker::LINE_LIST, + createMarkerScale(0.01, 0.0, 0.0), createMarkerColor(0.0, 1.0, 0.0, 0.999)); + + for (size_t i = 0; i < vehicle_polygons_.size(); ++i) { + for (size_t j = 0; j < vehicle_polygons_.at(i).size(); ++j) { + { + const auto & p = vehicle_polygons_.at(i).at(j); + marker.points.push_back(createPoint(p.x(), p.y(), p.z())); + } + if (j + 1 == vehicle_polygons_.at(i).size()) { + const auto & p = vehicle_polygons_.at(i).at(0); + marker.points.push_back(createPoint(p.x(), p.y(), p.z())); + } else { + const auto & p = vehicle_polygons_.at(i).at(j + 1); + marker.points.push_back(createPoint(p.x(), p.y(), p.z())); + } + } + } + msg.markers.push_back(marker); + } + + if (!collision_polygons_.empty()) { + auto marker = createDefaultMarker( + "map", current_time, "collision_polygons", 0, visualization_msgs::msg::Marker::LINE_LIST, + createMarkerScale(0.05, 0.0, 0.0), createMarkerColor(1.0, 0.0, 0.0, 0.999)); + + for (size_t i = 0; i < collision_polygons_.size(); ++i) { + for (size_t j = 0; j < collision_polygons_.at(i).size(); ++j) { + { + const auto & p = collision_polygons_.at(i).at(j); + marker.points.push_back(createPoint(p.x(), p.y(), p.z())); + } + if (j + 1 == collision_polygons_.at(i).size()) { + const auto & p = collision_polygons_.at(i).at(0); + marker.points.push_back(createPoint(p.x(), p.y(), p.z())); + } else { + const auto & p = collision_polygons_.at(i).at(j + 1); + marker.points.push_back(createPoint(p.x(), p.y(), p.z())); + } + } + } + msg.markers.push_back(marker); + } + + if (!slow_down_range_polygons_.empty()) { + auto marker = createDefaultMarker( + "map", current_time, "slow_down_detection_polygons", 0, + visualization_msgs::msg::Marker::LINE_LIST, createMarkerScale(0.01, 0.0, 0.0), + createMarkerColor(0.0, 1.0, 0.0, 0.999)); + + for (size_t i = 0; i < slow_down_range_polygons_.size(); ++i) { + for (size_t j = 0; j < slow_down_range_polygons_.at(i).size(); ++j) { + { + const auto & p = slow_down_range_polygons_.at(i).at(j); + marker.points.push_back(createPoint(p.x(), p.y(), p.z())); + } + if (j + 1 == slow_down_range_polygons_.at(i).size()) { + const auto & p = slow_down_range_polygons_.at(i).at(0); + marker.points.push_back(createPoint(p.x(), p.y(), p.z())); + } else { + const auto & p = slow_down_range_polygons_.at(i).at(j + 1); + marker.points.push_back(createPoint(p.x(), p.y(), p.z())); + } + } + } + msg.markers.push_back(marker); + } + + if (!slow_down_polygons_.empty()) { + auto marker = createDefaultMarker( + "map", current_time, "slow_down_polygons", 0, visualization_msgs::msg::Marker::LINE_LIST, + createMarkerScale(0.05, 0.0, 0.0), createMarkerColor(1.0, 1.0, 0.0, 0.999)); + + for (size_t i = 0; i < slow_down_polygons_.size(); ++i) { + for (size_t j = 0; j < slow_down_polygons_.at(i).size(); ++j) { + { + const auto & p = slow_down_polygons_.at(i).at(j); + marker.points.push_back(createPoint(p.x(), p.y(), p.z())); + } + if (j + 1 == slow_down_polygons_.at(i).size()) { + const auto & p = slow_down_polygons_.at(i).at(0); + marker.points.push_back(createPoint(p.x(), p.y(), p.z())); + } else { + const auto & p = slow_down_polygons_.at(i).at(j + 1); + marker.points.push_back(createPoint(p.x(), p.y(), p.z())); + } + } + } + msg.markers.push_back(marker); + } + + if (stop_pose_ptr_ != nullptr) { + const auto p = calcOffsetPose(*stop_pose_ptr_, base_link2front_, 0.0, 0.0); + const auto markers = createStopVirtualWallMarker(p, "obstacle on the path", current_time, 0); + appendMarkerArray(markers, &msg); + } + + if (slow_down_start_pose_ptr_ != nullptr && stop_pose_ptr_ == nullptr) { + const auto p = calcOffsetPose(*slow_down_start_pose_ptr_, base_link2front_, 0.0, 0.0); + + { + const auto markers = + createSlowDownVirtualWallMarker(p, "obstacle beside the path", current_time, 0); + appendMarkerArray(markers, &msg); + } + + { + auto markers = createSlowDownVirtualWallMarker(p, "slow down\nstart", current_time, 1); + markers.markers.front().ns = "slow_down_start_virtual_wall"; + markers.markers.back().ns = "slow_down_start_factor_text"; + appendMarkerArray(markers, &msg); + } + } + + if (slow_down_end_pose_ptr_ != nullptr && stop_pose_ptr_ == nullptr) { + const auto p = calcOffsetPose(*slow_down_end_pose_ptr_, base_link2front_, 0.0, 0.0); + auto markers = createSlowDownVirtualWallMarker(p, "slow down\nend", current_time, 2); + markers.markers.front().ns = "slow_down_end_virtual_wall"; + markers.markers.back().ns = "slow_down_end_factor_text"; + appendMarkerArray(markers, &msg); + } + + if (stop_obstacle_point_ptr_ != nullptr) { + auto marker = createDefaultMarker( + "map", current_time, "stop_obstacle_point", 0, visualization_msgs::msg::Marker::SPHERE, + createMarkerScale(0.25, 0.25, 0.25), createMarkerColor(1.0, 0.0, 0.0, 0.999)); + marker.pose.position = *stop_obstacle_point_ptr_; + msg.markers.push_back(marker); + } + + if (stop_obstacle_point_ptr_ != nullptr) { + auto marker = createDefaultMarker( + "map", current_time, "stop_obstacle_text", 0, + visualization_msgs::msg::Marker::TEXT_VIEW_FACING, createMarkerScale(0.0, 0.0, 1.0), + createMarkerColor(1.0, 1.0, 1.0, 0.999)); + marker.pose.position = *stop_obstacle_point_ptr_; + marker.pose.position.z += 2.0; + marker.text = "!"; + msg.markers.push_back(marker); + } + + if (slow_down_obstacle_point_ptr_ != nullptr) { + auto marker = createDefaultMarker( + "map", current_time, "slow_down_obstacle_point", 0, visualization_msgs::msg::Marker::SPHERE, + createMarkerScale(0.25, 0.25, 0.25), createMarkerColor(1.0, 0.0, 0.0, 0.999)); + marker.pose.position = *slow_down_obstacle_point_ptr_; + msg.markers.push_back(marker); + } + + if (slow_down_obstacle_point_ptr_ != nullptr) { + auto marker = createDefaultMarker( + "map", current_time, "slow_down_obstacle_text", 0, + visualization_msgs::msg::Marker::TEXT_VIEW_FACING, createMarkerScale(0.0, 0.0, 1.0), + createMarkerColor(1.0, 1.0, 1.0, 0.999)); + marker.pose.position = *slow_down_obstacle_point_ptr_; + marker.pose.position.z += 2.0; + marker.text = "!"; + msg.markers.push_back(marker); + } + + return msg; +} + +autoware_planning_msgs::msg::StopReasonArray ObstacleStopPlannerDebugNode::makeStopReasonArray() +{ + // create header + std_msgs::msg::Header header; + header.frame_id = "map"; + header.stamp = node_->now(); + + // create stop reason stamped + autoware_planning_msgs::msg::StopReason stop_reason_msg; + stop_reason_msg.reason = autoware_planning_msgs::msg::StopReason::OBSTACLE_STOP; + autoware_planning_msgs::msg::StopFactor stop_factor; + + if (stop_pose_ptr_ != nullptr) { + stop_factor.stop_pose = *stop_pose_ptr_; + if (stop_obstacle_point_ptr_ != nullptr) { + stop_factor.stop_factor_points.emplace_back(*stop_obstacle_point_ptr_); + } + stop_reason_msg.stop_factors.emplace_back(stop_factor); + } + + // create stop reason array + autoware_planning_msgs::msg::StopReasonArray stop_reason_array; + stop_reason_array.header = header; + stop_reason_array.stop_reasons.emplace_back(stop_reason_msg); + return stop_reason_array; +} + +} // namespace motion_planning diff --git a/planning/obstacle_stop_planner/src/node.cpp b/planning/obstacle_stop_planner/src/node.cpp new file mode 100644 index 0000000000000..1fcdbbff79365 --- /dev/null +++ b/planning/obstacle_stop_planner/src/node.cpp @@ -0,0 +1,1463 @@ +// 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. + +#include +#include +#include +#include +#include +#include +#include + +#define EIGEN_MPL2_ONLY +#include "obstacle_stop_planner/node.hpp" + +#include +#include + +#include + +#include +#include +#include + +namespace motion_planning +{ +using autoware_utils::calcAzimuthAngle; +using autoware_utils::calcDistance2d; +using autoware_utils::calcSignedArcLength; +using autoware_utils::createPoint; +using autoware_utils::findNearestIndex; +using autoware_utils::getRPY; + +namespace +{ +bool validCheckDecelPlan( + 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_STREAM( + rclcpp::get_logger("validCheckDecelPlan"), + "[validCheckDecelPlan] valid check error! v_target = " << v_target << ", v_end = " << v_end); + return false; + } + if (a_end < a_min || a_max < a_end) { + RCLCPP_DEBUG_STREAM( + rclcpp::get_logger("validCheckDecelPlan"), + "[validCheckDecelPlan] valid check error! a_target = " << a_target << ", a_end = " << a_end); + return false; + } + + return true; +} +/** + * @brief calculate distance until velocity is reached target velocity (TYPE1) + * @param (v0) current velocity [m/s] + * @param (vt) target velocity [m/s] + * @param (a0) current acceleration [m/ss] + * @param (am) minimum deceleration [m/ss] + * @param (ja) maximum jerk [m/sss] + * @param (jd) minimum jerk [m/sss] + * @param (t_min) duration of constant deceleration [s] + * @return moving distance until velocity is reached vt [m] + * @detail TODO(Satoshi Ota) + */ +boost::optional calcDecelDistPlanType1( + const double v0, const double vt, const double a0, const double am, const double ja, + const double jd, const double t_min) +{ + constexpr double epsilon = 1e-3; + + const double j1 = am < a0 ? jd : ja; + const double t1 = epsilon < (am - a0) / j1 ? (am - a0) / j1 : 0.0; + const double a1 = a0 + j1 * t1; + const double v1 = v0 + a0 * t1 + 0.5 * j1 * t1 * t1; + const double x1 = v0 * t1 + 0.5 * a0 * t1 * t1 + (1.0 / 6.0) * j1 * t1 * t1 * t1; + + const double t2 = epsilon < t_min ? t_min : 0.0; + const double a2 = a1; + const double v2 = v1 + a1 * t2; + const double x2 = x1 + v1 * t2 + 0.5 * a1 * t2 * t2; + + const double t3 = epsilon < (0.0 - am) / ja ? (0.0 - am) / ja : 0.0; + const double a3 = a2 + ja * t3; + const double v3 = v2 + a2 * t3 + 0.5 * ja * t3 * t3; + const double x3 = x2 + v2 * t3 + 0.5 * a2 * t3 * t3 + (1.0 / 6.0) * ja * 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 (!validCheckDecelPlan(v3, a3, vt, a_target, v_margin, a_margin)) { + return {}; + } + + return x3; +} +/** + * @brief calculate distance until velocity is reached target velocity (TYPE2) + * @param (v0) current velocity [m/s] + * @param (vt) target velocity [m/s] + * @param (a0) current acceleration [m/ss] + * @param (am) minimum deceleration [m/ss] + * @param (ja) maximum jerk [m/sss] + * @param (jd) minimum jerk [m/sss] + * @return moving distance until velocity is reached vt [m] + * @detail TODO(Satoshi Ota) + */ +boost::optional calcDecelDistPlanType2( + const double v0, const double vt, const double a0, const double ja, const double jd) +{ + constexpr double epsilon = 1e-3; + + const double a1_square = (vt - v0 - 0.5 * (0.0 - a0) / jd * a0) * (2.0 * ja * jd / (ja - jd)); + const double a1 = -std::sqrt(a1_square); + + const double t1 = epsilon < (a1 - a0) / jd ? (a1 - a0) / jd : 0.0; + const double v1 = v0 + a0 * t1 + 0.5 * jd * t1 * t1; + const double x1 = v0 * t1 + 0.5 * a0 * t1 * t1 + (1.0 / 6.0) * jd * t1 * t1 * t1; + + const double t2 = epsilon < (0.0 - a1) / ja ? (0.0 - a1) / ja : 0.0; + const double a2 = a1 + ja * t2; + const double v2 = v1 + a1 * t2 + 0.5 * ja * t2 * t2; + const double x2 = x1 + v1 * t2 + 0.5 * a1 * t2 * t2 + (1.0 / 6.0) * ja * t2 * t2 * t2; + + const double a_target = 0.0; + const double v_margin = 0.3; + const double a_margin = 0.1; + + if (!validCheckDecelPlan(v2, a2, vt, a_target, v_margin, a_margin)) { + return {}; + } + + return x2; +} +/** + * @brief calculate distance until velocity is reached target velocity (TYPE3) + * @param (v0) current velocity [m/s] + * @param (vt) target velocity [m/s] + * @param (a0) current acceleration [m/ss] + * @param (ja) maximum jerk [m/sss] + * @return moving distance until velocity is reached vt [m] + * @detail TODO(Satoshi Ota) + */ +boost::optional calcDecelDistPlanType3( + const double v0, const double vt, const double a0, const double ja) +{ + constexpr double epsilon = 1e-3; + + const double t_acc = (0.0 - a0) / ja; + + const double t1 = epsilon < t_acc ? t_acc : 0.0; + const double a1 = a0 + ja * t1; + const double v1 = v0 + a0 * t1 + 0.5 * ja * t1 * t1; + const double x1 = v0 * t1 + 0.5 * a0 * t1 * t1 + (1.0 / 6.0) * ja * t1 * t1 * t1; + + const double a_target = 0.0; + const double v_margin = 0.3; + const double a_margin = 0.1; + + if (!validCheckDecelPlan(v1, a1, vt, a_target, v_margin, a_margin)) { + return {}; + } + + return x1; +} +boost::optional calcDecelDistWithJerkAndAccConstraints( + const double current_vel, const double target_vel, const double current_acc, const double acc_min, + const double jerk_acc, const double jerk_dec) +{ + constexpr double epsilon = 1e-3; + const double t_dec = + acc_min < current_acc ? (acc_min - current_acc) / jerk_dec : (acc_min - current_acc) / jerk_acc; + const double t_acc = (0.0 - acc_min) / jerk_acc; + const double t_min = (target_vel - current_vel - current_acc * t_dec - + 0.5 * jerk_dec * t_dec * t_dec - 0.5 * acc_min * t_acc) / + acc_min; + + // check if it is possible to decelerate to the target velocity + // by simply bringing the current acceleration to zero. + const auto is_decel_needed = + 0.5 * (0.0 - current_acc) / jerk_acc * current_acc > target_vel - current_vel; + + if (t_min > epsilon) { + return calcDecelDistPlanType1( + current_vel, target_vel, current_acc, acc_min, jerk_acc, jerk_dec, t_min); + } else if (is_decel_needed || current_acc > epsilon) { + return calcDecelDistPlanType2(current_vel, target_vel, current_acc, jerk_acc, jerk_dec); + } else { + return calcDecelDistPlanType3(current_vel, target_vel, current_acc, jerk_acc); + } + + return {}; +} +boost::optional> calcFeasibleMarginAndVelocity( + const ObstacleStopPlannerNode::SlowDownParam & slow_down_param, + const double dist_baselink_to_obstacle, const double current_vel, const double current_acc) +{ + const auto & p = slow_down_param; + const auto & logger = rclcpp::get_logger("calcFeasibleMarginAndVelocity"); + constexpr double epsilon = 1e-4; + + if (current_vel < p.slow_down_vel + epsilon) { + return std::make_pair(p.forward_margin, p.slow_down_vel); + } + + for (double planning_jerk = p.jerk_start; planning_jerk > p.slow_down_min_jerk - epsilon; + planning_jerk += p.jerk_span) { + const double jerk_dec = planning_jerk; + const double jerk_acc = std::abs(planning_jerk); + + const auto planning_dec = + planning_jerk > p.normal_min_jerk ? p.limit_min_acc : p.normal_min_acc; + const auto stop_dist = calcDecelDistWithJerkAndAccConstraints( + current_vel, p.slow_down_vel, current_acc, planning_dec, jerk_acc, jerk_dec); + + if (!stop_dist) { + continue; + } + + if (stop_dist.get() + p.forward_margin < dist_baselink_to_obstacle) { + RCLCPP_DEBUG( + logger, "[found plan] dist:%-6.2f jerk:%-6.2f margin:%-6.2f v0:%-6.2f vt:%-6.2f", + stop_dist.get(), planning_jerk, p.forward_margin, p.slow_down_vel, current_vel); + return std::make_pair(p.forward_margin, p.slow_down_vel); + } + } + + { + const double jerk_dec = p.slow_down_min_jerk; + const double jerk_acc = std::abs(p.slow_down_min_jerk); + + const auto planning_dec = + p.slow_down_min_jerk > p.normal_min_jerk ? p.limit_min_acc : p.normal_min_acc; + const auto stop_dist = calcDecelDistWithJerkAndAccConstraints( + current_vel, p.slow_down_vel, current_acc, planning_dec, jerk_acc, jerk_dec); + + if (!stop_dist) { + return {}; + } + + if (stop_dist.get() + p.forward_margin_min < dist_baselink_to_obstacle) { + const auto planning_margin = dist_baselink_to_obstacle - stop_dist.get(); + RCLCPP_DEBUG( + logger, "[relax margin] dist:%-6.2f jerk:%-6.2f margin:%-6.2f v0:%-6.2f vt%-6.2f", + stop_dist.get(), p.slow_down_min_jerk, planning_margin, p.slow_down_vel, current_vel); + return std::make_pair(planning_margin, p.slow_down_vel); + } + } + + RCLCPP_DEBUG(logger, "relax target slow down velocity"); + return {}; +} +TrajectoryPoint getBackwardPointFromBasePoint( + const TrajectoryPoint & p_from, const TrajectoryPoint & p_to, const TrajectoryPoint & p_base, + const double backward_length) +{ + TrajectoryPoint output; + const double dx = p_to.pose.position.x - p_from.pose.position.x; + const double dy = p_to.pose.position.y - p_from.pose.position.y; + const double norm = std::hypot(dx, dy); + + output = p_base; + output.pose.position.x += backward_length * dx / norm; + output.pose.position.y += backward_length * dy / norm; + + return output; +} +boost::optional> getForwardInsertPointFromBasePoint( + const size_t base_idx, const TrajectoryPoints & trajectory, const double margin) +{ + if (base_idx + 1 > trajectory.size()) { + return {}; + } + + if (margin < std::numeric_limits::epsilon()) { + return std::make_pair(base_idx, trajectory.at(base_idx)); + } + + double length_sum = 0.0; + double length_residual = 0.0; + + for (size_t i = base_idx; i < trajectory.size() - 1; ++i) { + const auto & p_front = trajectory.at(i); + const auto & p_back = trajectory.at(i + 1); + + length_sum += calcDistance2d(p_front, p_back); + length_residual = length_sum - margin; + + if (length_residual >= std::numeric_limits::epsilon()) { + const auto p_insert = getBackwardPointFromBasePoint(p_back, p_front, p_back, length_residual); + + // p_front(trajectory.points.at(i)) is insert base point + return std::make_pair(i, p_insert); + } + } + + if (length_residual < std::numeric_limits::epsilon()) { + return std::make_pair(trajectory.size() - 1, trajectory.back()); + } + + return {}; +} +boost::optional> getBackwardInsertPointFromBasePoint( + const size_t base_idx, const TrajectoryPoints & trajectory, const double margin) +{ + if (base_idx + 1 > trajectory.size()) { + return {}; + } + + if (margin < std::numeric_limits::epsilon()) { + return std::make_pair(base_idx, trajectory.at(base_idx)); + } + + double length_sum = 0.0; + double length_residual = 0.0; + + for (size_t i = base_idx; 0 < i; --i) { + const auto & p_front = trajectory.at(i - 1); + const auto & p_back = trajectory.at(i); + + length_sum += calcDistance2d(p_front, p_back); + length_residual = length_sum - margin; + + if (length_residual >= std::numeric_limits::epsilon()) { + const auto p_insert = + getBackwardPointFromBasePoint(p_front, p_back, p_front, length_residual); + + // p_front(trajectory.points.at(i-1)) is insert base point + return std::make_pair(i - 1, p_insert); + } + } + + if (length_residual < std::numeric_limits::epsilon()) { + return std::make_pair(size_t(0), trajectory.front()); + } + + return {}; +} +boost::optional> findNearestFrontIndex( + const size_t start_idx, const TrajectoryPoints & trajectory, + const geometry_msgs::msg::Point & point) +{ + for (size_t i = start_idx; i < trajectory.size(); ++i) { + const auto & p_traj = trajectory.at(i).pose; + const auto yaw = getRPY(p_traj).z; + const Point2d p_traj_direction(std::cos(yaw), std::sin(yaw)); + const Point2d p_traj_to_target(point.x - p_traj.position.x, point.y - p_traj.position.y); + + const auto is_in_front_of_target_point = p_traj_direction.dot(p_traj_to_target) < 0.0; + const auto is_trajectory_end = i + 1 == trajectory.size(); + + if (is_in_front_of_target_point || is_trajectory_end) { + const auto dist_p_traj_to_target = p_traj_direction.normalized().dot(p_traj_to_target); + return std::make_pair(i, dist_p_traj_to_target); + } + } + + return {}; +} +bool isInFrontOfTargetPoint( + const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Point & point) +{ + const auto yaw = getRPY(pose).z; + const Point2d pose_direction(std::cos(yaw), std::sin(yaw)); + const Point2d to_target(point.x - pose.position.x, point.y - pose.position.y); + + return pose_direction.dot(to_target) < 0.0; +} +bool checkValidIndex( + const geometry_msgs::msg::Pose & p_base, const geometry_msgs::msg::Pose & p_next, + const geometry_msgs::msg::Pose & p_target) +{ + const Point2d base2target( + p_target.position.x - p_base.position.x, p_target.position.y - p_base.position.y); + const Point2d target2next( + p_next.position.x - p_target.position.x, p_next.position.y - p_target.position.y); + return base2target.dot(target2next) > 0.0; +} +std::string jsonDumpsPose(const geometry_msgs::msg::Pose & pose) +{ + const std::string json_dumps_pose = + (boost::format( + R"({"position":{"x":%lf,"y":%lf,"z":%lf},"orientation":{"w":%lf,"x":%lf,"y":%lf,"z":%lf}})") % + pose.position.x % pose.position.y % pose.position.z % pose.orientation.w % pose.orientation.x % + pose.orientation.y % pose.orientation.z) + .str(); + return json_dumps_pose; +} +diagnostic_msgs::msg::DiagnosticStatus makeStopReasonDiag( + const std::string stop_reason, const geometry_msgs::msg::Pose & stop_pose) +{ + diagnostic_msgs::msg::DiagnosticStatus stop_reason_diag; + diagnostic_msgs::msg::KeyValue stop_reason_diag_kv; + stop_reason_diag.level = diagnostic_msgs::msg::DiagnosticStatus::OK; + stop_reason_diag.name = "stop_reason"; + stop_reason_diag.message = stop_reason; + stop_reason_diag_kv.key = "stop_pose"; + stop_reason_diag_kv.value = jsonDumpsPose(stop_pose); + stop_reason_diag.values.push_back(stop_reason_diag_kv); + return stop_reason_diag; +} +} // namespace + +ObstacleStopPlannerNode::ObstacleStopPlannerNode(const rclcpp::NodeOptions & node_options) +: Node("obstacle_stop_planner", node_options) +{ + // Vehicle Parameters + vehicle_info_ = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo(); + + const auto & i = vehicle_info_; + + // Parameters + { + auto & p = node_param_; + p.enable_slow_down = declare_parameter("enable_slow_down", false); + p.max_velocity = declare_parameter("max_velocity", 20.0); + p.hunting_threshold = declare_parameter("hunting_threshold", 0.5); + p.lowpass_gain = declare_parameter("lowpass_gain", 0.9); + lpf_acc_ = std::make_shared(0.0, p.lowpass_gain); + } + + { + auto & p = stop_param_; + const std::string ns = "stop_planner."; + p.stop_margin = declare_parameter(ns + "stop_margin", 5.0); + p.min_behavior_stop_margin = declare_parameter(ns + "min_behavior_stop_margin", 2.0); + p.expand_stop_range = declare_parameter(ns + "expand_stop_range", 0.0); + p.extend_distance = declare_parameter(ns + "extend_distance", 0.0); + p.step_length = declare_parameter(ns + "step_length", 1.0); + p.stop_margin += i.max_longitudinal_offset_m; + p.min_behavior_stop_margin += i.max_longitudinal_offset_m; + p.stop_search_radius = + p.step_length + + std::hypot(i.vehicle_width_m / 2.0 + p.expand_stop_range, i.vehicle_length_m / 2.0); + } + + { + auto & p = slow_down_param_; + const std::string ns = "slow_down_planner."; + // common param + p.normal_min_jerk = declare_parameter("normal.min_jerk", -0.3); + p.normal_min_acc = declare_parameter("normal.min_acc", -1.0); + p.limit_min_jerk = declare_parameter("limit.min_jerk", -1.5); + p.limit_min_acc = declare_parameter("limit.min_acc", -2.5); + // slow down planner specific parameters + p.forward_margin = declare_parameter(ns + "forward_margin", 5.0); + p.backward_margin = declare_parameter(ns + "backward_margin", 5.0); + p.expand_slow_down_range = declare_parameter(ns + "expand_slow_down_range", 1.0); + p.max_slow_down_vel = declare_parameter(ns + "max_slow_down_vel", 4.0); + p.min_slow_down_vel = declare_parameter(ns + "min_slow_down_vel", 2.0); + // consider jerk/dec constraints in slow down + p.consider_constraints = declare_parameter(ns + "consider_constraints", false); + p.forward_margin_min = declare_parameter(ns + "forward_margin_min", 1.0); + p.forward_margin_span = declare_parameter(ns + "forward_margin_span", -0.1); + p.slow_down_min_jerk = declare_parameter(ns + "jerk_min_slow_down", -0.3); + p.jerk_start = declare_parameter(ns + "jerk_start", -0.1); + p.jerk_span = declare_parameter(ns + "jerk_span", -0.01); + p.slow_down_vel = declare_parameter(ns + "slow_down_vel", 1.39); + p.vel_threshold_reset_velocity_limit_ = + declare_parameter(ns + "vel_threshold_reset_velocity_limit_", 0.2); + p.dec_threshold_reset_velocity_limit_ = + declare_parameter(ns + "dec_threshold_reset_velocity_limit_", 0.1); + p.forward_margin += i.max_longitudinal_offset_m; + p.forward_margin_min += i.wheel_base_m + i.front_overhang_m; + p.backward_margin += i.rear_overhang_m; + p.slow_down_search_radius = + stop_param_.step_length + + std::hypot(i.vehicle_width_m / 2.0 + p.expand_slow_down_range, i.vehicle_length_m / 2.0); + } + + // Initializer + acc_controller_ = std::make_unique( + this, i.vehicle_width_m, i.vehicle_length_m, i.max_longitudinal_offset_m); + debug_ptr_ = std::make_shared(this, i.max_longitudinal_offset_m); + last_detection_time_ = this->now(); + + // Publishers + path_pub_ = this->create_publisher("~/output/trajectory", 1); + stop_reason_diag_pub_ = + this->create_publisher("~/output/stop_reason", 1); + pub_clear_velocity_limit_ = + this->create_publisher("~/output/velocity_limit_clear_command", 1); + pub_velocity_limit_ = this->create_publisher("~/output/max_velocity", 1); + + // Subscribers + obstacle_pointcloud_sub_ = this->create_subscription( + "~/input/pointcloud", rclcpp::SensorDataQoS(), + std::bind(&ObstacleStopPlannerNode::obstaclePointcloudCallback, this, std::placeholders::_1)); + path_sub_ = this->create_subscription( + "~/input/trajectory", 1, + std::bind(&ObstacleStopPlannerNode::pathCallback, this, std::placeholders::_1)); + current_velocity_sub_ = this->create_subscription( + "~/input/odometry", 1, + std::bind(&ObstacleStopPlannerNode::currentVelocityCallback, this, std::placeholders::_1)); + dynamic_object_sub_ = this->create_subscription( + "~/input/objects", 1, + std::bind(&ObstacleStopPlannerNode::dynamicObjectCallback, this, std::placeholders::_1)); + expand_stop_range_sub_ = this->create_subscription( + "~/input/expand_stop_range", 1, + std::bind( + &ObstacleStopPlannerNode::externalExpandStopRangeCallback, this, std::placeholders::_1)); +} + +void ObstacleStopPlannerNode::obstaclePointcloudCallback( + const sensor_msgs::msg::PointCloud2::ConstSharedPtr input_msg) +{ + obstacle_ros_pointcloud_ptr_ = std::make_shared(); + pcl::VoxelGrid filter; + pcl::PointCloud::Ptr pointcloud_ptr(new pcl::PointCloud); + pcl::PointCloud::Ptr no_height_pointcloud_ptr(new pcl::PointCloud); + pcl::PointCloud::Ptr no_height_filtered_pointcloud_ptr( + new pcl::PointCloud); + + pcl::fromROSMsg(*input_msg, *pointcloud_ptr); + + for (const auto & point : pointcloud_ptr->points) { + no_height_pointcloud_ptr->push_back(pcl::PointXYZ(point.x, point.y, 0.0)); + } + filter.setInputCloud(no_height_pointcloud_ptr); + filter.setLeafSize(0.05f, 0.05f, 100000.0f); + filter.filter(*no_height_filtered_pointcloud_ptr); + pcl::toROSMsg(*no_height_filtered_pointcloud_ptr, *obstacle_ros_pointcloud_ptr_); + obstacle_ros_pointcloud_ptr_->header = input_msg->header; +} + +void ObstacleStopPlannerNode::pathCallback(const Trajectory::ConstSharedPtr input_msg) +{ + if (!obstacle_ros_pointcloud_ptr_) { + RCLCPP_WARN_THROTTLE( + get_logger(), *get_clock(), std::chrono::milliseconds(1000).count(), + "waiting for obstacle pointcloud..."); + return; + } + + if (!current_velocity_ptr_ && node_param_.enable_slow_down) { + RCLCPP_WARN_THROTTLE( + get_logger(), *get_clock(), std::chrono::milliseconds(1000).count(), + "waiting for current velocity..."); + return; + } + + if (input_msg->points.empty()) { + return; + } + + PlannerData planner_data{}; + + getSelfPose(input_msg->header, tf_buffer_, planner_data.current_pose); + + Trajectory output_trajectory = *input_msg; + TrajectoryPoints output_trajectory_points = + autoware_utils::convertToTrajectoryPointArray(*input_msg); + + // trim trajectory from self pose + const auto base_trajectory = trimTrajectoryWithIndexFromSelfPose( + autoware_utils::convertToTrajectoryPointArray(*input_msg), planner_data.current_pose, + planner_data.trajectory_trim_index); + // extend trajectory to consider obstacles after the goal + const auto extend_trajectory = extendTrajectory(base_trajectory, stop_param_.extend_distance); + // decimate trajectory for calculation cost + const auto decimate_trajectory = decimateTrajectory( + extend_trajectory, stop_param_.step_length, planner_data.decimate_trajectory_index_map); + + // search obstacles within slow-down/collision area + searchObstacle(decimate_trajectory, output_trajectory_points, planner_data, input_msg->header); + // insert slow-down-section/stop-point + insertVelocity(output_trajectory_points, planner_data, input_msg->header); + + const auto no_slow_down_section = !planner_data.slow_down_require && !latest_slow_down_section_; + const auto no_hunting = (rclcpp::Time(input_msg->header.stamp) - last_detection_time_).seconds() > + node_param_.hunting_threshold; + if (node_param_.enable_slow_down && no_slow_down_section && set_velocity_limit_ && no_hunting) { + resetExternalVelocityLimit(); + } + + auto trajectory = autoware_utils::convertToTrajectory(output_trajectory_points); + trajectory.header = input_msg->header; + path_pub_->publish(trajectory); + publishDebugData(planner_data); +} + +void ObstacleStopPlannerNode::searchObstacle( + const TrajectoryPoints & decimate_trajectory, TrajectoryPoints & output, + PlannerData & planner_data, const std_msgs::msg::Header & trajectory_header) +{ + // search candidate obstacle pointcloud + pcl::PointCloud::Ptr slow_down_pointcloud_ptr(new pcl::PointCloud); + pcl::PointCloud::Ptr obstacle_candidate_pointcloud_ptr( + new pcl::PointCloud); + if (!searchPointcloudNearTrajectory( + decimate_trajectory, obstacle_ros_pointcloud_ptr_, obstacle_candidate_pointcloud_ptr, + trajectory_header)) { + return; + } + + for (size_t i = 0; i < decimate_trajectory.size() - 1; ++i) { + // create one step circle center for vehicle + const auto & p_front = decimate_trajectory.at(i).pose; + const auto & p_back = decimate_trajectory.at(i + 1).pose; + const auto prev_center_pose = getVehicleCenterFromBase(p_front); + const Point2d prev_center_point(prev_center_pose.position.x, prev_center_pose.position.y); + const auto next_center_pose = getVehicleCenterFromBase(p_back); + const Point2d next_center_point(next_center_pose.position.x, next_center_pose.position.y); + + if (node_param_.enable_slow_down) { + std::vector one_step_move_slow_down_range_polygon; + // create one step polygon for slow_down range + createOneStepPolygon( + p_front, p_back, one_step_move_slow_down_range_polygon, + slow_down_param_.expand_slow_down_range); + debug_ptr_->pushPolygon( + one_step_move_slow_down_range_polygon, p_front.position.z, PolygonType::SlowDownRange); + + planner_data.found_slow_down_points = withinPolygon( + one_step_move_slow_down_range_polygon, slow_down_param_.slow_down_search_radius, + prev_center_point, next_center_point, obstacle_candidate_pointcloud_ptr, + slow_down_pointcloud_ptr); + + const auto found_first_slow_down_points = + planner_data.found_slow_down_points && !planner_data.slow_down_require; + + if (found_first_slow_down_points) { + // found nearest slow down obstacle + planner_data.decimate_trajectory_slow_down_index = i; + planner_data.slow_down_require = true; + getNearestPoint( + *slow_down_pointcloud_ptr, p_front, &planner_data.nearest_slow_down_point, + &planner_data.nearest_collision_point_time); + getLateralNearestPoint( + *slow_down_pointcloud_ptr, p_front, &planner_data.lateral_nearest_slow_down_point, + &planner_data.lateral_deviation); + + debug_ptr_->pushObstaclePoint(planner_data.nearest_slow_down_point, PointType::SlowDown); + debug_ptr_->pushPolygon( + one_step_move_slow_down_range_polygon, p_front.position.z, PolygonType::SlowDown); + } + + } else { + slow_down_pointcloud_ptr = obstacle_candidate_pointcloud_ptr; + } + + { + std::vector one_step_move_vehicle_polygon; + // create one step polygon for vehicle + createOneStepPolygon( + p_front, p_back, one_step_move_vehicle_polygon, stop_param_.expand_stop_range); + debug_ptr_->pushPolygon( + one_step_move_vehicle_polygon, decimate_trajectory.at(i).pose.position.z, + PolygonType::Vehicle); + + pcl::PointCloud::Ptr collision_pointcloud_ptr( + new pcl::PointCloud); + collision_pointcloud_ptr->header = obstacle_candidate_pointcloud_ptr->header; + + planner_data.found_collision_points = withinPolygon( + one_step_move_vehicle_polygon, stop_param_.stop_search_radius, prev_center_point, + next_center_point, slow_down_pointcloud_ptr, collision_pointcloud_ptr); + + if (planner_data.found_collision_points) { + planner_data.decimate_trajectory_collision_index = i; + getNearestPoint( + *collision_pointcloud_ptr, p_front, &planner_data.nearest_collision_point, + &planner_data.nearest_collision_point_time); + + debug_ptr_->pushObstaclePoint(planner_data.nearest_collision_point, PointType::Stop); + debug_ptr_->pushPolygon( + one_step_move_vehicle_polygon, p_front.position.z, PolygonType::Collision); + + planner_data.stop_require = planner_data.found_collision_points; + acc_controller_->insertAdaptiveCruiseVelocity( + decimate_trajectory, planner_data.decimate_trajectory_collision_index, + planner_data.current_pose, planner_data.nearest_collision_point, + planner_data.nearest_collision_point_time, object_ptr_, current_velocity_ptr_, + &planner_data.stop_require, &output, trajectory_header); + + break; + } + } + } +} + +void ObstacleStopPlannerNode::insertVelocity( + TrajectoryPoints & output, PlannerData & planner_data, + const std_msgs::msg::Header & trajectory_header) +{ + if (planner_data.stop_require) { + // insert stop point + const auto traj_end_idx = output.size() - 1; + const auto idx = planner_data.decimate_trajectory_index_map.at( + planner_data.decimate_trajectory_collision_index) + + planner_data.trajectory_trim_index; + const auto index_with_dist_remain = findNearestFrontIndex( + std::min(idx, traj_end_idx), output, + createPoint( + planner_data.nearest_collision_point.x, planner_data.nearest_collision_point.y, 0)); + + if (index_with_dist_remain) { + const auto stop_point = searchInsertPoint( + index_with_dist_remain.get().first, output, index_with_dist_remain.get().second); + insertStopPoint(stop_point, output, planner_data.stop_reason_diag); + } + } + + if (planner_data.slow_down_require) { + // insert slow down point + const auto traj_end_idx = output.size() - 1; + const auto idx = planner_data.decimate_trajectory_index_map.at( + planner_data.decimate_trajectory_slow_down_index); + const auto index_with_dist_remain = findNearestFrontIndex( + std::min(idx, traj_end_idx), output, + createPoint( + planner_data.nearest_slow_down_point.x, planner_data.nearest_slow_down_point.y, 0)); + + if (index_with_dist_remain) { + const auto vehicle_idx = std::min(planner_data.trajectory_trim_index, traj_end_idx); + const auto dist_baselink_to_obstacle = + calcSignedArcLength(output, vehicle_idx, index_with_dist_remain.get().first); + + debug_ptr_->setDebugValues( + DebugValues::TYPE::OBSTACLE_DISTANCE, + dist_baselink_to_obstacle + index_with_dist_remain.get().second); + const auto slow_down_section = createSlowDownSection( + index_with_dist_remain.get().first, output, planner_data.lateral_deviation, + index_with_dist_remain.get().second, dist_baselink_to_obstacle); + + if ( + !latest_slow_down_section_ && + dist_baselink_to_obstacle + index_with_dist_remain.get().second < + vehicle_info_.max_longitudinal_offset_m) { + latest_slow_down_section_ = slow_down_section; + } + + insertSlowDownSection(slow_down_section, output); + } + + last_detection_time_ = trajectory_header.stamp; + } + + if (node_param_.enable_slow_down && latest_slow_down_section_) { + // check whether ego is in slow down section or not + const auto & p_start = latest_slow_down_section_.get().start_point.pose.position; + const auto & p_end = latest_slow_down_section_.get().end_point.pose.position; + const auto reach_slow_down_start_point = + isInFrontOfTargetPoint(planner_data.current_pose, p_start); + const auto reach_slow_down_end_point = isInFrontOfTargetPoint(planner_data.current_pose, p_end); + const auto is_in_slow_down_section = reach_slow_down_start_point && !reach_slow_down_end_point; + const auto index_with_dist_remain = findNearestFrontIndex(0, output, p_end); + + if (is_in_slow_down_section && index_with_dist_remain) { + const auto end_insert_point_with_idx = getBackwardInsertPointFromBasePoint( + index_with_dist_remain.get().first, output, -index_with_dist_remain.get().second); + + SlowDownSection slow_down_section{}; + slow_down_section.slow_down_start_idx = 0; + slow_down_section.start_point = output.front(); + slow_down_section.slow_down_end_idx = end_insert_point_with_idx.get().first; + slow_down_section.end_point = end_insert_point_with_idx.get().second; + slow_down_section.velocity = + set_velocity_limit_ ? std::numeric_limits::max() : slow_down_param_.slow_down_vel; + + insertSlowDownSection(slow_down_section, output); + } else { + latest_slow_down_section_ = {}; + } + } + + for (size_t i = 0; i < output.size() - 2; ++i) { + const auto & p_base = output.at(i).pose; + const auto & p_target = output.at(i + 1).pose; + const auto & p_next = output.at(i + 2).pose; + if (!checkValidIndex(p_base, p_next, p_target)) { + RCLCPP_ERROR(get_logger(), "detect bad index"); + } + } + + stop_reason_diag_pub_->publish(planner_data.stop_reason_diag); +} + +bool ObstacleStopPlannerNode::withinPolygon( + const std::vector & cv_polygon, const double radius, const Point2d & prev_point, + const Point2d & next_point, pcl::PointCloud::Ptr candidate_points_ptr, + pcl::PointCloud::Ptr within_points_ptr) +{ + Polygon2d boost_polygon; + bool find_within_points = false; + for (const auto & point : cv_polygon) { + boost_polygon.outer().push_back(bg::make(point.x, point.y)); + } + boost_polygon.outer().push_back(bg::make(cv_polygon.front().x, cv_polygon.front().y)); + + for (size_t j = 0; j < candidate_points_ptr->size(); ++j) { + Point2d point(candidate_points_ptr->at(j).x, candidate_points_ptr->at(j).y); + if (bg::distance(prev_point, point) < radius || bg::distance(next_point, point) < radius) { + if (bg::within(point, boost_polygon)) { + within_points_ptr->push_back(candidate_points_ptr->at(j)); + find_within_points = true; + } + } + } + return find_within_points; +} + +void ObstacleStopPlannerNode::externalExpandStopRangeCallback( + const ExpandStopRange::ConstSharedPtr input_msg) +{ + const auto & i = vehicle_info_; + stop_param_.expand_stop_range = input_msg->expand_stop_range; + stop_param_.stop_search_radius = + stop_param_.step_length + + std::hypot(i.vehicle_width_m / 2.0 + stop_param_.expand_stop_range, i.vehicle_length_m / 2.0); +} + +void ObstacleStopPlannerNode::insertStopPoint( + const StopPoint & stop_point, TrajectoryPoints & output, + diagnostic_msgs::msg::DiagnosticStatus & stop_reason_diag) +{ + const auto traj_end_idx = output.size() - 1; + const auto & stop_idx = stop_point.index; + + const auto & p_base = output.at(stop_idx); + const auto & p_next = output.at(std::min(stop_idx + 1, traj_end_idx)); + const auto & p_insert = stop_point.point; + + constexpr double min_dist = 1e-3; + + const auto is_p_base_and_p_insert_overlap = calcDistance2d(p_base, p_insert) < min_dist; + const auto is_p_next_and_p_insert_overlap = calcDistance2d(p_next, p_insert) < min_dist; + const auto is_valid_index = checkValidIndex(p_base.pose, p_next.pose, p_insert.pose); + + auto update_stop_idx = stop_idx; + + if (!is_p_base_and_p_insert_overlap && !is_p_next_and_p_insert_overlap && is_valid_index) { + // insert: start_idx and end_idx are shifted by one + output.insert(output.begin() + stop_idx + 1, p_insert); + update_stop_idx = std::min(update_stop_idx + 1, traj_end_idx); + } else if (is_p_next_and_p_insert_overlap) { + // not insert: p_insert is merged into p_next + update_stop_idx = std::min(update_stop_idx + 1, traj_end_idx); + } + + for (size_t i = update_stop_idx; i < output.size(); ++i) { + output.at(i).longitudinal_velocity_mps = 0.0; + } + + stop_reason_diag = makeStopReasonDiag("obstacle", p_insert.pose); + debug_ptr_->pushPose(p_insert.pose, PoseType::Stop); +} + +StopPoint ObstacleStopPlannerNode::searchInsertPoint( + const int idx, const TrajectoryPoints & base_trajectory, const double dist_remain) +{ + const auto max_dist_stop_point = + createTargetPoint(idx, stop_param_.stop_margin, base_trajectory, dist_remain); + const auto min_dist_stop_point = + createTargetPoint(idx, stop_param_.min_behavior_stop_margin, base_trajectory, dist_remain); + + // check if stop point is already inserted by behavior planner + bool is_inserted_already_stop_point = false; + const double epsilon = 1e-3; + for (int j = max_dist_stop_point.index - 1; j < static_cast(idx); ++j) { + if (std::abs(base_trajectory.at(std::max(j, 0)).longitudinal_velocity_mps) < epsilon) { + is_inserted_already_stop_point = true; + break; + } + } + // insert stop point + StopPoint stop_point{}; + stop_point.index = + !is_inserted_already_stop_point ? max_dist_stop_point.index : min_dist_stop_point.index; + stop_point.point = + !is_inserted_already_stop_point ? max_dist_stop_point.point : min_dist_stop_point.point; + return stop_point; +} + +StopPoint ObstacleStopPlannerNode::createTargetPoint( + const int idx, const double margin, const TrajectoryPoints & base_trajectory, + const double dist_remain) +{ + const auto update_margin_from_vehicle = margin - dist_remain; + const auto insert_point_with_idx = + getBackwardInsertPointFromBasePoint(idx, base_trajectory, update_margin_from_vehicle); + + if (!insert_point_with_idx) { + // TODO(Satoshi Ota) + return StopPoint{}; + } + + StopPoint stop_point{}; + stop_point.index = insert_point_with_idx.get().first; + stop_point.point = insert_point_with_idx.get().second; + + return stop_point; +} + +SlowDownSection ObstacleStopPlannerNode::createSlowDownSection( + const int idx, const TrajectoryPoints & base_trajectory, const double lateral_deviation, + const double dist_remain, const double dist_baselink_to_obstacle) +{ + if (!current_velocity_ptr_) { + // TODO(Satoshi Ota) + return SlowDownSection{}; + } + + if (slow_down_param_.consider_constraints) { + const auto & current_vel = current_velocity_ptr_->twist.twist.linear.x; + const auto margin_with_vel = calcFeasibleMarginAndVelocity( + slow_down_param_, dist_baselink_to_obstacle + dist_remain, current_vel, current_acc_); + + const auto relax_target_vel = margin_with_vel == boost::none; + if (relax_target_vel && !set_velocity_limit_) { + setExternalVelocityLimit(); + } + + const auto no_need_velocity_limit = + dist_baselink_to_obstacle + dist_remain > slow_down_param_.forward_margin; + if (set_velocity_limit_ && no_need_velocity_limit) { + resetExternalVelocityLimit(); + } + + const auto use_velocity_limit = relax_target_vel || set_velocity_limit_; + + const auto update_forward_margin_from_vehicle = + use_velocity_limit ? slow_down_param_.forward_margin_min - dist_remain + : margin_with_vel.get().first - dist_remain; + const auto update_backward_margin_from_vehicle = slow_down_param_.backward_margin + dist_remain; + + const auto velocity = + use_velocity_limit ? std::numeric_limits::max() : margin_with_vel.get().second; + + return createSlowDownSectionFromMargin( + idx, base_trajectory, update_forward_margin_from_vehicle, update_backward_margin_from_vehicle, + velocity); + } else { + const auto update_forward_margin_from_vehicle = slow_down_param_.forward_margin - dist_remain; + const auto update_backward_margin_from_vehicle = slow_down_param_.backward_margin + dist_remain; + + const auto velocity = + slow_down_param_.min_slow_down_vel + + (slow_down_param_.max_slow_down_vel - slow_down_param_.min_slow_down_vel) * + std::max(lateral_deviation - vehicle_info_.vehicle_width_m / 2, 0.0) / + slow_down_param_.expand_slow_down_range; + + return createSlowDownSectionFromMargin( + idx, base_trajectory, update_forward_margin_from_vehicle, update_backward_margin_from_vehicle, + velocity); + } + + return SlowDownSection{}; +} + +SlowDownSection ObstacleStopPlannerNode::createSlowDownSectionFromMargin( + const int idx, const TrajectoryPoints & base_trajectory, const double forward_margin, + const double backward_margin, const double velocity) +{ + // calc slow down start point + const auto start_insert_point_with_idx = + getBackwardInsertPointFromBasePoint(idx, base_trajectory, forward_margin); + // calc slow down end point + const auto end_insert_point_with_idx = + getForwardInsertPointFromBasePoint(idx, base_trajectory, backward_margin); + + if (!start_insert_point_with_idx || !end_insert_point_with_idx) { + // TODO(Satoshi Ota) + return SlowDownSection{}; + } + + SlowDownSection slow_down_section{}; + slow_down_section.slow_down_start_idx = start_insert_point_with_idx.get().first; + slow_down_section.start_point = start_insert_point_with_idx.get().second; + slow_down_section.slow_down_end_idx = end_insert_point_with_idx.get().first; + slow_down_section.end_point = end_insert_point_with_idx.get().second; + slow_down_section.velocity = velocity; + + return slow_down_section; +} + +void ObstacleStopPlannerNode::insertSlowDownSection( + const SlowDownSection & slow_down_section, TrajectoryPoints & output) +{ + const auto traj_end_idx = output.size() - 1; + const auto & start_idx = slow_down_section.slow_down_start_idx; + const auto & end_idx = slow_down_section.slow_down_end_idx; + + const auto p_base_start = output.at(start_idx); + const auto p_next_start = output.at(std::min(start_idx + 1, traj_end_idx)); + const auto & p_insert_start = slow_down_section.start_point; + + const auto p_base_end = output.at(end_idx); + const auto p_next_end = output.at(std::min(end_idx + 1, traj_end_idx)); + const auto & p_insert_end = slow_down_section.end_point; + + constexpr double min_dist = 1e-3; + + const auto is_valid_index_start = + checkValidIndex(p_base_start.pose, p_next_start.pose, p_insert_start.pose); + const auto is_start_p_base_and_p_insert_overlap = + calcDistance2d(p_base_start, p_insert_start) < min_dist; + const auto is_start_p_next_and_p_insert_overlap = + calcDistance2d(p_next_start, p_insert_start) < min_dist; + + auto update_start_idx = start_idx; + auto update_end_idx = end_idx; + + if ( + !is_start_p_base_and_p_insert_overlap && !is_start_p_next_and_p_insert_overlap && + is_valid_index_start) { + // insert: start_idx and end_idx are shifted by one + output.insert(output.begin() + start_idx + 1, p_insert_start); + update_start_idx = std::min(update_start_idx + 1, traj_end_idx); + update_end_idx = std::min(update_end_idx + 1, traj_end_idx); + } else if (is_start_p_next_and_p_insert_overlap) { + // not insert: p_insert is merged into p_next + update_start_idx = std::min(update_start_idx + 1, traj_end_idx); + } + + const auto is_end_p_base_and_p_insert_overlap = + calcDistance2d(p_base_end, p_insert_end) < min_dist; + const auto is_end_p_next_and_p_insert_overlap = + calcDistance2d(p_next_end, p_insert_end) < min_dist; + const auto is_valid_index_end = + checkValidIndex(p_base_end.pose, p_next_end.pose, p_insert_end.pose); + + if ( + !is_end_p_base_and_p_insert_overlap && !is_end_p_next_and_p_insert_overlap && + is_valid_index_end) { + // insert: end_idx is shifted by one + output.insert(output.begin() + update_end_idx + 1, p_insert_end); + update_end_idx = std::min(update_end_idx + 1, traj_end_idx); + } else if (is_end_p_next_and_p_insert_overlap) { + // not insert: p_insert is merged into p_next + update_end_idx = std::min(update_end_idx + 1, traj_end_idx); + } + + for (size_t i = update_start_idx; i <= update_end_idx; ++i) { + output.at(i).longitudinal_velocity_mps = std::min( + slow_down_section.velocity, static_cast(output.at(i).longitudinal_velocity_mps)); + } + + debug_ptr_->pushPose(p_base_start.pose, PoseType::SlowDownStart); + debug_ptr_->pushPose(p_base_end.pose, PoseType::SlowDownEnd); +} + +void ObstacleStopPlannerNode::dynamicObjectCallback( + const PredictedObjects::ConstSharedPtr input_msg) +{ + object_ptr_ = input_msg; +} + +void ObstacleStopPlannerNode::currentVelocityCallback( + const nav_msgs::msg::Odometry::ConstSharedPtr input_msg) +{ + current_velocity_ptr_ = input_msg; + + if (!prev_velocity_ptr_) { + prev_velocity_ptr_ = current_velocity_ptr_; + return; + } + + const double dv = + current_velocity_ptr_->twist.twist.linear.x - prev_velocity_ptr_->twist.twist.linear.x; + const double dt = std::max( + (rclcpp::Time(current_velocity_ptr_->header.stamp) - + rclcpp::Time(prev_velocity_ptr_->header.stamp)) + .seconds(), + 1e-03); + + const double accel = dv / dt; + + current_acc_ = lpf_acc_->filter(accel); + prev_velocity_ptr_ = current_velocity_ptr_; +} + +TrajectoryPoint ObstacleStopPlannerNode::getExtendTrajectoryPoint( + const double extend_distance, const TrajectoryPoint & goal_point) +{ + tf2::Transform map2goal; + tf2::fromMsg(goal_point.pose, map2goal); + tf2::Transform local_extend_point; + local_extend_point.setOrigin(tf2::Vector3(extend_distance, 0.0, 0.0)); + tf2::Quaternion q; + q.setRPY(0, 0, 0); + local_extend_point.setRotation(q); + const auto map2extend_point = map2goal * local_extend_point; + geometry_msgs::msg::Pose extend_pose; + tf2::toMsg(map2extend_point, extend_pose); + TrajectoryPoint extend_trajectory_point; + extend_trajectory_point.pose = extend_pose; + extend_trajectory_point.longitudinal_velocity_mps = goal_point.longitudinal_velocity_mps; + extend_trajectory_point.lateral_velocity_mps = goal_point.lateral_velocity_mps; + extend_trajectory_point.acceleration_mps2 = goal_point.acceleration_mps2; + return extend_trajectory_point; +} + +TrajectoryPoints ObstacleStopPlannerNode::extendTrajectory( + const TrajectoryPoints & input, const double extend_distance) +{ + TrajectoryPoints output = input; + + if (extend_distance < std::numeric_limits::epsilon()) { + return output; + } + + const auto goal_point = input.back(); + double interpolation_distance = 0.1; + + double extend_sum = 0.0; + while (extend_sum <= (extend_distance - interpolation_distance)) { + const auto extend_trajectory_point = getExtendTrajectoryPoint(extend_sum, goal_point); + output.push_back(extend_trajectory_point); + extend_sum += interpolation_distance; + } + const auto extend_trajectory_point = getExtendTrajectoryPoint(extend_distance, goal_point); + output.push_back(extend_trajectory_point); + + return output; +} + +TrajectoryPoints ObstacleStopPlannerNode::decimateTrajectory( + const TrajectoryPoints & input, const double step_length, + std::map & index_map) +{ + TrajectoryPoints output{}; + + double trajectory_length_sum = 0.0; + double next_length = 0.0; + + for (int i = 0; i < static_cast(input.size()) - 1; ++i) { + const auto & p_front = input.at(i); + const auto & p_back = input.at(i + 1); + constexpr double epsilon = 1e-3; + + if (next_length <= trajectory_length_sum + epsilon) { + const auto p_interpolate = + getBackwardPointFromBasePoint(p_front, p_back, p_back, next_length - trajectory_length_sum); + output.push_back(p_interpolate); + + index_map.insert(std::make_pair(output.size() - 1, size_t(i))); + next_length += step_length; + continue; + } + + trajectory_length_sum += calcDistance2d(p_front, p_back); + } + if (!input.empty()) { + output.push_back(input.back()); + index_map.insert(std::make_pair(output.size() - 1, input.size() - 1)); + } + + return output; +} + +TrajectoryPoints ObstacleStopPlannerNode::trimTrajectoryWithIndexFromSelfPose( + const TrajectoryPoints & input, const geometry_msgs::msg::Pose & self_pose, size_t & index) +{ + TrajectoryPoints output{}; + + double min_distance = 0.0; + size_t min_distance_index = 0; + bool is_init = false; + for (size_t i = 0; i < input.size(); ++i) { + const double x = input.at(i).pose.position.x - self_pose.position.x; + const double y = input.at(i).pose.position.y - self_pose.position.y; + const double squared_distance = x * x + y * y; + if (!is_init || squared_distance < min_distance * min_distance) { + is_init = true; + min_distance = std::sqrt(squared_distance); + min_distance_index = i; + } + } + for (size_t i = min_distance_index; i < input.size(); ++i) { + output.push_back(input.at(i)); + } + index = min_distance_index; + + return output; +} + +bool ObstacleStopPlannerNode::searchPointcloudNearTrajectory( + const TrajectoryPoints & trajectory, + const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_points_ptr, + pcl::PointCloud::Ptr output_points_ptr, + const std_msgs::msg::Header & trajectory_header) +{ + // transform pointcloud + geometry_msgs::msg::TransformStamped transform_stamped{}; + try { + transform_stamped = tf_buffer_.lookupTransform( + trajectory_header.frame_id, input_points_ptr->header.frame_id, input_points_ptr->header.stamp, + rclcpp::Duration::from_seconds(0.5)); + } catch (tf2::TransformException & ex) { + RCLCPP_ERROR_STREAM( + get_logger(), "Failed to look up transform from " << trajectory_header.frame_id << " to " + << input_points_ptr->header.frame_id); + return false; + } + + sensor_msgs::msg::PointCloud2 transformed_points{}; + const Eigen::Matrix4f affine_matrix = + tf2::transformToEigen(transform_stamped.transform).matrix().cast(); + pcl_ros::transformPointCloud(affine_matrix, *input_points_ptr, transformed_points); + pcl::PointCloud::Ptr transformed_points_ptr(new pcl::PointCloud); + pcl::fromROSMsg(transformed_points, *transformed_points_ptr); + + output_points_ptr->header = transformed_points_ptr->header; + + // search obstacle candidate pointcloud to reduce calculation cost + const double search_radius = node_param_.enable_slow_down + ? slow_down_param_.slow_down_search_radius + : stop_param_.stop_search_radius; + const double squared_radius = search_radius * search_radius; + for (const auto & trajectory_point : trajectory) { + const auto center_pose = getVehicleCenterFromBase(trajectory_point.pose); + for (const auto & point : transformed_points_ptr->points) { + const double x = center_pose.position.x - point.x; + const double y = center_pose.position.y - point.y; + const double squared_distance = x * x + y * y; + if (squared_distance < squared_radius) { + output_points_ptr->points.push_back(point); + } + } + } + return true; +} + +void ObstacleStopPlannerNode::createOneStepPolygon( + const geometry_msgs::msg::Pose & base_step_pose, const geometry_msgs::msg::Pose & next_step_pose, + std::vector & polygon, const double expand_width) +{ + std::vector one_step_move_vehicle_corner_points; + + const auto & i = vehicle_info_; + const auto & front_m = i.max_longitudinal_offset_m; + const auto & width_m = i.vehicle_width_m / 2.0 + expand_width; + const auto & back_m = i.rear_overhang_m; + // start step + { + const auto yaw = getRPY(base_step_pose).z; + one_step_move_vehicle_corner_points.push_back(cv::Point2d( + base_step_pose.position.x + std::cos(yaw) * front_m - std::sin(yaw) * width_m, + base_step_pose.position.y + std::sin(yaw) * front_m + std::cos(yaw) * width_m)); + one_step_move_vehicle_corner_points.push_back(cv::Point2d( + base_step_pose.position.x + std::cos(yaw) * front_m - std::sin(yaw) * -width_m, + base_step_pose.position.y + std::sin(yaw) * front_m + std::cos(yaw) * -width_m)); + one_step_move_vehicle_corner_points.push_back(cv::Point2d( + base_step_pose.position.x + std::cos(yaw) * -back_m - std::sin(yaw) * -width_m, + base_step_pose.position.y + std::sin(yaw) * -back_m + std::cos(yaw) * -width_m)); + one_step_move_vehicle_corner_points.push_back(cv::Point2d( + base_step_pose.position.x + std::cos(yaw) * -back_m - std::sin(yaw) * width_m, + base_step_pose.position.y + std::sin(yaw) * -back_m + std::cos(yaw) * width_m)); + } + // next step + { + const auto yaw = getRPY(next_step_pose).z; + one_step_move_vehicle_corner_points.push_back(cv::Point2d( + next_step_pose.position.x + std::cos(yaw) * front_m - std::sin(yaw) * width_m, + next_step_pose.position.y + std::sin(yaw) * front_m + std::cos(yaw) * width_m)); + one_step_move_vehicle_corner_points.push_back(cv::Point2d( + next_step_pose.position.x + std::cos(yaw) * front_m - std::sin(yaw) * -width_m, + next_step_pose.position.y + std::sin(yaw) * front_m + std::cos(yaw) * -width_m)); + one_step_move_vehicle_corner_points.push_back(cv::Point2d( + next_step_pose.position.x + std::cos(yaw) * -back_m - std::sin(yaw) * -width_m, + next_step_pose.position.y + std::sin(yaw) * -back_m + std::cos(yaw) * -width_m)); + one_step_move_vehicle_corner_points.push_back(cv::Point2d( + next_step_pose.position.x + std::cos(yaw) * -back_m - std::sin(yaw) * width_m, + next_step_pose.position.y + std::sin(yaw) * -back_m + std::cos(yaw) * width_m)); + } + convexHull(one_step_move_vehicle_corner_points, polygon); +} + +bool ObstacleStopPlannerNode::convexHull( + const std::vector & pointcloud, std::vector & polygon_points) +{ + cv::Point2d centroid; + centroid.x = 0; + centroid.y = 0; + for (const auto & point : pointcloud) { + centroid.x += point.x; + centroid.y += point.y; + } + centroid.x = centroid.x / static_cast(pointcloud.size()); + centroid.y = centroid.y / static_cast(pointcloud.size()); + + std::vector normalized_pointcloud; + std::vector normalized_polygon_points; + for (size_t i = 0; i < pointcloud.size(); ++i) { + normalized_pointcloud.push_back(cv::Point( + (pointcloud.at(i).x - centroid.x) * 1000.0, (pointcloud.at(i).y - centroid.y) * 1000.0)); + } + cv::convexHull(normalized_pointcloud, normalized_polygon_points); + + for (size_t i = 0; i < normalized_polygon_points.size(); ++i) { + cv::Point2d polygon_point; + polygon_point.x = (normalized_polygon_points.at(i).x / 1000.0 + centroid.x); + polygon_point.y = (normalized_polygon_points.at(i).y / 1000.0 + centroid.y); + polygon_points.push_back(polygon_point); + } + return true; +} + +bool ObstacleStopPlannerNode::getSelfPose( + const std_msgs::msg::Header & header, const tf2_ros::Buffer & tf_buffer, + geometry_msgs::msg::Pose & self_pose) +{ + try { + geometry_msgs::msg::TransformStamped transform; + transform = tf_buffer.lookupTransform( + header.frame_id, "base_link", header.stamp, rclcpp::Duration::from_seconds(0.1)); + self_pose.position.x = transform.transform.translation.x; + self_pose.position.y = transform.transform.translation.y; + self_pose.position.z = transform.transform.translation.z; + self_pose.orientation.x = transform.transform.rotation.x; + self_pose.orientation.y = transform.transform.rotation.y; + self_pose.orientation.z = transform.transform.rotation.z; + self_pose.orientation.w = transform.transform.rotation.w; + return true; + } catch (tf2::TransformException & ex) { + return false; + } +} + +void ObstacleStopPlannerNode::getNearestPoint( + const pcl::PointCloud & pointcloud, const geometry_msgs::msg::Pose & base_pose, + pcl::PointXYZ * nearest_collision_point, rclcpp::Time * nearest_collision_point_time) +{ + double min_norm = 0.0; + bool is_init = false; + const auto yaw = getRPY(base_pose).z; + const Eigen::Vector2d base_pose_vec(std::cos(yaw), std::sin(yaw)); + + for (size_t i = 0; i < pointcloud.size(); ++i) { + const Eigen::Vector2d pointcloud_vec( + pointcloud.at(i).x - base_pose.position.x, pointcloud.at(i).y - base_pose.position.y); + double norm = base_pose_vec.dot(pointcloud_vec); + if (norm < min_norm || !is_init) { + min_norm = norm; + *nearest_collision_point = pointcloud.at(i); + *nearest_collision_point_time = pcl_conversions::fromPCL(pointcloud.header).stamp; + is_init = true; + } + } +} + +void ObstacleStopPlannerNode::getLateralNearestPoint( + const pcl::PointCloud & pointcloud, const geometry_msgs::msg::Pose & base_pose, + pcl::PointXYZ * lateral_nearest_point, double * deviation) +{ + double min_norm = std::numeric_limits::max(); + const auto yaw = getRPY(base_pose).z; + const Eigen::Vector2d base_pose_vec(std::cos(yaw), std::sin(yaw)); + for (size_t i = 0; i < pointcloud.size(); ++i) { + const Eigen::Vector2d pointcloud_vec( + pointcloud.at(i).x - base_pose.position.x, pointcloud.at(i).y - base_pose.position.y); + double norm = + std::abs(base_pose_vec.x() * pointcloud_vec.y() - base_pose_vec.y() * pointcloud_vec.x()); + if (norm < min_norm) { + min_norm = norm; + *lateral_nearest_point = pointcloud.at(i); + } + } + *deviation = min_norm; +} + +geometry_msgs::msg::Pose ObstacleStopPlannerNode::getVehicleCenterFromBase( + const geometry_msgs::msg::Pose & base_pose) +{ + const auto & i = vehicle_info_; + const auto yaw = getRPY(base_pose).z; + + geometry_msgs::msg::Pose center_pose; + center_pose.position.x = + base_pose.position.x + (i.vehicle_length_m / 2.0 - i.rear_overhang_m) * std::cos(yaw); + center_pose.position.y = + base_pose.position.y + (i.vehicle_length_m / 2.0 - i.rear_overhang_m) * std::sin(yaw); + center_pose.position.z = base_pose.position.z; + center_pose.orientation = base_pose.orientation; + return center_pose; +} + +void ObstacleStopPlannerNode::setExternalVelocityLimit() +{ + const auto & p = slow_down_param_; + auto slow_down_limit_vel = std::make_shared(); + slow_down_limit_vel->stamp = this->now(); + slow_down_limit_vel->max_velocity = p.slow_down_vel; + slow_down_limit_vel->constraints.min_acceleration = + p.slow_down_min_jerk < p.normal_min_jerk ? p.limit_min_acc : p.normal_min_acc; + slow_down_limit_vel->constraints.max_jerk = std::abs(p.slow_down_min_jerk); + slow_down_limit_vel->constraints.min_jerk = p.slow_down_min_jerk; + slow_down_limit_vel->use_constraints = true; + slow_down_limit_vel->sender = "obstacle_stop_planner"; + + pub_velocity_limit_->publish(*slow_down_limit_vel); + set_velocity_limit_ = true; + + RCLCPP_INFO( + get_logger(), "set velocity limit. jerk:%-6.2f dec:%-6.2f", + slow_down_limit_vel->constraints.min_jerk, slow_down_limit_vel->constraints.min_acceleration); +} + +void ObstacleStopPlannerNode::resetExternalVelocityLimit() +{ + const auto current_vel = current_velocity_ptr_->twist.twist.linear.x; + const auto reach_target_vel = + current_vel < + slow_down_param_.slow_down_vel + slow_down_param_.vel_threshold_reset_velocity_limit_; + const auto constant_vel = + std::abs(current_acc_) < slow_down_param_.dec_threshold_reset_velocity_limit_; + const auto no_undershoot = reach_target_vel && constant_vel; + + if (!no_undershoot) { + return; + } + + auto velocity_limit_clear_command = std::make_shared(); + velocity_limit_clear_command->stamp = this->now(); + velocity_limit_clear_command->command = true; + velocity_limit_clear_command->sender = "obstacle_stop_planner"; + + pub_clear_velocity_limit_->publish(*velocity_limit_clear_command); + set_velocity_limit_ = false; + + RCLCPP_INFO(get_logger(), "reset velocity limit"); +} + +void ObstacleStopPlannerNode::publishDebugData(const PlannerData & planner_data) +{ + const auto & current_vel = current_velocity_ptr_->twist.twist.linear.x; + debug_ptr_->setDebugValues(DebugValues::TYPE::CURRENT_VEL, current_vel); + debug_ptr_->setDebugValues(DebugValues::TYPE::CURRENT_ACC, current_acc_); + debug_ptr_->setDebugValues( + DebugValues::TYPE::FLAG_FIND_SLOW_DOWN_OBSTACLE, planner_data.slow_down_require); + debug_ptr_->setDebugValues( + DebugValues::TYPE::FLAG_FIND_COLLISION_OBSTACLE, planner_data.stop_require); + debug_ptr_->setDebugValues(DebugValues::TYPE::FLAG_EXTERNAL, set_velocity_limit_); + + const auto now_adaptive_cruise = + !planner_data.stop_require && planner_data.found_collision_points; + debug_ptr_->setDebugValues(DebugValues::TYPE::FLAG_ADAPTIVE_CRUISE, now_adaptive_cruise); + + debug_ptr_->publish(); +} + +} // namespace motion_planning + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(motion_planning::ObstacleStopPlannerNode)