From 5fcd5eed85aab3f4f342a86e5fab754eca70ef45 Mon Sep 17 00:00:00 2001 From: slz Date: Thu, 3 Sep 2026 16:36:24 +0800 Subject: [PATCH] feat: support versioned preset launch configuration --- orbbec_camera/config/camera_params.yaml | 1 + .../config/camera_secondary_params.yaml | 1 + .../gemini_330_series_benchmark.launch.py | 3 +- .../gmsl_camera/gemini_330_gmsl.launch.py | 3 +- .../gemini_330_series_synced_verify.launch.py | 3 +- .../include/orbbec_camera/ob_camera_node.h | 1 + .../launch/gemini_330_series.launch.py | 3 +- .../gemini_330_series_low_cpu.launch.py | 3 +- .../gemini_intra_process_demo_launch.py | 3 +- orbbec_camera/src/ob_camera_node.cpp | 54 +++++++++++++++++-- orbbec_camera/src/ros_service.cpp | 15 ++++++ orbbec_camera/tools/firmware_update_tool.cpp | 10 +++- orbbec_camera/tools/list_camera_profile.cpp | 11 +++- orbbec_camera/tools/list_devices_node.cpp | 10 +++- orbbec_camera_msgs/srv/GetDeviceConfig.srv | 3 ++ 15 files changed, 110 insertions(+), 14 deletions(-) diff --git a/orbbec_camera/config/camera_params.yaml b/orbbec_camera/config/camera_params.yaml index d2cedfa3..60a68cd2 100644 --- a/orbbec_camera/config/camera_params.yaml +++ b/orbbec_camera/config/camera_params.yaml @@ -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 diff --git a/orbbec_camera/config/camera_secondary_params.yaml b/orbbec_camera/config/camera_secondary_params.yaml index 2e865f80..44d62d65 100644 --- a/orbbec_camera/config/camera_secondary_params.yaml +++ b/orbbec_camera/config/camera_secondary_params.yaml @@ -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 diff --git a/orbbec_camera/examples/benchmark/gemini_330_series_benchmark.launch.py b/orbbec_camera/examples/benchmark/gemini_330_series_benchmark.launch.py index 906b31cd..863dd123 100644 --- a/orbbec_camera/examples/benchmark/gemini_330_series_benchmark.launch.py +++ b/orbbec_camera/examples/benchmark/gemini_330_series_benchmark.launch.py @@ -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'), diff --git a/orbbec_camera/examples/gmsl_camera/gemini_330_gmsl.launch.py b/orbbec_camera/examples/gmsl_camera/gemini_330_gmsl.launch.py index 906b31cd..863dd123 100644 --- a/orbbec_camera/examples/gmsl_camera/gemini_330_gmsl.launch.py +++ b/orbbec_camera/examples/gmsl_camera/gemini_330_gmsl.launch.py @@ -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'), diff --git a/orbbec_camera/examples/multi_camera_synced_verification_tool/gemini_330_series_synced_verify.launch.py b/orbbec_camera/examples/multi_camera_synced_verification_tool/gemini_330_series_synced_verify.launch.py index 906b31cd..863dd123 100644 --- a/orbbec_camera/examples/multi_camera_synced_verification_tool/gemini_330_series_synced_verify.launch.py +++ b/orbbec_camera/examples/multi_camera_synced_verification_tool/gemini_330_series_synced_verify.launch.py @@ -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'), diff --git a/orbbec_camera/include/orbbec_camera/ob_camera_node.h b/orbbec_camera/include/orbbec_camera/ob_camera_node.h index 30427d15..97755f7c 100644 --- a/orbbec_camera/include/orbbec_camera/ob_camera_node.h +++ b/orbbec_camera/include/orbbec_camera/ob_camera_node.h @@ -1002,6 +1002,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; diff --git a/orbbec_camera/launch/gemini_330_series.launch.py b/orbbec_camera/launch/gemini_330_series.launch.py index 2bff99ba..5b80cd0e 100644 --- a/orbbec_camera/launch/gemini_330_series.launch.py +++ b/orbbec_camera/launch/gemini_330_series.launch.py @@ -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'), diff --git a/orbbec_camera/launch/gemini_330_series_low_cpu.launch.py b/orbbec_camera/launch/gemini_330_series_low_cpu.launch.py index f6480f17..4624d300 100644 --- a/orbbec_camera/launch/gemini_330_series_low_cpu.launch.py +++ b/orbbec_camera/launch/gemini_330_series_low_cpu.launch.py @@ -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'), diff --git a/orbbec_camera/launch/gemini_intra_process_demo_launch.py b/orbbec_camera/launch/gemini_intra_process_demo_launch.py index 4f742871..d280e1f5 100644 --- a/orbbec_camera/launch/gemini_intra_process_demo_launch.py +++ b/orbbec_camera/launch/gemini_intra_process_demo_launch.py @@ -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'), diff --git a/orbbec_camera/src/ob_camera_node.cpp b/orbbec_camera/src/ob_camera_node.cpp index 6d94249b..d7944103 100644 --- a/orbbec_camera/src/ob_camera_node.cpp +++ b/orbbec_camera/src/ob_camera_node.cpp @@ -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()) { @@ -4691,6 +4734,7 @@ void OBCameraNode::getParameters() { } else { setAndGetNodeParameter(device_preset_, "device_preset", ""); } + setAndGetNodeParameter(device_preset_version_, "device_preset_version", ""); setAndGetNodeParameter(enable_decimation_filter_, "enable_decimation_filter", false); setAndGetNodeParameter(enable_hdr_merge_, "enable_hdr_merge", false); setAndGetNodeParameter(enable_sequence_id_filter_, "enable_sequence_id_filter", false); diff --git a/orbbec_camera/src/ros_service.cpp b/orbbec_camera/src/ros_service.cpp index 4fd14f55..a03a20eb 100644 --- a/orbbec_camera/src/ros_service.cpp +++ b/orbbec_camera/src/ros_service.cpp @@ -1745,6 +1745,21 @@ void OBCameraNode::getDeviceConfigCallback(const std::shared_ptrgetCurrentPresetDepthWorkModeVersion(); + 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(); diff --git a/orbbec_camera/tools/firmware_update_tool.cpp b/orbbec_camera/tools/firmware_update_tool.cpp index 3fdb97b5..57133a2f 100644 --- a/orbbec_camera/tools/firmware_update_tool.cpp +++ b/orbbec_camera/tools/firmware_update_tool.cpp @@ -423,7 +423,15 @@ void logCurrentPresetList(const rclcpp::Logger &logger, const std::shared_ptrgetCount(); 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, diff --git a/orbbec_camera/tools/list_camera_profile.cpp b/orbbec_camera/tools/list_camera_profile.cpp index 78c03a6f..3e31a706 100644 --- a/orbbec_camera/tools/list_camera_profile.cpp +++ b/orbbec_camera/tools/list_camera_profile.cpp @@ -205,7 +205,16 @@ void printPreset(const std::shared_ptr& 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; } } diff --git a/orbbec_camera/tools/list_devices_node.cpp b/orbbec_camera/tools/list_devices_node.cpp index 6cdfcb36..274a25a7 100644 --- a/orbbec_camera/tools/list_devices_node.cpp +++ b/orbbec_camera/tools/list_devices_node.cpp @@ -181,7 +181,15 @@ void printPresetInfo(const std::shared_ptr &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) << ")"); } } diff --git a/orbbec_camera_msgs/srv/GetDeviceConfig.srv b/orbbec_camera_msgs/srv/GetDeviceConfig.srv index e7cdb8ab..42a8ba09 100644 --- a/orbbec_camera_msgs/srv/GetDeviceConfig.srv +++ b/orbbec_camera_msgs/srv/GetDeviceConfig.srv @@ -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