diff --git a/orbbec_camera/include/orbbec_camera/ob_camera_node.h b/orbbec_camera/include/orbbec_camera/ob_camera_node.h index 0e11495e..07907415 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 @@ -343,6 +344,10 @@ class OBCameraNode { void FillImuDataCopy(const IMUData& imu_data, std::deque& imu_msgs); bool setupFormatConvertType(OBFormat format); + 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); @@ -402,6 +407,8 @@ class OBCameraNode { std::map flip_stream_; std::map stream_name_; std::map> image_publishers_; + std::map::SharedPtr> + compressed_image_publishers_; std::map::SharedPtr> camera_info_publishers_; diff --git a/orbbec_camera/src/ob_camera_node.cpp b/orbbec_camera/src/ob_camera_node.cpp index 7ec59bea..4fc523d9 100644 --- a/orbbec_camera/src/ob_camera_node.cpp +++ b/orbbec_camera/src/ob_camera_node.cpp @@ -1436,13 +1436,22 @@ 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 && 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 && 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]; @@ -2076,8 +2085,11 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr &frame, if (frame == nullptr) { return; } - CHECK_NOTNULL(image_publishers_[stream_index]); - bool has_subscriber = image_publishers_[stream_index]->get_subscription_count() > 0; + CHECK_NOTNULL(image_publishers_.at(stream_index)); + const bool has_raw_image_subscriber = + image_publishers_.at(stream_index)->get_subscription_count() > 0; + const bool has_compressed_image_subscriber = hasCompressedImageSubscriber(stream_index); + bool has_subscriber = has_raw_image_subscriber || has_compressed_image_subscriber; has_subscriber = has_subscriber || camera_info_publishers_[stream_index]->get_subscription_count() > 0; has_subscriber = @@ -2210,6 +2222,10 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr &frame, if (isGemini335PID(pid)) { publishMetadata(frame, stream_index, camera_info.header); } + if (stream_index == COLOR && frame->format() == OB_FORMAT_MJPG && + has_compressed_image_subscriber) { + publishCompressedColorImage(frame, stream_index, timestamp, frame_id); + } CHECK_NOTNULL(image_publishers_[stream_index]); if (image_publishers_[stream_index]->get_subscription_count() == 0) { return; @@ -2867,4 +2883,27 @@ orbbec_camera_msgs::msg::IMUInfo OBCameraNode::createIMUInfo( return imu_info; } +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->data()); + msg.data.assign(data, data + frame->dataSize()); + it->second->publish(std::move(msg)); +} + } // namespace orbbec_camera