diff --git a/launch/tier4_localization_launch/launch/localization.launch.xml b/launch/tier4_localization_launch/launch/localization.launch.xml index f4d6679291849..9f51f7e6ea7a1 100644 --- a/launch/tier4_localization_launch/launch/localization.launch.xml +++ b/launch/tier4_localization_launch/launch/localization.launch.xml @@ -1,10 +1,10 @@ - - + + diff --git a/launch/tier4_localization_launch/launch/pose_twist_estimator/ar_tag_based_localizer.launch.xml b/launch/tier4_localization_launch/launch/pose_twist_estimator/ar_tag_based_localizer.launch.xml index ac7589ea2273b..98f5fcab7c91a 100644 --- a/launch/tier4_localization_launch/launch/pose_twist_estimator/ar_tag_based_localizer.launch.xml +++ b/launch/tier4_localization_launch/launch/pose_twist_estimator/ar_tag_based_localizer.launch.xml @@ -1,9 +1,11 @@ + + - + diff --git a/launch/tier4_localization_launch/launch/pose_twist_estimator/pose_twist_estimator.launch.xml b/launch/tier4_localization_launch/launch/pose_twist_estimator/pose_twist_estimator.launch.xml index 02c6da20e17da..559c307aa2fb1 100644 --- a/launch/tier4_localization_launch/launch/pose_twist_estimator/pose_twist_estimator.launch.xml +++ b/launch/tier4_localization_launch/launch/pose_twist_estimator/pose_twist_estimator.launch.xml @@ -1,152 +1,116 @@ + + + + + + + + + + + + + + + + + + - - - - - - - - - - - - - - - - - - - - - - - + + + - - - - - - - - - - - - - - - - - - - - + + + + + + + + - - + + - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + - - + + + - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/tier4_localization_launch/package.xml b/launch/tier4_localization_launch/package.xml index 9b4f727c9ce52..a0a1cc1f6df64 100644 --- a/launch/tier4_localization_launch/package.xml +++ b/launch/tier4_localization_launch/package.xml @@ -27,6 +27,7 @@ gyro_odometer ndt_scan_matcher pointcloud_preprocessor + pose_estimator_arbiter pose_initializer pose_instability_detector topic_tools diff --git a/localization/pose_estimator_arbiter/CMakeLists.txt b/localization/pose_estimator_arbiter/CMakeLists.txt new file mode 100644 index 0000000000000..9a47b654a6ab4 --- /dev/null +++ b/localization/pose_estimator_arbiter/CMakeLists.txt @@ -0,0 +1,58 @@ +cmake_minimum_required(VERSION 3.14) +project(pose_estimator_arbiter) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +find_package(glog REQUIRED) + +find_package(PCL REQUIRED COMPONENTS common) +include_directories(SYSTEM ${PCL_INCLUDE_DIRS}) + +# ============================== +# switch rule library +ament_auto_add_library(switch_rule + SHARED + src/pose_estimator_arbiter/switch_rule/enable_all_rule.cpp +) +target_include_directories(switch_rule PUBLIC src) + +# ============================== +# pose estimator arbiter node +ament_auto_add_executable(${PROJECT_NAME} + src/pose_estimator_arbiter/pose_estimator_arbiter_core.cpp + src/pose_estimator_arbiter/pose_estimator_arbiter_node.cpp +) +target_include_directories(${PROJECT_NAME} PUBLIC src) +target_link_libraries(${PROJECT_NAME} switch_rule glog::glog) + +# ============================== +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + + # define test definition macro + function(add_testcase filepath) + get_filename_component(filename ${filepath} NAME) + string(REGEX REPLACE ".cpp" "" test_name ${filename}) + ament_add_gmock(${test_name} ${filepath}) + target_include_directories(${test_name} PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/src) + target_link_libraries(${test_name} fmt) + ament_target_dependencies(${test_name} ${${PROJECT_NAME}_FOUND_BUILD_DEPENDS}) + endfunction() + + add_testcase(test/test_shared_data.cpp) + add_ros_test( + test/test_pose_estimator_arbiter.py + TIMEOUT "30" + ) +endif() + +# ============================== +# In practice, the example rule is not used as autoware code. +# It exists only for user reference and is tested only. +add_subdirectory(example_rule) + +# ============================== +ament_auto_package(INSTALL_TO_SHARE + launch +) diff --git a/localization/pose_estimator_arbiter/README.md b/localization/pose_estimator_arbiter/README.md new file mode 100644 index 0000000000000..3a32d5883642e --- /dev/null +++ b/localization/pose_estimator_arbiter/README.md @@ -0,0 +1,284 @@ +# pose_estimator_arbiter + +Table of contents: + +- [Abstract](#abstract) +- [Interface](#interfaces) +- [Architecture](#architecture) +- [How to launch](#how-to-launch) +- [Switching Rules](#switching-rules) +- [Pose Initialization](#pose-initialization) +- [Future Plans](#future-plans) + +## 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](https://github.com/orgs/autowarefoundation/discussions/3878) 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](example_rule/README.md) has source code that can be used as a reference for implementing the rules. + +### Supporting pose_estimators + +- [ndt_scan_matcher](https://github.com/autowarefoundation/autoware.universe/tree/main/localization/ndt_scan_matcher) +- [eagleye](https://autowarefoundation.github.io/autoware-documentation/main/how-to-guides/integrating-autoware/launch-autoware/localization/eagleye/) +- [yabloc](https://github.com/autowarefoundation/autoware.universe/tree/main/localization/yabloc) +- [landmark_based_localizer](https://github.com/autowarefoundation/autoware.universe/tree/main/localization/landmark_based_localizer) + +### 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)](https://drive.google.com/file/d/1MxLo1Sw6PdvfkyOYf_9A5dZ9uli1vPvS/view) +The rosbag is simulated data created by [AWSIM](https://tier4.github.io/AWSIM/). +The map is an edited version of the [original map data](https://github.com/tier4/AWSIM/releases/download/v1.1.0/nishishinjuku_autoware_map.zip) published on the AWSIM documentation page to make it suitable for multiple pose_estimators. + +```bash +ros2 launch autoware_launch logging_simulator.launch.xml \ + 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] +> +> ```bash +> 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. + +drawing + +### 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. + +drawing + +- **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`. + +```bash +ros2 launch autoware_launch logging_simulator.launch.xml \ + map_path:= \ + vehicle_model:=sample_vehicle \ + sensor_model:=awsim_sensor_kit \ + pose_source:=ndt_yabloc_artag_eagleye +``` + +Even if `pose_source` includes an unexpected string, it will be filtered appropriately. +Please see the table below for details. + +| given runtime argument | parsed pose_estimator_arbiter's param (pose_sources) | +| ------------------------------------------- | ---------------------------------------------------- | +| `pose_source:=ndt` | `["ndt"]` | +| `pose_source:=nan` | `[]` | +| `pose_source:=yabloc_ndt` | `["ndt","yabloc"]` | +| `pose_source:=yabloc_ndt_ndt_ndt` | `["ndt","yabloc"]` | +| `pose_source:=ndt_yabloc_eagleye` | `["ndt","yabloc","eagleye"]` | +| `pose_source:=ndt_yabloc_nan_eagleye_artag` | `["ndt","yabloc","eagleye","artag"]` | + +
+ +## 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. + +```mermaid +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`. + +
+Click to show details + +The following table is based on the runtime argument "pose_source" indicating which initial pose estimation methods are available and the parameters that should be provided to the pose_initialization node. +To avoid making the application too complicated, a priority is established so that NDT is always used when it is available. +(The pose_initializer will only perform NDT-based initial pose estimation when `ndt_enabled` and `yabloc_enabled` are both `true`). + +This table's usage is described from three perspectives: + +- **Autoware Users:** Autoware users do not need to consult this table. + They simply provide the desired combinations of pose_estimators, and the appropriate parameters are automatically provided to the pose_initializer. +- **Autoware Developers:** Autoware developers can consult this table to know which parameters are assigned. +- **Who implements New Pose Estimator Switching:** + Developers must extend this table and implement the assignment of appropriate parameters to the pose_initializer. + +| pose_source | invoked initialization method | `ndt_enabled` | `yabloc_enabled` | `gnss_enabled` | `sub_gnss_pose_cov` | +| :-------------------------: | ----------------------------- | ------------- | ---------------- | -------------- | -------------------------------------------- | +| ndt | ndt | true | false | true | /sensing/gnss/pose_with_covariance | +| yabloc | yabloc | false | true | true | /sensing/gnss/pose_with_covariance | +| eagleye | vehicle needs run for a while | false | false | true | /localization/pose_estimator/eagleye/... | +| artag | 2D Pose Estimate (RViz) | false | false | true | /sensing/gnss/pose_with_covariance | +| ndt, yabloc | ndt | ndt | true | true | /sensing/gnss/pose_with_covariance | +| ndt, eagleye | ndt | ndt | false | true | /sensing/gnss/pose_with_covariance | +| ndt, artag | ndt | ndt | false | true | /sensing/gnss/pose_with_covariance | +| yabloc, eagleye | yabloc | false | true | true | /sensing/gnss/pose_with_covariance | +| yabloc, artag | yabloc | false | true | true | /sensing/gnss/pose_with_covariance | +| eagleye, artag | vehicle needs run for a while | false | false | true | /localization/pose_estimator/eagleye/pose... | +| ndt, yabloc, eagleye | ndt | ndt | true | true | /sensing/gnss/pose_with_covariance | +| ndt, eagleye, artag | ndt | ndt | false | true | /sensing/gnss/pose_with_covariance | +| yabloc, eagleye, artag | yabloc | ndt | true | true | /sensing/gnss/pose_with_covariance | +| ndt, yabloc, eagleye, artag | ndt | ndt | true | true | /sensing/gnss/pose_with_covariance | + +
+ +## Future Plans + +
+Click to show details + +### gradually switching + +In the future, this package will provide not only ON/OFF switching, but also a mechanism for low frequency operation, such as 50% NDT & 50% YabLoc. + +### stopper for pose_estimators to be added in the future + +The basic strategy is to realize ON/OFF switching by relaying the input or output topics of that pose_estimator. +If pose_estimator involves time-series processing with heavy computations, it's not possible to pause and resume with just topic relaying. + +In such cases, there may not be generally applicable solutions, but the following methods may help: + +1. Completely stop and **reinitialize** time-series processing, as seen in the case of YabLoc. +2. Subscribe to `localization/kinematic_state` and **keep updating states** to ensure that the estimation does not break (relying on the output of the active pose_estimator). +3. The multiple pose_estimator **does not support** that particular pose_estimator. + +Please note that this issue is fundamental to realizing multiple pose_estimators, and it will arise regardless of the architecture proposed in this case. + +
diff --git a/localization/pose_estimator_arbiter/example_rule/CMakeLists.txt b/localization/pose_estimator_arbiter/example_rule/CMakeLists.txt new file mode 100644 index 0000000000000..333f92842b860 --- /dev/null +++ b/localization/pose_estimator_arbiter/example_rule/CMakeLists.txt @@ -0,0 +1,31 @@ +# example switch rule library +ament_auto_add_library(example_rule + SHARED + src/pose_estimator_arbiter/switch_rule/pcd_map_based_rule.cpp + src/pose_estimator_arbiter/switch_rule/vector_map_based_rule.cpp + src/pose_estimator_arbiter/rule_helper/pcd_occupancy.cpp + src/pose_estimator_arbiter/rule_helper/pose_estimator_area.cpp +) +target_include_directories(example_rule PUBLIC src example_rule/src) +target_link_libraries(example_rule switch_rule) + +# ============================== +# define test definition macro +function(add_testcase filepath) + get_filename_component(filename ${filepath} NAME) + string(REGEX REPLACE ".cpp" "" test_name ${filename}) + + ament_add_gtest(${test_name} ${filepath}) + target_link_libraries("${test_name}" switch_rule example_rule) + target_include_directories(${test_name} PUBLIC src) + ament_target_dependencies(${test_name} ${${PROJECT_NAME}_FOUND_BUILD_DEPENDS}) +endfunction() + +# ============================== +# build test +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + add_testcase(test/test_rule_helper.cpp) + add_testcase(test/test_vector_map_based_rule.cpp) + add_testcase(test/test_pcd_map_based_rule.cpp) +endif() diff --git a/localization/pose_estimator_arbiter/example_rule/README.md b/localization/pose_estimator_arbiter/example_rule/README.md new file mode 100644 index 0000000000000..c5bb80912607f --- /dev/null +++ b/localization/pose_estimator_arbiter/example_rule/README.md @@ -0,0 +1,99 @@ +# example rule + +The example rule provides a sample rule for controlling the arbiter. By combining the provided rules, it is possible to achieve demonstrations as follows. Users can extend the rules as needed by referencing this code, allowing them to control the arbiter as desired. + +## Demonstration + +The following video demonstrates the switching of four different pose estimators. + +
+ +## Switching Rules + +### Pcd Map Based Rule + +```mermaid +flowchart LR + A{PCD is enough dense } + A --true--> B[enable NDT] + A --false--> C[enable YabLoc] +``` + +### Vector Map Based Rule + +```mermaid +flowchart LR + A{ } + A --whatever --> _A[When the ego vehicle is in a predetermined pose_estimator_area,\n it enables the corresponding pose_estamtor.] +``` + +### Rule helpers + +Rule helpers are auxiliary tools for describing switching rules. + +- [PCD occupancy](#pcd-occupancy) +- [Pose estimator area](#pose-estimator-area) + +#### PCD occupancy + +drawing + +#### Pose estimator area + +The pose_estimator_area is a planar area described by polygon in lanelet2. +The height of the area is meaningless; it judges if the projection of its self-position is contained within the polygon or not. + +drawing + +A sample pose_estimator_area is shown below. The values provided below are placeholders. +To be correctly read, the area should have the type "pose_estimator_specify" and the subtype should be one of ndt, yabloc, eagleye, or artag. + +```xml + + + + + + + + + + + + + + + + + + + + + + + + + +... + + + + + + + + + + + + + + + + + + + + + +``` diff --git a/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/rule_helper/grid_key.hpp b/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/rule_helper/grid_key.hpp new file mode 100644 index 0000000000000..c366a7f02f4fe --- /dev/null +++ b/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/rule_helper/grid_key.hpp @@ -0,0 +1,68 @@ +// Copyright 2023 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef POSE_ESTIMATOR_ARBITER__RULE_HELPER__GRID_KEY_HPP_ +#define POSE_ESTIMATOR_ARBITER__RULE_HELPER__GRID_KEY_HPP_ +#include + +#include + +#include + +namespace pose_estimator_arbiter::rule_helper +{ +struct GridKey +{ + const float unit_length = 10.f; + int x, y; + + GridKey() : x(0), y(0) {} + GridKey(float x, float y) : x(std::floor(x / unit_length)), y(std::floor(y / unit_length)) {} + + pcl::PointXYZ get_center_point() const + { + pcl::PointXYZ xyz; + xyz.x = unit_length * (static_cast(x) + 0.5f); + xyz.y = unit_length * (static_cast(y) + 0.5f); + xyz.z = 0.f; + return xyz; + } + + friend bool operator==(const GridKey & one, const GridKey & other) + { + return one.x == other.x && one.y == other.y; + } + friend bool operator!=(const GridKey & one, const GridKey & other) { return !(one == other); } +}; + +} // namespace pose_estimator_arbiter::rule_helper + +// This is for unordered_map and unordered_set +namespace std +{ +template <> +struct hash +{ +public: + size_t operator()(const pose_estimator_arbiter::rule_helper::GridKey & grid) const + { + std::size_t seed = 0; + boost::hash_combine(seed, grid.x); + boost::hash_combine(seed, grid.y); + return seed; + } +}; +} // namespace std + +#endif // POSE_ESTIMATOR_ARBITER__RULE_HELPER__GRID_KEY_HPP_ diff --git a/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/rule_helper/pcd_occupancy.cpp b/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/rule_helper/pcd_occupancy.cpp new file mode 100644 index 0000000000000..72071b23b467f --- /dev/null +++ b/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/rule_helper/pcd_occupancy.cpp @@ -0,0 +1,112 @@ +// Copyright 2023 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "pose_estimator_arbiter/rule_helper/pcd_occupancy.hpp" + +#include "pose_estimator_arbiter/rule_helper/grid_key.hpp" + +#include + +#include + +namespace pose_estimator_arbiter::rule_helper +{ +PcdOccupancy::PcdOccupancy(int pcd_density_upper_threshold, int pcd_density_lower_threshold) +: pcd_density_upper_threshold_(pcd_density_upper_threshold), + pcd_density_lower_threshold_(pcd_density_lower_threshold) +{ +} + +bool PcdOccupancy::ndt_can_operate( + const geometry_msgs::msg::Point & position, std::string * optional_message) const +{ + if (!kdtree_) { + if (optional_message) { + *optional_message = "pcd is not subscribed yet"; + } + return false; + } + + const pcl::PointXYZ query{ + static_cast(position.x), static_cast(position.y), static_cast(position.z)}; + std::vector indices; + std::vector distances; + const int count = kdtree_->radiusSearch(query, 50, indices, distances, 0); + + static bool last_is_ndt_mode = true; + const bool is_ndt_mode = (last_is_ndt_mode) ? (count > pcd_density_lower_threshold_) + : (count > pcd_density_upper_threshold_); + last_is_ndt_mode = is_ndt_mode; + + std::stringstream ss; + ss << "pcd occupancy: " << count << " > " + << (last_is_ndt_mode ? pcd_density_lower_threshold_ : pcd_density_upper_threshold_); + + if (optional_message) { + *optional_message = ss.str(); + } + + return is_ndt_mode; +} + +visualization_msgs::msg::MarkerArray PcdOccupancy::debug_marker_array() const +{ + visualization_msgs::msg::Marker msg; + msg.ns = "pcd_occupancy"; + msg.id = 0; + msg.header.frame_id = "map"; + msg.scale.set__x(3.0f).set__y(3.0f).set__z(3.f); + msg.color.set__r(1.0f).set__g(1.0f).set__b(0.2f).set__a(1.0f); + + if (occupied_areas_) { + for (auto p : occupied_areas_->points) { + geometry_msgs::msg::Point geometry_point{}; + geometry_point.set__x(p.x).set__y(p.y).set__z(p.z); + msg.points.push_back(geometry_point); + } + } + + visualization_msgs::msg::MarkerArray msg_array; + msg_array.markers.push_back(msg); + + return msg_array; +} + +void PcdOccupancy::init(PointCloud2::ConstSharedPtr msg) +{ + if (kdtree_) { + // already initialized + return; + } + + pcl::PointCloud cloud; + pcl::fromROSMsg(*msg, cloud); + + std::unordered_map grid_point_count; + for (pcl::PointXYZ xyz : cloud) { + grid_point_count[GridKey(xyz.x, xyz.y)] += 1; + } + + occupied_areas_ = std::make_shared>(); + for (const auto [grid, count] : grid_point_count) { + if (count > 50) { + occupied_areas_->push_back(grid.get_center_point()); + } + } + + kdtree_ = pcl::make_shared>(); + kdtree_->setInputCloud(occupied_areas_); +} + +} // namespace pose_estimator_arbiter::rule_helper diff --git a/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/rule_helper/pcd_occupancy.hpp b/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/rule_helper/pcd_occupancy.hpp new file mode 100644 index 0000000000000..e5eb4ff091a31 --- /dev/null +++ b/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/rule_helper/pcd_occupancy.hpp @@ -0,0 +1,52 @@ +// Copyright 2023 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef POSE_ESTIMATOR_ARBITER__RULE_HELPER__PCD_OCCUPANCY_HPP_ +#define POSE_ESTIMATOR_ARBITER__RULE_HELPER__PCD_OCCUPANCY_HPP_ + +#include + +#include +#include + +#include +#include +#include + +namespace pose_estimator_arbiter::rule_helper +{ +class PcdOccupancy +{ + using PointCloud2 = sensor_msgs::msg::PointCloud2; + using MarkerArray = visualization_msgs::msg::MarkerArray; + +public: + explicit PcdOccupancy(int pcd_density_upper_threshold, int pcd_density_lower_threshold); + + MarkerArray debug_marker_array() const; + void init(PointCloud2::ConstSharedPtr msg); + bool ndt_can_operate( + const geometry_msgs::msg::Point & position, std::string * optional_message = nullptr) const; + +private: + const int pcd_density_upper_threshold_; + const int pcd_density_lower_threshold_; + + pcl::PointCloud::Ptr occupied_areas_{nullptr}; + pcl::KdTreeFLANN::Ptr kdtree_{nullptr}; +}; + +} // namespace pose_estimator_arbiter::rule_helper + +#endif // POSE_ESTIMATOR_ARBITER__RULE_HELPER__PCD_OCCUPANCY_HPP_ diff --git a/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/rule_helper/pose_estimator_area.cpp b/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/rule_helper/pose_estimator_area.cpp new file mode 100644 index 0000000000000..4eea34f9ae1ee --- /dev/null +++ b/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/rule_helper/pose_estimator_area.cpp @@ -0,0 +1,144 @@ +// Copyright 2023 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "pose_estimator_arbiter/rule_helper/pose_estimator_area.hpp" + +#include +#include +#include + +#include + +#include + +#include +#include +#include + +namespace pose_estimator_arbiter::rule_helper +{ +using BoostPoint = boost::geometry::model::d2::point_xy; +using BoostPolygon = boost::geometry::model::polygon; + +struct PoseEstimatorArea::Impl +{ + explicit Impl(rclcpp::Logger logger) : logger_(logger) {} + std::multimap bounding_boxes_; + + void init(HADMapBin::ConstSharedPtr msg); + bool within( + const geometry_msgs::msg::Point & point, const std::string & pose_estimator_name) const; + + MarkerArray debug_marker_array() const { return marker_array_; } + +private: + rclcpp::Logger logger_; + MarkerArray marker_array_; +}; + +PoseEstimatorArea::PoseEstimatorArea(const rclcpp::Logger & logger) : logger_(logger) +{ + impl_ = std::make_shared(logger_); +} + +PoseEstimatorArea::PoseEstimatorArea(rclcpp::Node * node) : PoseEstimatorArea(node->get_logger()) +{ +} + +void PoseEstimatorArea::init(HADMapBin::ConstSharedPtr msg) +{ + impl_->init(msg); +} + +bool PoseEstimatorArea::within( + const geometry_msgs::msg::Point & point, const std::string & pose_estimator_name) const +{ + return impl_->within(point, pose_estimator_name); +} + +PoseEstimatorArea::MarkerArray PoseEstimatorArea::debug_marker_array() const +{ + return impl_->debug_marker_array(); +} + +void PoseEstimatorArea::Impl::init(HADMapBin::ConstSharedPtr msg) +{ + if (!bounding_boxes_.empty()) { + // already initialized + return; + } + + lanelet::LaneletMapPtr lanelet_map(new lanelet::LaneletMap); + lanelet::utils::conversion::fromBinMsg(*msg, lanelet_map); + + const auto & polygon_layer = lanelet_map->polygonLayer; + RCLCPP_DEBUG_STREAM(logger_, "Polygon layer size: " << polygon_layer.size()); + + const std::string pose_estimator_specifying_attribute = "pose_estimator_specify"; + for (const auto & polygon : polygon_layer) { + // + const std::string type{polygon.attributeOr(lanelet::AttributeName::Type, "none")}; + RCLCPP_DEBUG_STREAM(logger_, "polygon type: " << type); + if (pose_estimator_specifying_attribute != type) { + continue; + } + + // + const std::string subtype{polygon.attributeOr(lanelet::AttributeName::Subtype, "none")}; + RCLCPP_DEBUG_STREAM(logger_, "polygon sub type: " << subtype); + + // Create a marker for visualization + Marker marker; + marker.type = Marker::LINE_STRIP; + marker.scale.set__x(0.2f).set__y(0.2f).set__z(0.2f); + marker.color.set__r(1.0f).set__g(1.0f).set__b(0.0f).set__a(1.0f); + marker.ns = subtype; + marker.header.frame_id = "map"; + marker.id = marker_array_.markers.size(); + + // Enqueue the vertices of the polygon to bounding box and visualization marker + BoostPolygon poly; + for (const lanelet::ConstPoint3d & p : polygon) { + poly.outer().push_back(BoostPoint(p.x(), p.y())); + + geometry_msgs::msg::Point point_msg; + point_msg.set__x(p.x()).set__y(p.y()).set__z(p.z()); + marker.points.push_back(point_msg); + } + + // Push the first vertex again to enclose the polygon + poly.outer().push_back(poly.outer().front()); + marker.points.push_back(marker.points.front()); + + // Push back the items + bounding_boxes_.emplace(subtype, poly); + marker_array_.markers.push_back(marker); + } +} + +bool PoseEstimatorArea::Impl::within( + const geometry_msgs::msg::Point & point, const std::string & pose_estimator_name) const +{ + const BoostPoint boost_point(point.x, point.y); + + auto range = bounding_boxes_.equal_range(pose_estimator_name); + + for (auto itr = range.first; itr != range.second; ++itr) { + if (!boost::geometry::disjoint(itr->second, boost_point)) { + return true; + } + } + return false; +} +} // namespace pose_estimator_arbiter::rule_helper diff --git a/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/rule_helper/pose_estimator_area.hpp b/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/rule_helper/pose_estimator_area.hpp new file mode 100644 index 0000000000000..d6901f9be2dbf --- /dev/null +++ b/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/rule_helper/pose_estimator_area.hpp @@ -0,0 +1,56 @@ +// Copyright 2023 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef POSE_ESTIMATOR_ARBITER__RULE_HELPER__POSE_ESTIMATOR_AREA_HPP_ +#define POSE_ESTIMATOR_ARBITER__RULE_HELPER__POSE_ESTIMATOR_AREA_HPP_ + +#include +#include + +#include +#include +#include + +#include +#include + +namespace pose_estimator_arbiter::rule_helper +{ +class PoseEstimatorArea +{ + using HADMapBin = autoware_auto_mapping_msgs::msg::HADMapBin; + using Marker = visualization_msgs::msg::Marker; + using MarkerArray = visualization_msgs::msg::MarkerArray; + +public: + explicit PoseEstimatorArea(rclcpp::Node * node); + explicit PoseEstimatorArea(const rclcpp::Logger & logger); + + // This is assumed to be called only once in the vector map callback. + void init(const HADMapBin::ConstSharedPtr msg); + + bool within( + const geometry_msgs::msg::Point & point, const std::string & pose_estimator_name) const; + + MarkerArray debug_marker_array() const; + +private: + struct Impl; + std::shared_ptr impl_{nullptr}; + rclcpp::Logger logger_; +}; + +} // namespace pose_estimator_arbiter::rule_helper + +#endif // POSE_ESTIMATOR_ARBITER__RULE_HELPER__POSE_ESTIMATOR_AREA_HPP_ diff --git a/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/switch_rule/pcd_map_based_rule.cpp b/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/switch_rule/pcd_map_based_rule.cpp new file mode 100644 index 0000000000000..4c3829316230b --- /dev/null +++ b/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/switch_rule/pcd_map_based_rule.cpp @@ -0,0 +1,76 @@ +// Copyright 2023 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "pose_estimator_arbiter/switch_rule/pcd_map_based_rule.hpp" + +#include + +namespace pose_estimator_arbiter::switch_rule +{ +PcdMapBasedRule::PcdMapBasedRule( + rclcpp::Node & node, const std::unordered_set & running_estimator_list, + const std::shared_ptr shared_data) +: BaseSwitchRule(node), running_estimator_list_(running_estimator_list), shared_data_(shared_data) +{ + const int pcd_density_upper_threshold = + tier4_autoware_utils::getOrDeclareParameter(node, "pcd_density_upper_threshold"); + const int pcd_density_lower_threshold = + tier4_autoware_utils::getOrDeclareParameter(node, "pcd_density_lower_threshold"); + + pcd_occupancy_ = std::make_unique( + pcd_density_upper_threshold, pcd_density_lower_threshold); + + // Register callback + shared_data_->point_cloud_map.register_callback( + [this](sensor_msgs::msg::PointCloud2::ConstSharedPtr msg) -> void { + pcd_occupancy_->init(msg); + }); +} + +PcdMapBasedRule::MarkerArray PcdMapBasedRule::debug_marker_array() +{ + MarkerArray array_msg; + + if (pcd_occupancy_) { + const auto & additional = pcd_occupancy_->debug_marker_array().markers; + array_msg.markers.insert(array_msg.markers.end(), additional.begin(), additional.end()); + } + + return array_msg; +} + +std::unordered_map PcdMapBasedRule::update() +{ + const auto position = shared_data_->localization_pose_cov()->pose.pose.position; + const bool ndt_can_operate = pcd_occupancy_->ndt_can_operate(position); + + if (ndt_can_operate) { + debug_string_ = "Enable ndt: "; + return { + {PoseEstimatorType::ndt, true}, + {PoseEstimatorType::yabloc, false}, + {PoseEstimatorType::eagleye, false}, + {PoseEstimatorType::artag, false}, + }; + } else { + debug_string_ = "Enable yabloc: "; + return { + {PoseEstimatorType::ndt, false}, + {PoseEstimatorType::yabloc, true}, + {PoseEstimatorType::eagleye, false}, + {PoseEstimatorType::artag, false}}; + } +} + +} // namespace pose_estimator_arbiter::switch_rule diff --git a/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/switch_rule/pcd_map_based_rule.hpp b/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/switch_rule/pcd_map_based_rule.hpp new file mode 100644 index 0000000000000..23fd0f812f700 --- /dev/null +++ b/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/switch_rule/pcd_map_based_rule.hpp @@ -0,0 +1,54 @@ +// Copyright 2023 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef POSE_ESTIMATOR_ARBITER__SWITCH_RULE__PCD_MAP_BASED_RULE_HPP_ +#define POSE_ESTIMATOR_ARBITER__SWITCH_RULE__PCD_MAP_BASED_RULE_HPP_ + +#include "pose_estimator_arbiter/pose_estimator_type.hpp" +#include "pose_estimator_arbiter/rule_helper/pcd_occupancy.hpp" +#include "pose_estimator_arbiter/shared_data.hpp" +#include "pose_estimator_arbiter/switch_rule/base_switch_rule.hpp" + +#include +#include +#include +#include + +namespace pose_estimator_arbiter::switch_rule +{ +class PcdMapBasedRule : public BaseSwitchRule +{ +public: + PcdMapBasedRule( + rclcpp::Node & node, const std::unordered_set & running_estimator_list, + const std::shared_ptr shared_data); + + std::unordered_map update() override; + + std::string debug_string() override { return debug_string_; } + + MarkerArray debug_marker_array() override; + +protected: + const std::unordered_set running_estimator_list_; + std::shared_ptr shared_data_{nullptr}; + + std::unique_ptr pcd_occupancy_{nullptr}; + + // Store the reason why which pose estimator is enabled + mutable std::string debug_string_; +}; +} // namespace pose_estimator_arbiter::switch_rule + +#endif // POSE_ESTIMATOR_ARBITER__SWITCH_RULE__PCD_MAP_BASED_RULE_HPP_ diff --git a/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/switch_rule/vector_map_based_rule.cpp b/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/switch_rule/vector_map_based_rule.cpp new file mode 100644 index 0000000000000..094434c62ac9c --- /dev/null +++ b/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/switch_rule/vector_map_based_rule.cpp @@ -0,0 +1,75 @@ +// Copyright 2023 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "pose_estimator_arbiter/switch_rule/vector_map_based_rule.hpp" + +#include + +namespace pose_estimator_arbiter::switch_rule +{ +VectorMapBasedRule::VectorMapBasedRule( + rclcpp::Node & node, const std::unordered_set & running_estimator_list, + const std::shared_ptr shared_data) +: BaseSwitchRule(node), running_estimator_list_(running_estimator_list), shared_data_(shared_data) +{ + pose_estimator_area_ = std::make_unique(node.get_logger()); + + // Register callback + shared_data_->vector_map.register_callback( + [this](autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg) -> void { + pose_estimator_area_->init(msg); + }); + + RCLCPP_INFO_STREAM(get_logger(), "VectorMapBasedRule is initialized successfully"); +} + +VectorMapBasedRule::MarkerArray VectorMapBasedRule::debug_marker_array() +{ + MarkerArray array_msg; + + if (pose_estimator_area_) { + const auto & additional = pose_estimator_area_->debug_marker_array().markers; + array_msg.markers.insert(array_msg.markers.end(), additional.begin(), additional.end()); + } + + return array_msg; +} + +std::unordered_map VectorMapBasedRule::update() +{ + const auto ego_position = shared_data_->localization_pose_cov()->pose.pose.position; + + std::unordered_map enable_list; + bool at_least_one_is_enabled = false; + for (const auto & estimator_type : running_estimator_list_) { + const std::string estimator_name{magic_enum::enum_name(estimator_type)}; + const bool result = pose_estimator_area_->within(ego_position, estimator_name); + enable_list.emplace(estimator_type, result); + + at_least_one_is_enabled |= result; + } + if (at_least_one_is_enabled) { + debug_string_ = + "Enable at least one pose_estimators: self vehicle is within the area of at least one " + "pose_estimator_area"; + } else { + debug_string_ = + "Enable no pose_estimator: self vehicle is out of the area of all pose_estimator_area"; + } + RCLCPP_DEBUG(get_logger(), "%s", debug_string_.c_str()); + + return enable_list; +} + +} // namespace pose_estimator_arbiter::switch_rule diff --git a/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/switch_rule/vector_map_based_rule.hpp b/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/switch_rule/vector_map_based_rule.hpp new file mode 100644 index 0000000000000..e3360f9692f86 --- /dev/null +++ b/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/switch_rule/vector_map_based_rule.hpp @@ -0,0 +1,55 @@ +// Copyright 2023 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef POSE_ESTIMATOR_ARBITER__SWITCH_RULE__VECTOR_MAP_BASED_RULE_HPP_ +#define POSE_ESTIMATOR_ARBITER__SWITCH_RULE__VECTOR_MAP_BASED_RULE_HPP_ + +#include "pose_estimator_arbiter/pose_estimator_type.hpp" +#include "pose_estimator_arbiter/rule_helper/pose_estimator_area.hpp" +#include "pose_estimator_arbiter/shared_data.hpp" +#include "pose_estimator_arbiter/switch_rule/base_switch_rule.hpp" + +#include +#include +#include +#include +#include + +namespace pose_estimator_arbiter::switch_rule +{ +class VectorMapBasedRule : public BaseSwitchRule +{ +public: + VectorMapBasedRule( + rclcpp::Node & node, const std::unordered_set & running_estimator_list, + const std::shared_ptr shared_data); + + std::unordered_map update() override; + + std::string debug_string() override { return debug_string_; } + + MarkerArray debug_marker_array() override; + +protected: + const std::unordered_set running_estimator_list_; + std::shared_ptr shared_data_{nullptr}; + + // Store the reason why which pose estimator is enabled + mutable std::string debug_string_; + + std::unique_ptr pose_estimator_area_{nullptr}; +}; +} // namespace pose_estimator_arbiter::switch_rule + +#endif // POSE_ESTIMATOR_ARBITER__SWITCH_RULE__VECTOR_MAP_BASED_RULE_HPP_ diff --git a/localization/pose_estimator_arbiter/example_rule/test/test_pcd_map_based_rule.cpp b/localization/pose_estimator_arbiter/example_rule/test/test_pcd_map_based_rule.cpp new file mode 100644 index 0000000000000..5febfa403363e --- /dev/null +++ b/localization/pose_estimator_arbiter/example_rule/test/test_pcd_map_based_rule.cpp @@ -0,0 +1,100 @@ +// Copyright 2023 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "pose_estimator_arbiter/switch_rule/pcd_map_based_rule.hpp" + +#include +#include + +#include + +class MockNode : public ::testing::Test +{ +protected: + virtual void SetUp() + { + rclcpp::init(0, nullptr); + node = std::make_shared("test_node"); + node->declare_parameter("pcd_density_lower_threshold", 1); + node->declare_parameter("pcd_density_upper_threshold", 5); + + const auto running_estimator_list = + std::unordered_set{ + pose_estimator_arbiter::PoseEstimatorType::ndt, + pose_estimator_arbiter::PoseEstimatorType::yabloc, + pose_estimator_arbiter::PoseEstimatorType::eagleye, + pose_estimator_arbiter::PoseEstimatorType::artag}; + + shared_data_ = std::make_shared(); + + rule_ = std::make_shared( + *node, running_estimator_list, shared_data_); + } + + std::shared_ptr node; + std::shared_ptr shared_data_; + std::shared_ptr rule_; + + virtual void TearDown() { rclcpp::shutdown(); } +}; + +TEST_F(MockNode, pcdMapBasedRule) +{ + // Create dummy pcd and set + { + pcl::PointCloud cloud; + for (float x = -10; x < 10.0; x += 0.2) { + for (float y = -10; y < 10.0; y += 0.2) { + cloud.push_back(pcl::PointXYZ(x, y, 0)); + } + } + + using PointCloud2 = sensor_msgs::msg::PointCloud2; + PointCloud2 msg; + pcl::toROSMsg(cloud, msg); + + // Set + shared_data_->point_cloud_map.set_and_invoke(std::make_shared(msg)); + } + + using PoseCovStamped = geometry_msgs::msg::PoseWithCovarianceStamped; + auto create_pose = [](double x, double y) -> PoseCovStamped { + PoseCovStamped msg; + msg.pose.pose.position.x = x; + msg.pose.pose.position.y = y; + return msg; + }; + + { + auto position = create_pose(5, 5); + shared_data_->localization_pose_cov.set_and_invoke( + std::make_shared(position)); + auto ret = rule_->update(); + EXPECT_TRUE(ret.at(pose_estimator_arbiter::PoseEstimatorType::ndt)); + EXPECT_FALSE(ret.at(pose_estimator_arbiter::PoseEstimatorType::yabloc)); + EXPECT_FALSE(ret.at(pose_estimator_arbiter::PoseEstimatorType::eagleye)); + EXPECT_FALSE(ret.at(pose_estimator_arbiter::PoseEstimatorType::artag)); + } + + { + auto position = create_pose(100, 100); + shared_data_->localization_pose_cov.set_and_invoke( + std::make_shared(position)); + auto ret = rule_->update(); + EXPECT_FALSE(ret.at(pose_estimator_arbiter::PoseEstimatorType::ndt)); + EXPECT_TRUE(ret.at(pose_estimator_arbiter::PoseEstimatorType::yabloc)); + EXPECT_FALSE(ret.at(pose_estimator_arbiter::PoseEstimatorType::eagleye)); + EXPECT_FALSE(ret.at(pose_estimator_arbiter::PoseEstimatorType::artag)); + } +} diff --git a/localization/pose_estimator_arbiter/example_rule/test/test_rule_helper.cpp b/localization/pose_estimator_arbiter/example_rule/test/test_rule_helper.cpp new file mode 100644 index 0000000000000..35ed8b04bfcad --- /dev/null +++ b/localization/pose_estimator_arbiter/example_rule/test/test_rule_helper.cpp @@ -0,0 +1,106 @@ +// Copyright 2023 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "pose_estimator_arbiter/rule_helper/grid_key.hpp" +#include "pose_estimator_arbiter/rule_helper/pcd_occupancy.hpp" +#include "pose_estimator_arbiter/rule_helper/pose_estimator_area.hpp" + +#include + +#include + +#include + +#include +#include +#include + +#include + +class MockNode : public ::testing::Test +{ +protected: + virtual void SetUp() + { + rclcpp::init(0, nullptr); + node = std::make_shared("test_node"); + } + + std::shared_ptr node{nullptr}; + + virtual void TearDown() { rclcpp::shutdown(); } +}; + +TEST_F(MockNode, poseEstimatorArea) +{ + auto create_polygon3d = []() -> lanelet::Polygon3d { + lanelet::Polygon3d polygon; + polygon.setAttribute(lanelet::AttributeName::Type, {"pose_estimator_specify"}); + polygon.setAttribute(lanelet::AttributeName::Subtype, {"ndt"}); + lanelet::Id index = 0; + polygon.push_back(lanelet::Point3d(index++, 0, 0)); + polygon.push_back(lanelet::Point3d(index++, 10, 0)); + polygon.push_back(lanelet::Point3d(index++, 10, 10)); + polygon.push_back(lanelet::Point3d(index++, 0, 10)); + return polygon; + }; + + lanelet::LaneletMapPtr lanelet_map(new lanelet::LaneletMap); + lanelet_map->add(create_polygon3d()); + + using HADMapBin = autoware_auto_mapping_msgs::msg::HADMapBin; + using Point = geometry_msgs::msg::Point; + HADMapBin msg; + lanelet::utils::conversion::toBinMsg(lanelet_map, &msg); + + pose_estimator_arbiter::rule_helper::PoseEstimatorArea pose_estimator_area(&(*node)); + pose_estimator_area.init(std::make_shared(msg)); + + EXPECT_TRUE(pose_estimator_area.within(Point().set__x(5).set__y(5).set__z(0), "ndt")); + EXPECT_FALSE(pose_estimator_area.within(Point().set__x(5).set__y(5).set__z(0), "yabloc")); + EXPECT_FALSE(pose_estimator_area.within(Point().set__x(-5).set__y(-5).set__z(0), "ndt")); + EXPECT_FALSE(pose_estimator_area.within(Point().set__x(-5).set__y(-5).set__z(0), "yabloc")); +} + +TEST_F(MockNode, pcdOccupancy) +{ + using pose_estimator_arbiter::rule_helper::PcdOccupancy; + const int pcd_density_upper_threshold = 20; + const int pcd_density_lower_threshold = 10; + + pose_estimator_arbiter::rule_helper::PcdOccupancy pcd_occupancy( + pcd_density_upper_threshold, pcd_density_lower_threshold); + + geometry_msgs::msg::Point point; + std::string message; + + // Since we have not yet given a point cloud, this returns false. + EXPECT_FALSE(pcd_occupancy.ndt_can_operate(point, &message)); +} + +TEST_F(MockNode, gridKey) +{ + using pose_estimator_arbiter::rule_helper::GridKey; + EXPECT_TRUE(GridKey(10., -5.) == GridKey(10., -10.)); + EXPECT_TRUE(GridKey(10., -5.) != GridKey(10., -15.)); + + EXPECT_TRUE(GridKey(10., -5.).get_center_point().x == 15.f); + EXPECT_TRUE(GridKey(10., -5.).get_center_point().y == -5.f); + EXPECT_TRUE(GridKey(10., -5.).get_center_point().z == 0.f); + + std::unordered_set set; + set.emplace(10., -5.); + EXPECT_EQ(set.count(GridKey(10., -5.)), 1ul); + EXPECT_EQ(set.count(GridKey(10., -15.)), 0ul); +} diff --git a/localization/pose_estimator_arbiter/example_rule/test/test_vector_map_based_rule.cpp b/localization/pose_estimator_arbiter/example_rule/test/test_vector_map_based_rule.cpp new file mode 100644 index 0000000000000..a0a983e2ad3b7 --- /dev/null +++ b/localization/pose_estimator_arbiter/example_rule/test/test_vector_map_based_rule.cpp @@ -0,0 +1,106 @@ +// Copyright 2023 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "pose_estimator_arbiter/switch_rule/vector_map_based_rule.hpp" + +#include + +#include + +#include +#include +#include + +#include + +class MockNode : public ::testing::Test +{ +protected: + virtual void SetUp() + { + rclcpp::init(0, nullptr); + node = std::make_shared("test_node"); + + const auto running_estimator_list = + std::unordered_set{ + pose_estimator_arbiter::PoseEstimatorType::ndt, + pose_estimator_arbiter::PoseEstimatorType::yabloc, + pose_estimator_arbiter::PoseEstimatorType::eagleye, + pose_estimator_arbiter::PoseEstimatorType::artag}; + + shared_data_ = std::make_shared(); + + rule_ = std::make_shared( + *node, running_estimator_list, shared_data_); + } + + std::shared_ptr node; + std::shared_ptr shared_data_; + std::shared_ptr rule_; + + virtual void TearDown() { rclcpp::shutdown(); } +}; + +TEST_F(MockNode, vectorMapBasedRule) +{ + // Create dummy lanelet2 and set + { + auto create_polygon3d = []() -> lanelet::Polygon3d { + lanelet::Polygon3d polygon; + polygon.setAttribute(lanelet::AttributeName::Type, {"pose_estimator_specify"}); + polygon.setAttribute(lanelet::AttributeName::Subtype, {"ndt"}); + lanelet::Id index = 0; + polygon.push_back(lanelet::Point3d(index++, 0, 0)); + polygon.push_back(lanelet::Point3d(index++, 10, 0)); + polygon.push_back(lanelet::Point3d(index++, 10, 10)); + polygon.push_back(lanelet::Point3d(index++, 0, 10)); + return polygon; + }; + + lanelet::LaneletMapPtr lanelet_map(new lanelet::LaneletMap); + lanelet_map->add(create_polygon3d()); + + using HADMapBin = autoware_auto_mapping_msgs::msg::HADMapBin; + HADMapBin msg; + lanelet::utils::conversion::toBinMsg(lanelet_map, &msg); + + // Set + shared_data_->vector_map.set_and_invoke(std::make_shared(msg)); + } + + using PoseCovStamped = geometry_msgs::msg::PoseWithCovarianceStamped; + auto create_pose = [](double x, double y) -> PoseCovStamped::ConstSharedPtr { + PoseCovStamped msg; + msg.pose.pose.position.x = x; + msg.pose.pose.position.y = y; + return std::make_shared(msg); + }; + + { + shared_data_->localization_pose_cov.set_and_invoke(create_pose(5, 5)); + auto ret = rule_->update(); + EXPECT_TRUE(ret.at(pose_estimator_arbiter::PoseEstimatorType::ndt)); + EXPECT_FALSE(ret.at(pose_estimator_arbiter::PoseEstimatorType::yabloc)); + EXPECT_FALSE(ret.at(pose_estimator_arbiter::PoseEstimatorType::eagleye)); + EXPECT_FALSE(ret.at(pose_estimator_arbiter::PoseEstimatorType::artag)); + } + { + shared_data_->localization_pose_cov.set_and_invoke(create_pose(15, 15)); + auto ret = rule_->update(); + EXPECT_FALSE(ret.at(pose_estimator_arbiter::PoseEstimatorType::ndt)); + EXPECT_FALSE(ret.at(pose_estimator_arbiter::PoseEstimatorType::yabloc)); + EXPECT_FALSE(ret.at(pose_estimator_arbiter::PoseEstimatorType::eagleye)); + EXPECT_FALSE(ret.at(pose_estimator_arbiter::PoseEstimatorType::artag)); + } +} diff --git a/localization/pose_estimator_arbiter/launch/pose_estimator_arbiter.launch.xml b/localization/pose_estimator_arbiter/launch/pose_estimator_arbiter.launch.xml new file mode 100644 index 0000000000000..0a708e3f48988 --- /dev/null +++ b/localization/pose_estimator_arbiter/launch/pose_estimator_arbiter.launch.xml @@ -0,0 +1,29 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/localization/pose_estimator_arbiter/media/architecture.drawio.svg b/localization/pose_estimator_arbiter/media/architecture.drawio.svg new file mode 100644 index 0000000000000..4a7a7fe355775 --- /dev/null +++ b/localization/pose_estimator_arbiter/media/architecture.drawio.svg @@ -0,0 +1,957 @@ + + + + + + + +
+
+
+ legend +
+
+
+ + legend + + + + + + +
+
+
+ pose_estimator_arbiter +
+
+
+ + pose_estimator_arbiter + + + + + + + +
+
+
+ /pose_estimator +
+ /pose_with_covariance +
+
+
+ + /pose_estimator... + + + + + + + +
+
+
+ /util/downsampled/pointcloud +
+
+
+ + /util/downsampled/pointcloud + + + + + + +
+
+
+ pointcluod_ +
+ preprocessor +
+
+
+ + pointcluod_... + + + + + + + +
+
+
+ /pose_estimator/yabloc/* +
+
+
+ + /pose_estimator/yabloc/* + + + + + + +
+
+
+ image_ +
+ preprocessor +
+
+
+ + image_... + + + + + + + +
+
+
+ /pose_estimator +
+ /pose_with_covariance +
+
+
+ + /pose_estimator... + + + + + + +
+
+
+ ndt_scan_matcher +
+
+
+ + ndt_scan_matcher + + + + + + + +
+
+
+ /pose_estimator +
+ /pose_with_covariance +
+
+
+ + /pose_estimator... + + + + + + +
+
+
+ yabloc_particle_filter +
+
+
+ + yabloc_particle_filt... + + + + + + + +
+
+
+ /pose_with_covariance +
+
+
+ + /pose_with_covariance + + + + + + + +
+
+
+ /twist_estimator +
+ /twist_with_covariance +
+
+
+ + /twist_estimator... + + + + + + +
+
+
+ twist_ +
+ estimator +
+
+
+ + twist_... + + + + + + + +
+
+
+ /sensing/camera +
+ /traffic_light/image_raw/relay +
+
+
+ + /sensing/camera... + + + + + + + + +
+
+
+ /sensing/lidar/top +
+ /outlier_filtered/pointcloud +
+
+
+ + /sensing/lidar/top... + + + + + + + +
+
+
+ /pose_estimator/eagleye +
+ /pose_with_covariance +
+ /to_relay +
+
+
+ + /pose_estimator/eagleye... + + + + + + +
+
+
+ eagleye +
+
+
+ + eagleye + + + + + + + +
+
+
+ /map/vector_map, +
+ /map/pointcloud_map, +
+ /initialization_state, +
+ /tf +
+
+
+ + /map/vector_map,... + + + + + + + +
+
+
+ /sensing/lidar/top +
+ /outlier_filtered/pointcloud/relay +
+
+
+ + /sensing/lidar/top... + + + + + + + + +
+
+
+ /sensing/camera +
+ /traffic_light/image_raw +
+
+
+ + /sensing/camera... + + + + + + +
+
+
+ EKF +
+ (pose_twist_fusion_filter) +
+
+
+ + EKF... + + + + + + + + +
+
+
+ /sensing/* +
+
+
+ + /sensing/* + + + + + + +
+
+
+ eagleye_stopper +
+
+
+ + eagleye_stopper + + + + + + + +
+
+
+ yabloc_suspend_srv +
+
+
+ + yabloc_suspend_srv + + + + + + +
+
+
+ yabloc_stopper +
+
+
+ + yabloc_stopper + + + + + + +
+
+
+ ndt_stopper +
+
+
+ + ndt_stopper + + + + + + +
+
+
+ switching rule +
+
+
+ + switching rule + + + + + + +
+
+
+ node +
+
+
+ + node + + + + + + +
+
+
+ module +
+
+
+ + module + + + + + + +
+
+
+ pointcluod_ +
+ preprocessor +
+
+
+ + pointcluod_... + + + + + + +
+
+
+ pointcluod_ +
+ preprocessor +
+
+
+ + pointcluod_... + + + + + + +
+
+
+ image_ +
+ preprocessor +
+
+
+ + image_... + + + + + + +
+
+
+ image_ +
+ preprocessor +
+
+
+ + image_... + + + + + + +
+
+
+ eagleye +
+
+
+ + eagleye + + + + + + +
+
+
+ eagleye +
+
+
+ + eagleye + + + + + + +
+
+
+ node +
+
+
+ + node + + + + + + +
+
+
+ node +
+
+
+ + node + + + + + + +
+
+
+ some nodes +
+
+
+ + some nodes + + + + + + +
+
+
+ NDT & YabLoc & Eagleye & AR-Tag +
+
+
+ + + NDT & YabLoc & Eagleye & AR-Tag + + + + + + + + +
+
+
+ base +
+ stopper +
+
+
+ + base... + + + + + + +
+
+
+ stopper +
+
+
+ + stopper + + + + + + + +
+
+
+ Inheritance +
+
+
+ + Inheritance + + + + + +
+
+
+ It has ON/OFF inteface +
+
+
+ + It has ON/OFF inteface + + + + + +
+
+
+ It has pose_estimator- +
+ specific features +
+
+
+ + It has pose_estimator-... + + + + + + +
+
+
+ artag_stopper +
+
+
+ + artag_stopper + + + + + + +
+
+
+ artag +
+
+
+ + artag + + + + + + + +
+
+
+ /sensing/camera +
+ /traffic_light/image_raw/relay +
+
+
+ + /sensing/camera... + + + + + + + +
+
+
+ /pose_with_covariance +
+
+
+ + /pose_with_covariance + + + + + + + + +
+
+
+ /sensing/camera +
+ /traffic_light/image_raw +
+
+
+ + /sensing/camera... + + + + + + + Text is not SVG - cannot display + + + diff --git a/localization/pose_estimator_arbiter/media/pcd_occupancy.drawio.svg b/localization/pose_estimator_arbiter/media/pcd_occupancy.drawio.svg new file mode 100644 index 0000000000000..6a20728e27fd9 --- /dev/null +++ b/localization/pose_estimator_arbiter/media/pcd_occupancy.drawio.svg @@ -0,0 +1,878 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ 1 +
+
+
+ + 1 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ 1 +
+
+
+ + 1 + + + + + + + +
+
+
+ 2 +
+
+
+ + 2 + + + + + + + + + + + + + + + + + + + +
+
+
+ 3 +
+
+
+ + 3 + + + + + + + +
+
+
+ 4 +
+
+
+ + 4 + + + + + + + +
+
+
+ 5 +
+
+
+ + 5 + + + + + + + + + + + + + + + + + + + +
+
+
+ 6 +
+
+
+ + 6 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ point cloud +
+
+
+ + point cloud + + + + + + +
+
+
+ non points area +
+
+
+ + non points area + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ occupied grids +
+
+
+ + occupied grids + + + + + + +
+
+
+ not occupied grids +
+
+
+ + not occupied grids + + + + + + + + + + +
+
+
+ surrounding occupied grid=1 +
+
+
+ + surrounding occupied... + + + + + + +
+
+
+ surrounding occupied grids = 6 +
+
+
+ + surrounding occupied... + + + + + + + + + +
+
+
+ divide into grid +
+
+
+ + divide into grid + + + + + +
+
+
+ check occupancy for each grids +
+
+
+ + check occupancy for each grids + + + + + +
+
+
+ + Every period, count the number of occupied grids around the ego. +
+ +
+
+
+ + Every period, count the number of occupied grids around the ego. + + + + + +
+
+
+ â‘  +
+
+
+ + â‘  + + + + + +
+
+
+ â‘¡ +
+
+
+ + â‘¡ + + + + + +
+
+
+ â‘¢ +
+
+
+ + â‘¢ + + + + + +
+
+
+ â‘£ +
+
+
+ + â‘£ + + + + + +
+
+
+ grid_size +
+
+
+ + grid_size + + + + + + + + + + + Text is not SVG - cannot display + + + diff --git a/localization/pose_estimator_arbiter/media/pose_estimator_area_in_vector_map.png b/localization/pose_estimator_arbiter/media/pose_estimator_area_in_vector_map.png new file mode 100644 index 0000000000000..68e6a9ff430e6 Binary files /dev/null and b/localization/pose_estimator_arbiter/media/pose_estimator_area_in_vector_map.png differ diff --git a/localization/pose_estimator_arbiter/media/single_pose_estimator.drawio.svg b/localization/pose_estimator_arbiter/media/single_pose_estimator.drawio.svg new file mode 100644 index 0000000000000..837571474b76a --- /dev/null +++ b/localization/pose_estimator_arbiter/media/single_pose_estimator.drawio.svg @@ -0,0 +1,880 @@ + + + + + + + +
+
+
+ legend +
+
+
+ + legend + + + + + + +
+
+
+ node +
+
+
+ + node + + + + + + +
+
+
+ module +
+
+
+ + module + + + + + + +
+
+
+ node +
+
+
+ + node + + + + + + +
+
+
+ node +
+
+
+ + node + + + + + + +
+
+
+ some nodes +
+
+
+ + some nodes + + + + + + + +
+
+
+ /util/downsampled/pointcloud +
+
+
+ + /util/downsampled/pointcloud + + + + + + +
+
+
+ pointcluod_ +
+ preprocessor +
+
+
+ + pointcluod_... + + + + + + + +
+
+
+ /pose_estimator/yabloc/* +
+
+
+ + /pose_estimator/yabloc/* + + + + + + +
+
+
+ image_ +
+ preprocessor +
+
+
+ + image_... + + + + + + + +
+
+
+ /pose_estimator +
+ /pose_with_covariance +
+
+
+ + /pose_estimator... + + + + + + +
+
+
+ ndt_scan_matcher +
+
+
+ + ndt_scan_matcher + + + + + + + +
+
+
+ /pose_estimator +
+ /pose_with_covariance +
+
+
+ + /pose_estimator... + + + + + + +
+
+
+ yabloc_particle_filter +
+
+
+ + yabloc_particle_filt... + + + + + + + +
+
+
+ /pose_with_covariance +
+
+
+ + /pose_with_covariance + + + + + + + + +
+
+
+ /sensing/lidar/top +
+ /outlier_filtered/pointcloud +
+
+
+ + /sensing/lidar/top... + + + + + + + +
+
+
+ /pose_estimator +
+ /pose_with_covariance +
+
+
+ + /pose_estimator... + + + + + + +
+
+
+ eagleye +
+
+
+ + eagleye + + + + + + + + +
+
+
+ /yabloc/image_processing +
+ /undistorted/image_raw +
+
+
+ + /yabloc/image_processing... + + + + + + +
+
+
+ EKF +
+ (pose_twist_fusion_filter) +
+
+
+ + EKF... + + + + + + + + +
+
+
+ /sensing/* +
+
+
+ + /sensing/* + + + + + + +
+
+
+ pointcluod_ +
+ preprocessor +
+
+
+ + pointcluod_... + + + + + + +
+
+
+ pointcluod_ +
+ preprocessor +
+
+
+ + pointcluod_... + + + + + + +
+
+
+ image_ +
+ preprocessor +
+
+
+ + image_... + + + + + + +
+
+
+ image_ +
+ preprocessor +
+
+
+ + image_... + + + + + + +
+
+
+ eagleye +
+
+
+ + eagleye + + + + + + +
+
+
+ eagleye +
+
+
+ + eagleye + + + + + + +
+
+
+ EKF +
+ (pose_twist_fusion_filter) +
+
+
+ + EKF... + + + + + + +
+
+
+ EKF +
+ (pose_twist_fusion_filter) +
+
+
+ + EKF... + + + + + + + +
+
+
+ /twist_estimator +
+ /twist_with_covariance +
+
+
+ + /twist_estimator... + + + + + + +
+
+
+ twist_ +
+ estimator +
+
+
+ + twist_... + + + + + + +
+
+
+ twist_ +
+ estimator +
+
+
+ + twist_... + + + + + + + +
+
+
+ /twist_estimator +
+ /twist_with_covariance +
+
+
+ + /twist_estimator... + + + + + + +
+
+
+ twist_ +
+ estimator +
+
+
+ + twist_... + + + + + + + +
+
+
+ /twist_estimator +
+ /twist_with_covariance +
+
+
+ + /twist_estimator... + + + + + + + + + +
+
+
+ only NDT +
+
+
+ + only NDT + + + + + + +
+
+
+ only YabLoc +
+
+
+ + only YabLoc + + + + + + +
+
+
+ only Eagleye +
+
+
+ + only Eagleye + + + + + + +
+
+
+ artag +
+
+
+ + artag + + + + + + + + +
+
+
+ /sensing/camera/ +
+ traffic_light/image_raw +
+
+
+ + /sensing/camera/... + + + + + + +
+
+
+ EKF +
+ (pose_twist_fusion_filter) +
+
+
+ + EKF... + + + + + + + +
+
+
+ /twist_estimator +
+ /twist_with_covariance +
+
+
+ + /twist_estimator... + + + + + + + +
+
+
+ only AR-Tag +
+
+
+ + only AR-Tag + + + + + + +
+
+
+ twist_ +
+ estimator +
+
+
+ + twist_... + + + + + + + +
+
+
+ /pose_estimator +
+ /pose_with_covariance +
+
+
+ + /pose_estimator... + + + + + + + + Text is not SVG - cannot display + + + diff --git a/localization/pose_estimator_arbiter/package.xml b/localization/pose_estimator_arbiter/package.xml new file mode 100644 index 0000000000000..b416d42c8617f --- /dev/null +++ b/localization/pose_estimator_arbiter/package.xml @@ -0,0 +1,43 @@ + + + + pose_estimator_arbiter + 0.1.0 + The arbiter of multiple pose estimators + Kento Yabuuchi + Apache License 2.0 + Kento Yabuuchi + + ament_cmake_auto + autoware_cmake + + autoware_adapi_v1_msgs + autoware_auto_mapping_msgs + diagnostic_msgs + geometry_msgs + lanelet2_extension + libgoogle-glog-dev + magic_enum + pcl_conversions + pcl_ros + pluginlib + sensor_msgs + std_msgs + std_srvs + tier4_autoware_utils + ublox_msgs + visualization_msgs + yabloc_particle_filter + + rclcpp + rclcpp_components + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + ros_testing + + + ament_cmake + + diff --git a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/pose_estimator_arbiter.hpp b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/pose_estimator_arbiter.hpp new file mode 100644 index 0000000000000..9e67dfc063964 --- /dev/null +++ b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/pose_estimator_arbiter.hpp @@ -0,0 +1,100 @@ +// Copyright 2023 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef POSE_ESTIMATOR_ARBITER__POSE_ESTIMATOR_ARBITER_HPP_ +#define POSE_ESTIMATOR_ARBITER__POSE_ESTIMATOR_ARBITER_HPP_ + +#include "pose_estimator_arbiter/shared_data.hpp" +#include "pose_estimator_arbiter/stopper/base_stopper.hpp" +#include "pose_estimator_arbiter/switch_rule/base_switch_rule.hpp" + +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include + +namespace pose_estimator_arbiter +{ +class PoseEstimatorArbiter : public rclcpp::Node +{ + using SetBool = std_srvs::srv::SetBool; + using String = std_msgs::msg::String; + using MarkerArray = visualization_msgs::msg::MarkerArray; + using Image = sensor_msgs::msg::Image; + using PointCloud2 = sensor_msgs::msg::PointCloud2; + using PoseCovStamped = geometry_msgs::msg::PoseWithCovarianceStamped; + using HADMapBin = autoware_auto_mapping_msgs::msg::HADMapBin; + using InitializationState = autoware_adapi_v1_msgs::msg::LocalizationInitializationState; + using DiagnosticArray = diagnostic_msgs::msg::DiagnosticArray; + +public: + PoseEstimatorArbiter(); + +private: + // Set of running pose estimators specified by ros param `pose_sources` + const std::unordered_set running_estimator_list_; + // Configuration to allow changing the log level by service + const std::unique_ptr logger_configure_; + + // This is passed to several modules (stoppers & rule) so that all modules can access common data + // without passing them as arguments. Also, modules can register subscriber callbacks through + // shared_data, avoiding the need to define duplicate subscribers for each module. + std::shared_ptr shared_data_{nullptr}; + + // Timer callback + rclcpp::TimerBase::SharedPtr timer_; + // Publishers + rclcpp::Publisher::SharedPtr pub_diag_; + rclcpp::Publisher::SharedPtr pub_debug_marker_array_; + rclcpp::Publisher::SharedPtr pub_debug_string_; + // Subscribers for stoppers + rclcpp::Subscription::SharedPtr sub_yabloc_input_; + rclcpp::Subscription::SharedPtr sub_artag_input_; + rclcpp::Subscription::SharedPtr sub_ndt_input_; + rclcpp::Subscription::SharedPtr sub_eagleye_output_; + // Subscribers for switch rules + rclcpp::Subscription::SharedPtr sub_localization_pose_cov_; + rclcpp::Subscription::SharedPtr sub_point_cloud_map_; + rclcpp::Subscription::SharedPtr sub_vector_map_; + rclcpp::Subscription::SharedPtr sub_initialization_state_; + + // Stoppers which enable/disable pose estimators + std::unordered_map stoppers_; + // Abstract class to determine which pose estimator should be used + std::shared_ptr switch_rule_{nullptr}; + + // Instruct all stopper to enable/disable + void toggle_all(bool enabled); + // Instruct each stopper to enable/disable + void toggle_each(const std::unordered_map & toggle_list); + + // Load switching rule according to the condition + void load_switch_rule(); + // Publish diagnostic messages + void publish_diagnostics() const; + + // Timer callback + void on_timer(); +}; +} // namespace pose_estimator_arbiter + +#endif // POSE_ESTIMATOR_ARBITER__POSE_ESTIMATOR_ARBITER_HPP_ diff --git a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/pose_estimator_arbiter_core.cpp b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/pose_estimator_arbiter_core.cpp new file mode 100644 index 0000000000000..4fc3fd9b914a6 --- /dev/null +++ b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/pose_estimator_arbiter_core.cpp @@ -0,0 +1,213 @@ +// Copyright 2023 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "pose_estimator_arbiter/pose_estimator_arbiter.hpp" +#include "pose_estimator_arbiter/pose_estimator_type.hpp" +#include "pose_estimator_arbiter/stopper/stopper_artag.hpp" +#include "pose_estimator_arbiter/stopper/stopper_eagleye.hpp" +#include "pose_estimator_arbiter/stopper/stopper_ndt.hpp" +#include "pose_estimator_arbiter/stopper/stopper_yabloc.hpp" +#include "pose_estimator_arbiter/switch_rule/enable_all_rule.hpp" + +#include + +namespace pose_estimator_arbiter +{ +// Parses ros param to get the estimator set that is running +static std::unordered_set parse_estimator_name_args( + const std::vector & arg, const rclcpp::Logger & logger) +{ + std::unordered_set running_estimator_list; + for (const auto & estimator_name : arg) { + const auto estimator = magic_enum::enum_cast(estimator_name); + + if (estimator.has_value()) { + running_estimator_list.insert(estimator.value()); + } else { + RCLCPP_ERROR_STREAM(logger, "invalid pose_estimator_name is specified: " << estimator_name); + } + } + + return running_estimator_list; +} + +PoseEstimatorArbiter::PoseEstimatorArbiter() +: Node("pose_estimator_arbiter"), + running_estimator_list_(parse_estimator_name_args( + declare_parameter>("pose_sources"), get_logger())), + logger_configure_(std::make_unique(this)) +{ + // Shared data + shared_data_ = std::make_shared(); + + // Publisher + pub_diag_ = create_publisher("/diagnostics", 10); + pub_debug_string_ = create_publisher("~/debug/string", 10); + pub_debug_marker_array_ = create_publisher("~/debug/marker_array", 10); + + // Define function to get running pose_estimator + const std::set running_estimator_set( + running_estimator_list_.begin(), running_estimator_list_.end()); + const auto is_running = [running_estimator_set](const PoseEstimatorType type) -> bool { + return running_estimator_set.count(type) != 0; + }; + + // QoS + const rclcpp::QoS sensor_qos = rclcpp::SensorDataQoS(); + const rclcpp::QoS latch_qos = rclcpp::QoS(1).transient_local().reliable(); + + // Create stoppers & subscribers + if (is_running(PoseEstimatorType::ndt)) { + stoppers_.emplace( + PoseEstimatorType::ndt, std::make_shared(this, shared_data_)); + sub_ndt_input_ = create_subscription( + "~/input/ndt/pointcloud", sensor_qos, shared_data_->ndt_input_points.create_callback()); + } + if (is_running(PoseEstimatorType::yabloc)) { + stoppers_.emplace( + PoseEstimatorType::yabloc, std::make_shared(this, shared_data_)); + sub_yabloc_input_ = create_subscription( + "~/input/yabloc/image", sensor_qos, shared_data_->yabloc_input_image.create_callback()); + } + if (is_running(PoseEstimatorType::eagleye)) { + stoppers_.emplace( + PoseEstimatorType::eagleye, std::make_shared(this, shared_data_)); + sub_eagleye_output_ = create_subscription( + "~/input/eagleye/pose_with_covariance", 5, /* this is not sensor topic */ + shared_data_->eagleye_output_pose_cov.create_callback()); + } + if (is_running(PoseEstimatorType::artag)) { + stoppers_.emplace( + PoseEstimatorType::artag, std::make_shared(this, shared_data_)); + sub_artag_input_ = create_subscription( + "~/input/artag/image", sensor_qos, shared_data_->artag_input_image.create_callback()); + } + + // Subscribers for switch rule + { + sub_localization_pose_cov_ = create_subscription( + "~/input/pose_with_covariance", 5, shared_data_->localization_pose_cov.create_callback()); + sub_point_cloud_map_ = create_subscription( + "~/input/pointcloud_map", latch_qos, shared_data_->point_cloud_map.create_callback()); + sub_vector_map_ = create_subscription( + "~/input/vector_map", latch_qos, shared_data_->vector_map.create_callback()); + sub_initialization_state_ = create_subscription( + "~/input/initialization_state", latch_qos, + shared_data_->initialization_state.create_callback()); + } + + // Load switching rule + load_switch_rule(); + + // Timer callback + auto on_timer = std::bind(&PoseEstimatorArbiter::on_timer, this); + timer_ = + rclcpp::create_timer(this, this->get_clock(), rclcpp::Rate(1).period(), std::move(on_timer)); + + // Enable all pose estimators at the first + toggle_all(true); +} + +void PoseEstimatorArbiter::load_switch_rule() +{ + // NOTE: In the future, some rule will be laid below + RCLCPP_INFO_STREAM(get_logger(), "load default switching rule"); + switch_rule_ = + std::make_shared(*this, running_estimator_list_, shared_data_); +} + +void PoseEstimatorArbiter::toggle_each( + const std::unordered_map & toggle_list) +{ + for (auto [type, stopper] : stoppers_) { + RCLCPP_DEBUG_STREAM( + get_logger(), magic_enum::enum_name(type) << " : " << std::boolalpha << toggle_list.at(type)); + + // If the rule implementation is perfect, toggle_list should contains all pose_estimator_type. + if (toggle_list.count(type) == 0) { + RCLCPP_ERROR_STREAM( + get_logger(), magic_enum::enum_name(type) << " is not included in toggle_list."); + continue; + } + + // Enable or disable according to toggle_list + if (toggle_list.at(type)) { + stopper->enable(); + } else { + stopper->disable(); + } + } +} + +void PoseEstimatorArbiter::toggle_all(bool enabled) +{ + // Create toggle_list + std::unordered_map toggle_list; + for (auto [type, stopper] : stoppers_) { + toggle_list.emplace(type, enabled); + } + + // Apply toggle_list + toggle_each(toggle_list); +} + +void PoseEstimatorArbiter::publish_diagnostics() const +{ + diagnostic_msgs::msg::DiagnosticStatus diag_status; + + // Temporary implementation + { + diag_status.name = "localization: " + std::string(this->get_name()); + diag_status.hardware_id = this->get_name(); + + diag_status.level = diagnostic_msgs::msg::DiagnosticStatus::OK; + diag_status.message = "OK"; + + // TODO(KYabuuchi) : Add more details + diagnostic_msgs::msg::KeyValue key_value_msg; + key_value_msg.key = "state"; + key_value_msg.value = "Further details have not been implemented yet."; + diag_status.values.push_back(key_value_msg); + } + + DiagnosticArray diag_msg; + diag_msg.header.stamp = this->now(); + diag_msg.status.push_back(diag_status); + + pub_diag_->publish(diag_msg); +} + +void PoseEstimatorArbiter::on_timer() +{ + // Toggle each stopper status + if (switch_rule_) { + const auto toggle_list = switch_rule_->update(); + toggle_each(toggle_list); + + // Publish std_msg::String for debug + pub_debug_string_->publish(String().set__data(switch_rule_->debug_string())); + // Publish visualization_msgs::MarkerArray for debug + pub_debug_marker_array_->publish(switch_rule_->debug_marker_array()); + + } else { + RCLCPP_WARN_STREAM( + get_logger(), "switch_rule is not activated. Therefore, enable all pose_estimators"); + toggle_all(true); + } + + // Publish diagnostic results periodically + publish_diagnostics(); +} + +} // namespace pose_estimator_arbiter diff --git a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/pose_estimator_arbiter_node.cpp b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/pose_estimator_arbiter_node.cpp new file mode 100644 index 0000000000000..e48507948520b --- /dev/null +++ b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/pose_estimator_arbiter_node.cpp @@ -0,0 +1,31 @@ +// Copyright 2023 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "pose_estimator_arbiter/pose_estimator_arbiter.hpp" + +#include + +int main(int argc, char * argv[]) +{ + google::InitGoogleLogging(argv[0]); + google::InstallFailureSignalHandler(); + + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(node); + executor.spin(); + rclcpp::shutdown(); + return 0; +} diff --git a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/pose_estimator_type.hpp b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/pose_estimator_type.hpp new file mode 100644 index 0000000000000..edf12537198a3 --- /dev/null +++ b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/pose_estimator_type.hpp @@ -0,0 +1,23 @@ +// Copyright 2023 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef POSE_ESTIMATOR_ARBITER__POSE_ESTIMATOR_TYPE_HPP_ +#define POSE_ESTIMATOR_ARBITER__POSE_ESTIMATOR_TYPE_HPP_ + +namespace pose_estimator_arbiter +{ +enum class PoseEstimatorType : int { ndt = 1, yabloc = 2, eagleye = 4, artag = 8 }; +} + +#endif // POSE_ESTIMATOR_ARBITER__POSE_ESTIMATOR_TYPE_HPP_ diff --git a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/shared_data.hpp b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/shared_data.hpp new file mode 100644 index 0000000000000..b9c3bd45c9e24 --- /dev/null +++ b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/shared_data.hpp @@ -0,0 +1,101 @@ +// Copyright 2023 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef POSE_ESTIMATOR_ARBITER__SHARED_DATA_HPP_ +#define POSE_ESTIMATOR_ARBITER__SHARED_DATA_HPP_ + +#include +#include +#include +#include +#include + +#include +#include +#include + +namespace pose_estimator_arbiter +{ +template +struct CallbackInvokingVariable +{ + CallbackInvokingVariable() {} + + explicit CallbackInvokingVariable(T initial_data) : value(initial_data) {} + + // Set data and invoke all callbacks + void set_and_invoke(T new_value) + { + value = new_value; + + // Call all callbacks with the new value + for (const auto & callback : callbacks) { + callback(value.value()); + } + } + + // Same as std::optional::has_value() + bool has_value() const { return value.has_value(); } + + // Same as std::optional::value() + const T operator()() const { return value.value(); } + + // Register callback function which is invoked when set_and_invoke() is called + void register_callback(std::function callback) const { callbacks.push_back(callback); } + + // Create subscription callback function which is used as below: + // auto subscriber = create_subscription("topic_name", 10, + // callback_invoking_variable.create_callback()); + auto create_callback() + { + return std::bind(&CallbackInvokingVariable::set_and_invoke, this, std::placeholders::_1); + } + +private: + // The latest data + std::optional value{std::nullopt}; + + // These functions are expected not to change the value variable + mutable std::vector> callbacks; +}; + +// This structure is handed to several modules as shared_ptr so that all modules can access data. +struct SharedData +{ +public: + using Image = sensor_msgs::msg::Image; + using PoseCovStamped = geometry_msgs::msg::PoseWithCovarianceStamped; + using PointCloud2 = sensor_msgs::msg::PointCloud2; + using HADMapBin = autoware_auto_mapping_msgs::msg::HADMapBin; + using InitializationState = autoware_adapi_v1_msgs::msg::LocalizationInitializationState; + + SharedData() {} + + // Used for stoppers + CallbackInvokingVariable eagleye_output_pose_cov; + CallbackInvokingVariable artag_input_image; + CallbackInvokingVariable ndt_input_points; + CallbackInvokingVariable yabloc_input_image; + + // Used for switch rule + CallbackInvokingVariable localization_pose_cov; + CallbackInvokingVariable point_cloud_map; + CallbackInvokingVariable vector_map; + CallbackInvokingVariable initialization_state{ + std::make_shared( + InitializationState{}.set__state(InitializationState::UNINITIALIZED))}; +}; + +} // namespace pose_estimator_arbiter +#endif // POSE_ESTIMATOR_ARBITER__SHARED_DATA_HPP_ diff --git a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/stopper/base_stopper.hpp b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/stopper/base_stopper.hpp new file mode 100644 index 0000000000000..fc2242a0b27e9 --- /dev/null +++ b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/stopper/base_stopper.hpp @@ -0,0 +1,47 @@ +// Copyright 2023 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef POSE_ESTIMATOR_ARBITER__STOPPER__BASE_STOPPER_HPP_ +#define POSE_ESTIMATOR_ARBITER__STOPPER__BASE_STOPPER_HPP_ + +#include "pose_estimator_arbiter/shared_data.hpp" + +#include + +#include + +namespace pose_estimator_arbiter::stopper +{ +class BaseStopper +{ +public: + using SharedPtr = std::shared_ptr; + + explicit BaseStopper(rclcpp::Node * node, const std::shared_ptr shared_data) + : logger_(node->get_logger()), shared_data_(shared_data) + { + } + + void enable() { set_enable(true); } + void disable() { set_enable(false); } + + virtual void set_enable(bool enabled) = 0; + +protected: + rclcpp::Logger logger_; + std::shared_ptr shared_data_{nullptr}; +}; +} // namespace pose_estimator_arbiter::stopper + +#endif // POSE_ESTIMATOR_ARBITER__STOPPER__BASE_STOPPER_HPP_ diff --git a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/stopper/stopper_artag.hpp b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/stopper/stopper_artag.hpp new file mode 100644 index 0000000000000..72a3f5412add0 --- /dev/null +++ b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/stopper/stopper_artag.hpp @@ -0,0 +1,57 @@ +// Copyright 2023 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef POSE_ESTIMATOR_ARBITER__STOPPER__STOPPER_ARTAG_HPP_ +#define POSE_ESTIMATOR_ARBITER__STOPPER__STOPPER_ARTAG_HPP_ + +#include "pose_estimator_arbiter/stopper/base_stopper.hpp" + +#include +#include + +#include +namespace pose_estimator_arbiter::stopper +{ +class StopperArTag : public BaseStopper +{ + using Image = sensor_msgs::msg::Image; + using SetBool = std_srvs::srv::SetBool; + +public: + explicit StopperArTag(rclcpp::Node * node, const std::shared_ptr shared_data) + : BaseStopper(node, shared_data) + { + ar_tag_is_enabled_ = true; + pub_image_ = node->create_publisher("~/output/artag/image", rclcpp::SensorDataQoS()); + + // Register callback + shared_data_->artag_input_image.register_callback([this](Image::ConstSharedPtr msg) -> void { + if (ar_tag_is_enabled_) { + pub_image_->publish(*msg); + } + }); + } + + void set_enable(bool enabled) override { ar_tag_is_enabled_ = enabled; } + +protected: + rclcpp::CallbackGroup::SharedPtr service_callback_group_; + +private: + bool ar_tag_is_enabled_; + rclcpp::Publisher::SharedPtr pub_image_; +}; +} // namespace pose_estimator_arbiter::stopper + +#endif // POSE_ESTIMATOR_ARBITER__STOPPER__STOPPER_ARTAG_HPP_ diff --git a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/stopper/stopper_eagleye.hpp b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/stopper/stopper_eagleye.hpp new file mode 100644 index 0000000000000..12bbe8c9769a1 --- /dev/null +++ b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/stopper/stopper_eagleye.hpp @@ -0,0 +1,53 @@ +// Copyright 2023 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef POSE_ESTIMATOR_ARBITER__STOPPER__STOPPER_EAGLEYE_HPP_ +#define POSE_ESTIMATOR_ARBITER__STOPPER__STOPPER_EAGLEYE_HPP_ +#include "pose_estimator_arbiter/stopper/base_stopper.hpp" + +#include + +#include + +namespace pose_estimator_arbiter::stopper +{ +class StopperEagleye : public BaseStopper +{ + using PoseCovStamped = geometry_msgs::msg::PoseWithCovarianceStamped; + +public: + explicit StopperEagleye(rclcpp::Node * node, const std::shared_ptr shared_data) + : BaseStopper(node, shared_data) + { + eagleye_is_enabled_ = true; + pub_pose_ = node->create_publisher("~/output/eagleye/pose_with_covariance", 5); + + // Register callback + shared_data_->eagleye_output_pose_cov.register_callback( + [this](PoseCovStamped::ConstSharedPtr msg) -> void { + if (eagleye_is_enabled_) { + pub_pose_->publish(*msg); + } + }); + } + + void set_enable(bool enabled) override { eagleye_is_enabled_ = enabled; } + +private: + bool eagleye_is_enabled_; + rclcpp::Publisher::SharedPtr pub_pose_; +}; +} // namespace pose_estimator_arbiter::stopper + +#endif // POSE_ESTIMATOR_ARBITER__STOPPER__STOPPER_EAGLEYE_HPP_ diff --git a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/stopper/stopper_ndt.hpp b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/stopper/stopper_ndt.hpp new file mode 100644 index 0000000000000..dacea02f77bde --- /dev/null +++ b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/stopper/stopper_ndt.hpp @@ -0,0 +1,54 @@ +// Copyright 2023 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef POSE_ESTIMATOR_ARBITER__STOPPER__STOPPER_NDT_HPP_ +#define POSE_ESTIMATOR_ARBITER__STOPPER__STOPPER_NDT_HPP_ +#include "pose_estimator_arbiter/stopper/base_stopper.hpp" + +#include + +#include + +namespace pose_estimator_arbiter::stopper +{ +class StopperNdt : public BaseStopper +{ + using PointCloud2 = sensor_msgs::msg::PointCloud2; + +public: + explicit StopperNdt(rclcpp::Node * node, const std::shared_ptr shared_data) + : BaseStopper(node, shared_data) + { + ndt_is_enabled_ = true; + pub_pointcloud_ = node->create_publisher( + "~/output/ndt/pointcloud", rclcpp::SensorDataQoS().keep_last(10)); + + // Register callback + shared_data_->ndt_input_points.register_callback( + [this](PointCloud2::ConstSharedPtr msg) -> void { + if (ndt_is_enabled_) { + pub_pointcloud_->publish(*msg); + } + }); + } + + void set_enable(bool enabled) override { ndt_is_enabled_ = enabled; } + +private: + rclcpp::Publisher::SharedPtr pub_pointcloud_; + bool ndt_is_enabled_; +}; +} // namespace pose_estimator_arbiter::stopper + +#endif // POSE_ESTIMATOR_ARBITER__STOPPER__STOPPER_NDT_HPP_ diff --git a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/stopper/stopper_yabloc.hpp b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/stopper/stopper_yabloc.hpp new file mode 100644 index 0000000000000..2cd8b66f4ffd4 --- /dev/null +++ b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/stopper/stopper_yabloc.hpp @@ -0,0 +1,90 @@ +// Copyright 2023 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef POSE_ESTIMATOR_ARBITER__STOPPER__STOPPER_YABLOC_HPP_ +#define POSE_ESTIMATOR_ARBITER__STOPPER__STOPPER_YABLOC_HPP_ +#include "pose_estimator_arbiter/stopper/base_stopper.hpp" + +#include +#include + +#include + +namespace pose_estimator_arbiter::stopper +{ +class StopperYabLoc : public BaseStopper +{ + using Image = sensor_msgs::msg::Image; + using SetBool = std_srvs::srv::SetBool; + +public: + explicit StopperYabLoc(rclcpp::Node * node, const std::shared_ptr shared_data) + : BaseStopper(node, shared_data) + { + yabloc_is_enabled_ = true; + pub_image_ = node->create_publisher("~/output/yabloc/image", rclcpp::SensorDataQoS()); + + service_callback_group_ = + node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + + // Prepare suspend service server + using namespace std::literals::chrono_literals; + enable_service_client_ = node->create_client( + "~/yabloc_trigger_srv", rmw_qos_profile_services_default, service_callback_group_); + while (!enable_service_client_->wait_for_service(1s) && rclcpp::ok()) { + RCLCPP_INFO( + node->get_logger(), "Waiting for service : %s", enable_service_client_->get_service_name()); + } + + // Register callback + shared_data_->yabloc_input_image.register_callback([this](Image::ConstSharedPtr msg) -> void { + if (yabloc_is_enabled_) { + pub_image_->publish(*msg); + } + }); + } + + void set_enable(bool enabled) override + { + if (yabloc_is_enabled_ != enabled) { + request_service(enabled); + } + yabloc_is_enabled_ = enabled; + } + +protected: + rclcpp::CallbackGroup::SharedPtr service_callback_group_; + + void request_service(bool flag) + { + using namespace std::literals::chrono_literals; + auto request = std::make_shared(); + request->data = flag; + + auto result_future = enable_service_client_->async_send_request(request); + std::future_status status = result_future.wait_for(1000ms); + if (status == std::future_status::ready) { + RCLCPP_DEBUG_STREAM(logger_, "stopper_yabloc dis/enabling service exited successfully"); + } else { + RCLCPP_ERROR_STREAM(logger_, "stopper_yabloc dis/enabling service exited unexpectedly"); + } + } + +private: + bool yabloc_is_enabled_; + rclcpp::Client::SharedPtr enable_service_client_; + rclcpp::Publisher::SharedPtr pub_image_; +}; +} // namespace pose_estimator_arbiter::stopper +#endif // POSE_ESTIMATOR_ARBITER__STOPPER__STOPPER_YABLOC_HPP_ diff --git a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/switch_rule/base_switch_rule.hpp b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/switch_rule/base_switch_rule.hpp new file mode 100644 index 0000000000000..5770f18efa32c --- /dev/null +++ b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/switch_rule/base_switch_rule.hpp @@ -0,0 +1,54 @@ +// Copyright 2023 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef POSE_ESTIMATOR_ARBITER__SWITCH_RULE__BASE_SWITCH_RULE_HPP_ +#define POSE_ESTIMATOR_ARBITER__SWITCH_RULE__BASE_SWITCH_RULE_HPP_ + +#include "pose_estimator_arbiter/pose_estimator_type.hpp" + +#include + +#include + +#include +#include +#include +#include + +namespace pose_estimator_arbiter::switch_rule +{ +class BaseSwitchRule +{ +protected: + using MarkerArray = visualization_msgs::msg::MarkerArray; + +public: + explicit BaseSwitchRule(rclcpp::Node & node) + : logger_ptr_(std::make_shared(node.get_logger())) + { + } + + virtual ~BaseSwitchRule() = default; + virtual std::unordered_map update() = 0; + virtual std::string debug_string() { return std::string{}; } + virtual MarkerArray debug_marker_array() { return MarkerArray{}; } + +protected: + rclcpp::Logger get_logger() const { return *logger_ptr_; } + std::shared_ptr logger_ptr_{nullptr}; +}; + +} // namespace pose_estimator_arbiter::switch_rule + +#endif // POSE_ESTIMATOR_ARBITER__SWITCH_RULE__BASE_SWITCH_RULE_HPP_ diff --git a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/switch_rule/enable_all_rule.cpp b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/switch_rule/enable_all_rule.cpp new file mode 100644 index 0000000000000..2b2e325c3d94b --- /dev/null +++ b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/switch_rule/enable_all_rule.cpp @@ -0,0 +1,43 @@ +// Copyright 2023 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "pose_estimator_arbiter/switch_rule/enable_all_rule.hpp" + +#include + +#include +#include +#include +#include + +namespace pose_estimator_arbiter::switch_rule +{ +EnableAllRule::EnableAllRule( + rclcpp::Node & node, const std::unordered_set & running_estimator_list, + const std::shared_ptr) +: BaseSwitchRule(node), running_estimator_list_(running_estimator_list) +{ +} + +std::unordered_map EnableAllRule::update() +{ + return { + {PoseEstimatorType::ndt, true}, + {PoseEstimatorType::yabloc, true}, + {PoseEstimatorType::eagleye, true}, + {PoseEstimatorType::artag, true}, + }; +} + +} // namespace pose_estimator_arbiter::switch_rule diff --git a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/switch_rule/enable_all_rule.hpp b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/switch_rule/enable_all_rule.hpp new file mode 100644 index 0000000000000..568226985b2ff --- /dev/null +++ b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/switch_rule/enable_all_rule.hpp @@ -0,0 +1,45 @@ +// Copyright 2023 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef POSE_ESTIMATOR_ARBITER__SWITCH_RULE__ENABLE_ALL_RULE_HPP_ +#define POSE_ESTIMATOR_ARBITER__SWITCH_RULE__ENABLE_ALL_RULE_HPP_ + +#include "pose_estimator_arbiter/shared_data.hpp" +#include "pose_estimator_arbiter/switch_rule/base_switch_rule.hpp" + +#include +#include +#include +#include + +namespace pose_estimator_arbiter::switch_rule +{ +class EnableAllRule : public BaseSwitchRule +{ +public: + EnableAllRule( + rclcpp::Node & node, const std::unordered_set & running_estimator_list, + const std::shared_ptr shared_data); + + virtual ~EnableAllRule() = default; + + std::unordered_map update() override; + +protected: + const std::unordered_set running_estimator_list_; +}; + +} // namespace pose_estimator_arbiter::switch_rule + +#endif // POSE_ESTIMATOR_ARBITER__SWITCH_RULE__ENABLE_ALL_RULE_HPP_ diff --git a/localization/pose_estimator_arbiter/test/test_pose_estimator_arbiter.py b/localization/pose_estimator_arbiter/test/test_pose_estimator_arbiter.py new file mode 100644 index 0000000000000..c419fb68e0571 --- /dev/null +++ b/localization/pose_estimator_arbiter/test/test_pose_estimator_arbiter.py @@ -0,0 +1,180 @@ +#!/usr/bin/env python3 + +# Copyright 2023 TIER IV, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import os +import time +import unittest + +from ament_index_python import get_package_share_directory +from geometry_msgs.msg import PoseWithCovarianceStamped +import launch +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import AnyLaunchDescriptionSource +import launch_testing +import pytest +import rclpy +from rclpy.qos import QoSProfile +from rclpy.qos import ReliabilityPolicy +from sensor_msgs.msg import Image +from sensor_msgs.msg import PointCloud2 +from std_srvs.srv import SetBool + +# This test confirms that all topics are relayed by arbiter. + + +@pytest.mark.launch_test +def generate_test_description(): + test_pose_estimator_arbiter_launch_file = os.path.join( + get_package_share_directory("pose_estimator_arbiter"), + "launch", + "pose_estimator_arbiter.launch.xml", + ) + + pose_estimator_arbiter = IncludeLaunchDescription( + AnyLaunchDescriptionSource(test_pose_estimator_arbiter_launch_file), + launch_arguments={ + "pose_sources": "[ndt, yabloc, eagleye, artag]", + "input_pointcloud": "/sensing/lidar/top/pointcloud", + }.items(), + ) + + return launch.LaunchDescription( + [ + pose_estimator_arbiter, + # Start tests right away - no need to wait for anything + launch_testing.actions.ReadyToTest(), + ] + ) + + +class TestPoseEstimatorArbiter(unittest.TestCase): + @classmethod + def setUpClass(cls): + # Initialize the ROS context for the test node + rclpy.init() + + @classmethod + def tearDownClass(cls): + # Shutdown the ROS context + rclpy.shutdown() + + def setUp(self): + # Create a ROS node for tests + self.test_node = rclpy.create_node("test_node") + + def tearDown(self): + self.test_node.destroy_node() + + def spin_for(self, duration_sec): + end_time = time.time() + duration_sec + while time.time() < end_time: + rclpy.spin_once(self.test_node, timeout_sec=0.1) + + def yabloc_suspend_service_callback(self, srv): + pass + + def publish_input_topics(self): + self.pub_ndt_input.publish(PointCloud2()) + rclpy.spin_once(self.test_node, timeout_sec=0.1) + self.pub_yabloc_input.publish(Image()) + rclpy.spin_once(self.test_node, timeout_sec=0.1) + self.pub_eagleye_input.publish(PoseWithCovarianceStamped()) + rclpy.spin_once(self.test_node, timeout_sec=0.1) + + def create_publishers_and_subscribers(self): + # Publisher + qos_profile = QoSProfile(depth=10, reliability=ReliabilityPolicy.BEST_EFFORT) + + self.pub_ndt_input = self.test_node.create_publisher( + PointCloud2, "/sensing/lidar/top/pointcloud", qos_profile + ) + # pub_yabloc_input is used for both yabloc and artag + self.pub_yabloc_input = self.test_node.create_publisher( + Image, "/sensing/camera/traffic_light/image_raw", qos_profile + ) + self.pub_eagleye_input = self.test_node.create_publisher( + PoseWithCovarianceStamped, + "/localization/pose_estimator/eagleye/pose_with_covariance/to_relay", + QoSProfile(depth=10), + ) + + # Subscriber + self.test_node.create_subscription( + PointCloud2, + "/sensing/lidar/top/pointcloud/relay", + lambda msg: self.ndt_relayed.append(msg.header), + qos_profile, + ) + self.test_node.create_subscription( + Image, + "/sensing/camera/traffic_light/image_raw/yabloc_relay", + lambda msg: self.yabloc_relayed.append(msg.header), + qos_profile, + ) + self.test_node.create_subscription( + Image, + "/sensing/camera/traffic_light/image_raw/artag_relay", + lambda msg: self.artag_relayed.append(msg.header), + qos_profile, + ) + self.test_node.create_subscription( + PoseWithCovarianceStamped, + "/localization/pose_estimator/pose_with_covariance", + lambda msg: self.eagleye_relayed.append(msg.header), + qos_profile, + ) + + def test_node_link(self): + # The arbiter waits for the service to start, so here it instantiates a meaningless service server. + self.test_node.create_service( + SetBool, + "/localization/pose_estimator/yabloc/pf/yabloc_trigger_srv", + self.yabloc_suspend_service_callback, + ) + + # Define subscription buffer + self.ndt_relayed = [] + self.yabloc_relayed = [] + self.artag_relayed = [] + self.eagleye_relayed = [] + + # Create publishers and subscribers + self.create_publishers_and_subscribers() + + # Wait 0.5 second for node to be ready + self.spin_for(0.5) + + # Publish dummy input topics + for _ in range(10): + self.publish_input_topics() + rclpy.spin_once(self.test_node, timeout_sec=0.1) + + # Wait 0.5 second for all topics to be subscribed + self.spin_for(0.5) + + # Confirm both topics are relayed + # In reality, 10topics should be received, but with a margin, 5 is used as the threshold. + self.assertGreater(len(self.ndt_relayed), 5) + self.assertGreater(len(self.yabloc_relayed), 5) + self.assertGreater(len(self.eagleye_relayed), 5) + self.assertGreater(len(self.artag_relayed), 5) + + +@launch_testing.post_shutdown_test() +class TestProcessOutput(unittest.TestCase): + def test_exit_code(self, proc_info): + # Check that process exits with code 0: no error + launch_testing.asserts.assertExitCodes(proc_info) diff --git a/localization/pose_estimator_arbiter/test/test_shared_data.cpp b/localization/pose_estimator_arbiter/test/test_shared_data.cpp new file mode 100644 index 0000000000000..3c1fa50b502a1 --- /dev/null +++ b/localization/pose_estimator_arbiter/test/test_shared_data.cpp @@ -0,0 +1,59 @@ +// Copyright 2023 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "pose_estimator_arbiter/shared_data.hpp" + +#include + +TEST(SharedData, callback_invoked_correctly) +{ + pose_estimator_arbiter::CallbackInvokingVariable variable; + + // register callback + bool callback_invoked = false; + int processed_value = 0; + variable.register_callback([&](const int & value) { + callback_invoked = true; + processed_value = 2 * value; + }); + + EXPECT_FALSE(variable.has_value()); + EXPECT_FALSE(callback_invoked); + EXPECT_EQ(processed_value, 0); + + // set value and invoke callback + const int expected_value = 10; + variable.set_and_invoke(expected_value); + + EXPECT_TRUE(variable.has_value()); + EXPECT_TRUE(callback_invoked); + EXPECT_TRUE(variable() == expected_value); + EXPECT_TRUE(processed_value == 2 * expected_value); +} + +TEST(SharedData, multiple_callback_invoked_correctly) +{ + pose_estimator_arbiter::CallbackInvokingVariable variable; + + // register callback + int callback_invoked_num = 0; + variable.register_callback([&](const int &) { callback_invoked_num++; }); + variable.register_callback([&](const int &) { callback_invoked_num++; }); + variable.register_callback([&](const int &) { callback_invoked_num++; }); + + // set value and invoke callback + variable.set_and_invoke(10); + + EXPECT_EQ(callback_invoked_num, 3); +}