feat: migrate localization_util from universe to core: v0.0

Signed-off-by: liuXinGangChina <[email protected]>

localization/autoware_localization_util/CMakeLists.txt

@@ -0,0 +1,30 @@
+cmake_minimum_required(VERSION 3.14)
+project(autoware_localization_util)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+ament_auto_add_library(${PROJECT_NAME} SHARED
+  src/util_func.cpp
+  src/diagnostics_module.cpp
+  src/smart_pose_buffer.cpp
+  src/tree_structured_parzen_estimator.cpp
+  src/covariance_ellipse.cpp
+)
+
+if(BUILD_TESTING)
+  find_package(ament_cmake_gtest REQUIRED)
+  ament_auto_add_gtest(test_smart_pose_buffer
+    test/test_smart_pose_buffer.cpp
+    src/smart_pose_buffer.cpp
+  )
+
+  ament_auto_add_gtest(test_tpe
+    test/test_tpe.cpp
+    src/tree_structured_parzen_estimator.cpp
+  )
+endif()
+
+ament_auto_package(
+  INSTALL_TO_SHARE
+)

localization/autoware_localization_util/README.md

@@ -0,0 +1,171 @@
+# autoware_localization_util
+
+## Overview
+`autoware_localization_util` is a collection of localization utility packages. It contains 5 individual libiary that used by autoware localization nodes.
+
+- `covariance_ellipse` 2d covariance visualization wrapper.
+- `diagnostics_module` a diagnostics library designed for localization nodes's diagnostics message management and publish.
+- `smart_pose_buffer` pose buffer management library which contains interpolate and data validation.
+- `tree_structured_parzen_estimator` A Tree Structured Parzen Estimator (AKA TSPE) library.
+- `util_func` A tool library which contains several function for localization nodes.
+
+## Design
+- `covariance_ellipse` Translate `geometry_msgs::msg::PoseWithCovariance` message into ellipse visual marker to demonstrate covariance distribution.
+- `diagnostics_module` Manage diagnostics message's content, level and publish timing of localization nodes.
+- `smart_pose_buffer` A buffer library which implements pose message buffering, pose interpolate and pose validation.
+- `tree_structured_parzen_estimator` A Probability Estimator (AKA TSPE) library that includes estimator and log likelihood ratio calculation.
+- `util_func` Tool function collection.
+
+## Usage
+### covariance_ellipse 
+Include header file to use:
+
+```cpp
+#include "autoware/localization_util/covariance_ellipse.hpp"
+```
+
+calculate ellipse and visualize
+```cpp
+autoware::localization_util::Ellipse ellipse_ = autoware::localization_util::calculate_xy_ellipse(input_msg->pose, scale_);
+
+  const auto ellipse_marker = autoware::localization_util::create_ellipse_marker(
+    ellipse_, input_msg->header, input_msg->pose);
+```
+
+### diagnostics_module
+
+Include header file to use:
+```cpp
+#include "autoware/localization_util/diagnostics_module.hpp"
+```
+
+init diagnostics manager
+```cpp
+std::unique_ptr<autoware::localization_util::DiagnosticsModule> diagnostics_ =
+    std::make_unique<autoware::localization_util::DiagnosticsModule>(this, "gyro_odometer_status");
+```
+
+clean message buffer, add message, set diagnostics message logging level and publish diagnostics message
+```cpp
+diagnostics_->clear();
+diagnostics_->add_key_value(
+"topic_time_stamp",
+static_cast<rclcpp::Time>(vehicle_twist_msg_ptr->header.stamp).nanoseconds());
+
+...
+
+message << "Please publish TF " << output_frame_ << " to "
+            << gyro_queue_.front().header.frame_id;
+    RCLCPP_ERROR_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, message.str());
+diagnostics_->update_level_and_message(
+      diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str());
+...
+
+diagnostics_->publish(vehicle_twist_msg_ptr->header.stamp);
+```
+
+### smart_pose_buffer
+
+buffer init
+```cpp
+#include "autoware/localization_util/smart_pose_buffer.hpp"
+
+using autoware::localization_util::SmartPoseBuffer;
+
+std::unique_ptr<autoware::localization_util::SmartPoseBuffer> initial_pose_buffer_;
+initial_pose_buffer_ = std::make_unique<SmartPoseBuffer>(
+    this->get_logger(), param_.validation.initial_pose_timeout_sec,
+    param_.validation.initial_pose_distance_tolerance_m);
+```
+
+interpolate and pop out old pose message
+```cpp
+std::optional<SmartPoseBuffer::InterpolateResult> interpolation_result_opt =
+initial_pose_buffer_->interpolate(sensor_ros_time);
+
+...
+
+initial_pose_buffer_->pop_old(sensor_ros_time);
+const SmartPoseBuffer::InterpolateResult & interpolation_result =
+interpolation_result_opt.value();
+```
+
+clear buffer
+```cpp
+initial_pose_buffer_->clear();
+```
+
+### tree_structured_parzen_estimator
+
+init the estimator.
+n_startup_trials -- The number of initial random trials in the TPE (Tree-Structured Parzen Estimator). This value should be equal to or less than 'initial_estimate_particles_num' and more than 0. If it is equal to 'initial_estimate_particles_num', the search will be the same as a full random search.
+
+```cpp
+#include "autoware/localization_util/tree_structured_parzen_estimator.hpp"
+
+using autoware::localization_util::TreeStructuredParzenEstimator;
+
+TreeStructuredParzenEstimator tpe(
+TreeStructuredParzenEstimator::Direction::MAXIMIZE,
+param_.initial_pose_estimation.n_startup_trials, sample_mean, sample_stddev);
+```
+
+get estimation result
+```cpp
+const TreeStructuredParzenEstimator::Input input = tpe.get_next_input();
+```
+
+add new data to the estimator
+```cpp
+TreeStructuredParzenEstimator::Input result(6);
+    result[0] = pose.position.x;
+    result[1] = pose.position.y;
+    result[2] = pose.position.z;
+    result[3] = rpy.x;
+    result[4] = rpy.y;
+    result[5] = rpy.z;
+tpe.add_trial(TreeStructuredParzenEstimator::Trial{result, ndt_result.transform_probability});
+```
+
+### util_func
+
+include header file to use
+```cpp
+#include "autoware/localization_util/util_func.hpp"
+
+using autoware::localization_util::exchange_color_crc;
+using autoware::localization_util::matrix4f_to_pose;
+using autoware::localization_util::point_to_vector3d;
+using autoware::localization_util::pose_to_matrix4f;
+```
+
+list of usefull function
+```cpp
+std_msgs::msg::ColorRGBA exchange_color_crc(double x);
+double calc_diff_for_radian(const double lhs_rad, const double rhs_rad);
+geometry_msgs::msg::Vector3 get_rpy(const geometry_msgs::msg::Pose & pose);
+geometry_msgs::msg::Vector3 get_rpy(const geometry_msgs::msg::PoseStamped & pose);
+geometry_msgs::msg::Vector3 get_rpy(const geometry_msgs::msg::PoseWithCovarianceStamped & pose);
+geometry_msgs::msg::Quaternion rpy_rad_to_quaternion(
+  const double r_rad, const double p_rad, const double y_rad);
+geometry_msgs::msg::Quaternion rpy_deg_to_quaternion(
+  const double r_deg, const double p_deg, const double y_deg);
+geometry_msgs::msg::Twist calc_twist(
+  const geometry_msgs::msg::PoseStamped & pose_a, const geometry_msgs::msg::PoseStamped & pose_b);
+geometry_msgs::msg::PoseStamped interpolate_pose(
+  const geometry_msgs::msg::PoseStamped & pose_a, const geometry_msgs::msg::PoseStamped & pose_b,
+  const rclcpp::Time & time_stamp);
+geometry_msgs::msg::PoseStamped interpolate_pose(
+  const geometry_msgs::msg::PoseWithCovarianceStamped & pose_a,
+  const geometry_msgs::msg::PoseWithCovarianceStamped & pose_b, const rclcpp::Time & time_stamp);
+Eigen::Affine3d pose_to_affine3d(const geometry_msgs::msg::Pose & ros_pose);
+Eigen::Matrix4f pose_to_matrix4f(const geometry_msgs::msg::Pose & ros_pose);
+geometry_msgs::msg::Pose matrix4f_to_pose(const Eigen::Matrix4f & eigen_pose_matrix);
+Eigen::Vector3d point_to_vector3d(const geometry_msgs::msg::Point & ros_pos);
+template <class T>
+T transform(const T & input, const geometry_msgs::msg::TransformStamped & transform);double norm(const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2);
+
+void output_pose_with_cov_to_log(
+  const rclcpp::Logger & logger, const std::string & prefix,
+  const geometry_msgs::msg::PoseWithCovarianceStamped & pose_with_cov);
+```

localization/autoware_localization_util/include/autoware/localization_util/covariance_ellipse.hpp

@@ -0,0 +1,44 @@
+// Copyright 2024 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 AUTOWARE__LOCALIZATION_UTIL__COVARIANCE_ELLIPSE_HPP_
+#define AUTOWARE__LOCALIZATION_UTIL__COVARIANCE_ELLIPSE_HPP_
+
+#include <Eigen/Dense>
+
+#include <geometry_msgs/msg/pose_with_covariance.hpp>
+#include <visualization_msgs/msg/marker_array.hpp>
+
+namespace autoware::localization_util
+{
+
+struct Ellipse
+{
+  double long_radius;
+  double short_radius;
+  double yaw;
+  Eigen::Matrix2d P;
+  double size_lateral_direction;
+};
+
+Ellipse calculate_xy_ellipse(
+  const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, const double scale);
+
+visualization_msgs::msg::Marker create_ellipse_marker(
+  const Ellipse & ellipse, const std_msgs::msg::Header & header,
+  const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance);
+
+}  // namespace autoware::localization_util
+
+#endif  // AUTOWARE__LOCALIZATION_UTIL__COVARIANCE_ELLIPSE_HPP_

localization/autoware_localization_util/include/autoware/localization_util/diagnostics_module.hpp

@@ -0,0 +1,64 @@
+// 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 AUTOWARE__LOCALIZATION_UTIL__DIAGNOSTICS_MODULE_HPP_
+#define AUTOWARE__LOCALIZATION_UTIL__DIAGNOSTICS_MODULE_HPP_
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <diagnostic_msgs/msg/diagnostic_array.hpp>
+
+#include <string>
+#include <vector>
+
+namespace autoware::localization_util
+{
+class DiagnosticsModule
+{
+public:
+  DiagnosticsModule(rclcpp::Node * node, const std::string & diagnostic_name);
+  void clear();
+  void add_key_value(const diagnostic_msgs::msg::KeyValue & key_value_msg);
+  template <typename T>
+  void add_key_value(const std::string & key, const T & value);
+  void update_level_and_message(const int8_t level, const std::string & message);
+  void publish(const rclcpp::Time & publish_time_stamp);
+
+private:
+  [[nodiscard]] diagnostic_msgs::msg::DiagnosticArray create_diagnostics_array(
+    const rclcpp::Time & publish_time_stamp) const;
+
+  rclcpp::Clock::SharedPtr clock_;
+  rclcpp::Publisher<diagnostic_msgs::msg::DiagnosticArray>::SharedPtr diagnostics_pub_;
+
+  diagnostic_msgs::msg::DiagnosticStatus diagnostics_status_msg_;
+};
+
+template <typename T>
+void DiagnosticsModule::add_key_value(const std::string & key, const T & value)
+{
+  diagnostic_msgs::msg::KeyValue key_value;
+  key_value.key = key;
+  key_value.value = std::to_string(value);
+  add_key_value(key_value);
+}
+
+template <>
+void DiagnosticsModule::add_key_value(const std::string & key, const std::string & value);
+template <>
+void DiagnosticsModule::add_key_value(const std::string & key, const bool & value);
+
+}  // namespace autoware::localization_util
+
+#endif  // AUTOWARE__LOCALIZATION_UTIL__DIAGNOSTICS_MODULE_HPP_

localization/autoware_localization_util/include/autoware/localization_util/matrix_type.hpp

@@ -0,0 +1,26 @@
+// Copyright 2021 TierIV
+//
+// 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 AUTOWARE__LOCALIZATION_UTIL__MATRIX_TYPE_HPP_
+#define AUTOWARE__LOCALIZATION_UTIL__MATRIX_TYPE_HPP_
+
+#include <Eigen/Core>
+
+namespace autoware::localization_util
+{
+using Matrix6d = Eigen::Matrix<double, 6, 6>;
+using RowMatrixXd = Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>;
+}  // namespace autoware::localization_util
+
+#endif  // AUTOWARE__LOCALIZATION_UTIL__MATRIX_TYPE_HPP_