From be88c2e496c56d2fc4976dbc6ba860d2704c0169 Mon Sep 17 00:00:00 2001
From: slz <shenlanzhou@orbbec.com>
Date: Thu, 3 Sep 2026 16:39:02 +0800
Subject: [PATCH] feat: expose color WB control property

---
 .../include/orbbec_camera/ob_camera_node.h    |  8 ++
 orbbec_camera/src/ros_service.cpp             | 75 +++++++++++++++++++
 2 files changed, 83 insertions(+)

diff --git a/orbbec_camera/include/orbbec_camera/ob_camera_node.h b/orbbec_camera/include/orbbec_camera/ob_camera_node.h
index 30427d15..35bd57f1 100644
--- a/orbbec_camera/include/orbbec_camera/ob_camera_node.h
+++ b/orbbec_camera/include/orbbec_camera/ob_camera_node.h
@@ -433,6 +433,12 @@ class OBCameraNode {
   void setWhiteBalanceCallback(const std::shared_ptr<SetInt32 ::Request>& request,
                                std::shared_ptr<SetInt32 ::Response>& response);
 
+  void getColorWbCtrlCallback(const std::shared_ptr<GetInt32::Request>& request,
+                              std::shared_ptr<GetInt32::Response>& response);
+
+  void setColorWbCtrlCallback(const std::shared_ptr<SetInt32::Request>& request,
+                              std::shared_ptr<SetInt32::Response>& response);
+
   void getAutoWhiteBalanceCallback(const std::shared_ptr<GetInt32::Request>& request,
                                    std::shared_ptr<GetInt32::Response>& response);
 
@@ -762,6 +768,8 @@ class OBCameraNode {
   std::map<stream_index_pair, rclcpp::Service<SetInt32>::SharedPtr> set_rotation_srv_;
   rclcpp::Service<GetInt32>::SharedPtr get_white_balance_srv_;
   rclcpp::Service<SetInt32>::SharedPtr set_white_balance_srv_;
+  rclcpp::Service<GetInt32>::SharedPtr get_color_wb_ctrl_srv_;
+  rclcpp::Service<SetInt32>::SharedPtr set_color_wb_ctrl_srv_;
   rclcpp::Service<GetInt32>::SharedPtr get_auto_white_balance_srv_;
   rclcpp::Service<SetBool>::SharedPtr set_auto_white_balance_srv_;
   rclcpp::Service<GetInt32>::SharedPtr get_ae_awb_status_srv_;
diff --git a/orbbec_camera/src/ros_service.cpp b/orbbec_camera/src/ros_service.cpp
index 4fd14f55..25168cfa 100644
--- a/orbbec_camera/src/ros_service.cpp
+++ b/orbbec_camera/src/ros_service.cpp
@@ -293,6 +293,20 @@ void OBCameraNode::setupCameraCtrlServices() {
                                   std::shared_ptr<SetInt32::Response> response) {
         setWhiteBalanceCallback(request, response);
       });
+  if (isPropertyReadable(device_, OB_PROP_COLOR_WB_CTRL_INT)) {
+    get_color_wb_ctrl_srv_ = node_->create_service<GetInt32>(
+        "get_color_wb_ctrl", [this](const std::shared_ptr<GetInt32::Request> request,
+                                    std::shared_ptr<GetInt32::Response> response) {
+          getColorWbCtrlCallback(request, response);
+        });
+  }
+  if (isPropertyWritable(device_, OB_PROP_COLOR_WB_CTRL_INT)) {
+    set_color_wb_ctrl_srv_ = node_->create_service<SetInt32>(
+        "set_color_wb_ctrl", [this](const std::shared_ptr<SetInt32::Request> request,
+                                    std::shared_ptr<SetInt32::Response> response) {
+          setColorWbCtrlCallback(request, response);
+        });
+  }
   get_auto_white_balance_srv_ = node_->create_service<GetInt32>(
       "get_auto_white_balance", [this](const std::shared_ptr<GetInt32::Request> request,
                                        std::shared_ptr<GetInt32::Response> response) {
@@ -1252,6 +1266,67 @@ void OBCameraNode::setWhiteBalanceCallback(const std::shared_ptr<SetInt32 ::Requ
   }
 }
 
+void OBCameraNode::getColorWbCtrlCallback(const std::shared_ptr<GetInt32::Request>& request,
+                                          std::shared_ptr<GetInt32::Response>& response) {
+  (void)request;
+  std::lock_guard<decltype(device_lock_)> lock(device_lock_);
+  try {
+    response->data = device_->getIntProperty(OB_PROP_COLOR_WB_CTRL_INT);
+    response->success = true;
+    response->message = "OK";
+  } catch (const ob::Error& e) {
+    response->success = false;
+    response->message = orbbec_camera::formatObErrorWithStatus(e);
+  } catch (const std::exception& e) {
+    response->success = false;
+    response->message = e.what();
+  } catch (...) {
+    response->success = false;
+    response->message = "unknown error";
+  }
+}
+
+void OBCameraNode::setColorWbCtrlCallback(const std::shared_ptr<SetInt32::Request>& request,
+                                          std::shared_ptr<SetInt32::Response>& response) {
+  if (!request) {
+    response->success = false;
+    response->message = "Invalid request";
+    return;
+  }
+
+  std::lock_guard<decltype(device_lock_)> lock(device_lock_);
+  try {
+    const auto range = device_->getIntPropertyRange(OB_PROP_COLOR_WB_CTRL_INT);
+    if (request->data < range.min || request->data > range.max) {
+      response->success = false;
+      response->message = "value out of range [" + std::to_string(range.min) + ", " +
+                          std::to_string(range.max) + "]";
+      return;
+    }
+
+    device_->setIntProperty(OB_PROP_COLOR_WB_CTRL_INT, request->data);
+    response->success = true;
+    response->message = "OK";
+    if (isPropertyReadable(device_, OB_PROP_COLOR_WB_CTRL_INT)) {
+      const auto current_value = device_->getIntProperty(OB_PROP_COLOR_WB_CTRL_INT);
+      response->success = current_value == request->data;
+      if (!response->success) {
+        response->message = "device reported " + std::to_string(current_value) + " after setting " +
+                            std::to_string(request->data);
+      }
+    }
+  } catch (const ob::Error& e) {
+    response->success = false;
+    response->message = orbbec_camera::formatObErrorWithStatus(e);
+  } catch (const std::exception& e) {
+    response->success = false;
+    response->message = e.what();
+  } catch (...) {
+    response->success = false;
+    response->message = "unknown error";
+  }
+}
+
 void OBCameraNode::getAutoWhiteBalanceCallback(const std::shared_ptr<GetInt32::Request>& request,
                                                std::shared_ptr<GetInt32::Response>& response) {
   (void)request;