diff --git a/orbbec_camera/config/tools/multisavergbir/multi_save_rgbir_params.json b/orbbec_camera/config/tools/multisavergbir/multi_save_rgbir_params.json index d557155a..dd1d3703 100644 --- a/orbbec_camera/config/tools/multisavergbir/multi_save_rgbir_params.json +++ b/orbbec_camera/config/tools/multisavergbir/multi_save_rgbir_params.json @@ -1,13 +1,14 @@ { "save_rgbir_params": { - "time_domain": "global", - "usb_ports": [ - "2-1", - "2-3" - ], - "camera_name": [ - "camera_01", - "camera_02" - ] + "time_domain": "global", + "stream_names": [], + "usb_ports": [ + "2-1", + "2-3" + ], + "camera_name": [ + "camera_01", + "camera_02" + ] } -} \ No newline at end of file +} diff --git a/orbbec_camera/examples/multi_camera_time_sync/image_sync_example_node.cpp b/orbbec_camera/examples/multi_camera_time_sync/image_sync_example_node.cpp index 48791ba7..eb446ad7 100755 --- a/orbbec_camera/examples/multi_camera_time_sync/image_sync_example_node.cpp +++ b/orbbec_camera/examples/multi_camera_time_sync/image_sync_example_node.cpp @@ -1,4 +1,5 @@ #include +#include #include #if __has_include() @@ -33,6 +34,7 @@ #include #include #include +#include #include using Image = sensor_msgs::msg::Image; @@ -69,7 +71,7 @@ class ImageSyncNode : public rclcpp::Node { if (sync_topics_.empty()) { sync_topics_ = discover_image_topics(); RCLCPP_INFO(this->get_logger(), - "Parameter sync_topics is empty. Auto-discovered %zu color/depth image topics.", + "Parameter sync_topics is empty. Auto-discovered %zu supported image topics.", sync_topics_.size()); } else { RCLCPP_INFO(this->get_logger(), "Using %zu image topics from parameter sync_topics.", @@ -166,6 +168,24 @@ class ImageSyncNode : public rclcpp::Node { str.compare(str.size() - suffix.size(), suffix.size(), suffix) == 0; } + static const std::array, 7> &supported_stream_suffixes() { + static const std::array, 7> suffixes = {{ + {"left_color", "/left_color/image_raw"}, + {"right_color", "/right_color/image_raw"}, + {"left_ir", "/left_ir/image_raw"}, + {"right_ir", "/right_ir/image_raw"}, + {"color", "/color/image_raw"}, + {"depth", "/depth/image_raw"}, + {"ir", "/ir/image_raw"}, + }}; + return suffixes; + } + + static bool is_supported_image_topic(const std::string &topic) { + return std::any_of(supported_stream_suffixes().begin(), supported_stream_suffixes().end(), + [&topic](const auto &entry) { return has_suffix(topic, entry.second); }); + } + static double stamp_to_seconds(const builtin_interfaces::msg::Time &stamp) { return static_cast(stamp.sec) + static_cast(stamp.nanosec) * 1e-9; } @@ -180,7 +200,7 @@ class ImageSyncNode : public rclcpp::Node { const auto names_and_types = this->get_topic_names_and_types(); for (const auto &entry : names_and_types) { const auto &topic = entry.first; - if (!has_suffix(topic, "/color/image_raw") && !has_suffix(topic, "/depth/image_raw")) { + if (!is_supported_image_topic(topic)) { continue; } @@ -207,8 +227,8 @@ class ImageSyncNode : public rclcpp::Node { void validate_topics() { if (sync_topics_.empty()) { throw std::runtime_error( - "No image topics to synchronize. Set parameter sync_topics or start color/depth cameras " - "before this node."); + "No image topics to synchronize. Set parameter sync_topics or start supported camera " + "streams before this node."); } std::vector deduplicated_topics; @@ -233,7 +253,7 @@ class ImageSyncNode : public rclcpp::Node { "Official ROS message_filters::Synchronizer supports at most 9 inputs, and this example " "supports 1-8 image topics. Found " + std::to_string(sync_topics_.size()) + - " color/depth image topics. Please pass <= 8 topics with sync_topics or split the sync " + " image topics. Please pass <= 8 topics with sync_topics or split the sync " "into multiple stages."); } } @@ -247,14 +267,13 @@ class ImageSyncNode : public rclcpp::Node { info.image_type = "image"; info.camera_name = topic; - const auto color_pos = topic.rfind("/color/image_raw"); - const auto depth_pos = topic.rfind("/depth/image_raw"); - if (color_pos != std::string::npos) { - info.image_type = "color"; - info.camera_name = topic.substr(0, color_pos); - } else if (depth_pos != std::string::npos) { - info.image_type = "depth"; - info.camera_name = topic.substr(0, depth_pos); + for (const auto &entry : supported_stream_suffixes()) { + const std::string suffix = entry.second; + if (has_suffix(topic, suffix)) { + info.image_type = entry.first; + info.camera_name = topic.substr(0, topic.size() - suffix.size()); + break; + } } const auto slash_pos = info.camera_name.find_last_of('/'); @@ -280,7 +299,17 @@ class ImageSyncNode : public rclcpp::Node { try { for (const auto &msg : msgs) { auto cv_image = cv_bridge::toCvShare(msg); - images.push_back(cv_image->image.clone()); + cv::Mat image; + if (msg->encoding == sensor_msgs::image_encodings::RGB8) { + cv::cvtColor(cv_image->image, image, cv::COLOR_RGB2BGR); + } else if (msg->encoding == sensor_msgs::image_encodings::RGBA8) { + cv::cvtColor(cv_image->image, image, cv::COLOR_RGBA2BGR); + } else if (msg->encoding == sensor_msgs::image_encodings::BGRA8) { + cv::cvtColor(cv_image->image, image, cv::COLOR_BGRA2BGR); + } else { + image = cv_image->image.clone(); + } + images.push_back(std::move(image)); timestamps.push_back(stamp_to_seconds(msg->header.stamp)); } } catch (cv_bridge::Exception &e) { @@ -459,8 +488,14 @@ class ImageSyncNode : public rclcpp::Node { cv::applyColorMap(tmp, image, cv::COLORMAP_JET); } else if (images[i].channels() == 3) { image = images[i].clone(); + } else if (images[i].channels() == 4) { + cv::cvtColor(images[i], image, cv::COLOR_BGRA2BGR); } else { - cv::cvtColor(images[i], image, cv::COLOR_GRAY2BGR); + RCLCPP_WARN(this->get_logger(), "Display first channel of unsupported %d-channel image %s", + images[i].channels(), topic_infos[i].topic.c_str()); + cv::Mat first_channel; + cv::extractChannel(images[i], first_channel, 0); + cv::cvtColor(first_channel, image, cv::COLOR_GRAY2BGR); } const std::string text = topic_infos[i].camera_name + " " + topic_infos[i].image_type + @@ -550,10 +585,8 @@ class ImageSyncNode : public rclcpp::Node { const double avg_diff = diff_sum_ / count_; std::cout << "\nImage Timestamp Difference Statistics" << std::endl; - std::cout << "cur: " << cur << " ms" - << " avg: " << avg_diff << " ms" - << " max: " << max_diff_ << " ms" - << " min: " << min_diff_ << " ms" << std::endl; + std::cout << "cur: " << cur << " ms" << " avg: " << avg_diff << " ms" << " max: " << max_diff_ + << " ms" << " min: " << min_diff_ << " ms" << std::endl; if (last_time_ == 0.0) { last_time_ = base_t; diff --git a/orbbec_camera/include/orbbec_camera/fps_delay_status.hpp b/orbbec_camera/include/orbbec_camera/fps_delay_status.hpp index e85501dc..ecb86a92 100644 --- a/orbbec_camera/include/orbbec_camera/fps_delay_status.hpp +++ b/orbbec_camera/include/orbbec_camera/fps_delay_status.hpp @@ -20,6 +20,7 @@ #include "orbbec_camera_msgs/msg/device_status.hpp" #include #include +#include #include namespace orbbec_camera { class FpsDelayStatus { @@ -58,38 +59,57 @@ class FpsDelayStatus { } 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; - msg.color_frame_rate_min = fps_min_; - msg.color_frame_rate_max = fps_max_; + 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); + } - msg.color_delay_ms_cur = last_delay_ms_; - msg.color_delay_ms_avg = frame_count_ > 0 ? delay_sum_ / frame_count_ : 0; - msg.color_delay_ms_min = delay_min_; - msg.color_delay_ms_max = delay_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); + } - 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; + 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_); - msg.depth_frame_rate_cur = last_fps_; - msg.depth_frame_rate_avg = frame_count_ > 0 ? fps_sum_ / frame_count_ : 0; - msg.depth_frame_rate_min = fps_min_; - msg.depth_frame_rate_max = fps_max_; + 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; - msg.depth_delay_ms_cur = last_delay_ms_; - msg.depth_delay_ms_avg = frame_count_ > 0 ? delay_sum_ / frame_count_ : 0; - msg.depth_delay_ms_min = delay_min_; - msg.depth_delay_ms_max = delay_max_; - - // RCLCPP_ERROR_STREAM(logger_, "Depth status: " << fps_sum_ << "," << frame_count_); + 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; @@ -99,7 +119,6 @@ class FpsDelayStatus { fps_min_ = delay_min_ = 0.0; } - private: mutable std::mutex mutex_; u_int64_t last_stream_timestamp_{0}; double last_delay_ms_{0.0}; diff --git a/orbbec_camera/include/orbbec_camera/frame_timestamp_csv_logger.h b/orbbec_camera/include/orbbec_camera/frame_timestamp_csv_logger.h index e9033a20..ba486036 100644 --- a/orbbec_camera/include/orbbec_camera/frame_timestamp_csv_logger.h +++ b/orbbec_camera/include/orbbec_camera/frame_timestamp_csv_logger.h @@ -21,7 +21,7 @@ namespace orbbec_camera { class FrameTimestampCsvLogger { public: - enum class OutputMode { SYNCED, COLOR, DEPTH }; + enum class OutputMode { SYNCED, COLOR, LEFT_COLOR, RIGHT_COLOR, DEPTH, LEFT_IR, RIGHT_IR }; FrameTimestampCsvLogger(bool drop_log_enabled, const std::string &csv_file_path, OutputMode output_mode, rclcpp::Logger logger); diff --git a/orbbec_camera/include/orbbec_camera/ob_camera_node.h b/orbbec_camera/include/orbbec_camera/ob_camera_node.h index 198179e6..085b2e47 100644 --- a/orbbec_camera/include/orbbec_camera/ob_camera_node.h +++ b/orbbec_camera/include/orbbec_camera/ob_camera_node.h @@ -237,10 +237,14 @@ class OBCameraNode { } 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); } bool checkUserCalibrationReady() { @@ -1186,12 +1190,18 @@ class OBCameraNode { bool show_fps_enable_ = false; bool enable_publish_extrinsic_ = false; std::unique_ptr fps_counter_color_{nullptr}; + std::unique_ptr fps_counter_left_color_{nullptr}; + std::unique_ptr fps_counter_right_color_{nullptr}; 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_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::string intra_camera_sync_reference_ = ""; std::string ae_reference_stream_; diff --git a/orbbec_camera/include/orbbec_camera/timestamp_csv_logger.h b/orbbec_camera/include/orbbec_camera/timestamp_csv_logger.h index 8c3029de..0d7d4713 100644 --- a/orbbec_camera/include/orbbec_camera/timestamp_csv_logger.h +++ b/orbbec_camera/include/orbbec_camera/timestamp_csv_logger.h @@ -22,7 +22,11 @@ class TimestampCsvLogger { std::string csv_file_path; bool frame_sync_enabled = false; bool color_enabled = false; + bool left_color_enabled = false; + bool right_color_enabled = false; bool depth_enabled = false; + bool left_ir_enabled = false; + bool right_ir_enabled = false; bool imu_sync_enabled = false; bool accel_enabled = false; bool gyro_enabled = false; @@ -44,6 +48,9 @@ class TimestampCsvLogger { const std::shared_ptr &depth_frame, int64_t arrival_system_us, int64_t arrival_steady_us, bool track_color, bool track_depth, bool color_image_publish_expected, bool depth_image_publish_expected); + void recordImageFrameArrival(OBStreamType stream_type, const std::shared_ptr &frame, + int64_t arrival_system_us, int64_t arrival_steady_us, + bool image_publish_expected); void recordImagePrePublish(OBStreamType stream_type, const std::shared_ptr &frame, int64_t publish_system_us, int64_t publish_steady_us); void recordImagePublishSkipped(OBStreamType stream_type, const std::shared_ptr &frame); @@ -64,7 +71,11 @@ class TimestampCsvLogger { std::atomic_bool shutdown_requested_{false}; std::unique_ptr synced_image_logger_; std::unique_ptr color_logger_; + std::unique_ptr left_color_logger_; + std::unique_ptr right_color_logger_; std::unique_ptr depth_logger_; + std::unique_ptr left_ir_logger_; + std::unique_ptr right_ir_logger_; std::unique_ptr synced_imu_logger_; std::unique_ptr accel_logger_; std::unique_ptr gyro_logger_; diff --git a/orbbec_camera/scripts/common_benchmark_node.py b/orbbec_camera/scripts/common_benchmark_node.py index a012d5a6..15c59b05 100644 --- a/orbbec_camera/scripts/common_benchmark_node.py +++ b/orbbec_camera/scripts/common_benchmark_node.py @@ -4,11 +4,14 @@ Support: ROS2 name: common_benchmark_node.py function: A ROS2 node to monitor and log the performance of an Orbbec camera node: - frame rates, delays, CPU and RAM usage, packet/frame loss statistics. + subscriber-side frame rates, end-to-end delays, CPU and RAM usage, + and estimated frame loss statistics. usage: ros2 run orbbec_camera common_benchmark_node.py --run_time 20 --csv_file /tmp/cam_log.csv You can also pass an ideal frame rate for drop detection: --ideal_fps 30 Monitor multiple cameras: --camera_names camera,camera01 + Select topics explicitly: --topics color,depth + Compressed images and point clouds require full topic names. """ import argparse @@ -21,11 +24,22 @@ import csv import os from collections import defaultdict from orbbec_camera_msgs.msg import DeviceStatus -from sensor_msgs.msg import Image +from sensor_msgs.msg import CompressedImage, Image, PointCloud2 from tabulate import tabulate CAMERA_NODE_NAMES = ["component_container", "orbbec_camera_node", "nodelet"] +MONITORED_STREAMS = ( + "color", + "depth", + "ir", + "left_ir", + "right_ir", + "left_color", + "right_color", +) +DISCOVERY_INTERVAL_SECONDS = 0.1 +DISCOVERY_DURATION_SECONDS = 1.0 DOCUMENTATION_URL = ( "https://orbbec.github.io/OrbbecSDK_ROS2/en/source/camera_devices/" "6_benchmark/benchmark_tools.html" @@ -99,24 +113,62 @@ def estimate_dropped_frames(dt, expected_interval): # ---------------------------------------------- class TopicTracker: - def __init__(self, logger=None): + def __init__(self, logger=None, sample_start_time=None): self.received = 0 - - self.last_time = None + self.sample_start_time = sample_start_time or time.monotonic() + self.last_sample_time = self.sample_start_time + self.last_sample_received = 0 + self.last_header_stamp = None + self.estimated_interval = None self.drop_frames = 0 self.logger = logger - def on_msg(self, header, avg_fps): - stamp = header.stamp.sec + header.stamp.nanosec * 1e-9 + def on_msg(self, header, ros_receive_time, ideal_fps): + """Record one received message and return its age in milliseconds.""" + header_stamp = header.stamp.sec + header.stamp.nanosec * 1e-9 self.received += 1 + delay_ms = None - if self.last_time is not None and avg_fps > 0: - dt = stamp - self.last_time - expected_interval = 1.0 / avg_fps - self.drop_frames += estimate_dropped_frames(dt, expected_interval) + if header_stamp > 0: + delay_ms = (ros_receive_time - header_stamp) * 1000.0 + if self.last_header_stamp is not None: + header_dt = header_stamp - self.last_header_stamp + if header_dt > 0: + expected_interval = ( + 1.0 / ideal_fps + if ideal_fps and ideal_fps > 0.0 + else self.estimated_interval + ) + if expected_interval is not None: + self.drop_frames += estimate_dropped_frames( + header_dt, expected_interval + ) + self.update_estimated_interval(header_dt) + self.last_header_stamp = header_stamp - self.last_time = stamp + return delay_ms + + def sample_fps(self, sample_time): + """Calculate current and average subscriber throughput.""" + window_elapsed = sample_time - self.last_sample_time + total_elapsed = sample_time - self.sample_start_time + if window_elapsed <= 0.0 or total_elapsed <= 0.0: + return None + + window_received = self.received - self.last_sample_received + current_fps = window_received / window_elapsed + average_fps = self.received / total_elapsed + self.last_sample_time = sample_time + self.last_sample_received = self.received + return current_fps, average_fps + + def update_estimated_interval(self, interval): + """Learn the nominal source interval while excluding likely frame gaps.""" + if self.estimated_interval is None or interval < 0.75 * self.estimated_interval: + self.estimated_interval = interval + elif interval <= 1.5 * self.estimated_interval: + self.estimated_interval = 0.9 * self.estimated_interval + 0.1 * interval def frames_loss_rate(self): total = self.received + self.drop_frames @@ -129,17 +181,27 @@ class TopicTracker: class CameraMonitorNode(Node): - def __init__(self, run_time, csv_file="camera_monitor_log.csv", ideal_fps: float = 0.0, camera_names=None): + def __init__( + self, + run_time, + csv_file="camera_monitor_log.csv", + ideal_fps: float = 0.0, + camera_names=None, + topics=None, + ): super().__init__("camera_monitor_node") self.run_time = run_time - self.start_time = time.time() + self.discovery_start_time = time.time() + self.discovery_start_monotonic = None + self.start_time = None self.process = psutil.Process(os.getpid()) self.first_data_collected = False self.camera_names = parse_camera_names(camera_names) self.node_names = {camera_name: "Not Found" for camera_name in self.camera_names} self.total_node_name = "Not Found" - # If > 0, use this ideal fps value for drop-frame detection instead of the reported average + # If > 0, use this ideal FPS for drop detection instead of learning the + # nominal interval from received image timestamps. self.ideal_fps = float(ideal_fps) if ideal_fps is not None else 0.0 self.finished = False @@ -153,22 +215,26 @@ class CameraMonitorNode(Node): "stats": defaultdict(make_stat), "cpu_stats": make_stat(), "ram_stats": make_stat(), - "trackers": { - "color": TopicTracker(logger=self.get_logger()), - "depth": TopicTracker(logger=self.get_logger()) - } + "trackers": {}, } self.total_cpu_stats = make_stat() self.total_ram_stats = make_stat() + self.topic_subscriptions = {} + self.topic_configs = {} + self.discovered_streams = {} + self.discovery_complete = False + self.discovery_timer = None + self.timer = None + self.requested_streams = self.parse_requested_topics(topics) # CSV self.csv_file = csv_file self.csv_fh = open(self.csv_file, "w", newline="") self.csv_writer = csv.writer(self.csv_fh) - self.csv_writer.writerow(self.build_csv_header()) - # subscriptions + # Device status subscriptions are always present. Data subscriptions + # are created once after automatic discovery or explicit selection. for camera_name in self.camera_names: ns = self.camera_namespace(camera_name) self.create_subscription( @@ -177,29 +243,26 @@ class CameraMonitorNode(Node): lambda msg, name=camera_name: self.status_callback(msg, name), 5 ) - self.create_subscription( - Image, - f"{ns}/color/image_raw", - lambda msg, name=camera_name: self.image_callback(msg, name, "color"), - 5 - ) - self.create_subscription( - Image, - f"{ns}/depth/image_raw", - lambda msg, name=camera_name: self.image_callback(msg, name, "depth"), - 5 - ) - # timer runs every 1s to update system stats, log csv and print status - self.timer = self.create_timer(1.0, self.timer_callback) + if self.requested_streams: + self.finish_topic_discovery(self.requested_streams) + else: + self.discovery_start_monotonic = time.monotonic() + self.discovery_timer = self.create_timer( + DISCOVERY_INTERVAL_SECONDS, self.update_topic_discovery + ) def timer_callback(self): + if not self.discovery_complete: + return + elapsed = time.time() - self.start_time if elapsed > self.run_time: self.finish() rclpy.shutdown() return + self.update_topic_fps_stats() camera_sys_stats, total_cpu, total_ram, self.total_node_name = self.get_camera_stats() for camera_name in self.camera_names: camera = self.cameras[camera_name] @@ -220,7 +283,8 @@ class CameraMonitorNode(Node): return self.finished = True - elapsed = time.time() - self.start_time + timer_start = self.start_time or self.discovery_start_time + elapsed = time.time() - timer_start try: self.csv_fh.close() except Exception: @@ -231,6 +295,150 @@ class CameraMonitorNode(Node): def camera_namespace(self, camera_name): return "/" + camera_name.strip("/") + def image_topic_name(self, camera_name, stream): + return f"{self.camera_namespace(camera_name)}/{stream}/image_raw" + + def make_raw_image_config(self, camera_name, stream): + return { + "camera_name": camera_name, + "topic_id": stream, + "topic_name": self.image_topic_name(camera_name, stream), + "msg_type": Image, + } + + def parse_full_topic(self, topic_name): + normalized_topic = "/" + topic_name.strip("/") + for camera_name in self.camera_names: + namespace_prefix = self.camera_namespace(camera_name) + "/" + if not normalized_topic.startswith(namespace_prefix): + continue + + relative_name = normalized_topic[len(namespace_prefix):] + for stream in MONITORED_STREAMS: + raw_name = f"{stream}/image_raw" + if relative_name == raw_name: + return self.make_raw_image_config(camera_name, stream) + if relative_name == f"{raw_name}/compressed": + return { + "camera_name": camera_name, + "topic_id": f"{stream}_compressed", + "topic_name": normalized_topic, + "msg_type": CompressedImage, + } + if relative_name == f"{raw_name}/compressedDepth": + return { + "camera_name": camera_name, + "topic_id": f"{stream}_compressed_depth", + "topic_name": normalized_topic, + "msg_type": CompressedImage, + } + + point_cloud_ids = { + "depth/points": "depth_points", + "depth_registered/points": "depth_registered_points", + } + if relative_name in point_cloud_ids: + return { + "camera_name": camera_name, + "topic_id": point_cloud_ids[relative_name], + "topic_name": normalized_topic, + "msg_type": PointCloud2, + } + return None + + def parse_requested_topics(self, topics): + if not topics: + return {} + + requested = str(topics).replace(";", ",").split(",") + selected = {} + for value in requested: + value = value.strip() + if not value: + continue + if value in MONITORED_STREAMS: + for camera_name in self.camera_names: + config = self.make_raw_image_config(camera_name, value) + selected[(camera_name, config["topic_id"])] = config + continue + + config = self.parse_full_topic(value) + if config is None: + raise ValueError( + f"Unsupported topic '{value}'. Specify a raw or compressed image topic, " + "or a depth/points or depth_registered/points topic under a configured " + "camera namespace." + ) + selected[(config["camera_name"], config["topic_id"])] = config + return selected + + def find_published_image_streams(self): + published_streams = {} + for camera_name in self.camera_names: + for stream in MONITORED_STREAMS: + config = self.make_raw_image_config(camera_name, stream) + key = (camera_name, config["topic_id"]) + if self.count_publishers(config["topic_name"]) > 0: + published_streams[key] = config + return published_streams + + def update_topic_discovery(self): + if self.discovery_complete: + return + + self.discovered_streams.update(self.find_published_image_streams()) + elapsed = time.monotonic() - self.discovery_start_monotonic + if elapsed >= DISCOVERY_DURATION_SECONDS: + self.finish_topic_discovery(self.discovered_streams) + + def finish_topic_discovery(self, selected_streams): + self.topic_configs = dict(selected_streams) + sample_start_time = time.monotonic() + for key, config in self.topic_configs.items(): + camera_name = config["camera_name"] + topic_id = config["topic_id"] + self.cameras[camera_name]["trackers"][topic_id] = TopicTracker( + logger=self.get_logger(), sample_start_time=sample_start_time + ) + self.topic_subscriptions[key] = self.create_subscription( + config["msg_type"], + config["topic_name"], + lambda msg, name=camera_name, selected_topic_id=topic_id: self.topic_callback( + msg, name, selected_topic_id + ), + 5, + ) + + self.csv_writer.writerow(self.build_csv_header()) + self.csv_fh.flush() + self.start_time = time.time() + self.discovery_complete = True + self.timer = self.create_timer(1.0, self.timer_callback) + if self.discovery_timer is not None: + self.discovery_timer.cancel() + + def topics_for_camera(self, camera_name): + return [ + config + for config in self.topic_configs.values() + if config["camera_name"] == camera_name + ] + + def update_topic_fps_stats(self): + sample_time = time.monotonic() + for config in self.topic_configs.values(): + camera = self.cameras[config["camera_name"]] + topic_id = config["topic_id"] + fps_sample = camera["trackers"][topic_id].sample_fps(sample_time) + if fps_sample is None: + continue + current_fps, average_fps = fps_sample + self.update_sample_stat( + camera["stats"][f"{topic_id}_fps"], + current_fps, + average=average_fps, + ) + def cmdline_has_camera_namespace(self, cmdline_args, camera_name): ns = self.camera_namespace(camera_name) candidates = [ @@ -311,32 +519,30 @@ class CameraMonitorNode(Node): camera["prev_online"] = msg.device_online - # update stats from DeviceStatus message fields - self.update_stats(camera["stats"], "color_fps", msg.color_frame_rate_cur, msg.color_frame_rate_min, msg.color_frame_rate_max, msg.color_frame_rate_avg) - self.update_stats(camera["stats"], "color_delay", msg.color_delay_ms_cur, msg.color_delay_ms_min, msg.color_delay_ms_max, msg.color_delay_ms_avg) - self.update_stats(camera["stats"], "depth_fps", msg.depth_frame_rate_cur, msg.depth_frame_rate_min, msg.depth_frame_rate_max, msg.depth_frame_rate_avg) - self.update_stats(camera["stats"], "depth_delay", msg.depth_delay_ms_cur, msg.depth_delay_ms_min, msg.depth_delay_ms_max, msg.depth_delay_ms_avg) - - def image_callback(self, msg: Image, camera_name: str, stream: str): - if stream not in ("color", "depth"): - return - header = msg.header + def topic_callback(self, msg, camera_name: str, topic_id: str): + self.first_data_collected = True camera = self.cameras[camera_name] - tracker = camera["trackers"][stream] - # Prefer a user-specified ideal fps for drop detection when provided. - fps_to_use = self.ideal_fps if (self.ideal_fps and self.ideal_fps > 0.0) else camera["stats"][f"{stream}_fps"]["avg"] - tracker.on_msg(header, fps_to_use) + camera["data_collected"] = True + tracker = camera["trackers"][topic_id] + ros_receive_time = self.get_clock().now().nanoseconds * 1e-9 + delay_ms = tracker.on_msg( + msg.header, + ros_receive_time, + self.ideal_fps, + ) - def update_stats(self, stats, key, cur, min_val, max_val, avg_val): - if min_val <= 1e-3 or avg_val < 0: # ignore invalid data - return - s = stats[key] - s["cur"] = (cur) - s["count"] += 1 - s["sum"] += avg_val - s["avg"] = s["sum"] / s["count"] if s["count"] > 0 else 0.0 - s["min"] = min(s["min"], min_val) - s["max"] = max(s["max"], max_val) + if delay_ms is not None: + self.update_sample_stat( + camera["stats"][f"{topic_id}_delay"], delay_ms + ) + + def update_sample_stat(self, stat, value, average=None): + stat["cur"] = value + stat["count"] += 1 + stat["sum"] += value + stat["avg"] = average if average is not None else stat["sum"] / stat["count"] + stat["min"] = min(stat["min"], value) + stat["max"] = max(stat["max"], value) def update_sys_stat(self, stat_dict, value, online=True): stat_dict["cur"] = value @@ -353,7 +559,7 @@ class CameraMonitorNode(Node): row = [round(elapsed, 2)] for camera_name in self.camera_names: camera = self.cameras[camera_name] - row.extend(self.build_camera_csv_values(camera)) + row.extend(self.build_camera_csv_values(camera_name, camera)) row.extend([ round(self.total_cpu_stats["cur"], 2), round(self.total_cpu_stats["avg"], 2), @@ -365,19 +571,42 @@ class CameraMonitorNode(Node): def build_csv_header(self): header = ["time(s)"] - camera_fields = [ - "connection_type", "status_online", "disconnects", - "color_fps_cur", "color_fps_avg", "color_fps_min", "color_fps_max", - "color_delay_cur", "color_delay_avg", "color_delay_min", "color_delay_max", - "depth_fps_cur", "depth_fps_avg", "depth_fps_min", "depth_fps_max", - "depth_delay_cur", "depth_delay_avg", "depth_delay_min", "depth_delay_max", - "cpu_cur", "cpu_avg", "cpu_min", "cpu_max", - "ram_cur", "ram_avg", "ram_min", "ram_max", - "color_frames_loss", "color_frames_loss_rate(%)", - "depth_frames_loss", "depth_frames_loss_rate(%)" - ] for camera_name in self.camera_names: - header.extend([f"{camera_name}_{field}" for field in camera_fields]) + header.extend( + [ + f"{camera_name}_connection_type", + f"{camera_name}_status_online", + f"{camera_name}_disconnects", + ] + ) + for config in self.topics_for_camera(camera_name): + topic_id = config["topic_id"] + header.extend( + [ + f"{camera_name}_{topic_id}_fps_cur", + f"{camera_name}_{topic_id}_fps_avg", + f"{camera_name}_{topic_id}_fps_min", + f"{camera_name}_{topic_id}_fps_max", + f"{camera_name}_{topic_id}_delay_cur", + f"{camera_name}_{topic_id}_delay_avg", + f"{camera_name}_{topic_id}_delay_min", + f"{camera_name}_{topic_id}_delay_max", + f"{camera_name}_{topic_id}_sub_lost_count", + f"{camera_name}_{topic_id}_sub_lost_rate(%)", + ] + ) + header.extend( + [ + f"{camera_name}_cpu_cur", + f"{camera_name}_cpu_avg", + f"{camera_name}_cpu_min", + f"{camera_name}_cpu_max", + f"{camera_name}_ram_cur", + f"{camera_name}_ram_avg", + f"{camera_name}_ram_min", + f"{camera_name}_ram_max", + ] + ) header.extend([ "total_cpu_cur", "total_cpu_avg", "total_cpu_min", "total_cpu_max", @@ -385,10 +614,7 @@ class CameraMonitorNode(Node): ]) return header - def build_camera_csv_values(self, camera): - color_tracker = camera["trackers"]["color"] - depth_tracker = camera["trackers"]["depth"] - + def build_camera_csv_values(self, camera_name, camera): def safe(k): v = camera["stats"].get(k, {}) return ( @@ -398,29 +624,47 @@ class CameraMonitorNode(Node): self.format_csv_number(v.get("max", 0.0)), ) - if not camera["prev_online"]: - return [ - camera["connection_type"], camera["prev_online"], camera["disconnect_count"], - *["N/A"] * 16, - round(camera["cpu_stats"]["cur"], 2), "N/A", "N/A", "N/A", - round(camera["ram_stats"]["cur"], 2), "N/A", "N/A", "N/A", - color_tracker.drop_frames, round(color_tracker.frames_loss_rate() * 100.0, 3), - depth_tracker.drop_frames, round(depth_tracker.frames_loss_rate() * 100.0, 3) - ] - - return [ - camera["connection_type"], camera["prev_online"], camera["disconnect_count"], - *safe("color_fps"), - *safe("color_delay"), - *safe("depth_fps"), - *safe("depth_delay"), - round(camera["cpu_stats"]["cur"], 2), round(camera["cpu_stats"]["avg"], 2), - self.format_csv_number(camera["cpu_stats"]["min"]), self.format_csv_number(camera["cpu_stats"]["max"]), - round(camera["ram_stats"]["cur"], 2), round(camera["ram_stats"]["avg"], 2), - self.format_csv_number(camera["ram_stats"]["min"]), self.format_csv_number(camera["ram_stats"]["max"]), - color_tracker.drop_frames, round(color_tracker.frames_loss_rate() * 100.0, 3), - depth_tracker.drop_frames, round(depth_tracker.frames_loss_rate() * 100.0, 3) + values = [ + camera["connection_type"], + camera["prev_online"], + camera["disconnect_count"], ] + for config in self.topics_for_camera(camera_name): + topic_id = config["topic_id"] + tracker = camera["trackers"][topic_id] + if camera["prev_online"]: + values.extend(safe(f"{topic_id}_fps")) + values.extend(safe(f"{topic_id}_delay")) + else: + values.extend(["N/A"] * 8) + values.extend( + [ + tracker.drop_frames, + round(tracker.frames_loss_rate() * 100.0, 3), + ] + ) + + if camera["prev_online"]: + values.extend( + [ + round(camera["cpu_stats"]["cur"], 2), + round(camera["cpu_stats"]["avg"], 2), + self.format_csv_number(camera["cpu_stats"]["min"]), + self.format_csv_number(camera["cpu_stats"]["max"]), + round(camera["ram_stats"]["cur"], 2), + round(camera["ram_stats"]["avg"], 2), + self.format_csv_number(camera["ram_stats"]["min"]), + self.format_csv_number(camera["ram_stats"]["max"]), + ] + ) + else: + values.extend( + [ + round(camera["cpu_stats"]["cur"], 2), "N/A", "N/A", "N/A", + round(camera["ram_stats"]["cur"], 2), "N/A", "N/A", "N/A", + ] + ) + return values def format_csv_number(self, value): if value == float("inf") or value == float("-inf"): @@ -436,25 +680,30 @@ class CameraMonitorNode(Node): rows = [] for camera_name in self.camera_names: camera = self.cameras[camera_name] - for stream in ["color", "depth"]: - fps_key = f"{stream}_fps" - delay_key = f"{stream}_delay" - topic_name = f"/{camera_name}/{stream}/image_raw" + for config in self.topics_for_camera(camera_name): + topic_id = config["topic_id"] + fps_key = f"{topic_id}_fps" + delay_key = f"{topic_id}_delay" + topic_name = config["topic_name"] if not camera["prev_online"]: rows.append([camera_name, topic_name, *["N/A"] * 10]) else: fps_vals = format_stats(camera["stats"][fps_key]) delay_vals = format_stats(camera["stats"][delay_key]) - tracker = camera["trackers"][stream] + tracker = camera["trackers"][topic_id] frames_loss = tracker.drop_frames frames_loss_rate = round(tracker.frames_loss_rate() * 100.0, 3) rows.append([camera_name, topic_name, *fps_vals, *delay_vals, frames_loss, frames_loss_rate]) - header_bottom = ["Camera", "Topic", "fps_cur", "fps_avg", "fps_min", "fps_max", "delay_cur(ms)", "delay_avg(ms)", "delay_min(ms)", "delay_max(ms)", "Pub_lost_count", "Pub_lost_rate(%)"] + header_bottom = [ + "Camera", "Topic", "fps_cur", "fps_avg", "fps_min", "fps_max", + "delay_cur(ms)", "delay_avg(ms)", "delay_min(ms)", "delay_max(ms)", + "Sub_lost_count", "Sub_lost_rate(%)", + ] os.system("clear") - print("Orbbec Camera Benchmark\n") + print("Orbbec Camera Subscriber Benchmark\n") print(tabulate([header_bottom] + rows, tablefmt="fancy_grid")) sys_rows = [] @@ -491,13 +740,36 @@ def main(argv=None): ) parser.add_argument("--run_time", type=str, default="10s", help="Total run time for monitoring, e.g., 10s, 5m, 1h.") parser.add_argument("--csv_file", type=str, default="camera_monitor_log.csv") - parser.add_argument("--ideal_fps", type=float, default=0.0, help="Optional ideal frame rate to use for drop detection (overrides reported avg).") + parser.add_argument( + "--ideal_fps", + type=float, + default=0.0, + help=( + "Optional ideal frame rate for subscriber-side drop detection; " + "otherwise it is learned from image timestamps." + ), + ) parser.add_argument("--camera_names", type=str, default="camera", help="Comma-separated camera namespaces, e.g., camera,camera01,camera02.") + parser.add_argument( + "--topics", + type=str, + default="", + help=( + "Comma-separated raw stream names or full raw/compressed image and point cloud " + "topics. Automatic discovery only selects raw image topics." + ), + ) cli_args, _ = parser.parse_known_args(argv) rclpy.init(args=argv) run_time = parse_duration(cli_args.run_time) - node = CameraMonitorNode(run_time, cli_args.csv_file, ideal_fps=cli_args.ideal_fps, camera_names=cli_args.camera_names) + node = CameraMonitorNode( + run_time, + cli_args.csv_file, + ideal_fps=cli_args.ideal_fps, + camera_names=cli_args.camera_names, + topics=cli_args.topics, + ) try: rclpy.spin(node) diff --git a/orbbec_camera/src/frame_timestamp_csv_logger.cpp b/orbbec_camera/src/frame_timestamp_csv_logger.cpp index 9100dcc1..474054aa 100644 --- a/orbbec_camera/src/frame_timestamp_csv_logger.cpp +++ b/orbbec_camera/src/frame_timestamp_csv_logger.cpp @@ -31,6 +31,47 @@ int64_t getExpectedIntervalUs(const std::shared_ptr &frame) { return static_cast(1000000.0 / static_cast(fps)); } +std::optional outputModeForStream(OBStreamType stream_type) { + using OutputMode = FrameTimestampCsvLogger::OutputMode; + switch (stream_type) { + case OB_STREAM_COLOR: + return OutputMode::COLOR; + case OB_STREAM_COLOR_LEFT: + return OutputMode::LEFT_COLOR; + case OB_STREAM_COLOR_RIGHT: + return OutputMode::RIGHT_COLOR; + case OB_STREAM_DEPTH: + return OutputMode::DEPTH; + case OB_STREAM_IR_LEFT: + return OutputMode::LEFT_IR; + case OB_STREAM_IR_RIGHT: + return OutputMode::RIGHT_IR; + default: + return std::nullopt; + } +} + +const char *outputModeName(FrameTimestampCsvLogger::OutputMode output_mode) { + using OutputMode = FrameTimestampCsvLogger::OutputMode; + switch (output_mode) { + case OutputMode::COLOR: + return "color"; + case OutputMode::LEFT_COLOR: + return "left_color"; + case OutputMode::RIGHT_COLOR: + return "right_color"; + case OutputMode::DEPTH: + return "depth"; + case OutputMode::LEFT_IR: + return "left_ir"; + case OutputMode::RIGHT_IR: + return "right_ir"; + case OutputMode::SYNCED: + return "synced"; + } + return "unknown"; +} + } // namespace FrameTimestampCsvLogger::FrameTimestampCsvLogger(bool drop_log_enabled, @@ -104,9 +145,8 @@ void FrameTimestampCsvLogger::recordStandaloneFrameArrival(OBStreamType stream_t int64_t arrival_system_us, int64_t arrival_steady_us, bool image_publish_expected) { - if (!enabled_ || !frame || !isTrackedStream(stream_type) || - (stream_type == OB_STREAM_COLOR && output_mode_ != OutputMode::COLOR) || - (stream_type == OB_STREAM_DEPTH && output_mode_ != OutputMode::DEPTH)) { + const auto expected_output_mode = outputModeForStream(stream_type); + if (!enabled_ || !frame || !expected_output_mode || output_mode_ != *expected_output_mode) { return; } recordStandaloneFrameArrivalInternal(stream_type, frame, arrival_system_us, arrival_steady_us, @@ -163,14 +203,11 @@ void FrameTimestampCsvLogger::shutdown() { FrameTimestampCsvLogger::TrackedStream FrameTimestampCsvLogger::toTrackedStream( OBStreamType stream_type) const { - if (stream_type == OB_STREAM_COLOR) { - return TrackedStream::COLOR; - } - return TrackedStream::DEPTH; + return stream_type == OB_STREAM_DEPTH ? TrackedStream::DEPTH : TrackedStream::COLOR; } bool FrameTimestampCsvLogger::isTrackedStream(OBStreamType stream_type) const { - return stream_type == OB_STREAM_COLOR || stream_type == OB_STREAM_DEPTH; + return outputModeForStream(stream_type).has_value(); } void FrameTimestampCsvLogger::recordFrameSetInternal( @@ -294,10 +331,12 @@ void FrameTimestampCsvLogger::completeImagePublishInternal( auto row_id_it = row_map.find(frame_index); if (row_id_it == row_map.end()) { if (publish_system_us.has_value()) { - RCLCPP_WARN_STREAM(logger_, - "Frame timestamp CSV logger missed row mapping for stream " - << (tracked_stream == TrackedStream::COLOR ? "color" : "depth") - << " frame index " << frame_index); + RCLCPP_WARN_STREAM( + logger_, "Frame timestamp CSV logger missed row mapping for stream " + << (output_mode_ == OutputMode::SYNCED + ? (tracked_stream == TrackedStream::COLOR ? "color" : "depth") + : outputModeName(output_mode_)) + << " frame index " << frame_index); } return; } @@ -357,7 +396,10 @@ void FrameTimestampCsvLogger::populateArrivalData(StreamState &state, TrackedStr previous.dropped_frames += lost_frames; RCLCPP_WARN_STREAM(logger_, "Frame drop detected: stage=SDK_RECEIVE" - << " stream=" << (stream == TrackedStream::COLOR ? "color" : "depth") + << " stream=" + << (output_mode_ == OutputMode::SYNCED + ? (stream == TrackedStream::COLOR ? "color" : "depth") + : outputModeName(output_mode_)) << " frame_index=" << state.frame_index << " dropped=" << previous.dropped_frames); } @@ -408,7 +450,10 @@ void FrameTimestampCsvLogger::populatePublishData(StreamState &state, TrackedStr previous.publish_dropped_frames += lost_frames; RCLCPP_WARN_STREAM(logger_, "Frame drop detected: stage=ROS_PUBLISH" - << " stream=" << (stream == TrackedStream::COLOR ? "color" : "depth") + << " stream=" + << (output_mode_ == OutputMode::SYNCED + ? (stream == TrackedStream::COLOR ? "color" : "depth") + : outputModeName(output_mode_)) << " frame_index=" << state.frame_index << " dropped=" << previous.publish_dropped_frames); } @@ -485,12 +530,12 @@ void FrameTimestampCsvLogger::eraseFrameIndexMappingLocked(const PendingRow &row } std::string FrameTimestampCsvLogger::serializeRow(const PendingRow &row) const { - if (output_mode_ == OutputMode::COLOR) { - return serializeStreamColumns(row.color); - } if (output_mode_ == OutputMode::DEPTH) { return serializeStreamColumns(row.depth); } + if (output_mode_ != OutputMode::SYNCED) { + return serializeStreamColumns(row.color); + } std::ostringstream ss; ss << serializeStreamColumns(row.color) << "," << serializeStreamColumns(row.depth); return ss.str(); @@ -560,14 +605,12 @@ std::string FrameTimestampCsvLogger::csvHeader() const { ss << prefix << "_sdk_delay_from_global_us,"; ss << prefix << "_sdk_delay_from_system_us"; }; - if (output_mode_ == OutputMode::SYNCED || output_mode_ == OutputMode::COLOR) { - append_stream_header("color"); - } if (output_mode_ == OutputMode::SYNCED) { + append_stream_header("color"); ss << ","; - } - if (output_mode_ == OutputMode::SYNCED || output_mode_ == OutputMode::DEPTH) { append_stream_header("depth"); + } else { + append_stream_header(outputModeName(output_mode_)); } return ss.str(); } @@ -641,10 +684,8 @@ void FrameTimestampCsvLogger::writerThreadMain() { std::string FrameTimestampCsvLogger::csvFilePathForIndex(uint64_t file_index) const { const std::filesystem::path original_path(csv_file_path_); std::string suffix; - if (output_mode_ == OutputMode::COLOR) { - suffix = "_color"; - } else if (output_mode_ == OutputMode::DEPTH) { - suffix = "_depth"; + if (output_mode_ != OutputMode::SYNCED) { + suffix = "_" + std::string(outputModeName(output_mode_)); } auto indexed_filename = original_path.stem().string() + suffix; diff --git a/orbbec_camera/src/ob_camera_node.cpp b/orbbec_camera/src/ob_camera_node.cpp index 3ed5f6a5..68879416 100644 --- a/orbbec_camera/src/ob_camera_node.cpp +++ b/orbbec_camera/src/ob_camera_node.cpp @@ -743,7 +743,11 @@ OBCameraNode::OBCameraNode(rclcpp::Node *node, std::shared_ptr devic timestamp_config.csv_file_path = frame_timestamp_csv_file_; timestamp_config.frame_sync_enabled = enable_frame_sync_; timestamp_config.color_enabled = enable_stream_[COLOR]; + timestamp_config.left_color_enabled = enable_stream_[COLOR_LEFT]; + timestamp_config.right_color_enabled = enable_stream_[COLOR_RIGHT]; timestamp_config.depth_enabled = enable_stream_[DEPTH]; + timestamp_config.left_ir_enabled = enable_stream_[INFRA1]; + timestamp_config.right_ir_enabled = enable_stream_[INFRA2]; timestamp_config.imu_sync_enabled = enable_sync_output_accel_gyro_; timestamp_config.accel_enabled = enable_stream_[ACCEL]; timestamp_config.gyro_enabled = enable_stream_[GYRO]; @@ -763,6 +767,8 @@ OBCameraNode::OBCameraNode(rclcpp::Node *node, std::shared_ptr devic is_camera_node_initialized_ = true; fps_counter_color_ = std::make_unique("Color", logger_, 1); + fps_counter_left_color_ = std::make_unique("Left Color", logger_, 1); + fps_counter_right_color_ = std::make_unique("Right Color", logger_, 1); fps_counter_depth_ = std::make_unique("Depth", logger_, 1); fps_counter_left_ir_ = std::make_unique("Left Ir", logger_, 1); fps_counter_right_ir_ = std::make_unique("Right Ir", logger_, 1); @@ -772,12 +778,18 @@ OBCameraNode::OBCameraNode(rclcpp::Node *node, std::shared_ptr devic log_level = LogLevel::INFO; } fps_counter_color_->setLogLevel(log_level); + fps_counter_left_color_->setLogLevel(log_level); + fps_counter_right_color_->setLogLevel(log_level); 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 @@ -3237,7 +3249,7 @@ void OBCameraNode::setupLeftIrPostProcessFilter() { } auto device_info = device_->getDeviceInfo(); CHECK_NOTNULL(device_info); - if (isGemini330SeriesPID(pid_)) { + if (isGemini330SeriesPID(pid_) || isGemini305SeriesPID(pid_)) { auto left_ir_sensor = device_->getSensor(OB_SENSOR_IR_LEFT); left_ir_filter_list_ = left_ir_sensor->createRecommendedFilters(); if (left_ir_filter_list_.empty()) { @@ -3278,7 +3290,7 @@ void OBCameraNode::setupRightIrPostProcessFilter() { } auto device_info = device_->getDeviceInfo(); CHECK_NOTNULL(device_info); - if (isGemini330SeriesPID(pid_)) { + if (isGemini330SeriesPID(pid_) || isGemini305SeriesPID(pid_)) { auto right_ir_sensor = device_->getSensor(OB_SENSOR_IR_RIGHT); right_ir_filter_list_ = right_ir_sensor->createRecommendedFilters(); if (right_ir_filter_list_.empty()) { @@ -3537,35 +3549,44 @@ void OBCameraNode::selectBaseStream() { void OBCameraNode::printSensorProfiles(const std::shared_ptr &sensor) { auto profiles = sensor->getStreamProfileList(); + const auto sensor_type = sensor->getType(); for (size_t i = 0; i < profiles->getCount(); i++) { auto origin_profile = profiles->getProfile(i); - if (sensor->getType() == OB_SENSOR_COLOR) { + if (sensor_type == OB_SENSOR_COLOR || sensor_type == OB_SENSOR_COLOR_LEFT || + sensor_type == OB_SENSOR_COLOR_RIGHT) { auto profile = origin_profile->as(); - RCLCPP_INFO_STREAM( - logger_, "color profile: " << profile->getWidth() << "x" << profile->getHeight() << " " - << profile->getFps() << "fps " << profile->getFormat()); - } else if (sensor->getType() == OB_SENSOR_DEPTH) { + const char *stream_name = sensor_type == OB_SENSOR_COLOR_LEFT ? "left_color" + : sensor_type == OB_SENSOR_COLOR_RIGHT ? "right_color" + : "color"; + RCLCPP_INFO_STREAM(logger_, stream_name << " profile: " << profile->getWidth() << "x" + << profile->getHeight() << " " << profile->getFps() + << "fps " << profile->getFormat()); + } else if (sensor_type == OB_SENSOR_DEPTH) { auto profile = origin_profile->as(); RCLCPP_INFO_STREAM( logger_, "depth profile: " << profile->getWidth() << "x" << profile->getHeight() << " " << profile->getFps() << "fps " << profile->getFormat()); - } else if (sensor->getType() == OB_SENSOR_IR) { + } else if (sensor_type == OB_SENSOR_IR || sensor_type == OB_SENSOR_IR_LEFT || + sensor_type == OB_SENSOR_IR_RIGHT) { auto profile = origin_profile->as(); - RCLCPP_INFO_STREAM(logger_, "ir profile: " << profile->getWidth() << "x" - << profile->getHeight() << " " << profile->getFps() - << "fps " << profile->getFormat()); - } else if (sensor->getType() == OB_SENSOR_ACCEL) { + const char *stream_name = sensor_type == OB_SENSOR_IR_LEFT ? "left_ir" + : sensor_type == OB_SENSOR_IR_RIGHT ? "right_ir" + : "ir"; + RCLCPP_INFO_STREAM(logger_, stream_name << " profile: " << profile->getWidth() << "x" + << profile->getHeight() << " " << profile->getFps() + << "fps " << profile->getFormat()); + } else if (sensor_type == OB_SENSOR_ACCEL) { auto profile = origin_profile->as(); RCLCPP_INFO_STREAM(logger_, "accel profile: sampleRate " << profile->getSampleRate() << " full scale_range " << profile->getFullScaleRange()); - } else if (sensor->getType() == OB_SENSOR_GYRO) { + } else if (sensor_type == OB_SENSOR_GYRO) { auto profile = origin_profile->as(); RCLCPP_INFO_STREAM(logger_, "gyro profile: sampleRate " << profile->getSampleRate() << " full scale_range " << profile->getFullScaleRange()); } else { - RCLCPP_INFO_STREAM(logger_, "unknown profile: " << magic_enum::enum_name(sensor->getType())); + RCLCPP_INFO_STREAM(logger_, "unknown profile: " << magic_enum::enum_name(sensor_type)); } } } @@ -3667,11 +3688,11 @@ void OBCameraNode::setupProfiles() { if (selected_profile->format() == OB_FORMAT_BGRA) { images_[elem] = cv::Mat(height_[elem], width_[elem], CV_8UC4, cv::Scalar(0, 0, 0, 0)); encoding_[elem] = sensor_msgs::image_encodings::BGRA8; - unit_step_size_[COLOR] = 4 * sizeof(uint8_t); + unit_step_size_[elem] = 4 * sizeof(uint8_t); } else if (selected_profile->format() == OB_FORMAT_RGBA) { images_[elem] = cv::Mat(height_[elem], width_[elem], CV_8UC4, cv::Scalar(0, 0, 0, 0)); encoding_[elem] = sensor_msgs::image_encodings::RGBA8; - unit_step_size_[COLOR] = 4 * sizeof(uint8_t); + unit_step_size_[elem] = 4 * sizeof(uint8_t); } else { images_[elem] = cv::Mat(height_[elem], width_[elem], image_format_[elem], cv::Scalar(0, 0, 0)); @@ -6501,6 +6522,20 @@ void OBCameraNode::onNewFrameSetCallback(std::shared_ptr frame_set final_color_frame, final_depth_frame, frame_set_arrival_system_us, frame_set_arrival_steady_us, track_color, track_depth, color_publish_expected, depth_publish_expected); + + const auto record_side_stream = [&](const stream_index_pair &stream_index, + OBFrameType frame_type) { + auto frame = frame_set->getFrame(frame_type); + if (enable_stream_[stream_index] && frame) { + timestamp_csv_logger_->recordImageFrameArrival(stream_index.first, frame, + frame_set_arrival_system_us, + frame_set_arrival_steady_us, true); + } + }; + record_side_stream(COLOR_LEFT, OB_FRAME_COLOR_LEFT); + record_side_stream(COLOR_RIGHT, OB_FRAME_COLOR_RIGHT); + record_side_stream(INFRA1, OB_FRAME_IR_LEFT); + record_side_stream(INFRA2, OB_FRAME_IR_RIGHT); } try { @@ -6542,10 +6577,12 @@ void OBCameraNode::onNewFrameSetCallback(std::shared_ptr frame_set setColorAutoExposureROI(); left_color_frame = processColorFrameFilter(left_color_frame); frame_set->pushFrame(left_color_frame); + fps_counter_left_color_->tick(); } if (right_color_frame) { right_color_frame = processColorFrameFilter(right_color_frame); frame_set->pushFrame(right_color_frame); + fps_counter_right_color_->tick(); } if (left_ir_frame) { left_ir_frame = processLeftIrFrameFilter(left_ir_frame); @@ -7065,6 +7102,7 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr &frame, if (video_frame->getMetadataValue(OB_FRAME_METADATA_TYPE_HDR_SEQUENCE_INDEX) == interleave_skip_index_) { RCLCPP_DEBUG(logger_, "interleave filter skip frame type: %d", frame->getType()); + record_image_publish_skipped(); return; } } @@ -7147,13 +7185,19 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr &frame, } if ((stream_index == COLOR || stream_index == COLOR_LEFT || stream_index == COLOR_RIGHT) && frame->getFormat() == OB_FORMAT_MJPG && has_compressed_image_subscriber) { - if (!has_raw_image_subscriber && stream_index == COLOR && log_image_timestamps) { + if (!has_raw_image_subscriber && log_image_timestamps) { timestamp_csv_logger_->recordImagePrePublish(stream_index.first, frame, getSystemNowUs(), getSteadyNowUs()); } publishCompressedColorImage(frame, stream_index, timestamp, frame_id); - if (!has_raw_image_subscriber && stream_index == COLOR) { - fps_delay_status_color_->tick(frame_timestamp); + 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); + } } } @@ -7178,10 +7222,12 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr &frame, } if (frame->getType() == OB_FRAME_COLOR_LEFT && !is_left_color_frame_decoded_) { RCLCPP_ERROR(logger_, "left color frame is not decoded"); + record_image_publish_skipped(); return; } if (frame->getType() == OB_FRAME_COLOR_RIGHT && !is_right_color_frame_decoded_) { RCLCPP_ERROR(logger_, "right color frame is not decoded"); + record_image_publish_skipped(); return; } if (frame->getType() == OB_FRAME_COLOR) { @@ -7241,8 +7287,16 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr &frame, } 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); } image_publishers_[stream_index]->publish(std::move(image_msg)); } diff --git a/orbbec_camera/src/timestamp_csv_logger.cpp b/orbbec_camera/src/timestamp_csv_logger.cpp index acd4f150..987abdc1 100644 --- a/orbbec_camera/src/timestamp_csv_logger.cpp +++ b/orbbec_camera/src/timestamp_csv_logger.cpp @@ -29,6 +29,18 @@ TimestampCsvLogger::TimestampCsvLogger(Config config, rclcpp::Logger logger) depth_logger_ = create_image_logger(FrameTimestampCsvLogger::OutputMode::DEPTH); } } + if (config.left_color_enabled) { + left_color_logger_ = create_image_logger(FrameTimestampCsvLogger::OutputMode::LEFT_COLOR); + } + if (config.right_color_enabled) { + right_color_logger_ = create_image_logger(FrameTimestampCsvLogger::OutputMode::RIGHT_COLOR); + } + if (config.left_ir_enabled) { + left_ir_logger_ = create_image_logger(FrameTimestampCsvLogger::OutputMode::LEFT_IR); + } + if (config.right_ir_enabled) { + right_ir_logger_ = create_image_logger(FrameTimestampCsvLogger::OutputMode::RIGHT_IR); + } if (config.csv_file_path.empty()) { return; @@ -64,7 +76,12 @@ bool TimestampCsvLogger::enabled() const { bool TimestampCsvLogger::imageEnabled() const { return (synced_image_logger_ && synced_image_logger_->enabled()) || - (color_logger_ && color_logger_->enabled()) || (depth_logger_ && depth_logger_->enabled()); + (color_logger_ && color_logger_->enabled()) || + (left_color_logger_ && left_color_logger_->enabled()) || + (right_color_logger_ && right_color_logger_->enabled()) || + (depth_logger_ && depth_logger_->enabled()) || + (left_ir_logger_ && left_ir_logger_->enabled()) || + (right_ir_logger_ && right_ir_logger_->enabled()); } bool TimestampCsvLogger::imageStreamEnabled(OBStreamType stream_type) const { @@ -104,6 +121,18 @@ void TimestampCsvLogger::recordImageFrameSet(const std::shared_ptr &c } } +void TimestampCsvLogger::recordImageFrameArrival(OBStreamType stream_type, + const std::shared_ptr &frame, + int64_t arrival_system_us, + int64_t arrival_steady_us, + bool image_publish_expected) { + auto *timestamp_logger = imageLoggerForStream(stream_type); + if (timestamp_logger) { + timestamp_logger->recordStandaloneFrameArrival(stream_type, frame, arrival_system_us, + arrival_steady_us, image_publish_expected); + } +} + void TimestampCsvLogger::recordImagePrePublish(OBStreamType stream_type, const std::shared_ptr &frame, int64_t publish_system_us, @@ -164,7 +193,11 @@ void TimestampCsvLogger::shutdown() noexcept { shutdown_logger(synced_image_logger_, "synced image timestamp CSV logger"); shutdown_logger(color_logger_, "color timestamp CSV logger"); + shutdown_logger(left_color_logger_, "left color timestamp CSV logger"); + shutdown_logger(right_color_logger_, "right color timestamp CSV logger"); shutdown_logger(depth_logger_, "depth timestamp CSV logger"); + shutdown_logger(left_ir_logger_, "left IR timestamp CSV logger"); + shutdown_logger(right_ir_logger_, "right IR timestamp CSV logger"); shutdown_logger(synced_imu_logger_, "synced IMU timestamp CSV logger"); shutdown_logger(accel_logger_, "accel timestamp CSV logger"); shutdown_logger(gyro_logger_, "gyro timestamp CSV logger"); @@ -177,6 +210,18 @@ FrameTimestampCsvLogger *TimestampCsvLogger::imageLoggerForStream(OBStreamType s if (stream_type == OB_STREAM_DEPTH) { return synced_image_logger_ ? synced_image_logger_.get() : depth_logger_.get(); } + if (stream_type == OB_STREAM_COLOR_LEFT) { + return left_color_logger_.get(); + } + if (stream_type == OB_STREAM_COLOR_RIGHT) { + return right_color_logger_.get(); + } + if (stream_type == OB_STREAM_IR_LEFT) { + return left_ir_logger_.get(); + } + if (stream_type == OB_STREAM_IR_RIGHT) { + return right_ir_logger_.get(); + } return nullptr; } diff --git a/orbbec_camera/tools/list_camera_profile.cpp b/orbbec_camera/tools/list_camera_profile.cpp index 7f3673a6..3c54d8c7 100644 --- a/orbbec_camera/tools/list_camera_profile.cpp +++ b/orbbec_camera/tools/list_camera_profile.cpp @@ -154,8 +154,11 @@ void listSensorProfiles(const std::shared_ptr& device) { << " | width: " << profile->getDecimationConfig().originWidth << " height: " << profile->getDecimationConfig().originHeight << " downscale:" << profile->getDecimationConfig().factor << std::endl; - } else if (sensor->getType() == OB_SENSOR_COLOR || sensor->getType() == OB_SENSOR_DEPTH || - sensor->getType() == OB_SENSOR_IR || sensor->getType() == OB_SENSOR_IR_LEFT || + } else if (sensor->getType() == OB_SENSOR_COLOR || + sensor->getType() == OB_SENSOR_COLOR_LEFT || + sensor->getType() == OB_SENSOR_COLOR_RIGHT || + sensor->getType() == OB_SENSOR_DEPTH || sensor->getType() == OB_SENSOR_IR || + sensor->getType() == OB_SENSOR_IR_LEFT || sensor->getType() == OB_SENSOR_IR_RIGHT) { auto profile = origin_profile->as(); std::cout << magic_enum::enum_name(sensor->getType()) << " profile: " << profile->getWidth() diff --git a/orbbec_camera/tools/multi_save_rgbir.cpp b/orbbec_camera/tools/multi_save_rgbir.cpp index 8a1a608b..43901740 100644 --- a/orbbec_camera/tools/multi_save_rgbir.cpp +++ b/orbbec_camera/tools/multi_save_rgbir.cpp @@ -1,72 +1,114 @@ - #include #include -#include -#include -#include "orbbec_camera/ob_camera_node.h" -#include "orbbec_camera_msgs/msg/metadata.hpp" -#include + +#include +#include +#include +#include +#include #include -#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include "orbbec_camera/ob_camera_node.h" +#include "orbbec_camera/utils.h" +#include "orbbec_camera_msgs/msg/metadata.hpp" + namespace orbbec_camera { namespace tools { -struct ImageMetadata { - std::vector> exposure_buffs; - std::vector> gain_buffs; +namespace { + +const std::array kSupportedStreamNames = { + "color", "left_color", "right_color", "ir", "left_ir", "right_ir", +}; +constexpr auto kStreamDiscoveryPollInterval = std::chrono::milliseconds(100); +constexpr auto kStreamDiscoveryStablePeriod = std::chrono::seconds(1); +constexpr auto kStreamDiscoveryTimeout = std::chrono::seconds(5); +constexpr size_t kMinimumPendingFrameLimit = 30; + +struct StreamTopicInfo { + std::string name; + bool metadata_available = false; + + bool operator==(const StreamTopicInfo &other) const { + return name == other.name && metadata_available == other.metadata_available; + } +}; + +bool isSupportedStreamName(const std::string &stream_name) { + return std::find(kSupportedStreamNames.begin(), kSupportedStreamNames.end(), stream_name) != + kSupportedStreamNames.end(); +} + +bool isColorCaptureStreamName(const std::string &stream_name) { + return stream_name == "color" || stream_name == "left_color" || stream_name == "right_color"; +} + +std::string cameraNamespace(const std::string &camera_name) { + if (!camera_name.empty() && camera_name.front() == '/') { + return camera_name; + } + return "/" + camera_name; +} + +} // namespace + +struct StreamCapture { + struct FrameMetadata { + std::string exposure; + std::string gain; + }; + + struct PendingImage { + cv::Mat image; + std::string current_timestamp; + std::string receive_timestamp; + }; + + std::vector images; + std::vector current_timestamps; + std::vector receive_timestamps; + std::vector frame_metadata; + std::map pending_images; + std::map pending_metadata; + bool metadata_required = false; + + void clear() { + images.clear(); + current_timestamps.clear(); + receive_timestamps.clear(); + frame_metadata.clear(); + pending_images.clear(); + pending_metadata.clear(); + } }; class MultiCameraSubscriber : public rclcpp::Node { public: explicit MultiCameraSubscriber(const rclcpp::NodeOptions &options) : Node("MultiCameraSubscriber", options) { - device_init(); - } - ~MultiCameraSubscriber() { - ir_image_buffers_.clear(); - ir_current_timestamp_buffers_.clear(); - ir_timestamp_buffers_.clear(); - color_image_buffers_.clear(); - color_current_timestamp_buffers_.clear(); - color_timestamp_buffers_.clear(); - left_ir_metadata_.exposure_buffs.clear(); - left_ir_metadata_.gain_buffs.clear(); - color_metadata_.exposure_buffs.clear(); - color_metadata_.gain_buffs.clear(); - } - void device_init() { - try { - auto context = std::make_unique(); - context->setLoggerSeverity(OBLogSeverity::OB_LOG_SEVERITY_NONE); - auto list = context->queryDeviceList(); - for (size_t i = 0; i < list->deviceCount(); i++) { - auto device = list->getDevice(i); - auto device_info = device->getDeviceInfo(); - auto pid = device_info->getPid(); - std::string serial = device_info->serialNumber(); - std::string uid = device_info->uid(); - auto usb_port = parseUsbPort(uid); - serial_numbers_[usb_port] = serial; - is_gemini330_ = isGemini330SeriesPID(pid); - } - } catch (ob::Error &e) { - RCLCPP_ERROR_STREAM(get_logger(), orbbec_camera::formatObErrorWithStatus(e)); - } catch (const std::exception &e) { - RCLCPP_ERROR_STREAM(get_logger(), e.what()); - } catch (...) { - RCLCPP_ERROR_STREAM(get_logger(), "unknown error"); + initializeDeviceInfo(); + loadParameters(); + for (size_t i = 0; i < usb_ports_.size(); ++i) { + usb_index_map_[usb_ports_[i]] = static_cast(i); } - params_init(); - for (size_t i = 0; i < usb_params_.size(); i++) { - usb_numbers_[i] = usb_params_[i]; - usb_index_map_[usb_params_[i]] = i; - } - for (const auto &pair : serial_numbers_) { - RCLCPP_INFO_STREAM(rclcpp::get_logger("multi_camera_subscriber"), - "usb_port: " << pair.first << ", serial: " << pair.second); - } - for (const auto &pair : usb_index_map_) { - RCLCPP_INFO_STREAM(rclcpp::get_logger("multi_camera_subscriber"), - "usb_port: " << pair.first << ", index: " << pair.second); + for (const auto &entry : serial_numbers_) { + RCLCPP_INFO(get_logger(), "usb_port: %s, serial: %s", entry.first.c_str(), + entry.second.c_str()); } capture_control_srv_ = this->create_service( "start_capture", std::bind(&MultiCameraSubscriber::controlCaptureCallback, this, @@ -74,328 +116,462 @@ class MultiCameraSubscriber : public rclcpp::Node { } private: - std::mutex image_mutex_; - std::mutex meta_mutex_; - void params_init() { + void initializeDeviceInfo() { + try { + auto context = std::make_unique(); + context->setLoggerSeverity(OBLogSeverity::OB_LOG_SEVERITY_NONE); + auto list = context->queryDeviceList(); + for (size_t i = 0; i < list->deviceCount(); ++i) { + auto device_info = list->getDevice(i)->getDeviceInfo(); + const auto usb_port = parseUsbPort(device_info->uid()); + serial_numbers_[usb_port] = device_info->serialNumber(); + } + } catch (const ob::Error &e) { + RCLCPP_ERROR_STREAM(get_logger(), orbbec_camera::formatObErrorWithStatus(e)); + } catch (const std::exception &e) { + RCLCPP_ERROR_STREAM(get_logger(), e.what()); + } catch (...) { + RCLCPP_ERROR(get_logger(), "unknown error while querying devices"); + } + } + + void loadParameters() { std::ifstream file( "install/orbbec_camera/share/orbbec_camera/config/tools/multisavergbir/" "multi_save_rgbir_params.json"); if (!file.is_open()) { - RCLCPP_ERROR(this->get_logger(), "Failed to open JSON file."); + RCLCPP_ERROR(get_logger(), "Failed to open JSON file."); return; } + nlohmann::json json_data; file >> json_data; - time_domain_ = json_data["save_rgbir_params"]["time_domain"].get(); - time_domain_ = - (time_domain_ == "device") ? "_d" : (time_domain_ == "global" ? "_g" : "_unknown"); - usb_params_ = json_data["save_rgbir_params"]["usb_ports"].get>(); - camera_name_ = json_data["save_rgbir_params"]["camera_name"].get>(); - left_ir_topics_.resize(camera_name_.size()); - left_ir_metadata_topic_.resize(camera_name_.size()); - color_topics_.resize(camera_name_.size()); - color_metadata_topic_.resize(camera_name_.size()); - for (size_t i = 0; i < camera_name_.size(); ++i) { - left_ir_topics_[i] = - "/" + camera_name_[i] + "/" + (is_gemini330_ ? "left_ir" : "ir") + "/image_raw"; - left_ir_metadata_topic_[i] = - "/" + camera_name_[i] + "/" + (is_gemini330_ ? "left_ir" : "ir") + "/metadata"; - color_topics_[i] = "/" + camera_name_[i] + "/color/image_raw"; - color_metadata_topic_[i] = "/" + camera_name_[i] + "/color/metadata"; + const auto ¶ms = json_data["save_rgbir_params"]; + const auto time_domain = params["time_domain"].get(); + time_domain_suffix_ = + time_domain == "device" ? "_d" : (time_domain == "global" ? "_g" : "_unknown"); + usb_ports_ = params["usb_ports"].get>(); + camera_names_ = params["camera_name"].get>(); + + if (params.contains("stream_names")) { + for (const auto &stream_name : params["stream_names"].get>()) { + if (!isSupportedStreamName(stream_name)) { + throw std::invalid_argument("Unsupported stream name in multi_save_rgbir config: " + + stream_name); + } + if (std::find(configured_stream_names_.begin(), configured_stream_names_.end(), + stream_name) == configured_stream_names_.end()) { + configured_stream_names_.push_back(stream_name); + } + } } } - void topic_init() { - ir_image_buffers_.resize(left_ir_topics_.size()); - color_image_buffers_.resize(left_ir_topics_.size()); - ir_current_timestamp_buffers_.resize(left_ir_topics_.size()); - color_current_timestamp_buffers_.resize(left_ir_topics_.size()); - ir_timestamp_buffers_.resize(left_ir_topics_.size()); - color_timestamp_buffers_.resize(left_ir_topics_.size()); - left_ir_metadata_.exposure_buffs.resize(left_ir_topics_.size()); - left_ir_metadata_.gain_buffs.resize(left_ir_topics_.size()); - color_metadata_.exposure_buffs.resize(left_ir_topics_.size()); - color_metadata_.gain_buffs.resize(left_ir_topics_.size()); - callback_called_ = std::vector(left_ir_topics_.size(), false); - auto custom_qos = rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_default)); - rclcpp::CallbackGroup::SharedPtr reentrant_callback_group_; - RCLCPP_INFO_STREAM(rclcpp::get_logger("multi_camera_subscriber"), - "camera_name_.size(): " << camera_name_.size()); - for (size_t i = 0; i < camera_name_.size(); ++i) { - RCLCPP_INFO_STREAM(rclcpp::get_logger("multi_camera_subscriber"), - "left_ir_topic: " << left_ir_topics_[i]); - RCLCPP_INFO_STREAM(rclcpp::get_logger("multi_camera_subscriber"), - "left_ir_metadata_topic_: " << left_ir_metadata_topic_[i]); - RCLCPP_INFO_STREAM(rclcpp::get_logger("multi_camera_subscriber"), - "color_topic: " << color_topics_[i]); - RCLCPP_INFO_STREAM(rclcpp::get_logger("multi_camera_subscriber"), - "color_metadata_topic_: " << color_metadata_topic_[i]); - reentrant_callback_group_ = this->create_callback_group(rclcpp::CallbackGroupType::Reentrant); - rclcpp::SubscriptionOptions ir_sub_options; - ir_sub_options.callback_group = reentrant_callback_group_; - rclcpp::SubscriptionOptions color_sub_options; - color_sub_options.callback_group = reentrant_callback_group_; + std::vector discoverStreams(const std::string &camera_name) const { + std::vector streams; + const auto names_and_types = this->get_topic_names_and_types(); + const std::string prefix = cameraNamespace(camera_name) + "/"; + for (const auto &stream_name : kSupportedStreamNames) { + const auto image_topic_it = names_and_types.find(prefix + stream_name + "/image_raw"); + if (image_topic_it == names_and_types.end()) { + continue; + } + const auto &image_types = image_topic_it->second; + if (std::find(image_types.begin(), image_types.end(), "sensor_msgs/msg/Image") == + image_types.end()) { + continue; + } - auto ir_sub = this->create_subscription( - left_ir_topics_[i], custom_qos, - [this, i](std::shared_ptr msg) { - this->irCallback(msg, i); - }, - ir_sub_options); - - auto ir_metadata_sub = this->create_subscription( - left_ir_metadata_topic_[i], custom_qos, - [this, i](std::shared_ptr msg) { - this->ir_meta_Callback(msg, i); - }); - - auto color_sub = this->create_subscription( - color_topics_[i], custom_qos, - [this, i](std::shared_ptr msg) { - this->colorCallback(msg, i); - }, - color_sub_options); - - auto color_metadata_sub = this->create_subscription( - color_metadata_topic_[i], custom_qos, - [this, i](std::shared_ptr msg) { - this->color_meta_Callback(msg, i); - }); - - ir_subscribers_.push_back(ir_sub); - ir_meta_subscribers_.push_back(ir_metadata_sub); - color_subscribers_.push_back(color_sub); - color_meta_subscribers_.push_back(color_metadata_sub); + const auto metadata_topic_it = names_and_types.find(prefix + stream_name + "/metadata"); + const bool metadata_available = + metadata_topic_it != names_and_types.end() && + std::find(metadata_topic_it->second.begin(), metadata_topic_it->second.end(), + "orbbec_camera_msgs/msg/Metadata") != metadata_topic_it->second.end(); + streams.push_back(StreamTopicInfo{stream_name, metadata_available}); } + return streams; } - std::string getCurrentTimes() { - auto now = std::chrono::system_clock::now(); - auto now_time_t = std::chrono::system_clock::to_time_t(now); - std::tm tm = *std::localtime(&now_time_t); - std::ostringstream date_stream; - date_stream << std::put_time(&tm, "%Y%m%d%H%M%S"); - std::string date_str = date_stream.str(); - return date_str; + std::vector> waitForStableStreams() const { + if (!configured_stream_names_.empty()) { + std::vector> configured_streams; + configured_streams.reserve(camera_names_.size()); + for (const auto &camera_name : camera_names_) { + const auto discovered_streams = discoverStreams(camera_name); + std::vector camera_streams; + camera_streams.reserve(configured_stream_names_.size()); + for (const auto &stream_name : configured_stream_names_) { + const auto discovered_it = std::find_if( + discovered_streams.begin(), discovered_streams.end(), + [&stream_name](const auto &stream) { return stream.name == stream_name; }); + camera_streams.push_back(StreamTopicInfo{ + stream_name, + discovered_it != discovered_streams.end() && discovered_it->metadata_available}); + } + configured_streams.push_back(std::move(camera_streams)); + } + return configured_streams; + } + + std::vector> candidate; + auto candidate_since = std::chrono::steady_clock::time_point{}; + const auto deadline = std::chrono::steady_clock::now() + kStreamDiscoveryTimeout; + while (rclcpp::ok() && std::chrono::steady_clock::now() < deadline) { + std::vector> current; + current.reserve(camera_names_.size()); + bool all_cameras_discovered = !camera_names_.empty(); + for (const auto &camera_name : camera_names_) { + current.push_back(discoverStreams(camera_name)); + all_cameras_discovered = all_cameras_discovered && !current.back().empty(); + } + + const auto now = std::chrono::steady_clock::now(); + if (!all_cameras_discovered) { + candidate.clear(); + } else if (current != candidate) { + candidate = std::move(current); + candidate_since = now; + } else if (now - candidate_since >= kStreamDiscoveryStablePeriod) { + return candidate; + } + + std::this_thread::sleep_for(kStreamDiscoveryPollInterval); + } + + RCLCPP_WARN_STREAM(get_logger(), + "Supported image topics did not become stable within " + << kStreamDiscoveryTimeout.count() + << " seconds; start all cameras and streams first, retry the request, " + "or configure stream_names explicitly"); + return {}; } - std::string generateFolderName(const std::string &serial_number, size_t serial_index) { - std::string path = std::string("multicamera_sync/output/") + currenttimes_ + "/" + - "TotalModeFrames/" + "/" + "SN" + serial_number + "_Index" + - std::to_string(serial_index); + bool initializeTopics() { + const auto streams_by_camera = waitForStableStreams(); + if (streams_by_camera.size() != camera_names_.size()) { + return false; + } + + callback_groups_.clear(); + image_subscribers_.clear(); + metadata_subscribers_.clear(); + captures_.resize(camera_names_.size()); + callback_called_.assign(camera_names_.size(), false); + const auto custom_qos = + rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_default)); + + for (size_t camera_index = 0; camera_index < camera_names_.size(); ++camera_index) { + const auto &streams = streams_by_camera[camera_index]; + const std::string prefix = cameraNamespace(camera_names_[camera_index]) + "/"; + auto callback_group = this->create_callback_group(rclcpp::CallbackGroupType::Reentrant); + callback_groups_.push_back(callback_group); + rclcpp::SubscriptionOptions options; + options.callback_group = callback_group; + + for (const auto &stream : streams) { + const auto &stream_name = stream.name; + StreamCapture capture; + capture.metadata_required = stream.metadata_available; + captures_[camera_index].emplace(stream_name, std::move(capture)); + const std::string image_topic = prefix + stream_name + "/image_raw"; + RCLCPP_INFO(get_logger(), "Subscribing to %s", image_topic.c_str()); + + image_subscribers_.push_back(this->create_subscription( + image_topic, custom_qos, + [this, camera_index, + stream_name](const std::shared_ptr image) { + imageCallback(image, camera_index, stream_name); + }, + options)); + if (stream.metadata_available) { + const std::string metadata_topic = prefix + stream_name + "/metadata"; + metadata_subscribers_.push_back( + this->create_subscription( + metadata_topic, custom_qos, + [this, camera_index, stream_name]( + const std::shared_ptr metadata) { + metadataCallback(metadata, camera_index, stream_name); + }, + options)); + } + } + } + return !captures_.empty() && std::all_of(captures_.begin(), captures_.end(), + [](const auto &streams) { return !streams.empty(); }); + } + + std::string currentDateTime() const { + const auto now = std::chrono::system_clock::now(); + const auto now_time = std::chrono::system_clock::to_time_t(now); + const std::tm time_info = *std::localtime(&now_time); + std::ostringstream output; + output << std::put_time(&time_info, "%Y%m%d%H%M%S"); + return output.str(); + } + + std::string generateFolderName(const std::string &serial_number, size_t serial_index) const { + const std::string path = "multicamera_sync/output/" + current_date_time_ + + "/TotalModeFrames/SN" + serial_number + "_Index" + + std::to_string(serial_index); std::filesystem::create_directories(path); return path; } - std::string getTimestamp() { - auto now = this->get_clock()->now(); - int64_t seconds = now.seconds(); - int64_t nanoseconds = now.nanoseconds() % 1000000000; - int64_t milliseconds = nanoseconds / 1000000; + + std::string receiveTimestamp() const { + const auto now = this->get_clock()->now(); + const int64_t seconds = now.seconds(); + const int64_t milliseconds = now.nanoseconds() % 1000000000 / 1000000; return std::to_string(seconds) + std::to_string(milliseconds); } - std::string getCurrentTimestamp(const sensor_msgs::msg::Image::ConstSharedPtr &image_msg) { - int64_t seconds = image_msg->header.stamp.sec; - int64_t nanoseconds = image_msg->header.stamp.nanosec; - - int64_t milliseconds = nanoseconds / 1000000; + std::string imageTimestamp(const sensor_msgs::msg::Image::ConstSharedPtr &image) const { + const int64_t milliseconds = image->header.stamp.nanosec / 1000000; std::ostringstream timestamp; - timestamp << seconds << std::setw(3) << std::setfill('0') << milliseconds; - + timestamp << image->header.stamp.sec << std::setw(3) << std::setfill('0') << milliseconds; return timestamp.str(); } - void saveAlignedImages(size_t index) { - auto &ir_images = ir_image_buffers_[index]; - auto &ir_current_timestamps = ir_current_timestamp_buffers_[index]; - auto &ir_timestamps = ir_timestamp_buffers_[index]; - auto &color_images = color_image_buffers_[index]; - auto &color_current_timestamps = color_current_timestamp_buffers_[index]; - auto &color_timestamps = color_timestamp_buffers_[index]; - auto &left_ir_meta_exposure = left_ir_metadata_.exposure_buffs[index]; - auto &left_ir_meta_gain = left_ir_metadata_.gain_buffs[index]; - auto &color_meta_exposure = color_metadata_.exposure_buffs[index]; - auto &color_meta_gain = color_metadata_.gain_buffs[index]; - callback_called_[index] = true; - if (ir_images.size() < static_cast(saving_images_number_) || - color_images.size() < static_cast(saving_images_number_)) { + bool captureReady(size_t camera_index) const { + if (camera_index >= captures_.size() || captures_[camera_index].empty()) { + return false; + } + return std::all_of( + captures_[camera_index].begin(), captures_[camera_index].end(), [this](const auto &entry) { + return entry.second.images.size() >= static_cast(saving_images_number_); + }); + } + + int64_t messageStampNs(const std_msgs::msg::Header &header) const { + return static_cast(header.stamp.sec) * 1000000000LL + header.stamp.nanosec; + } + + size_t pendingFrameLimit() const { + return std::max(kMinimumPendingFrameLimit, static_cast(saving_images_number_) * 2); + } + + template + void trimPendingFrames(std::map &pending) const { + while (pending.size() > pendingFrameLimit()) { + pending.erase(pending.begin()); + } + } + + void appendCompletedFrame(StreamCapture &capture, StreamCapture::PendingImage pending_image, + StreamCapture::FrameMetadata metadata = {}) { + if (capture.images.size() >= static_cast(saving_images_number_)) { return; } - RCLCPP_INFO_STREAM(rclcpp::get_logger("multi_camera_subscriber"), "index:" << index); - auto usb_iter = usb_index_map_.find(usb_numbers_[index]); - auto serial_iter = serial_numbers_.find(usb_numbers_[index]); - int usb_index = usb_iter->second; - if (serial_iter == serial_numbers_.end()) { - RCLCPP_INFO_STREAM(rclcpp::get_logger("multi_camera_subscriber"), "serial_iter is empty"); + capture.images.push_back(std::move(pending_image.image)); + capture.current_timestamps.push_back(std::move(pending_image.current_timestamp)); + capture.receive_timestamps.push_back(std::move(pending_image.receive_timestamp)); + capture.frame_metadata.push_back(std::move(metadata)); + } + + std::string metadataSuffix(const StreamCapture &capture, size_t frame_index) const { + if (frame_index >= capture.frame_metadata.size()) { + return ""; + } + std::string suffix; + if (!capture.frame_metadata[frame_index].exposure.empty()) { + suffix += "_e" + capture.frame_metadata[frame_index].exposure; + } + if (!capture.frame_metadata[frame_index].gain.empty()) { + suffix += "_d" + capture.frame_metadata[frame_index].gain; + } + return suffix; + } + + void saveImages(size_t camera_index) { + if (!captureReady(camera_index)) { return; } - std::string serial_index = serial_iter->second; + if (camera_index >= usb_ports_.size()) { + RCLCPP_ERROR(get_logger(), "Missing USB port configuration for camera index %zu", + camera_index); + return; + } + const auto serial_it = serial_numbers_.find(usb_ports_[camera_index]); + if (serial_it == serial_numbers_.end()) { + RCLCPP_ERROR(get_logger(), "No serial number found for USB port %s", + usb_ports_[camera_index].c_str()); + return; + } + const auto usb_index_it = usb_index_map_.find(usb_ports_[camera_index]); + const size_t usb_index = usb_index_it == usb_index_map_.end() + ? camera_index + : static_cast(usb_index_it->second); + const std::string &serial_number = serial_it->second; + const std::string folder = generateFolderName(serial_number, usb_index); + callback_called_[camera_index] = true; - for (size_t i = 0; i < static_cast(saving_images_number_); i++) { - std::string folder = generateFolderName(serial_index, usb_index); - std::string ir_filename = - folder + "/ir#left_SN" + serial_index + "_Index" + std::to_string(usb_index) + - time_domain_ + ir_current_timestamps[i] + "_f" + std::to_string(i) + "_s" + - ir_timestamps[i] + - (is_gemini330_ ? ("_e" + left_ir_meta_exposure[i] + "_d" + left_ir_meta_gain[i]) : "") + - "_.jpg"; - if (ir_images[i].empty()) { - RCLCPP_INFO_STREAM(rclcpp::get_logger("multi_camera_subscriber"), "over "); - continue; + for (const auto &entry : captures_[camera_index]) { + const std::string &stream_name = entry.first; + const auto &capture = entry.second; + for (size_t i = 0; i < static_cast(saving_images_number_); ++i) { + if (capture.images[i].empty()) { + continue; + } + const std::string filename = folder + "/" + stream_name + "_SN" + serial_number + "_Index" + + std::to_string(usb_index) + time_domain_suffix_ + + capture.current_timestamps[i] + "_f" + std::to_string(i) + + "_s" + capture.receive_timestamps[i] + + metadataSuffix(capture, i) + "_.jpg"; + cv::imwrite(filename, capture.images[i]); } - - cv::imwrite(ir_filename, ir_images[i]); - std::string color_filename = - folder + "/color_SN" + serial_index + "_Index" + std::to_string(usb_index) + - time_domain_ + color_current_timestamps[i] + "_f" + std::to_string(i) + "_s" + - color_timestamps[i] + - (is_gemini330_ ? ("_e" + color_meta_exposure[i] + "_d" + color_meta_gain[i]) : "") + - "_.jpg"; - if (color_images[i].empty()) { - continue; - } - cv::imwrite(color_filename, color_images[i]); - // RCLCPP_INFO(this->get_logger(), "Saved Color image to: %s", color_filename.c_str()); } - ir_image_buffers_[index].clear(); - ir_current_timestamp_buffers_[index].clear(); - ir_timestamp_buffers_[index].clear(); - color_image_buffers_[index].clear(); - color_current_timestamp_buffers_[index].clear(); - color_timestamp_buffers_[index].clear(); - left_ir_metadata_.exposure_buffs[index].clear(); - left_ir_metadata_.gain_buffs[index].clear(); - color_metadata_.exposure_buffs[index].clear(); - color_metadata_.gain_buffs[index].clear(); - RCLCPP_INFO_STREAM(rclcpp::get_logger("multi_camera_subscriber"), - "callback_called_ " << index << ":" << callback_called_[index]); - bool all_true = - std::all_of(callback_called_.begin(), callback_called_.end(), [](bool v) { return v; }); - if (all_true) { - RCLCPP_INFO_STREAM(rclcpp::get_logger("multi_camera_subscriber"), "over "); + for (auto &entry : captures_[camera_index]) { + entry.second.clear(); + } + const bool all_cameras_complete = std::all_of(callback_called_.begin(), callback_called_.end(), + [](bool value) { return value; }); + if (all_cameras_complete) { + RCLCPP_INFO(get_logger(), "Capture completed for all cameras"); saving_images_number_ = 0; - callback_called_.clear(); - callback_called_ = std::vector(left_ir_topics_.size(), false); + callback_called_.assign(camera_names_.size(), false); } } void controlCaptureCallback( const std::shared_ptr request, std::shared_ptr response) { - (void)response; - currenttimes_ = getCurrentTimes(); + std::lock_guard lock(capture_mutex_); + if (request->data <= 0) { + response->success = false; + response->message = "capture image count must be greater than zero"; + return; + } + if (!topics_initialized_) { + if (!initializeTopics()) { + callback_groups_.clear(); + image_subscribers_.clear(); + metadata_subscribers_.clear(); + captures_.clear(); + callback_called_.clear(); + response->success = false; + response->message = + "supported image streams did not become stable; retry after all cameras and streams " + "start or configure stream_names"; + return; + } + topics_initialized_ = true; + } + + for (auto &camera_captures : captures_) { + for (auto &entry : camera_captures) { + entry.second.clear(); + } + } + callback_called_.assign(camera_names_.size(), false); + current_date_time_ = currentDateTime(); saving_images_number_ = request->data; - if (!topic_init_) { - topic_init(); - topic_init_ = true; - } - RCLCPP_INFO_STREAM(rclcpp::get_logger("multi_camera_subscriber"), - "saving_images_number_: " << saving_images_number_); - } - void irCallback(std::shared_ptr image, size_t index) { - std::lock_guard lock(image_mutex_); - if (!callback_called_[index] && static_cast(saving_images_number_)) { - cv::Mat ir_mat = cv_bridge::toCvCopy(image, image->encoding)->image; - std::string current_timestamp_ir = getCurrentTimestamp(image); - std::string timestamp_ir = getTimestamp(); - ir_image_buffers_[index].push_back(ir_mat); - ir_current_timestamp_buffers_[index].push_back(current_timestamp_ir); - ir_timestamp_buffers_[index].push_back(timestamp_ir); - RCLCPP_INFO_STREAM(rclcpp::get_logger("multi_camera_subscriber"), - ":ir: " << index << ":" << ir_image_buffers_[index].size()); - if (ir_image_buffers_[index].size() >= static_cast(saving_images_number_) && - color_image_buffers_[index].size() >= static_cast(saving_images_number_) && - (!is_gemini330_ || (color_metadata_.exposure_buffs[index].size() >= - static_cast(saving_images_number_) && - left_ir_metadata_.exposure_buffs[index].size() >= - static_cast(saving_images_number_)))) { - saveAlignedImages(index); - } - } - } - void colorCallback(std::shared_ptr image, size_t index) { - std::lock_guard lock(image_mutex_); - if (!callback_called_[index] && static_cast(saving_images_number_)) { - cv::Mat color_mat = cv_bridge::toCvCopy(image, image->encoding)->image; - cv::Mat corrected_image; - cv::cvtColor(color_mat, corrected_image, cv::COLOR_RGB2BGR); - std::string current_timestamp_color = getCurrentTimestamp(image); - std::string timestamp_color = getTimestamp(); - color_image_buffers_[index].push_back(corrected_image); - color_current_timestamp_buffers_[index].push_back(current_timestamp_color); - color_timestamp_buffers_[index].push_back(timestamp_color); - RCLCPP_INFO_STREAM(rclcpp::get_logger("multi_camera_subscriber"), - ":color: " << index << ":" << color_image_buffers_[index].size()); - if (ir_image_buffers_[index].size() >= static_cast(saving_images_number_) && - color_image_buffers_[index].size() >= static_cast(saving_images_number_) && - (!is_gemini330_ || (color_metadata_.exposure_buffs[index].size() >= - static_cast(saving_images_number_) && - left_ir_metadata_.exposure_buffs[index].size() >= - static_cast(saving_images_number_)))) { - saveAlignedImages(index); - } - } + response->success = true; + response->message = "capture started"; + RCLCPP_INFO(get_logger(), "Capturing %d image(s) from each configured stream", + saving_images_number_); } - void ir_meta_Callback(std::shared_ptr msg, - size_t index) { - std::lock_guard lock(meta_mutex_); - if (!callback_called_[index] && static_cast(saving_images_number_)) { - nlohmann::json json_data = nlohmann::json::parse(msg->json_data); - left_ir_metadata_.exposure_buffs[index].push_back(json_data["exposure"].dump()); - left_ir_metadata_.gain_buffs[index].push_back(json_data["gain"].dump()); + void imageCallback(const std::shared_ptr image, + size_t camera_index, const std::string &stream_name) { + std::lock_guard lock(capture_mutex_); + if (saving_images_number_ <= 0 || callback_called_[camera_index]) { + return; } - } - void color_meta_Callback(std::shared_ptr msg, - size_t index) { - std::lock_guard lock(meta_mutex_); - if (!callback_called_[index] && static_cast(saving_images_number_)) { - nlohmann::json json_data = nlohmann::json::parse(msg->json_data); - color_metadata_.exposure_buffs[index].push_back(json_data["exposure"].dump()); - color_metadata_.gain_buffs[index].push_back(json_data["gain"].dump()); + auto &capture = captures_[camera_index].at(stream_name); + if (capture.images.size() >= static_cast(saving_images_number_)) { + saveImages(camera_index); + return; } + cv::Mat output = cv_bridge::toCvCopy(image, image->encoding)->image; + if (isColorCaptureStreamName(stream_name) && + image->encoding == sensor_msgs::image_encodings::RGB8) { + cv::Mat converted; + cv::cvtColor(output, converted, cv::COLOR_RGB2BGR); + output = converted; + } else if (isColorCaptureStreamName(stream_name) && + image->encoding == sensor_msgs::image_encodings::RGBA8) { + cv::Mat converted; + cv::cvtColor(output, converted, cv::COLOR_RGBA2BGRA); + output = converted; + } + StreamCapture::PendingImage pending_image{std::move(output), imageTimestamp(image), + receiveTimestamp()}; + if (capture.metadata_required) { + const auto stamp_ns = messageStampNs(image->header); + auto metadata_it = capture.pending_metadata.find(stamp_ns); + if (metadata_it == capture.pending_metadata.end()) { + capture.pending_images.insert_or_assign(stamp_ns, std::move(pending_image)); + trimPendingFrames(capture.pending_images); + return; + } + appendCompletedFrame(capture, std::move(pending_image), std::move(metadata_it->second)); + capture.pending_metadata.erase(metadata_it); + } else { + appendCompletedFrame(capture, std::move(pending_image)); + } + RCLCPP_INFO(get_logger(), "%s[%zu]: %zu/%d", stream_name.c_str(), camera_index, + capture.images.size(), saving_images_number_); + saveImages(camera_index); } + void metadataCallback(const std::shared_ptr metadata, + size_t camera_index, const std::string &stream_name) { + std::lock_guard lock(capture_mutex_); + if (saving_images_number_ <= 0 || callback_called_[camera_index]) { + return; + } + auto &capture = captures_[camera_index].at(stream_name); + if (!capture.metadata_required || + capture.images.size() >= static_cast(saving_images_number_)) { + return; + } + StreamCapture::FrameMetadata frame_metadata; + try { + const auto json_data = nlohmann::json::parse(metadata->json_data); + if (json_data.contains("exposure")) { + frame_metadata.exposure = json_data["exposure"].dump(); + } + if (json_data.contains("gain")) { + frame_metadata.gain = json_data["gain"].dump(); + } + } catch (const std::exception &e) { + RCLCPP_WARN(get_logger(), "Failed to parse %s metadata: %s", stream_name.c_str(), e.what()); + } + const auto stamp_ns = messageStampNs(metadata->header); + auto image_it = capture.pending_images.find(stamp_ns); + if (image_it == capture.pending_images.end()) { + capture.pending_metadata.insert_or_assign(stamp_ns, std::move(frame_metadata)); + trimPendingFrames(capture.pending_metadata); + return; + } + appendCompletedFrame(capture, std::move(image_it->second), std::move(frame_metadata)); + capture.pending_images.erase(image_it); + RCLCPP_INFO(get_logger(), "%s[%zu]: %zu/%d", stream_name.c_str(), camera_index, + capture.images.size(), saving_images_number_); + saveImages(camera_index); + } + + std::mutex capture_mutex_; + std::vector callback_groups_; + std::vector::SharedPtr> image_subscribers_; std::vector::SharedPtr> - ir_meta_subscribers_; - std::vector::SharedPtr> - color_meta_subscribers_; - std::vector::SharedPtr> ir_subscribers_; - std::vector::SharedPtr> color_subscribers_; + metadata_subscribers_; rclcpp::Service::SharedPtr capture_control_srv_; std::map usb_index_map_; std::map serial_numbers_; - std::array usb_numbers_; - - std::vector usb_params_; - std::vector camera_name_; - std::vector left_ir_metadata_topic_; - std::vector color_metadata_topic_; - std::vector left_ir_topics_; - std::vector color_topics_; - std::string time_domain_; - - std::vector> ir_image_buffers_; - std::vector> color_image_buffers_; - std::vector> ir_current_timestamp_buffers_; - std::vector> color_current_timestamp_buffers_; - std::vector> ir_timestamp_buffers_; - std::vector> color_timestamp_buffers_; - + std::vector usb_ports_; + std::vector camera_names_; + std::vector configured_stream_names_; + std::vector> captures_; std::vector callback_called_; - - std::string currenttimes_; - - int saving_images_number_ = 100; - - bool topic_init_ = false; - bool is_gemini330_ = true; - - ImageMetadata left_ir_metadata_ = ImageMetadata(); - ImageMetadata color_metadata_ = ImageMetadata(); + std::string time_domain_suffix_; + std::string current_date_time_; + int saving_images_number_ = 0; + bool topics_initialized_ = false; }; + } // namespace tools } // namespace orbbec_camera + RCLCPP_COMPONENTS_REGISTER_NODE(orbbec_camera::tools::MultiCameraSubscriber) diff --git a/orbbec_camera/tools/start_benchmark.cpp b/orbbec_camera/tools/start_benchmark.cpp index 5987a554..9d1fe622 100644 --- a/orbbec_camera/tools/start_benchmark.cpp +++ b/orbbec_camera/tools/start_benchmark.cpp @@ -19,6 +19,16 @@ class StartBenchmark : public rclcpp::Node { [this, i](std::shared_ptr msg) { this->color_Callback(msg, i); })); + left_color_subs_.push_back(this->create_subscription( + left_color_topics_[i], custom_qos, + [this, i](std::shared_ptr msg) { + this->leftColorCallback(msg, i); + })); + right_color_subs_.push_back(this->create_subscription( + right_color_topics_[i], custom_qos, + [this, i](std::shared_ptr msg) { + this->rightColorCallback(msg, i); + })); depth_subs_.push_back(this->create_subscription( depth_topics_[i], custom_qos, [this, i](std::shared_ptr msg) { @@ -45,6 +55,10 @@ class StartBenchmark : public rclcpp::Node { this->color_point_cloud_Callback(msg, i); })); RCLCPP_INFO_STREAM(rclcpp::get_logger("StartBenchmark"), color_topics_[i] << " is subed "); + RCLCPP_INFO_STREAM(rclcpp::get_logger("StartBenchmark"), + left_color_topics_[i] << " is subed "); + RCLCPP_INFO_STREAM(rclcpp::get_logger("StartBenchmark"), + right_color_topics_[i] << " is subed "); RCLCPP_INFO_STREAM(rclcpp::get_logger("StartBenchmark"), depth_topics_[i] << " is subed "); RCLCPP_INFO_STREAM(rclcpp::get_logger("StartBenchmark"), left_ir_topics_[i] << " is subed "); RCLCPP_INFO_STREAM(rclcpp::get_logger("StartBenchmark"), right_ir_topics_[i] << " is subed "); @@ -57,6 +71,8 @@ class StartBenchmark : public rclcpp::Node { private: std::vector::SharedPtr> color_subs_; + std::vector::SharedPtr> left_color_subs_; + std::vector::SharedPtr> right_color_subs_; std::vector::SharedPtr> depth_subs_; std::vector::SharedPtr> left_ir_subs_; std::vector::SharedPtr> right_ir_subs_; @@ -67,6 +83,8 @@ class StartBenchmark : public rclcpp::Node { std::vector camera_name_; std::vector color_topics_; + std::vector left_color_topics_; + std::vector right_color_topics_; std::vector depth_topics_; std::vector left_ir_topics_; std::vector right_ir_topics_; @@ -88,6 +106,8 @@ class StartBenchmark : public rclcpp::Node { camera_name_ = json_data["start_benchmark_params"]["camera_name"].get>(); color_topics_.resize(camera_name_.size()); + left_color_topics_.resize(camera_name_.size()); + right_color_topics_.resize(camera_name_.size()); depth_topics_.resize(camera_name_.size()); left_ir_topics_.resize(camera_name_.size()); right_ir_topics_.resize(camera_name_.size()); @@ -95,6 +115,8 @@ class StartBenchmark : public rclcpp::Node { color_point_cloud_topics_.resize(camera_name_.size()); for (size_t i = 0; i < camera_name_.size(); ++i) { color_topics_[i] = "/" + camera_name_[i] + "/color/image_raw"; + left_color_topics_[i] = "/" + camera_name_[i] + "/left_color/image_raw"; + right_color_topics_[i] = "/" + camera_name_[i] + "/right_color/image_raw"; depth_topics_[i] = "/" + camera_name_[i] + "/depth/image_raw"; left_ir_topics_[i] = "/" + camera_name_[i] + "/left_ir/image_raw"; right_ir_topics_[i] = "/" + camera_name_[i] + "/right_ir/image_raw"; @@ -108,6 +130,17 @@ class StartBenchmark : public rclcpp::Node { RCLCPP_DEBUG_STREAM(rclcpp::get_logger("StartBenchmark"), "time is : " << msg->step << "color is subed " << index << "is subed"); } + void leftColorCallback(std::shared_ptr msg, size_t index) { + std::lock_guard lock(image_mutex_); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("StartBenchmark"), + "time is : " << msg->step << "left_color is subed " << index << "is subed"); + } + void rightColorCallback(std::shared_ptr msg, size_t index) { + std::lock_guard lock(image_mutex_); + RCLCPP_DEBUG_STREAM( + rclcpp::get_logger("StartBenchmark"), + "time is : " << msg->step << "right_color is subed " << index << "is subed"); + } void depth_Callback(std::shared_ptr msg, size_t index) { std::lock_guard lock(image_mutex_); RCLCPP_DEBUG_STREAM(rclcpp::get_logger("StartBenchmark"), diff --git a/orbbec_camera_msgs/msg/DeviceStatus.msg b/orbbec_camera_msgs/msg/DeviceStatus.msg index 90662542..32b46777 100644 --- a/orbbec_camera_msgs/msg/DeviceStatus.msg +++ b/orbbec_camera_msgs/msg/DeviceStatus.msg @@ -11,6 +11,28 @@ 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 @@ -22,6 +44,28 @@ 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"