Skip to content

Commit

Permalink
feat: trajectory follower libraries (autowarefoundation#83)
Browse files Browse the repository at this point in the history
* 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>
  • Loading branch information
1 parent 1fbe6cf commit dfe48bd
Show file tree
Hide file tree
Showing 45 changed files with 6,646 additions and 0 deletions.
121 changes: 121 additions & 0 deletions control/trajectory_follower/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -0,0 +1,121 @@
# Copyright 2021 The Autoware Foundation
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
#    http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.

cmake_minimum_required(VERSION 3.5)

project(trajectory_follower)

# require that dependencies from package.xml be available
find_package(ament_cmake_auto REQUIRED)
ament_auto_find_build_dependencies(REQUIRED
${${PROJECT_NAME}_BUILD_DEPENDS}
${${PROJECT_NAME}_BUILDTOOL_DEPENDS}
)

# lateral_controller
set(LATERAL_CONTROLLER_LIB lateral_controller_lib)
set(LATERAL_CONTROLLER_LIB_SRC
src/interpolate.cpp
src/lowpass_filter.cpp
src/mpc.cpp
src/mpc_trajectory.cpp
src/mpc_utils.cpp
src/qp_solver/qp_solver_osqp.cpp
src/qp_solver/qp_solver_unconstr_fast.cpp
src/vehicle_model/vehicle_model_bicycle_dynamics.cpp
src/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.cpp
src/vehicle_model/vehicle_model_bicycle_kinematics.cpp
src/vehicle_model/vehicle_model_interface.cpp
)

set(LATERAL_CONTROLLER_LIB_HEADERS
include/trajectory_follower/visibility_control.hpp
include/trajectory_follower/interpolate.hpp
include/trajectory_follower/lowpass_filter.hpp
include/trajectory_follower/mpc.hpp
include/trajectory_follower/mpc_trajectory.hpp
include/trajectory_follower/mpc_utils.hpp
include/trajectory_follower/qp_solver/qp_solver_interface.hpp
include/trajectory_follower/qp_solver/qp_solver_osqp.hpp
include/trajectory_follower/qp_solver/qp_solver_unconstr_fast.hpp
include/trajectory_follower/vehicle_model/vehicle_model_bicycle_dynamics.hpp
include/trajectory_follower/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp
include/trajectory_follower/vehicle_model/vehicle_model_bicycle_kinematics.hpp
include/trajectory_follower/vehicle_model/vehicle_model_interface.hpp
)

# generate library
ament_auto_add_library(${LATERAL_CONTROLLER_LIB} SHARED
${LATERAL_CONTROLLER_LIB_SRC}
${LATERAL_CONTROLLER_LIB_HEADERS}
)
autoware_set_compile_options(${LATERAL_CONTROLLER_LIB})
target_compile_options(${LATERAL_CONTROLLER_LIB} PRIVATE -Wno-error=old-style-cast)

# longitudinal_controller
set(LONGITUDINAL_CONTROLLER_LIB longitudinal_controller_lib)
set(LONGITUDINAL_CONTROLLER_LIB_SRC
src/pid.cpp
src/smooth_stop.cpp
src/longitudinal_controller_utils.cpp
)

set(LONGITUDINAL_CONTROLLER_LIB_HEADERS
include/trajectory_follower/debug_values.hpp
include/trajectory_follower/pid.hpp
include/trajectory_follower/smooth_stop.hpp
include/trajectory_follower/longitudinal_controller_utils.hpp
)

# generate library
ament_auto_add_library(${LONGITUDINAL_CONTROLLER_LIB} SHARED
${LONGITUDINAL_CONTROLLER_LIB_SRC}
${LONGITUDINAL_CONTROLLER_LIB_HEADERS}
)
autoware_set_compile_options(${LONGITUDINAL_CONTROLLER_LIB})
target_compile_options(${LONGITUDINAL_CONTROLLER_LIB} PRIVATE -Wno-error=old-style-cast)

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

# Unit tests
set(TEST_LAT_SOURCES
test/test_mpc.cpp
test/test_mpc_trajectory.cpp
test/test_mpc_utils.cpp
test/test_interpolate.cpp
test/test_lowpass_filter.cpp
)
set(TEST_LATERAL_CONTROLLER_EXE test_lateral_controller)
ament_add_gtest(${TEST_LATERAL_CONTROLLER_EXE} ${TEST_LAT_SOURCES})
autoware_set_compile_options(${TEST_LATERAL_CONTROLLER_EXE})
target_link_libraries(${TEST_LATERAL_CONTROLLER_EXE} ${LATERAL_CONTROLLER_LIB})

set(TEST_LON_SOURCES
test/test_debug_values.cpp
test/test_pid.cpp
test/test_smooth_stop.cpp
test/test_longitudinal_controller_utils.cpp
)
set(TEST_LONGITUDINAL_CONTROLLER_EXE test_longitudinal_controller)
ament_add_gtest(${TEST_LONGITUDINAL_CONTROLLER_EXE} ${TEST_LON_SOURCES})
autoware_set_compile_options(${TEST_LONGITUDINAL_CONTROLLER_EXE})
target_link_libraries(${TEST_LONGITUDINAL_CONTROLLER_EXE} ${LONGITUDINAL_CONTROLLER_LIB})
endif()

# ament package generation and installing
ament_auto_package(INSTALL_TO_SHARE
)
21 changes: 21 additions & 0 deletions control/trajectory_follower/design/trajectory_follower-design.md
Original file line number Diff line number Diff line change
@@ -0,0 +1,21 @@
Trajectory Follower {#trajectory_follower-package-design}
===========

This is the design document for the `trajectory_follower` package.


# Purpose / Use cases
<!-- Required -->
<!-- Things to consider:
- Why did we implement this feature? -->
This package provides the library code used by the nodes of the `trajectory_follower_nodes` package.
Mainly, it implements two algorithms:
- Model-Predictive Control (MPC) for the computation of lateral steering commands.
- @subpage trajectory_follower-mpc-design
- PID control for the computation of velocity and acceleration commands.
- @subpage trajectory_follower-pid-design

# Related issues
<!-- Required -->
- https://gitlab.com/autowarefoundation/autoware.auto/AutowareAuto/-/issues/1057
- https://gitlab.com/autowarefoundation/autoware.auto/AutowareAuto/-/issues/1058
Original file line number Diff line number Diff line change
@@ -0,0 +1,65 @@
MPC (Trajectory Follower) {#trajectory_follower-mpc-design}
===========

This is the design document for the MPC implemented in the `trajectory_follower` package.

# Purpose / Use cases
<!-- Required -->
<!-- Things to consider:
- Why did we implement this feature? -->
Model Predictive Control (MPC) is used by the `trajectory_follower`
to calculate the lateral commands corresponding to a steering angle and a steering rate.

This implementation differs from the one in the `mpc_controller` package in several aspects.
- This is a linear MPC that only computes the steering command whereas
the `mpc_controller` uses a non-linear MPC which calculates coupled steering and velocity commands.
- The optimization problem solved by the linear MPC is simpler, making it less likely to fail.
- Tuning of the linear MPC is easier.

# Design
<!-- Required -->
<!-- Things to consider:
- How does it work? -->
MPC uses predictions of the vehicle's motion to optimize the immediate control command.

Different vehicle models are implemented:
- `kinematics` : bicycle kinematics model with steering 1st-order delay.
- `kinematics_no_delay` : bicycle kinematics model without steering delay.
- `dynamics` : bicycle dynamics model considering slip angle.

The `kinematics` model is being used by default. Please see the reference [1] for more details.


For the optimization, a Quadratric Programming (QP) solver is used
with two options are currently implemented:
- `unconstraint` : use least square method to solve unconstraint QP with eigen.
- `unconstraint_fast` : similar to unconstraint. This is faster, but lower accuracy for optimization.

## Filtering

Filtering is required for good noise reduction.
A [Butterworth filter](https://en.wikipedia.org/wiki/Butterworth_filter) is used for the yaw and lateral errors used as input of the MPC as well as for
the output steering angle.
Other filtering methods can be considered as long as the noise reduction performances are good
enough.
The moving average filter for example is not suited and can yield worse results than without any
filtering.

## Inputs / Outputs / API
<!-- Required -->
<!-- Things to consider:
- How do you use the package / API? -->
The `MPC` class (defined in `mpc.hpp`) provides the interface with the MPC algorithm.
Once a vehicle model, a QP solver, and the reference trajectory to follow have been set
(using `setVehicleModel()`, `setQPSolver()`, `setReferenceTrajectory()`), a lateral control command
can be calculated by providing the current steer, velocity, and pose to function `calculateMPC()`.

# References / External links
<!-- Optional -->
- [1] Jarrod M. Snider, "Automatic Steering Methods for Autonomous Automobile Path Tracking",
Robotics Institute, Carnegie Mellon University, February 2009.

# Related issues
<!-- Required -->
- https://gitlab.com/autowarefoundation/autoware.auto/AutowareAuto/-/issues/1057
- https://gitlab.com/autowarefoundation/autoware.auto/AutowareAuto/-/issues/1058
Original file line number Diff line number Diff line change
@@ -0,0 +1,88 @@
PID (Trajectory Follower) {#trajectory_follower-pid-design}
===========

This is the design document for the PID implemented in the `trajectory_follower` package.

# Purpose / Use cases
<!-- Required -->
<!-- Things to consider:
- Why did we implement this feature? -->
PID control is used by the `trajectory_follower`
to calculate longitudinal commands corresponding to a velocity and an acceleration.

# Design
<!-- Required -->
<!-- Things to consider:
- How does it work? -->
This PID control calculates the target acceleration from the deviation between the current ego-velocity and the target velocity.

This PID logic has a maximum value for the output of each term. This is to prevent the following:

- Large integral terms may cause unintended behavior by users.
- Unintended noise may cause the output of the derivative term to be very large.

Also, the integral term is not accumulated when the vehicle is stopped. This is to prevent unintended accumulation of the integral term in cases such as "Autoware assumes that the vehicle is engaged, but an external system has locked the vehicle to start.
On the other hand, if the vehicle gets stuck in a depression in the road surface when starting, the vehicle will not start forever, which is currently being addressed by developers.

At present, PID control is implemented from the viewpoint of trade-off between development/maintenance cost and performance.
This may be replaced by a higher performance controller (adaptive control or robust control) in future development.

@image html images/trajectory_follower-pid-diagram.svg "PID controller diagram"

## States

This module has four state transitions as shown below in order to handle special processing in a specific situation.

- **DRIVE**
- Executes target velocity tracking by PID control.
- It also applies the delay compensation and slope compensation.
- **STOPPING**
- Controls the motion just before stopping.
- Special sequence is performed to achieve accurate and smooth stopping.
- **STOPPED**
- Performs operations in the stopped state (e.g. brake hold)
- **EMERGENCY**.
- Enters an emergency state when certain conditions are met (e.g., when the vehicle has crossed a certain distance of a stop line).
- The recovery condition (whether or not to keep emergency state until the vehicle completely stops) or the deceleration in the emergency state are defined by parameters.

The state transition diagram is shown below.

@image html images/trajectory_follower-pid-states-diagram.svg "State Transitions"

## Time delay compensation

At high speeds, the delay of actuator systems such as gas pedals and brakes has a significant impact on driving accuracy.
Depending on the actuating principle of the vehicle,
the mechanism that physically controls the gas pedal and brake typically has a delay of about a hundred millisecond.

In this controller,
the predicted ego-velocity and the target velocity after the delay time are calculated and used for the feedback to address the time delay problem.

## Slope compensation

Based on the slope information, a compensation term is added to the target acceleration.

There are two sources of the slope information, which can be switched by a parameter.

- Pitch of the estimated ego-pose (default)
- Calculates the current slope from the pitch angle of the estimated ego-pose
- Pros: Easily available
- Cons: Cannot extract accurate slope information due to the influence of vehicle vibration.
- Z coordinate on the trajectory
- Calculates the road slope from the difference of z-coordinates between the front and rear wheel positions in the target trajectory
- Pros: More accurate than pitch information, if the z-coordinates of the route are properly maintained
- Pros: Can be used in combination with delay compensation (not yet implemented)
- Cons: z-coordinates of high-precision map is needed.
- Cons: Does not support free space planning (for now)

## Inputs / Outputs / API
<!-- Required -->
<!-- Things to consider:
- How do you use the package / API? -->
The `PIDController` class is straightforward to use.
First, gains and limits must be set (using `setGains()` and `setLimits()`) for the proportional (P), integral (I), and derivative (D) components.
Then, the velocity can be calculated by providing the current error and time step duration to the `calculate()` function.

# Related issues
<!-- Required -->
- https://gitlab.com/autowarefoundation/autoware.auto/AutowareAuto/-/issues/1058
Loading

0 comments on commit dfe48bd

Please sign in to comment.