feat(pose_estimator_arbiter): add pose_estimator_arbiter with simple switching rule

[KYabuuchi]

Description

This PR is derived from https://github.com/autowarefoundation/autoware.universe/pull/5846

Diff from original PR:

• added a new rule_helper (which made it possible to describe all pose_estimator switching with only a vector map)
• changed the default switching rule.
• created example_rule directory

TODO

• [ ] fix some test code
• [ ] improve a readme
• [ ] explain how to specify pose_estimator in vector map
• [x] after https://github.com/autowarefoundation/autoware.universe/pull/6076 is merged, adopt to it

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

[!IMPORTANT] Maps are also provided in the sample rosbag link, but please note that this PR cannot run properly. Please download the maps from the sample map link.

• sample data rosbag
• sample map (TIER IV INTERNAL LINK. I will replace it later.)

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.

Lanelet2 describes the activation of pose_estimator as follows

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.

https://github.com/autowarefoundation/autoware.universe/assets/24854875/696f2086-2a4f-47fe-9504-474017789917

[!IMPORTANT] 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}'

[!IMPORTANT] 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_estimator_arbiter/debug/marker_array /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

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.

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

[KYabuuchi]

I return this PR to draft again. I am going to remove the rules using lanelet2 from the default rules, leaving only the simple rules.

[KYabuuchi]

The latest PR is here https://github.com/autowarefoundation/autoware.universe/pull/6144