feat(yabloc): add camera and vector map localization

[KYabuuchi]

Description

Add YabLoc (localization with camera and vector map) as a new location estimation method.
The user can choose whether to run NDT or YabLoc by specifying localization_mode as an argument when launching autoware.

YabLoc is a pose_estimator consisting of multiple nodes, which estimates position and orientation from sensor inputs and publishes localization/pose_estimator/pose_with_covariance.

Packages related to YabLoc are located under localization/yabloc/.
The following figure shows where the code has been changed and added. Red frames indicate edited files, and red blocks indicate added files.

All yabloc-related nodes are launched in the following namespace

/localization/yabloc/image_processing/graph_segment
/localization/yabloc/image_processing/lanelet2_overlay
/localization/yabloc/image_processing/line_segments_overlay
/localization/yabloc/image_processing/lsd
/localization/yabloc/image_processing/segment_filter
/localization/yabloc/image_processing/transform_listener_impl_55bd3a9c5490
/localization/yabloc/image_processing/transform_listener_impl_5604d18c09f0
/localization/yabloc/image_processing/undistort
/localization/yabloc/initializer/camera_pose_initializer_node
/localization/yabloc/initializer/semseg_node
/localization/yabloc/initializer/transform_listener_impl_55fe01887f30
/localization/yabloc/map/ground_server
/localization/yabloc/map/ll2_decomposer
/localization/yabloc/pf/camera_corrector
/localization/yabloc/pf/gnss_corrector

Related links

• https://github.com/tier4/YabLoc
• https://github.com/orgs/autowarefoundation/discussions/3484

Tests performed

I confirmed that logging_simulator works correctly with the following ROSBAG created by AWSIM.

• rosbag : This link is TEIR IV internal link now. I will replace it with publicly available rosbag.
• rosbag(public) : please decompress after download
• map : This link was quoted from AWSIM tutorial.

How to launch with NDT

ros2 launch autoware_launch logging_simulator.launch.xml \
  map_path:=$HOME/autoware_map/nishishinjuku \
  vehicle_model:=sample_vehicle \
  sensor_model:=awsim_sensor_kit \
  control:=false \
  planning:=false \
  localization_mode:=lidar  # not necessary because the default mode is lidar

How to launch with YabLoc

ros2 launch autoware_launch logging_simulator.launch.xml \
  map_path:=$HOME/autoware_map/nishishinjuku \
  vehicle_model:=sample_vehicle \
  sensor_model:=awsim_sensor_kit \
  control:=false \
  planning:=false \
  localization_mode:=camera

yabloc_ndt_localizer_selection_para.mp4

⚠️ NOTE: In the video above, ndt and yabloc are specified for localization_mode, but lidar and camera are valid options in the latest version.

Notes for reviewers

This PR needs to be merged with

• feat(yabloc): add camera and vector map localization autoware_launch#393

There is also a PR to autoware-documentation.

• docs(yabloc): add yabloc guide autoware-documentation#394

Interface changes

Nothing.

Effects on system behavior

By default, it starts as before.
If localization_mode:=camera is added as an argument when launching autoware, YabLoc runs instead of NDT.

When yabloc is used, matching_score, which is NDT's diagnostics, is not published.
Note that the absence of matching_score does not affect the system error monitor.

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.
• 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.

[codecov]

Codecov Report

Patch coverage: 4.64% and project coverage change: -9.81 ⚠️

Comparison is base (d5a1ed6) 14.72% compared to head (6e3977f) 4.92%.

Additional details and impacted files

@@            Coverage Diff             @@
##             main   #3946       +/-   ##
==========================================
- Coverage   14.72%   4.92%    -9.81%     
==========================================
  Files        1464      69     -1395     
  Lines      103173    2337   -100836     
  Branches    30513     268    -30245     
==========================================
- Hits        15197     115    -15082     
+ Misses      71476    2145    -69331     
+ Partials    16500      77    -16423     

Flag	Coverage Δ
differential	4.92% <4.64%> (?)
total	?

Impacted Files	Coverage Δ
...zer/src/pose_initializer/pose_initializer_core.cpp	0.00% <0.00%> (ø)
...initializer/src/pose_initializer/yabloc_module.cpp	0.00% <0.00%> (ø)
...bloc_common/include/yabloc_common/ground_plane.hpp	0.00% <0.00%> (ø)
...c_common/ground_server/filter/moving_averaging.hpp	0.00% <0.00%> (ø)
...ommon/include/yabloc_common/ground_server/util.hpp	0.00% <0.00%> (ø)
...abloc/yabloc_common/src/camera_info_subscriber.cpp	0.00% <0.00%> (ø)
localization/yabloc/yabloc_common/src/color.cpp	0.00% <0.00%> (ø)
...ization/yabloc/yabloc_common/src/cv_decompress.cpp	0.00% <0.00%> (ø)
...yabloc/yabloc_common/src/extract_line_segments.cpp	0.00% <0.00%> (ø)
...oc_common/src/ground_server/ground_server_core.cpp	0.00% <0.00%> (ø)
... and 51 more

... and 1455 files with indirect coverage changes

☔ View full report in Codecov by Sentry.
📢 Do you have feedback about the report comment? Let us know in this issue.

[kminoda]

LGTM (Also confirmed that both YabLoc and NDT mode works with given sample rosbags)

[YamatoAndo]

LGTM about pose_initilizer