Merge branch 'v2/develop' into v2-main

orbbec_camera/CMakeLists.txt

@@ -34,6 +34,7 @@ set(dependencies
     orbbec_camera_msgs
     rcl_interfaces
     rclcpp
+    rclcpp_action
     rclcpp_components
     rmw
     sensor_msgs
@@ -158,6 +159,8 @@ set(SOURCE_FILES
   src/dynamic_params.cpp
   src/image_publisher.cpp
   src/frame_timestamp_csv_logger.cpp
+  src/imu_timestamp_csv_logger.cpp
+  src/timestamp_csv_logger.cpp
   src/ob_camera_node_driver.cpp
   src/ob_camera_node.cpp
   src/ob_lidar_node.cpp
@@ -276,6 +279,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 +386,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)

orbbec_camera/SDK/arm64/include/libobsensor/h/Device.h

@@ -401,7 +401,9 @@ OB_EXPORT void ob_device_set_state_changed_callback(ob_device *device, ob_device
 
 /**
  * @brief Enable or disable the device heartbeat.
- * @brief After enable the device heartbeat, the sdk will start a thread to send heartbeat signal to the device error every 3 seconds.
+ *
+ * When enabled, the SDK sends heartbeat signals at the device monitor polling interval. The default interval is 3000 ms and can be adjusted with
+ * ob_device_set_monitor_poll_interval. If the device does not support heartbeat, an error is reported through @p error.
 
  * @attention If the device does not receive the heartbeat signal for a long time, it will be disconnected and rebooted.
  *
@@ -412,7 +414,7 @@ OB_EXPORT void ob_device_set_state_changed_callback(ob_device *device, ob_device
 OB_EXPORT void ob_device_enable_heartbeat(ob_device *device, bool enable, ob_error **error);
 
 /**
- * @brief Enable or disable the device firmware log.
+ * @brief Enable or disable the device firmware log. If the device does not support firmware log, an error is reported through @p error.
  *
  * @param[in] device The device object.
  * @param[in] enable Whether to enable the firmware log.
@@ -430,6 +432,27 @@ OB_EXPORT void ob_device_enable_firmware_log(ob_device *device, bool enable, ob_
  */
 OB_EXPORT bool ob_device_is_firmware_log_enabled(ob_device *device, ob_error **error);
 
+/**
+ * @brief Set the device monitor polling interval.
+ *
+ * @param[in] device The device object.
+ * @param[in] interval_ms The polling interval for device heartbeat and firmware log retrieval, in milliseconds. The valid range is [1000, 10000];
+ * values outside this range are clamped to the nearest bound. If the device does not support device monitor polling, an error is reported through @p error.
+ * @param[out] error Pointer to an error object that will be set if an error occurs.
+ */
+OB_EXPORT void ob_device_set_monitor_poll_interval(ob_device *device, uint32_t interval_ms, ob_error **error);
+
+/**
+ * @brief Get the device monitor polling interval.
+ *
+ * @param[in] device The device object.
+ * @param[out] error Pointer to an error object that will be set if an error occurs.
+ *
+ * @return uint32_t The device monitor polling interval in milliseconds. If the device does not support device monitor polling, an error is reported
+ * through @p error.
+ */
+OB_EXPORT uint32_t ob_device_get_monitor_poll_interval(ob_device *device, ob_error **error);
+
 /**
  * @brief Synchronize the device time (synchronize hardwarePPS time to device)
  *

orbbec_camera/SDK/arm64/include/libobsensor/h/ObTypes.h

@@ -1774,6 +1774,15 @@ typedef struct {
     uint32_t factor;        ///< Decimation factor
 } OBHardwareDecimationConfig, ob_hardware_decimation_config;
 
+/**
+ * @details Defines the gain values for RGB channels.
+ */
+typedef struct {
+    uint16_t rGain;  ///< Red Channel Gain
+    uint16_t bGain;  ///< Blue Channel Gain
+    uint16_t gGain;  ///< Green Channel Gain
+} OBAwbGainParams, ob_awb_gain_params;
+
 /**
  * @brief Frame metadata types
  * @brief The frame metadata is a set of meta info generated by the device for current individual frame.

orbbec_camera/SDK/arm64/include/libobsensor/h/Property.h

@@ -682,6 +682,19 @@ typedef enum {
      */
     OB_PROP_MJPEG_QUALITY_INT = 277,
 
+    /**
+     * @brief Color AE/AWB status.
+     * @param value
+     *   - 0: Converging.
+     *          Both AE and AWB algorithms are dynamically adjusting,
+     *          and the parameters have not yet stabilized.
+     *
+     *   - 1: Dual Convergence.
+     *          Both AE and AWB algorithms have completed convergence,
+     *          and the system is in a stable imaging state.
+     */
+    OB_PROP_COLOR_AE_AWB_STATUS_INT = 287,
+
     /**
      * @brief Baseline calibration parameters
      */
@@ -789,6 +802,12 @@ typedef enum {
      */
     OB_STRUCT_DEVICE_IP_ADDR_CONFIG_V2 = 1088,
 
+    /**
+     * @brief Color AWB gain parameters
+     * @see OBAwbGainParams
+     */
+    OB_STRUCT_COLOR_AWB_GAIN = 1097,
+
     /**
      * @brief Color camera auto exposure
      */
@@ -1133,6 +1152,7 @@ typedef enum {
 #define OB_PROP_DEPTH_SOFT_FILTER_BOOL OB_PROP_DEPTH_NOISE_REMOVAL_FILTER_BOOL
 #define OB_PROP_DEPTH_MAX_DIFF_INT OB_PROP_DEPTH_NOISE_REMOVAL_FILTER_MAX_DIFF_INT
 #define OB_PROP_DEPTH_MAX_SPECKLE_SIZE_INT OB_PROP_DEPTH_NOISE_REMOVAL_FILTER_MAX_SPECKLE_SIZE_INT
+#define OB_PROP_COLOR_AE_AWB_STAT_INT OB_PROP_COLOR_AE_AWB_STATUS_INT
 
 /**
  * @brief The data type used to describe all property settings

orbbec_camera/SDK/arm64/include/libobsensor/hpp/Device.hpp

@@ -669,7 +669,9 @@ public:
 
     /**
      * @brief Enable or disable the device heartbeat.
-     * @brief After enable the device heartbeat, the sdk will start a thread to send heartbeat signal to the device error every 3 seconds.
+     *
+     * When enabled, the SDK sends heartbeat signals at the device monitor polling interval. The default interval is 3000 ms and can be adjusted with
+     * setMonitorPollInterval. If the device does not support heartbeat, an exception is thrown.
      *
      * @attention If the device does not receive the heartbeat signal for a long time, it will be disconnected and rebooted.
      *
@@ -682,7 +684,7 @@ public:
     }
 
     /**
-     * @brief Enable or disable the device firmware log.
+     * @brief Enable or disable the device firmware log. If the device does not support firmware log, an exception is thrown.
      *
      * @param[in] enable Whether to enable the firmware log.
      */
@@ -704,6 +706,31 @@ public:
         return enable;
     }
 
+    /**
+     * @brief Set the device monitor polling interval.
+     *
+     * @param[in] intervalMs The polling interval for device heartbeat and firmware log retrieval, in milliseconds. The valid range is [1000, 10000];
+     * values outside this range are clamped to the nearest bound. If the device does not support device monitor polling, an exception is thrown.
+     */
+    void setMonitorPollInterval(uint32_t intervalMs) const {
+        ob_error *error = nullptr;
+        ob_device_set_monitor_poll_interval(impl_, intervalMs, &error);
+        Error::handle(&error);
+    }
+
+    /**
+     * @brief Get the device monitor polling interval.
+     *
+     * @return uint32_t The device monitor polling interval in milliseconds.
+     * @throws Error If the device does not support device monitor polling.
+     */
+    uint32_t getMonitorPollInterval() const {
+        ob_error *error      = nullptr;
+        auto      intervalMs = ob_device_get_monitor_poll_interval(impl_, &error);
+        Error::handle(&error);
+        return intervalMs;
+    }
+
     /**
      * @brief Get the supported multi device sync mode bitmap of the device.
      * @brief For example, if the return value is 0b00001100, it means the device supports @ref OB_MULTI_DEVICE_SYNC_MODE_PRIMARY and @ref

orbbec_camera/SDK/arm64/lib/OrbbecSDKConfig-release.cmake

@@ -8,12 +8,12 @@ set(CMAKE_IMPORT_FILE_VERSION 1)
 # Import target "ob::OrbbecSDK" for configuration "Release"
 set_property(TARGET ob::OrbbecSDK APPEND PROPERTY IMPORTED_CONFIGURATIONS RELEASE)
 set_target_properties(ob::OrbbecSDK PROPERTIES
-  IMPORTED_LOCATION_RELEASE "${_IMPORT_PREFIX}/lib/libOrbbecSDK.so.2.9.3"
+  IMPORTED_LOCATION_RELEASE "${_IMPORT_PREFIX}/lib/libOrbbecSDK.so.2.10.1"
   IMPORTED_SONAME_RELEASE "libOrbbecSDK.so.2"
   )
 
 list(APPEND _cmake_import_check_targets ob::OrbbecSDK )
-list(APPEND _cmake_import_check_files_for_ob::OrbbecSDK "${_IMPORT_PREFIX}/lib/libOrbbecSDK.so.2.9.3" )
+list(APPEND _cmake_import_check_files_for_ob::OrbbecSDK "${_IMPORT_PREFIX}/lib/libOrbbecSDK.so.2.10.1" )
 
 # Commands beyond this point should not need to know the version.
 set(CMAKE_IMPORT_FILE_VERSION)

orbbec_camera/SDK/arm64/lib/OrbbecSDKVersion.cmake

@@ -9,19 +9,19 @@
 # The variable CVF_VERSION must be set before calling configure_file().
 
 
-set(PACKAGE_VERSION "2.9.3")
+set(PACKAGE_VERSION "2.10.1")
 
 if(PACKAGE_VERSION VERSION_LESS PACKAGE_FIND_VERSION)
   set(PACKAGE_VERSION_COMPATIBLE FALSE)
 else()
 
-  if("2.9.3" MATCHES "^([0-9]+)\\.")
+  if("2.10.1" MATCHES "^([0-9]+)\\.")
     set(CVF_VERSION_MAJOR "${CMAKE_MATCH_1}")
     if(NOT CVF_VERSION_MAJOR VERSION_EQUAL 0)
       string(REGEX REPLACE "^0+" "" CVF_VERSION_MAJOR "${CVF_VERSION_MAJOR}")
     endif()
   else()
-    set(CVF_VERSION_MAJOR "2.9.3")
+    set(CVF_VERSION_MAJOR "2.10.1")
   endif()
 
   if(PACKAGE_FIND_VERSION_RANGE)

orbbec_camera/SDK/arm64/lib/libOrbbecSDK.so.2

@@ -1 +1 @@
-libOrbbecSDK.so.2.9.3
+libOrbbecSDK.so.2.10.1

orbbec_camera/SDK/x64/include/libobsensor/h/Device.h

@@ -401,7 +401,9 @@ OB_EXPORT void ob_device_set_state_changed_callback(ob_device *device, ob_device
 
 /**
  * @brief Enable or disable the device heartbeat.
- * @brief After enable the device heartbeat, the sdk will start a thread to send heartbeat signal to the device error every 3 seconds.
+ *
+ * When enabled, the SDK sends heartbeat signals at the device monitor polling interval. The default interval is 3000 ms and can be adjusted with
+ * ob_device_set_monitor_poll_interval. If the device does not support heartbeat, an error is reported through @p error.
 
  * @attention If the device does not receive the heartbeat signal for a long time, it will be disconnected and rebooted.
  *
@@ -412,7 +414,7 @@ OB_EXPORT void ob_device_set_state_changed_callback(ob_device *device, ob_device
 OB_EXPORT void ob_device_enable_heartbeat(ob_device *device, bool enable, ob_error **error);
 
 /**
- * @brief Enable or disable the device firmware log.
+ * @brief Enable or disable the device firmware log. If the device does not support firmware log, an error is reported through @p error.
  *
  * @param[in] device The device object.
  * @param[in] enable Whether to enable the firmware log.
@@ -430,6 +432,27 @@ OB_EXPORT void ob_device_enable_firmware_log(ob_device *device, bool enable, ob_
  */
 OB_EXPORT bool ob_device_is_firmware_log_enabled(ob_device *device, ob_error **error);
 
+/**
+ * @brief Set the device monitor polling interval.
+ *
+ * @param[in] device The device object.
+ * @param[in] interval_ms The polling interval for device heartbeat and firmware log retrieval, in milliseconds. The valid range is [1000, 10000];
+ * values outside this range are clamped to the nearest bound. If the device does not support device monitor polling, an error is reported through @p error.
+ * @param[out] error Pointer to an error object that will be set if an error occurs.
+ */
+OB_EXPORT void ob_device_set_monitor_poll_interval(ob_device *device, uint32_t interval_ms, ob_error **error);
+
+/**
+ * @brief Get the device monitor polling interval.
+ *
+ * @param[in] device The device object.
+ * @param[out] error Pointer to an error object that will be set if an error occurs.
+ *
+ * @return uint32_t The device monitor polling interval in milliseconds. If the device does not support device monitor polling, an error is reported
+ * through @p error.
+ */
+OB_EXPORT uint32_t ob_device_get_monitor_poll_interval(ob_device *device, ob_error **error);
+
 /**
  * @brief Synchronize the device time (synchronize hardwarePPS time to device)
  *

orbbec_camera/SDK/x64/include/libobsensor/h/ObTypes.h

@@ -1774,6 +1774,15 @@ typedef struct {
     uint32_t factor;        ///< Decimation factor
 } OBHardwareDecimationConfig, ob_hardware_decimation_config;
 
+/**
+ * @details Defines the gain values for RGB channels.
+ */
+typedef struct {
+    uint16_t rGain;  ///< Red Channel Gain
+    uint16_t bGain;  ///< Blue Channel Gain
+    uint16_t gGain;  ///< Green Channel Gain
+} OBAwbGainParams, ob_awb_gain_params;
+
 /**
  * @brief Frame metadata types
  * @brief The frame metadata is a set of meta info generated by the device for current individual frame.

orbbec_camera/SDK/x64/include/libobsensor/h/Property.h

@@ -682,6 +682,19 @@ typedef enum {
      */
     OB_PROP_MJPEG_QUALITY_INT = 277,
 
+    /**
+     * @brief Color AE/AWB status.
+     * @param value
+     *   - 0: Converging.
+     *          Both AE and AWB algorithms are dynamically adjusting,
+     *          and the parameters have not yet stabilized.
+     *
+     *   - 1: Dual Convergence.
+     *          Both AE and AWB algorithms have completed convergence,
+     *          and the system is in a stable imaging state.
+     */
+    OB_PROP_COLOR_AE_AWB_STATUS_INT = 287,
+
     /**
      * @brief Baseline calibration parameters
      */
@@ -789,6 +802,12 @@ typedef enum {
      */
     OB_STRUCT_DEVICE_IP_ADDR_CONFIG_V2 = 1088,
 
+    /**
+     * @brief Color AWB gain parameters
+     * @see OBAwbGainParams
+     */
+    OB_STRUCT_COLOR_AWB_GAIN = 1097,
+
     /**
      * @brief Color camera auto exposure
      */
@@ -1133,6 +1152,7 @@ typedef enum {
 #define OB_PROP_DEPTH_SOFT_FILTER_BOOL OB_PROP_DEPTH_NOISE_REMOVAL_FILTER_BOOL
 #define OB_PROP_DEPTH_MAX_DIFF_INT OB_PROP_DEPTH_NOISE_REMOVAL_FILTER_MAX_DIFF_INT
 #define OB_PROP_DEPTH_MAX_SPECKLE_SIZE_INT OB_PROP_DEPTH_NOISE_REMOVAL_FILTER_MAX_SPECKLE_SIZE_INT
+#define OB_PROP_COLOR_AE_AWB_STAT_INT OB_PROP_COLOR_AE_AWB_STATUS_INT
 
 /**
  * @brief The data type used to describe all property settings

orbbec_camera/SDK/x64/include/libobsensor/hpp/Device.hpp

@@ -669,7 +669,9 @@ public:
 
     /**
      * @brief Enable or disable the device heartbeat.
-     * @brief After enable the device heartbeat, the sdk will start a thread to send heartbeat signal to the device error every 3 seconds.
+     *
+     * When enabled, the SDK sends heartbeat signals at the device monitor polling interval. The default interval is 3000 ms and can be adjusted with
+     * setMonitorPollInterval. If the device does not support heartbeat, an exception is thrown.
      *
      * @attention If the device does not receive the heartbeat signal for a long time, it will be disconnected and rebooted.
      *
@@ -682,7 +684,7 @@ public:
     }
 
     /**
-     * @brief Enable or disable the device firmware log.
+     * @brief Enable or disable the device firmware log. If the device does not support firmware log, an exception is thrown.
      *
      * @param[in] enable Whether to enable the firmware log.
      */
@@ -704,6 +706,31 @@ public:
         return enable;
     }
 
+    /**
+     * @brief Set the device monitor polling interval.
+     *
+     * @param[in] intervalMs The polling interval for device heartbeat and firmware log retrieval, in milliseconds. The valid range is [1000, 10000];
+     * values outside this range are clamped to the nearest bound. If the device does not support device monitor polling, an exception is thrown.
+     */
+    void setMonitorPollInterval(uint32_t intervalMs) const {
+        ob_error *error = nullptr;
+        ob_device_set_monitor_poll_interval(impl_, intervalMs, &error);
+        Error::handle(&error);
+    }
+
+    /**
+     * @brief Get the device monitor polling interval.
+     *
+     * @return uint32_t The device monitor polling interval in milliseconds.
+     * @throws Error If the device does not support device monitor polling.
+     */
+    uint32_t getMonitorPollInterval() const {
+        ob_error *error      = nullptr;
+        auto      intervalMs = ob_device_get_monitor_poll_interval(impl_, &error);
+        Error::handle(&error);
+        return intervalMs;
+    }
+
     /**
      * @brief Get the supported multi device sync mode bitmap of the device.
      * @brief For example, if the return value is 0b00001100, it means the device supports @ref OB_MULTI_DEVICE_SYNC_MODE_PRIMARY and @ref

orbbec_camera/SDK/x64/lib/OrbbecSDKConfig-release.cmake

@@ -8,12 +8,12 @@ set(CMAKE_IMPORT_FILE_VERSION 1)
 # Import target "ob::OrbbecSDK" for configuration "Release"
 set_property(TARGET ob::OrbbecSDK APPEND PROPERTY IMPORTED_CONFIGURATIONS RELEASE)
 set_target_properties(ob::OrbbecSDK PROPERTIES
-  IMPORTED_LOCATION_RELEASE "${_IMPORT_PREFIX}/lib/libOrbbecSDK.so.2.9.3"
+  IMPORTED_LOCATION_RELEASE "${_IMPORT_PREFIX}/lib/libOrbbecSDK.so.2.10.1"
   IMPORTED_SONAME_RELEASE "libOrbbecSDK.so.2"
   )
 
 list(APPEND _cmake_import_check_targets ob::OrbbecSDK )
-list(APPEND _cmake_import_check_files_for_ob::OrbbecSDK "${_IMPORT_PREFIX}/lib/libOrbbecSDK.so.2.9.3" )
+list(APPEND _cmake_import_check_files_for_ob::OrbbecSDK "${_IMPORT_PREFIX}/lib/libOrbbecSDK.so.2.10.1" )
 
 # Commands beyond this point should not need to know the version.
 set(CMAKE_IMPORT_FILE_VERSION)

orbbec_camera/SDK/x64/lib/OrbbecSDKVersion.cmake

@@ -9,19 +9,19 @@
 # The variable CVF_VERSION must be set before calling configure_file().
 
 
-set(PACKAGE_VERSION "2.9.3")
+set(PACKAGE_VERSION "2.10.1")
 
 if(PACKAGE_VERSION VERSION_LESS PACKAGE_FIND_VERSION)
   set(PACKAGE_VERSION_COMPATIBLE FALSE)
 else()
 
-  if("2.9.3" MATCHES "^([0-9]+)\\.")
+  if("2.10.1" MATCHES "^([0-9]+)\\.")
     set(CVF_VERSION_MAJOR "${CMAKE_MATCH_1}")
     if(NOT CVF_VERSION_MAJOR VERSION_EQUAL 0)
       string(REGEX REPLACE "^0+" "" CVF_VERSION_MAJOR "${CVF_VERSION_MAJOR}")
     endif()
   else()
-    set(CVF_VERSION_MAJOR "2.9.3")
+    set(CVF_VERSION_MAJOR "2.10.1")
   endif()
 
   if(PACKAGE_FIND_VERSION_RANGE)

orbbec_camera/SDK/x64/lib/libOrbbecSDK.so.2

@@ -1 +1 @@
-libOrbbecSDK.so.2.9.3
+libOrbbecSDK.so.2.10.1

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) |

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.

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 <rclcpp/rclcpp.hpp>
+#include <rclcpp_action/rclcpp_action.hpp>
+#include <std_srvs/srv/set_bool.hpp>
+
+#include <atomic>
+#include <chrono>
+#include <condition_variable>
+#include <cstdint>
+#include <functional>
+#include <future>
+#include <memory>
+#include <mutex>
+#include <nlohmann/json.hpp>
+#include <stdexcept>
+#include <string>
+#include <thread>
+
+#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<RunAeAwbLockTest>;
+  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<Metadata>(
+        "color/metadata", rclcpp::SensorDataQoS(),
+        std::bind(&AeAwbLockTestNode::colorMetadataCallback, this, std::placeholders::_1));
+
+    get_status_client_ = create_client<GetInt32>("get_color_ae_awb_status");
+    get_awb_gain_client_ = create_client<GetAwbGain>("get_color_awb_gain");
+    set_awb_gain_client_ = create_client<SetAwbGain>("set_color_awb_gain");
+    get_exposure_client_ = create_client<GetInt32>("get_color_exposure");
+    set_exposure_client_ = create_client<SetInt32>("set_color_exposure");
+    get_color_gain_client_ = create_client<GetInt32>("get_color_gain");
+    set_color_gain_client_ = create_client<SetInt32>("set_color_gain");
+    get_white_balance_client_ = create_client<GetInt32>("get_white_balance");
+    set_white_balance_client_ = create_client<SetInt32>("set_white_balance");
+    set_auto_exposure_client_ = create_client<SetBool>("set_color_auto_exposure");
+    set_auto_white_balance_client_ = create_client<SetBool>("set_auto_white_balance");
+
+    action_server_ = rclcpp_action::create_server<RunAeAwbLockTest>(
+        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<std::mutex> 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<const RunAeAwbLockTest::Goal> 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<GoalHandle> goal_handle) {
+    (void)goal_handle;
+    return rclcpp_action::CancelResponse::ACCEPT;
+  }
+
+  void handleAccepted(const std::shared_ptr<GoalHandle> goal_handle) {
+    std::lock_guard<std::mutex> lock(worker_mutex_);
+    if (worker_.joinable()) {
+      worker_.join();
+    }
+    worker_ = std::thread(&AeAwbLockTestNode::execute, this, goal_handle);
+  }
+
+  template <typename ServiceT>
+  void waitForService(const typename rclcpp::Client<ServiceT>::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>
+  typename ServiceT::Response::SharedPtr callService(
+      const typename rclcpp::Client<ServiceT>::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<GetInt32>(get_status_client_, "get_color_ae_awb_status", deadline);
+    waitForService<GetAwbGain>(get_awb_gain_client_, "get_color_awb_gain", deadline);
+    waitForService<SetAwbGain>(set_awb_gain_client_, "set_color_awb_gain", deadline);
+    waitForService<GetInt32>(get_exposure_client_, "get_color_exposure", deadline);
+    waitForService<SetInt32>(set_exposure_client_, "set_color_exposure", deadline);
+    waitForService<GetInt32>(get_color_gain_client_, "get_color_gain", deadline);
+    waitForService<SetInt32>(set_color_gain_client_, "set_color_gain", deadline);
+    waitForService<GetInt32>(get_white_balance_client_, "get_white_balance", deadline);
+    waitForService<SetInt32>(set_white_balance_client_, "set_white_balance", deadline);
+    waitForService<SetBool>(set_auto_exposure_client_, "set_color_auto_exposure", deadline);
+    waitForService<SetBool>(set_auto_white_balance_client_, "set_auto_white_balance", deadline);
+  }
+
+  int32_t getIntValue(const rclcpp::Client<GetInt32>::SharedPtr& client,
+                      const std::string& service_name, const Deadline& deadline) {
+    return callService<GetInt32>(client, std::make_shared<GetInt32::Request>(), 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<std::mutex> lock(metadata_mutex_);
+      latest_color_metadata_ = metadata;
+    }
+    metadata_cv_.notify_all();
+  }
+
+  ColorFrameMetadata getLatestColorMetadata(const Deadline& deadline) {
+    Metadata::ConstSharedPtr metadata;
+    {
+      std::unique_lock<std::mutex> 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<int32_t>(), json.at("gain").get<int32_t>(),
+              json.at("white_balance").get<int32_t>()};
+    } catch (const nlohmann::json::exception& e) {
+      throw std::runtime_error(std::string("invalid color frame metadata: ") + e.what());
+    }
+  }
+
+  void setIntValue(const rclcpp::Client<SetInt32>::SharedPtr& client,
+                   const std::string& service_name, int32_t value, const Deadline& deadline) {
+    auto request = std::make_shared<SetInt32::Request>();
+    request->data = value;
+    callService<SetInt32>(client, request, service_name, deadline);
+  }
+
+  GetAwbGain::Response::SharedPtr getAwbGain(const Deadline& deadline) {
+    return callService<GetAwbGain>(get_awb_gain_client_, std::make_shared<GetAwbGain::Request>(),
+                                   "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<SetAwbGain::Request>();
+    request->r_gain = r_gain;
+    request->b_gain = b_gain;
+    request->g_gain = g_gain;
+    callService<SetAwbGain>(set_awb_gain_client_, request, "set_color_awb_gain", deadline);
+  }
+
+  void setBoolValue(const rclcpp::Client<SetBool>::SharedPtr& client,
+                    const std::string& service_name, bool value, const Deadline& deadline) {
+    auto request = std::make_shared<SetBool::Request>();
+    request->data = value;
+    callService<SetBool>(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<GoalHandle>& goal_handle) const {
+    if (shutting_down_.load() || goal_handle->is_canceling()) {
+      throw CanceledError();
+    }
+  }
+
+  void publishFeedback(const std::shared_ptr<GoalHandle>& goal_handle, const std::string& phase,
+                       int32_t status) {
+    auto feedback = std::make_shared<RunAeAwbLockTest::Feedback>();
+    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<long long>(captured), static_cast<long long>(actual));
+    }
+  }
+
+  void execute(const std::shared_ptr<GoalHandle> goal_handle) {
+    ActiveGoalGuard active_guard(goal_active_);
+    auto result = std::make_shared<RunAeAwbLockTest::Result>();
+    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<RunAeAwbLockTest>::SharedPtr action_server_;
+  rclcpp::Subscription<Metadata>::SharedPtr color_metadata_subscription_;
+
+  rclcpp::Client<GetInt32>::SharedPtr get_status_client_;
+  rclcpp::Client<GetAwbGain>::SharedPtr get_awb_gain_client_;
+  rclcpp::Client<SetAwbGain>::SharedPtr set_awb_gain_client_;
+  rclcpp::Client<GetInt32>::SharedPtr get_exposure_client_;
+  rclcpp::Client<SetInt32>::SharedPtr set_exposure_client_;
+  rclcpp::Client<GetInt32>::SharedPtr get_color_gain_client_;
+  rclcpp::Client<SetInt32>::SharedPtr set_color_gain_client_;
+  rclcpp::Client<GetInt32>::SharedPtr get_white_balance_client_;
+  rclcpp::Client<SetInt32>::SharedPtr set_white_balance_client_;
+  rclcpp::Client<SetBool>::SharedPtr set_auto_exposure_client_;
+  rclcpp::Client<SetBool>::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<AeAwbLockTestNode>();
+  rclcpp::executors::MultiThreadedExecutor executor;
+  executor.add_node(node);
+  executor.spin();
+  rclcpp::shutdown();
+  return 0;
+}

orbbec_camera/examples/benchmark/gemini_330_series_benchmark.launch.py

@@ -235,6 +235,7 @@ def generate_launch_description():
         DeclareLaunchArgument('config_file_path', default_value=''),
         DeclareLaunchArgument('enable_heartbeat', default_value='false'),
         DeclareLaunchArgument('enable_firmware_log', default_value='false'),
+        DeclareLaunchArgument('monitor_poll_interval_sec', default_value='-1'),
         DeclareLaunchArgument('gmsl_trigger_fps', default_value='3000'),
         DeclareLaunchArgument('enable_gmsl_trigger', default_value='false'),
         DeclareLaunchArgument('disparity_range_mode', default_value='-1'),

orbbec_camera/examples/gmsl_camera/gemini_330_gmsl.launch.py

@@ -235,6 +235,7 @@ def generate_launch_description():
         DeclareLaunchArgument('config_file_path', default_value=''),
         DeclareLaunchArgument('enable_heartbeat', default_value='false'),
         DeclareLaunchArgument('enable_firmware_log', default_value='false'),
+        DeclareLaunchArgument('monitor_poll_interval_sec', default_value='-1'),
         DeclareLaunchArgument('gmsl_trigger_fps', default_value='3000'),
         DeclareLaunchArgument('enable_gmsl_trigger', default_value='false'),
         DeclareLaunchArgument('disparity_range_mode', default_value='-1'),

orbbec_camera/examples/multi_camera_synced_verification_tool/gemini_330_series_synced_verify.launch.py

@@ -235,6 +235,7 @@ def generate_launch_description():
         DeclareLaunchArgument('config_file_path', default_value=''),
         DeclareLaunchArgument('enable_heartbeat', default_value='false'),
         DeclareLaunchArgument('enable_firmware_log', default_value='false'),
+        DeclareLaunchArgument('monitor_poll_interval_sec', default_value='-1'),
         DeclareLaunchArgument('gmsl_trigger_fps', default_value='3000'),
         DeclareLaunchArgument('enable_gmsl_trigger', default_value='false'),
         DeclareLaunchArgument('disparity_range_mode', default_value='-1'),

orbbec_camera/examples/multi_camera_synced_verification_tool/gemini_synced_verify.launch.py

@@ -155,6 +155,7 @@ def generate_launch_description():
         DeclareLaunchArgument('laser_energy_level', default_value='-1'),
         DeclareLaunchArgument('enable_heartbeat', default_value='false'),
         DeclareLaunchArgument('enable_firmware_log', default_value='false'),
+        DeclareLaunchArgument('monitor_poll_interval_sec', default_value='-1'),
         DeclareLaunchArgument('time_domain', default_value='global'),
         DeclareLaunchArgument('use_intra_process_comms', default_value='false'),
         DeclareLaunchArgument('attach_component_container_enable', default_value='false'),

orbbec_camera/include/orbbec_camera/constants.h

@@ -23,8 +23,8 @@
 #define THREAD_NUM 4
 
 #define OB_ROS_MAJOR_VERSION 2
-#define OB_ROS_MINOR_VERSION 9
+#define OB_ROS_MINOR_VERSION 10
-#define OB_ROS_PATCH_VERSION 3
+#define OB_ROS_PATCH_VERSION 1
 
 #ifndef STRINGIFY
 #define STRINGIFY(arg) #arg

orbbec_camera/include/orbbec_camera/frame_timestamp_csv_logger.h

@@ -21,8 +21,10 @@ namespace orbbec_camera {
 
 class FrameTimestampCsvLogger {
  public:
+  enum class OutputMode { SYNCED, COLOR, DEPTH };
+
   FrameTimestampCsvLogger(bool drop_log_enabled, const std::string &csv_file_path,
-                          rclcpp::Logger logger);
+                          OutputMode output_mode, rclcpp::Logger logger);
 
   ~FrameTimestampCsvLogger() noexcept;
 
@@ -137,7 +139,7 @@ class FrameTimestampCsvLogger {
   std::string serializeStreamColumns(const StreamState &state) const;
   static std::string formatSecondsColumn(int64_t time_us);
   static std::string formatOptionalIntColumn(const std::optional<int64_t> &value);
-  static std::string csvHeader();
+  std::string csvHeader() const;
 
   void writerThreadMain();
   std::string csvFilePathForIndex(uint64_t file_index) const;
@@ -152,6 +154,7 @@ class FrameTimestampCsvLogger {
   std::atomic_bool csv_writer_failed_{false};
   bool queue_warning_active_ = false;
   std::string csv_file_path_;
+  OutputMode output_mode_;
   std::ofstream csv_stream_;
   std::thread writer_thread_;
   uint64_t csv_file_index_ = 0;

orbbec_camera/include/orbbec_camera/imu_timestamp_csv_logger.h

@@ -0,0 +1,99 @@
+#pragma once
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <atomic>
+#include <condition_variable>
+#include <cstdint>
+#include <deque>
+#include <fstream>
+#include <memory>
+#include <mutex>
+#include <optional>
+#include <string>
+#include <thread>
+
+#include "libobsensor/ObSensor.hpp"
+
+namespace orbbec_camera {
+
+class ImuTimestampCsvLogger {
+ public:
+  enum class OutputMode { SYNCED, ACCEL, GYRO };
+
+  ImuTimestampCsvLogger(const std::string &frame_csv_file_path, OutputMode output_mode,
+                        rclcpp::Logger logger);
+
+  ~ImuTimestampCsvLogger() noexcept;
+
+  ImuTimestampCsvLogger(const ImuTimestampCsvLogger &) = delete;
+  ImuTimestampCsvLogger &operator=(const ImuTimestampCsvLogger &) = delete;
+
+  void recordFrameSet(const std::shared_ptr<ob::Frame> &accel_frame,
+                      const std::shared_ptr<ob::Frame> &gyro_frame, int64_t arrival_system_us,
+                      std::optional<int64_t> publish_system_us);
+
+  void recordStandaloneFrame(OBStreamType stream_type, const std::shared_ptr<ob::Frame> &frame,
+                             int64_t arrival_system_us, std::optional<int64_t> publish_system_us);
+
+  void shutdown();
+
+  bool enabled() const { return enabled_; }
+
+ private:
+  struct StreamState {
+    bool has_frame = false;
+    int64_t device_ts_us = 0;
+    int64_t global_ts_us = 0;
+    int64_t sdk_system_ts_us = 0;
+    int64_t arrival_system_us = 0;
+    std::optional<int64_t> publish_system_us;
+  };
+
+  struct PendingRow {
+    uint64_t row_id = 0;
+    StreamState accel;
+    StreamState gyro;
+  };
+
+  void recordFrames(const std::shared_ptr<ob::Frame> &accel_frame,
+                    const std::shared_ptr<ob::Frame> &gyro_frame, int64_t arrival_system_us,
+                    std::optional<int64_t> publish_system_us);
+  static void populateStreamState(StreamState &state, const std::shared_ptr<ob::Frame> &frame,
+                                  int64_t arrival_system_us,
+                                  std::optional<int64_t> publish_system_us);
+
+  void enqueueCompletedRow(const PendingRow &row);
+  std::string serializeRow(const PendingRow &row) const;
+  static std::string serializeStreamColumns(const StreamState &state);
+  static std::string formatSecondsColumn(int64_t time_us);
+  static std::string formatOptionalSecondsColumn(const std::optional<int64_t> &value);
+  std::string csvHeader() const;
+
+  void writerThreadMain();
+  std::string csvFilePathForIndex(uint64_t file_index) const;
+  bool openCsvFile(uint64_t file_index);
+  bool rotateCsvFile();
+
+  rclcpp::Logger logger_;
+  bool enabled_ = false;
+  bool csv_enabled_ = false;
+  std::atomic_bool shutdown_requested_{false};
+  std::atomic_bool csv_writer_failed_{false};
+  bool queue_warning_active_ = false;
+  std::string frame_csv_file_path_;
+  OutputMode output_mode_;
+  std::ofstream csv_stream_;
+  std::thread writer_thread_;
+  uint64_t csv_file_index_ = 0;
+  uint64_t csv_rows_written_ = 0;
+
+  uint64_t next_row_id_ = 1;
+  std::deque<PendingRow> completed_rows_;
+
+  std::mutex state_mutex_;
+  std::mutex completed_rows_mutex_;
+  std::condition_variable completed_rows_cv_;
+};
+
+}  // namespace orbbec_camera

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"
@@ -73,7 +75,7 @@
 #include "orbbec_camera/image_publisher.h"
 #include "orbbec_camera/fps_counter.hpp"
 #include "orbbec_camera/fps_delay_status.hpp"
-#include "orbbec_camera/frame_timestamp_csv_logger.h"
+#include "orbbec_camera/timestamp_csv_logger.h"
 #include "jpeg_decoder.h"
 #include <std_msgs/msg/header.hpp>
 #include <fcntl.h>
@@ -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<SetBool::Request>& request,
                                    std::shared_ptr<SetBool::Response>& response);
 
+  void getAeAwbStatusCallback(const std::shared_ptr<GetInt32::Request>& request,
+                              std::shared_ptr<GetInt32::Response>& response);
+
+  void getAwbGainCallback(const std::shared_ptr<GetAwbGain::Request>& request,
+                          std::shared_ptr<GetAwbGain::Response>& response);
+
+  void setAwbGainCallback(const std::shared_ptr<SetAwbGain::Request>& request,
+                          std::shared_ptr<SetAwbGain::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);
@@ -613,7 +626,8 @@ class OBCameraNode {
                        const std::shared_ptr<ob::Frame>& frame);
 
   void onNewIMUFrameSyncOutputCallback(const std::shared_ptr<ob::Frame>& accelframe,
-                                       const std::shared_ptr<ob::Frame>& gryoframe);
+                                       const std::shared_ptr<ob::Frame>& gryoframe,
+                                       int64_t arrival_system_us);
 
   void onNewIMUFrameCallback(const std::shared_ptr<ob::Frame>& frame,
                              const stream_index_pair& stream_index);
@@ -750,6 +764,9 @@ class OBCameraNode {
   rclcpp::Service<SetInt32>::SharedPtr set_white_balance_srv_;
   rclcpp::Service<GetInt32>::SharedPtr get_auto_white_balance_srv_;
   rclcpp::Service<SetBool>::SharedPtr set_auto_white_balance_srv_;
+  rclcpp::Service<GetInt32>::SharedPtr get_ae_awb_status_srv_;
+  rclcpp::Service<GetAwbGain>::SharedPtr get_awb_gain_srv_;
+  rclcpp::Service<SetAwbGain>::SharedPtr set_awb_gain_srv_;
   rclcpp::Service<GetString>::SharedPtr get_sdk_version_srv_;
   rclcpp::Service<SetString>::SharedPtr switch_ir_camera_srv_;
   rclcpp::Service<SetString>::SharedPtr export_config_json_srv_;
@@ -1070,7 +1087,7 @@ class OBCameraNode {
   std::string time_domain_ = "global";  // device, system, global
   bool enable_frame_drop_log_ = false;
   std::string frame_timestamp_csv_file_;
-  std::unique_ptr<FrameTimestampCsvLogger> frame_timestamp_csv_logger_;
+  std::unique_ptr<TimestampCsvLogger> timestamp_csv_logger_;
   std::string exposure_range_mode_;
   std::string load_config_json_file_path_ = "";
   std::string export_config_json_file_path_ = "";
@@ -1087,6 +1104,7 @@ class OBCameraNode {
   double lrm_obstacle_distance_publish_rate_ = 10.0;
   bool enable_heartbeat_ = false;
   bool enable_firmware_log_ = false;
+  int monitor_poll_interval_sec_ = -1;
   bool enable_fps_boost_ = false;
   std::map<stream_index_pair, bool> enable_undistortion_;
   std::shared_ptr<ob::UnDistortionFilter> hw_d2c_color_undistortion_filter_;

orbbec_camera/include/orbbec_camera/timestamp_csv_logger.h

@@ -0,0 +1,73 @@
+#pragma once
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <atomic>
+#include <cstdint>
+#include <memory>
+#include <optional>
+#include <string>
+
+#include "libobsensor/ObSensor.hpp"
+
+namespace orbbec_camera {
+
+class FrameTimestampCsvLogger;
+class ImuTimestampCsvLogger;
+
+class TimestampCsvLogger {
+ public:
+  struct Config {
+    bool frame_drop_log_enabled = false;
+    std::string csv_file_path;
+    bool frame_sync_enabled = false;
+    bool color_enabled = false;
+    bool depth_enabled = false;
+    bool imu_sync_enabled = false;
+    bool accel_enabled = false;
+    bool gyro_enabled = false;
+  };
+
+  TimestampCsvLogger(Config config, rclcpp::Logger logger);
+  ~TimestampCsvLogger() noexcept;
+
+  TimestampCsvLogger(const TimestampCsvLogger &) = delete;
+  TimestampCsvLogger &operator=(const TimestampCsvLogger &) = delete;
+
+  bool enabled() const;
+  bool imageEnabled() const;
+  bool imageStreamEnabled(OBStreamType stream_type) const;
+  bool syncedImuEnabled() const;
+  bool standaloneImuEnabled(OBStreamType stream_type) const;
+
+  void recordImageFrameSet(const std::shared_ptr<ob::Frame> &color_frame,
+                           const std::shared_ptr<ob::Frame> &depth_frame, int64_t arrival_system_us,
+                           int64_t arrival_steady_us, bool track_color, bool track_depth,
+                           bool color_image_publish_expected, bool depth_image_publish_expected);
+  void recordImagePrePublish(OBStreamType stream_type, const std::shared_ptr<ob::Frame> &frame,
+                             int64_t publish_system_us, int64_t publish_steady_us);
+  void recordImagePublishSkipped(OBStreamType stream_type, const std::shared_ptr<ob::Frame> &frame);
+
+  void recordSyncedImu(const std::shared_ptr<ob::Frame> &accel_frame,
+                       const std::shared_ptr<ob::Frame> &gyro_frame, int64_t arrival_system_us,
+                       std::optional<int64_t> publish_system_us);
+  void recordStandaloneImu(OBStreamType stream_type, const std::shared_ptr<ob::Frame> &frame,
+                           int64_t arrival_system_us, std::optional<int64_t> publish_system_us);
+
+  void shutdown() noexcept;
+
+ private:
+  FrameTimestampCsvLogger *imageLoggerForStream(OBStreamType stream_type) const;
+  ImuTimestampCsvLogger *standaloneImuLoggerForStream(OBStreamType stream_type) const;
+
+  rclcpp::Logger logger_;
+  std::atomic_bool shutdown_requested_{false};
+  std::unique_ptr<FrameTimestampCsvLogger> synced_image_logger_;
+  std::unique_ptr<FrameTimestampCsvLogger> color_logger_;
+  std::unique_ptr<FrameTimestampCsvLogger> depth_logger_;
+  std::unique_ptr<ImuTimestampCsvLogger> synced_imu_logger_;
+  std::unique_ptr<ImuTimestampCsvLogger> accel_logger_;
+  std::unique_ptr<ImuTimestampCsvLogger> gyro_logger_;
+};
+
+}  // namespace orbbec_camera

orbbec_camera/launch/astra.launch.py

@@ -90,6 +90,7 @@ def generate_launch_description():
         DeclareLaunchArgument('laser_energy_level', default_value='-1'),
         DeclareLaunchArgument('enable_heartbeat', default_value='false'),
         DeclareLaunchArgument('enable_firmware_log', default_value='false'),
+        DeclareLaunchArgument('monitor_poll_interval_sec', default_value='-1'),
 
         DeclareLaunchArgument('color_mirror', default_value='false'),
         DeclareLaunchArgument('color_rotation', default_value='-1'),

orbbec_camera/launch/astra2.launch.py

@@ -115,6 +115,7 @@ def generate_launch_description():
         DeclareLaunchArgument("laser_energy_level", default_value="-1"),
         DeclareLaunchArgument("enable_heartbeat", default_value="false"),
         DeclareLaunchArgument("enable_firmware_log", default_value="false"),
+        DeclareLaunchArgument("monitor_poll_interval_sec", default_value="-1"),
         DeclareLaunchArgument("time_domain", default_value="global"),
         DeclareLaunchArgument("timestamp_clock_type", default_value=""),  # realtime or monotonic, default is realtime.
         DeclareLaunchArgument('enable_frame_drop_log', default_value='false'),

orbbec_camera/launch/dabai_a.launch.py

@@ -231,6 +231,7 @@ def generate_launch_description():
         DeclareLaunchArgument('config_file_path', default_value=''),
         DeclareLaunchArgument('enable_heartbeat', default_value='false'),
         DeclareLaunchArgument('enable_firmware_log', default_value='false'),
+        DeclareLaunchArgument('monitor_poll_interval_sec', default_value='-1'),
 
         #color image transport plugins
         DeclareLaunchArgument('color.image_raw.enable_pub_plugins',default_value='["image_transport/compressed", "image_transport/raw", "image_transport/theora"]'),

orbbec_camera/launch/dabai_al.launch.py

@@ -233,6 +233,7 @@ def generate_launch_description():
         DeclareLaunchArgument('config_file_path', default_value=''),
         DeclareLaunchArgument('enable_heartbeat', default_value='false'),
         DeclareLaunchArgument('enable_firmware_log', default_value='false'),
+        DeclareLaunchArgument('monitor_poll_interval_sec', default_value='-1'),
 
         #color image transport plugins
         DeclareLaunchArgument('color.image_raw.enable_pub_plugins',default_value='["image_transport/compressed", "image_transport/raw", "image_transport/theora"]'),

orbbec_camera/launch/dabai_dcw2.launch.py

@@ -104,6 +104,7 @@ def generate_launch_description():
         DeclareLaunchArgument('laser_energy_level', default_value='-1'),
         DeclareLaunchArgument('enable_heartbeat', default_value='false'),
         DeclareLaunchArgument('enable_firmware_log', default_value='false'),
+        DeclareLaunchArgument('monitor_poll_interval_sec', default_value='-1'),
         DeclareLaunchArgument('industry_mode', default_value=''),
     ]
 

orbbec_camera/launch/dabai_max_pro.launch.py

@@ -133,6 +133,7 @@ def generate_launch_description():
         DeclareLaunchArgument('enable_ldp', default_value='true'),
         DeclareLaunchArgument('enable_heartbeat', default_value='false'),
         DeclareLaunchArgument('enable_firmware_log', default_value='false'),
+        DeclareLaunchArgument('monitor_poll_interval_sec', default_value='-1'),
     ]
 
     # Node configuration

orbbec_camera/launch/femto.launch.py

@@ -93,6 +93,7 @@ def generate_launch_description():
         DeclareLaunchArgument("laser_energy_level", default_value="-1"),
         DeclareLaunchArgument("enable_heartbeat", default_value="false"),
         DeclareLaunchArgument("enable_firmware_log", default_value="false"),
+        DeclareLaunchArgument("monitor_poll_interval_sec", default_value="-1"),
         DeclareLaunchArgument("time_domain", default_value="device"),
         DeclareLaunchArgument("timestamp_clock_type", default_value=""),  # realtime or monotonic, default is realtime.
         DeclareLaunchArgument('enable_frame_drop_log', default_value='false'),

orbbec_camera/launch/femto_bolt.launch.py

@@ -121,6 +121,7 @@ def generate_launch_description():
         DeclareLaunchArgument("laser_energy_level", default_value="-1"),
         DeclareLaunchArgument("enable_heartbeat", default_value="false"),
         DeclareLaunchArgument("enable_firmware_log", default_value="false"),
+        DeclareLaunchArgument("monitor_poll_interval_sec", default_value="-1"),
         DeclareLaunchArgument("time_domain", default_value="global"),
         DeclareLaunchArgument("timestamp_clock_type", default_value=""),  # realtime or monotonic, default is realtime.
         DeclareLaunchArgument('enable_frame_drop_log', default_value='false'),

orbbec_camera/launch/femto_mega.launch.py

@@ -127,6 +127,7 @@ def generate_launch_description():
         DeclareLaunchArgument("laser_energy_level", default_value="-1"),
         DeclareLaunchArgument("enable_heartbeat", default_value="false"),
         DeclareLaunchArgument("enable_firmware_log", default_value="false"),
+        DeclareLaunchArgument("monitor_poll_interval_sec", default_value="-1"),
         DeclareLaunchArgument("time_domain", default_value="global"),
         DeclareLaunchArgument("timestamp_clock_type", default_value=""),  # realtime or monotonic, default is realtime.
         DeclareLaunchArgument('enable_frame_drop_log', default_value='false'),

orbbec_camera/launch/gemini2.launch.py

@@ -143,6 +143,7 @@ def generate_launch_description():
         DeclareLaunchArgument("laser_energy_level", default_value="-1"),
         DeclareLaunchArgument("enable_heartbeat", default_value="false"),
         DeclareLaunchArgument("enable_firmware_log", default_value="false"),
+        DeclareLaunchArgument("monitor_poll_interval_sec", default_value="-1"),
         DeclareLaunchArgument("time_domain", default_value="global"),
         DeclareLaunchArgument("timestamp_clock_type", default_value=""),  # realtime or monotonic, default is realtime.
         DeclareLaunchArgument('enable_frame_drop_log', default_value='false'),

orbbec_camera/launch/gemini210.launch.py

@@ -141,6 +141,7 @@ def generate_launch_description():
         DeclareLaunchArgument("laser_energy_level", default_value="-1"),
         DeclareLaunchArgument("enable_heartbeat", default_value="false"),
         DeclareLaunchArgument("enable_firmware_log", default_value="false"),
+        DeclareLaunchArgument("monitor_poll_interval_sec", default_value="-1"),
         DeclareLaunchArgument("time_domain", default_value="global"),
         DeclareLaunchArgument("timestamp_clock_type", default_value=""),  # realtime or monotonic, default is realtime.
         DeclareLaunchArgument('enable_frame_drop_log', default_value='false'),

orbbec_camera/launch/gemini2L.launch.py

@@ -195,6 +195,7 @@ def generate_launch_description():
         DeclareLaunchArgument("laser_energy_level", default_value="-1"),
         DeclareLaunchArgument("enable_heartbeat", default_value="false"),
         DeclareLaunchArgument("enable_firmware_log", default_value="false"),
+        DeclareLaunchArgument("monitor_poll_interval_sec", default_value="-1"),
         DeclareLaunchArgument("time_domain", default_value="global"),
         DeclareLaunchArgument("timestamp_clock_type", default_value=""),  # realtime or monotonic, default is realtime.
         DeclareLaunchArgument("enable_frame_drop_log", default_value="false"),

orbbec_camera/launch/gemini345.launch.py

@@ -230,6 +230,7 @@ def generate_launch_description():
         DeclareLaunchArgument('config_file_path', default_value=''),
         DeclareLaunchArgument('enable_heartbeat', default_value='false'),
         DeclareLaunchArgument('enable_firmware_log', default_value='false'),
+        DeclareLaunchArgument('monitor_poll_interval_sec', default_value='-1'),
 
         #color image transport plugins
         DeclareLaunchArgument('color.image_raw.enable_pub_plugins',default_value='["image_transport/compressed", "image_transport/raw", "image_transport/theora"]'),

orbbec_camera/launch/gemini345_lg.launch.py

@@ -233,6 +233,7 @@ def generate_launch_description():
         DeclareLaunchArgument('config_file_path', default_value=''),
         DeclareLaunchArgument('enable_heartbeat', default_value='false'),
         DeclareLaunchArgument('enable_firmware_log', default_value='false'),
+        DeclareLaunchArgument('monitor_poll_interval_sec', default_value='-1'),
         DeclareLaunchArgument('gmsl_trigger_fps', default_value='3000'),
         DeclareLaunchArgument('enable_gmsl_trigger', default_value='false'),
 

orbbec_camera/launch/gemini435_le.launch.py

@@ -264,6 +264,7 @@ def generate_launch_description():
         DeclareLaunchArgument('config_file_path', default_value=''),
         DeclareLaunchArgument('enable_heartbeat', default_value='false'),
         DeclareLaunchArgument('enable_firmware_log', default_value='false'),
+        DeclareLaunchArgument('monitor_poll_interval_sec', default_value='-1'),
         DeclareLaunchArgument('gmsl_trigger_fps', default_value='3000'),
         DeclareLaunchArgument('enable_gmsl_trigger', default_value='false'),
         DeclareLaunchArgument('disparity_range_mode', default_value='-1'),

orbbec_camera/launch/gemini_301_series.launch.py

@@ -291,6 +291,8 @@ def generate_launch_description():
         DeclareLaunchArgument('config_file_path', default_value=''),
         DeclareLaunchArgument('enable_heartbeat', default_value='false'),
         DeclareLaunchArgument('enable_firmware_log', default_value='false'),
+        DeclareLaunchArgument('enable_fps_boost', default_value='false'),
+        DeclareLaunchArgument('monitor_poll_interval_sec', default_value='-1'),
         DeclareLaunchArgument('gmsl_trigger_fps', default_value='3000'),
         DeclareLaunchArgument('enable_gmsl_trigger', default_value='false'),
         DeclareLaunchArgument('disparity_range_mode', default_value='-1'),

orbbec_camera/launch/gemini_330_series.launch.py

@@ -303,6 +303,7 @@ def generate_launch_description():
         DeclareLaunchArgument('config_file_path', default_value=''),
         DeclareLaunchArgument('enable_heartbeat', default_value='false'),
         DeclareLaunchArgument('enable_firmware_log', default_value='false'),
+        DeclareLaunchArgument('monitor_poll_interval_sec', default_value='-1'),
         DeclareLaunchArgument('gmsl_trigger_fps', default_value='3000'),
         DeclareLaunchArgument('enable_gmsl_trigger', default_value='false'),
         DeclareLaunchArgument('disparity_range_mode', default_value='-1'),

orbbec_camera/launch/gemini_330_series_low_cpu.launch.py

@@ -7,6 +7,12 @@ from launch_ros.actions import PushRosNamespace, ComposableNodeContainer, Node
 from launch_ros.descriptions import ComposableNode
 
 
+OPTIONAL_BOOLEAN_PARAMS = {
+    'enable_hardware_noise_removal_filter',
+    'enable_noise_removal_filter',
+}
+
+
 def load_yaml(file_path):
     with open(file_path, 'r') as f:
         return yaml.safe_load(f)
@@ -47,6 +53,10 @@ def load_parameters(context, args):
 
     result = {}
     for key, value in default_params.items():
+        # An empty optional boolean means "auto". Do not pass it to the ROS node,
+        # because a declared bool parameter cannot represent an unset value.
+        if key in OPTIONAL_BOOLEAN_PARAMS and value == '':
+            continue
         if key in skip_convert:
             result[key] = value
         elif 'enable_pub_plugins' in key:
@@ -230,8 +240,9 @@ def generate_launch_description():
         DeclareLaunchArgument('enable_hdr_merge', default_value='false'),
         DeclareLaunchArgument('enable_sequence_id_filter', default_value='false'),
         DeclareLaunchArgument('enable_threshold_filter', default_value='false'),
-        DeclareLaunchArgument('enable_hardware_noise_removal_filter', default_value='true'),
+        # Empty means that the node will not change the device's current setting.
-        DeclareLaunchArgument('enable_noise_removal_filter', default_value='false'),
+        DeclareLaunchArgument('enable_hardware_noise_removal_filter', default_value=''),
+        DeclareLaunchArgument('enable_noise_removal_filter', default_value=''),
         DeclareLaunchArgument('enable_disp_outliers_filter', default_value='false'),
         DeclareLaunchArgument('enable_spatial_filter', default_value='false'),
         DeclareLaunchArgument('enable_temporal_filter', default_value='false'),
@@ -287,6 +298,7 @@ def generate_launch_description():
         DeclareLaunchArgument('config_file_path', default_value=''),
         DeclareLaunchArgument('enable_heartbeat', default_value='false'),
         DeclareLaunchArgument('enable_firmware_log', default_value='false'),
+        DeclareLaunchArgument('monitor_poll_interval_sec', default_value='-1'),
         DeclareLaunchArgument('gmsl_trigger_fps', default_value='3000'),
         DeclareLaunchArgument('enable_gmsl_trigger', default_value='false'),
         DeclareLaunchArgument('disparity_range_mode', default_value='-1'),

orbbec_camera/launch/gemini_330_series_sdk_json.launch.py

@@ -142,6 +142,7 @@ def generate_launch_description():
         DeclareLaunchArgument('log_level', default_value='info'),
         DeclareLaunchArgument('log_file_name', default_value=''),
         DeclareLaunchArgument('config_file_path', default_value=''),
+        DeclareLaunchArgument('monitor_poll_interval_sec', default_value='-1'),
 
         DeclareLaunchArgument('force_ip_enable', default_value='false'),
         DeclareLaunchArgument('force_ip_mac', default_value=''),

orbbec_camera/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>orbbec_camera</name>
-  <version>2.9.3</version>
+  <version>2.10.1</version>
   <description>Orbbec Camera package</description>
   <maintainer email="[email protected]">yalian</maintainer>
   <license>Apache-2.0</license>
@@ -20,9 +20,10 @@
   <depend>rclcpp_components</depend>
   <depend>cv_bridge</depend>
   <depend>camera_info_manager</depend>
-  <depend version_gte="2.9.3">orbbec_camera_msgs</depend>
+  <depend version_gte="2.10.1">orbbec_camera_msgs</depend>
   <depend>builtin_interfaces</depend>
   <depend>rclcpp</depend>
+  <depend>rclcpp_action</depend>
   <depend>sensor_msgs</depend>
   <depend>std_msgs</depend>
   <depend>std_srvs</depend>

orbbec_camera/src/frame_timestamp_csv_logger.cpp

@@ -35,25 +35,26 @@ int64_t getExpectedIntervalUs(const std::shared_ptr<ob::Frame> &frame) {
 
 FrameTimestampCsvLogger::FrameTimestampCsvLogger(bool drop_log_enabled,
                                                  const std::string &csv_file_path,
-                                                 rclcpp::Logger logger)
+                                                 OutputMode output_mode, rclcpp::Logger logger)
     : logger_(std::move(logger)),
       enabled_(drop_log_enabled || !csv_file_path.empty()),
       csv_enabled_(!csv_file_path.empty()),
       drop_log_enabled_(drop_log_enabled),
-      csv_file_path_(csv_file_path) {
+      csv_file_path_(csv_file_path),
+      output_mode_(output_mode) {
   if (!enabled_) {
     return;
   }
 
   if (csv_enabled_) {
     try {
-      auto path = std::filesystem::path(csv_file_path_);
+      auto path = std::filesystem::path(csvFilePathForIndex(0));
       if (path.has_parent_path() && !std::filesystem::exists(path.parent_path())) {
         std::filesystem::create_directories(path.parent_path());
       }
     } catch (const std::exception &e) {
       RCLCPP_ERROR_STREAM(logger_, "Failed to prepare frame timestamp CSV path "
-                                       << csv_file_path_ << ": " << e.what());
+                                       << csvFilePathForIndex(0) << ": " << e.what());
       csv_enabled_ = false;
       csv_writer_failed_ = true;
     }
@@ -74,7 +75,7 @@ FrameTimestampCsvLogger::FrameTimestampCsvLogger(bool drop_log_enabled,
   if (enabled_) {
     RCLCPP_INFO_STREAM(logger_,
                        "Frame timestamp logger enabled: csv_file="
-                           << (csv_enabled_ ? csv_file_path_ : "disabled")
+                           << (csv_enabled_ ? csvFilePathForIndex(0) : "disabled")
                            << " frame_drop_log=" << (drop_log_enabled_ ? "enabled" : "disabled"));
   }
 }
@@ -90,6 +91,9 @@ void FrameTimestampCsvLogger::recordFrameSet(const std::shared_ptr<ob::Frame> &c
   if (!enabled_) {
     return;
   }
+  if (output_mode_ != OutputMode::SYNCED) {
+    return;
+  }
   recordFrameSetInternal(color_frame, depth_frame, arrival_system_us, arrival_steady_us,
                          track_color, track_depth, color_image_publish_expected,
                          depth_image_publish_expected);
@@ -100,7 +104,9 @@ void FrameTimestampCsvLogger::recordStandaloneFrameArrival(OBStreamType stream_t
                                                            int64_t arrival_system_us,
                                                            int64_t arrival_steady_us,
                                                            bool image_publish_expected) {
-  if (!enabled_ || !frame || !isTrackedStream(stream_type)) {
+  if (!enabled_ || !frame || !isTrackedStream(stream_type) ||
+      (stream_type == OB_STREAM_COLOR && output_mode_ != OutputMode::COLOR) ||
+      (stream_type == OB_STREAM_DEPTH && output_mode_ != OutputMode::DEPTH)) {
     return;
   }
   recordStandaloneFrameArrivalInternal(stream_type, frame, arrival_system_us, arrival_steady_us,
@@ -479,6 +485,12 @@ void FrameTimestampCsvLogger::eraseFrameIndexMappingLocked(const PendingRow &row
 }
 
 std::string FrameTimestampCsvLogger::serializeRow(const PendingRow &row) const {
+  if (output_mode_ == OutputMode::COLOR) {
+    return serializeStreamColumns(row.color);
+  }
+  if (output_mode_ == OutputMode::DEPTH) {
+    return serializeStreamColumns(row.depth);
+  }
   std::ostringstream ss;
   ss << serializeStreamColumns(row.color) << "," << serializeStreamColumns(row.depth);
   return ss.str();
@@ -529,9 +541,9 @@ std::string FrameTimestampCsvLogger::formatOptionalIntColumn(const std::optional
   return std::to_string(*value);
 }
 
-std::string FrameTimestampCsvLogger::csvHeader() {
+std::string FrameTimestampCsvLogger::csvHeader() const {
   std::ostringstream ss;
-  for (const auto *prefix : {"color", "depth"}) {
+  const auto append_stream_header = [&ss](const char *prefix) {
     ss << prefix << "_sdk_frame_index,";
     ss << prefix << "_hardware_frame_number,";
     ss << prefix << "_sensor_ts_sec,";
@@ -547,9 +559,15 @@ std::string FrameTimestampCsvLogger::csvHeader() {
     ss << prefix << "_arrival_to_publish_steady_us,";
     ss << prefix << "_sdk_delay_from_global_us,";
     ss << prefix << "_sdk_delay_from_system_us";
-    if (std::string(prefix) == "color") {
+  };
-      ss << ",";
+  if (output_mode_ == OutputMode::SYNCED || output_mode_ == OutputMode::COLOR) {
-    }
+    append_stream_header("color");
+  }
+  if (output_mode_ == OutputMode::SYNCED) {
+    ss << ",";
+  }
+  if (output_mode_ == OutputMode::SYNCED || output_mode_ == OutputMode::DEPTH) {
+    append_stream_header("depth");
   }
   return ss.str();
 }
@@ -621,13 +639,19 @@ void FrameTimestampCsvLogger::writerThreadMain() {
 }
 
 std::string FrameTimestampCsvLogger::csvFilePathForIndex(uint64_t file_index) const {
-  if (file_index == 0) {
+  const std::filesystem::path original_path(csv_file_path_);
-    return csv_file_path_;
+  std::string suffix;
+  if (output_mode_ == OutputMode::COLOR) {
+    suffix = "_color";
+  } else if (output_mode_ == OutputMode::DEPTH) {
+    suffix = "_depth";
   }
 
-  const std::filesystem::path original_path(csv_file_path_);
+  auto indexed_filename = original_path.stem().string() + suffix;
-  const auto indexed_filename = original_path.stem().string() + "_" + std::to_string(file_index) +
+  if (file_index != 0) {
-                                original_path.extension().string();
+    indexed_filename += "_" + std::to_string(file_index);
+  }
+  indexed_filename += original_path.extension().string();
   return (original_path.parent_path() / indexed_filename).string();
 }
 

orbbec_camera/src/imu_timestamp_csv_logger.cpp

@@ -0,0 +1,354 @@
+#include "orbbec_camera/imu_timestamp_csv_logger.h"
+
+#include <algorithm>
+#include <chrono>
+#include <filesystem>
+#include <iomanip>
+#include <sstream>
+#include <utility>
+#include <vector>
+
+namespace orbbec_camera {
+namespace {
+
+constexpr size_t kCompletedQueueSoftLimit = 1000;
+constexpr size_t kFlushBatchSize = 100;
+// The header occupies the first row, leaving 1,024,575 rows for IMU data.
+constexpr uint64_t kMaxCsvRowsPerFileIncludingHeader = 1'024'576;
+constexpr auto kFlushInterval = std::chrono::seconds(1);
+
+}  // namespace
+
+ImuTimestampCsvLogger::ImuTimestampCsvLogger(const std::string &frame_csv_file_path,
+                                             OutputMode output_mode, rclcpp::Logger logger)
+    : logger_(std::move(logger)),
+      enabled_(!frame_csv_file_path.empty()),
+      csv_enabled_(!frame_csv_file_path.empty()),
+      frame_csv_file_path_(frame_csv_file_path),
+      output_mode_(output_mode) {
+  if (!enabled_) {
+    return;
+  }
+
+  if (csv_enabled_) {
+    try {
+      const auto path = std::filesystem::path(csvFilePathForIndex(0));
+      if (path.has_parent_path() && !std::filesystem::exists(path.parent_path())) {
+        std::filesystem::create_directories(path.parent_path());
+      }
+    } catch (const std::exception &e) {
+      RCLCPP_ERROR_STREAM(logger_, "Failed to prepare IMU timestamp CSV path "
+                                       << csvFilePathForIndex(0) << ": " << e.what());
+      csv_enabled_ = false;
+      csv_writer_failed_ = true;
+    }
+  }
+
+  if (csv_enabled_ && !openCsvFile(0)) {
+    csv_enabled_ = false;
+    csv_writer_failed_ = true;
+  }
+
+  enabled_ = csv_enabled_;
+  if (csv_enabled_) {
+    writer_thread_ = std::thread([this]() { writerThreadMain(); });
+  }
+
+  if (enabled_) {
+    RCLCPP_INFO_STREAM(logger_,
+                       "IMU timestamp logger enabled: csv_file=" << csvFilePathForIndex(0));
+  }
+}
+
+ImuTimestampCsvLogger::~ImuTimestampCsvLogger() noexcept { shutdown(); }
+
+void ImuTimestampCsvLogger::recordFrameSet(const std::shared_ptr<ob::Frame> &accel_frame,
+                                           const std::shared_ptr<ob::Frame> &gyro_frame,
+                                           int64_t arrival_system_us,
+                                           std::optional<int64_t> publish_system_us) {
+  if (!enabled_ || output_mode_ != OutputMode::SYNCED || (!accel_frame && !gyro_frame)) {
+    return;
+  }
+  recordFrames(accel_frame, gyro_frame, arrival_system_us, publish_system_us);
+}
+
+void ImuTimestampCsvLogger::recordStandaloneFrame(OBStreamType stream_type,
+                                                  const std::shared_ptr<ob::Frame> &frame,
+                                                  int64_t arrival_system_us,
+                                                  std::optional<int64_t> publish_system_us) {
+  if (!enabled_ || !frame) {
+    return;
+  }
+  if (stream_type == OB_STREAM_ACCEL && output_mode_ == OutputMode::ACCEL) {
+    recordFrames(frame, nullptr, arrival_system_us, publish_system_us);
+  } else if (stream_type == OB_STREAM_GYRO && output_mode_ == OutputMode::GYRO) {
+    recordFrames(nullptr, frame, arrival_system_us, publish_system_us);
+  }
+}
+
+void ImuTimestampCsvLogger::shutdown() {
+  if (!enabled_) {
+    return;
+  }
+
+  {
+    std::lock_guard<std::mutex> state_lock(state_mutex_);
+    if (shutdown_requested_) {
+      return;
+    }
+    shutdown_requested_ = true;
+  }
+
+  completed_rows_cv_.notify_all();
+  if (writer_thread_.joinable()) {
+    writer_thread_.join();
+  }
+
+  if (csv_stream_.is_open()) {
+    csv_stream_.flush();
+    csv_stream_.close();
+  }
+}
+
+void ImuTimestampCsvLogger::recordFrames(const std::shared_ptr<ob::Frame> &accel_frame,
+                                         const std::shared_ptr<ob::Frame> &gyro_frame,
+                                         int64_t arrival_system_us,
+                                         std::optional<int64_t> publish_system_us) {
+  std::lock_guard<std::mutex> lock(state_mutex_);
+  if (shutdown_requested_) {
+    return;
+  }
+
+  PendingRow row;
+  row.row_id = next_row_id_++;
+  if (accel_frame) {
+    populateStreamState(row.accel, accel_frame, arrival_system_us, publish_system_us);
+  }
+  if (gyro_frame) {
+    populateStreamState(row.gyro, gyro_frame, arrival_system_us, publish_system_us);
+  }
+  enqueueCompletedRow(row);
+}
+
+void ImuTimestampCsvLogger::populateStreamState(StreamState &state,
+                                                const std::shared_ptr<ob::Frame> &frame,
+                                                int64_t arrival_system_us,
+                                                std::optional<int64_t> publish_system_us) {
+  state.has_frame = true;
+  state.device_ts_us = static_cast<int64_t>(frame->getTimeStampUs());
+  state.global_ts_us = static_cast<int64_t>(frame->getGlobalTimeStampUs());
+  state.sdk_system_ts_us = static_cast<int64_t>(frame->getSystemTimeStampUs());
+  state.arrival_system_us = arrival_system_us;
+  state.publish_system_us = publish_system_us;
+}
+
+void ImuTimestampCsvLogger::enqueueCompletedRow(const PendingRow &row) {
+  if (!csv_enabled_ || csv_writer_failed_) {
+    return;
+  }
+
+  std::lock_guard<std::mutex> queue_lock(completed_rows_mutex_);
+  completed_rows_.push_back(row);
+  if (completed_rows_.size() > kCompletedQueueSoftLimit) {
+    if (!queue_warning_active_) {
+      RCLCPP_WARN_STREAM(
+          logger_, "IMU timestamp CSV queue size exceeded " << kCompletedQueueSoftLimit << " rows");
+      queue_warning_active_ = true;
+    }
+  } else {
+    queue_warning_active_ = false;
+  }
+  completed_rows_cv_.notify_one();
+}
+
+std::string ImuTimestampCsvLogger::serializeRow(const PendingRow &row) const {
+  if (output_mode_ == OutputMode::ACCEL) {
+    return serializeStreamColumns(row.accel);
+  }
+  if (output_mode_ == OutputMode::GYRO) {
+    return serializeStreamColumns(row.gyro);
+  }
+  std::ostringstream ss;
+  ss << serializeStreamColumns(row.accel) << "," << serializeStreamColumns(row.gyro);
+  return ss.str();
+}
+
+std::string ImuTimestampCsvLogger::serializeStreamColumns(const StreamState &state) {
+  std::vector<std::string> fields(5, "");
+  if (state.has_frame) {
+    fields[0] = formatSecondsColumn(state.device_ts_us);
+    fields[1] = formatSecondsColumn(state.global_ts_us);
+    fields[2] = formatSecondsColumn(state.sdk_system_ts_us);
+    fields[3] = formatSecondsColumn(state.arrival_system_us);
+    fields[4] = formatOptionalSecondsColumn(state.publish_system_us);
+  }
+
+  std::ostringstream ss;
+  for (size_t i = 0; i < fields.size(); ++i) {
+    if (i != 0) {
+      ss << ",";
+    }
+    ss << fields[i];
+  }
+  return ss.str();
+}
+
+std::string ImuTimestampCsvLogger::formatSecondsColumn(int64_t time_us) {
+  std::ostringstream ss;
+  ss << std::fixed << std::setprecision(6) << (static_cast<long double>(time_us) / 1000000.0L);
+  return ss.str();
+}
+
+std::string ImuTimestampCsvLogger::formatOptionalSecondsColumn(
+    const std::optional<int64_t> &value) {
+  if (!value.has_value()) {
+    return "";
+  }
+  return formatSecondsColumn(*value);
+}
+
+std::string ImuTimestampCsvLogger::csvHeader() const {
+  std::ostringstream ss;
+  const auto append_stream_header = [&ss](const char *prefix) {
+    ss << prefix << "_device_ts_sec,";
+    ss << prefix << "_global_ts_sec,";
+    ss << prefix << "_system_ts_sec,";
+    ss << prefix << "_arrival_system_ts_sec,";
+    ss << prefix << "_publish_system_ts_sec";
+  };
+  if (output_mode_ == OutputMode::SYNCED || output_mode_ == OutputMode::ACCEL) {
+    append_stream_header("accel");
+  }
+  if (output_mode_ == OutputMode::SYNCED) {
+    ss << ",";
+  }
+  if (output_mode_ == OutputMode::SYNCED || output_mode_ == OutputMode::GYRO) {
+    append_stream_header("gyro");
+  }
+  return ss.str();
+}
+
+void ImuTimestampCsvLogger::writerThreadMain() {
+  if (!csv_enabled_ || csv_writer_failed_) {
+    return;
+  }
+
+  size_t rows_since_flush = 0;
+  auto last_flush = std::chrono::steady_clock::now();
+
+  while (true) {
+    std::deque<PendingRow> rows_to_write;
+    {
+      std::unique_lock<std::mutex> lock(completed_rows_mutex_);
+      completed_rows_cv_.wait_for(lock, kFlushInterval, [this]() {
+        return shutdown_requested_ || !completed_rows_.empty();
+      });
+      rows_to_write.swap(completed_rows_);
+    }
+
+    std::stable_sort(rows_to_write.begin(), rows_to_write.end(),
+                     [](const auto &lhs, const auto &rhs) { return lhs.row_id < rhs.row_id; });
+
+    for (const auto &row : rows_to_write) {
+      if (csv_rows_written_ >= kMaxCsvRowsPerFileIncludingHeader) {
+        if (!rotateCsvFile()) {
+          csv_writer_failed_ = true;
+          break;
+        }
+        rows_since_flush = 0;
+        last_flush = std::chrono::steady_clock::now();
+      }
+
+      if (!csv_stream_.is_open()) {
+        csv_writer_failed_ = true;
+        break;
+      }
+
+      csv_stream_ << serializeRow(row) << "\n";
+      if (!csv_stream_) {
+        RCLCPP_ERROR_STREAM(logger_, "Failed to write IMU timestamp CSV file: "
+                                         << csvFilePathForIndex(csv_file_index_));
+        csv_writer_failed_ = true;
+        break;
+      }
+      ++csv_rows_written_;
+      ++rows_since_flush;
+    }
+
+    if (csv_writer_failed_) {
+      break;
+    }
+
+    const auto now = std::chrono::steady_clock::now();
+    if (csv_stream_.is_open() && (rows_since_flush >= kFlushBatchSize ||
+                                  now - last_flush >= kFlushInterval || shutdown_requested_)) {
+      csv_stream_.flush();
+      rows_since_flush = 0;
+      last_flush = now;
+    }
+
+    std::lock_guard<std::mutex> lock(completed_rows_mutex_);
+    if (shutdown_requested_ && completed_rows_.empty()) {
+      break;
+    }
+  }
+}
+
+std::string ImuTimestampCsvLogger::csvFilePathForIndex(uint64_t file_index) const {
+  const std::filesystem::path original_path(frame_csv_file_path_);
+  std::string suffix;
+  if (output_mode_ == OutputMode::SYNCED) {
+    suffix = "_imu";
+  } else if (output_mode_ == OutputMode::ACCEL) {
+    suffix = "_accel";
+  } else {
+    suffix = "_gyro";
+  }
+  auto indexed_filename = original_path.stem().string() + suffix;
+  if (file_index != 0) {
+    indexed_filename += "_" + std::to_string(file_index);
+  }
+  indexed_filename += original_path.extension().string();
+  return (original_path.parent_path() / indexed_filename).string();
+}
+
+bool ImuTimestampCsvLogger::openCsvFile(uint64_t file_index) {
+  const auto file_path = csvFilePathForIndex(file_index);
+  csv_stream_.clear();
+  csv_stream_.open(file_path, std::ios::out | std::ios::trunc);
+  if (!csv_stream_.is_open()) {
+    RCLCPP_ERROR_STREAM(logger_, "Failed to open IMU timestamp CSV file: " << file_path);
+    return false;
+  }
+
+  csv_stream_ << csvHeader() << "\n";
+  csv_stream_.flush();
+  if (!csv_stream_) {
+    RCLCPP_ERROR_STREAM(logger_, "Failed to write IMU timestamp CSV header: " << file_path);
+    csv_stream_.close();
+    return false;
+  }
+
+  csv_file_index_ = file_index;
+  csv_rows_written_ = 1;
+  return true;
+}
+
+bool ImuTimestampCsvLogger::rotateCsvFile() {
+  if (csv_stream_.is_open()) {
+    csv_stream_.flush();
+    csv_stream_.close();
+  }
+
+  const auto next_file_index = csv_file_index_ + 1;
+  if (!openCsvFile(next_file_index)) {
+    return false;
+  }
+
+  RCLCPP_INFO_STREAM(logger_, "IMU timestamp CSV reached " << kMaxCsvRowsPerFileIncludingHeader
+                                                           << " rows; continuing in "
+                                                           << csvFilePathForIndex(csv_file_index_));
+  return true;
+}
+
+}  // namespace orbbec_camera

orbbec_camera/src/ob_camera_node.cpp

@@ -65,13 +65,6 @@ std::string OBCameraNode::normalizeDepthFilterName(const std::string &filter_nam
 
 namespace {
 
-constexpr char kEnhancedDepthSupportedTargetResolutions[] = "640x480/1280x720/1280x800";
-constexpr char kEnhancedDepthSupportedDepthFormats[] = "Y10/Y11/Y12/Y14/Y16/Z16";
-constexpr double kViewerColorizerGamma = 0.65;
-constexpr uint16_t kViewerColorizerMaxDistanceMm = 10000;
-constexpr uint16_t kViewerColorizerDefaultMinDistanceMm = 100;
-constexpr uint16_t kViewerColorizerG305MinDistanceMm = 40;
-
 std::string getDepthFilterStatusName(const std::string &filter_name) {
   if (filter_name == "SpatialAdvancedFilter") {
     return "SpatialFilter";
@@ -743,10 +736,19 @@ OBCameraNode::OBCameraNode(rclcpp::Node *node, std::shared_ptr<ob::Device> devic
   setupTopics();
 
   if (enable_frame_drop_log_ || !frame_timestamp_csv_file_.empty()) {
-    frame_timestamp_csv_logger_ = std::make_unique<FrameTimestampCsvLogger>(
+    TimestampCsvLogger::Config timestamp_config;
-        enable_frame_drop_log_, frame_timestamp_csv_file_, logger_);
+    timestamp_config.frame_drop_log_enabled = enable_frame_drop_log_;
-    if (!frame_timestamp_csv_logger_->enabled()) {
+    timestamp_config.csv_file_path = frame_timestamp_csv_file_;
-      frame_timestamp_csv_logger_.reset();
+    timestamp_config.frame_sync_enabled = enable_frame_sync_;
+    timestamp_config.color_enabled = enable_stream_[COLOR];
+    timestamp_config.depth_enabled = enable_stream_[DEPTH];
+    timestamp_config.imu_sync_enabled = enable_sync_output_accel_gyro_;
+    timestamp_config.accel_enabled = enable_stream_[ACCEL];
+    timestamp_config.gyro_enabled = enable_stream_[GYRO];
+    timestamp_csv_logger_ =
+        std::make_unique<TimestampCsvLogger>(std::move(timestamp_config), logger_);
+    if (!timestamp_csv_logger_->enabled()) {
+      timestamp_csv_logger_.reset();
     }
   }
 
@@ -816,15 +818,6 @@ void OBCameraNode::clean() noexcept {
   is_running_.store(false);
   is_camera_node_initialized_.store(false);
 
-  try {
-    if (frame_timestamp_csv_logger_) {
-      frame_timestamp_csv_logger_->shutdown();
-      frame_timestamp_csv_logger_.reset();
-    }
-  } catch (...) {
-    RCLCPP_WARN_STREAM(logger_, "Exception while shutting down frame timestamp CSV logger");
-  }
-
   // Stop diagnostic timer and updater first BEFORE acquiring device_lock to prevent deadlock
   try {
     if (diagnostic_timer_) {
@@ -884,6 +877,11 @@ void OBCameraNode::clean() noexcept {
     RCLCPP_DEBUG_STREAM(logger_, "Exception while stopping streams");
   }
 
+  if (timestamp_csv_logger_) {
+    timestamp_csv_logger_->shutdown();
+    timestamp_csv_logger_.reset();
+  }
+
   // Clean up d2c_viewer_ before cleaning buffers
   RCLCPP_DEBUG_STREAM(logger_, "Clean d2c_viewer");
   try {
@@ -1042,6 +1040,17 @@ void OBCameraNode::setupDevices() {
                                       OB_PROP_USB_SYNC_VOLTAGE_LEVEL_INT));
     }
   }
+  if (monitor_poll_interval_sec_ != -1) {
+    try {
+      const auto interval_ms = static_cast<uint32_t>(monitor_poll_interval_sec_) * 1000U;
+      device_->setMonitorPollInterval(interval_ms);
+      RCLCPP_INFO_STREAM(
+          logger_, "Current monitor poll interval: " << device_->getMonitorPollInterval() << " ms");
+    } catch (const ob::Error &e) {
+      RCLCPP_WARN_STREAM(logger_, "Skipping monitor poll interval configuration: "
+                                      << orbbec_camera::formatObErrorWithStatus(e));
+    }
+  }
   if (should_apply_launch_config("enable_heartbeat") &&
       device_->isPropertySupported(OB_PROP_HEARTBEAT_BOOL, OB_PERMISSION_READ_WRITE)) {
     TRY_TO_SET_PROPERTY(setBoolProperty, OB_PROP_HEARTBEAT_BOOL, enable_heartbeat_);
@@ -4196,8 +4205,14 @@ void OBCameraNode::startIMUSyncStream() {
       auto frameSet = frame->as<ob::FrameSet>();
       auto aFrame = frameSet->getFrame(OB_FRAME_ACCEL);
       auto gFrame = frameSet->getFrame(OB_FRAME_GYRO);
+      const bool log_imu_timestamps = is_camera_node_initialized_.load() && rclcpp::ok() &&
+                                      timestamp_csv_logger_ &&
+                                      timestamp_csv_logger_->syncedImuEnabled();
+      const auto arrival_system_us = log_imu_timestamps ? getSystemNowUs() : 0;
       if (aFrame && gFrame) {
-        onNewIMUFrameSyncOutputCallback(aFrame, gFrame);
+        onNewIMUFrameSyncOutputCallback(aFrame, gFrame, arrival_system_us);
+      } else if (log_imu_timestamps && (aFrame || gFrame)) {
+        timestamp_csv_logger_->recordSyncedImu(aFrame, gFrame, arrival_system_us, std::nullopt);
       }
     });
 
@@ -4763,6 +4778,16 @@ void OBCameraNode::getParameters() {
   setAndGetNodeParameter<int>(max_depth_limit_, "max_depth_limit", 0);
   setAndGetNodeParameter<bool>(enable_heartbeat_, "enable_heartbeat", false);
   setAndGetNodeParameter<bool>(enable_firmware_log_, "enable_firmware_log", false);
+  setAndGetNodeParameter<int>(monitor_poll_interval_sec_, "monitor_poll_interval_sec", -1);
+  if (monitor_poll_interval_sec_ != -1 &&
+      (monitor_poll_interval_sec_ < 1 || monitor_poll_interval_sec_ > 10)) {
+    const auto requested_monitor_poll_interval_sec = monitor_poll_interval_sec_;
+    monitor_poll_interval_sec_ = std::clamp(monitor_poll_interval_sec_, 1, 10);
+    RCLCPP_WARN_STREAM(logger_, "monitor_poll_interval_sec value "
+                                    << requested_monitor_poll_interval_sec
+                                    << " is out of range [1, 10], clamped to "
+                                    << monitor_poll_interval_sec_);
+  }
   setAndGetNodeParameter<bool>(enable_fps_boost_, "enable_fps_boost", false);
   setAndGetNodeParameter<std::string>(time_domain_, "time_domain", "global");
   time_domain_ = normalizeClosedSetParameterValue(logger_, "time_domain", time_domain_,
@@ -5132,6 +5157,9 @@ void OBCameraNode::setupPipelineConfig() {
 }
 
 bool OBCameraNode::validateEnhancedDepthFilterConfig(std::string &message) const {
+  constexpr char kEnhancedDepthSupportedTargetResolutions[] = "640x480/1280x720/1280x800";
+  constexpr char kEnhancedDepthSupportedDepthFormats[] = "Y10/Y11/Y12/Y14/Y16/Z16";
+
   if (!enable_stream_.count(COLOR) || !enable_stream_.at(COLOR) || !enable_stream_.count(DEPTH) ||
       !enable_stream_.at(DEPTH)) {
     message = "Enhanced depth filter requires color and depth streams";
@@ -5740,6 +5768,11 @@ cv::Mat OBCameraNode::colorizeDepthImage(const cv::Mat &depth_image,
     return {};
   }
 
+  constexpr double kViewerColorizerGamma = 0.65;
+  constexpr uint16_t kViewerColorizerMaxDistanceMm = 10000;
+  constexpr uint16_t kViewerColorizerDefaultMinDistanceMm = 100;
+  constexpr uint16_t kViewerColorizerG305MinDistanceMm = 40;
+
   cv::Mat depth_16u;
   depth_image.convertTo(depth_16u, CV_16UC1);
 
@@ -6312,7 +6345,7 @@ void OBCameraNode::onNewFrameSetCallback(std::shared_ptr<ob::FrameSet> frame_set
   if (frame_set == nullptr) {
     return;
   }
-  if (frame_timestamp_csv_logger_ && frame_timestamp_csv_logger_->enabled()) {
+  if (timestamp_csv_logger_ && timestamp_csv_logger_->imageEnabled()) {
     const auto frame_set_arrival_system_us = getSystemNowUs();
     const auto frame_set_arrival_steady_us = getSteadyNowUs();
     auto final_color_frame = frame_set->getFrame(OB_FRAME_COLOR);
@@ -6322,7 +6355,7 @@ void OBCameraNode::onNewFrameSetCallback(std::shared_ptr<ob::FrameSet> frame_set
     const bool color_publish_expected = track_color;
     const bool depth_publish_expected = track_depth;
 
-    frame_timestamp_csv_logger_->recordFrameSet(
+    timestamp_csv_logger_->recordImageFrameSet(
         final_color_frame, final_depth_frame, frame_set_arrival_system_us,
         frame_set_arrival_steady_us, track_color, track_depth, color_publish_expected,
         depth_publish_expected);
@@ -6797,7 +6830,7 @@ bool OBCameraNode::decodeColorFrameToBuffer(const std::shared_ptr<ob::Frame> &fr
       target_buffer_size = &rgb_buffer_size_;
     }
     if (video_frame->getDataSize() > *target_buffer_size) {
-      delete[](*target_buffer);
+      delete[] (*target_buffer);
       *target_buffer_size = video_frame->getDataSize();
       *target_buffer = new uint8_t[*target_buffer_size];
       buffer = *target_buffer;
@@ -6849,10 +6882,11 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame> &frame,
   if (frame == nullptr) {
     return;
   }
+  const bool log_image_timestamps =
+      timestamp_csv_logger_ && timestamp_csv_logger_->imageStreamEnabled(stream_index.first);
   const auto record_image_publish_skipped = [&]() {
-    if (frame_timestamp_csv_logger_ && frame_timestamp_csv_logger_->enabled() &&
+    if (log_image_timestamps) {
-        (stream_index == COLOR || stream_index == DEPTH)) {
+      timestamp_csv_logger_->recordImagePublishSkipped(stream_index.first, frame);
-      frame_timestamp_csv_logger_->recordImagePublishSkipped(stream_index.first, frame);
     }
   };
   CHECK_NOTNULL(image_publishers_[stream_index]);
@@ -6969,10 +7003,9 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame> &frame,
   }
   if ((stream_index == COLOR || stream_index == COLOR_LEFT || stream_index == COLOR_RIGHT) &&
       frame->getFormat() == OB_FORMAT_MJPG && has_compressed_image_subscriber) {
-    if (!has_raw_image_subscriber && stream_index == COLOR && frame_timestamp_csv_logger_ &&
+    if (!has_raw_image_subscriber && stream_index == COLOR && log_image_timestamps) {
-        frame_timestamp_csv_logger_->enabled()) {
+      timestamp_csv_logger_->recordImagePrePublish(stream_index.first, frame, getSystemNowUs(),
-      frame_timestamp_csv_logger_->recordPreImagePublish(stream_index.first, frame,
+                                                   getSteadyNowUs());
-                                                         getSystemNowUs(), getSteadyNowUs());
     }
     publishCompressedColorImage(frame, stream_index, timestamp, frame_id);
     if (!has_raw_image_subscriber && stream_index == COLOR) {
@@ -7058,10 +7091,9 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame> &frame,
       record_image_publish_skipped();
       return;
     }
-    if (frame_timestamp_csv_logger_ && frame_timestamp_csv_logger_->enabled() &&
+    if (log_image_timestamps) {
-        (stream_index == COLOR || stream_index == DEPTH)) {
+      timestamp_csv_logger_->recordImagePrePublish(stream_index.first, frame, getSystemNowUs(),
-      frame_timestamp_csv_logger_->recordPreImagePublish(stream_index.first, frame,
+                                                   getSteadyNowUs());
-                                                         getSystemNowUs(), getSteadyNowUs());
     }
     if (stream_index == COLOR) {
       fps_delay_status_color_->tick(frame_timestamp);
@@ -7248,11 +7280,20 @@ void OBCameraNode::saveImageToFile(const stream_index_pair &stream_index, const
 }
 
 void OBCameraNode::onNewIMUFrameSyncOutputCallback(const std::shared_ptr<ob::Frame> &accelframe,
-                                                   const std::shared_ptr<ob::Frame> &gryoframe) {
+                                                   const std::shared_ptr<ob::Frame> &gryoframe,
+                                                   int64_t arrival_system_us) {
   if (!is_camera_node_initialized_.load() || !rclcpp::ok()) {
     return;
   }
+  const auto record_timestamps = [&](std::optional<int64_t> publish_system_us) {
+    if (arrival_system_us != 0 && timestamp_csv_logger_ &&
+        timestamp_csv_logger_->syncedImuEnabled()) {
+      timestamp_csv_logger_->recordSyncedImu(accelframe, gryoframe, arrival_system_us,
+                                             publish_system_us);
+    }
+  };
   if (!imu_gyro_accel_publisher_) {
+    record_timestamps(std::nullopt);
     RCLCPP_ERROR_STREAM(logger_, "stream Accel Gryo publisher not initialized");
     return;
   }
@@ -7260,6 +7301,7 @@ void OBCameraNode::onNewIMUFrameSyncOutputCallback(const std::shared_ptr<ob::Fra
   has_subscriber = has_subscriber || imu_info_publishers_[GYRO]->get_subscription_count() > 0;
   has_subscriber = has_subscriber || imu_info_publishers_[ACCEL]->get_subscription_count() > 0;
   if (!has_subscriber) {
+    record_timestamps(std::nullopt);
     return;
   }
   auto imu_msg = sensor_msgs::msg::Imu();
@@ -7293,7 +7335,9 @@ void OBCameraNode::onNewIMUFrameSyncOutputCallback(const std::shared_ptr<ob::Fra
   imu_msg.linear_acceleration.y = accelData.y - accel_info.bias[1];
   imu_msg.linear_acceleration.z = accelData.z - accel_info.bias[2];
 
+  const auto publish_system_us = getSystemNowUs();
   imu_gyro_accel_publisher_->publish(imu_msg);
+  record_timestamps(publish_system_us);
 }
 
 void OBCameraNode::onNewIMUFrameCallback(const std::shared_ptr<ob::Frame> &frame,
@@ -7301,7 +7345,17 @@ void OBCameraNode::onNewIMUFrameCallback(const std::shared_ptr<ob::Frame> &frame
   if (!is_camera_node_initialized_.load() || !rclcpp::ok()) {
     return;
   }
+  const bool log_imu_timestamps =
+      timestamp_csv_logger_ && timestamp_csv_logger_->standaloneImuEnabled(stream_index.first);
+  const auto arrival_system_us = log_imu_timestamps ? getSystemNowUs() : 0;
+  const auto record_timestamps = [&](std::optional<int64_t> publish_system_us) {
+    if (log_imu_timestamps) {
+      timestamp_csv_logger_->recordStandaloneImu(stream_index.first, frame, arrival_system_us,
+                                                 publish_system_us);
+    }
+  };
   if (!imu_publishers_.count(stream_index)) {
+    record_timestamps(std::nullopt);
     RCLCPP_ERROR_STREAM(logger_,
                         "stream " << stream_name_[stream_index] << " publisher not initialized");
     return;
@@ -7310,6 +7364,7 @@ void OBCameraNode::onNewIMUFrameCallback(const std::shared_ptr<ob::Frame> &frame
   has_subscriber =
       has_subscriber || imu_info_publishers_[stream_index]->get_subscription_count() > 0;
   if (!has_subscriber) {
+    record_timestamps(std::nullopt);
     return;
   }
   auto imu_msg = sensor_msgs::msg::Imu();
@@ -7336,10 +7391,13 @@ void OBCameraNode::onNewIMUFrameCallback(const std::shared_ptr<ob::Frame> &frame
     imu_msg.linear_acceleration.y = data.y - imu_info.bias[1];
     imu_msg.linear_acceleration.z = data.z - imu_info.bias[2];
   } else {
+    record_timestamps(std::nullopt);
     RCLCPP_ERROR(logger_, "Unsupported IMU frame type");
     return;
   }
+  const auto publish_system_us = getSystemNowUs();
   imu_publishers_[stream_index]->publish(imu_msg);
+  record_timestamps(publish_system_us);
 }
 
 void OBCameraNode::setDefaultIMUMessage(sensor_msgs::msg::Imu &imu_msg) {
@@ -8434,9 +8492,9 @@ void OBCameraNode::setFilterCallback(const std::shared_ptr<SetFilter ::Request>
         if (request->filter_param.size() > 1) {
           temporal_filter->setDiffScale(request->filter_param[0]);
           temporal_filter->setWeight(request->filter_param[1]);
-          RCLCPP_INFO_STREAM(logger_, "Set TemporalFilter params: "
+          RCLCPP_INFO_STREAM(
-                                          << "\ndiff_scale:" << request->filter_param[0]
+              logger_, "Set TemporalFilter params: " << "\ndiff_scale:" << request->filter_param[0]
-                                          << "\nweight:" << request->filter_param[1]);
+                                                     << "\nweight:" << request->filter_param[1]);
           temporal_filter_diff_threshold_ = request->filter_param[0];
           temporal_filter_weight_ = request->filter_param[1];
         } else {

orbbec_camera/src/ros_service.cpp

@@ -303,6 +303,27 @@ void OBCameraNode::setupCameraCtrlServices() {
                                        std::shared_ptr<SetBool::Response> response) {
         setAutoWhiteBalanceCallback(request, response);
       });
+  if (isPropertyReadable(device_, OB_PROP_COLOR_AE_AWB_STAT_INT)) {
+    get_ae_awb_status_srv_ = node_->create_service<GetInt32>(
+        "get_color_ae_awb_status", [this](const std::shared_ptr<GetInt32::Request> request,
+                                          std::shared_ptr<GetInt32::Response> response) {
+          getAeAwbStatusCallback(request, response);
+        });
+  }
+  if (isPropertyReadable(device_, OB_STRUCT_COLOR_AWB_GAIN)) {
+    get_awb_gain_srv_ = node_->create_service<GetAwbGain>(
+        "get_color_awb_gain", [this](const std::shared_ptr<GetAwbGain::Request> request,
+                                     std::shared_ptr<GetAwbGain::Response> response) {
+          getAwbGainCallback(request, response);
+        });
+  }
+  if (isPropertyWritable(device_, OB_STRUCT_COLOR_AWB_GAIN)) {
+    set_awb_gain_srv_ = node_->create_service<SetAwbGain>(
+        "set_color_awb_gain", [this](const std::shared_ptr<SetAwbGain::Request> request,
+                                     std::shared_ptr<SetAwbGain::Response> response) {
+          setAwbGainCallback(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) {
@@ -1266,6 +1287,75 @@ void OBCameraNode::setAutoWhiteBalanceCallback(const std::shared_ptr<SetBool::Re
   }
 }
 
+void OBCameraNode::getAeAwbStatusCallback(const std::shared_ptr<GetInt32::Request>& request,
+                                          std::shared_ptr<GetInt32::Response>& 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<GetAwbGain::Request>& request,
+                                      std::shared_ptr<GetAwbGain::Response>& response) {
+  (void)request;
+  try {
+    OBAwbGainParams gain{};
+    uint32_t size = sizeof(gain);
+    device_->getStructuredData(OB_STRUCT_COLOR_AWB_GAIN, reinterpret_cast<uint8_t*>(&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<SetAwbGain::Request>& request,
+                                      std::shared_ptr<SetAwbGain::Response>& 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<const uint8_t*>(&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<std_srvs::srv::SetBool::Request>& request,
     std::shared_ptr<std_srvs::srv::SetBool::Response>& response,

orbbec_camera/src/timestamp_csv_logger.cpp

@@ -0,0 +1,194 @@
+#include "orbbec_camera/timestamp_csv_logger.h"
+
+#include "orbbec_camera/frame_timestamp_csv_logger.h"
+#include "orbbec_camera/imu_timestamp_csv_logger.h"
+
+#include <exception>
+#include <utility>
+
+namespace orbbec_camera {
+
+TimestampCsvLogger::TimestampCsvLogger(Config config, rclcpp::Logger logger)
+    : logger_(std::move(logger)) {
+  const auto create_image_logger = [this, &config](FrameTimestampCsvLogger::OutputMode mode) {
+    auto timestamp_logger = std::make_unique<FrameTimestampCsvLogger>(
+        config.frame_drop_log_enabled, config.csv_file_path, mode, logger_);
+    if (!timestamp_logger->enabled()) {
+      timestamp_logger.reset();
+    }
+    return timestamp_logger;
+  };
+
+  if (config.frame_sync_enabled && (config.color_enabled || config.depth_enabled)) {
+    synced_image_logger_ = create_image_logger(FrameTimestampCsvLogger::OutputMode::SYNCED);
+  } else {
+    if (config.color_enabled) {
+      color_logger_ = create_image_logger(FrameTimestampCsvLogger::OutputMode::COLOR);
+    }
+    if (config.depth_enabled) {
+      depth_logger_ = create_image_logger(FrameTimestampCsvLogger::OutputMode::DEPTH);
+    }
+  }
+
+  if (config.csv_file_path.empty()) {
+    return;
+  }
+
+  const auto create_imu_logger = [this, &config](ImuTimestampCsvLogger::OutputMode mode) {
+    auto timestamp_logger =
+        std::make_unique<ImuTimestampCsvLogger>(config.csv_file_path, mode, logger_);
+    if (!timestamp_logger->enabled()) {
+      timestamp_logger.reset();
+    }
+    return timestamp_logger;
+  };
+
+  if (config.imu_sync_enabled) {
+    synced_imu_logger_ = create_imu_logger(ImuTimestampCsvLogger::OutputMode::SYNCED);
+  } else {
+    if (config.accel_enabled) {
+      accel_logger_ = create_imu_logger(ImuTimestampCsvLogger::OutputMode::ACCEL);
+    }
+    if (config.gyro_enabled) {
+      gyro_logger_ = create_imu_logger(ImuTimestampCsvLogger::OutputMode::GYRO);
+    }
+  }
+}
+
+TimestampCsvLogger::~TimestampCsvLogger() noexcept { shutdown(); }
+
+bool TimestampCsvLogger::enabled() const {
+  return imageEnabled() || syncedImuEnabled() || standaloneImuEnabled(OB_STREAM_ACCEL) ||
+         standaloneImuEnabled(OB_STREAM_GYRO);
+}
+
+bool TimestampCsvLogger::imageEnabled() const {
+  return (synced_image_logger_ && synced_image_logger_->enabled()) ||
+         (color_logger_ && color_logger_->enabled()) || (depth_logger_ && depth_logger_->enabled());
+}
+
+bool TimestampCsvLogger::imageStreamEnabled(OBStreamType stream_type) const {
+  const auto *timestamp_logger = imageLoggerForStream(stream_type);
+  return timestamp_logger && timestamp_logger->enabled();
+}
+
+bool TimestampCsvLogger::syncedImuEnabled() const {
+  return synced_imu_logger_ && synced_imu_logger_->enabled();
+}
+
+bool TimestampCsvLogger::standaloneImuEnabled(OBStreamType stream_type) const {
+  const auto *timestamp_logger = standaloneImuLoggerForStream(stream_type);
+  return timestamp_logger && timestamp_logger->enabled();
+}
+
+void TimestampCsvLogger::recordImageFrameSet(const std::shared_ptr<ob::Frame> &color_frame,
+                                             const std::shared_ptr<ob::Frame> &depth_frame,
+                                             int64_t arrival_system_us, int64_t arrival_steady_us,
+                                             bool track_color, bool track_depth,
+                                             bool color_image_publish_expected,
+                                             bool depth_image_publish_expected) {
+  if (synced_image_logger_) {
+    synced_image_logger_->recordFrameSet(
+        color_frame, depth_frame, arrival_system_us, arrival_steady_us, track_color, track_depth,
+        color_image_publish_expected, depth_image_publish_expected);
+    return;
+  }
+
+  if (track_color && color_logger_) {
+    color_logger_->recordStandaloneFrameArrival(OB_STREAM_COLOR, color_frame, arrival_system_us,
+                                                arrival_steady_us, color_image_publish_expected);
+  }
+  if (track_depth && depth_logger_) {
+    depth_logger_->recordStandaloneFrameArrival(OB_STREAM_DEPTH, depth_frame, arrival_system_us,
+                                                arrival_steady_us, depth_image_publish_expected);
+  }
+}
+
+void TimestampCsvLogger::recordImagePrePublish(OBStreamType stream_type,
+                                               const std::shared_ptr<ob::Frame> &frame,
+                                               int64_t publish_system_us,
+                                               int64_t publish_steady_us) {
+  auto *timestamp_logger = imageLoggerForStream(stream_type);
+  if (timestamp_logger) {
+    timestamp_logger->recordPreImagePublish(stream_type, frame, publish_system_us,
+                                            publish_steady_us);
+  }
+}
+
+void TimestampCsvLogger::recordImagePublishSkipped(OBStreamType stream_type,
+                                                   const std::shared_ptr<ob::Frame> &frame) {
+  auto *timestamp_logger = imageLoggerForStream(stream_type);
+  if (timestamp_logger) {
+    timestamp_logger->recordImagePublishSkipped(stream_type, frame);
+  }
+}
+
+void TimestampCsvLogger::recordSyncedImu(const std::shared_ptr<ob::Frame> &accel_frame,
+                                         const std::shared_ptr<ob::Frame> &gyro_frame,
+                                         int64_t arrival_system_us,
+                                         std::optional<int64_t> publish_system_us) {
+  if (synced_imu_logger_) {
+    synced_imu_logger_->recordFrameSet(accel_frame, gyro_frame, arrival_system_us,
+                                       publish_system_us);
+  }
+}
+
+void TimestampCsvLogger::recordStandaloneImu(OBStreamType stream_type,
+                                             const std::shared_ptr<ob::Frame> &frame,
+                                             int64_t arrival_system_us,
+                                             std::optional<int64_t> publish_system_us) {
+  auto *timestamp_logger = standaloneImuLoggerForStream(stream_type);
+  if (timestamp_logger) {
+    timestamp_logger->recordStandaloneFrame(stream_type, frame, arrival_system_us,
+                                            publish_system_us);
+  }
+}
+
+void TimestampCsvLogger::shutdown() noexcept {
+  if (shutdown_requested_.exchange(true)) {
+    return;
+  }
+
+  const auto shutdown_logger = [this](auto &timestamp_logger, const char *name) {
+    if (!timestamp_logger) {
+      return;
+    }
+    try {
+      timestamp_logger->shutdown();
+    } catch (const std::exception &e) {
+      RCLCPP_WARN_STREAM(logger_, "Exception while shutting down " << name << ": " << e.what());
+    } catch (...) {
+      RCLCPP_WARN_STREAM(logger_, "Unknown exception while shutting down " << name);
+    }
+  };
+
+  shutdown_logger(synced_image_logger_, "synced image timestamp CSV logger");
+  shutdown_logger(color_logger_, "color timestamp CSV logger");
+  shutdown_logger(depth_logger_, "depth timestamp CSV logger");
+  shutdown_logger(synced_imu_logger_, "synced IMU timestamp CSV logger");
+  shutdown_logger(accel_logger_, "accel timestamp CSV logger");
+  shutdown_logger(gyro_logger_, "gyro timestamp CSV logger");
+}
+
+FrameTimestampCsvLogger *TimestampCsvLogger::imageLoggerForStream(OBStreamType stream_type) const {
+  if (stream_type == OB_STREAM_COLOR) {
+    return synced_image_logger_ ? synced_image_logger_.get() : color_logger_.get();
+  }
+  if (stream_type == OB_STREAM_DEPTH) {
+    return synced_image_logger_ ? synced_image_logger_.get() : depth_logger_.get();
+  }
+  return nullptr;
+}
+
+ImuTimestampCsvLogger *TimestampCsvLogger::standaloneImuLoggerForStream(
+    OBStreamType stream_type) const {
+  if (stream_type == OB_STREAM_ACCEL) {
+    return accel_logger_.get();
+  }
+  if (stream_type == OB_STREAM_GYRO) {
+    return gyro_logger_.get();
+  }
+  return nullptr;
+}
+
+}  // namespace orbbec_camera

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
 )

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

orbbec_camera_msgs/package.xml

@@ -2,13 +2,14 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>orbbec_camera_msgs</name>
-  <version>2.9.3</version>
+  <version>2.10.1</version>
   <description>A package containing orbbec camera messages definitions.</description>
   <maintainer email="[email protected]">yalian</maintainer>
   <license>Apache-2.0</license>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 
+  <depend>action_msgs</depend>
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
   <build_depend>rosidl_default_generators</build_depend>

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

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

orbbec_description/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>orbbec_description</name>
-  <version>2.9.3</version>
+  <version>2.10.1</version>
   <description>TODO: Package description</description>
   <maintainer email="[email protected]">yalian</maintainer>
   <license>Apache-2.0</license>