From 744d7346fc6e06d87a47b0555042a1b01f79b7f2 Mon Sep 17 00:00:00 2001 From: Takeshi Miura <57553950+1222-takeshi@users.noreply.github.com> Date: Mon, 6 Dec 2021 14:40:18 +0900 Subject: [PATCH] feat: add map_based_prediction package (#69) * release v0.4.0 * remove ROS1 packages temporarily Signed-off-by: mitsudome-r * Revert "remove ROS1 packages temporarily" This reverts commit 7c50d2f586d6a3480ec8c62b19d81f42de403d28. Signed-off-by: mitsudome-r * add COLCON_IGNORE to ros1 packages Signed-off-by: mitsudome-r * Rename launch files to launch.xml (#28) * Port map based prediction (#95) * Port to ROS2 Signed-off-by: Servando German Serrano * Add missing pkg to fix build error Signed-off-by: Servando German Serrano * Updated function Signed-off-by: Servando German Serrano * Fix launch Signed-off-by: Servando German Serrano * [map_based_detection] fix launch file (#115) Signed-off-by: mitsudome-r * Convert calls of Duration to Duration::from_seconds where appropriate (#131) * Rename h files to hpp (#142) * Change includes * Rename files * Adjustments to make things compile * Other packages * 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 * fixing trasient_local in ROS2 packages (#160) * [map_based_prediction] add remapping option for the launch file in map_based_prediction (#199) Signed-off-by: mitsudome-r * Enable lints in map_based_prediction (#153) * port naive_path_prediction from ROS1 to ROS2 (#106) * port naive_path_prediction from ROS1 to ROS2 edited CMakeLists.txt, package.xml, launch file node.cpp & node.hpp files. * review requested changes made in naive_path_prediction porting Co-authored-by: Takamasa Horibe * Ros2 v0.8.0 map based prediction (#299) * restore file name for v0.8.0 update Signed-off-by: Azumi Suzuki * fix typos in perception (#862) * Revert "restore file name for v0.8.0 update" This reverts commit 47ab860adfd6664193b741c61f976eee86c5267a. * restore the return type of a function Signed-off-by: Azumi Suzuki * delete commented out line Signed-off-by: Azumi Suzuki Co-authored-by: Kazuki Miyahara * Ros2 v0.8.0 naive path prediction (#305) * restore naive_path_prediction files for v0.8.0 update * fix topic name (#1016) * Revert "restore naive_path_prediction files for v0.8.0 update" This reverts commit 77a87c3dce4a3134d2cd11180b635748f9b0202a. Co-authored-by: Taichi Higashide * update target branch for ci (#309) * update target branch for ci Signed-off-by: mitsudome-r * [as]: Fix lint Signed-off-by: wep21 * [latlon_muxer]: Fix lint Signed-off-by: wep21 * [map_based_prediction]: Fix lint Signed-off-by: wep21 * [raw_vehicle_cmd_converter]: Fix lint Signed-off-by: wep21 * [remote_cmd_converter]: Fix lint Signed-off-by: wep21 * [turn_signal_decider]: Fix lint Signed-off-by: wep21 Co-authored-by: wep21 * fix duration (#445) * fix duration Signed-off-by: Kosuke Murakami * change to from_seconds Signed-off-by: Kosuke Murakami * fix other duration Signed-off-by: Kosuke Murakami * replace -1 with 0 Signed-off-by: Kosuke Murakami * apply ament_lint_common Signed-off-by: Kosuke Murakami * uncrustify Signed-off-by: Kosuke Murakami * add space Signed-off-by: Kosuke Murakami * add another space Signed-off-by: Kosuke Murakami * Fix typo in perception module (#440) * add use_sim-time option (#454) * Format launch files (#1219) Signed-off-by: Kenji Miyake * 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 * Unify Apache-2.0 license name (#1242) * Remove use_sim_time for set_parameter (#1260) Signed-off-by: wep21 * Perception components (#1368) * [bev_optical_flow]: component node Signed-off-by: wep21 * [object_merger]: component node Signed-off-by: wep21 * [object_range_splitter]: component node Signed-off-by: wep21 * [shape_estimation]: component node Signed-off-by: wep21 * [map_based_prediction]: component node Signed-off-by: wep21 * [naive_path_prediction]: component node Signed-off-by: wep21 * [roi_image_saver]: component node Signed-off-by: wep21 * [lidar_apollo_instance_segmentation]: component node Signed-off-by: wep21 * [object_flow_fusion]: component node Signed-off-by: wep21 * [traffic_light_map_based_detector]: component node Signed-off-by: wep21 * [dynamic_object_visualization]: component node Signed-off-by: wep21 * Fix typo Signed-off-by: wep21 * 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 some typos (#1941) * fix some typos * fix typo * Fix typo Signed-off-by: Kenji Miyake Co-authored-by: Kenji Miyake * Invoke code formatter at pre-commit (#1935) * Run ament_uncrustify at pre-commit * Reformat existing files * Fix copyright and cpplint errors Signed-off-by: Kenji Miyake Co-authored-by: Kenji Miyake * [map_based_prediction] add readme with reference (#2041) Signed-off-by: Takamasa Horibe * 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 * fix angle of prediction path (#2012) * Detection by tracker (#1910) * initial commit * backup * apply format * cosmetic change * implement divided under segmenterd clusters * cosmetic change * bug fix * bug fix * bug fix * modify launch * add debug and bug fix * bug fix * bug fix * add no found tracked object * modify parameters and cmake * bug fix * remove debug info * add readme * modify clustering launch * run pre-commit * cosmetic change * cosmetic change * cosmetic change * apply markdownlint * modify launch * modify for cpplint * modify qos * change int to size_T * bug fix * change perception qos * Update perception/object_recognition/detection/detection_by_tracker/package.xml Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> * cosmetic change * cosmetic change * fix launch * Update perception/object_recognition/detection/detection_by_tracker/src/utils.cpp Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> * modify header include order * change include order * Update perception/object_recognition/detection/detection_by_tracker/src/detection_by_tracker_core.cpp Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> * change to std::optional * cosmetic change * Update perception/object_recognition/detection/detection_by_tracker/src/detection_by_tracker_core.cpp Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> * Update perception/object_recognition/detection/detection_by_tracker/src/detection_by_tracker_core.cpp Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> * bug fix * modify readme Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> * Change formatter to clang-format and black (#2332) * Revert "Temporarily comment out pre-commit hooks" This reverts commit 748e9cdb145ce12f8b520bcbd97f5ff899fc28a3. * Replace ament_lint_common with autoware_lint_common Signed-off-by: Kenji Miyake * 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/map based prediction (#584) * port map_based_prediction * input tracked object * add convert function to predicted object * validate orientation availability * add trailer class * add document map based prediction (#620) * port map_based_prediction * input tracked object * add convert function to predicted object * validate orientation availability * add trailer class * addd README * delete depracated perception pkg (#645) * delete depracated pkg * delete object_flow_fusion and bev_optical_flow * consider upper size of path (#654) * consider upper size of path * misc * fic bug * Sync .auto branch with the latest branch in internal repository (#691) * add trajectory point offset in rviz plugin (#2270) * sync rc rc/v0.23.0 (#2258) * fix interpolation for insert point (#2228) * fix interpolation for insert point * to prev interpolation pkg * Revert "to prev interpolation pkg" This reverts commit 9eb145b5d36e297186015fb17c267ccd5b3c21ef. Co-authored-by: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com> Co-authored-by: taikitanaka * fix topic name (#2266) Signed-off-by: Takamasa Horibe * Add namespace to diag for dual_return_filter (#2269) * Add a function to make 'geometry_msgs::msg::TransformStamped' (#2250) * Add a function to make 'geometry_msgs::msg::TransformStamped' * Add 'child_frame_id' as an argument of 'pose2transform' * Simplify marker scale initialization (#2286) * Fix/crosswalk polygon (#2279) * extend crosswalk polygon * improve readability * fix polygon shape * Add warning when decel distance calculation fails (#2289) Signed-off-by: Makoto Kurihara * [motion_velocity_smoother] ignore debug print (#2292) * cosmetic change Signed-off-by: Takamasa Horibe * cahnge severity from WARN to DEBUG for debug info Signed-off-by: Takamasa Horibe * use util for stop_watch Signed-off-by: Takamasa Horibe * fix map based prediction (#2200) * fix map based prediction * fix format * change map based prediction * fix spells * fix spells in comments * fix for cpplint * fix some problems * fix format and code for clang-tidy * fix space for cpplint * Update Readme.md * Update perception/object_recognition/prediction/map_based_prediction/Readme.md * Update perception/object_recognition/prediction/map_based_prediction/Readme.md * Update perception/object_recognition/prediction/map_based_prediction/Readme.md * Update perception/object_recognition/prediction/map_based_prediction/Readme.md * Update perception/object_recognition/prediction/map_based_prediction/Readme.md * Update perception/object_recognition/prediction/map_based_prediction/Readme.md * fix vector access method * fix readme format * add parameter * Update perception/object_recognition/prediction/map_based_prediction/Readme.md Co-authored-by: Kazuki Miyahara * Update perception/object_recognition/prediction/map_based_prediction/Readme.md Co-authored-by: Kazuki Miyahara * Update perception/object_recognition/prediction/map_based_prediction/Readme.md Co-authored-by: Kazuki Miyahara * Update Readme.md * Update perception/object_recognition/prediction/map_based_prediction/Readme.md Co-authored-by: Kazuki Miyahara Co-authored-by: tkimura4 Co-authored-by: Kazuki Miyahara * remove failure condition for 0 velocity trajectory (#2295) Signed-off-by: Takamasa Horibe * [mpc_follower] remove stop distance condition from stopState decision (#1916) * [mpc_follower] remove stop distance condition from stopState decision Signed-off-by: Takamasa Horibe * add invalid index handling Signed-off-by: Takamasa Horibe * Move the debug marker initialization part to another file (#2288) * Move the debug marker initialization part to 'debug.cpp' * Make 'isLocalOptimalSolutionOscillation' independent from 'NDTScanMatcher' (#2300) * Remove an unused function 'getTransform' (#2301) * Simplify iteration of initial poses (#2310) * Make a transform object const (#2311) * Represent poses in 'std::vector' instead of 'geometry_msgs::msg::PoseArray' (#2312) * Feature/no stopping area (#2163) * add no stopping area module to behavior velocity planner * apply utils * add polygon interpolation module order stopline around area is considered * devide jpass udge with stop line polygon * update docs * rename file name * update to latest * minor change for marker * update license Co-authored-by: Yukihiro Saito * update license Co-authored-by: Yukihiro Saito * update license Co-authored-by: Yukihiro Saito * update license Co-authored-by: Yukihiro Saito * minor fix * add parameter tuning at experiment * update readme * format doc * apply comments * add exception gurd * cosmetic change * fix ament * fix typo and remove for statement * & to " " * better ns * return pass judge param * add missing stoppable condition * add clear pass judge and stoppable flag * add comment * precommit fix * cpplint Co-authored-by: Yukihiro Saito * sync rc rc/v0.23.0 (#2281) * Fix side shift planner (#2171) (#2172) * add print debug Signed-off-by: TakaHoribe * remove forward shift points when adding new point Signed-off-by: TakaHoribe * remove debug print Signed-off-by: TakaHoribe * format Signed-off-by: TakaHoribe * Fix remove threshold Co-authored-by: Fumiya Watanabe Co-authored-by: Takamasa Horibe * Fix/pull out and pull over (#2175) * delete unnecessary check * fix condition of starting pull out * Add emergency status API (#2174) (#2182) * Fix/mpc reset prev result (#2185) (#2195) * reset prev result * clean code * reset only raw_steer_cmd * Update control/mpc_follower/src/mpc_follower_core.cpp Co-authored-by: Takamasa Horibe Co-authored-by: Takamasa Horibe Co-authored-by: Takamasa Horibe * [hotfix] 1 path point exception after resampling (#2204) * fix 1 path point exception after resampling Signed-off-by: TakaHoribe * Apply suggestions from code review * Apply suggestions from code review Co-authored-by: Takamasa Horibe Co-authored-by: tkimura4 * [hotfix] Fix lane ids (#2211) * Fix lane ids * Prevent acceleration on avoidance (#2214) * prevent acceleration on avoidance Signed-off-by: TakaHoribe * fix param name Signed-off-by: TakaHoribe * parametrize avoidance acc Signed-off-by: Takamasa Horibe * change param name Signed-off-by: Takamasa Horibe * fix typo Signed-off-by: Takamasa Horibe * Fix qos in roi cluster fusion (#2218) * fix confidence (#2220) * too high confidence (#2229) * Fix/obstacle stop 0.23.0 (#2232) * fix unexpected slow down in sharp curves (#2181) * Fix/insert implementation (#2186) Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> * [hotfix] Remove exception in avoidance module (#2233) * Remove exception * Fix clock * Remove blank line * Update traffic light state if ref stop point is ahead of previous one (#2197) Signed-off-by: wep21 * fix interpolation for insert point (#2228) * fix interpolation for insert point * to prev interpolation pkg * fix index (#2265) * turn signal calculation (#2280) * add turn signal funtion in path shifter * add ros parameters Co-authored-by: Fumiya Watanabe Co-authored-by: Takamasa Horibe Co-authored-by: tkimura4 Co-authored-by: Takagi, Isamu <43976882+isamu-takagi@users.noreply.github.com> Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> Co-authored-by: Yukihiro Saito Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Co-authored-by: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com> Co-authored-by: Sugatyon <32741405+Sugatyon@users.noreply.github.com> * [behavior_path_planner] fix sudden path change around ego (#2305) (#2318) * fix return-from-ego shift point generation logic Signed-off-by: Takamasa Horibe * change param for trimSimilarGradShiftPoint Signed-off-by: Takamasa Horibe * add comment for issue Signed-off-by: Takamasa Horibe * update comment Signed-off-by: Takamasa Horibe * replace code with function (logic has not changed) Signed-off-by: Takamasa Horibe * move func to cpp Signed-off-by: Takamasa Horibe * add comment for issue Signed-off-by: Takamasa Horibe * fix typo Signed-off-by: Takamasa Horibe * Update planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp Co-authored-by: Kazuki Miyahara * Update planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp Co-authored-by: Kazuki Miyahara Co-authored-by: Kazuki Miyahara Co-authored-by: Takamasa Horibe Co-authored-by: Kazuki Miyahara * Add functions to make stamped scalar messages (#2317) * Fix/object yaw in intersection module (#2294) * fix object orientation * fix function name * add guard (#2321) * reduce cost (double to float) (#2298) * Add detail collision check (#2274) * Add detail collision check Signed-off-by: wep21 * Remove unused function Signed-off-by: wep21 * Fix arc length Signed-off-by: wep21 * Seperate time margin Signed-off-by: wep21 * Fix parameter name Signed-off-by: wep21 * Update Readme Signed-off-by: wep21 * Address review: Add comment for TimeDistanceArray Signed-off-by: wep21 * Run pre-commit Signed-off-by: wep21 * Fix cpplint Signed-off-by: wep21 * Add return for empty polygon Signed-off-by: wep21 * update CenterPoint (#2222) * update to model trained by mmdet3d * add vizualizer (debug) * for multi-frame inputs * chagne config * use autoware_utils::pi * project specific model and param * rename vfe -> encoder * rename general to common * update download link * update * fix * rename model_name * change training toolbox link * chage lint package * fix test error * commit suggestion * Feature/lane change detection (#2331) * add old information deleter * fix access bug * change to deque * update obstacle buffer * fix some bugs * add lane change detector * make a update lanelet function * fix code style * parameterize essential values * Update perception/object_recognition/prediction/map_based_prediction/src/map_based_prediction_ros.cpp Co-authored-by: Kazuki Miyahara * fix slash position * remove unnecessary lines * fix format * fix format * change to new enum * fix format * fix typo and add guard * change funciton name * add lane change description Co-authored-by: Kazuki Miyahara * Add Planning Evaluator (#2293) * Add prototype planning evaluator Produced data for dist between points, curvature, and relative angle * Cleanup the code to make adding metrics easier * Add remaining basic metrics (length, duration, vel, accel, jerk) * Add motion_evaluator to evaluate the actual ego motion + code cleanup * Add deviation metrics * Add naive stability metric * Handle invalid stat (TODO: fix the output file formatting) * Add parameter file and cleanup * Add basic obstacle metric (TTC not yet implemented) and fix output file format * Add basic time to collision * Add lateral-distance based stability metric * Add check (at init time) that metrics' maps are complete * Publish metrics as ParamaterDeclaration msg (for openscenario) * Use lookahead and start from ego_pose when calculating stability metrics * Code cleanup * Fix lint * Add tests * Fix bug with Frechet dist and the last traj point * Finish implementing tests * Fix lint * Code cleanup * Update README.md * Remove unused metric * Change msg type of published metrics to DiagnosticArray * fix format to fix pre-commit check Signed-off-by: Takamasa Horibe * fix yaml format to fix pre-commit check Signed-off-by: Takamasa Horibe * fix yaml format Signed-off-by: Takamasa Horibe * apply clang-format Signed-off-by: Takamasa Horibe * apply clang-format Signed-off-by: Takamasa Horibe * Update planning/planning_diagnostics/planning_evaluator/include/planning_evaluator/planning_evaluator_node.hpp * Update planning/planning_diagnostics/planning_evaluator/test/test_planning_evaluator_node.cpp * Update planning/planning_diagnostics/planning_evaluator/test/test_planning_evaluator_node.cpp * change lint format to autoware_lint_common Signed-off-by: Takamasa Horibe Co-authored-by: Takamasa Horibe * Add keep braking function at driving state (#2346) * Add keep braking function at driving state Signed-off-by: Makoto Kurihara * Remove debug messages Signed-off-by: Makoto Kurihara * Fix format Signed-off-by: Makoto Kurihara * Change diag_updater's pediod from default to 0.1sec (#2348) * add cross judgement and common signal function (#2319) * merge branch turn_signal_common * add turn signal function in signal decider * add cross judge in path_utilities and delete from turn_signal_decider * remove original signal calculation in lane change * omit substitution * replace turn signal decider in pull over function * modify cross judge logic * replace turn signal decider in avoidance * add readme of turn signal * update * delete print debug * update * delete lane change decider in path shifter * delete blank line * fix indent * fix typo * fix typo * decrease nest * run pre commit * Add 0 limit at forward jerk velocity filter (#2340) Signed-off-by: Makoto Kurihara * add time offset param to point cloud concatenation (#2303) * add offset param * clang-format Co-authored-by: Akihito OHSATO * Feature/add doc for keep braking function at driving state (#2366) * Add the description of brake keeping Signed-off-by: Makoto Kurihara * Add the english document Signed-off-by: Makoto Kurihara * Improve description Signed-off-by: Makoto Kurihara * Add english description Signed-off-by: Makoto Kurihara * Fix include files (#2339) Signed-off-by: Kenji Miyake * fix behavior intersection module * fix behavior no stopping area module * fix planning_evaluator * fix motion_velocity_smoother * rename variable * Revert "[mpc_follower] remove stop distance condition from stopState decision (#1916)" This reverts commit ff4f0b5a844d1f835f1b93bd3b36a76747b0cd02. * Revert "Add keep braking function at driving state (#2346)" This reverts commit f0478187db4c28bf6092c198723dcc5ec11a9c70. * Revert "Feature/add doc for keep braking function at driving state (#2366)" This reverts commit 66de2f3924a479049fce2d5c5c6b579cacbd3e49. * Fix orientation availability in centerpoint Signed-off-by: wep21 * fix test_trajectory.cpp * add target link libraries * Use .auto msg in test code for planniing evaluator Signed-off-by: wep21 * fix include Signed-off-by: wep21 Co-authored-by: Takayuki Murooka Co-authored-by: autoware-iv-sync-ci[bot] <87871706+autoware-iv-sync-ci[bot]@users.noreply.github.com> Co-authored-by: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com> Co-authored-by: taikitanaka Co-authored-by: Takamasa Horibe Co-authored-by: Takeshi Miura <57553950+1222-takeshi@users.noreply.github.com> Co-authored-by: Takeshi Ishita Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Co-authored-by: Makoto Kurihara Co-authored-by: purewater0901 <43805014+purewater0901@users.noreply.github.com> Co-authored-by: tkimura4 Co-authored-by: Kazuki Miyahara Co-authored-by: Yukihiro Saito Co-authored-by: Fumiya Watanabe Co-authored-by: Takagi, Isamu <43976882+isamu-takagi@users.noreply.github.com> Co-authored-by: Sugatyon <32741405+Sugatyon@users.noreply.github.com> Co-authored-by: s-murakami-esol <81723883+s-murakami-esol@users.noreply.github.com> Co-authored-by: Yusuke Muramatsu Co-authored-by: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> Co-authored-by: Shunsuke Miura <37187849+miursh@users.noreply.github.com> Co-authored-by: Shinnosuke Hirakawa <8327162+0x126@users.noreply.github.com> Co-authored-by: Akihito OHSATO Co-authored-by: Kenji Miyake * substitute time_step for object-path (#720) * fix: fix title of README.md * Apply suggestions from code review * Feature/update map based prediction readme (#762) * remove unused arg * remove unsed arg * update readme * remove unused arg Co-authored-by: mitsudome-r 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: nik-tier4 <71747268+nik-tier4@users.noreply.github.com> Co-authored-by: Takamasa Horibe Co-authored-by: s-azumi <38061530+s-azumi@users.noreply.github.com> Co-authored-by: Kazuki Miyahara Co-authored-by: tkimura4 Co-authored-by: Taichi Higashide Co-authored-by: wep21 Co-authored-by: Kosuke Murakami 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: Hiroki OTA Co-authored-by: Kenji Miyake Co-authored-by: Takeshi Ishita Co-authored-by: Keisuke Shima <19993104+KeisukeShima@users.noreply.github.com> Co-authored-by: Yukihiro Saito Co-authored-by: Sugatyon <32741405+Sugatyon@users.noreply.github.com> Co-authored-by: Takayuki Murooka Co-authored-by: autoware-iv-sync-ci[bot] <87871706+autoware-iv-sync-ci[bot]@users.noreply.github.com> Co-authored-by: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com> Co-authored-by: taikitanaka Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Co-authored-by: Makoto Kurihara Co-authored-by: purewater0901 <43805014+purewater0901@users.noreply.github.com> Co-authored-by: Fumiya Watanabe Co-authored-by: Takagi, Isamu <43976882+isamu-takagi@users.noreply.github.com> Co-authored-by: s-murakami-esol <81723883+s-murakami-esol@users.noreply.github.com> Co-authored-by: Yusuke Muramatsu Co-authored-by: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> Co-authored-by: Shunsuke Miura <37187849+miursh@users.noreply.github.com> Co-authored-by: Shinnosuke Hirakawa <8327162+0x126@users.noreply.github.com> Co-authored-by: Akihito OHSATO --- .../map_based_prediction/CMakeLists.txt | 36 + perception/map_based_prediction/README.md | 73 ++ .../include/cubic_spline.hpp | 254 ++++++ .../include/map_based_prediction.hpp | 79 ++ .../include/map_based_prediction_ros.hpp | 158 ++++ .../launch/map_based_prediction.launch.xml | 34 + perception/map_based_prediction/package.xml | 29 + .../src/map_based_prediction.cpp | 304 +++++++ .../src/map_based_prediction_ros.cpp | 760 ++++++++++++++++++ 9 files changed, 1727 insertions(+) create mode 100644 perception/map_based_prediction/CMakeLists.txt create mode 100644 perception/map_based_prediction/README.md create mode 100644 perception/map_based_prediction/include/cubic_spline.hpp create mode 100644 perception/map_based_prediction/include/map_based_prediction.hpp create mode 100644 perception/map_based_prediction/include/map_based_prediction_ros.hpp create mode 100644 perception/map_based_prediction/launch/map_based_prediction.launch.xml create mode 100644 perception/map_based_prediction/package.xml create mode 100644 perception/map_based_prediction/src/map_based_prediction.cpp create mode 100644 perception/map_based_prediction/src/map_based_prediction_ros.cpp diff --git a/perception/map_based_prediction/CMakeLists.txt b/perception/map_based_prediction/CMakeLists.txt new file mode 100644 index 0000000000000..d24698f7d181e --- /dev/null +++ b/perception/map_based_prediction/CMakeLists.txt @@ -0,0 +1,36 @@ +cmake_minimum_required(VERSION 3.5) +project(map_based_prediction) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) + set(CMAKE_CXX_STANDARD_REQUIRED ON) + set(CMAKE_CXX_EXTENSIONS OFF) +endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +find_package(Eigen3 REQUIRED) + +ament_auto_add_library(map_based_prediction_node SHARED + src/map_based_prediction_ros.cpp + src/map_based_prediction.cpp +) + +rclcpp_components_register_node(map_based_prediction_node + PLUGIN "MapBasedPredictionROS" + EXECUTABLE map_based_prediction +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_auto_package( + INSTALL_TO_SHARE + launch +) diff --git a/perception/map_based_prediction/README.md b/perception/map_based_prediction/README.md new file mode 100644 index 0000000000000..ee43ee89c81b1 --- /dev/null +++ b/perception/map_based_prediction/README.md @@ -0,0 +1,73 @@ +# map_based_prediction + +## Role + +`map_based_prediction` is a module to predict the future paths of other vehicles and pedestrians according to the shape of the map and the surrounding environment. + +## Inner-workings / Algorithms + +1. Get lanelet path + The first step is to get the lanelet of the current position of the car. After that, we obtain several trajectories based on the map. + +2. Lane Change Detection + After finding the current lanelet from the current position of the obstacle, our algorithm try to detect the lane change maneuver from the past positions of the obstacle. Our method uses the deviation between the obstacle's current position and its position one second ago and current position to determine if it is about to change lanes. The parameters used for the lane change decision are obtained by analyzing the data obtained from the experiment. We already confirmed that these parameters give the least number of false positives. + +3. Confidence calculation + We use the following metric to compute the distance to a certain lane. + + ```txt + d = x^T P x + ``` + + where `x=[lateral_dist, yaw_diff]` and `P` are covariance matrices. Therefore confidence values can be computed as + + ```txt + confidence = 1/d + ``` + + Finally, we normalize the confidence value to make it as probability value. Note that the standard deviation of the lateral distance and yaw difference is given by the user. + +4. Drawing predicted trajectories + From the current position and reference trajectories that we get in the step1, we create predicted trajectories by using Quintic polynomial. Note that, since this algorithm consider lateral and longitudinal motions separately, it sometimes generates dynamically-infeasible trajectories when the vehicle travels at a low speed. To deal with this problem, we only make straight line predictions when the vehicle speed is lower than a certain value (which is given as a parameter). + +## Inputs / Outputs + +### Input + +| Name | Type | Description | +| -------------------------------------------------- | ---------------------------------------------------- | ---------------------------------------- | +| `~/perception/object_recognition/tracking/objects` | `autoware_auto_perception_msgs::msg::TrackedObjects` | tracking objects without predicted path. | +| `~/vector_map` | `autoware_auto_mapping_msgs::msg::HADMapBin` | binary data of Lanelet2 Map. | + +### Output + +| Name | Type | Description | +| ------------------------ | ------------------------------------------------------ | ------------------------------------- | +| `~/objects` | `autoware_auto_perception_msgs::msg::PredictedObjects` | tracking objects with predicted path. | +| `~/objects_path_markers` | `visualization_msgs::msg::MarkerArray` | marker for visualization. | + +## Parameters + +| Parameter | Type | Description | +| ------------------------------------------- | ------ | ------------------------------------------------------------------------------------------------------------ | +| `prediction_time_horizon` | double | predict time duration for predicted path [s] | +| `prediction_sampling_delta_time` | double | sampling time for points in predicted path [s] | +| `min_velocity_for_map_based_prediction` | double | apply map-based prediction to the objects with higher velocity than this value | +| `dist_threshold_for_searching_lanelet` | double | The threshold of the angle used when searching for the lane to which the object belongs [rad] | +| `delta_yaw_threshold_for_searching_lanelet` | double | The threshold of the distance used when searching for the lane to which the object belongs [m] | +| `sigma_lateral_offset` | double | Standard deviation for lateral position of objects [m] | +| `sigma_yaw_angle` | double | Standard deviation yaw angle of objects [rad] | +| `history_time_length` | double | Time span of object information used for prediction [s] | +| `dist_ratio_threshold_to_left_bound` | double | Conditions for using lane change detection of objects. Distance to the left bound of lanelet. | +| `dist_ratio_threshold_to_right_bound` | double | Conditions for using lane change detection of objects. Distance to the right bound of lanelet. | +| `diff_dist_threshold_to_left_bound` | double | Conditions for using lane change detection of objects. Differential value of horizontal position of objects. | +| `diff_dist_threshold_to_right_bound` | double | Conditions for using lane change detection of objects. Differential value of horizontal position of objects. | + +## Assumptions / Known limits + +`map_based_prediction` can only predict future trajectories for cars, tracks and buses. + +## Reference + +1. M. Werling, J. Ziegler, S. Kammel, and S. Thrun, “Optimal trajectory generation for dynamic street scenario in a frenet frame,” IEEE International Conference on Robotics and Automation, Anchorage, Alaska, USA, May 2010. +2. A. Houenou, P. Bonnifait, V. Cherfaoui, and Wen Yao, “Vehicle trajectory prediction based on motion model and maneuver recognition,” in 2013 IEEE/RSJ International Conference on Intelligent Robots and Systems. IEEE, nov 2013, pp. 4363–4369. diff --git a/perception/map_based_prediction/include/cubic_spline.hpp b/perception/map_based_prediction/include/cubic_spline.hpp new file mode 100644 index 0000000000000..7e0826ceb1197 --- /dev/null +++ b/perception/map_based_prediction/include/cubic_spline.hpp @@ -0,0 +1,254 @@ +// Copyright 2018 Forrest +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +// THE SOFTWARE. + +#ifndef CUBIC_SPLINE_HPP_ +#define CUBIC_SPLINE_HPP_ + +#include + +#include +#include +#include +#include +#include +#include + +static std::vector vec_diff(const std::vector & input) +{ + std::vector output; + for (unsigned int i = 1; i < input.size(); i++) { + output.push_back(input[i] - input[i - 1]); + } + return output; +} + +static std::vector cum_sum(const std::vector & input) +{ + std::vector output; + double temp = 0; + for (unsigned int i = 0; i < input.size(); i++) { + temp += input[i]; + output.push_back(temp); + } + return output; +} + +class Spline +{ +public: + std::vector x; + std::vector y; + int nx; + std::vector h; + std::vector a; + std::vector b; + std::vector c; + // Eigen::VectorXf c; + std::vector d; + + Spline() {} + // d_i * (x-x_i)^3 + c_i * (x-x_i)^2 + b_i * (x-x_i) + a_i + Spline(const std::vector & x_, const std::vector & y_) + : x(x_), y(y_), nx(x_.size()), h(vec_diff(x_)), a(y_) + { + Eigen::MatrixXd A = calc_A(); + Eigen::VectorXd B = calc_B(); + // std::cerr << "A det " << A.determinant() << std::endl; + // std::cerr << "A QR" << A.colPivHouseholderQr().solve(B) << std::endl; + Eigen::VectorXd c_eigen = A.colPivHouseholderQr().solve(B); + // std::cerr << "c eigen " << c_eigen << std::endl; + double * c_pointer = c_eigen.data(); + c.assign(c_pointer, c_pointer + c_eigen.rows()); + + for (int i = 0; i < nx - 1; i++) { + d.push_back((c[i + 1] - c[i]) / (3.0 * h[i])); + b.push_back((a[i + 1] - a[i]) / h[i] - h[i] * (c[i + 1] + 2 * c[i]) / 3.0); + } + } + + double calc(double t) + { + if (t < x.front() || t > x.back()) { + std::cout << "Dangerous" << std::endl; + std::cout << t << std::endl; + throw std::invalid_argument("received value out of the pre-defined range"); + } + int seg_id = bisect(t, 0, nx); + double dx = t - x[seg_id]; + return a[seg_id] + b[seg_id] * dx + c[seg_id] * dx * dx + d[seg_id] * dx * dx * dx; + } + + double calc(double t, double s) + { + if (t < 0 || t > s) { + std::cout << "Dangerous" << std::endl; + std::cout << t << std::endl; + throw std::invalid_argument("received value out of the pre-defined range"); + } + int seg_id = bisect(t, 0, nx); + double dx = t - x[seg_id]; + return a[seg_id] + b[seg_id] * dx + c[seg_id] * dx * dx + d[seg_id] * dx * dx * dx; + } + + double calc_d(double t) + { + if (t < x.front() || t > x.back()) { + std::cout << "Dangerous" << std::endl; + std::cout << t << std::endl; + throw std::invalid_argument("received value out of the pre-defined range"); + } + int seg_id = bisect(t, 0, nx - 1); + double dx = t - x[seg_id]; + return b[seg_id] + 2 * c[seg_id] * dx + 3 * d[seg_id] * dx * dx; + } + + double calc_d(double t, double s) + { + if (t < 0 || t > s) { + std::cout << "Dangerous" << std::endl; + std::cout << t << std::endl; + throw std::invalid_argument("received value out of the pre-defined range"); + } + int seg_id = bisect(t, 0, nx - 1); + double dx = t - x[seg_id]; + return b[seg_id] + 2 * c[seg_id] * dx + 3 * d[seg_id] * dx * dx; + } + + double calc_dd(double t) + { + if (t < x.front() || t > x.back()) { + std::cout << "Dangerous" << std::endl; + std::cout << t << std::endl; + throw std::invalid_argument("received value out of the pre-defined range"); + } + int seg_id = bisect(t, 0, nx); + double dx = t - x[seg_id]; + return 2 * c[seg_id] + 6 * d[seg_id] * dx; + } + + double calc_dd(double t, double s) + { + if (t < 0.0 || t > s) { + std::cout << "Dangerous" << std::endl; + std::cout << t << std::endl; + throw std::invalid_argument("received value out of the pre-defined range"); + } + int seg_id = bisect(t, 0, nx); + double dx = t - x[seg_id]; + return 2 * c[seg_id] + 6 * d[seg_id] * dx; + } + +private: + Eigen::MatrixXd calc_A() + { + Eigen::MatrixXd A = Eigen::MatrixXd::Zero(nx, nx); + A(0, 0) = 1; + for (int i = 0; i < nx - 1; i++) { + if (i != nx - 2) { + A(i + 1, i + 1) = 2 * (h[i] + h[i + 1]); + } + A(i + 1, i) = h[i]; + A(i, i + 1) = h[i]; + } + A(0, 1) = 0.0; + A(nx - 1, nx - 2) = 0.0; + A(nx - 1, nx - 1) = 1.0; + return A; + } + Eigen::VectorXd calc_B() + { + Eigen::VectorXd B = Eigen::VectorXd::Zero(nx); + for (int i = 0; i < nx - 2; i++) { + B(i + 1) = 3.0 * (a[i + 2] - a[i + 1]) / h[i + 1] - 3.0 * (a[i + 1] - a[i]) / h[i]; + } + return B; + } + + int bisect(double t, int start, int end) + { + int mid = (start + end) / 2; + if (t == x[mid] || end - start <= 1) { + return mid; + } else if (t > x[mid]) { + return bisect(t, mid, end); + } else { + return bisect(t, start, mid); + } + } +}; + +class Spline2D +{ +public: + Spline sx; + Spline sy; + std::vector s; + double max_s_value_; + + Spline2D(const std::vector & x, const std::vector & y) + { + s = calc_s(x, y); + sx = Spline(s, x); + sy = Spline(s, y); + max_s_value_ = *std::max_element(s.begin(), s.end()); + } + + std::array calc_position(double s_t) + { + double x = sx.calc(s_t, max_s_value_); + double y = sy.calc(s_t, max_s_value_); + return {{x, y}}; + } + + double calc_curvature(double s_t) + { + double dx = sx.calc_d(s_t, max_s_value_); + double ddx = sx.calc_dd(s_t, max_s_value_); + double dy = sy.calc_d(s_t, max_s_value_); + double ddy = sy.calc_dd(s_t, max_s_value_); + return (ddy * dx - ddx * dy) / (dx * dx + dy * dy); + } + + double calc_yaw(double s_t) + { + double dx = sx.calc_d(s_t, max_s_value_); + double dy = sy.calc_d(s_t, max_s_value_); + return std::atan2(dy, dx); + } + +private: + std::vector calc_s(const std::vector & x, const std::vector & y) + { + std::vector ds; + std::vector out_s{0}; + std::vector dx = vec_diff(x); + std::vector dy = vec_diff(y); + + for (unsigned int i = 0; i < dx.size(); i++) { + ds.push_back(std::sqrt(dx[i] * dx[i] + dy[i] * dy[i])); + } + + std::vector cum_ds = cum_sum(ds); + out_s.insert(out_s.end(), cum_ds.begin(), cum_ds.end()); + return out_s; + } +}; + +#endif // CUBIC_SPLINE_HPP_ diff --git a/perception/map_based_prediction/include/map_based_prediction.hpp b/perception/map_based_prediction/include/map_based_prediction.hpp new file mode 100644 index 0000000000000..e6831c2be30f8 --- /dev/null +++ b/perception/map_based_prediction/include/map_based_prediction.hpp @@ -0,0 +1,79 @@ +// Copyright 2018-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 MAP_BASED_PREDICTION_HPP_ +#define MAP_BASED_PREDICTION_HPP_ + +#include +#include +#include +#include + +#include + +struct DynamicObjectWithLanes +{ + autoware_auto_perception_msgs::msg::TrackedObject object; + std::vector> lanes; + std::vector confidence; +}; + +struct DynamicObjectWithLanesArray +{ + std_msgs::msg::Header header; + std::vector objects; +}; + +class Spline2D; + +class MapBasedPrediction +{ +private: + double interpolating_resolution_; + double time_horizon_; + double sampling_delta_time_; + + bool getPredictedPath( + const double height, const double current_d_position, const double current_d_velocity, + const double current_s_position, const double current_s_velocity, + const std_msgs::msg::Header & origin_header, Spline2D & spline2d, + autoware_auto_perception_msgs::msg::PredictedPath & path); + + void getLinearPredictedPath( + const geometry_msgs::msg::Pose & object_pose, const geometry_msgs::msg::Twist & object_twist, + autoware_auto_perception_msgs::msg::PredictedPath & predicted_path); + + void normalizeLikelihood( + autoware_auto_perception_msgs::msg::PredictedObjectKinematics & predicted_object_kinematics); + + autoware_auto_perception_msgs::msg::PredictedObjectKinematics convertToPredictedKinematics( + const autoware_auto_perception_msgs::msg::TrackedObjectKinematics & tracked_object); + +public: + MapBasedPrediction( + double interpolating_resolution, double time_horizon, double sampling_delta_time); + + bool doPrediction( + const DynamicObjectWithLanesArray & in_objects, + std::vector & out_objects); + + bool doLinearPrediction( + const autoware_auto_perception_msgs::msg::PredictedObjects & in_objects, + std::vector & out_objects); + + autoware_auto_perception_msgs::msg::PredictedObject convertToPredictedObject( + const autoware_auto_perception_msgs::msg::TrackedObject & tracked_object); +}; + +#endif // MAP_BASED_PREDICTION_HPP_ diff --git a/perception/map_based_prediction/include/map_based_prediction_ros.hpp b/perception/map_based_prediction/include/map_based_prediction_ros.hpp new file mode 100644 index 0000000000000..5f477f0133f79 --- /dev/null +++ b/perception/map_based_prediction/include/map_based_prediction_ros.hpp @@ -0,0 +1,158 @@ +// Copyright 2018-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 MAP_BASED_PREDICTION_ROS_HPP_ +#define MAP_BASED_PREDICTION_ROS_HPP_ + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +namespace tf2_ros +{ +class Buffer; +class TransformListener; +} // namespace tf2_ros + +namespace lanelet +{ +class Lanelet; +class LaneletMap; +using LaneletMapPtr = std::shared_ptr; +namespace routing +{ +class RoutingGraph; +} // namespace routing +namespace traffic_rules +{ +class TrafficRules; +} // namespace traffic_rules +} // namespace lanelet + +struct ObjectData +{ + lanelet::ConstLanelets current_lanelets; + lanelet::ConstLanelets future_possible_lanelets; + geometry_msgs::msg::PoseStamped pose; +}; + +enum class Maneuver { + LANE_FOLLOW, + LEFT_LANE_CHANGE, + RIGHT_LANE_CHANGE, +}; + +class MapBasedPrediction; + +class MapBasedPredictionROS : public rclcpp::Node +{ +private: + double prediction_time_horizon_; + double prediction_sampling_delta_time_; + double min_velocity_for_map_based_prediction_; + double interpolating_resolution_; + double debug_accumulated_time_; + double dist_threshold_for_searching_lanelet_; + double delta_yaw_threshold_for_searching_lanelet_; + double sigma_lateral_offset_; + double sigma_yaw_angle_; + double history_time_length_; + double dist_ratio_threshold_to_left_bound_; + double dist_ratio_threshold_to_right_bound_; + double diff_dist_threshold_to_left_bound_; + double diff_dist_threshold_to_right_bound_; + + rclcpp::Subscription::SharedPtr sub_objects_; + rclcpp::Subscription::SharedPtr sub_map_; + rclcpp::Publisher::SharedPtr pub_objects_; + rclcpp::Publisher::SharedPtr pub_markers_; + + std::unordered_map> object_buffer_; + + std::shared_ptr tf_buffer_ptr_; + std::shared_ptr tf_listener_ptr_; + + std::shared_ptr lanelet_map_ptr_; + std::shared_ptr routing_graph_ptr_; + std::shared_ptr traffic_rules_ptr_; + std::shared_ptr map_based_prediction_; + + bool getSelfPose(geometry_msgs::msg::Pose & self_pose, const std_msgs::msg::Header & header); + bool getSelfPoseInMap(geometry_msgs::msg::Pose & self_pose); + + double getObjectYaw(const autoware_auto_perception_msgs::msg::TrackedObject & object); + double calculateLikelihood( + const std::vector & path, + const autoware_auto_perception_msgs::msg::TrackedObject & object); + + void addValidPath( + const lanelet::routing::LaneletPaths & candidate_paths, + lanelet::routing::LaneletPaths & valid_paths); + + void objectsCallback( + const autoware_auto_perception_msgs::msg::TrackedObjects::ConstSharedPtr in_objects); + void mapCallback(const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg); + + bool getClosestLanelets( + const autoware_auto_perception_msgs::msg::TrackedObject & object, + const lanelet::LaneletMapPtr & lanelet_map_ptr, lanelet::ConstLanelets & closest_lanelets); + + bool checkCloseLaneletCondition( + const std::pair & lanelet, + const autoware_auto_perception_msgs::msg::TrackedObject & object, + const lanelet::BasicPoint2d & search_point); + + void removeInvalidObject(const double current_time); + bool updateObjectBuffer( + const std_msgs::msg::Header & header, + const autoware_auto_perception_msgs::msg::TrackedObject & object, + lanelet::ConstLanelets & current_lanelets); + void updatePossibleLanelets( + const std::string object_id, const lanelet::routing::LaneletPaths & paths); + + double calcRightLateralOffset( + const lanelet::ConstLineString2d & bound_line, const geometry_msgs::msg::Pose & search_pose); + double calcLeftLateralOffset( + const lanelet::ConstLineString2d & bound_line, const geometry_msgs::msg::Pose & search_pose); + Maneuver detectLaneChange( + const autoware_auto_perception_msgs::msg::TrackedObject & object, + const lanelet::ConstLanelet & current_lanelet, const double current_time); + +public: + explicit MapBasedPredictionROS(const rclcpp::NodeOptions & node_options); +}; + +#endif // MAP_BASED_PREDICTION_ROS_HPP_ diff --git a/perception/map_based_prediction/launch/map_based_prediction.launch.xml b/perception/map_based_prediction/launch/map_based_prediction.launch.xml new file mode 100644 index 0000000000000..573fac87e379e --- /dev/null +++ b/perception/map_based_prediction/launch/map_based_prediction.launch.xml @@ -0,0 +1,34 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/perception/map_based_prediction/package.xml b/perception/map_based_prediction/package.xml new file mode 100644 index 0000000000000..b5e7f9ad941cf --- /dev/null +++ b/perception/map_based_prediction/package.xml @@ -0,0 +1,29 @@ + + + map_based_prediction + 0.1.0 + This package implements a map_based_prediction + + Kosuke Murakami + + Apache License 2.0 + + ament_cmake + + autoware_auto_perception_msgs + autoware_utils + lanelet2_extension + rclcpp + rclcpp_components + tf2 + tf2_geometry_msgs + tf2_ros + unique_identifier_msgs + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/perception/map_based_prediction/src/map_based_prediction.cpp b/perception/map_based_prediction/src/map_based_prediction.cpp new file mode 100644 index 0000000000000..34e7b748faa3f --- /dev/null +++ b/perception/map_based_prediction/src/map_based_prediction.cpp @@ -0,0 +1,304 @@ +// 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 + +MapBasedPrediction::MapBasedPrediction( + double interpolating_resolution, double time_horizon, double sampling_delta_time) +: interpolating_resolution_(interpolating_resolution), + time_horizon_(time_horizon), + sampling_delta_time_(sampling_delta_time) +{ +} + +bool MapBasedPrediction::doPrediction( + const DynamicObjectWithLanesArray & in_objects, + std::vector & out_objects) +{ + for (auto & object_with_lanes : in_objects.objects) { + autoware_auto_perception_msgs::msg::PredictedObject tmp_object; + tmp_object = convertToPredictedObject(object_with_lanes.object); + for (size_t path_id = 0; path_id < object_with_lanes.lanes.size(); ++path_id) { + std::vector tmp_x; + std::vector tmp_y; + std::vector path = object_with_lanes.lanes.at(path_id); + for (size_t i = 0; i < path.size(); i++) { + if (i > 0) { + double dist = autoware_utils::calcDistance2d(path.at(i), path.at(i - 1)); + if (dist < interpolating_resolution_) { + continue; + } + } + tmp_x.push_back(path.at(i).position.x); + tmp_y.push_back(path.at(i).position.y); + } + + // Generate Splined Trajectory Arc-Length Position and Yaw angle + Spline2D spline2d(tmp_x, tmp_y); + std::vector interpolated_points; + std::vector interpolated_yaws; + for (double s = 0.0; s < spline2d.s.back(); s += interpolating_resolution_) { + std::array point1 = spline2d.calc_position(s); + geometry_msgs::msg::Point g_point; + g_point.x = point1[0]; + g_point.y = point1[1]; + g_point.z = object_with_lanes.object.kinematics.pose_with_covariance.pose.position.z; + interpolated_points.push_back(g_point); + interpolated_yaws.push_back(spline2d.calc_yaw(s)); + } + + // calculate initial position in Frenet coordinate + // Optimal Trajectory Generation for Dynamic Street Scenarios in a Frenet Frame + // Path Planning for Highly Automated Driving on Embedded GPUs + geometry_msgs::msg::Point object_point = + object_with_lanes.object.kinematics.pose_with_covariance.pose.position; + + const size_t nearest_segment_idx = + autoware_utils::findNearestSegmentIndex(interpolated_points, object_point); + const double l = autoware_utils::calcLongitudinalOffsetToSegment( + interpolated_points, nearest_segment_idx, object_point); + const double current_s_position = + autoware_utils::calcSignedArcLength(interpolated_points, 0, nearest_segment_idx) + l; + if (current_s_position > spline2d.s.back()) { + continue; + } + std::array s_point = spline2d.calc_position(current_s_position); + geometry_msgs::msg::Point nearest_point = + autoware_utils::createPoint(s_point[0], s_point[1], object_point.z); + double current_d_position = autoware_utils::calcDistance2d(nearest_point, object_point); + const double lane_yaw = spline2d.calc_yaw(current_s_position); + const std::vector origin_v = {std::cos(lane_yaw), std::sin(lane_yaw)}; + const std::vector object_v = { + object_point.x - nearest_point.x, object_point.y - nearest_point.y}; + const double cross2d = object_v.at(0) * origin_v.at(1) - object_v.at(1) * origin_v.at(0); + if (cross2d < 0) { + current_d_position *= -1; + } + + // Does not consider orientation of twist since predicting lane-direction + const double current_d_velocity = + object_with_lanes.object.kinematics.twist_with_covariance.twist.linear.y; + const double current_s_velocity = + std::fabs(object_with_lanes.object.kinematics.twist_with_covariance.twist.linear.x); + + // Predict Path + autoware_auto_perception_msgs::msg::PredictedPath predicted_path; + getPredictedPath( + object_point.z, current_d_position, current_d_velocity, current_s_position, + current_s_velocity, in_objects.header, spline2d, predicted_path); + // substitute confidence value + predicted_path.confidence = object_with_lanes.confidence.at(path_id); + + // Check if the path is already generated + const double CLOSE_PATH_THRESHOLD = 0.1; + bool duplicate_flag = false; + for (const auto & prev_path : tmp_object.kinematics.predicted_paths) { + const auto prev_path_end = prev_path.path.back().position; + const auto current_path_end = predicted_path.path.back().position; + const double dist = autoware_utils::calcDistance2d(prev_path_end, current_path_end); + if (dist < CLOSE_PATH_THRESHOLD) { + duplicate_flag = true; + break; + } + } + + if (duplicate_flag) { + continue; + } + + tmp_object.kinematics.predicted_paths.push_back(predicted_path); + } + + normalizeLikelihood(tmp_object.kinematics); + out_objects.push_back(tmp_object); + } + return true; +} + +bool MapBasedPrediction::doLinearPrediction( + const autoware_auto_perception_msgs::msg::PredictedObjects & in_objects, + std::vector & out_objects) +{ + for (const auto & object : in_objects.objects) { + autoware_auto_perception_msgs::msg::PredictedPath path; + getLinearPredictedPath( + object.kinematics.initial_pose_with_covariance.pose, + object.kinematics.initial_twist_with_covariance.twist, path); + autoware_auto_perception_msgs::msg::PredictedObject tmp_object; + tmp_object = object; + tmp_object.kinematics.predicted_paths.push_back(path); + out_objects.push_back(tmp_object); + } + + return true; +} + +autoware_auto_perception_msgs::msg::PredictedObject MapBasedPrediction::convertToPredictedObject( + const autoware_auto_perception_msgs::msg::TrackedObject & tracked_object) +{ + autoware_auto_perception_msgs::msg::PredictedObject output; + output.object_id = tracked_object.object_id; + output.existence_probability = tracked_object.existence_probability; + output.classification = tracked_object.classification; + output.kinematics = convertToPredictedKinematics(tracked_object.kinematics); + output.shape = tracked_object.shape; + return output; +} + +autoware_auto_perception_msgs::msg::PredictedObjectKinematics +MapBasedPrediction::convertToPredictedKinematics( + const autoware_auto_perception_msgs::msg::TrackedObjectKinematics & tracked_object) +{ + autoware_auto_perception_msgs::msg::PredictedObjectKinematics output; + output.initial_pose_with_covariance = tracked_object.pose_with_covariance; + output.initial_twist_with_covariance = tracked_object.twist_with_covariance; + output.initial_acceleration_with_covariance = tracked_object.acceleration_with_covariance; + return output; +} + +void MapBasedPrediction::normalizeLikelihood( + autoware_auto_perception_msgs::msg::PredictedObjectKinematics & predicted_object_kinematics) +{ + // might not be the smartest way + double sum_confidence = 0.0; + for (const auto & path : predicted_object_kinematics.predicted_paths) { + sum_confidence += path.confidence; + } + + for (auto & path : predicted_object_kinematics.predicted_paths) { + path.confidence = path.confidence / sum_confidence; + } +} + +bool MapBasedPrediction::getPredictedPath( + const double height, const double current_d_position, const double current_d_velocity, + const double current_s_position, const double current_s_velocity, + const std_msgs::msg::Header & origin_header, Spline2D & spline2d, + autoware_auto_perception_msgs::msg::PredictedPath & path) +{ + // Quintic polynomial for d + // A = np.array([[T**3, T**4, T**5], + // [3 * T ** 2, 4 * T ** 3, 5 * T ** 4], + // [6 * T, 12 * T ** 2, 20 * T ** 3]]) + // A_inv = np.matrix([[10/(T**3), -4/(T**2), 1/(2*T)], + // [-15/(T**4), 7/(T**3), -1/(T**2)], + // [6/(T**5), -3/(T**4), 1/(2*T**3)]]) + // b = np.matrix([[xe - self.a0 - self.a1 * T - self.a2 * T**2], + // [vxe - self.a1 - 2 * self.a2 * T], + // [axe - 2 * self.a2]]) + double target_d_position = 0; + + double t = time_horizon_; + Eigen::Matrix3d a_3_inv; + a_3_inv << 10 / std::pow(t, 3), -4 / std::pow(t, 2), 1 / (2 * t), -15 / std::pow(t, 4), + 7 / std::pow(t, 3), -1 / std::pow(t, 2), 6 / std::pow(t, 5), -3 / std::pow(t, 4), + 1 / (2 * std::pow(t, 3)); + + double target_d_velocity = current_d_velocity; + double target_d_acceleration = 0; + Eigen::Vector3d b_3; + b_3 << target_d_position - current_d_position - current_d_velocity * t, + target_d_velocity - current_d_velocity, target_d_acceleration; + + Eigen::Vector3d x_3; + x_3 = a_3_inv * b_3; + + // Quadric polynomial + // A_inv = np.matrix([[1/(T**2), -1/(3*T)], + // [-1/(2*T**3), 1/(4*T**2)]]) + // b = np.matrix([[vxe - self.a1 - 2 * self.a2 * T], + // [axe - 2 * self.a2]]) + Eigen::Matrix2d a_2_inv; + a_2_inv << 1 / std::pow(t, 2), -1 / (3 * t), -1 / (2 * std::pow(t, 3)), 1 / (4 * std::pow(t, 2)); + double target_s_velocity = current_s_velocity; + Eigen::Vector2d b_2; + b_2 << target_s_velocity - current_s_velocity, 0; + Eigen::Vector2d x_2; + x_2 = a_2_inv * b_2; + + // sampling points from calculated path + double dt = sampling_delta_time_; + std::vector d_vec; + for (double i = 0; i < t; i += dt) { + const double calculated_d = current_d_position + current_d_velocity * i + 0 * 2 * i * i + + x_3(0) * i * i * i + x_3(1) * i * i * i * i + + x_3(2) * i * i * i * i * i; + const double calculated_s = current_s_position + current_s_velocity * i + 2 * 0 * i * i + + x_2(0) * i * i * i + x_2(1) * i * i * i * i; + + geometry_msgs::msg::Pose tmp_point; + if (calculated_s > spline2d.s.back()) { + break; + } + std::array p = spline2d.calc_position(calculated_s); + double yaw = spline2d.calc_yaw(calculated_s); + tmp_point.position.x = p[0] + std::cos(yaw - M_PI / 2.0) * calculated_d; + tmp_point.position.y = p[1] + std::sin(yaw - M_PI / 2.0) * calculated_d; + tmp_point.position.z = height; + tf2::Quaternion quat; + quat.setRPY(0.0, 0.0, yaw); + tmp_point.orientation = tf2::toMsg(quat); + path.path.push_back(tmp_point); + if (path.path.size() >= path.path.max_size()) { + break; + } + } + + path.time_step = rclcpp::Duration::from_seconds(dt); + return true; +} + +void MapBasedPrediction::getLinearPredictedPath( + const geometry_msgs::msg::Pose & object_pose, const geometry_msgs::msg::Twist & object_twist, + autoware_auto_perception_msgs::msg::PredictedPath & predicted_path) +{ + const double & sampling_delta_time = sampling_delta_time_; + const double & time_horizon = time_horizon_; + const double ep = 0.001; + + for (double dt = 0.0; dt < time_horizon + ep; dt += sampling_delta_time) { + geometry_msgs::msg::Pose tmp_pose; + geometry_msgs::msg::Pose object_frame_pose; + geometry_msgs::msg::Pose world_frame_pose; + object_frame_pose.position.x = object_twist.linear.x * dt; + object_frame_pose.position.y = object_twist.linear.y * dt; + tf2::Quaternion quat; + quat.setRPY(0.0, 0.0, 0.0); + object_frame_pose.orientation = tf2::toMsg(quat); + tf2::Transform tf_object2future; + tf2::Transform tf_world2object; + tf2::Transform tf_world2future; + + tf2::fromMsg(object_pose, tf_world2object); + tf2::fromMsg(object_frame_pose, tf_object2future); + tf_world2future = tf_world2object * tf_object2future; + tf2::toMsg(tf_world2future, world_frame_pose); + tmp_pose = world_frame_pose; + predicted_path.path.push_back(tmp_pose); + if (predicted_path.path.size() >= predicted_path.path.max_size()) { + break; + } + } + + predicted_path.confidence = 1.0; + predicted_path.time_step = rclcpp::Duration::from_seconds(sampling_delta_time); +} diff --git a/perception/map_based_prediction/src/map_based_prediction_ros.cpp b/perception/map_based_prediction/src/map_based_prediction_ros.cpp new file mode 100644 index 0000000000000..d77603e70f2a0 --- /dev/null +++ b/perception/map_based_prediction/src/map_based_prediction_ros.cpp @@ -0,0 +1,760 @@ +// Copyright 2018-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. + +#include "map_based_prediction_ros.hpp" + +#include "map_based_prediction.hpp" + +#include +#include + +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +std::string toHexString(const unique_identifier_msgs::msg::UUID & id) +{ + std::stringstream ss; + for (auto i = 0; i < 16; ++i) { + ss << std::hex << std::setfill('0') << std::setw(2) << +id.uuid[i]; + } + return ss.str(); +} + +MapBasedPredictionROS::MapBasedPredictionROS(const rclcpp::NodeOptions & node_options) +: Node("map_based_prediction", node_options), + interpolating_resolution_(0.5), + debug_accumulated_time_(0.0) +{ + [[maybe_unused]] auto ret = + rcutils_logging_set_logger_level(this->get_logger().get_name(), RCUTILS_LOG_SEVERITY_DEBUG); + + rclcpp::Clock::SharedPtr clock = std::make_shared(RCL_ROS_TIME); + tf_buffer_ptr_ = std::make_shared(clock); + tf_listener_ptr_ = std::make_shared(*tf_buffer_ptr_); + prediction_time_horizon_ = declare_parameter("prediction_time_horizon", 10.0); + prediction_sampling_delta_time_ = declare_parameter("prediction_sampling_delta_time", 0.5); + min_velocity_for_map_based_prediction_ = + declare_parameter("min_velocity_for_map_based_prediction", 1.0); + dist_threshold_for_searching_lanelet_ = + declare_parameter("dist_threshold_for_searching_lanelet", 3.0); + delta_yaw_threshold_for_searching_lanelet_ = + declare_parameter("delta_yaw_threshold_for_searching_lanelet", 0.785); + sigma_lateral_offset_ = declare_parameter("sigma_lateral_offset", 0.5); + sigma_yaw_angle_ = declare_parameter("sigma_yaw_angle", 5.0); + history_time_length_ = declare_parameter("history_time_length", 1.0); + dist_ratio_threshold_to_left_bound_ = + declare_parameter("dist_ratio_threshold_to_left_bound", -0.5); + dist_ratio_threshold_to_right_bound_ = + declare_parameter("dist_ratio_threshold_to_right_bound", 0.5); + diff_dist_threshold_to_left_bound_ = declare_parameter("diff_dist_threshold_to_left_bound", 0.29); + diff_dist_threshold_to_right_bound_ = + declare_parameter("diff_dist_threshold_to_right_bound", -0.29); + + map_based_prediction_ = std::make_shared( + interpolating_resolution_, prediction_time_horizon_, prediction_sampling_delta_time_); + + sub_objects_ = this->create_subscription( + "/perception/object_recognition/tracking/objects", 1, + std::bind(&MapBasedPredictionROS::objectsCallback, this, std::placeholders::_1)); + sub_map_ = this->create_subscription( + "/vector_map", rclcpp::QoS{1}.transient_local(), + std::bind(&MapBasedPredictionROS::mapCallback, this, std::placeholders::_1)); + + pub_objects_ = this->create_publisher( + "objects", rclcpp::QoS{1}); + pub_markers_ = this->create_publisher( + "objects_path_markers", rclcpp::QoS{1}); +} + +double MapBasedPredictionROS::getObjectYaw( + const autoware_auto_perception_msgs::msg::TrackedObject & object) +{ + if (object.kinematics.orientation_availability) { + return tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); + } + + geometry_msgs::msg::Pose object_frame_pose; + geometry_msgs::msg::Pose map_frame_pose; + object_frame_pose.position.x = object.kinematics.twist_with_covariance.twist.linear.x * 0.1; + object_frame_pose.position.y = object.kinematics.twist_with_covariance.twist.linear.y * 0.1; + tf2::Transform tf_object2future; + tf2::Transform tf_map2object; + tf2::Transform tf_map2future; + + tf2::fromMsg(object.kinematics.pose_with_covariance.pose, tf_map2object); + tf2::fromMsg(object_frame_pose, tf_object2future); + tf_map2future = tf_map2object * tf_object2future; + tf2::toMsg(tf_map2future, map_frame_pose); + double dx = map_frame_pose.position.x - object.kinematics.pose_with_covariance.pose.position.x; + double dy = map_frame_pose.position.y - object.kinematics.pose_with_covariance.pose.position.y; + return std::atan2(dy, dx); +} + +double MapBasedPredictionROS::calculateLikelihood( + const std::vector & path, + const autoware_auto_perception_msgs::msg::TrackedObject & object) +{ + // We compute the confidence value based on the object current position and angle + // Calculate path length + const double path_len = autoware_utils::calcArcLength(path); + const size_t nearest_segment_idx = autoware_utils::findNearestSegmentIndex( + path, object.kinematics.pose_with_covariance.pose.position); + const double l = autoware_utils::calcLongitudinalOffsetToSegment( + path, nearest_segment_idx, object.kinematics.pose_with_covariance.pose.position); + const double current_s_position = + autoware_utils::calcSignedArcLength(path, 0, nearest_segment_idx) + l; + // If the obstacle is ahead of this path, we assume the confidence for this path is 0 + if (current_s_position > path_len) { + return 0.0; + } + + // Euclid Lateral Distance + const double abs_d = std::fabs( + autoware_utils::calcLateralOffset(path, object.kinematics.pose_with_covariance.pose.position)); + + // Yaw Difference between obstacle and lane angle + const double lane_yaw = tf2::getYaw(path.at(nearest_segment_idx).orientation); + const double object_yaw = getObjectYaw(object); + const double delta_yaw = object_yaw - lane_yaw; + const double abs_norm_delta_yaw = std::fabs(autoware_utils::normalizeRadian(delta_yaw)); + + // Compute Chi-squared distributed (Equation (8) in the paper) + const double sigma_d = sigma_lateral_offset_; // Standard Deviation for lateral position + const double sigma_yaw = M_PI * sigma_yaw_angle_ / 180.0; // Standard Deviation for yaw angle + Eigen::Vector2d delta; + delta << abs_d, abs_norm_delta_yaw; + Eigen::Matrix2d P_inv; + P_inv << 1.0 / (sigma_d * sigma_d), 0.0, 0.0, 1.0 / (sigma_yaw * sigma_yaw); + const double MINIMUM_DISTANCE = 1e-6; + const double dist = std::max(delta.dot(P_inv * delta), MINIMUM_DISTANCE); + return 1.0 / dist; +} + +bool MapBasedPredictionROS::checkCloseLaneletCondition( + const std::pair & lanelet, + const autoware_auto_perception_msgs::msg::TrackedObject & object, + const lanelet::BasicPoint2d & search_point) +{ + // Step1. If we only have one point in the centerline, we will ignore the lanelet + if (lanelet.second.centerline().size() <= 1) { + return false; + } + + // Step2. Check if the obstacle is inside of this lanelet + if (!lanelet::geometry::inside(lanelet.second, search_point)) { + return false; + } + + // If the object is in the object buffer, we check if the target lanelet is + // inside the current lanelets id or following lanelets + const std::string object_id = toHexString(object.object_id); + if (object_buffer_.count(object_id) != 0) { + const std::vector & possible_lanelet = + object_buffer_.at(object_id).back().future_possible_lanelets; + + bool not_in_possible_lanelet = + std::find(possible_lanelet.begin(), possible_lanelet.end(), lanelet.second) == + possible_lanelet.end(); + if (not_in_possible_lanelet) { + return false; + } + } + + // Step3. Calculate the angle difference between the lane angle and obstacle angle + double object_yaw = getObjectYaw(object); + const double lane_yaw = lanelet::utils::getLaneletAngle( + lanelet.second, object.kinematics.pose_with_covariance.pose.position); + const double delta_yaw = object_yaw - lane_yaw; + const double normalized_delta_yaw = std::atan2(std::sin(delta_yaw), std::cos(delta_yaw)); + const double abs_norm_delta = std::fabs(normalized_delta_yaw); + + // Step4. Check if the closest lanelet is valid, and add all + // of the lanelets that are below max_dist and max_delta_yaw + if ( + lanelet.first < dist_threshold_for_searching_lanelet_ && + abs_norm_delta < delta_yaw_threshold_for_searching_lanelet_) { + return true; + } + + return false; +} + +bool MapBasedPredictionROS::getClosestLanelets( + const autoware_auto_perception_msgs::msg::TrackedObject & object, + const lanelet::LaneletMapPtr & lanelet_map_ptr_, lanelet::ConstLanelets & closest_lanelets) +{ + std::chrono::high_resolution_clock::time_point begin = std::chrono::high_resolution_clock::now(); + + // obstacle point + lanelet::BasicPoint2d search_point( + object.kinematics.pose_with_covariance.pose.position.x, + object.kinematics.pose_with_covariance.pose.position.y); + + // nearest lanelet + std::vector> surrounding_lanelets = + lanelet::geometry::findNearest(lanelet_map_ptr_->laneletLayer, search_point, 10); + + std::chrono::high_resolution_clock::time_point end = std::chrono::high_resolution_clock::now(); + std::chrono::nanoseconds time = std::chrono::duration_cast(end - begin); + debug_accumulated_time_ += time.count() / (1000.0 * 1000.0); + + // No Closest Lanelets + if (surrounding_lanelets.empty()) { + return false; + } + + const double MIN_DIST = 1e-6; + bool found_target_closest_lanelet = false; + for (const auto & lanelet : surrounding_lanelets) { + // Check if the close lanelets meet the necessary condition for start lanelets + if (checkCloseLaneletCondition(lanelet, object, search_point)) { + // If the lanelet meets the condition, + // then check if similar lanelet is inside the closest lanelet + bool is_duplicate = false; + for (const auto & closest_lanelet : closest_lanelets) { + const auto lanelet_end_p = lanelet.second.centerline2d().back(); + const auto closest_lanelet_end_p = closest_lanelet.centerline2d().back(); + const double dist = std::hypot( + lanelet_end_p.x() - closest_lanelet_end_p.x(), + lanelet_end_p.y() - closest_lanelet_end_p.y()); + if (dist < MIN_DIST) { + is_duplicate = true; + break; + } + } + if (is_duplicate) { + continue; + } + + found_target_closest_lanelet = true; + closest_lanelets.push_back(lanelet.second); + } + } + + // If the closest lanelet is valid, return true + return found_target_closest_lanelet; +} + +void MapBasedPredictionROS::removeInvalidObject(const double current_time) +{ + std::vector invalid_object_id; + for (auto iter = object_buffer_.begin(); iter != object_buffer_.end(); ++iter) { + const std::string object_id = iter->first; + std::deque & object_data = iter->second; + + // If object data is empty, we are going to delete the buffer for the obstacle + if (object_data.empty()) { + invalid_object_id.push_back(object_id); + continue; + } + const double latest_object_time = rclcpp::Time(object_data.back().pose.header.stamp).seconds(); + + // Delete Old Objects + if (current_time - latest_object_time > 2.0) { + invalid_object_id.push_back(object_id); + continue; + } + + // Delete old information + while (!object_data.empty()) { + const double post_object_time = rclcpp::Time(object_data.front().pose.header.stamp).seconds(); + if (current_time - post_object_time > 2.0) { + // Delete Old Position + object_data.pop_front(); + } else { + break; + } + } + + if (object_data.empty()) { + invalid_object_id.push_back(object_id); + continue; + } + } + + for (const auto & key : invalid_object_id) { + object_buffer_.erase(key); + } +} + +bool MapBasedPredictionROS::updateObjectBuffer( + const std_msgs::msg::Header & header, + const autoware_auto_perception_msgs::msg::TrackedObject & object, + lanelet::ConstLanelets & current_lanelets) +{ + using Label = autoware_auto_perception_msgs::msg::ObjectClassification; + // Ignore non-vehicle object + if ( + object.classification.front().label != Label::CAR && + object.classification.front().label != Label::BUS && + object.classification.front().label != Label::TRUCK) { + return false; + } + + // Get the current Lanelet + std::string object_id = toHexString(object.object_id); + + // Check whether the object is on the road + if (!getClosestLanelets(object, lanelet_map_ptr_, current_lanelets)) { + // This Object is not on the road + return false; + } + + // Get current Pose + geometry_msgs::msg::PoseStamped current_object_pose; + current_object_pose.header = header; + current_object_pose.pose = object.kinematics.pose_with_covariance.pose; + + // Update Object Buffer + if (object_buffer_.count(object_id) == 0) { + // New Object + ObjectData single_object_data; + single_object_data.current_lanelets = current_lanelets; + single_object_data.future_possible_lanelets = current_lanelets; + single_object_data.pose = current_object_pose; + const double object_yaw = getObjectYaw(object); + single_object_data.pose.pose.orientation = autoware_utils::createQuaternionFromYaw(object_yaw); + + std::deque object_data; + object_data.push_back(single_object_data); + + // Create new key, value pair in the buffer + object_buffer_.emplace(object_id, object_data); + } else { + // Object that is already in the object buffer + std::deque & object_data = object_buffer_.at(object_id); + + ObjectData single_object_data; + single_object_data.current_lanelets = current_lanelets; + single_object_data.future_possible_lanelets = current_lanelets; + single_object_data.pose = current_object_pose; + const double object_yaw = getObjectYaw(object); + single_object_data.pose.pose.orientation = autoware_utils::createQuaternionFromYaw(object_yaw); + + // push new object data + object_data.push_back(single_object_data); + } + + // If the obstacle is too slow, we do linear prediction + if ( + std::fabs(object.kinematics.twist_with_covariance.twist.linear.x) < + min_velocity_for_map_based_prediction_) { + return false; + } + + return true; +} + +void MapBasedPredictionROS::updatePossibleLanelets( + const std::string object_id, const lanelet::routing::LaneletPaths & paths) +{ + if (object_buffer_.count(object_id) != 0) { + std::vector & possible_lanelets = + object_buffer_.at(object_id).back().future_possible_lanelets; + for (const auto & path : paths) { + for (const auto & lanelet : path) { + bool not_in_buffer = + std::find(possible_lanelets.begin(), possible_lanelets.end(), lanelet) == + possible_lanelets.end(); + if (not_in_buffer) { + possible_lanelets.push_back(lanelet); + } + } + } + } +} + +void MapBasedPredictionROS::addValidPath( + const lanelet::routing::LaneletPaths & candidate_paths, + lanelet::routing::LaneletPaths & valid_paths) +{ + // Check if candidate paths are already in the valid paths + for (const auto & candidate_path : candidate_paths) { + bool already_searched = false; + for (const auto & valid_path : valid_paths) { + for (const auto & llt : valid_path) { + if (candidate_path.back().id() == llt.id()) { + already_searched = true; + } + } + } + if (!already_searched) { + valid_paths.push_back(candidate_path); + } + } +} + +double MapBasedPredictionROS::calcRightLateralOffset( + const lanelet::ConstLineString2d & bound_line, const geometry_msgs::msg::Pose & search_pose) +{ + std::vector bound_path(bound_line.size()); + for (size_t i = 0; i < bound_path.size(); ++i) { + const double x = bound_line[i].x(); + const double y = bound_line[i].y(); + bound_path[i] = autoware_utils::createPoint(x, y, 0.0); + } + + return std::fabs(autoware_utils::calcLateralOffset(bound_path, search_pose.position)); +} + +double MapBasedPredictionROS::calcLeftLateralOffset( + const lanelet::ConstLineString2d & bound_line, const geometry_msgs::msg::Pose & search_pose) +{ + return -calcRightLateralOffset(bound_line, search_pose); +} + +Maneuver MapBasedPredictionROS::detectLaneChange( + const autoware_auto_perception_msgs::msg::TrackedObject & object, + const lanelet::ConstLanelet & current_lanelet, const double current_time) +{ + // Step1. Check if we have the object in the buffer + const std::string object_id = toHexString(object.object_id); + if (object_buffer_.count(object_id) == 0) { + return Maneuver::LANE_FOLLOW; + } + + // Object History Data + const std::deque object_info = object_buffer_.at(object_id); + + // Step2. Get the previous id + int prev_id = static_cast(object_info.size()) - 1; + while (prev_id >= 0) { + const double prev_time = rclcpp::Time(object_info.at(prev_id).pose.header.stamp).seconds(); + if (current_time - prev_time > history_time_length_) { + break; + } + --prev_id; + } + + if (prev_id < 0) { + return Maneuver::LANE_FOLLOW; + } + + // Step3. Get closest previous lanelet ID + const auto prev_pose = object_info.at(static_cast(prev_id)).pose; + const lanelet::ConstLanelets prev_lanelets = + object_info.at(static_cast(prev_id)).current_lanelets; + if (prev_lanelets.empty()) { + return Maneuver::LANE_FOLLOW; + } + lanelet::ConstLanelet prev_lanelet = prev_lanelets.front(); + double closest_prev_yaw = std::numeric_limits::max(); + for (const auto & lanelet : prev_lanelets) { + const double lane_yaw = lanelet::utils::getLaneletAngle(lanelet, prev_pose.pose.position); + const double delta_yaw = tf2::getYaw(prev_pose.pose.orientation) - lane_yaw; + const double normalized_delta_yaw = autoware_utils::normalizeRadian(delta_yaw); + if (normalized_delta_yaw < closest_prev_yaw) { + closest_prev_yaw = normalized_delta_yaw; + prev_lanelet = lanelet; + } + } + + // Step4. Check if the vehicle has changed lane + const auto current_pose = object.kinematics.pose_with_covariance.pose; + const double dist = autoware_utils::calcDistance2d(prev_pose, current_pose); + lanelet::routing::LaneletPaths possible_paths = + routing_graph_ptr_->possiblePaths(prev_lanelet, dist + 2.0, 0, false); + bool has_lane_changed = true; + for (const auto & path : possible_paths) { + for (const auto & lanelet : path) { + if (lanelet == current_lanelet) { + has_lane_changed = false; + break; + } + } + } + + if (has_lane_changed) { + return Maneuver::LANE_FOLLOW; + } + + // Step5. Lane Change Detection + const lanelet::ConstLineString2d prev_left_bound = prev_lanelet.leftBound2d(); + const lanelet::ConstLineString2d prev_right_bound = prev_lanelet.rightBound2d(); + const lanelet::ConstLineString2d current_left_bound = current_lanelet.leftBound2d(); + const lanelet::ConstLineString2d current_right_bound = current_lanelet.rightBound2d(); + const double prev_left_dist = calcLeftLateralOffset(prev_left_bound, prev_pose.pose); + const double prev_right_dist = calcRightLateralOffset(prev_right_bound, prev_pose.pose); + const double current_left_dist = calcLeftLateralOffset(current_left_bound, current_pose); + const double current_right_dist = calcRightLateralOffset(current_right_bound, current_pose); + const double prev_lane_width = std::fabs(prev_left_dist) + std::fabs(prev_right_dist); + const double current_lane_width = std::fabs(current_left_dist) + std::fabs(current_right_dist); + if (prev_lane_width < 1e-3 || current_lane_width < 1e-3) { + RCLCPP_ERROR(get_logger(), "[Map Based Prediction]: Lane Width is too small"); + return Maneuver::LANE_FOLLOW; + } + + const double current_left_dist_ratio = current_left_dist / current_lane_width; + const double current_right_dist_ratio = current_right_dist / current_lane_width; + const double diff_left_current_prev = current_left_dist - prev_left_dist; + const double diff_right_current_prev = current_right_dist - prev_right_dist; + + if ( + current_left_dist_ratio > dist_ratio_threshold_to_left_bound_ && + diff_left_current_prev > diff_dist_threshold_to_left_bound_) { + return Maneuver::LEFT_LANE_CHANGE; + } else if ( + current_right_dist_ratio < dist_ratio_threshold_to_right_bound_ && + diff_right_current_prev < diff_dist_threshold_to_right_bound_) { + return Maneuver::RIGHT_LANE_CHANGE; + } + + return Maneuver::LANE_FOLLOW; +} + +void MapBasedPredictionROS::objectsCallback( + const autoware_auto_perception_msgs::msg::TrackedObjects::ConstSharedPtr in_objects) +{ + debug_accumulated_time_ = 0.0; + std::chrono::high_resolution_clock::time_point begin = std::chrono::high_resolution_clock::now(); + + if (!lanelet_map_ptr_) { + return; + } + + geometry_msgs::msg::TransformStamped world2map_transform; + geometry_msgs::msg::TransformStamped map2world_transform; + geometry_msgs::msg::TransformStamped debug_map2lidar_transform; + try { + world2map_transform = tf_buffer_ptr_->lookupTransform( + "map", // target + in_objects->header.frame_id, // src + in_objects->header.stamp, rclcpp::Duration::from_seconds(0.1)); + map2world_transform = tf_buffer_ptr_->lookupTransform( + in_objects->header.frame_id, // target + "map", // src + in_objects->header.stamp, rclcpp::Duration::from_seconds(0.1)); + debug_map2lidar_transform = tf_buffer_ptr_->lookupTransform( + "base_link", // target + "map", // src + rclcpp::Time(), rclcpp::Duration::from_seconds(0.1)); + } catch (tf2::TransformException & ex) { + RCLCPP_ERROR(get_logger(), ex.what()); + return; + } + + /////////////////////////////////////////////////////////// + /////////////////// Update Object Buffer ////////////////// + ////////////////////////////////////////////////////////// + const double objects_detected_time = rclcpp::Time(in_objects->header.stamp).seconds(); + removeInvalidObject(objects_detected_time); + + ///////////////////////////////////////////////////////// + ///////////////////// Prediction /////////////////////// + /////////////////////////////////////////////////////// + autoware_auto_perception_msgs::msg::PredictedObjects objects_without_map; + objects_without_map.header = in_objects->header; + DynamicObjectWithLanesArray prediction_input; + prediction_input.header = in_objects->header; + + for (const auto & object : in_objects->objects) { + std::string object_id = toHexString(object.object_id); + DynamicObjectWithLanes transformed_object; + transformed_object.object = object; + if (in_objects->header.frame_id != "map") { + geometry_msgs::msg::PoseStamped pose_in_map; + geometry_msgs::msg::PoseStamped pose_orig; + pose_orig.pose = object.kinematics.pose_with_covariance.pose; + tf2::doTransform(pose_orig, pose_in_map, world2map_transform); + transformed_object.object.kinematics.pose_with_covariance.pose = pose_in_map.pose; + } + + std_msgs::msg::Header transformed_header = in_objects->header; + transformed_header.frame_id = "map"; + lanelet::ConstLanelets start_lanelets; // current lanelet + if (!updateObjectBuffer(transformed_header, transformed_object.object, start_lanelets)) { + objects_without_map.objects.push_back( + map_based_prediction_->convertToPredictedObject(transformed_object.object)); + continue; + } + + // Obtain valid Paths + const double delta_horizon = 1.0; + const double obj_vel = object.kinematics.twist_with_covariance.twist.linear.x; + lanelet::routing::LaneletPaths paths; + for (const auto & start_lanelet : start_lanelets) { + // Step1. Lane Change Detection + // First: Right to Left Detection Result + // Second: Left to Right Detection Result + const Maneuver maneuver = detectLaneChange(object, start_lanelet, objects_detected_time); + + // Step2. Get the left lanelet + lanelet::routing::LaneletPaths left_paths; + auto opt_left = routing_graph_ptr_->left(start_lanelet); + if (!!opt_left) { + for (double horizon = prediction_time_horizon_; horizon > 0; horizon -= delta_horizon) { + const double search_dist = horizon * obj_vel + 10.0; + lanelet::routing::LaneletPaths tmp_paths = + routing_graph_ptr_->possiblePaths(*opt_left, search_dist, 0, false); + addValidPath(tmp_paths, left_paths); + } + } + + // Step3. Get the right lanelet + lanelet::routing::LaneletPaths right_paths; + auto opt_right = routing_graph_ptr_->right(start_lanelet); + if (!!opt_right) { + for (double horizon = prediction_time_horizon_; horizon > 0; horizon -= delta_horizon) { + const double search_dist = horizon * obj_vel + 10.0; + lanelet::routing::LaneletPaths tmp_paths = + routing_graph_ptr_->possiblePaths(*opt_right, search_dist, 0, false); + addValidPath(tmp_paths, right_paths); + } + } + + // Step4. Get the centerline + lanelet::routing::LaneletPaths center_paths; + for (double horizon = prediction_time_horizon_; horizon > 0; horizon -= delta_horizon) { + const double search_dist = horizon * obj_vel + 10.0; + lanelet::routing::LaneletPaths tmp_paths = + routing_graph_ptr_->possiblePaths(start_lanelet, search_dist, 0, false); + addValidPath(tmp_paths, center_paths); + } + + // Step5. Insert Valid Paths + if (!left_paths.empty() && maneuver == Maneuver::LEFT_LANE_CHANGE) { + paths.insert(paths.end(), left_paths.begin(), left_paths.end()); + } else if (!right_paths.empty() && maneuver == Maneuver::RIGHT_LANE_CHANGE) { + paths.insert(paths.end(), right_paths.begin(), right_paths.end()); + } else { + paths.insert(paths.end(), center_paths.begin(), center_paths.end()); + } + } + // If there is no valid path, we'll mark this object as map-less object + if (paths.empty()) { + objects_without_map.objects.push_back( + map_based_prediction_->convertToPredictedObject(transformed_object.object)); + continue; + } + + // Update Possible lanelet in the object buffer + updatePossibleLanelets(object_id, paths); + + std::vector> tmp_paths; + std::vector tmp_confidence; + for (const auto & path : paths) { + std::vector tmp_path; + + // Insert Positions. Note that we insert points from previous lanelet + if (!path.empty()) { + lanelet::ConstLanelets prev_lanelets = routing_graph_ptr_->previous(path.front()); + if (!prev_lanelets.empty()) { + lanelet::ConstLanelet prev_lanelet = prev_lanelets.front(); + for (const auto & point : prev_lanelet.centerline()) { + geometry_msgs::msg::Pose tmp_pose; + tmp_pose.position.x = point.x(); + tmp_pose.position.y = point.y(); + tmp_pose.position.z = point.z(); + tmp_path.push_back(tmp_pose); + } + } + } + + for (const auto & lanelet : path) { + for (const auto & point : lanelet.centerline()) { + geometry_msgs::msg::Pose tmp_pose; + tmp_pose.position.x = point.x(); + tmp_pose.position.y = point.y(); + tmp_pose.position.z = point.z(); + + // Prevent from inserting same points + if (!tmp_path.empty()) { + const auto prev_pose = tmp_path.back(); + const double tmp_dist = autoware_utils::calcDistance2d(prev_pose, tmp_pose); + if (tmp_dist < 1e-6) { + continue; + } + } + + tmp_path.push_back(tmp_pose); + } + } + + if (tmp_path.size() < 2) { + continue; + } + + // Compute yaw angles + for (size_t pose_id = 0; pose_id < tmp_path.size() - 1; ++pose_id) { + double tmp_yaw = std::atan2( + tmp_path.at(pose_id + 1).position.y - tmp_path.at(pose_id).position.y, + tmp_path.at(pose_id + 1).position.x - tmp_path.at(pose_id).position.x); + tf2::Quaternion quat; + quat.setRPY(0.0, 0.0, tmp_yaw); + tmp_path.at(pose_id).orientation = tf2::toMsg(quat); + } + tmp_path.back().orientation = tmp_path.at(tmp_path.size() - 2).orientation; + + ////////////////////////////////////////////////////////////////////// + // Calculate Confidence of each path(centerline) for this obstacle // + //////////////////////////////////////////////////////////////////// + const double confidence = calculateLikelihood(tmp_path, transformed_object.object); + // Ignore a path that has too low confidence + if (confidence < 1e-6) { + continue; + } + + tmp_paths.push_back(tmp_path); + tmp_confidence.push_back(confidence); + } + + transformed_object.lanes = tmp_paths; + transformed_object.confidence = tmp_confidence; + prediction_input.objects.push_back(transformed_object); + } + + std::chrono::high_resolution_clock::time_point end = std::chrono::high_resolution_clock::now(); + std::chrono::nanoseconds time = std::chrono::duration_cast(end - begin); + + std::vector out_objects_in_map; + map_based_prediction_->doPrediction(prediction_input, out_objects_in_map); + autoware_auto_perception_msgs::msg::PredictedObjects output; + output.header = in_objects->header; + output.header.frame_id = "map"; + output.objects = out_objects_in_map; + + std::vector out_objects_without_map; + map_based_prediction_->doLinearPrediction(objects_without_map, out_objects_without_map); + output.objects.insert( + output.objects.begin(), out_objects_without_map.begin(), out_objects_without_map.end()); + pub_objects_->publish(output); +} + +void MapBasedPredictionROS::mapCallback( + const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg) +{ + RCLCPP_INFO(get_logger(), "Start loading lanelet"); + lanelet_map_ptr_ = std::make_shared(); + lanelet::utils::conversion::fromBinMsg( + *msg, lanelet_map_ptr_, &traffic_rules_ptr_, &routing_graph_ptr_); + RCLCPP_INFO(get_logger(), "Map is loaded"); +} + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(MapBasedPredictionROS)