From be19842c4099b28722b63c97d7a8f4347817cf0c Mon Sep 17 00:00:00 2001
From: jj <957713278@qq.com>
Date: Thu, 3 Jul 2025 14:30:35 +0800
Subject: [PATCH] Add repetitive_scan_mode ,filter_level and vertical_fov
 params

---
 .../include/orbbec_camera/ob_lidar_node.h     |  3 ++
 orbbec_camera/launch/lidar.launch.py          |  3 ++
 orbbec_camera/src/ob_lidar_node.cpp           | 44 +++++++++++++++++++
 3 files changed, 50 insertions(+)

diff --git a/orbbec_camera/include/orbbec_camera/ob_lidar_node.h b/orbbec_camera/include/orbbec_camera/ob_lidar_node.h
index 1c631575..661e2a95 100644
--- a/orbbec_camera/include/orbbec_camera/ob_lidar_node.h
+++ b/orbbec_camera/include/orbbec_camera/ob_lidar_node.h
@@ -244,6 +244,9 @@ class OBLidarNode {
   float max_angle_ = 135.0;
   float min_range_ = 0.05;
   float max_range_ = 30.0;
+  int repetitive_scan_mode_ = -1;
+  int filter_level_ = -1;
+  float vertical_fov_ = -1;
 };
 
 }  // namespace orbbec_lidar
diff --git a/orbbec_camera/launch/lidar.launch.py b/orbbec_camera/launch/lidar.launch.py
index c8e033df..fa5310a5 100644
--- a/orbbec_camera/launch/lidar.launch.py
+++ b/orbbec_camera/launch/lidar.launch.py
@@ -60,6 +60,9 @@ def generate_launch_description():
         DeclareLaunchArgument('tf_publish_rate', default_value='0.0'),
         DeclareLaunchArgument('lidar_format', default_value='ANY'),#LIDAR_POINT, LIDAR_SPHERE_POINT, LIDAR_SCAN
         DeclareLaunchArgument('lidar_rate', default_value='20'),
+        DeclareLaunchArgument('repetitive_scan_mode', default_value='-1'),
+        DeclareLaunchArgument('filter_level', default_value='-1'),
+        DeclareLaunchArgument('vertical_fov', default_value='-1.0'),
         DeclareLaunchArgument('min_angle', default_value='-135.0'),
         DeclareLaunchArgument('max_angle', default_value='135.0'),
         DeclareLaunchArgument('min_range', default_value='0.05'),
diff --git a/orbbec_camera/src/ob_lidar_node.cpp b/orbbec_camera/src/ob_lidar_node.cpp
index 799edbfd..e8fb6e65 100644
--- a/orbbec_camera/src/ob_lidar_node.cpp
+++ b/orbbec_camera/src/ob_lidar_node.cpp
@@ -144,6 +144,9 @@ void OBLidarNode::getParameters() {
   setAndGetNodeParameter<float>(max_angle_, "max_angle", 135.0);
   setAndGetNodeParameter<float>(min_range_, "min_range", 0.05);
   setAndGetNodeParameter<float>(max_range_, "max_range", 30.0);
+  setAndGetNodeParameter<int>(repetitive_scan_mode_, "repetitive_scan_mode", -1);
+  setAndGetNodeParameter<int>(filter_level_, "filter_level", -1);
+  setAndGetNodeParameter<float>(vertical_fov_, "vertical_fov", -1.0);
 }
 
 void OBLidarNode::setupDevices() {
@@ -176,6 +179,47 @@ void OBLidarNode::setupDevices() {
                            << (device_->getIntProperty(OB_PROP_LIDAR_ECHO_MODE_INT) ? "First Echo"
                                                                                     : "Last Echo"));
   }
+  if (repetitive_scan_mode_ != -1 &&
+      device_->isPropertySupported(OB_PROP_LIDAR_REPETITIVE_SCAN_MODE_INT,
+                                   OB_PERMISSION_READ_WRITE)) {
+    auto range = device_->getIntPropertyRange(OB_PROP_LIDAR_REPETITIVE_SCAN_MODE_INT);
+    if (repetitive_scan_mode_ <= range.min || repetitive_scan_mode_ >= range.max) {
+      RCLCPP_ERROR(logger_,
+                   "repetitive scan mode value is out of range[%d,%d], please check the value",
+                   range.min, range.max);
+    } else {
+      TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_LIDAR_REPETITIVE_SCAN_MODE_INT,
+                          repetitive_scan_mode_);
+      RCLCPP_INFO_STREAM(logger_, "Setting repetitive scan mode to " << device_->getIntProperty(
+                                      OB_PROP_LIDAR_REPETITIVE_SCAN_MODE_INT));
+    }
+  }
+  if (filter_level_ != -1 &&
+      device_->isPropertySupported(OB_PROP_LIDAR_TAIL_FILTER_LEVEL_INT, OB_PERMISSION_READ_WRITE)) {
+    auto range = device_->getIntPropertyRange(OB_PROP_LIDAR_TAIL_FILTER_LEVEL_INT);
+    if (filter_level_ <= range.min || filter_level_ >= range.max) {
+      RCLCPP_ERROR(logger_, "filter level value is out of range[%d,%d], please check the value",
+                   range.min, range.max);
+    } else {
+      TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_LIDAR_TAIL_FILTER_LEVEL_INT, filter_level_);
+      TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_LIDAR_APPLY_CONFIGS_INT, 1);
+      RCLCPP_INFO_STREAM(logger_, "Setting filter level to " << device_->getIntProperty(
+                                      OB_PROP_LIDAR_TAIL_FILTER_LEVEL_INT));
+    }
+  }
+
+  if (vertical_fov_ != -1.0 &&
+      device_->isPropertySupported(OB_PROP_LIDAR_MEMS_FOV_SIZE_FLOAT, OB_PERMISSION_READ_WRITE)) {
+    auto range = device_->getFloatPropertyRange(OB_PROP_LIDAR_MEMS_FOV_SIZE_FLOAT);
+    if (vertical_fov_ <= range.min || vertical_fov_ >= range.max) {
+      RCLCPP_ERROR(logger_, "vertical fov value is out of range[%f,%f], please check the value",
+                   range.min, range.max);
+    } else {
+      TRY_TO_SET_PROPERTY(setFloatProperty, OB_PROP_LIDAR_MEMS_FOV_SIZE_FLOAT, vertical_fov_);
+      RCLCPP_INFO_STREAM(logger_, "Setting vertical fov to " << device_->getFloatProperty(
+                                      OB_PROP_LIDAR_MEMS_FOV_SIZE_FLOAT));
+    }
+  }
 }
 
 void OBLidarNode::setupProfiles() {