diff --git a/launch/tier4_localization_launch/launch/localization.launch.xml b/launch/tier4_localization_launch/launch/localization.launch.xml index cfc09459a5cb1..dd274ce247064 100644 --- a/launch/tier4_localization_launch/launch/localization.launch.xml +++ b/launch/tier4_localization_launch/launch/localization.launch.xml @@ -18,6 +18,11 @@ + + + + + diff --git a/launch/tier4_localization_launch/launch/pose_twist_estimator/lidar_marker_localizer.launch.xml b/launch/tier4_localization_launch/launch/pose_twist_estimator/lidar_marker_localizer.launch.xml new file mode 100644 index 0000000000000..f5be1cdd0c067 --- /dev/null +++ b/launch/tier4_localization_launch/launch/pose_twist_estimator/lidar_marker_localizer.launch.xml @@ -0,0 +1,43 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 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 c09443be8cab6..3d7192c7d68da 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 @@ -32,6 +32,42 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/localization/landmark_based_localizer/landmark_manager/src/landmark_manager.cpp b/localization/landmark_based_localizer/landmark_manager/src/landmark_manager.cpp index 57bfcde461af6..e977dbf6cf74a 100644 --- a/localization/landmark_based_localizer/landmark_manager/src/landmark_manager.cpp +++ b/localization/landmark_based_localizer/landmark_manager/src/landmark_manager.cpp @@ -50,7 +50,7 @@ void LandmarkManager::parse_landmarks( const auto & v2 = vertices[2]; const auto & v3 = vertices[3]; const double volume = (v1 - v0).cross(v2 - v0).dot(v3 - v0) / 6.0; - const double volume_threshold = 1e-5; + const double volume_threshold = 1e-3; if (volume > volume_threshold) { continue; } diff --git a/localization/landmark_based_localizer/lidar_marker_localizer/CMakeLists.txt b/localization/landmark_based_localizer/lidar_marker_localizer/CMakeLists.txt new file mode 100755 index 0000000000000..dbda867d947d8 --- /dev/null +++ b/localization/landmark_based_localizer/lidar_marker_localizer/CMakeLists.txt @@ -0,0 +1,30 @@ +cmake_minimum_required(VERSION 3.14) +project(lidar_marker_localizer) + +# find dependencies +find_package(autoware_cmake REQUIRED) +autoware_package() + +find_package(PCL REQUIRED COMPONENTS common io) +include_directories(${PCL_INCLUDE_DIRS}) +link_directories(${PCL_LIBRARY_DIRS}) + +ament_auto_add_executable(lidar_marker_localizer + src/lidar_marker_localizer.cpp + src/diagnostics/diagnostics_module.cpp + src/main.cpp +) + +target_link_libraries(lidar_marker_localizer + ${PCL_LIBRARIES} +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_auto_package(INSTALL_TO_SHARE + config + launch +) diff --git a/localization/landmark_based_localizer/lidar_marker_localizer/README.md b/localization/landmark_based_localizer/lidar_marker_localizer/README.md new file mode 100644 index 0000000000000..a3060ed7bea6f --- /dev/null +++ b/localization/landmark_based_localizer/lidar_marker_localizer/README.md @@ -0,0 +1,113 @@ +# LiDAR Marker Localizer + +**LiDARMarkerLocalizer** is a detect-reflector-based localization node . + +## Inputs / Outputs + +### `lidar_marker_localizer` node + +#### Input + +| Name | Type | Description | +| :--------------------- | :---------------------------------------------- | :--------------- | +| `~/input/lanelet2_map` | `autoware_auto_mapping_msgs::msg::HADMapBin` | Data of lanelet2 | +| `~/input/pointcloud` | `sensor_msgs::msg::PointCloud2` | PointCloud | +| `~/input/ekf_pose` | `geometry_msgs::msg::PoseWithCovarianceStamped` | EKF Pose | + +#### Output + +| Name | Type | Description | +| :------------------------------ | :---------------------------------------------- | :----------------------------------------------------------------- | +| `~/output/pose_with_covariance` | `geometry_msgs::msg::PoseWithCovarianceStamped` | Estimated pose | +| `~/debug/pose_with_covariance` | `geometry_msgs::msg::PoseWithCovarianceStamped` | [debug topic] Estimated pose | +| `~/debug/marker_detected` | `geometry_msgs::msg::PoseArray` | [debug topic] Detected marker poses | +| `~/debug/marker_mapped` | `visualization_msgs::msg::MarkerArray` | [debug topic] Loaded landmarks to visualize in Rviz as thin boards | +| `/diagnostics` | `diagnostic_msgs::msg::DiagnosticArray` | Diagnostics outputs | + +## Parameters + +{{ json_to_markdown("localization/landmark_based_localizer/lidar_marker_localizer/schema/lidar_marker_localizer.schema.json") }} + +## How to launch + +When launching Autoware, set `lidar-marker` for `pose_source`. + +```bash +ros2 launch autoware_launch ... \ + pose_source:=lidar-marker \ + ... +``` + +## Design + +### Flowchart + +```plantuml +@startuml + +group main process + start + if (Receive a map?) then (yes) + else (no) + stop + endif + + :Interpolate based on the received ego-vehicle's positions to align with sensor time; + + if (Could interpolate?) then (yes) + else (no) + stop + endif + + :Detect markers (see "Detection Algorithm"); + + :Calculate the distance from the ego-vehicle's positions to the nearest marker's position on the lanelet2 map; + + if (Find markers?) then (yes) + else (no) + if (the distance is nearby?) then (yes) + stop + note : Error. It should have been able to detect marker + else (no) + stop + note : Not Error. There are no markers around the ego-vehicle + endif + endif + + :Calculate the correction amount from the ego-vehicle's position; + + if (Is the found marker's position close to the one on the lanelet2 map?) then (yes) + else (no) + stop + note : Detected something that isn't a marker + endif + + :Publish result; + + stop +end group + +@enduml + +``` + +## Detection Algorithm + +![detection_algorithm](./doc_image/detection_algorithm.png) + +1. Split the LiDAR point cloud into rings along the x-axis of the base_link coordinate system at intervals of the `resolution` size. +2. Find the portion of intensity that matches the `intensity_pattern`. +3. Perform steps 1 and 2 for each ring, accumulate the matching indices, and detect portions where the count exceeds the `vote_threshold_for_detect_marker` as markers. + +## Sample Dataset + +- [Sample rosbag and map](https://drive.google.com/file/d/1FuGKbkWrvL_iKmtb45PO9SZl1vAaJFVG/view?usp=sharing) + +This dataset was acquired in National Institute for Land and Infrastructure Management, Full-scale tunnel experiment facility. +The reflectors were installed by [Taisei Corporation](https://www.taisei.co.jp/english/). + +## Collaborators + +- [TIER IV](https://tier4.jp/en/) +- [Taisei Corporation](https://www.taisei.co.jp/english/) +- [Yuri Shimizu](https://github.com/YuriShimizu824) diff --git a/localization/landmark_based_localizer/lidar_marker_localizer/config/lidar_marker_localizer.param.yaml b/localization/landmark_based_localizer/lidar_marker_localizer/config/lidar_marker_localizer.param.yaml new file mode 100644 index 0000000000000..1cd9a421ea4de --- /dev/null +++ b/localization/landmark_based_localizer/lidar_marker_localizer/config/lidar_marker_localizer.param.yaml @@ -0,0 +1,39 @@ +/**: + ros__parameters: + + # marker name + marker_name: "reflector" + + # for marker detection algorithm + resolution: 0.05 + # A sequence of high/low intensity to perform pattern matching. + # 1: high intensity (positive match), 0: not consider, -1: low intensity (negative match) + intensity_pattern: [-1, -1, 0, 1, 1, 1, 1, 1, 0, -1, -1] + match_intensity_difference_threshold: 20 + positive_match_num_threshold: 3 + negative_match_num_threshold: 3 + vote_threshold_for_detect_marker: 20 + marker_height_from_ground: 1.075 + + # for interpolate algorithm + self_pose_timeout_sec: 1.0 + self_pose_distance_tolerance_m: 1.0 + + # for validation + limit_distance_from_self_pose_to_nearest_marker: 2.0 + limit_distance_from_self_pose_to_marker: 2.0 + + # base_covariance + # [TBD] This value is dynamically scaled according to the distance at which markers are detected. + base_covariance: [0.04, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.04, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.00007569, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.00007569, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.00030625] + + # for save log + enable_save_log: false + savefile_directory_path: detected_reflector_intensity + savefile_name: detected_reflector_intensity + save_frame_id: velodyne_top diff --git a/localization/landmark_based_localizer/lidar_marker_localizer/doc_image/detection_algorithm.png b/localization/landmark_based_localizer/lidar_marker_localizer/doc_image/detection_algorithm.png new file mode 100644 index 0000000000000..905b432053078 Binary files /dev/null and b/localization/landmark_based_localizer/lidar_marker_localizer/doc_image/detection_algorithm.png differ diff --git a/localization/landmark_based_localizer/lidar_marker_localizer/launch/lidar_marker_localizer.launch.xml b/localization/landmark_based_localizer/lidar_marker_localizer/launch/lidar_marker_localizer.launch.xml new file mode 100644 index 0000000000000..ca007b3e51c21 --- /dev/null +++ b/localization/landmark_based_localizer/lidar_marker_localizer/launch/lidar_marker_localizer.launch.xml @@ -0,0 +1,24 @@ + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/localization/landmark_based_localizer/lidar_marker_localizer/package.xml b/localization/landmark_based_localizer/lidar_marker_localizer/package.xml new file mode 100755 index 0000000000000..808fa838a56b1 --- /dev/null +++ b/localization/landmark_based_localizer/lidar_marker_localizer/package.xml @@ -0,0 +1,41 @@ + + + + lidar_marker_localizer + 0.0.0 + The lidar_marker_localizer package + Yamato Ando + Shintaro Sakoda + Apache License 2.0 + Eijiro Takeuchi + Yamato Ando + Shintaro Sakoda + + ament_cmake_auto + autoware_cmake + + autoware_point_types + landmark_manager + lanelet2_extension + localization_util + pcl_conversions + pcl_msgs + pcl_ros + rclcpp + sensor_msgs + std_msgs + std_srvs + tf2 + tf2_eigen + tf2_geometry_msgs + tf2_ros + tf2_sensor_msgs + tier4_autoware_utils + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/localization/landmark_based_localizer/lidar_marker_localizer/schema/lidar_marker_localizer.schema.json b/localization/landmark_based_localizer/lidar_marker_localizer/schema/lidar_marker_localizer.schema.json new file mode 100644 index 0000000000000..df7c9011d9a4f --- /dev/null +++ b/localization/landmark_based_localizer/lidar_marker_localizer/schema/lidar_marker_localizer.schema.json @@ -0,0 +1,142 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Lidar Marker Localizer Node", + "type": "object", + "definitions": { + "lidar_marker_localizer": { + "type": "object", + "properties": { + "marker_name": { + "type": "string", + "description": "The name of the markers listed in the HD map.", + "default": "reflector" + }, + "resolution": { + "type": "number", + "description": "Grid size for marker detection algorithm. [m]", + "default": 0.05, + "minimum": 0.0 + }, + "intensity_pattern": { + "type": "array", + "description": "A sequence of high/low intensity to perform pattern matching. 1: high intensity (positive match), 0: not consider, -1: low intensity (negative match)", + "default": [-1, -1, 0, 1, 1, 1, 1, 1, 0, -1, -1] + }, + "match_intensity_difference_threshold": { + "type": "number", + "description": "Threshold for determining high/low.", + "default": 20, + "minimum": 0 + }, + "positive_match_num_threshold": { + "type": "number", + "description": "Threshold for the number of required matches with the pattern.", + "default": 3, + "minimum": 0 + }, + "negative_match_num_threshold": { + "type": "number", + "description": "Threshold for the number of required matches with the pattern.", + "default": 3, + "minimum": 0 + }, + "vote_threshold_for_detect_marker": { + "type": "number", + "description": "Threshold for the number of rings matching the pattern needed to detect it as a marker.", + "default": 20, + "minimum": 0 + }, + "marker_height_from_ground": { + "type": "number", + "description": "Height from the ground to the center of the marker. [m]", + "default": 1.075 + }, + "self_pose_timeout_sec": { + "type": "number", + "description": "Timeout for self pose. [sec]", + "default": 1.0, + "minimum": 0.0 + }, + "self_pose_distance_tolerance_m": { + "type": "number", + "description": "Tolerance for the distance between two points when performing linear interpolation. [m]", + "default": 1.0, + "minimum": 0.0 + }, + "limit_distance_from_self_pose_to_nearest_marker": { + "type": "number", + "description": "Distance limit for the purpose of determining whether the node should detect a marker. [m]", + "default": 2.0, + "minimum": 0.0 + }, + "limit_distance_from_self_pose_to_marker": { + "type": "number", + "description": "Distance limit for avoiding miss detection. [m]", + "default": 2.0, + "minimum": 0.0 + }, + "base_covariance": { + "type": "array", + "description": "Output covariance in the base_link coordinate.", + "default": [ + 0.04, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.04, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.00007569, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.00007569, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.00030625 + ] + }, + "enable_save_log": { + "type": "boolean", + "description": "", + "default": false + }, + "savefile_directory_path": { + "type": "string", + "description": "", + "default": "$(env HOME)/detected_reflector_intensity" + }, + "savefile_name": { + "type": "string", + "description": "", + "default": "detected_reflector_intensity" + }, + "save_frame_id": { + "type": "string", + "description": "", + "default": "velodyne_top" + } + }, + "required": [ + "resolution", + "intensity_pattern", + "match_intensity_difference_threshold", + "positive_match_num_threshold", + "negative_match_num_threshold", + "vote_threshold_for_detect_marker", + "self_pose_timeout_sec", + "self_pose_distance_tolerance_m", + "limit_distance_from_self_pose_to_nearest_marker", + "limit_distance_from_self_pose_to_marker", + "base_covariance", + "enable_save_log", + "savefile_directory_path", + "savefile_name", + "save_frame_id" + ], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/lidar_marker_localizer" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/localization/landmark_based_localizer/lidar_marker_localizer/src/diagnostics/diagnostics_module.cpp b/localization/landmark_based_localizer/lidar_marker_localizer/src/diagnostics/diagnostics_module.cpp new file mode 100644 index 0000000000000..3d6bb0cd56ca3 --- /dev/null +++ b/localization/landmark_based_localizer/lidar_marker_localizer/src/diagnostics/diagnostics_module.cpp @@ -0,0 +1,137 @@ +// 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 "diagnostics_module.hpp" + +#include + +#include + +#include +#include +#include + +DiagnosticsModule::DiagnosticsModule( + rclcpp::Node * node, const std::string & prefix_diagnostic_name, + const std::string & suffix_diagnostic_name) +{ + node_.reset(node); + if (node_ == nullptr) { + return; // TODO(YamatoAndo) throw error + } + + diagnostics_pub_ = + node_->create_publisher("/diagnostics", 10); + + const auto prefix_name = + prefix_diagnostic_name.empty() ? "" : (prefix_diagnostic_name + std::string(": ")); + const auto suffix_name = + suffix_diagnostic_name.empty() ? "" : (std::string(": ") + suffix_diagnostic_name); + diagnostics_status_msg_.name = prefix_name + std::string(node_->get_name()) + suffix_name; + diagnostics_status_msg_.hardware_id = node_->get_name(); +} + +void DiagnosticsModule::clear() +{ + clearKeyValue(); + clearLevelAndMessage(); +} + +void DiagnosticsModule::clearKeyValue() +{ + diagnostics_status_msg_.values.clear(); + diagnostics_status_msg_.values.shrink_to_fit(); +} + +void DiagnosticsModule::clearLevelAndMessage() +{ + diagnostics_status_msg_.level = diagnostic_msgs::msg::DiagnosticStatus::OK; + diagnostics_status_msg_.message = ""; +} + +void DiagnosticsModule::addKeyValue(const diagnostic_msgs::msg::KeyValue & key_value_msg) +{ + const auto it = findIteratorByKey(key_value_msg.key); + if (existIterator(it)) { + diagnostics_status_msg_.values + .at(std::distance(std::cbegin(diagnostics_status_msg_.values), it)) + .value = key_value_msg.value; // FIX ME + } else { + diagnostics_status_msg_.values.push_back(key_value_msg); + } +} + +template <> +void DiagnosticsModule::addKeyValue(const std::string & key, const std::string & value) +{ + diagnostic_msgs::msg::KeyValue key_value; + key_value.key = key; + key_value.value = value; + addKeyValue(key_value); +} + +template <> +void DiagnosticsModule::addKeyValue(const std::string & key, const bool & value) +{ + diagnostic_msgs::msg::KeyValue key_value; + key_value.key = key; + key_value.value = value ? "True" : "False"; + addKeyValue(key_value); +} + +void DiagnosticsModule::updateLevelAndMessage(const int8_t level, const std::string & message) +{ + if ((level > diagnostic_msgs::msg::DiagnosticStatus::OK)) { + if (!diagnostics_status_msg_.message.empty()) { + diagnostics_status_msg_.message += "; "; + } + diagnostics_status_msg_.message += message; + } + if (level > diagnostics_status_msg_.level) { + diagnostics_status_msg_.level = level; + } +} + +void DiagnosticsModule::publish() +{ + diagnostics_pub_->publish(createDiagnosticsArray()); +} + +diagnostic_msgs::msg::DiagnosticArray DiagnosticsModule::createDiagnosticsArray() const +{ + diagnostic_msgs::msg::DiagnosticArray diagnostics_msg; + diagnostics_msg.header.stamp = node_->now(); + diagnostics_msg.status.push_back(diagnostics_status_msg_); + + if (diagnostics_msg.status.at(0).level == diagnostic_msgs::msg::DiagnosticStatus::OK) { + diagnostics_msg.status.at(0).message = "OK"; + } + + return diagnostics_msg; +} + +std::vector::const_iterator DiagnosticsModule::findIteratorByKey( + const std::string & key) const +{ + const auto it = std::find_if( + std::begin(diagnostics_status_msg_.values), std::end(diagnostics_status_msg_.values), + [key](const auto & arg) { return arg.key == key; }); + return it; +} + +bool DiagnosticsModule::existIterator( + const std::vector::const_iterator & it) const +{ + return it != std::cend(diagnostics_status_msg_.values); +} diff --git a/localization/landmark_based_localizer/lidar_marker_localizer/src/diagnostics/diagnostics_module.hpp b/localization/landmark_based_localizer/lidar_marker_localizer/src/diagnostics/diagnostics_module.hpp new file mode 100644 index 0000000000000..14360a70b6076 --- /dev/null +++ b/localization/landmark_based_localizer/lidar_marker_localizer/src/diagnostics/diagnostics_module.hpp @@ -0,0 +1,66 @@ +// 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 DIAGNOSTICS__DIAGNOSTICS_MODULE_HPP_ +#define DIAGNOSTICS__DIAGNOSTICS_MODULE_HPP_ + +#include + +#include + +#include +#include + +class DiagnosticsModule +{ +public: + DiagnosticsModule( + rclcpp::Node * node, const std::string & prefix_diagnostic_name = "", + const std::string & suffix_diagnostic_name = ""); + void clear(); + void clearKeyValue(); + void clearLevelAndMessage(); + void addKeyValue(const diagnostic_msgs::msg::KeyValue & key_value_msg); + template + void addKeyValue(const std::string & key, const T & value); + void updateLevelAndMessage(const int8_t level, const std::string & message); + void publish(); + +private: + diagnostic_msgs::msg::DiagnosticArray createDiagnosticsArray() const; + std::vector::const_iterator findIteratorByKey( + const std::string & key) const; + bool existIterator(const std::vector::const_iterator & it) const; + + rclcpp::Node::SharedPtr node_; + rclcpp::Publisher::SharedPtr diagnostics_pub_; + + diagnostic_msgs::msg::DiagnosticStatus diagnostics_status_msg_; +}; + +template +void DiagnosticsModule::addKeyValue(const std::string & key, const T & value) +{ + diagnostic_msgs::msg::KeyValue key_value; + key_value.key = key; + key_value.value = std::to_string(value); + addKeyValue(key_value); +} + +template <> +void DiagnosticsModule::addKeyValue(const std::string & key, const std::string & value); +template <> +void DiagnosticsModule::addKeyValue(const std::string & key, const bool & value); + +#endif // DIAGNOSTICS__DIAGNOSTICS_MODULE_HPP_ diff --git a/localization/landmark_based_localizer/lidar_marker_localizer/src/lidar_marker_localizer.cpp b/localization/landmark_based_localizer/lidar_marker_localizer/src/lidar_marker_localizer.cpp new file mode 100644 index 0000000000000..900931b23fea5 --- /dev/null +++ b/localization/landmark_based_localizer/lidar_marker_localizer/src/lidar_marker_localizer.cpp @@ -0,0 +1,603 @@ +// 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 "lidar_marker_localizer.hpp" + +#include +#include +#include +#include + +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +LidarMarkerLocalizer::LidarMarkerLocalizer() : Node("lidar_marker_localizer"), is_activated_(false) +{ + using std::placeholders::_1; + using std::placeholders::_2; + + param_.marker_name = this->declare_parameter("marker_name"); + param_.resolution = this->declare_parameter("resolution"); + param_.intensity_pattern = this->declare_parameter>("intensity_pattern"); + param_.match_intensity_difference_threshold = + this->declare_parameter("match_intensity_difference_threshold"); + param_.positive_match_num_threshold = + this->declare_parameter("positive_match_num_threshold"); + param_.negative_match_num_threshold = + this->declare_parameter("negative_match_num_threshold"); + param_.vote_threshold_for_detect_marker = + this->declare_parameter("vote_threshold_for_detect_marker"); + param_.marker_height_from_ground = this->declare_parameter("marker_height_from_ground"); + param_.self_pose_timeout_sec = this->declare_parameter("self_pose_timeout_sec"); + param_.self_pose_distance_tolerance_m = + this->declare_parameter("self_pose_distance_tolerance_m"); + param_.limit_distance_from_self_pose_to_nearest_marker = + this->declare_parameter("limit_distance_from_self_pose_to_nearest_marker"); + param_.limit_distance_from_self_pose_to_marker = + this->declare_parameter("limit_distance_from_self_pose_to_marker"); + std::vector base_covariance = + this->declare_parameter>("base_covariance"); + for (std::size_t i = 0; i < base_covariance.size(); ++i) { + param_.base_covariance[i] = base_covariance[i]; + } + + param_.enable_save_log = this->declare_parameter("enable_save_log"); + param_.savefile_directory_path = this->declare_parameter("savefile_directory_path"); + param_.savefile_name = this->declare_parameter("savefile_name"); + param_.save_frame_id = this->declare_parameter("save_frame_id"); + + ekf_pose_buffer_ = std::make_unique( + this->get_logger(), param_.self_pose_timeout_sec, param_.self_pose_distance_tolerance_m); + + rclcpp::CallbackGroup::SharedPtr points_callback_group; + points_callback_group = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + auto points_sub_opt = rclcpp::SubscriptionOptions(); + points_sub_opt.callback_group = points_callback_group; + sub_points_ = this->create_subscription( + "~/input/pointcloud_ex", rclcpp::QoS(1).best_effort(), + std::bind(&LidarMarkerLocalizer::points_callback, this, _1), points_sub_opt); + + rclcpp::CallbackGroup::SharedPtr self_pose_callback_group; + self_pose_callback_group = + this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + auto self_pose_sub_opt = rclcpp::SubscriptionOptions(); + self_pose_sub_opt.callback_group = self_pose_callback_group; + sub_self_pose_ = this->create_subscription( + "~/input/ekf_pose", rclcpp::QoS(1), + std::bind(&LidarMarkerLocalizer::self_pose_callback, this, _1), self_pose_sub_opt); + sub_map_bin_ = this->create_subscription( + "~/input/lanelet2_map", rclcpp::QoS(10).durability(rclcpp::DurabilityPolicy::TransientLocal), + std::bind(&LidarMarkerLocalizer::map_bin_callback, this, _1)); + + pub_base_link_pose_with_covariance_on_map_ = + this->create_publisher("~/output/pose_with_covariance", 10); + rclcpp::QoS qos_marker = rclcpp::QoS(rclcpp::KeepLast(10)); + qos_marker.transient_local(); + qos_marker.reliable(); + pub_marker_mapped_ = this->create_publisher("~/debug/marker_mapped", qos_marker); + pub_marker_detected_ = this->create_publisher("~/debug/marker_detected", 10); + pub_debug_pose_with_covariance_ = + this->create_publisher("~/debug/pose_with_covariance", 10); + pub_marker_pointcloud = this->create_publisher("~/debug/marker_pointcloud", 10); + service_trigger_node_ = this->create_service( + "~/service/trigger_node_srv", + std::bind(&LidarMarkerLocalizer::service_trigger_node, this, _1, _2), + rclcpp::ServicesQoS().get_rmw_qos_profile(), points_callback_group); + + tf_buffer_ = std::make_shared(this->get_clock()); + tf_listener_ = std::make_shared(*tf_buffer_, this, false); + + diagnostics_module_.reset(new DiagnosticsModule(this, "localization", "")); +} + +void LidarMarkerLocalizer::initialize_diagnostics() +{ + diagnostics_module_->clear(); + diagnostics_module_->addKeyValue("is_received_map", false); + diagnostics_module_->addKeyValue("is_received_self_pose", false); + diagnostics_module_->addKeyValue("detect_marker_num", 0); + diagnostics_module_->addKeyValue("distance_self_pose_to_nearest_marker", 0.0); + diagnostics_module_->addKeyValue( + "limit_distance_from_self_pose_to_nearest_marker", + param_.limit_distance_from_self_pose_to_nearest_marker); + diagnostics_module_->addKeyValue("distance_lanelet2_marker_to_detected_marker", 0.0); + diagnostics_module_->addKeyValue( + "limit_distance_from_lanelet2_marker_to_detected_marker", + param_.limit_distance_from_self_pose_to_marker); +} + +void LidarMarkerLocalizer::map_bin_callback(const HADMapBin::ConstSharedPtr & msg) +{ + landmark_manager_.parse_landmarks(msg, param_.marker_name); + const MarkerArray marker_msg = landmark_manager_.get_landmarks_as_marker_array_msg(); + pub_marker_mapped_->publish(marker_msg); +} + +void LidarMarkerLocalizer::self_pose_callback( + const PoseWithCovarianceStamped::ConstSharedPtr & self_pose_msg_ptr) +{ + // TODO(YamatoAndo) + // if (!is_activated_) return; + + if (self_pose_msg_ptr->header.frame_id == "map") { + ekf_pose_buffer_->push_back(self_pose_msg_ptr); + } else { + RCLCPP_ERROR_STREAM_THROTTLE( + get_logger(), *this->get_clock(), 1000, + "Received initial pose message with frame_id " + << self_pose_msg_ptr->header.frame_id << ", but expected map. " + << "Please check the frame_id in the input topic and ensure it is correct."); + } +} + +void LidarMarkerLocalizer::points_callback(const PointCloud2::ConstSharedPtr & points_msg_ptr) +{ + initialize_diagnostics(); + + main_process(std::move(points_msg_ptr)); + + diagnostics_module_->publish(); +} + +void LidarMarkerLocalizer::main_process(const PointCloud2::ConstSharedPtr & points_msg_ptr) +{ + const builtin_interfaces::msg::Time sensor_ros_time = points_msg_ptr->header.stamp; + + // (1) check if the map have be received + const std::vector map_landmarks = landmark_manager_.get_landmarks(); + const bool is_received_map = !map_landmarks.empty(); + diagnostics_module_->addKeyValue("is_received_map", is_received_map); + if (!is_received_map) { + std::stringstream message; + message << "Not receive the landmark information. Please check if the vector map is being " + << "published and if the landmark information is correctly specified."; + RCLCPP_INFO_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, message.str()); + diagnostics_module_->updateLevelAndMessage( + diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); + return; + } + + // (2) get Self Pose + const std::optional interpolate_result = + ekf_pose_buffer_->interpolate(sensor_ros_time); + + const bool is_received_self_pose = interpolate_result != std::nullopt; + diagnostics_module_->addKeyValue("is_received_self_pose", is_received_self_pose); + if (!is_received_self_pose) { + std::stringstream message; + message << "Could not get self_pose. Please check if the self pose is being published and if " + << "the timestamp of the self pose is correctly specified"; + RCLCPP_INFO_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, message.str()); + diagnostics_module_->updateLevelAndMessage( + diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); + return; + } + + ekf_pose_buffer_->pop_old(sensor_ros_time); + const Pose self_pose = interpolate_result.value().interpolated_pose.pose.pose; + + // (3) detect marker + const std::vector detected_landmarks = + detect_landmarks(points_msg_ptr); + + const bool is_detected_marker = !detected_landmarks.empty(); + diagnostics_module_->addKeyValue("detect_marker_num", detected_landmarks.size()); + + // (4) check distance to the nearest marker + const landmark_manager::Landmark nearest_marker = get_nearest_landmark(self_pose, map_landmarks); + const Pose nearest_marker_pose_on_base_link = + tier4_autoware_utils::inverseTransformPose(nearest_marker.pose, self_pose); + + const double distance_from_self_pose_to_nearest_marker = + std::abs(nearest_marker_pose_on_base_link.position.x); + diagnostics_module_->addKeyValue( + "distance_self_pose_to_nearest_marker", distance_from_self_pose_to_nearest_marker); + + const bool is_exist_marker_within_self_pose = + distance_from_self_pose_to_nearest_marker < + param_.limit_distance_from_self_pose_to_nearest_marker; + + if (!is_detected_marker) { + if (!is_exist_marker_within_self_pose) { + std::stringstream message; + message << "Could not detect marker, because the distance from self_pose to nearest_marker " + << "is too far (" << distance_from_self_pose_to_nearest_marker << " [m])."; + diagnostics_module_->updateLevelAndMessage( + diagnostic_msgs::msg::DiagnosticStatus::OK, message.str()); + } else { + std::stringstream message; + message << "Could not detect marker, although the distance from self_pose to nearest_marker " + << "is near (" << distance_from_self_pose_to_nearest_marker << " [m])."; + RCLCPP_INFO_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, message.str()); + diagnostics_module_->updateLevelAndMessage( + diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); + } + return; + } + + // for debug + if (pub_marker_detected_->get_subscription_count() > 0) { + PoseArray pose_array_msg; + pose_array_msg.header.stamp = sensor_ros_time; + pose_array_msg.header.frame_id = "map"; + for (const landmark_manager::Landmark & landmark : detected_landmarks) { + const Pose detected_marker_on_map = + tier4_autoware_utils::transformPose(landmark.pose, self_pose); + pose_array_msg.poses.push_back(detected_marker_on_map); + } + pub_marker_detected_->publish(pose_array_msg); + } + + // (4) calculate diff pose + const Pose new_self_pose = + landmark_manager_.calculate_new_self_pose(detected_landmarks, self_pose, false); + + const double diff_x = new_self_pose.position.x - self_pose.position.x; + const double diff_y = new_self_pose.position.y - self_pose.position.y; + + const double diff_norm = std::hypot(diff_x, diff_y); + const bool is_exist_marker_within_lanelet2_map = + diff_norm < param_.limit_distance_from_self_pose_to_marker; + + diagnostics_module_->addKeyValue("distance_lanelet2_marker_to_detected_marker", diff_norm); + if (!is_exist_marker_within_lanelet2_map) { + std::stringstream message; + message << "The distance from lanelet2 to the detect marker is too far(" << diff_norm + << " [m]). The limit is " << param_.limit_distance_from_self_pose_to_marker << "."; + RCLCPP_INFO_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, message.str()); + diagnostics_module_->updateLevelAndMessage( + diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); + return; + } + + if (distance_from_self_pose_to_nearest_marker < 2.0) { + save_intensity(points_msg_ptr, detected_landmarks.at(0).pose); + } + + // (5) Apply diff pose to self pose + // only x and y is changed + PoseWithCovarianceStamped result; + result.header.stamp = sensor_ros_time; + result.header.frame_id = "map"; + result.pose.pose.position.x = new_self_pose.position.x; + result.pose.pose.position.y = new_self_pose.position.y; + result.pose.pose.position.z = self_pose.position.z; + result.pose.pose.orientation = self_pose.orientation; + + // set covariance + const Eigen::Quaterniond map_to_base_link_quat = Eigen::Quaterniond( + result.pose.pose.orientation.w, result.pose.pose.orientation.x, result.pose.pose.orientation.y, + result.pose.pose.orientation.z); + const Eigen::Matrix3d map_to_base_link_rotation = + map_to_base_link_quat.normalized().toRotationMatrix(); + result.pose.covariance = rotate_covariance(param_.base_covariance, map_to_base_link_rotation); + + pub_base_link_pose_with_covariance_on_map_->publish(result); + pub_debug_pose_with_covariance_->publish(result); +} + +void LidarMarkerLocalizer::service_trigger_node( + const SetBool::Request::SharedPtr req, SetBool::Response::SharedPtr res) +{ + is_activated_ = req->data; + if (is_activated_) { + ekf_pose_buffer_->clear(); + } else { + } + res->success = true; +} + +std::vector LidarMarkerLocalizer::detect_landmarks( + const PointCloud2::ConstSharedPtr & points_msg_ptr) +{ + // TODO(YamatoAndo) + // Transform sensor_frame to base_link + + pcl::PointCloud::Ptr points_ptr( + new pcl::PointCloud); + pcl::fromROSMsg(*points_msg_ptr, *points_ptr); + + if (points_ptr->empty()) { + RCLCPP_WARN_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, "No points!"); + return std::vector{}; + } + + std::vector> ring_points(128); + + float min_x = std::numeric_limits::max(); + float max_x = std::numeric_limits::lowest(); + for (const autoware_point_types::PointXYZIRADRT & point : points_ptr->points) { + ring_points[point.ring].push_back(point); + min_x = std::min(min_x, point.x); + max_x = std::max(max_x, point.x); + } + + // Check that the leaf size is not too small, given the size of the data + const int bin_num = static_cast((max_x - min_x) / param_.resolution + 1); + + // initialize variables + std::vector vote(bin_num, 0); + std::vector min_y(bin_num, std::numeric_limits::max()); + + // for each ring + for (const pcl::PointCloud & one_ring : ring_points) { + std::vector intensity_sum(bin_num, 0.0); + std::vector intensity_num(bin_num, 0); + std::vector average_intensity(bin_num, 0.0); + + for (const autoware_point_types::PointXYZIRADRT & point : one_ring.points) { + const int bin_index = static_cast((point.x - min_x) / param_.resolution); + intensity_sum[bin_index] += point.intensity; + intensity_num[bin_index]++; + min_y[bin_index] = std::min(min_y[bin_index], point.y); + } + + // calc average + for (int bin_index = 0; bin_index < bin_num; bin_index++) { + if (intensity_num[bin_index] == 0) { + continue; + } + + average_intensity[bin_index] = intensity_sum[bin_index] / intensity_num[bin_index]; + } + + // pattern matching + for (size_t i = 0; i <= bin_num - param_.intensity_pattern.size(); i++) { + int64_t pos = 0; + int64_t neg = 0; + double min_intensity = std::numeric_limits::max(); + double max_intensity = std::numeric_limits::lowest(); + + // find max_min + for (size_t j = 0; j < param_.intensity_pattern.size(); j++) { + if (intensity_num[i + j] == 0) { + continue; + } + + min_intensity = std::min(min_intensity, average_intensity[i + j]); + max_intensity = std::max(max_intensity, average_intensity[i + j]); + } + + if (max_intensity <= min_intensity) { + continue; + } + + const double center_intensity = (max_intensity - min_intensity) / 2.0 + min_intensity; + + for (size_t j = 0; j < param_.intensity_pattern.size(); j++) { + if (intensity_num[i + j] == 0) { + continue; + } + + if (param_.intensity_pattern[j] == 1) { + // check positive + if ( + average_intensity[i + j] > + center_intensity + param_.match_intensity_difference_threshold) { + pos++; + } + } else if (param_.intensity_pattern[j] == -1) { + // check negative + if ( + average_intensity[i + j] < + center_intensity - param_.match_intensity_difference_threshold) { + neg++; + } + } else { + // ignore param_.intensity_pattern[j] == 0 + } + } + + if ( + pos >= param_.positive_match_num_threshold && neg >= param_.negative_match_num_threshold) { + vote[i]++; + } + } + } + + std::vector detected_landmarks; + + for (size_t i = 0; i < bin_num - param_.intensity_pattern.size(); i++) { + if (vote[i] > param_.vote_threshold_for_detect_marker) { + const double bin_position = + static_cast(i) + static_cast(param_.intensity_pattern.size()) / 2.0; + Pose marker_pose_on_base_link; + marker_pose_on_base_link.position.x = bin_position * param_.resolution + min_x; + marker_pose_on_base_link.position.y = min_y[i]; + marker_pose_on_base_link.position.z = param_.marker_height_from_ground; + marker_pose_on_base_link.orientation = + tier4_autoware_utils::createQuaternionFromRPY(M_PI_2, 0.0, 0.0); // TODO(YamatoAndo) + detected_landmarks.push_back(landmark_manager::Landmark{"0", marker_pose_on_base_link}); + } + } + + return detected_landmarks; +} + +landmark_manager::Landmark LidarMarkerLocalizer::get_nearest_landmark( + const geometry_msgs::msg::Pose & self_pose, + const std::vector & landmarks) const +{ + landmark_manager::Landmark nearest_landmark; + double min_distance = std::numeric_limits::max(); + + for (const auto & landmark : landmarks) { + const double curr_distance = + tier4_autoware_utils::calcDistance3d(landmark.pose.position, self_pose.position); + + if (curr_distance > min_distance) { + continue; + } + + min_distance = curr_distance; + nearest_landmark = landmark; + } + return nearest_landmark; +} + +std::array LidarMarkerLocalizer::rotate_covariance( + const std::array & src_covariance, const Eigen::Matrix3d & rotation) const +{ + std::array ret_covariance = src_covariance; + + Eigen::Matrix3d src_cov; + src_cov << src_covariance[0], src_covariance[1], src_covariance[2], src_covariance[6], + src_covariance[7], src_covariance[8], src_covariance[12], src_covariance[13], + src_covariance[14]; + + Eigen::Matrix3d ret_cov; + ret_cov = rotation * src_cov * rotation.transpose(); + + for (Eigen::Index i = 0; i < 3; ++i) { + ret_covariance[i] = ret_cov(0, i); + ret_covariance[i + 6] = ret_cov(1, i); + ret_covariance[i + 12] = ret_cov(2, i); + } + + return ret_covariance; +} + +void LidarMarkerLocalizer::save_intensity( + const PointCloud2::ConstSharedPtr & points_msg_ptr, const Pose marker_pose) +{ + if (!param_.enable_save_log) { + return; + } + + // convert from ROSMsg to PCL + pcl::PointCloud::Ptr points_ptr( + new pcl::PointCloud); + pcl::fromROSMsg(*points_msg_ptr, *points_ptr); + + pcl::PointCloud::Ptr marker_points_ptr( + new pcl::PointCloud); + pcl::PointCloud::Ptr marker_points_xyzi_ptr(new pcl::PointCloud); + std::vector ring_array; + + // extract marker pointcloud + for (const autoware_point_types::PointXYZIRADRT & point : points_ptr->points) { + const double xy_distance = std::sqrt( + std::pow(point.x - marker_pose.position.x, 2.0) + + std::pow(point.y - marker_pose.position.y, 2.0)); + // const double z_distance = std::fabs(point.z - marker_pose.position.z); + // if (xy_distance < 0.40 && z_distance < 0.55) { + if (xy_distance < 0.40) { + marker_points_ptr->push_back(point); + + pcl::PointXYZI xyzi; + xyzi.x = point.x; + xyzi.y = point.y; + xyzi.z = point.z; + xyzi.intensity = point.intensity; + marker_points_xyzi_ptr->push_back(xyzi); + + ring_array.push_back(point.ring); + } + } + + // transform input_frame to save_frame_id + pcl::PointCloud::Ptr marker_points_sensor_frame_ptr( + new pcl::PointCloud); + transform_sensor_measurement( + param_.save_frame_id, points_msg_ptr->header.frame_id, marker_points_xyzi_ptr, + marker_points_sensor_frame_ptr); + + // to csv format + std::stringstream log_message; + size_t i = 0; + log_message << "point.position.x,point.position.y,point.position.z,point.intensity,point.ring" + << std::endl; + for (const auto & point : marker_points_sensor_frame_ptr->points) { + log_message << point.x; + log_message << "," << point.y; + log_message << "," << point.z; + log_message << "," << point.intensity; + log_message << "," << ring_array.at(i++); + log_message << std::endl; + } + + // create file name + const double times_seconds = rclcpp::Time(points_msg_ptr->header.stamp).seconds(); + double time_integer_tmp; + double time_decimal = std::modf(times_seconds, &time_integer_tmp); + long int time_integer = static_cast(time_integer_tmp); + struct tm * time_info; + time_info = std::localtime(&time_integer); + std::stringstream file_name; + file_name << param_.savefile_name << std::put_time(time_info, "%Y%m%d-%H%M%S") << "-" + << std::setw(3) << std::setfill('0') << static_cast((time_decimal)*1000) << ".csv"; + + // write log_message to file + std::filesystem::path savefile_directory_path = param_.savefile_directory_path; + std::filesystem::create_directories(savefile_directory_path); + std::ofstream csv_file(savefile_directory_path.append(file_name.str())); + csv_file << log_message.str(); + csv_file.close(); + std::cerr << savefile_directory_path.c_str() << std::endl; + + // visualize for debug + marker_points_sensor_frame_ptr->width = marker_points_sensor_frame_ptr->size(); + marker_points_sensor_frame_ptr->height = 1; + marker_points_sensor_frame_ptr->is_dense = false; + + PointCloud2 viz_pointcloud_msg; + pcl::toROSMsg(*marker_points_sensor_frame_ptr, viz_pointcloud_msg); + viz_pointcloud_msg.header = points_msg_ptr->header; + viz_pointcloud_msg.header.frame_id = param_.save_frame_id; + + pub_marker_pointcloud->publish(viz_pointcloud_msg); +} + +template +void LidarMarkerLocalizer::transform_sensor_measurement( + const std::string & source_frame, const std::string & target_frame, + const pcl::shared_ptr> & sensor_points_input_ptr, + pcl::shared_ptr> & sensor_points_output_ptr) +{ + if (source_frame == target_frame) { + sensor_points_output_ptr = sensor_points_input_ptr; + return; + } + + geometry_msgs::msg::TransformStamped transform; + try { + transform = tf_buffer_->lookupTransform(source_frame, target_frame, tf2::TimePointZero); + } catch (tf2::TransformException & ex) { + RCLCPP_WARN(this->get_logger(), "%s", ex.what()); + RCLCPP_WARN( + this->get_logger(), "Please publish TF %s to %s", target_frame.c_str(), source_frame.c_str()); + // Since there is no clear error handling policy, temporarily return as is. + sensor_points_output_ptr = sensor_points_input_ptr; + return; + } + + const geometry_msgs::msg::PoseStamped target_to_source_pose_stamped = + tier4_autoware_utils::transform2pose(transform); + const Eigen::Matrix4f base_to_sensor_matrix = + pose_to_matrix4f(target_to_source_pose_stamped.pose); + tier4_autoware_utils::transformPointCloud( + *sensor_points_input_ptr, *sensor_points_output_ptr, base_to_sensor_matrix); +} diff --git a/localization/landmark_based_localizer/lidar_marker_localizer/src/lidar_marker_localizer.hpp b/localization/landmark_based_localizer/lidar_marker_localizer/src/lidar_marker_localizer.hpp new file mode 100644 index 0000000000000..b76b56eda4129 --- /dev/null +++ b/localization/landmark_based_localizer/lidar_marker_localizer/src/lidar_marker_localizer.hpp @@ -0,0 +1,143 @@ +// 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 LIDAR_MARKER_LOCALIZER_HPP_ +#define LIDAR_MARKER_LOCALIZER_HPP_ + +#include "diagnostics/diagnostics_module.hpp" +#include "localization_util/smart_pose_buffer.hpp" + +#include + +#include +#include + +#include +#include + +#include +#include +#include +#include +#include + +#ifdef ROS_DISTRO_GALACTIC +#include +#include +#include +#else +#include + +#include +#include +#endif +#include + +#include + +#include +#include +#include + +class LidarMarkerLocalizer : public rclcpp::Node +{ + using HADMapBin = autoware_auto_mapping_msgs::msg::HADMapBin; + using MarkerArray = visualization_msgs::msg::MarkerArray; + using Pose = geometry_msgs::msg::Pose; + using PoseStamped = geometry_msgs::msg::PoseStamped; + using PoseWithCovarianceStamped = geometry_msgs::msg::PoseWithCovarianceStamped; + using PoseArray = geometry_msgs::msg::PoseArray; + using TransformStamped = geometry_msgs::msg::TransformStamped; + using Vector3 = geometry_msgs::msg::Vector3; + using PointCloud2 = sensor_msgs::msg::PointCloud2; + using SetBool = std_srvs::srv::SetBool; + using DiagnosticStatus = diagnostic_msgs::msg::DiagnosticStatus; + + struct Param + { + std::string marker_name; + + double resolution; + std::vector intensity_pattern; + int64_t match_intensity_difference_threshold; + int64_t positive_match_num_threshold; + int64_t negative_match_num_threshold; + int64_t vote_threshold_for_detect_marker; + double marker_height_from_ground; + + double self_pose_timeout_sec; + double self_pose_distance_tolerance_m; + + double limit_distance_from_self_pose_to_nearest_marker; + double limit_distance_from_self_pose_to_marker; + std::array base_covariance; + + bool enable_save_log; + std::string savefile_directory_path; + std::string savefile_name; + std::string save_frame_id; + }; + +public: + LidarMarkerLocalizer(); + +private: + void self_pose_callback(const PoseWithCovarianceStamped::ConstSharedPtr & self_pose_msg_ptr); + void points_callback(const PointCloud2::ConstSharedPtr & points_msg_ptr); + void map_bin_callback(const HADMapBin::ConstSharedPtr & msg); + void service_trigger_node( + const SetBool::Request::SharedPtr req, SetBool::Response::SharedPtr res); + + void initialize_diagnostics(); + void main_process(const PointCloud2::ConstSharedPtr & points_msg_ptr); + std::vector detect_landmarks( + const PointCloud2::ConstSharedPtr & points_msg_ptr); + landmark_manager::Landmark get_nearest_landmark( + const geometry_msgs::msg::Pose & self_pose, + const std::vector & landmarks) const; + std::array rotate_covariance( + const std::array & src_covariance, const Eigen::Matrix3d & rotation) const; + void save_intensity(const PointCloud2::ConstSharedPtr & points_msg_ptr, const Pose marker_pose); + + template + void transform_sensor_measurement( + const std::string & source_frame, const std::string & target_frame, + const pcl::shared_ptr> & sensor_points_input_ptr, + pcl::shared_ptr> & sensor_points_output_ptr); + + std::shared_ptr tf_listener_; + std::shared_ptr tf_buffer_; + + rclcpp::Subscription::SharedPtr sub_points_; + rclcpp::Subscription::SharedPtr sub_self_pose_; + rclcpp::Subscription::SharedPtr sub_map_bin_; + + rclcpp::Publisher::SharedPtr + pub_base_link_pose_with_covariance_on_map_; + rclcpp::Service::SharedPtr service_trigger_node_; + rclcpp::Publisher::SharedPtr pub_marker_mapped_; + rclcpp::Publisher::SharedPtr pub_marker_detected_; + rclcpp::Publisher::SharedPtr pub_debug_pose_with_covariance_; + rclcpp::Publisher::SharedPtr pub_marker_pointcloud; + + std::shared_ptr diagnostics_module_; + + Param param_; + bool is_activated_; + std::unique_ptr ekf_pose_buffer_; + + landmark_manager::LandmarkManager landmark_manager_; +}; + +#endif // LIDAR_MARKER_LOCALIZER_HPP_ diff --git a/localization/landmark_based_localizer/lidar_marker_localizer/src/main.cpp b/localization/landmark_based_localizer/lidar_marker_localizer/src/main.cpp new file mode 100644 index 0000000000000..615a20edc1d3f --- /dev/null +++ b/localization/landmark_based_localizer/lidar_marker_localizer/src/main.cpp @@ -0,0 +1,29 @@ +// 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 "lidar_marker_localizer.hpp" + +#include + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + auto node_ptr = std::make_shared(); + rclcpp::executors::MultiThreadedExecutor exec; + exec.add_node(node_ptr); + exec.spin(); + rclcpp::shutdown(); + + return 0; +}