Skip to content

Commit

Permalink
feat: add awapi adapter package (#103)
Browse files Browse the repository at this point in the history
* Feature/awapi awiv adapter first pr (#685)

* add base file of awapi_awiv_adapter

* publish autoware status

* update readme

* fix readme

* rename file

* add relay topic

* change msg name

* publish autoware status

* update readme

* add new message

* add lane change topic, obstacle avoidance topic

* update readme

* fix readme

* add namespace

* rename lane change available

* fix readme

* change pub hz

* change topic name

* change control mode and add gate_mode

* fix readme

* update readme

Signed-off-by: tanaka3 <[email protected]>

* fix typo (#687)

Signed-off-by: tanaka3 <[email protected]>

* move msg file to autoware_api_msgs (#693)

Signed-off-by: tanaka3 <[email protected]>

* Fix/awapi awiv adapter (#698)

* subscribe global rpt

* update readme

* update readme

* add warning

* fix readme

* update readme

Signed-off-by: tanaka3 <[email protected]>

* add check mark for awapi readme (#723)

* add check mark for awapi readme

* add blank

* add blank

* add blank

Signed-off-by: tanaka3 <[email protected]>

* Feature/stop reason api (#729)

* fix indent of readme

* add mock of stop reason aggregator

* add constptr to stop reason

* aggregate message

* add empty handling

* add nullptr handling

* fix bug

* update readme

* fix bug

* remove unused ifdef

* unify same reason msg to stop factor

Signed-off-by: tanaka3 <[email protected]>

* modify put/goal -> put/route (#736)

Signed-off-by: tanaka3 <[email protected]>

* Feature/add goal api (#734)

* add  goal api

* add msg

* update readme

* add  const auto

* clean code

Signed-off-by: tanaka3 <[email protected]>

* Feature/extract leaf diag info (#741)

* add mock function of extract leaf diag

* remove parent diag

* change method of isLeaf

* clean code

* change varibale/function name

* remove string predefine

* update readme

Signed-off-by: tanaka3 <[email protected]>

* update awapi readme (#747)

Signed-off-by: tanaka3 <[email protected]>

* Feature/change output awapi (#782)

* change ouput of awapi

* change error output to warn

Signed-off-by: tanaka3 <[email protected]>

* Feature/add battery to awapi (#804)

* implement battery fuel

* update readme

* add getGearInfo

* fix message name

* set default value:nan to battery

Signed-off-by: tanaka3 <[email protected]>

* remove ROS1 packages temporarily

Signed-off-by: mitsudome-r <[email protected]>
Signed-off-by: tanaka3 <[email protected]>

* Revert "remove ROS1 packages temporarily"

This reverts commit c85c1d6789b1a1e63c6b4e60fa4f3296dfb12e74.

Signed-off-by: mitsudome-r <[email protected]>
Signed-off-by: tanaka3 <[email protected]>

* add COLCON_IGNORE to ros1 packages

Signed-off-by: mitsudome-r <[email protected]>
Signed-off-by: tanaka3 <[email protected]>

* Rename launch files to launch.xml (#28)

Signed-off-by: tanaka3 <[email protected]>

* Port awapi_awiv_adapter to ROS2 (#51)

* Port autoware_api_msgs to ROS2

* Integrate topic_tools

* Don't inherit from Node, convert to milliseconds

* Remove _node suffix from non-node loggers

Signed-off-by: tanaka3 <[email protected]>

* Rename h files to hpp (#142)

* Change includes

* Rename files

* Adjustments to make things compile

* Other packages

Signed-off-by: tanaka3 <[email protected]>

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

Signed-off-by: tanaka3 <[email protected]>

* Use quotes for includes where appropriate (#144)

* Use quotes for includes where appropriate

* Fix lint tests

* Make tests pass hopefully

Signed-off-by: tanaka3 <[email protected]>

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

* Run uncrustify on the entire Pilot.Auto codebase

* Exclude open PRs

Signed-off-by: tanaka3 <[email protected]>

* Add linters (#227)

Signed-off-by: tanaka3 <[email protected]>

* Ros2 v0.8.0 awapi awiv adapter (#282)

Signed-off-by: tanaka3 <[email protected]>

* Ros2 v0.8.0 engage (#342)

* [autoware_vehicle_msgs]: Add engage message

Signed-off-by: wep21 <[email protected]>

* [as]: Update message

Signed-off-by: wep21 <[email protected]>

* [awapi_awiv_adapter]: Update message

Signed-off-by: wep21 <[email protected]>

* [web_controller]: Update message

Signed-off-by: wep21 <[email protected]>

* [vehicle_cmd_gate]: Update message

Signed-off-by: wep21 <[email protected]>

* [autoware_state_monitor]: Update message

Signed-off-by: wep21 <[email protected]>

* [autoware_control_msgs]: Remove EngageMode message

Signed-off-by: wep21 <[email protected]>

* [simple_planning_simulator]: Update message

Signed-off-by: wep21 <[email protected]>
Signed-off-by: tanaka3 <[email protected]>

* Ros2 v0.8.0 remove std msgs awapi (#348)

* [autoware_vehicle_msgs] add BatteryStatus msg

Signed-off-by: mitsudome-r <[email protected]>

* [autoware_planning_msgs] add ExpandStopRange and StopSpeedExceeded messages

Signed-off-by: mitsudome-r <[email protected]>

* [autoware_api_msgs] add DoorControlCommand, StopCommand, and VelocityLimit messages

Signed-off-by: mitsudome-r <[email protected]>

* remove std_msgs related to autoware_awaiv_adapter node

Signed-off-by: mitsudome-r <[email protected]>

* apply ament_uncrustify

Signed-off-by: mitsudome-r <[email protected]>

* fix build failure

Signed-off-by: mitsudome-r <[email protected]>

* fix test failures

Signed-off-by: mitsudome-r <[email protected]>

* address review commends

Signed-off-by: mitsudome-r <[email protected]>
Signed-off-by: tanaka3 <[email protected]>

* Awapi update v0.9.0 (#350)

* Feature/add current maxvel to awapi (#1142)

* add current max vel to awapi

* add readme

* apply format

Signed-off-by: mitsudome-r <[email protected]>

* fix build error

Signed-off-by: mitsudome-r <[email protected]>

* Add missing return (#1170)

Signed-off-by: Kenji Miyake <[email protected]>

* fix cast type

Signed-off-by: mitsudome-r <[email protected]>

Co-authored-by: Kenji Miyake <[email protected]>
Signed-off-by: tanaka3 <[email protected]>

* [awapi_awiv_adapter] fix bug in the launch file (#391)

Signed-off-by: mitsudome-r <[email protected]>
Signed-off-by: tanaka3 <[email protected]>

* modify pose_initialize for foa  (#382)

* pose_initialize for foa (#1171)

* pose_initialize for foa

Signed-off-by: Yamato ANDO <[email protected]>

* rename topic

Signed-off-by: Yamato ANDO <[email protected]>

* fix topic name

Signed-off-by: mitsudome-r <[email protected]>
Signed-off-by: tanaka3 <[email protected]>

* fix topic name (#404)

Signed-off-by: tanaka3 <[email protected]>

* [awapi_awiv_adapter] fix qos for some of the topics in awapi_awiv_adapter (#413)

Signed-off-by: mitsudome-r <[email protected]>
Signed-off-by: tanaka3 <[email protected]>

* Fix transform (#420)

* Replace rclcpp::Time(0) by tf2::TimePointZero() in lookupTransform

Signed-off-by: Kenji Miyake <[email protected]>

* Fix canTransform

Signed-off-by: Kenji Miyake <[email protected]>

* Fix test

Signed-off-by: Kenji Miyake <[email protected]>
Signed-off-by: tanaka3 <[email protected]>

* Fix typo in awapi module (#438)

Signed-off-by: tanaka3 <[email protected]>

* Import v0.9.1 (#431)

* add local optimal solution ocillation check to ndt_scan_matcher (#1182)

* Add obstacle_crush diagnostic (#1186)

Signed-off-by: Kenji Miyake <[email protected]>

* Fix diagnostics api (#1185)

* Fix diagnostics api

Signed-off-by: Kenji Miyake <[email protected]>

* Don't overwrite level

Signed-off-by: Kenji Miyake <[email protected]>

* Overwrite level of No Fault diagnostics

Signed-off-by: Kenji Miyake <[email protected]>

* Add missing diag in autoware_error_monitor.yaml (#1187)

Signed-off-by: Kenji Miyake <[email protected]>

* Filter hazard_status (#1191)

* Filter hazard_status

Signed-off-by: Kenji Miyake <[email protected]>

* Filter leaf diagnostics

Signed-off-by: Kenji Miyake <[email protected]>

* Fix wrong calculation of available memory. (#1168)

* Fixed wrong calculation of available memory.

* Added comments about output example of free -tb command.

* Change monitoring method to get HDD temperature and usage per specified device. (#1195)

* Changed monitoring method to get temperature and usage per specified device.

* Fixed test codes.

* Removed unnecessary (void) parameter.

* return input pointcloud when ground plane not found (#1190)

* fix yaw-smoothing bug (#1198)

* Fix lint

Signed-off-by: Kenji Miyake <[email protected]>

Co-authored-by: Taichi Higashide <[email protected]>
Co-authored-by: ito-san <[email protected]>
Co-authored-by: tkimura4 <[email protected]>
Signed-off-by: tanaka3 <[email protected]>

* add tl overwriter relay in awapi (#451)

* add tl overwriter relay in awapi

* add missed topic type

* fix typo of topic name

Signed-off-by: tanaka3 <[email protected]>

* add use_sim-time option (#454)

Signed-off-by: tanaka3 <[email protected]>

* Format launch files (#1219)

Signed-off-by: Kenji Miyake <[email protected]>
Signed-off-by: tanaka3 <[email protected]>

* Fix rolling build errors (#1225)

* Add missing include files

Signed-off-by: Kenji Miyake <[email protected]>

* Replace rclcpp::Duration

Signed-off-by: Kenji Miyake <[email protected]>

* Use reference for exceptions

Signed-off-by: Kenji Miyake <[email protected]>

* Use from_seconds

Signed-off-by: Kenji Miyake <[email protected]>
Signed-off-by: tanaka3 <[email protected]>

* Awapi relay component (#1237)

Signed-off-by: tanaka3 <[email protected]>

* Refine BSD license name (#1244)

Signed-off-by: tanaka3 <[email protected]>

* Remove use_sim_time for set_parameter (#1260)

Signed-off-by: wep21 <[email protected]>
Signed-off-by: tanaka3 <[email protected]>

* Clear hazard level when ignored (#1200) (#1289)

Signed-off-by: Kenji Miyake <[email protected]>

Co-authored-by: Kenji Miyake <[email protected]>
Signed-off-by: tanaka3 <[email protected]>

* fix flake8 (#1306)

Signed-off-by: tanaka3 <[email protected]>

* Add perception traffic light state output (#1302)

* Add perception traffic light state output

Signed-off-by: Takagi, Isamu <[email protected]>

* Modify message type

Signed-off-by: Takagi, Isamu <[email protected]>

* Apply message type to relay

Signed-off-by: Takagi, Isamu <[email protected]>
Signed-off-by: tanaka3 <[email protected]>

* Fix lint errors (#1378)

* Fix lint errors

Signed-off-by: Kenji Miyake <[email protected]>

* Fix variable names

Signed-off-by: Kenji Miyake <[email protected]>
Signed-off-by: tanaka3 <[email protected]>

* change msg type looking traffic light state (#1494)

* change msg type looking traffic light state (#1455)

* change msg type looking traffic light state

* fix msg name

* using namespace

* change function name

* fix typo

* add readme

* Update awapi/awapi_awiv_adapter/Readme.md

fix typo

Co-authored-by: Yukihiro Saito <[email protected]>

Co-authored-by: Yukihiro Saito <[email protected]>

* Remove deprecated message include

Signed-off-by: wep21 <[email protected]>

* Fix lint

Signed-off-by: wep21 <[email protected]>

Co-authored-by: tkimura4 <[email protected]>
Co-authored-by: Yukihiro Saito <[email protected]>
Signed-off-by: tanaka3 <[email protected]>

* Add markdownlint and prettier (#1661)

* Add markdownlint and prettier

Signed-off-by: Kenji Miyake <[email protected]>

* Ignore .param.yaml

Signed-off-by: Kenji Miyake <[email protected]>

* Apply format

Signed-off-by: Kenji Miyake <[email protected]>
Signed-off-by: tanaka3 <[email protected]>

* Fix -Wunused-parameter (#1836)

* Fix -Wunused-parameter

Signed-off-by: Kenji Miyake <[email protected]>

* Fix mistake

Signed-off-by: Kenji Miyake <[email protected]>

* fix spell

* Fix lint issues

Signed-off-by: Kenji Miyake <[email protected]>

* Ignore flake8 warnings

Signed-off-by: Kenji Miyake <[email protected]>

Co-authored-by: Hiroki OTA <[email protected]>
Signed-off-by: tanaka3 <[email protected]>

* Fix compiler warnings (#1837)

* Fix -Wunused-private-field

Signed-off-by: Kenji Miyake <[email protected]>

* Fix -Wunused-variable

Signed-off-by: Kenji Miyake <[email protected]>

* Fix -Wformat-security

Signed-off-by: Kenji Miyake <[email protected]>

* Fix -Winvalid-constexpr

Signed-off-by: Kenji Miyake <[email protected]>

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

Signed-off-by: Kenji Miyake <[email protected]>

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

Signed-off-by: Kenji Miyake <[email protected]>

* Fix -Winconsistent-missing-override

Signed-off-by: Kenji Miyake <[email protected]>

* Fix -Wrange-loop-construct

Signed-off-by: Kenji Miyake <[email protected]>

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

Signed-off-by: Kenji Miyake <[email protected]>

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

Signed-off-by: Kenji Miyake <[email protected]>

* Fix lint

Signed-off-by: Kenji Miyake <[email protected]>

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

Signed-off-by: Kenji Miyake <[email protected]>

* Fix mistake

Signed-off-by: Kenji Miyake <[email protected]>

* Fix -Wunused-parameter

Signed-off-by: Kenji Miyake <[email protected]>
Signed-off-by: tanaka3 <[email protected]>

* Add autoware api (#1979)

Signed-off-by: tanaka3 <[email protected]>

* Feature/add virtual traffic light planner (#1588)

Signed-off-by: tanaka3 <[email protected]>

* Move launch file of external_cmd_selector (#2017)

* Move launch file of external_cmd_selector

Signed-off-by: Takagi, Isamu <[email protected]>

* Add copyright

Signed-off-by: Takagi, Isamu <[email protected]>

* Use DeclareLaunchArgument

Signed-off-by: Takagi, Isamu <[email protected]>

* Fix external command api name

Signed-off-by: Takagi, Isamu <[email protected]>

* Move common parameters

Signed-off-by: Takagi, Isamu <[email protected]>

* Fix format

Signed-off-by: Takagi, Isamu <[email protected]>

* Fix format

Signed-off-by: Takagi, Isamu <[email protected]>
Signed-off-by: tanaka3 <[email protected]>

* Use EmergencyState instead of deprecated EmergencyMode (#2030)

* Use EmergencyState instead of deprecated EmergencyMode

Signed-off-by: Kenji Miyake <[email protected]>

* Use stamped type

Signed-off-by: Kenji Miyake <[email protected]>
Signed-off-by: tanaka3 <[email protected]>

* add sort-package-xml hook in pre-commit (#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 <[email protected]>

* fix awapi_autoware_state_publisher (#2085)

Signed-off-by: tanaka3 <[email protected]>

* Feature/porting behavior path planner (#1645)

* Add behavior path planner pkg with Lane Change (#1525)

* add lanelet extension funcs

Signed-off-by: Takamasa Horibe <[email protected]>

* add planning msgs for FOA

Signed-off-by: Takamasa Horibe <[email protected]>

* add behavior_path_planner pkg

Signed-off-by: Takamasa Horibe <[email protected]>

* apply clang format

Signed-off-by: Takamasa Horibe <[email protected]>

* add error handling for config load failure

Signed-off-by: Takamasa Horibe <[email protected]>

* replace word: foa with remote control

Signed-off-by: Takamasa Horibe <[email protected]>

* add readme

Signed-off-by: Takamasa Horibe <[email protected]>

* use pointer for return value of path

Signed-off-by: Takamasa Horibe <[email protected]>

* fix hz

Signed-off-by: Takamasa Horibe <[email protected]>

* remove debug print

Signed-off-by: Takamasa Horibe <[email protected]>

* remove shide-shift & avoidance related files

Signed-off-by: Takamasa Horibe <[email protected]>

* Clip path by goal

* add build depend for behavior tree cpp

Signed-off-by: Takamasa Horibe <[email protected]>

* temporally disable lint test in lanelet2_extension

Signed-off-by: Takamasa Horibe <[email protected]>

Co-authored-by: rej55 <[email protected]>

* Add avoidance module in behavior_path_planner (#1528)

* Revert "remove shide-shift & avoidance related files"

This reverts commit d819ea0291fca251012e4b9ffd16de3896830aa2.

* refactor findNewShiftPoint func

Signed-off-by: Takamasa Horibe <[email protected]>

* remove duplicated decleration

Signed-off-by: Takamasa Horibe <[email protected]>

* fix barkward length issue
 - add clipPathLenght func in avoidance

Signed-off-by: Takamasa Horibe <[email protected]>

* refactor:
 - translate english
 - minor modification for traffic distance

Signed-off-by: Takamasa Horibe <[email protected]>

* support debug marker in behavior_path_planner

Signed-off-by: Takamasa Horibe <[email protected]>

* clean up side shift module

Signed-off-by: Takamasa Horibe <[email protected]>

* change topic name

Signed-off-by: Takamasa Horibe <[email protected]>

* remove japanese

Signed-off-by: Takamasa Horibe <[email protected]>

* Update planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/include/behavior_path_planner/scene_module/side_shift/side_shift_module.hpp

Co-authored-by: Yukihiro Saito <[email protected]>

* fix typo

Signed-off-by: Takamasa Horibe <[email protected]>

* remove unused var

Signed-off-by: Takamasa Horibe <[email protected]>

* adress reviewer comments:
 - add const for variables
 - add comment
 - fix typo

Signed-off-by: Takamasa Horibe <[email protected]>

* fix typo

Signed-off-by: Takamasa Horibe <[email protected]>

Co-authored-by: Yukihiro Saito <[email protected]>

* Replace behavior_path utilities with autoware_utils (#1532)

* replace calcDistance

Signed-off-by: Takamasa Horibe <[email protected]>

* replace arange

Signed-off-by: Takamasa Horibe <[email protected]>

* replave convertToEigenPt with autoware_utils::fromMsg

Signed-off-by: Takamasa Horibe <[email protected]>

* replace normalizeRadian

Signed-off-by: Takamasa Horibe <[email protected]>

* cosmetic change

Signed-off-by: Takamasa Horibe <[email protected]>

* import #1526 into behavior path planner (#1531)

Signed-off-by: Takamasa Horibe <[email protected]>

* Fix/behavior path empty path output guard (#1536)

* add guard

Signed-off-by: Takamasa Horibe <[email protected]>

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

* fix lateral jerk calculation (#1549)

Signed-off-by: Takamasa Horibe <[email protected]>

* fix: error handling on exception in behavior_path_planner (#1551)

Signed-off-by: Takamasa Horibe <[email protected]>

* Fix ignore too steep avoidance path (#1550)

* ignore too steep path

Signed-off-by: Takamasa Horibe <[email protected]>

* Update planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp

* parametrize lateral jerk limit

Signed-off-by: Takamasa Horibe <[email protected]>

* Update planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp

Co-authored-by: tkimura4 <[email protected]>

Co-authored-by: tkimura4 <[email protected]>

* use offsetNoThrow and add error log (#1615)

Signed-off-by: Takamasa Horibe <[email protected]>

* Ignore object ahead goal for avoidance (#1618)

* Ignore object ahead goal for avoidance

* Add flag

* Fix position of definition of goal_pose

* Fix arclength calculation

* Fix position of definition of goal_pose

* fix intersection stop line (#1636)

* fix intersection stop line

* fix typo

* add document (#1635)

Signed-off-by: Takamasa Horibe <[email protected]>

* Port behavior path planner to ros2

Signed-off-by: wep21 <[email protected]>

* Apply lint

Signed-off-by: wep21 <[email protected]>

* Fix typo

Signed-off-by: wep21 <[email protected]>

* Fix map qos

Signed-off-by: wep21 <[email protected]>

* debug slope calculation in behavior (#1566)

* update

* update

* revert change of autoware_utils

* define getPose in behavior_path_planner

* update

* update

* update

* update

* interpolate z in obstacle_avoidance_planner

* update velocity controller

* fix detection area and scene

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

Co-authored-by: tkimura4 <[email protected]>

* update comment in velocity controller

* remove debug print

* update

Co-authored-by: tkimura4 <[email protected]>

* Address review: Fix config file name

Signed-off-by: wep21 <[email protected]>

* pre-commit fixes

Signed-off-by: wep21 <[email protected]>

* Fix redeclaring parameters

Signed-off-by: wep21 <[email protected]>

* Add missing tf2 geometry function

Signed-off-by: wep21 <[email protected]>

* Apply lint

Signed-off-by: wep21 <[email protected]>

* Fix rclcpp Time initialization

Signed-off-by: wep21 <[email protected]>

* Use now() instead of msg stamp

Signed-off-by: wep21 <[email protected]>

* Use throttle output in getExpandedLanelet

Signed-off-by: wep21 <[email protected]>

* Add missing const

Signed-off-by: wep21 <[email protected]>

* Fix lint

Signed-off-by: wep21 <[email protected]>

Co-authored-by: Takamasa Horibe <[email protected]>
Co-authored-by: rej55 <[email protected]>
Co-authored-by: Yukihiro Saito <[email protected]>
Co-authored-by: tkimura4 <[email protected]>
Co-authored-by: Takayuki Murooka <[email protected]>
Signed-off-by: tanaka3 <[email protected]>

* Feature/porting motion velocity smoother (#1653)

* Add motion_velocity_smoother (#1333)

* Refactor node and utilities

* Fix utilities

* Fix utilities

* Fixing...

* Runs with L2 smoother

* Use boost::optional

* Add Linf, JerkFiltered

* JerkFiltered

* Fix

* Fix awapi

* Fix bug

* Add config files

* Fix bug

* Fix bug and apply clang-format

* Remove unused variable

* Fix bug

* Change C-style cast to static_cast

* Add destructors

* Use smart pointers for members

* Add test

* Fix test code

* Tmp: add L2 norm of jerk in objective function

* Fix external velocity limit

* Fix interpolate in velocity controller and remove prevent move to close stop line

* add initial velocity and acceleration to the filter function

* Fix index calculation

* add new marge function

* handle edge case

* Tmp: skip osqp_interface build test

* Revert get_modified_package.sh

* Fix CI

* Fix version of osqp

* Fix

* Add design docment (JPN)

* Fix linear interpolation

* Refactor node and utilities

* Fix utilities

* Fix utilities

* Fixing...

* Runs with L2 smoother

* Use boost::optional

* Add Linf, JerkFiltered

* JerkFiltered

* Fix

* Fix awapi

* Fix bug

* Add config files

* Fix bug

* Fix bug and apply clang-format

* Remove unused variable

* Fix bug

* Change C-style cast to static_cast

* Add destructors

* Use smart pointers for members

* Add test

* Fix test code

* Tmp: add L2 norm of jerk in objective function

* Fix external velocity limit

* Fix interpolate in velocity controller and remove prevent move to close stop line

* add initial velocity and acceleration to the filter function

* Fix index calculation

* add new marge function

* handle edge case

* Tmp: skip osqp_interface build test

* Revert get_modified_package.sh

* Fix CI

* Fix version of osqp

* Fix

* Add design docment (JPN)

* Fix linear interpolation

* add new sample function

* Fix

* Revert velocity_controller

* Reset motion_velocity_optimizer

* Fix parameter setting

* Refactor and fix bugs

* Use autoware_utils

* Fix doc and bug

* Fix doc

* Fix external velocity limit and parametrize margin and jerk weight

* Fix typo

* Fix typo and remove old readme

* add stop point calculation and modify objective function

* Add parameter handling functions and add namespace

* Rename calcClosestTrajectoryPoint to calcInterpolatedTrajectoryPoint and remove unused function

* Fix applyExternalVelocityLimit and fix comments

* Fix variable name

* Fix variable name

* Fix yaml comment

* Add const

* Fix interpolaion

* Remove run() and change type of prev_closest_point_

* Update planning/scenario_planning/common/motion_velocity_smoother/src/smoother/smoother_base.cpp

Co-authored-by: tkimura4 <[email protected]>

* fix stop point error

* Fix linear interpolation and external velocity calculation

* Remove debug code

* Rename BaseParam

* Remove unused func and fix misc

* Fix package.xml, include, apply formatting

* Fix external velocity limit

* add debug calculation time

* modify calculation time debugger

* modify calculation time publisher

* rescale the calculation time

* Fix some problem

* Update planning/scenario_planning/common/motion_velocity_smoother/launch/motion_velocity_smoother.launch

Co-authored-by: tkimura4 <[email protected]>

Co-authored-by: yutaka <[email protected]>
Co-authored-by: tkimura4 <[email protected]>

* Add Resample Procedure after the optimization in motion_velocity_smoother (#1530)

* do resample after the optimization

* modify resample timing

* add 0 at the end of the resampeld output trajectory

* Manage parameters

* Fix format

* Fix format

* Devide resample function in other file

* Update default_motion_velocity_smoother.yaml

* Update default_motion_velocity_smoother.yaml

Co-authored-by: Fumiya Watanabe <[email protected]>

* Output debug trajectories in motion velocity smoother (#1533)

* add debug trajectory

* add rqt_multiplot setting files

* add calculation tiem visualization

* guard emtpy output (#1537)

* Motion velocity smoother doc (#1563)

* Fix doc

* Add english doc (tmp)

* Add english file

* Remove unused files

* Fix

* Fix typo

* Modify 1-size trajectory handling & warning messages (#1540)

* add new guard to delete unnecessary message

* change message

* add warning messages

* chnage message to warning

* chnage to throttle

* Fix jerk filter calculation (#1593)

* modify jerk filter calculation

* remove unnecessary code

* change the way of interp. of pose in smoother (#1600)

* Update smoother resampling (#1595)

* add new guard to delete unnecessary message

* change message

* add warning messages

* chnage message to warning

* chnage to throttle

* modify jerk filter calculation

* modify jerk filter calculation

* separate resampling code

* add new parameter

* add new parameter to dynamic reconfigure

* remove unnecessary code

* add sampling before optimization

* add const and blocker

* change access to at

* fix terminal length value

* add comments

* clean code

* add comment

* Fix/insert backward on reverse trajectory (#1602)

* remove unused code

Signed-off-by: Takamasa Horibe <[email protected]>

* move debug code

Signed-off-by: Takamasa Horibe <[email protected]>

* fix insert backward on reverse trajectory

Signed-off-by: Takamasa Horibe <[email protected]>

* Porting motion velocity smoother to ros2

Signed-off-by: wep21 <[email protected]>

* Fix launch and fix warning

* pre-commit fixes

* Remove unused argument

* Comment out unused parameter

Signed-off-by: wep21 <[email protected]>

* Add distance threshold in motion_velocity_smoother (#1659)

* Add distance threshold

* Remove parameters and use default value

* Fix for pre-commit

Signed-off-by: wep21 <[email protected]>

* Support Non-zero Stop Point Acceleration (#1651)

* Support non-zero stop point acceleration

* modify zero velocity id search function (#1690)

* modify zero velocity id search function

* Fix resample function and error handling

* Add comment

Co-authored-by: Fumiya Watanabe <[email protected]>

* Apply lint

Signed-off-by: wep21 <[email protected]>

* pre-commit fixes

Signed-off-by: wep21 <[email protected]>

Co-authored-by: Fumiya Watanabe <[email protected]>
Co-authored-by: yutaka <[email protected]>
Co-authored-by: tkimura4 <[email protected]>
Co-authored-by: purewater0901 <[email protected]>
Co-authored-by: Takamasa Horibe <[email protected]>
Co-authored-by: pre-commit <[email protected]>
Signed-off-by: tanaka3 <[email protected]>

* Fix/update max velocity topic name (#2236)

Signed-off-by: tanaka3 <[email protected]>

* 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 <[email protected]>

* Remove ament_cmake_uncrustify and ament_clang_format

Signed-off-by: Kenji Miyake <[email protected]>

* Apply Black

Signed-off-by: Kenji Miyake <[email protected]>

* Apply clang-format

Signed-off-by: Kenji Miyake <[email protected]>

* Fix build errors

Signed-off-by: Kenji Miyake <[email protected]>

* Fix for cpplint

* Fix include double quotes to angle brackets

Signed-off-by: Kenji Miyake <[email protected]>

* Apply clang-format

Signed-off-by: Kenji Miyake <[email protected]>

* Fix build errors

Signed-off-by: Kenji Miyake <[email protected]>
Signed-off-by: tanaka3 <[email protected]>

* Add COLCON_IGNORE (#500)

Signed-off-by: Kenji Miyake <[email protected]>
Signed-off-by: tanaka3 <[email protected]>

* Port awapi awiv adapter to .auto (#554)

* Remove COLCON_IGNORE

* Change AutowareState

* Change EmergencyState

* Change HazardStatus

* Split file for convert function

* Change Path

* Change Trajectory

* Change ControlMode

* Change Shift

* Change TurnSignal and move converter

* Change Steering

* Change VehicleCommand

* Fix EmergencyState

* Fix for pre-commit

* Move autoware state converter

* Fix control comand

* Change TwistStamped

* Remove unused header

Signed-off-by: tanaka3 <[email protected]>

* Use pacmod3 msgs (#610)

* Migration to pacmod3 msgs

Signed-off-by: wep21 <[email protected]>

* Fix action for pacmod

Signed-off-by: wep21 <[email protected]>

* Run pre-commit

Signed-off-by: wep21 <[email protected]>
Signed-off-by: tanaka3 <[email protected]>

* fix package.xml (#630)

Co-authored-by: Takayuki Murooka <[email protected]>
Signed-off-by: tanaka3 <[email protected]>

* adapt to actuation cmd/status as control msg (#646)

* adapt to actuation cmd/status as control msg

* fix readme

* fix topics

* fix remaing topics

* as to pacmod interface

* fix vehicle status

* add header to twist

* revert gyro_odometer_change

* revert twist topic change

* revert unchanged package

Signed-off-by: tanaka3 <[email protected]>

* fix awapi topics (#685)

Signed-off-by: tanaka3 <[email protected]>

* fix mixed route topic (#695)

Signed-off-by: tanaka3 <[email protected]>

* Fix/psim topics emergency handler awapi (#702)

* fix emergency handler

* fix awapi

* remove unused topic

* remove duplecated vehicle cmd

Signed-off-by: tanaka3 <[email protected]>

* Fix topic type of awapi relay (#701)

* Fix topic type of awapi relay

* Fix

Signed-off-by: tanaka3 <[email protected]>

* fix door cmd topic (#711)

* fix door cmd topic

* fix topic

Signed-off-by: tanaka3 <[email protected]>

* Update traffic light topic name (#729)

* Update traffic light topic name

Signed-off-by: wep21 <[email protected]>

* Update traffic light topic name in perception

Signed-off-by: wep21 <[email protected]>
Signed-off-by: tanaka3 <[email protected]>

* Update twist topic name (#736)

Signed-off-by: wep21 <[email protected]>
Signed-off-by: tanaka3 <[email protected]>

* fix: refactor readme

Co-authored-by: Takeshi Miura <[email protected]>

Co-authored-by: tkimura4 <[email protected]>
Co-authored-by: hiroyuki obinata <[email protected]>
Co-authored-by: mitsudome-r <[email protected]>
Co-authored-by: Nikolai Morin <[email protected]>
Co-authored-by: Jilada Eccleston <[email protected]>
Co-authored-by: Daisuke Nishimatsu <[email protected]>
Co-authored-by: Ryohsuke Mitsudome <[email protected]>
Co-authored-by: Kenji Miyake <[email protected]>
Co-authored-by: Kazuki Miyahara <[email protected]>
Co-authored-by: Taichi Higashide <[email protected]>
Co-authored-by: ito-san <[email protected]>
Co-authored-by: Shigeki Kobayashi <[email protected]>
Co-authored-by: Keisuke Shima <[email protected]>
Co-authored-by: Takagi, Isamu <[email protected]>
Co-authored-by: Yukihiro Saito <[email protected]>
Co-authored-by: Hiroki OTA <[email protected]>
Co-authored-by: Keisuke Shima <[email protected]>
Co-authored-by: k-obitsu <[email protected]>
Co-authored-by: Takamasa Horibe <[email protected]>
Co-authored-by: rej55 <[email protected]>
Co-authored-by: Takayuki Murooka <[email protected]>
Co-authored-by: yutaka <[email protected]>
Co-authored-by: purewater0901 <[email protected]>
Co-authored-by: pre-commit <[email protected]>
Co-authored-by: Satoshi OTA <[email protected]>
Co-authored-by: Takayuki Murooka <[email protected]>
Co-authored-by: Takeshi Miura <[email protected]>
  • Loading branch information
1 parent f34431d commit feef2b5
Show file tree
Hide file tree
Showing 27 changed files with 3,534 additions and 0 deletions.
35 changes: 35 additions & 0 deletions awapi/awapi_awiv_adapter/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -0,0 +1,35 @@
cmake_minimum_required(VERSION 3.5)
project(awapi_awiv_adapter)

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

find_package(ament_cmake_auto REQUIRED)
ament_auto_find_build_dependencies()

ament_auto_add_executable(awapi_awiv_adapter
src/awapi_awiv_adapter_node.cpp
src/awapi_awiv_adapter_core.cpp
src/awapi_vehicle_state_publisher.cpp
src/awapi_autoware_state_publisher.cpp
src/awapi_stop_reason_aggregator.cpp
src/awapi_v2x_aggregator.cpp
src/awapi_lane_change_state_publisher.cpp
src/awapi_obstacle_avoidance_state_publisher.cpp
src/awapi_max_velocity_publisher.cpp
src/awapi_autoware_util.cpp
src/awapi_pacmod_util.cpp
)

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

ament_auto_package(INSTALL_TO_SHARE launch)
299 changes: 299 additions & 0 deletions awapi/awapi_awiv_adapter/Readme.md

Large diffs are not rendered by default.

Original file line number Diff line number Diff line change
@@ -0,0 +1,81 @@
// Copyright 2020 Tier IV, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef AWAPI_AWIV_ADAPTER__AWAPI_AUTOWARE_STATE_PUBLISHER_HPP_
#define AWAPI_AWIV_ADAPTER__AWAPI_AUTOWARE_STATE_PUBLISHER_HPP_

#include "awapi_awiv_adapter/awapi_autoware_util.hpp"

#include <rclcpp/rclcpp.hpp>

#include <autoware_api_msgs/msg/awapi_autoware_status.hpp>

#include <set>
#include <string>
#include <vector>

namespace autoware_api
{
class AutowareIvAutowareStatePublisher
{
public:
explicit AutowareIvAutowareStatePublisher(rclcpp::Node & node);
void statePublisher(const AutowareInfo & aw_info);

private:
rclcpp::Logger logger_;
rclcpp::Clock::SharedPtr clock_;
// publisher
rclcpp::Publisher<autoware_api_msgs::msg::AwapiAutowareStatus>::SharedPtr pub_state_;

// parameter

/* parameter for judging goal now */
bool arrived_goal_;
autoware_auto_system_msgs::msg::AutowareState::_state_type prev_state_;

void getAutowareStateInfo(
const autoware_auto_system_msgs::msg::AutowareState::ConstSharedPtr & autoware_state_ptr,
autoware_api_msgs::msg::AwapiAutowareStatus * status);
void getControlModeInfo(
const autoware_auto_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr & control_mode_ptr,
autoware_api_msgs::msg::AwapiAutowareStatus * status);
void getGateModeInfo(
const autoware_control_msgs::msg::GateMode::ConstSharedPtr & gate_mode_ptr,
autoware_api_msgs::msg::AwapiAutowareStatus * status);
void getEmergencyStateInfo(
const autoware_auto_system_msgs::msg::EmergencyState::ConstSharedPtr & emergency_state_ptr,
autoware_api_msgs::msg::AwapiAutowareStatus * status);
void getCurrentMaxVelInfo(
const autoware_planning_msgs::msg::VelocityLimit::ConstSharedPtr & current_max_velocity_ptr,
autoware_api_msgs::msg::AwapiAutowareStatus * status);
void getHazardStatusInfo(
const AutowareInfo & aw_info, autoware_api_msgs::msg::AwapiAutowareStatus * status);
void getStopReasonInfo(
const autoware_planning_msgs::msg::StopReasonArray::ConstSharedPtr & stop_reason_ptr,
autoware_api_msgs::msg::AwapiAutowareStatus * status);
void getDiagInfo(
const AutowareInfo & aw_info, autoware_api_msgs::msg::AwapiAutowareStatus * status);
void getErrorDiagInfo(
const AutowareInfo & aw_info, autoware_api_msgs::msg::AwapiAutowareStatus * status);
void getGlobalRptInfo(
const pacmod3_msgs::msg::GlobalRpt::ConstSharedPtr & global_rpt_ptr,
autoware_api_msgs::msg::AwapiAutowareStatus * status);

bool isGoal(const autoware_auto_system_msgs::msg::AutowareState::ConstSharedPtr & autoware_state);
};

} // namespace autoware_api

#endif // AWAPI_AWIV_ADAPTER__AWAPI_AUTOWARE_STATE_PUBLISHER_HPP_
Original file line number Diff line number Diff line change
@@ -0,0 +1,148 @@
// Copyright 2020 Tier IV, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef AWAPI_AWIV_ADAPTER__AWAPI_AUTOWARE_UTIL_HPP_
#define AWAPI_AWIV_ADAPTER__AWAPI_AUTOWARE_UTIL_HPP_

#include <rclcpp/rclcpp.hpp>

#include <autoware_api_msgs/msg/stop_command.hpp>
#include <autoware_api_msgs/msg/velocity_limit.hpp>
#include <autoware_auto_control_msgs/msg/ackermann_control_command.hpp>
#include <autoware_auto_planning_msgs/msg/path.hpp>
#include <autoware_auto_planning_msgs/msg/trajectory.hpp>
#include <autoware_auto_system_msgs/msg/autoware_state.hpp>
#include <autoware_auto_system_msgs/msg/emergency_state.hpp>
#include <autoware_auto_system_msgs/msg/hazard_status_stamped.hpp>
#include <autoware_auto_vehicle_msgs/msg/control_mode_report.hpp>
#include <autoware_auto_vehicle_msgs/msg/gear_report.hpp>
#include <autoware_auto_vehicle_msgs/msg/hazard_lights_report.hpp>
#include <autoware_auto_vehicle_msgs/msg/steering_report.hpp>
#include <autoware_auto_vehicle_msgs/msg/turn_indicators_report.hpp>
#include <autoware_control_msgs/msg/gate_mode.hpp>
#include <autoware_planning_msgs/msg/is_avoidance_possible.hpp>
#include <autoware_planning_msgs/msg/lane_change_status.hpp>
#include <autoware_planning_msgs/msg/stop_reason_array.hpp>
#include <autoware_planning_msgs/msg/velocity_limit.hpp>
#include <autoware_v2x_msgs/msg/infrastructure_command_array.hpp>
#include <autoware_v2x_msgs/msg/virtual_traffic_light_state_array.hpp>
#include <autoware_vehicle_msgs/msg/battery_status.hpp>
#include <diagnostic_msgs/msg/diagnostic_array.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <nav_msgs/msg/odometry.hpp>
#include <pacmod3_msgs/msg/global_rpt.hpp>
#include <pacmod3_msgs/msg/system_rpt_int.hpp>
#include <sensor_msgs/msg/nav_sat_fix.hpp>

#include <tf2/utils.h>
#include <tf2_ros/transform_broadcaster.h>
#include <tf2_ros/transform_listener.h>

#include <memory>
#include <string>

namespace autoware_api
{
struct AutowareInfo
{
std::shared_ptr<geometry_msgs::msg::PoseStamped> current_pose_ptr;
autoware_auto_vehicle_msgs::msg::SteeringReport::ConstSharedPtr steer_ptr;
autoware_auto_control_msgs::msg::AckermannControlCommand::ConstSharedPtr vehicle_cmd_ptr;
autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport::ConstSharedPtr turn_indicators_ptr;
autoware_auto_vehicle_msgs::msg::HazardLightsReport::ConstSharedPtr hazard_lights_ptr;
nav_msgs::msg::Odometry::ConstSharedPtr odometry_ptr;
autoware_auto_vehicle_msgs::msg::GearReport::ConstSharedPtr gear_ptr;
autoware_vehicle_msgs::msg::BatteryStatus::ConstSharedPtr battery_ptr;
sensor_msgs::msg::NavSatFix::ConstSharedPtr nav_sat_ptr;
autoware_auto_system_msgs::msg::AutowareState::ConstSharedPtr autoware_state_ptr;
autoware_auto_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr control_mode_ptr;
autoware_control_msgs::msg::GateMode::ConstSharedPtr gate_mode_ptr;
autoware_auto_system_msgs::msg::EmergencyState::ConstSharedPtr emergency_state_ptr;
autoware_auto_system_msgs::msg::HazardStatusStamped::ConstSharedPtr hazard_status_ptr;
autoware_planning_msgs::msg::StopReasonArray::ConstSharedPtr stop_reason_ptr;
autoware_v2x_msgs::msg::InfrastructureCommandArray::ConstSharedPtr v2x_command_ptr;
autoware_v2x_msgs::msg::VirtualTrafficLightStateArray::ConstSharedPtr v2x_state_ptr;
diagnostic_msgs::msg::DiagnosticArray::ConstSharedPtr diagnostic_ptr;
pacmod3_msgs::msg::GlobalRpt::ConstSharedPtr global_rpt_ptr;
autoware_planning_msgs::msg::LaneChangeStatus::ConstSharedPtr lane_change_available_ptr;
autoware_planning_msgs::msg::LaneChangeStatus::ConstSharedPtr lane_change_ready_ptr;
autoware_auto_planning_msgs::msg::Path::ConstSharedPtr lane_change_candidate_ptr;
autoware_planning_msgs::msg::IsAvoidancePossible::ConstSharedPtr obstacle_avoid_ready_ptr;
autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr obstacle_avoid_candidate_ptr;
autoware_api_msgs::msg::VelocityLimit::ConstSharedPtr max_velocity_ptr;
autoware_planning_msgs::msg::VelocityLimit::ConstSharedPtr current_max_velocity_ptr;
autoware_api_msgs::msg::StopCommand::ConstSharedPtr temporary_stop_ptr;
autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr autoware_planning_traj_ptr;
pacmod3_msgs::msg::SystemRptInt::ConstSharedPtr door_state_ptr;
};

template <class T>
T waitForParam(
rclcpp::Node * node, const std::string & remote_node_name, const std::string & param_name)
{
std::chrono::seconds sec(1);

auto param_client = std::make_shared<rclcpp::SyncParametersClient>(node, remote_node_name);

while (!param_client->wait_for_service(sec)) {
if (!rclcpp::ok()) {
RCLCPP_ERROR(node->get_logger(), "Interrupted while waiting for the service.");
return {};
}
RCLCPP_INFO_THROTTLE(
node->get_logger(), *node->get_clock(), 1000 /* ms */, "waiting for node: %s, param: %s\n",
remote_node_name.c_str(), param_name.c_str());
}

if (param_client->has_parameter(param_name)) {
return param_client->get_parameter<T>(param_name);
}

return {};
}

double lowpass_filter(const double current_value, const double prev_value, const double gain);

namespace planning_util
{
bool calcClosestIndex(
const autoware_auto_planning_msgs::msg::Trajectory & traj, const geometry_msgs::msg::Pose & pose,
size_t & output_closest_idx, const double dist_thr = 10.0, const double angle_thr = M_PI / 2.0);

inline geometry_msgs::msg::Pose getPose(
const autoware_auto_planning_msgs::msg::Trajectory & traj, const int idx)
{
return traj.points.at(idx).pose;
}

inline double calcDist2d(const geometry_msgs::msg::Point & a, const geometry_msgs::msg::Point & b)
{
return std::hypot((a.x - b.x), (a.y - b.y));
}

double normalizeEulerAngle(double euler);

double calcArcLengthFromWayPoint(
const autoware_auto_planning_msgs::msg::Trajectory & input_path, const size_t src_idx,
const size_t dst_idx);

double calcDistanceAlongTrajectory(
const autoware_auto_planning_msgs::msg::Trajectory & trajectory,
const geometry_msgs::msg::Pose & current_pose, const geometry_msgs::msg::Pose & target_pose);

} // namespace planning_util

} // namespace autoware_api

#endif // AWAPI_AWIV_ADAPTER__AWAPI_AUTOWARE_UTIL_HPP_
Loading

0 comments on commit feef2b5

Please sign in to comment.