diff --git a/orbbec_camera/CMakeLists.txt b/orbbec_camera/CMakeLists.txt index 9bde88d9..05ecb0cc 100644 --- a/orbbec_camera/CMakeLists.txt +++ b/orbbec_camera/CMakeLists.txt @@ -29,6 +29,7 @@ set(dependencies OpenCV orbbec_camera_msgs rclcpp + rclcpp_action rclcpp_components sensor_msgs std_msgs @@ -195,6 +196,7 @@ add_orbbec_executable(set_device_ip tools/ip_config_tool.cpp) add_orbbec_executable(topic_statistics_node tools/topic_statistics.cpp) add_orbbec_executable(ob_benchmark_node tools/ob_benchmark.cpp) add_orbbec_executable(435le_example_node examples/Gemini_435Le_example_node/camera_example_node.cpp) +add_orbbec_executable(ae_awb_lock_test_node examples/ae_awb_lock/ae_awb_lock_test_node.cpp) add_orbbec_executable(service_benchmark_node scripts/service_benchmark_node.cpp) add_orbbec_executable(image_sync_example_node examples/multi_camera_time_sync/image_sync_example_node.cpp) @@ -257,7 +259,7 @@ if(DEFINED ENV{BUILDING_PACKAGE}) install(FILES ${CMAKE_CURRENT_SOURCE_DIR}/scripts/99-obsensor-libusb.rules DESTINATION /etc/udev/rules.d) endif() -install(TARGETS list_devices_node list_depth_work_mode_node list_camera_profile_mode_node firmware_update_tool topic_statistics_node service_benchmark_node ob_benchmark_node 435le_example_node ip_config_tool set_device_ip image_sync_example_node DESTINATION lib/${PROJECT_NAME}/ +install(TARGETS list_devices_node list_depth_work_mode_node list_camera_profile_mode_node firmware_update_tool topic_statistics_node service_benchmark_node ob_benchmark_node 435le_example_node ae_awb_lock_test_node ip_config_tool set_device_ip image_sync_example_node DESTINATION lib/${PROJECT_NAME}/ ) if(BUILD_TESTING) diff --git a/orbbec_camera/examples/README.MD b/orbbec_camera/examples/README.MD index 7755bddf..1eaaf1fa 100644 --- a/orbbec_camera/examples/README.MD +++ b/orbbec_camera/examples/README.MD @@ -8,7 +8,7 @@ These simple examples demonstrate how to easily use the camera with OrbbecSDK_RO | :---------------------------------------------------------------------------: | -------------------------------------------------------------------------------------------------------------------------------------------------------------------- | ---------------- | ----------------------------------------------------------------------------- | | [Net_camera](./net_camera) | How to use Net camera in OrbbecSDK_ROS2(Only Femto Mega nad Gemini 335Le) | ⭐️ | [Net_camera](https://orbbec.github.io/OrbbecSDK_ROS2/source/5_advanced_guide/configuration/net_camera.html) | | [Gmsl_camera](./gmsl_camera) | How to use GMSL camera.(Only Gemini 335Lg) | ⭐️ | [Gmsl_camera](https://orbbec.github.io/OrbbecSDK_ROS2/source/5_advanced_guide/multi_camera/gmsl_camera.html) | +| [AE/AWB lock test](./ae_awb_lock) | How to verify the AE/AWB convergence, capture, and manual lock-in flow through ROS 2 services. | ⭐️⭐️ | [README](./ae_awb_lock/README.md) | | [Benchmark](./benchmark) | The goal of this tool is to benchmark the performance of various OrbbecSDK_ROS2 camera configurations. | ⭐️⭐️ | [Benchmark](https://orbbec.github.io/OrbbecSDK_ROS2/source/5_advanced_guide/performance/benchmark.html) | | [Multi_camera_synced_verification_tool](./multi_camera_synced_verification_tool) | How to verify the synchronization accuracy of multi-camera synchronization. | ⭐️⭐️⭐️ | [Multi_camera_synced_verification_tool](https://orbbec.github.io/OrbbecSDK_ROS2/source/5_advanced_guide/multi_camera/multi_camera_synced_verification_tool.html) | - diff --git a/orbbec_camera/examples/ae_awb_lock/README.md b/orbbec_camera/examples/ae_awb_lock/README.md new file mode 100644 index 00000000..04cace1d --- /dev/null +++ b/orbbec_camera/examples/ae_awb_lock/README.md @@ -0,0 +1,37 @@ +# AE/AWB Lock Test + +This sample exposes a test-only ROS 2 action that verifies the AE/AWB capture and manual +lock-in flow through the camera driver's services. + +Run the camera driver and this sample in the same namespace: + +```bash +ros2 run orbbec_camera ae_awb_lock_test_node --ros-args -r __ns:=/camera +``` + +Send a goal and print feedback: + +```bash +ros2 action send_goal \ + /camera/run_ae_awb_lock_test \ + orbbec_camera_msgs/action/RunAeAwbLockTest \ + "{timeout_ms: 10000}" \ + --feedback +``` + +The sample enables auto exposure and auto white balance, waits until the SDK status equals `1`, +captures the current exposure, color gain, AWB gains, and color temperature, disables the auto +controls, and writes the captured values back in this order: + +1. Color exposure +2. Color gain +3. AWB R/B/G gains +4. Color temperature + +The final AWB gain readback must exactly match the captured value. Other readback differences are +reported as warnings because the device may quantize those controls. On failure or cancellation, +the sample restores auto exposure and auto white balance. + +Every feedback phase contains a fresh status value read from the camera service. The +`waiting_for_services` feedback is published after all required services become available, because +the status cannot be read before its service is ready. diff --git a/orbbec_camera/examples/ae_awb_lock/ae_awb_lock_test_node.cpp b/orbbec_camera/examples/ae_awb_lock/ae_awb_lock_test_node.cpp new file mode 100644 index 00000000..94025e8f --- /dev/null +++ b/orbbec_camera/examples/ae_awb_lock/ae_awb_lock_test_node.cpp @@ -0,0 +1,361 @@ +// Copyright (c) 2026 Orbbec Inc. All Rights Reserved. +// Licensed under the Apache License, Version 2.0. + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "orbbec_camera_msgs/action/run_ae_awb_lock_test.hpp" +#include "orbbec_camera_msgs/srv/get_awb_gain.hpp" +#include "orbbec_camera_msgs/srv/get_int32.hpp" +#include "orbbec_camera_msgs/srv/set_awb_gain.hpp" +#include "orbbec_camera_msgs/srv/set_int32.hpp" + +namespace { + +using namespace std::chrono_literals; + +constexpr int32_t kAeAwbConverged = 1; +constexpr uint32_t kDefaultTimeoutMs = 10000; +constexpr auto kPollInterval = 100ms; +constexpr auto kServiceWaitTimeout = 1s; +constexpr auto kServiceCallTimeout = 2s; + +class CanceledError : public std::runtime_error { + public: + CanceledError() : std::runtime_error("goal canceled") {} +}; + +class AeAwbLockTestNode : public rclcpp::Node { + public: + using RunAeAwbLockTest = orbbec_camera_msgs::action::RunAeAwbLockTest; + using GoalHandle = rclcpp_action::ServerGoalHandle; + using GetInt32 = orbbec_camera_msgs::srv::GetInt32; + using SetInt32 = orbbec_camera_msgs::srv::SetInt32; + using GetAwbGain = orbbec_camera_msgs::srv::GetAwbGain; + using SetAwbGain = orbbec_camera_msgs::srv::SetAwbGain; + using SetBool = std_srvs::srv::SetBool; + + AeAwbLockTestNode() : Node("ae_awb_lock_test_node") { + get_status_client_ = create_client("get_color_ae_awb_status"); + get_awb_gain_client_ = create_client("get_color_awb_gain"); + set_awb_gain_client_ = create_client("set_color_awb_gain"); + get_exposure_client_ = create_client("get_color_exposure"); + set_exposure_client_ = create_client("set_color_exposure"); + get_color_gain_client_ = create_client("get_color_gain"); + set_color_gain_client_ = create_client("set_color_gain"); + get_white_balance_client_ = create_client("get_white_balance"); + set_white_balance_client_ = create_client("set_white_balance"); + set_auto_exposure_client_ = create_client("set_color_auto_exposure"); + set_auto_white_balance_client_ = create_client("set_auto_white_balance"); + + action_server_ = rclcpp_action::create_server( + this, "run_ae_awb_lock_test", + std::bind(&AeAwbLockTestNode::handleGoal, this, std::placeholders::_1, + std::placeholders::_2), + std::bind(&AeAwbLockTestNode::handleCancel, this, std::placeholders::_1), + std::bind(&AeAwbLockTestNode::handleAccepted, this, std::placeholders::_1)); + } + + ~AeAwbLockTestNode() override { + shutting_down_.store(true); + std::lock_guard lock(worker_mutex_); + if (worker_.joinable()) { + worker_.join(); + } + } + + private: + struct ActiveGoalGuard { + explicit ActiveGoalGuard(std::atomic_bool& active) : active_(active) {} + ~ActiveGoalGuard() { active_.store(false); } + std::atomic_bool& active_; + }; + + rclcpp_action::GoalResponse handleGoal(const rclcpp_action::GoalUUID& uuid, + std::shared_ptr goal) { + (void)uuid; + (void)goal; + bool expected = false; + if (!goal_active_.compare_exchange_strong(expected, true)) { + RCLCPP_WARN(get_logger(), "Rejecting AE/AWB test goal: another goal is active"); + return rclcpp_action::GoalResponse::REJECT; + } + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + } + + rclcpp_action::CancelResponse handleCancel(const std::shared_ptr goal_handle) { + (void)goal_handle; + return rclcpp_action::CancelResponse::ACCEPT; + } + + void handleAccepted(const std::shared_ptr goal_handle) { + std::lock_guard lock(worker_mutex_); + if (worker_.joinable()) { + worker_.join(); + } + worker_ = std::thread(&AeAwbLockTestNode::execute, this, goal_handle); + } + + template + void waitForService(const typename rclcpp::Client::SharedPtr& client, + const std::string& service_name) { + if (!client->wait_for_service(kServiceWaitTimeout)) { + throw std::runtime_error(service_name + " is not available"); + } + } + + template + typename ServiceT::Response::SharedPtr callService( + const typename rclcpp::Client::SharedPtr& client, + const typename ServiceT::Request::SharedPtr& request, const std::string& service_name) { + auto future = client->async_send_request(request); + if (future.wait_for(kServiceCallTimeout) != std::future_status::ready) { + throw std::runtime_error(service_name + " timed out"); + } + auto response = future.get(); + if (!response->success) { + throw std::runtime_error(service_name + " failed: " + response->message); + } + return response; + } + + void waitForRequiredServices() { + waitForService(get_status_client_, "get_color_ae_awb_status"); + waitForService(get_awb_gain_client_, "get_color_awb_gain"); + waitForService(set_awb_gain_client_, "set_color_awb_gain"); + waitForService(get_exposure_client_, "get_color_exposure"); + waitForService(set_exposure_client_, "set_color_exposure"); + waitForService(get_color_gain_client_, "get_color_gain"); + waitForService(set_color_gain_client_, "set_color_gain"); + waitForService(get_white_balance_client_, "get_white_balance"); + waitForService(set_white_balance_client_, "set_white_balance"); + waitForService(set_auto_exposure_client_, "set_color_auto_exposure"); + waitForService(set_auto_white_balance_client_, "set_auto_white_balance"); + } + + int32_t getIntValue(const rclcpp::Client::SharedPtr& client, + const std::string& service_name) { + return callService(client, std::make_shared(), service_name)->data; + } + + int32_t getAeAwbStatus() { return getIntValue(get_status_client_, "get_color_ae_awb_status"); } + + void setIntValue(const rclcpp::Client::SharedPtr& client, + const std::string& service_name, int32_t value) { + auto request = std::make_shared(); + request->data = value; + callService(client, request, service_name); + } + + GetAwbGain::Response::SharedPtr getAwbGain() { + return callService(get_awb_gain_client_, std::make_shared(), + "get_color_awb_gain"); + } + + void setAwbGain(uint16_t r_gain, uint16_t b_gain, uint16_t g_gain) { + auto request = std::make_shared(); + request->r_gain = r_gain; + request->b_gain = b_gain; + request->g_gain = g_gain; + callService(set_awb_gain_client_, request, "set_color_awb_gain"); + } + + void setBoolValue(const rclcpp::Client::SharedPtr& client, + const std::string& service_name, bool value) { + auto request = std::make_shared(); + request->data = value; + callService(client, request, service_name); + } + + void setAutoMode(bool enabled) { + setBoolValue(set_auto_exposure_client_, "set_color_auto_exposure", enabled); + setBoolValue(set_auto_white_balance_client_, "set_auto_white_balance", enabled); + } + + void restoreAutoMode() noexcept { + try { + setBoolValue(set_auto_exposure_client_, "set_color_auto_exposure", true); + } catch (const std::exception& e) { + RCLCPP_ERROR(get_logger(), "Failed to restore auto exposure: %s", e.what()); + } + try { + setBoolValue(set_auto_white_balance_client_, "set_auto_white_balance", true); + } catch (const std::exception& e) { + RCLCPP_ERROR(get_logger(), "Failed to restore auto white balance: %s", e.what()); + } + } + + void throwIfCanceled(const std::shared_ptr& goal_handle) const { + if (shutting_down_.load() || goal_handle->is_canceling()) { + throw CanceledError(); + } + } + + void publishFeedback(const std::shared_ptr& goal_handle, const std::string& phase, + int32_t status) { + auto feedback = std::make_shared(); + feedback->phase = phase; + feedback->ae_awb_status = status; + goal_handle->publish_feedback(feedback); + } + + void warnIfChanged(const char* name, int64_t captured, int64_t actual) { + if (captured != actual) { + RCLCPP_WARN(get_logger(), "%s changed from %lld to %lld after writeback", name, + static_cast(captured), static_cast(actual)); + } + } + + void execute(const std::shared_ptr goal_handle) { + ActiveGoalGuard active_guard(goal_active_); + auto result = std::make_shared(); + bool workflow_started = false; + + try { + waitForRequiredServices(); + publishFeedback(goal_handle, "waiting_for_services", getAeAwbStatus()); + throwIfCanceled(goal_handle); + + workflow_started = true; + setAutoMode(true); + publishFeedback(goal_handle, "enabling_auto", getAeAwbStatus()); + + const uint32_t requested_timeout = goal_handle->get_goal()->timeout_ms; + const uint32_t timeout_ms = requested_timeout == 0 ? kDefaultTimeoutMs : requested_timeout; + const auto deadline = + std::chrono::steady_clock::now() + std::chrono::milliseconds(timeout_ms); + + int32_t status = 0; + do { + throwIfCanceled(goal_handle); + if (std::chrono::steady_clock::now() >= deadline) { + throw std::runtime_error("timed out waiting for AE/AWB convergence"); + } + status = getAeAwbStatus(); + publishFeedback(goal_handle, "waiting_for_convergence", status); + if (status != kAeAwbConverged) { + std::this_thread::sleep_for(kPollInterval); + } + } while (status != kAeAwbConverged); + + throwIfCanceled(goal_handle); + publishFeedback(goal_handle, "capturing_parameters", getAeAwbStatus()); + result->captured_exposure = getIntValue(get_exposure_client_, "get_color_exposure"); + result->captured_color_gain = getIntValue(get_color_gain_client_, "get_color_gain"); + auto captured_awb_gain = getAwbGain(); + result->captured_awb_r_gain = captured_awb_gain->r_gain; + result->captured_awb_b_gain = captured_awb_gain->b_gain; + result->captured_awb_g_gain = captured_awb_gain->g_gain; + result->captured_color_temperature = + getIntValue(get_white_balance_client_, "get_white_balance"); + + throwIfCanceled(goal_handle); + publishFeedback(goal_handle, "disabling_auto", getAeAwbStatus()); + setAutoMode(false); + throwIfCanceled(goal_handle); + + publishFeedback(goal_handle, "applying_exposure", getAeAwbStatus()); + setIntValue(set_exposure_client_, "set_color_exposure", result->captured_exposure); + throwIfCanceled(goal_handle); + + publishFeedback(goal_handle, "applying_color_gain", getAeAwbStatus()); + setIntValue(set_color_gain_client_, "set_color_gain", result->captured_color_gain); + throwIfCanceled(goal_handle); + + publishFeedback(goal_handle, "applying_awb_gain", getAeAwbStatus()); + setAwbGain(result->captured_awb_r_gain, result->captured_awb_b_gain, + result->captured_awb_g_gain); + throwIfCanceled(goal_handle); + + publishFeedback(goal_handle, "applying_color_temperature", getAeAwbStatus()); + setIntValue(set_white_balance_client_, "set_white_balance", + result->captured_color_temperature); + throwIfCanceled(goal_handle); + + publishFeedback(goal_handle, "verifying", getAeAwbStatus()); + result->actual_exposure = getIntValue(get_exposure_client_, "get_color_exposure"); + result->actual_color_gain = getIntValue(get_color_gain_client_, "get_color_gain"); + auto actual_awb_gain = getAwbGain(); + result->actual_awb_r_gain = actual_awb_gain->r_gain; + result->actual_awb_b_gain = actual_awb_gain->b_gain; + result->actual_awb_g_gain = actual_awb_gain->g_gain; + result->actual_color_temperature = + getIntValue(get_white_balance_client_, "get_white_balance"); + throwIfCanceled(goal_handle); + + if (result->captured_awb_r_gain != result->actual_awb_r_gain || + result->captured_awb_b_gain != result->actual_awb_b_gain || + result->captured_awb_g_gain != result->actual_awb_g_gain) { + throw std::runtime_error("AWB gain readback does not match the captured value"); + } + + warnIfChanged("Exposure", result->captured_exposure, result->actual_exposure); + warnIfChanged("Color gain", result->captured_color_gain, result->actual_color_gain); + warnIfChanged("Color temperature", result->captured_color_temperature, + result->actual_color_temperature); + + result->success = true; + result->message = "AE/AWB capture and lock-in completed"; + publishFeedback(goal_handle, "completed", getAeAwbStatus()); + goal_handle->succeed(result); + } catch (const CanceledError&) { + if (workflow_started) { + restoreAutoMode(); + } + result->success = false; + result->message = "AE/AWB test canceled"; + goal_handle->canceled(result); + } catch (const std::exception& e) { + if (workflow_started) { + restoreAutoMode(); + } + result->success = false; + result->message = e.what(); + RCLCPP_ERROR(get_logger(), "AE/AWB test failed: %s", e.what()); + goal_handle->abort(result); + } + } + + rclcpp_action::Server::SharedPtr action_server_; + + rclcpp::Client::SharedPtr get_status_client_; + rclcpp::Client::SharedPtr get_awb_gain_client_; + rclcpp::Client::SharedPtr set_awb_gain_client_; + rclcpp::Client::SharedPtr get_exposure_client_; + rclcpp::Client::SharedPtr set_exposure_client_; + rclcpp::Client::SharedPtr get_color_gain_client_; + rclcpp::Client::SharedPtr set_color_gain_client_; + rclcpp::Client::SharedPtr get_white_balance_client_; + rclcpp::Client::SharedPtr set_white_balance_client_; + rclcpp::Client::SharedPtr set_auto_exposure_client_; + rclcpp::Client::SharedPtr set_auto_white_balance_client_; + + std::atomic_bool goal_active_{false}; + std::atomic_bool shutting_down_{false}; + std::mutex worker_mutex_; + std::thread worker_; +}; + +} // namespace + +int main(int argc, char** argv) { + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(node); + executor.spin(); + rclcpp::shutdown(); + return 0; +} diff --git a/orbbec_camera/include/orbbec_camera/ob_camera_node.h b/orbbec_camera/include/orbbec_camera/ob_camera_node.h index 366b65d9..34619dff 100644 --- a/orbbec_camera/include/orbbec_camera/ob_camera_node.h +++ b/orbbec_camera/include/orbbec_camera/ob_camera_node.h @@ -57,12 +57,14 @@ #include "orbbec_camera_msgs/msg/depth_filters_status.hpp" #include "orbbec_camera_msgs/srv/get_device_config.hpp" #include "orbbec_camera_msgs/srv/get_device_info.hpp" +#include "orbbec_camera_msgs/srv/get_awb_gain.hpp" #include "orbbec_camera_msgs/msg/extrinsics.hpp" #include "orbbec_camera_msgs/msg/metadata.hpp" #include "orbbec_camera_msgs/msg/imu_info.hpp" #include "orbbec_camera_msgs/srv/get_int32.hpp" #include "orbbec_camera_msgs/srv/get_string.hpp" #include "orbbec_camera_msgs/srv/set_int32.hpp" +#include "orbbec_camera_msgs/srv/set_awb_gain.hpp" #include "orbbec_camera_msgs/srv/get_bool.hpp" #include "orbbec_camera_msgs/srv/set_string.hpp" #include "orbbec_camera_msgs/srv/set_filter.hpp" @@ -121,6 +123,8 @@ using GetDeviceInfo = orbbec_camera_msgs::srv::GetDeviceInfo; using Extrinsics = orbbec_camera_msgs::msg::Extrinsics; using SetInt32 = orbbec_camera_msgs::srv::SetInt32; using GetInt32 = orbbec_camera_msgs::srv::GetInt32; +using GetAwbGain = orbbec_camera_msgs::srv::GetAwbGain; +using SetAwbGain = orbbec_camera_msgs::srv::SetAwbGain; using GetString = orbbec_camera_msgs::srv::GetString; using SetString = orbbec_camera_msgs::srv::SetString; using SetBool = std_srvs::srv::SetBool; @@ -414,6 +418,15 @@ class OBCameraNode { void setAutoWhiteBalanceCallback(const std::shared_ptr& request, std::shared_ptr& response); + void getAeAwbStatusCallback(const std::shared_ptr& request, + std::shared_ptr& response); + + void getAwbGainCallback(const std::shared_ptr& request, + std::shared_ptr& response); + + void setAwbGainCallback(const std::shared_ptr& request, + std::shared_ptr& response); + void setAutoExposureCallback(const std::shared_ptr& request, std::shared_ptr& response, const stream_index_pair& stream_index); @@ -722,6 +735,9 @@ class OBCameraNode { rclcpp::Service::SharedPtr set_white_balance_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_; + rclcpp::Service::SharedPtr get_awb_gain_srv_; + rclcpp::Service::SharedPtr set_awb_gain_srv_; rclcpp::Service::SharedPtr get_sdk_version_srv_; rclcpp::Service::SharedPtr switch_ir_camera_srv_; rclcpp::Service::SharedPtr export_config_json_srv_; diff --git a/orbbec_camera/package.xml b/orbbec_camera/package.xml index b38780e1..5eebc0d4 100644 --- a/orbbec_camera/package.xml +++ b/orbbec_camera/package.xml @@ -20,6 +20,7 @@ orbbec_camera_msgs builtin_interfaces rclcpp + rclcpp_action sensor_msgs std_msgs std_srvs diff --git a/orbbec_camera/src/ros_service.cpp b/orbbec_camera/src/ros_service.cpp index 8c4be54b..492de828 100644 --- a/orbbec_camera/src/ros_service.cpp +++ b/orbbec_camera/src/ros_service.cpp @@ -298,6 +298,27 @@ void OBCameraNode::setupCameraCtrlServices() { std::shared_ptr response) { setAutoWhiteBalanceCallback(request, response); }); + if (isPropertyReadable(device_, OB_PROP_COLOR_AE_AWB_STAT_INT)) { + get_ae_awb_status_srv_ = node_->create_service( + "get_color_ae_awb_status", [this](const std::shared_ptr request, + std::shared_ptr response) { + getAeAwbStatusCallback(request, response); + }); + } + if (isPropertyReadable(device_, OB_STRUCT_COLOR_AWB_GAIN)) { + get_awb_gain_srv_ = node_->create_service( + "get_color_awb_gain", [this](const std::shared_ptr request, + std::shared_ptr response) { + getAwbGainCallback(request, response); + }); + } + if (isPropertyWritable(device_, OB_STRUCT_COLOR_AWB_GAIN)) { + set_awb_gain_srv_ = node_->create_service( + "set_color_awb_gain", [this](const std::shared_ptr request, + std::shared_ptr response) { + setAwbGainCallback(request, response); + }); + } get_device_srv_ = node_->create_service( "get_device_info", [this](const std::shared_ptr request, std::shared_ptr response) { @@ -1198,6 +1219,75 @@ void OBCameraNode::setAutoWhiteBalanceCallback(const std::shared_ptr& request, + std::shared_ptr& response) { + (void)request; + try { + response->data = device_->getIntProperty(OB_PROP_COLOR_AE_AWB_STAT_INT); + response->success = true; + } 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::getAwbGainCallback(const std::shared_ptr& request, + std::shared_ptr& response) { + (void)request; + try { + OBAwbGainParams gain{}; + uint32_t size = sizeof(gain); + device_->getStructuredData(OB_STRUCT_COLOR_AWB_GAIN, reinterpret_cast(&gain), &size); + response->r_gain = gain.rGain; + response->b_gain = gain.bGain; + response->g_gain = gain.gGain; + response->success = true; + } 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::setAwbGainCallback(const std::shared_ptr& request, + std::shared_ptr& response) { + try { + if (device_->getBoolProperty(OB_PROP_COLOR_AUTO_WHITE_BALANCE_BOOL)) { + response->success = false; + response->message = "auto white balance is enabled"; + return; + } + + OBAwbGainParams gain{}; + gain.rGain = request->r_gain; + gain.bGain = request->b_gain; + gain.gGain = request->g_gain; + device_->setStructuredData(OB_STRUCT_COLOR_AWB_GAIN, reinterpret_cast(&gain), + sizeof(gain)); + response->success = true; + } 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::setAutoExposureCallback( const std::shared_ptr& request, std::shared_ptr& response, diff --git a/orbbec_camera_msgs/CMakeLists.txt b/orbbec_camera_msgs/CMakeLists.txt index 347f4ed7..7cd2a763 100644 --- a/orbbec_camera_msgs/CMakeLists.txt +++ b/orbbec_camera_msgs/CMakeLists.txt @@ -7,6 +7,7 @@ endif() # find dependencies find_package(ament_cmake REQUIRED) +find_package(action_msgs REQUIRED) find_package(rosidl_default_generators REQUIRED) find_package(sensor_msgs REQUIRED) find_package(std_msgs REQUIRED) @@ -31,16 +32,20 @@ rosidl_generate_interfaces( "srv/GetDeviceInfo.srv" "srv/GetCameraInfo.srv" "srv/GetInt32.srv" + "srv/GetAwbGain.srv" "srv/GetString.srv" "srv/SetFilter.srv" "srv/SetInt32.srv" + "srv/SetAwbGain.srv" "srv/SetString.srv" "srv/SetArrays.srv" "srv/GetUserCalibParams.srv" "srv/SetUserCalibParams.srv" "srv/SetBagRecording.srv" "srv/SetStreamProfile.srv" + "action/RunAeAwbLockTest.action" DEPENDENCIES + action_msgs sensor_msgs std_msgs ) diff --git a/orbbec_camera_msgs/action/RunAeAwbLockTest.action b/orbbec_camera_msgs/action/RunAeAwbLockTest.action new file mode 100644 index 00000000..432edc57 --- /dev/null +++ b/orbbec_camera_msgs/action/RunAeAwbLockTest.action @@ -0,0 +1,23 @@ +# A value of zero uses the sample's default timeout of 10 seconds. +uint32 timeout_ms +--- +bool success +string message + +int32 captured_exposure +int32 captured_color_gain +uint16 captured_awb_r_gain +uint16 captured_awb_b_gain +uint16 captured_awb_g_gain +int32 captured_color_temperature + +int32 actual_exposure +int32 actual_color_gain +uint16 actual_awb_r_gain +uint16 actual_awb_b_gain +uint16 actual_awb_g_gain +int32 actual_color_temperature +--- +string phase +# Raw SDK status read for this phase. +int32 ae_awb_status diff --git a/orbbec_camera_msgs/package.xml b/orbbec_camera_msgs/package.xml index 5eea1a4e..91dfd7af 100644 --- a/orbbec_camera_msgs/package.xml +++ b/orbbec_camera_msgs/package.xml @@ -9,6 +9,7 @@ ament_cmake + action_msgs ament_lint_auto ament_lint_common rosidl_default_generators diff --git a/orbbec_camera_msgs/srv/GetAwbGain.srv b/orbbec_camera_msgs/srv/GetAwbGain.srv new file mode 100644 index 00000000..9fc71e2f --- /dev/null +++ b/orbbec_camera_msgs/srv/GetAwbGain.srv @@ -0,0 +1,7 @@ +--- +# Raw Q8.8 AWB channel gains. +uint16 r_gain +uint16 b_gain +uint16 g_gain +bool success +string message diff --git a/orbbec_camera_msgs/srv/SetAwbGain.srv b/orbbec_camera_msgs/srv/SetAwbGain.srv new file mode 100644 index 00000000..8b80e9c2 --- /dev/null +++ b/orbbec_camera_msgs/srv/SetAwbGain.srv @@ -0,0 +1,7 @@ +# Raw Q8.8 AWB channel gains. +uint16 r_gain +uint16 b_gain +uint16 g_gain +--- +bool success +string message