Add temperatures diagnostics updater

orbbec_camera/CMakeLists.txt

@@ -39,6 +39,8 @@ set(dependencies
   tf2_ros
   tf2_sensor_msgs
   Threads
+  diagnostic_updater
+  diagnostic_msgs
 )
 
 foreach (dep IN LISTS dependencies)
@@ -233,4 +235,4 @@ ament_export_include_directories(include ${ORBBEC_INCLUDE_DIR})
 ament_export_libraries(${PROJECT_NAME})
 ament_export_dependencies(${dependencies} ${ORBBEC_LIBS})
 
-ament_package()
+ament_package()

orbbec_camera/include/orbbec_camera/ob_camera_node.h

@@ -38,6 +38,7 @@
 #include <tf2/LinearMath/Transform.h>
 #include <std_srvs/srv/set_bool.hpp>
 #include <std_srvs/srv/empty.hpp>
+#include <diagnostic_updater/diagnostic_updater.hpp>
 
 #include <sensor_msgs/msg/camera_info.hpp>
 #include <camera_info_manager/camera_info_manager.hpp>
@@ -165,6 +166,10 @@ class OBCameraNode {
 
   void setupPipelineConfig();
 
+  void setupDiagnosticUpdater();
+
+  void onTemperatureUpdate(diagnostic_updater::DiagnosticStatusWrapper &status);
+
   void setupCameraCtrlServices();
 
   void stopStreams();
@@ -501,5 +506,7 @@ class OBCameraNode {
   rclcpp::Publisher<std_msgs::msg::String>::SharedPtr filter_status_pub_;
   nlohmann::json filter_status_;
   std::string align_mode_ = "HW";
+  std::unique_ptr<diagnostic_updater::Updater> diagnostic_updater_ = nullptr;
+  double diagnostic_period_ = 1.0;
 };
 }  // namespace orbbec_camera

orbbec_camera/launch/gemini2R.launch.py

@@ -109,6 +109,7 @@ def generate_launch_description():
         DeclareLaunchArgument('temporal_filter_weight', default_value='0.4'),
         DeclareLaunchArgument('hole_filling_filter_mode', default_value='FILL_TOP'),
         DeclareLaunchArgument('align_mode', default_value='SW'),
+        DeclareLaunchArgument('diagnostic_period', default_value='1.0'),
     ]
 
     # Node configuration

orbbec_camera/package.xml

@@ -26,6 +26,8 @@
   <depend>tf2_ros</depend>
   <depend>tf2_sensor_msgs</depend>
   <depend>tf2_msgs</depend>
+  <depend>diagnostic_updater</depend>
+  <depend>diagnostic_msgs</depend>
   <export>
     <build_type>ament_cmake</build_type>
   </export>

orbbec_camera/src/ob_camera_node.cpp

@@ -22,6 +22,7 @@
 #include "orbbec_camera/utils.h"
 #include <filesystem>
 #include <fstream>
+#include "diagnostic_msgs/msg/diagnostic_status.hpp"
 
 #if defined(USE_RK_HW_DECODER)
 #include "orbbec_camera/rk_mpp_decoder.h"
@@ -694,6 +695,7 @@ void OBCameraNode::getParameters() {
   setAndGetNodeParameter<std::string>(hole_filling_filter_mode_, "hole_filling_filter_mode",
                                       "FILL_TOP");
   setAndGetNodeParameter<std::string>(align_mode_, "align_mode", "HW");
+  setAndGetNodeParameter<double>(diagnostic_period_, "diagnostic_period", 1.0);
 }
 
 void OBCameraNode::setupTopics() {
@@ -702,6 +704,41 @@ void OBCameraNode::setupTopics() {
   setupProfiles();
   setupCameraCtrlServices();
   setupPublishers();
+  setupDiagnosticUpdater();
+}
+
+void OBCameraNode::onTemperatureUpdate(diagnostic_updater::DiagnosticStatusWrapper &status) {
+  try {
+    OBDeviceTemperature temperature;
+    uint32_t data_size = sizeof(OBDeviceTemperature);
+    device_->getStructuredData(OB_STRUCT_DEVICE_TEMPERATURE, &temperature, &data_size);
+    status.add("CPU Temperature", temperature.cpuTemp);
+    status.add("IR Temperature", temperature.irTemp);
+    status.add("LDM Temperature", temperature.ldmTemp);
+    status.add("MainBoard Temperature", temperature.mainBoardTemp);
+    status.add("TEC Temperature", temperature.tecTemp);
+    status.add("IMU Temperature", temperature.imuTemp);
+    status.add("RGB Temperature", temperature.rgbTemp);
+    status.add("Left IR Temperature", temperature.irLeftTemp);
+    status.add("Right IR Temperature", temperature.irRightTemp);
+    status.add("Chip Top Temperature", temperature.chipTopTemp);
+    status.add("Chip Bottom Temperature", temperature.chipBottomTemp);
+    status.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "Temperature is normal");
+  } catch (const ob::Error &e) {
+    status.summary(diagnostic_msgs::msg::DiagnosticStatus::ERROR, e.getMessage());
+  }
+}
+
+void OBCameraNode::setupDiagnosticUpdater() {
+  if (diagnostic_period_ <= 0.0) {
+    return;
+  }
+  RCLCPP_INFO_STREAM(logger_, "Publish diagnostics every " << diagnostic_period_ << " seconds");
+  auto info = device_->getDeviceInfo();
+  std::string serial_number = info->serialNumber();
+  diagnostic_updater_ = std::make_unique<diagnostic_updater::Updater>(node_, diagnostic_period_);
+  diagnostic_updater_->setHardwareID(serial_number);
+  diagnostic_updater_->add("Temperatures", this, &OBCameraNode::onTemperatureUpdate);
 }
 
 void OBCameraNode::setupPipelineConfig() {