Update dependencies and add diagnostic updater to OBCameraNode

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

@@ -62,6 +62,9 @@
 #include "magic_enum/magic_enum.hpp"
 #include "jpeg_decoder.h"
 
+#include <diagnostic_updater/diagnostic_updater.hpp>
+#include <diagnostic_msgs/msg/diagnostic_status.hpp>
+
 #define STREAM_NAME(sip)                                                                       \
   (static_cast<std::ostringstream&&>(std::ostringstream()                                      \
                                      << _stream_name[sip.first]                                \
@@ -159,6 +162,10 @@ class OBCameraNode {
 
   void setupTopics();
 
+  void setupDiagnosticUpdater();
+
+  void onTemperatureUpdate(diagnostic_updater::DiagnosticStatusWrapper& status);
+
   void setupPipelineConfig();
 
   void setupCameraCtrlServices();
@@ -455,5 +462,7 @@ class OBCameraNode {
   bool enable_depth_scale_ = true;
   bool is_openni_device_ = false;
   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/gemini2L.launch.py

@@ -85,6 +85,7 @@ def generate_launch_description():
         DeclareLaunchArgument('use_hardware_time', default_value='false'),
         DeclareLaunchArgument('enable_depth_scale', default_value='true'),
         DeclareLaunchArgument('align_mode', default_value='HW'),
+        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

@@ -53,6 +53,7 @@ OBCameraNode::OBCameraNode(rclcpp::Node *node, std::shared_ptr<ob::Device> devic
   compression_params_.push_back(cv::IMWRITE_PNG_STRATEGY_DEFAULT);
   setupDefaultImageFormat();
   setupTopics();
+  setupDiagnosticUpdater();
 #if defined(USE_RK_HW_DECODER)
   jpeg_decoder_ = std::make_unique<RKJPEGDecoder>(width_[COLOR], height_[COLOR]);
 #elif defined(USE_NV_HW_DECODER)
@@ -631,6 +632,40 @@ void OBCameraNode::setupTopics() {
   setupPublishers();
 }
 
+void OBCameraNode::setupDiagnosticUpdater() {
+  if (diagnostic_period_ < 0.0) {
+    return;
+  }
+  diagnostic_updater_ = std::make_unique<diagnostic_updater::Updater>(node_, diagnostic_period_);
+  auto info = device_->getDeviceInfo();
+  CHECK_NOTNULL(info);
+  std::string serial_number = info->serialNumber();
+  diagnostic_updater_->setHardwareID(serial_number);
+  diagnostic_updater_->add("Temperature", this, &OBCameraNode::onTemperatureUpdate);
+}
+
+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::setupPipelineConfig() {
   if (pipeline_config_) {
     pipeline_config_.reset();