From 134028503a647c2573caddba0fe4a86390d710c2 Mon Sep 17 00:00:00 2001
From: ob-yalian <luotao2025@163.com>
Date: Fri, 27 Feb 2026 14:20:35 +0800
Subject: [PATCH] feat: add set_disparity_search_offset service and callback to
 OBCameraNode

---
 .../include/orbbec_camera/ob_camera_node.h    |  3 +
 orbbec_camera/src/ros_service.cpp             | 55 +++++++++++++++++++
 2 files changed, 58 insertions(+)

diff --git a/orbbec_camera/include/orbbec_camera/ob_camera_node.h b/orbbec_camera/include/orbbec_camera/ob_camera_node.h
index 942dd0f9..4ca5445e 100644
--- a/orbbec_camera/include/orbbec_camera/ob_camera_node.h
+++ b/orbbec_camera/include/orbbec_camera/ob_camera_node.h
@@ -377,6 +377,8 @@ class OBCameraNode {
                                        std::shared_ptr<GetInt32::Response>& response);
   void setDisparityRangeModeCallback(const std::shared_ptr<SetInt32::Request>& request,
                                      std::shared_ptr<SetInt32::Response>& response);
+  void setDisparitySearchOffsetCallback(const std::shared_ptr<SetInt32::Request>& request,
+                                        std::shared_ptr<SetInt32::Response>& response);
   void setSYNCHostimeCallback(const std::shared_ptr<std_srvs::srv::SetBool::Request>& request,
                               std::shared_ptr<std_srvs::srv::SetBool::Response>& response);
   void sendSoftwareTriggerCallback(const std::shared_ptr<std_srvs::srv::SetBool::Request>& request,
@@ -605,6 +607,7 @@ class OBCameraNode {
   rclcpp::Service<SetInt32>::SharedPtr set_point_cloud_decimation_srv_;
   rclcpp::Service<GetInt32>::SharedPtr get_point_cloud_decimation_srv_;
   rclcpp::Service<SetInt32>::SharedPtr set_disparity_range_mode_srv_;
+  rclcpp::Service<SetInt32>::SharedPtr set_disparity_search_offset_srv_;
   rclcpp::Service<orbbec_camera_msgs::srv::GetBool>::SharedPtr get_streams_enable_srv_;
   rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr set_streams_enable_srv_;
   rclcpp::Service<GetUserCalibParams>::SharedPtr get_user_calib_params_srv_;
diff --git a/orbbec_camera/src/ros_service.cpp b/orbbec_camera/src/ros_service.cpp
index 70112a57..c1752ccf 100644
--- a/orbbec_camera/src/ros_service.cpp
+++ b/orbbec_camera/src/ros_service.cpp
@@ -288,6 +288,11 @@ void OBCameraNode::setupCameraCtrlServices() {
                                          std::shared_ptr<SetInt32::Response> response) {
         setDisparityRangeModeCallback(request, response);
       });
+  set_disparity_search_offset_srv_ = node_->create_service<SetInt32>(
+      "set_disparity_search_offset", [this](const std::shared_ptr<SetInt32::Request> request,
+                                            std::shared_ptr<SetInt32::Response> response) {
+        setDisparitySearchOffsetCallback(request, response);
+      });
 }
 
 void OBCameraNode::getPointCloudDecimationCallback(
@@ -386,6 +391,56 @@ void OBCameraNode::setDisparityRangeModeCallback(const std::shared_ptr<SetInt32:
   }
 }
 
+void OBCameraNode::setDisparitySearchOffsetCallback(
+    const std::shared_ptr<SetInt32::Request>& request,
+    std::shared_ptr<SetInt32::Response>& response) {
+  if (!request) {
+    response->success = false;
+    response->message = "Invalid request";
+    return;
+  }
+
+  try {
+    if (!device_->isPropertySupported(OB_PROP_DISP_SEARCH_OFFSET_INT, OB_PERMISSION_WRITE)) {
+      response->success = false;
+      response->message = "OB_PROP_DISP_SEARCH_OFFSET_INT is not supported";
+      return;
+    }
+
+    const bool allow_set = isGemini435LePID(pid_) || enable_stream_[DEPTH];
+    if (!allow_set) {
+      response->success = false;
+      response->message = "Disparity search offset can only be set when depth stream is enabled";
+      return;
+    }
+
+    auto range = device_->getIntPropertyRange(OB_PROP_DISP_SEARCH_OFFSET_INT);
+    if (request->data < range.min || request->data > range.max) {
+      response->success = false;
+      response->message =
+          "Invalid disparity search offset. Allowed values:" + std::to_string(range.min) + " to " +
+          std::to_string(range.max);
+      return;
+    }
+
+    device_->setIntProperty(OB_PROP_DISP_SEARCH_OFFSET_INT, request->data);
+    disparity_search_offset_ = request->data;
+
+    RCLCPP_INFO_STREAM(logger_, "Set disparity_search_offset to " << disparity_search_offset_);
+    response->success = true;
+    response->message = "disparity_search_offset updated";
+  } catch (const ob::Error& e) {
+    response->success = false;
+    response->message = e.getMessage();
+  } catch (const std::exception& e) {
+    response->success = false;
+    response->message = e.what();
+  } catch (...) {
+    response->success = false;
+    response->message = "unknown error";
+  }
+}
+
 void OBCameraNode::setStreamsEnableCallback(
     const std::shared_ptr<std_srvs::srv::SetBool::Request> request,
     std::shared_ptr<std_srvs::srv::SetBool::Response> response) {