From 6c443106454af76b143305b0704548f6da25542c Mon Sep 17 00:00:00 2001 From: ob-yalian Date: Tue, 23 Dec 2025 15:26:43 +0800 Subject: [PATCH] fix: optimize color undistortion logic by removing redundant code and ensuring proper publishing conditions --- orbbec_camera/src/ob_camera_node.cpp | 26 ++++++++++++-------------- 1 file changed, 12 insertions(+), 14 deletions(-) diff --git a/orbbec_camera/src/ob_camera_node.cpp b/orbbec_camera/src/ob_camera_node.cpp index 64ad3e71..53ae962e 100644 --- a/orbbec_camera/src/ob_camera_node.cpp +++ b/orbbec_camera/src/ob_camera_node.cpp @@ -3152,7 +3152,18 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr &frame, if (depth_registration_ && stream_index == DEPTH) { frame_id = depth_aligned_frame_id_[stream_index]; } - if (stream_index == COLOR && enable_color_undistortion_) { + if (stream_index == COLOR && enable_color_undistortion_ && + color_undistortion_publisher_->get_subscription_count() > 0) { + auto undistorted_image = undistortImage(image, intrinsic, distortion); + sensor_msgs::msg::Image::UniquePtr undistorted_image_msg(new sensor_msgs::msg::Image()); + cv_bridge::CvImage(std_msgs::msg::Header(), encoding_[stream_index], undistorted_image) + .toImageMsg(*undistorted_image_msg); + CHECK_NOTNULL(undistorted_image_msg.get()); + undistorted_image_msg->header.stamp = timestamp; + undistorted_image_msg->is_bigendian = false; + undistorted_image_msg->step = width * unit_step_size_[stream_index]; + undistorted_image_msg->header.frame_id = frame_id; + color_undistortion_publisher_->publish(std::move(undistorted_image_msg)); memset(&distortion, 0, sizeof(distortion)); } sensor_msgs::msg::CameraInfo camera_info{}; @@ -3235,19 +3246,6 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr &frame, fps_delay_status_depth_->tick(frame_timestamp); } image_publishers_[stream_index]->publish(std::move(image_msg)); - if (stream_index == COLOR && enable_color_undistortion_ && - color_undistortion_publisher_->get_subscription_count() > 0) { - auto undistorted_image = undistortImage(image, intrinsic, distortion); - sensor_msgs::msg::Image::UniquePtr undistorted_image_msg(new sensor_msgs::msg::Image()); - cv_bridge::CvImage(std_msgs::msg::Header(), encoding_[stream_index], undistorted_image) - .toImageMsg(*undistorted_image_msg); - CHECK_NOTNULL(undistorted_image_msg.get()); - undistorted_image_msg->header.stamp = timestamp; - undistorted_image_msg->is_bigendian = false; - undistorted_image_msg->step = width * unit_step_size_[stream_index]; - undistorted_image_msg->header.frame_id = frame_id; - color_undistortion_publisher_->publish(std::move(undistorted_image_msg)); - } } void OBCameraNode::publishMetadata(const std::shared_ptr &frame,