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_interface.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_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 <rclcpp/rclcpp.hpp>
 
@@ -22,16 +22,18 @@
 #include <string>
 #include <vector>
 
-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 <typename T>
   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,19 +48,14 @@ class DiagnosticsModule
 };
 
 template <typename T>
-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;
   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::universe_utils
 
-}  // namespace autoware::localization_util
-
-#endif  // AUTOWARE__LOCALIZATION_UTIL__DIAGNOSTICS_MODULE_HPP_
+#endif  // AUTOWARE__UNIVERSE_UTILS__ROS__DIAGNOSTICS_INTERFACE_HPP_

localization/autoware_localization_util/src/diagnostics_module.cpp → 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 <rclcpp/rclcpp.hpp>
 
@@ -21,9 +21,9 @@
 #include <algorithm>
 #include <string>
 
-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,25 +56,23 @@ 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;
   key_value.value = value;
   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;
   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)
+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

common/autoware_universe_utils/test/src/ros/test_diagnostics_interface.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_interface.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::DiagnosticInterface;
+
+class TestDiagnosticInterface : public ::testing::Test
+{
+protected:
+  void SetUp() override
+  {
+    // Create a test node
+    node_ = std::make_shared<rclcpp::Node>("test_diagnostics_interface");
+  }
+
+  // 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(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<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(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<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(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<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::DiagnosticInterface>(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_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<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::DiagnosticInterface> diagnostics_;
 };
 
 }  // namespace autoware::gyro_odometer