From c12f241f8e0c6af16a3cbad82daee7c99a2e6250 Mon Sep 17 00:00:00 2001 From: liuXinGangChina Date: Mon, 6 Jan 2025 11:21:06 +0800 Subject: [PATCH] fix: rebase due to pr-9714 under universe repo, remove diagnostics_module from localization_util : v0.1 Signed-off-by: liuXinGangChina --- .../autoware_localization_util/CMakeLists.txt | 1 - .../autoware_localization_util/README.md | 34 ------ .../localization_util/diagnostics_module.hpp | 64 ----------- .../src/diagnostics_module.cpp | 108 ------------------ 4 files changed, 207 deletions(-) delete mode 100644 localization/autoware_localization_util/include/autoware/localization_util/diagnostics_module.hpp delete mode 100644 localization/autoware_localization_util/src/diagnostics_module.cpp diff --git a/localization/autoware_localization_util/CMakeLists.txt b/localization/autoware_localization_util/CMakeLists.txt index dd18f3cb..de626331 100644 --- a/localization/autoware_localization_util/CMakeLists.txt +++ b/localization/autoware_localization_util/CMakeLists.txt @@ -6,7 +6,6 @@ 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 diff --git a/localization/autoware_localization_util/README.md b/localization/autoware_localization_util/README.md index efd52c1f..c48da4fe 100644 --- a/localization/autoware_localization_util/README.md +++ b/localization/autoware_localization_util/README.md @@ -4,14 +4,12 @@ `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. @@ -32,38 +30,6 @@ autoware::localization_util::Ellipse ellipse_ = autoware::localization_util::cal 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 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 deleted file mode 100644 index 8c19c941..00000000 --- a/localization/autoware_localization_util/include/autoware/localization_util/diagnostics_module.hpp +++ /dev/null @@ -1,64 +0,0 @@ -// 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/src/diagnostics_module.cpp b/localization/autoware_localization_util/src/diagnostics_module.cpp deleted file mode 100644 index 2b68dbf4..00000000 --- a/localization/autoware_localization_util/src/diagnostics_module.cpp +++ /dev/null @@ -1,108 +0,0 @@ -// 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