Merge branch 'feature/publish_raw_color_mjpg' into feature/main-timestamp-stat

orbbec_camera/include/orbbec_camera/ob_camera_node.h

@@ -39,6 +39,7 @@
 #include <diagnostic_updater/diagnostic_updater.hpp>
 
 #include <sensor_msgs/msg/camera_info.hpp>
+#include <sensor_msgs/msg/compressed_image.hpp>
 #include <camera_info_manager/camera_info_manager.hpp>
 
 #include <image_publisher/image_publisher.hpp>
@@ -343,6 +344,10 @@ class OBCameraNode {
   void FillImuDataCopy(const IMUData& imu_data, std::deque<sensor_msgs::msg::Imu>& imu_msgs);
 
   bool setupFormatConvertType(OBFormat format);
+  bool hasCompressedImageSubscriber(const stream_index_pair& stream_index) const;
+  void publishCompressedColorImage(const std::shared_ptr<ob::Frame>& 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<stream_index_pair, bool> flip_stream_;
   std::map<stream_index_pair, std::string> stream_name_;
   std::map<stream_index_pair, std::shared_ptr<image_publisher>> image_publishers_;
+  std::map<stream_index_pair, rclcpp::Publisher<sensor_msgs::msg::CompressedImage>::SharedPtr>
+      compressed_image_publishers_;
   std::map<stream_index_pair, rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr>
       camera_info_publishers_;
 

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<image_rcl_publisher>(*node_, topic, image_qos_profile);
     } else {
       image_publishers_[stream_index] =
           std::make_shared<image_transport_publisher>(*node_, topic, image_qos_profile);
     }
+    if (stream_index == COLOR && format_[stream_index] == OB_FORMAT_MJPG) {
+      compressed_image_publishers_[stream_index] =
+          node_->create_publisher<sensor_msgs::msg::CompressedImage>(
+              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<ob::Frame> &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<ob::Frame> &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<ob::Frame> &frame,
+                                               const stream_index_pair &stream_index,
+                                               const rclcpp::Time &timestamp,
+                                               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<const uint8_t *>(frame->data());
+  msg.data.assign(data, data + frame->dataSize());
+  it->second->publish(std::move(msg));
+}
+
 }  // namespace orbbec_camera