i/OrbbecSDK_ROS2
1
0
Fork 0
mirror of https://github.com/orbbec/OrbbecSDK_ROS2.git synced 2026-10-09 22:29:48 +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_);
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_) {
@@ -6057,7 +6062,9 @@ void OBCameraNode::publishDepthPointCloud(const std::shared_ptr<ob::FrameSet> &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_;
}
@@ -6557,11 +6564,12 @@ void OBCameraNode::onNewFrameSetCallback(std::shared_ptr<ob::FrameSet> 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 (!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) {
@@ -6584,12 +6592,13 @@ void OBCameraNode::onNewFrameSetCallback(std::shared_ptr<ob::FrameSet> 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);
}
}
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<ob::FrameSet>();
CHECK_NOTNULL(new_frame_set.get());
@@ -6604,8 +6613,8 @@ void OBCameraNode::onNewFrameSetCallback(std::shared_ptr<ob::FrameSet> 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()) {
@@ -7098,7 +7107,7 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame> &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{};