mirror of
https://github.com/orbbec/OrbbecSDK_ROS2.git
synced 2026-10-04 20:17:47 +08:00
Merge branch 'feat/imu-timestamp-csv' into v2/develop
This commit is contained in:
ob-yalian
@@ -159,6 +159,8 @@ set(SOURCE_FILES
|
||||
src/dynamic_params.cpp
|
||||
src/image_publisher.cpp
|
||||
src/frame_timestamp_csv_logger.cpp
|
||||
src/imu_timestamp_csv_logger.cpp
|
||||
src/timestamp_csv_logger.cpp
|
||||
src/ob_camera_node_driver.cpp
|
||||
src/ob_camera_node.cpp
|
||||
src/ob_lidar_node.cpp
|
||||
|
||||
@@ -21,8 +21,10 @@ namespace orbbec_camera {
|
||||
|
||||
class FrameTimestampCsvLogger {
|
||||
public:
|
||||
enum class OutputMode { SYNCED, COLOR, DEPTH };
|
||||
|
||||
FrameTimestampCsvLogger(bool drop_log_enabled, const std::string &csv_file_path,
|
||||
rclcpp::Logger logger);
|
||||
OutputMode output_mode, rclcpp::Logger logger);
|
||||
|
||||
~FrameTimestampCsvLogger() noexcept;
|
||||
|
||||
@@ -137,7 +139,7 @@ class FrameTimestampCsvLogger {
|
||||
std::string serializeStreamColumns(const StreamState &state) const;
|
||||
static std::string formatSecondsColumn(int64_t time_us);
|
||||
static std::string formatOptionalIntColumn(const std::optional<int64_t> &value);
|
||||
static std::string csvHeader();
|
||||
std::string csvHeader() const;
|
||||
|
||||
void writerThreadMain();
|
||||
std::string csvFilePathForIndex(uint64_t file_index) const;
|
||||
@@ -152,6 +154,7 @@ class FrameTimestampCsvLogger {
|
||||
std::atomic_bool csv_writer_failed_{false};
|
||||
bool queue_warning_active_ = false;
|
||||
std::string csv_file_path_;
|
||||
OutputMode output_mode_;
|
||||
std::ofstream csv_stream_;
|
||||
std::thread writer_thread_;
|
||||
uint64_t csv_file_index_ = 0;
|
||||
|
||||
@@ -0,0 +1,99 @@
|
||||
#pragma once
|
||||
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
|
||||
#include <atomic>
|
||||
#include <condition_variable>
|
||||
#include <cstdint>
|
||||
#include <deque>
|
||||
#include <fstream>
|
||||
#include <memory>
|
||||
#include <mutex>
|
||||
#include <optional>
|
||||
#include <string>
|
||||
#include <thread>
|
||||
|
||||
#include "libobsensor/ObSensor.hpp"
|
||||
|
||||
namespace orbbec_camera {
|
||||
|
||||
class ImuTimestampCsvLogger {
|
||||
public:
|
||||
enum class OutputMode { SYNCED, ACCEL, GYRO };
|
||||
|
||||
ImuTimestampCsvLogger(const std::string &frame_csv_file_path, OutputMode output_mode,
|
||||
rclcpp::Logger logger);
|
||||
|
||||
~ImuTimestampCsvLogger() noexcept;
|
||||
|
||||
ImuTimestampCsvLogger(const ImuTimestampCsvLogger &) = delete;
|
||||
ImuTimestampCsvLogger &operator=(const ImuTimestampCsvLogger &) = delete;
|
||||
|
||||
void recordFrameSet(const std::shared_ptr<ob::Frame> &accel_frame,
|
||||
const std::shared_ptr<ob::Frame> &gyro_frame, int64_t arrival_system_us,
|
||||
std::optional<int64_t> publish_system_us);
|
||||
|
||||
void recordStandaloneFrame(OBStreamType stream_type, const std::shared_ptr<ob::Frame> &frame,
|
||||
int64_t arrival_system_us, std::optional<int64_t> publish_system_us);
|
||||
|
||||
void shutdown();
|
||||
|
||||
bool enabled() const { return enabled_; }
|
||||
|
||||
private:
|
||||
struct StreamState {
|
||||
bool has_frame = false;
|
||||
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;
|
||||
std::optional<int64_t> publish_system_us;
|
||||
};
|
||||
|
||||
struct PendingRow {
|
||||
uint64_t row_id = 0;
|
||||
StreamState accel;
|
||||
StreamState gyro;
|
||||
};
|
||||
|
||||
void recordFrames(const std::shared_ptr<ob::Frame> &accel_frame,
|
||||
const std::shared_ptr<ob::Frame> &gyro_frame, int64_t arrival_system_us,
|
||||
std::optional<int64_t> publish_system_us);
|
||||
static void populateStreamState(StreamState &state, const std::shared_ptr<ob::Frame> &frame,
|
||||
int64_t arrival_system_us,
|
||||
std::optional<int64_t> publish_system_us);
|
||||
|
||||
void enqueueCompletedRow(const PendingRow &row);
|
||||
std::string serializeRow(const PendingRow &row) const;
|
||||
static std::string serializeStreamColumns(const StreamState &state);
|
||||
static std::string formatSecondsColumn(int64_t time_us);
|
||||
static std::string formatOptionalSecondsColumn(const std::optional<int64_t> &value);
|
||||
std::string csvHeader() const;
|
||||
|
||||
void writerThreadMain();
|
||||
std::string csvFilePathForIndex(uint64_t file_index) const;
|
||||
bool openCsvFile(uint64_t file_index);
|
||||
bool rotateCsvFile();
|
||||
|
||||
rclcpp::Logger logger_;
|
||||
bool enabled_ = false;
|
||||
bool csv_enabled_ = false;
|
||||
std::atomic_bool shutdown_requested_{false};
|
||||
std::atomic_bool csv_writer_failed_{false};
|
||||
bool queue_warning_active_ = false;
|
||||
std::string frame_csv_file_path_;
|
||||
OutputMode output_mode_;
|
||||
std::ofstream csv_stream_;
|
||||
std::thread writer_thread_;
|
||||
uint64_t csv_file_index_ = 0;
|
||||
uint64_t csv_rows_written_ = 0;
|
||||
|
||||
uint64_t next_row_id_ = 1;
|
||||
std::deque<PendingRow> completed_rows_;
|
||||
|
||||
std::mutex state_mutex_;
|
||||
std::mutex completed_rows_mutex_;
|
||||
std::condition_variable completed_rows_cv_;
|
||||
};
|
||||
|
||||
} // namespace orbbec_camera
|
||||
@@ -75,7 +75,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 "orbbec_camera/timestamp_csv_logger.h"
|
||||
#include "jpeg_decoder.h"
|
||||
#include <std_msgs/msg/header.hpp>
|
||||
#include <fcntl.h>
|
||||
@@ -626,7 +626,8 @@ class OBCameraNode {
|
||||
const std::shared_ptr<ob::Frame>& frame);
|
||||
|
||||
void onNewIMUFrameSyncOutputCallback(const std::shared_ptr<ob::Frame>& accelframe,
|
||||
const std::shared_ptr<ob::Frame>& gryoframe);
|
||||
const std::shared_ptr<ob::Frame>& gryoframe,
|
||||
int64_t arrival_system_us);
|
||||
|
||||
void onNewIMUFrameCallback(const std::shared_ptr<ob::Frame>& frame,
|
||||
const stream_index_pair& stream_index);
|
||||
@@ -1086,7 +1087,7 @@ class OBCameraNode {
|
||||
std::string time_domain_ = "global"; // device, system, global
|
||||
bool enable_frame_drop_log_ = false;
|
||||
std::string frame_timestamp_csv_file_;
|
||||
std::unique_ptr<FrameTimestampCsvLogger> frame_timestamp_csv_logger_;
|
||||
std::unique_ptr<TimestampCsvLogger> timestamp_csv_logger_;
|
||||
std::string exposure_range_mode_;
|
||||
std::string load_config_json_file_path_ = "";
|
||||
std::string export_config_json_file_path_ = "";
|
||||
|
||||
@@ -0,0 +1,73 @@
|
||||
#pragma once
|
||||
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
|
||||
#include <atomic>
|
||||
#include <cstdint>
|
||||
#include <memory>
|
||||
#include <optional>
|
||||
#include <string>
|
||||
|
||||
#include "libobsensor/ObSensor.hpp"
|
||||
|
||||
namespace orbbec_camera {
|
||||
|
||||
class FrameTimestampCsvLogger;
|
||||
class ImuTimestampCsvLogger;
|
||||
|
||||
class TimestampCsvLogger {
|
||||
public:
|
||||
struct Config {
|
||||
bool frame_drop_log_enabled = false;
|
||||
std::string csv_file_path;
|
||||
bool frame_sync_enabled = false;
|
||||
bool color_enabled = false;
|
||||
bool depth_enabled = false;
|
||||
bool imu_sync_enabled = false;
|
||||
bool accel_enabled = false;
|
||||
bool gyro_enabled = false;
|
||||
};
|
||||
|
||||
TimestampCsvLogger(Config config, rclcpp::Logger logger);
|
||||
~TimestampCsvLogger() noexcept;
|
||||
|
||||
TimestampCsvLogger(const TimestampCsvLogger &) = delete;
|
||||
TimestampCsvLogger &operator=(const TimestampCsvLogger &) = delete;
|
||||
|
||||
bool enabled() const;
|
||||
bool imageEnabled() const;
|
||||
bool imageStreamEnabled(OBStreamType stream_type) const;
|
||||
bool syncedImuEnabled() const;
|
||||
bool standaloneImuEnabled(OBStreamType stream_type) const;
|
||||
|
||||
void recordImageFrameSet(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);
|
||||
void recordImagePrePublish(OBStreamType stream_type, const std::shared_ptr<ob::Frame> &frame,
|
||||
int64_t publish_system_us, int64_t publish_steady_us);
|
||||
void recordImagePublishSkipped(OBStreamType stream_type, const std::shared_ptr<ob::Frame> &frame);
|
||||
|
||||
void recordSyncedImu(const std::shared_ptr<ob::Frame> &accel_frame,
|
||||
const std::shared_ptr<ob::Frame> &gyro_frame, int64_t arrival_system_us,
|
||||
std::optional<int64_t> publish_system_us);
|
||||
void recordStandaloneImu(OBStreamType stream_type, const std::shared_ptr<ob::Frame> &frame,
|
||||
int64_t arrival_system_us, std::optional<int64_t> publish_system_us);
|
||||
|
||||
void shutdown() noexcept;
|
||||
|
||||
private:
|
||||
FrameTimestampCsvLogger *imageLoggerForStream(OBStreamType stream_type) const;
|
||||
ImuTimestampCsvLogger *standaloneImuLoggerForStream(OBStreamType stream_type) const;
|
||||
|
||||
rclcpp::Logger logger_;
|
||||
std::atomic_bool shutdown_requested_{false};
|
||||
std::unique_ptr<FrameTimestampCsvLogger> synced_image_logger_;
|
||||
std::unique_ptr<FrameTimestampCsvLogger> color_logger_;
|
||||
std::unique_ptr<FrameTimestampCsvLogger> depth_logger_;
|
||||
std::unique_ptr<ImuTimestampCsvLogger> synced_imu_logger_;
|
||||
std::unique_ptr<ImuTimestampCsvLogger> accel_logger_;
|
||||
std::unique_ptr<ImuTimestampCsvLogger> gyro_logger_;
|
||||
};
|
||||
|
||||
} // namespace orbbec_camera
|
||||
@@ -35,25 +35,26 @@ int64_t getExpectedIntervalUs(const std::shared_ptr<ob::Frame> &frame) {
|
||||
|
||||
FrameTimestampCsvLogger::FrameTimestampCsvLogger(bool drop_log_enabled,
|
||||
const std::string &csv_file_path,
|
||||
rclcpp::Logger logger)
|
||||
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) {
|
||||
csv_file_path_(csv_file_path),
|
||||
output_mode_(output_mode) {
|
||||
if (!enabled_) {
|
||||
return;
|
||||
}
|
||||
|
||||
if (csv_enabled_) {
|
||||
try {
|
||||
auto path = std::filesystem::path(csv_file_path_);
|
||||
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 "
|
||||
<< csv_file_path_ << ": " << e.what());
|
||||
<< csvFilePathForIndex(0) << ": " << e.what());
|
||||
csv_enabled_ = false;
|
||||
csv_writer_failed_ = true;
|
||||
}
|
||||
@@ -74,7 +75,7 @@ FrameTimestampCsvLogger::FrameTimestampCsvLogger(bool drop_log_enabled,
|
||||
if (enabled_) {
|
||||
RCLCPP_INFO_STREAM(logger_,
|
||||
"Frame timestamp logger enabled: csv_file="
|
||||
<< (csv_enabled_ ? csv_file_path_ : "disabled")
|
||||
<< (csv_enabled_ ? csvFilePathForIndex(0) : "disabled")
|
||||
<< " frame_drop_log=" << (drop_log_enabled_ ? "enabled" : "disabled"));
|
||||
}
|
||||
}
|
||||
@@ -90,6 +91,9 @@ void FrameTimestampCsvLogger::recordFrameSet(const std::shared_ptr<ob::Frame> &c
|
||||
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);
|
||||
@@ -100,7 +104,9 @@ 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)) {
|
||||
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,
|
||||
@@ -479,6 +485,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);
|
||||
}
|
||||
std::ostringstream ss;
|
||||
ss << serializeStreamColumns(row.color) << "," << serializeStreamColumns(row.depth);
|
||||
return ss.str();
|
||||
@@ -529,9 +541,9 @@ std::string FrameTimestampCsvLogger::formatOptionalIntColumn(const std::optional
|
||||
return std::to_string(*value);
|
||||
}
|
||||
|
||||
std::string FrameTimestampCsvLogger::csvHeader() {
|
||||
std::string FrameTimestampCsvLogger::csvHeader() const {
|
||||
std::ostringstream ss;
|
||||
for (const auto *prefix : {"color", "depth"}) {
|
||||
const auto append_stream_header = [&ss](const char *prefix) {
|
||||
ss << prefix << "_sdk_frame_index,";
|
||||
ss << prefix << "_hardware_frame_number,";
|
||||
ss << prefix << "_sensor_ts_sec,";
|
||||
@@ -547,9 +559,15 @@ std::string FrameTimestampCsvLogger::csvHeader() {
|
||||
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 << ",";
|
||||
}
|
||||
};
|
||||
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();
|
||||
}
|
||||
@@ -621,13 +639,19 @@ void FrameTimestampCsvLogger::writerThreadMain() {
|
||||
}
|
||||
|
||||
std::string FrameTimestampCsvLogger::csvFilePathForIndex(uint64_t file_index) const {
|
||||
if (file_index == 0) {
|
||||
return csv_file_path_;
|
||||
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";
|
||||
}
|
||||
|
||||
const std::filesystem::path original_path(csv_file_path_);
|
||||
const auto indexed_filename = original_path.stem().string() + "_" + std::to_string(file_index) +
|
||||
original_path.extension().string();
|
||||
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();
|
||||
}
|
||||
|
||||
|
||||
@@ -0,0 +1,354 @@
|
||||
#include "orbbec_camera/imu_timestamp_csv_logger.h"
|
||||
|
||||
#include <algorithm>
|
||||
#include <chrono>
|
||||
#include <filesystem>
|
||||
#include <iomanip>
|
||||
#include <sstream>
|
||||
#include <utility>
|
||||
#include <vector>
|
||||
|
||||
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 IMU data.
|
||||
constexpr uint64_t kMaxCsvRowsPerFileIncludingHeader = 1'024'576;
|
||||
constexpr auto kFlushInterval = std::chrono::seconds(1);
|
||||
|
||||
} // namespace
|
||||
|
||||
ImuTimestampCsvLogger::ImuTimestampCsvLogger(const std::string &frame_csv_file_path,
|
||||
OutputMode output_mode, rclcpp::Logger logger)
|
||||
: logger_(std::move(logger)),
|
||||
enabled_(!frame_csv_file_path.empty()),
|
||||
csv_enabled_(!frame_csv_file_path.empty()),
|
||||
frame_csv_file_path_(frame_csv_file_path),
|
||||
output_mode_(output_mode) {
|
||||
if (!enabled_) {
|
||||
return;
|
||||
}
|
||||
|
||||
if (csv_enabled_) {
|
||||
try {
|
||||
const 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 IMU timestamp CSV path "
|
||||
<< csvFilePathForIndex(0) << ": " << e.what());
|
||||
csv_enabled_ = false;
|
||||
csv_writer_failed_ = true;
|
||||
}
|
||||
}
|
||||
|
||||
if (csv_enabled_ && !openCsvFile(0)) {
|
||||
csv_enabled_ = false;
|
||||
csv_writer_failed_ = true;
|
||||
}
|
||||
|
||||
enabled_ = csv_enabled_;
|
||||
if (csv_enabled_) {
|
||||
writer_thread_ = std::thread([this]() { writerThreadMain(); });
|
||||
}
|
||||
|
||||
if (enabled_) {
|
||||
RCLCPP_INFO_STREAM(logger_,
|
||||
"IMU timestamp logger enabled: csv_file=" << csvFilePathForIndex(0));
|
||||
}
|
||||
}
|
||||
|
||||
ImuTimestampCsvLogger::~ImuTimestampCsvLogger() noexcept { shutdown(); }
|
||||
|
||||
void ImuTimestampCsvLogger::recordFrameSet(const std::shared_ptr<ob::Frame> &accel_frame,
|
||||
const std::shared_ptr<ob::Frame> &gyro_frame,
|
||||
int64_t arrival_system_us,
|
||||
std::optional<int64_t> publish_system_us) {
|
||||
if (!enabled_ || output_mode_ != OutputMode::SYNCED || (!accel_frame && !gyro_frame)) {
|
||||
return;
|
||||
}
|
||||
recordFrames(accel_frame, gyro_frame, arrival_system_us, publish_system_us);
|
||||
}
|
||||
|
||||
void ImuTimestampCsvLogger::recordStandaloneFrame(OBStreamType stream_type,
|
||||
const std::shared_ptr<ob::Frame> &frame,
|
||||
int64_t arrival_system_us,
|
||||
std::optional<int64_t> publish_system_us) {
|
||||
if (!enabled_ || !frame) {
|
||||
return;
|
||||
}
|
||||
if (stream_type == OB_STREAM_ACCEL && output_mode_ == OutputMode::ACCEL) {
|
||||
recordFrames(frame, nullptr, arrival_system_us, publish_system_us);
|
||||
} else if (stream_type == OB_STREAM_GYRO && output_mode_ == OutputMode::GYRO) {
|
||||
recordFrames(nullptr, frame, arrival_system_us, publish_system_us);
|
||||
}
|
||||
}
|
||||
|
||||
void ImuTimestampCsvLogger::shutdown() {
|
||||
if (!enabled_) {
|
||||
return;
|
||||
}
|
||||
|
||||
{
|
||||
std::lock_guard<std::mutex> state_lock(state_mutex_);
|
||||
if (shutdown_requested_) {
|
||||
return;
|
||||
}
|
||||
shutdown_requested_ = true;
|
||||
}
|
||||
|
||||
completed_rows_cv_.notify_all();
|
||||
if (writer_thread_.joinable()) {
|
||||
writer_thread_.join();
|
||||
}
|
||||
|
||||
if (csv_stream_.is_open()) {
|
||||
csv_stream_.flush();
|
||||
csv_stream_.close();
|
||||
}
|
||||
}
|
||||
|
||||
void ImuTimestampCsvLogger::recordFrames(const std::shared_ptr<ob::Frame> &accel_frame,
|
||||
const std::shared_ptr<ob::Frame> &gyro_frame,
|
||||
int64_t arrival_system_us,
|
||||
std::optional<int64_t> publish_system_us) {
|
||||
std::lock_guard<std::mutex> lock(state_mutex_);
|
||||
if (shutdown_requested_) {
|
||||
return;
|
||||
}
|
||||
|
||||
PendingRow row;
|
||||
row.row_id = next_row_id_++;
|
||||
if (accel_frame) {
|
||||
populateStreamState(row.accel, accel_frame, arrival_system_us, publish_system_us);
|
||||
}
|
||||
if (gyro_frame) {
|
||||
populateStreamState(row.gyro, gyro_frame, arrival_system_us, publish_system_us);
|
||||
}
|
||||
enqueueCompletedRow(row);
|
||||
}
|
||||
|
||||
void ImuTimestampCsvLogger::populateStreamState(StreamState &state,
|
||||
const std::shared_ptr<ob::Frame> &frame,
|
||||
int64_t arrival_system_us,
|
||||
std::optional<int64_t> publish_system_us) {
|
||||
state.has_frame = true;
|
||||
state.device_ts_us = static_cast<int64_t>(frame->getTimeStampUs());
|
||||
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.publish_system_us = publish_system_us;
|
||||
}
|
||||
|
||||
void ImuTimestampCsvLogger::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_, "IMU timestamp CSV queue size exceeded " << kCompletedQueueSoftLimit << " rows");
|
||||
queue_warning_active_ = true;
|
||||
}
|
||||
} else {
|
||||
queue_warning_active_ = false;
|
||||
}
|
||||
completed_rows_cv_.notify_one();
|
||||
}
|
||||
|
||||
std::string ImuTimestampCsvLogger::serializeRow(const PendingRow &row) const {
|
||||
if (output_mode_ == OutputMode::ACCEL) {
|
||||
return serializeStreamColumns(row.accel);
|
||||
}
|
||||
if (output_mode_ == OutputMode::GYRO) {
|
||||
return serializeStreamColumns(row.gyro);
|
||||
}
|
||||
std::ostringstream ss;
|
||||
ss << serializeStreamColumns(row.accel) << "," << serializeStreamColumns(row.gyro);
|
||||
return ss.str();
|
||||
}
|
||||
|
||||
std::string ImuTimestampCsvLogger::serializeStreamColumns(const StreamState &state) {
|
||||
std::vector<std::string> fields(5, "");
|
||||
if (state.has_frame) {
|
||||
fields[0] = formatSecondsColumn(state.device_ts_us);
|
||||
fields[1] = formatSecondsColumn(state.global_ts_us);
|
||||
fields[2] = formatSecondsColumn(state.sdk_system_ts_us);
|
||||
fields[3] = formatSecondsColumn(state.arrival_system_us);
|
||||
fields[4] = formatOptionalSecondsColumn(state.publish_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 ImuTimestampCsvLogger::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 ImuTimestampCsvLogger::formatOptionalSecondsColumn(
|
||||
const std::optional<int64_t> &value) {
|
||||
if (!value.has_value()) {
|
||||
return "";
|
||||
}
|
||||
return formatSecondsColumn(*value);
|
||||
}
|
||||
|
||||
std::string ImuTimestampCsvLogger::csvHeader() const {
|
||||
std::ostringstream ss;
|
||||
const auto append_stream_header = [&ss](const char *prefix) {
|
||||
ss << prefix << "_device_ts_sec,";
|
||||
ss << prefix << "_global_ts_sec,";
|
||||
ss << prefix << "_system_ts_sec,";
|
||||
ss << prefix << "_arrival_system_ts_sec,";
|
||||
ss << prefix << "_publish_system_ts_sec";
|
||||
};
|
||||
if (output_mode_ == OutputMode::SYNCED || output_mode_ == OutputMode::ACCEL) {
|
||||
append_stream_header("accel");
|
||||
}
|
||||
if (output_mode_ == OutputMode::SYNCED) {
|
||||
ss << ",";
|
||||
}
|
||||
if (output_mode_ == OutputMode::SYNCED || output_mode_ == OutputMode::GYRO) {
|
||||
append_stream_header("gyro");
|
||||
}
|
||||
return ss.str();
|
||||
}
|
||||
|
||||
void ImuTimestampCsvLogger::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 IMU 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 ImuTimestampCsvLogger::csvFilePathForIndex(uint64_t file_index) const {
|
||||
const std::filesystem::path original_path(frame_csv_file_path_);
|
||||
std::string suffix;
|
||||
if (output_mode_ == OutputMode::SYNCED) {
|
||||
suffix = "_imu";
|
||||
} else if (output_mode_ == OutputMode::ACCEL) {
|
||||
suffix = "_accel";
|
||||
} else {
|
||||
suffix = "_gyro";
|
||||
}
|
||||
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 ImuTimestampCsvLogger::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 IMU timestamp CSV file: " << file_path);
|
||||
return false;
|
||||
}
|
||||
|
||||
csv_stream_ << csvHeader() << "\n";
|
||||
csv_stream_.flush();
|
||||
if (!csv_stream_) {
|
||||
RCLCPP_ERROR_STREAM(logger_, "Failed to write IMU timestamp CSV header: " << file_path);
|
||||
csv_stream_.close();
|
||||
return false;
|
||||
}
|
||||
|
||||
csv_file_index_ = file_index;
|
||||
csv_rows_written_ = 1;
|
||||
return true;
|
||||
}
|
||||
|
||||
bool ImuTimestampCsvLogger::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_, "IMU timestamp CSV reached " << kMaxCsvRowsPerFileIncludingHeader
|
||||
<< " rows; continuing in "
|
||||
<< csvFilePathForIndex(csv_file_index_));
|
||||
return true;
|
||||
}
|
||||
|
||||
} // namespace orbbec_camera
|
||||
@@ -736,10 +736,19 @@ OBCameraNode::OBCameraNode(rclcpp::Node *node, std::shared_ptr<ob::Device> devic
|
||||
setupTopics();
|
||||
|
||||
if (enable_frame_drop_log_ || !frame_timestamp_csv_file_.empty()) {
|
||||
frame_timestamp_csv_logger_ = std::make_unique<FrameTimestampCsvLogger>(
|
||||
enable_frame_drop_log_, frame_timestamp_csv_file_, logger_);
|
||||
if (!frame_timestamp_csv_logger_->enabled()) {
|
||||
frame_timestamp_csv_logger_.reset();
|
||||
TimestampCsvLogger::Config timestamp_config;
|
||||
timestamp_config.frame_drop_log_enabled = enable_frame_drop_log_;
|
||||
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.depth_enabled = enable_stream_[DEPTH];
|
||||
timestamp_config.imu_sync_enabled = enable_sync_output_accel_gyro_;
|
||||
timestamp_config.accel_enabled = enable_stream_[ACCEL];
|
||||
timestamp_config.gyro_enabled = enable_stream_[GYRO];
|
||||
timestamp_csv_logger_ =
|
||||
std::make_unique<TimestampCsvLogger>(std::move(timestamp_config), logger_);
|
||||
if (!timestamp_csv_logger_->enabled()) {
|
||||
timestamp_csv_logger_.reset();
|
||||
}
|
||||
}
|
||||
|
||||
@@ -809,15 +818,6 @@ void OBCameraNode::clean() noexcept {
|
||||
is_running_.store(false);
|
||||
is_camera_node_initialized_.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_) {
|
||||
@@ -877,6 +877,11 @@ void OBCameraNode::clean() noexcept {
|
||||
RCLCPP_DEBUG_STREAM(logger_, "Exception while stopping streams");
|
||||
}
|
||||
|
||||
if (timestamp_csv_logger_) {
|
||||
timestamp_csv_logger_->shutdown();
|
||||
timestamp_csv_logger_.reset();
|
||||
}
|
||||
|
||||
// Clean up d2c_viewer_ before cleaning buffers
|
||||
RCLCPP_DEBUG_STREAM(logger_, "Clean d2c_viewer");
|
||||
try {
|
||||
@@ -4200,8 +4205,14 @@ void OBCameraNode::startIMUSyncStream() {
|
||||
auto frameSet = frame->as<ob::FrameSet>();
|
||||
auto aFrame = frameSet->getFrame(OB_FRAME_ACCEL);
|
||||
auto gFrame = frameSet->getFrame(OB_FRAME_GYRO);
|
||||
const bool log_imu_timestamps = is_camera_node_initialized_.load() && rclcpp::ok() &&
|
||||
timestamp_csv_logger_ &&
|
||||
timestamp_csv_logger_->syncedImuEnabled();
|
||||
const auto arrival_system_us = log_imu_timestamps ? getSystemNowUs() : 0;
|
||||
if (aFrame && gFrame) {
|
||||
onNewIMUFrameSyncOutputCallback(aFrame, gFrame);
|
||||
onNewIMUFrameSyncOutputCallback(aFrame, gFrame, arrival_system_us);
|
||||
} else if (log_imu_timestamps && (aFrame || gFrame)) {
|
||||
timestamp_csv_logger_->recordSyncedImu(aFrame, gFrame, arrival_system_us, std::nullopt);
|
||||
}
|
||||
});
|
||||
|
||||
@@ -6334,7 +6345,7 @@ void OBCameraNode::onNewFrameSetCallback(std::shared_ptr<ob::FrameSet> frame_set
|
||||
if (frame_set == nullptr) {
|
||||
return;
|
||||
}
|
||||
if (frame_timestamp_csv_logger_ && frame_timestamp_csv_logger_->enabled()) {
|
||||
if (timestamp_csv_logger_ && timestamp_csv_logger_->imageEnabled()) {
|
||||
const auto frame_set_arrival_system_us = getSystemNowUs();
|
||||
const auto frame_set_arrival_steady_us = getSteadyNowUs();
|
||||
auto final_color_frame = frame_set->getFrame(OB_FRAME_COLOR);
|
||||
@@ -6344,7 +6355,7 @@ void OBCameraNode::onNewFrameSetCallback(std::shared_ptr<ob::FrameSet> frame_set
|
||||
const bool color_publish_expected = track_color;
|
||||
const bool depth_publish_expected = track_depth;
|
||||
|
||||
frame_timestamp_csv_logger_->recordFrameSet(
|
||||
timestamp_csv_logger_->recordImageFrameSet(
|
||||
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);
|
||||
@@ -6819,7 +6830,7 @@ bool OBCameraNode::decodeColorFrameToBuffer(const std::shared_ptr<ob::Frame> &fr
|
||||
target_buffer_size = &rgb_buffer_size_;
|
||||
}
|
||||
if (video_frame->getDataSize() > *target_buffer_size) {
|
||||
delete[](*target_buffer);
|
||||
delete[] (*target_buffer);
|
||||
*target_buffer_size = video_frame->getDataSize();
|
||||
*target_buffer = new uint8_t[*target_buffer_size];
|
||||
buffer = *target_buffer;
|
||||
@@ -6871,10 +6882,11 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame> &frame,
|
||||
if (frame == nullptr) {
|
||||
return;
|
||||
}
|
||||
const bool log_image_timestamps =
|
||||
timestamp_csv_logger_ && timestamp_csv_logger_->imageStreamEnabled(stream_index.first);
|
||||
const auto record_image_publish_skipped = [&]() {
|
||||
if (frame_timestamp_csv_logger_ && frame_timestamp_csv_logger_->enabled() &&
|
||||
(stream_index == COLOR || stream_index == DEPTH)) {
|
||||
frame_timestamp_csv_logger_->recordImagePublishSkipped(stream_index.first, frame);
|
||||
if (log_image_timestamps) {
|
||||
timestamp_csv_logger_->recordImagePublishSkipped(stream_index.first, frame);
|
||||
}
|
||||
};
|
||||
CHECK_NOTNULL(image_publishers_[stream_index]);
|
||||
@@ -6991,10 +7003,9 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame> &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 && frame_timestamp_csv_logger_ &&
|
||||
frame_timestamp_csv_logger_->enabled()) {
|
||||
frame_timestamp_csv_logger_->recordPreImagePublish(stream_index.first, frame,
|
||||
getSystemNowUs(), getSteadyNowUs());
|
||||
if (!has_raw_image_subscriber && stream_index == COLOR && 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) {
|
||||
@@ -7080,10 +7091,9 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame> &frame,
|
||||
record_image_publish_skipped();
|
||||
return;
|
||||
}
|
||||
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());
|
||||
if (log_image_timestamps) {
|
||||
timestamp_csv_logger_->recordImagePrePublish(stream_index.first, frame, getSystemNowUs(),
|
||||
getSteadyNowUs());
|
||||
}
|
||||
if (stream_index == COLOR) {
|
||||
fps_delay_status_color_->tick(frame_timestamp);
|
||||
@@ -7270,11 +7280,20 @@ void OBCameraNode::saveImageToFile(const stream_index_pair &stream_index, const
|
||||
}
|
||||
|
||||
void OBCameraNode::onNewIMUFrameSyncOutputCallback(const std::shared_ptr<ob::Frame> &accelframe,
|
||||
const std::shared_ptr<ob::Frame> &gryoframe) {
|
||||
const std::shared_ptr<ob::Frame> &gryoframe,
|
||||
int64_t arrival_system_us) {
|
||||
if (!is_camera_node_initialized_.load() || !rclcpp::ok()) {
|
||||
return;
|
||||
}
|
||||
const auto record_timestamps = [&](std::optional<int64_t> publish_system_us) {
|
||||
if (arrival_system_us != 0 && timestamp_csv_logger_ &&
|
||||
timestamp_csv_logger_->syncedImuEnabled()) {
|
||||
timestamp_csv_logger_->recordSyncedImu(accelframe, gryoframe, arrival_system_us,
|
||||
publish_system_us);
|
||||
}
|
||||
};
|
||||
if (!imu_gyro_accel_publisher_) {
|
||||
record_timestamps(std::nullopt);
|
||||
RCLCPP_ERROR_STREAM(logger_, "stream Accel Gryo publisher not initialized");
|
||||
return;
|
||||
}
|
||||
@@ -7282,6 +7301,7 @@ void OBCameraNode::onNewIMUFrameSyncOutputCallback(const std::shared_ptr<ob::Fra
|
||||
has_subscriber = has_subscriber || imu_info_publishers_[GYRO]->get_subscription_count() > 0;
|
||||
has_subscriber = has_subscriber || imu_info_publishers_[ACCEL]->get_subscription_count() > 0;
|
||||
if (!has_subscriber) {
|
||||
record_timestamps(std::nullopt);
|
||||
return;
|
||||
}
|
||||
auto imu_msg = sensor_msgs::msg::Imu();
|
||||
@@ -7315,7 +7335,9 @@ void OBCameraNode::onNewIMUFrameSyncOutputCallback(const std::shared_ptr<ob::Fra
|
||||
imu_msg.linear_acceleration.y = accelData.y - accel_info.bias[1];
|
||||
imu_msg.linear_acceleration.z = accelData.z - accel_info.bias[2];
|
||||
|
||||
const auto publish_system_us = getSystemNowUs();
|
||||
imu_gyro_accel_publisher_->publish(imu_msg);
|
||||
record_timestamps(publish_system_us);
|
||||
}
|
||||
|
||||
void OBCameraNode::onNewIMUFrameCallback(const std::shared_ptr<ob::Frame> &frame,
|
||||
@@ -7323,7 +7345,17 @@ void OBCameraNode::onNewIMUFrameCallback(const std::shared_ptr<ob::Frame> &frame
|
||||
if (!is_camera_node_initialized_.load() || !rclcpp::ok()) {
|
||||
return;
|
||||
}
|
||||
const bool log_imu_timestamps =
|
||||
timestamp_csv_logger_ && timestamp_csv_logger_->standaloneImuEnabled(stream_index.first);
|
||||
const auto arrival_system_us = log_imu_timestamps ? getSystemNowUs() : 0;
|
||||
const auto record_timestamps = [&](std::optional<int64_t> publish_system_us) {
|
||||
if (log_imu_timestamps) {
|
||||
timestamp_csv_logger_->recordStandaloneImu(stream_index.first, frame, arrival_system_us,
|
||||
publish_system_us);
|
||||
}
|
||||
};
|
||||
if (!imu_publishers_.count(stream_index)) {
|
||||
record_timestamps(std::nullopt);
|
||||
RCLCPP_ERROR_STREAM(logger_,
|
||||
"stream " << stream_name_[stream_index] << " publisher not initialized");
|
||||
return;
|
||||
@@ -7332,6 +7364,7 @@ void OBCameraNode::onNewIMUFrameCallback(const std::shared_ptr<ob::Frame> &frame
|
||||
has_subscriber =
|
||||
has_subscriber || imu_info_publishers_[stream_index]->get_subscription_count() > 0;
|
||||
if (!has_subscriber) {
|
||||
record_timestamps(std::nullopt);
|
||||
return;
|
||||
}
|
||||
auto imu_msg = sensor_msgs::msg::Imu();
|
||||
@@ -7358,10 +7391,13 @@ void OBCameraNode::onNewIMUFrameCallback(const std::shared_ptr<ob::Frame> &frame
|
||||
imu_msg.linear_acceleration.y = data.y - imu_info.bias[1];
|
||||
imu_msg.linear_acceleration.z = data.z - imu_info.bias[2];
|
||||
} else {
|
||||
record_timestamps(std::nullopt);
|
||||
RCLCPP_ERROR(logger_, "Unsupported IMU frame type");
|
||||
return;
|
||||
}
|
||||
const auto publish_system_us = getSystemNowUs();
|
||||
imu_publishers_[stream_index]->publish(imu_msg);
|
||||
record_timestamps(publish_system_us);
|
||||
}
|
||||
|
||||
void OBCameraNode::setDefaultIMUMessage(sensor_msgs::msg::Imu &imu_msg) {
|
||||
@@ -8456,9 +8492,9 @@ void OBCameraNode::setFilterCallback(const std::shared_ptr<SetFilter ::Request>
|
||||
if (request->filter_param.size() > 1) {
|
||||
temporal_filter->setDiffScale(request->filter_param[0]);
|
||||
temporal_filter->setWeight(request->filter_param[1]);
|
||||
RCLCPP_INFO_STREAM(logger_, "Set TemporalFilter params: "
|
||||
<< "\ndiff_scale:" << request->filter_param[0]
|
||||
<< "\nweight:" << request->filter_param[1]);
|
||||
RCLCPP_INFO_STREAM(
|
||||
logger_, "Set TemporalFilter params: " << "\ndiff_scale:" << request->filter_param[0]
|
||||
<< "\nweight:" << request->filter_param[1]);
|
||||
temporal_filter_diff_threshold_ = request->filter_param[0];
|
||||
temporal_filter_weight_ = request->filter_param[1];
|
||||
} else {
|
||||
|
||||
@@ -0,0 +1,194 @@
|
||||
#include "orbbec_camera/timestamp_csv_logger.h"
|
||||
|
||||
#include "orbbec_camera/frame_timestamp_csv_logger.h"
|
||||
#include "orbbec_camera/imu_timestamp_csv_logger.h"
|
||||
|
||||
#include <exception>
|
||||
#include <utility>
|
||||
|
||||
namespace orbbec_camera {
|
||||
|
||||
TimestampCsvLogger::TimestampCsvLogger(Config config, rclcpp::Logger logger)
|
||||
: logger_(std::move(logger)) {
|
||||
const auto create_image_logger = [this, &config](FrameTimestampCsvLogger::OutputMode mode) {
|
||||
auto timestamp_logger = std::make_unique<FrameTimestampCsvLogger>(
|
||||
config.frame_drop_log_enabled, config.csv_file_path, mode, logger_);
|
||||
if (!timestamp_logger->enabled()) {
|
||||
timestamp_logger.reset();
|
||||
}
|
||||
return timestamp_logger;
|
||||
};
|
||||
|
||||
if (config.frame_sync_enabled && (config.color_enabled || config.depth_enabled)) {
|
||||
synced_image_logger_ = create_image_logger(FrameTimestampCsvLogger::OutputMode::SYNCED);
|
||||
} else {
|
||||
if (config.color_enabled) {
|
||||
color_logger_ = create_image_logger(FrameTimestampCsvLogger::OutputMode::COLOR);
|
||||
}
|
||||
if (config.depth_enabled) {
|
||||
depth_logger_ = create_image_logger(FrameTimestampCsvLogger::OutputMode::DEPTH);
|
||||
}
|
||||
}
|
||||
|
||||
if (config.csv_file_path.empty()) {
|
||||
return;
|
||||
}
|
||||
|
||||
const auto create_imu_logger = [this, &config](ImuTimestampCsvLogger::OutputMode mode) {
|
||||
auto timestamp_logger =
|
||||
std::make_unique<ImuTimestampCsvLogger>(config.csv_file_path, mode, logger_);
|
||||
if (!timestamp_logger->enabled()) {
|
||||
timestamp_logger.reset();
|
||||
}
|
||||
return timestamp_logger;
|
||||
};
|
||||
|
||||
if (config.imu_sync_enabled) {
|
||||
synced_imu_logger_ = create_imu_logger(ImuTimestampCsvLogger::OutputMode::SYNCED);
|
||||
} else {
|
||||
if (config.accel_enabled) {
|
||||
accel_logger_ = create_imu_logger(ImuTimestampCsvLogger::OutputMode::ACCEL);
|
||||
}
|
||||
if (config.gyro_enabled) {
|
||||
gyro_logger_ = create_imu_logger(ImuTimestampCsvLogger::OutputMode::GYRO);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
TimestampCsvLogger::~TimestampCsvLogger() noexcept { shutdown(); }
|
||||
|
||||
bool TimestampCsvLogger::enabled() const {
|
||||
return imageEnabled() || syncedImuEnabled() || standaloneImuEnabled(OB_STREAM_ACCEL) ||
|
||||
standaloneImuEnabled(OB_STREAM_GYRO);
|
||||
}
|
||||
|
||||
bool TimestampCsvLogger::imageEnabled() const {
|
||||
return (synced_image_logger_ && synced_image_logger_->enabled()) ||
|
||||
(color_logger_ && color_logger_->enabled()) || (depth_logger_ && depth_logger_->enabled());
|
||||
}
|
||||
|
||||
bool TimestampCsvLogger::imageStreamEnabled(OBStreamType stream_type) const {
|
||||
const auto *timestamp_logger = imageLoggerForStream(stream_type);
|
||||
return timestamp_logger && timestamp_logger->enabled();
|
||||
}
|
||||
|
||||
bool TimestampCsvLogger::syncedImuEnabled() const {
|
||||
return synced_imu_logger_ && synced_imu_logger_->enabled();
|
||||
}
|
||||
|
||||
bool TimestampCsvLogger::standaloneImuEnabled(OBStreamType stream_type) const {
|
||||
const auto *timestamp_logger = standaloneImuLoggerForStream(stream_type);
|
||||
return timestamp_logger && timestamp_logger->enabled();
|
||||
}
|
||||
|
||||
void TimestampCsvLogger::recordImageFrameSet(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 (synced_image_logger_) {
|
||||
synced_image_logger_->recordFrameSet(
|
||||
color_frame, depth_frame, arrival_system_us, arrival_steady_us, track_color, track_depth,
|
||||
color_image_publish_expected, depth_image_publish_expected);
|
||||
return;
|
||||
}
|
||||
|
||||
if (track_color && color_logger_) {
|
||||
color_logger_->recordStandaloneFrameArrival(OB_STREAM_COLOR, color_frame, arrival_system_us,
|
||||
arrival_steady_us, color_image_publish_expected);
|
||||
}
|
||||
if (track_depth && depth_logger_) {
|
||||
depth_logger_->recordStandaloneFrameArrival(OB_STREAM_DEPTH, depth_frame, arrival_system_us,
|
||||
arrival_steady_us, depth_image_publish_expected);
|
||||
}
|
||||
}
|
||||
|
||||
void TimestampCsvLogger::recordImagePrePublish(OBStreamType stream_type,
|
||||
const std::shared_ptr<ob::Frame> &frame,
|
||||
int64_t publish_system_us,
|
||||
int64_t publish_steady_us) {
|
||||
auto *timestamp_logger = imageLoggerForStream(stream_type);
|
||||
if (timestamp_logger) {
|
||||
timestamp_logger->recordPreImagePublish(stream_type, frame, publish_system_us,
|
||||
publish_steady_us);
|
||||
}
|
||||
}
|
||||
|
||||
void TimestampCsvLogger::recordImagePublishSkipped(OBStreamType stream_type,
|
||||
const std::shared_ptr<ob::Frame> &frame) {
|
||||
auto *timestamp_logger = imageLoggerForStream(stream_type);
|
||||
if (timestamp_logger) {
|
||||
timestamp_logger->recordImagePublishSkipped(stream_type, frame);
|
||||
}
|
||||
}
|
||||
|
||||
void TimestampCsvLogger::recordSyncedImu(const std::shared_ptr<ob::Frame> &accel_frame,
|
||||
const std::shared_ptr<ob::Frame> &gyro_frame,
|
||||
int64_t arrival_system_us,
|
||||
std::optional<int64_t> publish_system_us) {
|
||||
if (synced_imu_logger_) {
|
||||
synced_imu_logger_->recordFrameSet(accel_frame, gyro_frame, arrival_system_us,
|
||||
publish_system_us);
|
||||
}
|
||||
}
|
||||
|
||||
void TimestampCsvLogger::recordStandaloneImu(OBStreamType stream_type,
|
||||
const std::shared_ptr<ob::Frame> &frame,
|
||||
int64_t arrival_system_us,
|
||||
std::optional<int64_t> publish_system_us) {
|
||||
auto *timestamp_logger = standaloneImuLoggerForStream(stream_type);
|
||||
if (timestamp_logger) {
|
||||
timestamp_logger->recordStandaloneFrame(stream_type, frame, arrival_system_us,
|
||||
publish_system_us);
|
||||
}
|
||||
}
|
||||
|
||||
void TimestampCsvLogger::shutdown() noexcept {
|
||||
if (shutdown_requested_.exchange(true)) {
|
||||
return;
|
||||
}
|
||||
|
||||
const auto shutdown_logger = [this](auto ×tamp_logger, const char *name) {
|
||||
if (!timestamp_logger) {
|
||||
return;
|
||||
}
|
||||
try {
|
||||
timestamp_logger->shutdown();
|
||||
} catch (const std::exception &e) {
|
||||
RCLCPP_WARN_STREAM(logger_, "Exception while shutting down " << name << ": " << e.what());
|
||||
} catch (...) {
|
||||
RCLCPP_WARN_STREAM(logger_, "Unknown exception while shutting down " << name);
|
||||
}
|
||||
};
|
||||
|
||||
shutdown_logger(synced_image_logger_, "synced image timestamp CSV logger");
|
||||
shutdown_logger(color_logger_, "color timestamp CSV logger");
|
||||
shutdown_logger(depth_logger_, "depth 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");
|
||||
}
|
||||
|
||||
FrameTimestampCsvLogger *TimestampCsvLogger::imageLoggerForStream(OBStreamType stream_type) const {
|
||||
if (stream_type == OB_STREAM_COLOR) {
|
||||
return synced_image_logger_ ? synced_image_logger_.get() : color_logger_.get();
|
||||
}
|
||||
if (stream_type == OB_STREAM_DEPTH) {
|
||||
return synced_image_logger_ ? synced_image_logger_.get() : depth_logger_.get();
|
||||
}
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
ImuTimestampCsvLogger *TimestampCsvLogger::standaloneImuLoggerForStream(
|
||||
OBStreamType stream_type) const {
|
||||
if (stream_type == OB_STREAM_ACCEL) {
|
||||
return accel_logger_.get();
|
||||
}
|
||||
if (stream_type == OB_STREAM_GYRO) {
|
||||
return gyro_logger_.get();
|
||||
}
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
} // namespace orbbec_camera
|
||||
Reference in New Issue
Block a user