i/OrbbecSDK_ROS2
1
0
Fork 0
mirror of https://github.com/orbbec/OrbbecSDK_ROS2.git synced 2026-09-12 11:10:19 +08:00
Code Issues Packages Projects Releases Wiki Activity

feat: rename frame timestamp CSV logging to frame drop logging and update related parameters

Browse Source
This commit is contained in:
slz
2026-06-05 14:07:23 +08:00
parent a43b358fea
commit 951e48e715
25 changed files with 97 additions and 105 deletions
Download Patch File Download Diff File Expand all files Collapse all files
.gitignore
+3
View File
@@ -769,3 +769,6 @@ FodyWeavers.xsd
# Additional files built by Visual Studio
# End of https://www.toptal.com/developers/gitignore/api/visualstudio,visualstudiocode,c++,python,jetbrains+all,ros,ros2
# Local agent instructions
AGENTS.md
orbbec_camera/include/orbbec_camera/frame_timestamp_csv_logger.h
+5 -4
View File
@@ -20,7 +20,8 @@ namespace orbbec_camera {
class FrameTimestampCsvLogger {
public:
FrameTimestampCsvLogger(bool enabled, const std::string &csv_file_path, rclcpp::Logger logger);
FrameTimestampCsvLogger(bool drop_log_enabled, const std::string &csv_file_path,
rclcpp::Logger logger);
~FrameTimestampCsvLogger() noexcept;
@@ -120,7 +121,6 @@ class FrameTimestampCsvLogger {
int64_t arrival_steady_us, bool publish_expected);
void populatePublishData(StreamState &state, TrackedStream stream, int64_t publish_system_us,
int64_t publish_steady_us);
void reportDropLogFormatOnce();
std::optional<int64_t> updateDelta(std::optional<int64_t> &previous, int64_t current);
@@ -141,10 +141,11 @@ class FrameTimestampCsvLogger {
rclcpp::Logger logger_;
bool enabled_ = false;
bool csv_enabled_ = false;
bool drop_log_enabled_ = false;
std::atomic_bool shutdown_requested_{false};
bool writer_failed_ = false;
bool csv_writer_failed_ = false;
bool queue_warning_active_ = false;
bool drop_log_format_reported_ = false;
std::string csv_file_path_;
std::ofstream csv_stream_;
std::thread writer_thread_;
orbbec_camera/include/orbbec_camera/ob_camera_node.h
+1 -1
View File
@@ -913,7 +913,7 @@ class OBCameraNode {
int min_depth_limit_ = 0;
int max_depth_limit_ = 0;
std::string time_domain_ = "global"; // device, system, global
bool enable_frame_timestamp_csv_ = false;
bool enable_frame_drop_log_ = false;
std::string frame_timestamp_csv_file_;
std::unique_ptr<FrameTimestampCsvLogger> frame_timestamp_csv_logger_;
std::string exposure_range_mode_;
orbbec_camera/launch/astra.launch.py
+1 -1
View File
@@ -69,7 +69,7 @@ def generate_launch_description():
DeclareLaunchArgument('ordered_pc', default_value='false'),
DeclareLaunchArgument('use_hardware_time', default_value='false'),
DeclareLaunchArgument('timestamp_clock_type', default_value=''), # realtime or monotonic, default is realtime.
DeclareLaunchArgument('enable_frame_timestamp_csv', default_value='false'),
DeclareLaunchArgument('enable_frame_drop_log', default_value='false'),
DeclareLaunchArgument('frame_timestamp_csv_file', default_value=''),
DeclareLaunchArgument('enable_color_undistortion', default_value='false'),
DeclareLaunchArgument('enable_depth_undistortion', default_value='false'),
orbbec_camera/launch/astra2.launch.py
+1 -1
View File
@@ -106,7 +106,7 @@ def generate_launch_description():
DeclareLaunchArgument("enable_firmware_log", default_value="false"),
DeclareLaunchArgument("time_domain", default_value="global"),
DeclareLaunchArgument("timestamp_clock_type", default_value=""), # realtime or monotonic, default is realtime.
DeclareLaunchArgument('enable_frame_timestamp_csv', default_value='false'),
DeclareLaunchArgument('enable_frame_drop_log', default_value='false'),
DeclareLaunchArgument('frame_timestamp_csv_file', default_value=''),
DeclareLaunchArgument('enable_color_undistortion', default_value='false'),
DeclareLaunchArgument('enable_depth_undistortion', default_value='false'),
orbbec_camera/launch/dabai_a.launch.py
+1 -1
View File
@@ -208,7 +208,7 @@ def generate_launch_description():
DeclareLaunchArgument('time_sync_period', default_value='6.0'), # seconds
DeclareLaunchArgument('time_domain', default_value='global'),# global, device, system
DeclareLaunchArgument('timestamp_clock_type', default_value=''),# realtime or monotonic, default is realtime.
DeclareLaunchArgument('enable_frame_timestamp_csv', default_value='false'),
DeclareLaunchArgument('enable_frame_drop_log', default_value='false'),
DeclareLaunchArgument('frame_timestamp_csv_file', default_value=''),
DeclareLaunchArgument('enable_color_undistortion', default_value='false'),
DeclareLaunchArgument('enable_depth_undistortion', default_value='false'),
orbbec_camera/launch/dabai_al.launch.py
+1 -1
View File
@@ -210,7 +210,7 @@ def generate_launch_description():
DeclareLaunchArgument('time_sync_period', default_value='6.0'), # seconds
DeclareLaunchArgument('time_domain', default_value='global'),# global, device, system
DeclareLaunchArgument('timestamp_clock_type', default_value=''),# realtime or monotonic, default is realtime.
DeclareLaunchArgument('enable_frame_timestamp_csv', default_value='false'),
DeclareLaunchArgument('enable_frame_drop_log', default_value='false'),
DeclareLaunchArgument('frame_timestamp_csv_file', default_value=''),
DeclareLaunchArgument('enable_color_undistortion', default_value='false'),
DeclareLaunchArgument('enable_depth_undistortion', default_value='false'),
orbbec_camera/launch/dabai_dcw2.launch.py
+1 -1
View File
@@ -83,7 +83,7 @@ def generate_launch_description():
DeclareLaunchArgument('ordered_pc', default_value='false'),
DeclareLaunchArgument('use_hardware_time', default_value='false'),
DeclareLaunchArgument('timestamp_clock_type', default_value=''), # realtime or monotonic, default is realtime.
DeclareLaunchArgument('enable_frame_timestamp_csv', default_value='false'),
DeclareLaunchArgument('enable_frame_drop_log', default_value='false'),
DeclareLaunchArgument('frame_timestamp_csv_file', default_value=''),
DeclareLaunchArgument('enable_color_undistortion', default_value='false'),
DeclareLaunchArgument('enable_depth_undistortion', default_value='false'),
orbbec_camera/launch/dabai_max_pro.launch.py
+1 -1
View File
@@ -77,7 +77,7 @@ def generate_launch_description():
DeclareLaunchArgument('enable_lut_noise_removal_filter', default_value='false'),
DeclareLaunchArgument('use_hardware_time', default_value='false'),
DeclareLaunchArgument('timestamp_clock_type', default_value=''), # realtime or monotonic, default is realtime.
DeclareLaunchArgument('enable_frame_timestamp_csv', default_value='false'),
DeclareLaunchArgument('enable_frame_drop_log', default_value='false'),
DeclareLaunchArgument('frame_timestamp_csv_file', default_value=''),
DeclareLaunchArgument('enable_color_undistortion', default_value='false'),
DeclareLaunchArgument('enable_depth_undistortion', default_value='false'),
orbbec_camera/launch/femto.launch.py
+1 -1
View File
@@ -84,7 +84,7 @@ def generate_launch_description():
DeclareLaunchArgument("enable_firmware_log", default_value="false"),
DeclareLaunchArgument("time_domain", default_value="device"),
DeclareLaunchArgument("timestamp_clock_type", default_value=""), # realtime or monotonic, default is realtime.
DeclareLaunchArgument('enable_frame_timestamp_csv', default_value='false'),
DeclareLaunchArgument('enable_frame_drop_log', default_value='false'),
DeclareLaunchArgument('frame_timestamp_csv_file', default_value=''),
DeclareLaunchArgument('enable_color_undistortion', default_value='false'),
DeclareLaunchArgument('enable_depth_undistortion', default_value='false'),
orbbec_camera/launch/femto_bolt.launch.py
+1 -1
View File
@@ -112,7 +112,7 @@ def generate_launch_description():
DeclareLaunchArgument("enable_firmware_log", default_value="false"),
DeclareLaunchArgument("time_domain", default_value="global"),
DeclareLaunchArgument("timestamp_clock_type", default_value=""), # realtime or monotonic, default is realtime.
DeclareLaunchArgument('enable_frame_timestamp_csv', default_value='false'),
DeclareLaunchArgument('enable_frame_drop_log', default_value='false'),
DeclareLaunchArgument('frame_timestamp_csv_file', default_value=''),
DeclareLaunchArgument('enable_color_undistortion', default_value='false'),
DeclareLaunchArgument('enable_depth_undistortion', default_value='false'),
orbbec_camera/launch/femto_mega.launch.py
+1 -1
View File
@@ -118,7 +118,7 @@ def generate_launch_description():
DeclareLaunchArgument("enable_firmware_log", default_value="false"),
DeclareLaunchArgument("time_domain", default_value="global"),
DeclareLaunchArgument("timestamp_clock_type", default_value=""), # realtime or monotonic, default is realtime.
DeclareLaunchArgument('enable_frame_timestamp_csv', default_value='false'),
DeclareLaunchArgument('enable_frame_drop_log', default_value='false'),
DeclareLaunchArgument('frame_timestamp_csv_file', default_value=''),
DeclareLaunchArgument('enable_color_undistortion', default_value='false'),
DeclareLaunchArgument('enable_depth_undistortion', default_value='false'),
orbbec_camera/launch/gemini2.launch.py
+1 -1
View File
@@ -134,7 +134,7 @@ def generate_launch_description():
DeclareLaunchArgument("enable_firmware_log", default_value="false"),
DeclareLaunchArgument("time_domain", default_value="global"),
DeclareLaunchArgument("timestamp_clock_type", default_value=""), # realtime or monotonic, default is realtime.
DeclareLaunchArgument('enable_frame_timestamp_csv', default_value='false'),
DeclareLaunchArgument('enable_frame_drop_log', default_value='false'),
DeclareLaunchArgument('frame_timestamp_csv_file', default_value=''),
DeclareLaunchArgument('enable_color_undistortion', default_value='false'),
DeclareLaunchArgument('enable_depth_undistortion', default_value='false'),
orbbec_camera/launch/gemini210.launch.py
+1 -1
View File
@@ -132,7 +132,7 @@ def generate_launch_description():
DeclareLaunchArgument("enable_firmware_log", default_value="false"),
DeclareLaunchArgument("time_domain", default_value="global"),
DeclareLaunchArgument("timestamp_clock_type", default_value=""), # realtime or monotonic, default is realtime.
DeclareLaunchArgument('enable_frame_timestamp_csv', default_value='false'),
DeclareLaunchArgument('enable_frame_drop_log', default_value='false'),
DeclareLaunchArgument('frame_timestamp_csv_file', default_value=''),
DeclareLaunchArgument('enable_color_undistortion', default_value='false'),
DeclareLaunchArgument('enable_depth_undistortion', default_value='false'),
orbbec_camera/launch/gemini2L.launch.py
+1 -1
View File
@@ -185,7 +185,7 @@ def generate_launch_description():
DeclareLaunchArgument("enable_firmware_log", default_value="false"),
DeclareLaunchArgument("time_domain", default_value="global"),
DeclareLaunchArgument("timestamp_clock_type", default_value=""), # realtime or monotonic, default is realtime.
DeclareLaunchArgument("enable_frame_timestamp_csv", default_value="false"),
DeclareLaunchArgument("enable_frame_drop_log", default_value="false"),
DeclareLaunchArgument("frame_timestamp_csv_file", default_value=""),
DeclareLaunchArgument("enable_color_undistortion", default_value="false"),
DeclareLaunchArgument("enable_depth_undistortion", default_value="false"),
orbbec_camera/launch/gemini305.launch.py
+1 -1
View File
@@ -273,7 +273,7 @@ def generate_launch_description():
DeclareLaunchArgument('time_sync_period', default_value='6.0'), # seconds
DeclareLaunchArgument('time_domain', default_value='global'),# global, device, system
DeclareLaunchArgument('timestamp_clock_type', default_value=''),# realtime or monotonic, default is realtime.
DeclareLaunchArgument('enable_frame_timestamp_csv', default_value='false'),
DeclareLaunchArgument('enable_frame_drop_log', default_value='false'),
DeclareLaunchArgument('frame_timestamp_csv_file', default_value=''),
DeclareLaunchArgument('enable_color_undistortion', default_value='false'),
DeclareLaunchArgument('enable_depth_undistortion', default_value='false'),
orbbec_camera/launch/gemini305_g.launch.py
+1 -1
View File
@@ -273,7 +273,7 @@ def generate_launch_description():
DeclareLaunchArgument('time_sync_period', default_value='6.0'), # seconds
DeclareLaunchArgument('time_domain', default_value='global'),# global, device, system
DeclareLaunchArgument('timestamp_clock_type', default_value=''),# realtime or monotonic, default is realtime.
DeclareLaunchArgument('enable_frame_timestamp_csv', default_value='false'),
DeclareLaunchArgument('enable_frame_drop_log', default_value='false'),
DeclareLaunchArgument('frame_timestamp_csv_file', default_value=''),
DeclareLaunchArgument('enable_color_undistortion', default_value='false'),
DeclareLaunchArgument('enable_depth_undistortion', default_value='false'),
orbbec_camera/launch/gemini345.launch.py
+1 -1
View File
@@ -207,7 +207,7 @@ def generate_launch_description():
DeclareLaunchArgument('time_sync_period', default_value='6.0'), # seconds
DeclareLaunchArgument('time_domain', default_value='global'),# global, device, system
DeclareLaunchArgument('timestamp_clock_type', default_value=''),# realtime or monotonic, default is realtime.
DeclareLaunchArgument('enable_frame_timestamp_csv', default_value='false'),
DeclareLaunchArgument('enable_frame_drop_log', default_value='false'),
DeclareLaunchArgument('frame_timestamp_csv_file', default_value=''),
DeclareLaunchArgument('enable_color_undistortion', default_value='false'),
DeclareLaunchArgument('enable_depth_undistortion', default_value='false'),
orbbec_camera/launch/gemini345_lg.launch.py
+1 -1
View File
@@ -210,7 +210,7 @@ def generate_launch_description():
DeclareLaunchArgument('time_sync_period', default_value='6.0'), # seconds
DeclareLaunchArgument('time_domain', default_value='global'),# global, device, system
DeclareLaunchArgument('timestamp_clock_type', default_value=''),# realtime or monotonic, default is realtime.
DeclareLaunchArgument('enable_frame_timestamp_csv', default_value='false'),
DeclareLaunchArgument('enable_frame_drop_log', default_value='false'),
DeclareLaunchArgument('frame_timestamp_csv_file', default_value=''),
DeclareLaunchArgument('enable_color_undistortion', default_value='false'),
DeclareLaunchArgument('enable_depth_undistortion', default_value='false'),
orbbec_camera/launch/gemini435_le.launch.py
+1 -1
View File
@@ -241,7 +241,7 @@ def generate_launch_description():
DeclareLaunchArgument('time_sync_period', default_value='6.0'), # seconds
DeclareLaunchArgument('time_domain', default_value='global'),# global, device, system
DeclareLaunchArgument('timestamp_clock_type', default_value=''),# realtime or monotonic, default is realtime.
DeclareLaunchArgument('enable_frame_timestamp_csv', default_value='false'),
DeclareLaunchArgument('enable_frame_drop_log', default_value='false'),
DeclareLaunchArgument('frame_timestamp_csv_file', default_value=''),
DeclareLaunchArgument('enable_color_undistortion', default_value='false'),
DeclareLaunchArgument('enable_depth_undistortion', default_value='false'),
orbbec_camera/launch/gemini_330_series.launch.py
+1 -1
View File
@@ -259,7 +259,7 @@ def generate_launch_description():
DeclareLaunchArgument('time_sync_period', default_value='6.0'), # seconds
DeclareLaunchArgument('time_domain', default_value='global'),# global, device, system
DeclareLaunchArgument('timestamp_clock_type', default_value=''),# realtime or monotonic, default is realtime.
DeclareLaunchArgument('enable_frame_timestamp_csv', default_value='false'),
DeclareLaunchArgument('enable_frame_drop_log', default_value='false'),
DeclareLaunchArgument('frame_timestamp_csv_file', default_value=''),
DeclareLaunchArgument('enable_color_undistortion', default_value='false'),
DeclareLaunchArgument('enable_depth_undistortion', default_value='false'),
orbbec_camera/launch/gemini_330_series_low_cpu.launch.py
+1 -1
View File
@@ -258,7 +258,7 @@ def generate_launch_description():
DeclareLaunchArgument('time_sync_period', default_value='6.0'), # seconds
DeclareLaunchArgument('time_domain', default_value='global'),# global, device, system
DeclareLaunchArgument('timestamp_clock_type', default_value=''),# realtime or monotonic, default is realtime.
DeclareLaunchArgument('enable_frame_timestamp_csv', default_value='false'),
DeclareLaunchArgument('enable_frame_drop_log', default_value='false'),
DeclareLaunchArgument('frame_timestamp_csv_file', default_value=''),
DeclareLaunchArgument('enable_color_undistortion', default_value='false'),
DeclareLaunchArgument('enable_depth_undistortion', default_value='false'),
orbbec_camera/launch/gemini_330_series_sdk_json.launch.py
+1 -1
View File
@@ -141,7 +141,7 @@ def generate_launch_description():
DeclareLaunchArgument('enable_frame_sync', default_value='true'),
DeclareLaunchArgument('frame_aggregate_mode', default_value='ANY'),
DeclareLaunchArgument('time_domain', default_value='global'),
DeclareLaunchArgument('enable_frame_timestamp_csv', default_value='false'),
DeclareLaunchArgument('enable_frame_drop_log', default_value='false'),
DeclareLaunchArgument('frame_timestamp_csv_file', default_value=''),
DeclareLaunchArgument('enable_color_undistortion', default_value='false'),
DeclareLaunchArgument('enable_depth_undistortion', default_value='false'),
orbbec_camera/src/frame_timestamp_csv_logger.cpp
+52 -63
View File
@@ -31,38 +31,47 @@ int64_t getExpectedIntervalUs(const std::shared_ptr<ob::Frame> &frame) {
} // namespace
FrameTimestampCsvLogger::FrameTimestampCsvLogger(bool enabled, const std::string &csv_file_path,
FrameTimestampCsvLogger::FrameTimestampCsvLogger(bool drop_log_enabled,
const std::string &csv_file_path,
rclcpp::Logger logger)
: logger_(std::move(logger)), enabled_(enabled), csv_file_path_(csv_file_path) {
: 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) {
if (!enabled_) {
return;
}
if (csv_file_path_.empty()) {
RCLCPP_INFO_STREAM(logger_,
"Frame timestamp CSV file is empty; only frame lost logs are enabled.");
return;
}
try {
auto path = std::filesystem::path(csv_file_path_);
if (path.has_parent_path() && !std::filesystem::exists(path.parent_path())) {
std::filesystem::create_directories(path.parent_path());
if (csv_enabled_) {
try {
auto path = std::filesystem::path(csv_file_path_);
if (path.has_parent_path() && !std::filesystem::exists(path.parent_path())) {
std::filesystem::create_directories(path.parent_path());
}
} catch (const std::exception &e) {
RCLCPP_ERROR_STREAM(logger_, "Failed to prepare frame timestamp CSV path "
<< csv_file_path_ << ": " << e.what());
csv_enabled_ = false;
csv_writer_failed_ = true;
}
} catch (const std::exception &e) {
RCLCPP_ERROR_STREAM(logger_, "Failed to prepare frame timestamp CSV path " << csv_file_path_
<< ": " << e.what());
enabled_ = false;
writer_failed_ = true;
return;
}
openCsvIfNeeded();
if (!enabled_) {
return;
if (csv_enabled_) {
openCsvIfNeeded();
}
writer_thread_ = std::thread([this]() { writerThreadMain(); });
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_ ? csv_file_path_ : "disabled")
<< " frame_drop_log=" << (drop_log_enabled_ ? "enabled" : "disabled"));
}
}
FrameTimestampCsvLogger::~FrameTimestampCsvLogger() noexcept { shutdown(); }
@@ -73,7 +82,7 @@ void FrameTimestampCsvLogger::recordFrameSet(const std::shared_ptr<ob::Frame> &c
bool track_color, bool track_depth,
bool color_image_publish_expected,
bool depth_image_publish_expected) {
if (!enabled_ || writer_failed_) {
if (!enabled_) {
return;
}
recordFrameSetInternal(color_frame, depth_frame, arrival_system_us, arrival_steady_us,
@@ -86,7 +95,7 @@ void FrameTimestampCsvLogger::recordStandaloneFrameArrival(OBStreamType stream_t
int64_t arrival_system_us,
int64_t arrival_steady_us,
bool image_publish_expected) {
if (!enabled_ || writer_failed_ || !frame || !isTrackedStream(stream_type)) {
if (!enabled_ || !frame || !isTrackedStream(stream_type)) {
return;
}
recordStandaloneFrameArrivalInternal(stream_type, frame, arrival_system_us, arrival_steady_us,
@@ -97,7 +106,7 @@ 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_ || writer_failed_ || !frame || !isTrackedStream(stream_type)) {
if (!enabled_ || !frame || !isTrackedStream(stream_type)) {
return;
}
recordPreImagePublishInternal(stream_type, frame, publish_system_us, publish_steady_us);
@@ -316,16 +325,13 @@ void FrameTimestampCsvLogger::populateArrivalData(StreamState &state, TrackedStr
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);
previous.dropped_frames += lost_frames;
reportDropLogFormatOnce();
RCLCPP_WARN_STREAM(
logger_, "SDK drop " << (stream == TrackedStream::COLOR ? "color" : "depth") << ": idx="
<< state.frame_index << " ts=" << state.device_ts_us << "us"
<< " gap=" << std::fixed << std::setprecision(1)
<< (static_cast<double>(device_ts_delta_us) / 1000.0) << "ms"
<< " ideal="
<< (static_cast<double>(state.expected_interval_us) / 1000.0) << "ms"
<< " total=" << previous.dropped_frames);
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=" << lost_frames);
}
}
}
if (frame->hasMetadata(OB_FRAME_METADATA_TYPE_FRAME_NUMBER)) {
@@ -369,16 +375,13 @@ void FrameTimestampCsvLogger::populatePublishData(StreamState &state, TrackedStr
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);
previous.publish_dropped_frames += lost_frames;
reportDropLogFormatOnce();
RCLCPP_WARN_STREAM(
logger_, "PUB drop " << (stream == TrackedStream::COLOR ? "color" : "depth") << ": idx="
<< state.frame_index << " ts=" << state.device_ts_us << "us"
<< " gap=" << std::fixed << std::setprecision(1)
<< (static_cast<double>(device_ts_delta_us) / 1000.0) << "ms"
<< " ideal="
<< (static_cast<double>(state.expected_interval_us) / 1000.0) << "ms"
<< " total=" << previous.publish_dropped_frames);
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=" << lost_frames);
}
}
}
previous.publish_device_ts_us = state.device_ts_us;
@@ -391,19 +394,6 @@ void FrameTimestampCsvLogger::populatePublishData(StreamState &state, TrackedStr
state.arrival_to_publish_steady_us = state.publish_steady_us.value() - state.arrival_steady_us;
}
void FrameTimestampCsvLogger::reportDropLogFormatOnce() {
if (drop_log_format_reported_) {
return;
}
drop_log_format_reported_ = true;
RCLCPP_INFO_STREAM(
logger_,
"Frame drop log enabled. Format: <SDK|PUB> drop <color|depth>: idx=<frame_index> "
"ts=<device_timestamp_us> gap=<actual_interval_ms> ideal=<expected_interval_ms> "
"total=<total_dropped_frames>. SDK means frame arrival from SDK; PUB means before ROS "
"image publish.");
}
std::optional<int64_t> FrameTimestampCsvLogger::updateDelta(std::optional<int64_t> &previous,
int64_t current) {
std::optional<int64_t> delta;
@@ -423,7 +413,7 @@ bool FrameTimestampCsvLogger::isRowReady(const PendingRow &row) const {
}
void FrameTimestampCsvLogger::enqueueCompletedRow(const PendingRow &row) {
if (csv_file_path_.empty()) {
if (!csv_enabled_ || csv_writer_failed_) {
return;
}
std::lock_guard<std::mutex> queue_lock(completed_rows_mutex_);
@@ -539,7 +529,7 @@ std::string FrameTimestampCsvLogger::csvHeader() {
}
void FrameTimestampCsvLogger::writerThreadMain() {
if (!enabled_ || writer_failed_) {
if (!csv_enabled_ || csv_writer_failed_) {
return;
}
@@ -582,13 +572,12 @@ void FrameTimestampCsvLogger::openCsvIfNeeded() {
csv_stream_.open(csv_file_path_, std::ios::out | std::ios::trunc);
if (!csv_stream_.is_open()) {
RCLCPP_ERROR_STREAM(logger_, "Failed to open frame timestamp CSV file: " << csv_file_path_);
enabled_ = false;
writer_failed_ = true;
csv_enabled_ = false;
csv_writer_failed_ = true;
return;
}
csv_stream_ << csvHeader() << "\n";
csv_stream_.flush();
RCLCPP_INFO_STREAM(logger_, "Frame timestamp CSV logger enabled: " << csv_file_path_);
}
} // namespace orbbec_camera
orbbec_camera/src/ob_camera_node.cpp
+16 -17
View File
@@ -192,11 +192,10 @@ std::string normalizeClosedSetParameterValue(const rclcpp::Logger &logger,
}
RCLCPP_ERROR_STREAM(logger, "Invalid parameter "
<< param_name << " " << formatParameterValue(value)
<< ". Valid values: "
<< formatValidParameterValues(valid_values)
<< ". Skip setting and use "
<< formatParameterValue(default_value) << ".");
<< param_name << " " << formatParameterValue(value)
<< ". Valid values: " << formatValidParameterValues(valid_values)
<< ". Skip setting and use "
<< formatParameterValue(default_value) << ".");
return default_value;
}
@@ -575,9 +574,9 @@ OBCameraNode::OBCameraNode(rclcpp::Node *node, std::shared_ptr<ob::Device> devic
setupDefaultImageFormat();
setupTopics();
if (enable_frame_timestamp_csv_) {
frame_timestamp_csv_logger_ =
std::make_unique<FrameTimestampCsvLogger>(true, frame_timestamp_csv_file_, logger_);
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();
}
@@ -3559,9 +3558,9 @@ void OBCameraNode::getParameters() {
setAndGetNodeParameter<std::string>(point_cloud_qos_, "point_cloud_qos", "default");
setAndGetNodeParameter<bool>(enable_d2c_viewer_, "enable_d2c_viewer", false);
setAndGetNodeParameter<std::string>(disparity_to_depth_mode_, "disparity_to_depth_mode", "");
disparity_to_depth_mode_ = normalizeClosedSetParameterValue(
logger_, "disparity_to_depth_mode", disparity_to_depth_mode_, {"", "HW", "SW", "disable"},
"");
disparity_to_depth_mode_ =
normalizeClosedSetParameterValue(logger_, "disparity_to_depth_mode", disparity_to_depth_mode_,
{"", "HW", "SW", "disable"}, "");
setAndGetNodeParameter<std::string>(depth_filter_config_, "depth_filter_config", "");
if (!depth_filter_config_.empty()) {
enable_depth_filter_ = true;
@@ -3728,9 +3727,9 @@ void OBCameraNode::getParameters() {
setAndGetNodeParameter<bool>(enable_heartbeat_, "enable_heartbeat", false);
setAndGetNodeParameter<bool>(enable_firmware_log_, "enable_firmware_log", false);
setAndGetNodeParameter<std::string>(time_domain_, "time_domain", "global");
time_domain_ = normalizeClosedSetParameterValue(
logger_, "time_domain", time_domain_, {"global", "device", "system"}, "global");
setAndGetNodeParameter<bool>(enable_frame_timestamp_csv_, "enable_frame_timestamp_csv", false);
time_domain_ = normalizeClosedSetParameterValue(logger_, "time_domain", time_domain_,
{"global", "device", "system"}, "global");
setAndGetNodeParameter<bool>(enable_frame_drop_log_, "enable_frame_drop_log", false);
setAndGetNodeParameter<std::string>(frame_timestamp_csv_file_, "frame_timestamp_csv_file", "");
setAndGetNodeParameter<std::string>(exposure_range_mode_, "exposure_range_mode", "");
setAndGetNodeParameter<std::string>(load_config_json_file_path_, "load_config_json_file_path",
@@ -3810,9 +3809,9 @@ void OBCameraNode::getParameters() {
setAndGetNodeParameter<int>(offset_index1_, "offset_index1", -1);
setAndGetNodeParameter<std::string>(frame_aggregate_mode_, "frame_aggregate_mode", "ANY");
frame_aggregate_mode_ = normalizeClosedSetParameterValue(
logger_, "frame_aggregate_mode", frame_aggregate_mode_,
{"full_frame", "color_frame", "ANY", "disable"}, "ANY");
frame_aggregate_mode_ =
normalizeClosedSetParameterValue(logger_, "frame_aggregate_mode", frame_aggregate_mode_,
{"full_frame", "color_frame", "ANY", "disable"}, "ANY");
setAndGetNodeParameter<bool>(show_fps_enable_, "show_fps_enable", false);
setAndGetNodeParameter<bool>(enable_publish_extrinsic_, "enable_publish_extrinsic", false);