From 4c932d47a87d5d774a4984a48f6676f1ce979160 Mon Sep 17 00:00:00 2001 From: xiexun Date: Thu, 28 Aug 2025 21:40:12 +0800 Subject: [PATCH] Add device_status topic for state monitoring --- .../orbbec_camera/fps_delay_status.hpp | 88 +++++++++++++++++++ .../include/orbbec_camera/ob_camera_node.h | 23 +++++ .../orbbec_camera/ob_camera_node_driver.h | 5 ++ orbbec_camera/src/ob_camera_node.cpp | 12 +++ orbbec_camera/src/ob_camera_node_driver.cpp | 25 ++++++ orbbec_camera_msgs/CMakeLists.txt | 1 + orbbec_camera_msgs/msg/DeviceStatus.msg | 32 +++++++ 7 files changed, 186 insertions(+) create mode 100644 orbbec_camera/include/orbbec_camera/fps_delay_status.hpp create mode 100644 orbbec_camera_msgs/msg/DeviceStatus.msg diff --git a/orbbec_camera/include/orbbec_camera/fps_delay_status.hpp b/orbbec_camera/include/orbbec_camera/fps_delay_status.hpp new file mode 100644 index 00000000..fcb1024e --- /dev/null +++ b/orbbec_camera/include/orbbec_camera/fps_delay_status.hpp @@ -0,0 +1,88 @@ +#pragma once + +#include +#include "orbbec_camera_msgs/msg/device_status.hpp" +#include +#include +#include + +namespace orbbec_camera { + +class FpsDelayStatus { +public: + explicit FpsDelayStatus():last_frame_stamp_(std::chrono::steady_clock::now()){} + void tick() { + std::lock_guard lock(mutex_); + auto now = std::chrono::steady_clock::now(); + double dt = std::chrono::duration_cast>(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 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::lowest(); + fps_min_ = delay_min_ = std::numeric_limits::max(); + } + void fillDepthStatus(orbbec_camera_msgs::msg::DeviceStatus &msg){ + std::lock_guard 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::lowest(); + fps_min_ = delay_min_ = std::numeric_limits::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::lowest()}; + double fps_min_{std::numeric_limits::max()}; + double delay_max_{std::numeric_limits::lowest()}; + double delay_min_{std::numeric_limits::max()}; + +}; + +} // namespace orbbec_camera diff --git a/orbbec_camera/include/orbbec_camera/ob_camera_node.h b/orbbec_camera/include/orbbec_camera/ob_camera_node.h index 46ed9f48..8d06bdb0 100644 --- a/orbbec_camera/include/orbbec_camera/ob_camera_node.h +++ b/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 #include @@ -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 fps_counter_depth_{nullptr}; std::unique_ptr fps_counter_left_ir_{nullptr}; std::unique_ptr fps_counter_right_ir_{nullptr}; + + std::unique_ptr fps_delay_status_color_{nullptr}; + std::unique_ptr fps_delay_status_depth_{nullptr}; + rclcpp::Publisher::SharedPtr device_status_pub_; }; } // namespace orbbec_camera diff --git a/orbbec_camera/include/orbbec_camera/ob_camera_node_driver.h b/orbbec_camera/include/orbbec_camera/ob_camera_node_driver.h index 82b45154..123f7cc2 100644 --- a/orbbec_camera/include/orbbec_camera/ob_camera_node_driver.h +++ b/orbbec_camera/include/orbbec_camera/ob_camera_node_driver.h @@ -27,6 +27,7 @@ #include #include #include +#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 request, std::shared_ptr 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 firmware_update_success_{false}; + rclcpp::TimerBase::SharedPtr device_status_timer_ = nullptr; + int device_status_interval_hz = 2; // 2Hz }; } // namespace orbbec_camera diff --git a/orbbec_camera/src/ob_camera_node.cpp b/orbbec_camera/src/ob_camera_node.cpp index 2d85732d..223bcf45 100644 --- a/orbbec_camera/src/ob_camera_node.cpp +++ b/orbbec_camera/src/ob_camera_node.cpp @@ -91,6 +91,9 @@ OBCameraNode::OBCameraNode(rclcpp::Node *node, std::shared_ptr 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(); + fps_delay_status_depth_ = std::make_unique(); } template @@ -2265,6 +2268,9 @@ void OBCameraNode::setupPublishers() { std_msgs::msg::String msg; msg.data = filter_status_.dump(2); filter_status_pub_->publish(msg); + + device_status_pub_ = + node_->create_publisher("device_status", extrinsics_qos); } void OBCameraNode::publishPointCloud(const std::shared_ptr &frame_set) { @@ -3062,6 +3068,12 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr &frame, image_msg->header.frame_id = frame_id; CHECK(image_publishers_.count(stream_index) > 0); saveImageToFile(stream_index, image, *image_msg); + if (stream_index == COLOR) { + fps_delay_status_color_->tick(); + } + else if (stream_index == DEPTH) { + fps_delay_status_depth_->tick(); + } image_publishers_[stream_index]->publish(std::move(image_msg)); if (stream_index == COLOR && enable_color_undistortion_ && color_undistortion_publisher_->get_subscription_count() > 0) { diff --git a/orbbec_camera/src/ob_camera_node_driver.cpp b/orbbec_camera/src/ob_camera_node_driver.cpp index c8e2ed2a..9382761f 100644 --- a/orbbec_camera/src/ob_camera_node_driver.cpp +++ b/orbbec_camera/src/ob_camera_node_driver.cpp @@ -231,6 +231,9 @@ void OBCameraNodeDriver::init() { CHECK_NOTNULL(check_connect_timer_); query_thread_ = std::make_shared([this]() { queryDevice(); }); reset_device_thread_ = std::make_shared([this]() { resetDevice(); }); + + device_status_timer_ = this->create_wall_timer( + std::chrono::milliseconds(1000 / device_status_interval_hz), [this]() { deviceStatusTimer(); }); } void OBCameraNodeDriver::onDeviceConnected(const std::shared_ptr &device_list) { @@ -442,6 +445,28 @@ void OBCameraNodeDriver::resetDevice() { } } +void OBCameraNodeDriver::deviceStatusTimer() { + if (ob_camera_node_ == nullptr) { + return; + } + orbbec_camera_msgs::msg::DeviceStatus status_msg; + status_msg.header.stamp = this->now(); + status_msg.device_online = device_connected_.load(); + + ob_camera_node_->getColorStatus(status_msg); + ob_camera_node_->getDepthStatus(status_msg); + + status_msg.connection_type = device_info_->getConnectionType(); + + auto camera_params = device_->getCalibrationCameraParamList(); + bool calibration_from_device = (camera_params != nullptr && camera_params->count() > 0); + status_msg.calibration_from_device = calibration_from_device; + status_msg.calibration_from_launch_param = ob_camera_node_->isParamCalibrated(); + // status_msg.calibration_from_user_file = ob_camera_node_->isWriteCustomerDataSuccess(); + + ob_camera_node_->publishDeviceStatus(status_msg); +} + void OBCameraNodeDriver::rebootDeviceCallback( const std::shared_ptr request, std::shared_ptr response) { diff --git a/orbbec_camera_msgs/CMakeLists.txt b/orbbec_camera_msgs/CMakeLists.txt index 7708fb2a..5a47dafd 100644 --- a/orbbec_camera_msgs/CMakeLists.txt +++ b/orbbec_camera_msgs/CMakeLists.txt @@ -17,6 +17,7 @@ find_package(std_msgs REQUIRED) rosidl_generate_interfaces( ${PROJECT_NAME} "msg/DeviceInfo.msg" + "msg/DeviceStatus.msg" "msg/Extrinsics.msg" "msg/Metadata.msg" "msg/IMUInfo.msg" diff --git a/orbbec_camera_msgs/msg/DeviceStatus.msg b/orbbec_camera_msgs/msg/DeviceStatus.msg new file mode 100644 index 00000000..fd59b3e2 --- /dev/null +++ b/orbbec_camera_msgs/msg/DeviceStatus.msg @@ -0,0 +1,32 @@ +std_msgs/Header header + +# --- Color stream --- +float64 color_frame_rate_cur +float64 color_frame_rate_avg +float64 color_frame_rate_min +float64 color_frame_rate_max + +float64 color_delay_ms_cur +float64 color_delay_ms_avg +float64 color_delay_ms_min +float64 color_delay_ms_max + +# --- Depth stream --- +float64 depth_frame_rate_cur +float64 depth_frame_rate_avg +float64 depth_frame_rate_min +float64 depth_frame_rate_max + +float64 depth_delay_ms_cur +float64 depth_delay_ms_avg +float64 depth_delay_ms_min +float64 depth_delay_ms_max + +# --- Device info --- +bool device_online +string connection_type # e.g. "USB2.0", "USB3.0", "GigE" + +# --- Calibration status --- +bool calibration_from_user_file +bool calibration_from_device +bool calibration_from_launch_param \ No newline at end of file