From a35886dfa2be92bf5d538472fc4071af1d77bf44 Mon Sep 17 00:00:00 2001 From: liuXinGangChina Date: Mon, 6 Jan 2025 09:55:15 +0800 Subject: [PATCH] feat: migrate localization_util from universe to core: v0.0 Signed-off-by: liuXinGangChina --- .../autoware_localization_util/CMakeLists.txt | 30 +++ .../autoware_localization_util/README.md | 171 ++++++++++++ .../localization_util/covariance_ellipse.hpp | 44 +++ .../localization_util/diagnostics_module.hpp | 64 +++++ .../localization_util/matrix_type.hpp | 26 ++ .../localization_util/smart_pose_buffer.hpp | 71 +++++ .../tree_structured_parzen_estimator.hpp | 87 ++++++ .../autoware/localization_util/util_func.hpp | 88 ++++++ .../autoware_localization_util/package.xml | 36 +++ .../src/covariance_ellipse.cpp | 90 +++++++ .../src/diagnostics_module.cpp | 108 ++++++++ .../src/smart_pose_buffer.cpp | 158 +++++++++++ .../src/tree_structured_parzen_estimator.cpp | 182 +++++++++++++ .../src/util_func.cpp | 254 ++++++++++++++++++ .../test/test_smart_pose_buffer.cpp | 109 ++++++++ .../test/test_tpe.cpp | 69 +++++ 16 files changed, 1587 insertions(+) create mode 100644 localization/autoware_localization_util/CMakeLists.txt create mode 100644 localization/autoware_localization_util/README.md create mode 100644 localization/autoware_localization_util/include/autoware/localization_util/covariance_ellipse.hpp create mode 100644 localization/autoware_localization_util/include/autoware/localization_util/diagnostics_module.hpp create mode 100644 localization/autoware_localization_util/include/autoware/localization_util/matrix_type.hpp create mode 100644 localization/autoware_localization_util/include/autoware/localization_util/smart_pose_buffer.hpp create mode 100644 localization/autoware_localization_util/include/autoware/localization_util/tree_structured_parzen_estimator.hpp create mode 100644 localization/autoware_localization_util/include/autoware/localization_util/util_func.hpp create mode 100644 localization/autoware_localization_util/package.xml create mode 100644 localization/autoware_localization_util/src/covariance_ellipse.cpp create mode 100644 localization/autoware_localization_util/src/diagnostics_module.cpp create mode 100644 localization/autoware_localization_util/src/smart_pose_buffer.cpp create mode 100644 localization/autoware_localization_util/src/tree_structured_parzen_estimator.cpp create mode 100644 localization/autoware_localization_util/src/util_func.cpp create mode 100644 localization/autoware_localization_util/test/test_smart_pose_buffer.cpp create mode 100644 localization/autoware_localization_util/test/test_tpe.cpp diff --git a/localization/autoware_localization_util/CMakeLists.txt b/localization/autoware_localization_util/CMakeLists.txt new file mode 100644 index 00000000..dd18f3cb --- /dev/null +++ b/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 +) diff --git a/localization/autoware_localization_util/README.md b/localization/autoware_localization_util/README.md new file mode 100644 index 00000000..efd52c1f --- /dev/null +++ b/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 diagnostics_ = + std::make_unique(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(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 initial_pose_buffer_; +initial_pose_buffer_ = std::make_unique( + 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 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 +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); +``` \ No newline at end of file diff --git a/localization/autoware_localization_util/include/autoware/localization_util/covariance_ellipse.hpp b/localization/autoware_localization_util/include/autoware/localization_util/covariance_ellipse.hpp new file mode 100644 index 00000000..abd0af46 --- /dev/null +++ b/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 + +#include +#include + +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_ diff --git a/localization/autoware_localization_util/include/autoware/localization_util/diagnostics_module.hpp b/localization/autoware_localization_util/include/autoware/localization_util/diagnostics_module.hpp new file mode 100644 index 00000000..8c19c941 --- /dev/null +++ b/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 + +#include + +#include +#include + +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 + 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::SharedPtr diagnostics_pub_; + + diagnostic_msgs::msg::DiagnosticStatus diagnostics_status_msg_; +}; + +template +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_ diff --git a/localization/autoware_localization_util/include/autoware/localization_util/matrix_type.hpp b/localization/autoware_localization_util/include/autoware/localization_util/matrix_type.hpp new file mode 100644 index 00000000..bda6ff19 --- /dev/null +++ b/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 + +namespace autoware::localization_util +{ +using Matrix6d = Eigen::Matrix; +using RowMatrixXd = Eigen::Matrix; +} // namespace autoware::localization_util + +#endif // AUTOWARE__LOCALIZATION_UTIL__MATRIX_TYPE_HPP_ diff --git a/localization/autoware_localization_util/include/autoware/localization_util/smart_pose_buffer.hpp b/localization/autoware_localization_util/include/autoware/localization_util/smart_pose_buffer.hpp new file mode 100644 index 00000000..8c10506c --- /dev/null +++ b/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 + +#include + +#include + +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 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 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_ diff --git a/localization/autoware_localization_util/include/autoware/localization_util/tree_structured_parzen_estimator.hpp b/localization/autoware_localization_util/include/autoware/localization_util/tree_structured_parzen_estimator.hpp new file mode 100644 index 00000000..ddf7625c --- /dev/null +++ b/localization/autoware_localization_util/include/autoware/localization_util/tree_structured_parzen_estimator.hpp @@ -0,0 +1,87 @@ +// 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__TREE_STRUCTURED_PARZEN_ESTIMATOR_HPP_ +#define AUTOWARE__LOCALIZATION_UTIL__TREE_STRUCTURED_PARZEN_ESTIMATOR_HPP_ + +/* +A implementation of tree-structured parzen estimator (TPE) +See below pdf for the TPE algorithm detail. +https://papers.nips.cc/paper_files/paper/2011/file/86e8f7ab32cfd12577bc2619bc635690-Paper.pdf + +Optuna is also used as a reference for implementation. +https://github.com/optuna/optuna +*/ + +#include +#include +#include + +namespace autoware::localization_util +{ +class TreeStructuredParzenEstimator +{ +public: + using Input = std::vector; + using Score = double; + struct Trial + { + Input input; + Score score; + }; + + enum Direction { + MINIMIZE = 0, + MAXIMIZE = 1, + }; + + enum Index { + TRANS_X = 0, + TRANS_Y = 1, + TRANS_Z = 2, + ANGLE_X = 3, + ANGLE_Y = 4, + ANGLE_Z = 5, + INDEX_NUM = 6, + }; + + TreeStructuredParzenEstimator() = delete; + TreeStructuredParzenEstimator( + const Direction direction, const int64_t n_startup_trials, std::vector sample_mean, + std::vector sample_stddev); + void add_trial(const Trial & trial); + [[nodiscard]] Input get_next_input() const; + +private: + static constexpr double max_good_rate = 0.10; + static constexpr int64_t n_ei_candidates = 100; + + static std::mt19937_64 engine; + + [[nodiscard]] double compute_log_likelihood_ratio(const Input & input) const; + [[nodiscard]] static double log_gaussian_pdf( + const Input & input, const Input & mu, const Input & sigma); + + std::vector trials_; + int64_t above_num_; + const Direction direction_; + const int64_t n_startup_trials_; + const int64_t input_dimension_; + const std::vector sample_mean_; + const std::vector sample_stddev_; + Input base_stddev_; +}; +} // namespace autoware::localization_util + +#endif // AUTOWARE__LOCALIZATION_UTIL__TREE_STRUCTURED_PARZEN_ESTIMATOR_HPP_ diff --git a/localization/autoware_localization_util/include/autoware/localization_util/util_func.hpp b/localization/autoware_localization_util/include/autoware/localization_util/util_func.hpp new file mode 100644 index 00000000..bef9968f --- /dev/null +++ b/localization/autoware_localization_util/include/autoware/localization_util/util_func.hpp @@ -0,0 +1,88 @@ +// 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__UTIL_FUNC_HPP_ +#define AUTOWARE__LOCALIZATION_UTIL__UTIL_FUNC_HPP_ + +#include +#include +#include +#include + +#ifdef ROS_DISTRO_GALACTIC +#include +#include +#else +#include + +#include +#endif + +#include +#include +#include +#include +#include +#include + +namespace autoware::localization_util +{ +// ref by http://takacity.blog.fc2.com/blog-entry-69.html +std_msgs::msg::ColorRGBA exchange_color_crc(double x); + +double calc_diff_for_radian(const double lhs_rad, const double rhs_rad); + +// x: roll, y: pitch, z: yaw +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 +T transform(const T & input, const geometry_msgs::msg::TransformStamped & transform) +{ + T output; + tf2::doTransform(input, output, transform); + return output; +} + +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); + +} // namespace autoware::localization_util + +#endif // AUTOWARE__LOCALIZATION_UTIL__UTIL_FUNC_HPP_ diff --git a/localization/autoware_localization_util/package.xml b/localization/autoware_localization_util/package.xml new file mode 100644 index 00000000..6b7a3600 --- /dev/null +++ b/localization/autoware_localization_util/package.xml @@ -0,0 +1,36 @@ + + + + autoware_localization_util + 0.39.0 + The autoware_localization_util package + Xingang Liu + Yamato Ando + Masahiro Sakamoto + Shintaro Sakoda + Kento Yabuuchi + NGUYEN Viet Anh + Taiki Yamada + Ryu Yamamoto + Apache License 2.0 + Yamato Ando + + ament_cmake_auto + autoware_cmake + + diagnostic_msgs + geometry_msgs + rclcpp + std_msgs + tf2 + tf2_eigen + tf2_geometry_msgs + visualization_msgs + + ament_cmake_cppcheck + ament_lint_auto + + + ament_cmake + + diff --git a/localization/autoware_localization_util/src/covariance_ellipse.cpp b/localization/autoware_localization_util/src/covariance_ellipse.cpp new file mode 100644 index 00000000..4a7d7190 --- /dev/null +++ b/localization/autoware_localization_util/src/covariance_ellipse.cpp @@ -0,0 +1,90 @@ +// 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. + +#include "autoware/localization_util/covariance_ellipse.hpp" + +#include +#ifdef ROS_DISTRO_GALACTIC +#include +#else +#include +#endif + +namespace autoware::localization_util +{ + +Ellipse calculate_xy_ellipse( + const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, const double scale) +{ + // input geometry_msgs::PoseWithCovariance contain 6x6 matrix + Eigen::Matrix2d xy_covariance; + const auto cov = pose_with_covariance.covariance; + xy_covariance(0, 0) = cov[0 * 6 + 0]; + xy_covariance(0, 1) = cov[0 * 6 + 1]; + xy_covariance(1, 0) = cov[1 * 6 + 0]; + xy_covariance(1, 1) = cov[1 * 6 + 1]; + + Eigen::SelfAdjointEigenSolver eigensolver(xy_covariance); + + Ellipse ellipse; + + // eigen values and vectors are sorted in ascending order + ellipse.long_radius = scale * std::sqrt(eigensolver.eigenvalues()(1)); + ellipse.short_radius = scale * std::sqrt(eigensolver.eigenvalues()(0)); + + // principal component vector + const Eigen::Vector2d pc_vector = eigensolver.eigenvectors().col(1); + ellipse.yaw = std::atan2(pc_vector.y(), pc_vector.x()); + + // ellipse size along lateral direction (body-frame) + ellipse.P = xy_covariance; + const double yaw_vehicle = tf2::getYaw(pose_with_covariance.pose.orientation); + const Eigen::Matrix2d & p_inv = ellipse.P.inverse(); + Eigen::MatrixXd e(2, 1); + e(0, 0) = std::cos(yaw_vehicle); + e(1, 0) = std::sin(yaw_vehicle); + const double d = std::sqrt((e.transpose() * p_inv * e)(0, 0) / p_inv.determinant()); + ellipse.size_lateral_direction = scale * d; + + return ellipse; +} + +visualization_msgs::msg::Marker create_ellipse_marker( + const Ellipse & ellipse, const std_msgs::msg::Header & header, + const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance) +{ + tf2::Quaternion quat; + quat.setEuler(0, 0, ellipse.yaw); + + const double ellipse_long_radius = std::min(ellipse.long_radius, 30.0); + const double ellipse_short_radius = std::min(ellipse.short_radius, 30.0); + visualization_msgs::msg::Marker marker; + marker.header = header; + marker.ns = "error_ellipse"; + marker.id = 0; + marker.type = visualization_msgs::msg::Marker::SPHERE; + marker.action = visualization_msgs::msg::Marker::ADD; + marker.pose = pose_with_covariance.pose; + marker.pose.orientation = tf2::toMsg(quat); + marker.scale.x = ellipse_long_radius * 2; + marker.scale.y = ellipse_short_radius * 2; + marker.scale.z = 0.01; + marker.color.a = 0.1; + marker.color.r = 0.0; + marker.color.g = 0.0; + marker.color.b = 1.0; + return marker; +} + +} // namespace autoware::localization_util diff --git a/localization/autoware_localization_util/src/diagnostics_module.cpp b/localization/autoware_localization_util/src/diagnostics_module.cpp new file mode 100644 index 00000000..2b68dbf4 --- /dev/null +++ b/localization/autoware_localization_util/src/diagnostics_module.cpp @@ -0,0 +1,108 @@ +// 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 "autoware/localization_util/diagnostics_module.hpp" + +#include + +#include + +#include +#include + +namespace autoware::localization_util +{ +DiagnosticsModule::DiagnosticsModule(rclcpp::Node * node, const std::string & diagnostic_name) +: clock_(node->get_clock()) +{ + diagnostics_pub_ = + node->create_publisher("/diagnostics", 10); + + diagnostics_status_msg_.name = + std::string(node->get_name()) + std::string(": ") + diagnostic_name; + diagnostics_status_msg_.hardware_id = node->get_name(); +} + +void DiagnosticsModule::clear() +{ + diagnostics_status_msg_.values.clear(); + diagnostics_status_msg_.values.shrink_to_fit(); + + diagnostics_status_msg_.level = diagnostic_msgs::msg::DiagnosticStatus::OK; + diagnostics_status_msg_.message = ""; +} + +void DiagnosticsModule::add_key_value(const diagnostic_msgs::msg::KeyValue & key_value_msg) +{ + auto it = std::find_if( + std::begin(diagnostics_status_msg_.values), std::end(diagnostics_status_msg_.values), + [key_value_msg](const auto & arg) { return arg.key == key_value_msg.key; }); + + if (it != std::cend(diagnostics_status_msg_.values)) { + it->value = key_value_msg.value; + } else { + diagnostics_status_msg_.values.push_back(key_value_msg); + } +} + +template <> +void DiagnosticsModule::add_key_value(const std::string & key, const std::string & value) +{ + diagnostic_msgs::msg::KeyValue key_value; + key_value.key = key; + key_value.value = value; + add_key_value(key_value); +} + +template <> +void DiagnosticsModule::add_key_value(const std::string & key, const bool & value) +{ + diagnostic_msgs::msg::KeyValue key_value; + key_value.key = key; + key_value.value = value ? "True" : "False"; + add_key_value(key_value); +} + +void DiagnosticsModule::update_level_and_message(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(const rclcpp::Time & publish_time_stamp) +{ + diagnostics_pub_->publish(create_diagnostics_array(publish_time_stamp)); +} + +diagnostic_msgs::msg::DiagnosticArray DiagnosticsModule::create_diagnostics_array( + const rclcpp::Time & publish_time_stamp) const +{ + diagnostic_msgs::msg::DiagnosticArray diagnostics_msg; + diagnostics_msg.header.stamp = publish_time_stamp; + 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; +} +} // namespace autoware::localization_util diff --git a/localization/autoware_localization_util/src/smart_pose_buffer.cpp b/localization/autoware_localization_util/src/smart_pose_buffer.cpp new file mode 100644 index 00000000..3b529d68 --- /dev/null +++ b/localization/autoware_localization_util/src/smart_pose_buffer.cpp @@ -0,0 +1,158 @@ +// 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 "autoware/localization_util/smart_pose_buffer.hpp" + +namespace autoware::localization_util +{ +SmartPoseBuffer::SmartPoseBuffer( + const rclcpp::Logger & logger, const double & pose_timeout_sec, + const double & pose_distance_tolerance_meters) +: logger_(logger), + pose_timeout_sec_(pose_timeout_sec), + pose_distance_tolerance_meters_(pose_distance_tolerance_meters) +{ +} + +std::optional SmartPoseBuffer::interpolate( + const rclcpp::Time & target_ros_time) +{ + InterpolateResult result; + + { + std::lock_guard lock(mutex_); + + if (pose_buffer_.size() < 2) { + RCLCPP_INFO(logger_, "pose_buffer_.size() < 2"); + return std::nullopt; + } + + const rclcpp::Time time_first = pose_buffer_.front()->header.stamp; + const rclcpp::Time time_last = pose_buffer_.back()->header.stamp; + + if (target_ros_time < time_first) { + RCLCPP_INFO(logger_, "Mismatch between pose timestamp and current timestamp"); + return std::nullopt; + } + + // [time_last < target_ros_time] is acceptable here. + // It is possible that the target_ros_time (often sensor timestamp) is newer than the latest + // timestamp of buffered pose (often EKF). + // However, if the timestamp difference is too large, + // it will later be rejected by validate_time_stamp_difference. + + // get the nearest poses + result.old_pose = *pose_buffer_.front(); + for (const PoseWithCovarianceStamped::ConstSharedPtr & pose_cov_msg_ptr : pose_buffer_) { + result.new_pose = *pose_cov_msg_ptr; + const rclcpp::Time pose_time_stamp = result.new_pose.header.stamp; + if (pose_time_stamp > target_ros_time) { + break; + } + result.old_pose = *pose_cov_msg_ptr; + } + } + + // check the time stamp + const bool is_old_pose_valid = validate_time_stamp_difference( + result.old_pose.header.stamp, target_ros_time, pose_timeout_sec_); + const bool is_new_pose_valid = validate_time_stamp_difference( + result.new_pose.header.stamp, target_ros_time, pose_timeout_sec_); + + // check the position jumping (ex. immediately after the initial pose estimation) + const bool is_pose_diff_valid = validate_position_difference( + result.old_pose.pose.pose.position, result.new_pose.pose.pose.position, + pose_distance_tolerance_meters_); + + // all validations must be true + if (!(is_old_pose_valid && is_new_pose_valid && is_pose_diff_valid)) { + return std::nullopt; + } + + // interpolate the pose + const geometry_msgs::msg::PoseStamped interpolated_pose_msg = + interpolate_pose(result.old_pose, result.new_pose, target_ros_time); + result.interpolated_pose.header = interpolated_pose_msg.header; + result.interpolated_pose.pose.pose = interpolated_pose_msg.pose; + // This does not interpolate the covariance. + // interpolated_pose.pose.covariance is always set to old_pose.covariance + result.interpolated_pose.pose.covariance = result.old_pose.pose.covariance; + return result; +} + +void SmartPoseBuffer::push_back(const PoseWithCovarianceStamped::ConstSharedPtr & pose_msg_ptr) +{ + std::lock_guard lock(mutex_); + if (!pose_buffer_.empty()) { + // Check for non-chronological timestamp order + // This situation can arise when replaying a rosbag multiple times + const rclcpp::Time end_time = pose_buffer_.back()->header.stamp; + const rclcpp::Time msg_time = pose_msg_ptr->header.stamp; + if (msg_time < end_time) { + // Clear the buffer if timestamps are reversed to maintain chronological order + pose_buffer_.clear(); + } + } + pose_buffer_.push_back(pose_msg_ptr); +} + +void SmartPoseBuffer::pop_old(const rclcpp::Time & target_ros_time) +{ + std::lock_guard lock(mutex_); + while (!pose_buffer_.empty()) { + if (rclcpp::Time(pose_buffer_.front()->header.stamp) >= target_ros_time) { + break; + } + pose_buffer_.pop_front(); + } +} + +void SmartPoseBuffer::clear() +{ + std::lock_guard lock(mutex_); + pose_buffer_.clear(); +} + +bool SmartPoseBuffer::validate_time_stamp_difference( + const rclcpp::Time & target_time, const rclcpp::Time & reference_time, + const double time_tolerance_sec) const +{ + const double dt = std::abs((target_time - reference_time).seconds()); + const bool success = dt < time_tolerance_sec; + if (!success) { + RCLCPP_WARN( + logger_, + "Validation error. The reference time is %lf[sec], but the target time is %lf[sec]. The " + "difference is %lf[sec] (the tolerance is %lf[sec]).", + reference_time.seconds(), target_time.seconds(), dt, time_tolerance_sec); + } + return success; +} + +bool SmartPoseBuffer::validate_position_difference( + const geometry_msgs::msg::Point & target_point, const geometry_msgs::msg::Point & reference_point, + const double distance_tolerance_m_) const +{ + const double distance = norm(target_point, reference_point); + const bool success = distance < distance_tolerance_m_; + if (!success) { + RCLCPP_WARN( + logger_, + "Validation error. The distance from reference position to target position is %lf[m] (the " + "tolerance is %lf[m]).", + distance, distance_tolerance_m_); + } + return success; +} +} // namespace autoware::localization_util diff --git a/localization/autoware_localization_util/src/tree_structured_parzen_estimator.cpp b/localization/autoware_localization_util/src/tree_structured_parzen_estimator.cpp new file mode 100644 index 00000000..064b33eb --- /dev/null +++ b/localization/autoware_localization_util/src/tree_structured_parzen_estimator.cpp @@ -0,0 +1,182 @@ +// 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 "autoware/localization_util/tree_structured_parzen_estimator.hpp" + +#include +#include +#include +#include + +namespace autoware::localization_util +{ +// random number generator +std::mt19937_64 TreeStructuredParzenEstimator::engine(0); + +TreeStructuredParzenEstimator::TreeStructuredParzenEstimator( + const Direction direction, const int64_t n_startup_trials, std::vector sample_mean, + std::vector sample_stddev) +: above_num_(0), + direction_(direction), + n_startup_trials_(n_startup_trials), + input_dimension_(INDEX_NUM), + sample_mean_(std::move(sample_mean)), + sample_stddev_(std::move(sample_stddev)) +{ + if (sample_mean_.size() != ANGLE_Z) { + std::cerr << "sample_mean size is invalid" << std::endl; + throw std::runtime_error("sample_mean size is invalid"); + } + if (sample_stddev_.size() != ANGLE_Z) { + std::cerr << "sample_stddev size is invalid" << std::endl; + throw std::runtime_error("sample_stddev size is invalid"); + } + // base_stddev_ is defined as the stable convergence range of ndt_scan_matcher. + base_stddev_.resize(input_dimension_); + base_stddev_[TRANS_X] = 0.25; // [m] + base_stddev_[TRANS_Y] = 0.25; // [m] + base_stddev_[TRANS_Z] = 0.25; // [m] + base_stddev_[ANGLE_X] = 1.0 / 180.0 * M_PI; // [rad] + base_stddev_[ANGLE_Y] = 1.0 / 180.0 * M_PI; // [rad] + base_stddev_[ANGLE_Z] = 2.5 / 180.0 * M_PI; // [rad] +} + +void TreeStructuredParzenEstimator::add_trial(const Trial & trial) +{ + trials_.push_back(trial); + std::sort(trials_.begin(), trials_.end(), [this](const Trial & lhs, const Trial & rhs) { + return (direction_ == Direction::MAXIMIZE ? lhs.score > rhs.score : lhs.score < rhs.score); + }); + above_num_ = std::min( + {static_cast(10), + static_cast(static_cast(trials_.size()) * max_good_rate)}); +} + +TreeStructuredParzenEstimator::Input TreeStructuredParzenEstimator::get_next_input() const +{ + std::normal_distribution dist_normal_trans_x( + sample_mean_[TRANS_X], sample_stddev_[TRANS_X]); + std::normal_distribution dist_normal_trans_y( + sample_mean_[TRANS_Y], sample_stddev_[TRANS_Y]); + std::normal_distribution dist_normal_trans_z( + sample_mean_[TRANS_Z], sample_stddev_[TRANS_Z]); + std::normal_distribution dist_normal_angle_x( + sample_mean_[ANGLE_X], sample_stddev_[ANGLE_X]); + std::normal_distribution dist_normal_angle_y( + sample_mean_[ANGLE_Y], sample_stddev_[ANGLE_Y]); + std::uniform_real_distribution dist_uniform_angle_z(-M_PI, M_PI); + + if (static_cast(trials_.size()) < n_startup_trials_ || above_num_ == 0) { + // Random sampling based on prior until the number of trials reaches `n_startup_trials_`. + Input input(input_dimension_); + input[TRANS_X] = dist_normal_trans_x(engine); + input[TRANS_Y] = dist_normal_trans_y(engine); + input[TRANS_Z] = dist_normal_trans_z(engine); + input[ANGLE_X] = dist_normal_angle_x(engine); + input[ANGLE_Y] = dist_normal_angle_y(engine); + input[ANGLE_Z] = dist_uniform_angle_z(engine); + return input; + } + + Input best_input; + double best_log_likelihood_ratio = std::numeric_limits::lowest(); + for (int64_t i = 0; i < n_ei_candidates; i++) { + Input input(input_dimension_); + input[TRANS_X] = dist_normal_trans_x(engine); + input[TRANS_Y] = dist_normal_trans_y(engine); + input[TRANS_Z] = dist_normal_trans_z(engine); + input[ANGLE_X] = dist_normal_angle_x(engine); + input[ANGLE_Y] = dist_normal_angle_y(engine); + input[ANGLE_Z] = dist_uniform_angle_z(engine); + const double log_likelihood_ratio = compute_log_likelihood_ratio(input); + if (log_likelihood_ratio > best_log_likelihood_ratio) { + best_log_likelihood_ratio = log_likelihood_ratio; + best_input = input; + } + } + return best_input; +} + +double TreeStructuredParzenEstimator::compute_log_likelihood_ratio(const Input & input) const +{ + const auto n = static_cast(trials_.size()); + + // The above KDE and the below KDE are calculated respectively, and the ratio is the criteria to + // select best sample. + std::vector above_logs; + std::vector below_logs; + + for (int64_t i = 0; i < n; i++) { + const double log_p = log_gaussian_pdf(input, trials_[i].input, base_stddev_); + if (i < above_num_) { + const double w = 1.0 / static_cast(above_num_); + const double log_w = std::log(w); + above_logs.push_back(log_p + log_w); + } else { + const double w = 1.0 / static_cast(n - above_num_); + const double log_w = std::log(w); + below_logs.push_back(log_p + log_w); + } + } + + auto log_sum_exp = [](const std::vector & log_vec) { + const double max = *std::max_element(log_vec.begin(), log_vec.end()); + double sum = std::accumulate( + log_vec.begin(), log_vec.end(), 0.0, + [max](double total, double log_v) { return total + std::exp(log_v - max); }); + return max + std::log(sum); + }; + + const double above = log_sum_exp(above_logs); + const double below = log_sum_exp(below_logs); + + // Multiply by a constant so that the score near the "below sample" becomes lower. + // cspell:disable-line TODO(Shintaro Sakoda): It's theoretically incorrect, consider it again + // later. + const double r = above - below * 5.0; + return r; +} + +double TreeStructuredParzenEstimator::log_gaussian_pdf( + const Input & input, const Input & mu, const Input & sigma) +{ + const double log_2pi = std::log(2.0 * M_PI); + auto log_gaussian_pdf_1d = [&](const double diff, const double sigma) { + return -0.5 * log_2pi - std::log(sigma) - (diff * diff) / (2.0 * sigma * sigma); + }; + + const auto n = static_cast(input.size()); + + double result = 0.0; + for (int64_t i = 0; i < n; i++) { + double diff = input[i] - mu[i]; + if (i == ANGLE_Z) { + // Normalize the loop variable to [-pi, pi) + while (diff >= M_PI) { + diff -= 2 * M_PI; + } + while (diff < -M_PI) { + diff += 2 * M_PI; + } + } + // Experimentally, it is better to consider only trans_xy and yaw, so ignore trans_z, angle_x, + // angle_y. + if (i == TRANS_Z || i == ANGLE_X || i == ANGLE_Y) { + continue; + } + result += log_gaussian_pdf_1d(diff, sigma[i]); + } + return result; +} +} // namespace autoware::localization_util diff --git a/localization/autoware_localization_util/src/util_func.cpp b/localization/autoware_localization_util/src/util_func.cpp new file mode 100644 index 00000000..6b019f77 --- /dev/null +++ b/localization/autoware_localization_util/src/util_func.cpp @@ -0,0 +1,254 @@ +// 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. + +#include "autoware/localization_util/util_func.hpp" + +#include "autoware/localization_util/matrix_type.hpp" + +namespace autoware::localization_util +{ +// ref by http://takacity.blog.fc2.com/blog-entry-69.html +std_msgs::msg::ColorRGBA exchange_color_crc(double x) +{ + std_msgs::msg::ColorRGBA color; + + x = std::max(x, 0.0); + x = std::min(x, 0.9999); + + if (x <= 0.25) { + color.b = 1.0; + color.g = static_cast(std::sin(x * 2.0 * M_PI)); + color.r = 0; + } else if (x <= 0.5) { + color.b = static_cast(std::sin(x * 2 * M_PI)); + color.g = 1.0; + color.r = 0; + } else if (x <= 0.75) { + color.b = 0; + color.g = 1.0; + color.r = static_cast(-std::sin(x * 2.0 * M_PI)); + } else { + color.b = 0; + color.g = static_cast(-std::sin(x * 2.0 * M_PI)); + color.r = 1.0; + } + color.a = 0.999; + return color; +} + +double calc_diff_for_radian(const double lhs_rad, const double rhs_rad) +{ + double diff_rad = lhs_rad - rhs_rad; + if (diff_rad > M_PI) { + diff_rad = diff_rad - 2 * M_PI; + } else if (diff_rad < -M_PI) { + diff_rad = diff_rad + 2 * M_PI; + } + return diff_rad; +} + +Eigen::Map make_eigen_covariance(const std::array & covariance) +{ + return {covariance.data(), 6, 6}; +} + +// x: roll, y: pitch, z: yaw +geometry_msgs::msg::Vector3 get_rpy(const geometry_msgs::msg::Pose & pose) +{ + geometry_msgs::msg::Vector3 rpy; + tf2::Quaternion q(pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w); + tf2::Matrix3x3(q).getRPY(rpy.x, rpy.y, rpy.z); + return rpy; +} + +geometry_msgs::msg::Vector3 get_rpy(const geometry_msgs::msg::PoseStamped & pose) +{ + return get_rpy(pose.pose); +} + +geometry_msgs::msg::Vector3 get_rpy(const geometry_msgs::msg::PoseWithCovarianceStamped & pose) +{ + return get_rpy(pose.pose.pose); +} + +geometry_msgs::msg::Quaternion rpy_rad_to_quaternion( + const double r_rad, const double p_rad, const double y_rad) +{ + tf2::Quaternion q; + q.setRPY(r_rad, p_rad, y_rad); + geometry_msgs::msg::Quaternion quaternion_msg; + quaternion_msg.x = q.x(); + quaternion_msg.y = q.y(); + quaternion_msg.z = q.z(); + quaternion_msg.w = q.w(); + return quaternion_msg; +} + +geometry_msgs::msg::Quaternion rpy_deg_to_quaternion( + const double r_deg, const double p_deg, const double y_deg) +{ + const double r_rad = r_deg * M_PI / 180.0; + const double p_rad = p_deg * M_PI / 180.0; + const double y_rad = y_deg * M_PI / 180.0; + return rpy_rad_to_quaternion(r_rad, p_rad, y_rad); +} + +geometry_msgs::msg::Twist calc_twist( + const geometry_msgs::msg::PoseStamped & pose_a, const geometry_msgs::msg::PoseStamped & pose_b) +{ + const rclcpp::Duration dt = rclcpp::Time(pose_b.header.stamp) - rclcpp::Time(pose_a.header.stamp); + const double dt_s = dt.seconds(); + + if (dt_s == 0) { + return geometry_msgs::msg::Twist(); + } + + const auto pose_a_rpy = get_rpy(pose_a); + const auto pose_b_rpy = get_rpy(pose_b); + + geometry_msgs::msg::Vector3 diff_xyz; + geometry_msgs::msg::Vector3 diff_rpy; + + diff_xyz.x = pose_b.pose.position.x - pose_a.pose.position.x; + diff_xyz.y = pose_b.pose.position.y - pose_a.pose.position.y; + diff_xyz.z = pose_b.pose.position.z - pose_a.pose.position.z; + diff_rpy.x = calc_diff_for_radian(pose_b_rpy.x, pose_a_rpy.x); + diff_rpy.y = calc_diff_for_radian(pose_b_rpy.y, pose_a_rpy.y); + diff_rpy.z = calc_diff_for_radian(pose_b_rpy.z, pose_a_rpy.z); + + geometry_msgs::msg::Twist twist; + twist.linear.x = diff_xyz.x / dt_s; + twist.linear.y = diff_xyz.y / dt_s; + twist.linear.z = diff_xyz.z / dt_s; + twist.angular.x = diff_rpy.x / dt_s; + twist.angular.y = diff_rpy.y / dt_s; + twist.angular.z = diff_rpy.z / dt_s; + + return twist; +} + +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) +{ + const rclcpp::Time pose_a_time_stamp = pose_a.header.stamp; + const rclcpp::Time pose_b_time_stamp = pose_b.header.stamp; + if ( + (pose_a_time_stamp.seconds() == 0.0) || (pose_b_time_stamp.seconds() == 0.0) || + (time_stamp.seconds() == 0.0)) { + return geometry_msgs::msg::PoseStamped(); + } + + const auto twist = calc_twist(pose_a, pose_b); + const double dt = (time_stamp - pose_a_time_stamp).seconds(); + + const auto pose_a_rpy = get_rpy(pose_a); + + geometry_msgs::msg::Vector3 xyz; + geometry_msgs::msg::Vector3 rpy; + xyz.x = pose_a.pose.position.x + twist.linear.x * dt; + xyz.y = pose_a.pose.position.y + twist.linear.y * dt; + xyz.z = pose_a.pose.position.z + twist.linear.z * dt; + rpy.x = pose_a_rpy.x + twist.angular.x * dt; + rpy.y = pose_a_rpy.y + twist.angular.y * dt; + rpy.z = pose_a_rpy.z + twist.angular.z * dt; + + tf2::Quaternion tf_quaternion; + tf_quaternion.setRPY(rpy.x, rpy.y, rpy.z); + + geometry_msgs::msg::PoseStamped pose; + pose.header = pose_a.header; + pose.header.stamp = time_stamp; + pose.pose.position.x = xyz.x; + pose.pose.position.y = xyz.y; + pose.pose.position.z = xyz.z; + pose.pose.orientation = tf2::toMsg(tf_quaternion); + return pose; +} + +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) +{ + geometry_msgs::msg::PoseStamped tmp_pose_a; + tmp_pose_a.header = pose_a.header; + tmp_pose_a.pose = pose_a.pose.pose; + + geometry_msgs::msg::PoseStamped tmp_pose_b; + tmp_pose_b.header = pose_b.header; + tmp_pose_b.pose = pose_b.pose.pose; + + return interpolate_pose(tmp_pose_a, tmp_pose_b, time_stamp); +} + +Eigen::Affine3d pose_to_affine3d(const geometry_msgs::msg::Pose & ros_pose) +{ + Eigen::Affine3d eigen_pose; + tf2::fromMsg(ros_pose, eigen_pose); + return eigen_pose; +} + +Eigen::Matrix4f pose_to_matrix4f(const geometry_msgs::msg::Pose & ros_pose) +{ + Eigen::Affine3d eigen_pose_affine = pose_to_affine3d(ros_pose); + Eigen::Matrix4f eigen_pose_matrix = eigen_pose_affine.matrix().cast(); + return eigen_pose_matrix; +} + +Eigen::Vector3d point_to_vector3d(const geometry_msgs::msg::Point & ros_pos) +{ + Eigen::Vector3d eigen_pos; + eigen_pos.x() = ros_pos.x; + eigen_pos.y() = ros_pos.y; + eigen_pos.z() = ros_pos.z; + return eigen_pos; +} + +geometry_msgs::msg::Pose matrix4f_to_pose(const Eigen::Matrix4f & eigen_pose_matrix) +{ + Eigen::Affine3d eigen_pose_affine; + eigen_pose_affine.matrix() = eigen_pose_matrix.cast(); + geometry_msgs::msg::Pose ros_pose = tf2::toMsg(eigen_pose_affine); + return ros_pose; +} + +double norm(const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2) +{ + const double dx = p1.x - p2.x; + const double dy = p1.y - p2.y; + const double dz = p1.z - p2.z; + return std::sqrt(dx * dx + dy * dy + dz * dz); +} + +void output_pose_with_cov_to_log( + const rclcpp::Logger & logger, const std::string & prefix, + const geometry_msgs::msg::PoseWithCovarianceStamped & pose_with_cov) +{ + const Eigen::Map covariance = + make_eigen_covariance(pose_with_cov.pose.covariance); + const geometry_msgs::msg::Pose pose = pose_with_cov.pose.pose; + geometry_msgs::msg::Vector3 rpy = get_rpy(pose); + rpy.x = rpy.x * 180.0 / M_PI; + rpy.y = rpy.y * 180.0 / M_PI; + rpy.z = rpy.z * 180.0 / M_PI; + + RCLCPP_DEBUG_STREAM( + logger, std::fixed << prefix << "," << pose.position.x << "," << pose.position.y << "," + << pose.position.z << "," << pose.orientation.x << "," << pose.orientation.y + << "," << pose.orientation.z << "," << pose.orientation.w << "," << rpy.x + << "," << rpy.y << "," << rpy.z << "," << covariance(0, 0) << "," + << covariance(1, 1) << "," << covariance(2, 2) << "," << covariance(3, 3) + << "," << covariance(4, 4) << "," << covariance(5, 5)); +} +} // namespace autoware::localization_util diff --git a/localization/autoware_localization_util/test/test_smart_pose_buffer.cpp b/localization/autoware_localization_util/test/test_smart_pose_buffer.cpp new file mode 100644 index 00000000..d5555568 --- /dev/null +++ b/localization/autoware_localization_util/test/test_smart_pose_buffer.cpp @@ -0,0 +1,109 @@ +// 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 "autoware/localization_util/smart_pose_buffer.hpp" +#include "autoware/localization_util/util_func.hpp" + +#include +#include + +#include +#include + +#include +#include +#include + +using PoseWithCovarianceStamped = geometry_msgs::msg::PoseWithCovarianceStamped; +using SmartPoseBuffer = autoware::localization_util::SmartPoseBuffer; + +bool compare_pose( + const PoseWithCovarianceStamped & pose_a, const PoseWithCovarianceStamped & pose_b) +{ + return pose_a.header.stamp == pose_b.header.stamp && + pose_a.header.frame_id == pose_b.header.frame_id && + pose_a.pose.covariance == pose_b.pose.covariance && + pose_a.pose.pose.position.x == pose_b.pose.pose.position.x && + pose_a.pose.pose.position.y == pose_b.pose.pose.position.y && + pose_a.pose.pose.position.z == pose_b.pose.pose.position.z && + pose_a.pose.pose.orientation.x == pose_b.pose.pose.orientation.x && + pose_a.pose.pose.orientation.y == pose_b.pose.pose.orientation.y && + pose_a.pose.pose.orientation.z == pose_b.pose.pose.orientation.z && + pose_a.pose.pose.orientation.w == pose_b.pose.pose.orientation.w; +} + +TEST(TestSmartPoseBuffer, interpolate_pose) // NOLINT +{ + rclcpp::Logger logger = rclcpp::get_logger("test_logger"); + const double pose_timeout_sec = 10.0; + const double pose_distance_tolerance_meters = 100.0; + SmartPoseBuffer smart_pose_buffer(logger, pose_timeout_sec, pose_distance_tolerance_meters); + + // first data + PoseWithCovarianceStamped::SharedPtr old_pose_ptr = std::make_shared(); + old_pose_ptr->header.stamp.sec = 10; + old_pose_ptr->header.stamp.nanosec = 0; + old_pose_ptr->pose.pose.position.x = 10.0; + old_pose_ptr->pose.pose.position.y = 20.0; + old_pose_ptr->pose.pose.position.z = 0.0; + old_pose_ptr->pose.pose.orientation = + autoware::localization_util::rpy_deg_to_quaternion(0.0, 0.0, 0.0); + smart_pose_buffer.push_back(old_pose_ptr); + + // second data + PoseWithCovarianceStamped::SharedPtr new_pose_ptr = std::make_shared(); + new_pose_ptr->header.stamp.sec = 20; + new_pose_ptr->header.stamp.nanosec = 0; + new_pose_ptr->pose.pose.position.x = 20.0; + new_pose_ptr->pose.pose.position.y = 40.0; + new_pose_ptr->pose.pose.position.z = 0.0; + new_pose_ptr->pose.pose.orientation = + autoware::localization_util::rpy_deg_to_quaternion(0.0, 0.0, 90.0); + smart_pose_buffer.push_back(new_pose_ptr); + + // interpolate + builtin_interfaces::msg::Time target_ros_time_msg; + target_ros_time_msg.sec = 15; + target_ros_time_msg.nanosec = 0; + const std::optional & interpolate_result = + smart_pose_buffer.interpolate(target_ros_time_msg); + ASSERT_TRUE(interpolate_result.has_value()); + const SmartPoseBuffer::InterpolateResult result = interpolate_result.value(); + + // check old + EXPECT_TRUE(compare_pose(result.old_pose, *old_pose_ptr)); + + // check new + EXPECT_TRUE(compare_pose(result.new_pose, *new_pose_ptr)); + + // check interpolated + EXPECT_EQ(result.interpolated_pose.header.stamp.sec, 15); + EXPECT_EQ(result.interpolated_pose.header.stamp.nanosec, static_cast(0)); + EXPECT_EQ(result.interpolated_pose.pose.pose.position.x, 15.0); + EXPECT_EQ(result.interpolated_pose.pose.pose.position.y, 30.0); + EXPECT_EQ(result.interpolated_pose.pose.pose.position.z, 0.0); + const auto rpy = autoware::localization_util::get_rpy(result.interpolated_pose.pose.pose); + EXPECT_NEAR(rpy.x * 180 / M_PI, 0.0, 1e-6); + EXPECT_NEAR(rpy.y * 180 / M_PI, 0.0, 1e-6); + EXPECT_NEAR(rpy.z * 180 / M_PI, 45.0, 1e-6); +} + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + ::testing::InitGoogleTest(&argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} diff --git a/localization/autoware_localization_util/test/test_tpe.cpp b/localization/autoware_localization_util/test/test_tpe.cpp new file mode 100644 index 00000000..6cbe3c2a --- /dev/null +++ b/localization/autoware_localization_util/test/test_tpe.cpp @@ -0,0 +1,69 @@ +// Copyright 2023 The Autoware Contributors +// +// 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 "autoware/localization_util/tree_structured_parzen_estimator.hpp" + +#include + +using TreeStructuredParzenEstimator = autoware::localization_util::TreeStructuredParzenEstimator; + +TEST(TreeStructuredParzenEstimatorTest, TPE_is_better_than_random_search_on_sphere_function) +{ + auto sphere_function = [](const TreeStructuredParzenEstimator::Input & input) { + double value = 0.0; + const auto n = static_cast(input.size()); + for (int64_t i = 0; i < n; i++) { + const double v = input[i] * 10; + value += v * v; + } + return value; + }; + + constexpr int64_t k_outer_trials_num = 20; + constexpr int64_t k_inner_trials_num = 200; + std::cout << std::fixed; + std::vector mean_scores; + std::vector sample_mean(5, 0.0); + std::vector sample_stddev{1.0, 1.0, 0.1, 0.1, 0.1}; + + for (const int64_t n_startup_trials : {k_inner_trials_num, k_inner_trials_num / 2}) { + const std::string method = ((n_startup_trials == k_inner_trials_num) ? "Random" : "TPE"); + + std::vector scores; + for (int64_t i = 0; i < k_outer_trials_num; i++) { + double best_score = std::numeric_limits::lowest(); + TreeStructuredParzenEstimator estimator( + TreeStructuredParzenEstimator::Direction::MAXIMIZE, n_startup_trials, sample_mean, + sample_stddev); + for (int64_t trial = 0; trial < k_inner_trials_num; trial++) { + const TreeStructuredParzenEstimator::Input input = estimator.get_next_input(); + const double score = -sphere_function(input); + estimator.add_trial({input, score}); + best_score = std::max(best_score, score); + } + scores.push_back(best_score); + } + + const double sum = std::accumulate(scores.begin(), scores.end(), 0.0); + const double mean = sum / static_cast(scores.size()); + mean_scores.push_back(mean); + double sq_sum = std::accumulate( + scores.begin(), scores.end(), 0.0, + [mean](double total, double score) { return total + (score - mean) * (score - mean); }); + const double stddev = std::sqrt(sq_sum / static_cast(scores.size())); + + std::cout << method << ", mean = " << mean << ", stddev = " << stddev << std::endl; + } + ASSERT_LT(mean_scores[0], mean_scores[1]); +}