feat(pose_estimator_arbiter): add pose_estimator_arbiter with simple switching rule

[KYabuuchi]

Description

This PR implement a mechanism to launch multiple pose_estimators, and allow for pausing and resuming specific ones according to the situation.
This PR introduces only one rule that will always enable all pose_estimators.

The package README describes the purpose in detail.

Related links

This PR is derived from #5846 & #6089

Diff from the original PR(#5846):

• Changed the default switching rule
• Created example_rule directory

Diff from the second PR(#6089):

• Moved vector_map_based_rule into example_rule/
• Changed the default rule to always enable all estimators
• Simplified example_rule to clarify roles

Related links

[Proposal] Runtime Localization Module Switching for Multi-Pose Estimators #3878

Tests performed

I have confirmed that ndt/yabloc/eagleye/artag is available as before with logging_simulator.launch.xml .
Also, I confirmed that by specifying multiple pose_estimators at startup, pose_estimator_arbiter is launched and arbitrates the pose_estimators according to the situation.

sample data

• sample map & rosbag

The rosbag is simulated data created by AWSIM.
The map is an edited version of the original map data published on the AWSIM documentation page to make it suitable for multiple pose_estimators.

how to launch

When start a single pose_estimator:

ros2 launch autoware_launch logging_simulator.launch.xml \
    map_path:=$HOME/MAP vehicle_model:=sample_vehicle sensor_model:=awsim_sensor_kit \
    pose_source:=ndt # or yabloc, eagleye or artag

When start multiple pose_estimator:

ros2 launch autoware_launch logging_simulator.launch.xml \
    map_path:=$HOME/MAP vehicle_model:=sample_vehicle sensor_model:=awsim_sensor_kit \
    pose_source:=ndt_yabloc_eagleye_artag # or ndt_yabloc, eagleye_artag and so on

expected behavior

When start a single pose_estimator, it launches the same nodes as before.

When start multiple pose_estimator, it works as follows.

quatro_switching_all_enable_demo_x4.mp4

• The sample map has areas that do not contain point clouds as shown in the following figure. When running with multiple pose_estimators, no fatal breakdowns will occur.

Tip

We can visualize the debug prints of pose_estimator_arbiter at any time with the following command
(I recommend enabling logging, because with the default rviz settings, we do not know which pose_estimator is running.)

ros2 service call /localization/pose_estimator_arbiter/config_logger \
    logging_demo/srv/ConfigLogger \
   '{logger_name: localization.pose_estimator_arbiter , level: debug}'

Caution

Eagleye requires a GNSS doppler, but AWSIM does not generate it. So I have stored my pseudo-generated dopplers in rosbag. Because of that, Eagleye's estimated position is not accurate.

Notes for reviewers

Tip

If you want to create an rviz screen like the demonstration video, please visualize the following topics.
/localization/pose_estiamtor/points_aligned
/localization/pose_estimator/yabloc/image_processing/lanelet2_overlay_image
/localization/pose_estimator/yabloc/pf/predicted_particles_marker
/localization/pose_estimator/ar_tag_based_localizer/debug/image
/localization/eagleye/pose_with_covariance/to_relay
/localization/pose_estimator/eagleye/pose_with_covariance/to_relay

Sorry for the large number of commits.

Interface changes

The default behavior remains the same.
Users can now specify multiple pose_estimators for pose_source . By default, ndt is launched.

Effects on system behavior

The default behavior remains the same.
Users can now specify multiple pose_estimators for pose_source . By default, ndt is launched.

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.

[SakodaShintaro]

@KYabuuchi
Sorry, there seems to be a conflict.

[KYabuuchi]

@SakodaShintaro Sorry, I fixed it.

[KYabuuchi]

For reviewers

Recently, the PR #6091 is merged. When checking this PR, please ensure that the following PRs are reflected 🙏

• sample_sensor_kit feat: always separate lidar preprocessing from pointcloud_container sample_sensor_kit_launch#85
• autoware_launch feat: always separate lidar preprocessing from pointcloud_container autoware_launch#796
• awsim_sensor_kit fix: fix container name handling RobotecAI/awsim_sensor_kit_launch#14

[SakodaShintaro]

Looks good to me

Although I have not conducted proper measurements, I think this pull request may also contribute to faster launch of Autoware.
(This is because multi-loading of eagleye and pose_initializer has been improved)

[SakodaShintaro]

I don't know why, but it seemed like the markdown notation was not working effectively in <details> tags. I think there is no need to modify it.

[codecov]

Codecov Report

Attention: 325 lines in your changes are missing coverage. Please review.

Comparison is base (9b4d7e4) 14.91% compared to head (17cc07b) 14.95%.
Report is 4 commits behind head on main.

Files	Patch %	Lines
..._estimator_arbiter/pose_estimator_arbiter_core.cpp	0.00%	84 Missing ⚠️
...se_estimator_arbiter/rule_helper/pcd_occupancy.cpp	38.09%	13 Missing and 13 partials ⚠️
.../pose_estimator_arbiter/stopper/stopper_yabloc.hpp	0.00%	25 Missing ⚠️
...r/example_rule/test/test_vector_map_based_rule.cpp	41.46%	0 Missing and 24 partials ⚠️
...imator_arbiter/rule_helper/pose_estimator_area.cpp	45.00%	3 Missing and 19 partials ⚠️
...iter/example_rule/test/test_pcd_map_based_rule.cpp	37.14%	0 Missing and 22 partials ⚠️
...tor_arbiter/example_rule/test/test_rule_helper.cpp	42.85%	0 Missing and 20 partials ⚠️
...ator_arbiter/switch_rule/vector_map_based_rule.cpp	42.30%	5 Missing and 10 partials ⚠️
...n/pose_estimator_arbiter/test/test_shared_data.cpp	34.78%	0 Missing and 15 partials ⚠️
...timator_arbiter/switch_rule/pcd_map_based_rule.cpp	50.00%	6 Missing and 6 partials ⚠️
... and 12 more

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #6144      +/-   ##
==========================================
+ Coverage   14.91%   14.95%   +0.04%     
==========================================
  Files        1817     1839      +22     
  Lines      125357   125804     +447     
  Branches    37640    37799     +159     
==========================================
+ Hits        18693    18815     +122     
- Misses      85652    85842     +190     
- Partials    21012    21147     +135     

Flag	Coverage Δ		*Carryforward flag
differential	27.29% <27.29%> (?)
total	14.91% <ø> (ø)		Carriedforward from 9b4d7e4

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

☔ View full report in Codecov by Sentry.
📢 Have feedback on the report? Share it here.