Alignment Closed Source

orbbec_camera/include/orbbec_camera/constants.h

@@ -21,9 +21,9 @@
 
 #define THREAD_NUM 4
 
-#define OB_ROS_MAJOR_VERSION 2
-#define OB_ROS_MINOR_VERSION 0
-#define OB_ROS_PATCH_VERSION 1
+#define OB_ROS_MAJOR_VERSION 1
+#define OB_ROS_MINOR_VERSION 5
+#define OB_ROS_PATCH_VERSION 11
 
 #ifndef STRINGIFY
 #define STRINGIFY(arg) #arg
@@ -128,5 +128,6 @@ const int32_t GEMINI_335LG_PID = 0x080B;  // Gemini 336Lg
 const int32_t GEMINI_336LG_PID = 0x080D;
 const int32_t GEMINI_335LE_PID = 0x080E;  // Gemini 335Le
 const int32_t GEMINI_336LE_PID = 0x0810;  // Gemini 335Le
+const int32_t DABAI_MAX_PID = 0x069a;     // dabai max
 
 }  // namespace orbbec_camera

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_;
 
@@ -462,8 +463,12 @@ class OBCameraNode {
   int color_exposure_ = -1;
   int color_gain_ = -1;
   int color_white_balance_ = -1;
+  int color_ae_max_exposure_ = -1;
+  int color_brightness_ = -1;
   int ir_exposure_ = -1;
   int ir_gain_ = -1;
+  int ir_ae_max_exposure_ = -1;
+  int ir_brightness_ = -1;
   int soft_filter_max_diff_ = -1;
   int soft_filter_speckle_size_ = -1;
   bool enable_frame_sync_ = false;
@@ -564,7 +569,10 @@ 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;
+  std::string cloud_frame_id_;
   std::vector<std::shared_ptr<ob::Filter>> filter_list_;
 };
 }  // namespace orbbec_camera

orbbec_camera/include/orbbec_camera/ob_camera_node_driver.h

@@ -70,6 +70,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_;
@@ -105,5 +106,6 @@ class OBCameraNodeDriver : public rclcpp::Node {
   rclcpp::Service<std_srvs::srv::Empty>::SharedPtr reboot_device_srv_ = nullptr;
   std::chrono::time_point<std::chrono::system_clock> start_time_;
   std::string extension_path_;
+  static backward::SignalHandling sh;  // for stack trace
 };
 }  // namespace orbbec_camera