feat!: move diagnostics_module from localization_util to unverse_utils

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_module.cpp
   src/ros/msg_operation.cpp
   src/ros/marker_helper.cpp
   src/ros/logger_level_configure.cpp

localization/autoware_localization_util/include/autoware/localization_util/diagnostics_module.hpp → common/autoware_universe_utils/include/autoware/universe_utils/ros/diagnostics_module.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_MODULE_HPP_
+#define AUTOWARE__UNIVERSE_UTILS__ROS__DIAGNOSTICS_MODULE_HPP_
 
 #include <rclcpp/rclcpp.hpp>
 
@@ -22,7 +22,7 @@
 #include <string>
 #include <vector>
 
-namespace autoware::localization_util
+namespace autoware::universe_utils
 {
 class DiagnosticsModule
 {
@@ -59,6 +59,6 @@ void DiagnosticsModule::add_key_value(const std::string & key, const std::string
 template <>
 void DiagnosticsModule::add_key_value(const std::string & key, const bool & value);
 
-}  // namespace autoware::localization_util
+}  // namespace autoware::universe_utils
 
-#endif  // AUTOWARE__LOCALIZATION_UTIL__DIAGNOSTICS_MODULE_HPP_
+#endif  // AUTOWARE__UNIVERSE_UTILS__ROS__DIAGNOSTICS_MODULE_HPP_

localization/autoware_localization_util/src/diagnostics_module.cpp → common/autoware_universe_utils/src/ros/diagnostics_module.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_module.hpp"
 
 #include <rclcpp/rclcpp.hpp>
 
@@ -21,7 +21,7 @@
 #include <algorithm>
 #include <string>
 
-namespace autoware::localization_util
+namespace autoware::universe_utils
 {
 DiagnosticsModule::DiagnosticsModule(rclcpp::Node * node, const std::string & diagnostic_name)
 : clock_(node->get_clock())
@@ -76,7 +76,7 @@ void DiagnosticsModule::add_key_value(const std::string & key, const bool & valu
 
 void DiagnosticsModule::update_level_and_message(const int8_t level, const std::string & message)
 {
-  if ((level > diagnostic_msgs::msg::DiagnosticStatus::OK)) {
+  if ((level >= diagnostic_msgs::msg::DiagnosticStatus::OK)) {
     if (!diagnostics_status_msg_.message.empty()) {
       diagnostics_status_msg_.message += "; ";
     }
@@ -105,4 +105,4 @@ diagnostic_msgs::msg::DiagnosticArray DiagnosticsModule::create_diagnostics_arra
 
   return diagnostics_msg;
 }
-}  // namespace autoware::localization_util
+}  // namespace autoware::universe_utils

common/autoware_universe_utils/test/src/ros/test_diagnostics_module.cpp

@@ -0,0 +1,176 @@
+// 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_module.hpp"
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <diagnostic_msgs/msg/diagnostic_array.hpp>
+#include <diagnostic_msgs/msg/diagnostic_status.hpp>
+#include <diagnostic_msgs/msg/key_value.hpp>
+
+#include <gtest/gtest.h>
+
+using autoware::universe_utils::DiagnosticsModule;
+
+class TestDiagnosticsModule : public ::testing::Test
+{
+protected:
+  void SetUp() override
+  {
+    // Create a test node
+    node_ = std::make_shared<rclcpp::Node>("test_diagnostics_module");
+  }
+
+  // Automatically destroyed at the end of each test
+  std::shared_ptr<rclcpp::Node> node_;
+};
+
+/*
+ * Test clear():
+ * Verify that calling clear() resets DiagnosticStatus values, level, and message.
+ */
+TEST_F(TestDiagnosticsModule, ClearTest)
+{
+  DiagnosticsModule 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<diagnostic_msgs::msg::DiagnosticArray>(
+    "/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(TestDiagnosticsModule, AddKeyValueTest)
+{
+  DiagnosticsModule 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<diagnostic_msgs::msg::DiagnosticArray>(
+    "/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(TestDiagnosticsModule, UpdateLevelAndMessageTest)
+{
+  DiagnosticsModule 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<diagnostic_msgs::msg::DiagnosticArray>(
+    "/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);
+}

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<autoware::localization_util::DiagnosticsModule>(this, "gyro_odometer_status");
+    std::make_unique<autoware::universe_utils::DiagnosticsModule>(this, "gyro_odometer_status");
 
   // TODO(YamatoAndo) createTimer
 }

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_module.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<geometry_msgs::msg::TwistWithCovarianceStamped> vehicle_twist_queue_;
   std::deque<sensor_msgs::msg::Imu> gyro_queue_;
 
-  std::unique_ptr<autoware::localization_util::DiagnosticsModule> diagnostics_;
+  std::unique_ptr<autoware::universe_utils::DiagnosticsModule> diagnostics_;
 };
 
 }  // namespace autoware::gyro_odometer

localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/src/lidar_marker_localizer.cpp

@@ -125,7 +125,7 @@ LidarMarkerLocalizer::LidarMarkerLocalizer(const rclcpp::NodeOptions & node_opti
   tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_, this, false);
 
   diagnostics_module_.reset(
-    new autoware::localization_util::DiagnosticsModule(this, "marker_detection_status"));
+    new autoware::universe_utils::DiagnosticsModule(this, "marker_detection_status"));
 }
 
 void LidarMarkerLocalizer::initialize_diagnostics()

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_module.hpp"
 
 #include <rclcpp/rclcpp.hpp>
 
@@ -134,7 +134,7 @@ class LidarMarkerLocalizer : public rclcpp::Node
   rclcpp::Publisher<PoseWithCovarianceStamped>::SharedPtr pub_debug_pose_with_covariance_;
   rclcpp::Publisher<PointCloud2>::SharedPtr pub_marker_pointcloud_;
 
-  std::shared_ptr<autoware::localization_util::DiagnosticsModule> diagnostics_module_;
+  std::shared_ptr<autoware::universe_utils::DiagnosticsModule> diagnostics_module_;
 
   Param param_;
   bool is_activated_;

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<autoware::universe_utils::LoggerLevelConfigure>(this);
 
   diagnostics_error_monitor_ =
-    std::make_unique<autoware::localization_util::DiagnosticsModule>(this, "ellipse_error_status");
+    std::make_unique<autoware::universe_utils::DiagnosticsModule>(this, "ellipse_error_status");
 }
 
 void LocalizationErrorMonitor::on_odom(nav_msgs::msg::Odometry::ConstSharedPtr input_msg)

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_module.hpp"
 
 #include <Eigen/Dense>
 #include <autoware/universe_utils/ros/logger_level_configure.hpp>
@@ -39,7 +39,7 @@ class LocalizationErrorMonitor : public rclcpp::Node
 
   std::unique_ptr<autoware::universe_utils::LoggerLevelConfigure> logger_configure_;
 
-  std::unique_ptr<autoware::localization_util::DiagnosticsModule> diagnostics_error_monitor_;
+  std::unique_ptr<autoware::universe_utils::DiagnosticsModule> diagnostics_error_monitor_;
 
   double scale_;
   double error_ellipse_size_;

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

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_module.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 DiagnosticsModule = autoware::universe_utils::DiagnosticsModule;
 
 class MapUpdateModule
 {

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_module.hpp"
 #include "hyper_parameters.hpp"
 #include "map_update_module.hpp"
 #include "ndt_omp/multigrid_ndt_omp.h"

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::DiagnosticsModule;
 
 tier4_debug_msgs::msg::Float32Stamped make_float32_stamped(
   const builtin_interfaces::msg::Time & stamp, const float data)

localization/autoware_pose_initializer/package.xml

@@ -21,7 +21,6 @@
 
   <depend>autoware_component_interface_specs</depend>
   <depend>autoware_component_interface_utils</depend>
-  <depend>autoware_localization_util</depend>
   <depend>autoware_map_height_fitter</depend>
   <depend>autoware_motion_utils</depend>
   <depend>autoware_universe_utils</depend>

localization/autoware_pose_initializer/src/pose_initializer_core.cpp

@@ -40,8 +40,8 @@ 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<autoware::localization_util::DiagnosticsModule>(
-    this, "pose_initializer_status");
+  diagnostics_pose_reliable_ =
+    std::make_unique<autoware::universe_utils::DiagnosticsModule>(this, "pose_initializer_status");
 
   if (declare_parameter<bool>("ekf_enabled")) {
     ekf_localization_trigger_ = std::make_unique<EkfLocalizationTriggerModule>(this);