add enable hardware d2d

orbbec_camera/include/orbbec_camera/constants.h

@@ -105,7 +105,7 @@ const int32_t OPENNI_START_PID = 0x0601;
 const int32_t OPENNI_END_PID = 0x06FF;
 const int32_t ASTRA_MINI_PID = 0x0404;
 const int32_t ASTRA_MINI_S_PID = 0x0407;
-
+const int GEMINI2_PID = 0x0670;
 const std::string DEFAULT_SEM_NAME = "orbbec_device_sem";
 const key_t DEFAULT_SEM_KEY = 0x0401;
 

orbbec_camera/include/orbbec_camera/ob_camera_node.h

@@ -339,5 +339,6 @@ class OBCameraNode {
   std::atomic_bool save_colored_point_cloud_{false};
   rclcpp::Service<std_srvs::srv::Empty>::SharedPtr save_images_srv_;
   rclcpp::Service<std_srvs::srv::Empty>::SharedPtr save_point_cloud_srv_;
+  bool enable_hardware_d2d_ = true;
 };
 }  // namespace orbbec_camera

orbbec_camera/include/orbbec_camera/ob_camera_node_driver.h

@@ -61,7 +61,7 @@ class OBCameraNodeDriver : public rclcpp::Node {
 
   static OBLogSeverity obLogSeverityFromString(const std::string_view& log_level);
 
-  void checkConnectTimer() const;
+  void checkConnectTimer();
 
   void queryDevice();
 

orbbec_camera/launch/gemini2.launch.xml

@@ -41,6 +41,7 @@
     <arg name="color_info_url" default=""/>
     <arg name="log_level" default="none"/>
     <arg name="enable_publish_extrinsic" default="false"/>
+    <arg name="enable_hardware_d2d" default="true"/>
     <group>
         <push-ros-namespace namespace="$(var camera_name)"/>
         <node name="camera" pkg="orbbec_camera" exec="orbbec_camera_node" output="screen">
@@ -84,6 +85,7 @@
             <param name="color_info_url" value="$(var color_info_url)"/>
             <param name="log_level" value="$(var log_level)"/>
             <param name="enable_publish_extrinsic" value="$(var enable_publish_extrinsic)"/>
+            <param name="enable_hardware_d2d" value="$(var enable_hardware_d2d)"/>
             <remap from="/$(var camera_name)/depth/color/points" to="/$(var camera_name)/depth_registered/points"/>
         </node>
     </group>

orbbec_camera/src/ob_camera_node.cpp

@@ -96,6 +96,10 @@ void OBCameraNode::setupDevices() {
       enable_stream_[stream_index] = false;
     }
   }
+  auto info = device_->getDeviceInfo();
+  if(enable_hardware_d2d_ && info->pid() == GEMINI2_PID){
+    device_->setBoolProperty(OB_PROP_DISPARITY_TO_DEPTH_BOOL, true);
+  }
 }
 
 void OBCameraNode::setupProfiles() {
@@ -259,6 +263,7 @@ void OBCameraNode::getParameters() {
   setAndGetNodeParameter<std::string>(point_cloud_qos_, "point_cloud_qos", "default");
   setAndGetNodeParameter(enable_publish_extrinsic_, "enable_publish_extrinsic", false);
   setAndGetNodeParameter(enable_d2c_viewer_, "enable_d2c_viewer", false);
+  setAndGetNodeParameter(enable_hardware_d2d_, "enable_hardware_d2d", true);
   if (enable_colored_point_cloud_) {
     depth_registration_ = true;
   }

orbbec_camera/src/ob_camera_node_driver.cpp

@@ -128,11 +128,13 @@ OBLogSeverity OBCameraNodeDriver::obLogSeverityFromString(const std::string_view
   }
 }
 
-void OBCameraNodeDriver::checkConnectTimer() const {
+void OBCameraNodeDriver::checkConnectTimer() {
   if (!device_connected_.load()) {
     RCLCPP_ERROR_STREAM(logger_,
                         "checkConnectTimer: device " << serial_number_ << " not connected");
     return;
+  } else if (!ob_camera_node_) {
+    device_connected_.store(false);
   }
 }