From a0410626633f03150bcbf24cb12c092ba0a0491f Mon Sep 17 00:00:00 2001 From: slz Date: Mon, 23 Mar 2026 18:12:11 +0800 Subject: [PATCH] feat: add support for publishing raw color MJPG images --- .../include/orbbec_camera/ob_camera_node.h | 7 +++ orbbec_camera/src/ob_camera_node.cpp | 48 +++++++++++++++++-- 2 files changed, 51 insertions(+), 4 deletions(-) diff --git a/orbbec_camera/include/orbbec_camera/ob_camera_node.h b/orbbec_camera/include/orbbec_camera/ob_camera_node.h index 8298d164..72baef28 100644 --- a/orbbec_camera/include/orbbec_camera/ob_camera_node.h +++ b/orbbec_camera/include/orbbec_camera/ob_camera_node.h @@ -39,6 +39,7 @@ #include #include +#include #include #include @@ -482,6 +483,10 @@ class OBCameraNode { bool setupFormatConvertType(OBFormat format); bool setupFormatConvertType(OBFormat format, ob::FormatConvertFilter& filter); + bool hasCompressedImageSubscriber(const stream_index_pair& stream_index) const; + void publishCompressedColorImage(const std::shared_ptr& frame, + const stream_index_pair& stream_index, + const rclcpp::Time& timestamp, const std::string& frame_id); orbbec_camera_msgs::msg::IMUInfo createIMUInfo(const stream_index_pair& stream_index); @@ -570,6 +575,8 @@ class OBCameraNode { std::map rotation_stream_; std::map stream_name_; std::map> image_publishers_; + std::map::SharedPtr> + compressed_image_publishers_; std::map::SharedPtr> camera_info_publishers_; std::map frame_info_logged_; diff --git a/orbbec_camera/src/ob_camera_node.cpp b/orbbec_camera/src/ob_camera_node.cpp index 3428dfa6..5da8d38f 100644 --- a/orbbec_camera/src/ob_camera_node.cpp +++ b/orbbec_camera/src/ob_camera_node.cpp @@ -2566,13 +2566,24 @@ void OBCameraNode::setupPublishers() { if (use_intra_process_) { image_qos_profile = rmw_qos_profile_default; } - if (use_intra_process_) { + const bool is_mjpg_color_stream = + (stream_index == COLOR || stream_index == COLOR_LEFT || stream_index == COLOR_RIGHT) && + format_[stream_index] == OB_FORMAT_MJPG; + if (use_intra_process_ || is_mjpg_color_stream) { image_publishers_[stream_index] = std::make_shared(*node_, topic, image_qos_profile); } else { image_publishers_[stream_index] = std::make_shared(*node_, topic, image_qos_profile); } + if ((stream_index == COLOR || stream_index == COLOR_LEFT || stream_index == COLOR_RIGHT) && + format_[stream_index] == OB_FORMAT_MJPG) { + compressed_image_publishers_[stream_index] = + node_->create_publisher( + topic + "/compressed", + rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(image_qos_profile), + image_qos_profile)); + } topic = name + "/camera_info"; auto camera_info_qos = camera_info_qos_[stream_index]; @@ -3506,8 +3517,8 @@ bool OBCameraNode::decodeColorFrameToBuffer(const std::shared_ptr &fr } bool has_subscriber = false; - if (image_publishers_.count(stream_index) && image_publishers_[stream_index]) { - has_subscriber = image_publishers_[stream_index]->get_subscription_count() > 0; + if (image_publishers_.count(stream_index) && image_publishers_.at(stream_index)) { + has_subscriber = image_publishers_.at(stream_index)->get_subscription_count() > 0; } if (stream_index == COLOR && enable_color_undistortion_ && color_undistortion_publisher_) { has_subscriber = true; @@ -3623,9 +3634,11 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr &frame, CHECK_NOTNULL(image_publishers_[stream_index]); const bool has_raw_image_subscriber = image_publishers_[stream_index]->get_subscription_count() > 0; + const bool has_compressed_image_subscriber = hasCompressedImageSubscriber(stream_index); const bool enable_undistortion_publish = (stream_index == COLOR && enable_color_undistortion_ && color_undistortion_publisher_); - bool has_subscriber = has_raw_image_subscriber || enable_undistortion_publish; + bool has_subscriber = + has_raw_image_subscriber || has_compressed_image_subscriber || enable_undistortion_publish; has_subscriber = has_subscriber || camera_info_publishers_[stream_index]->get_subscription_count() > 0; has_subscriber = @@ -3728,6 +3741,10 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr &frame, camera_info.p.at(3) = -fx * ex.trans[0] / 1000.0 + 0.0; camera_info.p.at(7) = -fy * ex.trans[1] / 1000.0 + 0.0; } + if ((stream_index == COLOR || stream_index == COLOR_LEFT || stream_index == COLOR_RIGHT) && + frame->getFormat() == OB_FORMAT_MJPG && has_compressed_image_subscriber) { + publishCompressedColorImage(frame, stream_index, timestamp, frame_id); + } CHECK_NOTNULL(image_publishers_[stream_index]); if (!has_raw_image_subscriber && !enable_undistortion_publish) { return; @@ -3810,6 +3827,29 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr &frame, } } +bool OBCameraNode::hasCompressedImageSubscriber(const stream_index_pair &stream_index) const { + auto it = compressed_image_publishers_.find(stream_index); + return it != compressed_image_publishers_.end() && it->second && + it->second->get_subscription_count() > 0; +} + +void OBCameraNode::publishCompressedColorImage(const std::shared_ptr &frame, + const stream_index_pair &stream_index, + const rclcpp::Time ×tamp, + const std::string &frame_id) { + auto it = compressed_image_publishers_.find(stream_index); + if (it == compressed_image_publishers_.end() || !it->second) { + return; + } + sensor_msgs::msg::CompressedImage msg; + msg.header.stamp = timestamp; + msg.header.frame_id = frame_id; + msg.format = "jpeg"; + const auto *data = static_cast(frame->getData()); + msg.data.assign(data, data + frame->getDataSize()); + it->second->publish(std::move(msg)); +} + void OBCameraNode::publishMetadata(const std::shared_ptr &frame, const stream_index_pair &stream_index, const std_msgs::msg::Header &header) {