feat(pose_initializer): support ad api

[isamu-takagi]

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

Description

Introduce AD API for localization. This PR is the pose_initializer, map_height_fitter, and GNSS module part of the diagram. Here are the changes and related packages:

Main change. The basic idea is to replace initialpose with API. Initialpose is only used for conversion to API by adaptor node. This ensures that all pose initialization is managed by pose_initializer. Finally, pose_initializer publishes the initialized pose to the other nodes as initialpose3d (this pose is aligned by NDT scan matcher) .

• pose_initializer
• tier4_localization_launch
• tier4_simulator_launch

Add the message and the internal localization interface definitions.

• autoware_ad_api_msgs
• component_interface_specs

Use initialpose3d instead of initialpose/initialpose2d.

• ekf_localizer
• simple_planning_simulator
• tier4_system_launch
• ad_service_state_monitor

Remove unused field seq and unused message PoseInitializationRequest.

• ndt_scan_matcher
• tier4_localization_msgs (related PR)

Replace from old initialize service.

• tier4_autoware_api_launch
• initial_pose_button_panel

Related links

https://github.com/autowarefoundation/autoware_launch/pull/72 https://github.com/tier4/tier4_autoware_msgs/pull/44

Tests performed

For planning simulation, check if pose initialization can be done at any time.

1. Merge related PRs.
2. Launch the planning simulation (sample map).
3. Check if pose initialization succeeds such as following command.

   ros2 service call /localization/initialize autoware_ad_api_msgs/srv/InitializeLocalization "
   pose:
     - header: {stamp: {sec: 1659431633, nanosec: 11821755}, frame_id: map}
       pose:
         pose:
           position: {x: 3730.68, y: 73730.09, z: 0.0}
           orientation: {x: 0.0, y: 0.0, z: 0.23509, w: 0.97197}"
   

For logging simulation, check if pose initialization can be done when vehicle stops.

1. Merge related PRs.
2. Launch the logging simulation (sample map).
3. Check if pose initialization succeeds such as following command when vehicle is stopped.

   ros2 service call /localization/initialize autoware_ad_api_msgs/srv/InitializeLocalization "
   pose:
     - header: {stamp: {sec: 1585897270, nanosec: 268281432}, frame_id: map}
       pose:
         pose:
           position: {x: 89569.453125, y: 42299.953125, z: 0.0}
           orientation: {x: 0.0, y: 0.0, z: 0.29236, w: 0.95631}
         covariance: [0.25, 0, 0, 0, 0, 0, 0, 0.25, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.0685389]"
   

4. Check if pose initialization fails when vehicle is not stopped.
5. Check if pose initialization succeeds by GNSS.

   ros2 service call /localization/initialize autoware_ad_api_msgs/srv/InitializeLocalization "pose: []"
   

Notes for reviewers

Pre-review checklist for the PR author

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

• [x] I've confirmed the contribution guidelines.
• [x] 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.
• [ ] The PR has been properly tested.
• [ ] The PR has been reviewed by the code owners.

Post-review checklist for the PR author

The PR author must check the checkboxes below before merging.

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

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

[yukkysaito]

What reason did you use upper camel case for function? I know google coding style recommend upper camel case for function. But many autoware code use lower camel case or snake case. So it may make to confuse. @mitsudome-r @xmfcx @kenji-miyake What do you think?

[isamu-takagi]

@yukkysaito I think that it will be unified with the snake_case https://github.com/autowarefoundation/autoware.universe/pull/1429#discussion_r932328837. I'll auto format later.

[codecov[bot]]

Codecov Report

Base: 10.43% // Head: 10.12% // Decreases project coverage by -0.30% :warning:

Coverage data is based on head (5686dfd) compared to base (e76ee5d). Patch coverage: 2.63% of modified lines in pull request are covered.

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #1500      +/-   ##
==========================================
- Coverage   10.43%   10.12%   -0.31%     
==========================================
  Files        1226     1212      -14     
  Lines       87947    86805    -1142     
  Branches    20457    20112     -345     
==========================================
- Hits         9176     8790     -386     
+ Misses      69269    68797     -472     
+ Partials     9502     9218     -284     

Flag	Coverage Δ		*Carryforward flag
differential	21.56% <1.98%> (?)
total	10.03% <25.00%> (-0.39%)	:arrow_down:	Carriedforward from 4ce0336

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

Impacted Files	Coverage Δ
...ose_button_panel/src/initial_pose_button_panel.cpp	0.00% <0.00%> (ø)
...ion/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp	0.00% <ø> (ø)
...r/src/map_height_fitter/map_height_fitter_core.cpp	0.00% <0.00%> (ø)
...r/src/map_height_fitter/map_height_fitter_node.cpp	0.00% <0.00%> (ø)
...e_initializer/src/pose_initializer/gnss_module.cpp	0.00% <0.00%> (ø)
...se_initializer/src/pose_initializer/ndt_module.cpp	0.00% <0.00%> (ø)
...zer/src/pose_initializer/pose_initializer_core.cpp	0.00% <0.00%> (ø)
...zer/src/pose_initializer/pose_initializer_node.cpp	0.00% <ø> (ø)
...ializer/src/pose_initializer/stop_check_module.cpp	0.00% <0.00%> (ø)
...nning_simulator/simple_planning_simulator_core.cpp	39.07% <0.00%> (ø)
... and 284 more

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

:umbrella: View full report at Codecov.
:loudspeaker: Do you have feedback about the report comment? Let us know in this issue.

[YamatoAndo]

@isamu-takagi Could you please add a readme for map_height_fitter?

[YamatoAndo]

Please add descriptions of LocalizationInitializationState.msg and InitializeLocalization.srv. It would be good to add a readme file and just link this.

[isamu-takagi]

Add message readme. https://github.com/autowarefoundation/autoware.universe/pull/1500/commits/8107b29b4d1c98481461687fcade986c8787221a

[YamatoAndo]

@isamu-takagi Can we initialize with 2DPoseEstimate in Rviz? I can't find the /localization/util/initialpose_rviz_helper node.

Also, it initialized "automatically" using GNSS positions before. Is it necessary to call ros service after this PR?

[YamatoAndo]

Can we initialize with 2DPoseEstimate in Rviz? I can't find the /localization/util/initialpose_rviz_helper node.

Also, it initialized "automatically" using GNSS positions before. Is it necessary to call ros service after this PR?

It seemed to be included in https://github.com/autowarefoundation/autoware.universe/pull/1431.

[YamatoAndo]

I performed the test described in Tests performed and confirmed that it works well.

[YamatoAndo]

@isamu-takagi Could you fix the CI/CD error build-and-test-differential / build-and-test-differential (humble, -cuda) (pull_request)) ?