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

Merge branch 'feat/imu-timestamp-csv' into v2/develop

Browse Source
This commit is contained in:
ob-yalian
2026-08-26 14:38:11 +08:00
parent 5df224e7a4 4d5b3a7370
commit 230dfb82cf
9 changed files with 839 additions and 53 deletions
Download Patch File Download Diff File Expand all files Collapse all files
orbbec_camera/src/ob_camera_node.cpp
+68 -32
View File
@@ -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 {