From 16d5cb1888d17ec750bb50f08f02bbd71d985c9c Mon Sep 17 00:00:00 2001 From: kminoda <44218668+kminoda@users.noreply.github.com> Date: Wed, 25 Dec 2024 12:10:15 +0900 Subject: [PATCH] feat!: move diagnostics_module from localization_util to unverse_utils (#9714) * feat!: move diagnostics_module from localization_util to unverse_utils Signed-off-by: kminoda * remove diagnostics module from localization_util Signed-off-by: kminoda * style(pre-commit): autofix * minor fix in pose_initializer Signed-off-by: kminoda * add test Signed-off-by: kminoda * style(pre-commit): autofix * remove unnecessary declaration Signed-off-by: kminoda * module -> interface Signed-off-by: kminoda * remove unnecessary equal expression Signed-off-by: kminoda * revert the remove of template function Signed-off-by: kminoda * style(pre-commit): autofix * use overload instead Signed-off-by: kminoda * include what you use -- test_diagnostics_interface.cpp Signed-off-by: kminoda --------- Signed-off-by: kminoda Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- common/autoware_universe_utils/CMakeLists.txt | 1 + .../ros/diagnostics_interface.hpp | 23 +-- .../src/ros/diagnostics_interface.cpp | 24 ++- .../src/ros/test_diagnostics_interface.cpp | 179 ++++++++++++++++++ .../src/gyro_odometer_core.cpp | 2 +- .../src/gyro_odometer_core.hpp | 4 +- .../src/lidar_marker_localizer.cpp | 42 ++-- .../src/lidar_marker_localizer.hpp | 4 +- .../CHANGELOG.rst | 4 +- .../src/localization_error_monitor.cpp | 2 +- .../src/localization_error_monitor.hpp | 4 +- .../autoware_localization_util/CMakeLists.txt | 1 - .../ndt_scan_matcher/map_update_module.hpp | 12 +- .../ndt_scan_matcher_core.hpp | 14 +- .../src/map_update_module.cpp | 10 +- .../src/ndt_scan_matcher_core.cpp | 14 +- .../autoware_pose_initializer/package.xml | 1 - .../src/pose_initializer_core.cpp | 2 +- .../src/pose_initializer_core.hpp | 5 +- 19 files changed, 261 insertions(+), 87 deletions(-) rename localization/autoware_localization_util/include/autoware/localization_util/diagnostics_module.hpp => common/autoware_universe_utils/include/autoware/universe_utils/ros/diagnostics_interface.hpp (70%) rename localization/autoware_localization_util/src/diagnostics_module.cpp => common/autoware_universe_utils/src/ros/diagnostics_interface.cpp (76%) create mode 100644 common/autoware_universe_utils/test/src/ros/test_diagnostics_interface.cpp diff --git a/common/autoware_universe_utils/CMakeLists.txt b/common/autoware_universe_utils/CMakeLists.txt index f295662e69091..72486b5de8818 100644 --- a/common/autoware_universe_utils/CMakeLists.txt +++ b/common/autoware_universe_utils/CMakeLists.txt @@ -21,6 +21,7 @@ ament_auto_add_library(autoware_universe_utils SHARED src/geometry/sat_2d.cpp src/math/sin_table.cpp src/math/trigonometry.cpp + src/ros/diagnostics_interface.cpp src/ros/msg_operation.cpp src/ros/marker_helper.cpp src/ros/logger_level_configure.cpp diff --git a/localization/autoware_localization_util/include/autoware/localization_util/diagnostics_module.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/ros/diagnostics_interface.hpp similarity index 70% rename from localization/autoware_localization_util/include/autoware/localization_util/diagnostics_module.hpp rename to common/autoware_universe_utils/include/autoware/universe_utils/ros/diagnostics_interface.hpp index 8c19c94127630..120aed7c7548e 100644 --- a/localization/autoware_localization_util/include/autoware/localization_util/diagnostics_module.hpp +++ b/common/autoware_universe_utils/include/autoware/universe_utils/ros/diagnostics_interface.hpp @@ -12,8 +12,8 @@ // 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_ +#ifndef AUTOWARE__UNIVERSE_UTILS__ROS__DIAGNOSTICS_INTERFACE_HPP_ +#define AUTOWARE__UNIVERSE_UTILS__ROS__DIAGNOSTICS_INTERFACE_HPP_ #include @@ -22,16 +22,18 @@ #include #include -namespace autoware::localization_util +namespace autoware::universe_utils { -class DiagnosticsModule +class DiagnosticInterface { public: - DiagnosticsModule(rclcpp::Node * node, const std::string & diagnostic_name); + DiagnosticInterface(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 add_key_value(const std::string & key, const std::string & value); + void add_key_value(const std::string & key, bool value); void update_level_and_message(const int8_t level, const std::string & message); void publish(const rclcpp::Time & publish_time_stamp); @@ -46,7 +48,7 @@ class DiagnosticsModule }; template -void DiagnosticsModule::add_key_value(const std::string & key, const T & value) +void DiagnosticInterface::add_key_value(const std::string & key, const T & value) { diagnostic_msgs::msg::KeyValue key_value; key_value.key = key; @@ -54,11 +56,6 @@ void DiagnosticsModule::add_key_value(const std::string & key, const T & 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::universe_utils -} // namespace autoware::localization_util - -#endif // AUTOWARE__LOCALIZATION_UTIL__DIAGNOSTICS_MODULE_HPP_ +#endif // AUTOWARE__UNIVERSE_UTILS__ROS__DIAGNOSTICS_INTERFACE_HPP_ diff --git a/localization/autoware_localization_util/src/diagnostics_module.cpp b/common/autoware_universe_utils/src/ros/diagnostics_interface.cpp similarity index 76% rename from localization/autoware_localization_util/src/diagnostics_module.cpp rename to common/autoware_universe_utils/src/ros/diagnostics_interface.cpp index 2b68dbf4f5890..978af27f202d4 100644 --- a/localization/autoware_localization_util/src/diagnostics_module.cpp +++ b/common/autoware_universe_utils/src/ros/diagnostics_interface.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware/localization_util/diagnostics_module.hpp" +#include "autoware/universe_utils/ros/diagnostics_interface.hpp" #include @@ -21,9 +21,9 @@ #include #include -namespace autoware::localization_util +namespace autoware::universe_utils { -DiagnosticsModule::DiagnosticsModule(rclcpp::Node * node, const std::string & diagnostic_name) +DiagnosticInterface::DiagnosticInterface(rclcpp::Node * node, const std::string & diagnostic_name) : clock_(node->get_clock()) { diagnostics_pub_ = @@ -34,7 +34,7 @@ DiagnosticsModule::DiagnosticsModule(rclcpp::Node * node, const std::string & di diagnostics_status_msg_.hardware_id = node->get_name(); } -void DiagnosticsModule::clear() +void DiagnosticInterface::clear() { diagnostics_status_msg_.values.clear(); diagnostics_status_msg_.values.shrink_to_fit(); @@ -43,7 +43,7 @@ void DiagnosticsModule::clear() diagnostics_status_msg_.message = ""; } -void DiagnosticsModule::add_key_value(const diagnostic_msgs::msg::KeyValue & key_value_msg) +void DiagnosticInterface::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), @@ -56,8 +56,7 @@ void DiagnosticsModule::add_key_value(const diagnostic_msgs::msg::KeyValue & key } } -template <> -void DiagnosticsModule::add_key_value(const std::string & key, const std::string & value) +void DiagnosticInterface::add_key_value(const std::string & key, const std::string & value) { diagnostic_msgs::msg::KeyValue key_value; key_value.key = key; @@ -65,8 +64,7 @@ void DiagnosticsModule::add_key_value(const std::string & key, const std::string add_key_value(key_value); } -template <> -void DiagnosticsModule::add_key_value(const std::string & key, const bool & value) +void DiagnosticInterface::add_key_value(const std::string & key, bool value) { diagnostic_msgs::msg::KeyValue key_value; key_value.key = key; @@ -74,7 +72,7 @@ void DiagnosticsModule::add_key_value(const std::string & key, const bool & valu add_key_value(key_value); } -void DiagnosticsModule::update_level_and_message(const int8_t level, const std::string & message) +void DiagnosticInterface::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()) { @@ -87,12 +85,12 @@ void DiagnosticsModule::update_level_and_message(const int8_t level, const std:: } } -void DiagnosticsModule::publish(const rclcpp::Time & publish_time_stamp) +void DiagnosticInterface::publish(const rclcpp::Time & publish_time_stamp) { diagnostics_pub_->publish(create_diagnostics_array(publish_time_stamp)); } -diagnostic_msgs::msg::DiagnosticArray DiagnosticsModule::create_diagnostics_array( +diagnostic_msgs::msg::DiagnosticArray DiagnosticInterface::create_diagnostics_array( const rclcpp::Time & publish_time_stamp) const { diagnostic_msgs::msg::DiagnosticArray diagnostics_msg; @@ -105,4 +103,4 @@ diagnostic_msgs::msg::DiagnosticArray DiagnosticsModule::create_diagnostics_arra return diagnostics_msg; } -} // namespace autoware::localization_util +} // namespace autoware::universe_utils diff --git a/common/autoware_universe_utils/test/src/ros/test_diagnostics_interface.cpp b/common/autoware_universe_utils/test/src/ros/test_diagnostics_interface.cpp new file mode 100644 index 0000000000000..a0683694cc2b7 --- /dev/null +++ b/common/autoware_universe_utils/test/src/ros/test_diagnostics_interface.cpp @@ -0,0 +1,179 @@ +// 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/universe_utils/ros/diagnostics_interface.hpp" + +#include + +#include +#include +#include + +#include + +#include +#include + +using autoware::universe_utils::DiagnosticInterface; + +class TestDiagnosticInterface : public ::testing::Test +{ +protected: + void SetUp() override + { + // Create a test node + node_ = std::make_shared("test_diagnostics_interface"); + } + + // Automatically destroyed at the end of each test + std::shared_ptr node_; +}; + +/* + * Test clear(): + * Verify that calling clear() resets DiagnosticStatus values, level, and message. + */ +TEST_F(TestDiagnosticInterface, ClearTest) +{ + DiagnosticInterface module(node_.get(), "test_diagnostic"); + + // Add some key-value pairs and update level/message + module.add_key_value("param1", 42); + module.update_level_and_message( + diagnostic_msgs::msg::DiagnosticStatus::WARN, "Something is not OK"); + + // Call clear() + module.clear(); + + // After calling clear(), publish and check the contents of the message + diagnostic_msgs::msg::DiagnosticArray::ConstSharedPtr received_msg; + auto sub = node_->create_subscription( + "/diagnostics", 10, + [&](diagnostic_msgs::msg::DiagnosticArray::ConstSharedPtr msg) { received_msg = msg; }); + + // Publish the message + module.publish(node_->now()); + + // Spin to allow the subscriber to receive messages + rclcpp::spin_some(node_); + + ASSERT_NE(nullptr, received_msg); + ASSERT_FALSE(received_msg->status.empty()); + const auto & status = received_msg->status.front(); + + // After clear(), key-value pairs should be empty + EXPECT_TRUE(status.values.empty()); + + // After clear(), level should be OK (=0) + EXPECT_EQ(status.level, diagnostic_msgs::msg::DiagnosticStatus::OK); + + // After clear(), message is empty internally, + // but "OK" is set during publishing when level == OK + EXPECT_EQ(status.message, "OK"); +} + +/* + * Test add_key_value(): + * Verify that adding the same key updates its value instead of adding a duplicate. + */ +TEST_F(TestDiagnosticInterface, AddKeyValueTest) +{ + DiagnosticInterface module(node_.get(), "test_diagnostic"); + + // Call the template version of add_key_value() with different types + module.add_key_value("string_key", std::string("initial_value")); + module.add_key_value("int_key", 123); + module.add_key_value("bool_key", true); + + // Overwrite the value for "string_key" + module.add_key_value("string_key", std::string("updated_value")); + + // Capture the published message to verify its contents + diagnostic_msgs::msg::DiagnosticArray::ConstSharedPtr received_msg; + auto sub = node_->create_subscription( + "/diagnostics", 10, + [&](diagnostic_msgs::msg::DiagnosticArray::ConstSharedPtr msg) { received_msg = msg; }); + module.publish(node_->now()); + rclcpp::spin_some(node_); + + ASSERT_NE(nullptr, received_msg); + ASSERT_FALSE(received_msg->status.empty()); + const auto & status = received_msg->status.front(); + + // Expect 3 key-value pairs + ASSERT_EQ(status.values.size(), 3U); + + // Helper lambda to find a value by key + auto find_value = [&](const std::string & key) -> std::string { + for (const auto & kv : status.values) { + if (kv.key == key) { + return kv.value; + } + } + return ""; + }; + + EXPECT_EQ(find_value("string_key"), "updated_value"); + EXPECT_EQ(find_value("int_key"), "123"); + EXPECT_EQ(find_value("bool_key"), "True"); + + // Status level should still be OK + EXPECT_EQ(status.level, diagnostic_msgs::msg::DiagnosticStatus::OK); + // Message should be "OK" + EXPECT_EQ(status.message, "OK"); +} + +/* + * Test update_level_and_message(): + * Verify that the level is updated to the highest severity and + * that messages are concatenated if level > OK. + */ +TEST_F(TestDiagnosticInterface, UpdateLevelAndMessageTest) +{ + DiagnosticInterface module(node_.get(), "test_diagnostic"); + + // Initial status is level=OK(0), message="" + module.update_level_and_message(diagnostic_msgs::msg::DiagnosticStatus::OK, "Initial OK"); + // Update to WARN (1) + module.update_level_and_message(diagnostic_msgs::msg::DiagnosticStatus::WARN, "Low battery"); + // Update to ERROR (2) + module.update_level_and_message(diagnostic_msgs::msg::DiagnosticStatus::ERROR, "Sensor failure"); + // Another WARN (1) after ERROR should not downgrade the overall level + module.update_level_and_message( + diagnostic_msgs::msg::DiagnosticStatus::WARN, "Should not override error"); + + diagnostic_msgs::msg::DiagnosticArray::ConstSharedPtr received_msg; + auto sub = node_->create_subscription( + "/diagnostics", 10, + [&](diagnostic_msgs::msg::DiagnosticArray::ConstSharedPtr msg) { received_msg = msg; }); + + module.publish(node_->now()); + rclcpp::spin_some(node_); + + ASSERT_NE(nullptr, received_msg); + ASSERT_FALSE(received_msg->status.empty()); + const auto & status = received_msg->status.front(); + + // Final level should be ERROR (2) + EXPECT_EQ(status.level, diagnostic_msgs::msg::DiagnosticStatus::ERROR); + + // The message should contain all parts that were added when level > OK. + // Thus, we expect something like: + // "Low battery; Sensor failure; Should not override error" + const std::string & final_message = status.message; + EXPECT_FALSE(final_message.find("Initial OK") != std::string::npos); + EXPECT_TRUE(final_message.find("Low battery") != std::string::npos); + EXPECT_TRUE(final_message.find("Sensor failure") != std::string::npos); + EXPECT_TRUE(final_message.find("Should not override error") != std::string::npos); +} diff --git a/localization/autoware_gyro_odometer/src/gyro_odometer_core.cpp b/localization/autoware_gyro_odometer/src/gyro_odometer_core.cpp index 9511f168f346f..d52fb5e798b58 100644 --- a/localization/autoware_gyro_odometer/src/gyro_odometer_core.cpp +++ b/localization/autoware_gyro_odometer/src/gyro_odometer_core.cpp @@ -73,7 +73,7 @@ GyroOdometerNode::GyroOdometerNode(const rclcpp::NodeOptions & node_options) "twist_with_covariance", rclcpp::QoS{10}); diagnostics_ = - std::make_unique(this, "gyro_odometer_status"); + std::make_unique(this, "gyro_odometer_status"); // TODO(YamatoAndo) createTimer } diff --git a/localization/autoware_gyro_odometer/src/gyro_odometer_core.hpp b/localization/autoware_gyro_odometer/src/gyro_odometer_core.hpp index 036b3d7cab527..70334738e9ce3 100644 --- a/localization/autoware_gyro_odometer/src/gyro_odometer_core.hpp +++ b/localization/autoware_gyro_odometer/src/gyro_odometer_core.hpp @@ -15,7 +15,7 @@ #ifndef GYRO_ODOMETER_CORE_HPP_ #define GYRO_ODOMETER_CORE_HPP_ -#include "autoware/localization_util/diagnostics_module.hpp" +#include "autoware/universe_utils/ros/diagnostics_interface.hpp" #include "autoware/universe_utils/ros/logger_level_configure.hpp" #include "autoware/universe_utils/ros/msg_covariance.hpp" #include "autoware/universe_utils/ros/transform_listener.hpp" @@ -80,7 +80,7 @@ class GyroOdometerNode : public rclcpp::Node std::deque vehicle_twist_queue_; std::deque gyro_queue_; - std::unique_ptr diagnostics_; + std::unique_ptr diagnostics_; }; } // namespace autoware::gyro_odometer diff --git a/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/src/lidar_marker_localizer.cpp b/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/src/lidar_marker_localizer.cpp index 2faf2d19168a5..bc7dbbcfc9ca1 100644 --- a/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/src/lidar_marker_localizer.cpp +++ b/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/src/lidar_marker_localizer.cpp @@ -124,22 +124,22 @@ LidarMarkerLocalizer::LidarMarkerLocalizer(const rclcpp::NodeOptions & node_opti tf_buffer_ = std::make_shared(this->get_clock()); tf_listener_ = std::make_shared(*tf_buffer_, this, false); - diagnostics_module_.reset( - new autoware::localization_util::DiagnosticsModule(this, "marker_detection_status")); + diagnostics_interface_.reset( + new autoware::universe_utils::DiagnosticInterface(this, "marker_detection_status")); } void LidarMarkerLocalizer::initialize_diagnostics() { - diagnostics_module_->clear(); - diagnostics_module_->add_key_value("is_received_map", false); - diagnostics_module_->add_key_value("is_received_self_pose", false); - diagnostics_module_->add_key_value("detect_marker_num", 0); - diagnostics_module_->add_key_value("distance_self_pose_to_nearest_marker", 0.0); - diagnostics_module_->add_key_value( + diagnostics_interface_->clear(); + diagnostics_interface_->add_key_value("is_received_map", false); + diagnostics_interface_->add_key_value("is_received_self_pose", false); + diagnostics_interface_->add_key_value("detect_marker_num", 0); + diagnostics_interface_->add_key_value("distance_self_pose_to_nearest_marker", 0.0); + diagnostics_interface_->add_key_value( "limit_distance_from_self_pose_to_nearest_marker", param_.limit_distance_from_self_pose_to_nearest_marker); - diagnostics_module_->add_key_value("distance_lanelet2_marker_to_detected_marker", 0.0); - diagnostics_module_->add_key_value( + diagnostics_interface_->add_key_value("distance_lanelet2_marker_to_detected_marker", 0.0); + diagnostics_interface_->add_key_value( "limit_distance_from_lanelet2_marker_to_detected_marker", param_.limit_distance_from_self_pose_to_marker); } @@ -176,7 +176,7 @@ void LidarMarkerLocalizer::points_callback(const PointCloud2::ConstSharedPtr & p main_process(points_msg_ptr); - diagnostics_module_->publish(sensor_ros_time); + diagnostics_interface_->publish(sensor_ros_time); } void LidarMarkerLocalizer::main_process(const PointCloud2::ConstSharedPtr & points_msg_ptr) @@ -186,13 +186,13 @@ void LidarMarkerLocalizer::main_process(const PointCloud2::ConstSharedPtr & poin // (1) check if the map have be received const std::vector map_landmarks = landmark_manager_.get_landmarks(); const bool is_received_map = !map_landmarks.empty(); - diagnostics_module_->add_key_value("is_received_map", is_received_map); + diagnostics_interface_->add_key_value("is_received_map", is_received_map); if (!is_received_map) { std::stringstream message; message << "Not receive the landmark information. Please check if the vector map is being " << "published and if the landmark information is correctly specified."; RCLCPP_INFO_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, message.str()); - diagnostics_module_->update_level_and_message( + diagnostics_interface_->update_level_and_message( diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); return; } @@ -202,13 +202,13 @@ void LidarMarkerLocalizer::main_process(const PointCloud2::ConstSharedPtr & poin interpolate_result = ekf_pose_buffer_->interpolate(sensor_ros_time); const bool is_received_self_pose = interpolate_result != std::nullopt; - diagnostics_module_->add_key_value("is_received_self_pose", is_received_self_pose); + diagnostics_interface_->add_key_value("is_received_self_pose", is_received_self_pose); if (!is_received_self_pose) { std::stringstream message; message << "Could not get self_pose. Please check if the self pose is being published and if " << "the timestamp of the self pose is correctly specified"; RCLCPP_INFO_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, message.str()); - diagnostics_module_->update_level_and_message( + diagnostics_interface_->update_level_and_message( diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); return; } @@ -221,7 +221,7 @@ void LidarMarkerLocalizer::main_process(const PointCloud2::ConstSharedPtr & poin detect_landmarks(points_msg_ptr); const bool is_detected_marker = !detected_landmarks.empty(); - diagnostics_module_->add_key_value("detect_marker_num", detected_landmarks.size()); + diagnostics_interface_->add_key_value("detect_marker_num", detected_landmarks.size()); // (4) check distance to the nearest marker const landmark_manager::Landmark nearest_marker = get_nearest_landmark(self_pose, map_landmarks); @@ -230,7 +230,7 @@ void LidarMarkerLocalizer::main_process(const PointCloud2::ConstSharedPtr & poin const double distance_from_self_pose_to_nearest_marker = std::abs(nearest_marker_pose_on_base_link.position.x); - diagnostics_module_->add_key_value( + diagnostics_interface_->add_key_value( "distance_self_pose_to_nearest_marker", distance_from_self_pose_to_nearest_marker); const bool is_exist_marker_within_self_pose = @@ -242,14 +242,14 @@ void LidarMarkerLocalizer::main_process(const PointCloud2::ConstSharedPtr & poin std::stringstream message; message << "Could not detect marker, because the distance from self_pose to nearest_marker " << "is too far (" << distance_from_self_pose_to_nearest_marker << " [m])."; - diagnostics_module_->update_level_and_message( + diagnostics_interface_->update_level_and_message( diagnostic_msgs::msg::DiagnosticStatus::OK, message.str()); } else { std::stringstream message; message << "Could not detect marker, although the distance from self_pose to nearest_marker " << "is near (" << distance_from_self_pose_to_nearest_marker << " [m])."; RCLCPP_INFO_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, message.str()); - diagnostics_module_->update_level_and_message( + diagnostics_interface_->update_level_and_message( diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); } return; @@ -279,13 +279,13 @@ void LidarMarkerLocalizer::main_process(const PointCloud2::ConstSharedPtr & poin const bool is_exist_marker_within_lanelet2_map = diff_norm < param_.limit_distance_from_self_pose_to_marker; - diagnostics_module_->add_key_value("distance_lanelet2_marker_to_detected_marker", diff_norm); + diagnostics_interface_->add_key_value("distance_lanelet2_marker_to_detected_marker", diff_norm); if (!is_exist_marker_within_lanelet2_map) { std::stringstream message; message << "The distance from lanelet2 to the detect marker is too far(" << diff_norm << " [m]). The limit is " << param_.limit_distance_from_self_pose_to_marker << "."; RCLCPP_INFO_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, message.str()); - diagnostics_module_->update_level_and_message( + diagnostics_interface_->update_level_and_message( diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); return; } diff --git a/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/src/lidar_marker_localizer.hpp b/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/src/lidar_marker_localizer.hpp index d1482c6912592..1b5672cff9d04 100644 --- a/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/src/lidar_marker_localizer.hpp +++ b/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/src/lidar_marker_localizer.hpp @@ -15,8 +15,8 @@ #ifndef LIDAR_MARKER_LOCALIZER_HPP_ #define LIDAR_MARKER_LOCALIZER_HPP_ -#include "autoware/localization_util/diagnostics_module.hpp" #include "autoware/localization_util/smart_pose_buffer.hpp" +#include "autoware/universe_utils/ros/diagnostics_interface.hpp" #include @@ -134,7 +134,7 @@ class LidarMarkerLocalizer : public rclcpp::Node rclcpp::Publisher::SharedPtr pub_debug_pose_with_covariance_; rclcpp::Publisher::SharedPtr pub_marker_pointcloud_; - std::shared_ptr diagnostics_module_; + std::shared_ptr diagnostics_interface_; Param param_; bool is_activated_; diff --git a/localization/autoware_localization_error_monitor/CHANGELOG.rst b/localization/autoware_localization_error_monitor/CHANGELOG.rst index e9583916e2e8f..69833a04a8d3a 100644 --- a/localization/autoware_localization_error_monitor/CHANGELOG.rst +++ b/localization/autoware_localization_error_monitor/CHANGELOG.rst @@ -43,8 +43,8 @@ Changelog for package autoware_localization_error_monitor * unify package.xml version to 0.37.0 * refactor(localization_util)!: prefix package and namespace with autoware (`#8922 `_) add autoware prefix to localization_util -* fix(localization_error_monitor, system diag): fix to use diagnostics_module in localization_util (`#8543 `_) - * fix(localization_error_monitor): fix to use diagnostics_module in localization_util +* fix(localization_error_monitor, system diag): fix to use diagnostics_interface in localization_util (`#8543 `_) + * fix(localization_error_monitor): fix to use diagnostics_interface in localization_util * fix: update media * fix: update component name * fix: rename include file diff --git a/localization/autoware_localization_error_monitor/src/localization_error_monitor.cpp b/localization/autoware_localization_error_monitor/src/localization_error_monitor.cpp index 63edbe5f6a9c7..5ebcd105d57ba 100644 --- a/localization/autoware_localization_error_monitor/src/localization_error_monitor.cpp +++ b/localization/autoware_localization_error_monitor/src/localization_error_monitor.cpp @@ -59,7 +59,7 @@ LocalizationErrorMonitor::LocalizationErrorMonitor(const rclcpp::NodeOptions & o logger_configure_ = std::make_unique(this); diagnostics_error_monitor_ = - std::make_unique(this, "ellipse_error_status"); + std::make_unique(this, "ellipse_error_status"); } void LocalizationErrorMonitor::on_odom(nav_msgs::msg::Odometry::ConstSharedPtr input_msg) diff --git a/localization/autoware_localization_error_monitor/src/localization_error_monitor.hpp b/localization/autoware_localization_error_monitor/src/localization_error_monitor.hpp index 7b26573964b38..b38958c420b19 100644 --- a/localization/autoware_localization_error_monitor/src/localization_error_monitor.hpp +++ b/localization/autoware_localization_error_monitor/src/localization_error_monitor.hpp @@ -16,7 +16,7 @@ #define LOCALIZATION_ERROR_MONITOR_HPP_ #include "autoware/localization_util/covariance_ellipse.hpp" -#include "autoware/localization_util/diagnostics_module.hpp" +#include "autoware/universe_utils/ros/diagnostics_interface.hpp" #include #include @@ -39,7 +39,7 @@ class LocalizationErrorMonitor : public rclcpp::Node std::unique_ptr logger_configure_; - std::unique_ptr diagnostics_error_monitor_; + std::unique_ptr diagnostics_error_monitor_; double scale_; double error_ellipse_size_; diff --git a/localization/autoware_localization_util/CMakeLists.txt b/localization/autoware_localization_util/CMakeLists.txt index dd18f3cbad932..de62633124f3d 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_ndt_scan_matcher/include/autoware/ndt_scan_matcher/map_update_module.hpp b/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/map_update_module.hpp index d2dce42e3923e..bf8fce2b302c0 100644 --- a/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/map_update_module.hpp +++ b/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/map_update_module.hpp @@ -15,8 +15,8 @@ #ifndef AUTOWARE__NDT_SCAN_MATCHER__MAP_UPDATE_MODULE_HPP_ #define AUTOWARE__NDT_SCAN_MATCHER__MAP_UPDATE_MODULE_HPP_ -#include "autoware/localization_util/diagnostics_module.hpp" #include "autoware/localization_util/util_func.hpp" +#include "autoware/universe_utils/ros/diagnostics_interface.hpp" #include "hyper_parameters.hpp" #include "ndt_omp/multigrid_ndt_omp.h" #include "particle.hpp" @@ -42,7 +42,7 @@ namespace autoware::ndt_scan_matcher { -using DiagnosticsModule = autoware::localization_util::DiagnosticsModule; +using DiagnosticInterface = autoware::universe_utils::DiagnosticInterface; class MapUpdateModule { @@ -63,19 +63,19 @@ class MapUpdateModule void callback_timer( const bool is_activated, const std::optional & position, - std::unique_ptr & diagnostics_ptr); + std::unique_ptr & diagnostics_ptr); [[nodiscard]] bool should_update_map( const geometry_msgs::msg::Point & position, - std::unique_ptr & diagnostics_ptr); + std::unique_ptr & diagnostics_ptr); void update_map( const geometry_msgs::msg::Point & position, - std::unique_ptr & diagnostics_ptr); + std::unique_ptr & diagnostics_ptr); // Update the specified NDT bool update_ndt( const geometry_msgs::msg::Point & position, NdtType & ndt, - std::unique_ptr & diagnostics_ptr); + std::unique_ptr & diagnostics_ptr); void publish_partial_pcd_map(); rclcpp::Publisher::SharedPtr loaded_pcd_pub_; diff --git a/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/ndt_scan_matcher_core.hpp b/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/ndt_scan_matcher_core.hpp index 22b6bfb2ff740..25b7417ffbe3c 100644 --- a/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/ndt_scan_matcher_core.hpp +++ b/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/ndt_scan_matcher_core.hpp @@ -17,8 +17,8 @@ #define FMT_HEADER_ONLY -#include "autoware/localization_util/diagnostics_module.hpp" #include "autoware/localization_util/smart_pose_buffer.hpp" +#include "autoware/universe_utils/ros/diagnostics_interface.hpp" #include "hyper_parameters.hpp" #include "map_update_module.hpp" #include "ndt_omp/multigrid_ndt_omp.h" @@ -211,12 +211,12 @@ class NDTScanMatcher : public rclcpp::Node std::unique_ptr regularization_pose_buffer_; std::atomic is_activated_; - std::unique_ptr diagnostics_scan_points_; - std::unique_ptr diagnostics_initial_pose_; - std::unique_ptr diagnostics_regularization_pose_; - std::unique_ptr diagnostics_map_update_; - std::unique_ptr diagnostics_ndt_align_; - std::unique_ptr diagnostics_trigger_node_; + std::unique_ptr diagnostics_scan_points_; + std::unique_ptr diagnostics_initial_pose_; + std::unique_ptr diagnostics_regularization_pose_; + std::unique_ptr diagnostics_map_update_; + std::unique_ptr diagnostics_ndt_align_; + std::unique_ptr diagnostics_trigger_node_; std::unique_ptr map_update_module_; std::unique_ptr logger_configure_; diff --git a/localization/autoware_ndt_scan_matcher/src/map_update_module.cpp b/localization/autoware_ndt_scan_matcher/src/map_update_module.cpp index 25b51b55aebd7..eea0b8f3d06c4 100644 --- a/localization/autoware_ndt_scan_matcher/src/map_update_module.cpp +++ b/localization/autoware_ndt_scan_matcher/src/map_update_module.cpp @@ -56,7 +56,7 @@ MapUpdateModule::MapUpdateModule( void MapUpdateModule::callback_timer( const bool is_activated, const std::optional & position, - std::unique_ptr & diagnostics_ptr) + std::unique_ptr & diagnostics_ptr) { // check is_activated diagnostics_ptr->add_key_value("is_activated", is_activated); @@ -86,7 +86,8 @@ void MapUpdateModule::callback_timer( } bool MapUpdateModule::should_update_map( - const geometry_msgs::msg::Point & position, std::unique_ptr & diagnostics_ptr) + const geometry_msgs::msg::Point & position, + std::unique_ptr & diagnostics_ptr) { last_update_position_mtx_.lock(); @@ -141,7 +142,8 @@ bool MapUpdateModule::out_of_map_range(const geometry_msgs::msg::Point & positio } void MapUpdateModule::update_map( - const geometry_msgs::msg::Point & position, std::unique_ptr & diagnostics_ptr) + const geometry_msgs::msg::Point & position, + std::unique_ptr & diagnostics_ptr) { diagnostics_ptr->add_key_value("is_need_rebuild", need_rebuild_); @@ -229,7 +231,7 @@ void MapUpdateModule::update_map( bool MapUpdateModule::update_ndt( const geometry_msgs::msg::Point & position, NdtType & ndt, - std::unique_ptr & diagnostics_ptr) + std::unique_ptr & diagnostics_ptr) { diagnostics_ptr->add_key_value("maps_size_before", ndt.getCurrentMapIDs().size()); diff --git a/localization/autoware_ndt_scan_matcher/src/ndt_scan_matcher_core.cpp b/localization/autoware_ndt_scan_matcher/src/ndt_scan_matcher_core.cpp index a1871023d525b..cef8724423bed 100644 --- a/localization/autoware_ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/autoware_ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -49,9 +49,9 @@ using autoware::localization_util::matrix4f_to_pose; using autoware::localization_util::point_to_vector3d; using autoware::localization_util::pose_to_matrix4f; -using autoware::localization_util::DiagnosticsModule; using autoware::localization_util::SmartPoseBuffer; using autoware::localization_util::TreeStructuredParzenEstimator; +using autoware::universe_utils::DiagnosticInterface; tier4_debug_msgs::msg::Float32Stamped make_float32_stamped( const builtin_interfaces::msg::Time & stamp, const float data) @@ -141,7 +141,7 @@ NDTScanMatcher::NDTScanMatcher(const rclcpp::NodeOptions & options) std::make_unique(this->get_logger(), value_as_unlimited, value_as_unlimited); diagnostics_regularization_pose_ = - std::make_unique(this, "regularization_pose_subscriber_status"); + std::make_unique(this, "regularization_pose_subscriber_status"); } sensor_aligned_pose_pub_ = @@ -209,13 +209,13 @@ NDTScanMatcher::NDTScanMatcher(const rclcpp::NodeOptions & options) map_update_module_ = std::make_unique(this, &ndt_ptr_mtx_, ndt_ptr_, param_.dynamic_map_loading); - diagnostics_scan_points_ = std::make_unique(this, "scan_matching_status"); + diagnostics_scan_points_ = std::make_unique(this, "scan_matching_status"); diagnostics_initial_pose_ = - std::make_unique(this, "initial_pose_subscriber_status"); - diagnostics_map_update_ = std::make_unique(this, "map_update_status"); - diagnostics_ndt_align_ = std::make_unique(this, "ndt_align_service_status"); + std::make_unique(this, "initial_pose_subscriber_status"); + diagnostics_map_update_ = std::make_unique(this, "map_update_status"); + diagnostics_ndt_align_ = std::make_unique(this, "ndt_align_service_status"); diagnostics_trigger_node_ = - std::make_unique(this, "trigger_node_service_status"); + std::make_unique(this, "trigger_node_service_status"); logger_configure_ = std::make_unique(this); } diff --git a/localization/autoware_pose_initializer/package.xml b/localization/autoware_pose_initializer/package.xml index eeeb46a8db3e5..de3ecd880ea45 100644 --- a/localization/autoware_pose_initializer/package.xml +++ b/localization/autoware_pose_initializer/package.xml @@ -21,7 +21,6 @@ autoware_component_interface_specs autoware_component_interface_utils - autoware_localization_util autoware_map_height_fitter autoware_motion_utils autoware_universe_utils diff --git a/localization/autoware_pose_initializer/src/pose_initializer_core.cpp b/localization/autoware_pose_initializer/src/pose_initializer_core.cpp index 5e9c68d2acc80..67d5c447c09b6 100644 --- a/localization/autoware_pose_initializer/src/pose_initializer_core.cpp +++ b/localization/autoware_pose_initializer/src/pose_initializer_core.cpp @@ -40,7 +40,7 @@ PoseInitializer::PoseInitializer(const rclcpp::NodeOptions & options) output_pose_covariance_ = get_covariance_parameter(this, "output_pose_covariance"); gnss_particle_covariance_ = get_covariance_parameter(this, "gnss_particle_covariance"); - diagnostics_pose_reliable_ = std::make_unique( + diagnostics_pose_reliable_ = std::make_unique( this, "pose_initializer_status"); if (declare_parameter("ekf_enabled")) { diff --git a/localization/autoware_pose_initializer/src/pose_initializer_core.hpp b/localization/autoware_pose_initializer/src/pose_initializer_core.hpp index 28d2eae08c3f1..5305bcc3ad347 100644 --- a/localization/autoware_pose_initializer/src/pose_initializer_core.hpp +++ b/localization/autoware_pose_initializer/src/pose_initializer_core.hpp @@ -15,10 +15,9 @@ #ifndef POSE_INITIALIZER_CORE_HPP_ #define POSE_INITIALIZER_CORE_HPP_ -#include "autoware/localization_util/diagnostics_module.hpp" - #include #include +#include #include #include @@ -61,7 +60,7 @@ class PoseInitializer : public rclcpp::Node std::unique_ptr ekf_localization_trigger_; std::unique_ptr ndt_localization_trigger_; std::unique_ptr logger_configure_; - std::unique_ptr diagnostics_pose_reliable_; + std::unique_ptr diagnostics_pose_reliable_; double stop_check_duration_; void change_node_trigger(bool flag, bool need_spin = false);