chore: add gemini_intra_process_demo

orbbec_camera/include/orbbec_camera/image_publisher.h

@@ -0,0 +1,52 @@
+// Copyright 2023 Intel Corporation. All Rights Reserved.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#pragma once
+
+#include <rclcpp/rclcpp.hpp>
+#include <sensor_msgs/msg/image.hpp>
+
+#include <image_transport/image_transport.hpp>
+namespace orbbec_camera {
+class image_publisher {
+ public:
+  virtual void publish(sensor_msgs::msg::Image::UniquePtr image_ptr) = 0;
+  virtual size_t get_subscription_count() const = 0;
+  virtual ~image_publisher() = default;
+};  // namespace image_publisher
+
+// Native RCL implementation of an image publisher (needed for intra-process communication)
+class image_rcl_publisher : public image_publisher {
+ public:
+  image_rcl_publisher(rclcpp::Node& node, const std::string& topic_name,
+                      const rmw_qos_profile_t& qos);
+  void publish(sensor_msgs::msg::Image::UniquePtr image_ptr) override;
+  size_t get_subscription_count() const override;
+
+ private:
+  rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr image_publisher_impl;
+};
+
+// image_transport implementation of an image publisher (adds a compressed image topic)
+class image_transport_publisher : public image_publisher {
+ public:
+  image_transport_publisher(rclcpp::Node& node, const std::string& topic_name,
+                            const rmw_qos_profile_t& qos);
+  void publish(sensor_msgs::msg::Image::UniquePtr image_ptr) override;
+  size_t get_subscription_count() const override;
+
+ private:
+  std::shared_ptr<image_transport::Publisher> image_publisher_impl;
+};
+}  // namespace orbbec_camera

orbbec_camera/include/orbbec_camera/ob_camera_node.h

@@ -60,6 +60,7 @@
 #include "orbbec_camera/dynamic_params.h"
 #include "orbbec_camera/d2c_viewer.h"
 #include "magic_enum/magic_enum.hpp"
+#include "orbbec_camera/image_publisher.h"
 #include "jpeg_decoder.h"
 #include <std_msgs/msg/string.hpp>
 
@@ -114,7 +115,7 @@ const stream_index_pair INFRA2{OB_STREAM_IR_RIGHT, 0};
 const stream_index_pair GYRO{OB_STREAM_GYRO, 0};
 const stream_index_pair ACCEL{OB_STREAM_ACCEL, 0};
 
-const std::vector<stream_index_pair> IMAGE_STREAMS = {DEPTH, INFRA0, COLOR, INFRA1, INFRA2};
+const std::vector<stream_index_pair> IMAGE_STREAMS = {COLOR,DEPTH, INFRA0, INFRA1, INFRA2};
 
 const std::vector<stream_index_pair> HID_STREAMS = {GYRO, ACCEL};
 
@@ -131,7 +132,7 @@ const std::map<OBStreamType, OBFrameType> STREAM_TYPE_TO_FRAME_TYPE = {
 class OBCameraNode {
  public:
   OBCameraNode(rclcpp::Node* node, std::shared_ptr<ob::Device> device,
-               std::shared_ptr<Parameters> parameters);
+               std::shared_ptr<Parameters> parameters, bool use_intra_process = false);
 
   template <class T>
   void setAndGetNodeParameter(
@@ -315,7 +316,7 @@ class OBCameraNode {
   void onNewColorFrameCallback();
 
   void saveImageToFile(const stream_index_pair& stream_index, const cv::Mat& image,
-                       const sensor_msgs::msg::Image::SharedPtr& image_msg);
+                       const sensor_msgs::msg::Image& image_msg);
 
   void onNewIMUFrameSyncOutputCallback(const std::shared_ptr<ob::Frame>& accelframe,
                                        const std::shared_ptr<ob::Frame>& gryoframe);
@@ -391,7 +392,7 @@ class OBCameraNode {
   std::map<stream_index_pair, bool> enable_stream_;
   std::map<stream_index_pair, bool> flip_stream_;
   std::map<stream_index_pair, std::string> stream_name_;
-  std::map<stream_index_pair, image_transport::Publisher> image_publishers_;
+  std::map<stream_index_pair, std::shared_ptr<image_publisher>> image_publishers_;
   std::map<stream_index_pair, rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr>
       camera_info_publishers_;
 
@@ -564,7 +565,8 @@ class OBCameraNode {
   std::chrono::milliseconds software_trigger_period_{33};
   bool enable_heartbeat_ = false;
   bool enable_color_undistortion_ = false;
-  image_transport::Publisher color_undistortion_publisher_;
+  std::shared_ptr<image_publisher> color_undistortion_publisher_;
   bool has_first_color_frame_ = false;
+  bool use_intra_process_ = false;
 };
 }  // namespace orbbec_camera

orbbec_camera/include/orbbec_camera/ob_camera_node_driver.h

@@ -69,6 +69,7 @@ class OBCameraNodeDriver : public rclcpp::Node {
                             std::shared_ptr<std_srvs::srv::Empty::Response> response);
 
  private:
+ const rclcpp::NodeOptions node_options_;
   std::string config_path_;
   std::unique_ptr<ob::Context> ctx_ = nullptr;
   rclcpp::Logger logger_;