Skip to content

Commit

Permalink
feat: add autoware_utils package (autowarefoundation#7)
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>

* add sample ros2 packages

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

* remove ROS1 packages

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

* Revert "remove ROS1 packages temporarily"

This reverts commit aaf2f7d3908ee5f865105ea9094ed358ea9d3c75.

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

* add COLCON_IGNORE to ros1 packages

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

* Port autoware_utils (#33)

* Port autoware_utils

* Fix function name

* Fix buildtool_depend

* Rename h files to hpp (#142)

* Change includes

* Rename files

* Adjustments to make things compile

* Other packages

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

* Use quotes for includes where appropriate (#144)

* Use quotes for includes where appropriate

* Fix lint tests

* Make tests pass hopefully

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

* Run uncrustify on the entire Pilot.Auto codebase

* Exclude open PRs

* Ros2 v0.8.0 autoware utils (#291)

* restore file name for v0.8.0 update

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

* Improve autoware utils (#986)

* Refactor

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

* Add features to autoware_utils

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

* Add lane departure checker (#928)

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

* Fix/remove duplicated point (#1107)

* Split PoseDeviaiton functions

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

* Remove unnecessary pose

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

* Don't insert point if duplicated point exists

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

* Fix condition of duplicated points

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

* Rename file name for v0.8.0 update

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

* Add dependencies into package.xml

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

* [autoware_utils]: Fix CMakeLists.txt and package.xml for lint and testing

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

* [autoware_utils]: Fix lint

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

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

* [autoware_utils]: fix traits (#326)

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

* Ros2 v0.8.0 lane departure checker (#327)

* Add lane departure checker (#928)

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

* Fix bug of lane_departure_checker (#1011)

* Fix bug of lane_departure_checker

Since preceeding lanelets are missing, when vehicle is at the beginning of a lanelet, it's mistakenly considered as out of lane.

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

* Fix typo

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

* Reduce computational cost of lane_departure_checker (#1026)

Lanelet visualization will be too slow when big lanelets are visualized.

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

* Fix typo (#1062)

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

* [lane_departure_checker]: Port to ROS2

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

* [lane_departure_checker]: Fix lint

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

* [autoware_utils]: Publish autoware debug msgs

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

* [lane_departure_checker]: Use autoware_debug_msgs instead of std_msgs

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

* [goal_distance_calculator]: Use autoware_debug_msgs instead of std_msgs

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

* [autoware_utils]: Add namespace to debug traits

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

* [lane_departure_checker]: Fix lint

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

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

* Ros2 v0.8.0 behavior velocity planner (#332)

* restore behavior velocity planner files for v0.8.0 update

* Fix detection area (#808)

* Remove PASS state

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

* Keep stopping after going over dead-line in STOP state

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

* Clear state after obstacle disappears

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

* Parameterize and change conditions

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

* Refactor

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

* add elapsed time checker

* Refactor

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

* Ignore inserting point if points are too close

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

* Rename NORMAL to GO

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

* Revert "Ignore inserting point if points are too close"

This reverts commit 91c8d620ea11e703611945cb9a50877d175f4ff6.

* Fix small bug

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

Co-authored-by: taichiH <azumade.30@gmail.com>

* fix timing of publish stop reason of stop_line (#845)

* fix timing of publish stop reason of stop_line

* Fix formatting

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

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

* Fix task registration of detection area module (#856)

* Fix task registration of detection area module

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

* Fix id of factor_text marker

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

* Do not interpolate if path size is 1 (#865)

* Revert "restore behavior velocity planner files for v0.8.0 update"

This reverts commit ce07d3c2bfae562d56fec46c4f81d5351fd936a1.

* fix some implement miss

* restore behavior velocity planner files for v0.8.0 update

* change intersection param (#861)

* Fix/intersection stop line (#841)

* add trimmed path function

* fix typo

* avoid to insert stop line outside intersection

* Add pass judge flag (#872)

* Add pass judge flag

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

* Fix wrong format

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

* Fix flag order in judge condition

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

* add ignore width with blind spot (#864)

* add ignore width with blind spot

* remove unused function

* fix typo

* fix margin

* remove bicycle from intersection stuck vehicle (#896)

* Revert "restore behavior velocity planner files for v0.8.0 update"

This reverts commit ff61ee56f2ce4728e53eb698836db60d68bf64b9.

* fix implement miss

* restore behavior velocity planner files for v0.8.0 update (fixed)

* Add external traffic light input (#912)

* Add external traffic light input

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

* Update topic namespace

* Fix typos in planning modules (#866)

* fix typos in planning

* fix corresponding typos in planning

* revert fixed typos temporarily due to its impact on launchers

* add exception handling to intersection module (#920)

* add exception handling

* output input path

* prevent passthrough chattering (#901)

Signed-off-by: Yukihiro Saito <yukky.saito@gmail.com>

* cosnider current_velocity to calculate intersection passing time (#880)

* cosnider current_velocity to calculate intersection passing time

* add fabs to current velocity

* Improve stuck vehicle detect (#824)

* intersection: modify stuck vehicle detect logic to consider the object shape

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

* intersection: consider ego-vehicle length when calculate stuck vehicle detection area

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

* intersection: change param stuck_vehicle_ignore_dist: 5.0 -> 7.0

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

* intersection: use vehicle_info/vehicle_length

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

* fix implement miss

* fix target pedestrian (#927)

Signed-off-by: Yukihiro Saito <yukky.saito@gmail.com>

* fix insert backward point (#953)

* fix insert backward point

* add comment

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

* remove unnecessary if

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

* Add stop line referred from road marking (#968)

* Add stop line referred from road marking

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

* Add stop line for walkway

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

* Feature/external input (#982)

* add external input msg

* impl external input(crosswalk, intersection)

* :put_litter_in_its_place:

* change to ++14

* fix bug

* fix bug

* cosmetic change

* Revert "restore behavior velocity planner files for v0.8.0 update"

This reverts commit ff61ee56f2ce4728e53eb698836db60d68bf64b9.

* Fix typo (#1013)

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

* Fix/blind spot insert point (#1027)

* Copy insert point from previous point

* Reset path unless state is stop

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

* change slow down speed (#1030)

* restore behavior velocity planner files for v0.8.0 update (fixed)

* Fix/walkway stop judge range (#1028)

* Fix walkway stop judge

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

* Make refer id same

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

* fix getObjectivePolygons() so that intersecting lanelet is added to objective lanelets even when there is no preceeding lanelets (#1010)

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

* Revert "restore behavior velocity planner files for v0.8.0 update (fixed)"

This reverts commit 39231f23eb17e39a418f28f5ccfe1df55b3fb45c.

* fix implement miss for marker

* restore behavior velocity planner files for v0.8.0 update (fixed)

* Fix/blind spot polygon (#1056)

* Fix detection area for blind spot

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

* Apply clang format

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

* Shrink conflicting area

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

* chane slowdown judgement (#1066)

* chane slowdown judgement

* cosmetic change

* Revert "restore behavior velocity planner files for v0.8.0 update (fixed)"

This reverts commit 39231f23eb17e39a418f28f5ccfe1df55b3fb45c.

* fixed for ros2 style

* restore behavior velocity planner files for v0.8.0 update (fixed)

* Remove unused headers in behavior_velocity_planner (#1072)

* publish walkway stop reason (#1073)

* Revert "restore behavior velocity planner files for v0.8.0 update (fixed)"

This reverts commit 39231f23eb17e39a418f28f5ccfe1df55b3fb45c.

* restore behavior velocity planner files for v0.8.0 update (fixed)

* Add correspondence marker of detection_area and stop_line (#1081)

* Fix comment

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

* Add const

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

* Add correspondence marker of detection_area and stop_line

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

* Add obstacle marker

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

* Fixup

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

* Add id marker

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

* Clear state when time rollback found

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

* Fix typo

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

* change stop to polygon line

* apply clang format

* fix code

* fix bug

Co-authored-by: taichiH <azumade.30@gmail.com>

* Fix pass judge idx (#1090)

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

* fix bug (#1087)

* Revert "restore behavior velocity planner files for v0.8.0 update (fixed)"

This reverts commit 39231f23eb17e39a418f28f5ccfe1df55b3fb45c.

* fix for ros2 style

* restore behavior velocity planner files for v0.8.0 update (fixed)

* Fix/remove duplicated point (#1107)

* Split PoseDeviaiton functions

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

* Remove unnecessary pose

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

* Don't insert point if duplicated point exists

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

* Fix condition of duplicated points

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

* seperate stop distance param (#1137)

* seperate stop distance param

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

* apply param for automotically generated stop line

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

* shift slow down line

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

* add param in yaml file

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

* fix prediction_time_margin bug (#1148)

* Revert "restore behavior velocity planner files for v0.8.0 update (fixed)"

This reverts commit 39231f23eb17e39a418f28f5ccfe1df55b3fb45c.

* fix ros2 style

* remove deprecated

* reformat

* add test temporary launch

* cpp_lint_namespace_what_you_use

* lint_comment_line_length

* pass ament cpplint

* fixe commit

* fix code

* remove unneccesary

* fix param structure

* fix missng params

* fix throttle time

Co-authored-by: tomoya.kimura <tomoya.kimura@tier4.jp>
Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com>
Co-authored-by: taichiH <azumade.30@gmail.com>
Co-authored-by: Kenji Miyake <kenji.miyake@tier4.jp>
Co-authored-by: Fumiya Watanabe <rej55.g@gmail.com>
Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com>
Co-authored-by: Kazuki Miyahara <kmiya@outlook.com>
Co-authored-by: Yukihiro Saito <yukky.saito@gmail.com>
Co-authored-by: Takamasa Horibe <horibe.takamasa@gmail.com>
Co-authored-by: Kosuke Murakami <kosuke.murakami@tier4.jp>
Co-authored-by: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com>

* Fix/lane departure checker (#386)

* Fix/lane departure checker (#1177)

* Add more processing time measurement

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

* Rename processing_time to processing_time_ms

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

* Refactor StopWatch class

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

* apply ament_uncrustify

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

* Fix autoware_utils (#419)

* Fix autoware_utils

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

* Apply format

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

* Add <memory>

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

* Ros2 fix topic name part1 (#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 rolling build errors (#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>

* Ros2 delete vehicle info (#1227)

* remove dependency on vehicle info in autoware_utils

* delete vehicle_info in autoware_utils

* Unify Apache-2.0 license name (#1242)

* Fix/fix utils (#1310)

* Add missing namespace to autoware_utils

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

* Add createVehicleInfo

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

* Add rethrow

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

* Format package.xml

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

* Fix usage of autoware_utils

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

* Add missing namespace comment

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

* Make a curvature calculation function in autoware_utils' library (#1388) (#1393)

* add curvature calculation function

* add two functions

* integrate 2 functions into 1 function by template function

* add unit test for curvature calculation

* modify spell

* Update common/util/autoware_utils/include/autoware_utils/math/curvature.h

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

* add modifications

* modify format

* update

* update

Co-authored-by: Kazuki Miyahara <kmiya@outlook.com>
Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

Co-authored-by: purewater0901 <43805014+purewater0901@users.noreply.github.com>
Co-authored-by: Kazuki Miyahara <kmiya@outlook.com>

* rename function from calculateCUrvature to calcCurvature (#1461) (#1486)

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

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

* Add trajectory functions in autoware_utils (#1410)(#1438)(#1416)(#1473)(#1491) (#1482)

* Add closest point index calculation function in autoware_utils (#1410)

* add nearest point calculator

* update

* add template

* update

* update

* update

* update variables name

* update variables name

* remove redundant expression

* add unit test

* update test

* test has some errors

* update

* update name

* update name

* modify bug

* update

* update

* add new test cases

* add new function

* add get pose function

* delete unnecessary file

* update test

* delete namespace

* add emtpy handler

* remove redundant test

* remove template

* add necessary files to autoware_utils (#1438)

* add necessary files

* Apply fix

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

* fix test

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

* Fix lint

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

* Fix build warning

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

* add some trajectory functions commonly used in planning/control (#1416)

* add some trajectory functions commonly used in p/c

* minor fix with reviews

* refactor based on reviews

* minor changes

* refactor based on reviews

* minor changes

* minor changes

* refactor based on reviews

* use std::vector<T> and T in findClosestIndex

* fix bugs that I wrote wrong

* refactor

* refactor

* fix bug of including optional.hpp twice

* refactor

* refactor

* fix bug

* fix bugs

* update

* apply clang-format

* add blank line

* add test for searchZeroVelocityIndex

* update

* update

* update

* add some tests

* closest -> nearest

* add some test

* update test

* not to use optional if not necessary

* not unsigned

* add explanation of segment

* update

* 契約プログラミング

* add signed to variable of length

* update

* update

* update

* update test

* update

* update test

* update

* update test

* update

* update

* update

* update

* update

* update

* update

* Remove wrapper for autoware_plannig_msgs

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

* Fix test

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

* Change implementation of calcLongitudinalOffsetToSegment

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

* Set default value for trajectory velocity

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

* Remove unused function

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

* add 3 cases for curve trajectory

* update

* Revert "update"

This reverts commit 6d4f3ee25f49dcca146340c9a1ed3f4c1ac4f55c.

* Revert "add 3 cases for curve trajectory"

This reverts commit d3f4751851afcdac41e160fecbb6bee9ac9f60a2.

* Add test case

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

* Fix comment

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

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

* use template in geometry.hpp (#1473)

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

* Fix include

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

* Fix errors

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

* Fix license

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

* Fix find nearest segment index (#1491)

* add odd test case, and confirmed that the test don't pass

* fix findNearestSegmentIndex, and confirmed all the test passed

* use calcLongitudinalOffsetToSegment

* use vector in test

* add deleted comment

Co-authored-by: purewater0901 <43805014+purewater0901@users.noreply.github.com>
Co-authored-by: Kenji Miyake <kenji.miyake@tier4.jp>
Co-authored-by: Takayuki Murooka <takayuki5168@gmail.com>
Co-authored-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* Feature/update autoware utils (#1501)(#1513)(#1522)(#1463)(#1503) (#1544)

* generalize searchZeroVelocityIndex Function (#1501)

* generalize searchZeroVelocityIndex Function

* update test

* fix test case

* update test case

* update terminal index in the test

* Fix/fix autoware utils geometry (#1513)

* Revert geometry.cpp

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

* Add default implementations

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

* add constant gravity (#1522)

* add arange & linspace in math util (#1463)

* [WIP] add arange & linspace in math util

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

* add unittest cases of arange and linspace

* implement and test arange and linspace in util library

* fix issues found in review

* fix typos

* fix issue found in review

* fix typo

* fix issues found in review

* fix issues found in review

Co-authored-by: nisshan-x <nisshan.x@gmail.com>

* extend findNearestIndex, and add tests (#1503)

* extend findNearestIndex, and add tests

* apply clang-format

* update

* hoge_threshold -> max_hoge

* _min/_max -> min_/max_, and update

* update

* update

* update

* update

* update

* Fix warnings

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

* Add reserve for autoware_utils::arange

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

Co-authored-by: purewater0901 <43805014+purewater0901@users.noreply.github.com>
Co-authored-by: Takayuki Murooka <takayuki5168@gmail.com>
Co-authored-by: Takamasa Horibe <horibe.takamasa@gmail.com>
Co-authored-by: nisshan-x <nisshan.x@gmail.com>

* Add update_param.hpp in autoware_utils (#1556)

* Add update_param.hpp in autoware_utils

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

* Change to bool

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

* Remove unnecesarry file for test (#1605)

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

* Feature/update autoware_utils (#1576) (#1599) (#1603) (#1619) (#1631) (#1650)

* Add tests for autoware_utils::StopWatch (#1832)

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

* Fix -Wunused-parameter (#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 (#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>

* Autoware util squared dist main merge (#1853)

* autoware_utils: use calcSquaredDistance in calcClosest

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

* add test

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

* Fix clang warnings (#1859)

* Fix -Wreturn-std-move

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

* Fix -Wunused-private-field

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

* Ignore -Wnonportable-include-path for mussp

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

* Fix -Wunused-const-variable

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

* Fix "can not be used when making a shared object"

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

* Fix clang-tidy warnings of autoware_utils (#1854)

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

* Revive constructor of StopWatch class (#1882)

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

* enable autonomous recovery (#1904)

* add createQuaternionFromYaw (#2120)

* add createQuaternionFromYaw

* add test

* change return value type of createQuaternionFromRPY from tf2::quat to geomety_msgs::msg::quat

* use geometry_msgs::msg::Quaternion in createQuaternionFromRPY in application

* implement fast findNearestIndex with threshold (#2142)

* implement fast findNearestIndex with threshold

* fix reviews

* add calcLateralLength (#2226)

* add calcLateralLength

* rename length to offset

* imeplemet in ROS2 (#1868)

* add new get point function for posewithcovariancestamped (#2156)

* add new get point function for posewithcovariancestamped

* fix format

* add behavior velocity planner utils (#2113)

* add gtest utils

* add state machine as utils

* add arc lane utils

* remove empty file

* add gtest for offset segment

* use calcdist2d in autoware util

* apply get point

* fix uncrustify

* fix conflict getPoint in behavior path

* apply commit suggestion

* slower than ros1

* Change formatter to clang-format and black (#2332)

* Revert "Temporarily comment out pre-commit hooks"

This reverts commit 748e9cdb145ce12f8b520bcbd97f5ff899fc28a3.

* Replace ament_lint_common with autoware_lint_common

Signed-off-by: Kenji Miyake <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 (#500)

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

* use autoware_auto_msgs (#489)

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

* remove COLCON_IGNORE (#502)

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

* Add trajectory converter (#512)

* Add trajectory converter

* Add comment

* Fix comment

* Fix comment

* Add TODO in comment

* Update common/util/autoware_utils/include/autoware_utils/trajectory/convert.hpp

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

* Fix comment

* Use doxygen style comment

* Fix function name

* Fix test code

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

* add README (#671)

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

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

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

* fix interpolation for insert point (#2228)

* fix interpolation for insert point

* to prev interpolation pkg

* Revert "to prev interpolation pkg"

This reverts commit 9eb145b5d36e297186015fb17c267ccd5b3c21ef.

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

* fix topic name (#2266)

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

* Add namespace to diag for dual_return_filter (#2269)

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

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

* Simplify marker scale initialization (#2286)

* Fix/crosswalk polygon (#2279)

* extend crosswalk polygon

* improve readability

* fix polygon shape

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

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

* [motion_velocity_smoother] ignore debug print (#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 (#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 (#2295)

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

* [mpc_follower] remove stop distance condition from stopState decision (#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 (#2288)

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

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

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

* Simplify iteration of initial poses (#2310)

* Make a transform object const (#2311)

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

* Feature/no stopping area (#2163)

* add no stopping area module to behavior velocity planner

* apply utils

* add polygon interpolation module order stopline around area is considered

* devide jpass udge with stop line polygon

* update docs

* rename file name

* update to latest

* minor change for marker

* update license

Co-authored-by: Yukihiro Saito <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 (#2281)

* Fix side shift planner (#2171) (#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 (#2175)

* delete unnecessary check

* fix condition of starting pull out

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

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

* reset prev result

* clean code

* reset only raw_steer_cmd

* Update control/mpc_follower/src/mpc_follower_core.cpp

Co-authored-by: Takamasa Horibe <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 (#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 (#2211)

* Fix lane ids

* Prevent acceleration on avoidance (#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 (#2218)

* fix confidence (#2220)

* too high confidence (#2229)

* Fix/obstacle stop 0.23.0 (#2232)

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

* Fix/insert implementation (#2186)

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

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

* Remove exception

* Fix clock

* Remove blank line

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

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

* fix interpolation for insert point (#2228)

* fix interpolation for insert point

* to prev interpolation pkg

* fix index (#2265)

* turn signal calculation (#2280)

* add turn signal funtion in path shifter

* add ros parameters

Co-authored-by: Fumiya Watanabe <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 (#2305) (#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 (#2317)

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

* fix object orientation

* fix function name

* add guard (#2321)

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

* Add detail collision check (#2274)

* Add detail collision check

Signed-off-by: wep21 <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  (#2222)

* update to model trained by mmdet3d

* add vizualizer (debug)

* for multi-frame inputs

* chagne config

* use autoware_utils::pi

* project specific model and param

* rename vfe -> encoder

* rename general to common

* update download link

* update

* fix

* rename model_name

* change training toolbox link

* chage lint package

* fix test error

* commit suggestion

* Feature/lane change detection (#2331)

* add old information deleter

* fix access bug

* change to deque

* update obstacle buffer

* fix some bugs

* add lane change detector

* make a update lanelet function

* fix code style

* parameterize essential values

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

Co-authored-by: Kazuki Miyahara <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  (#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 (#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 (#2348)

* add cross judgement and common signal function (#2319)

* merge branch turn_signal_common

* add turn signal function in signal decider

* add cross judge in path_utilities and delete from turn_signal_decider

* remove original signal calculation in lane change

* omit substitution

* replace turn signal decider in pull over function

* modify cross judge logic

* replace turn signal decider in avoidance

* add readme of turn signal

* update

* delete print debug

* update

* delete lane change decider in path shifter

* delete blank line

* fix indent

* fix typo

* fix typo

* decrease nest

* run pre commit

* Add 0 limit at forward jerk velocity filter (#2340)

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

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

* add offset param

* clang-format

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

* Feature/add doc for keep braking function at driving state (#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 (#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 (#1916)"

This reverts commit ff4f0b5a844d1f835f1b93bd3b36a76747b0cd02.

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

This reverts commit f0478187db4c28bf6092c198723dcc5ec11a9c70.

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

This reverts commit 66de2f3924a479049fce2d5c5c6b579cacbd3e49.

* Fix orientation availability in centerpoint

Signed-off-by: wep21 <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>

Co-authored-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp>
Co-authored-by: Nikolai Morin <nnmmgit@gmail.com>
Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com>
Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com>
Co-authored-by: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com>
Co-authored-by: taichiH <azumade.30@gmail.com>
Co-authored-by: Kenji Miyake <kenji.miyake@tier4.jp>
Co-authored-by: Fumiya Watanabe <rej55.g@gmail.com>
Co-authored-by: Kazuki Miyahara <kmiya@outlook.com>
Co-authored-by: Yukihiro Saito <yukky.saito@gmail.com>
Co-authored-by: Takamasa Horibe <horibe.takamasa@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: Takagi, Isamu <43976882+isamu-takagi@users.noreply.github.com>
Co-authored-by: purewater0901 <43805014+purewater0901@users.noreply.github.com>
Co-authored-by: Takayuki Murooka <takayuki5168@gmail.com>
Co-authored-by: nisshan-x <nisshan.x@gmail.com>
Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com>
Co-authored-by: Hiroki OTA <hiroki.ota@tier4.jp>
Co-authored-by: Taichi Higashide <taichi.higashide@tier4.jp>
Co-authored-by: Takayuki Murooka <takayuki.murooka@tier4.jp>
Co-authored-by: Takeshi Miura <57553950+1222-takeshi@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: taikitanaka <ttatcoder@outlook.jp>
Co-authored-by: Takeshi Ishita <ishitah.takeshi@gmail.com>
Co-authored-by: Makoto Kurihara <mkuri8m@gmail.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>
  • Loading branch information
1 parent 6c296b9 commit 00308c4
Show file tree
Hide file tree
Showing 36 changed files with 4,080 additions and 0 deletions.
38 changes: 38 additions & 0 deletions common/autoware_utils/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -0,0 +1,38 @@
cmake_minimum_required(VERSION 3.5)
project(autoware_utils)

### Compile options
if(NOT CMAKE_CXX_STANDARD)
set(CMAKE_CXX_STANDARD 17)
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(Boost REQUIRED)

ament_auto_add_library(autoware_utils SHARED
src/autoware_utils.cpp
src/planning/planning_marker_helper.cpp
)

# Test
if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
ament_lint_auto_find_test_dependencies()

find_package(ament_cmake_gtest REQUIRED)

file(GLOB_RECURSE test_files test/**/*.cpp)

ament_add_gtest(test_autoware_utils ${test_files})

target_link_libraries(test_autoware_utils
autoware_utils
)
endif()

ament_auto_package()
5 changes: 5 additions & 0 deletions common/autoware_utils/README.md
Original file line number Diff line number Diff line change
@@ -0,0 +1,5 @@
# autoware_utils

## Purpose

This package contains many common functions used by other packages, so please refer to them as needed.
37 changes: 37 additions & 0 deletions common/autoware_utils/include/autoware_utils/autoware_utils.hpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,37 @@
// 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 AUTOWARE_UTILS__AUTOWARE_UTILS_HPP_
#define AUTOWARE_UTILS__AUTOWARE_UTILS_HPP_

#include "autoware_utils/geometry/boost_geometry.hpp"
#include "autoware_utils/geometry/geometry.hpp"
#include "autoware_utils/geometry/pose_deviation.hpp"
#include "autoware_utils/math/constants.hpp"
#include "autoware_utils/math/normalization.hpp"
#include "autoware_utils/math/range.hpp"
#include "autoware_utils/math/unit_conversion.hpp"
#include "autoware_utils/planning/planning_marker_helper.hpp"
#include "autoware_utils/ros/debug_publisher.hpp"
#include "autoware_utils/ros/debug_traits.hpp"
#include "autoware_utils/ros/marker_helper.hpp"
#include "autoware_utils/ros/processing_time_publisher.hpp"
#include "autoware_utils/ros/self_pose_listener.hpp"
#include "autoware_utils/ros/transform_listener.hpp"
#include "autoware_utils/ros/update_param.hpp"
#include "autoware_utils/ros/wait_for_param.hpp"
#include "autoware_utils/system/stop_watch.hpp"
#include "autoware_utils/trajectory/trajectory.hpp"

#endif // AUTOWARE_UTILS__AUTOWARE_UTILS_HPP_
Original file line number Diff line number Diff line change
@@ -0,0 +1,96 @@
// 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 AUTOWARE_UTILS__GEOMETRY__BOOST_GEOMETRY_HPP_
#define AUTOWARE_UTILS__GEOMETRY__BOOST_GEOMETRY_HPP_

#include <boost/geometry/core/cs.hpp>
#include <boost/geometry/geometries/geometries.hpp>
#include <boost/geometry/geometries/register/point.hpp>

#define EIGEN_MPL2_ONLY
#include <Eigen/Core>
#include <Eigen/Geometry>

#include <geometry_msgs/msg/point.hpp>

namespace autoware_utils
{
// 2D
struct Point2d;
using Segment2d = boost::geometry::model::segment<Point2d>;
using Box2d = boost::geometry::model::box<Point2d>;
using LineString2d = boost::geometry::model::linestring<Point2d>;
using LinearRing2d = boost::geometry::model::ring<Point2d>;
using Polygon2d = boost::geometry::model::polygon<Point2d>;
using MultiPoint2d = boost::geometry::model::multi_point<Point2d>;
using MultiLineString2d = boost::geometry::model::multi_linestring<Point2d>;
using MultiPolygon2d = boost::geometry::model::multi_polygon<Point2d>;

// 3D
struct Point3d;
using Segment3d = boost::geometry::model::segment<Point3d>;
using Box3d = boost::geometry::model::box<Point3d>;
using LineString3d = boost::geometry::model::linestring<Point3d>;
using LinearRing3d = boost::geometry::model::ring<Point3d>;
using Polygon3d = boost::geometry::model::polygon<Point3d>;
using MultiPoint3d = boost::geometry::model::multi_point<Point3d>;
using MultiLineString3d = boost::geometry::model::multi_linestring<Point3d>;
using MultiPolygon3d = boost::geometry::model::multi_polygon<Point3d>;

struct Point2d : public Eigen::Vector2d
{
Point2d() = default;
Point2d(const double x, const double y) : Eigen::Vector2d(x, y) {}

[[nodiscard]] Point3d to_3d(const double z = 0.0) const;
};

struct Point3d : public Eigen::Vector3d
{
Point3d() = default;
Point3d(const double x, const double y, const double z) : Eigen::Vector3d(x, y, z) {}

[[nodiscard]] Point2d to_2d() const;
};

inline Point3d Point2d::to_3d(const double z) const { return Point3d{x(), y(), z}; }

inline Point2d Point3d::to_2d() const { return Point2d{x(), y()}; }

inline geometry_msgs::msg::Point toMsg(const Point3d & point)
{
geometry_msgs::msg::Point msg;
msg.x = point.x();
msg.y = point.y();
msg.z = point.z();
return msg;
}

inline Point3d fromMsg(const geometry_msgs::msg::Point & msg)
{
Point3d point;
point.x() = msg.x;
point.y() = msg.y;
point.z() = msg.z;
return point;
}
} // namespace autoware_utils

BOOST_GEOMETRY_REGISTER_POINT_2D( // NOLINT
autoware_utils::Point2d, double, cs::cartesian, x(), y()) // NOLINT
BOOST_GEOMETRY_REGISTER_POINT_3D( // NOLINT
autoware_utils::Point3d, double, cs::cartesian, x(), y(), z()) // NOLINT

#endif // AUTOWARE_UTILS__GEOMETRY__BOOST_GEOMETRY_HPP_
Loading

0 comments on commit 00308c4

Please sign in to comment.