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 08ed6ab7..f296554b 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() { @@ -1188,12 +1192,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 54cb5fc1..040b78bc 100644 --- a/orbbec_camera/scripts/common_benchmark_node.py +++ b/orbbec_camera/scripts/common_benchmark_node.py @@ -321,11 +321,24 @@ 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) + # Update every image stream from the native DeviceStatus fields. + for stream in MONITORED_STREAMS: + self.update_stats( + camera["stats"], + f"{stream}_fps", + getattr(msg, f"{stream}_frame_rate_cur"), + getattr(msg, f"{stream}_frame_rate_min"), + getattr(msg, f"{stream}_frame_rate_max"), + getattr(msg, f"{stream}_frame_rate_avg"), + ) + self.update_stats( + camera["stats"], + f"{stream}_delay", + getattr(msg, f"{stream}_delay_ms_cur"), + getattr(msg, f"{stream}_delay_ms_min"), + getattr(msg, f"{stream}_delay_ms_max"), + getattr(msg, f"{stream}_delay_ms_avg"), + ) def image_callback(self, msg: Image, camera_name: str, stream: str): if stream not in MONITORED_STREAMS: @@ -338,17 +351,7 @@ class CameraMonitorNode(Node): if self.ideal_fps and self.ideal_fps > 0.0 else camera["stats"][f"{stream}_fps"]["avg"] ) - stamp, observed_fps = tracker.on_msg(msg.header, fps_to_use) - - # DeviceStatus currently reports detailed values only for the main color/depth streams. - # Derive equivalent statistics from image timestamps for the side streams. - if stream not in ("color", "depth"): - if observed_fps is not None: - self.update_sample_stat(camera["stats"], f"{stream}_fps", observed_fps) - delay_ms = (self.get_clock().now().nanoseconds * 1e-9 - stamp) * 1000.0 - # Device-domain timestamps are not comparable with the ROS clock. - if 0.0 <= delay_ms <= 60000.0: - self.update_sample_stat(camera["stats"], f"{stream}_delay", delay_ms) + tracker.on_msg(msg.header, fps_to_use) 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 @@ -361,17 +364,6 @@ class CameraMonitorNode(Node): s["min"] = min(s["min"], min_val) s["max"] = max(s["max"], max_val) - def update_sample_stat(self, stats, key, value): - if value is None or value < 0.0: - return - s = stats[key] - s["cur"] = value - s["count"] += 1 - s["sum"] += value - s["avg"] = s["sum"] / s["count"] - s["min"] = min(s["min"], value) - s["max"] = max(s["max"], value) - def update_sys_stat(self, stat_dict, value, online=True): stat_dict["cur"] = value if value is None or value <= 0.0 or not online: 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 917ba861..4c1c4529 100644 --- a/orbbec_camera/src/ob_camera_node.cpp +++ b/orbbec_camera/src/ob_camera_node.cpp @@ -741,7 +741,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]; @@ -761,6 +765,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); @@ -770,12 +776,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 @@ -6473,6 +6485,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 { @@ -6514,10 +6540,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); @@ -7117,13 +7145,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); + } } } @@ -7148,10 +7182,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) { @@ -7211,8 +7247,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_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"