feat: add application config synchronization from JSON file

orbbec_camera/include/orbbec_camera/ob_camera_node.h

@@ -249,6 +249,8 @@ class OBCameraNode {
 
   bool isConfigJsonLoaded() const;
 
+  void syncConfigJsonApplicationConfig();
+
   void syncConfigJsonDeviceSettings();
 
   void syncConfigJsonFilterSettings(const std::vector<std::shared_ptr<ob::Filter>>& filters,

orbbec_camera/launch/gemini_330_series_sdk_json.launch.py

@@ -79,46 +79,19 @@ def generate_launch_description():
         DeclareLaunchArgument('load_config_json_file_path', default_value=''),
         DeclareLaunchArgument('export_config_json_file_path', default_value=''),
 
-        DeclareLaunchArgument('color_width', default_value='0'),
-        DeclareLaunchArgument('color_height', default_value='0'),
-        DeclareLaunchArgument('color_fps', default_value='0'),
-        DeclareLaunchArgument('color_format', default_value='ANY'),
-        DeclareLaunchArgument('enable_color', default_value='true'),
         DeclareLaunchArgument('color_qos', default_value='default'),
         DeclareLaunchArgument('color_camera_info_qos', default_value='default'),
 
-        DeclareLaunchArgument('depth_width', default_value='0'),
-        DeclareLaunchArgument('depth_height', default_value='0'),
-        DeclareLaunchArgument('depth_fps', default_value='0'),
-        DeclareLaunchArgument('depth_format', default_value='ANY'),
-        DeclareLaunchArgument('enable_depth', default_value='true'),
         DeclareLaunchArgument('depth_qos', default_value='default'),
         DeclareLaunchArgument('depth_camera_info_qos', default_value='default'),
-        DeclareLaunchArgument('depth_registration', default_value='false'),
 
-        DeclareLaunchArgument('left_ir_width', default_value='0'),
-        DeclareLaunchArgument('left_ir_height', default_value='0'),
-        DeclareLaunchArgument('left_ir_fps', default_value='0'),
-        DeclareLaunchArgument('left_ir_format', default_value='ANY'),
-        DeclareLaunchArgument('enable_left_ir', default_value='false'),
         DeclareLaunchArgument('left_ir_qos', default_value='default'),
         DeclareLaunchArgument('left_ir_camera_info_qos', default_value='default'),
 
-        DeclareLaunchArgument('right_ir_width', default_value='0'),
-        DeclareLaunchArgument('right_ir_height', default_value='0'),
-        DeclareLaunchArgument('right_ir_fps', default_value='0'),
-        DeclareLaunchArgument('right_ir_format', default_value='ANY'),
-        DeclareLaunchArgument('enable_right_ir', default_value='false'),
         DeclareLaunchArgument('right_ir_qos', default_value='default'),
         DeclareLaunchArgument('right_ir_camera_info_qos', default_value='default'),
 
         DeclareLaunchArgument('enable_sync_output_accel_gyro', default_value='false'),
-        DeclareLaunchArgument('enable_accel', default_value='false'),
-        DeclareLaunchArgument('accel_rate', default_value='200hz'),
-        DeclareLaunchArgument('accel_range', default_value='4g'),
-        DeclareLaunchArgument('enable_gyro', default_value='false'),
-        DeclareLaunchArgument('gyro_rate', default_value='200hz'),
-        DeclareLaunchArgument('gyro_range', default_value='1000dps'),
         DeclareLaunchArgument('linear_accel_cov', default_value='0.01'),
         DeclareLaunchArgument('angular_vel_cov', default_value='0.01'),
 
@@ -128,25 +101,14 @@ def generate_launch_description():
         DeclareLaunchArgument('color_info_url', default_value=''),
         DeclareLaunchArgument('enable_publish_extrinsic', default_value='false'),
 
-        DeclareLaunchArgument('enable_point_cloud', default_value='true'),
-        DeclareLaunchArgument('enable_colored_point_cloud', default_value='false'),
-        DeclareLaunchArgument('point_cloud_decimation_filter_factor', default_value='1'),
         DeclareLaunchArgument('point_cloud_qos', default_value='default'),
         DeclareLaunchArgument('cloud_frame_id', default_value=''),
         DeclareLaunchArgument('ordered_pc', default_value='false'),
         DeclareLaunchArgument('enable_depth_scale', default_value='true'),
-        DeclareLaunchArgument('align_mode', default_value='SW'),
-        DeclareLaunchArgument('align_target_stream', default_value='COLOR'),
 
-        DeclareLaunchArgument('enable_frame_sync', default_value='true'),
-        DeclareLaunchArgument('frame_aggregate_mode', default_value='ANY'),
         DeclareLaunchArgument('time_domain', default_value='global'),
         DeclareLaunchArgument('enable_frame_drop_log', default_value='false'),
         DeclareLaunchArgument('frame_timestamp_csv_file', default_value=''),
-        DeclareLaunchArgument('enable_color_undistortion', default_value='false'),
-        DeclareLaunchArgument('enable_depth_undistortion', default_value='false'),
-        DeclareLaunchArgument('enable_left_ir_undistortion', default_value='false'),
-        DeclareLaunchArgument('enable_right_ir_undistortion', default_value='false'),
         DeclareLaunchArgument('enable_d2c_viewer', default_value='false'),
         DeclareLaunchArgument('show_fps_enable', default_value='false'),
 

orbbec_camera/src/ob_camera_node.cpp

@@ -80,6 +80,30 @@ std::filesystem::path resolveConfigJsonFilePath(const std::string &file_path) {
   return path.lexically_normal();
 }
 
+bool configJsonContainsApplicationConfig(const std::string &file_path,
+                                         const rclcpp::Logger &logger) {
+  if (file_path.empty()) {
+    return false;
+  }
+
+  const auto resolved_file_path = resolveConfigJsonFilePath(file_path);
+  std::ifstream config_file(resolved_file_path);
+  if (!config_file.good()) {
+    return false;
+  }
+
+  try {
+    nlohmann::json config_json;
+    config_file >> config_json;
+    return config_json.contains("application_config");
+  } catch (const std::exception &e) {
+    RCLCPP_WARN_STREAM(logger, "Config JSON application_config check failed file="
+                                   << resolved_file_path.string() << " error=\"" << e.what()
+                                   << "\"");
+  }
+  return false;
+}
+
 std::string getDepthFilterStatusParamName(const std::string &filter_name,
                                           const std::string &param_name) {
   if (filter_name == "SpatialAdvancedFilter" && param_name == "disp_diff") {
@@ -1753,6 +1777,148 @@ bool OBCameraNode::isLaunchParamProvided(const std::string &param_name) const {
   return initial_ros_params_.find(param_name) != initial_ros_params_.end();
 }
 
+void OBCameraNode::syncConfigJsonApplicationConfig() {
+  if (!config_json_loaded_ ||
+      !configJsonContainsApplicationConfig(load_config_json_file_path_, logger_)) {
+    return;
+  }
+
+  try {
+    if (!ob::ApplicationConfig::isSupported(device_)) {
+      RCLCPP_WARN_STREAM(logger_,
+                         "Config JSON application_config is ignored because this device does not "
+                         "support SDK application config");
+      return;
+    }
+
+    auto application_config = ob::ApplicationConfig::get(device_);
+    CHECK_NOTNULL(application_config.get());
+
+    for (const auto &sensor_config : application_config->sensors()) {
+      if (!sensor_config || !sensor_config->streamProfile()) {
+        continue;
+      }
+
+      auto profile = sensor_config->streamProfile();
+      const stream_index_pair stream_index{profile->getType(), 0};
+      auto stream_name_it = stream_name_.find(stream_index);
+      if (stream_name_it == stream_name_.end()) {
+        RCLCPP_DEBUG_STREAM(logger_, "Config JSON application_config skips unsupported stream type="
+                                         << profile->getType());
+        continue;
+      }
+
+      const auto &stream_name = stream_name_it->second;
+      if (!isLaunchParamProvided("enable_" + stream_name)) {
+        enable_stream_[stream_index] = sensor_config->isStreamEnabled();
+      }
+
+      if (std::find(IMAGE_STREAMS.begin(), IMAGE_STREAMS.end(), stream_index) !=
+          IMAGE_STREAMS.end()) {
+        auto video_profile = profile->as<ob::VideoStreamProfile>();
+        if (!isLaunchParamProvided(stream_name + "_width")) {
+          width_[stream_index] = static_cast<int>(video_profile->width());
+        }
+        if (!isLaunchParamProvided(stream_name + "_height")) {
+          height_[stream_index] = static_cast<int>(video_profile->height());
+        }
+        if (!isLaunchParamProvided(stream_name + "_fps")) {
+          fps_[stream_index] = static_cast<int>(video_profile->fps());
+        }
+        if (!isLaunchParamProvided(stream_name + "_format")) {
+          format_[stream_index] = video_profile->format();
+          format_str_[stream_index] = OBFormatToString(format_[stream_index]);
+        }
+        if (!isLaunchParamProvided("enable_" + stream_name + "_undistortion")) {
+          enable_undistortion_[stream_index] = sensor_config->isUndistortionEnabled();
+        }
+      } else if (stream_index == ACCEL) {
+        auto accel_profile = profile->as<ob::AccelStreamProfile>();
+        if (!isLaunchParamProvided("accel_rate")) {
+          imu_rate_[ACCEL] = sampleRateToString(accel_profile->sampleRate());
+        }
+        if (!isLaunchParamProvided("accel_range")) {
+          imu_range_[ACCEL] = fullAccelScaleRangeToString(accel_profile->fullScaleRange());
+        }
+      } else if (stream_index == GYRO) {
+        auto gyro_profile = profile->as<ob::GyroStreamProfile>();
+        if (!isLaunchParamProvided("gyro_rate")) {
+          imu_rate_[GYRO] = sampleRateToString(gyro_profile->sampleRate());
+        }
+        if (!isLaunchParamProvided("gyro_range")) {
+          imu_range_[GYRO] = fullGyroScaleRangeToString(gyro_profile->fullScaleRange());
+        }
+      }
+    }
+
+    auto point_cloud_config = application_config->pointCloud();
+    if (point_cloud_config) {
+      const auto point_cloud_enabled = point_cloud_config->isEnabled();
+      const auto point_cloud_format = point_cloud_config->format();
+      if (!isLaunchParamProvided("enable_point_cloud")) {
+        enable_point_cloud_ = point_cloud_enabled && point_cloud_format == OB_FORMAT_POINT;
+      }
+      if (!isLaunchParamProvided("enable_colored_point_cloud")) {
+        enable_colored_point_cloud_ =
+            point_cloud_enabled && point_cloud_format == OB_FORMAT_RGB_POINT;
+      }
+      if (!isLaunchParamProvided("point_cloud_decimation_filter_factor")) {
+        point_cloud_decimation_filter_factor_ = point_cloud_config->decimationFactor();
+      }
+      if (!isLaunchParamProvided("enable_frame_sync")) {
+        enable_frame_sync_ = point_cloud_config->isFrameSyncEnabled();
+      }
+      if (!isLaunchParamProvided("frame_aggregate_mode")) {
+        frame_aggregate_mode_ = point_cloud_config->isAllFrameTypeRequired() ? "full_frame" : "ANY";
+      }
+
+      const auto align_mode = point_cloud_config->alignMode();
+      if (!isLaunchParamProvided("depth_registration")) {
+        depth_registration_ = align_mode != ALIGN_DISABLE;
+      }
+      if (!isLaunchParamProvided("align_mode")) {
+        if (align_mode == ALIGN_D2C_HW_MODE) {
+          align_mode_ = "HW";
+        } else if (align_mode == ALIGN_D2C_SW_MODE || align_mode == ALIGN_C2D_SW_MODE) {
+          align_mode_ = "SW";
+        }
+      }
+      if (!isLaunchParamProvided("align_target_stream")) {
+        if (align_mode == ALIGN_D2C_HW_MODE || align_mode == ALIGN_D2C_SW_MODE) {
+          align_target_stream_ = OB_STREAM_COLOR;
+        } else if (align_mode == ALIGN_C2D_SW_MODE) {
+          align_target_stream_ = OB_STREAM_DEPTH;
+        }
+      }
+    }
+
+    auto hdr_merge_config = application_config->hdrMerge();
+    if (hdr_merge_config && !isLaunchParamProvided("enable_hdr_merge")) {
+      enable_hdr_merge_ = hdr_merge_config->isEnabled();
+    }
+
+    auto device_decimation_config = application_config->deviceDecimation();
+    if (device_decimation_config && device_decimation_config->isEnabled() &&
+        !isLaunchParamProvided("preset_resolution_config")) {
+      const auto &config = device_decimation_config->presetResolutionConfig();
+      std::ostringstream ss;
+      ss << config.width << "," << config.height << "," << config.irDecimationFactor << ","
+         << config.depthDecimationFactor;
+      preset_resolution_config_ = ss.str();
+    }
+
+    RCLCPP_INFO_STREAM(logger_, "Config JSON application_config synced");
+  } catch (const ob::Error &e) {
+    RCLCPP_WARN_STREAM(logger_, "Config JSON application_config sync failed error=\""
+                                    << orbbec_camera::formatObErrorWithStatus(e) << "\"");
+  } catch (const std::exception &e) {
+    RCLCPP_WARN_STREAM(logger_,
+                       "Config JSON application_config sync failed error=\"" << e.what() << "\"");
+  } catch (...) {
+    RCLCPP_WARN_STREAM(logger_, "Config JSON application_config sync failed");
+  }
+}
+
 void OBCameraNode::syncConfigJsonDeviceSettings() {
   if (!config_json_loaded_) {
     return;
@@ -3883,6 +4049,7 @@ void OBCameraNode::setupTopics() {
     captureInitialRosParameters();
     getParameters();
     loadConfigJson();
+    syncConfigJsonApplicationConfig();
     setupDevices();
     setupDepthPostProcessFilter();
     setupColorPostProcessFilter();