diff --git a/orbbec_camera/examples/Gemini_435Le_example_node/camera_example_node.cpp b/orbbec_camera/examples/Gemini_435Le_example_node/camera_example_node.cpp index 0986ae40..874252d7 100644 --- a/orbbec_camera/examples/Gemini_435Le_example_node/camera_example_node.cpp +++ b/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()); diff --git a/orbbec_camera/include/orbbec_camera/fps_delay_status.hpp b/orbbec_camera/include/orbbec_camera/fps_delay_status.hpp deleted file mode 100644 index ecb86a92..00000000 --- a/orbbec_camera/include/orbbec_camera/fps_delay_status.hpp +++ /dev/null @@ -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 -#include "orbbec_camera_msgs/msg/device_status.hpp" -#include -#include -#include -#include -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 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(now2.time_since_epoch()).count(); - double delay_ms = - static_cast(ms_since_epoch) - static_cast(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 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::lowest()}; - double fps_min_{std::numeric_limits::max()}; - double delay_max_{std::numeric_limits::lowest()}; - double delay_min_{std::numeric_limits::max()}; - - LogLevel log_level_; - rclcpp::Logger logger_; -}; - -} // 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 939cd807..dd1efd4a 100644 --- a/orbbec_camera/include/orbbec_camera/ob_camera_node.h +++ b/orbbec_camera/include/orbbec_camera/ob_camera_node.h @@ -51,6 +51,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" @@ -77,7 +78,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 @@ -241,17 +242,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; @@ -369,6 +360,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(); @@ -1205,12 +1204,8 @@ class OBCameraNode { 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_left_color_{nullptr}; - std::unique_ptr fps_delay_status_right_color_{nullptr}; - std::unique_ptr fps_delay_status_depth_{nullptr}; - std::unique_ptr fps_delay_status_left_ir_{nullptr}; - std::unique_ptr fps_delay_status_right_ir_{nullptr}; + std::map> stream_status_trackers_; + mutable std::mutex stream_status_mutex_; std::string intra_camera_sync_reference_ = ""; std::string ae_reference_stream_; 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 c9a986af..6b594f13 100644 --- a/orbbec_camera/include/orbbec_camera/ob_camera_node_driver.h +++ b/orbbec_camera/include/orbbec_camera/ob_camera_node_driver.h @@ -158,7 +158,6 @@ class OBCameraNodeDriver : public rclcpp::Node { std::atomic is_reupdating_{false}; // Flag to track if we're in reupdate process std::atomic delay_stream_start_after_reconnect_{false}; rclcpp::TimerBase::SharedPtr device_status_timer_ = nullptr; - int device_status_interval_hz = 2; // 2Hz rclcpp::Publisher::SharedPtr device_status_pub_ = nullptr; std::string node_name_; bool force_ip_enable_{false}; diff --git a/orbbec_camera/include/orbbec_camera/stream_status.hpp b/orbbec_camera/include/orbbec_camera/stream_status.hpp new file mode 100644 index 00000000..7580c501 --- /dev/null +++ b/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 + +#include +#include +#include +#include +#include +#include +#include + +#include "orbbec_camera_msgs/msg/stream_status.hpp" + +namespace orbbec_camera { + +class StreamStatusTracker { + public: + using SubscriberCountFn = std::function; + + 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(now.time_since_epoch()).count(); + const double stamp_ms = + static_cast(stamp.sec) * 1000.0 + static_cast(stamp.nanosec) / 1000000.0; + + std::lock_guard 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 lock(mutex_); + const double window_seconds = std::chrono::duration(now - window_start_).count(); + status.topic_name = topic_name_; + status.has_subscribers = has_subscribers; + status.publish_rate_hz = + window_seconds > 0.0 ? static_cast(published_count_) / window_seconds : 0.0; + status.delay_ms_avg = + published_count_ > 0 ? delay_sum_ms_ / static_cast(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 diff --git a/orbbec_camera/src/ob_camera_node.cpp b/orbbec_camera/src/ob_camera_node.cpp index 8a88e81a..047e7e50 100644 --- a/orbbec_camera/src/ob_camera_node.cpp +++ b/orbbec_camera/src/ob_camera_node.cpp @@ -783,13 +783,6 @@ 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(logger_); - fps_delay_status_left_color_ = std::make_unique(logger_); - fps_delay_status_right_color_ = std::make_unique(logger_); - fps_delay_status_depth_ = std::make_unique(logger_); - fps_delay_status_left_ir_ = std::make_unique(logger_); - fps_delay_status_right_ir_ = std::make_unique(logger_); } template @@ -4341,6 +4334,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 &frame_set) { onNewFrameSetCallback(frame_set); @@ -5725,12 +5719,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(resolved_topic_name, std::move(subscriber_count)); + std::lock_guard lock(stream_status_mutex_); + stream_status_trackers_[topic_name] = std::move(tracker); +} + +void OBCameraNode::removeStreamStatus(const std::string &topic_name) { + std::lock_guard 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 tracker; + { + std::lock_guard 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> trackers; + { + std::lock_guard 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; } @@ -5738,6 +5791,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] = @@ -5748,13 +5802,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( 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); + }); + } } } @@ -5788,11 +5862,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( "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()); @@ -5837,6 +5923,9 @@ void OBCameraNode::setupPublishers() { } imu_gyro_accel_publisher_ = node_->create_publisher( 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( topic_name, rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(data_qos), data_qos)); @@ -5855,6 +5944,10 @@ void OBCameraNode::setupPublishers() { } imu_publishers_[stream_index] = node_->create_publisher( 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( @@ -6199,6 +6292,7 @@ void OBCameraNode::publishDepthPointCloud(const std::shared_ptr &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)); } @@ -6335,6 +6429,7 @@ void OBCameraNode::publishColoredPointCloud(const std::shared_ptr } } + recordStreamStatus("depth_registered/points", point_cloud_msg->header.stamp); depth_registration_cloud_pub_->publish(std::move(point_cloud_msg)); } std::shared_ptr OBCameraNode::processIrFrameFilter(std::shared_ptr &frame) { @@ -7162,9 +7257,17 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr &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 = @@ -7281,18 +7384,10 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr &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); @@ -7354,11 +7449,13 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr &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); @@ -7368,7 +7465,7 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr &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; } @@ -7376,18 +7473,11 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr &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)); } @@ -7395,8 +7485,13 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr &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 &frame, @@ -7413,6 +7508,7 @@ void OBCameraNode::publishCompressedColorImage(const std::shared_ptr msg.format = "jpeg"; const auto *data = static_cast(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)); } @@ -7625,6 +7721,8 @@ void OBCameraNode::onNewIMUFrameSyncOutputCallback(const std::shared_ptrpublish(imu_msg); record_timestamps(publish_system_us); } @@ -7685,6 +7783,7 @@ void OBCameraNode::onNewIMUFrameCallback(const std::shared_ptr &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); } diff --git a/orbbec_camera/src/ob_camera_node_driver.cpp b/orbbec_camera/src/ob_camera_node_driver.cpp index 1b1d6748..a44302dd 100644 --- a/orbbec_camera/src/ob_camera_node_driver.cpp +++ b/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); @@ -722,6 +721,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; @@ -733,30 +736,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(); } diff --git a/orbbec_camera_msgs/CMakeLists.txt b/orbbec_camera_msgs/CMakeLists.txt index c004d1d6..14acfc3a 100644 --- a/orbbec_camera_msgs/CMakeLists.txt +++ b/orbbec_camera_msgs/CMakeLists.txt @@ -18,6 +18,7 @@ find_package(std_msgs REQUIRED) rosidl_generate_interfaces( ${PROJECT_NAME} "msg/DeviceInfo.msg" + "msg/StreamStatus.msg" "msg/DeviceStatus.msg" "msg/DepthFilterParam.msg" "msg/DepthFilterState.msg" diff --git a/orbbec_camera_msgs/msg/DeviceStatus.msg b/orbbec_camera_msgs/msg/DeviceStatus.msg index 32b46777..5e07cda1 100644 --- a/orbbec_camera_msgs/msg/DeviceStatus.msg +++ b/orbbec_camera_msgs/msg/DeviceStatus.msg @@ -1,76 +1,7 @@ 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 - -# --- Left color stream --- -float64 left_color_frame_rate_cur -float64 left_color_frame_rate_avg -float64 left_color_frame_rate_min -float64 left_color_frame_rate_max - -float64 left_color_delay_ms_cur -float64 left_color_delay_ms_avg -float64 left_color_delay_ms_min -float64 left_color_delay_ms_max - -# --- Right color stream --- -float64 right_color_frame_rate_cur -float64 right_color_frame_rate_avg -float64 right_color_frame_rate_min -float64 right_color_frame_rate_max - -float64 right_color_delay_ms_cur -float64 right_color_delay_ms_avg -float64 right_color_delay_ms_min -float64 right_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 - -# --- Left IR stream --- -float64 left_ir_frame_rate_cur -float64 left_ir_frame_rate_avg -float64 left_ir_frame_rate_min -float64 left_ir_frame_rate_max - -float64 left_ir_delay_ms_cur -float64 left_ir_delay_ms_avg -float64 left_ir_delay_ms_min -float64 left_ir_delay_ms_max - -# --- Right IR stream --- -float64 right_ir_frame_rate_cur -float64 right_ir_frame_rate_avg -float64 right_ir_frame_rate_min -float64 right_ir_frame_rate_max - -float64 right_ir_delay_ms_cur -float64 right_ir_delay_ms_avg -float64 right_ir_delay_ms_min -float64 right_ir_delay_ms_max - -# --- Device info --- bool device_online -string connection_type # e.g. "USB2.0", "USB3.0", "GigE" - -# --- Calibration status --- +string connection_type bool customer_calibration_ready bool calibration_from_factory bool calibration_from_launch_param +orbbec_camera_msgs/StreamStatus[] streams diff --git a/orbbec_camera_msgs/msg/StreamStatus.msg b/orbbec_camera_msgs/msg/StreamStatus.msg new file mode 100644 index 00000000..11dc7956 --- /dev/null +++ b/orbbec_camera_msgs/msg/StreamStatus.msg @@ -0,0 +1,4 @@ +string topic_name +bool has_subscribers +float64 publish_rate_hz +float64 delay_ms_avg