Merge branch 'feat/preset-version-support' into v2/develop

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

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

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

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

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

orbbec_camera/include/orbbec_camera/ob_camera_node.h

@@ -1022,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/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'),

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

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

orbbec_camera/src/ob_camera_node.cpp

@@ -922,8 +922,11 @@ void OBCameraNode::clean() noexcept {
 }
 
 void OBCameraNode::setupDevices() {
-  if (!depth_work_mode_.empty() &&
+  if (is_playback_device_ && (!depth_work_mode_.empty() || !device_preset_.empty())) {
-      device_->isPropertySupported(OB_STRUCT_CURRENT_DEPTH_ALG_MODE, OB_PERMISSION_READ_WRITE)) {
+    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()) {
@@ -4738,6 +4781,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);

orbbec_camera/src/ros_service.cpp

@@ -1939,6 +1939,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/tools/firmware_update_tool.cpp

@@ -423,7 +423,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,

orbbec_camera/tools/list_camera_profile.cpp

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

@@ -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/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