feat: migrate autoware_localization_util from universe to core (#150)

Signed-off-by: liuXinGangChina <lxg19892021@gmail.com>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
Co-authored-by: SakodaShintaro <shintaro.sakoda@tier4.jp>
Co-authored-by: Yutaka Kondo <yutaka.kondo@youtalk.jp>

Author: 4 people

localization/autoware_localization_util/CMakeLists.txt

@@ -0,0 +1,33 @@
+cmake_minimum_required(VERSION 3.14)
+project(autoware_localization_util)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+include_directories(
+  include
+)
+
+ament_auto_add_library(${PROJECT_NAME} SHARED
+  src/util_func.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,149 @@
+# autoware_localization_util
+
+## Overview
+
+`autoware_localization_util` is a collection of localization utility packages. It contains 5 individual library that used by autoware localization nodes.
+
+- `covariance_ellipse` 2d covariance visualization wrapper.
+- `smart_pose_buffer` pose buffer management library which contains interpolate and data validation.
+- `tree_structured_parzen_estimator` A Tree Structured Parzen Estimator 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.
+- `smart_pose_buffer` A buffer library which implements pose message buffering, pose interpolate and pose validation.
+- `tree_structured_parzen_estimator` A Probability Estimator 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);
+```
+
+### 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 useful 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,46 @@
+// 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>
+
+#include <vector>
+
+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/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_

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

@@ -0,0 +1,71 @@
+// Copyright 2015-2019 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__SMART_POSE_BUFFER_HPP_
+#define AUTOWARE__LOCALIZATION_UTIL__SMART_POSE_BUFFER_HPP_
+
+#include "autoware/localization_util/util_func.hpp"
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
+
+#include <deque>
+
+namespace autoware::localization_util
+{
+class SmartPoseBuffer
+{
+private:
+  using PoseWithCovarianceStamped = geometry_msgs::msg::PoseWithCovarianceStamped;
+
+public:
+  struct InterpolateResult
+  {
+    PoseWithCovarianceStamped old_pose;
+    PoseWithCovarianceStamped new_pose;
+    PoseWithCovarianceStamped interpolated_pose;
+  };
+
+  SmartPoseBuffer() = delete;
+  SmartPoseBuffer(
+    const rclcpp::Logger & logger, const double & pose_timeout_sec,
+    const double & pose_distance_tolerance_meters);
+
+  std::optional<InterpolateResult> interpolate(const rclcpp::Time & target_ros_time);
+
+  void push_back(const PoseWithCovarianceStamped::ConstSharedPtr & pose_msg_ptr);
+
+  void pop_old(const rclcpp::Time & target_ros_time);
+
+  void clear();
+
+private:
+  rclcpp::Logger logger_;
+  std::deque<PoseWithCovarianceStamped::ConstSharedPtr> pose_buffer_;
+  std::mutex mutex_;  // This mutex is for pose_buffer_
+
+  const double pose_timeout_sec_;
+  const double pose_distance_tolerance_meters_;
+
+  [[nodiscard]] bool validate_time_stamp_difference(
+    const rclcpp::Time & target_time, const rclcpp::Time & reference_time,
+    const double time_tolerance_sec) const;
+  [[nodiscard]] bool validate_position_difference(
+    const geometry_msgs::msg::Point & target_point,
+    const geometry_msgs::msg::Point & reference_point, const double distance_tolerance_m_) const;
+};
+}  // namespace autoware::localization_util
+
+#endif  // AUTOWARE__LOCALIZATION_UTIL__SMART_POSE_BUFFER_HPP_