Add device_status topic for state monitoring

orbbec_camera/include/orbbec_camera/fps_delay_status.hpp

@@ -0,0 +1,88 @@
+#pragma once
+
+#include <rclcpp/rclcpp.hpp>
+#include "orbbec_camera_msgs/msg/device_status.hpp"
+#include <chrono>
+#include <functional>
+#include <mutex>
+
+namespace orbbec_camera {
+
+class FpsDelayStatus {
+public:
+  explicit FpsDelayStatus():last_frame_stamp_(std::chrono::steady_clock::now()){}
+  void tick() {
+    std::lock_guard<std::mutex> lock(mutex_);
+    auto now = std::chrono::steady_clock::now();
+    double dt = std::chrono::duration_cast<std::chrono::duration<double>>(now - last_frame_stamp_).count();
+    double fps = (dt > 0) ? (1.0 / dt) : 0.0;
+    double delay_ms = dt * 1000.0;
+
+    last_fps_ = fps;
+    last_delay_ms_ = delay_ms;
+    last_frame_stamp_ = now;
+
+    frame_count_++;
+    fps_sum_ += fps;
+    delay_sum_ += delay_ms;
+
+    fps_max_ = std::max(fps_max_, fps);
+    fps_min_ = std::min(fps_min_, fps);
+    delay_max_ = std::max(delay_max_, delay_ms);
+    delay_min_ = std::min(delay_min_, delay_ms);
+
+  }
+
+  void fillColorStatus(orbbec_camera_msgs::msg::DeviceStatus &msg){
+    std::lock_guard<std::mutex> lock(mutex_);
+    msg.color_frame_rate_cur = last_fps_;
+    msg.color_frame_rate_avg = frame_count_ > 0 ? fps_sum_ / frame_count_ : 0.0;
+    msg.color_frame_rate_min = fps_min_;
+    msg.color_frame_rate_max = fps_max_;
+
+    msg.color_delay_ms_cur = last_delay_ms_;
+    msg.color_delay_ms_avg = frame_count_ > 0 ? delay_sum_ / frame_count_ : 0.0;
+    msg.color_delay_ms_min = delay_min_;
+    msg.color_delay_ms_max = delay_max_;
+
+    frame_count_ = 0;
+    fps_sum_ = delay_sum_ = 0.0;
+    fps_max_ = delay_max_ = std::numeric_limits<double>::lowest();
+    fps_min_ = delay_min_ = std::numeric_limits<double>::max();
+  }
+  void fillDepthStatus(orbbec_camera_msgs::msg::DeviceStatus &msg){
+    std::lock_guard<std::mutex> lock(mutex_);
+    msg.depth_frame_rate_cur = last_fps_;
+    msg.depth_frame_rate_avg = frame_count_ > 0 ? fps_sum_ / frame_count_ : 0.0;
+    msg.depth_frame_rate_min = fps_min_;
+    msg.depth_frame_rate_max = fps_max_;
+
+    msg.depth_delay_ms_cur = last_delay_ms_;
+    msg.depth_delay_ms_avg = frame_count_ > 0 ? delay_sum_ / frame_count_ : 0.0;
+    msg.depth_delay_ms_min = delay_min_;
+    msg.depth_delay_ms_max = delay_max_;
+
+    frame_count_ = 0;
+    fps_sum_ = delay_sum_ = 0.0;
+    fps_max_ = delay_max_ = std::numeric_limits<double>::lowest();
+    fps_min_ = delay_min_ = std::numeric_limits<double>::max();
+  }
+
+
+private:
+  mutable std::mutex mutex_;
+  std::chrono::steady_clock::time_point last_frame_stamp_;
+  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()};
+
+};
+
+}  // namespace orbbec_camera

orbbec_camera/include/orbbec_camera/ob_camera_node.h

@@ -64,6 +64,7 @@
 #include "magic_enum/magic_enum.hpp"
 #include "orbbec_camera/image_publisher.h"
 #include "orbbec_camera/fps_counter.hpp"
+#include "orbbec_camera/fps_delay_status.hpp"
 #include "jpeg_decoder.h"
 #include <std_msgs/msg/string.hpp>
 #include <fcntl.h>
@@ -178,6 +179,24 @@ class OBCameraNode {
   void startGmslTrigger();
   void stopGmslTrigger();
 
+  bool isParamCalibrated() const{
+    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);
+  }
+
+  void getDepthStatus(orbbec_camera_msgs::msg::DeviceStatus &status_msg){
+    fps_delay_status_depth_->fillDepthStatus(status_msg);
+  }
+
+  void publishDeviceStatus(const orbbec_camera_msgs::msg::DeviceStatus &msg) {
+    if (device_status_pub_) {
+      device_status_pub_->publish(msg);
+    }
+  }
+
  private:
   struct IMUData {
     IMUData() = default;
@@ -763,5 +782,9 @@ class OBCameraNode {
   std::unique_ptr<FpsCounter> fps_counter_depth_{nullptr};
   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_depth_{nullptr};
+  rclcpp::Publisher<orbbec_camera_msgs::msg::DeviceStatus>::SharedPtr device_status_pub_;
 };
 }  // namespace orbbec_camera

orbbec_camera/include/orbbec_camera/ob_camera_node_driver.h

@@ -27,6 +27,7 @@
 #include <pthread.h>
 #include <std_srvs/srv/empty.hpp>
 #include <backward_ros/backward.hpp>
+#include "libobsensor/hpp/Device.hpp"
 
 namespace orbbec_camera {
 
@@ -69,6 +70,8 @@ class OBCameraNodeDriver : public rclcpp::Node {
 
   void resetDevice();
 
+  void deviceStatusTimer();
+
   void rebootDeviceCallback(const std::shared_ptr<std_srvs::srv::Empty::Request> request,
                             std::shared_ptr<std_srvs::srv::Empty::Response> response);
   void presetUpdateCallback(bool firstCall, OBFwUpdateState state, const char* message,
@@ -120,5 +123,7 @@ class OBCameraNodeDriver : public rclcpp::Node {
   static backward::SignalHandling sh;  // for stack trace
   std::string upgrade_firmware_;
   std::atomic<bool> firmware_update_success_{false};
+  rclcpp::TimerBase::SharedPtr device_status_timer_ = nullptr;
+  int device_status_interval_hz = 2;  // 2Hz
 };
 }  // namespace orbbec_camera