From dd0e3ae6177c25790ed1b1248728a72ec023eff7 Mon Sep 17 00:00:00 2001
From: jj <957713278@qq.com>
Date: Mon, 31 Mar 2025 14:19:14 +0800
Subject: [PATCH] Add hardware_noise_removal_filter_threshold param in
 gemini_330_series.launch.py

---
 .../include/orbbec_camera/ob_camera_node.h    |  1 +
 .../launch/gemini_330_series.launch.py        |  3 ++-
 orbbec_camera/src/ob_camera_node.cpp          | 22 +++++++++++++++++++
 3 files changed, 25 insertions(+), 1 deletion(-)

diff --git a/orbbec_camera/include/orbbec_camera/ob_camera_node.h b/orbbec_camera/include/orbbec_camera/ob_camera_node.h
index e3758f57..88351227 100644
--- a/orbbec_camera/include/orbbec_camera/ob_camera_node.h
+++ b/orbbec_camera/include/orbbec_camera/ob_camera_node.h
@@ -598,6 +598,7 @@ class OBCameraNode {
   int sequence_id_filter_id_ = -1;
   int threshold_filter_max_ = -1;
   int threshold_filter_min_ = -1;
+  float hardware_noise_removal_filter_threshold_ = -1.0;
   int noise_removal_filter_min_diff_ = 256;
   int noise_removal_filter_max_size_ = 80;
   float spatial_filter_alpha_ = -1;
diff --git a/orbbec_camera/launch/gemini_330_series.launch.py b/orbbec_camera/launch/gemini_330_series.launch.py
index 8c950aa3..bb99596c 100644
--- a/orbbec_camera/launch/gemini_330_series.launch.py
+++ b/orbbec_camera/launch/gemini_330_series.launch.py
@@ -165,7 +165,7 @@ def generate_launch_description():
         DeclareLaunchArgument('enable_hdr_merge', default_value='false'),
         DeclareLaunchArgument('enable_sequence_id_filter', default_value='false'),
         DeclareLaunchArgument('enable_threshold_filter', default_value='false'),
-        DeclareLaunchArgument('enable_hardware_noise_removal_filter', default_value='true'),
+        DeclareLaunchArgument('enable_hardware_noise_removal_filter', default_value='false'),
         DeclareLaunchArgument('enable_noise_removal_filter', default_value='true'),
         DeclareLaunchArgument('enable_spatial_filter', default_value='false'),
         DeclareLaunchArgument('enable_temporal_filter', default_value='false'),
@@ -174,6 +174,7 @@ def generate_launch_description():
         DeclareLaunchArgument('sequence_id_filter_id', default_value='-1'),
         DeclareLaunchArgument('threshold_filter_max', default_value='-1'),
         DeclareLaunchArgument('threshold_filter_min', default_value='-1'),
+        DeclareLaunchArgument('hardware_noise_removal_filter_threshold', default_value='-1.0'),
         DeclareLaunchArgument('noise_removal_filter_min_diff', default_value='256'),
         DeclareLaunchArgument('noise_removal_filter_max_size', default_value='80'),
         DeclareLaunchArgument('spatial_filter_alpha', default_value='-1.0'),
diff --git a/orbbec_camera/src/ob_camera_node.cpp b/orbbec_camera/src/ob_camera_node.cpp
index 03fd0481..b890243b 100644
--- a/orbbec_camera/src/ob_camera_node.cpp
+++ b/orbbec_camera/src/ob_camera_node.cpp
@@ -626,6 +626,16 @@ void OBCameraNode::setupDevices() {
                              enable_hardware_noise_removal_filter_);
     RCLCPP_INFO_STREAM(
         logger_, "Setting hardware_noise_removal_filter:" << enable_hardware_noise_removal_filter_);
+    if (device_->isPropertySupported(OB_PROP_HW_NOISE_REMOVE_FILTER_THRESHOLD_FLOAT,
+                                     OB_PERMISSION_READ_WRITE)) {
+      if (hardware_noise_removal_filter_threshold_ != -1.0 &&
+          enable_hardware_noise_removal_filter_) {
+        device_->setFloatProperty(OB_PROP_HW_NOISE_REMOVE_FILTER_THRESHOLD_FLOAT,
+                                  hardware_noise_removal_filter_threshold_);
+        RCLCPP_INFO_STREAM(logger_, "Setting hardware_noise_removal_filter_threshold :"
+                                        << hardware_noise_removal_filter_threshold_);
+      }
+    }
   }
 }
 void OBCameraNode::setupColorPostProcessFilter() {
@@ -1474,6 +1484,8 @@ void OBCameraNode::getParameters() {
   setAndGetNodeParameter<int>(sequence_id_filter_id_, "sequence_id_filter_id", -1);
   setAndGetNodeParameter<int>(threshold_filter_max_, "threshold_filter_max", -1);
   setAndGetNodeParameter<int>(threshold_filter_min_, "threshold_filter_min", -1);
+  setAndGetNodeParameter<float>(hardware_noise_removal_filter_threshold_,
+                              "hardware_noise_removal_filter_threshold", -1.0);
   setAndGetNodeParameter<int>(noise_removal_filter_min_diff_, "noise_removal_filter_min_diff", 256);
   setAndGetNodeParameter<int>(noise_removal_filter_max_size_, "noise_removal_filter_max_size", 80);
   setAndGetNodeParameter<float>(spatial_filter_alpha_, "spatial_filter_alpha", -1.0);
@@ -3263,6 +3275,16 @@ void OBCameraNode::setFilterCallback(const std::shared_ptr<SetFilter ::Request>
                                  request->filter_enable);
         RCLCPP_INFO_STREAM(logger_,
                            "Setting hardware_noise_removal_filter:" << request->filter_enable);
+        if (device_->isPropertySupported(OB_PROP_HW_NOISE_REMOVE_FILTER_THRESHOLD_FLOAT,
+                                         OB_PERMISSION_READ_WRITE)) {
+          if (hardware_noise_removal_filter_threshold_ != -1.0 &&
+              enable_hardware_noise_removal_filter_) {
+            device_->setFloatProperty(OB_PROP_HW_NOISE_REMOVE_FILTER_THRESHOLD_FLOAT,
+                                      hardware_noise_removal_filter_threshold_);
+            RCLCPP_INFO_STREAM(logger_, "Setting hardware_noise_removal_filter_threshold :"
+                                            << hardware_noise_removal_filter_threshold_);
+          }
+        }
       }
     } else if (request->filter_name == "SpatialAdvancedFilter") {
       auto spatial_filter = std::make_shared<ob::SpatialAdvancedFilter>();