add sync mode

orbbec_camera/include/orbbec_camera/ob_camera_node.h

@@ -339,6 +339,18 @@ 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_soft_filter_ = true;
+  bool enable_color_auto_exposure_ = true;
+  bool enable_ir_auto_exposure_ = true;
+  // Only for Gemini2 device
   bool enable_hardware_d2d_ = true;
+  std::string depth_work_mode_;
+  OBSyncMode sync_mode_ = OBSyncMode::OB_SYNC_MODE_CLOSE;
+  std::string sync_mode_str_;
+  int ir_trigger_signal_in_delay_ = 0;
+  int rgb_trigger_signal_in_delay_ = 0;
+  int device_trigger_signal_out_delay_ = 0;
+  std::string depth_precision_str_;
+  OB_DEPTH_PRECISION_LEVEL depth_precision_ = OB_PRECISION_0MM8;
 };
 }  // namespace orbbec_camera

orbbec_camera/include/orbbec_camera/utils.h

@@ -50,4 +50,9 @@ rmw_qos_profile_t getRMWQosProfileFromString(const std::string& str_qos);
 
 bool isOpenNIDevice(int pid);
 
+OB_DEPTH_PRECISION_LEVEL depthPrecisionLevelFromString(
+    const std::string& depth_precision_level_str);
+
+OBSyncMode OBSyncModeFromString(const std::string& mode);
+
 }  // namespace orbbec_camera

orbbec_camera/src/ob_camera_node.cpp

@@ -97,9 +97,36 @@ void OBCameraNode::setupDevices() {
     }
   }
   auto info = device_->getDeviceInfo();
-  if(enable_hardware_d2d_ && info->pid() == GEMINI2_PID){
+  if (enable_hardware_d2d_ && info->pid() == GEMINI2_PID) {
     device_->setBoolProperty(OB_PROP_DISPARITY_TO_DEPTH_BOOL, true);
   }
+  try {
+    if (!depth_work_mode_.empty()) {
+      device_->switchDepthWorkMode(depth_work_mode_.c_str());
+    }
+    if (sync_mode_ != OB_SYNC_MODE_CLOSE) {
+      OBDeviceSyncConfig sync_config;
+      sync_config.syncMode = sync_mode_;
+      sync_config.irTriggerSignalInDelay = ir_trigger_signal_in_delay_;
+      sync_config.rgbTriggerSignalInDelay = rgb_trigger_signal_in_delay_;
+      sync_config.deviceTriggerSignalOutDelay = device_trigger_signal_out_delay_;
+      device_->setSyncConfig(sync_config);
+    }
+    if (info->pid() == GEMINI2_PID) {
+      auto default_precision_level = device_->getIntProperty(OB_PROP_DEPTH_PRECISION_LEVEL_INT);
+      if (default_precision_level != depth_precision_) {
+        device_->setIntProperty(OB_PROP_DEPTH_PRECISION_LEVEL_INT, depth_precision_);
+      }
+    }
+
+    device_->setBoolProperty(OB_PROP_DEPTH_SOFT_FILTER_BOOL, enable_soft_filter_);
+    device_->setBoolProperty(OB_PROP_COLOR_AUTO_EXPOSURE_BOOL, enable_color_auto_exposure_);
+    device_->setBoolProperty(OB_PROP_IR_AUTO_EXPOSURE_BOOL, enable_ir_auto_exposure_);
+  } catch (const ob::Error& e) {
+    RCLCPP_ERROR_STREAM(logger_, "Failed to setup devices: " << e.getMessage());
+  } catch (const std::exception& e) {
+    RCLCPP_ERROR_STREAM(logger_, "Failed to setup devices: " << e.what());
+  }
 }
 
 void OBCameraNode::setupProfiles() {
@@ -264,6 +291,18 @@ void OBCameraNode::getParameters() {
   setAndGetNodeParameter(enable_publish_extrinsic_, "enable_publish_extrinsic", false);
   setAndGetNodeParameter(enable_d2c_viewer_, "enable_d2c_viewer", false);
   setAndGetNodeParameter(enable_hardware_d2d_, "enable_hardware_d2d", true);
+  setAndGetNodeParameter(enable_soft_filter_, "enable_soft_filter", true);
+  setAndGetNodeParameter(enable_color_auto_exposure_, "enable_color_auto_exposure", true);
+  setAndGetNodeParameter(enable_ir_auto_exposure_, "enable_ir_auto_exposure", true);
+  setAndGetNodeParameter<std::string>(depth_work_mode_, "depth_work_mode", "");
+  setAndGetNodeParameter<std::string>(sync_mode_str_, "sync_mode", "close");
+  setAndGetNodeParameter(ir_trigger_signal_in_delay_, "ir_trigger_signal_in_delay", 0);
+  setAndGetNodeParameter(rgb_trigger_signal_in_delay_, "rgb_trigger_signal_in_delay", 0);
+  setAndGetNodeParameter(device_trigger_signal_out_delay_, "device_trigger_signal_out_delay", 0);
+  setAndGetNodeParameter<std::string>(depth_precision_str_, "depth_precision", "0.8mm");
+  std::transform(sync_mode_str_.begin(), sync_mode_str_.end(), sync_mode_str_.begin(), ::toupper);
+  sync_mode_ = OBSyncModeFromString(sync_mode_str_);
+  depth_precision_ = depthPrecisionLevelFromString(depth_precision_str_);
   if (enable_colored_point_cloud_) {
     depth_registration_ = true;
   }

orbbec_camera/src/utils.cpp

@@ -264,4 +264,42 @@ bool isOpenNIDevice(int pid) {
   return false;
 }
 
+OB_DEPTH_PRECISION_LEVEL depthPrecisionLevelFromString(
+    const std::string &depth_precision_level_str) {
+  if (depth_precision_level_str == "1mm") {
+    return OB_PRECISION_1MM;
+  } else if (depth_precision_level_str == "0.8mm") {
+    return OB_PRECISION_0MM8;
+  } else if (depth_precision_level_str == "0.4mm") {
+    return OB_PRECISION_0MM4;
+  } else if (depth_precision_level_str == "0.2mm") {
+    return OB_PRECISION_0MM2;
+  } else if (depth_precision_level_str == "0.1mm") {
+    return OB_PRECISION_0MM1;
+  } else {
+    return OB_PRECISION_0MM8;
+  }
+}
+
+OBSyncMode OBSyncModeFromString(const std::string &mode) {
+  if (mode == "CLOSE") {
+    return OBSyncMode::OB_SYNC_MODE_CLOSE;
+  } else if (mode == "STANDALONE") {
+    return OBSyncMode::OB_SYNC_MODE_STANDALONE;
+  } else if (mode == "SECONDARY") {
+    return OBSyncMode::OB_SYNC_MODE_SECONDARY;
+  } else if (mode == "PRIMARY_MCU_TRIGGER") {
+    return OBSyncMode::OB_SYNC_MODE_PRIMARY_MCU_TRIGGER;
+  } else if (mode == "PRIMARY_IR_TRIGGER") {
+    return OBSyncMode::OB_SYNC_MODE_PRIMARY_IR_TRIGGER;
+  } else if (mode == "PRIMARY_SOFT_TRIGGER") {
+    return OBSyncMode::OB_SYNC_MODE_PRIMARY_SOFT_TRIGGER;
+  } else if (mode == "SECONDARY_SOFT_TRIGGER") {
+    return OBSyncMode::OB_SYNC_MODE_SECONDARY_SOFT_TRIGGER;
+  } else {
+    RCLCPP_ERROR_STREAM(rclcpp::get_logger("utils"), "Unknown OBSyncMode: " << mode);
+    return OBSyncMode::OB_SYNC_MODE_CLOSE;
+  }
+}
+
 }  // namespace orbbec_camera