Update default params

jian-dong · jian-dong · commit 25cf0c767b65 · 2024-05-15T10:13:25.000+08:00
diff --git a/include/orbbec_camera/ob_camera_node.h b/include/orbbec_camera/ob_camera_node.h
@@ -419,7 +419,7 @@ class OBCameraNode {
   // Only for Gemini2 device
   bool enable_hardware_d2d_ = true;
   std::string depth_work_mode_;
-  OBMultiDeviceSyncMode sync_mode_ = OB_MULTI_DEVICE_SYNC_MODE_FREE_RUN;
+  OBMultiDeviceSyncMode sync_mode_ = OB_MULTI_DEVICE_SYNC_MODE_STANDALONE;
   std::string sync_mode_str_;
   int depth_delay_us_ = 0;
   int color_delay_us_ = 0;
@@ -470,19 +470,23 @@ class OBCameraNode {
   bool enable_temporal_filter_ = false;
   bool enable_hole_filling_filter_ = false;
   // filter params
-  int decimation_filter_scale_range_ = 2;
-  int sequence_id_filter_id_ = 1;
-  int threshold_filter_max_ = 16000;
-  int threshold_filter_min_ = 0;
-  int noise_removal_filter_min_diff_ = 8;
-  int noise_removal_filter_max_size_ = 80;
-  float spatial_filter_alpha_ = 0.5;
-  int spatial_filter_diff_threshold_ = 8;
-  int spatial_filter_magnitude_ = 1;
-  int spatial_filter_radius_ = 1;
-  float temporal_filter_diff_threshold_ = 0.1;
-  float temporal_filter_weight_ = 0.4;
-  std::string hole_filling_filter_mode_ = "FILL_TOP";
+  int decimation_filter_scale_range_ = -1;
+  int sequence_id_filter_id_ = -1;
+  int threshold_filter_max_ = -1;
+  int threshold_filter_min_ = -1;
+  int noise_removal_filter_min_diff_ = -1;
+  int noise_removal_filter_max_size_ = -1;
+  float spatial_filter_alpha_ = -1.0;
+  int spatial_filter_diff_threshold_ = -1;
+  int spatial_filter_magnitude_ = -1;
+  int spatial_filter_radius_ = -1;
+  float temporal_filter_diff_threshold_ = -1.0;
+  float temporal_filter_weight_ = -1.0;
+  int hdr_merge_exposure_1_ = -1;
+  int hdr_merge_gain_1_ = -1;
+  int hdr_merge_exposure_2_ = -1;
+  int hdr_merge_gain_2_ = -1;
+  std::string hole_filling_filter_mode_;
   ros::Publisher filter_status_pub_;
   nlohmann::json filter_status_;
   std::shared_ptr<diagnostic_updater::Updater> diagnostic_updater_ = nullptr;
diff --git a/launch/dabai_dcl.launch b/launch/dabai_dcl.launch
@@ -51,7 +51,7 @@
     <arg name="enable_ldp" default="true"/>
     <!-- Configure the path for depth filter file, for example: /config/depthfilter/Gemini2_v1.7.json -->
     <arg name="depth_filter_config" default="" />
-    <arg name="sync_mode" default="free_run"/>
+    <arg name="sync_mode" default="standalone"/>
     <arg name="depth_delay_us" default="0"/>
     <arg name="color_delay_us" default="0"/>
     <arg name="trigger2image_delay_us" default="0"/>
diff --git a/launch/femto_bolt.launch b/launch/femto_bolt.launch
@@ -48,7 +48,7 @@
     <arg name="log_level" default="none"/>
     <arg name="enable_d2c_viewer" default="false"/>
     <arg name="enable_soft_filter" default="true"/>
-    <arg name="sync_mode" default="free_run"/>
+    <arg name="sync_mode" default="standalone"/>
     <arg name="depth_delay_us" default="0"/>
     <arg name="color_delay_us" default="0"/>
     <arg name="trigger2image_delay_us" default="0"/>
diff --git a/launch/femto_mega.launch b/launch/femto_mega.launch
@@ -54,7 +54,7 @@
     <arg name="port" default="0"/>
     <arg name="enable_d2c_viewer" default="false"/>
     <arg name="enable_soft_filter" default="true"/>
-    <arg name="sync_mode" default="free_run"/>
+    <arg name="sync_mode" default="standalone"/>
     <arg name="depth_delay_us" default="0"/>
     <arg name="color_delay_us" default="0"/>
     <arg name="trigger2image_delay_us" default="0"/>
diff --git a/launch/gemini2.launch b/launch/gemini2.launch
@@ -51,7 +51,7 @@
     <arg name="enable_ldp" default="true"/>
     <!-- Configure the path for depth filter file, for example: /config/depthfilter/Gemini2_v1.7.json -->
     <arg name="depth_filter_config" default="" />
-    <arg name="sync_mode" default="free_run"/>
+    <arg name="sync_mode" default="standalone"/>
     <arg name="depth_delay_us" default="0"/>
     <arg name="color_delay_us" default="0"/>
     <arg name="trigger2image_delay_us" default="0"/>
diff --git a/launch/gemini2L.launch b/launch/gemini2L.launch
@@ -52,7 +52,7 @@
     <arg name="enable_soft_filter" default="true"/>
     <arg name="soft_filter_max_diff" default="-1"/>
     <arg name="soft_filter_speckle_size" default="-1"/>
-    <arg name="sync_mode" default="free_run"/>
+    <arg name="sync_mode" default="standalone"/>
     <arg name="depth_delay_us" default="0"/>
     <arg name="color_delay_us" default="0"/>
     <arg name="trigger2image_delay_us" default="0"/>
diff --git a/launch/gemini2XL.launch b/launch/gemini2XL.launch
@@ -58,7 +58,7 @@
     <arg name="enable_soft_filter" default="true"/>
     <arg name="soft_filter_max_diff" default="-1"/>
     <arg name="soft_filter_speckle_size" default="-1"/>
-    <arg name="sync_mode" default="free_run"/>
+    <arg name="sync_mode" default="standalone"/>
     <arg name="depth_delay_us" default="0"/>
     <arg name="color_delay_us" default="0"/>
     <arg name="frame_sync" default="true"/>
diff --git a/launch/gemini2_nodelet.launch b/launch/gemini2_nodelet.launch
@@ -55,7 +55,7 @@
     <arg name="enable_soft_filter" default="true"/>
     <arg name="soft_filter_max_diff" default="-1"/>
     <arg name="soft_filter_speckle_size" default="-1"/>
-    <arg name="sync_mode" default="free_run"/>
+    <arg name="sync_mode" default="standalone"/>
     <arg name="depth_delay_us" default="0"/>
     <arg name="color_delay_us" default="0"/>
     <arg name="trigger2image_delay_us" default="0"/>
diff --git a/launch/gemini_330_series.launch b/launch/gemini_330_series.launch
@@ -15,7 +15,7 @@
     <arg name="diagnostics_frequency" default="1.0"/>
     <arg name="enable_laser" default="true"/>
     <arg name="laser_on_off_mode" default="0"/>
-    <arg name="sync_mode" default="free_run"/>
+    <arg name="sync_mode" default="standalone"/>
 
 
     <!-- Color camera parameters -->
@@ -58,7 +58,7 @@
     <arg name="enable_ir_long_exposure" default="false"/>
 
     <!-- Point cloud parameters -->
-    <arg name="enable_point_cloud" default="true"/>
+    <arg name="enable_point_cloud" default="false"/>
     <arg name="enable_colored_point_cloud" default="true"/>
     <arg name="ordered_pc" default="false"/>
     <!-- Gemini 335/335L only support SW align mode, Please DO NOT change it -->
@@ -74,17 +74,21 @@
     <arg name="enable_temporal_filter" default="false"/>
     <arg name="enable_hole_filling_filter" default="false"/>
     <arg name="sequence_id_filter_id" default="1"/>
-    <arg name="threshold_filter_max" default="16000"/>
-    <arg name="threshold_filter_min" default="0"/>
+    <arg name="threshold_filter_max" default="-1"/>
+    <arg name="threshold_filter_min" default="-1"/>
     <arg name="noise_removal_filter_min_diff" default="256"/>
     <arg name="noise_removal_filter_max_size" default="80"/>
-    <arg name="spatial_filter_alpha" default="0.5"/>
-    <arg name="spatial_filter_diff_threshold" default="8"/>
-    <arg name="spatial_filter_magnitude" default="1"/>
-    <arg name="spatial_filter_radius" default="1"/>
-    <arg name="temporal_filter_diff_threshold" default="0.1"/>
-    <arg name="temporal_filter_weight" default="0.4"/>
-    <arg name="hole_filling_filter_mode" default="FILL_TOP"/>
+    <arg name="spatial_filter_alpha" default="-1.0"/>
+    <arg name="spatial_filter_diff_threshold" default="-1"/>
+    <arg name="spatial_filter_magnitude" default="-1"/>
+    <arg name="spatial_filter_radius" default="-1"/>
+    <arg name="temporal_filter_diff_threshold" default="-1.-"/>
+    <arg name="temporal_filter_weight" default="-1.0"/>
+    <arg name="hole_filling_filter_mode" default=""/>
+    <arg name="hdr_merge_exposure_1" default="-1"/>
+    <arg name="hdr_merge_gain_1" default="-1"/>
+    <arg name="hdr_merge_exposure_2" default="-1"/>
+    <arg name="hdr_merge_gain_2" default="-1"/>
     <!-- IMU parameters -->
     <arg name="enable_sync_output_accel_gyro" default="true"/>
     <arg name="enable_accel" default="true"/>
@@ -175,6 +179,10 @@
             <param name="temporal_filter_diff_threshold" value="$(arg temporal_filter_diff_threshold)"/>
             <param name="temporal_filter_weight" value="$(arg temporal_filter_weight)"/>
             <param name="hole_filling_filter_mode" value="$(arg hole_filling_filter_mode)"/>
+            <param name="hdr_merge_exposure_1" value="$(arg hdr_merge_exposure_1)"/>
+            <param name="hdr_merge_gain_1" value="$(arg hdr_merge_gain_1)"/>
+            <param name="hdr_merge_exposure_2" value="$(arg hdr_merge_exposure_2)"/>
+            <param name="hdr_merge_gain_2" value="$(arg hdr_merge_gain_2)"/>
 
             <param name="enable_sync_output_accel_gyro" value="$(arg enable_sync_output_accel_gyro)"/>
             <param name="enable_accel" value="$(arg enable_accel)"/>
diff --git a/src/ob_camera_node.cpp b/src/ob_camera_node.cpp
@@ -141,7 +141,7 @@ void OBCameraNode::getParameters() {
   enable_ir_auto_exposure_ = nh_private_.param<bool>("enable_ir_auto_exposure", true);
   ir_exposure_ = nh_private_.param<int>("ir_exposure_", -1);
   enable_ir_long_exposure_ = nh_private_.param<bool>("enable_ir_long_exposure", false);
-  sync_mode_str_ = nh_private_.param<std::string>("sync_mode", "free_run");
+  sync_mode_str_ = nh_private_.param<std::string>("sync_mode", "standalone");
   std::transform(sync_mode_str_.begin(), sync_mode_str_.end(), sync_mode_str_.begin(), ::toupper);
   sync_mode_ = OBSyncModeFromString(sync_mode_str_);
   depth_delay_us_ = nh_private_.param<int>("depth_delay_us", 0);
@@ -192,23 +192,27 @@ void OBCameraNode::getParameters() {
   enable_sequenced_filter_ = nh_private_.param<bool>("enable_sequenced_filter", false);
   enable_threshold_filter_ = nh_private_.param<bool>("enable_threshold_filter", false);
   enable_noise_removal_filter_ = nh_private_.param<bool>("enable_noise_removal_filter", true);
-  enable_spatial_filter_ = nh_private_.param<bool>("enable_spatial_filter", true);
+  enable_spatial_filter_ = nh_private_.param<bool>("enable_spatial_filter", false);
   enable_temporal_filter_ = nh_private_.param<bool>("enable_temporal_filter", false);
   enable_hole_filling_filter_ = nh_private_.param<bool>("enable_hole_filling_filter", false);
-  decimation_filter_scale_range_ = nh_private_.param<int>("decimation_filter_scale_range", 2);
-  sequence_id_filter_id_ = nh_private_.param<int>("sequence_id_filter_id", 1);
-  threshold_filter_max_ = nh_private_.param<int>("threshold_filter_max", 16000);
-  threshold_filter_min_ = nh_private_.param<int>("threshold_filter_min", 0);
-  noise_removal_filter_min_diff_ = nh_private_.param<int>("noise_removal_filter_min_diff", 8);
+  decimation_filter_scale_range_ = nh_private_.param<int>("decimation_filter_scale_range", -1);
+  sequence_id_filter_id_ = nh_private_.param<int>("sequence_id_filter_id", -1);
+  threshold_filter_max_ = nh_private_.param<int>("threshold_filter_max", -1);
+  threshold_filter_min_ = nh_private_.param<int>("threshold_filter_min", -1);
+  noise_removal_filter_min_diff_ = nh_private_.param<int>("noise_removal_filter_min_diff", 250);
   noise_removal_filter_max_size_ = nh_private_.param<int>("noise_removal_filter_max_size", 80);
-  spatial_filter_alpha_ = nh_private_.param<float>("spatial_filter_alpha", 0.5);
-  spatial_filter_diff_threshold_ = nh_private_.param<int>("spatial_filter_diff_threshold", 8);
-  spatial_filter_magnitude_ = nh_private_.param<int>("spatial_filter_magnitude", 1);
-  spatial_filter_radius_ = nh_private_.param<int>("spatial_filter_radius", 1);
-  temporal_filter_diff_threshold_ = nh_private_.param<float>("temporal_filter_diff_threshold", 0.1);
-  temporal_filter_weight_ = nh_private_.param<float>("temporal_filter_weight", 0.4);
-  hole_filling_filter_mode_ =
-      nh_private_.param<std::string>("hole_filling_filter_mode", "FILL_TOP");
+  spatial_filter_alpha_ = nh_private_.param<float>("spatial_filter_alpha", -1.0);
+  spatial_filter_diff_threshold_ = nh_private_.param<int>("spatial_filter_diff_threshold", -1);
+  spatial_filter_magnitude_ = nh_private_.param<int>("spatial_filter_magnitude", -1);
+  spatial_filter_radius_ = nh_private_.param<int>("spatial_filter_radius", -1);
+  temporal_filter_diff_threshold_ =
+      nh_private_.param<float>("temporal_filter_diff_threshold", -1.0);
+  temporal_filter_weight_ = nh_private_.param<float>("temporal_filter_weight", -1.0);
+  hole_filling_filter_mode_ = nh_private_.param<std::string>("hole_filling_filter_mode", "");
+  hdr_merge_exposure_1_ = nh_private_.param<int>("hdr_merge_exposure_1", -1);
+  hdr_merge_gain_1_ = nh_private_.param<int>("hdr_merge_gain_1", -1);
+  hdr_merge_exposure_2_ = nh_private_.param<int>("hdr_merge_exposure_2", -1);
+  hdr_merge_gain_2_ = nh_private_.param<int>("hdr_merge_gain_2", -1);
   diagnostics_frequency_ = nh_private_.param<double>("diagnostics_frequency", 1.0);
   enable_laser_ = nh_private_.param<bool>("enable_laser", true);
   laser_on_off_mode_ = nh_private_.param<int>("laser_on_off_mode", 0);
diff --git a/src/ros_setup.cpp b/src/ros_setup.cpp
@@ -125,6 +125,19 @@ void OBCameraNode::setupDevices() {
     if (!device_preset_.empty()) {
       device_->loadPreset(device_preset_.c_str());
     }
+    if (sync_mode_ != OB_MULTI_DEVICE_SYNC_MODE_FREE_RUN) {
+      auto sync_config = device_->getMultiDeviceSyncConfig();
+      sync_config.syncMode = sync_mode_;
+      sync_config.depthDelayUs = depth_delay_us_;
+      sync_config.colorDelayUs = color_delay_us_;
+      sync_config.trigger2ImageDelayUs = trigger2image_delay_us_;
+      sync_config.triggerOutDelayUs = trigger_out_delay_us_;
+      sync_config.triggerOutEnable = trigger_out_enabled_;
+      device_->setMultiDeviceSyncConfig(sync_config);
+      if (device_->isPropertySupported(OB_PROP_SYNC_SIGNAL_TRIGGER_OUT_BOOL,
+                                       OB_PERMISSION_READ_WRITE)) {
+      }
+    }
     auto depth_sensor = device_->getSensor(OB_SENSOR_DEPTH);
     device_->setBoolProperty(OB_PROP_DEPTH_AUTO_EXPOSURE_BOOL, enable_ir_auto_exposure_);
     device_->setBoolProperty(OB_PROP_COLOR_AUTO_EXPOSURE_BOOL, enable_color_auto_exposure_);
@@ -190,37 +203,65 @@ void OBCameraNode::setupDevices() {
       }
       if (filter_name == "DecimationFilter" && enable_decimation_filter_) {
         auto decimation_filter = filter->as<ob::DecimationFilter>();
-        decimation_filter->setScaleValue(decimation_filter_scale_range_);
+        if (decimation_filter_scale_range_ != -1) {
+          decimation_filter->setScaleValue(decimation_filter_scale_range_);
+        }
       } else if (filter_name == "ThresholdFilter" && enable_threshold_filter_) {
         auto threshold_filter = filter->as<ob::ThresholdFilter>();
-        threshold_filter->setValueRange(threshold_filter_min_, threshold_filter_max_);
+        if (threshold_filter_min_ != -1 && threshold_filter_max_ != -1) {
+          threshold_filter->setValueRange(threshold_filter_min_, threshold_filter_max_);
+        }
       } else if (filter_name == "SpatialAdvancedFilter" && enable_spatial_filter_) {
         auto spatial_filter = filter->as<ob::SpatialAdvancedFilter>();
         OBSpatialAdvancedFilterParams params{};
-        params.alpha = spatial_filter_alpha_;
-        params.magnitude = spatial_filter_magnitude_;
-        params.radius = spatial_filter_radius_;
-        params.disp_diff = spatial_filter_diff_threshold_;
-        spatial_filter->setFilterParams(params);
+        if (spatial_filter_alpha_ != -1.0 && spatial_filter_magnitude_ != -1 &&
+            spatial_filter_radius_ != -1 && spatial_filter_diff_threshold_ != -1) {
+          params.alpha = spatial_filter_alpha_;
+          params.magnitude = spatial_filter_magnitude_;
+          params.radius = spatial_filter_radius_;
+          params.disp_diff = spatial_filter_diff_threshold_;
+          spatial_filter->setFilterParams(params);
+        }
       } else if (filter_name == "TemporalFilter" && enable_temporal_filter_) {
         auto temporal_filter = filter->as<ob::TemporalFilter>();
-        temporal_filter->setDiffScale(temporal_filter_diff_threshold_);
-        temporal_filter->setWeight(temporal_filter_weight_);
-      } else if (filter_name == "HoleFillingFilter") {
+        if (temporal_filter_diff_threshold_ != -1 && temporal_filter_weight_ != -1) {
+          temporal_filter->setDiffScale(temporal_filter_diff_threshold_);
+          temporal_filter->setWeight(temporal_filter_weight_);
+        }
+      } else if (filter_name == "HoleFillingFilter" && enable_hole_filling_filter_ &&
+                 !hole_filling_filter_mode_.empty()) {
         auto hole_filling_filter = filter->as<ob::HoleFillingFilter>();
         OBHoleFillingMode hole_filling_mode = holeFillingModeFromString(hole_filling_filter_mode_);
         hole_filling_filter->setFilterMode(hole_filling_mode);
       } else if (filter_name == "SequenceIdFilter" && enable_sequenced_filter_) {
         auto sequenced_filter = filter->as<ob::SequenceIdFilter>();
-        sequenced_filter->selectSequenceId(sequence_id_filter_id_);
+        if (sequence_id_filter_id_ != -1) {
+          sequenced_filter->selectSequenceId(sequence_id_filter_id_);
+        }
       } else if (filter_name == "NoiseRemovalFilter" && enable_noise_removal_filter_) {
         auto noise_removal_filter = filter->as<ob::NoiseRemovalFilter>();
         OBNoiseRemovalFilterParams params{};
-        params.disp_diff = noise_removal_filter_min_diff_;
-        params.max_size = noise_removal_filter_max_size_;
-        noise_removal_filter->setFilterParams(params);
-      } else if (filter_name == "HDRMerge") {
-        // do nothing
+        if (noise_removal_filter_min_diff_ != -1 && noise_removal_filter_max_size_ != -1) {
+          params.disp_diff = noise_removal_filter_min_diff_;
+          params.max_size = noise_removal_filter_max_size_;
+          noise_removal_filter->setFilterParams(params);
+        }
+      } else if (filter_name == "HDRMerge" && enable_hdr_merge_) {
+        auto hdr_merge_filter = filter->as<ob::HdrMerge>();
+        OBHdrConfig hdr_config{};
+        if (hdr_merge_exposure_1_ != -1 && hdr_merge_exposure_2_ != -1 && hdr_merge_gain_1_ != -1 &&
+            hdr_merge_gain_2_ != -1) {
+          hdr_config.exposure_1 = hdr_merge_exposure_1_;
+          hdr_config.exposure_2 = hdr_merge_exposure_2_;
+          hdr_config.gain_1 = hdr_merge_gain_1_;
+          hdr_config.gain_2 = hdr_merge_gain_2_;
+          ROS_INFO_STREAM("set HDRMerge exposure_1 to " << hdr_merge_exposure_1_);
+          ROS_INFO_STREAM("set HDRMerge exposure_2 to " << hdr_merge_exposure_2_);
+          ROS_INFO_STREAM("set HDRMerge gain_1 to " << hdr_merge_gain_1_);
+          ROS_INFO_STREAM("set HDRMerge gain_2 to " << hdr_merge_gain_2_);
+          device_->setStructuredData(OB_STRUCT_DEPTH_HDR_CONFIG, &hdr_config, sizeof(OBHdrConfig));
+        }
+
       } else {
         ROS_INFO_STREAM("Skip setting " << filter_name);
       }
@@ -236,7 +277,6 @@ void OBCameraNode::setupDevices() {
     if (device_->isPropertySupported(OB_PROP_IR_LONG_EXPOSURE_BOOL, OB_PERMISSION_WRITE)) {
       device_->setBoolProperty(OB_PROP_IR_LONG_EXPOSURE_BOOL, enable_ir_long_exposure_);
     }
-
   } catch (const ob::Error& e) {
     ROS_ERROR_STREAM("Failed to setup devices: " << e.getMessage());
   } catch (const std::exception& e) {
