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

Merge remote-tracking branch 'origin/fix/runtime-image-registration-mode' into v2/develop

Browse Source
This commit is contained in:
ob-yalian
2026-09-15 10:26:28 +08:00
parent d99ce55444 113a956afa
commit 37022461c3
2 changed files with 23 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
orbbec_camera/src/ob_camera_node.cpp
+19 -10
View File
@@ -5813,6 +5813,11 @@ void OBCameraNode::syncSoftwareAlignment() {
align_filter_ = std::make_unique<ob::Align>(align_target_stream_); align_filter_ = std::make_unique<ob::Align>(align_target_stream_);
RCLCPP_INFO_STREAM(logger_, "set align mode to " << align_mode_); RCLCPP_INFO_STREAM(logger_, "set align mode to " << align_mode_);
} }
if (align_target_stream_ != OB_STREAM_COLOR) {
releaseGlobalImageTransportPublisher(*node_, "depth/image_unaligned");
depth_unaligned_publisher_.reset();
return;
}
if (!depth_unaligned_publisher_) { if (!depth_unaligned_publisher_) {
const auto depth_image_qos_profile = getImageQosProfile(DEPTH); const auto depth_image_qos_profile = getImageQosProfile(DEPTH);
if (use_intra_process_) { if (use_intra_process_) {
@@ -6057,7 +6062,9 @@ void OBCameraNode::publishDepthPointCloud(const std::shared_ptr<ob::FrameSet> &f
} }
auto frame_timestamp = getFrameTimestampUs(depth_frame); auto frame_timestamp = getFrameTimestampUs(depth_frame);
auto timestamp = fromUsToROSTime(frame_timestamp); auto timestamp = fromUsToROSTime(frame_timestamp);
std::string frame_id = depth_registration_ ? optical_frame_id_[COLOR] : optical_frame_id_[DEPTH]; std::string frame_id = depth_registration_ && align_target_stream_ == OB_STREAM_COLOR
? depth_aligned_frame_id_[DEPTH]
: optical_frame_id_[DEPTH];
if (!cloud_frame_id_.empty()) { if (!cloud_frame_id_.empty()) {
frame_id = cloud_frame_id_; frame_id = cloud_frame_id_;
} }
@@ -6557,11 +6564,12 @@ void OBCameraNode::onNewFrameSetCallback(std::shared_ptr<ob::FrameSet> frame_set
} }
} }
if (depth_registration_ && align_filter_ && depth_frame) { if (depth_registration_ && align_filter_ && depth_frame) {
publishRawDepthImage(depth_frame); if (align_target_stream_ == OB_STREAM_COLOR) {
auto target_frame_type = STREAM_TYPE_TO_FRAME_TYPE.at(align_target_stream_); publishRawDepthImage(depth_frame);
if (!frame_set->getFrame(target_frame_type) || !color_frame) { }
RCLCPP_DEBUG_STREAM( if (!color_frame) {
logger_, "Depth registration requires depth and color frames, skip software alignment"); RCLCPP_DEBUG_STREAM(logger_, "Software alignment requires a color frame, skip frame set");
return;
} else { } else {
auto align_color_frame = color_frame; auto align_color_frame = color_frame;
if (align_target_stream_ == OB_STREAM_DEPTH) { if (align_target_stream_ == OB_STREAM_DEPTH) {
@@ -6584,12 +6592,13 @@ void OBCameraNode::onNewFrameSetCallback(std::shared_ptr<ob::FrameSet> frame_set
} }
if (!align_color_frame) { if (!align_color_frame) {
RCLCPP_ERROR_STREAM(logger_, "Failed to convert color frame for C2D alignment"); RCLCPP_ERROR_STREAM(logger_, "Failed to convert color frame for C2D alignment");
return;
} else if (align_color_frame != color_frame) { } else if (align_color_frame != color_frame) {
color_frame = align_color_frame; color_frame = align_color_frame;
frame_set->pushFrame(color_frame); frame_set->pushFrame(color_frame);
} }
} }
if (align_color_frame) { if (align_target_stream_ != OB_STREAM_DEPTH || align_color_frame) {
if (auto new_frame = align_filter_->process(frame_set)) { if (auto new_frame = align_filter_->process(frame_set)) {
auto new_frame_set = new_frame->as<ob::FrameSet>(); auto new_frame_set = new_frame->as<ob::FrameSet>();
CHECK_NOTNULL(new_frame_set.get()); CHECK_NOTNULL(new_frame_set.get());
@@ -6604,8 +6613,8 @@ void OBCameraNode::onNewFrameSetCallback(std::shared_ptr<ob::FrameSet> frame_set
} }
} else { } else {
RCLCPP_DEBUG_ONCE(logger_, RCLCPP_DEBUG_ONCE(logger_,
"Depth registration is disabled or align filter is null or depth frame is " "Depth registration is disabled, align filter is null, or depth frame is "
"null or color frame is null"); "null");
} }
if (enable_enhanced_depth_.load()) { if (enable_enhanced_depth_.load()) {
@@ -7098,7 +7107,7 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame> &frame,
distortion = camera_params.rgbDistortion; distortion = camera_params.rgbDistortion;
} }
std::string frame_id = optical_frame_id_[stream_index]; std::string frame_id = optical_frame_id_[stream_index];
if (depth_registration_ && stream_index == DEPTH) { if (depth_registration_ && align_target_stream_ == OB_STREAM_COLOR && stream_index == DEPTH) {
frame_id = depth_aligned_frame_id_[stream_index]; frame_id = depth_aligned_frame_id_[stream_index];
} }
sensor_msgs::msg::CameraInfo camera_info{}; sensor_msgs::msg::CameraInfo camera_info{};
orbbec_camera/src/ros_service.cpp
+4
View File
@@ -903,6 +903,8 @@ void OBCameraNode::setImageRegistrationModeCallback(
auto rollback_after_error = [&](const std::string& error_message) { auto rollback_after_error = [&](const std::string& error_message) {
try { try {
stopColorFrameThreads();
clearColorFrameQueues();
restore_old_mode(); restore_old_mode();
if (was_running && !pipeline_started_.load()) { if (was_running && !pipeline_started_.load()) {
startStreams(); startStreams();
@@ -924,6 +926,8 @@ void OBCameraNode::setImageRegistrationModeCallback(
if (was_running) { if (was_running) {
stopStreams(); stopStreams();
} }
stopColorFrameThreads();
clearColorFrameQueues();
apply_image_registration_mode(mode); apply_image_registration_mode(mode);