diff --git a/orbbec_camera/CMakeLists.txt b/orbbec_camera/CMakeLists.txt index 9b5709a1..3d64408e 100644 --- a/orbbec_camera/CMakeLists.txt +++ b/orbbec_camera/CMakeLists.txt @@ -34,6 +34,7 @@ set(dependencies orbbec_camera_msgs rcl_interfaces rclcpp + rclcpp_action rclcpp_components rmw sensor_msgs @@ -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) diff --git a/orbbec_camera/SDK/arm64/include/libobsensor/h/Device.h b/orbbec_camera/SDK/arm64/include/libobsensor/h/Device.h index bb1d7ebe..cde03ed3 100644 --- a/orbbec_camera/SDK/arm64/include/libobsensor/h/Device.h +++ b/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) * diff --git a/orbbec_camera/SDK/arm64/include/libobsensor/h/ObTypes.h b/orbbec_camera/SDK/arm64/include/libobsensor/h/ObTypes.h index ecb87703..1a234540 100644 --- a/orbbec_camera/SDK/arm64/include/libobsensor/h/ObTypes.h +++ b/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. diff --git a/orbbec_camera/SDK/arm64/include/libobsensor/h/Property.h b/orbbec_camera/SDK/arm64/include/libobsensor/h/Property.h index 43896ad2..486a54f0 100644 --- a/orbbec_camera/SDK/arm64/include/libobsensor/h/Property.h +++ b/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 diff --git a/orbbec_camera/SDK/arm64/include/libobsensor/hpp/Device.hpp b/orbbec_camera/SDK/arm64/include/libobsensor/hpp/Device.hpp index 36cf9e41..37a420c2 100644 --- a/orbbec_camera/SDK/arm64/include/libobsensor/hpp/Device.hpp +++ b/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 diff --git a/orbbec_camera/SDK/arm64/lib/OrbbecSDKConfig-release.cmake b/orbbec_camera/SDK/arm64/lib/OrbbecSDKConfig-release.cmake index ce05f521..231fafb1 100644 --- a/orbbec_camera/SDK/arm64/lib/OrbbecSDKConfig-release.cmake +++ b/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) diff --git a/orbbec_camera/SDK/arm64/lib/OrbbecSDKVersion.cmake b/orbbec_camera/SDK/arm64/lib/OrbbecSDKVersion.cmake index cfbccf3a..7851ea03 100644 --- a/orbbec_camera/SDK/arm64/lib/OrbbecSDKVersion.cmake +++ b/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) diff --git a/orbbec_camera/SDK/arm64/lib/extensions/filters/libFilterProcessor.so b/orbbec_camera/SDK/arm64/lib/extensions/filters/libFilterProcessor.so index 4de8c912..072d0d21 100644 Binary files a/orbbec_camera/SDK/arm64/lib/extensions/filters/libFilterProcessor.so and b/orbbec_camera/SDK/arm64/lib/extensions/filters/libFilterProcessor.so differ diff --git a/orbbec_camera/SDK/arm64/lib/extensions/filters/libob_priv_filter.so b/orbbec_camera/SDK/arm64/lib/extensions/filters/libob_priv_filter.so index 3d6f6aa8..2c64eb87 100644 Binary files a/orbbec_camera/SDK/arm64/lib/extensions/filters/libob_priv_filter.so and b/orbbec_camera/SDK/arm64/lib/extensions/filters/libob_priv_filter.so differ diff --git a/orbbec_camera/SDK/arm64/lib/extensions/firmwareupdater/libfirmwareupdater.so b/orbbec_camera/SDK/arm64/lib/extensions/firmwareupdater/libfirmwareupdater.so index 4f1f001d..434e72f4 100644 Binary files a/orbbec_camera/SDK/arm64/lib/extensions/firmwareupdater/libfirmwareupdater.so and b/orbbec_camera/SDK/arm64/lib/extensions/firmwareupdater/libfirmwareupdater.so differ diff --git a/orbbec_camera/SDK/arm64/lib/extensions/frameprocessor/libob_frame_processor.so b/orbbec_camera/SDK/arm64/lib/extensions/frameprocessor/libob_frame_processor.so index 250efc5f..c155c3c9 100644 Binary files a/orbbec_camera/SDK/arm64/lib/extensions/frameprocessor/libob_frame_processor.so and b/orbbec_camera/SDK/arm64/lib/extensions/frameprocessor/libob_frame_processor.so differ diff --git a/orbbec_camera/SDK/arm64/lib/libOrbbecSDK.so.2 b/orbbec_camera/SDK/arm64/lib/libOrbbecSDK.so.2 index 07122dbb..027ba8ff 120000 --- a/orbbec_camera/SDK/arm64/lib/libOrbbecSDK.so.2 +++ b/orbbec_camera/SDK/arm64/lib/libOrbbecSDK.so.2 @@ -1 +1 @@ -libOrbbecSDK.so.2.9.3 \ No newline at end of file +libOrbbecSDK.so.2.10.1 \ No newline at end of file diff --git a/orbbec_camera/SDK/arm64/lib/libOrbbecSDK.so.2.9.3 b/orbbec_camera/SDK/arm64/lib/libOrbbecSDK.so.2.10.1 similarity index 69% rename from orbbec_camera/SDK/arm64/lib/libOrbbecSDK.so.2.9.3 rename to orbbec_camera/SDK/arm64/lib/libOrbbecSDK.so.2.10.1 index 0d768c8e..c54fc1cd 100644 Binary files a/orbbec_camera/SDK/arm64/lib/libOrbbecSDK.so.2.9.3 and b/orbbec_camera/SDK/arm64/lib/libOrbbecSDK.so.2.10.1 differ diff --git a/orbbec_camera/SDK/x64/include/libobsensor/h/Device.h b/orbbec_camera/SDK/x64/include/libobsensor/h/Device.h index bb1d7ebe..cde03ed3 100644 --- a/orbbec_camera/SDK/x64/include/libobsensor/h/Device.h +++ b/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) * diff --git a/orbbec_camera/SDK/x64/include/libobsensor/h/ObTypes.h b/orbbec_camera/SDK/x64/include/libobsensor/h/ObTypes.h index ecb87703..1a234540 100644 --- a/orbbec_camera/SDK/x64/include/libobsensor/h/ObTypes.h +++ b/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. diff --git a/orbbec_camera/SDK/x64/include/libobsensor/h/Property.h b/orbbec_camera/SDK/x64/include/libobsensor/h/Property.h index 43896ad2..486a54f0 100644 --- a/orbbec_camera/SDK/x64/include/libobsensor/h/Property.h +++ b/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 diff --git a/orbbec_camera/SDK/x64/include/libobsensor/hpp/Device.hpp b/orbbec_camera/SDK/x64/include/libobsensor/hpp/Device.hpp index 36cf9e41..37a420c2 100644 --- a/orbbec_camera/SDK/x64/include/libobsensor/hpp/Device.hpp +++ b/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 diff --git a/orbbec_camera/SDK/x64/lib/OrbbecSDKConfig-release.cmake b/orbbec_camera/SDK/x64/lib/OrbbecSDKConfig-release.cmake index ce05f521..231fafb1 100644 --- a/orbbec_camera/SDK/x64/lib/OrbbecSDKConfig-release.cmake +++ b/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) diff --git a/orbbec_camera/SDK/x64/lib/OrbbecSDKVersion.cmake b/orbbec_camera/SDK/x64/lib/OrbbecSDKVersion.cmake index cfbccf3a..7851ea03 100644 --- a/orbbec_camera/SDK/x64/lib/OrbbecSDKVersion.cmake +++ b/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) diff --git a/orbbec_camera/SDK/x64/lib/extensions/filters/libFilterProcessor.so b/orbbec_camera/SDK/x64/lib/extensions/filters/libFilterProcessor.so index 420ea084..cf1d5f96 100644 Binary files a/orbbec_camera/SDK/x64/lib/extensions/filters/libFilterProcessor.so and b/orbbec_camera/SDK/x64/lib/extensions/filters/libFilterProcessor.so differ diff --git a/orbbec_camera/SDK/x64/lib/extensions/filters/libob_priv_filter.so b/orbbec_camera/SDK/x64/lib/extensions/filters/libob_priv_filter.so index 0d5d64b0..0409698a 100644 Binary files a/orbbec_camera/SDK/x64/lib/extensions/filters/libob_priv_filter.so and b/orbbec_camera/SDK/x64/lib/extensions/filters/libob_priv_filter.so differ diff --git a/orbbec_camera/SDK/x64/lib/extensions/firmwareupdater/libfirmwareupdater.so b/orbbec_camera/SDK/x64/lib/extensions/firmwareupdater/libfirmwareupdater.so index 3c0354e5..14f73eb6 100644 Binary files a/orbbec_camera/SDK/x64/lib/extensions/firmwareupdater/libfirmwareupdater.so and b/orbbec_camera/SDK/x64/lib/extensions/firmwareupdater/libfirmwareupdater.so differ diff --git a/orbbec_camera/SDK/x64/lib/extensions/frameprocessor/libob_frame_processor.so b/orbbec_camera/SDK/x64/lib/extensions/frameprocessor/libob_frame_processor.so index 6cd07848..b3e5608e 100644 Binary files a/orbbec_camera/SDK/x64/lib/extensions/frameprocessor/libob_frame_processor.so and b/orbbec_camera/SDK/x64/lib/extensions/frameprocessor/libob_frame_processor.so differ diff --git a/orbbec_camera/SDK/x64/lib/libOrbbecSDK.so.2 b/orbbec_camera/SDK/x64/lib/libOrbbecSDK.so.2 index 07122dbb..027ba8ff 120000 --- a/orbbec_camera/SDK/x64/lib/libOrbbecSDK.so.2 +++ b/orbbec_camera/SDK/x64/lib/libOrbbecSDK.so.2 @@ -1 +1 @@ -libOrbbecSDK.so.2.9.3 \ No newline at end of file +libOrbbecSDK.so.2.10.1 \ No newline at end of file diff --git a/orbbec_camera/SDK/x64/lib/libOrbbecSDK.so.2.9.3 b/orbbec_camera/SDK/x64/lib/libOrbbecSDK.so.2.10.1 similarity index 68% rename from orbbec_camera/SDK/x64/lib/libOrbbecSDK.so.2.9.3 rename to orbbec_camera/SDK/x64/lib/libOrbbecSDK.so.2.10.1 index fc3deb08..4b9023d2 100644 Binary files a/orbbec_camera/SDK/x64/lib/libOrbbecSDK.so.2.9.3 and b/orbbec_camera/SDK/x64/lib/libOrbbecSDK.so.2.10.1 differ diff --git a/orbbec_camera/examples/README.MD b/orbbec_camera/examples/README.MD index 536d3d45..f598b6f7 100644 --- a/orbbec_camera/examples/README.MD +++ b/orbbec_camera/examples/README.MD @@ -13,5 +13,6 @@ For command-line maintenance and diagnostic utilities, see the official | :---: | --- | :---: | --- | | [Net camera](./net_camera) | Launch supported Orbbec network cameras. | ⭐️ | [Network camera guide](https://orbbec.github.io/OrbbecSDK_ROS2/en/source/camera_devices/5_advanced_guide/configuration/net_camera.html) | | [GMSL camera](./gmsl_camera) | Launch single, multiple, or synchronized GMSL cameras. | ⭐️ | [GMSL camera guide](https://orbbec.github.io/OrbbecSDK_ROS2/en/source/camera_devices/5_advanced_guide/multi_camera/gmsl_camera.html) | +| [AE/AWB lock test](./ae_awb_lock) | Verify AE/AWB convergence, frame-metadata capture, and manual lock-in through ROS 2. | ⭐️⭐️ | [README](./ae_awb_lock/README.md) | | [Benchmark](./benchmark) | Benchmark the performance of different camera configurations. | ⭐️⭐️ | [Performance benchmark tools](https://orbbec.github.io/OrbbecSDK_ROS2/en/source/camera_devices/6_benchmark/benchmark_tools.html) | | [Multi-camera synchronization verification](./multi_camera_synced_verification_tool) | Verify synchronization accuracy across multiple cameras. | ⭐️⭐️⭐️ | [Synchronization verification guide](https://orbbec.github.io/OrbbecSDK_ROS2/en/source/camera_devices/5_advanced_guide/multi_camera/multi_camera_synced_verification_tool.html) | diff --git a/orbbec_camera/examples/ae_awb_lock/README.md b/orbbec_camera/examples/ae_awb_lock/README.md new file mode 100644 index 00000000..2812a166 --- /dev/null +++ b/orbbec_camera/examples/ae_awb_lock/README.md @@ -0,0 +1,46 @@ +# AE/AWB Lock Test + +This sample exposes a test-only ROS 2 action that verifies the AE/AWB capture and manual +lock-in flow through the camera driver's services and color-frame metadata. + +Run the camera driver and this sample in the same namespace: + +```bash +ros2 run orbbec_camera ae_awb_lock_test_node --ros-args -r __ns:=/camera +``` + +Send a goal and print feedback: + +```bash +ros2 action send_goal \ + /camera/run_ae_awb_lock_test \ + orbbec_camera_msgs/action/RunAeAwbLockTest \ + "{timeout_ms: 10000}" \ + --feedback +``` + +The sample subscribes to the relative `color/metadata` topic. It enables auto exposure and auto +white balance, waits until the SDK status equals `1`, captures exposure, color gain, and color +temperature from the latest color-frame metadata, and reads AWB R/B/G gains through the structured +property service. It then disables the auto controls and writes the captured values back in this +order: + +1. Color exposure +2. Color gain +3. AWB R/B/G gains +4. Color temperature + +The final AWB gain readback must exactly match the captured value. Other readback differences are +reported as warnings because the device may quantize those controls. On failure or cancellation, +the sample restores auto exposure and auto white balance. + +Every feedback phase contains a fresh status value read from the camera service. The +`waiting_for_services` feedback is published after all required services become available, because +the status cannot be read before its service is ready. + +The color stream must be enabled, and `/camera/color/metadata` must be available when using the +`/camera` namespace. The action fails instead of writing default values if no metadata arrives +before the goal timeout or if `exposure`, `gain`, or `white_balance` is missing. The goal timeout +covers the main workflow, including service discovery, service calls, convergence, capture, +writeback, and verification. Restoring auto exposure and auto white balance after a failure uses a +separate best-effort timeout. diff --git a/orbbec_camera/examples/ae_awb_lock/ae_awb_lock_test_node.cpp b/orbbec_camera/examples/ae_awb_lock/ae_awb_lock_test_node.cpp new file mode 100644 index 00000000..01977f53 --- /dev/null +++ b/orbbec_camera/examples/ae_awb_lock/ae_awb_lock_test_node.cpp @@ -0,0 +1,419 @@ +// Copyright (c) 2026 Orbbec Inc. All Rights Reserved. +// Licensed under the Apache License, Version 2.0. + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "orbbec_camera_msgs/action/run_ae_awb_lock_test.hpp" +#include "orbbec_camera_msgs/msg/metadata.hpp" +#include "orbbec_camera_msgs/srv/get_awb_gain.hpp" +#include "orbbec_camera_msgs/srv/get_int32.hpp" +#include "orbbec_camera_msgs/srv/set_awb_gain.hpp" +#include "orbbec_camera_msgs/srv/set_int32.hpp" + +namespace { + +using namespace std::chrono_literals; + +constexpr int32_t kAeAwbConverged = 1; +constexpr uint32_t kDefaultTimeoutMs = 10000; +constexpr auto kPollInterval = 100ms; +constexpr auto kRestoreServiceTimeout = 5s; + +class CanceledError : public std::runtime_error { + public: + CanceledError() : std::runtime_error("goal canceled") {} +}; + +class AeAwbLockTestNode : public rclcpp::Node { + public: + using RunAeAwbLockTest = orbbec_camera_msgs::action::RunAeAwbLockTest; + using GoalHandle = rclcpp_action::ServerGoalHandle; + using GetInt32 = orbbec_camera_msgs::srv::GetInt32; + using SetInt32 = orbbec_camera_msgs::srv::SetInt32; + using GetAwbGain = orbbec_camera_msgs::srv::GetAwbGain; + using SetAwbGain = orbbec_camera_msgs::srv::SetAwbGain; + using SetBool = std_srvs::srv::SetBool; + using Metadata = orbbec_camera_msgs::msg::Metadata; + using Deadline = std::chrono::steady_clock::time_point; + + AeAwbLockTestNode() : Node("ae_awb_lock_test_node") { + color_metadata_subscription_ = create_subscription( + "color/metadata", rclcpp::SensorDataQoS(), + std::bind(&AeAwbLockTestNode::colorMetadataCallback, this, std::placeholders::_1)); + + get_status_client_ = create_client("get_color_ae_awb_status"); + get_awb_gain_client_ = create_client("get_color_awb_gain"); + set_awb_gain_client_ = create_client("set_color_awb_gain"); + get_exposure_client_ = create_client("get_color_exposure"); + set_exposure_client_ = create_client("set_color_exposure"); + get_color_gain_client_ = create_client("get_color_gain"); + set_color_gain_client_ = create_client("set_color_gain"); + get_white_balance_client_ = create_client("get_white_balance"); + set_white_balance_client_ = create_client("set_white_balance"); + set_auto_exposure_client_ = create_client("set_color_auto_exposure"); + set_auto_white_balance_client_ = create_client("set_auto_white_balance"); + + action_server_ = rclcpp_action::create_server( + this, "run_ae_awb_lock_test", + std::bind(&AeAwbLockTestNode::handleGoal, this, std::placeholders::_1, + std::placeholders::_2), + std::bind(&AeAwbLockTestNode::handleCancel, this, std::placeholders::_1), + std::bind(&AeAwbLockTestNode::handleAccepted, this, std::placeholders::_1)); + } + + ~AeAwbLockTestNode() override { + shutting_down_.store(true); + metadata_cv_.notify_all(); + std::lock_guard lock(worker_mutex_); + if (worker_.joinable()) { + worker_.join(); + } + } + + private: + struct ColorFrameMetadata { + int32_t exposure; + int32_t gain; + int32_t white_balance; + }; + + struct ActiveGoalGuard { + explicit ActiveGoalGuard(std::atomic_bool& active) : active_(active) {} + ~ActiveGoalGuard() { active_.store(false); } + std::atomic_bool& active_; + }; + + rclcpp_action::GoalResponse handleGoal(const rclcpp_action::GoalUUID& uuid, + std::shared_ptr goal) { + (void)uuid; + (void)goal; + bool expected = false; + if (!goal_active_.compare_exchange_strong(expected, true)) { + RCLCPP_WARN(get_logger(), "Rejecting AE/AWB test goal: another goal is active"); + return rclcpp_action::GoalResponse::REJECT; + } + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + } + + rclcpp_action::CancelResponse handleCancel(const std::shared_ptr goal_handle) { + (void)goal_handle; + return rclcpp_action::CancelResponse::ACCEPT; + } + + void handleAccepted(const std::shared_ptr goal_handle) { + std::lock_guard lock(worker_mutex_); + if (worker_.joinable()) { + worker_.join(); + } + worker_ = std::thread(&AeAwbLockTestNode::execute, this, goal_handle); + } + + template + void waitForService(const typename rclcpp::Client::SharedPtr& client, + const std::string& service_name, const Deadline& deadline) { + const auto now = std::chrono::steady_clock::now(); + if (now >= deadline || !client->wait_for_service(deadline - now)) { + throw std::runtime_error(service_name + " is not available"); + } + } + + template + typename ServiceT::Response::SharedPtr callService( + const typename rclcpp::Client::SharedPtr& client, + const typename ServiceT::Request::SharedPtr& request, const std::string& service_name, + const Deadline& deadline) { + auto pending_request = client->async_send_request(request); + if (pending_request.wait_until(deadline) != std::future_status::ready) { + client->remove_pending_request(pending_request); + throw std::runtime_error(service_name + " timed out"); + } + auto response = pending_request.get(); + if (!response->success) { + throw std::runtime_error(service_name + " failed: " + response->message); + } + return response; + } + + void waitForRequiredServices(const Deadline& deadline) { + waitForService(get_status_client_, "get_color_ae_awb_status", deadline); + waitForService(get_awb_gain_client_, "get_color_awb_gain", deadline); + waitForService(set_awb_gain_client_, "set_color_awb_gain", deadline); + waitForService(get_exposure_client_, "get_color_exposure", deadline); + waitForService(set_exposure_client_, "set_color_exposure", deadline); + waitForService(get_color_gain_client_, "get_color_gain", deadline); + waitForService(set_color_gain_client_, "set_color_gain", deadline); + waitForService(get_white_balance_client_, "get_white_balance", deadline); + waitForService(set_white_balance_client_, "set_white_balance", deadline); + waitForService(set_auto_exposure_client_, "set_color_auto_exposure", deadline); + waitForService(set_auto_white_balance_client_, "set_auto_white_balance", deadline); + } + + int32_t getIntValue(const rclcpp::Client::SharedPtr& client, + const std::string& service_name, const Deadline& deadline) { + return callService(client, std::make_shared(), service_name, + deadline) + ->data; + } + + int32_t getAeAwbStatus(const Deadline& deadline) { + return getIntValue(get_status_client_, "get_color_ae_awb_status", deadline); + } + + void colorMetadataCallback(const Metadata::ConstSharedPtr& metadata) { + { + std::lock_guard lock(metadata_mutex_); + latest_color_metadata_ = metadata; + } + metadata_cv_.notify_all(); + } + + ColorFrameMetadata getLatestColorMetadata(const Deadline& deadline) { + Metadata::ConstSharedPtr metadata; + { + std::unique_lock lock(metadata_mutex_); + if (!metadata_cv_.wait_until(lock, deadline, [this] { + return latest_color_metadata_ != nullptr || shutting_down_.load(); + })) { + throw std::runtime_error("timed out waiting for color frame metadata"); + } + if (shutting_down_.load()) { + throw CanceledError(); + } + metadata = latest_color_metadata_; + } + + try { + const auto json = nlohmann::json::parse(metadata->json_data); + return {json.at("exposure").get(), json.at("gain").get(), + json.at("white_balance").get()}; + } catch (const nlohmann::json::exception& e) { + throw std::runtime_error(std::string("invalid color frame metadata: ") + e.what()); + } + } + + void setIntValue(const rclcpp::Client::SharedPtr& client, + const std::string& service_name, int32_t value, const Deadline& deadline) { + auto request = std::make_shared(); + request->data = value; + callService(client, request, service_name, deadline); + } + + GetAwbGain::Response::SharedPtr getAwbGain(const Deadline& deadline) { + return callService(get_awb_gain_client_, std::make_shared(), + "get_color_awb_gain", deadline); + } + + void setAwbGain(uint16_t r_gain, uint16_t b_gain, uint16_t g_gain, const Deadline& deadline) { + auto request = std::make_shared(); + request->r_gain = r_gain; + request->b_gain = b_gain; + request->g_gain = g_gain; + callService(set_awb_gain_client_, request, "set_color_awb_gain", deadline); + } + + void setBoolValue(const rclcpp::Client::SharedPtr& client, + const std::string& service_name, bool value, const Deadline& deadline) { + auto request = std::make_shared(); + request->data = value; + callService(client, request, service_name, deadline); + } + + void setAutoMode(bool enabled, const Deadline& deadline) { + setBoolValue(set_auto_exposure_client_, "set_color_auto_exposure", enabled, deadline); + setBoolValue(set_auto_white_balance_client_, "set_auto_white_balance", enabled, deadline); + } + + void restoreAutoMode() noexcept { + try { + const auto deadline = std::chrono::steady_clock::now() + kRestoreServiceTimeout; + setBoolValue(set_auto_exposure_client_, "set_color_auto_exposure", true, deadline); + } catch (const std::exception& e) { + RCLCPP_ERROR(get_logger(), "Failed to restore auto exposure: %s", e.what()); + } + try { + const auto deadline = std::chrono::steady_clock::now() + kRestoreServiceTimeout; + setBoolValue(set_auto_white_balance_client_, "set_auto_white_balance", true, deadline); + } catch (const std::exception& e) { + RCLCPP_ERROR(get_logger(), "Failed to restore auto white balance: %s", e.what()); + } + } + + void throwIfCanceled(const std::shared_ptr& goal_handle) const { + if (shutting_down_.load() || goal_handle->is_canceling()) { + throw CanceledError(); + } + } + + void publishFeedback(const std::shared_ptr& goal_handle, const std::string& phase, + int32_t status) { + auto feedback = std::make_shared(); + feedback->phase = phase; + feedback->ae_awb_status = status; + goal_handle->publish_feedback(feedback); + } + + void warnIfChanged(const char* name, int64_t captured, int64_t actual) { + if (captured != actual) { + RCLCPP_WARN(get_logger(), "%s changed from %lld to %lld after writeback", name, + static_cast(captured), static_cast(actual)); + } + } + + void execute(const std::shared_ptr goal_handle) { + ActiveGoalGuard active_guard(goal_active_); + auto result = std::make_shared(); + bool workflow_started = false; + const uint32_t requested_timeout = goal_handle->get_goal()->timeout_ms; + const uint32_t timeout_ms = requested_timeout == 0 ? kDefaultTimeoutMs : requested_timeout; + const auto deadline = std::chrono::steady_clock::now() + std::chrono::milliseconds(timeout_ms); + + try { + waitForRequiredServices(deadline); + publishFeedback(goal_handle, "waiting_for_services", getAeAwbStatus(deadline)); + throwIfCanceled(goal_handle); + + workflow_started = true; + setAutoMode(true, deadline); + publishFeedback(goal_handle, "enabling_auto", getAeAwbStatus(deadline)); + + int32_t status = 0; + do { + throwIfCanceled(goal_handle); + if (std::chrono::steady_clock::now() >= deadline) { + throw std::runtime_error("timed out waiting for AE/AWB convergence"); + } + status = getAeAwbStatus(deadline); + publishFeedback(goal_handle, "waiting_for_convergence", status); + if (status != kAeAwbConverged) { + std::this_thread::sleep_for(kPollInterval); + } + } while (status != kAeAwbConverged); + + throwIfCanceled(goal_handle); + publishFeedback(goal_handle, "capturing_parameters", getAeAwbStatus(deadline)); + const auto captured_metadata = getLatestColorMetadata(deadline); + result->captured_exposure = captured_metadata.exposure; + result->captured_color_gain = captured_metadata.gain; + result->captured_color_temperature = captured_metadata.white_balance; + auto captured_awb_gain = getAwbGain(deadline); + result->captured_awb_r_gain = captured_awb_gain->r_gain; + result->captured_awb_b_gain = captured_awb_gain->b_gain; + result->captured_awb_g_gain = captured_awb_gain->g_gain; + + throwIfCanceled(goal_handle); + publishFeedback(goal_handle, "disabling_auto", getAeAwbStatus(deadline)); + setAutoMode(false, deadline); + throwIfCanceled(goal_handle); + + publishFeedback(goal_handle, "applying_exposure", getAeAwbStatus(deadline)); + setIntValue(set_exposure_client_, "set_color_exposure", result->captured_exposure, deadline); + throwIfCanceled(goal_handle); + + publishFeedback(goal_handle, "applying_color_gain", getAeAwbStatus(deadline)); + setIntValue(set_color_gain_client_, "set_color_gain", result->captured_color_gain, deadline); + throwIfCanceled(goal_handle); + + publishFeedback(goal_handle, "applying_awb_gain", getAeAwbStatus(deadline)); + setAwbGain(result->captured_awb_r_gain, result->captured_awb_b_gain, + result->captured_awb_g_gain, deadline); + throwIfCanceled(goal_handle); + + publishFeedback(goal_handle, "applying_color_temperature", getAeAwbStatus(deadline)); + setIntValue(set_white_balance_client_, "set_white_balance", + result->captured_color_temperature, deadline); + throwIfCanceled(goal_handle); + + publishFeedback(goal_handle, "verifying", getAeAwbStatus(deadline)); + result->actual_exposure = getIntValue(get_exposure_client_, "get_color_exposure", deadline); + result->actual_color_gain = getIntValue(get_color_gain_client_, "get_color_gain", deadline); + auto actual_awb_gain = getAwbGain(deadline); + result->actual_awb_r_gain = actual_awb_gain->r_gain; + result->actual_awb_b_gain = actual_awb_gain->b_gain; + result->actual_awb_g_gain = actual_awb_gain->g_gain; + result->actual_color_temperature = + getIntValue(get_white_balance_client_, "get_white_balance", deadline); + throwIfCanceled(goal_handle); + + if (result->captured_awb_r_gain != result->actual_awb_r_gain || + result->captured_awb_b_gain != result->actual_awb_b_gain || + result->captured_awb_g_gain != result->actual_awb_g_gain) { + throw std::runtime_error("AWB gain readback does not match the captured value"); + } + + warnIfChanged("Exposure", result->captured_exposure, result->actual_exposure); + warnIfChanged("Color gain", result->captured_color_gain, result->actual_color_gain); + warnIfChanged("Color temperature", result->captured_color_temperature, + result->actual_color_temperature); + + result->success = true; + result->message = "AE/AWB capture and lock-in completed"; + publishFeedback(goal_handle, "completed", getAeAwbStatus(deadline)); + goal_handle->succeed(result); + } catch (const CanceledError&) { + if (workflow_started) { + restoreAutoMode(); + } + result->success = false; + result->message = "AE/AWB test canceled"; + goal_handle->canceled(result); + } catch (const std::exception& e) { + if (workflow_started) { + restoreAutoMode(); + } + result->success = false; + result->message = e.what(); + RCLCPP_ERROR(get_logger(), "AE/AWB test failed: %s", e.what()); + goal_handle->abort(result); + } + } + + rclcpp_action::Server::SharedPtr action_server_; + rclcpp::Subscription::SharedPtr color_metadata_subscription_; + + rclcpp::Client::SharedPtr get_status_client_; + rclcpp::Client::SharedPtr get_awb_gain_client_; + rclcpp::Client::SharedPtr set_awb_gain_client_; + rclcpp::Client::SharedPtr get_exposure_client_; + rclcpp::Client::SharedPtr set_exposure_client_; + rclcpp::Client::SharedPtr get_color_gain_client_; + rclcpp::Client::SharedPtr set_color_gain_client_; + rclcpp::Client::SharedPtr get_white_balance_client_; + rclcpp::Client::SharedPtr set_white_balance_client_; + rclcpp::Client::SharedPtr set_auto_exposure_client_; + rclcpp::Client::SharedPtr set_auto_white_balance_client_; + + std::atomic_bool goal_active_{false}; + std::atomic_bool shutting_down_{false}; + Metadata::ConstSharedPtr latest_color_metadata_; + std::mutex metadata_mutex_; + std::condition_variable metadata_cv_; + std::mutex worker_mutex_; + std::thread worker_; +}; + +} // namespace + +int main(int argc, char** argv) { + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(node); + executor.spin(); + rclcpp::shutdown(); + return 0; +} diff --git a/orbbec_camera/examples/benchmark/gemini_330_series_benchmark.launch.py b/orbbec_camera/examples/benchmark/gemini_330_series_benchmark.launch.py index 73c2e8ce..906b31cd 100644 --- a/orbbec_camera/examples/benchmark/gemini_330_series_benchmark.launch.py +++ b/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'), diff --git a/orbbec_camera/examples/gmsl_camera/gemini_330_gmsl.launch.py b/orbbec_camera/examples/gmsl_camera/gemini_330_gmsl.launch.py index 73c2e8ce..906b31cd 100644 --- a/orbbec_camera/examples/gmsl_camera/gemini_330_gmsl.launch.py +++ b/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'), diff --git a/orbbec_camera/examples/multi_camera_synced_verification_tool/gemini_330_series_synced_verify.launch.py b/orbbec_camera/examples/multi_camera_synced_verification_tool/gemini_330_series_synced_verify.launch.py index 73c2e8ce..906b31cd 100644 --- a/orbbec_camera/examples/multi_camera_synced_verification_tool/gemini_330_series_synced_verify.launch.py +++ b/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'), diff --git a/orbbec_camera/examples/multi_camera_synced_verification_tool/gemini_synced_verify.launch.py b/orbbec_camera/examples/multi_camera_synced_verification_tool/gemini_synced_verify.launch.py index bc210a2c..fc63be43 100644 --- a/orbbec_camera/examples/multi_camera_synced_verification_tool/gemini_synced_verify.launch.py +++ b/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'), diff --git a/orbbec_camera/include/orbbec_camera/constants.h b/orbbec_camera/include/orbbec_camera/constants.h index d52c1faf..0872e8ff 100644 --- a/orbbec_camera/include/orbbec_camera/constants.h +++ b/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_PATCH_VERSION 3 +#define OB_ROS_MINOR_VERSION 10 +#define OB_ROS_PATCH_VERSION 1 #ifndef STRINGIFY #define STRINGIFY(arg) #arg diff --git a/orbbec_camera/include/orbbec_camera/frame_timestamp_csv_logger.h b/orbbec_camera/include/orbbec_camera/frame_timestamp_csv_logger.h index 3ce38971..e9033a20 100644 --- a/orbbec_camera/include/orbbec_camera/frame_timestamp_csv_logger.h +++ b/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 &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; diff --git a/orbbec_camera/include/orbbec_camera/imu_timestamp_csv_logger.h b/orbbec_camera/include/orbbec_camera/imu_timestamp_csv_logger.h new file mode 100644 index 00000000..ded1c003 --- /dev/null +++ b/orbbec_camera/include/orbbec_camera/imu_timestamp_csv_logger.h @@ -0,0 +1,99 @@ +#pragma once + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#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 &accel_frame, + const std::shared_ptr &gyro_frame, int64_t arrival_system_us, + std::optional publish_system_us); + + void recordStandaloneFrame(OBStreamType stream_type, const std::shared_ptr &frame, + int64_t arrival_system_us, std::optional 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 publish_system_us; + }; + + struct PendingRow { + uint64_t row_id = 0; + StreamState accel; + StreamState gyro; + }; + + void recordFrames(const std::shared_ptr &accel_frame, + const std::shared_ptr &gyro_frame, int64_t arrival_system_us, + std::optional publish_system_us); + static void populateStreamState(StreamState &state, const std::shared_ptr &frame, + int64_t arrival_system_us, + std::optional 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 &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 completed_rows_; + + std::mutex state_mutex_; + std::mutex completed_rows_mutex_; + std::condition_variable completed_rows_cv_; +}; + +} // namespace orbbec_camera diff --git a/orbbec_camera/include/orbbec_camera/ob_camera_node.h b/orbbec_camera/include/orbbec_camera/ob_camera_node.h index 6f6fa908..30427d15 100644 --- a/orbbec_camera/include/orbbec_camera/ob_camera_node.h +++ b/orbbec_camera/include/orbbec_camera/ob_camera_node.h @@ -54,12 +54,14 @@ #include "orbbec_camera_msgs/msg/depth_filters_status.hpp" #include "orbbec_camera_msgs/srv/get_device_config.hpp" #include "orbbec_camera_msgs/srv/get_device_info.hpp" +#include "orbbec_camera_msgs/srv/get_awb_gain.hpp" #include "orbbec_camera_msgs/msg/extrinsics.hpp" #include "orbbec_camera_msgs/msg/metadata.hpp" #include "orbbec_camera_msgs/msg/imu_info.hpp" #include "orbbec_camera_msgs/srv/get_int32.hpp" #include "orbbec_camera_msgs/srv/get_string.hpp" #include "orbbec_camera_msgs/srv/set_int32.hpp" +#include "orbbec_camera_msgs/srv/set_awb_gain.hpp" #include "orbbec_camera_msgs/srv/get_bool.hpp" #include "orbbec_camera_msgs/srv/set_string.hpp" #include "orbbec_camera_msgs/srv/set_filter.hpp" @@ -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 #include @@ -137,6 +139,8 @@ using GetDeviceInfo = orbbec_camera_msgs::srv::GetDeviceInfo; using Extrinsics = orbbec_camera_msgs::msg::Extrinsics; using SetInt32 = orbbec_camera_msgs::srv::SetInt32; using GetInt32 = orbbec_camera_msgs::srv::GetInt32; +using GetAwbGain = orbbec_camera_msgs::srv::GetAwbGain; +using SetAwbGain = orbbec_camera_msgs::srv::SetAwbGain; using GetString = orbbec_camera_msgs::srv::GetString; using SetString = orbbec_camera_msgs::srv::SetString; using SetBool = std_srvs::srv::SetBool; @@ -435,6 +439,15 @@ class OBCameraNode { void setAutoWhiteBalanceCallback(const std::shared_ptr& request, std::shared_ptr& response); + void getAeAwbStatusCallback(const std::shared_ptr& request, + std::shared_ptr& response); + + void getAwbGainCallback(const std::shared_ptr& request, + std::shared_ptr& response); + + void setAwbGainCallback(const std::shared_ptr& request, + std::shared_ptr& response); + void setAutoExposureCallback(const std::shared_ptr& request, std::shared_ptr& response, const stream_index_pair& stream_index); @@ -613,7 +626,8 @@ class OBCameraNode { const std::shared_ptr& frame); void onNewIMUFrameSyncOutputCallback(const std::shared_ptr& accelframe, - const std::shared_ptr& gryoframe); + const std::shared_ptr& gryoframe, + int64_t arrival_system_us); void onNewIMUFrameCallback(const std::shared_ptr& frame, const stream_index_pair& stream_index); @@ -750,6 +764,9 @@ class OBCameraNode { rclcpp::Service::SharedPtr set_white_balance_srv_; rclcpp::Service::SharedPtr get_auto_white_balance_srv_; rclcpp::Service::SharedPtr set_auto_white_balance_srv_; + rclcpp::Service::SharedPtr get_ae_awb_status_srv_; + rclcpp::Service::SharedPtr get_awb_gain_srv_; + rclcpp::Service::SharedPtr set_awb_gain_srv_; rclcpp::Service::SharedPtr get_sdk_version_srv_; rclcpp::Service::SharedPtr switch_ir_camera_srv_; rclcpp::Service::SharedPtr export_config_json_srv_; @@ -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 frame_timestamp_csv_logger_; + std::unique_ptr 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 enable_undistortion_; std::shared_ptr hw_d2c_color_undistortion_filter_; diff --git a/orbbec_camera/include/orbbec_camera/timestamp_csv_logger.h b/orbbec_camera/include/orbbec_camera/timestamp_csv_logger.h new file mode 100644 index 00000000..8c3029de --- /dev/null +++ b/orbbec_camera/include/orbbec_camera/timestamp_csv_logger.h @@ -0,0 +1,73 @@ +#pragma once + +#include + +#include +#include +#include +#include +#include + +#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 &color_frame, + const std::shared_ptr &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 &frame, + int64_t publish_system_us, int64_t publish_steady_us); + void recordImagePublishSkipped(OBStreamType stream_type, const std::shared_ptr &frame); + + void recordSyncedImu(const std::shared_ptr &accel_frame, + const std::shared_ptr &gyro_frame, int64_t arrival_system_us, + std::optional publish_system_us); + void recordStandaloneImu(OBStreamType stream_type, const std::shared_ptr &frame, + int64_t arrival_system_us, std::optional 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 synced_image_logger_; + std::unique_ptr color_logger_; + std::unique_ptr depth_logger_; + std::unique_ptr synced_imu_logger_; + std::unique_ptr accel_logger_; + std::unique_ptr gyro_logger_; +}; + +} // namespace orbbec_camera diff --git a/orbbec_camera/launch/astra.launch.py b/orbbec_camera/launch/astra.launch.py index 2f3fa9c9..b6287a27 100644 --- a/orbbec_camera/launch/astra.launch.py +++ b/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'), diff --git a/orbbec_camera/launch/astra2.launch.py b/orbbec_camera/launch/astra2.launch.py index 2cbe3331..8a27aa13 100644 --- a/orbbec_camera/launch/astra2.launch.py +++ b/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'), diff --git a/orbbec_camera/launch/dabai_a.launch.py b/orbbec_camera/launch/dabai_a.launch.py index 576137f7..c105ddd5 100644 --- a/orbbec_camera/launch/dabai_a.launch.py +++ b/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"]'), diff --git a/orbbec_camera/launch/dabai_al.launch.py b/orbbec_camera/launch/dabai_al.launch.py index d839bbf5..6dc42693 100644 --- a/orbbec_camera/launch/dabai_al.launch.py +++ b/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"]'), diff --git a/orbbec_camera/launch/dabai_dcw2.launch.py b/orbbec_camera/launch/dabai_dcw2.launch.py index 15d2af68..79ee5eaa 100644 --- a/orbbec_camera/launch/dabai_dcw2.launch.py +++ b/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=''), ] diff --git a/orbbec_camera/launch/dabai_max_pro.launch.py b/orbbec_camera/launch/dabai_max_pro.launch.py index 212ca930..b97a1865 100644 --- a/orbbec_camera/launch/dabai_max_pro.launch.py +++ b/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 diff --git a/orbbec_camera/launch/femto.launch.py b/orbbec_camera/launch/femto.launch.py index beeafdac..391530bc 100644 --- a/orbbec_camera/launch/femto.launch.py +++ b/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'), diff --git a/orbbec_camera/launch/femto_bolt.launch.py b/orbbec_camera/launch/femto_bolt.launch.py index c8dcdc9f..6e4b0203 100644 --- a/orbbec_camera/launch/femto_bolt.launch.py +++ b/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'), diff --git a/orbbec_camera/launch/femto_mega.launch.py b/orbbec_camera/launch/femto_mega.launch.py index 5331ae6e..ffd878f3 100644 --- a/orbbec_camera/launch/femto_mega.launch.py +++ b/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'), diff --git a/orbbec_camera/launch/gemini2.launch.py b/orbbec_camera/launch/gemini2.launch.py index 1e35bfc3..e497e113 100644 --- a/orbbec_camera/launch/gemini2.launch.py +++ b/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'), diff --git a/orbbec_camera/launch/gemini210.launch.py b/orbbec_camera/launch/gemini210.launch.py index 8143ba88..3d9ccd37 100644 --- a/orbbec_camera/launch/gemini210.launch.py +++ b/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'), diff --git a/orbbec_camera/launch/gemini2L.launch.py b/orbbec_camera/launch/gemini2L.launch.py index 26056a6b..ef422db4 100644 --- a/orbbec_camera/launch/gemini2L.launch.py +++ b/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"), diff --git a/orbbec_camera/launch/gemini345.launch.py b/orbbec_camera/launch/gemini345.launch.py index 31337156..197fd52b 100644 --- a/orbbec_camera/launch/gemini345.launch.py +++ b/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"]'), diff --git a/orbbec_camera/launch/gemini345_lg.launch.py b/orbbec_camera/launch/gemini345_lg.launch.py index 56a08aa1..cf22d547 100644 --- a/orbbec_camera/launch/gemini345_lg.launch.py +++ b/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'), diff --git a/orbbec_camera/launch/gemini435_le.launch.py b/orbbec_camera/launch/gemini435_le.launch.py index 72efb2b0..42b757c5 100644 --- a/orbbec_camera/launch/gemini435_le.launch.py +++ b/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'), diff --git a/orbbec_camera/launch/gemini_301_series.launch.py b/orbbec_camera/launch/gemini_301_series.launch.py index 06e9b248..d78fc15a 100644 --- a/orbbec_camera/launch/gemini_301_series.launch.py +++ b/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'), diff --git a/orbbec_camera/launch/gemini_330_series.launch.py b/orbbec_camera/launch/gemini_330_series.launch.py index 65b894eb..2bff99ba 100644 --- a/orbbec_camera/launch/gemini_330_series.launch.py +++ b/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'), diff --git a/orbbec_camera/launch/gemini_330_series_low_cpu.launch.py b/orbbec_camera/launch/gemini_330_series_low_cpu.launch.py index 8f071da2..f6480f17 100644 --- a/orbbec_camera/launch/gemini_330_series_low_cpu.launch.py +++ b/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'), - DeclareLaunchArgument('enable_noise_removal_filter', default_value='false'), + # Empty means that the node will not change the device's current setting. + 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'), diff --git a/orbbec_camera/launch/gemini_330_series_sdk_json.launch.py b/orbbec_camera/launch/gemini_330_series_sdk_json.launch.py index 4b18c2ba..435827f2 100644 --- a/orbbec_camera/launch/gemini_330_series_sdk_json.launch.py +++ b/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=''), diff --git a/orbbec_camera/package.xml b/orbbec_camera/package.xml index 790cf788..90cce657 100644 --- a/orbbec_camera/package.xml +++ b/orbbec_camera/package.xml @@ -2,7 +2,7 @@ orbbec_camera - 2.9.3 + 2.10.1 Orbbec Camera package yalian Apache-2.0 @@ -20,9 +20,10 @@ rclcpp_components cv_bridge camera_info_manager - orbbec_camera_msgs + orbbec_camera_msgs builtin_interfaces rclcpp + rclcpp_action sensor_msgs std_msgs std_srvs diff --git a/orbbec_camera/src/frame_timestamp_csv_logger.cpp b/orbbec_camera/src/frame_timestamp_csv_logger.cpp index 2b34460d..9100dcc1 100644 --- a/orbbec_camera/src/frame_timestamp_csv_logger.cpp +++ b/orbbec_camera/src/frame_timestamp_csv_logger.cpp @@ -35,25 +35,26 @@ int64_t getExpectedIntervalUs(const std::shared_ptr &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 &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) { - return csv_file_path_; + const std::filesystem::path original_path(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_); - const auto indexed_filename = original_path.stem().string() + "_" + std::to_string(file_index) + - original_path.extension().string(); + 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(); } diff --git a/orbbec_camera/src/imu_timestamp_csv_logger.cpp b/orbbec_camera/src/imu_timestamp_csv_logger.cpp new file mode 100644 index 00000000..9a486053 --- /dev/null +++ b/orbbec_camera/src/imu_timestamp_csv_logger.cpp @@ -0,0 +1,354 @@ +#include "orbbec_camera/imu_timestamp_csv_logger.h" + +#include +#include +#include +#include +#include +#include +#include + +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 &accel_frame, + const std::shared_ptr &gyro_frame, + int64_t arrival_system_us, + std::optional 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 &frame, + int64_t arrival_system_us, + std::optional 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 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 &accel_frame, + const std::shared_ptr &gyro_frame, + int64_t arrival_system_us, + std::optional publish_system_us) { + std::lock_guard 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 &frame, + int64_t arrival_system_us, + std::optional publish_system_us) { + state.has_frame = true; + state.device_ts_us = static_cast(frame->getTimeStampUs()); + state.global_ts_us = static_cast(frame->getGlobalTimeStampUs()); + state.sdk_system_ts_us = static_cast(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 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 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(time_us) / 1000000.0L); + return ss.str(); +} + +std::string ImuTimestampCsvLogger::formatOptionalSecondsColumn( + const std::optional &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 rows_to_write; + { + std::unique_lock 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 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 diff --git a/orbbec_camera/src/ob_camera_node.cpp b/orbbec_camera/src/ob_camera_node.cpp index 69670685..9d26eb53 100644 --- a/orbbec_camera/src/ob_camera_node.cpp +++ b/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 devic setupTopics(); if (enable_frame_drop_log_ || !frame_timestamp_csv_file_.empty()) { - frame_timestamp_csv_logger_ = std::make_unique( - enable_frame_drop_log_, frame_timestamp_csv_file_, logger_); - if (!frame_timestamp_csv_logger_->enabled()) { - frame_timestamp_csv_logger_.reset(); + TimestampCsvLogger::Config timestamp_config; + timestamp_config.frame_drop_log_enabled = enable_frame_drop_log_; + timestamp_config.csv_file_path = frame_timestamp_csv_file_; + 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(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(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(); 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(max_depth_limit_, "max_depth_limit", 0); setAndGetNodeParameter(enable_heartbeat_, "enable_heartbeat", false); setAndGetNodeParameter(enable_firmware_log_, "enable_firmware_log", false); + setAndGetNodeParameter(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(enable_fps_boost_, "enable_fps_boost", false); setAndGetNodeParameter(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 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 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 &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 &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() && - (stream_index == COLOR || stream_index == DEPTH)) { - frame_timestamp_csv_logger_->recordImagePublishSkipped(stream_index.first, frame); + if (log_image_timestamps) { + 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 &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_ && - frame_timestamp_csv_logger_->enabled()) { - frame_timestamp_csv_logger_->recordPreImagePublish(stream_index.first, frame, - getSystemNowUs(), getSteadyNowUs()); + if (!has_raw_image_subscriber && stream_index == COLOR && log_image_timestamps) { + timestamp_csv_logger_->recordImagePrePublish(stream_index.first, frame, 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 &frame, record_image_publish_skipped(); return; } - if (frame_timestamp_csv_logger_ && frame_timestamp_csv_logger_->enabled() && - (stream_index == COLOR || stream_index == DEPTH)) { - frame_timestamp_csv_logger_->recordPreImagePublish(stream_index.first, frame, - getSystemNowUs(), getSteadyNowUs()); + if (log_image_timestamps) { + timestamp_csv_logger_->recordImagePrePublish(stream_index.first, frame, 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 &accelframe, - const std::shared_ptr &gryoframe) { + const std::shared_ptr &gryoframe, + int64_t arrival_system_us) { if (!is_camera_node_initialized_.load() || !rclcpp::ok()) { return; } + const auto record_timestamps = [&](std::optional 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_ptrget_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_ptrpublish(imu_msg); + record_timestamps(publish_system_us); } void OBCameraNode::onNewIMUFrameCallback(const std::shared_ptr &frame, @@ -7301,7 +7345,17 @@ void OBCameraNode::onNewIMUFrameCallback(const std::shared_ptr &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 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 &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 &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 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: " - << "\ndiff_scale:" << request->filter_param[0] - << "\nweight:" << request->filter_param[1]); + RCLCPP_INFO_STREAM( + logger_, "Set TemporalFilter params: " << "\ndiff_scale:" << request->filter_param[0] + << "\nweight:" << request->filter_param[1]); temporal_filter_diff_threshold_ = request->filter_param[0]; temporal_filter_weight_ = request->filter_param[1]; } else { diff --git a/orbbec_camera/src/ros_service.cpp b/orbbec_camera/src/ros_service.cpp index a263ce1c..4fd14f55 100644 --- a/orbbec_camera/src/ros_service.cpp +++ b/orbbec_camera/src/ros_service.cpp @@ -303,6 +303,27 @@ void OBCameraNode::setupCameraCtrlServices() { std::shared_ptr response) { setAutoWhiteBalanceCallback(request, response); }); + if (isPropertyReadable(device_, OB_PROP_COLOR_AE_AWB_STAT_INT)) { + get_ae_awb_status_srv_ = node_->create_service( + "get_color_ae_awb_status", [this](const std::shared_ptr request, + std::shared_ptr response) { + getAeAwbStatusCallback(request, response); + }); + } + if (isPropertyReadable(device_, OB_STRUCT_COLOR_AWB_GAIN)) { + get_awb_gain_srv_ = node_->create_service( + "get_color_awb_gain", [this](const std::shared_ptr request, + std::shared_ptr response) { + getAwbGainCallback(request, response); + }); + } + if (isPropertyWritable(device_, OB_STRUCT_COLOR_AWB_GAIN)) { + set_awb_gain_srv_ = node_->create_service( + "set_color_awb_gain", [this](const std::shared_ptr request, + std::shared_ptr response) { + setAwbGainCallback(request, response); + }); + } get_device_srv_ = node_->create_service( "get_device_info", [this](const std::shared_ptr request, std::shared_ptr response) { @@ -1266,6 +1287,75 @@ void OBCameraNode::setAutoWhiteBalanceCallback(const std::shared_ptr& request, + std::shared_ptr& response) { + (void)request; + try { + response->data = device_->getIntProperty(OB_PROP_COLOR_AE_AWB_STAT_INT); + response->success = true; + } catch (const ob::Error& e) { + response->success = false; + response->message = orbbec_camera::formatObErrorWithStatus(e); + } catch (const std::exception& e) { + response->success = false; + response->message = e.what(); + } catch (...) { + response->success = false; + response->message = "unknown error"; + } +} + +void OBCameraNode::getAwbGainCallback(const std::shared_ptr& request, + std::shared_ptr& response) { + (void)request; + try { + OBAwbGainParams gain{}; + uint32_t size = sizeof(gain); + device_->getStructuredData(OB_STRUCT_COLOR_AWB_GAIN, reinterpret_cast(&gain), &size); + response->r_gain = gain.rGain; + response->b_gain = gain.bGain; + response->g_gain = gain.gGain; + response->success = true; + } catch (const ob::Error& e) { + response->success = false; + response->message = orbbec_camera::formatObErrorWithStatus(e); + } catch (const std::exception& e) { + response->success = false; + response->message = e.what(); + } catch (...) { + response->success = false; + response->message = "unknown error"; + } +} + +void OBCameraNode::setAwbGainCallback(const std::shared_ptr& request, + std::shared_ptr& response) { + try { + if (device_->getBoolProperty(OB_PROP_COLOR_AUTO_WHITE_BALANCE_BOOL)) { + response->success = false; + response->message = "auto white balance is enabled"; + return; + } + + OBAwbGainParams gain{}; + gain.rGain = request->r_gain; + gain.bGain = request->b_gain; + gain.gGain = request->g_gain; + device_->setStructuredData(OB_STRUCT_COLOR_AWB_GAIN, reinterpret_cast(&gain), + sizeof(gain)); + response->success = true; + } catch (const ob::Error& e) { + response->success = false; + response->message = orbbec_camera::formatObErrorWithStatus(e); + } catch (const std::exception& e) { + response->success = false; + response->message = e.what(); + } catch (...) { + response->success = false; + response->message = "unknown error"; + } +} + void OBCameraNode::setAutoExposureCallback( const std::shared_ptr& request, std::shared_ptr& response, diff --git a/orbbec_camera/src/timestamp_csv_logger.cpp b/orbbec_camera/src/timestamp_csv_logger.cpp new file mode 100644 index 00000000..acd4f150 --- /dev/null +++ b/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 +#include + +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( + 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(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 &color_frame, + const std::shared_ptr &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 &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 &frame) { + auto *timestamp_logger = imageLoggerForStream(stream_type); + if (timestamp_logger) { + timestamp_logger->recordImagePublishSkipped(stream_type, frame); + } +} + +void TimestampCsvLogger::recordSyncedImu(const std::shared_ptr &accel_frame, + const std::shared_ptr &gyro_frame, + int64_t arrival_system_us, + std::optional 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 &frame, + int64_t arrival_system_us, + std::optional 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 ×tamp_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 diff --git a/orbbec_camera_msgs/CMakeLists.txt b/orbbec_camera_msgs/CMakeLists.txt index b71437b9..5212ea6d 100644 --- a/orbbec_camera_msgs/CMakeLists.txt +++ b/orbbec_camera_msgs/CMakeLists.txt @@ -7,6 +7,7 @@ endif() # find dependencies find_package(ament_cmake REQUIRED) +find_package(action_msgs REQUIRED) find_package(rosidl_default_generators REQUIRED) find_package(sensor_msgs REQUIRED) find_package(std_msgs REQUIRED) @@ -31,16 +32,20 @@ rosidl_generate_interfaces( "srv/GetDeviceInfo.srv" "srv/GetCameraInfo.srv" "srv/GetInt32.srv" + "srv/GetAwbGain.srv" "srv/GetString.srv" "srv/SetFilter.srv" "srv/SetInt32.srv" + "srv/SetAwbGain.srv" "srv/SetString.srv" "srv/SetArrays.srv" "srv/GetUserCalibParams.srv" "srv/SetUserCalibParams.srv" "srv/SetBagRecording.srv" "srv/SetStreamProfile.srv" + "action/RunAeAwbLockTest.action" DEPENDENCIES + action_msgs sensor_msgs std_msgs ) diff --git a/orbbec_camera_msgs/action/RunAeAwbLockTest.action b/orbbec_camera_msgs/action/RunAeAwbLockTest.action new file mode 100644 index 00000000..4e22deb7 --- /dev/null +++ b/orbbec_camera_msgs/action/RunAeAwbLockTest.action @@ -0,0 +1,24 @@ +# Timeout budget for the main workflow. Recovery after failure uses a separate timeout. +# A value of zero uses the 10-second default. +uint32 timeout_ms +--- +bool success +string message + +int32 captured_exposure +int32 captured_color_gain +uint16 captured_awb_r_gain +uint16 captured_awb_b_gain +uint16 captured_awb_g_gain +int32 captured_color_temperature + +int32 actual_exposure +int32 actual_color_gain +uint16 actual_awb_r_gain +uint16 actual_awb_b_gain +uint16 actual_awb_g_gain +int32 actual_color_temperature +--- +string phase +# Raw SDK status read for this phase. +int32 ae_awb_status diff --git a/orbbec_camera_msgs/package.xml b/orbbec_camera_msgs/package.xml index 5eea1a4e..dfcac903 100644 --- a/orbbec_camera_msgs/package.xml +++ b/orbbec_camera_msgs/package.xml @@ -2,13 +2,14 @@ orbbec_camera_msgs - 2.9.3 + 2.10.1 A package containing orbbec camera messages definitions. yalian Apache-2.0 ament_cmake + action_msgs ament_lint_auto ament_lint_common rosidl_default_generators diff --git a/orbbec_camera_msgs/srv/GetAwbGain.srv b/orbbec_camera_msgs/srv/GetAwbGain.srv new file mode 100644 index 00000000..9fc71e2f --- /dev/null +++ b/orbbec_camera_msgs/srv/GetAwbGain.srv @@ -0,0 +1,7 @@ +--- +# Raw Q8.8 AWB channel gains. +uint16 r_gain +uint16 b_gain +uint16 g_gain +bool success +string message diff --git a/orbbec_camera_msgs/srv/SetAwbGain.srv b/orbbec_camera_msgs/srv/SetAwbGain.srv new file mode 100644 index 00000000..8b80e9c2 --- /dev/null +++ b/orbbec_camera_msgs/srv/SetAwbGain.srv @@ -0,0 +1,7 @@ +# Raw Q8.8 AWB channel gains. +uint16 r_gain +uint16 b_gain +uint16 g_gain +--- +bool success +string message diff --git a/orbbec_description/package.xml b/orbbec_description/package.xml index 10234781..b7e89822 100644 --- a/orbbec_description/package.xml +++ b/orbbec_description/package.xml @@ -2,7 +2,7 @@ orbbec_description - 2.9.3 + 2.10.1 TODO: Package description yalian Apache-2.0