feat: add set_disparity_range_mode service and callback to OBCameraNode

orbbec_camera/include/orbbec_camera/ob_camera_node.h

@@ -375,6 +375,8 @@ class OBCameraNode {
                                        std::shared_ptr<SetInt32::Response>& response);
   void getPointCloudDecimationCallback(const std::shared_ptr<GetInt32::Request>& request,
                                        std::shared_ptr<GetInt32::Response>& response);
+  void setDisparityRangeModeCallback(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,
@@ -602,6 +604,7 @@ class OBCameraNode {
   rclcpp::Service<SetFilter>::SharedPtr set_filter_srv_;
   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<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_;

orbbec_camera/src/ros_service.cpp

@@ -283,6 +283,11 @@ void OBCameraNode::setupCameraCtrlServices() {
                                            std::shared_ptr<GetInt32::Response> response) {
         getPointCloudDecimationCallback(request, response);
       });
+  set_disparity_range_mode_srv_ = node_->create_service<SetInt32>(
+      "set_disparity_range_mode", [this](const std::shared_ptr<SetInt32::Request> request,
+                                         std::shared_ptr<SetInt32::Response> response) {
+        setDisparityRangeModeCallback(request, response);
+      });
 }
 
 void OBCameraNode::getPointCloudDecimationCallback(
@@ -330,6 +335,55 @@ void OBCameraNode::setPointCloudDecimationCallback(
   }
 }
 
+void OBCameraNode::setDisparityRangeModeCallback(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_RANGE_MODE_INT, OB_PERMISSION_WRITE)) {
+      response->success = false;
+      response->message = "OB_PROP_DISP_SEARCH_RANGE_MODE_INT is not supported";
+      return;
+    }
+
+    const bool allow_set = isGemini435LePID(pid_) || enable_stream_[DEPTH];
+    if (!allow_set) {
+      response->success = false;
+      response->message = "Disparity range mode can only be set when depth stream is enabled";
+      return;
+    }
+
+    auto range = device_->getIntPropertyRange(OB_PROP_DISP_SEARCH_RANGE_MODE_INT);
+    if (hw_mode_index < range.min || hw_mode_index > range.max) {
+      response->success = false;
+      response->message =
+          "Invalid disparity range mode. Allowed values:" + std::to_string(range.min) + " to " +
+          std::to_string(range.max);
+      return;
+    }
+
+    device_->setIntProperty(OB_PROP_DISP_SEARCH_RANGE_MODE_INT, hw_mode_index);
+    disparity_range_mode_ = requested_mode_value;
+
+    RCLCPP_INFO_STREAM(logger_, "Set disparity_range_mode to " << requested_mode_value);
+    response->success = true;
+    response->message = "disparity_range_mode 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) {