From d205f8a8907f06955ba5fe258743b59a462dea18 Mon Sep 17 00:00:00 2001
From: Tatsuro Sakaguchi <tatsuro.sakaguchi@g.softbank.co.jp>
Date: Wed, 25 Dec 2024 19:27:57 +0900
Subject: [PATCH] Publish diagnostic

---
 audio_capture/CMakeLists.txt             |  2 ++
 audio_capture/package.xml                |  2 ++
 audio_capture/src/audio_capture_node.cpp | 29 ++++++++++++++++++++----
 3 files changed, 29 insertions(+), 4 deletions(-)

diff --git a/audio_capture/CMakeLists.txt b/audio_capture/CMakeLists.txt
index d9dbbd81..3e880d32 100644
--- a/audio_capture/CMakeLists.txt
+++ b/audio_capture/CMakeLists.txt
@@ -13,6 +13,7 @@ find_package(ament_cmake REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(rclcpp_components REQUIRED)
 find_package(audio_common_msgs REQUIRED)
+find_package(diagnostic_updater REQUIRED)
 find_package(PkgConfig)
 pkg_check_modules(GST1.0 gstreamer-1.0 REQUIRED)
 find_package(Boost REQUIRED COMPONENTS thread)
@@ -26,6 +27,7 @@ ament_target_dependencies(${PROJECT_NAME}
   rclcpp
   rclcpp_components
   audio_common_msgs
+  diagnostic_updater
 )
 
 target_link_libraries(${PROJECT_NAME} ${GST1.0_LIBRARIES} ${Boost_LIBRARIES})
diff --git a/audio_capture/package.xml b/audio_capture/package.xml
index 26b96200..69c44f4a 100644
--- a/audio_capture/package.xml
+++ b/audio_capture/package.xml
@@ -22,6 +22,7 @@
 
    <build_depend>audio_common_msgs</build_depend>
    <build_depend>boost</build_depend>
+   <build_depend>diagnostic_updater</build_depend>
    <build_depend>rclcpp</build_depend>
    <build_depend>rclcpp_components</build_depend>
 
@@ -29,6 +30,7 @@
    <build_depend>libgstreamer-plugins-base1.0-dev</build_depend>
 
    <exec_depend>audio_common_msgs</exec_depend>
+   <exec_depend>diagnostic_updater</exec_depend>
    <exec_depend>launch_xml</exec_depend>
    <exec_depend>rclcpp</exec_depend>
    <exec_depend>rclcpp_components</exec_depend>
diff --git a/audio_capture/src/audio_capture_node.cpp b/audio_capture/src/audio_capture_node.cpp
index d546380c..e1661bda 100644
--- a/audio_capture/src/audio_capture_node.cpp
+++ b/audio_capture/src/audio_capture_node.cpp
@@ -3,6 +3,8 @@
 #include <gst/app/gstappsink.h>
 #include <boost/thread.hpp>
 
+#include <diagnostic_updater/diagnostic_updater.hpp>
+#include <diagnostic_updater/publisher.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <rclcpp_components/register_node_macro.hpp>
 
@@ -17,7 +19,8 @@ namespace audio_capture
     public:
       AudioCaptureNode(const rclcpp::NodeOptions &options)
       :
-       Node("audio_capture_node", options)
+       Node("audio_capture_node", options),
+       updater_(this)
       {
         gst_init(nullptr, nullptr);
 
@@ -55,9 +58,23 @@ namespace audio_capture
         this->get_parameter("device", device);
 
         _pub = this->create_publisher<audio_common_msgs::msg::AudioData>("audio", 10);
-        _pub_stamped = this->create_publisher<audio_common_msgs::msg::AudioDataStamped>("audio_stamped", 10);
         _pub_info = this->create_publisher<audio_common_msgs::msg::AudioInfo>("audio_info", 1);
 
+        rclcpp::Publisher<audio_common_msgs::msg::AudioDataStamped>::SharedPtr pub_stamped =
+          this->create_publisher<audio_common_msgs::msg::AudioDataStamped>("audio_stamped", 10);
+
+        this->declare_parameter<double>("desired_rate", 100.0);
+        _desired_rate = this->get_parameter("desired_rate").as_double();
+        this->declare_parameter<double>("diagnostic_tolerance", 0.1);
+        auto tolerance = this->get_parameter("diagnostic_tolerance").as_double();
+
+        updater_.setHardwareID("microphone");
+        _diagnosed_pub_stamped =
+          std::make_shared<diagnostic_updater::DiagnosedPublisher<audio_common_msgs::msg::AudioDataStamped>>(
+            pub_stamped, updater_,
+            diagnostic_updater::FrequencyStatusParam(&_desired_rate, &_desired_rate, tolerance, 10),
+            diagnostic_updater::TimeStampStatusParam());
+
         _loop = g_main_loop_new(NULL, false);
         _pipeline = gst_pipeline_new("ros_pipeline");
         GstClock *clock = gst_system_clock_obtain();
@@ -201,7 +218,7 @@ namespace audio_capture
 
       void publishStamped( const audio_common_msgs::msg::AudioDataStamped &msg )
       {
-        _pub_stamped->publish(msg);
+        _diagnosed_pub_stamped->publish(msg);
       }
 
       static GstFlowReturn onNewBuffer (GstAppSink *appsink, gpointer userData)
@@ -252,7 +269,6 @@ namespace audio_capture
 
     private:
       rclcpp::Publisher<audio_common_msgs::msg::AudioData>::SharedPtr _pub;
-      rclcpp::Publisher<audio_common_msgs::msg::AudioDataStamped>::SharedPtr _pub_stamped;
       rclcpp::Publisher<audio_common_msgs::msg::AudioInfo>::SharedPtr _pub_info;
 
       rclcpp::TimerBase::SharedPtr _timer_info;
@@ -264,6 +280,11 @@ namespace audio_capture
       int _bitrate, _channels, _depth, _sample_rate;
       GMainLoop *_loop;
       std::string _format, _sample_format;
+
+      diagnostic_updater::Updater updater_;
+      double _desired_rate;
+      std::shared_ptr<diagnostic_updater::DiagnosedPublisher<audio_common_msgs::msg::AudioDataStamped>>
+        _diagnosed_pub_stamped;
   };
 }