diff --git a/orbbec_camera/CMakeLists.txt b/orbbec_camera/CMakeLists.txt index 9b5709a1..f1bec2d9 100644 --- a/orbbec_camera/CMakeLists.txt +++ b/orbbec_camera/CMakeLists.txt @@ -34,6 +34,7 @@ set(dependencies orbbec_camera_msgs rcl_interfaces rclcpp + rclcpp_action rclcpp_components rmw sensor_msgs @@ -276,6 +277,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) orbbec_target_dependencies(topic_statistics_node statistics_msgs) @@ -382,7 +384,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 536d3d45..f598b6f7 100644 --- a/orbbec_camera/examples/README.MD +++ b/orbbec_camera/examples/README.MD @@ -13,5 +13,6 @@ For command-line maintenance and diagnostic utilities, see the official | :---: | --- | :---: | --- | | [Net camera](./net_camera) | Launch supported Orbbec network cameras. | ⭐️ | [Network camera guide](https://orbbec.github.io/OrbbecSDK_ROS2/en/source/camera_devices/5_advanced_guide/configuration/net_camera.html) | | [GMSL camera](./gmsl_camera) | Launch single, multiple, or synchronized GMSL cameras. | ⭐️ | [GMSL camera guide](https://orbbec.github.io/OrbbecSDK_ROS2/en/source/camera_devices/5_advanced_guide/multi_camera/gmsl_camera.html) | +| [AE/AWB lock test](./ae_awb_lock) | Verify AE/AWB convergence, frame-metadata capture, and manual lock-in through ROS 2. | ⭐️⭐️ | [README](./ae_awb_lock/README.md) | | [Benchmark](./benchmark) | Benchmark the performance of different camera configurations. | ⭐️⭐️ | [Performance benchmark tools](https://orbbec.github.io/OrbbecSDK_ROS2/en/source/camera_devices/6_benchmark/benchmark_tools.html) | | [Multi-camera synchronization verification](./multi_camera_synced_verification_tool) | Verify synchronization accuracy across multiple cameras. | ⭐️⭐️⭐️ | [Synchronization verification guide](https://orbbec.github.io/OrbbecSDK_ROS2/en/source/camera_devices/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..2812a166 --- /dev/null +++ b/orbbec_camera/examples/ae_awb_lock/README.md @@ -0,0 +1,46 @@ +# 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 and color-frame metadata. + +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 subscribes to the relative `color/metadata` topic. It enables auto exposure and auto +white balance, waits until the SDK status equals `1`, captures exposure, color gain, and color +temperature from the latest color-frame metadata, and reads AWB R/B/G gains through the structured +property service. It then 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. + +The color stream must be enabled, and `/camera/color/metadata` must be available when using the +`/camera` namespace. The action fails instead of writing default values if no metadata arrives +before the goal timeout or if `exposure`, `gain`, or `white_balance` is missing. The goal timeout +covers the main workflow, including service discovery, service calls, convergence, capture, +writeback, and verification. Restoring auto exposure and auto white balance after a failure uses a +separate best-effort timeout. 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..01977f53 --- /dev/null +++ b/orbbec_camera/examples/ae_awb_lock/ae_awb_lock_test_node.cpp @@ -0,0 +1,419 @@ +// 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 +#include + +#include "orbbec_camera_msgs/action/run_ae_awb_lock_test.hpp" +#include "orbbec_camera_msgs/msg/metadata.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 kRestoreServiceTimeout = 5s; + +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; + using Metadata = orbbec_camera_msgs::msg::Metadata; + using Deadline = std::chrono::steady_clock::time_point; + + AeAwbLockTestNode() : Node("ae_awb_lock_test_node") { + color_metadata_subscription_ = create_subscription( + "color/metadata", rclcpp::SensorDataQoS(), + std::bind(&AeAwbLockTestNode::colorMetadataCallback, this, std::placeholders::_1)); + + 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); + metadata_cv_.notify_all(); + std::lock_guard lock(worker_mutex_); + if (worker_.joinable()) { + worker_.join(); + } + } + + private: + struct ColorFrameMetadata { + int32_t exposure; + int32_t gain; + int32_t white_balance; + }; + + 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, const Deadline& deadline) { + const auto now = std::chrono::steady_clock::now(); + if (now >= deadline || !client->wait_for_service(deadline - now)) { + 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, + const Deadline& deadline) { + auto pending_request = client->async_send_request(request); + if (pending_request.wait_until(deadline) != std::future_status::ready) { + client->remove_pending_request(pending_request); + throw std::runtime_error(service_name + " timed out"); + } + auto response = pending_request.get(); + if (!response->success) { + throw std::runtime_error(service_name + " failed: " + response->message); + } + return response; + } + + void waitForRequiredServices(const Deadline& deadline) { + waitForService(get_status_client_, "get_color_ae_awb_status", deadline); + waitForService(get_awb_gain_client_, "get_color_awb_gain", deadline); + waitForService(set_awb_gain_client_, "set_color_awb_gain", deadline); + waitForService(get_exposure_client_, "get_color_exposure", deadline); + waitForService(set_exposure_client_, "set_color_exposure", deadline); + waitForService(get_color_gain_client_, "get_color_gain", deadline); + waitForService(set_color_gain_client_, "set_color_gain", deadline); + waitForService(get_white_balance_client_, "get_white_balance", deadline); + waitForService(set_white_balance_client_, "set_white_balance", deadline); + waitForService(set_auto_exposure_client_, "set_color_auto_exposure", deadline); + waitForService(set_auto_white_balance_client_, "set_auto_white_balance", deadline); + } + + int32_t getIntValue(const rclcpp::Client::SharedPtr& client, + const std::string& service_name, const Deadline& deadline) { + return callService(client, std::make_shared(), service_name, + deadline) + ->data; + } + + int32_t getAeAwbStatus(const Deadline& deadline) { + return getIntValue(get_status_client_, "get_color_ae_awb_status", deadline); + } + + void colorMetadataCallback(const Metadata::ConstSharedPtr& metadata) { + { + std::lock_guard lock(metadata_mutex_); + latest_color_metadata_ = metadata; + } + metadata_cv_.notify_all(); + } + + ColorFrameMetadata getLatestColorMetadata(const Deadline& deadline) { + Metadata::ConstSharedPtr metadata; + { + std::unique_lock lock(metadata_mutex_); + if (!metadata_cv_.wait_until(lock, deadline, [this] { + return latest_color_metadata_ != nullptr || shutting_down_.load(); + })) { + throw std::runtime_error("timed out waiting for color frame metadata"); + } + if (shutting_down_.load()) { + throw CanceledError(); + } + metadata = latest_color_metadata_; + } + + try { + const auto json = nlohmann::json::parse(metadata->json_data); + return {json.at("exposure").get(), json.at("gain").get(), + json.at("white_balance").get()}; + } catch (const nlohmann::json::exception& e) { + throw std::runtime_error(std::string("invalid color frame metadata: ") + e.what()); + } + } + + void setIntValue(const rclcpp::Client::SharedPtr& client, + const std::string& service_name, int32_t value, const Deadline& deadline) { + auto request = std::make_shared(); + request->data = value; + callService(client, request, service_name, deadline); + } + + GetAwbGain::Response::SharedPtr getAwbGain(const Deadline& deadline) { + return callService(get_awb_gain_client_, std::make_shared(), + "get_color_awb_gain", deadline); + } + + void setAwbGain(uint16_t r_gain, uint16_t b_gain, uint16_t g_gain, const Deadline& deadline) { + 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", deadline); + } + + void setBoolValue(const rclcpp::Client::SharedPtr& client, + const std::string& service_name, bool value, const Deadline& deadline) { + auto request = std::make_shared(); + request->data = value; + callService(client, request, service_name, deadline); + } + + void setAutoMode(bool enabled, const Deadline& deadline) { + setBoolValue(set_auto_exposure_client_, "set_color_auto_exposure", enabled, deadline); + setBoolValue(set_auto_white_balance_client_, "set_auto_white_balance", enabled, deadline); + } + + void restoreAutoMode() noexcept { + try { + const auto deadline = std::chrono::steady_clock::now() + kRestoreServiceTimeout; + setBoolValue(set_auto_exposure_client_, "set_color_auto_exposure", true, deadline); + } catch (const std::exception& e) { + RCLCPP_ERROR(get_logger(), "Failed to restore auto exposure: %s", e.what()); + } + try { + const auto deadline = std::chrono::steady_clock::now() + kRestoreServiceTimeout; + setBoolValue(set_auto_white_balance_client_, "set_auto_white_balance", true, deadline); + } 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; + 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); + + try { + waitForRequiredServices(deadline); + publishFeedback(goal_handle, "waiting_for_services", getAeAwbStatus(deadline)); + throwIfCanceled(goal_handle); + + workflow_started = true; + setAutoMode(true, deadline); + publishFeedback(goal_handle, "enabling_auto", getAeAwbStatus(deadline)); + + 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(deadline); + 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(deadline)); + const auto captured_metadata = getLatestColorMetadata(deadline); + result->captured_exposure = captured_metadata.exposure; + result->captured_color_gain = captured_metadata.gain; + result->captured_color_temperature = captured_metadata.white_balance; + auto captured_awb_gain = getAwbGain(deadline); + 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; + + throwIfCanceled(goal_handle); + publishFeedback(goal_handle, "disabling_auto", getAeAwbStatus(deadline)); + setAutoMode(false, deadline); + throwIfCanceled(goal_handle); + + publishFeedback(goal_handle, "applying_exposure", getAeAwbStatus(deadline)); + setIntValue(set_exposure_client_, "set_color_exposure", result->captured_exposure, deadline); + throwIfCanceled(goal_handle); + + publishFeedback(goal_handle, "applying_color_gain", getAeAwbStatus(deadline)); + setIntValue(set_color_gain_client_, "set_color_gain", result->captured_color_gain, deadline); + throwIfCanceled(goal_handle); + + publishFeedback(goal_handle, "applying_awb_gain", getAeAwbStatus(deadline)); + setAwbGain(result->captured_awb_r_gain, result->captured_awb_b_gain, + result->captured_awb_g_gain, deadline); + throwIfCanceled(goal_handle); + + publishFeedback(goal_handle, "applying_color_temperature", getAeAwbStatus(deadline)); + setIntValue(set_white_balance_client_, "set_white_balance", + result->captured_color_temperature, deadline); + throwIfCanceled(goal_handle); + + publishFeedback(goal_handle, "verifying", getAeAwbStatus(deadline)); + result->actual_exposure = getIntValue(get_exposure_client_, "get_color_exposure", deadline); + result->actual_color_gain = getIntValue(get_color_gain_client_, "get_color_gain", deadline); + auto actual_awb_gain = getAwbGain(deadline); + 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", deadline); + 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(deadline)); + 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::Subscription::SharedPtr color_metadata_subscription_; + + 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}; + Metadata::ConstSharedPtr latest_color_metadata_; + std::mutex metadata_mutex_; + std::condition_variable metadata_cv_; + 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/constants.h b/orbbec_camera/include/orbbec_camera/constants.h index d52c1faf..e54656c4 100644 --- a/orbbec_camera/include/orbbec_camera/constants.h +++ b/orbbec_camera/include/orbbec_camera/constants.h @@ -24,7 +24,7 @@ #define OB_ROS_MAJOR_VERSION 2 #define OB_ROS_MINOR_VERSION 9 -#define OB_ROS_PATCH_VERSION 3 +#define OB_ROS_PATCH_VERSION 3.1 #ifndef STRINGIFY #define STRINGIFY(arg) #arg diff --git a/orbbec_camera/include/orbbec_camera/ob_camera_node.h b/orbbec_camera/include/orbbec_camera/ob_camera_node.h index 9899af92..6a21c296 100644 --- a/orbbec_camera/include/orbbec_camera/ob_camera_node.h +++ b/orbbec_camera/include/orbbec_camera/ob_camera_node.h @@ -54,12 +54,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" @@ -137,6 +139,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; @@ -435,6 +439,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); @@ -750,6 +763,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 790cf788..2c43c8c3 100644 --- a/orbbec_camera/package.xml +++ b/orbbec_camera/package.xml @@ -23,6 +23,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 a263ce1c..4fd14f55 100644 --- a/orbbec_camera/src/ros_service.cpp +++ b/orbbec_camera/src/ros_service.cpp @@ -303,6 +303,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) { @@ -1266,6 +1287,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 b71437b9..5212ea6d 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..4e22deb7 --- /dev/null +++ b/orbbec_camera_msgs/action/RunAeAwbLockTest.action @@ -0,0 +1,24 @@ +# Timeout budget for the main workflow. Recovery after failure uses a separate timeout. +# A value of zero uses the 10-second default. +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