From b82ebc51fce600db1734f61af5b92977d226c20d Mon Sep 17 00:00:00 2001 From: slz Date: Mon, 13 Apr 2026 10:41:51 +0800 Subject: [PATCH 1/2] feat: add frame timestamp CSV logging functionality --- orbbec_camera/CMakeLists.txt | 1 + .../frame_timestamp_csv_logger.h | 155 ++++++ .../include/orbbec_camera/ob_camera_node.h | 6 +- .../launch/gemini_330_series.launch.py | 2 + .../src/frame_timestamp_csv_logger.cpp | 510 ++++++++++++++++++ orbbec_camera/src/ob_camera_node.cpp | 67 ++- 6 files changed, 736 insertions(+), 5 deletions(-) create mode 100644 orbbec_camera/include/orbbec_camera/frame_timestamp_csv_logger.h create mode 100644 orbbec_camera/src/frame_timestamp_csv_logger.cpp diff --git a/orbbec_camera/CMakeLists.txt b/orbbec_camera/CMakeLists.txt index a4b6ba75..d2b3826b 100644 --- a/orbbec_camera/CMakeLists.txt +++ b/orbbec_camera/CMakeLists.txt @@ -124,6 +124,7 @@ set(SOURCE_FILES src/d2c_viewer.cpp src/dynamic_params.cpp src/image_publisher.cpp + src/frame_timestamp_csv_logger.cpp src/ob_camera_node_driver.cpp src/ob_camera_node.cpp src/ob_lidar_node.cpp diff --git a/orbbec_camera/include/orbbec_camera/frame_timestamp_csv_logger.h b/orbbec_camera/include/orbbec_camera/frame_timestamp_csv_logger.h new file mode 100644 index 00000000..bb7823f9 --- /dev/null +++ b/orbbec_camera/include/orbbec_camera/frame_timestamp_csv_logger.h @@ -0,0 +1,155 @@ +#pragma once + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "libobsensor/ObSensor.hpp" + +namespace orbbec_camera { + +class FrameTimestampCsvLogger { + public: + FrameTimestampCsvLogger(bool enabled, const std::string &csv_file_path, rclcpp::Logger logger); + + ~FrameTimestampCsvLogger() noexcept; + + FrameTimestampCsvLogger(const FrameTimestampCsvLogger &) = delete; + FrameTimestampCsvLogger &operator=(const FrameTimestampCsvLogger &) = delete; + + void recordFrameSet(const std::shared_ptr &color_frame, + 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 recordStandaloneFrameArrival(OBStreamType stream_type, + const std::shared_ptr &frame, + int64_t arrival_system_us, int64_t arrival_steady_us, + bool image_publish_expected); + + void recordPreImagePublish(OBStreamType stream_type, const std::shared_ptr &frame, + int64_t publish_system_us, int64_t publish_steady_us); + + void shutdown(); + + bool enabled() const { return enabled_; } + + private: + enum class TrackedStream { COLOR, DEPTH }; + + struct StreamState { + bool has_frame = false; + bool publish_expected = false; + bool final = false; + uint64_t frame_index = 0; + + int64_t device_ts_us = 0; + int64_t global_ts_us = 0; + int64_t sdk_system_ts_us = 0; + int64_t arrival_system_us = 0; + int64_t arrival_steady_us = 0; + std::optional publish_system_us; + std::optional publish_steady_us; + + std::optional device_ts_delta_us; + std::optional global_ts_delta_us; + std::optional sdk_system_ts_delta_us; + std::optional arrival_system_delta_us; + std::optional arrival_steady_delta_us; + std::optional publish_system_delta_us; + std::optional publish_steady_delta_us; + std::optional arrival_to_publish_system_us; + std::optional arrival_to_publish_steady_us; + std::optional sdk_delay_from_global_us; + std::optional sdk_delay_from_system_us; + }; + + struct PendingRow { + uint64_t row_id = 0; + StreamState color; + StreamState depth; + }; + + struct PreviousStreamTimestamps { + std::optional device_ts_us; + std::optional global_ts_us; + std::optional sdk_system_ts_us; + std::optional arrival_system_us; + std::optional arrival_steady_us; + std::optional publish_system_us; + std::optional publish_steady_us; + }; + + TrackedStream toTrackedStream(OBStreamType stream_type) const; + bool isTrackedStream(OBStreamType stream_type) const; + + void recordFrameSetInternal(const std::shared_ptr &color_frame, + 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 recordStandaloneFrameArrivalInternal(OBStreamType stream_type, + const std::shared_ptr &frame, + int64_t arrival_system_us, int64_t arrival_steady_us, + bool image_publish_expected); + void recordPreImagePublishInternal(OBStreamType stream_type, + const std::shared_ptr &frame, + int64_t publish_system_us, int64_t publish_steady_us); + + void populateArrivalData(StreamState &state, TrackedStream stream, + const std::shared_ptr &frame, int64_t arrival_system_us, + int64_t arrival_steady_us, bool publish_expected); + void populatePublishData(StreamState &state, TrackedStream stream, int64_t publish_system_us, + int64_t publish_steady_us); + + std::optional updateDelta(std::optional &previous, int64_t current); + + void finalizeStreamWithoutPublish(StreamState &state); + bool isRowReady(const PendingRow &row) const; + void enqueueCompletedRow(const PendingRow &row); + void flushPendingRowsLocked(std::vector &rows); + void eraseFrameIndexMappingLocked(const PendingRow &row); + + std::string serializeRow(const PendingRow &row) const; + std::string serializeStreamColumns(const StreamState &state) const; + static std::string formatSecondsColumn(int64_t time_us); + static std::string formatOptionalIntColumn(const std::optional &value); + static std::string csvHeader(); + + void writerThreadMain(); + void openCsvIfNeeded(); + + rclcpp::Logger logger_; + bool enabled_ = false; + std::atomic_bool shutdown_requested_{false}; + bool writer_failed_ = false; + bool queue_warning_active_ = false; + std::string csv_file_path_; + std::ofstream csv_stream_; + std::thread writer_thread_; + + uint64_t next_row_id_ = 1; + std::unordered_map pending_rows_; + std::unordered_map color_frame_index_to_row_id_; + std::unordered_map depth_frame_index_to_row_id_; + PreviousStreamTimestamps color_previous_; + PreviousStreamTimestamps depth_previous_; + + std::deque completed_rows_; + + mutable std::mutex state_mutex_; + std::mutex completed_rows_mutex_; + std::condition_variable completed_rows_cv_; +}; + +} // namespace orbbec_camera diff --git a/orbbec_camera/include/orbbec_camera/ob_camera_node.h b/orbbec_camera/include/orbbec_camera/ob_camera_node.h index e1b67f3a..a2da94ca 100644 --- a/orbbec_camera/include/orbbec_camera/ob_camera_node.h +++ b/orbbec_camera/include/orbbec_camera/ob_camera_node.h @@ -67,6 +67,7 @@ #include "orbbec_camera/image_publisher.h" #include "orbbec_camera/fps_counter.hpp" #include "orbbec_camera/fps_delay_status.hpp" +#include "orbbec_camera/frame_timestamp_csv_logger.h" #include "jpeg_decoder.h" #include #include @@ -409,7 +410,7 @@ class OBCameraNode { std::shared_ptr& response); void setAEReferenceStreamCallback(const std::shared_ptr& request, - std::shared_ptr& response); + std::shared_ptr& response); void setAEStrategyCallback(const std::shared_ptr& request, std::shared_ptr& response); @@ -848,6 +849,9 @@ class OBCameraNode { int min_depth_limit_ = 0; int max_depth_limit_ = 0; std::string time_domain_ = "global"; // device, system, global + bool enable_frame_timestamp_csv_ = false; + std::string frame_timestamp_csv_file_; + std::unique_ptr frame_timestamp_csv_logger_; std::string exposure_range_mode_ = "default"; std::string load_config_json_file_path_ = ""; std::string export_config_json_file_path_ = ""; diff --git a/orbbec_camera/launch/gemini_330_series.launch.py b/orbbec_camera/launch/gemini_330_series.launch.py index cb00c70a..72fa5d2a 100644 --- a/orbbec_camera/launch/gemini_330_series.launch.py +++ b/orbbec_camera/launch/gemini_330_series.launch.py @@ -257,6 +257,8 @@ def generate_launch_description(): DeclareLaunchArgument('enable_sync_host_time', default_value='true'), DeclareLaunchArgument('time_sync_period', default_value='6.0'), # seconds DeclareLaunchArgument('time_domain', default_value='global'),# global, device, system + DeclareLaunchArgument('enable_frame_timestamp_csv', default_value='false'), + DeclareLaunchArgument('frame_timestamp_csv_file', default_value=''), DeclareLaunchArgument('enable_color_undistortion', default_value='false'), DeclareLaunchArgument('config_file_path', default_value=''), DeclareLaunchArgument('enable_heartbeat', default_value='false'), diff --git a/orbbec_camera/src/frame_timestamp_csv_logger.cpp b/orbbec_camera/src/frame_timestamp_csv_logger.cpp new file mode 100644 index 00000000..84d51724 --- /dev/null +++ b/orbbec_camera/src/frame_timestamp_csv_logger.cpp @@ -0,0 +1,510 @@ +#include "orbbec_camera/frame_timestamp_csv_logger.h" + +#include +#include +#include +#include +#include + +namespace orbbec_camera { +namespace { + +constexpr size_t kCompletedQueueSoftLimit = 1000; +constexpr size_t kFlushBatchSize = 100; +constexpr auto kFlushInterval = std::chrono::seconds(1); + +} // namespace + +FrameTimestampCsvLogger::FrameTimestampCsvLogger(bool enabled, const std::string &csv_file_path, + rclcpp::Logger logger) + : logger_(std::move(logger)), enabled_(enabled), csv_file_path_(csv_file_path) { + if (!enabled_) { + return; + } + + try { + auto path = std::filesystem::path(csv_file_path_); + if (path.has_parent_path() && !std::filesystem::exists(path.parent_path())) { + std::filesystem::create_directories(path.parent_path()); + } + } catch (const std::exception &e) { + RCLCPP_ERROR_STREAM(logger_, "Failed to prepare frame timestamp CSV path " << csv_file_path_ + << ": " << e.what()); + enabled_ = false; + writer_failed_ = true; + return; + } + + openCsvIfNeeded(); + if (!enabled_) { + return; + } + + writer_thread_ = std::thread([this]() { writerThreadMain(); }); +} + +FrameTimestampCsvLogger::~FrameTimestampCsvLogger() noexcept { shutdown(); } + +void FrameTimestampCsvLogger::recordFrameSet(const std::shared_ptr &color_frame, + 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) { + if (!enabled_ || writer_failed_) { + return; + } + recordFrameSetInternal(color_frame, depth_frame, arrival_system_us, arrival_steady_us, + track_color, track_depth, color_image_publish_expected, + depth_image_publish_expected); +} + +void FrameTimestampCsvLogger::recordStandaloneFrameArrival(OBStreamType stream_type, + const std::shared_ptr &frame, + int64_t arrival_system_us, + int64_t arrival_steady_us, + bool image_publish_expected) { + if (!enabled_ || writer_failed_ || !frame || !isTrackedStream(stream_type)) { + return; + } + recordStandaloneFrameArrivalInternal(stream_type, frame, arrival_system_us, arrival_steady_us, + image_publish_expected); +} + +void FrameTimestampCsvLogger::recordPreImagePublish(OBStreamType stream_type, + const std::shared_ptr &frame, + int64_t publish_system_us, + int64_t publish_steady_us) { + if (!enabled_ || writer_failed_ || !frame || !isTrackedStream(stream_type)) { + return; + } + recordPreImagePublishInternal(stream_type, frame, publish_system_us, publish_steady_us); +} + +void FrameTimestampCsvLogger::shutdown() { + if (!enabled_) { + return; + } + + { + std::lock_guard state_lock(state_mutex_); + if (shutdown_requested_) { + return; + } + shutdown_requested_ = true; + + std::vector rows_to_flush; + flushPendingRowsLocked(rows_to_flush); + for (const auto &row : rows_to_flush) { + enqueueCompletedRow(row); + } + } + + completed_rows_cv_.notify_all(); + if (writer_thread_.joinable()) { + writer_thread_.join(); + } + + if (csv_stream_.is_open()) { + csv_stream_.flush(); + csv_stream_.close(); + } +} + +FrameTimestampCsvLogger::TrackedStream FrameTimestampCsvLogger::toTrackedStream( + OBStreamType stream_type) const { + if (stream_type == OB_STREAM_COLOR) { + return TrackedStream::COLOR; + } + return TrackedStream::DEPTH; +} + +bool FrameTimestampCsvLogger::isTrackedStream(OBStreamType stream_type) const { + return stream_type == OB_STREAM_COLOR || stream_type == OB_STREAM_DEPTH; +} + +void FrameTimestampCsvLogger::recordFrameSetInternal( + const std::shared_ptr &color_frame, 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) { + if (!track_color && !track_depth) { + return; + } + + std::vector ready_rows; + { + std::lock_guard lock(state_mutex_); + if (shutdown_requested_) { + return; + } + + PendingRow row; + row.row_id = next_row_id_++; + + if (track_color && color_frame) { + populateArrivalData(row.color, TrackedStream::COLOR, color_frame, arrival_system_us, + arrival_steady_us, color_image_publish_expected); + color_frame_index_to_row_id_[row.color.frame_index] = row.row_id; + if (!color_image_publish_expected) { + finalizeStreamWithoutPublish(row.color); + } + } else { + row.color.final = true; + } + + if (track_depth && depth_frame) { + populateArrivalData(row.depth, TrackedStream::DEPTH, depth_frame, arrival_system_us, + arrival_steady_us, depth_image_publish_expected); + depth_frame_index_to_row_id_[row.depth.frame_index] = row.row_id; + if (!depth_image_publish_expected) { + finalizeStreamWithoutPublish(row.depth); + } + } else { + row.depth.final = true; + } + + pending_rows_.emplace(row.row_id, row); + if (isRowReady(row)) { + auto it = pending_rows_.find(row.row_id); + if (it != pending_rows_.end()) { + ready_rows.push_back(it->second); + eraseFrameIndexMappingLocked(it->second); + pending_rows_.erase(it); + } + } + } + + for (const auto &ready_row : ready_rows) { + enqueueCompletedRow(ready_row); + } +} + +void FrameTimestampCsvLogger::recordStandaloneFrameArrivalInternal( + OBStreamType stream_type, const std::shared_ptr &frame, int64_t arrival_system_us, + int64_t arrival_steady_us, bool image_publish_expected) { + std::optional ready_row; + { + std::lock_guard lock(state_mutex_); + if (shutdown_requested_) { + return; + } + + PendingRow row; + row.row_id = next_row_id_++; + + const auto tracked_stream = toTrackedStream(stream_type); + auto &state = tracked_stream == TrackedStream::COLOR ? row.color : row.depth; + auto &other_state = tracked_stream == TrackedStream::COLOR ? row.depth : row.color; + + populateArrivalData(state, tracked_stream, frame, arrival_system_us, arrival_steady_us, + image_publish_expected); + other_state.final = true; + + if (tracked_stream == TrackedStream::COLOR) { + color_frame_index_to_row_id_[state.frame_index] = row.row_id; + } else { + depth_frame_index_to_row_id_[state.frame_index] = row.row_id; + } + + if (!image_publish_expected) { + finalizeStreamWithoutPublish(state); + } + + pending_rows_.emplace(row.row_id, row); + if (isRowReady(row)) { + auto it = pending_rows_.find(row.row_id); + if (it != pending_rows_.end()) { + ready_row = it->second; + eraseFrameIndexMappingLocked(*ready_row); + pending_rows_.erase(it); + } + } + } + + if (ready_row.has_value()) { + enqueueCompletedRow(*ready_row); + } +} + +void FrameTimestampCsvLogger::recordPreImagePublishInternal(OBStreamType stream_type, + const std::shared_ptr &frame, + int64_t publish_system_us, + int64_t publish_steady_us) { + std::optional ready_row; + const auto frame_index = frame->getIndex(); + + { + std::lock_guard lock(state_mutex_); + if (shutdown_requested_) { + return; + } + + const auto tracked_stream = toTrackedStream(stream_type); + auto &row_map = tracked_stream == TrackedStream::COLOR ? color_frame_index_to_row_id_ + : depth_frame_index_to_row_id_; + auto row_id_it = row_map.find(frame_index); + if (row_id_it == row_map.end()) { + RCLCPP_WARN_STREAM(logger_, + "Frame timestamp CSV logger missed row mapping for stream " + << (tracked_stream == TrackedStream::COLOR ? "color" : "depth") + << " frame index " << frame_index); + return; + } + const auto row_id = row_id_it->second; + + auto pending_it = pending_rows_.find(row_id); + if (pending_it == pending_rows_.end()) { + return; + } + + auto &state = tracked_stream == TrackedStream::COLOR ? pending_it->second.color + : pending_it->second.depth; + populatePublishData(state, tracked_stream, publish_system_us, publish_steady_us); + state.final = true; + + if (isRowReady(pending_it->second)) { + ready_row = pending_it->second; + eraseFrameIndexMappingLocked(*ready_row); + pending_rows_.erase(pending_it); + } + } + + if (ready_row.has_value()) { + enqueueCompletedRow(*ready_row); + } +} + +void FrameTimestampCsvLogger::populateArrivalData(StreamState &state, TrackedStream stream, + const std::shared_ptr &frame, + int64_t arrival_system_us, + int64_t arrival_steady_us, + bool publish_expected) { + auto &previous = stream == TrackedStream::COLOR ? color_previous_ : depth_previous_; + + state.has_frame = true; + state.publish_expected = publish_expected; + state.frame_index = frame->getIndex(); + state.device_ts_us = static_cast(frame->getTimeStampUs()); + state.global_ts_us = static_cast(frame->getGlobalTimeStampUs()); + state.sdk_system_ts_us = static_cast(frame->getSystemTimeStampUs()); + state.arrival_system_us = arrival_system_us; + state.arrival_steady_us = arrival_steady_us; + state.device_ts_delta_us = updateDelta(previous.device_ts_us, state.device_ts_us); + state.global_ts_delta_us = updateDelta(previous.global_ts_us, state.global_ts_us); + state.sdk_system_ts_delta_us = updateDelta(previous.sdk_system_ts_us, state.sdk_system_ts_us); + state.arrival_system_delta_us = updateDelta(previous.arrival_system_us, state.arrival_system_us); + state.arrival_steady_delta_us = updateDelta(previous.arrival_steady_us, state.arrival_steady_us); + state.sdk_delay_from_global_us = state.arrival_system_us - state.global_ts_us; + state.sdk_delay_from_system_us = state.arrival_system_us - state.sdk_system_ts_us; +} + +void FrameTimestampCsvLogger::populatePublishData(StreamState &state, TrackedStream stream, + int64_t publish_system_us, + int64_t publish_steady_us) { + auto &previous = stream == TrackedStream::COLOR ? color_previous_ : depth_previous_; + + state.publish_system_us = publish_system_us; + state.publish_steady_us = publish_steady_us; + state.publish_system_delta_us = + updateDelta(previous.publish_system_us, state.publish_system_us.value()); + state.publish_steady_delta_us = + updateDelta(previous.publish_steady_us, state.publish_steady_us.value()); + state.arrival_to_publish_system_us = state.publish_system_us.value() - state.arrival_system_us; + state.arrival_to_publish_steady_us = state.publish_steady_us.value() - state.arrival_steady_us; +} + +std::optional FrameTimestampCsvLogger::updateDelta(std::optional &previous, + int64_t current) { + std::optional delta; + if (previous.has_value()) { + delta = current - previous.value(); + } + previous = current; + return delta; +} + +void FrameTimestampCsvLogger::finalizeStreamWithoutPublish(StreamState &state) { + state.final = true; +} + +bool FrameTimestampCsvLogger::isRowReady(const PendingRow &row) const { + return row.color.final && row.depth.final; +} + +void FrameTimestampCsvLogger::enqueueCompletedRow(const PendingRow &row) { + std::lock_guard queue_lock(completed_rows_mutex_); + completed_rows_.push_back(row); + if (completed_rows_.size() > kCompletedQueueSoftLimit) { + if (!queue_warning_active_) { + RCLCPP_WARN_STREAM(logger_, "Frame timestamp CSV queue size exceeded " + << kCompletedQueueSoftLimit << " rows"); + queue_warning_active_ = true; + } + } else { + queue_warning_active_ = false; + } + completed_rows_cv_.notify_one(); +} + +void FrameTimestampCsvLogger::flushPendingRowsLocked(std::vector &rows) { + rows.reserve(rows.size() + pending_rows_.size()); + for (auto &item : pending_rows_) { + auto row = item.second; + row.color.final = true; + row.depth.final = true; + rows.push_back(std::move(row)); + } + pending_rows_.clear(); + color_frame_index_to_row_id_.clear(); + depth_frame_index_to_row_id_.clear(); +} + +void FrameTimestampCsvLogger::eraseFrameIndexMappingLocked(const PendingRow &row) { + if (row.color.has_frame) { + color_frame_index_to_row_id_.erase(row.color.frame_index); + } + if (row.depth.has_frame) { + depth_frame_index_to_row_id_.erase(row.depth.frame_index); + } +} + +std::string FrameTimestampCsvLogger::serializeRow(const PendingRow &row) const { + std::ostringstream ss; + ss << serializeStreamColumns(row.color) << "," << serializeStreamColumns(row.depth); + return ss.str(); +} + +std::string FrameTimestampCsvLogger::serializeStreamColumns(const StreamState &state) const { + std::vector fields(19, ""); + if (state.has_frame) { + fields[0] = std::to_string(state.frame_index); + fields[1] = formatSecondsColumn(state.device_ts_us); + fields[2] = formatOptionalIntColumn(state.device_ts_delta_us); + fields[3] = formatSecondsColumn(state.global_ts_us); + fields[4] = formatOptionalIntColumn(state.global_ts_delta_us); + fields[5] = formatSecondsColumn(state.sdk_system_ts_us); + fields[6] = formatOptionalIntColumn(state.sdk_system_ts_delta_us); + fields[7] = formatSecondsColumn(state.arrival_system_us); + fields[8] = formatOptionalIntColumn(state.arrival_system_delta_us); + fields[9] = formatSecondsColumn(state.arrival_steady_us); + fields[10] = formatOptionalIntColumn(state.arrival_steady_delta_us); + if (state.publish_system_us.has_value()) { + fields[11] = formatSecondsColumn(state.publish_system_us.value()); + } + fields[12] = formatOptionalIntColumn(state.publish_system_delta_us); + if (state.publish_steady_us.has_value()) { + fields[13] = formatSecondsColumn(state.publish_steady_us.value()); + } + fields[14] = formatOptionalIntColumn(state.publish_steady_delta_us); + fields[15] = formatOptionalIntColumn(state.arrival_to_publish_system_us); + fields[16] = formatOptionalIntColumn(state.arrival_to_publish_steady_us); + fields[17] = formatOptionalIntColumn(state.sdk_delay_from_global_us); + fields[18] = formatOptionalIntColumn(state.sdk_delay_from_system_us); + } + + std::ostringstream ss; + for (size_t i = 0; i < fields.size(); ++i) { + if (i != 0) { + ss << ","; + } + ss << fields[i]; + } + return ss.str(); +} + +std::string FrameTimestampCsvLogger::formatSecondsColumn(int64_t time_us) { + std::ostringstream ss; + ss << std::fixed << std::setprecision(6) << (static_cast(time_us) / 1000000.0L); + return ss.str(); +} + +std::string FrameTimestampCsvLogger::formatOptionalIntColumn(const std::optional &value) { + if (!value.has_value()) { + return ""; + } + return std::to_string(*value); +} + +std::string FrameTimestampCsvLogger::csvHeader() { + std::ostringstream ss; + for (const auto *prefix : {"color", "depth"}) { + ss << prefix << "_frame_index,"; + ss << prefix << "_device_ts_sec,"; + ss << prefix << "_device_ts_delta_us,"; + ss << prefix << "_global_ts_sec,"; + ss << prefix << "_global_ts_delta_us,"; + ss << prefix << "_system_ts_sec,"; + ss << prefix << "_system_ts_delta_us,"; + ss << prefix << "_arrival_system_sec,"; + ss << prefix << "_arrival_system_delta_us,"; + ss << prefix << "_arrival_steady_sec,"; + ss << prefix << "_arrival_steady_delta_us,"; + ss << prefix << "_publish_system_sec,"; + ss << prefix << "_publish_system_delta_us,"; + ss << prefix << "_publish_steady_sec,"; + ss << prefix << "_publish_steady_delta_us,"; + ss << prefix << "_arrival_to_publish_system_us,"; + ss << prefix << "_arrival_to_publish_steady_us,"; + ss << prefix << "_sdk_delay_from_global_us,"; + ss << prefix << "_sdk_delay_from_system_us"; + if (std::string(prefix) == "color") { + ss << ","; + } + } + return ss.str(); +} + +void FrameTimestampCsvLogger::writerThreadMain() { + if (!enabled_ || writer_failed_) { + return; + } + + size_t rows_since_flush = 0; + auto last_flush = std::chrono::steady_clock::now(); + + while (true) { + std::deque rows_to_write; + { + std::unique_lock lock(completed_rows_mutex_); + completed_rows_cv_.wait_for(lock, kFlushInterval, [this]() { + return shutdown_requested_ || !completed_rows_.empty(); + }); + rows_to_write.swap(completed_rows_); + } + + for (const auto &row : rows_to_write) { + if (csv_stream_.is_open()) { + csv_stream_ << serializeRow(row) << "\n"; + rows_since_flush++; + } + } + + const auto now = std::chrono::steady_clock::now(); + if (csv_stream_.is_open() && (rows_since_flush >= kFlushBatchSize || + now - last_flush >= kFlushInterval || shutdown_requested_)) { + csv_stream_.flush(); + rows_since_flush = 0; + last_flush = now; + } + + std::lock_guard lock(completed_rows_mutex_); + if (shutdown_requested_ && completed_rows_.empty()) { + break; + } + } +} + +void FrameTimestampCsvLogger::openCsvIfNeeded() { + csv_stream_.open(csv_file_path_, std::ios::out | std::ios::trunc); + if (!csv_stream_.is_open()) { + RCLCPP_ERROR_STREAM(logger_, "Failed to open frame timestamp CSV file: " << csv_file_path_); + enabled_ = false; + writer_failed_ = true; + return; + } + csv_stream_ << csvHeader() << "\n"; + csv_stream_.flush(); + RCLCPP_INFO_STREAM(logger_, "Frame timestamp CSV logger enabled: " << csv_file_path_); +} + +} // namespace orbbec_camera diff --git a/orbbec_camera/src/ob_camera_node.cpp b/orbbec_camera/src/ob_camera_node.cpp index 70fa9811..43686bb3 100644 --- a/orbbec_camera/src/ob_camera_node.cpp +++ b/orbbec_camera/src/ob_camera_node.cpp @@ -38,6 +38,22 @@ namespace orbbec_camera { using namespace std::chrono_literals; +namespace { + +int64_t getSystemNowUs() { + return std::chrono::duration_cast( + std::chrono::system_clock::now().time_since_epoch()) + .count(); +} + +int64_t getSteadyNowUs() { + return std::chrono::duration_cast( + std::chrono::steady_clock::now().time_since_epoch()) + .count(); +} + +} // namespace + OBCameraNode::OBCameraNode(rclcpp::Node *node, std::shared_ptr device, std::shared_ptr parameters, bool use_intra_process) : node_(node), @@ -64,6 +80,20 @@ OBCameraNode::OBCameraNode(rclcpp::Node *node, std::shared_ptr devic compression_params_.push_back(cv::IMWRITE_PNG_STRATEGY_DEFAULT); setupDefaultImageFormat(); setupTopics(); + + if (enable_frame_timestamp_csv_) { + if (frame_timestamp_csv_file_.empty()) { + frame_timestamp_csv_file_ = + (std::filesystem::current_path() / (camera_name_ + "_frame_timestamp_stats.csv")) + .string(); + } + frame_timestamp_csv_logger_ = + std::make_unique(true, frame_timestamp_csv_file_, logger_); + if (!frame_timestamp_csv_logger_->enabled()) { + frame_timestamp_csv_logger_.reset(); + } + } + #if defined(USE_RK_HW_DECODER) if (enable_stream_[COLOR] && width_.count(COLOR) && height_.count(COLOR)) { jpeg_decoder_ = std::make_unique(width_[COLOR], height_[COLOR]); @@ -165,6 +195,15 @@ void OBCameraNode::clean() noexcept { // Set running flag to false first to signal all operations to stop is_running_.store(false); + try { + if (frame_timestamp_csv_logger_) { + frame_timestamp_csv_logger_->shutdown(); + frame_timestamp_csv_logger_.reset(); + } + } catch (...) { + RCLCPP_WARN_STREAM(logger_, "Exception while shutting down frame timestamp CSV logger"); + } + // Stop diagnostic timer and updater first BEFORE acquiring device_lock to prevent deadlock try { if (diagnostic_timer_) { @@ -2222,6 +2261,8 @@ void OBCameraNode::getParameters() { setAndGetNodeParameter(enable_firmware_log_, "enable_firmware_log", false); setAndGetNodeParameter(enable_color_undistortion_, "enable_color_undistortion", false); setAndGetNodeParameter(time_domain_, "time_domain", "global"); + setAndGetNodeParameter(enable_frame_timestamp_csv_, "enable_frame_timestamp_csv", false); + setAndGetNodeParameter(frame_timestamp_csv_file_, "frame_timestamp_csv_file", ""); setAndGetNodeParameter(exposure_range_mode_, "exposure_range_mode", "default"); setAndGetNodeParameter(load_config_json_file_path_, "load_config_json_file_path", ""); @@ -3155,8 +3196,7 @@ void OBCameraNode::setDepthAutoExposureROI() { return; } if (isGemini305SeriesPID(pid_) && ae_reference_stream_ == "color") { - RCLCPP_WARN_STREAM(logger_, - "Skip setting depth AE ROI because AE Reference Stream is color"); + RCLCPP_WARN_STREAM(logger_, "Skip setting depth AE ROI because AE Reference Stream is color"); depth_roi_has_run = true; return; } @@ -3201,8 +3241,7 @@ void OBCameraNode::setColorAutoExposureROI() { return; } if (isGemini305SeriesPID(pid_) && ae_reference_stream_ == "depth") { - RCLCPP_WARN_STREAM(logger_, - "Skip setting color AE ROI because AE Reference Stream is depth"); + RCLCPP_WARN_STREAM(logger_, "Skip setting color AE ROI because AE Reference Stream is depth"); color_roi_has_run = true; return; } @@ -3264,6 +3303,8 @@ void OBCameraNode::onNewFrameSetCallback(std::shared_ptr frame_set if (frame_set == nullptr) { return; } + const auto frame_set_arrival_system_us = getSystemNowUs(); + const auto frame_set_arrival_steady_us = getSteadyNowUs(); try { if (!tf_published_) { publishStaticTransforms(); @@ -3328,6 +3369,19 @@ void OBCameraNode::onNewFrameSetCallback(std::shared_ptr frame_set "null or color frame is null"); } + auto final_color_frame = frame_set->getFrame(OB_FRAME_COLOR); + auto final_depth_frame = frame_set->getFrame(OB_FRAME_DEPTH); + if (frame_timestamp_csv_logger_ && frame_timestamp_csv_logger_->enabled()) { + const bool track_color = enable_stream_[COLOR] && static_cast(final_color_frame); + const bool track_depth = enable_stream_[DEPTH] && static_cast(final_depth_frame); + const bool color_publish_expected = track_color; + const bool depth_publish_expected = track_depth; + frame_timestamp_csv_logger_->recordFrameSet( + 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); + } + // Refresh frame from current frameset before logging to reflect post-filter/alignment output. for (const auto &stream_index : IMAGE_STREAMS) { if (!enable_stream_[stream_index]) { @@ -3849,6 +3903,11 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr &frame, fps_delay_status_depth_->tick(frame_timestamp); } if (has_raw_image_subscriber) { + if (frame_timestamp_csv_logger_ && frame_timestamp_csv_logger_->enabled() && + (stream_index == COLOR || stream_index == DEPTH)) { + frame_timestamp_csv_logger_->recordPreImagePublish(stream_index.first, frame, + getSystemNowUs(), getSteadyNowUs()); + } image_publishers_[stream_index]->publish(std::move(image_msg)); } } From 1b1b6715928e15541daab3a46db285e96d485c0e Mon Sep 17 00:00:00 2001 From: slz Date: Mon, 13 Apr 2026 10:42:09 +0800 Subject: [PATCH 2/2] feat: add frame timestamp CSV logging arguments to launch files --- orbbec_camera/launch/astra.launch.py | 2 ++ orbbec_camera/launch/astra2.launch.py | 2 ++ orbbec_camera/launch/dabai_a.launch.py | 2 ++ orbbec_camera/launch/dabai_al.launch.py | 2 ++ orbbec_camera/launch/dabai_dcw2.launch.py | 2 ++ orbbec_camera/launch/dabai_max_pro.launch.py | 2 ++ orbbec_camera/launch/femto.launch.py | 2 ++ orbbec_camera/launch/femto_bolt.launch.py | 2 ++ orbbec_camera/launch/femto_mega.launch.py | 2 ++ orbbec_camera/launch/gemini2.launch.py | 2 ++ orbbec_camera/launch/gemini210.launch.py | 2 ++ orbbec_camera/launch/gemini2L.launch.py | 2 ++ orbbec_camera/launch/gemini305.launch.py | 2 ++ orbbec_camera/launch/gemini305_g.launch.py | 2 ++ orbbec_camera/launch/gemini345.launch.py | 2 ++ orbbec_camera/launch/gemini345_lg.launch.py | 2 ++ orbbec_camera/launch/gemini435_le.launch.py | 2 ++ orbbec_camera/launch/orbbec_camera.launch.py | 4 +++- 18 files changed, 37 insertions(+), 1 deletion(-) diff --git a/orbbec_camera/launch/astra.launch.py b/orbbec_camera/launch/astra.launch.py index 579fa0b3..4d693b64 100644 --- a/orbbec_camera/launch/astra.launch.py +++ b/orbbec_camera/launch/astra.launch.py @@ -68,6 +68,8 @@ def generate_launch_description(): DeclareLaunchArgument('enable_ldp', default_value='true'), DeclareLaunchArgument('ordered_pc', default_value='false'), DeclareLaunchArgument('use_hardware_time', default_value='false'), + DeclareLaunchArgument('enable_frame_timestamp_csv', default_value='false'), + DeclareLaunchArgument('frame_timestamp_csv_file', default_value=''), DeclareLaunchArgument('enable_depth_scale', default_value='true'), DeclareLaunchArgument('align_mode', default_value='HW'), DeclareLaunchArgument('laser_energy_level', default_value='-1'), diff --git a/orbbec_camera/launch/astra2.launch.py b/orbbec_camera/launch/astra2.launch.py index a71ab107..41901ef8 100644 --- a/orbbec_camera/launch/astra2.launch.py +++ b/orbbec_camera/launch/astra2.launch.py @@ -105,6 +105,8 @@ def generate_launch_description(): DeclareLaunchArgument("enable_heartbeat", default_value="false"), DeclareLaunchArgument("enable_firmware_log", default_value="false"), DeclareLaunchArgument("time_domain", default_value="global"), + DeclareLaunchArgument('enable_frame_timestamp_csv', default_value='false'), + DeclareLaunchArgument('frame_timestamp_csv_file', default_value=''), ] # Node configuration diff --git a/orbbec_camera/launch/dabai_a.launch.py b/orbbec_camera/launch/dabai_a.launch.py index ec904f89..cea90efe 100644 --- a/orbbec_camera/launch/dabai_a.launch.py +++ b/orbbec_camera/launch/dabai_a.launch.py @@ -206,6 +206,8 @@ def generate_launch_description(): DeclareLaunchArgument('retry_on_usb3_detection_failure', default_value='false'), DeclareLaunchArgument('enable_sync_host_time', default_value='true'), DeclareLaunchArgument('time_domain', default_value='global'),# global, device, system + DeclareLaunchArgument('enable_frame_timestamp_csv', default_value='false'), + DeclareLaunchArgument('frame_timestamp_csv_file', default_value=''), DeclareLaunchArgument('enable_color_undistortion', default_value='false'), DeclareLaunchArgument('config_file_path', default_value=''), DeclareLaunchArgument('enable_heartbeat', default_value='false'), diff --git a/orbbec_camera/launch/dabai_al.launch.py b/orbbec_camera/launch/dabai_al.launch.py index b43d6cd9..9d200a18 100644 --- a/orbbec_camera/launch/dabai_al.launch.py +++ b/orbbec_camera/launch/dabai_al.launch.py @@ -208,6 +208,8 @@ def generate_launch_description(): DeclareLaunchArgument('laser_energy_level', default_value='-1'), DeclareLaunchArgument('enable_sync_host_time', default_value='true'), DeclareLaunchArgument('time_domain', default_value='global'),# global, device, system + DeclareLaunchArgument('enable_frame_timestamp_csv', default_value='false'), + DeclareLaunchArgument('frame_timestamp_csv_file', default_value=''), DeclareLaunchArgument('enable_color_undistortion', default_value='false'), DeclareLaunchArgument('config_file_path', default_value=''), DeclareLaunchArgument('enable_heartbeat', default_value='false'), diff --git a/orbbec_camera/launch/dabai_dcw2.launch.py b/orbbec_camera/launch/dabai_dcw2.launch.py index 730b8f48..3c12b994 100644 --- a/orbbec_camera/launch/dabai_dcw2.launch.py +++ b/orbbec_camera/launch/dabai_dcw2.launch.py @@ -82,6 +82,8 @@ def generate_launch_description(): DeclareLaunchArgument('enable_lut_noise_removal_filter', default_value='false'), DeclareLaunchArgument('ordered_pc', default_value='false'), DeclareLaunchArgument('use_hardware_time', default_value='false'), + DeclareLaunchArgument('enable_frame_timestamp_csv', default_value='false'), + DeclareLaunchArgument('frame_timestamp_csv_file', default_value=''), DeclareLaunchArgument('enable_depth_scale', default_value='true'), DeclareLaunchArgument('align_mode', default_value='HW'), DeclareLaunchArgument('laser_energy_level', default_value='-1'), diff --git a/orbbec_camera/launch/dabai_max_pro.launch.py b/orbbec_camera/launch/dabai_max_pro.launch.py index 0deb592d..75a91626 100644 --- a/orbbec_camera/launch/dabai_max_pro.launch.py +++ b/orbbec_camera/launch/dabai_max_pro.launch.py @@ -76,6 +76,8 @@ def generate_launch_description(): DeclareLaunchArgument('enable_mgc_noise_removal_filter', default_value='false'), DeclareLaunchArgument('enable_lut_noise_removal_filter', default_value='false'), DeclareLaunchArgument('use_hardware_time', default_value='false'), + DeclareLaunchArgument('enable_frame_timestamp_csv', default_value='false'), + DeclareLaunchArgument('frame_timestamp_csv_file', default_value=''), DeclareLaunchArgument('align_mode', default_value='HW'), DeclareLaunchArgument('laser_energy_level', default_value='-1'), DeclareLaunchArgument('enable_heartbeat', default_value='false'), diff --git a/orbbec_camera/launch/femto.launch.py b/orbbec_camera/launch/femto.launch.py index 0646d796..ff348226 100644 --- a/orbbec_camera/launch/femto.launch.py +++ b/orbbec_camera/launch/femto.launch.py @@ -83,6 +83,8 @@ def generate_launch_description(): DeclareLaunchArgument("enable_heartbeat", default_value="false"), DeclareLaunchArgument("enable_firmware_log", default_value="false"), DeclareLaunchArgument("time_domain", default_value="device"), + DeclareLaunchArgument('enable_frame_timestamp_csv', default_value='false'), + DeclareLaunchArgument('frame_timestamp_csv_file', default_value=''), DeclareLaunchArgument('device_preset', default_value='Custom'), ] diff --git a/orbbec_camera/launch/femto_bolt.launch.py b/orbbec_camera/launch/femto_bolt.launch.py index a6ebb8bf..d2f416f0 100644 --- a/orbbec_camera/launch/femto_bolt.launch.py +++ b/orbbec_camera/launch/femto_bolt.launch.py @@ -111,6 +111,8 @@ def generate_launch_description(): DeclareLaunchArgument("enable_heartbeat", default_value="false"), DeclareLaunchArgument("enable_firmware_log", default_value="false"), DeclareLaunchArgument("time_domain", default_value="global"), + DeclareLaunchArgument('enable_frame_timestamp_csv', default_value='false'), + DeclareLaunchArgument('frame_timestamp_csv_file', default_value=''), DeclareLaunchArgument('device_preset', default_value='Custom'), ] diff --git a/orbbec_camera/launch/femto_mega.launch.py b/orbbec_camera/launch/femto_mega.launch.py index 884b27d1..4186589d 100644 --- a/orbbec_camera/launch/femto_mega.launch.py +++ b/orbbec_camera/launch/femto_mega.launch.py @@ -117,6 +117,8 @@ def generate_launch_description(): DeclareLaunchArgument("enable_heartbeat", default_value="false"), DeclareLaunchArgument("enable_firmware_log", default_value="false"), DeclareLaunchArgument("time_domain", default_value="global"), + DeclareLaunchArgument('enable_frame_timestamp_csv', default_value='false'), + DeclareLaunchArgument('frame_timestamp_csv_file', default_value=''), DeclareLaunchArgument('device_preset', default_value='Custom'), # Force IP parameters diff --git a/orbbec_camera/launch/gemini2.launch.py b/orbbec_camera/launch/gemini2.launch.py index ba98dd4a..8fcb71e2 100644 --- a/orbbec_camera/launch/gemini2.launch.py +++ b/orbbec_camera/launch/gemini2.launch.py @@ -131,6 +131,8 @@ def generate_launch_description(): DeclareLaunchArgument("enable_heartbeat", default_value="false"), DeclareLaunchArgument("enable_firmware_log", default_value="false"), DeclareLaunchArgument("time_domain", default_value="global"), + DeclareLaunchArgument('enable_frame_timestamp_csv', default_value='false'), + DeclareLaunchArgument('frame_timestamp_csv_file', default_value=''), ] # Node configuration diff --git a/orbbec_camera/launch/gemini210.launch.py b/orbbec_camera/launch/gemini210.launch.py index e0d5ec79..b9b3c4f3 100644 --- a/orbbec_camera/launch/gemini210.launch.py +++ b/orbbec_camera/launch/gemini210.launch.py @@ -131,6 +131,8 @@ def generate_launch_description(): DeclareLaunchArgument("enable_heartbeat", default_value="false"), DeclareLaunchArgument("enable_firmware_log", default_value="false"), DeclareLaunchArgument("time_domain", default_value="global"), + DeclareLaunchArgument('enable_frame_timestamp_csv', default_value='false'), + DeclareLaunchArgument('frame_timestamp_csv_file', default_value=''), ] # Node configuration diff --git a/orbbec_camera/launch/gemini2L.launch.py b/orbbec_camera/launch/gemini2L.launch.py index ac0ff4a3..52aa6572 100644 --- a/orbbec_camera/launch/gemini2L.launch.py +++ b/orbbec_camera/launch/gemini2L.launch.py @@ -121,6 +121,8 @@ def generate_launch_description(): DeclareLaunchArgument("enable_firmware_log", default_value="false"), DeclareLaunchArgument("enable_noise_removal_filter", default_value="false"), DeclareLaunchArgument("time_domain", default_value="global"), + DeclareLaunchArgument('enable_frame_timestamp_csv', default_value='false'), + DeclareLaunchArgument('frame_timestamp_csv_file', default_value=''), ] # Node configuration diff --git a/orbbec_camera/launch/gemini305.launch.py b/orbbec_camera/launch/gemini305.launch.py index 48811cec..82bd72f1 100644 --- a/orbbec_camera/launch/gemini305.launch.py +++ b/orbbec_camera/launch/gemini305.launch.py @@ -273,6 +273,8 @@ def generate_launch_description(): DeclareLaunchArgument('enable_sync_host_time', default_value='true'), DeclareLaunchArgument('time_sync_period', default_value='6.0'), # seconds DeclareLaunchArgument('time_domain', default_value='global'),# global, device, system + DeclareLaunchArgument('enable_frame_timestamp_csv', default_value='false'), + DeclareLaunchArgument('frame_timestamp_csv_file', default_value=''), DeclareLaunchArgument('enable_color_undistortion', default_value='false'), DeclareLaunchArgument('config_file_path', default_value=''), DeclareLaunchArgument('enable_heartbeat', default_value='false'), diff --git a/orbbec_camera/launch/gemini305_g.launch.py b/orbbec_camera/launch/gemini305_g.launch.py index 48811cec..82bd72f1 100644 --- a/orbbec_camera/launch/gemini305_g.launch.py +++ b/orbbec_camera/launch/gemini305_g.launch.py @@ -273,6 +273,8 @@ def generate_launch_description(): DeclareLaunchArgument('enable_sync_host_time', default_value='true'), DeclareLaunchArgument('time_sync_period', default_value='6.0'), # seconds DeclareLaunchArgument('time_domain', default_value='global'),# global, device, system + DeclareLaunchArgument('enable_frame_timestamp_csv', default_value='false'), + DeclareLaunchArgument('frame_timestamp_csv_file', default_value=''), DeclareLaunchArgument('enable_color_undistortion', default_value='false'), DeclareLaunchArgument('config_file_path', default_value=''), DeclareLaunchArgument('enable_heartbeat', default_value='false'), diff --git a/orbbec_camera/launch/gemini345.launch.py b/orbbec_camera/launch/gemini345.launch.py index 1df7ee8f..2225269d 100644 --- a/orbbec_camera/launch/gemini345.launch.py +++ b/orbbec_camera/launch/gemini345.launch.py @@ -205,6 +205,8 @@ def generate_launch_description(): DeclareLaunchArgument('retry_on_usb3_detection_failure', default_value='false'), DeclareLaunchArgument('enable_sync_host_time', default_value='true'), DeclareLaunchArgument('time_domain', default_value='global'),# global, device, system + DeclareLaunchArgument('enable_frame_timestamp_csv', default_value='false'), + DeclareLaunchArgument('frame_timestamp_csv_file', default_value=''), DeclareLaunchArgument('enable_color_undistortion', default_value='false'), DeclareLaunchArgument('config_file_path', default_value=''), DeclareLaunchArgument('enable_heartbeat', default_value='false'), diff --git a/orbbec_camera/launch/gemini345_lg.launch.py b/orbbec_camera/launch/gemini345_lg.launch.py index b43d6cd9..9d200a18 100644 --- a/orbbec_camera/launch/gemini345_lg.launch.py +++ b/orbbec_camera/launch/gemini345_lg.launch.py @@ -208,6 +208,8 @@ def generate_launch_description(): DeclareLaunchArgument('laser_energy_level', default_value='-1'), DeclareLaunchArgument('enable_sync_host_time', default_value='true'), DeclareLaunchArgument('time_domain', default_value='global'),# global, device, system + DeclareLaunchArgument('enable_frame_timestamp_csv', default_value='false'), + DeclareLaunchArgument('frame_timestamp_csv_file', default_value=''), DeclareLaunchArgument('enable_color_undistortion', default_value='false'), DeclareLaunchArgument('config_file_path', default_value=''), DeclareLaunchArgument('enable_heartbeat', default_value='false'), diff --git a/orbbec_camera/launch/gemini435_le.launch.py b/orbbec_camera/launch/gemini435_le.launch.py index 856118a0..99b65fc7 100644 --- a/orbbec_camera/launch/gemini435_le.launch.py +++ b/orbbec_camera/launch/gemini435_le.launch.py @@ -240,6 +240,8 @@ def generate_launch_description(): DeclareLaunchArgument('enable_sync_host_time', default_value='true'), DeclareLaunchArgument('time_sync_period', default_value='6.0'), # seconds DeclareLaunchArgument('time_domain', default_value='global'),# global, device, system + DeclareLaunchArgument('enable_frame_timestamp_csv', default_value='false'), + DeclareLaunchArgument('frame_timestamp_csv_file', default_value=''), DeclareLaunchArgument('enable_color_undistortion', default_value='false'), DeclareLaunchArgument('config_file_path', default_value=''), DeclareLaunchArgument('enable_heartbeat', default_value='false'), diff --git a/orbbec_camera/launch/orbbec_camera.launch.py b/orbbec_camera/launch/orbbec_camera.launch.py index 3a6104f3..33418d43 100644 --- a/orbbec_camera/launch/orbbec_camera.launch.py +++ b/orbbec_camera/launch/orbbec_camera.launch.py @@ -33,6 +33,8 @@ def generate_launch_arguments(): DeclareLaunchArgument('device_num', default_value='1'), DeclareLaunchArgument('sync_mode', default_value='standalone'), DeclareLaunchArgument('trigger_out_enabled', default_value='true'), + DeclareLaunchArgument('enable_frame_timestamp_csv', default_value='false'), + DeclareLaunchArgument('frame_timestamp_csv_file', default_value=''), #instra-process demo set DeclareLaunchArgument('use_intra_process_comms', default_value='false'), DeclareLaunchArgument('attach_component_container_enable', default_value='false'), @@ -182,4 +184,4 @@ def generate_launch_description(): args + [ OpaqueFunction(function=lambda context: create_node_action(context, args)) ] - ) \ No newline at end of file + )