diff --git a/orbbec_camera/examples/README.MD b/orbbec_camera/examples/README.MD index 1eaaf1fa..4d379c24 100644 --- a/orbbec_camera/examples/README.MD +++ b/orbbec_camera/examples/README.MD @@ -8,7 +8,6 @@ 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) | +| [AE/AWB lock test](./ae_awb_lock) | How to verify the AE/AWB convergence, frame-metadata capture, and manual lock-in flow through ROS 2. | ⭐️⭐️ | [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 index 04cace1d..c7ce9dd3 100644 --- a/orbbec_camera/examples/ae_awb_lock/README.md +++ b/orbbec_camera/examples/ae_awb_lock/README.md @@ -1,7 +1,7 @@ # 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. +lock-in flow through the camera driver's services and color-frame metadata. Run the camera driver and this sample in the same namespace: @@ -19,9 +19,11 @@ ros2 action send_goal \ --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: +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 @@ -35,3 +37,7 @@ 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 +within two seconds or if `exposure`, `gain`, or `white_balance` is missing. 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 index 94025e8f..0381df41 100644 --- 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 @@ -7,16 +7,19 @@ #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" @@ -31,6 +34,7 @@ constexpr uint32_t kDefaultTimeoutMs = 10000; constexpr auto kPollInterval = 100ms; constexpr auto kServiceWaitTimeout = 1s; constexpr auto kServiceCallTimeout = 2s; +constexpr auto kMetadataWaitTimeout = 2s; class CanceledError : public std::runtime_error { public: @@ -46,8 +50,13 @@ class AeAwbLockTestNode : public rclcpp::Node { 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; 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"); @@ -70,6 +79,7 @@ class AeAwbLockTestNode : public rclcpp::Node { ~AeAwbLockTestNode() override { shutting_down_.store(true); + metadata_cv_.notify_all(); std::lock_guard lock(worker_mutex_); if (worker_.joinable()) { worker_.join(); @@ -77,6 +87,12 @@ class AeAwbLockTestNode : public rclcpp::Node { } 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); } @@ -152,6 +168,38 @@ class AeAwbLockTestNode : public rclcpp::Node { int32_t getAeAwbStatus() { return getIntValue(get_status_client_, "get_color_ae_awb_status"); } + void colorMetadataCallback(const Metadata::ConstSharedPtr& metadata) { + { + std::lock_guard lock(metadata_mutex_); + latest_color_metadata_ = metadata; + } + metadata_cv_.notify_all(); + } + + ColorFrameMetadata getLatestColorMetadata() { + Metadata::ConstSharedPtr metadata; + { + std::unique_lock lock(metadata_mutex_); + if (!metadata_cv_.wait_for(lock, kMetadataWaitTimeout, [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) { auto request = std::make_shared(); @@ -252,14 +300,14 @@ class AeAwbLockTestNode : public rclcpp::Node { 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"); + const auto captured_metadata = getLatestColorMetadata(); + 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(); 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()); @@ -329,6 +377,7 @@ class AeAwbLockTestNode : public rclcpp::Node { } 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_; @@ -344,6 +393,9 @@ class AeAwbLockTestNode : public rclcpp::Node { 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_; };