Skip to content
This repository has been archived by the owner on Jul 3, 2023. It is now read-only.

Commit

Permalink
feat: add lidar centerpoint package (#108)
Browse files Browse the repository at this point in the history
* Feature/porting centerpoint (autowarefoundation#1658)

* Feature CenterPoint TensorRT (autowarefoundation#1300)

* init centerpoint trt

* update

* [update] libtorch 1.8.1 -> 1.6.0

* fix

* update gitignore

* add license

* more efficient voxel_generator

* remove unnecessary rosparams

* libtorch source-code build

* update gitignore

* following the guidelines

* fix rosparam

* change input params

* add reference repository

* update the links to download trained model

* fix license

* package ver.

* update README

* publish msgs when subscribed

* clang-format

* check if the pre-trained file changes

* remove the download of libtorch package

* update

* update README

* remove megToTensor

* disable libtorch CPU parallelization

* remove unnecessary pointcloud transformation

* format

* fix ERROR `/usr/bin/ld: -lCUDA_cublas_device_LIBRARY-NOTFOUND` using cmake 3.10.2

* remove voxel_generator_cuda

* revert CUDA_cublas

* add nan checker for debug

* remove nan checker

* fix to filter invalid features

* fix densification pointcloud order

* add invalid feature filter

* revert change

* change cmake version (autowarefoundation#1547)

* Fix cpu build error of CenterPoint (autowarefoundation#1548)

* skip build without CUDA

* fix find Torch

* fix cmake of lidar_centerpoint (autowarefoundation#1557)

* Porting centerpoint to ros2

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

* Rename headers(.h -> .hpp)

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

* pre-commit fixes

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

* Use sensor data qos

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

* Address review: Remove unused compile option

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

* Address review: Fix topic name

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

* Address review: Add new line

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

* Address review: Add subscriber check

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

* Address review: Use autoware utils

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

Co-authored-by: Yusuke Muramatsu <yukke42@users.noreply.github.com>
Co-authored-by: Satoshi Tanaka <satoshi.tanaka@tier4.jp>
Co-authored-by: tkimura4 <tomoya.kimura@tier4.jp>

* Fix centerpoint object qos (autowarefoundation#1747)

Signed-off-by: Takayuki AKAMINE <takayuki.akamine@tier4.jp>

Co-authored-by: Takayuki AKAMINE <takayuki.akamine@tier4.jp>

* Fix package.xml (autowarefoundation#2056)

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

* 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>

* fix for createQuaternionFromRPY/Yaw (autowarefoundation#2154)

* 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>

* port lidar_centerpoint (autowarefoundation#527)

* delete COLCON_IGNORE

* change autoware_perception_msgs to autoware_auto_perception_msgs

* use existence_probability

* use orientation_availability

* about about existence_probability

* 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>

* Update README.md

Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com>
Co-authored-by: Yusuke Muramatsu <yukke42@users.noreply.github.com>
Co-authored-by: Satoshi Tanaka <satoshi.tanaka@tier4.jp>
Co-authored-by: tkimura4 <tomoya.kimura@tier4.jp>
Co-authored-by: Takayuki AKAMINE <takayuki.akamine@tier4.jp>
Co-authored-by: Kenji Miyake <kenji.miyake@tier4.jp>
Co-authored-by: Yukihiro Saito <yukky.saito@gmail.com>
Co-authored-by: Takayuki Murooka <takayuki5168@gmail.com>
Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com>
Co-authored-by: autoware-iv-sync-ci[bot] <87871706+autoware-iv-sync-ci[bot]@users.noreply.github.com>
Co-authored-by: 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: Kazuki Miyahara <kmiya@outlook.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: 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
1 parent 7da1565 commit 5ef7648
Show file tree
Hide file tree
Showing 22 changed files with 1,935 additions and 0 deletions.
1 change: 1 addition & 0 deletions perception/lidar_centerpoint/.gitignore
Original file line number Diff line number Diff line change
@@ -0,0 +1 @@
/data/
174 changes: 174 additions & 0 deletions perception/lidar_centerpoint/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -0,0 +1,174 @@
cmake_minimum_required(VERSION 3.5)
project(lidar_centerpoint)

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()

set(CUDA_VERBOSE OFF)

# set flags for CUDA availability
option(CUDA_AVAIL "CUDA available" OFF)
find_package(CUDA)
if(CUDA_FOUND)
find_library(CUBLAS_LIBRARIES cublas HINTS
${CUDA_TOOLKIT_ROOT_DIR}/lib64
${CUDA_TOOLKIT_ROOT_DIR}/lib
)
if(CUDA_VERBOSE)
message("CUDA is available!")
message("CUDA Libs: ${CUDA_LIBRARIES}")
message("CUDA Headers: ${CUDA_INCLUDE_DIRS}")
endif()
# Note: cublas_device was depreciated in CUDA version 9.2
# https://forums.developer.nvidia.com/t/where-can-i-find-libcublas-device-so-or-libcublas-device-a/67251/4
# In LibTorch, CUDA_cublas_device_LIBRARY is used.
unset(CUDA_cublas_device_LIBRARY CACHE)
set(CUDA_AVAIL ON)
else()
message("CUDA NOT FOUND")
set(CUDA_AVAIL OFF)
endif()

# set flags for TensorRT availability
option(TRT_AVAIL "TensorRT available" OFF)
# try to find the tensorRT modules
find_library(NVINFER nvinfer)
find_library(NVONNXPARSER nvonnxparser)
if(NVINFER AND NVONNXPARSER)
if(CUDA_VERBOSE)
message("TensorRT is available!")
message("NVINFER: ${NVINFER}")
message("NVONNXPARSER: ${NVONNXPARSER}")
endif()
set(TRT_AVAIL ON)
else()
message("TensorRT is NOT Available")
set(TRT_AVAIL OFF)
endif()

option(TORCH_AVAIL "Torch available" OFF)
if(CUDA_FOUND)
set(Torch_DIR /usr/local/libtorch/share/cmake/Torch)
find_package(Torch)
if(TORCH_FOUND)
if(CUDA_VERBOSE)
message(STATUS "TORCH_INCLUDE_DIRS: ${TORCH_INCLUDE_DIRS}")
message(STATUS "TORCH_LIBRARIES: ${TORCH_LIBRARIES}")
endif()
set(TORCH_AVAIL ON)
else()
message("Torch NOT FOUND")
set(TORCH_AVAIL OFF)
endif()
endif()

if(TRT_AVAIL AND CUDA_AVAIL AND TORCH_AVAIL)
# Download trained models
find_program(GDOWN_AVAIL "gdown")
if(NOT GDOWN_AVAIL)
message("gdown: command not found. External files could not be downloaded.")
endif()

set(DATA_PATH ${CMAKE_CURRENT_SOURCE_DIR}/data)
execute_process(COMMAND mkdir -p ${DATA_PATH})

function(download FILE_NAME GFILE_ID FILE_HASH)
# https://drive.google.com/file/d/GFILE_ID/view
message(STATUS "Checking and downloading ${FILE_NAME}")
set(FILE_PATH ${DATA_PATH}/${FILE_NAME})
if(EXISTS ${FILE_PATH})
file(MD5 ${FILE_PATH} EXISTING_FILE_HASH)
if(NOT ${FILE_HASH} EQUAL ${EXISTING_FILE_HASH})
message(STATUS "... file hash changes. Downloading now ...")
execute_process(COMMAND gdown --quiet https://drive.google.com//uc?id=${GFILE_ID} -O ${FILE_PATH})
endif()
else()
message(STATUS "... file doesn't exists. Downloading now ...")
execute_process(COMMAND gdown --quiet https://drive.google.com//uc?id=${GFILE_ID} -O ${FILE_PATH})
endif()
endfunction()

# default model
download(pts_voxel_encoder_default.onnx 1_8OCQmrPm_R4ZVh70QsS9HZo6uGrlbgz 01b860612e497591c4375d90dff61ef7)
download(pts_voxel_encoder_default.pt 1RZ7cuDnI-RBrDiWe-2vEs16mR_z0e9Uo 33136caa97e3bcef2cf3e04bbc93d1e4)
download(pts_backbone_neck_head_default.onnx 1UxDyt8T-TMJS7Ujx-1vbbqGRfDbMUZg2 e23a8ad4ea440f923e44dbe072b070da)
download(pts_backbone_neck_head_default.pt 1toAhmOriX8bwVI-ohuas9_2EBZnltoXh eb0df29b30acf9c1082ac4490af0bbc5)

# aip_x2 model
download(pts_voxel_encoder_aip_x2.onnx 1x-NAHQ3W0lbLmjJlrL6Nhvdq8yz6Ux0n 65eeb95c5e48ebfe6894146cdb48c160)
download(pts_voxel_encoder_aip_x2.pt 1jzKopAhXWjnEgo_v8rtYy0hQIayUE-oL 4db81ce8edc6571aa0afb1ae43ee72e9)
download(pts_backbone_neck_head_aip_x2.onnx 1l2fdIQcBWr3-6stVoNkudnL4OZaPqmNT a33c8910fd9c9c910b10904d3cd96717)
download(pts_backbone_neck_head_aip_x2.pt 18iOAlRsjvcWoUG9KiL1PlD7OY5mi9BSw 274fdf1580dd899e36c050c1366f1883)

find_package(ament_cmake_auto REQUIRED)
ament_auto_find_build_dependencies()

include_directories(
lib/include
${CUDA_INCLUDE_DIRS}
${TORCH_INCLUDE_DIRS}
)

### centerpoint ###
ament_auto_add_library(centerpoint SHARED
lib/src/pointcloud_densification.cpp
lib/src/voxel_generator.cpp
lib/src/centerpoint_trt.cpp
lib/src/tensorrt_wrapper.cpp
lib/src/network_trt.cpp
)

target_link_libraries(centerpoint
${NVINFER}
${NVONNXPARSER}
${NVINFER_PLUGIN}
${CUDA_LIBRARIES}
${CUBLAS_LIBRARIES}
${CUDA_curand_LIBRARY}
${TORCH_LIBRARIES}
)

## node ##
ament_auto_add_library(lidar_centerpoint_component SHARED
src/node.cpp
)

target_link_libraries(lidar_centerpoint_component
centerpoint
)

rclcpp_components_register_node(lidar_centerpoint_component
PLUGIN "centerpoint::LidarCenterPointNode"
EXECUTABLE lidar_centerpoint_node
)

if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
ament_lint_auto_find_test_dependencies()
endif()

ament_auto_package(
INSTALL_TO_SHARE
launch
data
config
)
else()
find_package(ament_cmake_auto REQUIRED)
ament_auto_find_build_dependencies()

if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
ament_lint_auto_find_test_dependencies()
endif()
ament_auto_package(
INSTALL_TO_SHARE
launch
)
endif()
61 changes: 61 additions & 0 deletions perception/lidar_centerpoint/README.md
Original file line number Diff line number Diff line change
@@ -0,0 +1,61 @@
# CenterPoint TensorRT

This is a 3D object detection implementation of CenterPoint supporting TensorRT inference.

The object.existence_probability is stored the value of classification confidence of DNN, not probability.

## Parameters

### Input Topics

| Name | Type | Description |
| ---------------- | ----------- | ------------------------------------ |
| input/pointcloud | PointCloud2 | Point Clouds (x, y, z and intensity) |

### Output Topics

| Name | Type | Description |
| ------------------------------ | ----------------------------- | ---------------------- |
| output/objects | DynamicObjectWithFeatureArray | 3D Bounding Box |
| debug/pointcloud_densification | PointCloud2 | multi-frame pointcloud |

### ROS Parameters

| Name | Type | Description | Default |
| ------------------------- | ------ | ----------------------------------------------------------- | ------- |
| score_threshold | float | detected objects with score less than threshold are ignored | `0.4` |
| densification_base_frame | string | the base frame id to fuse multi-frame pointcloud | `map` |
| densification_past_frames | int | the number of past frames to fuse with the current frame | `1` |
| use_encoder_trt | bool | use TensorRT VoxelFeatureEncoder | `false` |
| use_head_trt | bool | use TensorRT DetectionHead | `true` |
| trt_precision | string | TensorRT inference precision: `fp32` or `fp16` | `fp16` |
| encoder_onnx_path | string | path to VoxelFeatureEncoder ONNX file | |
| encoder_engine_path | string | path to VoxelFeatureEncoder TensorRT Engine file | |
| encoder_pt_path | string | path to VoxelFeatureEncoder TorchScript file | |
| head_onnx_path | string | path to DetectionHead ONNX file | |
| head_engine_path | string | path to DetectionHead TensorRT Engine file | |
| head_pt_path | string | path to DetectionHead TorchScript file | |

## For Developers

If you have an error like `'GOMP_4.5' not found`, replace the OpenMP library in libtorch.

```bash
sudo apt install libgomp1 -y
rm /path/to/libtorch/lib/libgomp-75eea7e8.so.1
ln -s /usr/lib/x86_64-linux-gnu/libgomp.so.1 /path/to/libtorch/lib/libgomp-75eea7e8.so.1
```

## Reference

Yin, Tianwei, Xingyi Zhou, and Philipp Krähenbühl. "Center-based 3d object detection and tracking." arXiv preprint arXiv:2006.11275 (2020).

## Reference Repositories

- <https://github.com/tianweiy/CenterPoint>
- <https://github.com/open-mmlab/OpenPCDet>
- <https://github.com/poodarchu/Det3D>
- <https://github.com/xingyizhou/CenterNet>
- <https://github.com/lzccccc/SMOKE>
- <https://github.com/yukkysaito/autoware_perception>
- <https://github.com/pytorch/pytorch>
3 changes: 3 additions & 0 deletions perception/lidar_centerpoint/config/aip_x2.param.yaml
Original file line number Diff line number Diff line change
@@ -0,0 +1,3 @@
/**:
ros__parameters:
class_names: ["CAR", "TRUCK", "BUS", "BICYCLE", "PEDESTRIAN"]
4 changes: 4 additions & 0 deletions perception/lidar_centerpoint/config/default.param.yaml
Original file line number Diff line number Diff line change
@@ -0,0 +1,4 @@
/**:
ros__parameters:
class_names: ["CAR", "PEDESTRIAN", "BICYCLE"]
rename_car_to_truck_and_bus: true
75 changes: 75 additions & 0 deletions perception/lidar_centerpoint/include/lidar_centerpoint/node.hpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,75 @@
// Copyright 2021 Tier IV, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef LIDAR_CENTERPOINT__NODE_HPP_
#define LIDAR_CENTERPOINT__NODE_HPP_

#include <centerpoint_trt.hpp>
#include <config.hpp>
#include <pointcloud_densification.hpp>
#include <rclcpp/rclcpp.hpp>

#include <autoware_auto_perception_msgs/msg/detected_object_kinematics.hpp>
#include <autoware_auto_perception_msgs/msg/detected_objects.hpp>
#include <autoware_auto_perception_msgs/msg/object_classification.hpp>
#include <autoware_auto_perception_msgs/msg/shape.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>

#include <memory>
#include <string>
#include <vector>

namespace centerpoint
{
using Label = autoware_auto_perception_msgs::msg::ObjectClassification;

class LidarCenterPointNode : public rclcpp::Node
{
public:
explicit LidarCenterPointNode(const rclcpp::NodeOptions & node_options);

private:
void pointCloudCallback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr input_pointcloud_msg);

static uint8_t getSemanticType(const std::string & class_name);
static bool isCarLikeVehicleLabel(const uint8_t label);

rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr pointcloud_sub_;
rclcpp::Publisher<autoware_auto_perception_msgs::msg::DetectedObjects>::SharedPtr objects_pub_;
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pointcloud_pub_;

float score_threshold_{0.0};
std::string densification_base_frame_;
int densification_past_frames_{0};
bool use_encoder_trt_{false};
bool use_head_trt_{false};
std::string trt_precision_;

std::string encoder_onnx_path_;
std::string encoder_engine_path_;
std::string encoder_pt_path_;
std::string head_onnx_path_;
std::string head_engine_path_;
std::string head_pt_path_;

std::vector<std::string> class_names_;
bool rename_car_to_truck_and_bus_{false};

std::unique_ptr<PointCloudDensification> densification_ptr_{nullptr};
std::unique_ptr<CenterPointTRT> detector_ptr_{nullptr};
};

} // namespace centerpoint

#endif // LIDAR_CENTERPOINT__NODE_HPP_
23 changes: 23 additions & 0 deletions perception/lidar_centerpoint/launch/lidar_centerpoint.launch.xml
Original file line number Diff line number Diff line change
@@ -0,0 +1,23 @@
<?xml version="1.0"?>
<launch>
<arg name="input/pointcloud" default="/sensing/lidar/top/rectified/pointcloud"/>
<arg name="output/objects" default="objects"/>
<arg name="model_name" default="default" description="options: `default` or `aip_x2`"/>
<arg name="model_path" default="$(find-pkg-share lidar_centerpoint)/data"/>
<arg name="model_param_path" default="$(find-pkg-share lidar_centerpoint)/config/$(var model_name).param.yaml"/>

<node pkg="lidar_centerpoint" exec="lidar_centerpoint_node"
name="lidar_centerpoint" output="screen">
<remap from="~/input/pointcloud" to="$(var input/pointcloud)"/>
<remap from="~/output/objects" to="$(var output/objects)"/>
<param name="score_threshold" value="0.45"/>
<param name="densification_base_frame" value="map"/>
<param name="densification_past_frames" value="1"/>
<param name="use_head_trt" value="true"/>
<param name="trt_precision" value="fp16"/>
<param name="encoder_pt_path" value="$(var model_path)/pts_voxel_encoder_$(var model_name).pt"/>
<param name="head_onnx_path" value="$(var model_path)/pts_backbone_neck_head_$(var model_name).onnx"/>
<param name="head_engine_path" value="$(var model_path)/pts_backbone_neck_head_$(var model_name).engine"/>
<param from="$(var model_param_path)"/>
</node>
</launch>
Loading

0 comments on commit 5ef7648

Please sign in to comment.