From 0581d923d72f40a433eea8d2b2aab10bed56a81b Mon Sep 17 00:00:00 2001
From: James Pace <jpace121@gmail.com>
Date: Sat, 13 Jun 2026 13:33:35 -0400
Subject: [PATCH] Add diagnostic pub.

---
 CMakeLists.txt              | 22 +++++++++++-
 package.xml                 |  1 +
 src/DiagnosticPublisher.cpp | 67 +++++++++++++++++++++++++++++++++++++
 3 files changed, 89 insertions(+), 1 deletion(-)
 create mode 100644 src/DiagnosticPublisher.cpp

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 1d65ef5..c1a44e5 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -11,8 +11,10 @@ find_package(rclcpp REQUIRED)
 find_package(rclcpp_components REQUIRED)
 find_package(image_transport REQUIRED)
 find_package(sensor_msgs REQUIRED)
+find_package(diagnostic_msgs REQUIRED)
 find_package(OpenCV REQUIRED)
 find_package(cv_bridge REQUIRED)
+find_package(fmt REQUIRED)
 
 add_library(image_publisher_component SHARED src/ImagePublisher.cpp)
 target_include_directories(image_publisher_component PUBLIC
@@ -51,8 +53,26 @@ rclcpp_components_register_node(position_publisher_component
     EXECUTABLE position_publisher
 )
 
+add_library(diagnostic_publisher_component SHARED src/DiagnosticPublisher.cpp)
+target_include_directories(diagnostic_publisher_component PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include/${PROJECT_NAME}>)
+target_compile_features(diagnostic_publisher_component PUBLIC c_std_99 cxx_std_17)  # Require C99 and C++17
+target_link_libraries(diagnostic_publisher_component
+    rclcpp::rclcpp
+    rclcpp_components::component
+    fmt
+    ${diagnostic_msgs_TARGETS}
+    ${std_msgs_TARGETS}
+)
+
+rclcpp_components_register_node(diagnostic_publisher_component
+    PLUGIN "j7s::DiagnosticPublisher"
+    EXECUTABLE diagnostic_publisher
+)
+
 ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET)
-install(TARGETS image_publisher_component position_publisher_component
+install(TARGETS image_publisher_component position_publisher_component diagnostic_publisher_component
         EXPORT export_${PROJECT_NAME}
         ARCHIVE DESTINATION lib
         LIBRARY DESTINATION lib
diff --git a/package.xml b/package.xml
index 49db2f5..c900fae 100644
--- a/package.xml
+++ b/package.xml
@@ -10,6 +10,7 @@
   <buildtool_depend>ament_cmake</buildtool_depend>
 
   <depend>cv_bridge</depend>
+  <depend>diagnostic_msgs</depend>
   <depend>image_transport</depend>
   <depend>sensor_msgs</depend>
   <depend>std_msgs</depend>
diff --git a/src/DiagnosticPublisher.cpp b/src/DiagnosticPublisher.cpp
new file mode 100644
index 0000000..4de719f
--- /dev/null
+++ b/src/DiagnosticPublisher.cpp
@@ -0,0 +1,67 @@
+#include <rclcpp/rclcpp.hpp>
+#include <diagnostic_msgs/msg/diagnostic_array.hpp>
+#include <diagnostic_msgs/msg/diagnostic_status.hpp>
+#include <fmt/core.h>
+
+#include <chrono>
+
+using namespace std::chrono_literals;
+
+namespace j7s {
+    class DiagnosticPublisher : public rclcpp::Node {
+    public:
+        DiagnosticPublisher(const rclcpp::NodeOptions& options);
+    private:
+        uint8_t statusFromString(const std::string& string) const;
+        std::shared_ptr<rclcpp::Publisher<diagnostic_msgs::msg::DiagnosticArray>> m_publisher;
+        rclcpp::TimerBase::SharedPtr m_timer;
+
+        const std::string m_diagLevel;
+        const std::string m_nodeName;
+    };
+};
+
+namespace j7s {
+    DiagnosticPublisher::DiagnosticPublisher(const rclcpp::NodeOptions& options):
+        Node("diagnostic_publisher", options),
+        m_publisher(this->create_publisher<diagnostic_msgs::msg::DiagnosticArray>("/diagnostics", 1)),
+        m_timer{},
+        m_diagLevel{this->declare_parameter<std::string>("diag_level")},
+        m_nodeName{this->declare_parameter<std::string>("node_name")}
+    {
+        m_timer = this->create_timer(10ms, [this]() -> void {
+            diagnostic_msgs::msg::DiagnosticArray msg;
+            msg.header.stamp = this->now();
+
+            diagnostic_msgs::msg::DiagnosticStatus statusMsg;
+            statusMsg.level = statusFromString(m_diagLevel);
+            statusMsg.name = fmt::format("robot/{}/test status", m_nodeName);
+            statusMsg.message = "am ok";
+            statusMsg.hardware_id = "test_robot";
+            diagnostic_msgs::msg::KeyValue keyValue;
+            keyValue.key = "stat";
+            keyValue.value = "0.1";
+            statusMsg.values.push_back(keyValue);
+
+            msg.status.push_back(statusMsg);
+
+            m_publisher->publish(msg);
+        });
+    }
+
+    uint8_t DiagnosticPublisher::statusFromString(const std::string& string) const
+    {
+        if(string == "OK")
+        {
+            return diagnostic_msgs::msg::DiagnosticStatus::OK;
+        }
+        else if(string == "WARN")
+        {
+            return diagnostic_msgs::msg::DiagnosticStatus::WARN;
+        }
+        return diagnostic_msgs::msg::DiagnosticStatus::ERROR;
+    }
+};
+
+#include <rclcpp_components/register_node_macro.hpp>
+RCLCPP_COMPONENTS_REGISTER_NODE(j7s::DiagnosticPublisher)