From 9960d22a32aeb866eca3ad453be55626de25da5f Mon Sep 17 00:00:00 2001 From: slz Date: Mon, 7 Sep 2026 17:17:45 +0800 Subject: [PATCH 1/3] fix: correct runtime software alignment behavior --- orbbec_camera/src/ob_camera_node.cpp | 22 ++++++++++++---------- 1 file changed, 12 insertions(+), 10 deletions(-) diff --git a/orbbec_camera/src/ob_camera_node.cpp b/orbbec_camera/src/ob_camera_node.cpp index 917ba861..2a7ddc32 100644 --- a/orbbec_camera/src/ob_camera_node.cpp +++ b/orbbec_camera/src/ob_camera_node.cpp @@ -6036,7 +6036,9 @@ void OBCameraNode::publishDepthPointCloud(const std::shared_ptr &f } auto frame_timestamp = getFrameTimestampUs(depth_frame); 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()) { frame_id = cloud_frame_id_; } @@ -6536,11 +6538,11 @@ void OBCameraNode::onNewFrameSetCallback(std::shared_ptr frame_set } } if (depth_registration_ && align_filter_ && depth_frame) { - publishRawDepthImage(depth_frame); - auto target_frame_type = STREAM_TYPE_TO_FRAME_TYPE.at(align_target_stream_); - if (!frame_set->getFrame(target_frame_type) || !color_frame) { - RCLCPP_DEBUG_STREAM( - logger_, "Depth registration requires depth and color frames, skip software alignment"); + if (align_target_stream_ == OB_STREAM_COLOR) { + publishRawDepthImage(depth_frame); + } + if (align_target_stream_ == OB_STREAM_DEPTH && !color_frame) { + RCLCPP_DEBUG_STREAM(logger_, "C2D alignment requires a color frame, skip alignment"); } else { auto align_color_frame = color_frame; if (align_target_stream_ == OB_STREAM_DEPTH) { @@ -6568,7 +6570,7 @@ void OBCameraNode::onNewFrameSetCallback(std::shared_ptr frame_set 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)) { auto new_frame_set = new_frame->as(); CHECK_NOTNULL(new_frame_set.get()); @@ -6583,8 +6585,8 @@ void OBCameraNode::onNewFrameSetCallback(std::shared_ptr frame_set } } else { RCLCPP_DEBUG_ONCE(logger_, - "Depth registration is disabled or align filter is null or depth frame is " - "null or color frame is null"); + "Depth registration is disabled, align filter is null, or depth frame is " + "null"); } if (enable_enhanced_depth_.load()) { @@ -7077,7 +7079,7 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr &frame, distortion = camera_params.rgbDistortion; } 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]; } sensor_msgs::msg::CameraInfo camera_info{}; From c28fcb90525c1ec82f3d301b7454e52aa00d8ff5 Mon Sep 17 00:00:00 2001 From: slz Date: Mon, 7 Sep 2026 17:19:37 +0800 Subject: [PATCH 2/3] fix: limit unaligned depth topic to software D2C --- orbbec_camera/src/ob_camera_node.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/orbbec_camera/src/ob_camera_node.cpp b/orbbec_camera/src/ob_camera_node.cpp index 2a7ddc32..e3fe768a 100644 --- a/orbbec_camera/src/ob_camera_node.cpp +++ b/orbbec_camera/src/ob_camera_node.cpp @@ -5792,6 +5792,11 @@ void OBCameraNode::syncSoftwareAlignment() { align_filter_ = std::make_unique(align_target_stream_); 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_) { const auto depth_image_qos_profile = getImageQosProfile(DEPTH); if (use_intra_process_) { From 113a956afa1ff17c8c6a95940f265ce82a105799 Mon Sep 17 00:00:00 2001 From: slz Date: Mon, 14 Sep 2026 16:47:22 +0800 Subject: [PATCH 3/3] fix: improve software alignment handling and cleanup in image registration mode --- orbbec_camera/src/ob_camera_node.cpp | 6 ++++-- orbbec_camera/src/ros_service.cpp | 4 ++++ 2 files changed, 8 insertions(+), 2 deletions(-) diff --git a/orbbec_camera/src/ob_camera_node.cpp b/orbbec_camera/src/ob_camera_node.cpp index e3fe768a..cf0a3748 100644 --- a/orbbec_camera/src/ob_camera_node.cpp +++ b/orbbec_camera/src/ob_camera_node.cpp @@ -6546,8 +6546,9 @@ void OBCameraNode::onNewFrameSetCallback(std::shared_ptr frame_set if (align_target_stream_ == OB_STREAM_COLOR) { publishRawDepthImage(depth_frame); } - if (align_target_stream_ == OB_STREAM_DEPTH && !color_frame) { - RCLCPP_DEBUG_STREAM(logger_, "C2D alignment requires a color frame, skip alignment"); + if (!color_frame) { + RCLCPP_DEBUG_STREAM(logger_, "Software alignment requires a color frame, skip frame set"); + return; } else { auto align_color_frame = color_frame; if (align_target_stream_ == OB_STREAM_DEPTH) { @@ -6570,6 +6571,7 @@ void OBCameraNode::onNewFrameSetCallback(std::shared_ptr frame_set } if (!align_color_frame) { RCLCPP_ERROR_STREAM(logger_, "Failed to convert color frame for C2D alignment"); + return; } else if (align_color_frame != color_frame) { color_frame = align_color_frame; frame_set->pushFrame(color_frame); diff --git a/orbbec_camera/src/ros_service.cpp b/orbbec_camera/src/ros_service.cpp index 95ee5d86..9b80665f 100644 --- a/orbbec_camera/src/ros_service.cpp +++ b/orbbec_camera/src/ros_service.cpp @@ -903,6 +903,8 @@ void OBCameraNode::setImageRegistrationModeCallback( auto rollback_after_error = [&](const std::string& error_message) { try { + stopColorFrameThreads(); + clearColorFrameQueues(); restore_old_mode(); if (was_running && !pipeline_started_.load()) { startStreams(); @@ -924,6 +926,8 @@ void OBCameraNode::setImageRegistrationModeCallback( if (was_running) { stopStreams(); } + stopColorFrameThreads(); + clearColorFrameQueues(); apply_image_registration_mode(mode);