Skip to content

Commit

Permalink
feat: add map_based_prediction package (autowarefoundation#69)
Browse files Browse the repository at this point in the history 
* release v0.4.0

* remove ROS1 packages temporarily

Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp>

* Revert "remove ROS1 packages temporarily"

This reverts commit 7c50d2f586d6a3480ec8c62b19d81f42de403d28.

Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp>

* add COLCON_IGNORE to ros1 packages

Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp>

* Rename launch files to launch.xml (autowarefoundation#28)

* Port map based prediction (autowarefoundation#95)

* Port to ROS2

Signed-off-by: Servando German Serrano <servando.german.serrano@linaro.org>

* Add missing pkg to fix build error

Signed-off-by: Servando German Serrano <servando.german.serrano@linaro.org>

* Updated function

Signed-off-by: Servando German Serrano <servando.german.serrano@linaro.org>

* Fix launch

Signed-off-by: Servando German Serrano <servando.german.serrano@linaro.org>

* [map_based_detection] fix launch file (autowarefoundation#115)

Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp>

* Convert calls of Duration to Duration::from_seconds where appropriate (autowarefoundation#131)

* Rename h files to hpp (autowarefoundation#142)

* Change includes

* Rename files

* Adjustments to make things compile

* Other packages

* Adjust copyright notice on 532 out of 699 source files (autowarefoundation#143)

* Use quotes for includes where appropriate (autowarefoundation#144)

* Use quotes for includes where appropriate

* Fix lint tests

* Make tests pass hopefully

* Run uncrustify on the entire Pilot.Auto codebase (autowarefoundation#151)

* Run uncrustify on the entire Pilot.Auto codebase

* Exclude open PRs

* fixing trasient_local in ROS2 packages (autowarefoundation#160)

* [map_based_prediction] add remapping option for the launch file in map_based_prediction (autowarefoundation#199)

Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp>

* Enable lints in map_based_prediction (autowarefoundation#153)

* port naive_path_prediction from ROS1 to ROS2 (autowarefoundation#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 <horibe.takamasa@gmail.com>

* Ros2 v0.8.0 map based prediction (autowarefoundation#299)

* restore file name for v0.8.0 update

Signed-off-by: Azumi Suzuki <azumi.suzuki@tier4.jp>

* fix typos in perception (autowarefoundation#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 <azumi.suzuki@tier4.jp>

* delete commented out line

Signed-off-by: Azumi Suzuki <azumi.suzuki@tier4.jp>

Co-authored-by: Kazuki Miyahara <kmiya@outlook.com>

* Ros2 v0.8.0 naive path prediction (autowarefoundation#305)

* restore naive_path_prediction files for v0.8.0 update

* fix topic name (autowarefoundation#1016)

* Revert "restore naive_path_prediction files for v0.8.0 update"

This reverts commit 77a87c3dce4a3134d2cd11180b635748f9b0202a.

Co-authored-by: Taichi Higashide <taichi.higashide@tier4.jp>

* update target branch for ci (autowarefoundation#309)

* update target branch for ci

Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp>

* [as]: Fix lint

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* [latlon_muxer]: Fix lint

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* [map_based_prediction]: Fix lint

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* [raw_vehicle_cmd_converter]: Fix lint

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* [remote_cmd_converter]: Fix lint

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* [turn_signal_decider]: Fix lint

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

Co-authored-by: wep21 <border_goldenmarket@yahoo.co.jp>

* fix duration (autowarefoundation#445)

* fix duration

Signed-off-by: Kosuke Murakami <kosuke.murakami@tier4.jp>

* change to from_seconds

Signed-off-by: Kosuke Murakami <kosuke.murakami@tier4.jp>

* fix other duration

Signed-off-by: Kosuke Murakami <kosuke.murakami@tier4.jp>

* replace -1 with 0

Signed-off-by: Kosuke Murakami <kosuke.murakami@tier4.jp>

* apply ament_lint_common

Signed-off-by: Kosuke Murakami <kosuke.murakami@tier4.jp>

* uncrustify

Signed-off-by: Kosuke Murakami <kosuke.murakami@tier4.jp>

* add space

Signed-off-by: Kosuke Murakami <kosuke.murakami@tier4.jp>

* add another space

Signed-off-by: Kosuke Murakami <kosuke.murakami@tier4.jp>

* Fix typo in perception module (autowarefoundation#440)

* add use_sim-time option (autowarefoundation#454)

* Format launch files (autowarefoundation#1219)

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Fix rolling build errors (autowarefoundation#1225)

* Add missing include files

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Replace rclcpp::Duration

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Use reference for exceptions

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Use from_seconds

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Unify Apache-2.0 license name (autowarefoundation#1242)

* Remove use_sim_time for set_parameter (autowarefoundation#1260)

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* Perception components (autowarefoundation#1368)

* [bev_optical_flow]: component node

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* [object_merger]: component node

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* [object_range_splitter]: component node

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* [shape_estimation]: component node

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* [map_based_prediction]: component node

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* [naive_path_prediction]: component node

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* [roi_image_saver]: component node

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* [lidar_apollo_instance_segmentation]: component node

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* [object_flow_fusion]: component node

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* [traffic_light_map_based_detector]: component node

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* [dynamic_object_visualization]: component node

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* Fix typo

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* Fix -Wunused-parameter (autowarefoundation#1836)

* Fix -Wunused-parameter

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Fix mistake

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* fix spell

* Fix lint issues

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Ignore flake8 warnings

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

Co-authored-by: Hiroki OTA <hiroki.ota@tier4.jp>

* fix some typos (autowarefoundation#1941)

* fix some typos

* fix typo

* Fix typo

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

Co-authored-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Invoke code formatter at pre-commit (autowarefoundation#1935)

* Run ament_uncrustify at pre-commit

* Reformat existing files
* Fix copyright and cpplint errors

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>
Co-authored-by: Kenji Miyake <kenji.miyake@tier4.jp>

* [map_based_prediction] add readme with reference (autowarefoundation#2041)

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* add sort-package-xml hook in pre-commit (autowarefoundation#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 (autowarefoundation#2012)

* Detection by tracker (autowarefoundation#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 (autowarefoundation#2332)

* Revert "Temporarily comment out pre-commit hooks"

This reverts commit 748e9cdb145ce12f8b520bcbd97f5ff899fc28a3.

* Replace ament_lint_common with autoware_lint_common

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Remove ament_cmake_uncrustify and ament_clang_format

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Apply Black

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Apply clang-format

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Fix build errors

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Fix for cpplint

* Fix include double quotes to angle brackets

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Apply clang-format

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Fix build errors

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Add COLCON_IGNORE (autowarefoundation#500)

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Auto/map based prediction (autowarefoundation#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 (autowarefoundation#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 (autowarefoundation#645)

* delete depracated pkg

* delete object_flow_fusion and bev_optical_flow

* consider upper size of path (autowarefoundation#654)

* consider upper size of path

* misc

* fic bug

* Sync .auto branch with the latest branch in internal repository (autowarefoundation#691)

* add trajectory point offset in rviz plugin (autowarefoundation#2270)

* sync rc rc/v0.23.0 (autowarefoundation#2258)

* fix interpolation for insert point (autowarefoundation#2228)

* fix interpolation for insert point

* to prev interpolation pkg

* Revert "to prev interpolation pkg"

This reverts commit 9eb145b5d36e297186015fb17c267ccd5b3c21ef.

Co-authored-by: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com>
Co-authored-by: taikitanaka <ttatcoder@outlook.jp>

* fix topic name (autowarefoundation#2266)

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* Add namespace to diag for dual_return_filter (autowarefoundation#2269)

* Add a function to make 'geometry_msgs::msg::TransformStamped' (autowarefoundation#2250)

* Add a function to make 'geometry_msgs::msg::TransformStamped'
* Add 'child_frame_id' as an argument of 'pose2transform'

* Simplify marker scale initialization (autowarefoundation#2286)

* Fix/crosswalk polygon (autowarefoundation#2279)

* extend crosswalk polygon

* improve readability

* fix polygon shape

* Add warning when decel distance calculation fails (autowarefoundation#2289)

Signed-off-by: Makoto Kurihara <mkuri8m@gmail.com>

* [motion_velocity_smoother] ignore debug print (autowarefoundation#2292)

* cosmetic change

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* cahnge severity from WARN to DEBUG for debug info

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* use util for stop_watch

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* fix map based prediction (autowarefoundation#2200)

* fix map based prediction

* fix format

* change map based prediction

* fix spells

* fix spells in comments

* fix for cpplint

* fix some problems

* fix format and code for clang-tidy

* fix space for cpplint

* Update Readme.md

* Update perception/object_recognition/prediction/map_based_prediction/Readme.md

* Update perception/object_recognition/prediction/map_based_prediction/Readme.md

* Update perception/object_recognition/prediction/map_based_prediction/Readme.md

* Update perception/object_recognition/prediction/map_based_prediction/Readme.md

* Update perception/object_recognition/prediction/map_based_prediction/Readme.md

* Update perception/object_recognition/prediction/map_based_prediction/Readme.md

* fix vector access method

* fix readme format

* add parameter

* Update perception/object_recognition/prediction/map_based_prediction/Readme.md

Co-authored-by: Kazuki Miyahara <kmiya@outlook.com>

* Update perception/object_recognition/prediction/map_based_prediction/Readme.md

Co-authored-by: Kazuki Miyahara <kmiya@outlook.com>

* Update perception/object_recognition/prediction/map_based_prediction/Readme.md

Co-authored-by: Kazuki Miyahara <kmiya@outlook.com>

* Update Readme.md

* Update perception/object_recognition/prediction/map_based_prediction/Readme.md

Co-authored-by: Kazuki Miyahara <kmiya@outlook.com>

Co-authored-by: tkimura4 <tomoya.kimura@tier4.jp>
Co-authored-by: Kazuki Miyahara <kmiya@outlook.com>

* remove failure condition for 0 velocity trajectory (autowarefoundation#2295)

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* [mpc_follower] remove stop distance condition from stopState decision (autowarefoundation#1916)

* [mpc_follower] remove stop distance condition from stopState decision

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* add invalid index handling

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* Move the debug marker initialization part to another file (autowarefoundation#2288)

* Move the debug marker initialization part to 'debug.cpp'

* Make 'isLocalOptimalSolutionOscillation' independent from 'NDTScanMatcher' (autowarefoundation#2300)

* Remove an unused function 'getTransform' (autowarefoundation#2301)

* Simplify iteration of initial poses (autowarefoundation#2310)

* Make a transform object const (autowarefoundation#2311)

* Represent poses in 'std::vector' instead of 'geometry_msgs::msg::PoseArray' (autowarefoundation#2312)

* Feature/no stopping area (autowarefoundation#2163)

* add no stopping area module to behavior velocity planner

* apply utils

* add polygon interpolation module order stopline around area is considered

* devide jpass udge with stop line polygon

* update docs

* rename file name

* update to latest

* minor change for marker

* update license

Co-authored-by: Yukihiro Saito <yukky.saito@gmail.com>

* update license

Co-authored-by: Yukihiro Saito <yukky.saito@gmail.com>

* update license

Co-authored-by: Yukihiro Saito <yukky.saito@gmail.com>

* update license

Co-authored-by: Yukihiro Saito <yukky.saito@gmail.com>

* minor fix

* add parameter tuning at experiment

* update readme

* format doc

* apply comments

* add exception gurd

* cosmetic change

* fix ament

* fix typo and remove for statement

* & to " "

* better ns

* return pass judge param

* add missing stoppable condition

* add clear pass judge and stoppable flag

* add comment

* precommit fix

* cpplint

Co-authored-by: Yukihiro Saito <yukky.saito@gmail.com>

* sync rc rc/v0.23.0 (autowarefoundation#2281)

* Fix side shift planner (autowarefoundation#2171) (autowarefoundation#2172)

* add print debug

Signed-off-by: TakaHoribe <horibe.takamasa@gmail.com>

* remove forward shift points when adding new point

Signed-off-by: TakaHoribe <horibe.takamasa@gmail.com>

* remove debug print

Signed-off-by: TakaHoribe <horibe.takamasa@gmail.com>

* format

Signed-off-by: TakaHoribe <horibe.takamasa@gmail.com>

* Fix remove threshold

Co-authored-by: Fumiya Watanabe <rej55.g@gmail.com>

Co-authored-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* Fix/pull out and pull over (autowarefoundation#2175)

* delete unnecessary check

* fix condition of starting pull out

* Add emergency status API (autowarefoundation#2174) (autowarefoundation#2182)

* Fix/mpc reset prev result (autowarefoundation#2185) (autowarefoundation#2195)

* reset prev result

* clean code

* reset only raw_steer_cmd

* Update control/mpc_follower/src/mpc_follower_core.cpp

Co-authored-by: Takamasa Horibe <horibe.takamasa@gmail.com>

Co-authored-by: Takamasa Horibe <horibe.takamasa@gmail.com>

Co-authored-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* [hotfix] 1 path point exception after resampling (autowarefoundation#2204)

* fix 1 path point exception after resampling

Signed-off-by: TakaHoribe <horibe.takamasa@gmail.com>

* Apply suggestions from code review

* Apply suggestions from code review

Co-authored-by: Takamasa Horibe <horibe.takamasa@gmail.com>

Co-authored-by: tkimura4 <tomoya.kimura@tier4.jp>

* [hotfix] Fix lane ids (autowarefoundation#2211)

* Fix lane ids

* Prevent acceleration on avoidance (autowarefoundation#2214)

* prevent acceleration on avoidance

Signed-off-by: TakaHoribe <horibe.takamasa@gmail.com>

* fix param name

Signed-off-by: TakaHoribe <horibe.takamasa@gmail.com>

* parametrize avoidance acc

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* change param name

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* fix typo

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* Fix qos in roi cluster fusion (autowarefoundation#2218)

* fix confidence (autowarefoundation#2220)

* too high confidence (autowarefoundation#2229)

* Fix/obstacle stop 0.23.0 (autowarefoundation#2232)

* fix unexpected slow down in sharp curves (autowarefoundation#2181)

* Fix/insert implementation (autowarefoundation#2186)

Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com>

* [hotfix] Remove exception in avoidance module (autowarefoundation#2233)

* Remove exception

* Fix clock

* Remove blank line

* Update traffic light state if ref stop point is ahead of previous one (autowarefoundation#2197)

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* fix interpolation for insert point (autowarefoundation#2228)

* fix interpolation for insert point

* to prev interpolation pkg

* fix index (autowarefoundation#2265)

* turn signal calculation (#2280)

* add turn signal funtion in path shifter

* add ros parameters

Co-authored-by: Fumiya Watanabe <rej55.g@gmail.com>
Co-authored-by: Takamasa Horibe <horibe.takamasa@gmail.com>
Co-authored-by: tkimura4 <tomoya.kimura@tier4.jp>
Co-authored-by: Takagi, Isamu <43976882+isamu-takagi@users.noreply.github.com>
Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com>
Co-authored-by: Yukihiro Saito <yukky.saito@gmail.com>
Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com>
Co-authored-by: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com>
Co-authored-by: Sugatyon <32741405+Sugatyon@users.noreply.github.com>

* [behavior_path_planner] fix sudden path change around ego (autowarefoundation#2305) (autowarefoundation#2318)

* fix return-from-ego shift point generation logic

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* change param for trimSimilarGradShiftPoint

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* add comment for issue

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* update comment

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* replace code with function (logic has not changed)

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* move func to cpp

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* add comment for issue

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* fix typo

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* Update planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp

Co-authored-by: Kazuki Miyahara <kmiya@outlook.com>

* Update planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp

Co-authored-by: Kazuki Miyahara <kmiya@outlook.com>

Co-authored-by: Kazuki Miyahara <kmiya@outlook.com>

Co-authored-by: Takamasa Horibe <horibe.takamasa@gmail.com>
Co-authored-by: Kazuki Miyahara <kmiya@outlook.com>

* Add functions to make stamped scalar messages (autowarefoundation#2317)

* Fix/object yaw in intersection module (autowarefoundation#2294)

* fix object orientation

* fix function name

* add guard (autowarefoundation#2321)

* reduce cost (double to float) (autowarefoundation#2298)

* Add detail collision check (autowarefoundation#2274)

* Add detail collision check

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* Remove unused function

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* Fix arc length

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* Seperate time margin

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* Fix parameter name

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* Update Readme

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* Address review: Add comment for TimeDistanceArray

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* Run pre-commit

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* Fix cpplint

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* Add return for empty polygon

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* update CenterPoint  (autowarefoundation#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 (autowarefoundation#2331)

* add old information deleter

* fix access bug

* change to deque

* update obstacle buffer

* fix some bugs

* add lane change detector

* make a update lanelet function

* fix code style

* parameterize essential values

* Update perception/object_recognition/prediction/map_based_prediction/src/map_based_prediction_ros.cpp

Co-authored-by: Kazuki Miyahara <kmiya@outlook.com>

* fix slash position

* remove unnecessary lines

* fix format

* fix format

* change to new enum

* fix format

* fix typo and add guard

* change funciton name

* add lane change description

Co-authored-by: Kazuki Miyahara <kmiya@outlook.com>

* Add Planning Evaluator  (autowarefoundation#2293)

* Add prototype planning evaluator

Produced data for dist between points, curvature, and relative angle

* Cleanup the code to make adding metrics easier

* Add remaining basic metrics (length, duration, vel, accel, jerk)

* Add motion_evaluator to evaluate the actual ego motion + code cleanup

* Add deviation metrics

* Add naive stability metric

* Handle invalid stat (TODO: fix the output file formatting)

* Add parameter file and cleanup

* Add basic obstacle metric (TTC not yet implemented) and fix output file format

* Add basic time to collision

* Add lateral-distance based stability metric

* Add check (at init time) that metrics' maps are complete

* Publish metrics as ParamaterDeclaration msg (for openscenario)

* Use lookahead and start from ego_pose when calculating stability metrics

* Code cleanup

* Fix lint

* Add tests

* Fix bug with Frechet dist and the last traj point

* Finish implementing tests

* Fix lint

* Code cleanup

* Update README.md

* Remove unused metric

* Change msg type of published metrics to DiagnosticArray

* fix format to fix pre-commit check

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* fix yaml format to fix pre-commit check

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* fix yaml format

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* apply clang-format

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* apply clang-format

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* Update planning/planning_diagnostics/planning_evaluator/include/planning_evaluator/planning_evaluator_node.hpp

* Update planning/planning_diagnostics/planning_evaluator/test/test_planning_evaluator_node.cpp

* Update planning/planning_diagnostics/planning_evaluator/test/test_planning_evaluator_node.cpp

* change lint format to autoware_lint_common

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

Co-authored-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* Add keep braking function at driving state (autowarefoundation#2346)

* Add keep braking function at driving state

Signed-off-by: Makoto Kurihara <mkuri8m@gmail.com>

* Remove debug messages

Signed-off-by: Makoto Kurihara <mkuri8m@gmail.com>

* Fix format

Signed-off-by: Makoto Kurihara <mkuri8m@gmail.com>

* Change diag_updater's pediod from default to 0.1sec (autowarefoundation#2348)

* add cross judgement and common signal function (autowarefoundation#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 (autowarefoundation#2340)

Signed-off-by: Makoto Kurihara <mkuri8m@gmail.com>

* add time offset param to point cloud concatenation (autowarefoundation#2303)

* add offset param

* clang-format

Co-authored-by: Akihito OHSATO <aohsato@gmail.com>

* Feature/add doc for keep braking function at driving state (autowarefoundation#2366)

* Add the description of brake keeping

Signed-off-by: Makoto Kurihara <mkuri8m@gmail.com>

* Add the english document

Signed-off-by: Makoto Kurihara <mkuri8m@gmail.com>

* Improve description

Signed-off-by: Makoto Kurihara <mkuri8m@gmail.com>

* Add english description

Signed-off-by: Makoto Kurihara <mkuri8m@gmail.com>

* Fix include files (autowarefoundation#2339)

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* fix behavior intersection module

* fix behavior no stopping area module

* fix planning_evaluator

* fix motion_velocity_smoother

* rename variable

* Revert "[mpc_follower] remove stop distance condition from stopState decision (autowarefoundation#1916)"

This reverts commit ff4f0b5a844d1f835f1b93bd3b36a76747b0cd02.

* Revert "Add keep braking function at driving state (autowarefoundation#2346)"

This reverts commit f0478187db4c28bf6092c198723dcc5ec11a9c70.

* Revert "Feature/add doc for keep braking function at driving state (autowarefoundation#2366)"

This reverts commit 66de2f3924a479049fce2d5c5c6b579cacbd3e49.

* Fix orientation availability in centerpoint

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* fix test_trajectory.cpp

* add target link libraries

* Use .auto msg in test code for planniing evaluator

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* fix include

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

Co-authored-by: Takayuki Murooka <takayuki5168@gmail.com>
Co-authored-by: autoware-iv-sync-ci[bot] <87871706+autoware-iv-sync-ci[bot]@users.noreply.github.com>
Co-authored-by: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com>
Co-authored-by: taikitanaka <ttatcoder@outlook.jp>
Co-authored-by: Takamasa Horibe <horibe.takamasa@gmail.com>
Co-authored-by: Takeshi Miura <57553950+1222-takeshi@users.noreply.github.com>
Co-authored-by: Takeshi Ishita <ishitah.takeshi@gmail.com>
Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com>
Co-authored-by: Makoto Kurihara <mkuri8m@gmail.com>
Co-authored-by: purewater0901 <43805014+purewater0901@users.noreply.github.com>
Co-authored-by: tkimura4 <tomoya.kimura@tier4.jp>
Co-authored-by: Kazuki Miyahara <kmiya@outlook.com>
Co-authored-by: Yukihiro Saito <yukky.saito@gmail.com>
Co-authored-by: Fumiya Watanabe <rej55.g@gmail.com>
Co-authored-by: Takagi, Isamu <43976882+isamu-takagi@users.noreply.github.com>
Co-authored-by: Sugatyon <32741405+Sugatyon@users.noreply.github.com>
Co-authored-by: s-murakami-esol <81723883+s-murakami-esol@users.noreply.github.com>
Co-authored-by: Yusuke Muramatsu <yukke42@users.noreply.github.com>
Co-authored-by: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com>
Co-authored-by: Shunsuke Miura <37187849+miursh@users.noreply.github.com>
Co-authored-by: Shinnosuke Hirakawa <8327162+0x126@users.noreply.github.com>
Co-authored-by: Akihito OHSATO <aohsato@gmail.com>
Co-authored-by: Kenji Miyake <kenji.miyake@tier4.jp>

* substitute time_step for object-path (autowarefoundation#720)

* fix: fix title of README.md

* Apply suggestions from code review

* Feature/update map based prediction readme (autowarefoundation#762)

* remove unused arg

* remove unsed arg

* update readme

* remove unused arg

Co-authored-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp>
Co-authored-by: Nikolai Morin <nnmmgit@gmail.com>
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 <horibe.takamasa@gmail.com>
Co-authored-by: s-azumi <38061530+s-azumi@users.noreply.github.com>
Co-authored-by: Kazuki Miyahara <kmiya@outlook.com>
Co-authored-by: tkimura4 <tomoya.kimura@tier4.jp>
Co-authored-by: Taichi Higashide <taichi.higashide@tier4.jp>
Co-authored-by: wep21 <border_goldenmarket@yahoo.co.jp>
Co-authored-by: Kosuke Murakami <kosuke.murakami@tier4.jp>
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 <hiroki.ota@tier4.jp>
Co-authored-by: Kenji Miyake <kenji.miyake@tier4.jp>
Co-authored-by: Takeshi Ishita <ishitah.takeshi@gmail.com>
Co-authored-by: Keisuke Shima <19993104+KeisukeShima@users.noreply.github.com>
Co-authored-by: Yukihiro Saito <yukky.saito@gmail.com>
Co-authored-by: Sugatyon <32741405+Sugatyon@users.noreply.github.com>
Co-authored-by: Takayuki Murooka <takayuki5168@gmail.com>
Co-authored-by: autoware-iv-sync-ci[bot] <87871706+autoware-iv-sync-ci[bot]@users.noreply.github.com>
Co-authored-by: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com>
Co-authored-by: taikitanaka <ttatcoder@outlook.jp>
Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com>
Co-authored-by: Makoto Kurihara <mkuri8m@gmail.com>
Co-authored-by: purewater0901 <43805014+purewater0901@users.noreply.github.com>
Co-authored-by: Fumiya Watanabe <rej55.g@gmail.com>
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 <yukke42@users.noreply.github.com>
Co-authored-by: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com>
Co-authored-by: Shunsuke Miura <37187849+miursh@users.noreply.github.com>
Co-authored-by: Shinnosuke Hirakawa <8327162+0x126@users.noreply.github.com>
Co-authored-by: Akihito OHSATO <aohsato@gmail.com>
  • Loading branch information
@1222-takeshi @mitsudome-r
@nnmm @sgermanserrano @nik-tier4 @TakaHoribe @s-azumi @kmiya @tkimura4 @k0suke-murakami @kenji-miyake @wep21 @h-ohta @IshitaTakeshi @KeisukeShima @yukkysaito @kyoichi-sugahara @takayuki5168 @tier4-autoware-private-bot @taikitanaka3 @satoshi-ota @mkuri @purewater0901 @rej55 @isamu-takagi @s-murakami-esol @yukke42 @maxime-clem @miursh @0x126 @aohsato
36 people authored Dec 6, 2021
1 parent 4b91ac2 commit f0e4cf4
Show file tree
Hide file tree
Showing 9 changed files with 1,727 additions and 0 deletions.
36 changes: 36 additions & 0 deletions perception/map_based_prediction/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
@@ -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
)
73 changes: 73 additions & 0 deletions perception/map_based_prediction/README.md
View file
Original file line number Diff line number Diff line change
@@ -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.
254 changes: 254 additions & 0 deletions perception/map_based_prediction/include/cubic_spline.hpp
View file
Original file line number Diff line number Diff line change
@@ -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 <eigen3/Eigen/Eigen>

#include <algorithm>
#include <array>
#include <iostream>
#include <stdexcept>
#include <string>
#include <vector>

static std::vector<double> vec_diff(const std::vector<double> & input)
{
std::vector<double> output;
for (unsigned int i = 1; i < input.size(); i++) {
output.push_back(input[i] - input[i - 1]);
}
return output;
}

static std::vector<double> cum_sum(const std::vector<double> & input)
{
std::vector<double> 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<double> x;
std::vector<double> y;
int nx;
std::vector<double> h;
std::vector<double> a;
std::vector<double> b;
std::vector<double> c;
// Eigen::VectorXf c;
std::vector<double> 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<double> & x_, const std::vector<double> & 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<double> s;
double max_s_value_;

Spline2D(const std::vector<double> & x, const std::vector<double> & 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<double, 2> 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<double> calc_s(const std::vector<double> & x, const std::vector<double> & y)
{
std::vector<double> ds;
std::vector<double> out_s{0};
std::vector<double> dx = vec_diff(x);
std::vector<double> 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<double> cum_ds = cum_sum(ds);
out_s.insert(out_s.end(), cum_ds.begin(), cum_ds.end());
return out_s;
}
};

#endif // CUBIC_SPLINE_HPP_
Loading

0 comments on commit f0e4cf4

Please sign in to comment.