mirror of
https://github.com/orbbec/OrbbecSDK_ROS2.git
synced 2026-10-05 04:27:46 +08:00
refactor: centralize timestamp CSV logger management
This commit is contained in:
slz
@@ -159,6 +159,7 @@ set(SOURCE_FILES
|
||||
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
|
||||
|
||||
@@ -70,8 +70,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/imu_timestamp_csv_logger.h"
|
||||
#include "orbbec_camera/timestamp_csv_logger.h"
|
||||
#include "jpeg_decoder.h"
|
||||
#include <std_msgs/msg/header.hpp>
|
||||
#include <fcntl.h>
|
||||
@@ -1023,12 +1022,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<FrameTimestampCsvLogger> color_timestamp_csv_logger_;
|
||||
std::unique_ptr<FrameTimestampCsvLogger> depth_timestamp_csv_logger_;
|
||||
std::unique_ptr<ImuTimestampCsvLogger> imu_timestamp_csv_logger_;
|
||||
std::unique_ptr<ImuTimestampCsvLogger> accel_timestamp_csv_logger_;
|
||||
std::unique_ptr<ImuTimestampCsvLogger> gyro_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
|
||||
@@ -738,54 +738,19 @@ OBCameraNode::OBCameraNode(rclcpp::Node *node, std::shared_ptr<ob::Device> devic
|
||||
setupTopics();
|
||||
|
||||
if (enable_frame_drop_log_ || !frame_timestamp_csv_file_.empty()) {
|
||||
if (enable_frame_sync_ && (enable_stream_[COLOR] || enable_stream_[DEPTH])) {
|
||||
frame_timestamp_csv_logger_ = std::make_unique<FrameTimestampCsvLogger>(
|
||||
enable_frame_drop_log_, frame_timestamp_csv_file_,
|
||||
FrameTimestampCsvLogger::OutputMode::SYNCED, logger_);
|
||||
if (!frame_timestamp_csv_logger_->enabled()) {
|
||||
frame_timestamp_csv_logger_.reset();
|
||||
}
|
||||
} else {
|
||||
if (enable_stream_[COLOR]) {
|
||||
color_timestamp_csv_logger_ = std::make_unique<FrameTimestampCsvLogger>(
|
||||
enable_frame_drop_log_, frame_timestamp_csv_file_,
|
||||
FrameTimestampCsvLogger::OutputMode::COLOR, logger_);
|
||||
if (!color_timestamp_csv_logger_->enabled()) {
|
||||
color_timestamp_csv_logger_.reset();
|
||||
}
|
||||
}
|
||||
if (enable_stream_[DEPTH]) {
|
||||
depth_timestamp_csv_logger_ = std::make_unique<FrameTimestampCsvLogger>(
|
||||
enable_frame_drop_log_, frame_timestamp_csv_file_,
|
||||
FrameTimestampCsvLogger::OutputMode::DEPTH, logger_);
|
||||
if (!depth_timestamp_csv_logger_->enabled()) {
|
||||
depth_timestamp_csv_logger_.reset();
|
||||
}
|
||||
}
|
||||
}
|
||||
if (!frame_timestamp_csv_file_.empty() && (enable_stream_[ACCEL] || enable_stream_[GYRO])) {
|
||||
if (enable_sync_output_accel_gyro_) {
|
||||
imu_timestamp_csv_logger_ = std::make_unique<ImuTimestampCsvLogger>(
|
||||
frame_timestamp_csv_file_, ImuTimestampCsvLogger::OutputMode::SYNCED, logger_);
|
||||
if (!imu_timestamp_csv_logger_->enabled()) {
|
||||
imu_timestamp_csv_logger_.reset();
|
||||
}
|
||||
} else {
|
||||
if (enable_stream_[ACCEL]) {
|
||||
accel_timestamp_csv_logger_ = std::make_unique<ImuTimestampCsvLogger>(
|
||||
frame_timestamp_csv_file_, ImuTimestampCsvLogger::OutputMode::ACCEL, logger_);
|
||||
if (!accel_timestamp_csv_logger_->enabled()) {
|
||||
accel_timestamp_csv_logger_.reset();
|
||||
}
|
||||
}
|
||||
if (enable_stream_[GYRO]) {
|
||||
gyro_timestamp_csv_logger_ = std::make_unique<ImuTimestampCsvLogger>(
|
||||
frame_timestamp_csv_file_, ImuTimestampCsvLogger::OutputMode::GYRO, logger_);
|
||||
if (!gyro_timestamp_csv_logger_->enabled()) {
|
||||
gyro_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();
|
||||
}
|
||||
}
|
||||
|
||||
@@ -855,53 +820,9 @@ 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");
|
||||
}
|
||||
try {
|
||||
if (color_timestamp_csv_logger_) {
|
||||
color_timestamp_csv_logger_->shutdown();
|
||||
color_timestamp_csv_logger_.reset();
|
||||
}
|
||||
} catch (...) {
|
||||
RCLCPP_WARN_STREAM(logger_, "Exception while shutting down color timestamp CSV logger");
|
||||
}
|
||||
try {
|
||||
if (depth_timestamp_csv_logger_) {
|
||||
depth_timestamp_csv_logger_->shutdown();
|
||||
depth_timestamp_csv_logger_.reset();
|
||||
}
|
||||
} catch (...) {
|
||||
RCLCPP_WARN_STREAM(logger_, "Exception while shutting down depth timestamp CSV logger");
|
||||
}
|
||||
try {
|
||||
if (imu_timestamp_csv_logger_) {
|
||||
imu_timestamp_csv_logger_->shutdown();
|
||||
imu_timestamp_csv_logger_.reset();
|
||||
}
|
||||
} catch (...) {
|
||||
RCLCPP_WARN_STREAM(logger_, "Exception while shutting down IMU timestamp CSV logger");
|
||||
}
|
||||
try {
|
||||
if (accel_timestamp_csv_logger_) {
|
||||
accel_timestamp_csv_logger_->shutdown();
|
||||
accel_timestamp_csv_logger_.reset();
|
||||
}
|
||||
} catch (...) {
|
||||
RCLCPP_WARN_STREAM(logger_, "Exception while shutting down accel timestamp CSV logger");
|
||||
}
|
||||
try {
|
||||
if (gyro_timestamp_csv_logger_) {
|
||||
gyro_timestamp_csv_logger_->shutdown();
|
||||
gyro_timestamp_csv_logger_.reset();
|
||||
}
|
||||
} catch (...) {
|
||||
RCLCPP_WARN_STREAM(logger_, "Exception while shutting down gyro timestamp CSV logger");
|
||||
if (timestamp_csv_logger_) {
|
||||
timestamp_csv_logger_->shutdown();
|
||||
timestamp_csv_logger_.reset();
|
||||
}
|
||||
|
||||
// Stop diagnostic timer and updater first BEFORE acquiring device_lock to prevent deadlock
|
||||
@@ -4214,13 +4135,13 @@ void OBCameraNode::startIMUSyncStream() {
|
||||
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() &&
|
||||
imu_timestamp_csv_logger_ &&
|
||||
imu_timestamp_csv_logger_->enabled();
|
||||
timestamp_csv_logger_ &&
|
||||
timestamp_csv_logger_->syncedImuEnabled();
|
||||
const auto arrival_system_us = log_imu_timestamps ? getSystemNowUs() : 0;
|
||||
if (aFrame && gFrame) {
|
||||
onNewIMUFrameSyncOutputCallback(aFrame, gFrame, arrival_system_us);
|
||||
} else if (log_imu_timestamps && (aFrame || gFrame)) {
|
||||
imu_timestamp_csv_logger_->recordFrameSet(aFrame, gFrame, arrival_system_us, std::nullopt);
|
||||
timestamp_csv_logger_->recordSyncedImu(aFrame, gFrame, arrival_system_us, std::nullopt);
|
||||
}
|
||||
});
|
||||
|
||||
@@ -6192,9 +6113,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()) ||
|
||||
(color_timestamp_csv_logger_ && color_timestamp_csv_logger_->enabled()) ||
|
||||
(depth_timestamp_csv_logger_ && depth_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);
|
||||
@@ -6204,23 +6123,10 @@ void OBCameraNode::onNewFrameSetCallback(std::shared_ptr<ob::FrameSet> frame_set
|
||||
const bool color_publish_expected = track_color;
|
||||
const bool depth_publish_expected = track_depth;
|
||||
|
||||
if (frame_timestamp_csv_logger_) {
|
||||
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);
|
||||
} else {
|
||||
if (track_color && color_timestamp_csv_logger_) {
|
||||
color_timestamp_csv_logger_->recordStandaloneFrameArrival(
|
||||
OB_STREAM_COLOR, final_color_frame, frame_set_arrival_system_us,
|
||||
frame_set_arrival_steady_us, color_publish_expected);
|
||||
}
|
||||
if (track_depth && depth_timestamp_csv_logger_) {
|
||||
depth_timestamp_csv_logger_->recordStandaloneFrameArrival(
|
||||
OB_STREAM_DEPTH, final_depth_frame, frame_set_arrival_system_us,
|
||||
frame_set_arrival_steady_us, depth_publish_expected);
|
||||
}
|
||||
}
|
||||
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);
|
||||
}
|
||||
|
||||
try {
|
||||
@@ -6714,17 +6620,11 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame> &frame,
|
||||
if (frame == nullptr) {
|
||||
return;
|
||||
}
|
||||
FrameTimestampCsvLogger *timestamp_csv_logger = nullptr;
|
||||
if (stream_index == COLOR) {
|
||||
timestamp_csv_logger = frame_timestamp_csv_logger_ ? frame_timestamp_csv_logger_.get()
|
||||
: color_timestamp_csv_logger_.get();
|
||||
} else if (stream_index == DEPTH) {
|
||||
timestamp_csv_logger = frame_timestamp_csv_logger_ ? frame_timestamp_csv_logger_.get()
|
||||
: depth_timestamp_csv_logger_.get();
|
||||
}
|
||||
const bool log_image_timestamps =
|
||||
timestamp_csv_logger_ && timestamp_csv_logger_->imageStreamEnabled(stream_index.first);
|
||||
const auto record_image_publish_skipped = [&]() {
|
||||
if (timestamp_csv_logger && timestamp_csv_logger->enabled()) {
|
||||
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]);
|
||||
@@ -6841,10 +6741,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 && timestamp_csv_logger &&
|
||||
timestamp_csv_logger->enabled()) {
|
||||
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) {
|
||||
@@ -6916,9 +6815,9 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame> &frame,
|
||||
record_image_publish_skipped();
|
||||
return;
|
||||
}
|
||||
if (timestamp_csv_logger && timestamp_csv_logger->enabled()) {
|
||||
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);
|
||||
@@ -7039,10 +6938,10 @@ void OBCameraNode::onNewIMUFrameSyncOutputCallback(const std::shared_ptr<ob::Fra
|
||||
return;
|
||||
}
|
||||
const auto record_timestamps = [&](std::optional<int64_t> publish_system_us) {
|
||||
if (arrival_system_us != 0 && imu_timestamp_csv_logger_ &&
|
||||
imu_timestamp_csv_logger_->enabled()) {
|
||||
imu_timestamp_csv_logger_->recordFrameSet(accelframe, gryoframe, arrival_system_us,
|
||||
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_) {
|
||||
@@ -7098,14 +6997,13 @@ void OBCameraNode::onNewIMUFrameCallback(const std::shared_ptr<ob::Frame> &frame
|
||||
if (!is_camera_node_initialized_.load() || !rclcpp::ok()) {
|
||||
return;
|
||||
}
|
||||
auto *timestamp_csv_logger =
|
||||
stream_index == ACCEL ? accel_timestamp_csv_logger_.get() : gyro_timestamp_csv_logger_.get();
|
||||
const bool log_imu_timestamps = timestamp_csv_logger && timestamp_csv_logger->enabled();
|
||||
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->recordStandaloneFrame(stream_index.first, frame, arrival_system_us,
|
||||
publish_system_us);
|
||||
timestamp_csv_logger_->recordStandaloneImu(stream_index.first, frame, arrival_system_us,
|
||||
publish_system_us);
|
||||
}
|
||||
};
|
||||
if (!imu_publishers_.count(stream_index)) {
|
||||
|
||||
@@ -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() || (!config.accel_enabled && !config.gyro_enabled)) {
|
||||
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