i/OrbbecSDK_ROS2
1
0
Fork 0
mirror of https://github.com/orbbec/OrbbecSDK_ROS2.git synced 2026-10-05 20:47:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'v2/develop' into v2-main

Browse Source
This commit is contained in:
slz
2026-09-03 21:36:57 +08:00
parent 9d92f4718a f878bec3df
commit 7e855fa058
56 changed files with 1778 additions and 264 deletions
Download Patch File Download Diff File Expand all files Collapse all files
orbbec_camera/CMakeLists.txt
+23 -1
View File
@@ -269,6 +269,22 @@ orbbec_target_dependencies(${PROJECT_NAME}
rclcpp_components_register_node(
${PROJECT_NAME} PLUGIN "orbbec_camera::OBCameraNodeDriver" EXECUTABLE orbbec_camera_node
)
add_library(gige_action_command_component SHARED src/gige_action_command_node.cpp)
target_include_directories(gige_action_command_component PUBLIC ${COMMON_INCLUDE_DIRS})
target_link_directories(gige_action_command_component PRIVATE ${ORBBEC_LIBS_DIR})
target_link_libraries(gige_action_command_component
${orbbec_camera_msgs_TARGETS}
OrbbecSDK
rclcpp::rclcpp
)
orbbec_target_dependencies(gige_action_command_component rclcpp_components)
rclcpp_components_register_node(
gige_action_command_component
PLUGIN "orbbec_camera::GigEActionCommandNode"
EXECUTABLE gige_action_command_node
)
# Add nodes using the macro
add_orbbec_executable(list_devices_node tools/list_devices_node.cpp)
add_orbbec_executable(list_depth_work_mode_node tools/list_depth_work_mode.cpp)
@@ -360,7 +376,8 @@ rclcpp_components_register_node(
)
# Install rules
install(TARGETS ${PROJECT_NAME} frame_latency start_benchmark multi_save_rgbir ARCHIVE DESTINATION lib
install(TARGETS ${PROJECT_NAME} gige_action_command_component frame_latency start_benchmark
multi_save_rgbir ARCHIVE DESTINATION lib
LIBRARY DESTINATION lib RUNTIME DESTINATION bin
)
@@ -391,7 +408,12 @@ install(TARGETS list_devices_node list_depth_work_mode_node list_camera_profile_
if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
find_package(ament_cmake_gtest REQUIRED)
ament_lint_auto_find_test_dependencies()
ament_add_gtest(camera_info_distortion_test test/camera_info_distortion_test.cpp)
target_link_directories(camera_info_distortion_test PRIVATE ${ORBBEC_LIBS_DIR})
target_link_libraries(camera_info_distortion_test ${PROJECT_NAME})
endif()
ament_export_include_directories(include)
orbbec_camera/SDK/arm64/include/libobsensor/h/Advanced.h
+33
View File
@@ -101,6 +101,16 @@ OB_EXPORT void ob_delete_depth_work_mode_list(ob_depth_work_mode_list *work_mode
*/
OB_EXPORT const char *ob_device_get_current_preset_name(const ob_device *device, ob_error **error);
/**
* @brief Get the version of the current depth work mode.
*
* @param[in] device The device object.
* @param[out] error Pointer to an error object that will be set if an error occurs.
*
* @return const char* The version string, e.g. "1.2.3". Empty string when the device does not
* support versioned modes.
*/
OB_EXPORT const char *ob_device_get_current_preset_depth_work_mode_version(const ob_device *device, ob_error **error);
/**
* @brief Get the available preset list.
* @attention After loading the preset, the settings in the preset will set to the device immediately. Therefore, it is recommended to re-read the device
@@ -113,6 +123,17 @@ OB_EXPORT const char *ob_device_get_current_preset_name(const ob_device *device,
*/
OB_EXPORT void ob_device_load_preset(ob_device *device, const char *preset_name, ob_error **error);
/**
* @brief Load the preset by preset name and the target depth work mode version.
* When multiple presets share the same name, the version selects the exact one.
*
* @param[in] device The device object.
* @param[in] preset_name The preset name. The name should be one of the preset names returned by @ref ob_device_get_available_preset_list.
* @param[in] version The target depth work mode version string, e.g. "1.2.3". Must not be empty.
* @param[out] error Pointer to an error object that will be set if an error occurs.
*/
OB_EXPORT void ob_device_load_preset_by_depth_work_mode_version(ob_device *device, const char *preset_name, const char *version, ob_error **error);
/**
* @brief Load preset from json string.
* @brief After loading the custom preset, the settings in the custom preset will set to the device immediately.
@@ -217,6 +238,18 @@ OB_EXPORT const char *ob_device_preset_list_get_name(const ob_device_preset_list
*/
OB_EXPORT bool ob_device_preset_list_has_preset(const ob_device_preset_list *preset_list, const char *preset_name, ob_error **error);
/**
* @brief Get the target depth work mode version of the preset at the specified index.
*
* @param[in] preset_list Data structure containing a list of presets
* @param[in] index Index of the target preset
* @param[out] error Pointer to an error object that will be set if an error occurs.
*
* @return const char* The version string of the depth work mode, e.g. "1.2.3". Empty string when the
* device does not support versioned modes.
*/
OB_EXPORT const char *ob_device_preset_list_get_depth_work_mode_version(const ob_device_preset_list *preset_list, uint32_t index, ob_error **error);
/**
* @brief Check if the device supports the frame interleave feature.
*
orbbec_camera/SDK/arm64/include/libobsensor/h/Context.h
+14
View File
@@ -80,6 +80,20 @@ OB_EXPORT void ob_enable_net_device_enumeration(ob_context *context, bool enable
*/
OB_EXPORT bool ob_force_ip_config(const char *macAddress, ob_net_ip_config config, ob_error **error);
/**
* @brief Send a GigE Vision Action Command via GVCP.
*
* @param[in] deviceKey Device key to match.
* @param[in] groupKey Group key to match against the camera's Action blocks.
* @param[in] groupMask Group mask, bitwise-ANDed with each Action block's mask.
* @param[in] destIp Destination IPv4 address. NULL or "255.255.255.255" uses broadcast.
* @param[in] scheduledTime PTP absolute timestamp. 0 = immediate trigger, non-zero = scheduled.
* @param[out] error Pointer to an error object that will be populated if an error occurs.
*
* @return bool true on success. Fire-and-forget; does not wait for camera ACK.
*/
OB_EXPORT bool ob_send_action_command(uint32_t deviceKey, uint32_t groupKey, uint32_t groupMask, const char *destIp, uint64_t scheduledTime, ob_error **error);
/**
* @brief Set the GVCP port scheme used for network device discovery and control.
*
orbbec_camera/SDK/arm64/include/libobsensor/h/ObTypes.h
+6
View File
@@ -1578,6 +1578,12 @@ typedef enum {
* @brief The device captures data in software synchronization mode, starting acquisition based on the system time.
*/
OB_MULTI_DEVICE_SYNC_MODE_SOFTWARE_SYNCED = 1 << 8,
/**
* @brief Group actions trigger mode.
* @note The device waits for a GVCP ACTION_CMD matching the configured action parameters.
*/
OB_MULTI_DEVICE_SYNC_MODE_GROUP_ACTIONS = 1 << 9,
} ob_multi_device_sync_mode,
OBMultiDeviceSyncMode;
orbbec_camera/SDK/arm64/include/libobsensor/h/Property.h
+44
View File
@@ -674,6 +674,7 @@ typedef enum {
/**
* @brief Enable FPS boost in trigger mode
* Not effective for devices connected via USB 2.x or lower
*/
OB_PROP_FPS_BOOST_BOOL = 275,
@@ -695,6 +696,44 @@ typedef enum {
*/
OB_PROP_COLOR_AE_AWB_STATUS_INT = 287,
/**
* @brief Number of Action Signal blocks supported by the device
*/
OB_PROP_ACTION_SIGNAL_COUNT_INT = 296,
/**
* @brief Action Device Key, shared across all Action Commands
*/
OB_PROP_ACTION_DEVICE_KEY_INT = 297,
/**
* @brief Maximum number of scheduled Action Commands that can be queued by the device
*/
OB_PROP_ACTION_SCHEDULED_COMMAND_QUEUE_SIZE_INT = 298,
/**
* @brief Action Signal selector, 0..N-1; subsequent selector-scoped reads/writes target the selected block.
* @note The selector is stateful. The selector, Group Key, and Group Mask
* must be written serially as one caller-controlled sequence. Do not
* interleave this sequence with Action Command property operations
* from another thread on the same device.
*/
OB_PROP_ACTION_SELECTOR_INT = 299,
/**
* @brief Action Group Key for the currently selected block.
* @note Must be written serially with OB_PROP_ACTION_SELECTOR_INT and
* OB_PROP_ACTION_GROUP_MASK_INT on the same device.
*/
OB_PROP_ACTION_GROUP_KEY_INT = 300,
/**
* @brief Action Group Mask for the currently selected block.
* @note Must be written serially with OB_PROP_ACTION_SELECTOR_INT and
* OB_PROP_ACTION_GROUP_KEY_INT on the same device.
*/
OB_PROP_ACTION_GROUP_MASK_INT = 301,
/**
* @brief Baseline calibration parameters
*/
@@ -978,6 +1017,11 @@ typedef enum {
*/
OB_PROP_DEPTH_AUTO_EXPOSURE_PRIORITY_INT = 2052,
/**
* @brief Color camera WB control
*/
OB_PROP_COLOR_WB_CTRL_INT = 2053,
/**
* @brief Software disparity to depth
*/
orbbec_camera/SDK/arm64/include/libobsensor/hpp/Context.hpp
+18
View File
@@ -157,6 +157,24 @@ public:
return res;
}
/**
* @brief Send a GigE Vision Action Command via GVCP.
*
* @param deviceKey Device key to match.
* @param groupKey Group key to match.
* @param groupMask Group mask, bitwise-ANDed with Action block masks.
* @param destIp Destination IPv4 address. Defaults to "255.255.255.255" for broadcast.
* @param scheduledTime PTP timestamp. 0 = immediate, non-zero = scheduled.
*
* @return bool true on success.
*/
bool sendActionCommand(uint32_t deviceKey, uint32_t groupKey, uint32_t groupMask, const char *destIp = "255.255.255.255", uint64_t scheduledTime = 0) {
ob_error *error = nullptr;
bool ok = ob_send_action_command(deviceKey, groupKey, groupMask, destIp, scheduledTime, &error);
Error::handle(&error);
return ok;
}
/**
* @brief Set the GVCP port scheme used for network device discovery and control.
*
orbbec_camera/SDK/arm64/include/libobsensor/hpp/Device.hpp
+40
View File
@@ -880,6 +880,31 @@ public:
Error::handle(&error);
}
/**
* @brief load the preset by preset name and the target depth work mode version.
* The version is forwarded to the depth work mode switch when loading the preset.
* @param[in] presetName The preset name to set. The name should be one of the preset names returned by @ref getAvailablePresetList.
* @param[in] version The target depth work mode version string, e.g. "1.2.3". Must not be empty.
*/
void loadPreset(const char *presetName, const char *version) const {
ob_error *error = nullptr;
ob_device_load_preset_by_depth_work_mode_version(impl_, presetName, version, &error);
Error::handle(&error);
}
/**
* @brief Get the version of the current depth work mode.
*
* @return const char* The version string, e.g. "1.2.3". Empty string when the device
* does not support versioned modes.
*/
const char *getCurrentPresetDepthWorkModeVersion() const {
ob_error *error = nullptr;
const char *version = ob_device_get_current_preset_depth_work_mode_version(impl_, &error);
Error::handle(&error);
return version;
}
/**
* @brief Get available preset list
* @brief The available preset list usually defined by the device manufacturer and restores on the device.
@@ -1836,6 +1861,21 @@ public:
return name;
}
/**
* @brief Get the depth work mode version of the preset at the specified index
*
* @param[in] index the index of the device preset
*
* @return const char* the version string of the depth work mode, e.g. "1.2.3". Empty string
* when the device does not support versioned modes.
*/
const char *getDepthWorkModeVersion(uint32_t index) {
ob_error *error = nullptr;
const char *version = ob_device_preset_list_get_depth_work_mode_version(impl_, index, &error);
Error::handle(&error);
return version;
}
/**
* @brief check if the preset list contains the special name preset.
* @param[in] name The name of the preset
orbbec_camera/SDK/arm64/include/libobsensor/hpp/Filter.hpp
+10 -2
View File
@@ -177,8 +177,12 @@ public:
* @return std::shared_ptr< Frame > The processed frame.
*/
virtual std::shared_ptr<Frame> process(std::shared_ptr<const Frame> frame) const {
const ob_frame *frameImpl = nullptr;
if(frame) {
frameImpl = frame->getImpl();
}
ob_error *error = nullptr;
auto result = ob_filter_process(impl_, frame->getImpl(), &error);
auto result = ob_filter_process(impl_, frameImpl, &error);
Error::handle(&error);
if(!result) {
return nullptr;
@@ -192,8 +196,12 @@ public:
* @param[in] frame The pending frame. The processing result is returned by the callback function.
*/
virtual void pushFrame(std::shared_ptr<Frame> frame) const {
const ob_frame *frameImpl = nullptr;
if(frame) {
frameImpl = frame->getImpl();
}
ob_error *error = nullptr;
ob_filter_push_frame(impl_, frame->getImpl(), &error);
ob_filter_push_frame(impl_, frameImpl, &error);
Error::handle(&error);
}
orbbec_camera/SDK/arm64/lib/OrbbecSDKConfig-release.cmake
+2 -2
View File
@@ -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.10.1"
IMPORTED_LOCATION_RELEASE "${_IMPORT_PREFIX}/lib/libOrbbecSDK.so.2.10.2"
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.10.1" )
list(APPEND _cmake_import_check_files_for_ob::OrbbecSDK "${_IMPORT_PREFIX}/lib/libOrbbecSDK.so.2.10.2" )
# Commands beyond this point should not need to know the version.
set(CMAKE_IMPORT_FILE_VERSION)
orbbec_camera/SDK/arm64/lib/OrbbecSDKVersion.cmake
+3 -3
View File
@@ -9,19 +9,19 @@
# The variable CVF_VERSION must be set before calling configure_file().
set(PACKAGE_VERSION "2.10.1")
set(PACKAGE_VERSION "2.10.2")
if(PACKAGE_VERSION VERSION_LESS PACKAGE_FIND_VERSION)
set(PACKAGE_VERSION_COMPATIBLE FALSE)
else()
if("2.10.1" MATCHES "^([0-9]+)\\.")
if("2.10.2" 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.10.1")
set(CVF_VERSION_MAJOR "2.10.2")
endif()
if(PACKAGE_FIND_VERSION_RANGE)
orbbec_camera/SDK/arm64/lib/libOrbbecSDK.so.2
+1 -1
View File
@@ -1 +1 @@
libOrbbecSDK.so.2.10.1
libOrbbecSDK.so.2.10.2
orbbec_camera/SDK/arm64/lib/libOrbbecSDK.so.2.10.1 → orbbec_camera/SDK/arm64/lib/libOrbbecSDK.so.2.10.2
BIN
View File
Binary file not shown.
orbbec_camera/SDK/x64/include/libobsensor/h/Advanced.h
+33
View File
@@ -101,6 +101,16 @@ OB_EXPORT void ob_delete_depth_work_mode_list(ob_depth_work_mode_list *work_mode
*/
OB_EXPORT const char *ob_device_get_current_preset_name(const ob_device *device, ob_error **error);
/**
* @brief Get the version of the current depth work mode.
*
* @param[in] device The device object.
* @param[out] error Pointer to an error object that will be set if an error occurs.
*
* @return const char* The version string, e.g. "1.2.3". Empty string when the device does not
* support versioned modes.
*/
OB_EXPORT const char *ob_device_get_current_preset_depth_work_mode_version(const ob_device *device, ob_error **error);
/**
* @brief Get the available preset list.
* @attention After loading the preset, the settings in the preset will set to the device immediately. Therefore, it is recommended to re-read the device
@@ -113,6 +123,17 @@ OB_EXPORT const char *ob_device_get_current_preset_name(const ob_device *device,
*/
OB_EXPORT void ob_device_load_preset(ob_device *device, const char *preset_name, ob_error **error);
/**
* @brief Load the preset by preset name and the target depth work mode version.
* When multiple presets share the same name, the version selects the exact one.
*
* @param[in] device The device object.
* @param[in] preset_name The preset name. The name should be one of the preset names returned by @ref ob_device_get_available_preset_list.
* @param[in] version The target depth work mode version string, e.g. "1.2.3". Must not be empty.
* @param[out] error Pointer to an error object that will be set if an error occurs.
*/
OB_EXPORT void ob_device_load_preset_by_depth_work_mode_version(ob_device *device, const char *preset_name, const char *version, ob_error **error);
/**
* @brief Load preset from json string.
* @brief After loading the custom preset, the settings in the custom preset will set to the device immediately.
@@ -217,6 +238,18 @@ OB_EXPORT const char *ob_device_preset_list_get_name(const ob_device_preset_list
*/
OB_EXPORT bool ob_device_preset_list_has_preset(const ob_device_preset_list *preset_list, const char *preset_name, ob_error **error);
/**
* @brief Get the target depth work mode version of the preset at the specified index.
*
* @param[in] preset_list Data structure containing a list of presets
* @param[in] index Index of the target preset
* @param[out] error Pointer to an error object that will be set if an error occurs.
*
* @return const char* The version string of the depth work mode, e.g. "1.2.3". Empty string when the
* device does not support versioned modes.
*/
OB_EXPORT const char *ob_device_preset_list_get_depth_work_mode_version(const ob_device_preset_list *preset_list, uint32_t index, ob_error **error);
/**
* @brief Check if the device supports the frame interleave feature.
*
orbbec_camera/SDK/x64/include/libobsensor/h/Context.h
+14
View File
@@ -80,6 +80,20 @@ OB_EXPORT void ob_enable_net_device_enumeration(ob_context *context, bool enable
*/
OB_EXPORT bool ob_force_ip_config(const char *macAddress, ob_net_ip_config config, ob_error **error);
/**
* @brief Send a GigE Vision Action Command via GVCP.
*
* @param[in] deviceKey Device key to match.
* @param[in] groupKey Group key to match against the camera's Action blocks.
* @param[in] groupMask Group mask, bitwise-ANDed with each Action block's mask.
* @param[in] destIp Destination IPv4 address. NULL or "255.255.255.255" uses broadcast.
* @param[in] scheduledTime PTP absolute timestamp. 0 = immediate trigger, non-zero = scheduled.
* @param[out] error Pointer to an error object that will be populated if an error occurs.
*
* @return bool true on success. Fire-and-forget; does not wait for camera ACK.
*/
OB_EXPORT bool ob_send_action_command(uint32_t deviceKey, uint32_t groupKey, uint32_t groupMask, const char *destIp, uint64_t scheduledTime, ob_error **error);
/**
* @brief Set the GVCP port scheme used for network device discovery and control.
*
orbbec_camera/SDK/x64/include/libobsensor/h/ObTypes.h
+6
View File
@@ -1578,6 +1578,12 @@ typedef enum {
* @brief The device captures data in software synchronization mode, starting acquisition based on the system time.
*/
OB_MULTI_DEVICE_SYNC_MODE_SOFTWARE_SYNCED = 1 << 8,
/**
* @brief Group actions trigger mode.
* @note The device waits for a GVCP ACTION_CMD matching the configured action parameters.
*/
OB_MULTI_DEVICE_SYNC_MODE_GROUP_ACTIONS = 1 << 9,
} ob_multi_device_sync_mode,
OBMultiDeviceSyncMode;
orbbec_camera/SDK/x64/include/libobsensor/h/Property.h
+44
View File
@@ -674,6 +674,7 @@ typedef enum {
/**
* @brief Enable FPS boost in trigger mode
* Not effective for devices connected via USB 2.x or lower
*/
OB_PROP_FPS_BOOST_BOOL = 275,
@@ -695,6 +696,44 @@ typedef enum {
*/
OB_PROP_COLOR_AE_AWB_STATUS_INT = 287,
/**
* @brief Number of Action Signal blocks supported by the device
*/
OB_PROP_ACTION_SIGNAL_COUNT_INT = 296,
/**
* @brief Action Device Key, shared across all Action Commands
*/
OB_PROP_ACTION_DEVICE_KEY_INT = 297,
/**
* @brief Maximum number of scheduled Action Commands that can be queued by the device
*/
OB_PROP_ACTION_SCHEDULED_COMMAND_QUEUE_SIZE_INT = 298,
/**
* @brief Action Signal selector, 0..N-1; subsequent selector-scoped reads/writes target the selected block.
* @note The selector is stateful. The selector, Group Key, and Group Mask
* must be written serially as one caller-controlled sequence. Do not
* interleave this sequence with Action Command property operations
* from another thread on the same device.
*/
OB_PROP_ACTION_SELECTOR_INT = 299,
/**
* @brief Action Group Key for the currently selected block.
* @note Must be written serially with OB_PROP_ACTION_SELECTOR_INT and
* OB_PROP_ACTION_GROUP_MASK_INT on the same device.
*/
OB_PROP_ACTION_GROUP_KEY_INT = 300,
/**
* @brief Action Group Mask for the currently selected block.
* @note Must be written serially with OB_PROP_ACTION_SELECTOR_INT and
* OB_PROP_ACTION_GROUP_KEY_INT on the same device.
*/
OB_PROP_ACTION_GROUP_MASK_INT = 301,
/**
* @brief Baseline calibration parameters
*/
@@ -978,6 +1017,11 @@ typedef enum {
*/
OB_PROP_DEPTH_AUTO_EXPOSURE_PRIORITY_INT = 2052,
/**
* @brief Color camera WB control
*/
OB_PROP_COLOR_WB_CTRL_INT = 2053,
/**
* @brief Software disparity to depth
*/
orbbec_camera/SDK/x64/include/libobsensor/hpp/Context.hpp
+18
View File
@@ -157,6 +157,24 @@ public:
return res;
}
/**
* @brief Send a GigE Vision Action Command via GVCP.
*
* @param deviceKey Device key to match.
* @param groupKey Group key to match.
* @param groupMask Group mask, bitwise-ANDed with Action block masks.
* @param destIp Destination IPv4 address. Defaults to "255.255.255.255" for broadcast.
* @param scheduledTime PTP timestamp. 0 = immediate, non-zero = scheduled.
*
* @return bool true on success.
*/
bool sendActionCommand(uint32_t deviceKey, uint32_t groupKey, uint32_t groupMask, const char *destIp = "255.255.255.255", uint64_t scheduledTime = 0) {
ob_error *error = nullptr;
bool ok = ob_send_action_command(deviceKey, groupKey, groupMask, destIp, scheduledTime, &error);
Error::handle(&error);
return ok;
}
/**
* @brief Set the GVCP port scheme used for network device discovery and control.
*
orbbec_camera/SDK/x64/include/libobsensor/hpp/Device.hpp
+40
View File
@@ -880,6 +880,31 @@ public:
Error::handle(&error);
}
/**
* @brief load the preset by preset name and the target depth work mode version.
* The version is forwarded to the depth work mode switch when loading the preset.
* @param[in] presetName The preset name to set. The name should be one of the preset names returned by @ref getAvailablePresetList.
* @param[in] version The target depth work mode version string, e.g. "1.2.3". Must not be empty.
*/
void loadPreset(const char *presetName, const char *version) const {
ob_error *error = nullptr;
ob_device_load_preset_by_depth_work_mode_version(impl_, presetName, version, &error);
Error::handle(&error);
}
/**
* @brief Get the version of the current depth work mode.
*
* @return const char* The version string, e.g. "1.2.3". Empty string when the device
* does not support versioned modes.
*/
const char *getCurrentPresetDepthWorkModeVersion() const {
ob_error *error = nullptr;
const char *version = ob_device_get_current_preset_depth_work_mode_version(impl_, &error);
Error::handle(&error);
return version;
}
/**
* @brief Get available preset list
* @brief The available preset list usually defined by the device manufacturer and restores on the device.
@@ -1836,6 +1861,21 @@ public:
return name;
}
/**
* @brief Get the depth work mode version of the preset at the specified index
*
* @param[in] index the index of the device preset
*
* @return const char* the version string of the depth work mode, e.g. "1.2.3". Empty string
* when the device does not support versioned modes.
*/
const char *getDepthWorkModeVersion(uint32_t index) {
ob_error *error = nullptr;
const char *version = ob_device_preset_list_get_depth_work_mode_version(impl_, index, &error);
Error::handle(&error);
return version;
}
/**
* @brief check if the preset list contains the special name preset.
* @param[in] name The name of the preset
orbbec_camera/SDK/x64/include/libobsensor/hpp/Filter.hpp
+10 -2
View File
@@ -177,8 +177,12 @@ public:
* @return std::shared_ptr< Frame > The processed frame.
*/
virtual std::shared_ptr<Frame> process(std::shared_ptr<const Frame> frame) const {
const ob_frame *frameImpl = nullptr;
if(frame) {
frameImpl = frame->getImpl();
}
ob_error *error = nullptr;
auto result = ob_filter_process(impl_, frame->getImpl(), &error);
auto result = ob_filter_process(impl_, frameImpl, &error);
Error::handle(&error);
if(!result) {
return nullptr;
@@ -192,8 +196,12 @@ public:
* @param[in] frame The pending frame. The processing result is returned by the callback function.
*/
virtual void pushFrame(std::shared_ptr<Frame> frame) const {
const ob_frame *frameImpl = nullptr;
if(frame) {
frameImpl = frame->getImpl();
}
ob_error *error = nullptr;
ob_filter_push_frame(impl_, frame->getImpl(), &error);
ob_filter_push_frame(impl_, frameImpl, &error);
Error::handle(&error);
}
orbbec_camera/SDK/x64/lib/OrbbecSDKConfig-release.cmake
+2 -2
View File
@@ -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.10.1"
IMPORTED_LOCATION_RELEASE "${_IMPORT_PREFIX}/lib/libOrbbecSDK.so.2.10.2"
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.10.1" )
list(APPEND _cmake_import_check_files_for_ob::OrbbecSDK "${_IMPORT_PREFIX}/lib/libOrbbecSDK.so.2.10.2" )
# Commands beyond this point should not need to know the version.
set(CMAKE_IMPORT_FILE_VERSION)
orbbec_camera/SDK/x64/lib/OrbbecSDKVersion.cmake
+3 -3
View File
@@ -9,19 +9,19 @@
# The variable CVF_VERSION must be set before calling configure_file().
set(PACKAGE_VERSION "2.10.1")
set(PACKAGE_VERSION "2.10.2")
if(PACKAGE_VERSION VERSION_LESS PACKAGE_FIND_VERSION)
set(PACKAGE_VERSION_COMPATIBLE FALSE)
else()
if("2.10.1" MATCHES "^([0-9]+)\\.")
if("2.10.2" 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.10.1")
set(CVF_VERSION_MAJOR "2.10.2")
endif()
if(PACKAGE_FIND_VERSION_RANGE)
orbbec_camera/SDK/x64/lib/libOrbbecSDK.so.2
+1 -1
View File
@@ -1 +1 @@
libOrbbecSDK.so.2.10.1
libOrbbecSDK.so.2.10.2
orbbec_camera/SDK/x64/lib/libOrbbecSDK.so.2.10.1 → orbbec_camera/SDK/x64/lib/libOrbbecSDK.so.2.10.2
BIN
View File
Binary file not shown.
orbbec_camera/config/camera_params.yaml
+1
View File
@@ -6,6 +6,7 @@ enable_point_cloud: false
enable_colored_point_cloud: false
enable_ldp: false
device_preset: "Default"
device_preset_version: ""
enable_laser: true
# enable_laser: true
orbbec_camera/config/camera_secondary_params.yaml
+1
View File
@@ -6,6 +6,7 @@ enable_point_cloud: false
enable_colored_point_cloud: false
enable_ldp: false
device_preset: "Default"
device_preset_version: ""
enable_laser: true
# enable_laser: true
orbbec_camera/examples/README.MD
+1
View File
@@ -12,6 +12,7 @@ For command-line maintenance and diagnostic utilities, see the official
| Source | Purpose | Experience Level | Official Guide |
| :---: | --- | :---: | --- |
| [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) |
| [GigE Action Command](./gige_action_command) | Configure and trigger a Gemini 335Le camera group over GVCP. | ⭐️⭐️ | [README](./gige_action_command/README.md) |
| [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) |
orbbec_camera/examples/benchmark/gemini_330_series_benchmark.launch.py
+2 -1
View File
@@ -43,7 +43,7 @@ def load_parameters(context, args):
if config_file_path:
yaml_params = load_yaml(config_file_path)
default_params = merge_params(default_params, yaml_params)
skip_convert = {'config_file_path', 'usb_port', 'serial_number'}
skip_convert = {'config_file_path', 'usb_port', 'serial_number', 'device_preset_version'}
return {
key: (value if key in skip_convert else convert_value(value))
for key, value in default_params.items()
@@ -223,6 +223,7 @@ def generate_launch_description():
DeclareLaunchArgument('enable_laser', default_value='true'),
DeclareLaunchArgument('depth_precision', default_value=''),
DeclareLaunchArgument('device_preset', default_value='Default'),
DeclareLaunchArgument('device_preset_version', default_value=''),
DeclareLaunchArgument('retry_on_usb3_detection_failure', default_value='false'),
DeclareLaunchArgument('laser_energy_level', default_value='-1'),
DeclareLaunchArgument('enable_sync_host_time', default_value='false'),
orbbec_camera/examples/gige_action_command/README.md
+68
View File
@@ -0,0 +1,68 @@
# GigE Action Command
This example starts two Gemini 335Le cameras in Group Actions synchronization mode and one
host-side Action Command sender. The sender is intentionally created once at the top level because
a GVCP Action Command can trigger multiple cameras.
## Requirements
- Gemini 335Le firmware 1.8.24 or later
- Orbbec SDK 2.10.2 or later
- Both cameras and the host on the same network
Before running the example, change the two `net_device_ip` values in
`multi_gige_action_command.launch.py` to match the cameras.
## Start the cameras and sender
```bash
ros2 launch orbbec_camera multi_gige_action_command.launch.py
```
The launch file creates these device-scoped configuration services and one network-scoped sender:
```text
/camera_01/get_action_config
/camera_01/set_action_config
/camera_02/get_action_config
/camera_02/set_action_config
/gige_action_command_node/send_action_command
```
## Configure the cameras
Configure Action Signal block 0 on both cameras with matching keys and masks:
```bash
ros2 service call /camera_01/set_action_config \
orbbec_camera_msgs/srv/SetActionConfig \
"{device_key: 1, selector: 0, group_key: 1, group_mask: 1}"
ros2 service call /camera_02/set_action_config \
orbbec_camera_msgs/srv/SetActionConfig \
"{device_key: 1, selector: 0, group_key: 1, group_mask: 1}"
```
Read the configuration back when needed:
```bash
ros2 service call /camera_01/get_action_config \
orbbec_camera_msgs/srv/GetActionConfig \
"{selector: 0}"
```
## Trigger the group
Send an immediate broadcast command. Every camera whose device key, group key, and group mask
match the request will be triggered:
```bash
ros2 service call /gige_action_command_node/send_action_command \
orbbec_camera_msgs/srv/SendActionCommand \
"{device_key: 1, group_key: 1, group_mask: 1, destination_ip: '255.255.255.255', scheduled_time: 0}"
```
`success: true` means the host dispatched the GVCP command; the protocol does not return a device
acknowledgment. A nonzero `scheduled_time` uses a GVCP/PTP timestamp, with seconds in the upper
32 bits and nanoseconds in the lower 32 bits. Scheduled triggering requires the cameras and sender
host to use synchronized time.
orbbec_camera/examples/gige_action_command/multi_gige_action_command.launch.py
+53
View File
@@ -0,0 +1,53 @@
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import GroupAction, IncludeLaunchDescription, TimerAction
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch_ros.actions import Node
def generate_launch_description():
package_dir = get_package_share_directory("orbbec_camera")
camera_launch = os.path.join(
package_dir, "launch", "gemini_330_series.launch.py"
)
camera_01 = IncludeLaunchDescription(
PythonLaunchDescriptionSource(camera_launch),
launch_arguments={
"camera_name": "camera_01",
"enumerate_net_device": "false",
"net_device_ip": "192.168.1.10",
"net_device_port": "8090",
"sync_mode": "group_actions",
"log_file_name": "camera_01.log",
}.items(),
)
camera_02 = IncludeLaunchDescription(
PythonLaunchDescriptionSource(camera_launch),
launch_arguments={
"camera_name": "camera_02",
"enumerate_net_device": "false",
"net_device_ip": "192.168.1.11",
"net_device_port": "8090",
"sync_mode": "group_actions",
"log_file_name": "camera_02.log",
}.items(),
)
action_command_node = Node(
package="orbbec_camera",
executable="gige_action_command_node",
name="gige_action_command_node",
output="screen",
)
return LaunchDescription(
[
action_command_node,
TimerAction(period=0.0, actions=[GroupAction([camera_01])]),
TimerAction(period=2.0, actions=[GroupAction([camera_02])]),
]
)
orbbec_camera/examples/gmsl_camera/gemini_330_gmsl.launch.py
+2 -1
View File
@@ -43,7 +43,7 @@ def load_parameters(context, args):
if config_file_path:
yaml_params = load_yaml(config_file_path)
default_params = merge_params(default_params, yaml_params)
skip_convert = {'config_file_path', 'usb_port', 'serial_number'}
skip_convert = {'config_file_path', 'usb_port', 'serial_number', 'device_preset_version'}
return {
key: (value if key in skip_convert else convert_value(value))
for key, value in default_params.items()
@@ -223,6 +223,7 @@ def generate_launch_description():
DeclareLaunchArgument('enable_laser', default_value='true'),
DeclareLaunchArgument('depth_precision', default_value=''),
DeclareLaunchArgument('device_preset', default_value='Default'),
DeclareLaunchArgument('device_preset_version', default_value=''),
DeclareLaunchArgument('retry_on_usb3_detection_failure', default_value='false'),
DeclareLaunchArgument('laser_energy_level', default_value='-1'),
DeclareLaunchArgument('enable_sync_host_time', default_value='false'),
orbbec_camera/examples/multi_camera_synced_verification_tool/gemini_330_series_synced_verify.launch.py
+2 -1
View File
@@ -43,7 +43,7 @@ def load_parameters(context, args):
if config_file_path:
yaml_params = load_yaml(config_file_path)
default_params = merge_params(default_params, yaml_params)
skip_convert = {'config_file_path', 'usb_port', 'serial_number'}
skip_convert = {'config_file_path', 'usb_port', 'serial_number', 'device_preset_version'}
return {
key: (value if key in skip_convert else convert_value(value))
for key, value in default_params.items()
@@ -223,6 +223,7 @@ def generate_launch_description():
DeclareLaunchArgument('enable_laser', default_value='true'),
DeclareLaunchArgument('depth_precision', default_value=''),
DeclareLaunchArgument('device_preset', default_value='Default'),
DeclareLaunchArgument('device_preset_version', default_value=''),
DeclareLaunchArgument('retry_on_usb3_detection_failure', default_value='false'),
DeclareLaunchArgument('laser_energy_level', default_value='-1'),
DeclareLaunchArgument('enable_sync_host_time', default_value='false'),
orbbec_camera/include/orbbec_camera/constants.h
+1 -1
View File
@@ -24,7 +24,7 @@
#define OB_ROS_MAJOR_VERSION 2
#define OB_ROS_MINOR_VERSION 10
#define OB_ROS_PATCH_VERSION 1
#define OB_ROS_PATCH_VERSION 2
#ifndef STRINGIFY
#define STRINGIFY(arg) #arg
orbbec_camera/include/orbbec_camera/gige_action_command_node.h
+42
View File
@@ -0,0 +1,42 @@
/*******************************************************************************
* Copyright (c) 2026 Orbbec 3D Technology, Inc
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*******************************************************************************/
#pragma once
#include <memory>
#include <rclcpp/rclcpp.hpp>
#include "libobsensor/ObSensor.hpp"
#include "orbbec_camera_msgs/srv/send_action_command.hpp"
namespace orbbec_camera {
class GigEActionCommandNode : public rclcpp::Node {
public:
explicit GigEActionCommandNode(const rclcpp::NodeOptions& node_options = rclcpp::NodeOptions());
private:
void sendActionCommandCallback(
const std::shared_ptr<orbbec_camera_msgs::srv::SendActionCommand::Request> request,
std::shared_ptr<orbbec_camera_msgs::srv::SendActionCommand::Response> response);
std::unique_ptr<ob::Context> context_;
rclcpp::Service<orbbec_camera_msgs::srv::SendActionCommand>::SharedPtr
send_action_command_service_;
};
} // namespace orbbec_camera
orbbec_camera/include/orbbec_camera/ob_camera_node.h
+21
View File
@@ -53,6 +53,7 @@
#include "orbbec_camera_msgs/msg/depth_filter_state.hpp"
#include "orbbec_camera_msgs/msg/depth_filters_status.hpp"
#include "orbbec_camera_msgs/srv/get_device_config.hpp"
#include "orbbec_camera_msgs/srv/get_action_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"
@@ -65,6 +66,7 @@
#include "orbbec_camera_msgs/srv/get_bool.hpp"
#include "orbbec_camera_msgs/srv/set_string.hpp"
#include "orbbec_camera_msgs/srv/set_filter.hpp"
#include "orbbec_camera_msgs/srv/set_action_config.hpp"
#include "orbbec_camera_msgs/srv/set_arrays.hpp"
#include "orbbec_camera_msgs/srv/set_stream_profile.hpp"
#include "orbbec_camera_msgs/srv/get_user_calib_params.hpp"
@@ -135,6 +137,7 @@
namespace orbbec_camera {
using GetDeviceConfig = orbbec_camera_msgs::srv::GetDeviceConfig;
using GetActionConfig = orbbec_camera_msgs::srv::GetActionConfig;
using GetDeviceInfo = orbbec_camera_msgs::srv::GetDeviceInfo;
using Extrinsics = orbbec_camera_msgs::msg::Extrinsics;
using SetInt32 = orbbec_camera_msgs::srv::SetInt32;
@@ -146,6 +149,7 @@ using SetString = orbbec_camera_msgs::srv::SetString;
using SetBool = std_srvs::srv::SetBool;
using GetBool = orbbec_camera_msgs::srv::GetBool;
using SetFilter = orbbec_camera_msgs::srv::SetFilter;
using SetActionConfig = orbbec_camera_msgs::srv::SetActionConfig;
using SetArrays = orbbec_camera_msgs::srv::SetArrays;
using SetStreamProfile = orbbec_camera_msgs::srv::SetStreamProfile;
using SetUserCalibParams = orbbec_camera_msgs::srv::SetUserCalibParams;
@@ -433,6 +437,12 @@ class OBCameraNode {
void setWhiteBalanceCallback(const std::shared_ptr<SetInt32 ::Request>& request,
std::shared_ptr<SetInt32 ::Response>& response);
void getColorWbCtrlCallback(const std::shared_ptr<GetInt32::Request>& request,
std::shared_ptr<GetInt32::Response>& response);
void setColorWbCtrlCallback(const std::shared_ptr<SetInt32::Request>& request,
std::shared_ptr<SetInt32::Response>& response);
void getAutoWhiteBalanceCallback(const std::shared_ptr<GetInt32::Request>& request,
std::shared_ptr<GetInt32::Response>& response);
@@ -481,6 +491,12 @@ class OBCameraNode {
void getDeviceConfigCallback(const std::shared_ptr<GetDeviceConfig::Request>& request,
std::shared_ptr<GetDeviceConfig::Response>& response);
void getActionConfigCallback(const std::shared_ptr<GetActionConfig::Request>& request,
std::shared_ptr<GetActionConfig::Response>& response);
void setActionConfigCallback(const std::shared_ptr<SetActionConfig::Request>& request,
std::shared_ptr<SetActionConfig::Response>& response);
void getSDKVersion(const std::shared_ptr<GetString::Request>& request,
std::shared_ptr<GetString::Response>& response);
@@ -762,6 +778,8 @@ class OBCameraNode {
std::map<stream_index_pair, rclcpp::Service<SetInt32>::SharedPtr> set_rotation_srv_;
rclcpp::Service<GetInt32>::SharedPtr get_white_balance_srv_;
rclcpp::Service<SetInt32>::SharedPtr set_white_balance_srv_;
rclcpp::Service<GetInt32>::SharedPtr get_color_wb_ctrl_srv_;
rclcpp::Service<SetInt32>::SharedPtr set_color_wb_ctrl_srv_;
rclcpp::Service<GetInt32>::SharedPtr get_auto_white_balance_srv_;
rclcpp::Service<SetBool>::SharedPtr set_auto_white_balance_srv_;
rclcpp::Service<GetInt32>::SharedPtr get_ae_awb_status_srv_;
@@ -778,6 +796,8 @@ class OBCameraNode {
std::map<stream_index_pair, rclcpp::Service<SetArrays>::SharedPtr> set_ae_roi_srv_;
rclcpp::Service<GetDeviceInfo>::SharedPtr get_device_srv_;
rclcpp::Service<GetDeviceConfig>::SharedPtr get_device_config_srv_;
rclcpp::Service<GetActionConfig>::SharedPtr get_action_config_srv_;
rclcpp::Service<SetActionConfig>::SharedPtr set_action_config_srv_;
rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr set_laser_enable_srv_;
rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr set_ldp_enable_srv_;
rclcpp::Service<orbbec_camera_msgs::srv::GetBool>::SharedPtr get_ldp_status_srv_;
@@ -1002,6 +1022,7 @@ class OBCameraNode {
int left_ir_decimation_factor_ = 1;
int right_ir_decimation_factor_ = 1;
std::string device_preset_;
std::string device_preset_version_;
// filter switch
bool enable_decimation_filter_ = false;
bool enable_hdr_merge_ = false;
orbbec_camera/include/orbbec_camera/utils.h
+4 -4
View File
@@ -78,18 +78,18 @@ inline std::string formatObErrorWithStatus(const ob::Error& e) {
#define TRY_TO_SET_PROPERTY(func, property, value) \
try { \
device_->func(property, value); \
device_->func((property), (value)); \
} catch (const ob::Error& e) { \
RCLCPP_ERROR_STREAM(logger_, "Failed to set " << property << " to " << value << " in " \
RCLCPP_ERROR_STREAM(logger_, "Failed to set " << (property) << " to " << (value) << " in " \
<< __FUNCTION__ << " at line " << __LINE__ \
<< ": " \
<< orbbec_camera::formatObErrorWithStatus(e)); \
} catch (const std::exception& e) { \
RCLCPP_ERROR_STREAM(logger_, "Failed to set " << property << " to " << value << " in " \
RCLCPP_ERROR_STREAM(logger_, "Failed to set " << (property) << " to " << (value) << " in " \
<< __FUNCTION__ << " at line " << __LINE__ \
<< ": " << e.what()); \
} catch (...) { \
RCLCPP_ERROR_STREAM(logger_, "Failed to set " << property << " to " << value << " in " \
RCLCPP_ERROR_STREAM(logger_, "Failed to set " << (property) << " to " << (value) << " in " \
<< __FUNCTION__ << " at line " << __LINE__); \
}
orbbec_camera/launch/gemini_330_series.launch.py
+2 -1
View File
@@ -49,7 +49,7 @@ def load_parameters(context, args):
yaml_params = load_yaml(config_file_path)
default_params = merge_params(default_params, yaml_params)
skip_convert = {'config_file_path', 'usb_port', 'serial_number', 'bag_record_filename', 'bag_filename',
'enhanced_depth_model_path', 'depth_colorizer_mode'}
'enhanced_depth_model_path', 'depth_colorizer_mode', 'device_preset_version'}
result = {}
for key, value in default_params.items():
@@ -287,6 +287,7 @@ def generate_launch_description():
DeclareLaunchArgument('enable_laser', default_value='true'),
DeclareLaunchArgument('depth_precision', default_value=''),
DeclareLaunchArgument('device_preset', default_value='Default'),
DeclareLaunchArgument('device_preset_version', default_value=''),
DeclareLaunchArgument('color_preset', default_value='Default'),# color preset name reported by the device
DeclareLaunchArgument('retry_on_usb3_detection_failure', default_value='false'),
DeclareLaunchArgument('laser_energy_level', default_value='-1'),
orbbec_camera/launch/gemini_330_series_low_cpu.launch.py
+2 -1
View File
@@ -49,7 +49,7 @@ def load_parameters(context, args):
yaml_params = load_yaml(config_file_path)
default_params = merge_params(default_params, yaml_params)
skip_convert = {'config_file_path', 'usb_port', 'serial_number', 'bag_record_filename', 'bag_filename',
'enhanced_depth_model_path', 'depth_colorizer_mode'}
'enhanced_depth_model_path', 'depth_colorizer_mode', 'device_preset_version'}
result = {}
for key, value in default_params.items():
@@ -282,6 +282,7 @@ def generate_launch_description():
DeclareLaunchArgument('enable_laser', default_value='true'),
DeclareLaunchArgument('depth_precision', default_value=''),
DeclareLaunchArgument('device_preset', default_value='Default'),
DeclareLaunchArgument('device_preset_version', default_value=''),
DeclareLaunchArgument('color_preset', default_value='Default'),# color preset name reported by the device
DeclareLaunchArgument('retry_on_usb3_detection_failure', default_value='false'),
DeclareLaunchArgument('laser_energy_level', default_value='-1'),
orbbec_camera/launch/gemini_intra_process_demo_launch.py
+2 -1
View File
@@ -43,7 +43,7 @@ def load_parameters(context, args):
yaml_params = load_yaml(config_file_path)
default_params = merge_params(default_params, yaml_params)
skip_convert = {'config_file_path', 'usb_port', 'serial_number', 'bag_record_filename', 'bag_filename',
'depth_colorizer_mode'}
'depth_colorizer_mode', 'device_preset_version'}
return {
key: (value if key in skip_convert else convert_value(value))
for key, value in default_params.items()
@@ -171,6 +171,7 @@ def generate_launch_description():
DeclareLaunchArgument('enable_laser', default_value='true'),
DeclareLaunchArgument('depth_precision', default_value=''),
DeclareLaunchArgument('device_preset', default_value='Default'),
DeclareLaunchArgument('device_preset_version', default_value=''),
DeclareLaunchArgument('retry_on_usb3_detection_failure', default_value='false'),
DeclareLaunchArgument('laser_energy_level', default_value='-1'),
DeclareLaunchArgument('enable_sync_host_time', default_value='true'),
orbbec_camera/package.xml
+4 -2
View File
@@ -2,7 +2,7 @@
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>orbbec_camera</name>
<version>2.10.1</version>
<version>2.10.2</version>
<description>Orbbec Camera package</description>
<maintainer email="[email protected]">yalian</maintainer>
<license>Apache-2.0</license>
@@ -20,7 +20,7 @@
<depend>rclcpp_components</depend>
<depend>cv_bridge</depend>
<depend>camera_info_manager</depend>
<depend version_gte="2.10.1">orbbec_camera_msgs</depend>
<depend version_gte="2.10.2">orbbec_camera_msgs</depend>
<depend>builtin_interfaces</depend>
<depend>rclcpp</depend>
<depend>rclcpp_action</depend>
@@ -55,6 +55,8 @@
<exec_depend>python3-yaml</exec_depend>
<exec_depend>rclpy</exec_depend>
<test_depend>ament_cmake_gtest</test_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
orbbec_camera/scripts/update_bundled_sdk.sh
Executable
+322
View File
@@ -0,0 +1,322 @@
#!/usr/bin/env bash
set -Eeuo pipefail
readonly SCRIPT_DIR="$(cd -- "$(dirname -- "${BASH_SOURCE[0]}")" && pwd -P)"
readonly PACKAGE_DIR="$(cd -- "${SCRIPT_DIR}/.." && pwd -P)"
readonly SDK_ROOT="${PACKAGE_DIR}/SDK"
readonly ROS_CONFIG_FILE="${PACKAGE_DIR}/config/OrbbecSDKConfig_v2.0.xml"
dry_run=false
work_dir=""
arm64_backup_dir=""
x64_backup_dir=""
config_backup_file=""
usage() {
cat <<EOF
Usage:
$(basename "$0") [--dry-run] <arm64-or-x86_64-sdk-directory>
Replace the bundled Orbbec SDK for both supported ROS package architectures.
The other SDK directory is inferred by swapping the input path suffix between
"_arm64" and "_x86_64".
Only the files used or distributed by orbbec_camera are copied:
- include/libobsensor/
- lib/libOrbbecSDK.so*
- lib/OrbbecSDKConfig-release.cmake
- lib/OrbbecSDKConfig.cmake
- lib/OrbbecSDKVersion.cmake
- lib/extensions/
- lib/OrbbecSDKConfig.xml -> config/OrbbecSDKConfig_v2.0.xml
SDK/licenses/ is left unchanged.
Example:
$(basename "$0") \\
/home/user/Downloads/SDK/OrbbecSDK_v2.10.2_linux_arm64
EOF
}
die() {
echo "Error: $*" >&2
exit 1
}
cleanup_work_dir() {
if [[ -n "${work_dir}" && "${work_dir}" == "${PACKAGE_DIR}"/.sdk-update.* ]]; then
rm -rf -- "${work_dir}"
fi
}
backup_exists() {
[[ -n "${arm64_backup_dir}" && -d "${arm64_backup_dir}" ]] ||
[[ -n "${x64_backup_dir}" && -d "${x64_backup_dir}" ]] ||
[[ -n "${config_backup_file}" && -e "${config_backup_file}" ]]
}
restore_backup() {
backup_exists || return 0
echo "Update failed; restoring the original SDK libraries and runtime configuration..." >&2
if [[ -n "${config_backup_file}" && -e "${config_backup_file}" ]]; then
if [[ -e "${ROS_CONFIG_FILE}" ]]; then
mv -- "${ROS_CONFIG_FILE}" "${work_dir}/OrbbecSDKConfig.failed.xml" || return 1
fi
mv -- "${config_backup_file}" "${ROS_CONFIG_FILE}" || return 1
fi
if [[ -n "${x64_backup_dir}" && -d "${x64_backup_dir}" ]]; then
if [[ -e "${SDK_ROOT}/x64" ]]; then
mv -- "${SDK_ROOT}/x64" "${work_dir}/x64.failed" || return 1
fi
mv -- "${x64_backup_dir}" "${SDK_ROOT}/x64" || return 1
fi
if [[ -n "${arm64_backup_dir}" && -d "${arm64_backup_dir}" ]]; then
if [[ -e "${SDK_ROOT}/arm64" ]]; then
mv -- "${SDK_ROOT}/arm64" "${work_dir}/arm64.failed" || return 1
fi
mv -- "${arm64_backup_dir}" "${SDK_ROOT}/arm64" || return 1
fi
}
on_exit() {
local status=$?
trap - EXIT
if ((status != 0)) && backup_exists; then
if ! restore_backup; then
echo "Error: automatic rollback failed; recovery files remain in ${work_dir}" >&2
exit "${status}"
fi
fi
if ! backup_exists; then
cleanup_work_dir
fi
exit "${status}"
}
trap on_exit EXIT
trap 'exit 130' INT TERM HUP
if [[ "${1:-}" == "--help" || "${1:-}" == "-h" ]]; then
usage
exit 0
fi
if [[ "${1:-}" == "--dry-run" ]]; then
dry_run=true
shift
fi
[[ $# -eq 1 ]] || {
usage >&2
exit 2
}
for command_name in basename cat cmp cp dirname file find mkdir mktemp mv readlink rm wc; do
command -v "${command_name}" >/dev/null 2>&1 || die "required command not found: ${command_name}"
done
[[ -d "${SDK_ROOT}" && ! -L "${SDK_ROOT}" ]] ||
die "bundled SDK must be a real directory: ${SDK_ROOT}"
[[ -d "${SDK_ROOT}/arm64" && ! -L "${SDK_ROOT}/arm64" ]] ||
die "bundled ARM64 SDK must be a real directory: ${SDK_ROOT}/arm64"
[[ -d "${SDK_ROOT}/x64" && ! -L "${SDK_ROOT}/x64" ]] ||
die "bundled x86-64 SDK must be a real directory: ${SDK_ROOT}/x64"
[[ -d "${SDK_ROOT}/licenses" && ! -L "${SDK_ROOT}/licenses" ]] ||
die "bundled SDK licenses must be a real directory: ${SDK_ROOT}/licenses"
[[ -f "${ROS_CONFIG_FILE}" && ! -L "${ROS_CONFIG_FILE}" ]] ||
die "ROS SDK configuration must be a real file: ${ROS_CONFIG_FILE}"
[[ -d "$1" ]] || die "SDK directory not found: $1"
input_source="$(readlink -f -- "$1")"
case "${input_source}" in
*_arm64)
arm64_source="${input_source}"
x64_source="${input_source%_arm64}_x86_64"
;;
*_x86_64)
x64_source="${input_source}"
arm64_source="${input_source%_x86_64}_arm64"
;;
*)
die "SDK directory name must end with _arm64 or _x86_64: ${input_source}"
;;
esac
[[ -d "${arm64_source}" ]] || die "paired ARM64 SDK directory not found: ${arm64_source}"
[[ -d "${x64_source}" ]] || die "paired x86-64 SDK directory not found: ${x64_source}"
case "${arm64_source}" in
"${SDK_ROOT}" | "${SDK_ROOT}"/*) die "the ARM64 source cannot be inside ${SDK_ROOT}" ;;
esac
case "${x64_source}" in
"${SDK_ROOT}" | "${SDK_ROOT}"/*) die "the x86-64 source cannot be inside ${SDK_ROOT}" ;;
esac
core_library_path() {
local source_dir=$1
local core_link="${source_dir}/lib/libOrbbecSDK.so"
[[ -e "${core_link}" ]] || die "missing core SDK library: ${core_link}"
readlink -f -- "${core_link}"
}
sdk_version() {
local core_library
core_library="$(core_library_path "$1")"
local filename=${core_library##*/}
[[ "${filename}" == libOrbbecSDK.so.* ]] ||
die "cannot determine SDK version from core library: ${core_library}"
echo "${filename#libOrbbecSDK.so.}"
}
verify_architecture() {
local source_dir=$1
local expected_arch=$2
local core_library
local description
core_library="$(core_library_path "${source_dir}")"
description="$(file -Lb -- "${core_library}")"
case "${expected_arch}" in
arm64)
[[ "${description}" == *"ARM aarch64"* || "${description}" == *"AArch64"* ]] ||
die "${source_dir} is not an ARM64 SDK (${description})"
;;
x64)
[[ "${description}" == *"x86-64"* ]] ||
die "${source_dir} is not an x86-64 SDK (${description})"
;;
*)
die "internal error: unsupported architecture ${expected_arch}"
;;
esac
}
verify_symlinks_within() {
local root_dir
local link_path
local resolved_path
root_dir="$(readlink -f -- "$1")"
while IFS= read -r -d '' link_path; do
if ! resolved_path="$(readlink -f -- "${link_path}")"; then
die "broken symbolic link: ${link_path}"
fi
case "${resolved_path}" in
"${root_dir}"/*) ;;
*) die "symbolic link points outside ${root_dir}: ${link_path} -> ${resolved_path}" ;;
esac
done < <(find "${root_dir}" -type l -print0)
}
verify_source_layout() {
local source_dir=$1
local expected_arch=$2
local required_path
for required_path in \
include/libobsensor/ObSensor.h \
include/libobsensor/ObSensor.hpp \
lib/OrbbecSDKConfig-release.cmake \
lib/OrbbecSDKConfig.cmake \
lib/OrbbecSDKVersion.cmake \
lib/OrbbecSDKConfig.xml; do
[[ -f "${source_dir}/${required_path}" ]] ||
die "missing required ${expected_arch} SDK file: ${source_dir}/${required_path}"
done
[[ -d "${source_dir}/lib/extensions" ]] ||
die "missing ${expected_arch} SDK extensions directory: ${source_dir}/lib/extensions"
verify_architecture "${source_dir}" "${expected_arch}"
verify_symlinks_within "${source_dir}/lib"
}
copy_architecture() {
local source_dir=$1
local target_arch=$2
local destination_dir=$3
local cmake_file
local copied_core_library=false
mkdir -p -- "${destination_dir}/include" "${destination_dir}/lib"
cp -a -- "${source_dir}/include/libobsensor" "${destination_dir}/include/"
while IFS= read -r -d '' library; do
cp -a -- "${library}" "${destination_dir}/lib/"
copied_core_library=true
done < <(find "${source_dir}/lib" -maxdepth 1 \( -type f -o -type l \) \
-name 'libOrbbecSDK.so*' -print0)
[[ "${copied_core_library}" == true ]] ||
die "no core SDK libraries were copied for ${target_arch}"
for cmake_file in \
OrbbecSDKConfig-release.cmake \
OrbbecSDKConfig.cmake \
OrbbecSDKVersion.cmake; do
cp -a -- "${source_dir}/lib/${cmake_file}" "${destination_dir}/lib/"
done
cp -a -- "${source_dir}/lib/extensions" "${destination_dir}/lib/"
}
verify_source_layout "${arm64_source}" arm64
verify_source_layout "${x64_source}" x64
cmp -s -- "${arm64_source}/lib/OrbbecSDKConfig.xml" \
"${x64_source}/lib/OrbbecSDKConfig.xml" ||
die "ARM64 and x86-64 OrbbecSDKConfig.xml files do not match"
arm64_version="$(sdk_version "${arm64_source}")"
x64_version="$(sdk_version "${x64_source}")"
[[ "${arm64_version}" == "${x64_version}" ]] ||
die "SDK versions do not match: ARM64=${arm64_version}, x86-64=${x64_version}"
work_dir="$(mktemp -d -- "${PACKAGE_DIR}/.sdk-update.XXXXXX")"
staged_sdk="${work_dir}/SDK.new"
staged_config="${work_dir}/OrbbecSDKConfig_v2.0.xml.new"
mkdir -p -- "${staged_sdk}"
copy_architecture "${arm64_source}" arm64 "${staged_sdk}/arm64"
copy_architecture "${x64_source}" x64 "${staged_sdk}/x64"
cp -a -- "${x64_source}/lib/OrbbecSDKConfig.xml" "${staged_config}"
verify_architecture "${staged_sdk}/arm64" arm64
verify_architecture "${staged_sdk}/x64" x64
verify_symlinks_within "${staged_sdk}"
sdk_file_count="$(find "${staged_sdk}" \( -type f -o -type l \) | wc -l)"
file_count=$((sdk_file_count + 1))
echo "Validated Orbbec SDK ${arm64_version} for ARM64 and x86-64."
echo "Staged ${file_count} ROS package files, including ${ROS_CONFIG_FILE}."
if [[ "${dry_run}" == true ]]; then
echo "Dry run complete; SDK libraries, licenses, and runtime configuration were not changed."
exit 0
fi
arm64_backup_dir="${work_dir}/arm64.old"
x64_backup_dir="${work_dir}/x64.old"
config_backup_file="${work_dir}/OrbbecSDKConfig_v2.0.xml.old"
mv -- "${SDK_ROOT}/arm64" "${arm64_backup_dir}"
mv -- "${SDK_ROOT}/x64" "${x64_backup_dir}"
mv -- "${ROS_CONFIG_FILE}" "${config_backup_file}"
mv -- "${staged_sdk}/arm64" "${SDK_ROOT}/arm64"
mv -- "${staged_sdk}/x64" "${SDK_ROOT}/x64"
mv -- "${staged_config}" "${ROS_CONFIG_FILE}"
# The verified SDK and runtime configuration are now active. Mark the
# transaction as committed; the exit handler removes the temporary backups.
arm64_backup_dir=""
x64_backup_dir=""
config_backup_file=""
echo "Updated bundled ARM64, x86-64, and runtime configuration to Orbbec SDK ${arm64_version}."
echo "Left ${SDK_ROOT}/licenses unchanged."
orbbec_camera/src/gige_action_command_node.cpp
+78
View File
@@ -0,0 +1,78 @@
/*******************************************************************************
* Copyright (c) 2026 Orbbec 3D Technology, Inc
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*******************************************************************************/
#include "orbbec_camera/gige_action_command_node.h"
#include <exception>
#include <functional>
#include <sstream>
#include <string>
#include "rclcpp_components/register_node_macro.hpp"
namespace orbbec_camera {
namespace {
std::string formatObError(const ob::Error& error) {
std::ostringstream stream;
stream << (error.getMessage() ? error.getMessage() : "Unknown OB error")
<< " status:" << static_cast<int>(error.getStatus());
return stream.str();
}
} // namespace
GigEActionCommandNode::GigEActionCommandNode(const rclcpp::NodeOptions& node_options)
: Node("gige_action_command_node", node_options), context_(std::make_unique<ob::Context>()) {
context_->enableNetDeviceEnumeration(true);
send_action_command_service_ = create_service<orbbec_camera_msgs::srv::SendActionCommand>(
"~/send_action_command", std::bind(&GigEActionCommandNode::sendActionCommandCallback, this,
std::placeholders::_1, std::placeholders::_2));
RCLCPP_INFO(get_logger(), "GigE Action Command service is ready");
}
void GigEActionCommandNode::sendActionCommandCallback(
const std::shared_ptr<orbbec_camera_msgs::srv::SendActionCommand::Request> request,
std::shared_ptr<orbbec_camera_msgs::srv::SendActionCommand::Response> response) {
if (!request) {
response->success = false;
response->message = "Invalid request";
return;
}
const std::string destination_ip =
request->destination_ip.empty() ? "255.255.255.255" : request->destination_ip;
try {
response->success =
context_->sendActionCommand(request->device_key, request->group_key, request->group_mask,
destination_ip.c_str(), request->scheduled_time);
response->message =
response->success ? "Action Command dispatched" : "SDK failed to send Action Command";
} catch (const ob::Error& error) {
response->success = false;
response->message = formatObError(error);
} catch (const std::exception& error) {
response->success = false;
response->message = error.what();
} catch (...) {
response->success = false;
response->message = "Unknown error";
}
}
} // namespace orbbec_camera
RCLCPP_COMPONENTS_REGISTER_NODE(orbbec_camera::GigEActionCommandNode)
orbbec_camera/src/ob_camera_node.cpp
+314 -199
View File
@@ -922,8 +922,11 @@ void OBCameraNode::clean() noexcept {
}
void OBCameraNode::setupDevices() {
if (!depth_work_mode_.empty() &&
device_->isPropertySupported(OB_STRUCT_CURRENT_DEPTH_ALG_MODE, OB_PERMISSION_READ_WRITE)) {
if (is_playback_device_ && (!depth_work_mode_.empty() || !device_preset_.empty())) {
RCLCPP_INFO_STREAM(logger_, "Skip device preset selection during bag playback");
} else if (!depth_work_mode_.empty() &&
device_->isPropertySupported(OB_STRUCT_CURRENT_DEPTH_ALG_MODE,
OB_PERMISSION_READ_WRITE)) {
auto depthModeList = device_->getDepthWorkModeList();
for (uint32_t i = 0; i < depthModeList->getCount(); i++) {
RCLCPP_INFO_STREAM(logger_, "depthModeList[" << i << "]: " << (*depthModeList)[i].name);
@@ -935,10 +938,48 @@ void OBCameraNode::setupDevices() {
RCLCPP_DEBUG_STREAM(logger_, "Available presets:");
auto preset_list = device_->getAvailablePresetList();
for (uint32_t i = 0; i < preset_list->getCount(); i++) {
RCLCPP_DEBUG_STREAM(logger_, "Preset " << i << ": " << preset_list->getName(i));
std::string version;
try {
const char *version_value = preset_list->getDepthWorkModeVersion(i);
version = version_value == nullptr ? "" : version_value;
} catch (...) {
// Older firmware can enumerate presets but may not report per-preset versions.
}
RCLCPP_DEBUG_STREAM(logger_, "Preset " << i << ": " << preset_list->getName(i)
<< ", depth work mode version: "
<< (version.empty() ? "not available" : version));
}
if (device_preset_version_.empty()) {
device_->loadPreset(device_preset_.c_str());
} else {
device_->loadPreset(device_preset_.c_str(), device_preset_version_.c_str());
}
std::string current_preset = device_preset_;
std::string current_version;
try {
const char *preset_name = device_->getCurrentPresetName();
current_preset = preset_name == nullptr ? device_preset_ : preset_name;
} catch (...) {
// Loading succeeded; failure to read back the name should not mark the load as failed.
}
try {
const char *version = device_->getCurrentPresetDepthWorkModeVersion();
current_version = version == nullptr ? "" : version;
} catch (...) {
// Older firmware does not report the current preset's depth work mode version.
}
RCLCPP_INFO_STREAM(logger_,
"Loaded device preset: "
<< current_preset << ", depth work mode version: "
<< (current_version.empty() ? "not available" : current_version));
if (!device_preset_version_.empty() && !current_version.empty() &&
current_version != device_preset_version_) {
RCLCPP_WARN_STREAM(logger_, "Requested device preset depth work mode version "
<< device_preset_version_ << ", but device reports "
<< current_version);
}
TRY_EXECUTE_BLOCK(device_->loadPreset(device_preset_.c_str()));
RCLCPP_INFO_STREAM(logger_, "Loaded device preset: " << device_->getCurrentPresetName());
} catch (const ob::Error &e) {
RCLCPP_ERROR_STREAM(
logger_, "Failed to load device preset: " << orbbec_camera::formatObErrorWithStatus(e));
@@ -947,6 +988,8 @@ void OBCameraNode::setupDevices() {
} catch (...) {
RCLCPP_ERROR_STREAM(logger_, "Failed to load device preset");
}
} else if (!device_preset_version_.empty()) {
RCLCPP_WARN_STREAM(logger_, "Ignore device_preset_version because device_preset is empty");
}
if (!preset_resolution_config_.empty()) {
@@ -1036,8 +1079,9 @@ void OBCameraNode::setupDevices() {
} else {
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_USB_SYNC_VOLTAGE_LEVEL_INT,
sync_io_voltage_level_);
RCLCPP_INFO_STREAM(logger_, "Current sync IO voltage level: " << device_->getIntProperty(
OB_PROP_USB_SYNC_VOLTAGE_LEVEL_INT));
TRY_EXECUTE_BLOCK(RCLCPP_INFO_STREAM(
logger_, "Current sync IO voltage level: "
<< device_->getIntProperty(OB_PROP_USB_SYNC_VOLTAGE_LEVEL_INT)));
}
}
if (monitor_poll_interval_sec_ != -1) {
@@ -1054,16 +1098,16 @@ void OBCameraNode::setupDevices() {
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_);
RCLCPP_INFO_STREAM(
logger_,
"Current heartbeat: " << (device_->getBoolProperty(OB_PROP_HEARTBEAT_BOOL) ? "ON" : "OFF"));
TRY_EXECUTE_BLOCK(RCLCPP_INFO_STREAM(
logger_, "Current heartbeat: "
<< (device_->getBoolProperty(OB_PROP_HEARTBEAT_BOOL) ? "ON" : "OFF")));
}
if (should_apply_launch_config("enable_fps_boost") &&
device_->isPropertySupported(OB_PROP_FPS_BOOST_BOOL, OB_PERMISSION_READ_WRITE)) {
TRY_TO_SET_PROPERTY(setBoolProperty, OB_PROP_FPS_BOOST_BOOL, enable_fps_boost_);
RCLCPP_INFO_STREAM(
logger_,
"Current fps boost: " << (device_->getBoolProperty(OB_PROP_FPS_BOOST_BOOL) ? "ON" : "OFF"));
TRY_EXECUTE_BLOCK(RCLCPP_INFO_STREAM(
logger_, "Current fps boost: "
<< (device_->getBoolProperty(OB_PROP_FPS_BOOST_BOOL) ? "ON" : "OFF")));
}
if (should_apply_launch_config("enable_firmware_log")) {
device_->enableFirmwareLog(enable_firmware_log_);
@@ -1072,14 +1116,14 @@ void OBCameraNode::setupDevices() {
if (max_depth_limit_ > 0 &&
device_->isPropertySupported(OB_PROP_MAX_DEPTH_INT, OB_PERMISSION_READ_WRITE)) {
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_MAX_DEPTH_INT, max_depth_limit_);
RCLCPP_INFO_STREAM(
logger_, "Current max depth limit: " << device_->getIntProperty(OB_PROP_MAX_DEPTH_INT));
TRY_EXECUTE_BLOCK(RCLCPP_INFO_STREAM(
logger_, "Current max depth limit: " << device_->getIntProperty(OB_PROP_MAX_DEPTH_INT)));
}
if (min_depth_limit_ > 0 &&
device_->isPropertySupported(OB_PROP_MIN_DEPTH_INT, OB_PERMISSION_READ_WRITE)) {
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_MIN_DEPTH_INT, min_depth_limit_);
RCLCPP_INFO_STREAM(
logger_, "Current min depth limit: " << device_->getIntProperty(OB_PROP_MIN_DEPTH_INT));
TRY_EXECUTE_BLOCK(RCLCPP_INFO_STREAM(
logger_, "Current min depth limit: " << device_->getIntProperty(OB_PROP_MIN_DEPTH_INT)));
}
if (laser_energy_level_ != -1 &&
device_->isPropertySupported(OB_PROP_LASER_ENERGY_LEVEL_INT, OB_PERMISSION_READ_WRITE)) {
@@ -1089,8 +1133,10 @@ void OBCameraNode::setupDevices() {
"Laser energy level is out of range " << range.min << " - " << range.max);
} else {
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_LASER_ENERGY_LEVEL_INT, laser_energy_level_);
auto new_laser_energy_level = device_->getIntProperty(OB_PROP_LASER_ENERGY_LEVEL_INT);
RCLCPP_INFO_STREAM(logger_, "Current energy level: " << new_laser_energy_level);
TRY_EXECUTE_BLOCK({
auto new_laser_energy_level = device_->getIntProperty(OB_PROP_LASER_ENERGY_LEVEL_INT);
RCLCPP_INFO_STREAM(logger_, "Current energy level: " << new_laser_energy_level);
});
}
}
if (depth_registration_ && align_mode_ == "SW") {
@@ -1102,22 +1148,42 @@ void OBCameraNode::setupDevices() {
sensors_.find(DEPTH) != sensors_.end() &&
device_->isPropertySupported(OB_PROP_DISPARITY_TO_DEPTH_BOOL, OB_PERMISSION_READ_WRITE) &&
device_->isPropertySupported(OB_PROP_SDK_DISPARITY_TO_DEPTH_BOOL, OB_PERMISSION_READ_WRITE)) {
if (disparity_to_depth_mode_ == "HW") {
device_->setBoolProperty(OB_PROP_DISPARITY_TO_DEPTH_BOOL, 1);
device_->setBoolProperty(OB_PROP_SDK_DISPARITY_TO_DEPTH_BOOL, 0);
RCLCPP_INFO_STREAM(logger_, "Disparity to depth mode: HW");
} else if (disparity_to_depth_mode_ == "SW") {
device_->setBoolProperty(OB_PROP_DISPARITY_TO_DEPTH_BOOL, 0);
device_->setBoolProperty(OB_PROP_SDK_DISPARITY_TO_DEPTH_BOOL, 1);
RCLCPP_INFO_STREAM(logger_, "Disparity to depth mode: SW");
} else if (disparity_to_depth_mode_ == "disable") {
device_->setBoolProperty(OB_PROP_DISPARITY_TO_DEPTH_BOOL, 0);
device_->setBoolProperty(OB_PROP_SDK_DISPARITY_TO_DEPTH_BOOL, 0);
RCLCPP_INFO_STREAM(logger_, "Disparity to depth mode: disabled");
} else {
RCLCPP_WARN_STREAM(logger_, "Unknown disparity to depth mode '"
<< disparity_to_depth_mode_ << "', keeping default settings");
}
TRY_EXECUTE_BLOCK({
bool expected_hardware_enabled = false;
bool expected_software_enabled = false;
bool mode_supported = true;
if (disparity_to_depth_mode_ == "HW") {
expected_hardware_enabled = true;
device_->setBoolProperty(OB_PROP_SDK_DISPARITY_TO_DEPTH_BOOL, false);
device_->setBoolProperty(OB_PROP_DISPARITY_TO_DEPTH_BOOL, true);
} else if (disparity_to_depth_mode_ == "SW") {
expected_software_enabled = true;
device_->setBoolProperty(OB_PROP_DISPARITY_TO_DEPTH_BOOL, false);
device_->setBoolProperty(OB_PROP_SDK_DISPARITY_TO_DEPTH_BOOL, true);
} else if (disparity_to_depth_mode_ == "disable") {
device_->setBoolProperty(OB_PROP_DISPARITY_TO_DEPTH_BOOL, false);
device_->setBoolProperty(OB_PROP_SDK_DISPARITY_TO_DEPTH_BOOL, false);
} else {
RCLCPP_WARN_STREAM(logger_, "Unknown disparity to depth mode '"
<< disparity_to_depth_mode_
<< "', keeping default settings");
mode_supported = false;
}
if (mode_supported) {
const bool hardware_enabled = device_->getBoolProperty(OB_PROP_DISPARITY_TO_DEPTH_BOOL);
const bool software_enabled = device_->getBoolProperty(OB_PROP_SDK_DISPARITY_TO_DEPTH_BOOL);
if (hardware_enabled != expected_hardware_enabled ||
software_enabled != expected_software_enabled) {
RCLCPP_ERROR_STREAM(logger_, "Failed to apply disparity to depth mode "
<< disparity_to_depth_mode_
<< ": device reports hardware=" << hardware_enabled
<< ", software=" << software_enabled);
} else {
RCLCPP_INFO_STREAM(logger_, "Disparity to depth mode: " << disparity_to_depth_mode_);
}
}
});
}
try {
if (should_apply_launch_config("enable_ldp") &&
@@ -1153,69 +1219,75 @@ void OBCameraNode::setupDevices() {
range.min, range.max);
} else {
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_LASER_POWER_LEVEL_CONTROL_INT, ldp_power_level_);
RCLCPP_INFO_STREAM(logger_, "Current lrm power level: " << device_->getIntProperty(
OB_PROP_LASER_POWER_LEVEL_CONTROL_INT));
TRY_EXECUTE_BLOCK(RCLCPP_INFO_STREAM(
logger_, "Current lrm power level: "
<< device_->getIntProperty(OB_PROP_LASER_POWER_LEVEL_CONTROL_INT)));
}
}
if (should_apply_launch_config("enable_laser") &&
device_->isPropertySupported(OB_PROP_LASER_CONTROL_INT, OB_PERMISSION_READ_WRITE)) {
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_LASER_CONTROL_INT, enable_laser_);
RCLCPP_INFO_STREAM(logger_,
"Current G300 laser control: "
<< (device_->getIntProperty(OB_PROP_LASER_CONTROL_INT) ? "ON" : "OFF"));
TRY_EXECUTE_BLOCK(RCLCPP_INFO_STREAM(
logger_, "Current G300 laser control: "
<< (device_->getIntProperty(OB_PROP_LASER_CONTROL_INT) ? "ON" : "OFF")));
}
if (should_apply_launch_config("enable_laser") &&
device_->isPropertySupported(OB_PROP_LASER_BOOL, OB_PERMISSION_READ_WRITE)) {
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_LASER_BOOL, enable_laser_);
RCLCPP_INFO_STREAM(
TRY_EXECUTE_BLOCK(RCLCPP_INFO_STREAM(
logger_,
"Current laser control: " << (device_->getIntProperty(OB_PROP_LASER_BOOL) ? "ON" : "OFF"));
"Current laser control: " << (device_->getIntProperty(OB_PROP_LASER_BOOL) ? "ON" : "OFF")));
}
if (!sync_mode_str_.empty()) {
auto sync_config = device_->getMultiDeviceSyncConfig();
std::transform(sync_mode_str_.begin(), sync_mode_str_.end(), sync_mode_str_.begin(), ::toupper);
sync_mode_ = OBSyncModeFromString(sync_mode_str_);
sync_config.syncMode = sync_mode_;
sync_config.depthDelayUs = depth_delay_us_;
sync_config.colorDelayUs = color_delay_us_;
sync_config.trigger2ImageDelayUs = trigger2image_delay_us_;
sync_config.triggerOutDelayUs = trigger_out_delay_us_;
sync_config.triggerOutEnable = trigger_out_enabled_;
sync_config.framesPerTrigger = frames_per_trigger_;
TRY_EXECUTE_BLOCK(device_->setMultiDeviceSyncConfig(sync_config));
sync_config = device_->getMultiDeviceSyncConfig();
RCLCPP_INFO_STREAM(logger_,
"Current sync mode: " << magic_enum::enum_name(sync_config.syncMode));
if (sync_mode_ == OB_MULTI_DEVICE_SYNC_MODE_SOFTWARE_TRIGGERING) {
RCLCPP_INFO_STREAM(logger_, "Frames per trigger: " << sync_config.framesPerTrigger);
TRY_EXECUTE_BLOCK({
auto sync_config = device_->getMultiDeviceSyncConfig();
std::transform(sync_mode_str_.begin(), sync_mode_str_.end(), sync_mode_str_.begin(),
::toupper);
sync_mode_ = OBSyncModeFromString(sync_mode_str_);
sync_config.syncMode = sync_mode_;
sync_config.depthDelayUs = depth_delay_us_;
sync_config.colorDelayUs = color_delay_us_;
sync_config.trigger2ImageDelayUs = trigger2image_delay_us_;
sync_config.triggerOutDelayUs = trigger_out_delay_us_;
sync_config.triggerOutEnable = trigger_out_enabled_;
sync_config.framesPerTrigger = frames_per_trigger_;
device_->setMultiDeviceSyncConfig(sync_config);
sync_config = device_->getMultiDeviceSyncConfig();
RCLCPP_INFO_STREAM(logger_,
"Software trigger period " << software_trigger_period_.count() << " ms");
software_trigger_timer_ = node_->create_wall_timer(software_trigger_period_, [this]() {
if (software_trigger_enabled_) {
TRY_EXECUTE_BLOCK(device_->triggerCapture());
}
});
}
"Current sync mode: " << magic_enum::enum_name(sync_config.syncMode));
if (sync_config.syncMode == OB_MULTI_DEVICE_SYNC_MODE_SOFTWARE_TRIGGERING) {
RCLCPP_INFO_STREAM(logger_, "Frames per trigger: " << sync_config.framesPerTrigger);
RCLCPP_INFO_STREAM(logger_,
"Software trigger period " << software_trigger_period_.count() << " ms");
software_trigger_timer_ = node_->create_wall_timer(software_trigger_period_, [this]() {
if (software_trigger_enabled_) {
TRY_EXECUTE_BLOCK(device_->triggerCapture());
}
});
}
});
}
if (should_apply_launch_config("enable_ptp_config") &&
device_->isPropertySupported(OB_DEVICE_PTP_CLOCK_SYNC_ENABLE_BOOL,
OB_PERMISSION_READ_WRITE)) {
device_->setBoolProperty(OB_DEVICE_PTP_CLOCK_SYNC_ENABLE_BOOL, enable_ptp_config_);
RCLCPP_INFO_STREAM(
TRY_TO_SET_PROPERTY(setBoolProperty, OB_DEVICE_PTP_CLOCK_SYNC_ENABLE_BOOL, enable_ptp_config_);
TRY_EXECUTE_BLOCK(RCLCPP_INFO_STREAM(
logger_, "Current PTP Config: "
<< (device_->getBoolProperty(OB_DEVICE_PTP_CLOCK_SYNC_ENABLE_BOOL) ? "ON"
: "OFF"));
: "OFF")));
}
if (device_->isPropertySupported(OB_PROP_DEPTH_PRECISION_LEVEL_INT, OB_PERMISSION_READ_WRITE) &&
!depth_precision_str_.empty()) {
auto default_precision_level = device_->getIntProperty(OB_PROP_DEPTH_PRECISION_LEVEL_INT);
if (default_precision_level != depth_precision_) {
device_->setIntProperty(OB_PROP_DEPTH_PRECISION_LEVEL_INT, depth_precision_);
const auto current_depth_precision =
device_->getIntProperty(OB_PROP_DEPTH_PRECISION_LEVEL_INT);
RCLCPP_INFO_STREAM(logger_, "Current depth precision: "
<< depthPrecisionLevelToString(current_depth_precision));
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_DEPTH_PRECISION_LEVEL_INT, depth_precision_);
TRY_EXECUTE_BLOCK({
const auto current_depth_precision =
device_->getIntProperty(OB_PROP_DEPTH_PRECISION_LEVEL_INT);
RCLCPP_INFO_STREAM(logger_, "Current depth precision: "
<< depthPrecisionLevelToString(current_depth_precision));
});
}
} else if (device_->isPropertySupported(OB_PROP_DEPTH_UNIT_FLEXIBLE_ADJUSTMENT_FLOAT,
OB_PERMISSION_READ_WRITE) &&
@@ -1230,10 +1302,10 @@ void OBCameraNode::setupDevices() {
} else {
TRY_TO_SET_PROPERTY(setFloatProperty, OB_PROP_DEPTH_UNIT_FLEXIBLE_ADJUSTMENT_FLOAT,
depth_unit_flexible_adjustment);
RCLCPP_INFO_STREAM(
TRY_EXECUTE_BLOCK(RCLCPP_INFO_STREAM(
logger_, "Current depth unit: "
<< device_->getFloatProperty(OB_PROP_DEPTH_UNIT_FLEXIBLE_ADJUSTMENT_FLOAT)
<< "mm");
<< "mm"));
}
}
@@ -1258,9 +1330,9 @@ void OBCameraNode::setupDevices() {
if (should_apply_launch_config(stream_name_[stream_index] + "_mirror") &&
device_->isPropertySupported(mirrorPropertyID, OB_PERMISSION_WRITE)) {
TRY_TO_SET_PROPERTY(setBoolProperty, mirrorPropertyID, mirror_stream_[stream_index]);
RCLCPP_INFO_STREAM(
TRY_EXECUTE_BLOCK(RCLCPP_INFO_STREAM(
logger_, "Current " << stream_name_[stream_index] << " mirror: "
<< (device_->getBoolProperty(mirrorPropertyID) ? "ON" : "OFF"));
<< (device_->getBoolProperty(mirrorPropertyID) ? "ON" : "OFF")));
}
OBPropertyID flipPropertyID = OB_PROP_DEPTH_FLIP_BOOL;
if (stream_index == COLOR) {
@@ -1281,9 +1353,9 @@ void OBCameraNode::setupDevices() {
if (should_apply_launch_config(stream_name_[stream_index] + "_flip") &&
device_->isPropertySupported(flipPropertyID, OB_PERMISSION_WRITE)) {
TRY_TO_SET_PROPERTY(setBoolProperty, flipPropertyID, flip_stream_[stream_index]);
RCLCPP_INFO_STREAM(logger_,
"Current " << stream_name_[stream_index] << " flip: "
<< (device_->getBoolProperty(flipPropertyID) ? "ON" : "OFF"));
TRY_EXECUTE_BLOCK(RCLCPP_INFO_STREAM(
logger_, "Current " << stream_name_[stream_index] << " flip: "
<< (device_->getBoolProperty(flipPropertyID) ? "ON" : "OFF")));
}
OBPropertyID rotationPropertyID = OB_PROP_DEPTH_ROTATE_INT;
if (stream_index == COLOR) {
@@ -1304,8 +1376,9 @@ void OBCameraNode::setupDevices() {
if (rotation_stream_[stream_index] != -1 &&
device_->isPropertySupported(rotationPropertyID, OB_PERMISSION_WRITE)) {
TRY_TO_SET_PROPERTY(setIntProperty, rotationPropertyID, rotation_stream_[stream_index]);
RCLCPP_INFO_STREAM(logger_, "Current " << stream_name_[stream_index] << " rotation: "
<< device_->getIntProperty(rotationPropertyID));
TRY_EXECUTE_BLOCK(RCLCPP_INFO_STREAM(
logger_, "Current " << stream_name_[stream_index]
<< " rotation: " << device_->getIntProperty(rotationPropertyID)));
}
}
}
@@ -1314,10 +1387,10 @@ void OBCameraNode::setupDevices() {
device_->isPropertySupported(OB_PROP_COLOR_AUTO_WHITE_BALANCE_BOOL, OB_PERMISSION_WRITE)) {
TRY_TO_SET_PROPERTY(setBoolProperty, OB_PROP_COLOR_AUTO_WHITE_BALANCE_BOOL,
enable_color_auto_white_balance_);
RCLCPP_INFO_STREAM(
TRY_EXECUTE_BLOCK(RCLCPP_INFO_STREAM(
logger_,
"Current color auto white balance: "
<< (device_->getBoolProperty(OB_PROP_COLOR_AUTO_WHITE_BALANCE_BOOL) ? "ON" : "OFF"));
<< (device_->getBoolProperty(OB_PROP_COLOR_AUTO_WHITE_BALANCE_BOOL) ? "ON" : "OFF")));
}
if (should_apply_launch_config("color_preset") && !color_preset_.empty()) {
try {
@@ -1370,8 +1443,9 @@ void OBCameraNode::setupDevices() {
range.min, range.max);
} else {
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_COLOR_EXPOSURE_INT, color_exposure_);
RCLCPP_INFO_STREAM(logger_, "Current color exposure: "
<< device_->getIntProperty(OB_PROP_COLOR_EXPOSURE_INT));
TRY_EXECUTE_BLOCK(RCLCPP_INFO_STREAM(
logger_,
"Current color exposure: " << device_->getIntProperty(OB_PROP_COLOR_EXPOSURE_INT)));
}
}
if (color_gain_ != -1 &&
@@ -1382,8 +1456,8 @@ void OBCameraNode::setupDevices() {
range.min, range.max);
} else {
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_COLOR_GAIN_INT, color_gain_);
RCLCPP_INFO_STREAM(logger_,
"Current color gain: " << device_->getIntProperty(OB_PROP_COLOR_GAIN_INT));
TRY_EXECUTE_BLOCK(RCLCPP_INFO_STREAM(
logger_, "Current color gain: " << device_->getIntProperty(OB_PROP_COLOR_GAIN_INT)));
}
}
if (color_mjpeg_quality_ != -1) {
@@ -1397,8 +1471,9 @@ void OBCameraNode::setupDevices() {
range.min, range.max);
} else {
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_MJPEG_QUALITY_INT, color_mjpeg_quality_);
RCLCPP_INFO_STREAM(logger_, "Current color MJPEG quality: "
<< device_->getIntProperty(OB_PROP_MJPEG_QUALITY_INT));
TRY_EXECUTE_BLOCK(RCLCPP_INFO_STREAM(
logger_,
"Current color MJPEG quality: " << device_->getIntProperty(OB_PROP_MJPEG_QUALITY_INT)));
}
}
}
@@ -1407,19 +1482,19 @@ void OBCameraNode::setupDevices() {
int set_enable_color_auto_exposure_priority = enable_color_auto_exposure_priority_ ? 1 : 0;
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_COLOR_AUTO_EXPOSURE_PRIORITY_INT,
set_enable_color_auto_exposure_priority);
RCLCPP_INFO_STREAM(
TRY_EXECUTE_BLOCK(RCLCPP_INFO_STREAM(
logger_,
"Current color auto exposure priority: "
<< (device_->getIntProperty(OB_PROP_COLOR_AUTO_EXPOSURE_PRIORITY_INT) ? "ON" : "OFF"));
<< (device_->getIntProperty(OB_PROP_COLOR_AUTO_EXPOSURE_PRIORITY_INT) ? "ON" : "OFF")));
}
if (should_apply_launch_config("enable_color_auto_exposure") &&
device_->isPropertySupported(OB_PROP_COLOR_AUTO_EXPOSURE_BOOL, OB_PERMISSION_WRITE)) {
TRY_TO_SET_PROPERTY(setBoolProperty, OB_PROP_COLOR_AUTO_EXPOSURE_BOOL,
enable_color_auto_exposure_);
RCLCPP_INFO_STREAM(
TRY_EXECUTE_BLOCK(RCLCPP_INFO_STREAM(
logger_,
"Current color auto exposure: "
<< (device_->getBoolProperty(OB_PROP_COLOR_AUTO_EXPOSURE_BOOL) ? "ON" : "OFF"));
<< (device_->getBoolProperty(OB_PROP_COLOR_AUTO_EXPOSURE_BOOL) ? "ON" : "OFF")));
}
if (color_white_balance_ != -1 &&
device_->isPropertySupported(OB_PROP_COLOR_WHITE_BALANCE_INT, OB_PERMISSION_WRITE)) {
@@ -1430,8 +1505,9 @@ void OBCameraNode::setupDevices() {
range.min, range.max);
} else {
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_COLOR_WHITE_BALANCE_INT, color_white_balance_);
RCLCPP_INFO_STREAM(logger_, "Current color white balance: "
<< device_->getIntProperty(OB_PROP_COLOR_WHITE_BALANCE_INT));
TRY_EXECUTE_BLOCK(RCLCPP_INFO_STREAM(
logger_, "Current color white balance: "
<< device_->getIntProperty(OB_PROP_COLOR_WHITE_BALANCE_INT)));
}
}
@@ -1445,8 +1521,9 @@ void OBCameraNode::setupDevices() {
} else {
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_COLOR_AE_MAX_EXPOSURE_INT,
color_ae_max_exposure_);
RCLCPP_INFO_STREAM(logger_, "Current color AE max exposure: " << device_->getIntProperty(
OB_PROP_COLOR_AE_MAX_EXPOSURE_INT));
TRY_EXECUTE_BLOCK(RCLCPP_INFO_STREAM(
logger_, "Current color AE max exposure: "
<< device_->getIntProperty(OB_PROP_COLOR_AE_MAX_EXPOSURE_INT)));
}
}
if (color_ae_max_gain_ != -1 &&
@@ -1458,8 +1535,9 @@ void OBCameraNode::setupDevices() {
range.min, range.max);
} else {
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_COLOR_AE_MAX_GAIN_INT, color_ae_max_gain_);
RCLCPP_INFO_STREAM(logger_, "Current color AE max gain: "
<< device_->getIntProperty(OB_PROP_COLOR_AE_MAX_GAIN_INT));
TRY_EXECUTE_BLOCK(RCLCPP_INFO_STREAM(
logger_,
"Current color AE max gain: " << device_->getIntProperty(OB_PROP_COLOR_AE_MAX_GAIN_INT)));
}
}
if (color_brightness_ != -1 &&
@@ -1470,8 +1548,9 @@ void OBCameraNode::setupDevices() {
range.min, range.max);
} else {
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_COLOR_BRIGHTNESS_INT, color_brightness_);
RCLCPP_INFO_STREAM(logger_, "Current color brightness: "
<< device_->getIntProperty(OB_PROP_COLOR_BRIGHTNESS_INT));
TRY_EXECUTE_BLOCK(RCLCPP_INFO_STREAM(
logger_,
"Current color brightness: " << device_->getIntProperty(OB_PROP_COLOR_BRIGHTNESS_INT)));
}
}
if (color_roi_brightness_ != -1 &&
@@ -1483,8 +1562,9 @@ void OBCameraNode::setupDevices() {
range.min, range.max);
} else {
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_COLOR_ROI_BRIGHTNESS_INT, color_roi_brightness_);
RCLCPP_INFO_STREAM(logger_, "Current color roi brightness: "
<< device_->getIntProperty(OB_PROP_COLOR_ROI_BRIGHTNESS_INT));
TRY_EXECUTE_BLOCK(RCLCPP_INFO_STREAM(
logger_, "Current color roi brightness: "
<< device_->getIntProperty(OB_PROP_COLOR_ROI_BRIGHTNESS_INT)));
}
}
if (color_sharpness_ != -1 &&
@@ -1495,8 +1575,9 @@ void OBCameraNode::setupDevices() {
range.min, range.max);
} else {
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_COLOR_SHARPNESS_INT, color_sharpness_);
RCLCPP_INFO_STREAM(logger_, "Current color sharpness: "
<< device_->getIntProperty(OB_PROP_COLOR_SHARPNESS_INT));
TRY_EXECUTE_BLOCK(RCLCPP_INFO_STREAM(
logger_,
"Current color sharpness: " << device_->getIntProperty(OB_PROP_COLOR_SHARPNESS_INT)));
}
}
if (color_gamma_ != -1 &&
@@ -1507,8 +1588,8 @@ void OBCameraNode::setupDevices() {
range.min, range.max);
} else {
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_COLOR_GAMMA_INT, color_gamma_);
RCLCPP_INFO_STREAM(
logger_, "Current color gamma: " << device_->getIntProperty(OB_PROP_COLOR_GAMMA_INT));
TRY_EXECUTE_BLOCK(RCLCPP_INFO_STREAM(
logger_, "Current color gamma: " << device_->getIntProperty(OB_PROP_COLOR_GAMMA_INT)));
}
}
if (color_saturation_ != -1 &&
@@ -1519,8 +1600,9 @@ void OBCameraNode::setupDevices() {
range.min, range.max);
} else {
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_COLOR_SATURATION_INT, color_saturation_);
RCLCPP_INFO_STREAM(logger_, "Current color saturation: "
<< device_->getIntProperty(OB_PROP_COLOR_SATURATION_INT));
TRY_EXECUTE_BLOCK(RCLCPP_INFO_STREAM(
logger_,
"Current color saturation: " << device_->getIntProperty(OB_PROP_COLOR_SATURATION_INT)));
}
}
if (color_contrast_ != -1 &&
@@ -1531,8 +1613,9 @@ void OBCameraNode::setupDevices() {
range.min, range.max);
} else {
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_COLOR_CONTRAST_INT, color_contrast_);
RCLCPP_INFO_STREAM(logger_, "Current color contrast: "
<< device_->getIntProperty(OB_PROP_COLOR_CONTRAST_INT));
TRY_EXECUTE_BLOCK(RCLCPP_INFO_STREAM(
logger_,
"Current color contrast: " << device_->getIntProperty(OB_PROP_COLOR_CONTRAST_INT)));
}
}
if (color_hue_ != -1 &&
@@ -1543,29 +1626,32 @@ void OBCameraNode::setupDevices() {
range.min, range.max);
} else {
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_COLOR_HUE_INT, color_hue_);
RCLCPP_INFO_STREAM(logger_,
"Current color hue: " << device_->getIntProperty(OB_PROP_COLOR_HUE_INT));
TRY_EXECUTE_BLOCK(RCLCPP_INFO_STREAM(
logger_, "Current color hue: " << device_->getIntProperty(OB_PROP_COLOR_HUE_INT)));
}
}
if (color_backlight_compensation_ != -1 &&
device_->isPropertySupported(OB_PROP_COLOR_BACKLIGHT_COMPENSATION_INT, OB_PERMISSION_WRITE)) {
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_COLOR_BACKLIGHT_COMPENSATION_INT,
color_backlight_compensation_);
RCLCPP_INFO_STREAM(logger_, "Current color backlight compensation: " << device_->getIntProperty(
OB_PROP_COLOR_BACKLIGHT_COMPENSATION_INT));
TRY_EXECUTE_BLOCK(RCLCPP_INFO_STREAM(
logger_, "Current color backlight compensation: "
<< device_->getIntProperty(OB_PROP_COLOR_BACKLIGHT_COMPENSATION_INT)));
}
if (color_denoising_level_ != -1 &&
device_->isPropertySupported(OB_PROP_COLOR_DENOISING_LEVEL_INT, OB_PERMISSION_WRITE)) {
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_COLOR_DENOISING_LEVEL_INT, color_denoising_level_);
RCLCPP_INFO_STREAM(logger_, "Current color denoising level: "
<< device_->getIntProperty(OB_PROP_COLOR_DENOISING_LEVEL_INT));
TRY_EXECUTE_BLOCK(RCLCPP_INFO_STREAM(
logger_, "Current color denoising level: "
<< device_->getIntProperty(OB_PROP_COLOR_DENOISING_LEVEL_INT)));
}
if (should_apply_launch_config("color_anti_flicker") &&
device_->isPropertySupported(OB_PROP_COLOR_ANTI_FLICKER_BOOL, OB_PERMISSION_WRITE)) {
TRY_TO_SET_PROPERTY(setBoolProperty, OB_PROP_COLOR_ANTI_FLICKER_BOOL, color_anti_flicker_);
RCLCPP_INFO_STREAM(
logger_, "Current color anti-flicker to "
<< (device_->getBoolProperty(OB_PROP_COLOR_ANTI_FLICKER_BOOL) ? "ON" : "OFF"));
TRY_EXECUTE_BLOCK(RCLCPP_INFO_STREAM(
logger_,
"Current color anti-flicker to "
<< (device_->getBoolProperty(OB_PROP_COLOR_ANTI_FLICKER_BOOL) ? "ON" : "OFF")));
}
if (!color_powerline_freq_.empty() &&
device_->isPropertySupported(OB_PROP_COLOR_POWER_LINE_FREQUENCY_INT, OB_PERMISSION_WRITE)) {
@@ -1578,10 +1664,13 @@ void OBCameraNode::setupDevices() {
} else if (color_powerline_freq_ == "auto") {
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_COLOR_POWER_LINE_FREQUENCY_INT, 3);
}
const auto current_color_powerline_freq =
device_->getIntProperty(OB_PROP_COLOR_POWER_LINE_FREQUENCY_INT);
RCLCPP_INFO_STREAM(logger_, "Current color powerline freq: " << colorPowerLineFrequencyToString(
current_color_powerline_freq));
TRY_EXECUTE_BLOCK({
const auto current_color_powerline_freq =
device_->getIntProperty(OB_PROP_COLOR_POWER_LINE_FREQUENCY_INT);
RCLCPP_INFO_STREAM(logger_,
"Current color powerline freq: "
<< colorPowerLineFrequencyToString(current_color_powerline_freq));
});
}
if (depth_exposure_ != -1 &&
device_->isPropertySupported(OB_PROP_DEPTH_EXPOSURE_INT, OB_PERMISSION_WRITE)) {
@@ -1591,8 +1680,9 @@ void OBCameraNode::setupDevices() {
range.min, range.max);
} else {
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_DEPTH_EXPOSURE_INT, depth_exposure_);
RCLCPP_INFO_STREAM(logger_, "Current depth exposure: "
<< device_->getIntProperty(OB_PROP_DEPTH_EXPOSURE_INT));
TRY_EXECUTE_BLOCK(RCLCPP_INFO_STREAM(
logger_,
"Current depth exposure: " << device_->getIntProperty(OB_PROP_DEPTH_EXPOSURE_INT)));
}
}
if (depth_gain_ != -1 &&
@@ -1603,8 +1693,8 @@ void OBCameraNode::setupDevices() {
range.min, range.max);
} else {
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_DEPTH_GAIN_INT, depth_gain_);
RCLCPP_INFO_STREAM(logger_,
"Current depth gain: " << device_->getIntProperty(OB_PROP_DEPTH_GAIN_INT));
TRY_EXECUTE_BLOCK(RCLCPP_INFO_STREAM(
logger_, "Current depth gain: " << device_->getIntProperty(OB_PROP_DEPTH_GAIN_INT)));
}
}
if (should_apply_launch_config("enable_depth_auto_exposure_priority") &&
@@ -1613,17 +1703,17 @@ void OBCameraNode::setupDevices() {
int set_enable_depth_auto_exposure_priority = enable_depth_auto_exposure_priority_ ? 1 : 0;
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_DEPTH_AUTO_EXPOSURE_PRIORITY_INT,
set_enable_depth_auto_exposure_priority);
RCLCPP_INFO_STREAM(
TRY_EXECUTE_BLOCK(RCLCPP_INFO_STREAM(
logger_,
"Current depth auto exposure priority: "
<< (device_->getIntProperty(OB_PROP_DEPTH_AUTO_EXPOSURE_PRIORITY_INT) ? "ON" : "OFF"));
<< (device_->getIntProperty(OB_PROP_DEPTH_AUTO_EXPOSURE_PRIORITY_INT) ? "ON" : "OFF")));
}
if (should_apply_launch_config("enable_ir_auto_exposure") &&
device_->isPropertySupported(OB_PROP_IR_AUTO_EXPOSURE_BOOL, OB_PERMISSION_WRITE)) {
TRY_TO_SET_PROPERTY(setBoolProperty, OB_PROP_IR_AUTO_EXPOSURE_BOOL, enable_ir_auto_exposure_);
RCLCPP_INFO_STREAM(
TRY_EXECUTE_BLOCK(RCLCPP_INFO_STREAM(
logger_, "Current IR auto exposure: "
<< (device_->getBoolProperty(OB_PROP_IR_AUTO_EXPOSURE_BOOL) ? "ON" : "OFF"));
<< (device_->getBoolProperty(OB_PROP_IR_AUTO_EXPOSURE_BOOL) ? "ON" : "OFF")));
}
if (mean_intensity_set_point_ != -1 &&
device_->isPropertySupported(OB_PROP_IR_BRIGHTNESS_INT, OB_PERMISSION_WRITE)) {
@@ -1633,8 +1723,9 @@ void OBCameraNode::setupDevices() {
range.min, range.max);
} else {
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_IR_BRIGHTNESS_INT, mean_intensity_set_point_);
RCLCPP_INFO_STREAM(logger_, "Current depth brightness: "
<< device_->getIntProperty(OB_PROP_IR_BRIGHTNESS_INT));
TRY_EXECUTE_BLOCK(RCLCPP_INFO_STREAM(
logger_,
"Current depth brightness: " << device_->getIntProperty(OB_PROP_IR_BRIGHTNESS_INT)));
}
}
// ir ae max
@@ -1647,8 +1738,9 @@ void OBCameraNode::setupDevices() {
range.min, range.max);
} else {
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_IR_AE_MAX_EXPOSURE_INT, ir_ae_max_exposure_);
RCLCPP_INFO_STREAM(logger_, "Current IR AE max exposure: "
<< device_->getIntProperty(OB_PROP_IR_AE_MAX_EXPOSURE_INT));
TRY_EXECUTE_BLOCK(RCLCPP_INFO_STREAM(
logger_, "Current IR AE max exposure: "
<< device_->getIntProperty(OB_PROP_IR_AE_MAX_EXPOSURE_INT)));
}
}
// ir brightness
@@ -1660,8 +1752,9 @@ void OBCameraNode::setupDevices() {
range.min, range.max);
} else {
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_IR_BRIGHTNESS_INT, ir_brightness_);
RCLCPP_INFO_STREAM(
logger_, "Current IR brightness: " << device_->getIntProperty(OB_PROP_IR_BRIGHTNESS_INT));
TRY_EXECUTE_BLOCK(RCLCPP_INFO_STREAM(
logger_,
"Current IR brightness: " << device_->getIntProperty(OB_PROP_IR_BRIGHTNESS_INT)));
}
}
if (ir_exposure_ != -1 &&
@@ -1672,8 +1765,8 @@ void OBCameraNode::setupDevices() {
range.min, range.max);
} else {
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_IR_EXPOSURE_INT, ir_exposure_);
RCLCPP_INFO_STREAM(
logger_, "Current IR exposure: " << device_->getIntProperty(OB_PROP_IR_EXPOSURE_INT));
TRY_EXECUTE_BLOCK(RCLCPP_INFO_STREAM(
logger_, "Current IR exposure: " << device_->getIntProperty(OB_PROP_IR_EXPOSURE_INT)));
}
}
if (ir_gain_ != -1 && device_->isPropertySupported(OB_PROP_IR_GAIN_INT, OB_PERMISSION_WRITE)) {
@@ -1683,16 +1776,16 @@ void OBCameraNode::setupDevices() {
range.min, range.max);
} else {
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_IR_GAIN_INT, ir_gain_);
RCLCPP_INFO_STREAM(logger_,
"Current IR gain: " << device_->getIntProperty(OB_PROP_IR_GAIN_INT));
TRY_EXECUTE_BLOCK(RCLCPP_INFO_STREAM(
logger_, "Current IR gain: " << device_->getIntProperty(OB_PROP_IR_GAIN_INT)));
}
}
if (should_apply_launch_config("enable_ir_long_exposure") &&
device_->isPropertySupported(OB_PROP_IR_LONG_EXPOSURE_BOOL, OB_PERMISSION_WRITE)) {
TRY_TO_SET_PROPERTY(setBoolProperty, OB_PROP_IR_LONG_EXPOSURE_BOOL, enable_ir_long_exposure_);
RCLCPP_INFO_STREAM(
TRY_EXECUTE_BLOCK(RCLCPP_INFO_STREAM(
logger_, "Current IR long exposure: "
<< (device_->getBoolProperty(OB_PROP_IR_LONG_EXPOSURE_BOOL) ? "ON" : "OFF"));
<< (device_->getBoolProperty(OB_PROP_IR_LONG_EXPOSURE_BOOL) ? "ON" : "OFF")));
}
if (should_apply_launch_config("enable_noise_removal_filter") && enable_noise_removal_filter_ &&
@@ -1710,11 +1803,13 @@ void OBCameraNode::setupDevices() {
"the value",
range.min, range.max);
} else {
device_->setIntProperty(OB_PROP_DEPTH_MAX_DIFF_INT, noise_removal_filter_min_diff_);
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_DEPTH_MAX_DIFF_INT,
noise_removal_filter_min_diff_);
}
}
RCLCPP_INFO_STREAM(logger_, "Current noise_removal_filter_min_diff: "
<< device_->getIntProperty(OB_PROP_DEPTH_MAX_DIFF_INT));
TRY_EXECUTE_BLOCK(
RCLCPP_INFO_STREAM(logger_, "Current noise_removal_filter_min_diff: "
<< device_->getIntProperty(OB_PROP_DEPTH_MAX_DIFF_INT)));
}
if (should_apply_launch_config("enable_noise_removal_filter") && enable_noise_removal_filter_ &&
@@ -1732,34 +1827,38 @@ void OBCameraNode::setupDevices() {
"the value",
range.min, range.max);
} else {
device_->setIntProperty(OB_PROP_DEPTH_MAX_SPECKLE_SIZE_INT, noise_removal_filter_max_size_);
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_DEPTH_MAX_SPECKLE_SIZE_INT,
noise_removal_filter_max_size_);
}
}
RCLCPP_INFO_STREAM(logger_, "Current noise_removal_filter_max_size: "
<< device_->getIntProperty(OB_PROP_DEPTH_MAX_SPECKLE_SIZE_INT));
TRY_EXECUTE_BLOCK(RCLCPP_INFO_STREAM(
logger_, "Current noise_removal_filter_max_size: "
<< device_->getIntProperty(OB_PROP_DEPTH_MAX_SPECKLE_SIZE_INT)));
}
if (should_apply_launch_config("enable_noise_removal_filter") &&
sensors_.find(DEPTH) != sensors_.end() &&
device_->isPropertySupported(OB_PROP_DEPTH_SOFT_FILTER_BOOL, OB_PERMISSION_READ_WRITE)) {
device_->setBoolProperty(OB_PROP_DEPTH_SOFT_FILTER_BOOL, enable_noise_removal_filter_);
TRY_TO_SET_PROPERTY(setBoolProperty, OB_PROP_DEPTH_SOFT_FILTER_BOOL,
enable_noise_removal_filter_);
RCLCPP_INFO_STREAM(logger_, "Set noise removal filter to "
<< (enable_noise_removal_filter_ ? "true" : "false"));
}
if (should_apply_launch_config("enable_disp_outliers_filter") &&
sensors_.find(DEPTH) != sensors_.end() &&
device_->isPropertySupported(OB_PROP_DEPTH_OUTLIERS_FILTER_BOOL, OB_PERMISSION_READ_WRITE)) {
device_->setBoolProperty(OB_PROP_DEPTH_OUTLIERS_FILTER_BOOL, enable_disp_outliers_filter_);
TRY_TO_SET_PROPERTY(setBoolProperty, OB_PROP_DEPTH_OUTLIERS_FILTER_BOOL,
enable_disp_outliers_filter_);
RCLCPP_INFO_STREAM(
logger_, "Set DispOutliersFilter to " << (enable_disp_outliers_filter_ ? "true" : "false"));
}
if (disp_outliers_filter_search_mode_ != -1 && sensors_.find(DEPTH) != sensors_.end() &&
device_->isPropertySupported(OB_PROP_DEPTH_OUTLIERS_FILTER_SEARCH_MODE_INT,
OB_PERMISSION_READ_WRITE)) {
device_->setIntProperty(OB_PROP_DEPTH_OUTLIERS_FILTER_SEARCH_MODE_INT,
disp_outliers_filter_search_mode_);
RCLCPP_INFO_STREAM(
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_DEPTH_OUTLIERS_FILTER_SEARCH_MODE_INT,
disp_outliers_filter_search_mode_);
TRY_EXECUTE_BLOCK(RCLCPP_INFO_STREAM(
logger_, "Current DispOutliersFilter search mode: "
<< device_->getIntProperty(OB_PROP_DEPTH_OUTLIERS_FILTER_SEARCH_MODE_INT));
<< device_->getIntProperty(OB_PROP_DEPTH_OUTLIERS_FILTER_SEARCH_MODE_INT)));
}
if (disparity_range_mode_ != -1 &&
device_->isPropertySupported(OB_PROP_DISP_SEARCH_RANGE_MODE_INT, OB_PERMISSION_WRITE)) {
@@ -1772,30 +1871,33 @@ void OBCameraNode::setupDevices() {
} else {
RCLCPP_ERROR(logger_, "disparity range mode does not support this setting");
}
const auto current_disparity_range_mode =
device_->getIntProperty(OB_PROP_DISP_SEARCH_RANGE_MODE_INT);
RCLCPP_INFO_STREAM(logger_, "Current disparity range mode: "
<< disparityRangeModeToString(current_disparity_range_mode));
TRY_EXECUTE_BLOCK({
const auto current_disparity_range_mode =
device_->getIntProperty(OB_PROP_DISP_SEARCH_RANGE_MODE_INT);
RCLCPP_INFO_STREAM(logger_, "Current disparity range mode: "
<< disparityRangeModeToString(current_disparity_range_mode));
});
}
if (should_apply_launch_config("enable_hardware_noise_removal_filter") &&
device_->isPropertySupported(OB_PROP_HW_NOISE_REMOVE_FILTER_ENABLE_BOOL,
OB_PERMISSION_READ_WRITE)) {
device_->setBoolProperty(OB_PROP_HW_NOISE_REMOVE_FILTER_ENABLE_BOOL,
enable_hardware_noise_removal_filter_);
RCLCPP_INFO_STREAM(
TRY_TO_SET_PROPERTY(setBoolProperty, OB_PROP_HW_NOISE_REMOVE_FILTER_ENABLE_BOOL,
enable_hardware_noise_removal_filter_);
TRY_EXECUTE_BLOCK(RCLCPP_INFO_STREAM(
logger_,
"Set hardware noise removal filter to "
<< (device_->getBoolProperty(OB_PROP_HW_NOISE_REMOVE_FILTER_ENABLE_BOOL) ? "true"
: "false"));
: "false")));
if (device_->isPropertySupported(OB_PROP_HW_NOISE_REMOVE_FILTER_THRESHOLD_FLOAT,
OB_PERMISSION_READ_WRITE)) {
if (hardware_noise_removal_filter_threshold_ != -1.0 &&
enable_hardware_noise_removal_filter_) {
device_->setFloatProperty(OB_PROP_HW_NOISE_REMOVE_FILTER_THRESHOLD_FLOAT,
hardware_noise_removal_filter_threshold_);
RCLCPP_INFO_STREAM(logger_, "Current hardware noise removal filter threshold: "
<< device_->getFloatProperty(
OB_PROP_HW_NOISE_REMOVE_FILTER_THRESHOLD_FLOAT));
TRY_TO_SET_PROPERTY(setFloatProperty, OB_PROP_HW_NOISE_REMOVE_FILTER_THRESHOLD_FLOAT,
hardware_noise_removal_filter_threshold_);
TRY_EXECUTE_BLOCK(RCLCPP_INFO_STREAM(
logger_,
"Current hardware noise removal filter threshold: "
<< device_->getFloatProperty(OB_PROP_HW_NOISE_REMOVE_FILTER_THRESHOLD_FLOAT)));
}
}
}
@@ -1808,30 +1910,33 @@ void OBCameraNode::setupDevices() {
} else {
RCLCPP_ERROR(logger_, "exposure range mode does not support this setting");
}
const auto current_exposure_range_mode =
device_->getIntProperty(OB_PROP_DEVICE_PERFORMANCE_MODE_INT);
RCLCPP_INFO_STREAM(logger_, "Current exposure range mode: "
<< exposureRangeModeToString(current_exposure_range_mode));
TRY_EXECUTE_BLOCK({
const auto current_exposure_range_mode =
device_->getIntProperty(OB_PROP_DEVICE_PERFORMANCE_MODE_INT);
RCLCPP_INFO_STREAM(logger_, "Current exposure range mode: "
<< exposureRangeModeToString(current_exposure_range_mode));
});
}
if (should_apply_launch_config("enable_accel_data_correction") &&
device_->isPropertySupported(OB_PROP_SDK_ACCEL_FRAME_TRANSFORMED_BOOL, OB_PERMISSION_WRITE)) {
TRY_TO_SET_PROPERTY(setBoolProperty, OB_PROP_SDK_ACCEL_FRAME_TRANSFORMED_BOOL,
enable_accel_data_correction_);
RCLCPP_INFO_STREAM(
TRY_EXECUTE_BLOCK(RCLCPP_INFO_STREAM(
logger_,
"Current accel data correction: "
<< (device_->getBoolProperty(OB_PROP_SDK_ACCEL_FRAME_TRANSFORMED_BOOL) ? "ON" : "OFF"));
<< (device_->getBoolProperty(OB_PROP_SDK_ACCEL_FRAME_TRANSFORMED_BOOL) ? "ON"
: "OFF")));
}
if (should_apply_launch_config("enable_gyro_data_correction") &&
device_->isPropertySupported(OB_PROP_SDK_GYRO_FRAME_TRANSFORMED_BOOL, OB_PERMISSION_WRITE)) {
TRY_TO_SET_PROPERTY(setBoolProperty, OB_PROP_SDK_GYRO_FRAME_TRANSFORMED_BOOL,
enable_gyro_data_correction_);
RCLCPP_INFO_STREAM(
TRY_EXECUTE_BLOCK(RCLCPP_INFO_STREAM(
logger_,
"Current gyro data correction: "
<< (device_->getBoolProperty(OB_PROP_SDK_GYRO_FRAME_TRANSFORMED_BOOL) ? "ON" : "OFF"));
<< (device_->getBoolProperty(OB_PROP_SDK_GYRO_FRAME_TRANSFORMED_BOOL) ? "ON" : "OFF")));
}
if (isGemini335PID(pid_) && !intra_camera_sync_reference_.empty() &&
if (!intra_camera_sync_reference_.empty() &&
device_->isPropertySupported(OB_PROP_INTRA_CAMERA_SYNC_REFERENCE_INT, OB_PERMISSION_WRITE)) {
if (intra_camera_sync_reference_ == "Start") {
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_INTRA_CAMERA_SYNC_REFERENCE_INT, 0);
@@ -1842,19 +1947,22 @@ void OBCameraNode::setupDevices() {
} else {
RCLCPP_ERROR(logger_, "intra camera sync reference does not support this setting");
}
const auto current_intra_camera_sync_reference =
device_->getIntProperty(OB_PROP_INTRA_CAMERA_SYNC_REFERENCE_INT);
RCLCPP_INFO_STREAM(
logger_, "Current intra camera sync reference: "
<< intraCameraSyncReferenceToString(current_intra_camera_sync_reference));
TRY_EXECUTE_BLOCK({
const auto current_intra_camera_sync_reference =
device_->getIntProperty(OB_PROP_INTRA_CAMERA_SYNC_REFERENCE_INT);
RCLCPP_INFO_STREAM(
logger_, "Current intra camera sync reference: "
<< intraCameraSyncReferenceToString(current_intra_camera_sync_reference));
});
}
if (should_apply_launch_config("ae_strategy") &&
device_->isPropertySupported(OB_PROP_DEVICE_AE_STRATEGY_INT, OB_PERMISSION_WRITE)) {
device_->setIntProperty(OB_PROP_DEVICE_AE_STRATEGY_INT, (ae_strategy_ == "motion" ? 0 : 1));
RCLCPP_INFO_STREAM(
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_DEVICE_AE_STRATEGY_INT,
(ae_strategy_ == "motion" ? 1 : 0));
TRY_EXECUTE_BLOCK(RCLCPP_INFO_STREAM(
logger_, "Current Sports Mode: "
<< (device_->getIntProperty(OB_PROP_DEVICE_AE_STRATEGY_INT) == 0 ? "ON"
: "OFF"));
<< (device_->getIntProperty(OB_PROP_DEVICE_AE_STRATEGY_INT) == 1 ? "ON"
: "OFF")));
}
if (should_apply_launch_config("ae_reference_stream") &&
@@ -1862,10 +1970,13 @@ void OBCameraNode::setupDevices() {
device_->isPropertySupported(OB_PROP_DEVICE_AE_REFERENCE_INT, OB_PERMISSION_WRITE)) {
if (device_->isPropertySupported(OB_PROP_DEVICE_AE_REFERENCE_INT, OB_PERMISSION_WRITE)) {
auto ae_reference = ae_reference_stream_ == "depth" ? 0 : 1;
device_->setIntProperty(OB_PROP_DEVICE_AE_REFERENCE_INT, ae_reference);
auto current_ae_reference = device_->getIntProperty(OB_PROP_DEVICE_AE_REFERENCE_INT);
RCLCPP_INFO_STREAM(logger_, "Current AE Reference: "
<< (current_ae_reference == 0 ? "depthbased" : "colorbased"));
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_DEVICE_AE_REFERENCE_INT, ae_reference);
TRY_EXECUTE_BLOCK({
auto current_ae_reference = device_->getIntProperty(OB_PROP_DEVICE_AE_REFERENCE_INT);
RCLCPP_INFO_STREAM(
logger_,
"Current AE Reference: " << (current_ae_reference == 0 ? "depthbased" : "colorbased"));
});
}
}
}
@@ -4154,11 +4265,11 @@ void OBCameraNode::startStreams() {
if (device_->isPropertySupported(OB_PROP_FRAME_INTERLEAVE_ENABLE_BOOL, OB_PERMISSION_WRITE)) {
TRY_TO_SET_PROPERTY(setBoolProperty, OB_PROP_FRAME_INTERLEAVE_ENABLE_BOOL,
interleave_frame_enable_);
RCLCPP_INFO_STREAM(
TRY_EXECUTE_BLOCK(RCLCPP_INFO_STREAM(
logger_,
"Enable enable_interleave_depth_frame to "
<< (device_->getBoolProperty(OB_PROP_FRAME_INTERLEAVE_ENABLE_BOOL) ? "true"
: "false"));
: "false")));
}
}
// set interleave larse PATTERN_SYNC_DELAY
@@ -4168,9 +4279,9 @@ void OBCameraNode::startStreams() {
(sync_mode_str_ == "PRIMARY" || sync_mode_str_ == "SOFTWARE_TRIGGERING")) {
std::this_thread::sleep_for(std::chrono::milliseconds(1000));
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_FRAME_INTERLEAVE_LASER_PATTERN_SYNC_DELAY_INT, 0);
RCLCPP_INFO_STREAM(logger_,
"Current interleave laser pattern sync delay: " << device_->getIntProperty(
OB_PROP_FRAME_INTERLEAVE_LASER_PATTERN_SYNC_DELAY_INT));
TRY_EXECUTE_BLOCK(RCLCPP_INFO_STREAM(
logger_, "Current interleave laser pattern sync delay: " << device_->getIntProperty(
OB_PROP_FRAME_INTERLEAVE_LASER_PATTERN_SYNC_DELAY_INT)));
}
pipeline_started_.store(true);
}
@@ -4408,7 +4519,10 @@ int OBCameraNode::closeSocSyncPwmTrigger() {
}
void OBCameraNode::startGmslTrigger() {
if (gmsl_trigger_fps_ > 0 && enable_gmsl_trigger_) {
if (!enable_gmsl_trigger_) {
return;
}
if (gmsl_trigger_fps_ > 0) {
RCLCPP_WARN_STREAM(logger_, "Start HardwareTrigger by soc-trigger-source. gmsl_trigger_fps_: "
<< gmsl_trigger_fps_);
openSocSyncPwmTrigger(gmsl_trigger_fps_);
@@ -4691,6 +4805,7 @@ void OBCameraNode::getParameters() {
} else {
setAndGetNodeParameter<std::string>(device_preset_, "device_preset", "");
}
setAndGetNodeParameter<std::string>(device_preset_version_, "device_preset_version", "");
setAndGetNodeParameter<bool>(enable_decimation_filter_, "enable_decimation_filter", false);
setAndGetNodeParameter<bool>(enable_hdr_merge_, "enable_hdr_merge", false);
setAndGetNodeParameter<bool>(enable_sequence_id_filter_, "enable_sequence_id_filter", false);
@@ -7371,7 +7486,7 @@ void OBCameraNode::onNewIMUFrameCallback(const std::shared_ptr<ob::Frame> &frame
setDefaultIMUMessage(imu_msg);
imu_msg.header.frame_id = optical_frame_id_[stream_index];
auto timestamp = fromUsToROSTime(frame->getTimeStampUs());
auto timestamp = fromUsToROSTime(getFrameTimestampUs(frame));
imu_msg.header.stamp = timestamp;
auto imu_info = createIMUInfo(stream_index);
orbbec_camera/src/ob_lidar_node.cpp
+17 -11
View File
@@ -205,8 +205,10 @@ void OBLidarNode::setupDevices() {
}
}
if (device_->isPropertySupported(OB_PROP_HEARTBEAT_BOOL, OB_PERMISSION_READ_WRITE)) {
RCLCPP_INFO_STREAM(logger_, "Current heartbeat: " << (enable_heartbeat_ ? "ON" : "OFF"));
TRY_TO_SET_PROPERTY(setBoolProperty, OB_PROP_HEARTBEAT_BOOL, enable_heartbeat_);
TRY_EXECUTE_BLOCK(RCLCPP_INFO_STREAM(
logger_, "Current heartbeat: "
<< (device_->getBoolProperty(OB_PROP_HEARTBEAT_BOOL) ? "ON" : "OFF")));
}
if (!echo_mode_.empty() &&
device_->isPropertySupported(OB_PROP_LIDAR_SPECIFIC_MODE_INT, OB_PERMISSION_READ_WRITE)) {
@@ -215,10 +217,10 @@ void OBLidarNode::setupDevices() {
} else if (echo_mode_ == "First Echo") {
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_LIDAR_SPECIFIC_MODE_INT, 1);
}
RCLCPP_INFO_STREAM(
TRY_EXECUTE_BLOCK(RCLCPP_INFO_STREAM(
logger_, "Current echo mode: " << (device_->getIntProperty(OB_PROP_LIDAR_SPECIFIC_MODE_INT)
? "First Echo"
: "Last Echo"));
: "Last Echo")));
}
if (repetitive_scan_mode_ != -1 &&
device_->isPropertySupported(OB_PROP_LIDAR_REPETITIVE_SCAN_MODE_INT,
@@ -231,8 +233,9 @@ void OBLidarNode::setupDevices() {
} else {
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_LIDAR_REPETITIVE_SCAN_MODE_INT,
repetitive_scan_mode_);
RCLCPP_INFO_STREAM(logger_, "Current repetitive scan mode: " << device_->getIntProperty(
OB_PROP_LIDAR_REPETITIVE_SCAN_MODE_INT));
TRY_EXECUTE_BLOCK(RCLCPP_INFO_STREAM(
logger_, "Current repetitive scan mode: "
<< device_->getIntProperty(OB_PROP_LIDAR_REPETITIVE_SCAN_MODE_INT)));
}
}
if (filter_level_ != -1 &&
@@ -242,10 +245,12 @@ void OBLidarNode::setupDevices() {
RCLCPP_ERROR(logger_, "filter level value is out of range[%d,%d], please check the value",
range.min, range.max);
} else {
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_LIDAR_TAIL_FILTER_LEVEL_INT, filter_level_);
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_LIDAR_APPLY_CONFIGS_INT, 1);
RCLCPP_INFO_STREAM(logger_, "Current filter level: " << device_->getIntProperty(
OB_PROP_LIDAR_TAIL_FILTER_LEVEL_INT));
TRY_EXECUTE_BLOCK({
device_->setIntProperty(OB_PROP_LIDAR_TAIL_FILTER_LEVEL_INT, filter_level_);
device_->setIntProperty(OB_PROP_LIDAR_APPLY_CONFIGS_INT, 1);
RCLCPP_INFO_STREAM(logger_, "Current filter level: " << device_->getIntProperty(
OB_PROP_LIDAR_TAIL_FILTER_LEVEL_INT));
});
}
}
@@ -257,8 +262,9 @@ void OBLidarNode::setupDevices() {
range.min, range.max);
} else {
TRY_TO_SET_PROPERTY(setFloatProperty, OB_PROP_LIDAR_MEMS_FOV_SIZE_FLOAT, vertical_fov_);
RCLCPP_INFO_STREAM(logger_, "Current vertical fov: " << device_->getFloatProperty(
OB_PROP_LIDAR_MEMS_FOV_SIZE_FLOAT));
TRY_EXECUTE_BLOCK(RCLCPP_INFO_STREAM(
logger_, "Current vertical fov: "
<< device_->getFloatProperty(OB_PROP_LIDAR_MEMS_FOV_SIZE_FLOAT)));
}
}
}
orbbec_camera/src/ros_service.cpp
+194
View File
@@ -112,6 +112,8 @@ std::string OBSyncModeToString(const OBMultiDeviceSyncMode& mode) {
return "SOFTWARE_TRIGGERING";
case OBMultiDeviceSyncMode::OB_MULTI_DEVICE_SYNC_MODE_HARDWARE_TRIGGERING:
return "HARDWARE_TRIGGERING";
case OBMultiDeviceSyncMode::OB_MULTI_DEVICE_SYNC_MODE_GROUP_ACTIONS:
return "GROUP_ACTIONS";
default:
return "FREE_RUN";
}
@@ -293,6 +295,20 @@ void OBCameraNode::setupCameraCtrlServices() {
std::shared_ptr<SetInt32::Response> response) {
setWhiteBalanceCallback(request, response);
});
if (isPropertyReadable(device_, OB_PROP_COLOR_WB_CTRL_INT)) {
get_color_wb_ctrl_srv_ = node_->create_service<GetInt32>(
"get_color_wb_ctrl", [this](const std::shared_ptr<GetInt32::Request> request,
std::shared_ptr<GetInt32::Response> response) {
getColorWbCtrlCallback(request, response);
});
}
if (isPropertyWritable(device_, OB_PROP_COLOR_WB_CTRL_INT)) {
set_color_wb_ctrl_srv_ = node_->create_service<SetInt32>(
"set_color_wb_ctrl", [this](const std::shared_ptr<SetInt32::Request> request,
std::shared_ptr<SetInt32::Response> response) {
setColorWbCtrlCallback(request, response);
});
}
get_auto_white_balance_srv_ = node_->create_service<GetInt32>(
"get_auto_white_balance", [this](const std::shared_ptr<GetInt32::Request> request,
std::shared_ptr<GetInt32::Response> response) {
@@ -334,6 +350,28 @@ void OBCameraNode::setupCameraCtrlServices() {
std::shared_ptr<GetDeviceConfig::Response> response) {
getDeviceConfigCallback(request, response);
});
if (isPropertyReadable(device_, OB_PROP_ACTION_SIGNAL_COUNT_INT) &&
isPropertyReadable(device_, OB_PROP_ACTION_DEVICE_KEY_INT) &&
isPropertyWritable(device_, OB_PROP_ACTION_SELECTOR_INT) &&
isPropertyReadable(device_, OB_PROP_ACTION_GROUP_KEY_INT) &&
isPropertyReadable(device_, OB_PROP_ACTION_GROUP_MASK_INT)) {
get_action_config_srv_ = node_->create_service<GetActionConfig>(
"get_action_config", [this](const std::shared_ptr<GetActionConfig::Request> request,
std::shared_ptr<GetActionConfig::Response> response) {
getActionConfigCallback(request, response);
});
}
if (isPropertyReadable(device_, OB_PROP_ACTION_SIGNAL_COUNT_INT) &&
isPropertyWritable(device_, OB_PROP_ACTION_DEVICE_KEY_INT) &&
isPropertyWritable(device_, OB_PROP_ACTION_SELECTOR_INT) &&
isPropertyWritable(device_, OB_PROP_ACTION_GROUP_KEY_INT) &&
isPropertyWritable(device_, OB_PROP_ACTION_GROUP_MASK_INT)) {
set_action_config_srv_ = node_->create_service<SetActionConfig>(
"set_action_config", [this](const std::shared_ptr<SetActionConfig::Request> request,
std::shared_ptr<SetActionConfig::Response> response) {
setActionConfigCallback(request, response);
});
}
get_sdk_version_srv_ = node_->create_service<GetString>(
"get_sdk_version",
[this](const std::shared_ptr<GetString::Request> request,
@@ -1252,6 +1290,67 @@ void OBCameraNode::setWhiteBalanceCallback(const std::shared_ptr<SetInt32 ::Requ
}
}
void OBCameraNode::getColorWbCtrlCallback(const std::shared_ptr<GetInt32::Request>& request,
std::shared_ptr<GetInt32::Response>& response) {
(void)request;
std::lock_guard<decltype(device_lock_)> lock(device_lock_);
try {
response->data = device_->getIntProperty(OB_PROP_COLOR_WB_CTRL_INT);
response->success = true;
response->message = "OK";
} 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::setColorWbCtrlCallback(const std::shared_ptr<SetInt32::Request>& request,
std::shared_ptr<SetInt32::Response>& response) {
if (!request) {
response->success = false;
response->message = "Invalid request";
return;
}
std::lock_guard<decltype(device_lock_)> lock(device_lock_);
try {
const auto range = device_->getIntPropertyRange(OB_PROP_COLOR_WB_CTRL_INT);
if (request->data < range.min || request->data > range.max) {
response->success = false;
response->message = "value out of range [" + std::to_string(range.min) + ", " +
std::to_string(range.max) + "]";
return;
}
device_->setIntProperty(OB_PROP_COLOR_WB_CTRL_INT, request->data);
response->success = true;
response->message = "OK";
if (isPropertyReadable(device_, OB_PROP_COLOR_WB_CTRL_INT)) {
const auto current_value = device_->getIntProperty(OB_PROP_COLOR_WB_CTRL_INT);
response->success = current_value == request->data;
if (!response->success) {
response->message = "device reported " + std::to_string(current_value) + " after setting " +
std::to_string(request->data);
}
}
} 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::getAutoWhiteBalanceCallback(const std::shared_ptr<GetInt32::Request>& request,
std::shared_ptr<GetInt32::Response>& response) {
(void)request;
@@ -1598,6 +1697,86 @@ void OBCameraNode::getDeviceInfoCallback(const std::shared_ptr<GetDeviceInfo::Re
}
}
void OBCameraNode::getActionConfigCallback(const std::shared_ptr<GetActionConfig::Request>& request,
std::shared_ptr<GetActionConfig::Response>& response) {
if (!request) {
response->success = false;
response->message = "Invalid request";
return;
}
std::lock_guard<decltype(device_lock_)> lock(device_lock_);
try {
const int action_signal_count = device_->getIntProperty(OB_PROP_ACTION_SIGNAL_COUNT_INT);
if (action_signal_count <= 0 ||
request->selector >= static_cast<uint32_t>(action_signal_count)) {
response->success = false;
response->message = "selector must be less than action signal count " +
std::to_string(std::max(action_signal_count, 0));
return;
}
device_->setIntProperty(OB_PROP_ACTION_SELECTOR_INT, static_cast<int32_t>(request->selector));
response->action_signal_count = static_cast<uint32_t>(action_signal_count);
response->device_key =
static_cast<uint32_t>(device_->getIntProperty(OB_PROP_ACTION_DEVICE_KEY_INT));
response->group_key =
static_cast<uint32_t>(device_->getIntProperty(OB_PROP_ACTION_GROUP_KEY_INT));
response->group_mask =
static_cast<uint32_t>(device_->getIntProperty(OB_PROP_ACTION_GROUP_MASK_INT));
response->success = true;
response->message = "OK";
} 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::setActionConfigCallback(const std::shared_ptr<SetActionConfig::Request>& request,
std::shared_ptr<SetActionConfig::Response>& response) {
if (!request) {
response->success = false;
response->message = "Invalid request";
return;
}
std::lock_guard<decltype(device_lock_)> lock(device_lock_);
try {
const int action_signal_count = device_->getIntProperty(OB_PROP_ACTION_SIGNAL_COUNT_INT);
if (action_signal_count <= 0 ||
request->selector >= static_cast<uint32_t>(action_signal_count)) {
response->success = false;
response->message = "selector must be less than action signal count " +
std::to_string(std::max(action_signal_count, 0));
return;
}
device_->setIntProperty(OB_PROP_ACTION_DEVICE_KEY_INT,
static_cast<int32_t>(request->device_key));
device_->setIntProperty(OB_PROP_ACTION_SELECTOR_INT, static_cast<int32_t>(request->selector));
device_->setIntProperty(OB_PROP_ACTION_GROUP_KEY_INT, static_cast<int32_t>(request->group_key));
device_->setIntProperty(OB_PROP_ACTION_GROUP_MASK_INT,
static_cast<int32_t>(request->group_mask));
response->success = true;
response->message = "OK";
} 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::getDeviceConfigCallback(const std::shared_ptr<GetDeviceConfig::Request>& request,
std::shared_ptr<GetDeviceConfig::Response>& response) {
(void)request;
@@ -1745,6 +1924,21 @@ void OBCameraNode::getDeviceConfigCallback(const std::shared_ptr<GetDeviceConfig
RCLCPP_DEBUG_STREAM(logger_, "Failed to get current preset");
}
try {
const char* version = device_->getCurrentPresetDepthWorkModeVersion();
if (version != nullptr) {
response->preset_depth_work_mode_version = version;
}
} catch (const ob::Error& e) {
RCLCPP_DEBUG_STREAM(logger_, "Failed to get current preset depth work mode version: "
<< orbbec_camera::formatObErrorWithStatus(e));
} catch (const std::exception& e) {
RCLCPP_DEBUG_STREAM(logger_,
"Failed to get current preset depth work mode version: " << e.what());
} catch (...) {
RCLCPP_DEBUG_STREAM(logger_, "Failed to get current preset depth work mode version");
}
try {
if (device_->isColorPresetSupported()) {
const char* color_preset_name = device_->getCurrentColorPresetName();
orbbec_camera/src/utils.cpp
+11 -10
View File
@@ -95,15 +95,14 @@ sensor_msgs::msg::CameraInfo convertToCameraInfo(OBCameraIntrinsic intrinsic,
info.distortion_model = getDistortionModels(distortion);
info.width = intrinsic.width;
info.height = intrinsic.height;
info.d.resize(8, 0.0);
info.d[0] = distortion.k1;
info.d[1] = distortion.k2;
info.d[2] = distortion.p1;
info.d[3] = distortion.p2;
info.d[4] = distortion.k3;
info.d[5] = distortion.k4;
info.d[6] = distortion.k5;
info.d[7] = distortion.k6;
if (info.distortion_model == sensor_msgs::distortion_models::RATIONAL_POLYNOMIAL) {
info.d = {distortion.k1, distortion.k2, distortion.p1, distortion.p2,
distortion.k3, distortion.k4, distortion.k5, distortion.k6};
} else if (info.distortion_model == sensor_msgs::distortion_models::EQUIDISTANT) {
info.d = {distortion.k1, distortion.k2, distortion.k3, distortion.k4};
} else {
info.d = {distortion.k1, distortion.k2, distortion.p1, distortion.p2, distortion.k3};
}
bool all_zero = std::all_of(info.d.begin(), info.d.end(), [](double val) { return val == 0.0; });
info.roi.do_rectify = all_zero;
@@ -679,6 +678,8 @@ OBMultiDeviceSyncMode OBSyncModeFromString(const std::string &mode) {
return OBMultiDeviceSyncMode::OB_MULTI_DEVICE_SYNC_MODE_SOFTWARE_TRIGGERING;
} else if (mode == "HARDWARE_TRIGGERING") {
return OBMultiDeviceSyncMode::OB_MULTI_DEVICE_SYNC_MODE_HARDWARE_TRIGGERING;
} else if (mode == "GROUP_ACTIONS") {
return OBMultiDeviceSyncMode::OB_MULTI_DEVICE_SYNC_MODE_GROUP_ACTIONS;
} else {
return OBMultiDeviceSyncMode::OB_MULTI_DEVICE_SYNC_MODE_FREE_RUN;
}
@@ -1174,7 +1175,7 @@ std::string getDistortionModels(OBCameraDistortion distortion) {
case OB_DISTORTION_BROWN_CONRADY:
return sensor_msgs::distortion_models::PLUMB_BOB;
case OB_DISTORTION_BROWN_CONRADY_K6:
return sensor_msgs::distortion_models::PLUMB_BOB;
return sensor_msgs::distortion_models::RATIONAL_POLYNOMIAL;
case OB_DISTORTION_KANNALA_BRANDT4:
return sensor_msgs::distortion_models::EQUIDISTANT;
default:
orbbec_camera/test/camera_info_distortion_test.cpp
+73
View File
@@ -0,0 +1,73 @@
#include <gtest/gtest.h>
#include <vector>
#include "orbbec_camera/utils.h"
namespace orbbec_camera {
namespace {
OBCameraIntrinsic makeIntrinsic() {
OBCameraIntrinsic intrinsic{};
intrinsic.width = 1280;
intrinsic.height = 800;
intrinsic.fx = 600.0F;
intrinsic.fy = 601.0F;
intrinsic.cx = 640.0F;
intrinsic.cy = 400.0F;
return intrinsic;
}
OBCameraDistortion makeDistortion(OBCameraDistortionModel model) {
OBCameraDistortion distortion{};
distortion.k1 = 0.1F;
distortion.k2 = 0.2F;
distortion.k3 = 0.3F;
distortion.k4 = 0.4F;
distortion.k5 = 0.5F;
distortion.k6 = 0.6F;
distortion.p1 = 0.01F;
distortion.p2 = 0.02F;
distortion.model = model;
return distortion;
}
TEST(CameraInfoDistortionTest, ConvertsBrownConradyToPlumbBob) {
const auto intrinsic = makeIntrinsic();
const auto distortion = makeDistortion(OB_DISTORTION_BROWN_CONRADY);
const auto info = convertToCameraInfo(intrinsic, distortion, intrinsic.width);
EXPECT_EQ(info.distortion_model, sensor_msgs::distortion_models::PLUMB_BOB);
EXPECT_EQ(info.d, std::vector<double>({distortion.k1, distortion.k2, distortion.p1, distortion.p2,
distortion.k3}));
}
TEST(CameraInfoDistortionTest, KeepsK6ModelWhenHigherOrderCoefficientsAreZero) {
const auto intrinsic = makeIntrinsic();
auto distortion = makeDistortion(OB_DISTORTION_BROWN_CONRADY_K6);
distortion.k4 = 0.0F;
distortion.k5 = 0.0F;
distortion.k6 = 0.0F;
const auto info = convertToCameraInfo(intrinsic, distortion, intrinsic.width);
EXPECT_EQ(info.distortion_model, sensor_msgs::distortion_models::RATIONAL_POLYNOMIAL);
EXPECT_EQ(info.d,
std::vector<double>({distortion.k1, distortion.k2, distortion.p1, distortion.p2,
distortion.k3, distortion.k4, distortion.k5, distortion.k6}));
}
TEST(CameraInfoDistortionTest, ConvertsKannalaBrandtToEquidistant) {
const auto intrinsic = makeIntrinsic();
const auto distortion = makeDistortion(OB_DISTORTION_KANNALA_BRANDT4);
const auto info = convertToCameraInfo(intrinsic, distortion, intrinsic.width);
EXPECT_EQ(info.distortion_model, sensor_msgs::distortion_models::EQUIDISTANT);
EXPECT_EQ(info.d,
std::vector<double>({distortion.k1, distortion.k2, distortion.k3, distortion.k4}));
}
} // namespace
} // namespace orbbec_camera
orbbec_camera/tools/firmware_update_tool.cpp
+142 -10
View File
@@ -70,10 +70,19 @@ struct FirmwareUpdateResult {
bool success = false;
bool need_reupdate = false;
bool retryable = true;
bool reboot_command_sent = false;
OBFwUpdateState final_state = STAT_START;
std::chrono::steady_clock::time_point reboot_started_at;
std::string error_message;
};
struct DeviceIdentity {
std::string serial_number;
std::string uid;
std::string ip_address;
bool is_network = false;
};
std::string trim(std::string value) {
const auto not_space = [](unsigned char ch) { return !std::isspace(ch); };
value.erase(value.begin(), std::find_if(value.begin(), value.end(), not_space));
@@ -405,6 +414,29 @@ bool isBootDeviceName(const std::string &name) {
return lower == "boot" || lower.find("boot") != std::string::npos;
}
std::string safeString(const char *value) { return value == nullptr ? "" : value; }
DeviceIdentity getDeviceIdentity(const std::shared_ptr<ob::DeviceInfo> &device_info) {
DeviceIdentity identity;
identity.serial_number = safeString(device_info->getSerialNumber());
identity.uid = safeString(device_info->getUid());
identity.ip_address = safeString(device_info->getIpAddress());
identity.is_network = safeString(device_info->getConnectionType()) == "Ethernet";
return identity;
}
void logRebootReconnectElapsed(const rclcpp::Logger &logger, const FirmwareUpdateResult &result,
const char *stage) {
if (!result.reboot_command_sent) {
return;
}
const auto elapsed_ms = std::chrono::duration_cast<std::chrono::milliseconds>(
std::chrono::steady_clock::now() - result.reboot_started_at)
.count();
RCLCPP_INFO(logger, "[%s] Device detected after reboot in %lld ms", stage,
static_cast<long long>(elapsed_ms));
}
void printUpdateProgress(const std::string &task, OBFwUpdateState state, const char *message,
uint8_t percent) {
std::cout << "[" << task << "] " << static_cast<uint32_t>(percent) << "% | "
@@ -423,7 +455,15 @@ void logCurrentPresetList(const rclcpp::Logger &logger, const std::shared_ptr<ob
const uint32_t count = preset_list->getCount();
RCLCPP_INFO(logger, "[%s] Current preset count: %u", stage, count);
for (uint32_t i = 0; i < count; ++i) {
RCLCPP_INFO(logger, "[%s] Preset[%u]: %s", stage, i, preset_list->getName(i));
const char *version = nullptr;
try {
version = preset_list->getDepthWorkModeVersion(i);
} catch (...) {
// Older firmware can enumerate presets without exposing version information.
}
RCLCPP_INFO(logger, "[%s] Preset[%u]: %s, depth work mode version: %s", stage, i,
preset_list->getName(i),
version == nullptr || version[0] == '\0' ? "not available" : version);
}
} catch (const ob::Error &e) {
RCLCPP_WARN(logger, "[%s] Failed to query preset list: %s", stage,
@@ -499,33 +539,94 @@ std::shared_ptr<ob::Device> connectDevice(const rclcpp::Logger &logger,
return device;
}
bool matchesDeviceIdentity(const std::shared_ptr<ob::DeviceList> &list, uint32_t index,
const DeviceIdentity &identity) {
if (!identity.serial_number.empty()) {
try {
if (safeString(list->getSerialNumber(index)) == identity.serial_number) {
return true;
}
} catch (const ob::Error &) {
}
}
if (!identity.uid.empty()) {
try {
if (safeString(list->getUid(index)) == identity.uid) {
return true;
}
} catch (const ob::Error &) {
}
}
if (identity.is_network && identity.serial_number.empty() && identity.uid.empty() &&
!identity.ip_address.empty()) {
try {
return safeString(list->getIpAddress(index)) == identity.ip_address;
} catch (const ob::Error &) {
}
}
return false;
}
std::shared_ptr<ob::Device> findEnumeratedDevice(const std::shared_ptr<ob::DeviceList> &list,
const DeviceIdentity &identity) {
for (uint32_t i = 0; i < list->getCount(); ++i) {
try {
if (matchesDeviceIdentity(list, i, identity)) {
return list->getDevice(i, OB_DEVICE_DEFAULT_ACCESS);
}
} catch (const ob::Error &) {
continue;
}
}
return nullptr;
}
std::shared_ptr<ob::Device> waitForReconnect(const rclcpp::Logger &logger,
const std::shared_ptr<ob::Context> &ctx,
const CliArgs &args, bool require_non_boot = false) {
const CliArgs &args, const DeviceIdentity &identity,
bool require_non_boot = false,
bool require_reboot_transition = false) {
const auto deadline =
std::chrono::steady_clock::now() + std::chrono::seconds(args.reconnect_timeout_sec);
bool reboot_transition_observed = !require_reboot_transition;
while (std::chrono::steady_clock::now() < deadline) {
try {
auto device = connectDevice(logger, ctx, args);
auto list = ctx->queryDeviceList();
auto device = findEnumeratedDevice(list, identity);
if (device) {
auto device_info = device->getDeviceInfo();
const std::string name = device_info->getName();
const std::string serial = device_info->getSerialNumber();
const bool is_boot = isBootDeviceName(name);
RCLCPP_INFO(logger, "Device reconnected: %s (%s)", name.c_str(), serial.c_str());
if (!reboot_transition_observed) {
if (is_boot) {
reboot_transition_observed = true;
RCLCPP_INFO(logger, "Device entered boot stage: %s (%s)", name.c_str(), serial.c_str());
} else {
std::this_thread::sleep_for(std::chrono::milliseconds(args.reconnect_poll_ms));
continue;
}
}
if (require_non_boot && is_boot) {
RCLCPP_INFO(logger, "Device is still in boot stage, waiting for normal mode...");
std::this_thread::sleep_for(std::chrono::milliseconds(args.reconnect_poll_ms));
continue;
}
RCLCPP_INFO(logger, "Device reconnected: %s (%s)", name.c_str(), serial.c_str());
return device;
}
reboot_transition_observed = true;
} catch (const ob::Error &e) {
reboot_transition_observed = true;
RCLCPP_WARN(logger, "Reconnect attempt failed (SDK): %s",
orbbec_camera::formatObErrorWithStatus(e).c_str());
} catch (const std::exception &e) {
reboot_transition_observed = true;
RCLCPP_WARN(logger, "Reconnect attempt failed: %s", e.what());
} catch (...) {
reboot_transition_observed = true;
RCLCPP_WARN(logger, "Reconnect attempt failed: unknown error");
}
std::this_thread::sleep_for(std::chrono::milliseconds(args.reconnect_poll_ms));
@@ -536,7 +637,8 @@ std::shared_ptr<ob::Device> waitForReconnect(const rclcpp::Logger &logger,
std::shared_ptr<ob::Device> waitForReconnectUntil(
const rclcpp::Logger &logger, const std::shared_ptr<ob::Context> &ctx, const CliArgs &args,
bool require_non_boot, const std::chrono::steady_clock::time_point &deadline) {
const DeviceIdentity &identity, bool require_non_boot,
const std::chrono::steady_clock::time_point &deadline, bool require_reboot_transition = false) {
const auto now = std::chrono::steady_clock::now();
if (now >= deadline) {
throw std::runtime_error("Timeout waiting for device reconnection");
@@ -548,7 +650,8 @@ std::shared_ptr<ob::Device> waitForReconnectUntil(
bounded_args.reconnect_timeout_sec = std::max(1, static_cast<int>((remaining_ms + 999) / 1000));
bounded_args.reconnect_poll_ms =
std::max(100, std::min(bounded_args.reconnect_poll_ms, static_cast<int>(remaining_ms)));
return waitForReconnect(logger, ctx, bounded_args, require_non_boot);
return waitForReconnect(logger, ctx, bounded_args, identity, require_non_boot,
require_reboot_transition);
}
bool updatePresetFirmware(const rclcpp::Logger &logger, const std::shared_ptr<ob::Device> &device,
@@ -684,7 +787,9 @@ FirmwareUpdateResult updateFirmware(const rclcpp::Logger &logger,
waitForFirmwareLogDrain(logger);
}
RCLCPP_INFO(logger, "Rebooting device after firmware update...");
result.reboot_started_at = std::chrono::steady_clock::now();
device->reboot();
result.reboot_command_sent = true;
RCLCPP_INFO(logger, "Device reboot command sent.");
return result;
}
@@ -746,8 +851,14 @@ int main(int argc, char **argv) {
try {
auto device = connectDevice(logger, ctx, run_args);
auto device_info = device->getDeviceInfo();
const auto device_identity = getDeviceIdentity(device_info);
RCLCPP_INFO(logger, "Selected device: %s, SN: %s, UID: %s", device_info->getName(),
device_info->getSerialNumber(), device_info->getUid());
if (device_identity.is_network) {
ctx->enableNetDeviceEnumeration(true);
RCLCPP_INFO(logger,
"Network device detected; reconnect will be verified by device enumeration.");
}
const bool enable_firmware_log = isSdkLogEnabled(run_args.sdk_log_level);
bool firmware_log_enabled = enable_firmware_log && enableFirmwareLog(logger, device);
@@ -769,6 +880,7 @@ int main(int argc, char **argv) {
? "First firmware update failed"
: first_update.error_message);
}
FirmwareUpdateResult final_update = first_update;
if (first_update.need_reupdate) {
RCLCPP_INFO(
@@ -776,7 +888,10 @@ int main(int argc, char **argv) {
"Firmware requires reboot and second update. Waiting for device reconnect...");
const auto second_deadline = std::chrono::steady_clock::now() +
std::chrono::seconds(run_args.reconnect_timeout_sec);
device = waitForReconnectUntil(logger, ctx, run_args, true, second_deadline);
device.reset();
device = waitForReconnectUntil(logger, ctx, run_args, device_identity, true,
second_deadline, true);
logRebootReconnectElapsed(logger, first_update, "first update reconnect");
firmware_log_enabled = enable_firmware_log && enableFirmwareLog(logger, device);
bool second_ok = false;
while (std::chrono::steady_clock::now() < second_deadline) {
@@ -795,14 +910,20 @@ int main(int argc, char **argv) {
RCLCPP_WARN(logger, "Second firmware update attempt failed: %s, retrying...",
second_update.error_message.c_str());
}
device = waitForReconnectUntil(logger, ctx, run_args, true, second_deadline);
device.reset();
device = waitForReconnectUntil(logger, ctx, run_args, device_identity, true,
second_deadline);
firmware_log_enabled = enable_firmware_log && enableFirmwareLog(logger, device);
continue;
}
final_update = second_update;
if (second_update.need_reupdate) {
RCLCPP_WARN(logger,
"Second attempt still requires reupdate, waiting and retrying...");
device = waitForReconnectUntil(logger, ctx, run_args, true, second_deadline);
device.reset();
device = waitForReconnectUntil(logger, ctx, run_args, device_identity, true,
second_deadline, true);
logRebootReconnectElapsed(logger, second_update, "reupdate reconnect");
firmware_log_enabled = enable_firmware_log && enableFirmwareLog(logger, device);
continue;
}
@@ -811,7 +932,9 @@ int main(int argc, char **argv) {
} catch (const ob::Error &e) {
RCLCPP_WARN(logger, "Second update transient error: %s, retrying...",
orbbec_camera::formatObErrorWithStatus(e).c_str());
device = waitForReconnectUntil(logger, ctx, run_args, true, second_deadline);
device.reset();
device = waitForReconnectUntil(logger, ctx, run_args, device_identity, true,
second_deadline);
firmware_log_enabled = enable_firmware_log && enableFirmwareLog(logger, device);
}
}
@@ -819,6 +942,15 @@ int main(int argc, char **argv) {
throw std::runtime_error("Second firmware update failed after retries");
}
}
RCLCPP_INFO(logger, "Waiting for device to reconnect after final reboot...");
device.reset();
device = waitForReconnect(logger, ctx, run_args, device_identity, true, true);
logRebootReconnectElapsed(logger, final_update, "final reconnect");
device_info = device->getDeviceInfo();
RCLCPP_INFO(logger, "Device is online after firmware update: %s, SN: %s, firmware: %s",
device_info->getName(), device_info->getSerialNumber(),
device_info->getFirmwareVersion());
}
success_count++;
orbbec_camera/tools/list_camera_profile.cpp
+10 -1
View File
@@ -205,7 +205,16 @@ void printPreset(const std::shared_ptr<ob::Device>& device) {
std::cout << "Preset list:" << std::endl;
for (uint32_t i = 0; i < preset_list->getCount(); i++) {
auto name = preset_list->getName(i);
std::cout << "Preset list[" << i << "]: " << name << std::endl;
std::string version;
try {
const char* version_value = preset_list->getDepthWorkModeVersion(i);
version = version_value == nullptr ? "" : version_value;
} catch (...) {
// Older firmware can enumerate presets without exposing version information.
}
std::cout << "Preset list[" << i << "]: " << name
<< ", depth work mode version: " << (version.empty() ? "not available" : version)
<< std::endl;
}
}
orbbec_camera/tools/list_devices_node.cpp
+9 -1
View File
@@ -181,7 +181,15 @@ void printPresetInfo(const std::shared_ptr<ob::Device> &device) {
for (uint32_t i = 0; i < preset_count; ++i) {
const char *preset_name = preset_list->getName(i);
if (preset_name != nullptr && preset_name[0] != '\0') {
RCLCPP_INFO_STREAM(logger, " - " << preset_name);
std::string version;
try {
const char *version_value = preset_list->getDepthWorkModeVersion(i);
version = version_value == nullptr ? "" : version_value;
} catch (...) {
// Older firmware can enumerate presets without exposing version information.
}
RCLCPP_INFO_STREAM(logger, " - " << preset_name << " (depth work mode version: "
<< (version.empty() ? "not available" : version) << ")");
}
}
orbbec_camera_msgs/CMakeLists.txt
+3
View File
@@ -28,6 +28,7 @@ rosidl_generate_interfaces(
"msg/RGBD.msg"
"msg/StreamProfile.msg"
"srv/GetBool.srv"
"srv/GetActionConfig.srv"
"srv/GetDeviceConfig.srv"
"srv/GetDeviceInfo.srv"
"srv/GetCameraInfo.srv"
@@ -35,6 +36,7 @@ rosidl_generate_interfaces(
"srv/GetAwbGain.srv"
"srv/GetString.srv"
"srv/SetFilter.srv"
"srv/SetActionConfig.srv"
"srv/SetInt32.srv"
"srv/SetAwbGain.srv"
"srv/SetString.srv"
@@ -43,6 +45,7 @@ rosidl_generate_interfaces(
"srv/SetUserCalibParams.srv"
"srv/SetBagRecording.srv"
"srv/SetStreamProfile.srv"
"srv/SendActionCommand.srv"
"action/RunAeAwbLockTest.action"
DEPENDENCIES
action_msgs
orbbec_camera_msgs/package.xml
+1 -1
View File
@@ -2,7 +2,7 @@
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>orbbec_camera_msgs</name>
<version>2.10.1</version>
<version>2.10.2</version>
<description>A package containing orbbec camera messages definitions.</description>
<maintainer email="[email protected]">yalian</maintainer>
<license>Apache-2.0</license>
orbbec_camera_msgs/srv/GetActionConfig.srv
+8
View File
@@ -0,0 +1,8 @@
uint32 selector
---
uint32 action_signal_count
uint32 device_key
uint32 group_key
uint32 group_mask
bool success
string message
orbbec_camera_msgs/srv/GetDeviceConfig.srv
+3
View File
@@ -4,7 +4,10 @@ string schema_version
# Effective device configuration state that is not provided by existing device/version services.
string device_preset
# Legacy preset package version reported by the device extension information.
string preset_version
# Current preset's target depth work mode version. Empty when unsupported or unavailable.
string preset_depth_work_mode_version
string color_preset
string depth_precision
string disparity_to_depth_mode
orbbec_camera_msgs/srv/SendActionCommand.srv
+12
View File
@@ -0,0 +1,12 @@
uint32 device_key
uint32 group_key
uint32 group_mask
# IPv4 destination. Leave empty to use the SDK broadcast address (255.255.255.255).
string destination_ip
# GVCP/PTP timestamp: upper 32 bits are seconds and lower 32 bits are nanoseconds.
# Zero sends the command immediately.
uint64 scheduled_time
---
# Success means that the SDK dispatched the command; Action Command has no device acknowledgment.
bool success
string message
orbbec_camera_msgs/srv/SetActionConfig.srv
+7
View File
@@ -0,0 +1,7 @@
uint32 device_key
uint32 selector
uint32 group_key
uint32 group_mask
---
bool success
string message
orbbec_description/package.xml
+1 -1
View File
@@ -2,7 +2,7 @@
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>orbbec_description</name>
<version>2.10.1</version>
<version>2.10.2</version>
<description>TODO: Package description</description>
<maintainer email="[email protected]">yalian</maintainer>
<license>Apache-2.0</license>