feat: implement stream status tracking and update device status message structure

orbbec_camera/examples/Gemini_435Le_example_node/camera_example_node.cpp

@@ -193,14 +193,12 @@ class CameraExampleNode : public rclcpp::Node {
   void deviceStatusCallback(const orbbec_camera_msgs::msg::DeviceStatus::SharedPtr msg) {
     RCLCPP_INFO(this->get_logger(), "-------orbbec camera real-time status-------");
     RCLCPP_INFO(this->get_logger(), "--------------------------------------------");
-    RCLCPP_INFO(this->get_logger(), "Color Frame Rate: Cur: %.2f, Avg: %.2f",
-                msg->color_frame_rate_cur, msg->color_frame_rate_avg);
-    RCLCPP_INFO(this->get_logger(), "Depth Frame Rate: Cur: %.2f, Avg: %.2f",
-                msg->depth_frame_rate_cur, msg->depth_frame_rate_avg);
-    RCLCPP_INFO(this->get_logger(), "Color Delay (ms): Cur: %.2f, Avg: %.2f",
-                msg->color_delay_ms_cur, msg->color_delay_ms_avg);
-    RCLCPP_INFO(this->get_logger(), "Depth Delay (ms): Cur: %.2f, Avg: %.2f",
-                msg->depth_delay_ms_cur, msg->depth_delay_ms_avg);
+    for (const auto &stream : msg->streams) {
+      RCLCPP_INFO(this->get_logger(),
+                  "Topic: %s, Subscribers: %s, Rate: %.2f Hz, Avg Delay: %.2f ms",
+                  stream.topic_name.c_str(), stream.has_subscribers ? "true" : "false",
+                  stream.publish_rate_hz, stream.delay_ms_avg);
+    }
 
     RCLCPP_INFO(this->get_logger(), "Device Online: %s", msg->device_online ? "True" : "False");
     RCLCPP_INFO(this->get_logger(), "Connection Type: %s", msg->connection_type.c_str());

orbbec_camera/include/orbbec_camera/fps_delay_status.hpp

@@ -1,139 +0,0 @@
-/*******************************************************************************
- * Copyright (c) 2023 Orbbec 3D Technology, Inc
- *
- * 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.
- *******************************************************************************/
-
-#pragma once
-
-#include <rclcpp/rclcpp.hpp>
-#include "orbbec_camera_msgs/msg/device_status.hpp"
-#include <chrono>
-#include <functional>
-#include <limits>
-#include <mutex>
-namespace orbbec_camera {
-class FpsDelayStatus {
- public:
-  explicit FpsDelayStatus(rclcpp::Logger logger) : log_level_(LogLevel::INFO), logger_(logger) {}
-
-  void tick(u_int64_t stream_timestamp) {
-    std::lock_guard<std::mutex> lock(mutex_);
-
-    double dt = (stream_timestamp - last_stream_timestamp_) / 1000000.0;
-    double fps = (dt > 0) ? (1.0 / dt) : 0.0;
-
-    // Convert now to milliseconds since steady_clock epoch
-    auto now2 = std::chrono::system_clock::now();
-    uint64_t ms_since_epoch =
-        std::chrono::duration_cast<std::chrono::milliseconds>(now2.time_since_epoch()).count();
-    double delay_ms =
-        static_cast<double>(ms_since_epoch) - static_cast<double>(stream_timestamp / 1000.0);
-
-    frame_count_++;
-    fps_sum_ += fps;
-    delay_sum_ += delay_ms;
-
-    last_fps_ = fps;
-    last_delay_ms_ = delay_ms;
-    last_stream_timestamp_ = stream_timestamp;
-
-    if (fps_max_ <= 0) fps_max_ = fps;
-    if (fps_min_ <= 0) fps_min_ = fps;
-    fps_max_ = std::max(fps_max_, fps);
-    fps_min_ = std::min(fps_min_, fps);
-    if (delay_max_ <= 0) delay_max_ = delay_ms;
-    if (delay_min_ <= 0) delay_min_ = delay_ms;
-    delay_max_ = std::max(delay_max_, delay_ms);
-    delay_min_ = std::min(delay_min_, delay_ms);
-  }
-
-  void fillColorStatus(orbbec_camera_msgs::msg::DeviceStatus &msg) {
-    fillStatus(msg.color_frame_rate_cur, msg.color_frame_rate_avg, msg.color_frame_rate_min,
-               msg.color_frame_rate_max, msg.color_delay_ms_cur, msg.color_delay_ms_avg,
-               msg.color_delay_ms_min, msg.color_delay_ms_max);
-  }
-
-  void fillLeftColorStatus(orbbec_camera_msgs::msg::DeviceStatus &msg) {
-    fillStatus(msg.left_color_frame_rate_cur, msg.left_color_frame_rate_avg,
-               msg.left_color_frame_rate_min, msg.left_color_frame_rate_max,
-               msg.left_color_delay_ms_cur, msg.left_color_delay_ms_avg,
-               msg.left_color_delay_ms_min, msg.left_color_delay_ms_max);
-  }
-
-  void fillRightColorStatus(orbbec_camera_msgs::msg::DeviceStatus &msg) {
-    fillStatus(msg.right_color_frame_rate_cur, msg.right_color_frame_rate_avg,
-               msg.right_color_frame_rate_min, msg.right_color_frame_rate_max,
-               msg.right_color_delay_ms_cur, msg.right_color_delay_ms_avg,
-               msg.right_color_delay_ms_min, msg.right_color_delay_ms_max);
-  }
-
-  void fillDepthStatus(orbbec_camera_msgs::msg::DeviceStatus &msg) {
-    fillStatus(msg.depth_frame_rate_cur, msg.depth_frame_rate_avg, msg.depth_frame_rate_min,
-               msg.depth_frame_rate_max, msg.depth_delay_ms_cur, msg.depth_delay_ms_avg,
-               msg.depth_delay_ms_min, msg.depth_delay_ms_max);
-  }
-
-  void fillLeftIrStatus(orbbec_camera_msgs::msg::DeviceStatus &msg) {
-    fillStatus(msg.left_ir_frame_rate_cur, msg.left_ir_frame_rate_avg, msg.left_ir_frame_rate_min,
-               msg.left_ir_frame_rate_max, msg.left_ir_delay_ms_cur, msg.left_ir_delay_ms_avg,
-               msg.left_ir_delay_ms_min, msg.left_ir_delay_ms_max);
-  }
-
-  void fillRightIrStatus(orbbec_camera_msgs::msg::DeviceStatus &msg) {
-    fillStatus(msg.right_ir_frame_rate_cur, msg.right_ir_frame_rate_avg,
-               msg.right_ir_frame_rate_min, msg.right_ir_frame_rate_max, msg.right_ir_delay_ms_cur,
-               msg.right_ir_delay_ms_avg, msg.right_ir_delay_ms_min, msg.right_ir_delay_ms_max);
-  }
-
- private:
-  void fillStatus(double &frame_rate_cur, double &frame_rate_avg, double &frame_rate_min,
-                  double &frame_rate_max, double &delay_ms_cur, double &delay_ms_avg,
-                  double &delay_ms_min, double &delay_ms_max) {
-    std::lock_guard<std::mutex> lock(mutex_);
-    frame_rate_cur = last_fps_;
-    frame_rate_avg = frame_count_ > 0 ? fps_sum_ / frame_count_ : 0;
-    frame_rate_min = frame_count_ > 0 ? fps_min_ : 0;
-    frame_rate_max = frame_count_ > 0 ? fps_max_ : 0;
-
-    delay_ms_cur = last_delay_ms_;
-    delay_ms_avg = frame_count_ > 0 ? delay_sum_ / frame_count_ : 0;
-    delay_ms_min = frame_count_ > 0 ? delay_min_ : 0;
-    delay_ms_max = frame_count_ > 0 ? delay_max_ : 0;
-
-    last_delay_ms_ = 0.0;
-    last_fps_ = 0.0;
-    frame_count_ = 0;
-    fps_sum_ = delay_sum_ = 0.0;
-    fps_max_ = delay_max_ = 0.0;
-    fps_min_ = delay_min_ = 0.0;
-  }
-
-  mutable std::mutex mutex_;
-  u_int64_t last_stream_timestamp_{0};
-  double last_delay_ms_{0.0};
-  double last_fps_{0.0};
-
-  int frame_count_{0};
-  double fps_sum_{0.0};
-  double delay_sum_{0.0};
-  double fps_max_{std::numeric_limits<double>::lowest()};
-  double fps_min_{std::numeric_limits<double>::max()};
-  double delay_max_{std::numeric_limits<double>::lowest()};
-  double delay_min_{std::numeric_limits<double>::max()};
-
-  LogLevel log_level_;
-  rclcpp::Logger logger_;
-};
-
-}  // namespace orbbec_camera

orbbec_camera/include/orbbec_camera/ob_camera_node.h

@@ -50,6 +50,7 @@
 #include "libobsensor/ObSensor.hpp"
 
 #include "orbbec_camera_msgs/msg/device_info.hpp"
+#include "orbbec_camera_msgs/msg/device_status.hpp"
 #include "orbbec_camera_msgs/msg/depth_filter_state.hpp"
 #include "orbbec_camera_msgs/msg/depth_filters_status.hpp"
 #include "orbbec_camera_msgs/srv/get_device_config.hpp"
@@ -76,7 +77,7 @@
 #include "orbbec_camera/d2c_viewer.h"
 #include "orbbec_camera/image_publisher.h"
 #include "orbbec_camera/fps_counter.hpp"
-#include "orbbec_camera/fps_delay_status.hpp"
+#include "orbbec_camera/stream_status.hpp"
 #include "orbbec_camera/timestamp_csv_logger.h"
 #include "jpeg_decoder.h"
 #include <std_msgs/msg/header.hpp>
@@ -235,17 +236,7 @@ class OBCameraNode {
     return (color_info_manager_ && color_info_manager_->isCalibrated() && ir_info_manager_ &&
             ir_info_manager_->isCalibrated());
   }
-  void getColorStatus(orbbec_camera_msgs::msg::DeviceStatus& status_msg) {
-    fps_delay_status_color_->fillColorStatus(status_msg);
-    fps_delay_status_left_color_->fillLeftColorStatus(status_msg);
-    fps_delay_status_right_color_->fillRightColorStatus(status_msg);
-  }
-
-  void getDepthStatus(orbbec_camera_msgs::msg::DeviceStatus& status_msg) {
-    fps_delay_status_depth_->fillDepthStatus(status_msg);
-    fps_delay_status_left_ir_->fillLeftIrStatus(status_msg);
-    fps_delay_status_right_ir_->fillRightIrStatus(status_msg);
-  }
+  void fillStreamStatus(orbbec_camera_msgs::msg::DeviceStatus& status_msg);
 
   bool checkUserCalibrationReady() {
     static bool first_check = true;
@@ -363,6 +354,14 @@ class OBCameraNode {
 
   void setupPublishers();
 
+  void registerStreamStatus(const std::string& topic_name,
+                            StreamStatusTracker::SubscriberCountFn subscriber_count);
+  void removeStreamStatus(const std::string& topic_name);
+  void recordStreamStatus(const std::string& topic_name,
+                          const builtin_interfaces::msg::Time& stamp);
+  std::string resolveStreamStatusTopic(const std::string& topic_name) const;
+  std::string compressedStreamStatusTopic(const stream_index_pair& stream_index) const;
+
   void syncSoftwareAlignment();
 
   void publishDepthFiltersStatus();
@@ -1199,12 +1198,8 @@ class OBCameraNode {
   std::unique_ptr<FpsCounter> fps_counter_left_ir_{nullptr};
   std::unique_ptr<FpsCounter> fps_counter_right_ir_{nullptr};
 
-  std::unique_ptr<FpsDelayStatus> fps_delay_status_color_{nullptr};
-  std::unique_ptr<FpsDelayStatus> fps_delay_status_left_color_{nullptr};
-  std::unique_ptr<FpsDelayStatus> fps_delay_status_right_color_{nullptr};
-  std::unique_ptr<FpsDelayStatus> fps_delay_status_depth_{nullptr};
-  std::unique_ptr<FpsDelayStatus> fps_delay_status_left_ir_{nullptr};
-  std::unique_ptr<FpsDelayStatus> fps_delay_status_right_ir_{nullptr};
+  std::map<std::string, std::shared_ptr<StreamStatusTracker>> stream_status_trackers_;
+  mutable std::mutex stream_status_mutex_;
 
   std::string intra_camera_sync_reference_ = "";
   std::string ae_reference_stream_;

orbbec_camera/include/orbbec_camera/ob_camera_node_driver.h

@@ -157,7 +157,6 @@ class OBCameraNodeDriver : public rclcpp::Node {
   std::atomic<bool> is_reupdating_{false};  // Flag to track if we're in reupdate process
   std::atomic<bool> delay_stream_start_after_reconnect_{false};
   rclcpp::TimerBase::SharedPtr device_status_timer_ = nullptr;
-  int device_status_interval_hz = 2;  // 2Hz
   rclcpp::Publisher<orbbec_camera_msgs::msg::DeviceStatus>::SharedPtr device_status_pub_ = nullptr;
   std::string node_name_;
   bool force_ip_enable_{false};

orbbec_camera/include/orbbec_camera/stream_status.hpp

@@ -0,0 +1,80 @@
+/*******************************************************************************
+ * Copyright (c) 2023 Orbbec 3D Technology, Inc
+ *
+ * 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.
+ *******************************************************************************/
+
+#pragma once
+
+#include <builtin_interfaces/msg/time.hpp>
+
+#include <chrono>
+#include <cstddef>
+#include <cstdint>
+#include <functional>
+#include <mutex>
+#include <string>
+#include <utility>
+
+#include "orbbec_camera_msgs/msg/stream_status.hpp"
+
+namespace orbbec_camera {
+
+class StreamStatusTracker {
+ public:
+  using SubscriberCountFn = std::function<size_t()>;
+
+  StreamStatusTracker(std::string topic_name, SubscriberCountFn subscriber_count)
+      : topic_name_(std::move(topic_name)),
+        subscriber_count_(std::move(subscriber_count)),
+        window_start_(std::chrono::steady_clock::now()) {}
+
+  void record(const builtin_interfaces::msg::Time& stamp) {
+    const auto now = std::chrono::system_clock::now();
+    const double now_ms = std::chrono::duration<double, std::milli>(now.time_since_epoch()).count();
+    const double stamp_ms =
+        static_cast<double>(stamp.sec) * 1000.0 + static_cast<double>(stamp.nanosec) / 1000000.0;
+
+    std::lock_guard<std::mutex> lock(mutex_);
+    published_count_++;
+    delay_sum_ms_ += now_ms - stamp_ms;
+  }
+
+  void fill(orbbec_camera_msgs::msg::StreamStatus& status) {
+    const bool has_subscribers = subscriber_count_ && subscriber_count_() > 0;
+    const auto now = std::chrono::steady_clock::now();
+
+    std::lock_guard<std::mutex> lock(mutex_);
+    const double window_seconds = std::chrono::duration<double>(now - window_start_).count();
+    status.topic_name = topic_name_;
+    status.has_subscribers = has_subscribers;
+    status.publish_rate_hz =
+        window_seconds > 0.0 ? static_cast<double>(published_count_) / window_seconds : 0.0;
+    status.delay_ms_avg =
+        published_count_ > 0 ? delay_sum_ms_ / static_cast<double>(published_count_) : 0.0;
+
+    published_count_ = 0;
+    delay_sum_ms_ = 0.0;
+    window_start_ = now;
+  }
+
+ private:
+  std::string topic_name_;
+  SubscriberCountFn subscriber_count_;
+  std::chrono::steady_clock::time_point window_start_;
+  std::mutex mutex_;
+  uint32_t published_count_{0};
+  double delay_sum_ms_{0.0};
+};
+
+}  // namespace orbbec_camera

orbbec_camera/src/ob_camera_node.cpp

@@ -783,13 +783,6 @@ OBCameraNode::OBCameraNode(rclcpp::Node *node, std::shared_ptr<ob::Device> devic
   fps_counter_depth_->setLogLevel(log_level);
   fps_counter_left_ir_->setLogLevel(log_level);
   fps_counter_right_ir_->setLogLevel(log_level);
-
-  fps_delay_status_color_ = std::make_unique<FpsDelayStatus>(logger_);
-  fps_delay_status_left_color_ = std::make_unique<FpsDelayStatus>(logger_);
-  fps_delay_status_right_color_ = std::make_unique<FpsDelayStatus>(logger_);
-  fps_delay_status_depth_ = std::make_unique<FpsDelayStatus>(logger_);
-  fps_delay_status_left_ir_ = std::make_unique<FpsDelayStatus>(logger_);
-  fps_delay_status_right_ir_ = std::make_unique<FpsDelayStatus>(logger_);
 }
 
 template <class T>
@@ -4345,6 +4338,7 @@ void OBCameraNode::startStreams() {
                         "Failed to start pipeline: " << orbbec_camera::formatObErrorWithStatus(e));
     RCLCPP_INFO_STREAM(logger_, "try to disable ir stream and try again");
     enable_stream_[INFRA0] = false;
+    setupImagePublisher(INFRA0);
     setupPipelineConfig();
     pipeline_->start(pipeline_config_, [this](const std::shared_ptr<ob::FrameSet> &frame_set) {
       onNewFrameSetCallback(frame_set);
@@ -5727,12 +5721,71 @@ void OBCameraNode::setupCameraInfo() {
   }
 }
 
+std::string OBCameraNode::resolveStreamStatusTopic(const std::string &topic_name) const {
+  return node_->get_node_topics_interface()->resolve_topic_name(topic_name);
+}
+
+std::string OBCameraNode::compressedStreamStatusTopic(const stream_index_pair &stream_index) const {
+  const std::string topic = stream_name_.at(stream_index) + "/image_raw";
+  return topic + (stream_index == DEPTH ? "/compressedDepth" : "/compressed");
+}
+
+void OBCameraNode::registerStreamStatus(const std::string &topic_name,
+                                        StreamStatusTracker::SubscriberCountFn subscriber_count) {
+  const auto resolved_topic_name = resolveStreamStatusTopic(topic_name);
+  auto tracker =
+      std::make_shared<StreamStatusTracker>(resolved_topic_name, std::move(subscriber_count));
+  std::lock_guard<std::mutex> lock(stream_status_mutex_);
+  stream_status_trackers_[topic_name] = std::move(tracker);
+}
+
+void OBCameraNode::removeStreamStatus(const std::string &topic_name) {
+  std::lock_guard<std::mutex> lock(stream_status_mutex_);
+  stream_status_trackers_.erase(topic_name);
+}
+
+void OBCameraNode::recordStreamStatus(const std::string &topic_name,
+                                      const builtin_interfaces::msg::Time &stamp) {
+  std::shared_ptr<StreamStatusTracker> tracker;
+  {
+    std::lock_guard<std::mutex> lock(stream_status_mutex_);
+    const auto iter = stream_status_trackers_.find(topic_name);
+    if (iter == stream_status_trackers_.end()) {
+      return;
+    }
+    tracker = iter->second;
+  }
+  tracker->record(stamp);
+}
+
+void OBCameraNode::fillStreamStatus(orbbec_camera_msgs::msg::DeviceStatus &status_msg) {
+  std::vector<std::shared_ptr<StreamStatusTracker>> trackers;
+  {
+    std::lock_guard<std::mutex> lock(stream_status_mutex_);
+    trackers.reserve(stream_status_trackers_.size());
+    for (const auto &[topic_name, tracker] : stream_status_trackers_) {
+      (void)topic_name;
+      trackers.push_back(tracker);
+    }
+  }
+
+  status_msg.streams.clear();
+  status_msg.streams.reserve(trackers.size());
+  for (const auto &tracker : trackers) {
+    status_msg.streams.emplace_back();
+    tracker->fill(status_msg.streams.back());
+  }
+}
+
 void OBCameraNode::setupImagePublisher(const stream_index_pair &stream_index) {
   const std::string topic = stream_name_[stream_index] + "/image_raw";
   if (!enable_stream_[stream_index]) {
     releaseGlobalImageTransportPublisher(*node_, topic);
     image_publishers_.erase(stream_index);
     compressed_image_publishers_.erase(stream_index);
+    removeStreamStatus(topic);
+    removeStreamStatus(topic + "/compressed");
+    removeStreamStatus(compressedStreamStatusTopic(stream_index));
     return;
   }
 
@@ -5740,6 +5793,7 @@ void OBCameraNode::setupImagePublisher(const stream_index_pair &stream_index) {
   const bool is_mjpg_color_stream =
       (stream_index == COLOR || stream_index == COLOR_LEFT || stream_index == COLOR_RIGHT) &&
       format_[stream_index] == OB_FORMAT_MJPG;
+  const bool uses_image_transport = !use_intra_process_ && !is_mjpg_color_stream;
   if (use_intra_process_ || is_mjpg_color_stream) {
     releaseGlobalImageTransportPublisher(*node_, topic);
     image_publishers_[stream_index] =
@@ -5750,13 +5804,33 @@ void OBCameraNode::setupImagePublisher(const stream_index_pair &stream_index) {
   }
   RCLCPP_INFO_STREAM(logger_, topic << " QoS: " << getRMWQosProfileDescription(image_qos_profile));
 
+  const auto image_publisher = image_publishers_.at(stream_index);
+  if (uses_image_transport) {
+    registerStreamStatus(topic, [this, topic]() { return node_->count_subscribers(topic); });
+  } else {
+    registerStreamStatus(topic, [image_publisher]() {
+      return image_publisher ? image_publisher->get_subscription_count() : 0;
+    });
+  }
+
   if (is_mjpg_color_stream) {
     compressed_image_publishers_[stream_index] =
         node_->create_publisher<sensor_msgs::msg::CompressedImage>(
             topic + "/compressed",
             rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(image_qos_profile), image_qos_profile));
+    const auto compressed_publisher = compressed_image_publishers_.at(stream_index);
+    registerStreamStatus(topic + "/compressed", [compressed_publisher]() {
+      return compressed_publisher ? compressed_publisher->get_subscription_count() : 0;
+    });
   } else {
     compressed_image_publishers_.erase(stream_index);
+    removeStreamStatus(compressedStreamStatusTopic(stream_index));
+    if (uses_image_transport) {
+      const auto compressed_topic = compressedStreamStatusTopic(stream_index);
+      registerStreamStatus(compressed_topic, [this, compressed_topic]() {
+        return node_->count_subscribers(compressed_topic);
+      });
+    }
   }
 }
 
@@ -5790,11 +5864,23 @@ void OBCameraNode::setupPublishers() {
         "depth_registered/points",
         rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(point_cloud_qos_profile),
                     point_cloud_qos_profile));
+    const auto publisher = depth_registration_cloud_pub_;
+    registerStreamStatus("depth_registered/points", [publisher]() {
+      return publisher ? publisher->get_subscription_count() : 0;
+    });
+  } else {
+    removeStreamStatus("depth_registered/points");
   }
   if (enable_point_cloud_) {
     depth_cloud_pub_ = node_->create_publisher<PointCloud2>(
         "depth/points", rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(point_cloud_qos_profile),
                                     point_cloud_qos_profile));
+    const auto publisher = depth_cloud_pub_;
+    registerStreamStatus("depth/points", [publisher]() {
+      return publisher ? publisher->get_subscription_count() : 0;
+    });
+  } else {
+    removeStreamStatus("depth/points");
   }
   auto device_info = device_->getDeviceInfo();
   CHECK_NOTNULL(device_info.get());
@@ -5839,6 +5925,9 @@ void OBCameraNode::setupPublishers() {
     }
     imu_gyro_accel_publisher_ = node_->create_publisher<sensor_msgs::msg::Imu>(
         topic_name, rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(data_qos), data_qos));
+    const auto publisher = imu_gyro_accel_publisher_;
+    registerStreamStatus(
+        topic_name, [publisher]() { return publisher ? publisher->get_subscription_count() : 0; });
     topic_name = stream_name_[GYRO] + "/imu_info";
     imu_info_publishers_[GYRO] = node_->create_publisher<orbbec_camera_msgs::msg::IMUInfo>(
         topic_name, rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(data_qos), data_qos));
@@ -5857,6 +5946,10 @@ void OBCameraNode::setupPublishers() {
       }
       imu_publishers_[stream_index] = node_->create_publisher<sensor_msgs::msg::Imu>(
           data_topic_name, rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(data_qos), data_qos));
+      const auto publisher = imu_publishers_.at(stream_index);
+      registerStreamStatus(data_topic_name, [publisher]() {
+        return publisher ? publisher->get_subscription_count() : 0;
+      });
       data_topic_name = stream_name_[stream_index] + "/imu_info";
       imu_info_publishers_[stream_index] =
           node_->create_publisher<orbbec_camera_msgs::msg::IMUInfo>(
@@ -6201,6 +6294,7 @@ void OBCameraNode::publishDepthPointCloud(const std::shared_ptr<ob::FrameSet> &f
       RCLCPP_ERROR_STREAM(logger_, "Failed to save point cloud: " << e.what());
     }
   }
+  recordStreamStatus("depth/points", point_cloud_msg->header.stamp);
   depth_cloud_pub_->publish(std::move(point_cloud_msg));
 }
 
@@ -6337,6 +6431,7 @@ void OBCameraNode::publishColoredPointCloud(const std::shared_ptr<ob::FrameSet>
     }
   }
 
+  recordStreamStatus("depth_registered/points", point_cloud_msg->header.stamp);
   depth_registration_cloud_pub_->publish(std::move(point_cloud_msg));
 }
 std::shared_ptr<ob::Frame> OBCameraNode::processIrFrameFilter(std::shared_ptr<ob::Frame> &frame) {
@@ -7164,9 +7259,17 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame> &frame,
     }
   };
   CHECK_NOTNULL(image_publishers_[stream_index]);
-  const bool has_raw_image_subscriber =
-      image_publishers_[stream_index]->get_subscription_count() > 0;
+  const bool has_explicit_compressed_publisher =
+      compressed_image_publishers_.count(stream_index) > 0 &&
+      compressed_image_publishers_.at(stream_index);
+  bool has_raw_image_subscriber = image_publishers_[stream_index]->get_subscription_count() > 0;
+  if (!use_intra_process_ && !has_explicit_compressed_publisher) {
+    has_raw_image_subscriber =
+        node_->count_subscribers(stream_name_[stream_index] + "/image_raw") > 0;
+  }
   const bool has_compressed_image_subscriber = hasCompressedImageSubscriber(stream_index);
+  const bool has_image_transport_compressed_subscriber =
+      has_compressed_image_subscriber && !has_explicit_compressed_publisher;
   bool has_subscriber =
       has_raw_image_subscriber || has_compressed_image_subscriber || save_images_[stream_index];
   has_subscriber =
@@ -7283,18 +7386,10 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame> &frame,
                                                    getSteadyNowUs());
     }
     publishCompressedColorImage(frame, stream_index, timestamp, frame_id);
-    if (!has_raw_image_subscriber) {
-      if (stream_index == COLOR) {
-        fps_delay_status_color_->tick(frame_timestamp);
-      } else if (stream_index == COLOR_LEFT) {
-        fps_delay_status_left_color_->tick(frame_timestamp);
-      } else {
-        fps_delay_status_right_color_->tick(frame_timestamp);
-      }
-    }
   }
 
-  if (!has_raw_image_subscriber && !save_images_[stream_index]) {
+  if (!has_raw_image_subscriber && !save_images_[stream_index] &&
+      !has_image_transport_compressed_subscriber) {
     CHECK(camera_info_publishers_.count(stream_index) > 0);
     camera_info_publishers_[stream_index]->publish(camera_info);
     publishMetadata(frame, stream_index, camera_info.header);
@@ -7356,11 +7451,13 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame> &frame,
       }
     }
   }
-  if (!has_raw_image_subscriber && !save_images_[stream_index]) {
+  if (!has_raw_image_subscriber && !save_images_[stream_index] &&
+      !has_image_transport_compressed_subscriber) {
     return;
   }
   CHECK(image_publishers_.count(stream_index) > 0);
-  if (has_raw_image_subscriber || save_images_[stream_index]) {
+  if (has_raw_image_subscriber || has_image_transport_compressed_subscriber ||
+      save_images_[stream_index]) {
     sensor_msgs::msg::Image::UniquePtr image_msg(new sensor_msgs::msg::Image());
     cv_bridge::CvImage(std_msgs::msg::Header(), image_encoding, image_to_publish)
         .toImageMsg(*image_msg);
@@ -7370,7 +7467,7 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame> &frame,
     image_msg->step = image_step;
     image_msg->header.frame_id = frame_id;
     saveImageToFile(stream_index, raw_image, image_to_publish, *image_msg, frame);
-    if (!has_raw_image_subscriber) {
+    if (!has_raw_image_subscriber && !has_image_transport_compressed_subscriber) {
       record_image_publish_skipped();
       return;
     }
@@ -7378,18 +7475,11 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame> &frame,
       timestamp_csv_logger_->recordImagePrePublish(stream_index.first, frame, getSystemNowUs(),
                                                    getSteadyNowUs());
     }
-    if (stream_index == COLOR) {
-      fps_delay_status_color_->tick(frame_timestamp);
-    } else if (stream_index == COLOR_LEFT) {
-      fps_delay_status_left_color_->tick(frame_timestamp);
-    } else if (stream_index == COLOR_RIGHT) {
-      fps_delay_status_right_color_->tick(frame_timestamp);
-    } else if (stream_index == DEPTH) {
-      fps_delay_status_depth_->tick(frame_timestamp);
-    } else if (stream_index == INFRA1) {
-      fps_delay_status_left_ir_->tick(frame_timestamp);
-    } else if (stream_index == INFRA2) {
-      fps_delay_status_right_ir_->tick(frame_timestamp);
+    if (has_raw_image_subscriber) {
+      recordStreamStatus(stream_name_[stream_index] + "/image_raw", image_msg->header.stamp);
+    }
+    if (has_image_transport_compressed_subscriber) {
+      recordStreamStatus(compressedStreamStatusTopic(stream_index), image_msg->header.stamp);
     }
     image_publishers_[stream_index]->publish(std::move(image_msg));
   }
@@ -7397,8 +7487,13 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame> &frame,
 
 bool OBCameraNode::hasCompressedImageSubscriber(const stream_index_pair &stream_index) const {
   auto it = compressed_image_publishers_.find(stream_index);
-  return it != compressed_image_publishers_.end() && it->second &&
-         it->second->get_subscription_count() > 0;
+  if (it != compressed_image_publishers_.end() && it->second) {
+    return it->second->get_subscription_count() > 0;
+  }
+  if (use_intra_process_) {
+    return false;
+  }
+  return node_->count_subscribers(compressedStreamStatusTopic(stream_index)) > 0;
 }
 
 void OBCameraNode::publishCompressedColorImage(const std::shared_ptr<ob::Frame> &frame,
@@ -7415,6 +7510,7 @@ void OBCameraNode::publishCompressedColorImage(const std::shared_ptr<ob::Frame>
   msg.format = "jpeg";
   const auto *data = static_cast<const uint8_t *>(frame->getData());
   msg.data.assign(data, data + frame->getDataSize());
+  recordStreamStatus(stream_name_[stream_index] + "/image_raw/compressed", msg.header.stamp);
   it->second->publish(std::move(msg));
 }
 
@@ -7627,6 +7723,8 @@ void OBCameraNode::onNewIMUFrameSyncOutputCallback(const std::shared_ptr<ob::Fra
   imu_msg.linear_acceleration.z = accelData.z - accel_info.bias[2];
 
   const auto publish_system_us = getSystemNowUs();
+  recordStreamStatus(stream_name_[GYRO] + "_" + stream_name_[ACCEL] + "/sample",
+                     imu_msg.header.stamp);
   imu_gyro_accel_publisher_->publish(imu_msg);
   record_timestamps(publish_system_us);
 }
@@ -7687,6 +7785,7 @@ void OBCameraNode::onNewIMUFrameCallback(const std::shared_ptr<ob::Frame> &frame
     return;
   }
   const auto publish_system_us = getSystemNowUs();
+  recordStreamStatus(stream_name_[stream_index] + "/sample", imu_msg.header.stamp);
   imu_publishers_[stream_index]->publish(imu_msg);
   record_timestamps(publish_system_us);
 }

orbbec_camera/src/ob_camera_node_driver.cpp

@@ -442,8 +442,7 @@ void OBCameraNodeDriver::init() {
   CHECK_NOTNULL(check_connect_timer_);
   if (device_type_ == "camera") {
     device_status_timer_ =
-        this->create_wall_timer(std::chrono::milliseconds(1000 / device_status_interval_hz),
-                                [this]() { deviceStatusTimer(); });
+        this->create_wall_timer(std::chrono::seconds(1), [this]() { deviceStatusTimer(); });
     auto qos = rclcpp::QoS(1).transient_local();
     if (node_options_.use_intra_process_comms()) {
       qos = rclcpp::QoS(1);
@@ -714,6 +713,10 @@ void OBCameraNodeDriver::deviceStatusTimer() {
   status_msg.calibration_from_launch_param = false;
   status_msg.customer_calibration_ready = false;
 
+  if (ob_camera_node_) {
+    ob_camera_node_->fillStreamStatus(status_msg);
+  }
+
   // Flag to track if device communication error occurs
   bool device_communication_error = false;
 
@@ -725,30 +728,6 @@ void OBCameraNodeDriver::deviceStatusTimer() {
     if (reset_lock.owns_lock() && !reset_device_flag_) {
       // Only get device-specific info if we have a valid camera node and device
       if (ob_camera_node_) {
-        // Safely get color and depth status - these may access device
-        try {
-          ob_camera_node_->getColorStatus(status_msg);
-          ob_camera_node_->getDepthStatus(status_msg);
-        } catch (const ob::Error &e) {
-          std::string error_msg = orbbec_camera::formatObErrorWithStatus(e);
-          if (error_msg.find("Device is deactivated") != std::string::npos ||
-              error_msg.find("disconnected") != std::string::npos ||
-              error_msg.find("Send control transfer failed") != std::string::npos) {
-            RCLCPP_WARN(
-                logger_,
-                "Device communication error in %s at line %d: %s - Device may be disconnected",
-                __FUNCTION__, __LINE__, error_msg.c_str());
-            device_communication_error = true;
-          } else {
-            RCLCPP_ERROR(logger_, "Error in %s at line %d: %s", __FUNCTION__, __LINE__,
-                         error_msg.c_str());
-          }
-        } catch (const std::exception &e) {
-          RCLCPP_ERROR(logger_, "Exception in %s at line %d: %s", __FUNCTION__, __LINE__, e.what());
-        } catch (...) {
-          RCLCPP_ERROR(logger_, "Unknown exception in %s at line %d", __FUNCTION__, __LINE__);
-        }
-
         // These should be safe as they don't directly access hardware
         status_msg.calibration_from_launch_param = ob_camera_node_->isParamCalibrated();
       }