Skip to content

Commit

Permalink
feat: add obstacle_avoidance_planner packages (autowarefoundation#89)
Browse files Browse the repository at this point in the history
* release v0.4.0

* add dynamic_reconfigure to obstacle avoidance planner (autowarefoundation#673)

* fix apply dynamic param (autowarefoundation#704)

* add warning logging to foa.data.is_is_avoidance_possible (autowarefoundation#725)

* Feature/improve obstacle avoidance (autowarefoundation#731)

* add license terms (autowarefoundation#941)

* remove ROS1 packages temporarily

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

* Revert "remove ROS1 packages temporarily"

This reverts commit 10813fa0e7b67c9ad6c7d4df6aeee0931c866b41.

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 of obstacle avoidance planner to ROS2 (autowarefoundation#83)

* Port to ROS2

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

* Removed ros/console include

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

* Added THROTTLE

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

* Changed numbers to floats

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

* Removed <double> from params

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

* Added new msg

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

* Use new msg

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

* Replaced dynamic reconfigure

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

* Added EnableAvoidance.msg

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

* Fixed parameter mismatch

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

* Fix launch files (autowarefoundation#122)

* [surround_obstacle_checker] add parameter and arguments to launch file

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

* [obstacle_stop_planner] modify launch file to remap trajectory from argument

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

* [obstacle_avoidance_planner] modify launch file to remap topics from arguments

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

* [motion_velocity_optimizer] modify launch file to enable remapping from argument

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

* [lane_change_planner] [obstacle_avoidance_planner] fix clock type (autowarefoundation#183)

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

* Add linters to obstacle_avoidance_planner (autowarefoundation#158)

* Added linters to obstacle_avoidance_planner

* Fix header issues

* Fix linting issues

* Fix more linting issues

* Fix even more linting issues

* Final linting fixes

* Fix dependencies

* Use ament_cmake_cppcheck and ament_cmake_cpplint

* [obstacle_avoidance_planner] add missing return values (autowarefoundation#214)

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

* Obstacle avoidance planner 0.8 (autowarefoundation#308)

* fix rosparam typo

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

* empty check before access

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

* modify way to fix the points

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

* apply format

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

* add baselink2front dist for virutual_wall

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

* add avoiding object type

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

* add exception handling for avoiding object

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

* apply format

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

* add vehicle_info dependency

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

* modify to pass lint

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

* fix lint

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

* add vehicle_info_param in launch

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

* modify nested pram declaration

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

* apply ament-uncrastify

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

* fix boost_none handling (autowarefoundation#349)

* fix boost_none handling

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

* add another one

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

* add another one

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

* add another one

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

* Rename ROS-related .yaml to .param.yaml (autowarefoundation#352)

* Rename ROS-related .yaml to .param.yaml

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

* Remove prefix 'default_' of yaml files

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

* Rename vehicle_info.yaml to vehicle_info.param.yaml

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

* Rename diagnostic_aggregator's param files

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

* Fix overlooked parameters

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

* rename vehicle_info_param to vehicle_param_file (autowarefoundation#353)

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

* Ros2 fix topic name part1 (autowarefoundation#408)

* Fix topic name of lane_departure_checker debug

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* Fix topic name of mpc_follower debug

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* Fix topic name of velocity_controller debug

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* Fix topic name of motion_velocity_optimizer debug

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* Fix topic name of lane_change_planner debug

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* Fix topic name of behavior_velocity_planner debug

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* Fix topic name of obstacle_avoidance_planner debug

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* Fix topic name of behavior_velocity_planner

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* Fix topic name of motion_velocity_optimizer

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* Fix topic name of lane_departure_checker

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* Fix topic name of mpc_follower

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* Fix topic name of behavior_velocity_planner

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* Fix topic name of velocity_controller

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* Fix topic name of lane_change_planner

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* Fix topic name of obstacle_avoidance_planner

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* Fix topic name of obstacle_stop_planner

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* Fix topic name of costmap_generator

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* Fix topic name of freespace_planner

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* Fix topic name of surround_obstacle_checker

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* Fix topic name of costmap_generator

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* Fix topic name of emergency_handler

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* Fix lint errors

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* Fix typo

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* Fix typo in planning module (autowarefoundation#432)

* Fix typo in planning module

* Fix remaining errors

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

* Apply cpplint and uncrustify.

Co-authored-by: Kenji Miyake <kenji.miyake@tier4.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>

* update log (autowarefoundation#456)

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

* fix bug (autowarefoundation#457)

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

* Sync public repo (autowarefoundation#1228)

* [simple_planning_simulator] add readme (autowarefoundation#424)

* add readme of simple_planning_simulator

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

* Update simulator/simple_planning_simulator/README.md

* set transit_margin_time to intersect. planner (autowarefoundation#460)

* Fix pose2twist (autowarefoundation#462)

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* Ros2 vehicle info param server (autowarefoundation#447)

* add vehicle_info_param_server

* update vehicle info

* apply format

* fix bug

* skip unnecessary search

* delete vehicle param file

* fix bug

* Ros2 fix topic name part2 (autowarefoundation#425)

* Fix topic name of traffic_light_classifier

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* Fix topic name of traffic_light_visualization

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* Fix topic name of traffic_light_ssd_fine_detector

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* Fix topic name of traffic_light_map_based_detector

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* Fix lint traffic_light_recognition

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* Fix lint traffic_light_ssd_fine_detector

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* Fix lint traffic_light_classifier

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* Fix lint traffic_light_classifier

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* Fix lint traffic_light_ssd_fine_detector

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* Fix issues in hdd_reader (autowarefoundation#466)

* Fix some issues detected by Coverity Scan and Clang-Tidy

* Update launch command

* Add more `close(new_sock)`

* Simplify the definitions of struct

* fix: re-construct laneletMapLayer for reindex RTree (autowarefoundation#463)

* Rviz overlay render fix (autowarefoundation#461)

* Moved painiting in SteeringAngle plugin to update()

Signed-off-by: Adam Dabrowski <adam.dabrowski@robotec.ai>

* super class now back to MFD

Signed-off-by: Adam Dabrowski <adam.dabrowski@robotec.ai>

* uncrustified

Signed-off-by: Adam Dabrowski <adam.dabrowski@robotec.ai>

* acquire data in mutex

Signed-off-by: Adam Dabrowski <adam.dabrowski@robotec.ai>

* back to RTD as superclass

Signed-off-by: Adam Dabrowski <adam.dabrowski@robotec.ai>

* Rviz overlay render in update (autowarefoundation#465)

* Moved painiting in SteeringAngle plugin to update()

Signed-off-by: Adam Dabrowski <adam.dabrowski@robotec.ai>

* super class now back to MFD

Signed-off-by: Adam Dabrowski <adam.dabrowski@robotec.ai>

* uncrustified

Signed-off-by: Adam Dabrowski <adam.dabrowski@robotec.ai>

* acquire data in mutex

Signed-off-by: Adam Dabrowski <adam.dabrowski@robotec.ai>

* removed unnecessary includes and some dead code

Signed-off-by: Adam Dabrowski <adam.dabrowski@robotec.ai>

* Adepted remaining vehicle plugin classes to render-in-update concept. Returned to MFD superclass

Signed-off-by: Adam Dabrowski <adam.dabrowski@robotec.ai>

* restored RTD superclass

Signed-off-by: Adam Dabrowski <adam.dabrowski@robotec.ai>

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: Kazuki Miyahara <kmiya@outlook.com>
Co-authored-by: Makoto Tokunaga <vios-fish@users.noreply.github.com>
Co-authored-by: Adam Dąbrowski <adam.dabrowski@robotec.ai>

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

* Make planning modules components (autowarefoundation#1263)

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

* Remove use_sim_time for set_parameter (autowarefoundation#1260)

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

* Porting small fix (autowarefoundation#1288)

* Delete unused code (autowarefoundation#1183)

* Fix control topic name of closest_velocity_checker.py (autowarefoundation#1174)

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

* Add comments for livox tag (autowarefoundation#1188)

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

* Clear return value (autowarefoundation#1193)

* Change tracker model of unknown object (autowarefoundation#1204)

* treat polygon points as relative (autowarefoundation#1205)

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

* hotfix: reference velocity in consideration of vehicle gear (autowarefoundation#1213)

* fix reference velocity for vehicle gear

* add initialization

* revert

* add comment

* change max area param (autowarefoundation#1218)

* Fix an identical code for different branches (autowarefoundation#1230)

* Update livox_tag_filter.launch.xml

* Fixup

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

Co-authored-by: shin <8327162+0x126@users.noreply.github.com>
Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com>
Co-authored-by: Fumiya Watanabe <rej55.g@gmail.com>
Co-authored-by: Yukihiro Saito <yukky.saito@gmail.com>
Co-authored-by: Kosuke Murakami <kosuke.murakami@tier4.jp>
Co-authored-by: Hiroki OTA <hiroki.ota@tier4.jp>
Co-authored-by: tkimura4 <tomoya.kimura@tier4.jp>
Co-authored-by: Kazuki Miyahara <kmiya@outlook.com>
Co-authored-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Refactor vehicle info util (autowarefoundation#1305)

* Update license

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

* Refactor vehicle_info_util

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

* Rename and split files

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

* Fix interfaces

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

* Fix bug and add error handling

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

* Add "// namespace"

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

* Add missing include

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

* Fix lint errors (autowarefoundation#1378)

* Fix lint errors

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

* Fix variable names

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

* Disable debug output of obstacle_avoidance_planner by default (autowarefoundation#1391)

* Disable debug output of obstacle_avoidance_planner by default

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

* Fix typo

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

* suppress warnings for obstacle avoidance planner (autowarefoundation#1764)

* add Werror

* fix Duration

* fix debug.cpp

* fix node.cpp

* fix util.cpp

* fix for loop for mpt_optimizer.cpp

* fix unused for mpt_optimizer.cpp

* fix process_cv.cpp

* fix sign compare for eb_path_optimizer.cpp

* fix unused for eb_path_optimizer.cpp

* fix for cpplint

* fix for cpplint

* re fix debug.cpp

* fix typo

* fix typo

* re fix process_cv.cpp

* fix typo

* re fix util.cpp

* re re fix util.cpp

* 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 compiler warnings (autowarefoundation#1837)

* Fix -Wunused-private-field

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

* Fix -Wunused-variable

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

* Fix -Wformat-security

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

* Fix -Winvalid-constexpr

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

* Fix -Wdelete-non-abstract-non-virtual-dtor

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

* Fix -Wdelete-abstract-non-virtual-dtor

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

* Fix -Winconsistent-missing-override

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

* Fix -Wrange-loop-construct

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

* Fix "invalid application of 'sizeof' to an incomplete type"

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

* Ignore -Wgnu-anonymous-struct and -Wnested-anon-types

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

* Fix lint

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

* Ignore -Wno-deprecated-declarations in CUDA-related packages

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

* Fix mistake

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

* Fix -Wunused-parameter

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

* Fix spellcheck fail for some packages (autowarefoundation#1842)

* fix transed -> transformed

* fix Urefex to Uref_ex

* fix spell

* Yrefex -> Yref_ex

* add space

* fix for spellcheck

* fix for lint

* fix feedbacked to feedback

* delete Yref_ex

* [obstacle avoidance planner] move doc from develop (autowarefoundation#1898)

* move doc from develop

* add figures

* update docs

* fix

* update doc

* use /approx

* use \sin \cos \tan

* use \arctan

* use \cos

* 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

* remove spline_interpolation, and create interpolation package that ha… (autowarefoundation#2114)

* remove spline_interpolation and add interpolation package

* fix bugs and add simple test

* modify application for interpolation package

* add some tests and fix ci test

* add some tests

* add -Werror

* move functions to anonymous

* fix reviews

* fix typo

* fix review and lint

* add tests

* resolve TODOs

* add README.md

* Update common/math/interpolation/test/src/test_interpolation_utils.cpp

Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com>

* Update common/math/interpolation/test/src/test_interpolation_utils.cpp

Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com>

* Update common/math/interpolation/test/src/test_interpolation_utils.cpp

Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com>

* fix README.md

* dealt with reviews

* update README.md

* update README.md

* Update common/math/interpolation/README.md

* fix some bugs

* fix typo

* update README.md

* Update common/math/interpolation/README.md

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

* fix test

* Update common/math/interpolation/README.md

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

* fix cdots

* fix README.md

* Update common/math/interpolation/README.md

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

Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com>
Co-authored-by: Kazuki Miyahara <kmiya@outlook.com>

* update doc (autowarefoundation#2164)

* update doc

* Update planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/obstacle_avoidance_planner-design.ja.md

* Update planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/obstacle_avoidance_planner-design.ja.md

* Update planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/obstacle_avoidance_planner-design.ja.md

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

* Update planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/obstacle_avoidance_planner-design.ja.md

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

* fix \min, \max

* Update planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/obstacle_avoidance_planner-design.ja.md

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

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

* update readme for how to debug (autowarefoundation#2253)

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

* add explanation of drivable area in obstacle avoidance planner (autowarefoundation#2291)

* add explanation of drivable area in obstacle avoidance planner

* fix spell

* add explanations

* Sync develop/fix interpolation and speed up calculation (autowarefoundation#2290)

* debug slope calculation in behavior (autowarefoundation#1566) (autowarefoundation#1727)

* debug slope calculation in behavior (autowarefoundation#1566)

* update

* update

* revert change of autoware_utils

* define getPose in behavior_path_planner

* update

* update

* update

* update

* interpolate z in obstacle_avoidance_planner

* update velocity controller

* fix detection area and scene

* Update planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/src/utilities.cpp

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

* update comment in velocity controller

* remove debug print

* update

Co-authored-by: tkimura4 <tomoya.kimura@tier4.jp>
Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* Fix typo

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

* Fix variable names

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

* Fix uncrustify

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

* Fix mistake

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

Co-authored-by: Takayuki Murooka <takayuki5168@gmail.com>
Co-authored-by: tkimura4 <tomoya.kimura@tier4.jp>
Co-authored-by: Kenji Miyake <kenji.miyake@tier4.jp>

* add underscore to marker namespace (autowarefoundation#1785)

Co-authored-by: satoshi-ota <satoshi.ota@gmail.com>

* [obstacle_avoidance_planner] remove useless get nearest idx (autowarefoundation#1851)

* modify velocity insertion in obstacle_avoidance_planner

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

* [obstacle_avoidance_planner] reduce calculation cost in relative/absolute transformation (autowarefoundation#1844)

* implement relative/absolute transformation without yaw but directly sin/cos of yaw

* rename: relative_p to absolute_p

* assigin point.z to rel/abs_z

* fix cpp-lint error

Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com>
Co-authored-by: Takayuki Murooka <takayuki5168@gmail.com>
Co-authored-by: tkimura4 <tomoya.kimura@tier4.jp>
Co-authored-by: Kenji Miyake <kenji.miyake@tier4.jp>
Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com>
Co-authored-by: satoshi-ota <satoshi.ota@gmail.com>
Co-authored-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* Feature/obstacle avoidance planner some improvements (autowarefoundation#1964)

* add some improvements

* fix typo

* Revert "Feature/obstacle avoidance planner some improvements (autowarefoundation#1964)" (autowarefoundation#2000)

This reverts commit 07795e57bf03cb2fbb95a8f82253d8757b4686e9.

* 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 obstacle avoidance planner (autowarefoundation#484)

* use_auto_msgs

* use predicted object

* use autoware_auto_perception_msgs

* complile succeeded

* use odom

* assign header to twist staped as well

* rename to readme.md

Co-authored-by: Takayuki Murooka <takayuki.murooka@tier4.jp>

* [apply_predicted_obj_type] adapt to autoware auto msgs (autowarefoundation#564)

* fix obj shape

* fix obj shape

* fix goal pose

* update README.md in obstacle avoidance planner (autowarefoundation#555)

* update README.md

* fix typo

* update README.md

* fix typo

* minor change

Co-authored-by: Takayuki Murooka <takayuki.murooka@tier4.jp>

* update iv_msgs -> auto_msgs in planning readme (autowarefoundation#576)

* update iv_msgs -> auto_msgs in planning readme

* minor change

* some fix

* some fix

Co-authored-by: Takayuki Murooka <takayuki.murooka@tier4.jp>

* Back port .auto control packages (autowarefoundation#571)

* Implement Lateral and Longitudinal Control Muxer

* [autowarefoundation#570] Porting wf_simulator

* [autowarefoundation#1189] Deactivate flaky test in 'trajectory_follower_nodes'

* [autowarefoundation#1189] Fix flacky test in 'trajectory_follower_nodes/latlon_muxer'

* [autowarefoundation#1057] Add osqp_interface package

* [autowarefoundation#1057] Add library code for MPC-based lateral control

* [autowarefoundation#1271] Use std::abs instead of abs

* [autowarefoundation#1057] Implement Lateral Controller for Cargo ODD

* [autowarefoundation#1246] Resolve "Test case names currently use snake_case but should be CamelCase"

* [autowarefoundation#1325] Deactivate flaky smoke test in 'trajectory_follower_nodes'

* [autowarefoundation#1058] Add library code of longitudinal controller

* Fix build error for trajectory follower

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

* Fix build error for trajectory follower nodes

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

* [autowarefoundation#1272] Add AckermannControlCommand support to simple_planning_simulator

* [autowarefoundation#1058] Add Longitudinal Controller node

* [autowarefoundation#1058] Rename velocity_controller -> longitudinal_controller

* [autowarefoundation#1058] Update CMakeLists.txt for the longitudinal_controller_node

* [autowarefoundation#1058] Add smoke test python launch file

* [autowarefoundation#1058] Use LowPassFilter1d from trajectory_follower

* [autowarefoundation#1058] Use autoware_auto_msgs

* [autowarefoundation#1058] Changes for .auto (debug msg tmp fix, common func, tf listener)

* [autowarefoundation#1058] Remove unused parameters

* [autowarefoundation#1058] Fix ros test

* [autowarefoundation#1058] Rm default params from declare_parameters + use autoware types

* [autowarefoundation#1058] Use default param file to setup NodeOptions in the ros test

* [autowarefoundation#1058] Fix docstring

* [autowarefoundation#1058] Replace receiving a Twist with a VehicleKinematicState

* [autowarefoundation#1058] Change class variables format to m_ prefix

* [autowarefoundation#1058] Fix plugin name of LongitudinalController in CMakeLists.txt

* [autowarefoundation#1058] Fix copyright dates

* [autowarefoundation#1058] Reorder includes

* [autowarefoundation#1058] Add some tests (~89% coverage without disabling flaky tests)

* [autowarefoundation#1058] Add more tests (90+% coverage without disabling flaky tests)

* [autowarefoundation#1058] Use Float32MultiArrayDiagnostic message for debug and slope

* [autowarefoundation#1058] Calculate wheel_base value from vehicle parameters

* [autowarefoundation#1058] Cleanup redundant logger setting in tests

* [autowarefoundation#1058] Set ROS_DOMAIN_ID when running tests to prevent CI failures

* [autowarefoundation#1058] Remove TF listener and use published vehicle state instead

* [autowarefoundation#1058] Change smoke tests to use autoware_testing

* [autowarefoundation#1058] Add plotjuggler cfg for both lateral and longitudinal control

* [autowarefoundation#1058] Improve design documents

* [autowarefoundation#1058] Disable flaky test

* [autowarefoundation#1058] Properly transform vehicle state in longitudinal node

* [autowarefoundation#1058] Fix TF buffer of lateral controller

* [autowarefoundation#1058] Tuning of lateral controller for LGSVL

* [autowarefoundation#1058] Fix formating

* [autowarefoundation#1058] Fix /tf_static sub to be transient_local

* [autowarefoundation#1058] Fix yaw recalculation of reverse trajs in the lateral controller

* modify trajectory_follower for galactic build

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

* [autowarefoundation#1379] Update trajectory_follower

* [autowarefoundation#1379] Update simple_planning_simulator

* [autowarefoundation#1379] Update trajectory_follower_nodes

* apply trajectory msg modification in control

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

* move directory

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

* remote control/trajectory_follower level dorectpry

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

* remove .iv trajectory follower

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

* use .auto trajectory_follower

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

* remove .iv simple_planning_simulator & osqp_interface

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

* use .iv simple_planning_simulator & osqp_interface

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

* add tmp_autoware_auto_dependencies

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

* tmporally add autoware_auto_msgs

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

* apply .auto message split

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

* fix build depend

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

* fix packages using osqp

* fix autoware_auto_geometry

* ignore lint of some packages

* ignore ament_lint of some packages

* ignore lint/pre-commit of trajectory_follower_nodes

* disable unit tests of some packages

Co-authored-by: Maxime CLEMENT <maxime.clement@tier4.jp>
Co-authored-by: Joshua Whitley <josh.whitley@autoware.org>
Co-authored-by: Igor Bogoslavskyi <igor.bogoslavskyi@gmail.com>
Co-authored-by: MIURA Yasuyuki <kokosabu@gmail.com>
Co-authored-by: wep21 <border_goldenmarket@yahoo.co.jp>
Co-authored-by: tomoya.kimura <tomoya.kimura@tier4.jp>

* [planning module]fix bug (autowarefoundation#643)

* fix topic name

* remove angular z from LinearInterpolation in motion_velocity_smoother

* fix readme

* ci(pre-commit): autofix

Co-authored-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp>
Co-authored-by: Taichi Higashide <taichi.higashide@tier4.jp>
Co-authored-by: Satoshi Tanaka <st14.828soccer@gmail.com>
Co-authored-by: Kosuke Murakami <kosuke.murakami@tier4.jp>
Co-authored-by: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com>
Co-authored-by: Nikolai Morin <nnmmgit@gmail.com>
Co-authored-by: Servando <43142004+sgermanserrano@users.noreply.github.com>
Co-authored-by: Esteve Fernandez <esteve@apache.org>
Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com>
Co-authored-by: Takagi, Isamu <43976882+isamu-takagi@users.noreply.github.com>
Co-authored-by: Kazuki Miyahara <kmiya@outlook.com>
Co-authored-by: Kenji Miyake <kenji.miyake@tier4.jp>
Co-authored-by: Takamasa Horibe <horibe.takamasa@gmail.com>
Co-authored-by: Makoto Tokunaga <vios-fish@users.noreply.github.com>
Co-authored-by: Adam Dąbrowski <adam.dabrowski@robotec.ai>
Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com>
Co-authored-by: Keisuke Shima <keisuke.shima@tier4.jp>
Co-authored-by: shin <8327162+0x126@users.noreply.github.com>
Co-authored-by: Fumiya Watanabe <rej55.g@gmail.com>
Co-authored-by: Yukihiro Saito <yukky.saito@gmail.com>
Co-authored-by: Hiroki OTA <hiroki.ota@tier4.jp>
Co-authored-by: Takayuki Murooka <takayuki5168@gmail.com>
Co-authored-by: Keisuke Shima <19993104+KeisukeShima@users.noreply.github.com>
Co-authored-by: Makoto Kurihara <mkuri8m@gmail.com>
Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com>
Co-authored-by: satoshi-ota <satoshi.ota@gmail.com>
Co-authored-by: Takayuki Murooka <takayuki.murooka@tier4.jp>
Co-authored-by: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com>
Co-authored-by: Maxime CLEMENT <maxime.clement@tier4.jp>
Co-authored-by: Joshua Whitley <josh.whitley@autoware.org>
Co-authored-by: Igor Bogoslavskyi <igor.bogoslavskyi@gmail.com>
Co-authored-by: MIURA Yasuyuki <kokosabu@gmail.com>
Co-authored-by: wep21 <border_goldenmarket@yahoo.co.jp>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
  • Loading branch information
1 parent df8a080 commit 788fcf5
Show file tree
Hide file tree
Showing 39 changed files with 8,908 additions and 0 deletions.
55 changes: 55 additions & 0 deletions planning/obstacle_avoidance_planner/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -0,0 +1,55 @@
cmake_minimum_required(VERSION 3.5)
project(obstacle_avoidance_planner)

if(NOT CMAKE_CXX_STANDARD)
set(CMAKE_CXX_STANDARD 14)
set(CMAKE_CXX_STANDARD_REQUIRED ON)
set(CMAKE_CXX_EXTENSIONS OFF)
endif()
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic -Werror)
endif()

find_package(ament_cmake_auto REQUIRED)
ament_auto_find_build_dependencies()

find_package(Eigen3 REQUIRED)
find_package(OpenCV REQUIRED)

ament_auto_add_library(obstacle_avoidance_planner SHARED
src/vehicle_model/vehicle_model_interface.cpp
src/vehicle_model/vehicle_model_bicycle_kinematics.cpp
src/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.cpp
src/util.cpp
src/debug.cpp
src/eb_path_optimizer.cpp
src/mpt_optimizer.cpp
src/process_cv.cpp
src/node.cpp
)

target_include_directories(obstacle_avoidance_planner
PUBLIC
${OpenCV_INCLUDE_DIRS}
)

target_link_libraries(obstacle_avoidance_planner
${OpenCV_LIBRARIES}
)

rclcpp_components_register_node(obstacle_avoidance_planner
PLUGIN "ObstacleAvoidancePlanner"
EXECUTABLE obstacle_avoidance_planner_node
)

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

ament_auto_package(
INSTALL_TO_SHARE
launch
config
scripts
)
384 changes: 384 additions & 0 deletions planning/obstacle_avoidance_planner/README.md

Large diffs are not rendered by default.

Original file line number Diff line number Diff line change
@@ -0,0 +1,99 @@
/**:
ros__parameters:
# trajectory total/fixing length
trajectory_length: 200 # total trajectory length[m]
forward_fixing_distance: 20.0 # forward fixing length from base_link[m]
backward_fixing_distance: 5.0 # backward fixing length from base_link[m]

# clearance(distance) when generating trajectory
clearance_from_road: 0.2 # clearance from road boundary[m]
clearance_from_object: 1.0 # clearance from object[m]
min_object_clearance_for_joint: 3.2 # minimum clearance from object[m] when judging is_including_only_smooth_range
extra_desired_clearance_from_road: 0.0 # extra desired clearance from road

# clearance for unique points
clearance_for_straight_line: 0.05 # minimum optimizing range around straight points
clearance_for_joint: 0.1 # minimum optimizing range around joint points
clearance_for_only_smoothing: 0.1 # minimum optimizing range when applying only smoothing
range_for_extend_joint: 0.1 # minimum optimizing range around joint points for extending
clearance_from_object_for_straight: 10.0 # minimum clearance from object when judging straight line

# avoiding param
max_avoiding_objects_velocity_ms: 0.5 # maximum velocity for avoiding objects[m/s]
max_avoiding_ego_velocity_ms: 6.0 # maximum ego velocity when avoiding objects[m/s]
center_line_width: 1.7 # replaced with /vehicle_info/vehicle_width when is_using_vehicle_config is true. center line width around path points used for judging that object is required to avoid or not
acceleration_for_non_deceleration_range: 1.0 # assumed acceleration for calculating non deceleration range
avoiding_object_type: # avoiding object class
unknown: true
car: true
truck: true
bus: true
bicycle: true
motorbike: true
pedestrian: true
animal: true

# solving quadratic programming
qp_max_iteration: 10000 # max iteration when solving QP
qp_eps_abs: 1.0e-8 # eps abs when solving OSQP
qp_eps_rel: 1.0e-11 # eps rel when solving OSQP
qp_eps_abs_for_extending: 1.0e-8 # eps abs when solving OSQP for extending
qp_eps_rel_for_extending: 1.0e-9 # eps rel when solving OSQP for extending
qp_eps_abs_for_visualizing: 1.0e-6 # eps abs when solving OSQP for visualizing
qp_eps_rel_for_visualizing: 1.0e-8 # eps rel when solving OSQP for visualizing

# constrain space
is_getting_constraints_close2path_points: true # generate trajectory closer to path points
max_x_constrain_search_range: 0.0 # maximum x search range in local coordinate
coef_x_constrain_search_resolution: 1.0 # coef for fine sampling when exploring x direction
coef_y_constrain_search_resolution: 0.5 # coef for fine sampling when exploring y direction
keep_space_shape_x: 0.2 # keep space for x direction from base_link[m]
keep_space_shape_y: 0.1 # replaced with /vehicle_info/vehicle_width when is_using_vehicle_config is true. keep space for y direction from base_link[m]
max_lon_space_for_driveable_constraint: 0.5 # maximum lon(x) space when optimizing point[m]

is_publishing_clearance_map: true # publish clearance map as nav_msgs::OccupancyGrid
is_publishing_area_with_objects: true # publish occupancy map as nav_msgs::OccupancyGrid
enable_avoidance: true # enable avoidance function
is_using_vehicle_config: true # use vehicle config
num_sampling_points: 100 # number of optimizing points
num_joint_buffer_points: 3 # number of joint buffer points
num_joint_buffer_points_for_extending: 6 # number of joint buffer points for extending
num_offset_for_begin_idx: 2 # number of shifting points for beginning non-fix idx
num_fix_points_for_extending: 50 # number of fixing points when extending
delta_arc_length_for_optimization: 1.0 # delta arc length when optimizing[m]
delta_arc_length_for_trajectory: 0.1 # delta arc length for trajectory[m]
delta_dist_threshold_for_closest_point: 3.0 # delta dist thres for closest point[m]
delta_yaw_threshold_for_closest_point: 1.046 #M_PI/3.0. delta yaw thres for closest point
delta_yaw_threshold_for_straight: 0.02 # delta dist thres for straight point

# mpt param
# vehicle param for mpt
max_steer_deg: 40.0 # max steering angle [deg]
steer_tau: 0.1 # steering dynamics time constant (1d approximation) [s]

# trajectory param for mpt
num_curvature_sampling_points: 5 # number of sampling points when calculating curvature
delta_arc_length_for_mpt_points: 0.5 # delta arc length when generating mpt[m]
forward_fixing_mpt_distance: 10 # forward fixing distance for MPT

# optimization param for mpt
is_hard_fixing_terminal_point: false # hard fixing terminal point
base_point_weight: 2000.0 # slack weight for lateral error around base_link
top_point_weight: 1000.0 # slack weight for lateral error around vehicle-top point
mid_point_weight: 1000.0 # slack weight for lateral error around the middle point
lat_error_weight: 10.0 # weight for lateral error
yaw_error_weight: 0.0 # weight for yaw error
steer_input_weight: 0.01 # weight for steering input
steer_rate_weight: 1.0 # weight for steering rate
steer_acc_weight: 0.000001 # weight for steering acceleration
terminal_lat_error_weight: 0.0 # weight for lateral error at terminal point
terminal_yaw_error_weight: 100.0 # weight for yaw error at terminal point
terminal_path_lat_error_weight: 1000.0 # weight for lateral error at path end point
terminal_path_yaw_error_weight: 1000.0 # weight for yaw error at path end point
zero_ff_steer_angle: 0.5 # threshold that feed-forward angle becomes zero

# replanning & trimming trajectory param outside algorithm
min_delta_dist_for_replan: 10.0 # minimum delta dist thres for replan[m]
min_delta_time_sec_for_replan: 1.0 # minimum delta time for replan[second]
max_dist_for_extending_end_point: 5.0 # minimum delta dist thres for extending last point[m]
distance_for_path_shape_change_detection: 2.0 # minimum delta dist thres for detecting path shape change
Original file line number Diff line number Diff line change
@@ -0,0 +1,105 @@
// Copyright 2020 Tier IV, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef OBSTACLE_AVOIDANCE_PLANNER__DEBUG_HPP_
#define OBSTACLE_AVOIDANCE_PLANNER__DEBUG_HPP_

#include <opencv2/core.hpp>

#include <autoware_auto_perception_msgs/msg/predicted_object.hpp>
#include <autoware_auto_planning_msgs/msg/trajectory_point.hpp>
#include <nav_msgs/msg/occupancy_grid.hpp>
#include <visualization_msgs/msg/marker_array.hpp>

#include <string>
#include <vector>

struct ConstrainRectangle;
struct Bounds;
struct DebugData;
struct VehicleParam;

namespace util
{
struct Footprint;
}

visualization_msgs::msg::MarkerArray getDebugVisualizationMarker(
const DebugData & debug_data,
const std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> & optimized_points,
const VehicleParam & vehicle_param);

geometry_msgs::msg::Pose getVirtualWallPose(
const geometry_msgs::msg::Pose & target_pose, const VehicleParam & vehicle_param);

visualization_msgs::msg::MarkerArray getDebugPointsMarkers(
const std::vector<geometry_msgs::msg::Point> & interpolated_points,
const std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> & optimized_points,
const std::vector<geometry_msgs::msg::Point> & straight_points,
const std::vector<geometry_msgs::msg::Pose> & fixed_points,
const std::vector<geometry_msgs::msg::Pose> & non_fixed_points);

visualization_msgs::msg::MarkerArray getDebugConstrainMarkers(
const std::vector<ConstrainRectangle> & constrain_ranges, const std::string & ns);

visualization_msgs::msg::MarkerArray getObjectsMarkerArray(
const std::vector<autoware_auto_perception_msgs::msg::PredictedObject> & objects,
const std::string & ns, const double r, const double g, const double b);

visualization_msgs::msg::MarkerArray getRectanglesMarkerArray(
const std::vector<util::Footprint> & rects, const std::string & ns, const double r,
const double g, const double b);

visualization_msgs::msg::MarkerArray getRectanglesNumMarkerArray(
const std::vector<util::Footprint> & rects, const std::string & ns, const double r,
const double g, const double b);

visualization_msgs::msg::MarkerArray getPointsMarkerArray(
const std::vector<geometry_msgs::msg::Pose> & points, const std::string & ns, const double r,
const double g, const double b);

visualization_msgs::msg::MarkerArray getPointsMarkerArray(
const std::vector<geometry_msgs::msg::Point> & points, const std::string & ns, const double r,
const double g, const double b);

visualization_msgs::msg::MarkerArray getPointsTextMarkerArray(
const std::vector<geometry_msgs::msg::Pose> & points, const std::string & ns, const double r,
const double g, const double b);

visualization_msgs::msg::MarkerArray getPointsTextMarkerArray(
const std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> & points,
const std::string & ns, const double r, const double g, const double b);

visualization_msgs::msg::MarkerArray getBaseBoundsLineMarkerArray(
const std::vector<Bounds> & bounds, const std::vector<geometry_msgs::msg::Pose> & candidate_p0,
const std::string & ns, const double r, const double g, const double b);

visualization_msgs::msg::MarkerArray getTopBoundsLineMarkerArray(
const std::vector<Bounds> & bounds, const std::vector<geometry_msgs::msg::Pose> & candidate_p1,
const std::string & ns, const double r, const double g, const double b);

visualization_msgs::msg::MarkerArray getMidBoundsLineMarkerArray(
const std::vector<Bounds> & bounds, const std::vector<geometry_msgs::msg::Pose> & candidate_top,
const std::string & ns, const double r, const double g, const double b);

visualization_msgs::msg::MarkerArray getVirtualWallMarkerArray(
const geometry_msgs::msg::Pose & pose, const std::string & ns, const double r, const double g,
const double b);

visualization_msgs::msg::MarkerArray getVirtualWallTextMarkerArray(
const geometry_msgs::msg::Pose & pose, const std::string & ns, const double r, const double g,
const double b);

nav_msgs::msg::OccupancyGrid getDebugCostmap(
const cv::Mat & clearance_map, const nav_msgs::msg::OccupancyGrid & occupancy_grid);
#endif // OBSTACLE_AVOIDANCE_PLANNER__DEBUG_HPP_
Loading

0 comments on commit 788fcf5

Please sign in to comment.