Add more debug log

Author: jian-dong

README.MD

@@ -241,6 +241,7 @@ The following launch parameters are available:
   recognized as USB 2.0.
   It is recommended to set this parameter to `false` when using a USB 2.0 connection to avoid unnecessary resets.
 
+**IMPORTANT**: *Please carefully read the instructions regarding software filtering settings at [this link](https://www.orbbec.com/docs/g330-use-depth-post-processing-blocks/). If you are uncertain, do not modify these settings.*
 ## Depth work mode switch:
 
 - Before starting the camera, depth work mode (depth_work_mode) can be configured for the corresponding xxx.launch

include/orbbec_camera/utils.h

@@ -57,6 +57,10 @@ bool isOpenNIDevice(int pid);
 
 OBMultiDeviceSyncMode OBSyncModeFromString(const std::string &mode);
 
+std::string OBSyncModeToString(const OBMultiDeviceSyncMode &mode);
+
+std::ostream &operator<<(std::ostream &os, const OBMultiDeviceSyncMode &rhs);
+
 OB_SAMPLE_RATE sampleRateFromString(std::string &sample_rate);
 
 std::string sampleRateToString(const OB_SAMPLE_RATE &sample_rate);

src/ob_camera_node.cpp

@@ -142,8 +142,6 @@ void OBCameraNode::getParameters() {
   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", "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);
   color_delay_us_ = nh_private_.param<int>("color_delay_us", 0);
   trigger2image_delay_us_ = nh_private_.param<int>("trigger2image_delay_us", 0);
@@ -199,7 +197,7 @@ void OBCameraNode::getParameters() {
   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_min_diff_ = nh_private_.param<int>("noise_removal_filter_min_diff", 256);
   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", -1.0);
   spatial_filter_diff_threshold_ = nh_private_.param<int>("spatial_filter_diff_threshold", -1);

src/ros_setup.cpp

@@ -125,19 +125,25 @@ void OBCameraNode::setupDevices() {
     if (!device_preset_.empty()) {
       device_->loadPreset(device_preset_.c_str());
     }
-    if (sync_mode_ != OB_MULTI_DEVICE_SYNC_MODE_FREE_RUN) {
+    if (!sync_mode_str_.empty() &&
+        device_->isPropertySupported(OB_PROP_SYNC_SIGNAL_TRIGGER_OUT_BOOL,
+                                     OB_PERMISSION_READ_WRITE)) {
       auto sync_config = device_->getMultiDeviceSyncConfig();
+      ROS_INFO_STREAM("current sync mode: " << sync_config.syncMode);
+      std::transform(sync_mode_str_.begin(), sync_mode_str_.end(), sync_mode_str_.begin(),
+                     ::toupper);
+      sync_mode_ = OBSyncModeFromString(sync_mode_str_);
       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)) {
-      }
+      sync_config = device_->getMultiDeviceSyncConfig();
+      ROS_INFO_STREAM("set sync mode to " << sync_config.syncMode);
     }
+
     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_);
@@ -244,6 +250,8 @@ void OBCameraNode::setupDevices() {
         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_;
+          ROS_INFO_STREAM("set NoiseRemovalFilter disp_diff to " << noise_removal_filter_min_diff_);
+          ROS_INFO_STREAM("set NoiseRemovalFilter max_size to " << noise_removal_filter_max_size_);
           noise_removal_filter->setFilterParams(params);
         }
       } else if (filter_name == "HDRMerge" && enable_hdr_merge_) {
@@ -255,6 +263,7 @@ void OBCameraNode::setupDevices() {
           hdr_config.exposure_2 = hdr_merge_exposure_2_;
           hdr_config.gain_1 = hdr_merge_gain_1_;
           hdr_config.gain_2 = hdr_merge_gain_2_;
+          hdr_config.enable = true;
           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_);

src/utils.cpp

@@ -456,6 +456,32 @@ OBMultiDeviceSyncMode OBSyncModeFromString(const std::string &mode) {
   }
 }
 
+std::string OBSyncModeToString(const OBMultiDeviceSyncMode &mode) {
+  switch (mode) {
+    case OBMultiDeviceSyncMode::OB_MULTI_DEVICE_SYNC_MODE_FREE_RUN:
+      return "FREE_RUN";
+    case OBMultiDeviceSyncMode::OB_MULTI_DEVICE_SYNC_MODE_STANDALONE:
+      return "STANDALONE";
+    case OBMultiDeviceSyncMode::OB_MULTI_DEVICE_SYNC_MODE_PRIMARY:
+      return "PRIMARY";
+    case OBMultiDeviceSyncMode::OB_MULTI_DEVICE_SYNC_MODE_SECONDARY:
+      return "SECONDARY";
+    case OBMultiDeviceSyncMode::OB_MULTI_DEVICE_SYNC_MODE_SECONDARY_SYNCED:
+      return "SECONDARY_SYNCED";
+    case OBMultiDeviceSyncMode::OB_MULTI_DEVICE_SYNC_MODE_SOFTWARE_TRIGGERING:
+      return "SOFTWARE_TRIGGERING";
+    case OBMultiDeviceSyncMode::OB_MULTI_DEVICE_SYNC_MODE_HARDWARE_TRIGGERING:
+      return "HARDWARE_TRIGGERING";
+    default:
+      return "FREE_RUN";
+  }
+}
+
+std::ostream &operator<<(std::ostream &os, const OBMultiDeviceSyncMode &rhs) {
+  os << OBSyncModeToString(rhs);
+  return os;
+}
+
 OB_SAMPLE_RATE sampleRateFromString(std::string &sample_rate) {
   // covert to lower case
   std::transform(sample_rate.begin(), sample_rate.end(), sample_rate.begin(), ::tolower);