add auto white balance ctrl

orbbec_camera/include/orbbec_camera/ob_camera_node.h

@@ -170,6 +170,12 @@ class OBCameraNode {
   void setWhiteBalanceCallback(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);
+
+  void setAutoWhiteBalanceCallback(const std::shared_ptr<SetBool::Request>& request,
+                                   std::shared_ptr<SetBool ::Response>& response);
+
   void setAutoExposureCallback(const std::shared_ptr<std_srvs::srv::SetBool::Request>& request,
                                std::shared_ptr<std_srvs::srv::SetBool::Response>& response,
                                const stream_index_pair& stream_index);
@@ -242,6 +248,7 @@ class OBCameraNode {
   std::map<stream_index_pair, std::string> qos_;
   std::map<stream_index_pair, std::string> info_qos_;
   std::map<stream_index_pair, ob_format> format_;
+  std::map<stream_index_pair, std::string> format_str_;
   std::map<ob_stream_type, int> image_format_;
   std::map<stream_index_pair, std::vector<std::shared_ptr<ob::VideoStreamProfile>>>
       enabled_profiles_;
@@ -267,6 +274,8 @@ class OBCameraNode {
   std::map<stream_index_pair, rclcpp::Service<SetBool>::SharedPtr> toggle_sensor_srv_;
   rclcpp::Service<GetInt32>::SharedPtr get_white_balance_srv_;  // only rgb
   rclcpp::Service<SetInt32>::SharedPtr set_white_balance_srv_;
+  rclcpp::Service<GetInt32>::SharedPtr get_auto_white_balance_srv_;  // only rgb
+  rclcpp::Service<SetBool>::SharedPtr set_auto_white_balance_srv_;
   rclcpp::Service<GetString>::SharedPtr get_sdk_version_srv_;
   std::map<stream_index_pair, rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr>
       set_auto_exposure_srv_;  // only rgb color

orbbec_camera/src/ros_service.cpp

@@ -52,15 +52,14 @@ void OBCameraNode::setupCameraCtrlServices() {
                                             std::shared_ptr<SetInt32::Response> response) {
           setGainCallback(request, response, stream_index);
         });
-    if (stream_index.first == OB_STREAM_COLOR || stream_index.first == OB_STREAM_DEPTH) {
+    service_name = "set_" + stream_name + "_auto_exposure";
-      service_name = "set_" + stream_name + "_auto_exposure";
+    set_auto_exposure_srv_[stream_index] = node_->create_service<SetBool>(
-      set_auto_exposure_srv_[stream_index] = node_->create_service<SetBool>(
+        service_name,
-          service_name,
+        [this, stream_index = stream_index](const std::shared_ptr<SetBool::Request> request,
-          [this, stream_index = stream_index](const std::shared_ptr<SetBool::Request> request,
+                                            std::shared_ptr<SetBool::Response> response) {
-                                              std::shared_ptr<SetBool::Response> response) {
+          setAutoExposureCallback(request, response, stream_index);
-            setAutoExposureCallback(request, response, stream_index);
+        });
-          });
+
-    }
     service_name = "toggle_" + stream_name;
 
     toggle_sensor_srv_[stream_index] = node_->create_service<SetBool>(
@@ -105,6 +104,16 @@ void OBCameraNode::setupCameraCtrlServices() {
                                   std::shared_ptr<SetInt32::Response> response) {
         setWhiteBalanceCallback(request, response);
       });
+  get_auto_white_balance_srv_ = node_->create_service<GetInt32>(
+      "get_white_balance", [this](const std::shared_ptr<GetInt32::Request> request,
+                                  std::shared_ptr<GetInt32::Response> response) {
+        getAutoWhiteBalanceCallback(request, response);
+      });
+  set_auto_white_balance_srv_ = node_->create_service<SetBool>(
+      "set_white_balance", [this](const std::shared_ptr<SetBool::Request> request,
+                                  std::shared_ptr<SetBool::Response> response) {
+        setAutoWhiteBalanceCallback(request, response);
+      });
   get_device_srv_ = node_->create_service<GetDeviceInfo>(
       "get_device_info", [this](const std::shared_ptr<GetDeviceInfo::Request> request,
                                 std::shared_ptr<GetDeviceInfo::Response> response) {
@@ -248,6 +257,39 @@ void OBCameraNode::setWhiteBalanceCallback(const std::shared_ptr<SetInt32 ::Requ
   }
 }
 
+void OBCameraNode::getAutoWhiteBalanceCallback(const std::shared_ptr<GetInt32::Request>& request,
+                                               std::shared_ptr<GetInt32::Response>& response) {
+  (void)request;
+  try {
+    response->data = device_->getBoolProperty(OB_PROP_COLOR_AUTO_WHITE_BALANCE_BOOL);
+    response->success = true;
+  } catch (const ob::Error& e) {
+    response->success = false;
+    response->message = e.getMessage();
+  } catch (const std::exception& e) {
+    response->message = e.what();
+  } catch (...) {
+    response->success = false;
+    response->message = "unknown error";
+  }
+}
+
+void OBCameraNode::setAutoWhiteBalanceCallback(const std::shared_ptr<SetBool::Request>& request,
+                                               std::shared_ptr<SetBool::Response>& response) {
+  try {
+    device_->setBoolProperty(OB_PROP_COLOR_AUTO_WHITE_BALANCE_BOOL, request->data);
+    response->success = true;
+  } catch (const ob::Error& e) {
+    response->success = false;
+    response->message = e.getMessage();
+  } catch (const std::exception& e) {
+    response->message = e.what();
+  } catch (...) {
+    response->success = false;
+    response->message = "unknown error";
+  }
+}
+
 void OBCameraNode::setAutoExposureCallback(
     const std::shared_ptr<std_srvs::srv::SetBool::Request>& request,
     std::shared_ptr<std_srvs::srv::SetBool::Response>& response,