Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

feat(metrics_calculation): add kinematic and localization evaluators with metrics #928

Conversation

djargot
Copy link
Contributor

@djargot djargot commented May 18, 2022

Description

This PR extends planning evaluator with other evaluators and various metrics calculation.

  • Create frameworks for different types of metrics:
  • Extract overlapping code to generic evaluator that other evaluators can inherit from
  • Add example metrics
  • Add all tests
  • Add README

Related links

autowarefoundation/autoware-projects#11

Tests performed

Notes for reviewers

Work in progress.

Pre-review checklist for the PR author

In-review checklist for the PR reviewers

The PR reviewers must check the checkboxes below before approval.

  • The PR follows the pull request guidelines.
  • The PR has been properly tested.
  • The PR has been reviewed by the code owners.

Post-review checklist for the PR author

The PR author must check the checkboxes below before merging.

  • There are no open discussions or they are tracked via tickets.
  • The PR is ready for merge.

After all checkboxes are checked, anyone who has write access can merge the PR.

@djargot djargot force-pushed the feature/add-localization-metrics-calculation branch 2 times, most recently from 784e800 to e756c85 Compare May 18, 2022 20:44
@djargot djargot changed the title [WIP] Feat - Add various evaluators with metrics [WIP] feat - Add various evaluators with metrics May 18, 2022
YoheiMishina pushed a commit to YoheiMishina/autoware.universe that referenced this pull request Jun 9, 2022
* 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>
YoheiMishina added a commit to YoheiMishina/autoware.universe that referenced this pull request Jun 9, 2022
* Feature/goal distance calculator (autowarefoundation#311)

* add initial package

* half lint

* pass cpplint

* add pose deviation

* add autoware utils

* fix prefix

* beautify

* add ament copyright

* fix ament format

* fix param name

* refactor param

* refactor params

* [goal_sistance_calculator]: Fix CMakeLists.txt

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

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

* ros2-v0.8.0-goal_distance_calulator (autowarefoundation#319)

* update to component

* refactor node name

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

* Add lane departure checker (autowarefoundation#928)

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

* Fix bug of lane_departure_checker (autowarefoundation#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 (autowarefoundation#1026)

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

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

* Fix typo (autowarefoundation#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>

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

* add use_sim-time option (autowarefoundation#454)

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

* Remove use_sim_time for set_parameter (autowarefoundation#1260)

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

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

* goal_distance_calculator port to .auto (autowarefoundation#525)

* goal_distance_calculator port to .auto

* Fix formatting

* Fix pre commit

* [goal_distance_calculator]add readme (autowarefoundation#545)

* add readme

* Update common/util/goal_distance_calculator/Readme.md

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

* fix format

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

* [path_distance_calculator]add readme (autowarefoundation#547)

* add readme of path_distance_calculator

* update readme

* fix readme

* update description

* ci(pre-commit): autofix

Co-authored-by: wep21 <border_goldenmarket@yahoo.co.jp>
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: tkimura4 <tomoya.kimura@tier4.jp>
Co-authored-by: Kazuki Miyahara <kmiya@outlook.com>
Co-authored-by: Yohei Mishina <66298900+YoheiMishina@users.noreply.github.com>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
YoheiMishina pushed a commit to YoheiMishina/autoware.universe that referenced this pull request Jun 9, 2022
* Ros2 v0.8.0 lane departure checker (autowarefoundation#327)

* Add lane departure checker (autowarefoundation#928)

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

* Fix bug of lane_departure_checker (autowarefoundation#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 (autowarefoundation#1026)

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

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

* Fix typo (autowarefoundation#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>
Signed-off-by: tanaka3 <ttatcoder@outlook.jp>

* Ros2 v0.8.0 obstacle collision checker (autowarefoundation#316)

* Feature/obstacle collision checker (autowarefoundation#1063)

* Add template

* Remove unnecessary code

* Add obstacle_collision_checker

* add braking distance

* delete unuse file

* change resample traj

* Format files

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

* Add author

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

* Remove vehicle_footprint visualization

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

* Move package under control/trajectory_follower

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

* Add space

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

* Fix visualization error

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

* Remove comment out lines

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

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

* ros2 porting

* remove "dynamic_reconfigure"

* fix CMAKELists.txt & apply lint

* fix paramCallback

* remove unnecessary comment

* fix include statement

Co-authored-by: Satoshi Tanaka <st14.828soccer@gmail.com>
Co-authored-by: Kenji Miyake <kenji.miyake@tier4.jp>
Signed-off-by: tanaka3 <ttatcoder@outlook.jp>

* Ros2 v0.8.0 fix packages (autowarefoundation#351)

* add subscription to QoS

* add vihicle_param _file to simple_planning_sim

* update cmake/packages.xml

* comment out unused parameter

* apply lint

* add vehicle_info_util to lane_change_planner

* add vehicle_info_util to vehicle_cmd_gate

* fix cmake of simple planning simulator

* update cmake/packages.xml of vehicle cmd gate

* apply lint

* apply lint

* add latch option to autoware_state_monitor

* delete unused comment

Signed-off-by: tanaka3 <ttatcoder@outlook.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>
Signed-off-by: tanaka3 <ttatcoder@outlook.jp>

* Fix/lane departure checker (autowarefoundation#386)

* Fix/lane departure checker (autowarefoundation#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>
Signed-off-by: tanaka3 <ttatcoder@outlook.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>
Signed-off-by: tanaka3 <ttatcoder@outlook.jp>

* Fix typo in control module (autowarefoundation#428)

* Fix typo in control module

* Change admissible_yaw_error to admissible_yaw_error_rad

* Change 90.0 deg to 1.57 rad and remove trailing whitespace

Signed-off-by: tanaka3 <ttatcoder@outlook.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>
Signed-off-by: tanaka3 <ttatcoder@outlook.jp>

* add use_sim-time option (autowarefoundation#454)

Signed-off-by: tanaka3 <ttatcoder@outlook.jp>

* Ros2 delete vehicle info (autowarefoundation#1227)

* remove dependency on vehicle info in autoware_utils

* delete vehicle_info in autoware_utils

Signed-off-by: tanaka3 <ttatcoder@outlook.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>
Signed-off-by: tanaka3 <ttatcoder@outlook.jp>

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

Signed-off-by: tanaka3 <ttatcoder@outlook.jp>

* Make control modules components (autowarefoundation#1262)

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

* Remove use_sim_time for set_parameter (autowarefoundation#1260)

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

* Remove vehicle info param server (autowarefoundation#1304)

* Remove vehicle info param server

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

* Fix ament_uncrustify

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

* Fix ament_uncrustify

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

Co-authored-by: Kenji Miyake <kenji.miyake@tier4.jp>
Signed-off-by: tanaka3 <ttatcoder@outlook.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>
Signed-off-by: tanaka3 <ttatcoder@outlook.jp>

* Fix/fix utils (autowarefoundation#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>
Signed-off-by: tanaka3 <ttatcoder@outlook.jp>

* Initialize Input and Output struct correctly to avoid the undefined behavior (autowarefoundation#1408)

* Initialize Input and Output struct correctly to avoid the undefined behavior

This fixes the following UBSan error:
```
/src/autoware/autoware.iv/control/trajectory_follower/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp:363:15: runtime error: load of value 104, which is not a valid value for type 'bool'
src/autoware/autoware.iv/control/trajectory_follower/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp:358:15: runtime error: load of value 114, which is not a valid value for type 'bool'
```

* Improve how to initialize struct with NSDMI (non-static data member initializer)

* Fix styles

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

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

* Fix -Wunused-parameter (autowarefoundation#1836)

* Fix -Wunused-parameter

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

* Fix mistake

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

* fix spell

* Fix lint issues

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

* Ignore flake8 warnings

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

Co-authored-by: Hiroki OTA <hiroki.ota@tier4.jp>
Signed-off-by: tanaka3 <ttatcoder@outlook.jp>

* Feature/expand footprint (autowarefoundation#1757)

Signed-off-by: tanaka3 <ttatcoder@outlook.jp>

* 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

Signed-off-by: tanaka3 <ttatcoder@outlook.jp>

* Fix processing time output of lane_departure_checker (autowarefoundation#2071)

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

* Fix missing declare_parameter of lane_departure_checker (autowarefoundation#2073)

* Fix missing declare_parameter of lane_departure_checker

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

* Change marker color

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

* reduce lanelet calc of lane_departure_checker (improved logic) (#2102)

* reduce lanelet calc of lane_departure_checker (improved logic)

* fix ament_cpplint error (delete redundant blank line)

* modify createVehiclePassingAreas not to use unnecessary local variable

Signed-off-by: tanaka3 <ttatcoder@outlook.jp>

* 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>
Signed-off-by: tanaka3 <ttatcoder@outlook.jp>

* Add COLCON_IGNORE (autowarefoundation#500)

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

* port obstacle collision checker (autowarefoundation#481)

* port obstacle collision checker

* remove COLCON_IGNORE

* use odometry instead of twist

* rename topic name input/twist -> input/odometry

* add nav_msgs

Co-authored-by: Takayuki Murooka <takayuki.murooka@tier4.jp>
Signed-off-by: tanaka3 <ttatcoder@outlook.jp>

* port lane_departure_checker (autowarefoundation#499)

* Use autoware_auto_msgs

* Fix document

* Remove COLCON_IGNORE

* Use TrajectoryPointArray for resampling

* Rename TrajectoryPointArray to TrajectoryPoints, fix order of member variable

Signed-off-by: tanaka3 <ttatcoder@outlook.jp>

* add readme to obstacle collision checker (autowarefoundation#541)

* add readme to obstacle collision checker

* fix spelling

* Format Doc

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

* Fix Typo

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

* Remove Duplicated

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

* update readme

* To publish diag error

* Update readme

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

* fix uml

Co-authored-by: Kazuki Miyahara <kmiya@outlook.com>
Co-authored-by: tkimura4 <tomoya.kimura@tier4.jp>
Signed-off-by: tanaka3 <ttatcoder@outlook.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>
Signed-off-by: tanaka3 <ttatcoder@outlook.jp>

* Fix message interface and tests of 'trajectory_follower_nodes' (autowarefoundation#617)

* Update longitudinal_controller_node to use VehicleOdometry

* Update lateral_controller_node for VehicleOdometry and SteeringReport

* Fix tests

Signed-off-by: tanaka3 <ttatcoder@outlook.jp>

* Update lateral controller (autowarefoundation#708)

* Fix parameter names of mpc_follower (autowarefoundation#1376)

* remove yaw_recalc param in mpc (autowarefoundation#1241) (autowarefoundation#1476)

* parameterize curvature num (autowarefoundation#1674) (autowarefoundation#1577)

* fix rosparam steer_rate_lim_degs to steer_rate_lim_dps in mpc_follower (autowarefoundation#1848)

* Fix spellcheck fail for some packages autowarefoundation#1842

* use interpolation::slerp (autowarefoundation#2161)

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

* add add guard (autowarefoundation#2184)

* add-mpc-optimization-status-print (autowarefoundation#2189)

* Apply ament_uncrustify

* Update control/trajectory_follower/src/qp_solver/qp_solver_osqp.cpp

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

Co-authored-by: Takamasa Horibe <horibe.takamasa@gmail.com>
Signed-off-by: tanaka3 <ttatcoder@outlook.jp>

* velocity_controller sync to .iv develop latest (autowarefoundation#699)

* non extrapolate velocity in lerpTrajectory to avoid negative velocity just before vehicle stops (autowarefoundation#2033)

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

* Add keep braking function at driving state

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

* Remove debug messages

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

* Fix format

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

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

* Add the description of brake keeping

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

* Add the english document

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

* Improve description

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

* Add english description

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

Co-authored-by: Takayuki Murooka <takayuki5168@gmail.com>
Co-authored-by: Makoto Kurihara <mkuri8m@gmail.com>
Co-authored-by: Fumiya Watanabe <rej55.g@gmail.com>
Signed-off-by: tanaka3 <ttatcoder@outlook.jp>

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: Shinnosuke Hirakawa <8327162+0x126@users.noreply.github.com>
Co-authored-by: Satoshi Tanaka <st14.828soccer@gmail.com>
Co-authored-by: Kenji Miyake <kenji.miyake@tier4.jp>
Co-authored-by: tkimura4 <tomoya.kimura@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: Kazuki Miyahara <kmiya@outlook.com>
Co-authored-by: Kosuke Murakami <kosuke.murakami@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: Hiroki OTA <hiroki.ota@tier4.jp>
Co-authored-by: Keisuke Shima <19993104+KeisukeShima@users.noreply.github.com>
Co-authored-by: v-hara8206-esol <88299881+v-hara8206-esol@users.noreply.github.com>
Co-authored-by: Takayuki Murooka <takayuki5168@gmail.com>
Co-authored-by: Takayuki Murooka <takayuki.murooka@tier4.jp>
Co-authored-by: Fumiya Watanabe <rej55.g@gmail.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: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com>
Co-authored-by: Makoto Kurihara <mkuri8m@gmail.com>
@djargot djargot force-pushed the feature/add-localization-metrics-calculation branch 3 times, most recently from 770d5e1 to 65e32fd Compare July 19, 2022 17:39
@WJaworskiRobotec WJaworskiRobotec self-requested a review July 19, 2022 17:59
@codecov
Copy link

codecov bot commented Jul 19, 2022

Codecov Report

Merging #928 (4a9d682) into main (8433c13) will increase coverage by 0.17%.
The diff coverage is 47.86%.

@@           Coverage Diff            @@
##            main    #928      +/-   ##
========================================
+ Coverage   9.66%   9.83%   +0.17%     
========================================
  Files       1099    1113      +14     
  Lines      77299   77650     +351     
  Branches   17789   17972     +183     
========================================
+ Hits        7468    7636     +168     
- Misses     62551   62585      +34     
- Partials    7280    7429     +149     
Flag Coverage Δ *Carryforward flag
differential 47.86% <47.86%> (?)
total 9.64% <ø> (ø) Carriedforward from 53cf1ba

*This pull request uses carry forward flags. Click here to find out more.

Impacted Files Coverage Δ
...aluator/include/kinematic_evaluator/parameters.hpp 0.00% <0.00%> (ø)
...ator/include/localization_evaluator/parameters.hpp 0.00% <0.00%> (ø)
...tor/kinematic_evaluator/src/metrics_calculator.cpp 37.50% <37.50%> (ø)
...nematic_evaluator/src/kinematic_evaluator_node.cpp 40.54% <40.54%> (ø)
.../localization_evaluator/src/metrics_calculator.cpp 42.85% <42.85%> (ø)
...c_evaluator/test/test_kinematic_evaluator_node.cpp 43.10% <43.10%> (ø)
...tion_evaluator/src/localization_evaluator_node.cpp 48.52% <48.52%> (ø)
...valuator/test/test_localization_evaluator_node.cpp 48.61% <48.61%> (ø)
..._evaluator/include/localization_evaluator/stat.hpp 56.25% <56.25%> (ø)
...tor/include/kinematic_evaluator/metrics/metric.hpp 60.00% <60.00%> (ø)
... and 4 more

Continue to review full report at Codecov.

Legend - Click here to learn more
Δ = absolute <relative> (impact), ø = not affected, ? = missing data
Powered by Codecov. Last update 8433c13...4a9d682. Read the comment docs.

Copy link
Contributor

@WJaworskiRobotec WJaworskiRobotec left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Looks good for now. Some refactor will be needed after adding more evaluators

@djargot djargot force-pushed the feature/add-localization-metrics-calculation branch from c8ffcba to 4fd06a3 Compare July 20, 2022 11:25
djargot and others added 10 commits July 20, 2022 13:31
Signed-off-by: djargot <dominik.jargot@robotec.ai>
Signed-off-by: djargot <dominik.jargot@robotec.ai>
Signed-off-by: djargot <dominik.jargot@robotec.ai>
Signed-off-by: djargot <dominik.jargot@robotec.ai>
Signed-off-by: djargot <dominik.jargot@robotec.ai>
Signed-off-by: djargot <dominik.jargot@robotec.ai>
Signed-off-by: djargot <dominik.jargot@robotec.ai>
Signed-off-by: djargot <dominik.jargot@robotec.ai>
Signed-off-by: djargot <dominik.jargot@robotec.ai>
Signed-off-by: djargot <dominik.jargot@robotec.ai>
@djargot djargot force-pushed the feature/add-localization-metrics-calculation branch from 4fd06a3 to 53cf1ba Compare July 20, 2022 11:31
@djargot djargot changed the title [WIP] feat - Add various evaluators with metrics feat - Add kinematic and localization evaluators with metrics Jul 20, 2022
Signed-off-by: djargot <dominik.jargot@robotec.ai>
@djargot djargot changed the title feat - Add kinematic and localization evaluators with metrics feat(Add kinematic and localization evaluators with metrics) Jul 20, 2022
@djargot djargot changed the title feat(Add kinematic and localization evaluators with metrics) feat(metrics_calculation): Add kinematic and localization evaluators with metrics Jul 20, 2022
@djargot djargot changed the title feat(metrics_calculation): Add kinematic and localization evaluators with metrics feat(metrics_calculation): add kinematic and localization evaluators with metrics Jul 20, 2022
@WJaworskiRobotec WJaworskiRobotec merged commit 314a34c into autowarefoundation:main Jul 20, 2022
@taikitanaka3
Copy link
Contributor

taikitanaka3 commented Aug 3, 2022

@WJaworskiRobotec @djargot
Could you please add one of tier4 members as reviewer when you add a huge commit?
@mitsudome-r @yukkysaito @TakaHoribe

@mitsudome-r
Copy link
Member

mitsudome-r commented Aug 3, 2022

@taikitanaka3
This is not a pull request to modify TIER IV author's code, and I don't see the reason why there needs a review from TIER IV. Please be aware that TIER IV is not an owner of Autoware.Universe. The PR was there for over a month until it was merged, and if you have any concerns, you should've assigned yourself as a reviewer to the PR.

@WJaworskiRobotec
Copy link
Contributor

@taikitanaka3 this commit is about adding entirely new component according to the task from the Simulation WG - not used by Autoware stack by default, only for simulation based testing. We will create further PRs for perception-related evaluators, @mitsudome-r should we add someone else as reviewers (@djargot is working on the task so I can review it if it's not a problem)?

@taikitanaka3
Copy link
Contributor

taikitanaka3 commented Aug 3, 2022

@WJaworskiRobotec
Thank you for contribution.
My suggestion is to add architects as a reviewer to discuss with code base.
but not always requirement.

@WJaworskiRobotec
Copy link
Contributor

@taikitanaka3 sure, we might discuss the structure a bit later - we had plan to also merge existing evaluators into one package. Then we might need discussion with the architects where to put our proposal of evaluators.

boyali referenced this pull request in boyali/autoware.universe Sep 28, 2022
…with metrics (tier4#928)

* initial skeleton of localization evaluator

Signed-off-by: djargot <dominik.jargot@robotec.ai>

* Add simple localization evaliuation framework

Signed-off-by: djargot <dominik.jargot@robotec.ai>

* Clean and add tests

Signed-off-by: djargot <dominik.jargot@robotec.ai>

* ci(pre-commit): autofix

Signed-off-by: djargot <dominik.jargot@robotec.ai>

* Clean localization evaluator

Signed-off-by: djargot <dominik.jargot@robotec.ai>

* Update kinematic evaluator

Signed-off-by: djargot <dominik.jargot@robotec.ai>

* ci(pre-commit): autofix

Signed-off-by: djargot <dominik.jargot@robotec.ai>

* dependency fix

Signed-off-by: djargot <dominik.jargot@robotec.ai>

* Fix localization evaluator tests

Signed-off-by: djargot <dominik.jargot@robotec.ai>

* ci(pre-commit): autofix

Signed-off-by: djargot <dominik.jargot@robotec.ai>

* Add missing includes and remove unnecessary quotes

Signed-off-by: djargot <dominik.jargot@robotec.ai>

Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
Co-authored-by: Wojciech Jaworski <wojciech.jaworski@robotec.ai>
boyali referenced this pull request in boyali/autoware.universe Oct 3, 2022
…with metrics (tier4#928)

* initial skeleton of localization evaluator

Signed-off-by: djargot <dominik.jargot@robotec.ai>

* Add simple localization evaliuation framework

Signed-off-by: djargot <dominik.jargot@robotec.ai>

* Clean and add tests

Signed-off-by: djargot <dominik.jargot@robotec.ai>

* ci(pre-commit): autofix

Signed-off-by: djargot <dominik.jargot@robotec.ai>

* Clean localization evaluator

Signed-off-by: djargot <dominik.jargot@robotec.ai>

* Update kinematic evaluator

Signed-off-by: djargot <dominik.jargot@robotec.ai>

* ci(pre-commit): autofix

Signed-off-by: djargot <dominik.jargot@robotec.ai>

* dependency fix

Signed-off-by: djargot <dominik.jargot@robotec.ai>

* Fix localization evaluator tests

Signed-off-by: djargot <dominik.jargot@robotec.ai>

* ci(pre-commit): autofix

Signed-off-by: djargot <dominik.jargot@robotec.ai>

* Add missing includes and remove unnecessary quotes

Signed-off-by: djargot <dominik.jargot@robotec.ai>

Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
Co-authored-by: Wojciech Jaworski <wojciech.jaworski@robotec.ai>
boyali referenced this pull request in boyali/autoware.universe Oct 3, 2022
…with metrics (tier4#928)

* initial skeleton of localization evaluator

Signed-off-by: djargot <dominik.jargot@robotec.ai>

* Add simple localization evaliuation framework

Signed-off-by: djargot <dominik.jargot@robotec.ai>

* Clean and add tests

Signed-off-by: djargot <dominik.jargot@robotec.ai>

* ci(pre-commit): autofix

Signed-off-by: djargot <dominik.jargot@robotec.ai>

* Clean localization evaluator

Signed-off-by: djargot <dominik.jargot@robotec.ai>

* Update kinematic evaluator

Signed-off-by: djargot <dominik.jargot@robotec.ai>

* ci(pre-commit): autofix

Signed-off-by: djargot <dominik.jargot@robotec.ai>

* dependency fix

Signed-off-by: djargot <dominik.jargot@robotec.ai>

* Fix localization evaluator tests

Signed-off-by: djargot <dominik.jargot@robotec.ai>

* ci(pre-commit): autofix

Signed-off-by: djargot <dominik.jargot@robotec.ai>

* Add missing includes and remove unnecessary quotes

Signed-off-by: djargot <dominik.jargot@robotec.ai>

Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
Co-authored-by: Wojciech Jaworski <wojciech.jaworski@robotec.ai>
yukke42 pushed a commit to tzhong518/autoware.universe that referenced this pull request Oct 14, 2022
…with metrics (autowarefoundation#928)

* initial skeleton of localization evaluator

Signed-off-by: djargot <dominik.jargot@robotec.ai>

* Add simple localization evaliuation framework

Signed-off-by: djargot <dominik.jargot@robotec.ai>

* Clean and add tests

Signed-off-by: djargot <dominik.jargot@robotec.ai>

* ci(pre-commit): autofix

Signed-off-by: djargot <dominik.jargot@robotec.ai>

* Clean localization evaluator

Signed-off-by: djargot <dominik.jargot@robotec.ai>

* Update kinematic evaluator

Signed-off-by: djargot <dominik.jargot@robotec.ai>

* ci(pre-commit): autofix

Signed-off-by: djargot <dominik.jargot@robotec.ai>

* dependency fix

Signed-off-by: djargot <dominik.jargot@robotec.ai>

* Fix localization evaluator tests

Signed-off-by: djargot <dominik.jargot@robotec.ai>

* ci(pre-commit): autofix

Signed-off-by: djargot <dominik.jargot@robotec.ai>

* Add missing includes and remove unnecessary quotes

Signed-off-by: djargot <dominik.jargot@robotec.ai>

Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
Co-authored-by: Wojciech Jaworski <wojciech.jaworski@robotec.ai>
boyali referenced this pull request in boyali/autoware.universe Oct 19, 2022
…with metrics (tier4#928)

* initial skeleton of localization evaluator

Signed-off-by: djargot <dominik.jargot@robotec.ai>

* Add simple localization evaliuation framework

Signed-off-by: djargot <dominik.jargot@robotec.ai>

* Clean and add tests

Signed-off-by: djargot <dominik.jargot@robotec.ai>

* ci(pre-commit): autofix

Signed-off-by: djargot <dominik.jargot@robotec.ai>

* Clean localization evaluator

Signed-off-by: djargot <dominik.jargot@robotec.ai>

* Update kinematic evaluator

Signed-off-by: djargot <dominik.jargot@robotec.ai>

* ci(pre-commit): autofix

Signed-off-by: djargot <dominik.jargot@robotec.ai>

* dependency fix

Signed-off-by: djargot <dominik.jargot@robotec.ai>

* Fix localization evaluator tests

Signed-off-by: djargot <dominik.jargot@robotec.ai>

* ci(pre-commit): autofix

Signed-off-by: djargot <dominik.jargot@robotec.ai>

* Add missing includes and remove unnecessary quotes

Signed-off-by: djargot <dominik.jargot@robotec.ai>

Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
Co-authored-by: Wojciech Jaworski <wojciech.jaworski@robotec.ai>
YoshiRi pushed a commit to YoshiRi/autoware.universe that referenced this pull request Nov 16, 2023
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

Successfully merging this pull request may close these issues.

4 participants