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

fix: correct out-of-order timestamp CSV records

Browse Source
This commit is contained in:
slz
2026-07-13 14:17:46 +08:00
parent b41e8bbda1
commit c26d074171
3 changed files with 48 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files
orbbec_camera/include/orbbec_camera/frame_timestamp_csv_logger.h
+5 -3
View File
@@ -40,6 +40,7 @@ class FrameTimestampCsvLogger {
void recordPreImagePublish(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 shutdown();
@@ -112,9 +113,10 @@ class FrameTimestampCsvLogger {
const std::shared_ptr<ob::Frame> &frame,
int64_t arrival_system_us, int64_t arrival_steady_us,
bool image_publish_expected);
void recordPreImagePublishInternal(OBStreamType stream_type,
const std::shared_ptr<ob::Frame> &frame,
int64_t publish_system_us, int64_t publish_steady_us);
void completeImagePublishInternal(OBStreamType stream_type,
const std::shared_ptr<ob::Frame> &frame,
std::optional<int64_t> publish_system_us,
std::optional<int64_t> publish_steady_us);
void populateArrivalData(StreamState &state, TrackedStream stream,
const std::shared_ptr<ob::Frame> &frame, int64_t arrival_system_us,
orbbec_camera/src/frame_timestamp_csv_logger.cpp
+30 -11
View File
@@ -109,7 +109,15 @@ void FrameTimestampCsvLogger::recordPreImagePublish(OBStreamType stream_type,
if (!enabled_ || !frame || !isTrackedStream(stream_type)) {
return;
}
recordPreImagePublishInternal(stream_type, frame, publish_system_us, publish_steady_us);
completeImagePublishInternal(stream_type, frame, publish_system_us, publish_steady_us);
}
void FrameTimestampCsvLogger::recordImagePublishSkipped(OBStreamType stream_type,
const std::shared_ptr<ob::Frame> &frame) {
if (!enabled_ || !frame || !isTrackedStream(stream_type)) {
return;
}
completeImagePublishInternal(stream_type, frame, std::nullopt, std::nullopt);
}
void FrameTimestampCsvLogger::shutdown() {
@@ -257,10 +265,9 @@ void FrameTimestampCsvLogger::recordStandaloneFrameArrivalInternal(
}
}
void FrameTimestampCsvLogger::recordPreImagePublishInternal(OBStreamType stream_type,
const std::shared_ptr<ob::Frame> &frame,
int64_t publish_system_us,
int64_t publish_steady_us) {
void FrameTimestampCsvLogger::completeImagePublishInternal(
OBStreamType stream_type, const std::shared_ptr<ob::Frame> &frame,
std::optional<int64_t> publish_system_us, std::optional<int64_t> publish_steady_us) {
std::optional<PendingRow> ready_row;
const auto frame_index = frame->getIndex();
@@ -275,10 +282,12 @@ void FrameTimestampCsvLogger::recordPreImagePublishInternal(OBStreamType stream_
: depth_frame_index_to_row_id_;
auto row_id_it = row_map.find(frame_index);
if (row_id_it == row_map.end()) {
RCLCPP_WARN_STREAM(logger_,
"Frame timestamp CSV logger missed row mapping for stream "
<< (tracked_stream == TrackedStream::COLOR ? "color" : "depth")
<< " frame index " << frame_index);
if (publish_system_us.has_value()) {
RCLCPP_WARN_STREAM(logger_,
"Frame timestamp CSV logger missed row mapping for stream "
<< (tracked_stream == TrackedStream::COLOR ? "color" : "depth")
<< " frame index " << frame_index);
}
return;
}
const auto row_id = row_id_it->second;
@@ -290,8 +299,16 @@ void FrameTimestampCsvLogger::recordPreImagePublishInternal(OBStreamType stream_
auto &state = tracked_stream == TrackedStream::COLOR ? pending_it->second.color
: pending_it->second.depth;
populatePublishData(state, tracked_stream, publish_system_us, publish_steady_us);
state.final = true;
if (state.final) {
return;
}
if (publish_system_us.has_value() && publish_steady_us.has_value()) {
populatePublishData(state, tracked_stream, publish_system_us.value(),
publish_steady_us.value());
state.final = true;
} else {
finalizeStreamWithoutPublish(state);
}
if (isRowReady(pending_it->second)) {
ready_row = pending_it->second;
@@ -440,6 +457,8 @@ void FrameTimestampCsvLogger::flushPendingRowsLocked(std::vector<PendingRow> &ro
row.depth.final = true;
rows.push_back(std::move(row));
}
std::stable_sort(rows.begin(), rows.end(),
[](const auto &lhs, const auto &rhs) { return lhs.row_id < rhs.row_id; });
pending_rows_.clear();
color_frame_index_to_row_id_.clear();
depth_frame_index_to_row_id_.clear();
orbbec_camera/src/ob_camera_node.cpp
+13
View File
@@ -6238,6 +6238,12 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame> &frame,
if (frame == nullptr) {
return;
}
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);
}
};
CHECK_NOTNULL(image_publishers_[stream_index]);
const bool has_raw_image_subscriber =
image_publishers_[stream_index]->get_subscription_count() > 0;
@@ -6250,6 +6256,7 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame> &frame,
has_subscriber || (metadata_publishers_.count(stream_index) &&
metadata_publishers_[stream_index]->get_subscription_count() > 0);
if (!has_subscriber) {
record_image_publish_skipped();
return;
}
std::shared_ptr<ob::VideoFrame> video_frame;
@@ -6278,6 +6285,7 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame> &frame,
}
if (!video_frame) {
RCLCPP_ERROR(logger_, "Failed to convert frame to video frame");
record_image_publish_skipped();
return;
}
int width = static_cast<int>(video_frame->getWidth());
@@ -6286,11 +6294,13 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame> &frame,
auto timestamp = fromUsToROSTime(frame_timestamp);
if (!device_) {
RCLCPP_ERROR_STREAM(logger_, "device is null in onNewFrameCallback");
record_image_publish_skipped();
return;
}
auto device_info = device_->getDeviceInfo();
if (!device_info || !device_info.get()) {
RCLCPP_ERROR_STREAM(logger_, "device_info is null in onNewFrameCallback");
record_image_publish_skipped();
return;
}
OBCameraIntrinsic intrinsic;
@@ -6363,6 +6373,7 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame> &frame,
CHECK(camera_info_publishers_.count(stream_index) > 0);
camera_info_publishers_[stream_index]->publish(camera_info);
publishMetadata(frame, stream_index, camera_info.header);
record_image_publish_skipped();
return;
}
@@ -6374,6 +6385,7 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame> &frame,
(has_raw_image_subscriber || save_images_[stream_index])) {
if (frame->getType() == OB_FRAME_COLOR && !is_color_frame_decoded_) {
RCLCPP_ERROR(logger_, "color frame is not decoded");
record_image_publish_skipped();
return;
}
if (frame->getType() == OB_FRAME_COLOR_LEFT && !is_left_color_frame_decoded_) {
@@ -6418,6 +6430,7 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame> &frame,
image_msg->header.frame_id = frame_id;
saveImageToFile(stream_index, image, *image_msg);
if (!has_raw_image_subscriber) {
record_image_publish_skipped();
return;
}
if (frame_timestamp_csv_logger_ && frame_timestamp_csv_logger_->enabled() &&