mirror of
https://github.com/orbbec/OrbbecSDK_ROS2.git
synced 2026-10-06 13:07:47 +08:00
698 lines
25 KiB
C++
698 lines
25 KiB
C++
#include "orbbec_camera/frame_timestamp_csv_logger.h"
|
|
|
|
#include <algorithm>
|
|
#include <chrono>
|
|
#include <filesystem>
|
|
#include <iomanip>
|
|
#include <sstream>
|
|
#include <utility>
|
|
|
|
namespace orbbec_camera {
|
|
namespace {
|
|
|
|
constexpr size_t kCompletedQueueSoftLimit = 1000;
|
|
constexpr size_t kFlushBatchSize = 100;
|
|
// The header occupies the first row, leaving 1,024,575 rows for frame data.
|
|
constexpr uint64_t kMaxCsvRowsPerFileIncludingHeader = 1'024'576;
|
|
constexpr auto kFlushInterval = std::chrono::seconds(1);
|
|
|
|
int64_t getExpectedIntervalUs(const std::shared_ptr<ob::Frame> &frame) {
|
|
if (!frame || !frame->is<ob::VideoFrame>()) {
|
|
return 0;
|
|
}
|
|
auto stream_profile = frame->getStreamProfile();
|
|
if (!stream_profile || !stream_profile->is<ob::VideoStreamProfile>()) {
|
|
return 0;
|
|
}
|
|
const auto fps = stream_profile->as<ob::VideoStreamProfile>()->getFps();
|
|
if (fps == 0) {
|
|
return 0;
|
|
}
|
|
return static_cast<int64_t>(1000000.0 / static_cast<double>(fps));
|
|
}
|
|
|
|
} // namespace
|
|
|
|
FrameTimestampCsvLogger::FrameTimestampCsvLogger(bool drop_log_enabled,
|
|
const std::string &csv_file_path,
|
|
OutputMode output_mode, rclcpp::Logger logger)
|
|
: logger_(std::move(logger)),
|
|
enabled_(drop_log_enabled || !csv_file_path.empty()),
|
|
csv_enabled_(!csv_file_path.empty()),
|
|
drop_log_enabled_(drop_log_enabled),
|
|
csv_file_path_(csv_file_path),
|
|
output_mode_(output_mode) {
|
|
if (!enabled_) {
|
|
return;
|
|
}
|
|
|
|
if (csv_enabled_) {
|
|
try {
|
|
auto path = std::filesystem::path(csvFilePathForIndex(0));
|
|
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 "
|
|
<< csvFilePathForIndex(0) << ": " << e.what());
|
|
csv_enabled_ = false;
|
|
csv_writer_failed_ = true;
|
|
}
|
|
}
|
|
|
|
if (csv_enabled_) {
|
|
if (!openCsvFile(0)) {
|
|
csv_enabled_ = false;
|
|
csv_writer_failed_ = true;
|
|
}
|
|
}
|
|
|
|
enabled_ = csv_enabled_ || drop_log_enabled_;
|
|
if (csv_enabled_) {
|
|
writer_thread_ = std::thread([this]() { writerThreadMain(); });
|
|
}
|
|
|
|
if (enabled_) {
|
|
RCLCPP_INFO_STREAM(logger_,
|
|
"Frame timestamp logger enabled: csv_file="
|
|
<< (csv_enabled_ ? csvFilePathForIndex(0) : "disabled")
|
|
<< " frame_drop_log=" << (drop_log_enabled_ ? "enabled" : "disabled"));
|
|
}
|
|
}
|
|
|
|
FrameTimestampCsvLogger::~FrameTimestampCsvLogger() noexcept { shutdown(); }
|
|
|
|
void FrameTimestampCsvLogger::recordFrameSet(const std::shared_ptr<ob::Frame> &color_frame,
|
|
const std::shared_ptr<ob::Frame> &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_) {
|
|
return;
|
|
}
|
|
if (output_mode_ != OutputMode::SYNCED) {
|
|
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<ob::Frame> &frame,
|
|
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)) {
|
|
return;
|
|
}
|
|
recordStandaloneFrameArrivalInternal(stream_type, frame, arrival_system_us, arrival_steady_us,
|
|
image_publish_expected);
|
|
}
|
|
|
|
void FrameTimestampCsvLogger::recordPreImagePublish(OBStreamType stream_type,
|
|
const std::shared_ptr<ob::Frame> &frame,
|
|
int64_t publish_system_us,
|
|
int64_t publish_steady_us) {
|
|
if (!enabled_ || !frame || !isTrackedStream(stream_type)) {
|
|
return;
|
|
}
|
|
completeImagePublishInternal(stream_type, frame, publish_system_us, publish_steady_us);
|
|
}
|
|
|
|
void FrameTimestampCsvLogger::recordImagePublishSkipped(OBStreamType stream_type,
|
|
const std::shared_ptr<ob::Frame> &frame) {
|
|
if (!enabled_ || !frame || !isTrackedStream(stream_type)) {
|
|
return;
|
|
}
|
|
completeImagePublishInternal(stream_type, frame, std::nullopt, std::nullopt);
|
|
}
|
|
|
|
void FrameTimestampCsvLogger::shutdown() {
|
|
if (!enabled_) {
|
|
return;
|
|
}
|
|
|
|
{
|
|
std::lock_guard<std::mutex> state_lock(state_mutex_);
|
|
if (shutdown_requested_) {
|
|
return;
|
|
}
|
|
shutdown_requested_ = true;
|
|
|
|
std::vector<PendingRow> 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<ob::Frame> &color_frame, const std::shared_ptr<ob::Frame> &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<PendingRow> ready_rows;
|
|
{
|
|
std::lock_guard<std::mutex> 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<ob::Frame> &frame, int64_t arrival_system_us,
|
|
int64_t arrival_steady_us, bool image_publish_expected) {
|
|
std::optional<PendingRow> ready_row;
|
|
{
|
|
std::lock_guard<std::mutex> 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::completeImagePublishInternal(
|
|
OBStreamType stream_type, const std::shared_ptr<ob::Frame> &frame,
|
|
std::optional<int64_t> publish_system_us, std::optional<int64_t> publish_steady_us) {
|
|
std::optional<PendingRow> ready_row;
|
|
const auto frame_index = frame->getIndex();
|
|
|
|
{
|
|
std::lock_guard<std::mutex> 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()) {
|
|
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);
|
|
}
|
|
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;
|
|
if (state.final) {
|
|
return;
|
|
}
|
|
if (publish_system_us.has_value() && publish_steady_us.has_value()) {
|
|
populatePublishData(state, tracked_stream, publish_system_us.value(),
|
|
publish_steady_us.value());
|
|
state.final = true;
|
|
} else {
|
|
finalizeStreamWithoutPublish(state);
|
|
}
|
|
|
|
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<ob::Frame> &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<int64_t>(frame->getTimeStampUs());
|
|
if (previous.expected_interval_us <= 0) {
|
|
previous.expected_interval_us = getExpectedIntervalUs(frame);
|
|
}
|
|
state.expected_interval_us = previous.expected_interval_us;
|
|
if (previous.device_ts_us.has_value() && state.expected_interval_us > 0) {
|
|
const auto device_ts_delta_us = state.device_ts_us - previous.device_ts_us.value();
|
|
if (device_ts_delta_us > state.expected_interval_us * 3 / 2) {
|
|
const auto lost_frames =
|
|
std::max<int64_t>(1, device_ts_delta_us / state.expected_interval_us - 1);
|
|
if (drop_log_enabled_) {
|
|
previous.dropped_frames += lost_frames;
|
|
RCLCPP_WARN_STREAM(logger_,
|
|
"Frame drop detected: stage=SDK_RECEIVE"
|
|
<< " stream=" << (stream == TrackedStream::COLOR ? "color" : "depth")
|
|
<< " frame_index=" << state.frame_index
|
|
<< " dropped=" << previous.dropped_frames);
|
|
}
|
|
}
|
|
}
|
|
if (frame->hasMetadata(OB_FRAME_METADATA_TYPE_FRAME_NUMBER)) {
|
|
state.metadata_frame_number =
|
|
static_cast<int64_t>(frame->getMetadataValue(OB_FRAME_METADATA_TYPE_FRAME_NUMBER));
|
|
} else {
|
|
state.metadata_frame_number.reset();
|
|
}
|
|
if (frame->hasMetadata(OB_FRAME_METADATA_TYPE_SENSOR_TIMESTAMP)) {
|
|
state.sensor_ts_us =
|
|
static_cast<int64_t>(frame->getMetadataValue(OB_FRAME_METADATA_TYPE_SENSOR_TIMESTAMP));
|
|
} else {
|
|
state.sensor_ts_us.reset();
|
|
}
|
|
state.global_ts_us = static_cast<int64_t>(frame->getGlobalTimeStampUs());
|
|
state.sdk_system_ts_us = static_cast<int64_t>(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);
|
|
if (state.sensor_ts_us.has_value()) {
|
|
state.sensor_ts_delta_us = updateDelta(previous.sensor_ts_us, state.sensor_ts_us.value());
|
|
} else {
|
|
state.sensor_ts_delta_us.reset();
|
|
previous.sensor_ts_us.reset();
|
|
}
|
|
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);
|
|
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_;
|
|
|
|
if (previous.publish_device_ts_us.has_value()) {
|
|
const auto device_ts_delta_us = state.device_ts_us - previous.publish_device_ts_us.value();
|
|
if (state.expected_interval_us > 0 && device_ts_delta_us > state.expected_interval_us * 3 / 2) {
|
|
const auto lost_frames =
|
|
std::max<int64_t>(1, device_ts_delta_us / state.expected_interval_us - 1);
|
|
if (drop_log_enabled_) {
|
|
previous.publish_dropped_frames += lost_frames;
|
|
RCLCPP_WARN_STREAM(logger_,
|
|
"Frame drop detected: stage=ROS_PUBLISH"
|
|
<< " stream=" << (stream == TrackedStream::COLOR ? "color" : "depth")
|
|
<< " frame_index=" << state.frame_index
|
|
<< " dropped=" << previous.publish_dropped_frames);
|
|
}
|
|
}
|
|
}
|
|
previous.publish_device_ts_us = state.device_ts_us;
|
|
|
|
state.publish_system_us = publish_system_us;
|
|
state.publish_steady_us = publish_steady_us;
|
|
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_steady_us = state.publish_steady_us.value() - state.arrival_steady_us;
|
|
}
|
|
|
|
std::optional<int64_t> FrameTimestampCsvLogger::updateDelta(std::optional<int64_t> &previous,
|
|
int64_t current) {
|
|
std::optional<int64_t> 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) {
|
|
if (!csv_enabled_ || csv_writer_failed_) {
|
|
return;
|
|
}
|
|
std::lock_guard<std::mutex> 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<PendingRow> &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));
|
|
}
|
|
std::stable_sort(rows.begin(), rows.end(),
|
|
[](const auto &lhs, const auto &rhs) { return lhs.row_id < rhs.row_id; });
|
|
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 {
|
|
if (output_mode_ == OutputMode::COLOR) {
|
|
return serializeStreamColumns(row.color);
|
|
}
|
|
if (output_mode_ == OutputMode::DEPTH) {
|
|
return serializeStreamColumns(row.depth);
|
|
}
|
|
std::ostringstream ss;
|
|
ss << serializeStreamColumns(row.color) << "," << serializeStreamColumns(row.depth);
|
|
return ss.str();
|
|
}
|
|
|
|
std::string FrameTimestampCsvLogger::serializeStreamColumns(const StreamState &state) const {
|
|
std::vector<std::string> fields(15, "");
|
|
if (state.has_frame) {
|
|
fields[0] = std::to_string(state.frame_index);
|
|
fields[1] = formatOptionalIntColumn(state.metadata_frame_number);
|
|
if (state.sensor_ts_us.has_value()) {
|
|
fields[2] = formatSecondsColumn(state.sensor_ts_us.value());
|
|
}
|
|
fields[3] = formatOptionalIntColumn(state.sensor_ts_delta_us);
|
|
fields[4] = formatSecondsColumn(state.device_ts_us);
|
|
fields[5] = formatOptionalIntColumn(state.device_ts_delta_us);
|
|
fields[6] = formatSecondsColumn(state.global_ts_us);
|
|
fields[7] = formatOptionalIntColumn(state.global_ts_delta_us);
|
|
fields[8] = formatSecondsColumn(state.sdk_system_ts_us);
|
|
fields[9] = formatOptionalIntColumn(state.sdk_system_ts_delta_us);
|
|
fields[10] = formatOptionalIntColumn(state.arrival_steady_delta_us);
|
|
fields[11] = formatOptionalIntColumn(state.publish_steady_delta_us);
|
|
fields[12] = formatOptionalIntColumn(state.arrival_to_publish_steady_us);
|
|
fields[13] = formatOptionalIntColumn(state.sdk_delay_from_global_us);
|
|
fields[14] = 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<long double>(time_us) / 1000000.0L);
|
|
return ss.str();
|
|
}
|
|
|
|
std::string FrameTimestampCsvLogger::formatOptionalIntColumn(const std::optional<int64_t> &value) {
|
|
if (!value.has_value()) {
|
|
return "";
|
|
}
|
|
return std::to_string(*value);
|
|
}
|
|
|
|
std::string FrameTimestampCsvLogger::csvHeader() const {
|
|
std::ostringstream ss;
|
|
const auto append_stream_header = [&ss](const char *prefix) {
|
|
ss << prefix << "_sdk_frame_index,";
|
|
ss << prefix << "_hardware_frame_number,";
|
|
ss << prefix << "_sensor_ts_sec,";
|
|
ss << prefix << "_sensor_ts_delta_us,";
|
|
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_steady_delta_us,";
|
|
ss << prefix << "_publish_steady_delta_us,";
|
|
ss << prefix << "_arrival_to_publish_steady_us,";
|
|
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) {
|
|
ss << ",";
|
|
}
|
|
if (output_mode_ == OutputMode::SYNCED || output_mode_ == OutputMode::DEPTH) {
|
|
append_stream_header("depth");
|
|
}
|
|
return ss.str();
|
|
}
|
|
|
|
void FrameTimestampCsvLogger::writerThreadMain() {
|
|
if (!csv_enabled_ || csv_writer_failed_) {
|
|
return;
|
|
}
|
|
|
|
size_t rows_since_flush = 0;
|
|
auto last_flush = std::chrono::steady_clock::now();
|
|
|
|
while (true) {
|
|
std::deque<PendingRow> rows_to_write;
|
|
{
|
|
std::unique_lock<std::mutex> lock(completed_rows_mutex_);
|
|
completed_rows_cv_.wait_for(lock, kFlushInterval, [this]() {
|
|
return shutdown_requested_ || !completed_rows_.empty();
|
|
});
|
|
rows_to_write.swap(completed_rows_);
|
|
}
|
|
|
|
std::stable_sort(rows_to_write.begin(), rows_to_write.end(),
|
|
[](const auto &lhs, const auto &rhs) { return lhs.row_id < rhs.row_id; });
|
|
|
|
for (const auto &row : rows_to_write) {
|
|
if (csv_rows_written_ >= kMaxCsvRowsPerFileIncludingHeader) {
|
|
if (!rotateCsvFile()) {
|
|
csv_writer_failed_ = true;
|
|
break;
|
|
}
|
|
rows_since_flush = 0;
|
|
last_flush = std::chrono::steady_clock::now();
|
|
}
|
|
|
|
if (!csv_stream_.is_open()) {
|
|
csv_writer_failed_ = true;
|
|
break;
|
|
}
|
|
|
|
csv_stream_ << serializeRow(row) << "\n";
|
|
if (!csv_stream_) {
|
|
RCLCPP_ERROR_STREAM(logger_, "Failed to write frame timestamp CSV file: "
|
|
<< csvFilePathForIndex(csv_file_index_));
|
|
csv_writer_failed_ = true;
|
|
break;
|
|
}
|
|
++csv_rows_written_;
|
|
++rows_since_flush;
|
|
}
|
|
|
|
if (csv_writer_failed_) {
|
|
break;
|
|
}
|
|
|
|
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<std::mutex> lock(completed_rows_mutex_);
|
|
if (shutdown_requested_ && completed_rows_.empty()) {
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
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";
|
|
}
|
|
|
|
auto indexed_filename = original_path.stem().string() + suffix;
|
|
if (file_index != 0) {
|
|
indexed_filename += "_" + std::to_string(file_index);
|
|
}
|
|
indexed_filename += original_path.extension().string();
|
|
return (original_path.parent_path() / indexed_filename).string();
|
|
}
|
|
|
|
bool FrameTimestampCsvLogger::openCsvFile(uint64_t file_index) {
|
|
const auto file_path = csvFilePathForIndex(file_index);
|
|
csv_stream_.clear();
|
|
csv_stream_.open(file_path, std::ios::out | std::ios::trunc);
|
|
if (!csv_stream_.is_open()) {
|
|
RCLCPP_ERROR_STREAM(logger_, "Failed to open frame timestamp CSV file: " << file_path);
|
|
return false;
|
|
}
|
|
|
|
csv_stream_ << csvHeader() << "\n";
|
|
csv_stream_.flush();
|
|
if (!csv_stream_) {
|
|
RCLCPP_ERROR_STREAM(logger_, "Failed to write frame timestamp CSV header: " << file_path);
|
|
csv_stream_.close();
|
|
return false;
|
|
}
|
|
|
|
csv_file_index_ = file_index;
|
|
csv_rows_written_ = 1;
|
|
return true;
|
|
}
|
|
|
|
bool FrameTimestampCsvLogger::rotateCsvFile() {
|
|
if (csv_stream_.is_open()) {
|
|
csv_stream_.flush();
|
|
csv_stream_.close();
|
|
}
|
|
|
|
const auto next_file_index = csv_file_index_ + 1;
|
|
if (!openCsvFile(next_file_index)) {
|
|
return false;
|
|
}
|
|
|
|
RCLCPP_INFO_STREAM(logger_, "Frame timestamp CSV reached "
|
|
<< kMaxCsvRowsPerFileIncludingHeader << " rows; continuing in "
|
|
<< csvFilePathForIndex(csv_file_index_));
|
|
return true;
|
|
}
|
|
|
|
} // namespace orbbec_camera
|