pose_estimator_arbiter#
Table of contents:
Abstract#
This package launches multiple pose estimators and provides the capability to stop or resume specific pose estimators based on the situation. It provides provisional switching rules and will be adaptable to a wide variety of rules in the future.
Please refer to this discussion about other ideas on implementation.
Why do we need a stop/resume mechanism?#
It is possible to launch multiple pose_estimators and fuse them using a Kalman filter by editing launch files. However, this approach is not preferable due to computational costs.
Particularly, NDT and YabLoc are computationally intensive, and it's not recommended to run them simultaneously. Also, even if both can be activated at the same time, the Kalman Filter may be affected by one of them giving bad output.
[!NOTE] Currently, there is ONLY A RULE implemented that always enables all pose_estimators. If users want to toggle pose_estimator with their own rules, they need to add new rules. by referring to example_rule. The example_rule has source code that can be used as a reference for implementing the rules.
Supporting pose_estimators#
Demonstration#
The following video demonstrates the switching of four different pose estimators.
Users can reproduce the demonstration using the following data and launch command:
sample data (rosbag & map) 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.
ros2 launch autoware_launch logging_simulator.launch.xml \
map_path:=<your-map-path> \
vehicle_model:=sample_vehicle \
sensor_model:=awsim_sensor_kit \
pose_source:=ndt_yabloc_artag_eagleye
Interfaces#
Click to show details
### Parameters There are no parameters. ### Services | Name | Type | Description | | ---------------- | ------------------------------- | ------------------------------- | | `/config_logger` | logging_demo::srv::ConfigLogger | service to modify logging level | ### Clients | Name | Type | Description | | --------------------- | --------------------- | --------------------------------- | | `/yabloc_suspend_srv` | std_srv::srv::SetBool | service to stop or restart yabloc | ### Subscriptions For pose estimator arbitration: | Name | Type | Description | | ------------------------------------- | --------------------------------------------- | -------------- | | `/input/artag/image` | sensor_msgs::msg::Image | ArTag input | | `/input/yabloc/image` | sensor_msgs::msg::Image | YabLoc input | | `/input/eagleye/pose_with_covariance` | geometry_msgs::msg::PoseWithCovarianceStamped | Eagleye output | | `/input/ndt/pointcloud` | sensor_msgs::msg::PointCloud2 | NDT input | For switching rule: | Name | Type | Description | | ----------------------------- | ------------------------------------------------------------ | --------------------------------- | | `/input/vector_map` | autoware_auto_mapping_msgs::msg::HADMapBin | vector map | | `/input/pose_with_covariance` | geometry_msgs::msg::PoseWithCovarianceStamped | localization final output | | `/input/initialization_state` | autoware_adapi_v1_msgs::msg::LocalizationInitializationState | localization initialization state | ### Publications | Name | Type | Description | | -------------------------------------- | --------------------------------------------- | ------------------------------------------------------ | | `/output/artag/image` | sensor_msgs::msg::Image | relayed ArTag input | | `/output/yabloc/image` | sensor_msgs::msg::Image | relayed YabLoc input | | `/output/eagleye/pose_with_covariance` | geometry_msgs::msg::PoseWithCovarianceStamped | relayed Eagleye output | | `/output/ndt/pointcloud` | sensor_msgs::msg::PointCloud2 | relayed NDT input | | `/output/debug/marker_array` | visualization_msgs::msg::MarkerArray | [debug topic] everything for visualization | | `/output/debug/string` | visualization_msgs::msg::MarkerArray | [debug topic] debug information such as current status |Trouble Shooting#
If it does not seems to work, users can get more information in the following ways.
[!TIP]
ros2 service call /localization/pose_estimator_arbiter/config_logger logging_demo/srv/ConfigLogger \ '{logger_name: localization.pose_estimator_arbiter, level: debug}'
Architecture#
Click to show details
### Case of running a single pose estimator When each pose_estimator is run alone, this package does nothing. Following figure shows the node configuration when NDT, YabLoc Eagleye and AR-Tag are run independently.
### Case of running multiple pose estimators
When running multiple pose_estimators, pose_estimator_arbiter is executed.
It comprises a **switching rule** and **stoppers** corresponding to each pose_estimator.
- Stoppers control the pose_estimator activity by relaying inputs or outputs, or by requesting a suspend service.
- Switching rules determine which pose_estimator to use.
Which stoppers and switching rules are instantiated depends on the runtime arguments at startup.
Following figure shows the node configuration when all pose_estimator are run simultaneously.
- **NDT**
The NDT stopper relays topics in the front side of the point cloud pre-processor.
- **YabLoc**
The YabLoc stopper relays input image topics in the frontend of the image pre-processor.
YabLoc includes a particle filter process that operates on a timer, and even when image topics are not streamed, the particle prediction process continues to work.
To address this, the YabLoc stopper also has a service client for explicitly stopping and resuming YabLoc.
- **Eagleye**
The Eagleye stopper relays Eagleye's output pose topics in the backend of Eagleye's estimation process.
Eagleye performs time-series processing internally, and it can't afford to stop the input stream.
Furthermore, Eagleye's estimation process is lightweight enough to be run continuously without a significant load, so the relay is inserted in the backend.
- **ArTag**
The ArTag stopper relays image topics in the front side of the landmark localizer.
How to launch#
Click to show details
The user can launch the desired pose_estimators by giving the pose_estimator names as a concatenation of underscores for the runtime argument `pose_source`.ros2 launch autoware_launch logging_simulator.launch.xml \
map_path:=<your-map-path> \
vehicle_model:=sample_vehicle \
sensor_model:=awsim_sensor_kit \
pose_source:=ndt_yabloc_artag_eagleye
Switching Rules#
Click to show details
Currently, **ONLY ONE RULE** (`enable_all_rule`) is implemented. In the future, several rules will be implemented and users will be able to select rules. > [!TIP] > There are presets available to extend the rules. If you want to extend the rules, please see [example_rule](./example_rule/README.md). ### Enable All Rule This is the default and simplest rule. This rule enables all pose_estimators regardless of their current state.flowchart LR
A{ }
A --whatever --> _A[enable all pose_estimators]Pose Initialization#
When using multiple pose_estimators, it is necessary to appropriately adjust the parameters provided to the pose_initializer.