diff --git a/orbbec_camera/include/orbbec_camera/ob_camera_node.h b/orbbec_camera/include/orbbec_camera/ob_camera_node.h index 0e5a067d..36f740a5 100644 --- a/orbbec_camera/include/orbbec_camera/ob_camera_node.h +++ b/orbbec_camera/include/orbbec_camera/ob_camera_node.h @@ -437,6 +437,12 @@ class OBCameraNode { void setWhiteBalanceCallback(const std::shared_ptr& request, std::shared_ptr& response); + void getColorWbCtrlCallback(const std::shared_ptr& request, + std::shared_ptr& response); + + void setColorWbCtrlCallback(const std::shared_ptr& request, + std::shared_ptr& response); + void getAutoWhiteBalanceCallback(const std::shared_ptr& request, std::shared_ptr& response); @@ -772,6 +778,8 @@ class OBCameraNode { std::map::SharedPtr> set_rotation_srv_; rclcpp::Service::SharedPtr get_white_balance_srv_; rclcpp::Service::SharedPtr set_white_balance_srv_; + rclcpp::Service::SharedPtr get_color_wb_ctrl_srv_; + rclcpp::Service::SharedPtr set_color_wb_ctrl_srv_; rclcpp::Service::SharedPtr get_auto_white_balance_srv_; rclcpp::Service::SharedPtr set_auto_white_balance_srv_; rclcpp::Service::SharedPtr get_ae_awb_status_srv_; diff --git a/orbbec_camera/src/ros_service.cpp b/orbbec_camera/src/ros_service.cpp index 5814728e..1f6031cd 100644 --- a/orbbec_camera/src/ros_service.cpp +++ b/orbbec_camera/src/ros_service.cpp @@ -296,6 +296,20 @@ void OBCameraNode::setupCameraCtrlServices() { std::shared_ptr response) { setWhiteBalanceCallback(request, response); }); + if (isPropertyReadable(device_, OB_PROP_COLOR_WB_CTRL_INT)) { + get_color_wb_ctrl_srv_ = node_->create_service( + "get_color_wb_ctrl", [this](const std::shared_ptr request, + std::shared_ptr response) { + getColorWbCtrlCallback(request, response); + }); + } + if (isPropertyWritable(device_, OB_PROP_COLOR_WB_CTRL_INT)) { + set_color_wb_ctrl_srv_ = node_->create_service( + "set_color_wb_ctrl", [this](const std::shared_ptr request, + std::shared_ptr response) { + setColorWbCtrlCallback(request, response); + }); + } get_auto_white_balance_srv_ = node_->create_service( "get_auto_white_balance", [this](const std::shared_ptr request, std::shared_ptr response) { @@ -1277,6 +1291,67 @@ void OBCameraNode::setWhiteBalanceCallback(const std::shared_ptr& request, + std::shared_ptr& response) { + (void)request; + std::lock_guard 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& request, + std::shared_ptr& response) { + if (!request) { + response->success = false; + response->message = "Invalid request"; + return; + } + + std::lock_guard 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& request, std::shared_ptr& response) { (void)request;