feat(default_ad_api): add localization api #1431

isamu-takagi · 2022-07-26T04:37:25Z

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

Description

Introduce AD API for routing. This PR is the rviz helper, auto initial pose and AD API part of the diagram.
Here are the changes for each package:

autoware_ad_api_msgs: Add the localization API messages.
autoware_ad_api_specs: Add the localization API definitions.
component_interface_specs; Add the internal localization interface definitions.
default_ad_api: Add the localization API.
default_ad_api_helpers: Add the adaptor for RViz localization topics, and auto initialize node by GNSS.

Tests performed

For planning simulation.

Merge all related PRs.
Launch planning simulation.
Check if the localization succeeds when initialpose from RViz.

For logging simulation.

Merge all related PRs.
Launch logging simulation (sample map).
Check if the localization succeeds automatically when the vehicle is stopped.
Disable auto initial pose (feat(autoware_launch): add auto initial pose autoware_launch#71)
- Check if the localization succeeds when initialpose from RViz when vehicle is stopped.
- Check if the localization failes when initialpose from RViz when vehicle is not stopped.

Pre-review checklist for the PR author

The PR author must check the checkboxes below when creating the PR.

I've confirmed the contribution guidelines.
The PR follows the pull request guidelines.

In-review checklist for the PR reviewers

The PR reviewers must check the checkboxes below before approval.

The PR follows the pull request guidelines.

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.

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

codecov · 2022-07-26T12:49:46Z

Codecov Report

Merging #1431 (de6a32c) into main (d9fae78) will increase coverage by 0.45%.
The diff coverage is 5.17%.

@@            Coverage Diff             @@
##             main    #1431      +/-   ##
==========================================
+ Coverage   10.25%   10.71%   +0.45%     
==========================================
  Files        1199     1108      -91     
  Lines       86374    78505    -7869     
  Branches    19891    18660    -1231     
==========================================
- Hits         8860     8410     -450     
+ Misses      68294    61169    -7125     
+ Partials     9220     8926     -294

Flag	Coverage Δ		*Carryforward flag
differential	`10.67% <5.17%> (?)`
total	`10.69% <ø> (+0.45%)`	⬆️	Carriedforward from 7eaefe2

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

Impacted Files	Coverage Δ
...lpers/ad_api_adaptors/src/initial_pose_adaptor.cpp	`0.00% <0.00%> (ø)`
...ose_initializer/src/automatic_pose_initializer.cpp	`0.00% <0.00%> (ø)`
system/default_ad_api/src/localization.cpp	`42.85% <42.85%> (ø)`
...vehicle_model/sim_model_ideal_steer_acc_geared.cpp	`64.15% <0.00%> (-18.87%)`	⬇️
...ail/dds_fastrtps/response_status__type_support.cpp	`0.00% <0.00%> (-16.08%)`	⬇️
..._msgs/msg/detail/response_status__type_support.cpp	`0.00% <0.00%> (-15.39%)`	⬇️
...ils/include/motion_utils/trajectory/trajectory.hpp	`77.86% <0.00%> (-0.89%)`	⬇️
...or_path_planner/src/behavior_path_planner_node.cpp	`0.23% <0.00%> (-0.01%)`	⬇️
planning/surround_obstacle_checker/src/node.cpp	`0.00% <0.00%> (ø)`
localization/ekf_localizer/src/ekf_localizer.cpp	`0.00% <0.00%> (ø)`
... and 118 more

Help us with your feedback. Take ten seconds to tell us how you rate us. Have a feature suggestion? Share it here.

isamu-takagi · 2022-08-01T03:19:55Z

I'll split the PR.

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

isamu-takagi · 2022-08-02T11:10:32Z

This PR is ready.

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

system/default_ad_api_helpers/README.md

kenji-miyake · 2022-08-10T04:33:56Z

common/autoware_ad_api_msgs/localization/srv/InitializeLocalization.srv

+uint16 ERROR_UNSAFE = 1
+uint16 ERROR_GNSS_SUPPORT = 2
+uint16 ERROR_GNSS = 3
+uint16 ERROR_ESTIMATION = 4


Could you write brief descriptions of these error types?

Error Description

ERROR_UNSAFE Requested when unsafe, such as while driving.

ERROR_GNSS_SUPPORT GNSS is not supported.

ERROR_GNSS GNSS is supported but an error related to GNSS occurred.

ERROR_ESTIMATION An error occurred while pose estimation.

kenji-miyake · 2022-08-10T04:37:42Z

common/component_interface_specs/include/component_interface_specs/localization.hpp

+struct InitializationState
+{
+  using Message = autoware_ad_api_msgs::msg::LocalizationInitializationState;
+  static constexpr char name[] = "/localization/initialization_state";


@yukkysaito @mitsudome-r Do you have any opinions on the namings of Initialize and InitializationState?

I suppose these are okay.

launch/tier4_autoware_api_launch/launch/autoware_api.launch.xml

system/default_ad_api/src/localization.cpp

system/default_ad_api_helpers/src/initial_pose_adaptor.hpp

system/default_ad_api_helpers/src/initial_pose_adaptor.cpp

system/default_ad_api_helpers/src/automatic_pose_initializer.cpp

system/default_ad_api_helpers/src/initial_pose_adaptor.cpp

kenji-miyake · 2022-08-10T05:09:27Z

system/default_ad_api_helpers/README.md

+| Interface    | Local Name     | Global Name                       | Description                               |
+| ------------ | -------------- | --------------------------------- | ----------------------------------------- |
+| Subscription | initialpose    | /initialpose                      | The pose for localization initialization. |
+| Client       | fit_map_height | /localization/util/fit_map_height | To fix initial pose to map height         |


Could you tell me where this service is implemented, please?

I've found it in #1500.

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

…api/localization

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

…api/localization

…/autoware.universe into feature/ad-api/localization

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

system/default_ad_api_helpers/ad_api_adaptors/launch/rviz_adaptors.launch.xml

system/default_ad_api_helpers/ad_api_adaptors/README.md

system/default_ad_api_helpers/ad_api_adaptors/config/initial_pose.param.yaml

kminoda · 2022-09-01T01:23:25Z

system/default_ad_api_helpers/automatic_pose_initializer/src/automatic_pose_initializer.cpp

+  adaptor.init_cli(cli_initialize_, group_cli_);
+  adaptor.init_sub(sub_state_, [this](const State::Message::ConstSharedPtr msg) { state_ = *msg; });
+
+  const auto period = rclcpp::Rate(1.0).period();


As far as I tried on my PC, by setting the rate to higher value (e.g. rclcpp::Rate(2.0)), the automatic_pose_initializer started keep requesting for the pose initialization infinitely. It seems that the timer_ was kept called, and this blocked sub_state_ to update the state_.state from UNINITIALIZED to INITIALIZED.

Would you check if this happens in your PC, too? And if so, do you have any idea how to fix this issue?

Oh sorry my comment was a bit late. I would open an issue for this.

@isamu-takagi #1750

* feat(default_ad_api): add localization api Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * docs: add readme Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * feat: add auto initial pose Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * feat(autoware_ad_api_msgs): define localization interface Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * fix(default_ad_api): fix interface definition Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * feat(default_ad_api): modify interface version api to use spec package Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * feat(default_ad_api): modify interface version api to use spec package Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * fix: pre-commit Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * fix: pre-commit Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * fix: pre-commit Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * fix: copyright Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * feat: split helper package Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * fix: change topic name to local Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * fix: style Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * fix: style Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * fix: style Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * fix: remove needless keyword Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * feat: change api helper node namespace Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * fix: fix launch file path Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

isamu-takagi force-pushed the feature/ad-api/localization branch from 9c6aee2 to 0b6d5b9 Compare July 26, 2022 04:38

isamu-takagi self-assigned this Jul 26, 2022

isamu-takagi changed the title ~~feat(default_ad_api): add localization~~ feat(default_ad_api): add localization api Jul 26, 2022

isamu-takagi force-pushed the feature/ad-api/localization branch from f57a754 to b1841cb Compare July 26, 2022 13:10

This was referenced Jul 27, 2022

feat(component_interface_utils): update service utility #1429

Merged

feat(pose_initializer): change pose initialization #1448

Closed

isamu-takagi requested review from kenji-miyake, YamatoAndo and yukkysaito August 1, 2022 03:16

feat(default_ad_api): add localization api

b6ede8b

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

isamu-takagi force-pushed the feature/ad-api/localization branch from 662a1f3 to b6ede8b Compare August 2, 2022 09:30

isamu-takagi requested review from mitsudome-r and removed request for YamatoAndo August 2, 2022 09:31

isamu-takagi mentioned this pull request Aug 2, 2022

feat(autoware_launch): add auto initial pose autowarefoundation/autoware_launch#71

Closed

4 tasks

isamu-takagi marked this pull request as ready for review August 2, 2022 10:44

docs: add readme

ec2b06c

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

feat: add auto initial pose

8c987eb

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

isamu-takagi mentioned this pull request Aug 9, 2022

feat(default_ad_api): add routing api #1494

Merged

7 tasks

kenji-miyake reviewed Aug 10, 2022

View reviewed changes

isamu-takagi added 2 commits August 10, 2022 16:57

Merge branch 'main' into feature/ad-api/localization

0dcffb2

feat(autoware_ad_api_msgs): define localization interface

d74888e

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

isamu-takagi mentioned this pull request Aug 10, 2022

feat(autoware_ad_api_specs): define localization interface #1560

Merged

4 tasks

Merge branch 'feature/ad-api/localization-interface' into feature/ad-…

20dba3f

…api/localization

mitsudome-r mentioned this pull request Aug 16, 2022

Positioning error in wrong zone when NDT starts #1421

Closed

3 tasks

isamu-takagi added 3 commits August 24, 2022 17:33

fix(default_ad_api): fix interface definition

6f51ee0

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

feat(default_ad_api): modify interface version api to use spec package

ee6b428

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

feat(default_ad_api): modify interface version api to use spec package

fdde4f2

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

isamu-takagi and others added 15 commits August 25, 2022 19:51

Merge branch 'main' into feature/ad-api/localization-interface

b423776

Merge branch 'feature/ad-api/localization-interface' into feature/ad-…

566554e

…api/localization

Merge branch 'feature/ad-api/localization' of github.com:isamu-takagi…

122701c

…/autoware.universe into feature/ad-api/localization

fix: copyright

e5db1a7

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

feat: split helper package

7f6cfda

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

fix: change topic name to local

bc02465

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

fix: style

acdfc3f

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

fix: style

f4ce862

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

fix: style

cf1f540

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

fix: remove needless keyword

b94a6d8

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

Merge branch 'main' into feature/ad-api/localization

5431400

Merge branch 'main' into feature/ad-api/localization

925589b

feat: change api helper node namespace

2af3217

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

Merge branch 'main' into feature/ad-api/localization

7eaefe2

fix: fix launch file path

de6a32c

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

kenji-miyake reviewed Aug 30, 2022

View reviewed changes

system/default_ad_api_helpers/ad_api_adaptors/launch/rviz_adaptors.launch.xml Show resolved Hide resolved

system/default_ad_api_helpers/ad_api_adaptors/launch/rviz_adaptors.launch.xml Show resolved Hide resolved

system/default_ad_api_helpers/ad_api_adaptors/README.md Show resolved Hide resolved

YamatoAndo mentioned this pull request Aug 30, 2022

feat(pose_initializer)!: support ad api #1500

Merged

7 tasks

kenji-miyake reviewed Aug 31, 2022

View reviewed changes

system/default_ad_api_helpers/ad_api_adaptors/config/initial_pose.param.yaml Show resolved Hide resolved

kenji-miyake approved these changes Sep 1, 2022

View reviewed changes

isamu-takagi merged commit dcf99e5 into autowarefoundation:main Sep 1, 2022

isamu-takagi deleted the feature/ad-api/localization branch September 1, 2022 01:20

kminoda reviewed Sep 1, 2022

View reviewed changes

kminoda mentioned this pull request Sep 1, 2022

Pose initialization keeps running via automatic_pose_initializer #1750

Closed

3 tasks

tier4-autoware-public-bot bot mentioned this pull request Sep 2, 2022

chore: sync upstream tier4/autoware.universe#100

Merged

tkhmy mentioned this pull request Sep 16, 2022

feat: sync launch for init pose tier4/autoware_launch-old#486

Merged

8 tasks

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

feat(default_ad_api): add localization api #1431

feat(default_ad_api): add localization api #1431

isamu-takagi commented Jul 26, 2022 •

edited

Loading

codecov bot commented Jul 26, 2022 •

edited

Loading

isamu-takagi commented Aug 1, 2022

isamu-takagi commented Aug 2, 2022

kenji-miyake Aug 10, 2022

isamu-takagi Aug 10, 2022

kenji-miyake Aug 10, 2022 •

edited

Loading

kenji-miyake Aug 25, 2022

kenji-miyake Aug 10, 2022

kenji-miyake Aug 10, 2022

kminoda Sep 1, 2022

kminoda Sep 1, 2022

kminoda Sep 1, 2022

Error	Description
ERROR_UNSAFE	Requested when unsafe, such as while driving.
ERROR_GNSS_SUPPORT	GNSS is not supported.
ERROR_GNSS	GNSS is supported but an error related to GNSS occurred.
ERROR_ESTIMATION	An error occurred while pose estimation.

feat(default_ad_api): add localization api #1431

feat(default_ad_api): add localization api #1431

Conversation

isamu-takagi commented Jul 26, 2022 • edited Loading

Description

Related links

Tests performed

Pre-review checklist for the PR author

In-review checklist for the PR reviewers

Post-review checklist for the PR author

codecov bot commented Jul 26, 2022 • edited Loading

Codecov Report

isamu-takagi commented Aug 1, 2022

isamu-takagi commented Aug 2, 2022

Choose a reason for hiding this comment

Choose a reason for hiding this comment

kenji-miyake Aug 10, 2022 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

isamu-takagi commented Jul 26, 2022 •

edited

Loading

codecov bot commented Jul 26, 2022 •

edited

Loading

kenji-miyake Aug 10, 2022 •

edited

Loading