/*******************************************************************************
 * Copyright (c) 2023 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/ob_camera_node.h"
#include <rclcpp/rclcpp.hpp>
#include <thread>
#include <geometry_msgs/msg/transform_stamped.hpp>
#include <sstream>
#include <algorithm>
#include <cctype>
#include <cmath>
#include <cstdlib>
#include <queue>
#include <unordered_map>
#include <unordered_set>
#include <vector>
#include <magic_enum/magic_enum.hpp>

#include "orbbec_camera/utils.h"
#include <filesystem>
#include <fstream>
#include "diagnostic_msgs/msg/diagnostic_status.hpp"
#include "libobsensor/hpp/Utils.hpp"

#if defined(USE_RK_HW_DECODER)
#include "orbbec_camera/rk_mpp_decoder.h"
#elif defined(USE_NV_HW_DECODER)
#include "orbbec_camera/jetson_nv_decoder.h"
#endif

#include <malloc.h>

namespace orbbec_camera {
using namespace std::chrono_literals;

std::string OBCameraNode::normalizeDepthFilterName(const std::string &filter_name) {
  if (filter_name == "HardwareNoiseRemoval") {
    return "HardwareNoiseRemovalFilter";
  }
  if (filter_name == "SpatialFilter") {
    return "SpatialAdvancedFilter";
  }
  if (filter_name == "DispOutliers" || filter_name == "DepthOutliersFilter") {
    return "DispOutliersFilter";
  }
  if (filter_name == "EnhancedDepth" || filter_name == "EnhancedDepthFilter") {
    return "EnhancedDepthFilter";
  }
  return filter_name;
}

namespace {

constexpr char kEnhancedDepthSupportedTargetResolutions[] = "640x480/1280x720/1280x800";
constexpr char kEnhancedDepthSupportedDepthFormats[] = "Y10/Y11/Y12/Y14/Y16/Z16";

std::string getDepthFilterStatusName(const std::string &filter_name) {
  if (filter_name == "SpatialAdvancedFilter") {
    return "SpatialFilter";
  }
  if (filter_name == "DisparityTransform") {
    return "DisparityToDepth";
  }
  return filter_name;
}

const std::unordered_map<OBFormat, OBConvertFormat> &enhancedDepthColorFormatMap() {
  static const std::unordered_map<OBFormat, OBConvertFormat> kFormatMap = {
      {OB_FORMAT_YUYV, FORMAT_YUYV_TO_RGB}, {OB_FORMAT_UYVY, FORMAT_UYVY_TO_RGB},
      {OB_FORMAT_MJPG, FORMAT_MJPG_TO_RGB}, {OB_FORMAT_BGR, FORMAT_BGR_TO_RGB},
      {OB_FORMAT_RGBA, FORMAT_RGBA_TO_RGB}, {OB_FORMAT_Y16, FORMAT_Y16_TO_RGB},
      {OB_FORMAT_Y8, FORMAT_Y8_TO_RGB},
  };
  return kFormatMap;
}

bool isEnhancedDepthColorFormatSupported(OBFormat format) {
  return format == OB_FORMAT_RGB || enhancedDepthColorFormatMap().count(format) > 0;
}

std::filesystem::path resolveConfigJsonFilePath(const std::string &file_path) {
  std::filesystem::path path(file_path);
  if ((file_path == "~" || file_path.rfind("~/", 0) == 0) && std::getenv("HOME") != nullptr) {
    path = std::filesystem::path(std::getenv("HOME"));
    if (file_path.size() > 2) {
      path /= file_path.substr(2);
    }
  }
  if (path.is_relative()) {
    path = std::filesystem::absolute(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") {
    return "diff_threshold";
  }
  if (filter_name == "SpatialModerateFilter" && param_name == "disp_diff") {
    return "diff_threshold";
  }
  if (filter_name == "TemporalFilter" && param_name == "diff_scale") {
    return "diff_threshold";
  }
  if (filter_name == "DecimationFilter" && param_name == "decimate") {
    return "scale";
  }
  return param_name;
}

std::string getDepthFilterConfigParamName(const std::string &filter_name,
                                          const std::string &param_name) {
  if ((filter_name == "SpatialAdvancedFilter" || filter_name == "SpatialModerateFilter") &&
      param_name == "diff_threshold") {
    return "disp_diff";
  }
  if (filter_name == "TemporalFilter" && param_name == "diff_threshold") {
    return "diff_scale";
  }
  if (filter_name == "DecimationFilter" && param_name == "scale") {
    return "decimate";
  }
  if (filter_name == "SequenceIdFilter" && (param_name == "id" || param_name == "sequence_id" ||
                                            param_name == "sequence_id_filter_id")) {
    return "sequenceid";
  }
  if (filter_name == "HoleFillingFilter" && param_name == "mode") {
    return "hole_filling_mode";
  }
  return param_name;
}

bool shouldExposeDepthFilterParams(const std::string &filter_name) {
  return filter_name != "MgcNoiseRemovalFilter" && filter_name != "LutNoiseRemovalFilter" &&
         filter_name != "DisparityTransform" && filter_name != "EdgeNoiseRemovalFilter";
}

std::string formatFilterConfigValue(const OBFilterConfigSchemaItem &config_schema, double value) {
  switch (config_schema.type) {
    case OB_FILTER_CONFIG_VALUE_TYPE_INT: {
      return std::to_string(static_cast<long long>(value));
    }
    case OB_FILTER_CONFIG_VALUE_TYPE_BOOLEAN:
      return value != 0.0 ? std::string("true") : std::string("false");
    case OB_FILTER_CONFIG_VALUE_TYPE_FLOAT:
    default: {
      std::ostringstream ss;
      ss << value;
      return ss.str();
    }
  }
}

std::string trimFilterConfigValue(const std::string &value) {
  auto begin = value.begin();
  while (begin != value.end() && std::isspace(static_cast<unsigned char>(*begin))) {
    ++begin;
  }
  auto end = value.end();
  while (end != begin && std::isspace(static_cast<unsigned char>(*(end - 1)))) {
    --end;
  }
  return std::string(begin, end);
}

std::string lowerFilterConfigValue(std::string value) {
  std::transform(value.begin(), value.end(), value.begin(),
                 [](unsigned char c) { return static_cast<char>(std::tolower(c)); });
  return value;
}

bool equalsIgnoreCase(std::string lhs, std::string rhs) {
  std::transform(lhs.begin(), lhs.end(), lhs.begin(),
                 [](unsigned char c) { return static_cast<char>(std::tolower(c)); });
  std::transform(rhs.begin(), rhs.end(), rhs.begin(),
                 [](unsigned char c) { return static_cast<char>(std::tolower(c)); });
  return lhs == rhs;
}

std::string dispOutliersSearchModeToString(int search_mode) {
  switch (search_mode) {
    case 0:
      return "FULL";
    case 1:
      return "OFFSET_80";
    default:
      return "";
  }
}

bool parseDispOutliersSearchMode(const std::string &raw_value, int &search_mode,
                                 std::string &message, bool allow_sdk_default = false) {
  const auto value = trimFilterConfigValue(raw_value);
  const auto lower_value = lowerFilterConfigValue(value);
  if (allow_sdk_default && lower_value.empty()) {
    search_mode = -1;
    return true;
  }
  if (lower_value == "full") {
    search_mode = 0;
    return true;
  }
  if (lower_value == "offset_80") {
    search_mode = 1;
    return true;
  }

  message = allow_sdk_default
                ? "Filter config 'search_mode' expects one of FULL, OFFSET_80, or empty"
                : "Filter config 'search_mode' expects one of FULL, OFFSET_80";
  return false;
}

std::string lowerParameterValue(std::string value) {
  std::transform(value.begin(), value.end(), value.begin(),
                 [](unsigned char c) { return static_cast<char>(std::tolower(c)); });
  return value;
}

std::string formatParameterValue(const std::string &value) {
  return value.empty() ? std::string("\"\"") : "'" + value + "'";
}

std::string formatValidParameterValues(const std::vector<std::string> &valid_values) {
  std::stringstream ss;
  for (size_t i = 0; i < valid_values.size(); ++i) {
    if (i != 0) {
      ss << ", ";
    }
    ss << formatParameterValue(valid_values[i]);
  }
  return ss.str();
}

std::string normalizeClosedSetParameterValue(const rclcpp::Logger &logger,
                                             const std::string &param_name,
                                             const std::string &value,
                                             const std::vector<std::string> &valid_values,
                                             const std::string &default_value) {
  const auto lower_value = lowerParameterValue(value);
  for (const auto &valid_value : valid_values) {
    if (lower_value == lowerParameterValue(valid_value)) {
      return valid_value;
    }
  }

  RCLCPP_ERROR_STREAM(logger, "Invalid parameter "
                                  << param_name << " " << formatParameterValue(value)
                                  << ". Valid values: " << formatValidParameterValues(valid_values)
                                  << ". Skip setting and use "
                                  << formatParameterValue(default_value) << ".");
  return default_value;
}

bool parseFilterConfigDouble(const std::string &raw_value, double &parsed_value,
                             std::string &message) {
  const auto value = trimFilterConfigValue(raw_value);
  if (value.empty()) {
    message = "Filter config value is empty";
    return false;
  }

  try {
    size_t parsed_chars = 0;
    parsed_value = std::stod(value, &parsed_chars);
    if (parsed_chars != value.size()) {
      message = "Filter config value '" + raw_value + "' is not a valid number";
      return false;
    }
  } catch (const std::exception &) {
    message = "Filter config value '" + raw_value + "' is not a valid number";
    return false;
  }
  return true;
}

bool parseFilterConfigValue(const OBFilterConfigSchemaItem &schema, const std::string &raw_value,
                            double &parsed_value, std::string &message) {
  const auto value = trimFilterConfigValue(raw_value);
  if (schema.type == OB_FILTER_CONFIG_VALUE_TYPE_BOOLEAN) {
    const auto lower_value = lowerFilterConfigValue(value);
    if (lower_value == "true" || lower_value == "1") {
      parsed_value = 1.0;
      return true;
    }
    if (lower_value == "false" || lower_value == "0") {
      parsed_value = 0.0;
      return true;
    }
    message = "Filter config '" + std::string(schema.name) + "' expects a boolean value";
    return false;
  }

  if (!parseFilterConfigDouble(value, parsed_value, message)) {
    return false;
  }

  if (schema.type == OB_FILTER_CONFIG_VALUE_TYPE_INT && std::floor(parsed_value) != parsed_value) {
    message = "Filter config '" + std::string(schema.name) + "' expects an integer value";
    return false;
  }

  if (parsed_value < schema.min || parsed_value > schema.max) {
    std::ostringstream ss;
    ss << "Filter config '" << schema.name << "' value " << parsed_value << " is out of range ["
       << schema.min << ", " << schema.max << "]";
    message = ss.str();
    return false;
  }

  return true;
}

int64_t getSystemNowUs() {
  return std::chrono::duration_cast<std::chrono::microseconds>(
             std::chrono::system_clock::now().time_since_epoch())
      .count();
}

int64_t getSteadyNowUs() {
  return std::chrono::duration_cast<std::chrono::microseconds>(
             std::chrono::steady_clock::now().time_since_epoch())
      .count();
}

}  // namespace

void OBCameraNode::appendDepthFilterParam(DepthFilterState &filter_state, const std::string &name,
                                          const std::string &value) {
  orbbec_camera_msgs::msg::DepthFilterParam param;
  param.name = name;
  param.value = value;
  filter_state.params.push_back(param);
}

DepthFilterState OBCameraNode::buildDepthFilterState(
    const std::string &filter_name, bool enabled, const std::shared_ptr<ob::Filter> &filter) const {
  const auto normalized_filter_name = normalizeDepthFilterName(filter_name);
  DepthFilterState filter_state;
  filter_state.filter_name = getDepthFilterStatusName(normalized_filter_name);
  filter_state.enabled = enabled;
  auto to_param_value = [](const auto &value) {
    std::ostringstream ss;
    ss << value;
    return ss.str();
  };

  if (normalized_filter_name == "NoiseRemovalFilter") {
    appendDepthFilterParam(filter_state, "min_diff",
                           to_param_value(noise_removal_filter_min_diff_));
    appendDepthFilterParam(filter_state, "max_size",
                           to_param_value(noise_removal_filter_max_size_));
  } else if (normalized_filter_name == "HardwareNoiseRemovalFilter") {
    appendDepthFilterParam(filter_state, "threshold",
                           to_param_value(hardware_noise_removal_filter_threshold_));
  } else if (normalized_filter_name == "DispOutliersFilter") {
    appendDepthFilterParam(filter_state, "search_mode",
                           dispOutliersSearchModeToString(disp_outliers_filter_search_mode_));
  }

  if (filter_state.params.empty() && filter &&
      shouldExposeDepthFilterParams(normalized_filter_name)) {
    auto format_filter_config_value = [](const OBFilterConfigSchemaItem &config_schema,
                                         double value) {
      switch (config_schema.type) {
        case OB_FILTER_CONFIG_VALUE_TYPE_INT: {
          return std::to_string(static_cast<long long>(value));
        }
        case OB_FILTER_CONFIG_VALUE_TYPE_BOOLEAN:
          return value != 0.0 ? std::string("true") : std::string("false");
        case OB_FILTER_CONFIG_VALUE_TYPE_FLOAT:
        default: {
          std::ostringstream ss;
          ss << value;
          return ss.str();
        }
      }
    };

    try {
      for (const auto &config_schema : filter->getConfigSchemaVec()) {
        if (config_schema.name == nullptr || config_schema.name[0] == '\0') {
          continue;
        }
        appendDepthFilterParam(
            filter_state, getDepthFilterStatusParamName(normalized_filter_name, config_schema.name),
            format_filter_config_value(config_schema, filter->getConfigValue(config_schema.name)));
      }
    } catch (const std::exception &) {
      // Keep the state without dynamic params if runtime querying fails.
    }
  }

  return filter_state;
}

DepthFilterState OBCameraNode::buildEnhancedDepthFilterState() const {
  DepthFilterState filter_state;
  filter_state.filter_name = "EnhancedDepthFilter";
  filter_state.enabled = enable_enhanced_depth_.load();
  appendDepthFilterParam(filter_state, "confidence_threshold",
                         std::to_string(enhanced_depth_confidence_threshold_));
  return filter_state;
}

void OBCameraNode::publishDepthFiltersStatus() {
  if (!depth_filters_status_pub_) {
    return;
  }

  std::vector<std::shared_ptr<ob::Filter>> depth_filters_snapshot;
  {
    std::lock_guard<std::mutex> depth_filter_lock(depth_filter_mutex_);
    depth_filters_snapshot = depth_filter_list_;
  }

  auto find_depth_filter =
      [&depth_filters_snapshot](const std::string &filter_name) -> std::shared_ptr<ob::Filter> {
    const auto normalized_name = normalizeDepthFilterName(filter_name);
    auto it = std::find_if(depth_filters_snapshot.begin(), depth_filters_snapshot.end(),
                           [&normalized_name](const auto &filter) {
                             return normalizeDepthFilterName(filter->type()) == normalized_name ||
                                    normalizeDepthFilterName(filter->getName()) == normalized_name;
                           });
    if (it == depth_filters_snapshot.end()) {
      return nullptr;
    }
    return *it;
  };

  auto sync_filter_enabled = [&find_depth_filter](const std::string &filter_name,
                                                  bool &cached_state) {
    auto filter = find_depth_filter(filter_name);
    if (!filter) {
      return;
    }
    try {
      cached_state = filter->isEnabled();
    } catch (const std::exception &) {
      // Keep the cached value if runtime querying fails.
    }
  };

  sync_filter_enabled("DecimationFilter", enable_decimation_filter_);
  sync_filter_enabled("HDRMerge", enable_hdr_merge_);
  sync_filter_enabled("SequenceIdFilter", enable_sequence_id_filter_);
  sync_filter_enabled("SpatialAdvancedFilter", enable_spatial_filter_);
  sync_filter_enabled("TemporalFilter", enable_temporal_filter_);
  sync_filter_enabled("HoleFillingFilter", enable_hole_filling_filter_);
  sync_filter_enabled("EdgeNoiseRemovalFilter", enable_edge_noise_removal_filter_);
  sync_filter_enabled("DisparityTransform", enable_disparity_to_depth_);
  sync_filter_enabled("ThresholdFilter", enable_threshold_filter_);
  sync_filter_enabled("SpatialFastFilter", enable_spatial_fast_filter_);
  sync_filter_enabled("SpatialModerateFilter", enable_spatial_moderate_filter_);
  sync_filter_enabled("FalsePositiveFilter", enable_false_positive_filter_);
  sync_filter_enabled("MgcNoiseRemovalFilter", enable_mgc_noise_removal_filter_);
  sync_filter_enabled("LutNoiseRemovalFilter", enable_lut_noise_removal_filter_);

  if (device_->isPropertySupported(OB_PROP_DEPTH_SOFT_FILTER_BOOL, OB_PERMISSION_READ_WRITE)) {
    try {
      enable_noise_removal_filter_ = device_->getBoolProperty(OB_PROP_DEPTH_SOFT_FILTER_BOOL);
    } catch (const std::exception &) {
      // Keep the cached value if runtime querying fails.
    }
  }
  if (device_->isPropertySupported(OB_PROP_DEPTH_MAX_DIFF_INT, OB_PERMISSION_WRITE)) {
    try {
      noise_removal_filter_min_diff_ = device_->getIntProperty(OB_PROP_DEPTH_MAX_DIFF_INT);
    } catch (const std::exception &) {
      // Keep the cached value if runtime querying fails.
    }
  }
  if (device_->isPropertySupported(OB_PROP_DEPTH_MAX_SPECKLE_SIZE_INT, OB_PERMISSION_WRITE)) {
    try {
      noise_removal_filter_max_size_ = device_->getIntProperty(OB_PROP_DEPTH_MAX_SPECKLE_SIZE_INT);
    } catch (const std::exception &) {
      // Keep the cached value if runtime querying fails.
    }
  }
  if (device_->isPropertySupported(OB_PROP_HW_NOISE_REMOVE_FILTER_ENABLE_BOOL,
                                   OB_PERMISSION_READ_WRITE)) {
    try {
      enable_hardware_noise_removal_filter_ =
          device_->getBoolProperty(OB_PROP_HW_NOISE_REMOVE_FILTER_ENABLE_BOOL);
    } catch (const std::exception &) {
      // Keep the cached value if runtime querying fails.
    }
  }
  if (device_->isPropertySupported(OB_PROP_HW_NOISE_REMOVE_FILTER_THRESHOLD_FLOAT,
                                   OB_PERMISSION_READ_WRITE)) {
    try {
      hardware_noise_removal_filter_threshold_ =
          device_->getFloatProperty(OB_PROP_HW_NOISE_REMOVE_FILTER_THRESHOLD_FLOAT);
    } catch (const std::exception &) {
      // Keep the cached value if runtime querying fails.
    }
  }
  if (device_->isPropertySupported(OB_PROP_DEPTH_OUTLIERS_FILTER_BOOL, OB_PERMISSION_READ_WRITE)) {
    try {
      enable_disp_outliers_filter_ = device_->getBoolProperty(OB_PROP_DEPTH_OUTLIERS_FILTER_BOOL);
    } catch (const std::exception &) {
      // Keep the cached value if runtime querying fails.
    }
  }
  if (device_->isPropertySupported(OB_PROP_DEPTH_OUTLIERS_FILTER_SEARCH_MODE_INT,
                                   OB_PERMISSION_READ_WRITE)) {
    try {
      disp_outliers_filter_search_mode_ =
          device_->getIntProperty(OB_PROP_DEPTH_OUTLIERS_FILTER_SEARCH_MODE_INT);
    } catch (const std::exception &) {
      // Keep the cached value if runtime querying fails.
    }
  }

  if (auto filter = find_depth_filter("DecimationFilter")) {
    try {
      decimation_filter_scale_ =
          static_cast<int>(filter->as<ob::DecimationFilter>()->getScaleValue());
    } catch (const std::exception &) {
      // Keep the cached value if runtime querying fails.
    }
  }
  if (auto filter = find_depth_filter("SequenceIdFilter")) {
    try {
      sequence_id_filter_id_ = filter->as<ob::SequenceIdFilter>()->getSelectSequenceId();
    } catch (const std::exception &) {
      // Keep the cached value if runtime querying fails.
    }
  }
  if (auto filter = find_depth_filter("ThresholdFilter")) {
    try {
      threshold_filter_min_ = static_cast<int>(filter->getConfigValue("min"));
      threshold_filter_max_ = static_cast<int>(filter->getConfigValue("max"));
    } catch (const std::exception &) {
      // Keep the cached value if runtime querying fails.
    }
  }
  if (auto filter = find_depth_filter("SpatialAdvancedFilter")) {
    try {
      auto params = filter->as<ob::SpatialAdvancedFilter>()->getFilterParams();
      spatial_filter_alpha_ = params.alpha;
      spatial_filter_diff_threshold_ = params.disp_diff;
      spatial_filter_magnitude_ = params.magnitude;
      spatial_filter_radius_ = params.radius;
    } catch (const std::exception &) {
      // Keep the cached value if runtime querying fails.
    }
  }
  if (auto filter = find_depth_filter("TemporalFilter")) {
    try {
      temporal_filter_diff_threshold_ = static_cast<float>(filter->getConfigValue("diff_scale"));
      temporal_filter_weight_ = static_cast<float>(filter->getConfigValue("weight"));
    } catch (const std::exception &) {
      // Keep the cached value if runtime querying fails.
    }
  }
  if (auto filter = find_depth_filter("SpatialFastFilter")) {
    try {
      auto params = filter->as<ob::SpatialFastFilter>()->getFilterParams();
      spatial_fast_filter_radius_ = params.radius;
    } catch (const std::exception &) {
      // Keep the cached value if runtime querying fails.
    }
  }
  if (auto filter = find_depth_filter("SpatialModerateFilter")) {
    try {
      auto params = filter->as<ob::SpatialModerateFilter>()->getFilterParams();
      spatial_moderate_filter_diff_threshold_ = params.disp_diff;
      spatial_moderate_filter_magnitude_ = params.magnitude;
      spatial_moderate_filter_radius_ = params.radius;
    } catch (const std::exception &) {
      // Keep the cached value if runtime querying fails.
    }
  }

  DepthFiltersStatus msg;
  msg.header.stamp = node_->now();
  msg.header.frame_id = camera_name_;

  const bool noise_removal_filter_supported =
      device_->isPropertySupported(OB_PROP_DEPTH_SOFT_FILTER_BOOL, OB_PERMISSION_READ_WRITE) ||
      device_->isPropertySupported(OB_PROP_DEPTH_MAX_DIFF_INT, OB_PERMISSION_WRITE) ||
      device_->isPropertySupported(OB_PROP_DEPTH_MAX_SPECKLE_SIZE_INT, OB_PERMISSION_WRITE);
  const bool hardware_noise_removal_filter_supported =
      device_->isPropertySupported(OB_PROP_HW_NOISE_REMOVE_FILTER_ENABLE_BOOL,
                                   OB_PERMISSION_READ_WRITE) ||
      device_->isPropertySupported(OB_PROP_HW_NOISE_REMOVE_FILTER_THRESHOLD_FLOAT,
                                   OB_PERMISSION_READ_WRITE);
  const bool disp_outliers_filter_supported =
      device_->isPropertySupported(OB_PROP_DEPTH_OUTLIERS_FILTER_BOOL, OB_PERMISSION_READ_WRITE) ||
      device_->isPropertySupported(OB_PROP_DEPTH_OUTLIERS_FILTER_SEARCH_MODE_INT,
                                   OB_PERMISSION_READ_WRITE);

  std::vector<std::string> ordered_filter_names;
  ordered_filter_names.reserve(depth_filters_snapshot.size() + 3);
  auto append_unique_filter_name = [&ordered_filter_names](const std::string &filter_name) {
    if (std::find(ordered_filter_names.begin(), ordered_filter_names.end(), filter_name) ==
        ordered_filter_names.end()) {
      ordered_filter_names.push_back(filter_name);
    }
  };
  for (const auto &filter : depth_filters_snapshot) {
    if (!filter) {
      continue;
    }
    append_unique_filter_name(normalizeDepthFilterName(filter->type()));
  }
  if (noise_removal_filter_supported) {
    append_unique_filter_name("NoiseRemovalFilter");
  }
  if (hardware_noise_removal_filter_supported) {
    append_unique_filter_name("HardwareNoiseRemovalFilter");
  }
  if (disp_outliers_filter_supported) {
    append_unique_filter_name("DispOutliersFilter");
  }
  append_unique_filter_name("EnhancedDepthFilter");

  msg.filters.reserve(ordered_filter_names.size());
  for (const auto &filter_name : ordered_filter_names) {
    if (filter_name == "EnhancedDepthFilter") {
      msg.filters.push_back(buildEnhancedDepthFilterState());
      continue;
    }
    bool enabled = false;
    auto filter = find_depth_filter(filter_name);
    if (filter_name == "NoiseRemovalFilter") {
      enabled = enable_noise_removal_filter_;
    } else if (filter_name == "HardwareNoiseRemovalFilter") {
      enabled = enable_hardware_noise_removal_filter_;
    } else if (filter_name == "DispOutliersFilter") {
      enabled = enable_disp_outliers_filter_;
    }
    if (filter && filter_name != "DispOutliersFilter") {
      try {
        enabled = filter->isEnabled();
      } catch (const std::exception &) {
        // Keep default value when runtime querying fails.
      }
    }
    msg.filters.push_back(buildDepthFilterState(filter_name, enabled, filter));
  }
  depth_filters_status_pub_->publish(msg);
}

void OBCameraNode::publishLrmObstacleDistance() {
  if (!lrm_obstacle_distance_pub_) {
    return;
  }
  if (lrm_obstacle_distance_pub_->get_subscription_count() == 0) {
    return;
  }
  try {
    std_msgs::msg::Int32 msg;
    {
      std::lock_guard<decltype(device_lock_)> lock(device_lock_);
      msg.data = device_->getIntProperty(OB_PROP_LDP_MEASURE_DISTANCE_INT);
    }
    lrm_obstacle_distance_pub_->publish(msg);
  } catch (const ob::Error &e) {
    auto message = orbbec_camera::formatObErrorWithStatus(e);
    RCLCPP_WARN_THROTTLE(logger_, *node_->get_clock(), 5000,
                         "Failed to publish LRM obstacle distance: %s", message.c_str());
  } catch (const std::exception &e) {
    RCLCPP_WARN_THROTTLE(logger_, *node_->get_clock(), 5000,
                         "Failed to publish LRM obstacle distance: %s", e.what());
  } catch (...) {
    RCLCPP_WARN_THROTTLE(logger_, *node_->get_clock(), 5000,
                         "Failed to publish LRM obstacle distance: unknown error");
  }
}

OBCameraNode::OBCameraNode(rclcpp::Node *node, std::shared_ptr<ob::Device> device,
                           std::shared_ptr<Parameters> parameters, bool use_intra_process,
                           bool is_playback_device)
    : node_(node),
      device_(std::move(device)),
      parameters_(std::move(parameters)),
      logger_(node->get_logger()),
      use_intra_process_(use_intra_process),
      is_playback_device_(is_playback_device) {
  pid_ = device_->getDeviceInfo()->getPid();
  RCLCPP_INFO_STREAM(logger_,
                     "OBCameraNode: use_intra_process: " << (use_intra_process ? "ON" : "OFF"));
  is_running_.store(true);
  stream_name_[COLOR] = "color";
  stream_name_[COLOR_LEFT] = "left_color";
  stream_name_[COLOR_RIGHT] = "right_color";
  stream_name_[DEPTH] = "depth";
  stream_name_[INFRA0] = "ir";
  stream_name_[INFRA1] = "left_ir";
  stream_name_[INFRA2] = "right_ir";
  stream_name_[ACCEL] = "accel";
  stream_name_[GYRO] = "gyro";
  compression_params_.push_back(cv::IMWRITE_PNG_COMPRESSION);
  compression_params_.push_back(0);
  compression_params_.push_back(cv::IMWRITE_PNG_STRATEGY);
  compression_params_.push_back(cv::IMWRITE_PNG_STRATEGY_DEFAULT);
  setupDefaultImageFormat();
  setupTopics();

  if (enable_frame_drop_log_ || !frame_timestamp_csv_file_.empty()) {
    if (enable_frame_sync_ && (enable_stream_[COLOR] || enable_stream_[DEPTH])) {
      frame_timestamp_csv_logger_ = std::make_unique<FrameTimestampCsvLogger>(
          enable_frame_drop_log_, frame_timestamp_csv_file_,
          FrameTimestampCsvLogger::OutputMode::SYNCED, logger_);
      if (!frame_timestamp_csv_logger_->enabled()) {
        frame_timestamp_csv_logger_.reset();
      }
    } else {
      if (enable_stream_[COLOR]) {
        color_timestamp_csv_logger_ = std::make_unique<FrameTimestampCsvLogger>(
            enable_frame_drop_log_, frame_timestamp_csv_file_,
            FrameTimestampCsvLogger::OutputMode::COLOR, logger_);
        if (!color_timestamp_csv_logger_->enabled()) {
          color_timestamp_csv_logger_.reset();
        }
      }
      if (enable_stream_[DEPTH]) {
        depth_timestamp_csv_logger_ = std::make_unique<FrameTimestampCsvLogger>(
            enable_frame_drop_log_, frame_timestamp_csv_file_,
            FrameTimestampCsvLogger::OutputMode::DEPTH, logger_);
        if (!depth_timestamp_csv_logger_->enabled()) {
          depth_timestamp_csv_logger_.reset();
        }
      }
    }
    if (!frame_timestamp_csv_file_.empty() && (enable_stream_[ACCEL] || enable_stream_[GYRO])) {
      if (enable_sync_output_accel_gyro_) {
        imu_timestamp_csv_logger_ = std::make_unique<ImuTimestampCsvLogger>(
            frame_timestamp_csv_file_, ImuTimestampCsvLogger::OutputMode::SYNCED, logger_);
        if (!imu_timestamp_csv_logger_->enabled()) {
          imu_timestamp_csv_logger_.reset();
        }
      } else {
        if (enable_stream_[ACCEL]) {
          accel_timestamp_csv_logger_ = std::make_unique<ImuTimestampCsvLogger>(
              frame_timestamp_csv_file_, ImuTimestampCsvLogger::OutputMode::ACCEL, logger_);
          if (!accel_timestamp_csv_logger_->enabled()) {
            accel_timestamp_csv_logger_.reset();
          }
        }
        if (enable_stream_[GYRO]) {
          gyro_timestamp_csv_logger_ = std::make_unique<ImuTimestampCsvLogger>(
              frame_timestamp_csv_file_, ImuTimestampCsvLogger::OutputMode::GYRO, logger_);
          if (!gyro_timestamp_csv_logger_->enabled()) {
            gyro_timestamp_csv_logger_.reset();
          }
        }
      }
    }
  }

  if (enable_d2c_viewer_) {
    auto rgb_qos = getRMWQosProfileFromString(image_qos_[COLOR]);
    auto depth_qos = getRMWQosProfileFromString(image_qos_[DEPTH]);
    d2c_viewer_ = std::make_unique<D2CViewer>(node_, rgb_qos, depth_qos, use_intra_process_);
  }
  setupImageBuffers();
  is_camera_node_initialized_ = true;

  fps_counter_color_ = std::make_unique<FpsCounter>("Color", logger_, 1);
  fps_counter_depth_ = std::make_unique<FpsCounter>("Depth", logger_, 1);
  fps_counter_left_ir_ = std::make_unique<FpsCounter>("Left Ir", logger_, 1);
  fps_counter_right_ir_ = std::make_unique<FpsCounter>("Right Ir", logger_, 1);

  LogLevel log_level = LogLevel::DEBUG;
  if (show_fps_enable_) {
    log_level = LogLevel::INFO;
  }
  fps_counter_color_->setLogLevel(log_level);
  fps_counter_depth_->setLogLevel(log_level);
  fps_counter_left_ir_->setLogLevel(log_level);
  fps_counter_right_ir_->setLogLevel(log_level);

  fps_delay_status_color_ = std::make_unique<FpsDelayStatus>(logger_);
  fps_delay_status_depth_ = std::make_unique<FpsDelayStatus>(logger_);
}

template <class T>
void OBCameraNode::setAndGetNodeParameter(
    T &param, const std::string &param_name, const T &default_value,
    const rcl_interfaces::msg::ParameterDescriptor &parameter_descriptor) {
  try {
    param = parameters_
                ->setParam(param_name, rclcpp::ParameterValue(default_value),
                           std::function<void(const rclcpp::Parameter &)>(), parameter_descriptor)
                .get<T>();
  } catch (const rclcpp::ParameterTypeException &ex) {
    RCLCPP_ERROR_STREAM(logger_, "Failed to set parameter: " << param_name << ". " << ex.what());
    throw;
  }
}

OBCameraNode::~OBCameraNode() noexcept { clean(); }

void OBCameraNode::rebootDevice() {
  RCLCPP_DEBUG_STREAM(logger_, "Cleaning before rebooting device");
  malloc_trim(0);
  clean();
  malloc_trim(0);
  std::lock_guard<decltype(device_lock_)> lock(device_lock_);
  RCLCPP_INFO_STREAM(logger_, "Rebooting device");
  if (device_) {
    device_->reboot();
  }
  malloc_trim(0);
  RCLCPP_DEBUG_STREAM(logger_, "Reboot device complete");
}

void OBCameraNode::clean() noexcept {
  if (cleaning_.exchange(true)) {
    RCLCPP_DEBUG(logger_, "clean() already running, skip re-entry");
    return;
  }
  // Set running flags to false first to signal all operations to stop
  is_running_.store(false);
  is_camera_node_initialized_.store(false);

  try {
    if (frame_timestamp_csv_logger_) {
      frame_timestamp_csv_logger_->shutdown();
      frame_timestamp_csv_logger_.reset();
    }
  } catch (...) {
    RCLCPP_WARN_STREAM(logger_, "Exception while shutting down frame timestamp CSV logger");
  }
  try {
    if (color_timestamp_csv_logger_) {
      color_timestamp_csv_logger_->shutdown();
      color_timestamp_csv_logger_.reset();
    }
  } catch (...) {
    RCLCPP_WARN_STREAM(logger_, "Exception while shutting down color timestamp CSV logger");
  }
  try {
    if (depth_timestamp_csv_logger_) {
      depth_timestamp_csv_logger_->shutdown();
      depth_timestamp_csv_logger_.reset();
    }
  } catch (...) {
    RCLCPP_WARN_STREAM(logger_, "Exception while shutting down depth timestamp CSV logger");
  }
  try {
    if (imu_timestamp_csv_logger_) {
      imu_timestamp_csv_logger_->shutdown();
      imu_timestamp_csv_logger_.reset();
    }
  } catch (...) {
    RCLCPP_WARN_STREAM(logger_, "Exception while shutting down IMU timestamp CSV logger");
  }
  try {
    if (accel_timestamp_csv_logger_) {
      accel_timestamp_csv_logger_->shutdown();
      accel_timestamp_csv_logger_.reset();
    }
  } catch (...) {
    RCLCPP_WARN_STREAM(logger_, "Exception while shutting down accel timestamp CSV logger");
  }
  try {
    if (gyro_timestamp_csv_logger_) {
      gyro_timestamp_csv_logger_->shutdown();
      gyro_timestamp_csv_logger_.reset();
    }
  } catch (...) {
    RCLCPP_WARN_STREAM(logger_, "Exception while shutting down gyro timestamp CSV logger");
  }

  // Stop diagnostic timer and updater first BEFORE acquiring device_lock to prevent deadlock
  try {
    if (diagnostic_timer_) {
      diagnostic_timer_->cancel();
      // Wait for any currently executing timer callbacks to complete
      {
        std::unique_lock<std::mutex> lk(diagnostic_mutex_);
        diagnostic_cv_.wait(lk, [this]() { return !diagnostic_running_; });
      }
      diagnostic_timer_.reset();
    }
    if (software_trigger_timer_) {
      software_trigger_timer_->cancel();
      software_trigger_timer_.reset();
    }
    if (lrm_obstacle_distance_timer_) {
      lrm_obstacle_distance_timer_->cancel();
      lrm_obstacle_distance_timer_.reset();
    }
    if (diagnostic_updater_) {
      diagnostic_updater_.reset();
    }
  } catch (...) {
    // Ignore exceptions during diagnostic cleanup
  }

  // Now acquire the device lock for the rest of the cleanup
  std::lock_guard<decltype(device_lock_)> lock(device_lock_);
  RCLCPP_DEBUG_STREAM(logger_, "Do OBCameraNode clean");

  RCLCPP_DEBUG_STREAM(logger_, "Stop tf thread");
  try {
    if (tf_thread_ && tf_thread_->joinable()) {
      tf_cv_.notify_all();  // Wake up tf thread if it's waiting
      tf_thread_->join();
    }
  } catch (...) {
    RCLCPP_DEBUG_STREAM(logger_, "Exception while stopping tf thread");
  }

  RCLCPP_DEBUG_STREAM(logger_, "Stop color frame thread");
  try {
    stopColorFrameThreads();
  } catch (...) {
    RCLCPP_DEBUG_STREAM(logger_, "Exception while stopping color frame thread");
  }

  RCLCPP_DEBUG_STREAM(logger_, "stop streams");
  try {
    stopIMU();
    stopStreams();
    {
      std::lock_guard<std::mutex> lk(frame_info_logged_mutex_);
      frame_info_logged_.clear();
    }
  } catch (...) {
    RCLCPP_DEBUG_STREAM(logger_, "Exception while stopping streams");
  }

  // Clean up d2c_viewer_ before cleaning buffers
  RCLCPP_DEBUG_STREAM(logger_, "Clean d2c_viewer");
  try {
    if (d2c_viewer_) {
      d2c_viewer_.reset();
    }
  } catch (...) {
    RCLCPP_DEBUG_STREAM(logger_, "Exception while cleaning up d2c_viewer");
  }

  RCLCPP_DEBUG_STREAM(logger_, "Clean up buffers");
  try {
    delete[] rgb_buffer_;
    rgb_buffer_ = nullptr;
    rgb_buffer_size_ = 0;
    delete[] rgb_buffer_left_;
    rgb_buffer_left_ = nullptr;
    rgb_buffer_left_size_ = 0;
    delete[] rgb_buffer_right_;
    rgb_buffer_right_ = nullptr;
    rgb_buffer_right_size_ = 0;

    if (jpeg_decoder_) {
      jpeg_decoder_.reset();
    }
    if (jpeg_decoder_left_) {
      jpeg_decoder_left_.reset();
    }
    if (jpeg_decoder_right_) {
      jpeg_decoder_right_.reset();
    }
  } catch (...) {
    RCLCPP_DEBUG_STREAM(logger_, "Exception while cleaning up buffers");
  }

  RCLCPP_DEBUG_STREAM(logger_, "OBCameraNode cleanup complete");
  cleaning_.store(false);
}

void OBCameraNode::setupDevices() {
  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);
    }
    TRY_EXECUTE_BLOCK(device_->switchDepthWorkMode(depth_work_mode_.c_str()));
    RCLCPP_INFO_STREAM(logger_, "Set device preset: " << depth_work_mode_);
  } else if (!device_preset_.empty()) {
    try {
      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));
      }
      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));
    } catch (const std::exception &e) {
      RCLCPP_ERROR_STREAM(logger_, "Failed to load device preset: " << e.what());
    } catch (...) {
      RCLCPP_ERROR_STREAM(logger_, "Failed to load device preset");
    }
  }

  if (!preset_resolution_config_.empty()) {
    OBPresetResolutionConfig presetResolutionConfig;
    std::istringstream iss(preset_resolution_config_);
    std::string token;
    std::vector<int> values;
    values.reserve(4);
    while (std::getline(iss, token, ',')) {
      values.push_back(std::stoi(token));
    }

    if (values.size() >= 4) {
      presetResolutionConfig.width = values[0];
      presetResolutionConfig.height = values[1];
      presetResolutionConfig.irDecimationFactor = values[2];
      presetResolutionConfig.depthDecimationFactor = values[3];
    } else {
      RCLCPP_WARN_STREAM(
          logger_,
          "Invalid preset_resolution_config parameter. "
          "Expected format: width,height,ir_decimation_factor,depth_decimation_factor");
    }

    RCLCPP_INFO_STREAM(
        logger_, "Set preset resolution config: "
                     << "width=" << presetResolutionConfig.width
                     << ", height=" << presetResolutionConfig.height
                     << ", ir_decimation=" << presetResolutionConfig.irDecimationFactor
                     << ", depth_decimation=" << presetResolutionConfig.depthDecimationFactor);

    TRY_EXECUTE_BLOCK(device_->setStructuredData(OB_STRUCT_PRESET_RESOLUTION_CONFIG,
                                                 (uint8_t *)&presetResolutionConfig,
                                                 sizeof(presetResolutionConfig)));
  }

  auto sensor_list = device_->getSensorList();
  for (size_t i = 0; i < sensor_list->getCount(); i++) {
    auto sensor = sensor_list->getSensor(i);
    auto profiles = sensor->getStreamProfileList();
    for (size_t j = 0; j < profiles->getCount(); j++) {
      auto profile = profiles->getProfile(j);
      stream_index_pair sip{profile->getType(), 0};
      if (sensors_.find(sip) != sensors_.end()) {
        continue;
      }
      sensors_[sip] = sensor;
    }
  }

  for (const auto &[stream_index, enable] : enable_stream_) {
    if (enable && sensors_.find(stream_index) == sensors_.end()) {
      RCLCPP_DEBUG_STREAM(logger_, magic_enum::enum_name(stream_index.first)
                                       << " sensor not supported by current device, skipping");
      enable_stream_[stream_index] = false;
    }
  }
  auto device_info = device_->getDeviceInfo();
  CHECK_NOTNULL(device_info);

  if (is_playback_device_) {
    // Everything below this point only tunes real-hardware-only behavior
    // (heartbeat, USB3 retry, laser, depth limits, multi-device sync, PTP,
    // etc.). Those SDK device components aren't registered on a playback
    // device, so querying them throws instead of returning "not supported".
    // None of it is needed to correctly publish a recorded .bag file.
    return;
  }

  auto should_apply_launch_config = [this](const std::string &param_name) {
    return isLaunchParamProvided(param_name);
  };

  if (should_apply_launch_config("retry_on_usb3_detection_failure") &&
      retry_on_usb3_detection_failure_ &&
      device_->isPropertySupported(OB_PROP_DEVICE_USB3_REPEAT_IDENTIFY_BOOL,
                                   OB_PERMISSION_READ_WRITE)) {
    TRY_TO_SET_PROPERTY(setBoolProperty, OB_PROP_DEVICE_USB3_REPEAT_IDENTIFY_BOOL,
                        retry_on_usb3_detection_failure_);
  }
  if (sync_io_voltage_level_ != -1 &&
      device_->isPropertySupported(OB_PROP_USB_SYNC_VOLTAGE_LEVEL_INT, OB_PERMISSION_READ_WRITE)) {
    auto range = device_->getIntPropertyRange(OB_PROP_USB_SYNC_VOLTAGE_LEVEL_INT);
    if (sync_io_voltage_level_ < range.min || sync_io_voltage_level_ > range.max) {
      RCLCPP_ERROR_STREAM(
          logger_, "sync IO voltage level is out of range " << range.min << " - " << range.max);
    } 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));
    }
  }
  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"));
  }
  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"));
  }
  if (should_apply_launch_config("enable_firmware_log")) {
    device_->enableFirmwareLog(enable_firmware_log_);
    RCLCPP_INFO_STREAM(logger_, "Set firmware log to " << (enable_firmware_log_ ? "ON" : "OFF"));
  }
  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));
  }
  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));
  }
  if (laser_energy_level_ != -1 &&
      device_->isPropertySupported(OB_PROP_LASER_ENERGY_LEVEL_INT, OB_PERMISSION_READ_WRITE)) {
    auto range = device_->getIntPropertyRange(OB_PROP_LASER_ENERGY_LEVEL_INT);
    if (laser_energy_level_ < range.min || laser_energy_level_ > range.max) {
      RCLCPP_ERROR_STREAM(logger_,
                          "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);
    }
  }
  if (depth_registration_ && align_mode_ == "SW") {
    RCLCPP_DEBUG_STREAM(logger_, "Create align filter");
    align_filter_ = std::make_unique<ob::Align>(align_target_stream_);
    RCLCPP_INFO_STREAM(logger_, "set align mode to " << align_mode_);
  }
  if (should_apply_launch_config("disparity_to_depth_mode") && !disparity_to_depth_mode_.empty() &&
      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 {
    if (should_apply_launch_config("enable_ldp") &&
        device_->isPropertySupported(OB_PROP_LDP_BOOL, OB_PERMISSION_READ_WRITE)) {
      if (device_->isPropertySupported(OB_PROP_LASER_CONTROL_INT, OB_PERMISSION_READ_WRITE)) {
        auto laser_enable = device_->getIntProperty(OB_PROP_LASER_CONTROL_INT);
        device_->setBoolProperty(OB_PROP_LDP_BOOL, enable_ldp_);
        device_->setIntProperty(OB_PROP_LASER_CONTROL_INT, laser_enable);
      } else if (device_->isPropertySupported(OB_PROP_LASER_BOOL, OB_PERMISSION_READ_WRITE)) {
        if (!enable_ldp_) {
          auto laser_enable = device_->getIntProperty(OB_PROP_LASER_BOOL);
          device_->setBoolProperty(OB_PROP_LDP_BOOL, enable_ldp_);
          std::this_thread::sleep_for(std::chrono::milliseconds(3));
          device_->setIntProperty(OB_PROP_LASER_BOOL, laser_enable);
        } else {
          device_->setBoolProperty(OB_PROP_LDP_BOOL, enable_ldp_);
        }
      }
      RCLCPP_INFO_STREAM(
          logger_, "Current LDP: " << (device_->getBoolProperty(OB_PROP_LDP_BOOL) ? "ON" : "OFF"));
    }
  } catch (const ob::Error &e) {
    RCLCPP_WARN_STREAM(logger_,
                       "Skipping LDP configuration: " << orbbec_camera::formatObErrorWithStatus(e));
  } catch (const std::exception &e) {
    RCLCPP_WARN_STREAM(logger_, "Skipping LDP configuration: " << e.what());
  }
  if (ldp_power_level_ != -1 &&
      device_->isPropertySupported(OB_PROP_LASER_POWER_LEVEL_CONTROL_INT, OB_PERMISSION_WRITE)) {
    auto range = device_->getIntPropertyRange(OB_PROP_LASER_POWER_LEVEL_CONTROL_INT);
    if (ldp_power_level_ < range.min || ldp_power_level_ > range.max) {
      RCLCPP_ERROR(logger_, "ldp power level value is out of range[%d,%d], please check the value",
                   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));
    }
  }
  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"));
  }
  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(
        logger_,
        "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);
      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(
        logger_, "Current PTP Config: "
                     << (device_->getBoolProperty(OB_DEVICE_PTP_CLOCK_SYNC_ENABLE_BOOL) ? "ON"
                                                                                        : "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));
    }
  } else if (device_->isPropertySupported(OB_PROP_DEPTH_UNIT_FLEXIBLE_ADJUSTMENT_FLOAT,
                                          OB_PERMISSION_READ_WRITE) &&
             !depth_precision_str_.empty()) {
    auto depth_unit_flexible_adjustment = depthPrecisionFromString(depth_precision_str_);
    auto range = device_->getFloatPropertyRange(OB_PROP_DEPTH_UNIT_FLEXIBLE_ADJUSTMENT_FLOAT);
    RCLCPP_INFO_STREAM(logger_,
                       "Depth unit flexible adjustment range: " << range.min << " - " << range.max);
    if (depth_unit_flexible_adjustment < range.min || depth_unit_flexible_adjustment > range.max) {
      RCLCPP_ERROR_STREAM(
          logger_, "depth unit flexible adjustment value is out of range, please check the value");
    } else {
      TRY_TO_SET_PROPERTY(setFloatProperty, OB_PROP_DEPTH_UNIT_FLEXIBLE_ADJUSTMENT_FLOAT,
                          depth_unit_flexible_adjustment);
      RCLCPP_INFO_STREAM(
          logger_, "Current depth unit: "
                       << device_->getFloatProperty(OB_PROP_DEPTH_UNIT_FLEXIBLE_ADJUSTMENT_FLOAT)
                       << "mm");
    }
  }

  for (const auto &stream_index : IMAGE_STREAMS) {
    if (enable_stream_[stream_index]) {
      OBPropertyID mirrorPropertyID = OB_PROP_DEPTH_MIRROR_BOOL;
      if (stream_index == COLOR) {
        mirrorPropertyID = OB_PROP_COLOR_MIRROR_BOOL;
      } else if (stream_index == DEPTH) {
        mirrorPropertyID = OB_PROP_DEPTH_MIRROR_BOOL;
      } else if (stream_index == INFRA0) {
        mirrorPropertyID = OB_PROP_IR_MIRROR_BOOL;
      } else if (stream_index == INFRA1) {
        mirrorPropertyID = OB_PROP_IR_MIRROR_BOOL;
      } else if (stream_index == INFRA2) {
        mirrorPropertyID = OB_PROP_IR_RIGHT_MIRROR_BOOL;
      } else if (stream_index == COLOR_LEFT) {
        mirrorPropertyID = OB_PROP_COLOR_LEFT_MIRROR_BOOL;
      } else if (stream_index == COLOR_RIGHT) {
        mirrorPropertyID = OB_PROP_COLOR_RIGHT_MIRROR_BOOL;
      }
      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(
            logger_, "Current " << stream_name_[stream_index] << " mirror: "
                                << (device_->getBoolProperty(mirrorPropertyID) ? "ON" : "OFF"));
      }
      OBPropertyID flipPropertyID = OB_PROP_DEPTH_FLIP_BOOL;
      if (stream_index == COLOR) {
        flipPropertyID = OB_PROP_COLOR_FLIP_BOOL;
      } else if (stream_index == DEPTH) {
        flipPropertyID = OB_PROP_DEPTH_FLIP_BOOL;
      } else if (stream_index == INFRA0) {
        flipPropertyID = OB_PROP_IR_FLIP_BOOL;
      } else if (stream_index == INFRA1) {
        flipPropertyID = OB_PROP_IR_FLIP_BOOL;
      } else if (stream_index == INFRA2) {
        flipPropertyID = OB_PROP_IR_RIGHT_FLIP_BOOL;
      } else if (stream_index == COLOR_LEFT) {
        flipPropertyID = OB_PROP_COLOR_LEFT_FLIP_BOOL;
      } else if (stream_index == COLOR_RIGHT) {
        flipPropertyID = OB_PROP_COLOR_RIGHT_FLIP_BOOL;
      }
      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"));
      }
      OBPropertyID rotationPropertyID = OB_PROP_DEPTH_ROTATE_INT;
      if (stream_index == COLOR) {
        rotationPropertyID = OB_PROP_COLOR_ROTATE_INT;
      } else if (stream_index == DEPTH) {
        rotationPropertyID = OB_PROP_DEPTH_ROTATE_INT;
      } else if (stream_index == INFRA0) {
        rotationPropertyID = OB_PROP_IR_ROTATE_INT;
      } else if (stream_index == INFRA1) {
        rotationPropertyID = OB_PROP_IR_ROTATE_INT;
      } else if (stream_index == INFRA2) {
        rotationPropertyID = OB_PROP_IR_RIGHT_ROTATE_INT;
      } else if (stream_index == COLOR_LEFT) {
        rotationPropertyID = OB_PROP_COLOR_LEFT_ROTATE_INT;
      } else if (stream_index == COLOR_RIGHT) {
        rotationPropertyID = OB_PROP_COLOR_RIGHT_ROTATE_INT;
      }
      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));
      }
    }
  }

  if (should_apply_launch_config("enable_color_auto_white_balance") &&
      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(
        logger_,
        "Current color auto white balance: "
            << (device_->getBoolProperty(OB_PROP_COLOR_AUTO_WHITE_BALANCE_BOOL) ? "ON" : "OFF"));
  }
  if (should_apply_launch_config("color_preset") && !color_preset_.empty()) {
    try {
      if (!device_->isColorPresetSupported()) {
        RCLCPP_WARN_STREAM(logger_, "Color preset is not supported by this device");
      } else {
        auto color_preset_list = device_->getColorPresetList();
        std::string selected_preset;
        std::ostringstream supported_presets;
        bool has_supported_preset = false;
        const uint32_t preset_count = color_preset_list ? color_preset_list->getCount() : 0;
        for (uint32_t i = 0; i < preset_count; ++i) {
          const char *preset_name = color_preset_list->getName(i);
          if (preset_name == nullptr || preset_name[0] == '\0') {
            continue;
          }
          if (has_supported_preset) {
            supported_presets << ", ";
          }
          supported_presets << preset_name;
          has_supported_preset = true;
          if (equalsIgnoreCase(color_preset_, preset_name)) {
            selected_preset = preset_name;
          }
        }

        if (selected_preset.empty()) {
          RCLCPP_WARN_STREAM(logger_, "Unsupported color_preset: " << color_preset_
                                                                   << ". Supported values: "
                                                                   << supported_presets.str());
        } else {
          device_->switchColorPreset(selected_preset.c_str());
          const char *current_preset = device_->getCurrentColorPresetName();
          color_preset_ = current_preset != nullptr ? current_preset : selected_preset;
          RCLCPP_INFO_STREAM(logger_, "Current color preset: " << color_preset_);
        }
      }
    } catch (const ob::Error &e) {
      RCLCPP_WARN_STREAM(
          logger_, "Failed to set color preset: " << orbbec_camera::formatObErrorWithStatus(e));
    } catch (const std::exception &e) {
      RCLCPP_WARN_STREAM(logger_, "Failed to set color preset: " << e.what());
    }
  }
  if (color_exposure_ != -1 &&
      device_->isPropertySupported(OB_PROP_COLOR_EXPOSURE_INT, OB_PERMISSION_WRITE)) {
    auto range = device_->getIntPropertyRange(OB_PROP_COLOR_EXPOSURE_INT);
    if (color_exposure_ < range.min || color_exposure_ > range.max) {
      RCLCPP_ERROR(logger_, "color exposure value is out of range[%d,%d], please check the value",
                   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));
    }
  }
  if (color_gain_ != -1 &&
      device_->isPropertySupported(OB_PROP_COLOR_GAIN_INT, OB_PERMISSION_WRITE)) {
    auto range = device_->getIntPropertyRange(OB_PROP_COLOR_GAIN_INT);
    if (color_gain_ < range.min || color_gain_ > range.max) {
      RCLCPP_ERROR(logger_, "color gain value is out of range[%d,%d], please check the value",
                   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));
    }
  }
  if (color_mjpeg_quality_ != -1) {
    if (!device_->isPropertySupported(OB_PROP_MJPEG_QUALITY_INT, OB_PERMISSION_WRITE)) {
      RCLCPP_WARN_STREAM(logger_, "color_mjpeg_quality is not supported by this device");
    } else {
      auto range = device_->getIntPropertyRange(OB_PROP_MJPEG_QUALITY_INT);
      if (color_mjpeg_quality_ < range.min || color_mjpeg_quality_ > range.max) {
        RCLCPP_ERROR(logger_,
                     "color MJPEG quality value is out of range[%d,%d], please check the value",
                     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));
      }
    }
  }
  if (should_apply_launch_config("enable_color_auto_exposure_priority") &&
      device_->isPropertySupported(OB_PROP_COLOR_AUTO_EXPOSURE_PRIORITY_INT, OB_PERMISSION_WRITE)) {
    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(
        logger_,
        "Current color auto exposure priority: "
            << (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(
        logger_,
        "Current color auto exposure: "
            << (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)) {
    auto range = device_->getIntPropertyRange(OB_PROP_COLOR_WHITE_BALANCE_INT);
    if (color_white_balance_ < range.min || color_white_balance_ > range.max) {
      RCLCPP_ERROR(logger_,
                   "color white balance value is out of range[%d,%d], please check the value",
                   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));
    }
  }

  if (color_ae_max_exposure_ != -1 &&
      device_->isPropertySupported(OB_PROP_COLOR_AE_MAX_EXPOSURE_INT, OB_PERMISSION_WRITE)) {
    auto range = device_->getIntPropertyRange(OB_PROP_COLOR_AE_MAX_EXPOSURE_INT);
    if (color_ae_max_exposure_ < range.min || color_ae_max_exposure_ > range.max) {
      RCLCPP_ERROR(logger_,
                   "color AE max exposure value is out of range[%d,%d], please check the value",
                   range.min, range.max);
    } 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));
    }
  }
  if (color_ae_max_gain_ != -1 &&
      device_->isPropertySupported(OB_PROP_COLOR_AE_MAX_GAIN_INT, OB_PERMISSION_WRITE)) {
    auto range = device_->getIntPropertyRange(OB_PROP_COLOR_AE_MAX_GAIN_INT);
    if (color_ae_max_gain_ < range.min || color_ae_max_gain_ > range.max) {
      RCLCPP_ERROR(logger_,
                   "color AE max gain value is out of range[%d,%d], please check the value",
                   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));
    }
  }
  if (color_brightness_ != -1 &&
      device_->isPropertySupported(OB_PROP_COLOR_BRIGHTNESS_INT, OB_PERMISSION_WRITE)) {
    auto range = device_->getIntPropertyRange(OB_PROP_COLOR_BRIGHTNESS_INT);
    if (color_brightness_ < range.min || color_brightness_ > range.max) {
      RCLCPP_ERROR(logger_, "color brightness value is out of range[%d,%d], please check the value",
                   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));
    }
  }
  if (color_roi_brightness_ != -1 &&
      device_->isPropertySupported(OB_PROP_COLOR_ROI_BRIGHTNESS_INT, OB_PERMISSION_WRITE)) {
    auto range = device_->getIntPropertyRange(OB_PROP_COLOR_ROI_BRIGHTNESS_INT);
    if (color_roi_brightness_ < range.min || color_roi_brightness_ > range.max) {
      RCLCPP_ERROR(logger_,
                   "color roi brightness value is out of range[%d,%d], please check the value",
                   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));
    }
  }
  if (color_sharpness_ != -1 &&
      device_->isPropertySupported(OB_PROP_COLOR_SHARPNESS_INT, OB_PERMISSION_WRITE)) {
    auto range = device_->getIntPropertyRange(OB_PROP_COLOR_SHARPNESS_INT);
    if (color_sharpness_ < range.min || color_sharpness_ > range.max) {
      RCLCPP_ERROR(logger_, "color sharpness value is out of range[%d,%d], please check the value",
                   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));
    }
  }
  if (color_gamma_ != -1 &&
      device_->isPropertySupported(OB_PROP_COLOR_GAMMA_INT, OB_PERMISSION_WRITE)) {
    auto range = device_->getIntPropertyRange(OB_PROP_COLOR_GAMMA_INT);
    if (color_gamma_ < range.min || color_gamma_ > range.max) {
      RCLCPP_ERROR(logger_, "color gamm value is out of range[%d,%d], please check the value",
                   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));
    }
  }
  if (color_saturation_ != -1 &&
      device_->isPropertySupported(OB_PROP_COLOR_SATURATION_INT, OB_PERMISSION_WRITE)) {
    auto range = device_->getIntPropertyRange(OB_PROP_COLOR_SATURATION_INT);
    if (color_saturation_ < range.min || color_saturation_ > range.max) {
      RCLCPP_ERROR(logger_, "color saturation value is out of range[%d,%d], please check the value",
                   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));
    }
  }
  if (color_contrast_ != -1 &&
      device_->isPropertySupported(OB_PROP_COLOR_CONTRAST_INT, OB_PERMISSION_WRITE)) {
    auto range = device_->getIntPropertyRange(OB_PROP_COLOR_CONTRAST_INT);
    if (color_contrast_ < range.min || color_contrast_ > range.max) {
      RCLCPP_ERROR(logger_, "color contrast value is out of range[%d,%d], please check the value",
                   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));
    }
  }
  if (color_hue_ != -1 &&
      device_->isPropertySupported(OB_PROP_COLOR_HUE_INT, OB_PERMISSION_WRITE)) {
    auto range = device_->getIntPropertyRange(OB_PROP_COLOR_HUE_INT);
    if (color_hue_ < range.min || color_hue_ > range.max) {
      RCLCPP_ERROR(logger_, "color hue value is out of range[%d,%d], please check the value",
                   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));
    }
  }
  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));
  }
  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));
  }
  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"));
  }
  if (!color_powerline_freq_.empty() &&
      device_->isPropertySupported(OB_PROP_COLOR_POWER_LINE_FREQUENCY_INT, OB_PERMISSION_WRITE)) {
    if (color_powerline_freq_ == "disable") {
      TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_COLOR_POWER_LINE_FREQUENCY_INT, 0);
    } else if (color_powerline_freq_ == "50hz") {
      TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_COLOR_POWER_LINE_FREQUENCY_INT, 1);
    } else if (color_powerline_freq_ == "60hz") {
      TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_COLOR_POWER_LINE_FREQUENCY_INT, 2);
    } 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));
  }
  if (depth_exposure_ != -1 &&
      device_->isPropertySupported(OB_PROP_DEPTH_EXPOSURE_INT, OB_PERMISSION_WRITE)) {
    auto range = device_->getIntPropertyRange(OB_PROP_DEPTH_EXPOSURE_INT);
    if (depth_exposure_ < range.min || depth_exposure_ > range.max) {
      RCLCPP_ERROR(logger_, "depth exposure value is out of range[%d,%d], please check the value",
                   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));
    }
  }
  if (depth_gain_ != -1 &&
      device_->isPropertySupported(OB_PROP_DEPTH_GAIN_INT, OB_PERMISSION_WRITE)) {
    auto range = device_->getIntPropertyRange(OB_PROP_DEPTH_GAIN_INT);
    if (depth_gain_ < range.min || depth_gain_ > range.max) {
      RCLCPP_ERROR(logger_, "depth gain value is out of range[%d,%d], please check the value",
                   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));
    }
  }
  if (should_apply_launch_config("enable_depth_auto_exposure_priority") &&
      sensors_.find(DEPTH) != sensors_.end() &&
      device_->isPropertySupported(OB_PROP_DEPTH_AUTO_EXPOSURE_PRIORITY_INT, OB_PERMISSION_WRITE)) {
    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(
        logger_,
        "Current depth auto exposure priority: "
            << (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(
        logger_, "Current IR auto exposure: "
                     << (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)) {
    auto range = device_->getIntPropertyRange(OB_PROP_IR_BRIGHTNESS_INT);
    if (mean_intensity_set_point_ < range.min || mean_intensity_set_point_ > range.max) {
      RCLCPP_ERROR(logger_, "depth brightness value is out of range[%d,%d], please check the value",
                   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));
    }
  }
  // ir ae max
  if (ir_ae_max_exposure_ != -1 &&
      device_->isPropertySupported(OB_PROP_IR_AE_MAX_EXPOSURE_INT, OB_PERMISSION_WRITE)) {
    auto range = device_->getIntPropertyRange(OB_PROP_IR_AE_MAX_EXPOSURE_INT);
    if (ir_ae_max_exposure_ < range.min || ir_ae_max_exposure_ > range.max) {
      RCLCPP_ERROR(logger_,
                   "IR AE max exposure value is out of range[%d,%d], please check the value",
                   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));
    }
  }
  // ir brightness
  if (ir_brightness_ != -1 &&
      device_->isPropertySupported(OB_PROP_IR_BRIGHTNESS_INT, OB_PERMISSION_WRITE)) {
    auto range = device_->getIntPropertyRange(OB_PROP_IR_BRIGHTNESS_INT);
    if (ir_brightness_ < range.min || ir_brightness_ > range.max) {
      RCLCPP_ERROR(logger_, "IR brightness value is out of range[%d,%d], please check the value",
                   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));
    }
  }
  if (ir_exposure_ != -1 &&
      device_->isPropertySupported(OB_PROP_IR_EXPOSURE_INT, OB_PERMISSION_WRITE)) {
    auto range = device_->getIntPropertyRange(OB_PROP_IR_EXPOSURE_INT);
    if (ir_exposure_ < range.min || ir_exposure_ > range.max) {
      RCLCPP_ERROR(logger_, "ir exposure value is out of range[%d,%d], please check the value",
                   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));
    }
  }
  if (ir_gain_ != -1 && device_->isPropertySupported(OB_PROP_IR_GAIN_INT, OB_PERMISSION_WRITE)) {
    auto range = device_->getIntPropertyRange(OB_PROP_IR_GAIN_INT);
    if (ir_gain_ < range.min || ir_gain_ > range.max) {
      RCLCPP_ERROR(logger_, "ir gain value is out of range[%d,%d], please check the value",
                   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));
    }
  }
  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(
        logger_, "Current IR long exposure: "
                     << (device_->getBoolProperty(OB_PROP_IR_LONG_EXPOSURE_BOOL) ? "ON" : "OFF"));
  }

  if (should_apply_launch_config("enable_noise_removal_filter") && enable_noise_removal_filter_ &&
      sensors_.find(DEPTH) != sensors_.end() &&
      device_->isPropertySupported(OB_PROP_DEPTH_MAX_DIFF_INT, OB_PERMISSION_WRITE)) {
    auto default_noise_removal_filter_min_diff =
        device_->getIntProperty(OB_PROP_DEPTH_MAX_DIFF_INT);
    if (noise_removal_filter_min_diff_ != -1 &&
        default_noise_removal_filter_min_diff != noise_removal_filter_min_diff_) {
      auto range = device_->getIntPropertyRange(OB_PROP_DEPTH_MAX_DIFF_INT);
      if (noise_removal_filter_min_diff_ < range.min ||
          noise_removal_filter_min_diff_ > range.max) {
        RCLCPP_ERROR(logger_,
                     "noise removal filter min diff value is out of range[%d,%d], please check "
                     "the value",
                     range.min, range.max);
      } else {
        device_->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));
  }

  if (should_apply_launch_config("enable_noise_removal_filter") && enable_noise_removal_filter_ &&
      sensors_.find(DEPTH) != sensors_.end() &&
      device_->isPropertySupported(OB_PROP_DEPTH_MAX_SPECKLE_SIZE_INT, OB_PERMISSION_WRITE)) {
    auto default_noise_removal_filter_max_size =
        device_->getIntProperty(OB_PROP_DEPTH_MAX_SPECKLE_SIZE_INT);
    if (noise_removal_filter_max_size_ != -1 &&
        default_noise_removal_filter_max_size != noise_removal_filter_max_size_) {
      auto range = device_->getIntPropertyRange(OB_PROP_DEPTH_MAX_SPECKLE_SIZE_INT);
      if (noise_removal_filter_max_size_ < range.min ||
          noise_removal_filter_max_size_ > range.max) {
        RCLCPP_ERROR(logger_,
                     "noise removal filter max size value is out of range[%d,%d], please check "
                     "the value",
                     range.min, range.max);
      } else {
        device_->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));
  }
  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_);
    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_);
    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(
        logger_, "Current DispOutliersFilter search mode: "
                     << 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)) {
    if (disparity_range_mode_ == 64) {
      TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_DISP_SEARCH_RANGE_MODE_INT, 0);
    } else if (disparity_range_mode_ == 128) {
      TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_DISP_SEARCH_RANGE_MODE_INT, 1);
    } else if (disparity_range_mode_ == 256) {
      TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_DISP_SEARCH_RANGE_MODE_INT, 2);
    } 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));
  }
  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(
        logger_,
        "Set hardware noise removal filter to "
            << (device_->getBoolProperty(OB_PROP_HW_NOISE_REMOVE_FILTER_ENABLE_BOOL) ? "true"
                                                                                     : "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));
      }
    }
  }
  if (!exposure_range_mode_.empty() && exposure_range_mode_ != "default" &&
      device_->isPropertySupported(OB_PROP_DEVICE_PERFORMANCE_MODE_INT, OB_PERMISSION_WRITE)) {
    if (exposure_range_mode_ == "ultimate") {
      TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_DEVICE_PERFORMANCE_MODE_INT, 1);
    } else if (exposure_range_mode_ == "regular") {
      TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_DEVICE_PERFORMANCE_MODE_INT, 0);
    } 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));
  }
  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(
        logger_,
        "Current accel data correction: "
            << (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(
        logger_,
        "Current gyro data correction: "
            << (device_->getBoolProperty(OB_PROP_SDK_GYRO_FRAME_TRANSFORMED_BOOL) ? "ON" : "OFF"));
  }
  if (isGemini335PID(pid_) && !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);
    } else if (intra_camera_sync_reference_ == "Middle") {
      TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_INTRA_CAMERA_SYNC_REFERENCE_INT, 1);
    } else if (intra_camera_sync_reference_ == "End") {
      TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_INTRA_CAMERA_SYNC_REFERENCE_INT, 2);
    } 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));
  }
  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(
        logger_, "Current Sports Mode: "
                     << (device_->getIntProperty(OB_PROP_DEVICE_AE_STRATEGY_INT) == 0 ? "ON"
                                                                                      : "OFF"));
  }

  if (should_apply_launch_config("ae_reference_stream") &&
      (ae_reference_stream_ == "depth" || ae_reference_stream_ == "color") &&
      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"));
    }
  }
}

bool OBCameraNode::exportConfigJsonToFile(const std::string &file_path, std::string &message) {
  if (file_path.empty()) {
    message = "Config json export file path is empty";
    RCLCPP_ERROR_STREAM(logger_, message);
    return false;
  }

  try {
    const auto resolved_file_path = resolveConfigJsonFilePath(file_path);
    const auto parent_path = resolved_file_path.parent_path();
    if (!parent_path.empty()) {
      std::filesystem::create_directories(parent_path);
    }

    const auto resolved_file_path_str = resolved_file_path.string();
    syncApplicationSensorConfigForExport();
    syncApplicationPointCloudConfigForExport();
    syncApplicationHdrMergeConfigForExport();
    device_->exportSettingsAsPresetJsonFile(resolved_file_path_str.c_str());
    if (!std::filesystem::exists(resolved_file_path)) {
      message = "Failed to export config json file: file not found after export path=" +
                resolved_file_path_str;
      RCLCPP_ERROR_STREAM(logger_, message);
      return false;
    }

    message = "Exported config json file path: " + resolved_file_path_str;
    RCLCPP_INFO_STREAM(logger_, message);
    return true;
  } catch (const ob::Error &e) {
    message = "Failed to export config json file: " + orbbec_camera::formatObErrorWithStatus(e);
    RCLCPP_ERROR_STREAM(logger_, message);
  } catch (const std::exception &e) {
    message = std::string("Failed to export config json file: ") + e.what();
    RCLCPP_ERROR_STREAM(logger_, message);
  } catch (...) {
    message = "Failed to export config json file";
    RCLCPP_ERROR_STREAM(logger_, message);
  }
  return false;
}

void OBCameraNode::exportConfigJsonIfRequested() {
  if (export_config_json_file_path_.empty()) {
    return;
  }
  std::string message;
  exportConfigJsonToFile(export_config_json_file_path_, message);
}

bool OBCameraNode::isConfigJsonLoaded() const { return config_json_loaded_; }

void OBCameraNode::syncApplicationSensorConfigForExport() {
  if (!device_) {
    return;
  }

  try {
    if (!ob::ApplicationConfig::isSupported(device_)) {
      RCLCPP_DEBUG_STREAM(logger_, "Skip exporting application_config sensors: unsupported device");
      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;
      }

      const auto current_profile = sensor_config->streamProfile();
      const stream_index_pair stream_index{current_profile->getType(), 0};
      const auto stream_name_it = stream_name_.find(stream_index);
      if (stream_name_it == stream_name_.end()) {
        RCLCPP_DEBUG_STREAM(logger_,
                            "Skip exporting application_config sensor: unsupported stream type="
                                << magic_enum::enum_name(current_profile->getType()));
        continue;
      }

      auto export_sensor_config =
          std::make_shared<ob::ApplicationSensorConfig>(sensor_config->sensorType());
      const auto enable_stream_it = enable_stream_.find(stream_index);
      export_sensor_config->enableStream(enable_stream_it != enable_stream_.end()
                                             ? enable_stream_it->second
                                             : sensor_config->isStreamEnabled());

      const auto stream_profile_it = stream_profile_.find(stream_index);
      export_sensor_config->setStreamProfile(stream_profile_it != stream_profile_.end() &&
                                                     stream_profile_it->second
                                                 ? stream_profile_it->second
                                                 : current_profile);

      const auto enable_undistortion_it = enable_undistortion_.find(stream_index);
      export_sensor_config->enableUndistortion(enable_undistortion_it != enable_undistortion_.end()
                                                   ? enable_undistortion_it->second
                                                   : sensor_config->isUndistortionEnabled());

      application_config->setSensor(export_sensor_config);
    }
  } catch (const ob::Error &e) {
    RCLCPP_WARN_STREAM(logger_, "Export application_config sensors sync failed error=\""
                                    << orbbec_camera::formatObErrorWithStatus(e) << "\"");
  } catch (const std::exception &e) {
    RCLCPP_WARN_STREAM(
        logger_, "Export application_config sensors sync failed error=\"" << e.what() << "\"");
  } catch (...) {
    RCLCPP_WARN_STREAM(logger_, "Export application_config sensors sync failed");
  }
}

void OBCameraNode::syncApplicationPointCloudConfigForExport() {
  if (!device_) {
    return;
  }

  try {
    if (!ob::ApplicationConfig::isSupported(device_)) {
      RCLCPP_DEBUG_STREAM(logger_,
                          "Skip exporting application_config point_cloud: unsupported device");
      return;
    }

    auto application_config = ob::ApplicationConfig::get(device_);
    CHECK_NOTNULL(application_config.get());

    auto point_cloud_config = std::make_shared<ob::ApplicationPointCloudConfig>();
    point_cloud_config->enable(enable_point_cloud_ || enable_colored_point_cloud_);
    point_cloud_config->setFormat(enable_colored_point_cloud_ ? OB_FORMAT_RGB_POINT
                                                              : OB_FORMAT_POINT);
    point_cloud_config->setDecimationFactor(std::max(1, point_cloud_decimation_filter_factor_));

    auto align_mode = ALIGN_DISABLE;
    if (depth_registration_) {
      if (align_mode_ == "HW") {
        align_mode = ALIGN_D2C_HW_MODE;
      } else if (align_target_stream_ == OB_STREAM_DEPTH) {
        align_mode = ALIGN_C2D_SW_MODE;
      } else {
        align_mode = ALIGN_D2C_SW_MODE;
      }
    }
    point_cloud_config->setAlignMode(align_mode);
    point_cloud_config->enableFrameSync(enable_frame_sync_);
    point_cloud_config->setAllFrameTypeRequired(frame_aggregate_mode_ == "full_frame");
    point_cloud_config->enableMatchTargetResolution(
        align_mode == ALIGN_D2C_HW_MODE ? enable_depth_scale_ : true);

    application_config->setPointCloud(point_cloud_config);
  } catch (const ob::Error &e) {
    RCLCPP_WARN_STREAM(logger_, "Export application_config point_cloud sync failed error=\""
                                    << orbbec_camera::formatObErrorWithStatus(e) << "\"");
  } catch (const std::exception &e) {
    RCLCPP_WARN_STREAM(
        logger_, "Export application_config point_cloud sync failed error=\"" << e.what() << "\"");
  } catch (...) {
    RCLCPP_WARN_STREAM(logger_, "Export application_config point_cloud sync failed");
  }
}

void OBCameraNode::syncApplicationHdrMergeConfigForExport() {
  if (!device_) {
    return;
  }

  try {
    if (!ob::ApplicationConfig::isSupported(device_)) {
      RCLCPP_DEBUG_STREAM(logger_,
                          "Skip exporting application_config hdr_merge: unsupported device");
      return;
    }

    auto application_config = ob::ApplicationConfig::get(device_);
    CHECK_NOTNULL(application_config.get());

    auto hdr_merge_config = std::make_shared<ob::ApplicationHDRMergeConfig>();
    hdr_merge_config->enable(enable_hdr_merge_);
    hdr_merge_config->enableIR(true);

    application_config->setHDRMerge(hdr_merge_config);
  } catch (const ob::Error &e) {
    RCLCPP_WARN_STREAM(logger_, "Export application_config hdr_merge sync failed error=\""
                                    << orbbec_camera::formatObErrorWithStatus(e) << "\"");
  } catch (const std::exception &e) {
    RCLCPP_WARN_STREAM(
        logger_, "Export application_config hdr_merge sync failed error=\"" << e.what() << "\"");
  } catch (...) {
    RCLCPP_WARN_STREAM(logger_, "Export application_config hdr_merge sync failed");
  }
}

void OBCameraNode::loadConfigJson() {
  if (load_config_json_file_path_.empty()) {
    return;
  }

  const auto resolved_file_path = resolveConfigJsonFilePath(load_config_json_file_path_);
  const auto resolved_file_path_str = resolved_file_path.string();
  std::ifstream load_config_file(resolved_file_path);
  if (!load_config_file.good()) {
    RCLCPP_WARN_STREAM(logger_, "Config JSON load skip file=" << resolved_file_path_str
                                                              << " reason=file_not_found");
    return;
  }

  try {
    device_->loadPresetFromJsonFile(resolved_file_path_str.c_str());
    config_json_loaded_ = true;
    RCLCPP_INFO_STREAM(logger_, "Config JSON loaded file=" << resolved_file_path_str);
  } catch (const ob::Error &e) {
    config_json_loaded_ = false;
    RCLCPP_ERROR_STREAM(logger_, "Config JSON load failed file="
                                     << resolved_file_path_str << " error=\""
                                     << orbbec_camera::formatObErrorWithStatus(e) << "\"");
  } catch (const std::exception &e) {
    config_json_loaded_ = false;
    RCLCPP_ERROR_STREAM(logger_, "Config JSON load failed file="
                                     << resolved_file_path_str << " error=\"" << e.what() << "\"");
  } catch (...) {
    config_json_loaded_ = false;
    RCLCPP_ERROR_STREAM(logger_, "Config JSON load failed file=" << resolved_file_path_str);
  }
}

void OBCameraNode::captureInitialRosParameters() {
  initial_ros_params_.clear();
  const auto overrides = node_->get_node_parameters_interface()->get_parameter_overrides();
  for (const auto &param : overrides) {
    initial_ros_params_.insert(param.first);
  }
}

bool OBCameraNode::isLaunchParamProvided(const std::string &param_name) const {
  if (param_name.empty()) {
    return false;
  }
  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;
  }

  auto can_read = [this](OBPropertyID property_id) {
    return device_->isPropertySupported(property_id, OB_PERMISSION_READ) ||
           device_->isPropertySupported(property_id, OB_PERMISSION_READ_WRITE);
  };
  auto can_write = [this](OBPropertyID property_id) {
    return device_->isPropertySupported(property_id, OB_PERMISSION_WRITE) ||
           device_->isPropertySupported(property_id, OB_PERMISSION_READ_WRITE);
  };
  auto log_readback = [this](const std::string &scope, const std::string &name, const auto &value) {
    std::ostringstream ss;
    ss << std::boolalpha << value;
    RCLCPP_INFO_STREAM(logger_,
                       "Config final readback [" << scope << "] " << name << "=" << ss.str());
  };
  auto log_readback_fields = [this](const std::string &scope, const std::string &fields) {
    RCLCPP_INFO_STREAM(logger_, "Config final readback [" << scope << "] " << fields);
  };
  auto sync_bool = [&](const char *scope, const char *param_name, bool &member,
                       OBPropertyID property_id) {
    if (!can_read(property_id)) {
      return;
    }
    try {
      member = device_->getBoolProperty(property_id);
      log_readback(scope, param_name, member);
    } catch (const std::exception &e) {
      RCLCPP_DEBUG_STREAM(logger_, "Config final readback failed [" << scope << "] " << param_name
                                                                    << " error=\"" << e.what()
                                                                    << "\"");
    }
  };
  auto sync_int = [&](const char *scope, const char *param_name, int &member,
                      OBPropertyID property_id) {
    if (!can_read(property_id)) {
      return;
    }
    try {
      member = device_->getIntProperty(property_id);
      log_readback(scope, param_name, member);
    } catch (const std::exception &e) {
      RCLCPP_DEBUG_STREAM(logger_, "Config final readback failed [" << scope << "] " << param_name
                                                                    << " error=\"" << e.what()
                                                                    << "\"");
    }
  };
  auto sync_float = [&](const char *scope, const char *param_name, float &member,
                        OBPropertyID property_id) {
    if (!can_read(property_id)) {
      return;
    }
    try {
      member = device_->getFloatProperty(property_id);
      log_readback(scope, param_name, member);
    } catch (const std::exception &e) {
      RCLCPP_DEBUG_STREAM(logger_, "Config final readback failed [" << scope << "] " << param_name
                                                                    << " error=\"" << e.what()
                                                                    << "\"");
    }
  };
  auto sync_stream_orientation = [&](const char *param_prefix,
                                     const stream_index_pair &stream_index,
                                     OBPropertyID flip_property_id, OBPropertyID mirror_property_id,
                                     OBPropertyID rotation_property_id) {
    if (can_read(flip_property_id)) {
      try {
        flip_stream_[stream_index] = device_->getBoolProperty(flip_property_id);
        log_readback(std::string(param_prefix) + ".orientation", "flip",
                     flip_stream_[stream_index]);
      } catch (const std::exception &) {
      }
    }
    if (can_read(mirror_property_id)) {
      try {
        mirror_stream_[stream_index] = device_->getBoolProperty(mirror_property_id);
        log_readback(std::string(param_prefix) + ".orientation", "mirror",
                     mirror_stream_[stream_index]);
      } catch (const std::exception &) {
      }
    }
    if (can_read(rotation_property_id)) {
      try {
        rotation_stream_[stream_index] = device_->getIntProperty(rotation_property_id);
        log_readback(std::string(param_prefix) + ".orientation", "rotation",
                     rotation_stream_[stream_index]);
      } catch (const std::exception &) {
      }
    }
  };

  sync_bool("device", "enable_heartbeat", enable_heartbeat_, OB_PROP_HEARTBEAT_BOOL);
  sync_bool("device", "retry_on_usb3_detection_failure", retry_on_usb3_detection_failure_,
            OB_PROP_DEVICE_USB3_REPEAT_IDENTIFY_BOOL);
  sync_bool("device", "enable_ptp_config", enable_ptp_config_,
            OB_DEVICE_PTP_CLOCK_SYNC_ENABLE_BOOL);

  try {
    device_preset_ = device_->getCurrentPresetName();
    log_readback("depth", "device_preset", device_preset_);
  } catch (const std::exception &e) {
    RCLCPP_DEBUG_STREAM(
        logger_, "Config final readback failed [depth] device_preset error=\"" << e.what() << "\"");
  }

  if (can_read(OB_PROP_DEPTH_AUTO_EXPOSURE_PRIORITY_INT)) {
    try {
      enable_depth_auto_exposure_priority_ =
          device_->getIntProperty(OB_PROP_DEPTH_AUTO_EXPOSURE_PRIORITY_INT) != 0;
      log_readback("depth", "enable_depth_auto_exposure_priority",
                   enable_depth_auto_exposure_priority_);
    } catch (const std::exception &) {
    }
  }
  if (can_read(OB_PROP_DEPTH_AUTO_EXPOSURE_BOOL)) {
    try {
      enable_ir_auto_exposure_ = device_->getBoolProperty(OB_PROP_DEPTH_AUTO_EXPOSURE_BOOL);
      log_readback("depth", "enable_ir_auto_exposure", enable_ir_auto_exposure_);
    } catch (const std::exception &) {
    }
  }
  sync_int("depth", "ir_ae_max_exposure", ir_ae_max_exposure_, OB_PROP_IR_AE_MAX_EXPOSURE_INT);
  sync_int("depth", "mean_intensity_set_point", mean_intensity_set_point_,
           OB_PROP_IR_BRIGHTNESS_INT);
  sync_int("depth", "depth_exposure", depth_exposure_, OB_PROP_DEPTH_EXPOSURE_INT);
  sync_int("depth", "ir_exposure", ir_exposure_, OB_PROP_IR_EXPOSURE_INT);
  sync_int("depth", "depth_gain", depth_gain_, OB_PROP_DEPTH_GAIN_INT);
  sync_int("depth", "ir_gain", ir_gain_, OB_PROP_IR_GAIN_INT);
  if (can_read(OB_PROP_DEPTH_UNIT_FLEXIBLE_ADJUSTMENT_FLOAT)) {
    try {
      const auto depth_unit =
          device_->getFloatProperty(OB_PROP_DEPTH_UNIT_FLEXIBLE_ADJUSTMENT_FLOAT);
      depth_precision_str_ = std::to_string(depth_unit) + "mm";
      log_readback("depth", "depth_unit", depth_unit);
      log_readback("depth", "depth_precision", depth_precision_str_);
    } catch (const std::exception &) {
    }
  }
  if (can_read(OB_PROP_LASER_CONTROL_INT)) {
    try {
      enable_laser_ = device_->getIntProperty(OB_PROP_LASER_CONTROL_INT) != 0;
      log_readback("depth", "enable_laser", enable_laser_);
    } catch (const std::exception &) {
    }
  } else {
    sync_bool("depth", "enable_laser", enable_laser_, OB_PROP_LASER_BOOL);
  }
  sync_int("depth", "laser_energy_level", laser_energy_level_, OB_PROP_LASER_ENERGY_LEVEL_INT);
  sync_bool("depth", "enable_ldp", enable_ldp_, OB_PROP_LDP_BOOL);

  if (can_read(OB_STRUCT_DEPTH_AE_ROI)) {
    try {
      OBRegionOfInterest config{};
      uint32_t data_size = sizeof(config);
      device_->getStructuredData(OB_STRUCT_DEPTH_AE_ROI, reinterpret_cast<uint8_t *>(&config),
                                 &data_size);
      depth_ae_roi_left_ = config.x0_left;
      depth_ae_roi_top_ = config.y0_top;
      depth_ae_roi_right_ = config.x1_right;
      depth_ae_roi_bottom_ = config.y1_bottom;
      std::ostringstream fields;
      fields << "left=" << depth_ae_roi_left_ << " top=" << depth_ae_roi_top_
             << " right=" << depth_ae_roi_right_ << " bottom=" << depth_ae_roi_bottom_;
      log_readback_fields("depth.ae_roi", fields.str());
    } catch (const std::exception &) {
    }
  }

  sync_bool("depth.interleave", "enable", interleave_frame_enable_,
            OB_PROP_FRAME_INTERLEAVE_ENABLE_BOOL);
  sync_int("depth.interleave", "skip_index", interleave_skip_index_,
           OB_PROP_FRAME_INTERLEAVE_CONFIG_INDEX_INT);
  try {
    const auto *frame_interleave_name = device_->getCurrentFrameInterleaveName();
    if (frame_interleave_name != nullptr) {
      std::string mode(frame_interleave_name);
      std::string lower_mode = mode;
      std::transform(lower_mode.begin(), lower_mode.end(), lower_mode.begin(), ::tolower);
      if (lower_mode.find("laser") != std::string::npos) {
        interleave_ae_mode_ = "laser";
      } else if (lower_mode.find("hdr") != std::string::npos) {
        interleave_ae_mode_ = "hdr";
      } else {
        interleave_ae_mode_.clear();
      }
      log_readback("depth.interleave", "ae_mode", interleave_ae_mode_);
    }
  } catch (const std::exception &) {
  }
  if (!interleave_ae_mode_.empty() && can_write(OB_PROP_FRAME_INTERLEAVE_CONFIG_INDEX_INT)) {
    int original_interleave_index = interleave_skip_index_;
    bool has_original_interleave_index = false;
    if (can_read(OB_PROP_FRAME_INTERLEAVE_CONFIG_INDEX_INT)) {
      try {
        original_interleave_index =
            device_->getIntProperty(OB_PROP_FRAME_INTERLEAVE_CONFIG_INDEX_INT);
        has_original_interleave_index = true;
      } catch (const std::exception &) {
      }
    }

    auto read_int_property = [&](OBPropertyID property_id, int &value) {
      if (!can_read(property_id)) {
        return;
      }
      try {
        value = device_->getIntProperty(property_id);
      } catch (const std::exception &) {
      }
    };
    auto sync_interleave_param = [&](int config_index, int &laser_control, int &depth_exposure,
                                     int &depth_gain, int &ir_brightness, int &ir_ae_max_exposure) {
      try {
        device_->setIntProperty(OB_PROP_FRAME_INTERLEAVE_CONFIG_INDEX_INT, config_index);
        read_int_property(OB_PROP_LASER_CONTROL_INT, laser_control);
        read_int_property(OB_PROP_DEPTH_EXPOSURE_INT, depth_exposure);
        read_int_property(OB_PROP_DEPTH_GAIN_INT, depth_gain);
        read_int_property(OB_PROP_IR_BRIGHTNESS_INT, ir_brightness);
        read_int_property(OB_PROP_IR_AE_MAX_EXPOSURE_INT, ir_ae_max_exposure);
        std::ostringstream fields;
        fields << "laser_control=" << laser_control << " depth_exposure=" << depth_exposure
               << " depth_gain=" << depth_gain << " depth_brightness=" << ir_brightness
               << " depth_ae_max_exposure=" << ir_ae_max_exposure;
        log_readback_fields("depth.interleave.params." + std::to_string(config_index),
                            fields.str());
      } catch (const std::exception &e) {
        RCLCPP_DEBUG_STREAM(logger_, "Config final readback failed [depth.interleave.params."
                                         << config_index << "] error=\"" << e.what() << "\"");
      }
    };

    if (interleave_ae_mode_ == "hdr") {
      sync_interleave_param(0, hdr_index0_laser_control_, hdr_index0_depth_exposure_,
                            hdr_index0_depth_gain_, hdr_index0_ir_brightness_,
                            hdr_index0_ir_ae_max_exposure_);
      sync_interleave_param(1, hdr_index1_laser_control_, hdr_index1_depth_exposure_,
                            hdr_index1_depth_gain_, hdr_index1_ir_brightness_,
                            hdr_index1_ir_ae_max_exposure_);
    } else if (interleave_ae_mode_ == "laser") {
      sync_interleave_param(0, laser_index0_laser_control_, laser_index0_depth_exposure_,
                            laser_index0_depth_gain_, laser_index0_ir_brightness_,
                            laser_index0_ir_ae_max_exposure_);
      sync_interleave_param(1, laser_index1_laser_control_, laser_index1_depth_exposure_,
                            laser_index1_depth_gain_, laser_index1_ir_brightness_,
                            laser_index1_ir_ae_max_exposure_);
    }

    if (has_original_interleave_index) {
      try {
        device_->setIntProperty(OB_PROP_FRAME_INTERLEAVE_CONFIG_INDEX_INT,
                                original_interleave_index);
      } catch (const std::exception &e) {
        RCLCPP_DEBUG_STREAM(logger_,
                            "Failed to restore frame_interleave.config_index: " << e.what());
      }
    }
  }

  try {
    const bool hw = can_read(OB_PROP_DISPARITY_TO_DEPTH_BOOL) &&
                    device_->getBoolProperty(OB_PROP_DISPARITY_TO_DEPTH_BOOL);
    const bool sw = can_read(OB_PROP_SDK_DISPARITY_TO_DEPTH_BOOL) &&
                    device_->getBoolProperty(OB_PROP_SDK_DISPARITY_TO_DEPTH_BOOL);
    disparity_to_depth_mode_ = hw ? "HW" : (sw ? "SW" : "disable");
    log_readback("depth", "disparity_to_depth_mode", disparity_to_depth_mode_);
  } catch (const std::exception &) {
  }
  if (can_read(OB_PROP_DISP_SEARCH_RANGE_MODE_INT)) {
    try {
      disparity_range_mode_ = std::stoi(
          disparityRangeModeToString(device_->getIntProperty(OB_PROP_DISP_SEARCH_RANGE_MODE_INT)));
      log_readback("depth", "disparity_range_mode", disparity_range_mode_);
    } catch (const std::exception &) {
    }
  }
  sync_int("depth", "disparity_search_offset", disparity_search_offset_,
           OB_PROP_DISP_SEARCH_OFFSET_INT);

  sync_bool("depth", "enable_hardware_noise_removal_filter", enable_hardware_noise_removal_filter_,
            OB_PROP_HW_NOISE_REMOVE_FILTER_ENABLE_BOOL);
  sync_float("depth", "hardware_noise_removal_filter_threshold",
             hardware_noise_removal_filter_threshold_,
             OB_PROP_HW_NOISE_REMOVE_FILTER_THRESHOLD_FLOAT);
  sync_bool("depth", "enable_noise_removal_filter", enable_noise_removal_filter_,
            OB_PROP_DEPTH_SOFT_FILTER_BOOL);
  sync_int("depth", "noise_removal_filter_min_diff", noise_removal_filter_min_diff_,
           OB_PROP_DEPTH_MAX_DIFF_INT);
  sync_int("depth", "noise_removal_filter_max_size", noise_removal_filter_max_size_,
           OB_PROP_DEPTH_MAX_SPECKLE_SIZE_INT);
  sync_bool("depth", "enable_disp_outliers_filter", enable_disp_outliers_filter_,
            OB_PROP_DEPTH_OUTLIERS_FILTER_BOOL);
  sync_int("depth", "disp_outliers_filter_search_mode", disp_outliers_filter_search_mode_,
           OB_PROP_DEPTH_OUTLIERS_FILTER_SEARCH_MODE_INT);

  sync_stream_orientation("depth", DEPTH, OB_PROP_DEPTH_FLIP_BOOL, OB_PROP_DEPTH_MIRROR_BOOL,
                          OB_PROP_DEPTH_ROTATE_INT);
  sync_stream_orientation("color", COLOR, OB_PROP_COLOR_FLIP_BOOL, OB_PROP_COLOR_MIRROR_BOOL,
                          OB_PROP_COLOR_ROTATE_INT);
  sync_stream_orientation("left_ir", INFRA1, OB_PROP_IR_FLIP_BOOL, OB_PROP_IR_MIRROR_BOOL,
                          OB_PROP_IR_ROTATE_INT);
  sync_stream_orientation("right_ir", INFRA2, OB_PROP_IR_RIGHT_FLIP_BOOL,
                          OB_PROP_IR_RIGHT_MIRROR_BOOL, OB_PROP_IR_RIGHT_ROTATE_INT);

  if (can_read(OB_PROP_COLOR_AUTO_EXPOSURE_PRIORITY_INT)) {
    try {
      enable_color_auto_exposure_priority_ =
          device_->getIntProperty(OB_PROP_COLOR_AUTO_EXPOSURE_PRIORITY_INT) != 0;
      log_readback("color", "enable_color_auto_exposure_priority",
                   enable_color_auto_exposure_priority_);
    } catch (const std::exception &) {
    }
  }
  sync_bool("color", "enable_color_auto_exposure", enable_color_auto_exposure_,
            OB_PROP_COLOR_AUTO_EXPOSURE_BOOL);
  sync_int("color", "color_denoising_level", color_denoising_level_,
           OB_PROP_COLOR_DENOISING_LEVEL_INT);
  sync_int("color", "color_ae_max_exposure", color_ae_max_exposure_,
           OB_PROP_COLOR_AE_MAX_EXPOSURE_INT);
  sync_int("color", "color_exposure", color_exposure_, OB_PROP_COLOR_EXPOSURE_INT);
  sync_int("color", "color_gain", color_gain_, OB_PROP_COLOR_GAIN_INT);
  sync_int("color", "color_mjpeg_quality", color_mjpeg_quality_, OB_PROP_MJPEG_QUALITY_INT);
  sync_int("color", "color_brightness", color_brightness_, OB_PROP_COLOR_BRIGHTNESS_INT);
  sync_bool("color", "enable_color_auto_white_balance", enable_color_auto_white_balance_,
            OB_PROP_COLOR_AUTO_WHITE_BALANCE_BOOL);
  sync_int("color", "color_white_balance", color_white_balance_, OB_PROP_COLOR_WHITE_BALANCE_INT);
  sync_int("color", "color_sharpness", color_sharpness_, OB_PROP_COLOR_SHARPNESS_INT);
  sync_int("color", "color_gamma", color_gamma_, OB_PROP_COLOR_GAMMA_INT);
  sync_int("color", "color_hue", color_hue_, OB_PROP_COLOR_HUE_INT);
  sync_int("color", "color_backlight_compensation", color_backlight_compensation_,
           OB_PROP_COLOR_BACKLIGHT_COMPENSATION_INT);
  sync_int("color", "color_contrast", color_contrast_, OB_PROP_COLOR_CONTRAST_INT);
  sync_int("color", "color_saturation", color_saturation_, OB_PROP_COLOR_SATURATION_INT);
  if (can_read(OB_PROP_COLOR_POWER_LINE_FREQUENCY_INT)) {
    try {
      color_powerline_freq_ = colorPowerLineFrequencyToString(
          device_->getIntProperty(OB_PROP_COLOR_POWER_LINE_FREQUENCY_INT));
      log_readback("color", "color_powerline_freq", color_powerline_freq_);
    } catch (const std::exception &) {
    }
  }
  sync_bool("color", "color_anti_flicker", color_anti_flicker_, OB_PROP_COLOR_ANTI_FLICKER_BOOL);
  try {
    if (device_->isColorPresetSupported()) {
      const char *color_preset = device_->getCurrentColorPresetName();
      if (color_preset != nullptr) {
        color_preset_ = color_preset;
        log_readback("color", "color_preset", color_preset_);
      }
    }
  } catch (const std::exception &) {
  }
  if (can_read(OB_STRUCT_COLOR_AE_ROI)) {
    try {
      OBRegionOfInterest config{};
      uint32_t data_size = sizeof(config);
      device_->getStructuredData(OB_STRUCT_COLOR_AE_ROI, reinterpret_cast<uint8_t *>(&config),
                                 &data_size);
      color_ae_roi_left_ = config.x0_left;
      color_ae_roi_top_ = config.y0_top;
      color_ae_roi_right_ = config.x1_right;
      color_ae_roi_bottom_ = config.y1_bottom;
      std::ostringstream fields;
      fields << "left=" << color_ae_roi_left_ << " top=" << color_ae_roi_top_
             << " right=" << color_ae_roi_right_ << " bottom=" << color_ae_roi_bottom_;
      log_readback_fields("color.ae_roi", fields.str());
    } catch (const std::exception &) {
    }
  }
}

void OBCameraNode::syncConfigJsonFilterSettings(
    const std::vector<std::shared_ptr<ob::Filter>> &filters, const std::string &sensor_name) {
  if (!config_json_loaded_) {
    return;
  }

  auto filter_scope = [&](const std::string &filter_name) {
    return "filter." + sensor_name + "." + normalizeDepthFilterName(filter_name);
  };
  auto log_readback = [this](const std::string &scope, const std::string &name, const auto &value) {
    std::ostringstream ss;
    ss << std::boolalpha << value;
    RCLCPP_INFO_STREAM(logger_,
                       "Config final readback [" << scope << "] " << name << "=" << ss.str());
  };
  auto log_readback_fields = [this](const std::string &scope, const std::string &fields) {
    RCLCPP_INFO_STREAM(logger_, "Config final readback [" << scope << "] " << fields);
  };
  auto sync_filter_enabled = [&](const std::vector<std::shared_ptr<ob::Filter>> &filter_list,
                                 const std::string &filter_name, const std::string &param_name,
                                 bool &member) {
    auto it = std::find_if(filter_list.begin(), filter_list.end(), [&](const auto &filter) {
      return filter &&
             normalizeDepthFilterName(filter->type()) == normalizeDepthFilterName(filter_name);
    });
    if (it == filter_list.end()) {
      return std::shared_ptr<ob::Filter>{};
    }
    try {
      member = (*it)->isEnabled();
      log_readback(filter_scope(filter_name), param_name, member);
    } catch (const std::exception &) {
    }
    return *it;
  };

  if (sensor_name == "depth") {
    if (auto filter = sync_filter_enabled(filters, "DecimationFilter", "enable_decimation_filter",
                                          enable_decimation_filter_)) {
      try {
        decimation_filter_scale_ =
            static_cast<int>(filter->as<ob::DecimationFilter>()->getScaleValue());
        log_readback(filter_scope("DecimationFilter"), "scale", decimation_filter_scale_);
      } catch (const std::exception &) {
      }
    }
    if (auto filter = sync_filter_enabled(filters, "ThresholdFilter", "enable_threshold_filter",
                                          enable_threshold_filter_)) {
      try {
        threshold_filter_min_ = static_cast<int>(filter->getConfigValue("min"));
        threshold_filter_max_ = static_cast<int>(filter->getConfigValue("max"));
        std::ostringstream fields;
        fields << "min=" << threshold_filter_min_ << " max=" << threshold_filter_max_;
        log_readback_fields(filter_scope("ThresholdFilter"), fields.str());
      } catch (const std::exception &) {
      }
    }
    sync_filter_enabled(filters, "HDRMerge", "enable_hdr_merge", enable_hdr_merge_);
    if (auto filter = sync_filter_enabled(filters, "SequenceIdFilter", "enable_sequence_id_filter",
                                          enable_sequence_id_filter_)) {
      try {
        sequence_id_filter_id_ = filter->as<ob::SequenceIdFilter>()->getSelectSequenceId();
        log_readback(filter_scope("SequenceIdFilter"), "id", sequence_id_filter_id_);
      } catch (const std::exception &) {
      }
    }
    if (auto filter =
            sync_filter_enabled(filters, "SpatialFastFilter", "enable_spatial_fast_filter",
                                enable_spatial_fast_filter_)) {
      try {
        spatial_fast_filter_radius_ = filter->as<ob::SpatialFastFilter>()->getFilterParams().radius;
        log_readback(filter_scope("SpatialFastFilter"), "radius",
                     static_cast<int>(spatial_fast_filter_radius_));
      } catch (const std::exception &) {
      }
    }
    if (auto filter =
            sync_filter_enabled(filters, "SpatialModerateFilter", "enable_spatial_moderate_filter",
                                enable_spatial_moderate_filter_)) {
      try {
        auto params = filter->as<ob::SpatialModerateFilter>()->getFilterParams();
        spatial_moderate_filter_diff_threshold_ = params.disp_diff;
        spatial_moderate_filter_magnitude_ = params.magnitude;
        spatial_moderate_filter_radius_ = params.radius;
        std::ostringstream fields;
        fields << "diff_threshold=" << spatial_moderate_filter_diff_threshold_
               << " magnitude=" << spatial_moderate_filter_magnitude_
               << " radius=" << spatial_moderate_filter_radius_;
        log_readback_fields(filter_scope("SpatialModerateFilter"), fields.str());
      } catch (const std::exception &) {
      }
    }
    if (auto filter = sync_filter_enabled(filters, "SpatialAdvancedFilter", "enable_spatial_filter",
                                          enable_spatial_filter_)) {
      try {
        auto params = filter->as<ob::SpatialAdvancedFilter>()->getFilterParams();
        spatial_filter_alpha_ = params.alpha;
        spatial_filter_diff_threshold_ = params.disp_diff;
        spatial_filter_magnitude_ = params.magnitude;
        spatial_filter_radius_ = params.radius;
        std::ostringstream fields;
        fields << "alpha=" << spatial_filter_alpha_
               << " diff_threshold=" << spatial_filter_diff_threshold_
               << " magnitude=" << spatial_filter_magnitude_
               << " radius=" << spatial_filter_radius_;
        log_readback_fields(filter_scope("SpatialAdvancedFilter"), fields.str());
      } catch (const std::exception &) {
      }
    }
    if (auto filter = sync_filter_enabled(filters, "TemporalFilter", "enable_temporal_filter",
                                          enable_temporal_filter_)) {
      try {
        temporal_filter_diff_threshold_ = static_cast<float>(filter->getConfigValue("diff_scale"));
        temporal_filter_weight_ = static_cast<float>(filter->getConfigValue("weight"));
        std::ostringstream fields;
        fields << "diff_threshold=" << temporal_filter_diff_threshold_
               << " weight=" << temporal_filter_weight_;
        log_readback_fields(filter_scope("TemporalFilter"), fields.str());
      } catch (const std::exception &) {
      }
    }
    if (auto filter =
            sync_filter_enabled(filters, "HoleFillingFilter", "enable_hole_filling_filter",
                                enable_hole_filling_filter_)) {
      try {
        hole_filling_filter_mode_ =
            std::to_string(static_cast<int>(filter->as<ob::HoleFillingFilter>()->getFilterMode()));
        log_readback(filter_scope("HoleFillingFilter"), "mode", hole_filling_filter_mode_);
      } catch (const std::exception &) {
      }
    }
    sync_filter_enabled(filters, "EdgeNoiseRemovalFilter", "enable_edge_noise_removal_filter",
                        enable_edge_noise_removal_filter_);
    if (auto filter =
            sync_filter_enabled(filters, "FalsePositiveFilter", "enable_false_positive_filter",
                                enable_false_positive_filter_)) {
      try {
        const auto config_schema_vec = filter->getConfigSchemaVec();
        for (const auto &config_schema : config_schema_vec) {
          if (config_schema.name == nullptr || config_schema.name[0] == '\0') {
            continue;
          }
          try {
            const auto value = filter->getConfigValue(config_schema.name);
            log_readback(filter_scope("FalsePositiveFilter"), config_schema.name,
                         formatFilterConfigValue(config_schema, value));
          } catch (const std::exception &) {
          }
        }
      } catch (const std::exception &) {
      }
    }
    sync_filter_enabled(filters, "DisparityTransform", "enable_disparity_to_depth",
                        enable_disparity_to_depth_);
  } else if (sensor_name == "color" || sensor_name == "left_color" ||
             sensor_name == "right_color") {
    bool *enable_member = &enable_color_decimation_filter_;
    int *scale_member = &color_decimation_filter_scale_;
    std::string param_name = "enable_color_decimation_filter";
    if (sensor_name == "left_color") {
      enable_member = &enable_left_color_decimation_filter_;
      scale_member = &left_color_decimation_filter_scale_;
      param_name = "enable_left_color_decimation_filter";
    } else if (sensor_name == "right_color") {
      enable_member = &enable_right_color_decimation_filter_;
      scale_member = &right_color_decimation_filter_scale_;
      param_name = "enable_right_color_decimation_filter";
    }
    if (auto filter =
            sync_filter_enabled(filters, "DecimationFilter", param_name, *enable_member)) {
      try {
        *scale_member = static_cast<int>(filter->as<ob::DecimationFilter>()->getScaleValue());
        log_readback(filter_scope("DecimationFilter"), "scale", *scale_member);
      } catch (const std::exception &) {
      }
    }
  } else if (sensor_name == "left_ir") {
    if (auto filter =
            sync_filter_enabled(filters, "SequenceIdFilter", "enable_left_ir_sequence_id_filter",
                                enable_left_ir_sequence_id_filter_)) {
      try {
        left_ir_sequence_id_filter_id_ = filter->as<ob::SequenceIdFilter>()->getSelectSequenceId();
        log_readback(filter_scope("SequenceIdFilter"), "id", left_ir_sequence_id_filter_id_);
      } catch (const std::exception &) {
      }
    }
  } else if (sensor_name == "right_ir") {
    if (auto filter =
            sync_filter_enabled(filters, "SequenceIdFilter", "enable_right_ir_sequence_id_filter",
                                enable_right_ir_sequence_id_filter_)) {
      try {
        right_ir_sequence_id_filter_id_ = filter->as<ob::SequenceIdFilter>()->getSelectSequenceId();
        log_readback(filter_scope("SequenceIdFilter"), "id", right_ir_sequence_id_filter_id_);
      } catch (const std::exception &) {
      }
    }
  }
}

void OBCameraNode::setupColorPostProcessFilter() {
  if (!enable_stream_[COLOR] && !enable_stream_[COLOR_LEFT] && !enable_stream_[COLOR_RIGHT]) {
    return;
  }
  try {
    auto color_sensor = device_->getSensor(OB_SENSOR_COLOR);
    if (color_sensor) {
      color_filter_list_ = color_sensor->createRecommendedFilters();
    }
  } catch (const std::exception &e) {
    RCLCPP_DEBUG_STREAM(logger_, "Main color sensor not found, trying left/right color sensors");
    auto left_color_sensor = device_->getSensor(OB_SENSOR_COLOR_LEFT);
    if (left_color_sensor) {
      left_color_filter_list_ = left_color_sensor->createRecommendedFilters();
    }
    auto right_color_sensor = device_->getSensor(OB_SENSOR_COLOR_RIGHT);
    if (right_color_sensor) {
      right_color_filter_list_ = right_color_sensor->createRecommendedFilters();
    }
  }
  if (color_filter_list_.empty() && left_color_filter_list_.empty() &&
      right_color_filter_list_.empty()) {
    RCLCPP_DEBUG_STREAM(logger_, "Color sensor filter lists are empty");
  }
  for (size_t i = 0; i < color_filter_list_.size(); i++) {
    auto filter = color_filter_list_[i];
    std::map<std::string, bool> filter_params = {
        {"DecimationFilter", enable_color_decimation_filter_},
    };
    std::map<std::string, std::string> filter_param_names = {
        {"DecimationFilter", "enable_color_decimation_filter"},
    };
    std::string filter_name = filter->type();
    RCLCPP_DEBUG_STREAM(logger_, "Configuring color filter: " << filter_name);
    if (filter_params.find(filter_name) != filter_params.end() &&
        isLaunchParamProvided(filter_param_names[filter_name])) {
      const auto *value = filter_params[filter_name] ? "enabled" : "disabled";
      RCLCPP_INFO_STREAM(logger_, "Set color filter " << filter_name << " to " << value);
      filter->enable(filter_params[filter_name]);
    }
    if (filter_name == "DecimationFilter" && enable_color_decimation_filter_) {
      auto decimation_filter = filter->as<ob::DecimationFilter>();
      auto range = decimation_filter->getScaleRange();
      if (color_decimation_filter_scale_ != -1 && color_decimation_filter_scale_ <= range.max &&
          color_decimation_filter_scale_ >= range.min) {
        decimation_filter->setScaleValue(color_decimation_filter_scale_);
      }
      if (color_decimation_filter_scale_ != -1 && (color_decimation_filter_scale_ < range.min ||
                                                   color_decimation_filter_scale_ > range.max)) {
        RCLCPP_ERROR_STREAM(logger_, "Color Decimation filter scale value is out of range "
                                         << range.min << " - " << range.max);
      }
      RCLCPP_INFO_STREAM(logger_, "Current color decimation filter scale value: "
                                      << static_cast<int>(decimation_filter->getScaleValue()));
    }
  }

  for (size_t i = 0; i < left_color_filter_list_.size(); i++) {
    auto filter = left_color_filter_list_[i];
    std::map<std::string, bool> filter_params = {
        {"DecimationFilter", enable_left_color_decimation_filter_},
    };
    std::map<std::string, std::string> filter_param_names = {
        {"DecimationFilter", "enable_left_color_decimation_filter"},
    };
    std::string filter_name = filter->type();
    RCLCPP_DEBUG_STREAM(logger_, "Configuring left color filter: " << filter_name);
    if (filter_params.find(filter_name) != filter_params.end() &&
        isLaunchParamProvided(filter_param_names[filter_name])) {
      const auto *value = filter_params[filter_name] ? "enabled" : "disabled";
      RCLCPP_INFO_STREAM(logger_, "Set left color filter " << filter_name << " to " << value);
      filter->enable(filter_params[filter_name]);
    }
    if (filter_name == "DecimationFilter" && enable_left_color_decimation_filter_) {
      auto decimation_filter = filter->as<ob::DecimationFilter>();
      auto range = decimation_filter->getScaleRange();
      if (left_color_decimation_filter_scale_ != -1 &&
          left_color_decimation_filter_scale_ <= range.max &&
          left_color_decimation_filter_scale_ >= range.min) {
        decimation_filter->setScaleValue(left_color_decimation_filter_scale_);
      }
      if (left_color_decimation_filter_scale_ != -1 &&
          (left_color_decimation_filter_scale_ < range.min ||
           left_color_decimation_filter_scale_ > range.max)) {
        RCLCPP_ERROR_STREAM(logger_, "Left Color Decimation filter scale value is out of range "
                                         << range.min << " - " << range.max);
      }
      RCLCPP_INFO_STREAM(logger_, "Current left color decimation filter scale value: "
                                      << static_cast<int>(decimation_filter->getScaleValue()));
    }
  }

  for (size_t i = 0; i < right_color_filter_list_.size(); i++) {
    auto filter = right_color_filter_list_[i];
    std::map<std::string, bool> filter_params = {
        {"DecimationFilter", enable_right_color_decimation_filter_},
    };
    std::map<std::string, std::string> filter_param_names = {
        {"DecimationFilter", "enable_right_color_decimation_filter"},
    };
    std::string filter_name = filter->type();
    RCLCPP_DEBUG_STREAM(logger_, "Configuring right color filter: " << filter_name);
    if (filter_params.find(filter_name) != filter_params.end() &&
        isLaunchParamProvided(filter_param_names[filter_name])) {
      const auto *value = filter_params[filter_name] ? "enabled" : "disabled";
      RCLCPP_INFO_STREAM(logger_, "Set right color filter " << filter_name << " to " << value);
      filter->enable(filter_params[filter_name]);
    }
    if (filter_name == "DecimationFilter" && enable_right_color_decimation_filter_) {
      auto decimation_filter = filter->as<ob::DecimationFilter>();
      auto range = decimation_filter->getScaleRange();
      if (right_color_decimation_filter_scale_ != -1 &&
          right_color_decimation_filter_scale_ <= range.max &&
          right_color_decimation_filter_scale_ >= range.min) {
        decimation_filter->setScaleValue(right_color_decimation_filter_scale_);
      }
      if (right_color_decimation_filter_scale_ != -1 &&
          (right_color_decimation_filter_scale_ < range.min ||
           right_color_decimation_filter_scale_ > range.max)) {
        RCLCPP_ERROR_STREAM(logger_, "Right Color Decimation filter scale value is out of range "
                                         << range.min << " - " << range.max);
      }
      RCLCPP_INFO_STREAM(logger_, "Current right color decimation filter scale value: "
                                      << static_cast<int>(decimation_filter->getScaleValue()));
    }
  }
  auto device_info = device_->getDeviceInfo();
  CHECK_NOTNULL(device_info);
  if (pid_ == GEMINI2_PID || pid_ == GEMINI2L_PID) {
    if (isLaunchParamProvided("enable_color_decimation_filter") &&
        enable_color_decimation_filter_) {
      auto decimation_filter = std::make_shared<ob::DecimationFilter>();
      decimation_filter->enable(true);
      color_filter_list_.push_back(decimation_filter);
      auto range = decimation_filter->getScaleRange();
      if (color_decimation_filter_scale_ != -1 && color_decimation_filter_scale_ <= range.max &&
          color_decimation_filter_scale_ >= range.min) {
        decimation_filter->setScaleValue(color_decimation_filter_scale_);
      }
      if (color_decimation_filter_scale_ != -1 && (color_decimation_filter_scale_ < range.min ||
                                                   color_decimation_filter_scale_ > range.max)) {
        RCLCPP_ERROR_STREAM(logger_, "Color Decimation filter scale value is out of range "
                                         << range.min << " - " << range.max);
      }
      RCLCPP_INFO_STREAM(logger_, "Current color decimation filter scale value: "
                                      << static_cast<int>(decimation_filter->getScaleValue()));
    }
  }
}

void OBCameraNode::setupIrPostProcessFilter() {
  if (!enable_stream_[INFRA0]) {
    return;
  }
  try {
    auto ir_sensor = device_->getSensor(OB_SENSOR_IR);
    ir_filter_list_ = ir_sensor->createRecommendedFilters();
    if (ir_filter_list_.empty()) {
      RCLCPP_DEBUG_STREAM(logger_, "IR sensor filter list is empty");
    }
  } catch (const ob::Error &e) {
    RCLCPP_WARN_STREAM(logger_,
                       "Failed to setup ir filters: " << orbbec_camera::formatObErrorWithStatus(e));
  }
}

void OBCameraNode::setupUndistortionFilters() {
  auto remove_undistortion_filter = [](std::vector<std::shared_ptr<ob::Filter>> &filters) {
    filters.erase(std::remove_if(filters.begin(), filters.end(),
                                 [](const std::shared_ptr<ob::Filter> &filter) {
                                   return filter &&
                                          std::string(filter->type()) == "UnDistortionFilter";
                                 }),
                  filters.end());
  };

  auto find_or_create_filter =
      [&](std::vector<std::shared_ptr<ob::Filter>> &filters,
          OBStreamType stream_type) -> std::shared_ptr<ob::UnDistortionFilter> {
    for (auto &filter : filters) {
      if (filter && std::string(filter->type()) == "UnDistortionFilter") {
        auto undistortion_filter = filter->as<ob::UnDistortionFilter>();
        undistortion_filter->setStreamType(stream_type);
        undistortion_filter->enable(true);
        return undistortion_filter;
      }
    }
    auto undistortion_filter = std::make_shared<ob::UnDistortionFilter>(stream_type);
    undistortion_filter->enable(true);
    filters.push_back(undistortion_filter);
    return undistortion_filter;
  };

  auto setup_stream_filter = [&](const stream_index_pair &stream_index,
                                 std::vector<std::shared_ptr<ob::Filter>> &filters) {
    if (!enable_stream_[stream_index] || !enable_undistortion_[stream_index]) {
      return;
    }
    if (stream_index == LIDAR) {
      RCLCPP_WARN_STREAM(logger_, "Undistortion is not supported for lidar stream");
      return;
    }
    if (stream_index == COLOR && shouldUseHwD2CColorUndistortion()) {
      remove_undistortion_filter(filters);
      hw_d2c_color_undistortion_filter_ = std::make_shared<ob::UnDistortionFilter>(OB_STREAM_COLOR);
      hw_d2c_color_undistortion_filter_->enable(true);
      RCLCPP_INFO_STREAM(logger_,
                         "Enable color undistortion with HW D2C depth intrinsic projection");
      return;
    }
    auto undistortion_filter = find_or_create_filter(filters, stream_index.first);
    undistortion_filter->clearNewCameraMatrix();
    RCLCPP_INFO_STREAM(logger_, "Enable " << stream_name_[stream_index] << " undistortion");
  };

  setup_stream_filter(COLOR, color_filter_list_);
  setup_stream_filter(COLOR_LEFT, left_color_filter_list_);
  setup_stream_filter(COLOR_RIGHT, right_color_filter_list_);
  setup_stream_filter(DEPTH, depth_filter_list_);
  setup_stream_filter(INFRA0, ir_filter_list_);
  setup_stream_filter(INFRA1, left_ir_filter_list_);
  setup_stream_filter(INFRA2, right_ir_filter_list_);
}

bool OBCameraNode::shouldUseHwD2CColorUndistortion() const {
  auto color_undistortion = enable_undistortion_.find(COLOR);
  return color_undistortion != enable_undistortion_.end() && color_undistortion->second &&
         isDabaiASeriesForHwD2C(pid_) && depth_registration_ && align_mode_ == "HW" &&
         align_target_stream_ == OB_STREAM_COLOR && enable_stream_.at(COLOR) &&
         enable_stream_.at(DEPTH);
}

void OBCameraNode::configureHwD2CColorUndistortion(const std::shared_ptr<ob::Frame> &depth_frame) {
  if (!hw_d2c_color_undistortion_filter_ || hw_d2c_color_undistortion_configured_) {
    return;
  }
  if (!depth_frame) {
    RCLCPP_WARN_ONCE(logger_,
                     "Skip HW D2C color undistortion setup because depth frame is not available");
    return;
  }
  auto stream_profile = depth_frame->getStreamProfile();
  if (!stream_profile) {
    RCLCPP_WARN_ONCE(logger_,
                     "Skip HW D2C color undistortion setup because depth stream profile is null");
    return;
  }
  auto video_profile = stream_profile->as<ob::VideoStreamProfile>();
  if (!video_profile) {
    RCLCPP_WARN_ONCE(logger_,
                     "Skip HW D2C color undistortion setup because depth profile is not video");
    return;
  }
  hw_d2c_color_undistortion_filter_->setNewCameraMatrix(video_profile->getIntrinsic());
  hw_d2c_color_undistortion_configured_ = true;
  RCLCPP_INFO_STREAM(logger_, "Configured HW D2C color undistortion with depth camera intrinsic");
}

void OBCameraNode::applyHwD2CColorUndistortion(std::shared_ptr<ob::FrameSet> &frame_set,
                                               const std::shared_ptr<ob::Frame> &depth_frame) {
  if (!frame_set || !hw_d2c_color_undistortion_filter_) {
    return;
  }
  configureHwD2CColorUndistortion(depth_frame);
  if (!hw_d2c_color_undistortion_configured_) {
    return;
  }
  auto undistorted_frame = hw_d2c_color_undistortion_filter_->process(frame_set);
  if (!undistorted_frame) {
    RCLCPP_WARN_STREAM(logger_, "HW D2C color undistortion filter returned null frame");
    return;
  }
  auto undistorted_frame_set = undistorted_frame->as<ob::FrameSet>();
  if (!undistorted_frame_set) {
    RCLCPP_WARN_STREAM(logger_, "HW D2C color undistortion filter returned non-frameset output");
    return;
  }
  frame_set = undistorted_frame_set;
}

void OBCameraNode::setupLeftIrPostProcessFilter() {
  if (!enable_stream_[INFRA1]) {
    return;
  }
  auto device_info = device_->getDeviceInfo();
  CHECK_NOTNULL(device_info);
  if (isGemini335PID(pid_)) {
    auto left_ir_sensor = device_->getSensor(OB_SENSOR_IR_LEFT);
    left_ir_filter_list_ = left_ir_sensor->createRecommendedFilters();
    if (left_ir_filter_list_.empty()) {
      RCLCPP_DEBUG_STREAM(logger_, "Left IR sensor filter list is empty");
      return;
    }
    for (size_t i = 0; i < left_ir_filter_list_.size(); i++) {
      auto filter = left_ir_filter_list_[i];
      std::map<std::string, bool> filter_params = {
          {"SequenceIdFilter", enable_left_ir_sequence_id_filter_},
      };
      std::map<std::string, std::string> filter_param_names = {
          {"SequenceIdFilter", "enable_left_ir_sequence_id_filter"},
      };
      std::string filter_name = filter->type();
      RCLCPP_DEBUG_STREAM(logger_, "Configuring left IR filter: " << filter_name);
      if (filter_params.find(filter_name) != filter_params.end() &&
          isLaunchParamProvided(filter_param_names[filter_name])) {
        const auto *value = filter_params[filter_name] ? "enabled" : "disabled";
        RCLCPP_INFO_STREAM(logger_, "Set left IR filter " << filter_name << " to " << value);
        filter->enable(filter_params[filter_name]);
      }
      if (filter_name == "SequenceIdFilter" && enable_left_ir_sequence_id_filter_) {
        auto sequenced_filter = filter->as<ob::SequenceIdFilter>();
        if (left_ir_sequence_id_filter_id_ != -1) {
          sequenced_filter->selectSequenceId(left_ir_sequence_id_filter_id_);
        }
        RCLCPP_INFO_STREAM(logger_, "Current left ir SequenceIdFilter ID: "
                                        << sequenced_filter->getSelectSequenceId());
      }
    }
  }
}

void OBCameraNode::setupRightIrPostProcessFilter() {
  if (!enable_stream_[INFRA2]) {
    return;
  }
  auto device_info = device_->getDeviceInfo();
  CHECK_NOTNULL(device_info);
  if (isGemini335PID(pid_)) {
    auto right_ir_sensor = device_->getSensor(OB_SENSOR_IR_RIGHT);
    right_ir_filter_list_ = right_ir_sensor->createRecommendedFilters();
    if (right_ir_filter_list_.empty()) {
      RCLCPP_DEBUG_STREAM(logger_, "Right IR sensor filter list is empty");
      return;
    }
    for (size_t i = 0; i < right_ir_filter_list_.size(); i++) {
      auto filter = right_ir_filter_list_[i];
      std::map<std::string, bool> filter_params = {
          {"SequenceIdFilter", enable_right_ir_sequence_id_filter_},
      };
      std::map<std::string, std::string> filter_param_names = {
          {"SequenceIdFilter", "enable_right_ir_sequence_id_filter"},
      };
      std::string filter_name = filter->type();
      RCLCPP_DEBUG_STREAM(logger_, "Configuring right IR filter: " << filter_name);
      if (filter_params.find(filter_name) != filter_params.end() &&
          isLaunchParamProvided(filter_param_names[filter_name])) {
        const auto *value = filter_params[filter_name] ? "enabled" : "disabled";
        RCLCPP_INFO_STREAM(logger_, "Set right IR filter " << filter_name << " to " << value);
        filter->enable(filter_params[filter_name]);
      }
      if (filter_name == "SequenceIdFilter" && enable_right_ir_sequence_id_filter_) {
        auto sequenced_filter = filter->as<ob::SequenceIdFilter>();
        if (right_ir_sequence_id_filter_id_ != -1) {
          sequenced_filter->selectSequenceId(right_ir_sequence_id_filter_id_);
        }
        RCLCPP_INFO_STREAM(logger_, "Current right ir SequenceIdFilter ID: "
                                        << sequenced_filter->getSelectSequenceId());
      }
    }
  }
}
void OBCameraNode::setupDepthPostProcessFilter() {
  if (!enable_stream_[DEPTH]) {
    return;
  }
  auto depth_sensor = device_->getSensor(OB_SENSOR_DEPTH);
  // set depth sensor to filter
  depth_filter_list_ = depth_sensor->createRecommendedFilters();
  if (depth_filter_list_.empty()) {
    RCLCPP_DEBUG_STREAM(logger_, "Depth sensor filter list is empty");
    return;
  }
  std::map<std::string, bool> filter_params = {
      {"DecimationFilter", enable_decimation_filter_},
      {"HDRMerge", enable_hdr_merge_},
      {"SequenceIdFilter", enable_sequence_id_filter_},
      {"SpatialAdvancedFilter", enable_spatial_filter_},
      {"TemporalFilter", enable_temporal_filter_},
      {"HoleFillingFilter", enable_hole_filling_filter_},
      {"EdgeNoiseRemovalFilter", enable_edge_noise_removal_filter_},
      {"DisparityTransform", enable_disparity_to_depth_},
      {"ThresholdFilter", enable_threshold_filter_},
      {"SpatialFastFilter", enable_spatial_fast_filter_},
      {"SpatialModerateFilter", enable_spatial_moderate_filter_},
      {"FalsePositiveFilter", enable_false_positive_filter_},
      {"MgcNoiseRemovalFilter", enable_mgc_noise_removal_filter_},
      {"LutNoiseRemovalFilter", enable_lut_noise_removal_filter_},
  };
  std::map<std::string, std::string> filter_param_names = {
      {"DecimationFilter", "enable_decimation_filter"},
      {"HDRMerge", "enable_hdr_merge"},
      {"SequenceIdFilter", "enable_sequence_id_filter"},
      {"SpatialAdvancedFilter", "enable_spatial_filter"},
      {"TemporalFilter", "enable_temporal_filter"},
      {"HoleFillingFilter", "enable_hole_filling_filter"},
      {"EdgeNoiseRemovalFilter", "enable_edge_noise_removal_filter"},
      {"DisparityTransform", "enable_disparity_to_depth"},
      {"ThresholdFilter", "enable_threshold_filter"},
      {"SpatialFastFilter", "enable_spatial_fast_filter"},
      {"SpatialModerateFilter", "enable_spatial_moderate_filter"},
      {"FalsePositiveFilter", "enable_false_positive_filter"},
      {"MgcNoiseRemovalFilter", "enable_mgc_noise_removal_filter"},
      {"LutNoiseRemovalFilter", "enable_lut_noise_removal_filter"},
  };

  for (size_t i = 0; i < depth_filter_list_.size(); i++) {
    auto filter = depth_filter_list_[i];
    std::string filter_name = filter->type();
    RCLCPP_DEBUG_STREAM(logger_, "Configuring depth filter: " << filter_name);
    if (filter_params.find(filter_name) != filter_params.end() &&
        isLaunchParamProvided(filter_param_names[filter_name])) {
      const auto *value = filter_params[filter_name] ? "enabled" : "disabled";
      RCLCPP_INFO_STREAM(logger_, "Set depth filter " << filter_name << " to " << value);
      filter->enable(filter_params[filter_name]);
      filter_status_[filter_name] = filter_params[filter_name];
    }
    if (filter_name == "DecimationFilter" && enable_decimation_filter_) {
      auto decimation_filter = filter->as<ob::DecimationFilter>();
      auto range = decimation_filter->getScaleRange();
      if (decimation_filter_scale_ != -1 && decimation_filter_scale_ <= range.max &&
          decimation_filter_scale_ >= range.min) {
        decimation_filter->setScaleValue(decimation_filter_scale_);
      }
      if (decimation_filter_scale_ != -1 &&
          (decimation_filter_scale_ < range.min || decimation_filter_scale_ > range.max)) {
        RCLCPP_ERROR_STREAM(logger_, "Decimation filter scale value is out of range "
                                         << range.min << " - " << range.max);
      }
      RCLCPP_INFO_STREAM(logger_, "Current decimation filter scale value: "
                                      << static_cast<int>(decimation_filter->getScaleValue()));
    } else if (filter_name == "ThresholdFilter" && enable_threshold_filter_) {
      auto threshold_filter = filter->as<ob::ThresholdFilter>();
      if (threshold_filter_min_ != -1 && threshold_filter_max_ != -1) {
        threshold_filter->setValueRange(threshold_filter_min_, threshold_filter_max_);
      }
      RCLCPP_INFO_STREAM(logger_, "Current threshold filter value range: "
                                      << static_cast<int>(threshold_filter->getConfigValue("min"))
                                      << " - "
                                      << static_cast<int>(threshold_filter->getConfigValue("max")));
    } else if (filter_name == "SpatialAdvancedFilter" && enable_spatial_filter_) {
      auto spatial_filter = filter->as<ob::SpatialAdvancedFilter>();
      if (spatial_filter_alpha_ != -1.0 && spatial_filter_magnitude_ != -1 &&
          spatial_filter_radius_ != -1 && spatial_filter_diff_threshold_ != -1) {
        OBSpatialAdvancedFilterParams params{};
        params.alpha = spatial_filter_alpha_;
        params.magnitude = spatial_filter_magnitude_;
        params.radius = spatial_filter_radius_;
        params.disp_diff = spatial_filter_diff_threshold_;
        spatial_filter->setFilterParams(params);
      }
      auto current_params = spatial_filter->getFilterParams();
      RCLCPP_INFO_STREAM(logger_, "Current SpatialFilter params: "
                                      << "alpha=" << current_params.alpha << ", disp_diff="
                                      << current_params.disp_diff << ", magnitude="
                                      << static_cast<int>(current_params.magnitude)
                                      << ", radius=" << current_params.radius);
    } else if (filter_name == "TemporalFilter" && enable_temporal_filter_) {
      auto temporal_filter = filter->as<ob::TemporalFilter>();
      if (temporal_filter_diff_threshold_ != -1.0 && temporal_filter_weight_ != -1.0) {
        temporal_filter->setDiffScale(temporal_filter_diff_threshold_);
        temporal_filter->setWeight(temporal_filter_weight_);
      }
      RCLCPP_INFO_STREAM(
          logger_,
          "Current TemporalFilter params: "
              << "diff_scale=" << static_cast<float>(temporal_filter->getConfigValue("diff_scale"))
              << ", weight=" << static_cast<float>(temporal_filter->getConfigValue("weight")));
    } else if (filter_name == "HoleFillingFilter" && enable_hole_filling_filter_ &&
               !hole_filling_filter_mode_.empty()) {
      auto hole_filling_filter = filter->as<ob::HoleFillingFilter>();
      RCLCPP_INFO_STREAM(logger_,
                         "Default hole filling filter mode: " << hole_filling_filter_mode_);
      OBHoleFillingMode hole_filling_mode = holeFillingModeFromString(hole_filling_filter_mode_);
      hole_filling_filter->setFilterMode(hole_filling_mode);
      RCLCPP_INFO_STREAM(logger_, "Current HoleFillingFilter mode: "
                                      << static_cast<int>(hole_filling_filter->getFilterMode()));
    } else if (filter_name == "SequenceIdFilter" && enable_sequence_id_filter_) {
      auto sequenced_filter = filter->as<ob::SequenceIdFilter>();
      if (sequence_id_filter_id_ != -1) {
        sequenced_filter->selectSequenceId(sequence_id_filter_id_);
      }
      RCLCPP_INFO_STREAM(
          logger_, "Current SequenceIdFilter ID: " << sequenced_filter->getSelectSequenceId());
    } else if (filter_name == "HDRMerge" && enable_hdr_merge_) {
      if (hdr_merge_exposure_1_ != -1 && hdr_merge_gain_1_ != -1 && hdr_merge_exposure_2_ != -1 &&
          hdr_merge_gain_2_ != -1) {
        auto hdr_merge_filter = filter->as<ob::HdrMerge>();
        hdr_merge_filter->enable(true);
        auto config = OBHdrConfig();
        config.enable = true;
        config.exposure_1 = hdr_merge_exposure_1_;
        config.gain_1 = hdr_merge_gain_1_;
        config.exposure_2 = hdr_merge_exposure_2_;
        config.gain_2 = hdr_merge_gain_2_;
        device_->setStructuredData(OB_STRUCT_DEPTH_HDR_CONFIG,
                                   reinterpret_cast<const uint8_t *>(&config), sizeof(config));
        uint32_t config_size = sizeof(config);
        device_->getStructuredData(OB_STRUCT_DEPTH_HDR_CONFIG, reinterpret_cast<uint8_t *>(&config),
                                   &config_size);
        RCLCPP_INFO_STREAM(
            logger_, "Current HDRMerge params: "
                         << "exposure_1=" << config.exposure_1 << ", gain_1=" << config.gain_1
                         << ", exposure_2=" << config.exposure_2 << ", gain_2=" << config.gain_2);
      }
    } else if (filter_name == "SpatialFastFilter" && enable_spatial_fast_filter_) {
      auto spatial_fast_filter = filter->as<ob::SpatialFastFilter>();
      OBSpatialFastFilterParams params{};
      if (spatial_fast_filter_radius_ != -1) {
        params.radius = spatial_fast_filter_radius_;
        spatial_fast_filter->setFilterParams(params);
      }
      auto current_params = spatial_fast_filter->getFilterParams();
      RCLCPP_INFO_STREAM(
          logger_, "Current SpatialFastFilter radius: " << static_cast<int>(current_params.radius));
    } else if (filter_name == "SpatialModerateFilter" && enable_spatial_moderate_filter_) {
      auto spatial_moderate_filter = filter->as<ob::SpatialModerateFilter>();
      OBSpatialModerateFilterParams params{};
      if (spatial_moderate_filter_diff_threshold_ != -1 &&
          spatial_moderate_filter_magnitude_ != -1 && spatial_moderate_filter_radius_ != -1) {
        params.magnitude = spatial_moderate_filter_magnitude_;
        params.radius = spatial_moderate_filter_radius_;
        params.disp_diff = spatial_moderate_filter_diff_threshold_;
        spatial_moderate_filter->setFilterParams(params);
      }
      auto current_params = spatial_moderate_filter->getFilterParams();
      RCLCPP_INFO_STREAM(logger_, "Current SpatialModerateFilter params: "
                                      << "disp_diff=" << current_params.disp_diff << ", magnitude="
                                      << static_cast<int>(current_params.magnitude)
                                      << ", radius=" << static_cast<int>(current_params.radius));

    } else {
      RCLCPP_DEBUG_STREAM(logger_, "Skip setting filter: " << filter_name);
    }
  }
  auto device_info = device_->getDeviceInfo();
  CHECK_NOTNULL(device_info);
  if (pid_ == GEMINI2_PID || pid_ == GEMINI2L_PID) {
    if (enable_decimation_filter_) {
      auto decimation_filter = std::make_shared<ob::DecimationFilter>();
      decimation_filter->enable(true);
      depth_filter_list_.push_back(decimation_filter);
      auto range = decimation_filter->getScaleRange();
      if (decimation_filter_scale_ != -1 && decimation_filter_scale_ <= range.max &&
          decimation_filter_scale_ >= range.min) {
        decimation_filter->setScaleValue(decimation_filter_scale_);
      }
      if (decimation_filter_scale_ != -1 &&
          (decimation_filter_scale_ < range.min || decimation_filter_scale_ > range.max)) {
        RCLCPP_ERROR_STREAM(logger_, "Decimation filter scale value is out of range "
                                         << range.min << " - " << range.max);
      }
      RCLCPP_INFO_STREAM(logger_, "Current decimation filter scale value: "
                                      << static_cast<int>(decimation_filter->getScaleValue()));
    }
  }
  set_filter_srv_ = node_->create_service<SetFilter>(
      "set_filter", [this](const std::shared_ptr<SetFilter ::Request> request,
                           std::shared_ptr<SetFilter ::Response> response) {
        setFilterCallback(request, response);
      });
}

void OBCameraNode::selectBaseStream() {
  if (enable_stream_[DEPTH]) {
    base_stream_ = DEPTH;
  } else if (enable_stream_[INFRA0]) {
    base_stream_ = INFRA0;
  } else if (enable_stream_[INFRA1]) {
    base_stream_ = INFRA1;
  } else if (enable_stream_[INFRA2]) {
    base_stream_ = INFRA2;
  } else if (enable_stream_[COLOR_LEFT]) {
    base_stream_ = COLOR_LEFT;
  } else if (enable_stream_[COLOR_RIGHT]) {
    base_stream_ = COLOR_RIGHT;
  } else if (enable_stream_[COLOR]) {
    base_stream_ = COLOR;
  }
}

void OBCameraNode::printSensorProfiles(const std::shared_ptr<ob::Sensor> &sensor) {
  auto profiles = sensor->getStreamProfileList();
  for (size_t i = 0; i < profiles->getCount(); i++) {
    auto origin_profile = profiles->getProfile(i);
    if (sensor->getType() == OB_SENSOR_COLOR) {
      auto profile = origin_profile->as<ob::VideoStreamProfile>();
      RCLCPP_INFO_STREAM(
          logger_, "color profile: " << profile->getWidth() << "x" << profile->getHeight() << " "
                                     << profile->getFps() << "fps " << profile->getFormat());
    } else if (sensor->getType() == OB_SENSOR_DEPTH) {
      auto profile = origin_profile->as<ob::VideoStreamProfile>();
      RCLCPP_INFO_STREAM(
          logger_, "depth profile: " << profile->getWidth() << "x" << profile->getHeight() << " "
                                     << profile->getFps() << "fps " << profile->getFormat());
    } else if (sensor->getType() == OB_SENSOR_IR) {
      auto profile = origin_profile->as<ob::VideoStreamProfile>();
      RCLCPP_INFO_STREAM(logger_, "ir profile: " << profile->getWidth() << "x"
                                                 << profile->getHeight() << " " << profile->getFps()
                                                 << "fps " << profile->getFormat());
    } else if (sensor->getType() == OB_SENSOR_ACCEL) {
      auto profile = origin_profile->as<ob::AccelStreamProfile>();
      RCLCPP_INFO_STREAM(logger_, "accel profile: sampleRate " << profile->getSampleRate()
                                                               << "  full scale_range "
                                                               << profile->getFullScaleRange());
    } else if (sensor->getType() == OB_SENSOR_GYRO) {
      auto profile = origin_profile->as<ob::GyroStreamProfile>();
      RCLCPP_INFO_STREAM(logger_, "gyro profile: sampleRate " << profile->getSampleRate()
                                                              << "  full scale_range "
                                                              << profile->getFullScaleRange());
    } else {
      RCLCPP_INFO_STREAM(logger_, "unknown profile: " << magic_enum::enum_name(sensor->getType()));
    }
  }
}

void OBCameraNode::setupProfiles() {
  // Image stream
  for (const auto &elem : IMAGE_STREAMS) {
    if (enable_stream_[elem]) {
      const auto &sensor = sensors_[elem];
      CHECK_NOTNULL(sensor.get());
      auto profiles = sensor->getStreamProfileList();
      CHECK_NOTNULL(profiles.get());
      CHECK(profiles->getCount() > 0);
      for (size_t i = 0; i < profiles->getCount(); i++) {
        auto base_profile = profiles->getProfile(i)->as<ob::VideoStreamProfile>();
        if (base_profile == nullptr) {
          throw std::runtime_error("Failed to get profile " + std::to_string(i));
        }
        auto profile = base_profile->as<ob::VideoStreamProfile>();
        if (profile == nullptr) {
          throw std::runtime_error("Failed cast profile to VideoStreamProfile");
        }
        RCLCPP_DEBUG_STREAM(
            logger_, "Sensor profile: "
                         << "stream_type: " << magic_enum::enum_name(profile->getType())
                         << "Format: " << profile->getFormat() << ", Width: " << profile->getWidth()
                         << ", Height: " << profile->getHeight() << ", FPS: " << profile->getFps());
        supported_profiles_[elem].emplace_back(profile);
      }
      std::shared_ptr<ob::VideoStreamProfile> selected_profile;
      std::shared_ptr<ob::VideoStreamProfile> default_profile;
      try {
        if (width_[elem] == 0 && height_[elem] == 0 && fps_[elem] == 0 &&
            format_[elem] == OB_FORMAT_UNKNOWN) {
          selected_profile = profiles->getProfile(0)->as<ob::VideoStreamProfile>();
        } else {
          if (isGemini305SeriesPID(pid_) && elem == DEPTH) {
            OBHardwareDecimationConfig conf;
            conf.originWidth = width_[elem];
            conf.originHeight = height_[elem];
            conf.factor = depth_decimation_factor_;
            selected_profile = profiles->getVideoStreamProfile(conf, format_[elem], fps_[elem]);
          } else if (isGemini305SeriesPID(pid_) && elem == INFRA1) {
            OBHardwareDecimationConfig conf;
            conf.originWidth = width_[elem];
            conf.originHeight = height_[elem];
            conf.factor = left_ir_decimation_factor_;
            selected_profile = profiles->getVideoStreamProfile(conf, format_[elem], fps_[elem]);
          } else if (isGemini305SeriesPID(pid_) && elem == INFRA2) {
            OBHardwareDecimationConfig conf;
            conf.originWidth = width_[elem];
            conf.originHeight = height_[elem];
            conf.factor = right_ir_decimation_factor_;
            selected_profile = profiles->getVideoStreamProfile(conf, format_[elem], fps_[elem]);
          } else {
            selected_profile = profiles->getVideoStreamProfile(width_[elem], height_[elem],
                                                               format_[elem], fps_[elem]);
          }
        }

      } catch (const ob::Error &ex) {
        RCLCPP_ERROR_STREAM(logger_, "Failed to get "
                                         << stream_name_[elem] << "  profile: "
                                         << orbbec_camera::formatObErrorWithStatus(ex));
        RCLCPP_ERROR_STREAM(
            logger_, "Stream: " << magic_enum::enum_name(elem.first)
                                << ", Stream Index: " << elem.second << ", Width: " << width_[elem]
                                << ", Height: " << height_[elem] << ", FPS: " << fps_[elem]
                                << ", Format: " << magic_enum::enum_name(format_[elem]));
        RCLCPP_ERROR(logger_,
                     "Error: The device might be connected via USB 2.0. Please verify your "
                     "configuration and try again. The current process will now exit.");
        RCLCPP_INFO_STREAM(logger_, "Available profiles:");
        printSensorProfiles(sensor);
        RCLCPP_ERROR(logger_, "Failed to configure the requested stream profile, exiting.");
        exit(-1);
      }

      if (!selected_profile) {
        RCLCPP_WARN_STREAM(logger_,
                           "Requested stream configuration is not supported by the device: "
                               << "stream=" << magic_enum::enum_name(elem.first)
                               << ", stream_index=" << elem.second << ", width=" << width_[elem]
                               << ", height=" << height_[elem] << ", fps=" << fps_[elem]
                               << ", format=" << magic_enum::enum_name(format_[elem]));
        if (default_profile) {
          RCLCPP_WARN_STREAM(logger_, "Using the default profile instead");
          RCLCPP_WARN_STREAM(logger_, "Default profile FPS: " << default_profile->getFps());
          selected_profile = default_profile;
        } else {
          RCLCPP_ERROR_STREAM(logger_, "No default profile found, disabling stream "
                                           << magic_enum::enum_name(elem.first));
          enable_stream_[elem] = false;
          continue;
        }
      }
      CHECK_NOTNULL(selected_profile);
      stream_profile_[elem] = selected_profile;
      height_[elem] = static_cast<int>(selected_profile->getHeight());
      width_[elem] = static_cast<int>(selected_profile->getWidth());
      fps_[elem] = static_cast<int>(selected_profile->getFps());
      format_[elem] = selected_profile->getFormat();
      updateImageConfig(elem);
      if (selected_profile->format() == OB_FORMAT_BGRA) {
        images_[elem] = cv::Mat(height_[elem], width_[elem], CV_8UC4, cv::Scalar(0, 0, 0, 0));
        encoding_[elem] = sensor_msgs::image_encodings::BGRA8;
        unit_step_size_[COLOR] = 4 * sizeof(uint8_t);
      } else if (selected_profile->format() == OB_FORMAT_RGBA) {
        images_[elem] = cv::Mat(height_[elem], width_[elem], CV_8UC4, cv::Scalar(0, 0, 0, 0));
        encoding_[elem] = sensor_msgs::image_encodings::RGBA8;
        unit_step_size_[COLOR] = 4 * sizeof(uint8_t);
      } else {
        images_[elem] =
            cv::Mat(height_[elem], width_[elem], image_format_[elem], cv::Scalar(0, 0, 0));
      }
    }
  }
  // IMU
  for (const auto &stream_index : HID_STREAMS) {
    if (!enable_stream_[stream_index]) {
      continue;
    }
    try {
      auto profile_list = sensors_[stream_index]->getStreamProfileList();
      if (stream_index == ACCEL) {
        auto full_scale_range = fullAccelScaleRangeFromString(imu_range_[stream_index]);
        auto sample_rate = sampleRateFromString(imu_rate_[stream_index]);
        auto profile = profile_list->getAccelStreamProfile(full_scale_range, sample_rate);
        stream_profile_[stream_index] = profile;
      } else if (stream_index == GYRO) {
        auto full_scale_range = fullGyroScaleRangeFromString(imu_range_[stream_index]);
        auto sample_rate = sampleRateFromString(imu_rate_[stream_index]);
        auto profile = profile_list->getGyroStreamProfile(full_scale_range, sample_rate);
        stream_profile_[stream_index] = profile;
      }
      RCLCPP_INFO_STREAM(logger_, "stream " << stream_name_[stream_index] << " full scale range "
                                            << imu_range_[stream_index] << " sample rate "
                                            << imu_rate_[stream_index]);
    } catch (const ob::Error &e) {
      RCLCPP_INFO_STREAM(logger_, "Failed to setup << "
                                      << stream_name_[stream_index]
                                      << " profile: " << orbbec_camera::formatObErrorWithStatus(e));
      enable_stream_[stream_index] = false;
      stream_profile_[stream_index] = nullptr;
    }
  }
}

std::shared_ptr<ob::VideoStreamProfile> OBCameraNode::selectVideoStreamProfile(
    const stream_index_pair &stream_index, int width, int height, int fps, OBFormat format) {
  auto sensor_it = sensors_.find(stream_index);
  if (sensor_it == sensors_.end() || !sensor_it->second) {
    throw std::runtime_error("Sensor is not available for stream " + stream_name_[stream_index]);
  }
  auto profiles = sensor_it->second->getStreamProfileList();
  if (!profiles || profiles->getCount() == 0) {
    throw std::runtime_error("No stream profiles available for stream " +
                             stream_name_[stream_index]);
  }

  std::shared_ptr<ob::VideoStreamProfile> selected_profile;
  if (width == 0 && height == 0 && fps == 0) {
    selected_profile = profiles->getProfile(0)->as<ob::VideoStreamProfile>();
  } else if (isGemini305SeriesPID(pid_) && stream_index == DEPTH) {
    OBHardwareDecimationConfig conf;
    conf.originWidth = width;
    conf.originHeight = height;
    conf.factor = depth_decimation_factor_;
    selected_profile = profiles->getVideoStreamProfile(conf, format, fps);
  } else if (isGemini305SeriesPID(pid_) && stream_index == INFRA1) {
    OBHardwareDecimationConfig conf;
    conf.originWidth = width;
    conf.originHeight = height;
    conf.factor = left_ir_decimation_factor_;
    selected_profile = profiles->getVideoStreamProfile(conf, format, fps);
  } else if (isGemini305SeriesPID(pid_) && stream_index == INFRA2) {
    OBHardwareDecimationConfig conf;
    conf.originWidth = width;
    conf.originHeight = height;
    conf.factor = right_ir_decimation_factor_;
    selected_profile = profiles->getVideoStreamProfile(conf, format, fps);
  } else {
    selected_profile = profiles->getVideoStreamProfile(width, height, format, fps);
  }

  if (!selected_profile) {
    throw std::runtime_error("Requested stream profile is not supported");
  }
  return selected_profile;
}

std::optional<stream_index_pair> OBCameraNode::getImageStreamByName(
    const std::string &stream_name) const {
  if (stream_name == "color") {
    return COLOR;
  }
  if (stream_name == "left_color") {
    return COLOR_LEFT;
  }
  if (stream_name == "right_color") {
    return COLOR_RIGHT;
  }
  if (stream_name == "depth") {
    return DEPTH;
  }
  if (stream_name == "ir") {
    return INFRA0;
  }
  if (stream_name == "left_ir") {
    return INFRA1;
  }
  if (stream_name == "right_ir") {
    return INFRA2;
  }
  return std::nullopt;
}

bool OBCameraNode::validateStreamProfileRequest(
    const std::shared_ptr<SetStreamProfile::Request> &request,
    std::vector<PendingStreamProfile> &pending_profiles, std::string &message) {
  pending_profiles.clear();
  if (!request || request->profiles.empty()) {
    message = "profiles is empty";
    return false;
  }

  std::unordered_set<std::string> requested_streams;
  bool has_changes = false;
  for (const auto &profile : request->profiles) {
    const auto stream_index = getImageStreamByName(profile.stream_name);
    if (!stream_index) {
      message = "Unsupported stream_name: " + profile.stream_name +
                ". Supported stream_name values: color, left_color, right_color, depth, ir, "
                "left_ir, right_ir";
      return false;
    }
    if (!requested_streams.insert(profile.stream_name).second) {
      message = "Duplicated stream_name: " + profile.stream_name;
      return false;
    }
    if (!enable_stream_[*stream_index]) {
      message = "Stream is not enabled: " + profile.stream_name;
      return false;
    }

    if (profile.width < 0 || profile.height < 0 || profile.fps < 0) {
      message = profile.stream_name + " width, height and fps must be non-negative";
      return false;
    }
    if ((profile.width > 0) != (profile.height > 0)) {
      message = profile.stream_name + " width and height must be provided together";
      return false;
    }
    if (profile.width <= 0 && profile.fps <= 0 && profile.format.empty()) {
      message = profile.stream_name + " must provide resolution, fps or format";
      return false;
    }

    const int requested_width = profile.width > 0 ? profile.width : width_[*stream_index];
    const int requested_height = profile.height > 0 ? profile.height : height_[*stream_index];
    const int requested_fps = profile.fps > 0 ? profile.fps : fps_[*stream_index];
    if (requested_width <= 0 || requested_height <= 0 || requested_fps <= 0) {
      message = profile.stream_name + " current width, height and fps must be positive";
      return false;
    }

    OBFormat requested_format = format_[*stream_index];
    if (!profile.format.empty()) {
      std::string format_name;
      format_name.reserve(profile.format.size());
      std::transform(profile.format.begin(), profile.format.end(), std::back_inserter(format_name),
                     [](unsigned char ch) { return static_cast<char>(std::toupper(ch)); });
      if (format_name == "ANY") {
        requested_format = OB_FORMAT_UNKNOWN;
      } else {
        requested_format = OBFormatFromString(format_name);
        if (requested_format == OB_FORMAT_UNKNOWN) {
          message = "Unsupported format: " + profile.format;
          return false;
        }
      }
    }

    try {
      auto selected_profile = selectVideoStreamProfile(
          *stream_index, requested_width, requested_height, requested_fps, requested_format);
      has_changes = has_changes ||
                    static_cast<int>(selected_profile->getWidth()) != width_[*stream_index] ||
                    static_cast<int>(selected_profile->getHeight()) != height_[*stream_index] ||
                    static_cast<int>(selected_profile->getFps()) != fps_[*stream_index] ||
                    selected_profile->getFormat() != format_[*stream_index];
      pending_profiles.push_back(
          {*stream_index, requested_width, requested_height, requested_fps, selected_profile});
    } catch (const ob::Error &e) {
      message = "Unsupported profile for " + profile.stream_name + ": " +
                orbbec_camera::formatObErrorWithStatus(e);
      return false;
    } catch (const std::exception &e) {
      message = "Unsupported profile for " + profile.stream_name + ": " + e.what();
      return false;
    }
  }
  if (!has_changes) {
    message = "requested stream profiles are already active";
    return false;
  }
  return true;
}

bool OBCameraNode::applyStreamProfiles(const std::vector<PendingStreamProfile> &pending_profiles,
                                       std::string &message) {
  if (pending_profiles.empty()) {
    message = "profiles is empty";
    return false;
  }

  std::lock_guard<decltype(device_lock_)> lock(device_lock_);
  try {
    const bool restart_pipeline = pipeline_started_.load();
    const bool interleave_frame_enable = interleave_frame_enable_;
    if (restart_pipeline) {
      stopStreams();
      interleave_frame_enable_ = interleave_frame_enable;
    }
    stopColorFrameThreads();
    clearColorFrameQueues();

    bool profile_affects_hw_d2c_color_undistortion = false;
    for (const auto &pending_profile : pending_profiles) {
      const auto &stream_index = pending_profile.stream_index;
      if (stream_index == COLOR || stream_index == DEPTH) {
        profile_affects_hw_d2c_color_undistortion = true;
      }
      auto selected_profile = pending_profile.profile;
      const auto old_format = format_[stream_index];
      stream_profile_[stream_index] = selected_profile;
      height_[stream_index] = static_cast<int>(selected_profile->getHeight());
      width_[stream_index] = static_cast<int>(selected_profile->getWidth());
      fps_[stream_index] = static_cast<int>(selected_profile->getFps());
      format_[stream_index] = selected_profile->getFormat();
      format_str_[stream_index] = OBFormatToString(format_[stream_index]);
      updateImageConfig(stream_index);
      if (old_format != format_[stream_index]) {
        setupImagePublisher(stream_index);
      }
      if (selected_profile->format() == OB_FORMAT_BGRA) {
        images_[stream_index] =
            cv::Mat(height_[stream_index], width_[stream_index], CV_8UC4, cv::Scalar(0, 0, 0, 0));
        encoding_[stream_index] = sensor_msgs::image_encodings::BGRA8;
        unit_step_size_[stream_index] = 4 * sizeof(uint8_t);
      } else if (selected_profile->format() == OB_FORMAT_RGBA) {
        images_[stream_index] =
            cv::Mat(height_[stream_index], width_[stream_index], CV_8UC4, cv::Scalar(0, 0, 0, 0));
        encoding_[stream_index] = sensor_msgs::image_encodings::RGBA8;
        unit_step_size_[stream_index] = 4 * sizeof(uint8_t);
      } else {
        images_[stream_index] = cv::Mat(height_[stream_index], width_[stream_index],
                                        image_format_[stream_index], cv::Scalar(0, 0, 0));
      }
    }
    if (profile_affects_hw_d2c_color_undistortion && shouldUseHwD2CColorUndistortion()) {
      hw_d2c_color_undistortion_configured_ = false;
    }

    setupImageBuffers();
    clearColorFrameQueues();
    {
      std::lock_guard<std::mutex> frame_info_lock(frame_info_logged_mutex_);
      frame_info_logged_.clear();
    }
    if (restart_pipeline) {
      startStreams();
      message = "stream profiles updated";
    } else {
      message = "stream profiles updated, changes will take effect when streams are started";
    }
    return true;
  } catch (const ob::Error &e) {
    message = orbbec_camera::formatObErrorWithStatus(e);
  } catch (const std::exception &e) {
    message = e.what();
  } catch (...) {
    message = "unknown error";
  }
  return false;
}

void OBCameraNode::clearColorFrameQueues() {
  {
    std::lock_guard<std::mutex> lock(color_frame_queue_lock_);
    std::queue<std::shared_ptr<ob::FrameSet>> empty;
    std::swap(color_frame_queue_, empty);
  }
  {
    std::lock_guard<std::mutex> lock(left_color_frame_queue_lock_);
    std::queue<std::shared_ptr<ob::FrameSet>> empty;
    std::swap(left_color_frame_queue_, empty);
  }
  {
    std::lock_guard<std::mutex> lock(right_color_frame_queue_lock_);
    std::queue<std::shared_ptr<ob::FrameSet>> empty;
    std::swap(right_color_frame_queue_, empty);
  }
  is_color_frame_decoded_ = false;
  is_left_color_frame_decoded_ = false;
  is_right_color_frame_decoded_ = false;
}

void OBCameraNode::stopColorFrameThreads() {
  if (!colorFrameThread_ && !leftColorFrameThread_ && !rightColorFrameThread_) {
    return;
  }

  stop_color_frame_threads_.store(true);
  color_frame_queue_cv_.notify_all();
  left_color_frame_queue_cv_.notify_all();
  right_color_frame_queue_cv_.notify_all();

  if (colorFrameThread_ && colorFrameThread_->joinable()) {
    colorFrameThread_->join();
  }
  if (leftColorFrameThread_ && leftColorFrameThread_->joinable()) {
    leftColorFrameThread_->join();
  }
  if (rightColorFrameThread_ && rightColorFrameThread_->joinable()) {
    rightColorFrameThread_->join();
  }

  colorFrameThread_.reset();
  leftColorFrameThread_.reset();
  rightColorFrameThread_.reset();
  stop_color_frame_threads_.store(false);
}

void OBCameraNode::setupImageBuffers() {
  delete[] rgb_buffer_;
  rgb_buffer_ = nullptr;
  rgb_buffer_size_ = 0;
  delete[] rgb_buffer_left_;
  rgb_buffer_left_ = nullptr;
  rgb_buffer_left_size_ = 0;
  delete[] rgb_buffer_right_;
  rgb_buffer_right_ = nullptr;
  rgb_buffer_right_size_ = 0;
  jpeg_decoder_.reset();
  jpeg_decoder_left_.reset();
  jpeg_decoder_right_.reset();

#if defined(USE_RK_HW_DECODER)
  if (enable_stream_[COLOR] && width_.count(COLOR) && height_.count(COLOR)) {
    jpeg_decoder_ = std::make_unique<RKJPEGDecoder>(width_[COLOR], height_[COLOR]);
  }
  if (enable_stream_[COLOR_LEFT] && width_.count(COLOR_LEFT) && height_.count(COLOR_LEFT)) {
    jpeg_decoder_left_ = std::make_unique<RKJPEGDecoder>(width_[COLOR_LEFT], height_[COLOR_LEFT]);
  }
  if (enable_stream_[COLOR_RIGHT] && width_.count(COLOR_RIGHT) && height_.count(COLOR_RIGHT)) {
    jpeg_decoder_right_ =
        std::make_unique<RKJPEGDecoder>(width_[COLOR_RIGHT], height_[COLOR_RIGHT]);
  }
#elif defined(USE_NV_HW_DECODER)
  if (enable_stream_[COLOR] && width_.count(COLOR) && height_.count(COLOR)) {
    jpeg_decoder_ = std::make_unique<JetsonNvJPEGDecoder>(width_[COLOR], height_[COLOR]);
  }
  if (enable_stream_[COLOR_LEFT] && width_.count(COLOR_LEFT) && height_.count(COLOR_LEFT)) {
    jpeg_decoder_left_ =
        std::make_unique<JetsonNvJPEGDecoder>(width_[COLOR_LEFT], height_[COLOR_LEFT]);
  }
  if (enable_stream_[COLOR_RIGHT] && width_.count(COLOR_RIGHT) && height_.count(COLOR_RIGHT)) {
    jpeg_decoder_right_ =
        std::make_unique<JetsonNvJPEGDecoder>(width_[COLOR_RIGHT], height_[COLOR_RIGHT]);
  }
#endif
  if (enable_stream_[COLOR] && width_.count(COLOR) && height_.count(COLOR)) {
    rgb_buffer_size_ = static_cast<size_t>(width_[COLOR]) * height_[COLOR] * 4;
    rgb_buffer_ = new uint8_t[rgb_buffer_size_];
  }
  if (enable_stream_[COLOR_LEFT] && width_.count(COLOR_LEFT) && height_.count(COLOR_LEFT)) {
    rgb_buffer_left_size_ = static_cast<size_t>(width_[COLOR_LEFT]) * height_[COLOR_LEFT] * 4;
    rgb_buffer_left_ = new uint8_t[rgb_buffer_left_size_];
  }
  if (enable_stream_[COLOR_RIGHT] && width_.count(COLOR_RIGHT) && height_.count(COLOR_RIGHT)) {
    rgb_buffer_right_size_ = static_cast<size_t>(width_[COLOR_RIGHT]) * height_[COLOR_RIGHT] * 4;
    rgb_buffer_right_ = new uint8_t[rgb_buffer_right_size_];
  }
  rgb_point_cloud_buffer_size_ = 0;
  xy_table_data_size_ = 0;
  if (enable_colored_point_cloud_ && enable_stream_[DEPTH] && enable_stream_[COLOR]) {
    rgb_point_cloud_buffer_size_ = width_[COLOR] * height_[COLOR] * sizeof(OBColorPoint);
    xy_table_data_size_ = width_[DEPTH] * height_[DEPTH] * 2;
  }
}

void OBCameraNode::updateImageConfig(const stream_index_pair &stream_index) {
  const auto format = format_[stream_index];
  const bool is_depth_stream = stream_index.first == OB_STREAM_DEPTH;
  const bool is_ir_stream = stream_index.first == OB_STREAM_IR ||
                            stream_index.first == OB_STREAM_IR_LEFT ||
                            stream_index.first == OB_STREAM_IR_RIGHT;
  const bool is_color_stream =
      stream_index == COLOR || stream_index == COLOR_LEFT || stream_index == COLOR_RIGHT;

  if (format == OB_FORMAT_Y8 || format == OB_FORMAT_GRAY) {
    image_format_[stream_index] = CV_8UC1;
    encoding_[stream_index] = is_depth_stream ? sensor_msgs::image_encodings::TYPE_8UC1
                                              : sensor_msgs::image_encodings::MONO8;
    unit_step_size_[stream_index] = sizeof(uint8_t);
  } else if (format == OB_FORMAT_Y10 || format == OB_FORMAT_Y11 || format == OB_FORMAT_Y12 ||
             format == OB_FORMAT_Y14 || format == OB_FORMAT_Y16 || format == OB_FORMAT_Z16 ||
             format == OB_FORMAT_RW16) {
    image_format_[stream_index] = CV_16UC1;
    encoding_[stream_index] = is_depth_stream ? sensor_msgs::image_encodings::TYPE_16UC1
                                              : sensor_msgs::image_encodings::MONO16;
    unit_step_size_[stream_index] = sizeof(uint16_t);
  } else if (format == OB_FORMAT_MJPG || format == OB_FORMAT_MJPEG) {
    if (is_ir_stream) {
      image_format_[stream_index] = CV_8UC1;
      encoding_[stream_index] = sensor_msgs::image_encodings::MONO8;
      unit_step_size_[stream_index] = sizeof(uint8_t);
    } else if (is_color_stream) {
      image_format_[stream_index] = CV_8UC3;
      encoding_[stream_index] = sensor_msgs::image_encodings::RGB8;
      unit_step_size_[stream_index] = 3 * sizeof(uint8_t);
    }
  } else if (format == OB_FORMAT_BGR) {
    image_format_[stream_index] = CV_8UC3;
    encoding_[stream_index] = sensor_msgs::image_encodings::BGR8;
    unit_step_size_[stream_index] = 3 * sizeof(uint8_t);
  } else if (format == OB_FORMAT_RGB || format == OB_FORMAT_RGB888) {
    image_format_[stream_index] = CV_8UC3;
    encoding_[stream_index] = sensor_msgs::image_encodings::RGB8;
    unit_step_size_[stream_index] = 3 * sizeof(uint8_t);
  } else if (format == OB_FORMAT_BGRA) {
    image_format_[stream_index] = CV_8UC4;
    encoding_[stream_index] = sensor_msgs::image_encodings::BGRA8;
    unit_step_size_[stream_index] = 4 * sizeof(uint8_t);
  } else if (format == OB_FORMAT_RGBA) {
    image_format_[stream_index] = CV_8UC4;
    encoding_[stream_index] = sensor_msgs::image_encodings::RGBA8;
    unit_step_size_[stream_index] = 4 * sizeof(uint8_t);
  }
}
int OBCameraNode::init_interleave_hdr_param() {
  auto set_int_if_provided = [this](OBPropertyID property_id, int value) {
    if (value != -1) {
      device_->setIntProperty(property_id, value);
    }
  };

  device_->setIntProperty(OB_PROP_FRAME_INTERLEAVE_CONFIG_INDEX_INT, 1);
  if (!isnotLaserDevices(pid_) && hdr_index1_laser_control_ != -1) {
    device_->setIntProperty(OB_PROP_LASER_CONTROL_INT, hdr_index1_laser_control_);
  }
  set_int_if_provided(OB_PROP_DEPTH_EXPOSURE_INT, hdr_index1_depth_exposure_);
  set_int_if_provided(OB_PROP_IR_EXPOSURE_INT, hdr_index1_depth_exposure_);
  set_int_if_provided(OB_PROP_DEPTH_GAIN_INT, hdr_index1_depth_gain_);
  set_int_if_provided(OB_PROP_IR_BRIGHTNESS_INT, hdr_index1_ir_brightness_);
  set_int_if_provided(OB_PROP_IR_AE_MAX_EXPOSURE_INT, hdr_index1_ir_ae_max_exposure_);

  // set interleaveae
  device_->setIntProperty(OB_PROP_FRAME_INTERLEAVE_CONFIG_INDEX_INT, 0);
  if (!isnotLaserDevices(pid_) && hdr_index0_laser_control_ != -1) {
    device_->setIntProperty(OB_PROP_LASER_CONTROL_INT, hdr_index0_laser_control_);
  }
  set_int_if_provided(OB_PROP_DEPTH_EXPOSURE_INT, hdr_index0_depth_exposure_);
  set_int_if_provided(OB_PROP_IR_EXPOSURE_INT, hdr_index0_depth_exposure_);
  set_int_if_provided(OB_PROP_DEPTH_GAIN_INT, hdr_index0_depth_gain_);
  set_int_if_provided(OB_PROP_IR_BRIGHTNESS_INT, hdr_index0_ir_brightness_);
  set_int_if_provided(OB_PROP_IR_AE_MAX_EXPOSURE_INT, hdr_index0_ir_ae_max_exposure_);
  return 0;
}

int OBCameraNode::init_interleave_laser_param() {
  auto set_int_if_provided = [this](OBPropertyID property_id, int value) {
    if (value != -1) {
      device_->setIntProperty(property_id, value);
    }
  };

  device_->setIntProperty(OB_PROP_FRAME_INTERLEAVE_CONFIG_INDEX_INT, 1);
  set_int_if_provided(OB_PROP_LASER_CONTROL_INT, laser_index1_laser_control_);
  set_int_if_provided(OB_PROP_DEPTH_EXPOSURE_INT, laser_index1_depth_exposure_);
  set_int_if_provided(OB_PROP_IR_EXPOSURE_INT, laser_index1_depth_exposure_);
  set_int_if_provided(OB_PROP_DEPTH_GAIN_INT, laser_index1_depth_gain_);
  set_int_if_provided(OB_PROP_IR_BRIGHTNESS_INT, laser_index1_ir_brightness_);
  set_int_if_provided(OB_PROP_IR_AE_MAX_EXPOSURE_INT, laser_index1_ir_ae_max_exposure_);

  // set interleaveae
  device_->setIntProperty(OB_PROP_FRAME_INTERLEAVE_CONFIG_INDEX_INT, 0);
  set_int_if_provided(OB_PROP_LASER_CONTROL_INT, laser_index0_laser_control_);
  set_int_if_provided(OB_PROP_DEPTH_EXPOSURE_INT, laser_index0_depth_exposure_);
  set_int_if_provided(OB_PROP_IR_EXPOSURE_INT, laser_index0_depth_exposure_);
  set_int_if_provided(OB_PROP_DEPTH_GAIN_INT, laser_index0_depth_gain_);
  set_int_if_provided(OB_PROP_IR_BRIGHTNESS_INT, laser_index0_ir_brightness_);
  set_int_if_provided(OB_PROP_IR_AE_MAX_EXPOSURE_INT, laser_index0_ir_ae_max_exposure_);
  return 0;
}
void OBCameraNode::startStreams() {
  if (pipeline_ != nullptr) {
    pipeline_.reset();
  }
  pipeline_ = std::make_unique<ob::Pipeline>(device_);

  try {
    setupPipelineConfig();
    pipeline_->start(pipeline_config_, [this](const std::shared_ptr<ob::FrameSet> &frame_set) {
      onNewFrameSetCallback(frame_set);
    });
  } catch (const ob::Error &e) {
    RCLCPP_ERROR_STREAM(logger_,
                        "Failed to start pipeline: " << orbbec_camera::formatObErrorWithStatus(e));
    RCLCPP_INFO_STREAM(logger_, "try to disable ir stream and try again");
    enable_stream_[INFRA0] = false;
    setupPipelineConfig();
    pipeline_->start(pipeline_config_, [this](const std::shared_ptr<ob::FrameSet> &frame_set) {
      onNewFrameSetCallback(frame_set);
    });
  } catch (...) {
    RCLCPP_ERROR_STREAM(logger_, "Failed to start pipeline");
    throw std::runtime_error("Failed to start pipeline");
  }
  if (enable_stream_[COLOR] && !colorFrameThread_) {
    colorFrameThread_ = std::make_shared<std::thread>([this]() { onNewColorFrameCallback(); });
  }
  if (enable_stream_[COLOR_LEFT] && !leftColorFrameThread_) {
    leftColorFrameThread_ =
        std::make_shared<std::thread>([this]() { onNewLeftColorFrameCallback(); });
  }
  if (enable_stream_[COLOR_RIGHT] && !rightColorFrameThread_) {
    rightColorFrameThread_ =
        std::make_shared<std::thread>([this]() { onNewRightColorFrameCallback(); });
  }
  if (enable_frame_sync_) {
    RCLCPP_INFO_STREAM(logger_, "Enable frame sync");
    TRY_EXECUTE_BLOCK(pipeline_->enableFrameSync());
  } else {
    RCLCPP_INFO_STREAM(logger_, "Disable frame sync");
    TRY_EXECUTE_BLOCK(pipeline_->disableFrameSync());
  }
  std::this_thread::sleep_for(std::chrono::milliseconds(1000));
  // set interleave mode
  if (interleave_ae_mode_ == "hdr" && interleave_frame_enable_) {
    RCLCPP_INFO_STREAM(logger_, "Set interleave mode to hdr");
    device_->loadFrameInterleave("Depth from HDR");
    init_interleave_hdr_param();
  } else if (interleave_ae_mode_ == "laser" && interleave_frame_enable_) {
    RCLCPP_INFO_STREAM(logger_, "Set interleave mode to laser");
    device_->loadFrameInterleave("Laser On-Off");
    init_interleave_laser_param();
  } else {
    RCLCPP_DEBUG_STREAM(logger_, "Set interleave mode to nothing");
  }
  // enable interleave frame
  if ((interleave_ae_mode_ == "hdr") || (interleave_ae_mode_ == "laser")) {
    RCLCPP_INFO_STREAM(logger_, "current interleave_ae_mode_: " << interleave_ae_mode_);
    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(
          logger_,
          "Enable enable_interleave_depth_frame to "
              << (device_->getBoolProperty(OB_PROP_FRAME_INTERLEAVE_ENABLE_BOOL) ? "true"
                                                                                 : "false"));
    }
  }
  // set interleave larse PATTERN_SYNC_DELAY
  if ((interleave_ae_mode_ == "laser") && interleave_frame_enable_ &&
      device_->isPropertySupported(OB_PROP_FRAME_INTERLEAVE_LASER_PATTERN_SYNC_DELAY_INT,
                                   OB_PERMISSION_READ_WRITE) &&
      (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));
  }
  pipeline_started_.store(true);
}

void OBCameraNode::startIMUSyncStream() {
  if (imuPipeline_ != nullptr) {
    imuPipeline_.reset();
  }

  imuPipeline_ = std::make_unique<ob::Pipeline>(device_);
  if (imu_sync_output_start_) {
    return;
  }

  // ACCEL
  auto accelProfiles = imuPipeline_->getStreamProfileList(OB_SENSOR_ACCEL);
  auto accel_range = fullAccelScaleRangeFromString(imu_range_[ACCEL]);
  auto accel_rate = sampleRateFromString(imu_rate_[ACCEL]);
  auto accelProfile = accelProfiles->getAccelStreamProfile(accel_range, accel_rate);
  // GYRO
  auto gyroProfiles = imuPipeline_->getStreamProfileList(OB_SENSOR_GYRO);
  auto gyro_range = fullGyroScaleRangeFromString(imu_range_[GYRO]);
  auto gyro_rate = sampleRateFromString(imu_rate_[GYRO]);
  auto gyroProfile = gyroProfiles->getGyroStreamProfile(gyro_range, gyro_rate);
  std::shared_ptr<ob::Config> imuConfig = std::make_shared<ob::Config>();
  imuConfig->enableStream(accelProfile);
  imuConfig->enableStream(gyroProfile);
  TRY_EXECUTE_BLOCK(imuPipeline_->enableFrameSync());

  try {
    imuPipeline_->start(imuConfig, [&](std::shared_ptr<ob::Frame> frame) {
      auto frameSet = frame->as<ob::FrameSet>();
      auto aFrame = frameSet->getFrame(OB_FRAME_ACCEL);
      auto gFrame = frameSet->getFrame(OB_FRAME_GYRO);
      const bool log_imu_timestamps = is_camera_node_initialized_.load() && rclcpp::ok() &&
                                      imu_timestamp_csv_logger_ &&
                                      imu_timestamp_csv_logger_->enabled();
      const auto arrival_system_us = log_imu_timestamps ? getSystemNowUs() : 0;
      if (aFrame && gFrame) {
        onNewIMUFrameSyncOutputCallback(aFrame, gFrame, arrival_system_us);
      } else if (log_imu_timestamps && (aFrame || gFrame)) {
        imu_timestamp_csv_logger_->recordFrameSet(aFrame, gFrame, arrival_system_us, std::nullopt);
      }
    });

    imu_sync_output_start_ = true;
    RCLCPP_INFO_STREAM(
        logger_, "start accel stream with range: " << fullAccelScaleRangeToString(accel_range)
                                                   << ",rate:" << sampleRateToString(accel_rate)
                                                   << ", and start gyro stream with range:"
                                                   << fullGyroScaleRangeToString(gyro_range)
                                                   << ",rate:" << sampleRateToString(gyro_rate));
  } catch (const ob::Error &e) {
    RCLCPP_ERROR_STREAM(
        logger_, "Failed to start IMU sync stream: " << orbbec_camera::formatObErrorWithStatus(e));
    imu_sync_output_start_ = false;
  } catch (...) {
    RCLCPP_ERROR_STREAM(
        logger_, "Failed to start IMU stream, please check the imu_rate and imu_range parameters.");
    imu_sync_output_start_ = false;
  }
}

void OBCameraNode::startIMU() {
  if (enable_sync_output_accel_gyro_) {
    startIMUSyncStream();
  } else {
    for (const auto &stream_index : HID_STREAMS) {
      if (enable_stream_[stream_index] && !imu_started_[stream_index]) {
        auto imu_profile = stream_profile_[stream_index];
        CHECK_NOTNULL(imu_profile);
        RCLCPP_INFO_STREAM(logger_, "start " << stream_name_[stream_index] << " stream");
        CHECK_NOTNULL(sensors_[stream_index]);
        sensors_[stream_index]->start(
            imu_profile, [this, stream_index](const std::shared_ptr<ob::Frame> &frame) {
              onNewIMUFrameCallback(frame, stream_index);
            });
        imu_started_[stream_index] = true;
      }
    }
  }
}

void OBCameraNode::restartPlaybackStreams() {
  if (!is_playback_device_ || !is_running_.load()) {
    return;
  }

  try {
    stopStreams();
    startStreams();
    stopIMU();
    startIMU();
  } catch (const ob::Error &e) {
    RCLCPP_WARN_STREAM(logger_, "Failed to restart playback streams: "
                                    << orbbec_camera::formatObErrorWithStatus(e));
  } catch (const std::exception &e) {
    RCLCPP_WARN_STREAM(logger_, "Failed to restart playback streams: " << e.what());
  } catch (...) {
    RCLCPP_WARN_STREAM(logger_, "Failed to restart playback streams");
  }
}

void OBCameraNode::stopStreams() {
  std::lock_guard<decltype(device_lock_)> lock(device_lock_);

  if (!pipeline_started_ || !pipeline_) {
    RCLCPP_DEBUG_STREAM(logger_, "pipeline not started or not exist, skip stop pipeline");
    return;
  }

  // Mark pipeline as stopping to prevent new operations
  pipeline_started_.store(false);

  try {
    // Check if device is still valid before stopping pipeline
    if (device_ && pipeline_) {
      pipeline_->stop();

      // disable interleave frame only if device is still connected
      if ((interleave_ae_mode_ == "hdr") || (interleave_ae_mode_ == "laser")) {
        try {
          RCLCPP_DEBUG_STREAM(logger_, "Current interleave AE mode: " << interleave_ae_mode_);
          if (device_->isPropertySupported(OB_PROP_FRAME_INTERLEAVE_ENABLE_BOOL,
                                           OB_PERMISSION_WRITE)) {
            interleave_frame_enable_ = false;
            RCLCPP_DEBUG_STREAM(logger_, "Set enable_interleave_depth_frame to "
                                             << (interleave_frame_enable_ ? "true" : "false"));
            TRY_TO_SET_PROPERTY(setBoolProperty, OB_PROP_FRAME_INTERLEAVE_ENABLE_BOOL,
                                interleave_frame_enable_);
          }
        } catch (const ob::Error &e) {
          RCLCPP_WARN_STREAM(logger_, "Failed to disable interleave frame during shutdown: "
                                          << orbbec_camera::formatObErrorWithStatus(e));
        } catch (...) {
          RCLCPP_WARN_STREAM(logger_, "Failed to disable interleave frame during shutdown");
        }
      }
    } else {
      RCLCPP_WARN_STREAM(logger_,
                         "Device or pipeline not available during stop - likely disconnected");
    }
  } catch (const ob::Error &e) {
    RCLCPP_ERROR_STREAM(logger_,
                        "Failed to stop pipeline: " << orbbec_camera::formatObErrorWithStatus(e));
  } catch (...) {
    RCLCPP_ERROR_STREAM(logger_, "Failed to stop pipeline");
  }
}

void OBCameraNode::stopIMU() {
  std::lock_guard<decltype(device_lock_)> lock(device_lock_);

  if (enable_sync_output_accel_gyro_) {
    if (!imu_sync_output_start_ || !imuPipeline_) {
      RCLCPP_DEBUG_STREAM(logger_,
                          "IMU pipeline not started or unavailable, skip stopping IMU pipeline");
      return;
    }
    try {
      imuPipeline_->stop();
    } catch (const ob::Error &e) {
      RCLCPP_ERROR_STREAM(
          logger_, "Failed to stop imu pipeline: " << orbbec_camera::formatObErrorWithStatus(e));
    } catch (...) {
      RCLCPP_ERROR_STREAM(logger_, "Failed to stop imu pipeline");
    }
    imu_sync_output_start_.store(false);
  } else {
    for (const auto &stream_index : HID_STREAMS) {
      if (imu_started_[stream_index]) {
        CHECK(sensors_.count(stream_index));
        RCLCPP_DEBUG_STREAM(logger_, "Stop " << stream_name_[stream_index] << " stream");
        try {
          sensors_[stream_index]->stop();
        } catch (const ob::Error &e) {
          RCLCPP_ERROR_STREAM(logger_, "Failed to stop "
                                           << stream_name_[stream_index] << " stream: "
                                           << orbbec_camera::formatObErrorWithStatus(e));
        }
        imu_started_[stream_index] = false;
      }
    }
  }
}

// cs_param_t rd_par = {0, 0}, param = {1, 3000}; //30
int OBCameraNode::openSocSyncPwmTrigger(uint16_t fps) {
  const char *devicePath = DEVICE_PATH;
  const int TRIGGER_MODE_ENABLE = 1;
  const int TRIGGER_MODE_DISABLE = 0;

  int ret = -1;
  cs_param_t param = {TRIGGER_MODE_ENABLE, fps};
  cs_param_t rd_par = {TRIGGER_MODE_DISABLE, 0};

  if (access(devicePath, F_OK) != 0) {
    std::cerr << "Device node " << devicePath << " does not exist." << std::endl;
    return ret;
  }
  gmsl_trigger_fd_ = open(DEVICE_PATH, O_RDWR);
  if (gmsl_trigger_fd_ < 0) {
    perror("open device failed\n");
    return gmsl_trigger_fd_;
  }

  std::cout << "Written param mode=" << param.mode << ", fps=" << param.fps << std::endl;
  ret = write(gmsl_trigger_fd_, &param, sizeof(param));
  if (ret < 0) {
    perror("write device failed\n");
    close(gmsl_trigger_fd_);
    return ret;
  }

  ret = read(gmsl_trigger_fd_, &rd_par, sizeof(rd_par));
  if (ret < 0) {
    perror("read device failed\n");
    close(gmsl_trigger_fd_);
    return ret;
  }
  std::cout << "Read param mode=" << rd_par.mode << ", fps=" << rd_par.fps << std::endl;

  std::cout << "Start hardware triggering..." << std::endl;

  return 0;
}
int OBCameraNode::closeSocSyncPwmTrigger() {
  if (gmsl_trigger_fd_ >= 0) {
    close(gmsl_trigger_fd_);
    gmsl_trigger_fd_ = -1;  // Reset file descriptors
    std::cout << "close camSync success" << std::endl;
    return 0;
  }
  return -1;
}

void OBCameraNode::startGmslTrigger() {
  if (gmsl_trigger_fps_ > 0 && enable_gmsl_trigger_) {
    RCLCPP_WARN_STREAM(logger_, "Start HardwareTrigger by soc-trigger-source. gmsl_trigger_fps_: "
                                    << gmsl_trigger_fps_);
    openSocSyncPwmTrigger(gmsl_trigger_fps_);
  } else {
    RCLCPP_WARN_STREAM(logger_,
                       "Start HardwareTrigger by soc-trigger-source. gmsl_trigger_fps_ illegal: "
                           << gmsl_trigger_fps_);
  }
}
void OBCameraNode::stopGmslTrigger() { closeSocSyncPwmTrigger(); }

void OBCameraNode::setupDefaultImageFormat() {
  format_[DEPTH] = OB_FORMAT_Y16;
  format_str_[DEPTH] = "Y16";
  image_format_[DEPTH] = CV_16UC1;
  encoding_[DEPTH] = sensor_msgs::image_encodings::TYPE_16UC1;
  unit_step_size_[DEPTH] = sizeof(uint16_t);
  format_[INFRA0] = OB_FORMAT_Y16;
  format_str_[INFRA0] = "Y16";
  image_format_[INFRA0] = CV_16UC1;
  encoding_[INFRA0] = sensor_msgs::image_encodings::MONO16;
  unit_step_size_[INFRA0] = sizeof(uint16_t);

  format_[INFRA1] = OB_FORMAT_Y16;
  format_str_[INFRA1] = "Y16";
  image_format_[INFRA1] = CV_16UC1;
  encoding_[INFRA1] = sensor_msgs::image_encodings::MONO16;
  unit_step_size_[INFRA1] = sizeof(uint16_t);

  format_[INFRA2] = OB_FORMAT_Y16;
  format_str_[INFRA2] = "Y16";
  image_format_[INFRA2] = CV_16UC1;
  encoding_[INFRA2] = sensor_msgs::image_encodings::MONO16;
  unit_step_size_[INFRA2] = sizeof(uint16_t);

  image_format_[COLOR] = CV_8UC3;
  encoding_[COLOR] = sensor_msgs::image_encodings::RGB8;
  unit_step_size_[COLOR] = 3 * sizeof(uint8_t);

  image_format_[COLOR_LEFT] = CV_8UC3;
  encoding_[COLOR_LEFT] = sensor_msgs::image_encodings::RGB8;
  unit_step_size_[COLOR_LEFT] = 3 * sizeof(uint8_t);

  image_format_[COLOR_RIGHT] = CV_8UC3;
  encoding_[COLOR_RIGHT] = sensor_msgs::image_encodings::RGB8;
  unit_step_size_[COLOR_RIGHT] = 3 * sizeof(uint8_t);
}

void OBCameraNode::getParameters() {
  setAndGetNodeParameter<std::string>(camera_name_, "camera_name", "camera");
  camera_link_frame_id_ = camera_name_ + "_link";
  for (auto stream_index : IMAGE_STREAMS) {
    std::string param_name = stream_name_[stream_index] + "_width";
    setAndGetNodeParameter(width_[stream_index], param_name, 0);
    param_name = stream_name_[stream_index] + "_height";
    setAndGetNodeParameter(height_[stream_index], param_name, 0);
    param_name = stream_name_[stream_index] + "_fps";
    setAndGetNodeParameter(fps_[stream_index], param_name, 0);
    param_name = "enable_" + stream_name_[stream_index];
    if (stream_index == DEPTH) {
      setAndGetNodeParameter(enable_stream_[stream_index], param_name, true);
    } else {
      setAndGetNodeParameter(enable_stream_[stream_index], param_name, false);
    }
    param_name = stream_name_[stream_index] + "_flip";
    setAndGetNodeParameter<bool>(flip_stream_[stream_index], param_name, false);
    param_name = stream_name_[stream_index] + "_mirror";
    setAndGetNodeParameter<bool>(mirror_stream_[stream_index], param_name, false);
    param_name = stream_name_[stream_index] + "_rotation";
    setAndGetNodeParameter<int>(rotation_stream_[stream_index], param_name, -1);
    param_name = camera_name_ + "_" + stream_name_[stream_index] + "_frame_id";
    std::string default_frame_id = camera_name_ + "_" + stream_name_[stream_index] + "_frame";
    setAndGetNodeParameter(frame_id_[stream_index], param_name, default_frame_id);
    std::string default_optical_frame_id =
        camera_name_ + "_" + stream_name_[stream_index] + "_optical_frame";
    param_name = stream_name_[stream_index] + "_optical_frame_id";
    setAndGetNodeParameter(optical_frame_id_[stream_index], param_name, default_optical_frame_id);
    param_name = stream_name_[stream_index] + "_format";
    setAndGetNodeParameter(format_str_[stream_index], param_name, format_str_[stream_index]);
    format_[stream_index] = OBFormatFromString(format_str_[stream_index]);
    updateImageConfig(stream_index);
    param_name = stream_name_[stream_index] + "_qos";
    setAndGetNodeParameter<std::string>(image_qos_[stream_index], param_name, "default");
    param_name = stream_name_[stream_index] + "_camera_info_qos";
    setAndGetNodeParameter<std::string>(camera_info_qos_[stream_index], param_name, "default");
    param_name = "enable_" + stream_name_[stream_index] + "_undistortion";
    setAndGetNodeParameter<bool>(enable_undistortion_[stream_index], param_name, false);
  }

  for (auto stream_index : IMAGE_STREAMS) {
    depth_aligned_frame_id_[stream_index] = optical_frame_id_[COLOR];
  }

  accel_gyro_frame_id_ = camera_name_ + "_accel_gyro_optical_frame";

  setAndGetNodeParameter<bool>(enable_sync_output_accel_gyro_, "enable_sync_output_accel_gyro",
                               false);
  for (const auto &stream_index : HID_STREAMS) {
    std::string param_name = stream_name_[stream_index] + "_qos";
    setAndGetNodeParameter<std::string>(imu_qos_[stream_index], param_name, "default");
    param_name = "enable_" + stream_name_[stream_index];
    setAndGetNodeParameter<bool>(enable_stream_[stream_index], param_name, false);
    if (enable_sync_output_accel_gyro_) {
      enable_stream_[stream_index] = true;
    }
    param_name = stream_name_[stream_index] + "_rate";
    setAndGetNodeParameter<std::string>(imu_rate_[stream_index], param_name, "");
    param_name = stream_name_[stream_index] + "_range";
    setAndGetNodeParameter<std::string>(imu_range_[stream_index], param_name, "");
    param_name = camera_name_ + "_" + stream_name_[stream_index] + "_frame_id";
    std::string default_frame_id = camera_name_ + "_" + stream_name_[stream_index] + "_frame";
    setAndGetNodeParameter(frame_id_[stream_index], param_name, default_frame_id);
    std::string default_optical_frame_id =
        camera_name_ + "_" + stream_name_[stream_index] + "_optical_frame";
    param_name = stream_name_[stream_index] + "_optical_frame_id";
    setAndGetNodeParameter(optical_frame_id_[stream_index], param_name, default_optical_frame_id);
    depth_aligned_frame_id_[stream_index] =
        camera_name_ + "_" + stream_name_[COLOR] + "_optical_frame";
  }
  setAndGetNodeParameter<bool>(publish_tf_, "publish_tf", true);
  setAndGetNodeParameter<double>(tf_publish_rate_, "tf_publish_rate", 0.0);
  setAndGetNodeParameter<bool>(depth_registration_, "depth_registration", false);
  bool enable_enhanced_depth = false;
  setAndGetNodeParameter<bool>(enable_enhanced_depth, "enable_enhanced_depth", false);
  enable_enhanced_depth_.store(enable_enhanced_depth);
  setAndGetNodeParameter<std::string>(enhanced_depth_model_path_, "enhanced_depth_model_path", "");
  setAndGetNodeParameter<int>(enhanced_depth_confidence_threshold_,
                              "enhanced_depth_confidence_threshold", 51);
  setAndGetNodeParameter<bool>(enable_point_cloud_, "enable_point_cloud", false);
  setAndGetNodeParameter<std::string>(ir_info_url_, "ir_info_url", "");
  setAndGetNodeParameter<std::string>(color_info_url_, "color_info_url", "");
  setAndGetNodeParameter<bool>(enable_colored_point_cloud_, "enable_colored_point_cloud", false);
  setAndGetNodeParameter<bool>(enable_point_cloud_, "enable_point_cloud", false);
  setAndGetNodeParameter<int>(point_cloud_decimation_filter_factor_,
                              "point_cloud_decimation_filter_factor", 1);
  setAndGetNodeParameter<std::string>(point_cloud_qos_, "point_cloud_qos", "default");
  setAndGetNodeParameter<bool>(enable_d2c_viewer_, "enable_d2c_viewer", false);
  setAndGetNodeParameter<std::string>(disparity_to_depth_mode_, "disparity_to_depth_mode", "");
  disparity_to_depth_mode_ =
      normalizeClosedSetParameterValue(logger_, "disparity_to_depth_mode", disparity_to_depth_mode_,
                                       {"", "HW", "SW", "disable"}, "");
  setAndGetNodeParameter<std::string>(depth_filter_config_, "depth_filter_config", "");
  if (!depth_filter_config_.empty()) {
    enable_depth_filter_ = true;
  }
  setAndGetNodeParameter<bool>(enable_frame_sync_, "enable_frame_sync", false);
  setAndGetNodeParameter<bool>(enable_color_auto_exposure_priority_,
                               "enable_color_auto_exposure_priority", false);
  setAndGetNodeParameter<bool>(enable_color_auto_exposure_, "enable_color_auto_exposure", true);
  setAndGetNodeParameter<bool>(enable_color_auto_white_balance_, "enable_color_auto_white_balance",
                               true);
  setAndGetNodeParameter<int>(color_ae_roi_left_, "color_ae_roi_left", -1);
  setAndGetNodeParameter<int>(color_ae_roi_top_, "color_ae_roi_top", -1);
  setAndGetNodeParameter<int>(color_ae_roi_right_, "color_ae_roi_right", -1);
  setAndGetNodeParameter<int>(color_ae_roi_bottom_, "color_ae_roi_bottom", -1);
  setAndGetNodeParameter<int>(color_exposure_, "color_exposure", -1);
  setAndGetNodeParameter<int>(color_gain_, "color_gain", -1);
  setAndGetNodeParameter<int>(color_mjpeg_quality_, "color_mjpeg_quality", -1);
  setAndGetNodeParameter<int>(color_white_balance_, "color_white_balance", -1);
  setAndGetNodeParameter<int>(color_ae_max_exposure_, "color_ae_max_exposure", -1);
  setAndGetNodeParameter<int>(color_ae_max_gain_, "color_ae_max_gain", -1);
  setAndGetNodeParameter<int>(color_brightness_, "color_brightness", -1);
  setAndGetNodeParameter<int>(color_roi_brightness_, "color_roi_brightness", -1);
  setAndGetNodeParameter<int>(color_sharpness_, "color_sharpness", -1);
  setAndGetNodeParameter<int>(color_gamma_, "color_gamma", -1);
  setAndGetNodeParameter<int>(color_saturation_, "color_saturation", -1);
  setAndGetNodeParameter<int>(color_contrast_, "color_contrast", -1);
  setAndGetNodeParameter<int>(color_hue_, "color_hue", -1);
  setAndGetNodeParameter<int>(color_backlight_compensation_, "color_backlight_compensation", -1);
  setAndGetNodeParameter<bool>(color_anti_flicker_, "color_anti_flicker", false);
  setAndGetNodeParameter<int>(color_denoising_level_, "color_denoising_level", -1);
  setAndGetNodeParameter<std::string>(color_powerline_freq_, "color_powerline_freq", "");
  setAndGetNodeParameter<std::string>(color_preset_, "color_preset", "");
  setAndGetNodeParameter<bool>(enable_color_decimation_filter_, "enable_color_decimation_filter",
                               false);
  setAndGetNodeParameter<int>(color_decimation_filter_scale_, "color_decimation_filter_scale", -1);
  setAndGetNodeParameter<bool>(enable_left_color_decimation_filter_,
                               "enable_left_color_decimation_filter", false);
  setAndGetNodeParameter<int>(left_color_decimation_filter_scale_,
                              "left_color_decimation_filter_scale", -1);
  setAndGetNodeParameter<bool>(enable_right_color_decimation_filter_,
                               "enable_right_color_decimation_filter", false);
  setAndGetNodeParameter<int>(right_color_decimation_filter_scale_,
                              "right_color_decimation_filter_scale", -1);
  setAndGetNodeParameter<bool>(enable_depth_auto_exposure_priority_,
                               "enable_depth_auto_exposure_priority", false);
  setAndGetNodeParameter<int>(depth_ae_roi_left_, "depth_ae_roi_left", -1);
  setAndGetNodeParameter<int>(depth_ae_roi_top_, "depth_ae_roi_top", -1);
  setAndGetNodeParameter<int>(depth_ae_roi_right_, "depth_ae_roi_right", -1);
  setAndGetNodeParameter<int>(depth_ae_roi_bottom_, "depth_ae_roi_bottom", -1);
  setAndGetNodeParameter<int>(depth_exposure_, "depth_exposure", -1);
  setAndGetNodeParameter<int>(depth_gain_, "depth_gain", -1);
  setAndGetNodeParameter<int>(depth_brightness_, "depth_brightness", -1);
  setAndGetNodeParameter<int>(mean_intensity_set_point_, "mean_intensity_set_point",
                              depth_brightness_);
  setAndGetNodeParameter<std::string>(depth_precision_str_, "depth_precision", "");
  setAndGetNodeParameter<bool>(enable_ir_auto_exposure_, "enable_ir_auto_exposure", true);
  setAndGetNodeParameter<int>(ir_exposure_, "ir_exposure", -1);
  setAndGetNodeParameter<int>(ir_gain_, "ir_gain", -1);
  setAndGetNodeParameter<int>(ir_ae_max_exposure_, "ir_ae_max_exposure", -1);
  setAndGetNodeParameter<int>(ir_brightness_, "ir_brightness", -1);
  setAndGetNodeParameter<bool>(enable_ir_long_exposure_, "enable_ir_long_exposure", true);
  setAndGetNodeParameter<bool>(enable_right_ir_sequence_id_filter_,
                               "enable_right_ir_sequence_id_filter", false);
  setAndGetNodeParameter<int>(right_ir_sequence_id_filter_id_, "right_ir_sequence_id_filter_id",
                              -1);
  setAndGetNodeParameter<bool>(enable_left_ir_sequence_id_filter_,
                               "enable_left_ir_sequence_id_filter", false);
  setAndGetNodeParameter<int>(left_ir_sequence_id_filter_id_, "left_ir_sequence_id_filter_id", -1);
  setAndGetNodeParameter<std::string>(preset_resolution_config_, "preset_resolution_config", "");
  setAndGetNodeParameter<std::string>(sync_mode_str_, "sync_mode", "");
  setAndGetNodeParameter<int>(depth_delay_us_, "depth_delay_us", 0);
  setAndGetNodeParameter<int>(color_delay_us_, "color_delay_us", 0);
  setAndGetNodeParameter<int>(trigger2image_delay_us_, "trigger2image_delay_us", 0);
  setAndGetNodeParameter<int>(trigger_out_delay_us_, "trigger_out_delay_us", 0);
  setAndGetNodeParameter<bool>(trigger_out_enabled_, "trigger_out_enabled", true);
  setAndGetNodeParameter<bool>(software_trigger_enabled_, "software_trigger_enabled", true);
  setAndGetNodeParameter<bool>(enable_ptp_config_, "enable_ptp_config", false);
  setAndGetNodeParameter<std::string>(cloud_frame_id_, "cloud_frame_id", "");
  if (enable_colored_point_cloud_ || enable_d2c_viewer_) {
    depth_registration_ = true;
  }
  if (!enable_stream_[COLOR]) {
    enable_colored_point_cloud_ = false;
    depth_registration_ = false;
  }
  setAndGetNodeParameter<bool>(enable_ldp_, "enable_ldp", true);
  setAndGetNodeParameter<int>(ldp_power_level_, "ldp_power_level", -1);
  setAndGetNodeParameter<double>(linear_accel_cov_, "linear_accel_cov", 0.0003);
  setAndGetNodeParameter<double>(angular_vel_cov_, "angular_vel_cov", 0.02);
  setAndGetNodeParameter<bool>(ordered_pc_, "ordered_pc", false);
  setAndGetNodeParameter<int>(max_save_images_count_, "max_save_images_count", 10);
  setAndGetNodeParameter<bool>(enable_depth_scale_, "enable_depth_scale", true);
  setAndGetNodeParameter<int>(depth_decimation_factor_, "depth_decimation_factor", 1);
  setAndGetNodeParameter<int>(left_ir_decimation_factor_, "left_ir_decimation_factor", 1);
  setAndGetNodeParameter<int>(right_ir_decimation_factor_, "right_ir_decimation_factor", 1);
  setAndGetNodeParameter<std::string>(depth_work_mode_, "depth_work_mode", "");
  if (isDepthWorkModeDevices(device_->getDeviceInfo()->getPid())) {
    setAndGetNodeParameter<std::string>(depth_work_mode_, "device_preset", "");
  } else {
    setAndGetNodeParameter<std::string>(device_preset_, "device_preset", "");
  }
  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);
  setAndGetNodeParameter<bool>(enable_disparity_to_depth_, "enable_disparity_to_depth", true);
  setAndGetNodeParameter<bool>(enable_threshold_filter_, "enable_threshold_filter", false);
  setAndGetNodeParameter<bool>(enable_hardware_noise_removal_filter_,
                               "enable_hardware_noise_removal_filter", true);
  setAndGetNodeParameter<bool>(enable_noise_removal_filter_, "enable_noise_removal_filter", true);
  setAndGetNodeParameter<bool>(enable_spatial_filter_, "enable_spatial_filter", false);
  setAndGetNodeParameter<bool>(enable_temporal_filter_, "enable_temporal_filter", false);
  setAndGetNodeParameter<bool>(enable_hole_filling_filter_, "enable_hole_filling_filter", false);
  setAndGetNodeParameter<bool>(enable_edge_noise_removal_filter_,
                               "enable_edge_noise_removal_filter", false);
  setAndGetNodeParameter<bool>(enable_spatial_fast_filter_, "enable_spatial_fast_filter", false);
  setAndGetNodeParameter<bool>(enable_spatial_moderate_filter_, "enable_spatial_moderate_filter",
                               false);
  setAndGetNodeParameter<bool>(enable_mgc_noise_removal_filter_, "enable_mgc_noise_removal_filter",
                               false);
  setAndGetNodeParameter<bool>(enable_lut_noise_removal_filter_, "enable_lut_noise_removal_filter",
                               false);
  setAndGetNodeParameter<bool>(enable_disp_outliers_filter_, "enable_disp_outliers_filter", false);
  std::string disp_outliers_filter_search_mode;
  setAndGetNodeParameter<std::string>(disp_outliers_filter_search_mode,
                                      "disp_outliers_filter_search_mode", "");
  std::string disp_outliers_filter_search_mode_message;
  if (!parseDispOutliersSearchMode(disp_outliers_filter_search_mode,
                                   disp_outliers_filter_search_mode_,
                                   disp_outliers_filter_search_mode_message, true)) {
    RCLCPP_WARN_STREAM(logger_, "Invalid disp_outliers_filter_search_mode value "
                                    << disp_outliers_filter_search_mode << ". "
                                    << disp_outliers_filter_search_mode_message << "; using empty");
    disp_outliers_filter_search_mode_ = -1;
  }
  setAndGetNodeParameter<int>(decimation_filter_scale_, "decimation_filter_scale", -1);
  setAndGetNodeParameter<int>(sequence_id_filter_id_, "sequence_id_filter_id", -1);
  setAndGetNodeParameter<int>(threshold_filter_max_, "threshold_filter_max", -1);
  setAndGetNodeParameter<int>(threshold_filter_min_, "threshold_filter_min", -1);
  setAndGetNodeParameter<float>(hardware_noise_removal_filter_threshold_,
                                "hardware_noise_removal_filter_threshold", -1.0);
  setAndGetNodeParameter<int>(noise_removal_filter_min_diff_, "noise_removal_filter_min_diff", -1);
  setAndGetNodeParameter<int>(noise_removal_filter_max_size_, "noise_removal_filter_max_size", -1);
  setAndGetNodeParameter<float>(spatial_filter_alpha_, "spatial_filter_alpha", -1.0);
  setAndGetNodeParameter<int>(spatial_filter_diff_threshold_, "spatial_filter_diff_threshold", -1);
  setAndGetNodeParameter<int>(spatial_filter_magnitude_, "spatial_filter_magnitude", -1);
  setAndGetNodeParameter<int>(spatial_filter_radius_, "spatial_filter_radius", -1);
  setAndGetNodeParameter<int>(spatial_fast_filter_radius_, "spatial_fast_filter_radius", -1);
  setAndGetNodeParameter<int>(spatial_moderate_filter_diff_threshold_,
                              "spatial_moderate_filter_diff_threshold", -1);
  setAndGetNodeParameter<int>(spatial_moderate_filter_magnitude_,
                              "spatial_moderate_filter_magnitude", -1);
  setAndGetNodeParameter<int>(spatial_moderate_filter_radius_, "spatial_moderate_filter_radius",
                              -1);
  setAndGetNodeParameter<float>(temporal_filter_diff_threshold_, "temporal_filter_diff_threshold",
                                -1.0);
  setAndGetNodeParameter<float>(temporal_filter_weight_, "temporal_filter_weight", -1.0);
  setAndGetNodeParameter<std::string>(hole_filling_filter_mode_, "hole_filling_filter_mode", "");
  setAndGetNodeParameter<bool>(enable_false_positive_filter_, "enable_false_positive_filter",
                               false);
  setAndGetNodeParameter<int>(hdr_merge_exposure_1_, "hdr_merge_exposure_1", -1);
  setAndGetNodeParameter<int>(hdr_merge_gain_1_, "hdr_merge_gain_1", -1);
  setAndGetNodeParameter<int>(hdr_merge_exposure_2_, "hdr_merge_exposure_2", -1);
  setAndGetNodeParameter<int>(hdr_merge_gain_2_, "hdr_merge_gain_2", -1);
  setAndGetNodeParameter<std::string>(align_mode_, "align_mode", "HW");
  align_mode_ =
      normalizeClosedSetParameterValue(logger_, "align_mode", align_mode_, {"HW", "SW"}, "HW");
  setAndGetNodeParameter<double>(diagnostic_period_, "diagnostic_period", 0.0);
  setAndGetNodeParameter<bool>(enable_laser_, "enable_laser", true);
  std::string align_target_stream_str_;
  setAndGetNodeParameter<std::string>(align_target_stream_str_, "align_target_stream", "COLOR");
  align_target_stream_str_ = normalizeClosedSetParameterValue(
      logger_, "align_target_stream", align_target_stream_str_, {"COLOR", "DEPTH"}, "COLOR");
  if (depth_registration_ && align_mode_ == "HW" && align_target_stream_str_ != "COLOR") {
    RCLCPP_ERROR_STREAM(logger_,
                        "HW D2C only supports COLOR as align_target_stream; got "
                            << align_target_stream_str_
                            << ". Skip setting and use 'COLOR'. Use align_mode:=SW to align to "
                               "DEPTH.");
    align_target_stream_str_ = "COLOR";
  }
  align_target_stream_ = obStreamTypeFromString(align_target_stream_str_);
  setAndGetNodeParameter<bool>(retry_on_usb3_detection_failure_, "retry_on_usb3_detection_failure",
                               false);
  setAndGetNodeParameter<int>(laser_energy_level_, "laser_energy_level", -1);
  setAndGetNodeParameter<int>(min_depth_limit_, "min_depth_limit", 0);
  setAndGetNodeParameter<int>(max_depth_limit_, "max_depth_limit", 0);
  setAndGetNodeParameter<bool>(enable_heartbeat_, "enable_heartbeat", false);
  setAndGetNodeParameter<bool>(enable_firmware_log_, "enable_firmware_log", false);
  setAndGetNodeParameter<bool>(enable_fps_boost_, "enable_fps_boost", false);
  setAndGetNodeParameter<std::string>(time_domain_, "time_domain", "global");
  time_domain_ = normalizeClosedSetParameterValue(logger_, "time_domain", time_domain_,
                                                  {"global", "device", "system"}, "global");
  setAndGetNodeParameter<bool>(enable_frame_drop_log_, "enable_frame_drop_log", false);
  setAndGetNodeParameter<std::string>(frame_timestamp_csv_file_, "frame_timestamp_csv_file", "");
  setAndGetNodeParameter<std::string>(exposure_range_mode_, "exposure_range_mode", "");
  setAndGetNodeParameter<std::string>(load_config_json_file_path_, "load_config_json_file_path",
                                      "");
  setAndGetNodeParameter<std::string>(export_config_json_file_path_, "export_config_json_file_path",
                                      "");
  setAndGetNodeParameter<bool>(enable_accel_data_correction_, "enable_accel_data_correction", true);
  setAndGetNodeParameter<bool>(enable_gyro_data_correction_, "enable_gyro_data_correction", true);
  auto device_info = device_->getDeviceInfo();
  CHECK_NOTNULL(device_info.get());

  if (device_preset_ == "Dual Color Streams") {
    RCLCPP_INFO_STREAM(logger_,
                       "Using Double Color preset, only left and right color streams are enabled.");
    enable_stream_[COLOR] = false;
    enable_stream_[DEPTH] = false;
    enable_stream_[INFRA0] = false;
    enable_stream_[INFRA1] = false;
    enable_stream_[INFRA2] = false;
    enable_stream_[LIDAR] = false;
    enable_stream_[COLOR_LEFT] = true;
    enable_stream_[COLOR_RIGHT] = true;

    enable_point_cloud_ = false;
    enable_colored_point_cloud_ = false;
    depth_registration_ = false;
    enable_d2c_viewer_ = false;
    enable_depth_filter_ = false;
    enable_undistortion_[COLOR] = false;
  }

  if (isOpenNIDevice(pid_)) {
    time_domain_ = "system";
  }
  if (time_domain_ == "global" && !is_playback_device_) {
    device_->enableGlobalTimestamp(true);
  }
  setAndGetNodeParameter<int>(frames_per_trigger_, "frames_per_trigger", 1);
  int software_trigger_period = 33;
  setAndGetNodeParameter<int>(software_trigger_period, "software_trigger_period", 33);
  software_trigger_period_ = std::chrono::milliseconds(software_trigger_period);
  setAndGetNodeParameter<int>(gmsl_trigger_fps_, "gmsl_trigger_fps", 3000);
  setAndGetNodeParameter<bool>(enable_gmsl_trigger_, "enable_gmsl_trigger", false);
  setAndGetNodeParameter<std::string>(interleave_ae_mode_, "interleave_ae_mode", "");
  setAndGetNodeParameter<bool>(interleave_frame_enable_, "interleave_frame_enable", false);
  setAndGetNodeParameter<bool>(interleave_skip_enable_, "interleave_skip_enable", false);
  setAndGetNodeParameter<int>(interleave_skip_index_, "interleave_skip_index", -1);

  // hdr and laser interleave params
  setAndGetNodeParameter<int>(hdr_index1_laser_control_, "hdr_index1_laser_control", -1);
  setAndGetNodeParameter<int>(hdr_index1_depth_exposure_, "hdr_index1_depth_exposure", -1);
  setAndGetNodeParameter<int>(hdr_index1_depth_gain_, "hdr_index1_depth_gain", -1);
  setAndGetNodeParameter<int>(hdr_index1_ir_brightness_, "hdr_index1_ir_brightness", -1);
  setAndGetNodeParameter<int>(hdr_index1_ir_ae_max_exposure_, "hdr_index1_ir_ae_max_exposure", -1);
  setAndGetNodeParameter<int>(hdr_index0_laser_control_, "hdr_index0_laser_control", -1);
  setAndGetNodeParameter<int>(hdr_index0_depth_exposure_, "hdr_index0_depth_exposure", -1);
  setAndGetNodeParameter<int>(hdr_index0_depth_gain_, "hdr_index0_depth_gain", -1);
  setAndGetNodeParameter<int>(hdr_index0_ir_brightness_, "hdr_index0_ir_brightness", -1);
  setAndGetNodeParameter<int>(hdr_index0_ir_ae_max_exposure_, "hdr_index0_ir_ae_max_exposure", -1);

  setAndGetNodeParameter<int>(laser_index1_laser_control_, "laser_index1_laser_control", -1);
  setAndGetNodeParameter<int>(laser_index1_depth_exposure_, "laser_index1_depth_exposure", -1);
  setAndGetNodeParameter<int>(laser_index1_depth_gain_, "laser_index1_depth_gain", -1);
  setAndGetNodeParameter<int>(laser_index1_ir_brightness_, "laser_index1_ir_brightness", -1);
  setAndGetNodeParameter<int>(laser_index1_ir_ae_max_exposure_, "laser_index1_ir_ae_max_exposure",
                              -1);
  setAndGetNodeParameter<int>(laser_index0_laser_control_, "laser_index0_laser_control", -1);
  setAndGetNodeParameter<int>(laser_index0_depth_exposure_, "laser_index0_depth_exposure", -1);
  setAndGetNodeParameter<int>(laser_index0_depth_gain_, "laser_index0_depth_gain", -1);
  setAndGetNodeParameter<int>(laser_index0_ir_brightness_, "laser_index0_ir_brightness", -1);
  setAndGetNodeParameter<int>(laser_index0_ir_ae_max_exposure_, "laser_index0_ir_ae_max_exposure",
                              -1);
  setAndGetNodeParameter<int>(disparity_range_mode_, "disparity_range_mode", -1);
  setAndGetNodeParameter<int>(disparity_search_offset_, "disparity_search_offset", -1);
  setAndGetNodeParameter<bool>(disparity_offset_config_, "disparity_offset_config", false);
  setAndGetNodeParameter<int>(offset_index0_, "offset_index0", -1);
  setAndGetNodeParameter<int>(offset_index1_, "offset_index1", -1);
  setAndGetNodeParameter<int>(sync_io_voltage_level_, "sync_io_voltage_level", -1);

  setAndGetNodeParameter<std::string>(frame_aggregate_mode_, "frame_aggregate_mode", "ANY");
  frame_aggregate_mode_ =
      normalizeClosedSetParameterValue(logger_, "frame_aggregate_mode", frame_aggregate_mode_,
                                       {"full_frame", "color_frame", "ANY", "disable"}, "ANY");

  setAndGetNodeParameter<bool>(show_fps_enable_, "show_fps_enable", false);
  setAndGetNodeParameter<bool>(enable_publish_extrinsic_, "enable_publish_extrinsic", false);
  setAndGetNodeParameter<bool>(enable_lrm_obstacle_distance_publish_,
                               "enable_lrm_obstacle_distance_publish", false);
  setAndGetNodeParameter<double>(lrm_obstacle_distance_publish_rate_,
                                 "lrm_obstacle_distance_publish_rate", 10.0);
  if (enable_lrm_obstacle_distance_publish_ && !enable_ldp_) {
    RCLCPP_INFO_STREAM(logger_, "enable_lrm_obstacle_distance_publish is true, enabling LDP");
    enable_ldp_ = true;
  }
  if (lrm_obstacle_distance_publish_rate_ <= 0.0) {
    RCLCPP_WARN_STREAM(logger_, "Invalid lrm_obstacle_distance_publish_rate "
                                    << lrm_obstacle_distance_publish_rate_
                                    << ", using 10.0 Hz instead");
    lrm_obstacle_distance_publish_rate_ = 10.0;
  }
  setAndGetNodeParameter<std::string>(intra_camera_sync_reference_, "intra_camera_sync_reference",
                                      "Middle");
  setAndGetNodeParameter<std::string>(ae_reference_stream_, "ae_reference_stream", "");
  setAndGetNodeParameter<std::string>(ae_strategy_, "ae_strategy", "");

  RCLCPP_INFO_STREAM(logger_, "Current time domain: " << time_domain_);
  RCLCPP_DEBUG_STREAM(logger_, "hdr_index1_laser_control_ "
                                   << hdr_index1_laser_control_ << " hdr_index1_depth_exposure_ "
                                   << hdr_index1_depth_exposure_ << " hdr_index1_depth_gain_ "
                                   << hdr_index1_depth_gain_ << " hdr_index1_ir_brightness_ "
                                   << hdr_index1_ir_brightness_
                                   << " hdr_index1_ir_ae_max_exposure_ "
                                   << hdr_index1_ir_ae_max_exposure_ << "\n");
  RCLCPP_DEBUG_STREAM(logger_, "hdr_index0_laser_control_ "
                                   << hdr_index0_laser_control_ << " hdr_index0_depth_exposure_ "
                                   << hdr_index0_depth_exposure_ << " hdr_index0_depth_gain_ "
                                   << hdr_index0_depth_gain_ << " hdr_index0_ir_brightness_ "
                                   << hdr_index0_ir_brightness_
                                   << " hdr_index0_ir_ae_max_exposure_ "
                                   << hdr_index0_ir_ae_max_exposure_ << "\n");
  RCLCPP_DEBUG_STREAM(logger_,
                      "laser_index1_laser_control_ "
                          << laser_index1_laser_control_ << " laser_index1_depth_exposure_ "
                          << laser_index1_depth_exposure_ << " laser_index1_depth_gain_ "
                          << laser_index1_depth_gain_ << " laser_index1_ir_brightness_ "
                          << laser_index1_ir_brightness_ << " laser_index1_ir_ae_max_exposure_ "
                          << laser_index1_ir_ae_max_exposure_ << "\n");
  RCLCPP_DEBUG_STREAM(logger_,
                      "laser_index0_laser_control_ "
                          << laser_index0_laser_control_ << " laser_index0_depth_exposure_ "
                          << laser_index0_depth_exposure_ << " laser_index0_depth_gain_ "
                          << laser_index0_depth_gain_ << " laser_index0_ir_brightness_ "
                          << laser_index0_ir_brightness_ << " laser_index0_ir_ae_max_exposure_ "
                          << laser_index0_ir_ae_max_exposure_ << "\n");
}

void OBCameraNode::setupTopics() {
  try {
    captureInitialRosParameters();
    getParameters();
    loadConfigJson();
    syncConfigJsonApplicationConfig();
    setupDevices();
    setupDepthPostProcessFilter();
    setupColorPostProcessFilter();
    setupIrPostProcessFilter();
    setupRightIrPostProcessFilter();
    setupLeftIrPostProcessFilter();
    setupUndistortionFilters();
    syncConfigJsonDeviceSettings();
    syncConfigJsonFilterSettings(depth_filter_list_, "depth");
    syncConfigJsonFilterSettings(color_filter_list_, "color");
    syncConfigJsonFilterSettings(left_color_filter_list_, "left_color");
    syncConfigJsonFilterSettings(right_color_filter_list_, "right_color");
    syncConfigJsonFilterSettings(left_ir_filter_list_, "left_ir");
    syncConfigJsonFilterSettings(right_ir_filter_list_, "right_ir");
    setupProfiles();
    if (enable_enhanced_depth_.load()) {
      std::string message;
      if (!ensureEnhancedDepthFilter(message)) {
        throw std::runtime_error(message);
      }
    }
    setupCameraInfo();
    selectBaseStream();
    setupCameraCtrlServices();
    setupPublishers();
    setupDiagnosticUpdater();
    exportConfigJsonIfRequested();
  } catch (const ob::Error &e) {
    RCLCPP_ERROR_STREAM(logger_,
                        "Failed to setup topics: " << orbbec_camera::formatObErrorWithStatus(e));
    throw std::runtime_error(orbbec_camera::formatObErrorWithStatus(e));
  } catch (const std::exception &e) {
    RCLCPP_ERROR_STREAM(logger_, "Failed to setup topics: " << e.what());
    throw std::runtime_error(e.what());
  } catch (...) {
    RCLCPP_ERROR(logger_, "Failed to setup topics");
    throw std::runtime_error("Failed to setup topics");
  }
}

void OBCameraNode::onTemperatureUpdate(diagnostic_updater::DiagnosticStatusWrapper &status) {
  try {
    // Check to ensure we're not shutting down and device is valid
    if (!is_running_.load() || !is_camera_node_initialized_.load()) {
      status.summary(diagnostic_msgs::msg::DiagnosticStatus::STALE,
                     "Device disconnected or shutting down");
      return;
    }

    // Try to acquire device lock with timeout to avoid blocking during shutdown
    std::unique_lock<decltype(device_lock_)> lock(device_lock_, std::try_to_lock);
    if (!lock.owns_lock()) {
      status.summary(diagnostic_msgs::msg::DiagnosticStatus::STALE, "Device busy or shutting down");
      return;
    }

    if (!device_) {
      status.summary(diagnostic_msgs::msg::DiagnosticStatus::STALE, "Device not available");
      return;
    }

    // Additional safety check - verify device is actually accessible
    try {
      auto device_info = device_->getDeviceInfo();
      if (!device_info) {
        status.summary(diagnostic_msgs::msg::DiagnosticStatus::STALE, "Device info not available");
        return;
      }
    } catch (...) {
      status.summary(diagnostic_msgs::msg::DiagnosticStatus::STALE, "Device not accessible");
      return;
    }

    OBDeviceTemperature temperature;
    uint32_t data_size = sizeof(OBDeviceTemperature);
    device_->getStructuredData(OB_STRUCT_DEVICE_TEMPERATURE,
                               reinterpret_cast<uint8_t *>(&temperature), &data_size);
    status.add("CPU Temperature", temperature.cpuTemp);
    status.add("IR Temperature", temperature.irTemp);
    status.add("LDM Temperature", temperature.ldmTemp);
    status.add("MainBoard Temperature", temperature.mainBoardTemp);
    status.add("TEC Temperature", temperature.tecTemp);
    status.add("IMU Temperature", temperature.imuTemp);
    status.add("RGB Temperature", temperature.rgbTemp);
    status.add("Left IR Temperature", temperature.irLeftTemp);
    status.add("Right IR Temperature", temperature.irRightTemp);
    status.add("Chip Top Temperature", temperature.chipTopTemp);
    status.add("Chip Bottom Temperature", temperature.chipBottomTemp);
    status.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "Temperature is normal");
  } catch (const ob::Error &e) {
    RCLCPP_ERROR_STREAM(
        logger_, "Failed to TemperatureUpdate1: " << orbbec_camera::formatObErrorWithStatus(e));
    status.summary(diagnostic_msgs::msg::DiagnosticStatus::ERROR,
                   orbbec_camera::formatObErrorWithStatus(e));
  } catch (const std::exception &e) {
    RCLCPP_ERROR_STREAM(logger_, "Failed to TemperatureUpdate2: " << e.what());
    status.summary(diagnostic_msgs::msg::DiagnosticStatus::ERROR, e.what());
  } catch (...) {
    RCLCPP_ERROR(logger_, "Failed to TemperatureUpdate3: Device is deactivated/disconnected!");
    status.summary(diagnostic_msgs::msg::DiagnosticStatus::ERROR, "Unknown error");
  }
}

void OBCameraNode::setupDiagnosticUpdater() {
  if (diagnostic_period_ <= 0.0 || is_playback_device_) {
    // Temperature reporting requires live hardware monitoring that doesn't
    // exist for a recorded .bag file; skip to avoid repeated SDK errors.
    return;
  }
  try {
    RCLCPP_INFO_STREAM(logger_, "Publish diagnostics every " << diagnostic_period_ << " seconds");
    auto info = device_->getDeviceInfo();
    std::string serial_number = info->getSerialNumber();
    diagnostic_updater_ = std::make_unique<diagnostic_updater::Updater>(node_, 10000.0);
    diagnostic_updater_->setHardwareID(serial_number);
    diagnostic_updater_->add("Temperatures", this, &OBCameraNode::onTemperatureUpdate);
    diagnostic_timer_ =
        node_->create_wall_timer(std::chrono::seconds(int(diagnostic_period_)), [this]() {
          try {
            // Check if we're still running and all components are valid
            if (!is_running_.load() || !diagnostic_updater_ ||
                !is_camera_node_initialized_.load() || !device_) {
              return;
            }

            // Try to acquire device lock with timeout to avoid blocking during shutdown
            std::unique_lock<decltype(device_lock_)> lock(device_lock_, std::try_to_lock);
            if (!lock.owns_lock()) {
              // Device is busy or shutting down, skip this update
              return;
            }
            // Mark diagnostic as running to prevent concurrent reset/publish races
            {
              std::lock_guard<std::mutex> lk(diagnostic_mutex_);
              diagnostic_running_ = true;
            }
            try {
              diagnostic_updater_->force_update();
            } catch (...) {
              std::lock_guard<std::mutex> lk(diagnostic_mutex_);
              diagnostic_running_ = false;
              diagnostic_cv_.notify_all();
              throw;
            }
            {
              std::lock_guard<std::mutex> lk(diagnostic_mutex_);
              diagnostic_running_ = false;
            }
            diagnostic_cv_.notify_all();
          } catch (const ob::Error &e) {
            RCLCPP_WARN_STREAM(
                logger_, "Diagnostic update failed: " << orbbec_camera::formatObErrorWithStatus(e)
                                                      << " - Device may be disconnected");
            // Stop the diagnostic timer if device is having issues
            try {
              if (diagnostic_timer_) {
                diagnostic_timer_->cancel();
                diagnostic_timer_.reset();
              }
            } catch (...) {
              // Ignore cleanup exceptions
            }
          } catch (const std::exception &e) {
            RCLCPP_WARN_STREAM(logger_, "Diagnostic update failed: " << e.what());
          } catch (...) {
            RCLCPP_WARN(logger_, "Diagnostic update failed: Unknown error");
          }
        });
  } catch (const ob::Error &e) {
    RCLCPP_ERROR_STREAM(logger_, "Failed to setup diagnostic updater: "
                                     << orbbec_camera::formatObErrorWithStatus(e));
  } catch (const std::exception &e) {
    RCLCPP_ERROR_STREAM(logger_, "Failed to setup diagnostic updater: " << e.what());
  } catch (...) {
    RCLCPP_ERROR(logger_, "Failed to TemperatureUpdate");
  }
}

void OBCameraNode::setupPipelineConfig() {
  if (pipeline_config_) {
    pipeline_config_.reset();
  }
  pipeline_config_ = std::make_shared<ob::Config>();
  auto device_info = device_->getDeviceInfo();
  CHECK_NOTNULL(device_info.get());
  if (depth_registration_ && enable_stream_[COLOR] && enable_stream_[DEPTH] &&
      align_mode_ == "HW") {
    OBAlignMode align_mode = ALIGN_D2C_HW_MODE;
    RCLCPP_INFO_STREAM(logger_, "set align mode to " << align_mode_);
    pipeline_config_->setAlignMode(align_mode);
    RCLCPP_INFO_STREAM(logger_, "enable depth scale " << (enable_depth_scale_ ? "ON" : "OFF"));
    pipeline_config_->setDepthScaleRequire(enable_depth_scale_);
  }
  for (const auto &stream_index : IMAGE_STREAMS) {
    if (enable_stream_[stream_index]) {
      RCLCPP_DEBUG_STREAM(logger_, "Enable " << stream_name_[stream_index] << " stream");
      auto profile = stream_profile_[stream_index]->as<ob::VideoStreamProfile>();

      if (stream_index == COLOR && align_target_stream_ == OB_STREAM_COLOR && align_filter_) {
        auto video_profile = profile;
        align_filter_->setAlignToStreamProfile(video_profile);
      }
      if (stream_index == DEPTH && align_target_stream_ == OB_STREAM_DEPTH && align_filter_) {
        auto video_profile = profile;
        align_filter_->setAlignToStreamProfile(video_profile);
      }
      pipeline_config_->enableStream(stream_profile_[stream_index]);
    }
  }

  if (frame_aggregate_mode_ == "full_frame") {
    pipeline_config_->setFrameAggregateOutputMode(OB_FRAME_AGGREGATE_OUTPUT_FULL_FRAME_REQUIRE);
  } else if (frame_aggregate_mode_ == "color_frame") {
    pipeline_config_->setFrameAggregateOutputMode(OB_FRAME_AGGREGATE_OUTPUT_COLOR_FRAME_REQUIRE);
  } else if (frame_aggregate_mode_ == "disable") {
    pipeline_config_->setFrameAggregateOutputMode(OB_FRAME_AGGREGATE_OUTPUT_DISABLE);
  } else {
    pipeline_config_->setFrameAggregateOutputMode(OB_FRAME_AGGREGATE_OUTPUT_ANY_SITUATION);
  }
}

bool OBCameraNode::validateEnhancedDepthFilterConfig(std::string &message) const {
  if (!enable_stream_.count(COLOR) || !enable_stream_.at(COLOR) || !enable_stream_.count(DEPTH) ||
      !enable_stream_.at(DEPTH)) {
    message = "Enhanced depth filter requires color and depth streams";
    return false;
  }
  if (!depth_registration_) {
    message = "Enhanced depth filter requires D2C/C2D align mode";
    return false;
  }
  if (align_mode_ != "HW" && align_mode_ != "SW") {
    message = "Enhanced depth filter requires D2C/C2D align mode";
    return false;
  }
  if (align_mode_ == "HW" && align_target_stream_ != OB_STREAM_COLOR) {
    message = "Enhanced depth filter requires HW D2C align target COLOR";
    return false;
  }
  if (align_mode_ == "SW" && align_target_stream_ != OB_STREAM_COLOR &&
      align_target_stream_ != OB_STREAM_DEPTH) {
    message = "Enhanced depth filter requires D2C/C2D align mode";
    return false;
  }

  auto color_it = stream_profile_.find(COLOR);
  auto depth_it = stream_profile_.find(DEPTH);
  if (color_it == stream_profile_.end() || !color_it->second || depth_it == stream_profile_.end() ||
      !depth_it->second) {
    message = "Enhanced depth filter requires color and depth stream profiles";
    return false;
  }
  auto color_profile = color_it->second->as<ob::VideoStreamProfile>();
  auto depth_profile = depth_it->second->as<ob::VideoStreamProfile>();
  if (!color_profile || !depth_profile) {
    message = "Enhanced depth filter requires video stream profiles";
    return false;
  }

  const bool d2c = align_mode_ == "HW" || align_target_stream_ == OB_STREAM_COLOR;
  const OBStreamType align_to_stream = d2c ? OB_STREAM_COLOR : OB_STREAM_DEPTH;
  if (!ob::EnhancedDepthFilter::isSupportedResolution(color_profile->getType(), align_to_stream,
                                                      color_profile->getWidth(),
                                                      color_profile->getHeight())) {
    message = std::string("Enhanced depth filter requires supported target resolutions: ") +
              kEnhancedDepthSupportedTargetResolutions;
    return false;
  }
  if (!ob::EnhancedDepthFilter::isSupportedResolution(depth_profile->getType(), align_to_stream,
                                                      depth_profile->getWidth(),
                                                      depth_profile->getHeight()) ||
      !ob::EnhancedDepthFilter::isSupportedFormat(OB_STREAM_DEPTH, depth_profile->getFormat())) {
    message = std::string("Enhanced depth filter requires supported target resolutions: ") +
              kEnhancedDepthSupportedTargetResolutions +
              " and depth formats: " + kEnhancedDepthSupportedDepthFormats;
    return false;
  }
  if (!isEnhancedDepthColorFormatSupported(color_profile->getFormat())) {
    message =
        "Unsupported color stream format for enhanced depth filter. Supported formats are: YUYV "
        "UYVY MJPG BGR RGBA Y16 Y8 RGB";
    return false;
  }
  return true;
}

void OBCameraNode::applyEnhancedDepthConfidenceThreshold() {
  if (!enhanced_depth_filter_ || enhanced_depth_confidence_threshold_ < 0) {
    return;
  }
  auto range = enhanced_depth_filter_->getConfidenceThresholdRange();
  const int confidence_threshold = enhanced_depth_confidence_threshold_;
  if (confidence_threshold < range.min || confidence_threshold > range.max) {
    std::ostringstream ss;
    ss << "Enhanced depth confidence threshold is out of range " << range.min << " - " << range.max;
    throw std::runtime_error(ss.str());
  }
  enhanced_depth_filter_->setConfidenceThreshold(static_cast<uint32_t>(confidence_threshold));
}

bool OBCameraNode::ensureEnhancedDepthFilter(std::string &message) {
  if (!validateEnhancedDepthFilterConfig(message)) {
    return false;
  }

  std::lock_guard<std::mutex> lock(enhanced_depth_filter_mutex_);
  try {
    if (!enhanced_depth_filter_) {
      if (enhanced_depth_model_path_.empty()) {
        message = "Enhanced depth filter requires enhanced_depth_model_path";
        return false;
      }
      std::ifstream model_file(enhanced_depth_model_path_);
      if (!model_file.good()) {
        message = "Enhanced depth model file not found: " + enhanced_depth_model_path_;
        return false;
      }
      if (!device_->isLicenseAuthorizationSupported()) {
        message = "Enhanced depth filter requires device license authorization support";
        return false;
      }
      auto license_info = device_->readLicenseInfo();
      RCLCPP_INFO_STREAM(logger_, "Enhanced depth license info: " << license_info);
      if (license_info.empty()) {
        message = "Enhanced depth filter requires device license info";
        return false;
      }
      RCLCPP_INFO_STREAM(logger_, "Creating enhanced depth filter with model path: "
                                      << enhanced_depth_model_path_);
      enhanced_depth_filter_ =
          std::make_shared<ob::EnhancedDepthFilter>(device_, enhanced_depth_model_path_);
    }
    const bool d2c = align_mode_ == "HW" || align_target_stream_ == OB_STREAM_COLOR;
    auto target_profile = d2c ? stream_profile_.at(COLOR)->as<ob::VideoStreamProfile>()
                              : stream_profile_.at(DEPTH)->as<ob::VideoStreamProfile>();
    enhanced_depth_filter_->setResolution(target_profile->getWidth(), target_profile->getHeight());
    applyEnhancedDepthConfidenceThreshold();
    setupConfidencePublishers();
  } catch (const ob::Error &e) {
    message =
        "Failed to create enhanced depth filter: " + orbbec_camera::formatObErrorWithStatus(e);
    return false;
  } catch (const std::exception &e) {
    message = "Failed to create enhanced depth filter: " + std::string(e.what());
    return false;
  }
  return true;
}

bool OBCameraNode::convertEnhancedDepthColorFrame(const std::shared_ptr<ob::FrameSet> &frame_set) {
  auto color_frame = frame_set ? frame_set->getFrame(OB_FRAME_COLOR) : nullptr;
  if (!color_frame) {
    return false;
  }
  const auto color_format = color_frame->getFormat();
  if (color_format == OB_FORMAT_RGB) {
    return true;
  }
  const auto &format_map = enhancedDepthColorFormatMap();
  auto it = format_map.find(color_format);
  if (it == format_map.end()) {
    RCLCPP_ERROR_STREAM_THROTTLE(
        logger_, *node_->get_clock(), 1000,
        "Unsupported color stream format for enhanced depth filter: " << color_format);
    return false;
  }

  try {
    enhanced_depth_format_convert_filter_.setFormatConvertType(it->second);
    auto converted = enhanced_depth_format_convert_filter_.process(color_frame);
    if (!converted) {
      RCLCPP_ERROR_THROTTLE(logger_, *node_->get_clock(), 1000,
                            "Enhanced depth color format conversion failed");
      return false;
    }
    frame_set->pushFrame(converted);
  } catch (const ob::Error &e) {
    RCLCPP_ERROR_STREAM_THROTTLE(logger_, *node_->get_clock(), 1000,
                                 "Enhanced depth color format conversion failed: "
                                     << orbbec_camera::formatObErrorWithStatus(e));
    return false;
  } catch (const std::exception &e) {
    RCLCPP_ERROR_STREAM_THROTTLE(logger_, *node_->get_clock(), 1000,
                                 "Enhanced depth color format conversion failed: " << e.what());
    return false;
  }
  return true;
}

std::shared_ptr<ob::FrameSet> OBCameraNode::processEnhancedDepthFilter(
    const std::shared_ptr<ob::FrameSet> &frame_set) {
  if (!frame_set) {
    return frame_set;
  }
  {
    std::lock_guard<std::mutex> lock(enhanced_depth_filter_mutex_);
    if (!enable_enhanced_depth_.load()) {
      return frame_set;
    }
  }

  std::string message;
  if (!ensureEnhancedDepthFilter(message)) {
    RCLCPP_ERROR_STREAM_THROTTLE(logger_, *node_->get_clock(), 1000, message);
    return frame_set;
  }
  auto original_color_frame = frame_set->getFrame(OB_FRAME_COLOR);
  if (!original_color_frame || !frame_set->getFrame(OB_FRAME_DEPTH)) {
    RCLCPP_ERROR_THROTTLE(logger_, *node_->get_clock(), 1000,
                          "Enhanced depth filter requires color and depth frames");
    return frame_set;
  }

  std::shared_ptr<ob::EnhancedDepthFilter> filter;
  {
    std::lock_guard<std::mutex> lock(enhanced_depth_filter_mutex_);
    if (!enable_enhanced_depth_.load()) {
      return frame_set;
    }
    filter = enhanced_depth_filter_;
  }

  try {
    // Convert color only in the filter input so downstream publishers keep the configured format.
    auto cloned_frame_set = ob::FrameFactory::createFrameFromOtherFrame(frame_set, false);
    if (!cloned_frame_set || !cloned_frame_set->is<ob::FrameSet>()) {
      RCLCPP_ERROR_THROTTLE(logger_, *node_->get_clock(), 1000,
                            "Failed to clone frameset for enhanced depth filter");
      return frame_set;
    }
    auto filter_frame_set = cloned_frame_set->as<ob::FrameSet>();
    if (!convertEnhancedDepthColorFrame(filter_frame_set)) {
      return frame_set;
    }
    auto processed = filter->process(filter_frame_set);
    if (!processed || !processed->is<ob::FrameSet>()) {
      RCLCPP_ERROR_THROTTLE(logger_, *node_->get_clock(), 1000,
                            "Enhanced depth filter returned invalid frameset");
      return frame_set;
    }
    auto processed_frame_set = processed->as<ob::FrameSet>();
    processed_frame_set->pushFrame(original_color_frame);
    publishConfidenceFrame(processed_frame_set->getFrame(OB_FRAME_CONFIDENCE));
    return processed_frame_set;
  } catch (const ob::Error &e) {
    RCLCPP_ERROR_STREAM_THROTTLE(
        logger_, *node_->get_clock(), 1000,
        "Enhanced depth filter failed: " << orbbec_camera::formatObErrorWithStatus(e));
  } catch (const std::exception &e) {
    RCLCPP_ERROR_STREAM_THROTTLE(logger_, *node_->get_clock(), 1000,
                                 "Enhanced depth filter failed: " << e.what());
  }
  return frame_set;
}

void OBCameraNode::setupConfidencePublishers() {
  if (confidence_image_publisher_) {
    return;
  }
  auto image_qos_profile = getRMWQosProfileFromString(image_qos_[DEPTH]);
  if (use_intra_process_) {
    image_qos_profile = rmw_qos_profile_default;
  }
  confidence_image_publisher_ = node_->create_publisher<sensor_msgs::msg::Image>(
      "confidence/image_raw",
      rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(image_qos_profile), image_qos_profile));
}

void OBCameraNode::publishConfidenceFrame(const std::shared_ptr<ob::Frame> &confidence_frame) {
  if (!confidence_frame || !confidence_frame->is<ob::VideoFrame>()) {
    return;
  }
  setupConfidencePublishers();
  if (!confidence_image_publisher_ || confidence_image_publisher_->get_subscription_count() == 0) {
    return;
  }

  auto video_frame = confidence_frame->as<ob::VideoFrame>();
  const int width = static_cast<int>(video_frame->getWidth());
  const int height = static_cast<int>(video_frame->getHeight());
  int image_type = CV_8UC1;
  std::string encoding = sensor_msgs::image_encodings::MONO8;
  int unit_step_size = sizeof(uint8_t);
  if (confidence_frame->getFormat() == OB_FORMAT_Y16) {
    image_type = CV_16UC1;
    encoding = sensor_msgs::image_encodings::MONO16;
    unit_step_size = sizeof(uint16_t);
  } else if (confidence_frame->getFormat() != OB_FORMAT_Y8) {
    RCLCPP_ERROR_STREAM_THROTTLE(
        logger_, *node_->get_clock(), 1000,
        "Unsupported confidence frame format: " << confidence_frame->getFormat());
    return;
  }

  if (confidence_image_.empty() || confidence_image_.cols != width ||
      confidence_image_.rows != height || confidence_image_.type() != image_type) {
    confidence_image_.create(height, width, image_type);
  }
  memcpy(confidence_image_.data, video_frame->getData(), video_frame->getDataSize());

  auto timestamp = fromUsToROSTime(getFrameTimestampUs(confidence_frame));
  std::string frame_id =
      depth_registration_ ? depth_aligned_frame_id_[DEPTH] : optical_frame_id_[DEPTH];

  sensor_msgs::msg::Image::UniquePtr image_msg(new sensor_msgs::msg::Image());
  cv_bridge::CvImage(std_msgs::msg::Header(), encoding, confidence_image_).toImageMsg(*image_msg);
  image_msg->header.stamp = timestamp;
  image_msg->header.frame_id = frame_id;
  image_msg->is_bigendian = false;
  image_msg->step = width * unit_step_size;
  confidence_image_publisher_->publish(std::move(image_msg));
}

void OBCameraNode::setupCameraInfo() {
  const auto create_camera_info_manager = [this](const std::string &camera_name,
                                                 const std::string &camera_info_url) {
#ifdef ORBBEC_CAMERA_INFO_MANAGER_USES_NODE_INTERFACES
#ifdef ORBBEC_CAMERA_INFO_MANAGER_USES_RCLCPP_QOS
    return std::make_unique<camera_info_manager::CameraInfoManager>(
        node_->get_node_base_interface(), node_->get_node_services_interface(),
        node_->get_node_logging_interface(), camera_name, camera_info_url,
        rclcpp::SystemDefaultsQoS());
#else
    return std::make_unique<camera_info_manager::CameraInfoManager>(
        node_->get_node_base_interface(), node_->get_node_services_interface(),
        node_->get_node_logging_interface(), camera_name, camera_info_url, rmw_qos_profile_default);
#endif
#else
    return std::make_unique<camera_info_manager::CameraInfoManager>(node_, camera_name,
                                                                    camera_info_url);
#endif
  };

  const std::string color_camera_name = camera_name_ + "_color";
  if (!color_info_url_.empty()) {
    color_info_manager_ = create_camera_info_manager(color_camera_name, color_info_url_);
  }
  const std::string ir_camera_name = camera_name_ + "_ir";
  if (!ir_info_url_.empty()) {
    ir_info_manager_ = create_camera_info_manager(ir_camera_name, ir_info_url_);
  }
}

void OBCameraNode::setupImagePublisher(const stream_index_pair &stream_index) {
  if (!enable_stream_[stream_index]) {
    image_publishers_.erase(stream_index);
    compressed_image_publishers_.erase(stream_index);
    return;
  }

  const std::string topic = stream_name_[stream_index] + "/image_raw";
  auto image_qos_profile = getRMWQosProfileFromString(image_qos_[stream_index]);
  if (use_intra_process_) {
    image_qos_profile = rmw_qos_profile_default;
  }

  const bool is_mjpg_color_stream =
      (stream_index == COLOR || stream_index == COLOR_LEFT || stream_index == COLOR_RIGHT) &&
      format_[stream_index] == OB_FORMAT_MJPG;
  if (use_intra_process_ || is_mjpg_color_stream) {
    image_publishers_[stream_index] =
        std::make_shared<image_rcl_publisher>(*node_, topic, image_qos_profile);
  } else {
    image_publishers_[stream_index] =
        std::make_shared<image_transport_publisher>(*node_, topic, image_qos_profile);
  }

  if (is_mjpg_color_stream) {
    compressed_image_publishers_[stream_index] =
        node_->create_publisher<sensor_msgs::msg::CompressedImage>(
            topic + "/compressed",
            rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(image_qos_profile), image_qos_profile));
  } else {
    compressed_image_publishers_.erase(stream_index);
  }
}

void OBCameraNode::setupPublishers() {
  using PointCloud2 = sensor_msgs::msg::PointCloud2;
  using CameraInfo = sensor_msgs::msg::CameraInfo;
  auto point_cloud_qos_profile = getRMWQosProfileFromString(point_cloud_qos_);
  if (use_intra_process_) {
    point_cloud_qos_profile = rmw_qos_profile_default;
  }
  if (enable_colored_point_cloud_) {
    depth_registration_cloud_pub_ = node_->create_publisher<PointCloud2>(
        "depth_registered/points",
        rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(point_cloud_qos_profile),
                    point_cloud_qos_profile));
  }
  if (enable_point_cloud_) {
    depth_cloud_pub_ = node_->create_publisher<PointCloud2>(
        "depth/points", rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(point_cloud_qos_profile),
                                    point_cloud_qos_profile));
  }
  auto device_info = device_->getDeviceInfo();
  CHECK_NOTNULL(device_info.get());
  for (const auto &stream_index : IMAGE_STREAMS) {
    if (!enable_stream_[stream_index]) {
      continue;
    }
    std::string name = stream_name_[stream_index];
    setupImagePublisher(stream_index);

    std::string topic = name + "/camera_info";
    auto camera_info_qos = camera_info_qos_[stream_index];
    auto camera_info_qos_profile = getRMWQosProfileFromString(camera_info_qos);
    if (use_intra_process_) {
      camera_info_qos_profile = rmw_qos_profile_default;
    }
    camera_info_publishers_[stream_index] = node_->create_publisher<CameraInfo>(
        topic, rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(camera_info_qos_profile),
                           camera_info_qos_profile));
    if (isPublishMetaData(pid_)) {
      metadata_publishers_[stream_index] =
          node_->create_publisher<orbbec_camera_msgs::msg::Metadata>(
              name + "/metadata",
              rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(camera_info_qos_profile),
                          camera_info_qos_profile));
    }
  }

  syncSoftwareAlignment();

  if (enable_sync_output_accel_gyro_) {
    std::string topic_name = stream_name_[GYRO] + "_" + stream_name_[ACCEL] + "/sample";
    auto data_qos = getRMWQosProfileFromString(imu_qos_[GYRO]);
    if (use_intra_process_) {
      data_qos = rmw_qos_profile_default;
    }
    imu_gyro_accel_publisher_ = node_->create_publisher<sensor_msgs::msg::Imu>(
        topic_name, rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(data_qos), data_qos));
    topic_name = stream_name_[GYRO] + "/imu_info";
    imu_info_publishers_[GYRO] = node_->create_publisher<orbbec_camera_msgs::msg::IMUInfo>(
        topic_name, rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(data_qos), data_qos));
    topic_name = stream_name_[ACCEL] + "/imu_info";
    imu_info_publishers_[ACCEL] = node_->create_publisher<orbbec_camera_msgs::msg::IMUInfo>(
        topic_name, rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(data_qos), data_qos));
  } else {
    for (const auto &stream_index : HID_STREAMS) {
      if (!enable_stream_[stream_index]) {
        continue;
      }
      std::string data_topic_name = stream_name_[stream_index] + "/sample";
      auto data_qos = getRMWQosProfileFromString(imu_qos_[stream_index]);
      if (use_intra_process_) {
        data_qos = rmw_qos_profile_default;
      }
      imu_publishers_[stream_index] = node_->create_publisher<sensor_msgs::msg::Imu>(
          data_topic_name, rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(data_qos), data_qos));
      data_topic_name = stream_name_[stream_index] + "/imu_info";
      imu_info_publishers_[stream_index] =
          node_->create_publisher<orbbec_camera_msgs::msg::IMUInfo>(
              data_topic_name,
              rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(data_qos), data_qos));
    }
  }
  auto extrinsics_qos = rclcpp::QoS(1).transient_local();
  if (use_intra_process_) {
    extrinsics_qos = rclcpp::QoS(1);
  }
  if (enable_stream_[DEPTH] && enable_stream_[INFRA0] && enable_publish_extrinsic_) {
    depth_to_other_extrinsics_publishers_[INFRA0] =
        node_->create_publisher<orbbec_camera_msgs::msg::Extrinsics>("depth_to_ir", extrinsics_qos);
  }
  if (enable_stream_[DEPTH] && enable_stream_[COLOR] && enable_publish_extrinsic_) {
    depth_to_other_extrinsics_publishers_[COLOR] =
        node_->create_publisher<orbbec_camera_msgs::msg::Extrinsics>("depth_to_color",
                                                                     extrinsics_qos);
  }
  if (enable_stream_[DEPTH] && enable_stream_[INFRA1] && enable_publish_extrinsic_) {
    depth_to_other_extrinsics_publishers_[INFRA1] =
        node_->create_publisher<orbbec_camera_msgs::msg::Extrinsics>("depth_to_left_ir",
                                                                     extrinsics_qos);
  }
  if (enable_stream_[DEPTH] && enable_stream_[INFRA2] && enable_publish_extrinsic_) {
    depth_to_other_extrinsics_publishers_[INFRA2] =
        node_->create_publisher<orbbec_camera_msgs::msg::Extrinsics>("depth_to_right_ir",
                                                                     extrinsics_qos);
  }
  if (enable_stream_[DEPTH] && enable_stream_[ACCEL] && enable_publish_extrinsic_) {
    depth_to_other_extrinsics_publishers_[ACCEL] =
        node_->create_publisher<orbbec_camera_msgs::msg::Extrinsics>("depth_to_accel",
                                                                     extrinsics_qos);
  }
  if (enable_stream_[DEPTH] && enable_stream_[GYRO] && enable_publish_extrinsic_) {
    depth_to_other_extrinsics_publishers_[GYRO] =
        node_->create_publisher<orbbec_camera_msgs::msg::Extrinsics>("depth_to_gyro",
                                                                     extrinsics_qos);
  }
  if (enable_stream_[COLOR_LEFT] && enable_stream_[COLOR_RIGHT] && enable_publish_extrinsic_) {
    depth_to_other_extrinsics_publishers_[COLOR_LEFT] =
        node_->create_publisher<orbbec_camera_msgs::msg::Extrinsics>("left_color_to_right_color",
                                                                     extrinsics_qos);
  }
  depth_filters_status_pub_ =
      node_->create_publisher<DepthFiltersStatus>("depth_filters/status", extrinsics_qos);
  publishDepthFiltersStatus();

  if (enable_lrm_obstacle_distance_publish_) {
    lrm_obstacle_distance_pub_ =
        node_->create_publisher<std_msgs::msg::Int32>("lrm/obstacle_distance", rclcpp::QoS(10));
    RCLCPP_INFO_STREAM(logger_, "Publishing LRM obstacle distance on lrm/obstacle_distance at "
                                    << lrm_obstacle_distance_publish_rate_ << " Hz");
    auto publish_period = std::chrono::duration_cast<std::chrono::milliseconds>(
        std::chrono::duration<double>(1.0 / lrm_obstacle_distance_publish_rate_));
    if (publish_period < std::chrono::milliseconds(1)) {
      publish_period = std::chrono::milliseconds(1);
    }
    lrm_obstacle_distance_timer_ =
        node_->create_wall_timer(publish_period, [this]() { publishLrmObstacleDistance(); });
  }
}

void OBCameraNode::syncSoftwareAlignment() {
  if (depth_registration_ && align_mode_ == "SW") {
    if (!align_filter_) {
      align_filter_ = std::make_unique<ob::Align>(align_target_stream_);
      RCLCPP_INFO_STREAM(logger_, "set align mode to " << align_mode_);
    }
    if (!depth_unaligned_publisher_) {
      auto depth_image_qos_profile = getRMWQosProfileFromString(image_qos_[DEPTH]);
      if (use_intra_process_) {
        depth_image_qos_profile = rmw_qos_profile_default;
      }
      if (use_intra_process_) {
        depth_unaligned_publisher_ = std::make_shared<image_rcl_publisher>(
            *node_, "depth/image_unaligned", depth_image_qos_profile);
      } else {
        depth_unaligned_publisher_ = std::make_shared<image_transport_publisher>(
            *node_, "depth/image_unaligned", depth_image_qos_profile);
      }
    }
    return;
  }

  align_filter_.reset();
  depth_unaligned_publisher_.reset();
}

void OBCameraNode::publishPointCloud(const std::shared_ptr<ob::FrameSet> &frame_set) {
  try {
    if (depth_registration_ || enable_colored_point_cloud_) {
      if (frame_set->depthFrame() != nullptr && frame_set->colorFrame() != nullptr) {
        publishColoredPointCloud(frame_set);
      }
    }

    if (enable_point_cloud_ && frame_set->depthFrame() != nullptr) {
      publishDepthPointCloud(frame_set);
    }

  } catch (const ob::Error &e) {
    RCLCPP_ERROR_STREAM(logger_, orbbec_camera::formatObErrorWithStatus(e));
  } catch (const std::exception &e) {
    RCLCPP_ERROR_STREAM(logger_, e.what());
  } catch (...) {
    RCLCPP_ERROR_STREAM(logger_, "publishPointCloud with unknown error");
  }
}

void OBCameraNode::publishRawDepthImage(const std::shared_ptr<ob::Frame> &depth_frame) {
  if (!depth_frame || !depth_unaligned_publisher_ || !depth_registration_ ||
      depth_unaligned_publisher_->get_subscription_count() == 0) {
    return;
  }

  auto video_frame = depth_frame->as<ob::DepthFrame>();
  if (!video_frame) {
    return;
  }

  int width = static_cast<int>(video_frame->getWidth());
  int height = static_cast<int>(video_frame->getHeight());
  auto frame_timestamp = getFrameTimestampUs(depth_frame);
  auto timestamp = fromUsToROSTime(frame_timestamp);

  std::string frame_id = optical_frame_id_[DEPTH];

  cv::Mat depth_image(height, width, image_format_[DEPTH]);
  memcpy(depth_image.data, video_frame->getData(), video_frame->getDataSize());

  auto depth_scale = video_frame->getValueScale();
  depth_image.convertTo(depth_image, depth_image.type(), depth_scale);

  sensor_msgs::msg::Image::UniquePtr image_msg(new sensor_msgs::msg::Image());
  cv_bridge::CvImage(std_msgs::msg::Header(), encoding_[DEPTH], depth_image).toImageMsg(*image_msg);
  image_msg->header.stamp = timestamp;
  image_msg->is_bigendian = false;
  image_msg->step = width * unit_step_size_[DEPTH];
  image_msg->header.frame_id = frame_id;

  depth_unaligned_publisher_->publish(std::move(image_msg));
}

void OBCameraNode::publishDepthPointCloud(const std::shared_ptr<ob::FrameSet> &frame_set) {
  if (!depth_cloud_pub_ || depth_cloud_pub_->get_subscription_count() == 0 ||
      !enable_point_cloud_) {
    return;
  }
  std::lock_guard<decltype(point_cloud_mutex_)> point_cloud_msg_lock(point_cloud_mutex_);
  auto depth_frame = frame_set->depthFrame();
  if (!depth_frame) {
    RCLCPP_ERROR_STREAM(logger_, "depth frame is null");
    return;
  }
  if (!pipeline_) {
    RCLCPP_ERROR_STREAM(logger_, "pipeline is null in publishDepthPointCloud");
    return;
  }
  auto camera_params = pipeline_->getCameraParam();
  if (!device_) {
    RCLCPP_ERROR_STREAM(logger_, "device is null in publishDepthPointCloud");
    return;
  }
  auto device_info = device_->getDeviceInfo();
  if (!device_info || !device_info.get()) {
    RCLCPP_ERROR_STREAM(logger_, "device_info is null in publishDepthPointCloud");
    return;
  }
  if (depth_registration_ || pid_ == DABAI_MAX_PID) {
    camera_params.depthIntrinsic = camera_params.rgbIntrinsic;
  }
  depth_point_cloud_filter_.setCameraParam(camera_params);
  float depth_scale = depth_frame->getValueScale();
  depth_point_cloud_filter_.setPositionDataScaled(depth_scale);
  depth_point_cloud_filter_.setCreatePointFormat(OB_FORMAT_POINT);
  depth_point_cloud_filter_.setDecimationFactor(point_cloud_decimation_filter_factor_);
  auto result_frame = depth_point_cloud_filter_.process(depth_frame);
  if (!result_frame) {
    RCLCPP_ERROR_STREAM(logger_, "Failed to process depth frame");
    return;
  }
  auto point_size = result_frame->dataSize() / sizeof(OBPoint);
  auto *points = static_cast<OBPoint *>(result_frame->data());
  auto width = depth_frame->width() / point_cloud_decimation_filter_factor_;
  auto height = depth_frame->height() / point_cloud_decimation_filter_factor_;
  auto point_cloud_msg = std::make_unique<sensor_msgs::msg::PointCloud2>();
  sensor_msgs::PointCloud2Modifier modifier(*point_cloud_msg);
  modifier.setPointCloud2FieldsByString(1, "xyz");
  modifier.resize(width * height);
  point_cloud_msg->width = width;
  point_cloud_msg->height = height;
  point_cloud_msg->row_step = point_cloud_msg->width * point_cloud_msg->point_step;
  point_cloud_msg->data.resize(point_cloud_msg->height * point_cloud_msg->row_step);
  sensor_msgs::PointCloud2Iterator<float> iter_x(*point_cloud_msg, "x");
  sensor_msgs::PointCloud2Iterator<float> iter_y(*point_cloud_msg, "y");
  sensor_msgs::PointCloud2Iterator<float> iter_z(*point_cloud_msg, "z");
  const static float MIN_DISTANCE = 20.0;     // 2cm
  const static float MAX_DISTANCE = 10000.0;  // 10m
  const static float min_depth = MIN_DISTANCE / depth_scale;
  const static float max_depth = MAX_DISTANCE / depth_scale;
  size_t valid_count = 0;
  for (size_t i = 0; i < point_size; i++) {
    bool valid_point = points[i].z >= min_depth && points[i].z <= max_depth;
    if (valid_point || ordered_pc_) {
      *iter_x = static_cast<float>(points[i].x / 1000.0);
      *iter_y = static_cast<float>(points[i].y / 1000.0);
      *iter_z = static_cast<float>(points[i].z / 1000.0);
      ++iter_x, ++iter_y, ++iter_z;
      valid_count++;
    }
  }
  if (valid_count == 0) {
    RCLCPP_WARN(logger_, "No valid point in point cloud");
    return;
  }
  if (!ordered_pc_) {
    point_cloud_msg->is_dense = true;
    point_cloud_msg->width = valid_count;
    point_cloud_msg->height = 1;
    modifier.resize(valid_count);
    point_cloud_msg->row_step = point_cloud_msg->width * point_cloud_msg->point_step;
  }
  auto frame_timestamp = getFrameTimestampUs(depth_frame);
  auto timestamp = fromUsToROSTime(frame_timestamp);
  std::string frame_id = depth_registration_ ? optical_frame_id_[COLOR] : optical_frame_id_[DEPTH];
  if (!cloud_frame_id_.empty()) {
    frame_id = cloud_frame_id_;
  }
  point_cloud_msg->header.stamp = timestamp;
  point_cloud_msg->header.frame_id = frame_id;
  if (save_point_cloud_) {
    save_point_cloud_ = false;
    auto now = std::time(nullptr);
    std::stringstream ss;
    ss << std::put_time(std::localtime(&now), "%Y%m%d_%H%M%S");
    auto current_path = std::filesystem::current_path().string();
    std::string filename = current_path + "/point_cloud/points_" + ss.str() + ".ply";
    if (!std::filesystem::exists(current_path + "/point_cloud")) {
      std::filesystem::create_directory(current_path + "/point_cloud");
    }
    RCLCPP_INFO_STREAM(logger_, "Saving point cloud to " << filename);
    try {
      saveDepthPointsToPly(point_cloud_msg, filename);
    } catch (const std::exception &e) {
      RCLCPP_ERROR_STREAM(logger_, "Failed to save point cloud: " << e.what());
    }
  }
  depth_cloud_pub_->publish(std::move(point_cloud_msg));
}

void OBCameraNode::publishColoredPointCloud(const std::shared_ptr<ob::FrameSet> &frame_set) {
  if (!depth_registration_cloud_pub_ ||
      depth_registration_cloud_pub_->get_subscription_count() == 0 ||
      !enable_colored_point_cloud_ || !frame_set) {
    return;
  }
  std::lock_guard<decltype(point_cloud_mutex_)> point_cloud_msg_lock(point_cloud_mutex_);

  auto depth_frame = frame_set->depthFrame();
  auto color_frame = frame_set->colorFrame();

  if (!depth_frame || !color_frame) {
    return;
  }

  auto depth_width = depth_frame->getWidth();
  auto depth_height = depth_frame->getHeight();
  auto color_width = color_frame->getWidth();
  auto color_height = color_frame->getHeight();
  if (depth_width != color_width || depth_height != color_height) {
    RCLCPP_DEBUG(logger_, "Depth (%d x %d) and color (%d x %d) frame size mismatch", depth_width,
                 depth_height, color_width, color_height);
    return;
  }

  if (!pipeline_) {
    RCLCPP_ERROR_STREAM(logger_, "pipeline is null in publishColoredPointCloud");
    return;
  }
  auto camera_params = pipeline_->getCameraParam();
  if (!device_) {
    RCLCPP_ERROR_STREAM(logger_, "device is null in publishColoredPointCloud");
    return;
  }
  auto device_info = device_->getDeviceInfo();
  if (!device_info || !device_info.get()) {
    RCLCPP_ERROR_STREAM(logger_, "device_info is null in publishColoredPointCloud");
    return;
  }
  if (depth_registration_ || pid_ == DABAI_MAX_PID) {
    camera_params.depthIntrinsic = camera_params.rgbIntrinsic;
  }

  color_point_cloud_filter_.setCameraParam(camera_params);
  auto depth_scale = depth_frame->getValueScale();
  color_point_cloud_filter_.setPositionDataScaled(depth_scale);
  color_point_cloud_filter_.setCreatePointFormat(OB_FORMAT_RGB_POINT);
  color_point_cloud_filter_.setDecimationFactor(point_cloud_decimation_filter_factor_);
  auto result_frame = color_point_cloud_filter_.process(frame_set);
  if (!result_frame) {
    RCLCPP_ERROR_STREAM(logger_, "Failed to process depth frame");
    return;
  }
  auto point_size = result_frame->dataSize() / sizeof(OBColorPoint);
  auto *point_cloud = static_cast<OBColorPoint *>(result_frame->data());
  auto width = color_frame->getWidth() / point_cloud_decimation_filter_factor_;
  auto height = color_frame->getHeight() / point_cloud_decimation_filter_factor_;
  auto point_cloud_msg = std::make_unique<sensor_msgs::msg::PointCloud2>();
  sensor_msgs::PointCloud2Modifier modifier(*point_cloud_msg);
  modifier.setPointCloud2FieldsByString(1, "xyz");
  modifier.resize(width * height);
  point_cloud_msg->width = width;
  point_cloud_msg->height = height;
  std::string format_str = "rgb";
  point_cloud_msg->point_step =
      addPointField(*point_cloud_msg, format_str, 1, sensor_msgs::msg::PointField::FLOAT32,
                    static_cast<int>(point_cloud_msg->point_step));
  point_cloud_msg->row_step = point_cloud_msg->width * point_cloud_msg->point_step;
  point_cloud_msg->data.resize(point_cloud_msg->height * point_cloud_msg->row_step);
  sensor_msgs::PointCloud2Iterator<float> iter_x(*point_cloud_msg, "x");
  sensor_msgs::PointCloud2Iterator<float> iter_y(*point_cloud_msg, "y");
  sensor_msgs::PointCloud2Iterator<float> iter_z(*point_cloud_msg, "z");
  sensor_msgs::PointCloud2Iterator<uint8_t> iter_r(*point_cloud_msg, "r");
  sensor_msgs::PointCloud2Iterator<uint8_t> iter_g(*point_cloud_msg, "g");
  sensor_msgs::PointCloud2Iterator<uint8_t> iter_b(*point_cloud_msg, "b");
  size_t valid_count = 0;
  static const float MIN_DISTANCE = 20.0;
  static const float MAX_DISTANCE = 10000.0;
  static float min_depth = MIN_DISTANCE / depth_scale;
  static float max_depth = MAX_DISTANCE / depth_scale;
  for (size_t i = 0; i < point_size; i++) {
    bool valid_point = point_cloud[i].z >= min_depth && point_cloud[i].z <= max_depth;
    if (valid_point || ordered_pc_) {
      *iter_x = static_cast<float>(point_cloud[i].x / 1000.0);
      *iter_y = static_cast<float>(point_cloud[i].y / 1000.0);
      *iter_z = static_cast<float>(point_cloud[i].z / 1000.0);
      *iter_r = static_cast<uint8_t>(point_cloud[i].r);
      *iter_g = static_cast<uint8_t>(point_cloud[i].g);
      *iter_b = static_cast<uint8_t>(point_cloud[i].b);
      ++iter_x, ++iter_y, ++iter_z, ++iter_r, ++iter_g, ++iter_b;
      ++valid_count;
    }
  }

  if (valid_count == 0) {
    RCLCPP_WARN(logger_, "No valid points in point cloud");
    return;
  }
  if (!ordered_pc_) {
    point_cloud_msg->is_dense = true;
    point_cloud_msg->width = valid_count;
    point_cloud_msg->height = 1;
    modifier.resize(valid_count);
    point_cloud_msg->row_step = point_cloud_msg->width * point_cloud_msg->point_step;
  }
  auto frame_timestamp = getFrameTimestampUs(depth_frame);
  std::string frame_id = optical_frame_id_[COLOR];
  if (!cloud_frame_id_.empty()) {
    frame_id = cloud_frame_id_;
  }
  auto timestamp = fromUsToROSTime(frame_timestamp);
  point_cloud_msg->header.stamp = timestamp;
  point_cloud_msg->header.frame_id = frame_id;
  if (save_colored_point_cloud_) {
    save_colored_point_cloud_ = false;
    auto now = std::time(nullptr);
    std::stringstream ss;
    ss << std::put_time(std::localtime(&now), "%Y%m%d_%H%M%S");
    auto current_path = std::filesystem::current_path().string();
    std::string filename = current_path + "/point_cloud/colored_points_" + ss.str() + ".ply";
    if (!std::filesystem::exists(current_path + "/point_cloud")) {
      std::filesystem::create_directory(current_path + "/point_cloud");
    }
    RCLCPP_INFO_STREAM(logger_, "Saving point cloud to " << filename);
    try {
      saveRGBPointCloudMsgToPly(point_cloud_msg, filename);
    } catch (const std::exception &e) {
      RCLCPP_ERROR_STREAM(logger_, "Failed to save point cloud: " << e.what());
    } catch (...) {
      RCLCPP_ERROR(logger_, "Failed to save point cloud");
    }
  }

  depth_registration_cloud_pub_->publish(std::move(point_cloud_msg));
}
std::shared_ptr<ob::Frame> OBCameraNode::processIrFrameFilter(std::shared_ptr<ob::Frame> &frame) {
  if (frame == nullptr || frame->getType() != OB_FRAME_IR) {
    return nullptr;
  }
  for (size_t i = 0; i < ir_filter_list_.size(); i++) {
    auto filter = ir_filter_list_[i];
    CHECK_NOTNULL(filter.get());
    if (filter->isEnabled() && frame != nullptr) {
      frame = filter->process(frame);
      if (frame == nullptr) {
        RCLCPP_ERROR_STREAM(logger_, "Ir filter process failed");
        break;
      }
    }
  }
  return frame;
}

std::shared_ptr<ob::Frame> OBCameraNode::processRightIrFrameFilter(
    std::shared_ptr<ob::Frame> &frame) {
  if (frame == nullptr || frame->getType() != OB_FRAME_IR_RIGHT) {
    return nullptr;
  }
  for (size_t i = 0; i < right_ir_filter_list_.size(); i++) {
    auto filter = right_ir_filter_list_[i];
    CHECK_NOTNULL(filter.get());
    if (filter->isEnabled() && frame != nullptr) {
      frame = filter->process(frame);
      if (frame == nullptr) {
        RCLCPP_ERROR_STREAM(logger_, "Right Ir filter process failed");
        break;
      }
    }
  }
  return frame;
}
std::shared_ptr<ob::Frame> OBCameraNode::processLeftIrFrameFilter(
    std::shared_ptr<ob::Frame> &frame) {
  if (frame == nullptr || frame->getType() != OB_FRAME_IR_LEFT) {
    return nullptr;
  }
  for (size_t i = 0; i < left_ir_filter_list_.size(); i++) {
    auto filter = left_ir_filter_list_[i];
    CHECK_NOTNULL(filter.get());
    if (filter->isEnabled() && frame != nullptr) {
      frame = filter->process(frame);
      if (frame == nullptr) {
        RCLCPP_ERROR_STREAM(logger_, "Left Ir filter process failed");
        break;
      }
    }
  }
  return frame;
}
std::shared_ptr<ob::Frame> OBCameraNode::processColorFrameFilter(
    std::shared_ptr<ob::Frame> &frame) {
  if (frame == nullptr) {
    return nullptr;
  }
  auto frame_type = frame->getType();
  if (frame_type == OB_FRAME_COLOR) {
    for (size_t i = 0; i < color_filter_list_.size(); i++) {
      auto filter = color_filter_list_[i];
      CHECK_NOTNULL(filter.get());
      if (filter->isEnabled() && frame != nullptr) {
        frame = filter->process(frame);
        if (frame == nullptr) {
          RCLCPP_ERROR_STREAM(logger_, "Color filter process failed");
          break;
        }
      }
    }
    return frame;
  } else if (frame_type == OB_FRAME_COLOR_LEFT) {
    for (size_t i = 0; i < left_color_filter_list_.size(); i++) {
      auto filter = left_color_filter_list_[i];
      CHECK_NOTNULL(filter.get());
      if (filter->isEnabled() && frame != nullptr) {
        frame = filter->process(frame);
        if (frame == nullptr) {
          RCLCPP_ERROR_STREAM(logger_, "Left color filter process failed");
          break;
        }
      }
    }
    return frame;
  } else if (frame_type == OB_FRAME_COLOR_RIGHT) {
    for (size_t i = 0; i < right_color_filter_list_.size(); i++) {
      auto filter = right_color_filter_list_[i];
      CHECK_NOTNULL(filter.get());
      if (filter->isEnabled() && frame != nullptr) {
        frame = filter->process(frame);
        if (frame == nullptr) {
          RCLCPP_ERROR_STREAM(logger_, "Right color filter process failed");
          break;
        }
      }
    }
    return frame;
  }
  return nullptr;
}
std::shared_ptr<ob::Frame> OBCameraNode::processDepthFrameFilter(
    std::shared_ptr<ob::Frame> &frame) {
  if (frame == nullptr || frame->getType() != OB_FRAME_DEPTH) {
    return nullptr;
  }
  std::lock_guard<std::mutex> depth_filter_lock(depth_filter_mutex_);
  for (size_t i = 0; i < depth_filter_list_.size(); i++) {
    auto filter = depth_filter_list_[i];
    CHECK_NOTNULL(filter.get());
    if (filter->isEnabled() && frame != nullptr) {
      frame = filter->process(frame);
      if (frame == nullptr) {
        RCLCPP_WARN_STREAM(logger_, "Depth filter process failed, frame is null");
        break;
      }
    }
  }
  return frame;
}
void OBCameraNode::setDisparitySearchOffset() {
  static bool has_run = false;
  auto config = OBDispOffsetConfig();
  if (has_run) {
    return;
  }
  if (device_->isPropertySupported(OB_PROP_DISP_SEARCH_OFFSET_INT, OB_PERMISSION_WRITE)) {
    if (disparity_search_offset_ >= 0 && disparity_search_offset_ <= 127) {
      device_->setIntProperty(OB_PROP_DISP_SEARCH_OFFSET_INT, disparity_search_offset_);
      RCLCPP_INFO_STREAM(logger_, "Set disparity search offset to " << disparity_search_offset_);
    }
    if (offset_index0_ >= 0 && offset_index0_ <= 127 && offset_index1_ >= 0 &&
        offset_index1_ <= 127) {
      config.enable = disparity_offset_config_;
      config.offset0 = offset_index0_;
      config.offset1 = offset_index1_;
      config.reserved = 0;

      device_->setStructuredData(OB_STRUCT_DISP_OFFSET_CONFIG,
                                 reinterpret_cast<const uint8_t *>(&config), sizeof(config));
      RCLCPP_INFO_STREAM(logger_, "disparity_offset_config: "
                                      << disparity_offset_config_ << "  offset_index0:"
                                      << offset_index0_ << "  offset_index1:" << offset_index1_);
    }
  }
  has_run = true;
}

void OBCameraNode::setDepthAutoExposureROI() {
  static bool depth_roi_has_run = false;
  if (depth_roi_has_run) {
    return;
  }
  if (isGemini305SeriesPID(pid_) && ae_reference_stream_ == "color") {
    RCLCPP_WARN_STREAM(logger_, "Skip setting depth AE ROI because AE Reference Stream is color");
    depth_roi_has_run = true;
    return;
  }
  if (device_->isPropertySupported(OB_STRUCT_DEPTH_AE_ROI, OB_PERMISSION_READ_WRITE)) {
    auto config = OBRegionOfInterest();
    uint32_t data_size = sizeof(config);
    device_->getStructuredData(OB_STRUCT_DEPTH_AE_ROI, reinterpret_cast<uint8_t *>(&config),
                               &data_size);
    if (depth_ae_roi_left_ != -1) {
      config.x0_left = (depth_ae_roi_left_ < 0) ? 0 : depth_ae_roi_left_;
      config.x0_left =
          (depth_ae_roi_left_ > width_[DEPTH] - 1) ? width_[DEPTH] - 1 : config.x0_left;
    }
    if (depth_ae_roi_top_ != -1) {
      config.y0_top = (depth_ae_roi_top_ < 0) ? 0 : depth_ae_roi_top_;
      config.y0_top = (depth_ae_roi_top_ > height_[DEPTH] - 1) ? height_[DEPTH] - 1 : config.y0_top;
    }
    if (depth_ae_roi_right_ != -1) {
      config.x1_right = (depth_ae_roi_right_ < 0) ? 0 : depth_ae_roi_right_;
      config.x1_right =
          (depth_ae_roi_right_ > width_[DEPTH] - 1) ? width_[DEPTH] - 1 : config.x1_right;
    }
    if (depth_ae_roi_bottom_ != -1) {
      config.y1_bottom = (depth_ae_roi_bottom_ < 0) ? 0 : depth_ae_roi_bottom_;
      config.y1_bottom =
          (depth_ae_roi_bottom_ > height_[DEPTH] - 1) ? height_[DEPTH] - 1 : config.y1_bottom;
    }
    device_->setStructuredData(OB_STRUCT_DEPTH_AE_ROI, reinterpret_cast<const uint8_t *>(&config),
                               sizeof(config));
    device_->getStructuredData(OB_STRUCT_DEPTH_AE_ROI, reinterpret_cast<uint8_t *>(&config),
                               &data_size);
    RCLCPP_INFO_STREAM(logger_, "Set depth AE ROI to " << config.x0_left << ", " << config.x1_right
                                                       << ", " << config.y0_top << ", "
                                                       << config.y1_bottom);
  }
  depth_roi_has_run = true;
}

void OBCameraNode::setColorAutoExposureROI() {
  static bool color_roi_has_run = false;
  if (color_roi_has_run) {
    return;
  }
  if (isGemini305SeriesPID(pid_) && ae_reference_stream_ == "depth") {
    RCLCPP_WARN_STREAM(logger_, "Skip setting color AE ROI because AE Reference Stream is depth");
    color_roi_has_run = true;
    return;
  }
  if (device_->isPropertySupported(OB_STRUCT_COLOR_AE_ROI, OB_PERMISSION_READ_WRITE)) {
    auto config = OBRegionOfInterest();
    uint32_t data_size = sizeof(config);
    device_->getStructuredData(OB_STRUCT_COLOR_AE_ROI, reinterpret_cast<uint8_t *>(&config),
                               &data_size);
    if (color_ae_roi_left_ != -1) {
      config.x0_left = (color_ae_roi_left_ < 0) ? 0 : color_ae_roi_left_;
      config.x0_left =
          (color_ae_roi_left_ > width_[COLOR] - 1) ? width_[COLOR] - 1 : config.x0_left;
    }
    if (color_ae_roi_top_ != -1) {
      config.y0_top = (color_ae_roi_top_ < 0) ? 0 : color_ae_roi_top_;
      config.y0_top = (color_ae_roi_top_ > height_[COLOR] - 1) ? height_[COLOR] - 1 : config.y0_top;
    }
    if (color_ae_roi_right_ != -1) {
      config.x1_right = (color_ae_roi_right_ < 0) ? 0 : color_ae_roi_right_;
      config.x1_right =
          (color_ae_roi_right_ > width_[COLOR] - 1) ? width_[COLOR] - 1 : config.x1_right;
    }
    if (color_ae_roi_bottom_ != -1) {
      config.y1_bottom = (color_ae_roi_bottom_ < 0) ? 0 : color_ae_roi_bottom_;
      config.y1_bottom =
          (color_ae_roi_bottom_ > height_[COLOR] - 1) ? height_[COLOR] - 1 : config.y1_bottom;
    }
    device_->setStructuredData(OB_STRUCT_COLOR_AE_ROI, reinterpret_cast<const uint8_t *>(&config),
                               sizeof(config));
    device_->getStructuredData(OB_STRUCT_COLOR_AE_ROI, reinterpret_cast<uint8_t *>(&config),
                               &data_size);
    RCLCPP_INFO_STREAM(logger_, "Set color AE ROI to " << config.x0_left << ", " << config.x1_right
                                                       << ", " << config.y0_top << ", "
                                                       << config.y1_bottom);
  }
  color_roi_has_run = true;
}

uint64_t OBCameraNode::getFrameTimestampUs(const std::shared_ptr<ob::Frame> &frame) {
  if (frame == nullptr) {
    RCLCPP_WARN(logger_, "getFrameTimestampUs: frame is nullptr, return 0");
    return 0;
  }
  if (time_domain_ == "device") {
    return frame->getTimeStampUs();
  } else if (time_domain_ == "global") {
    return frame->getGlobalTimeStampUs();
  } else {
    return frame->getSystemTimeStampUs();
  }
}
void OBCameraNode::onNewFrameSetCallback(std::shared_ptr<ob::FrameSet> frame_set) {
  if (!is_running_.load()) {
    return;
  }
  if (!is_camera_node_initialized_.load()) {
    return;
  }
  if (frame_set == nullptr) {
    return;
  }
  if ((frame_timestamp_csv_logger_ && frame_timestamp_csv_logger_->enabled()) ||
      (color_timestamp_csv_logger_ && color_timestamp_csv_logger_->enabled()) ||
      (depth_timestamp_csv_logger_ && depth_timestamp_csv_logger_->enabled())) {
    const auto frame_set_arrival_system_us = getSystemNowUs();
    const auto frame_set_arrival_steady_us = getSteadyNowUs();
    auto final_color_frame = frame_set->getFrame(OB_FRAME_COLOR);
    auto final_depth_frame = frame_set->getFrame(OB_FRAME_DEPTH);
    const bool track_color = enable_stream_[COLOR] && static_cast<bool>(final_color_frame);
    const bool track_depth = enable_stream_[DEPTH] && static_cast<bool>(final_depth_frame);
    const bool color_publish_expected = track_color;
    const bool depth_publish_expected = track_depth;

    if (frame_timestamp_csv_logger_) {
      frame_timestamp_csv_logger_->recordFrameSet(
          final_color_frame, final_depth_frame, frame_set_arrival_system_us,
          frame_set_arrival_steady_us, track_color, track_depth, color_publish_expected,
          depth_publish_expected);
    } else {
      if (track_color && color_timestamp_csv_logger_) {
        color_timestamp_csv_logger_->recordStandaloneFrameArrival(
            OB_STREAM_COLOR, final_color_frame, frame_set_arrival_system_us,
            frame_set_arrival_steady_us, color_publish_expected);
      }
      if (track_depth && depth_timestamp_csv_logger_) {
        depth_timestamp_csv_logger_->recordStandaloneFrameArrival(
            OB_STREAM_DEPTH, final_depth_frame, frame_set_arrival_system_us,
            frame_set_arrival_steady_us, depth_publish_expected);
      }
    }
  }

  try {
    if (!tf_published_) {
      publishStaticTransforms();
      tf_published_ = true;
    }
    auto device_info = device_->getDeviceInfo();
    CHECK_NOTNULL(device_info);
    auto depth_frame = frame_set->getFrame(OB_FRAME_DEPTH);
    auto color_frame = frame_set->getFrame(OB_FRAME_COLOR);
    auto left_ir_frame = frame_set->getFrame(OB_FRAME_IR_LEFT);
    auto right_ir_frame = frame_set->getFrame(OB_FRAME_IR_RIGHT);
    auto left_color_frame = frame_set->getFrame(OB_FRAME_COLOR_LEFT);
    auto right_color_frame = frame_set->getFrame(OB_FRAME_COLOR_RIGHT);
    auto ir_frame = frame_set->getFrame(OB_FRAME_IR);
    auto depth_frame_for_hw_d2c_undistortion = depth_frame;
    if (depth_frame) {
      setDisparitySearchOffset();
      setDepthAutoExposureROI();
      depth_frame = processDepthFrameFilter(depth_frame);
      if (depth_frame) {
        frame_set->pushFrame(depth_frame);
        fps_counter_depth_->tick();
      }
    }
    if (shouldUseHwD2CColorUndistortion() && color_frame) {
      applyHwD2CColorUndistortion(frame_set, depth_frame_for_hw_d2c_undistortion);
      depth_frame = frame_set->getFrame(OB_FRAME_DEPTH);
      color_frame = frame_set->getFrame(OB_FRAME_COLOR);
    }
    if (color_frame) {
      setColorAutoExposureROI();
      color_frame = processColorFrameFilter(color_frame);
      frame_set->pushFrame(color_frame);
      fps_counter_color_->tick();
    }
    if (left_color_frame) {
      setColorAutoExposureROI();
      left_color_frame = processColorFrameFilter(left_color_frame);
      frame_set->pushFrame(left_color_frame);
    }
    if (right_color_frame) {
      right_color_frame = processColorFrameFilter(right_color_frame);
      frame_set->pushFrame(right_color_frame);
    }
    if (left_ir_frame) {
      left_ir_frame = processLeftIrFrameFilter(left_ir_frame);
      frame_set->pushFrame(left_ir_frame);
      fps_counter_left_ir_->tick();
    }
    if (right_ir_frame) {
      right_ir_frame = processRightIrFrameFilter(right_ir_frame);
      frame_set->pushFrame(right_ir_frame);
      fps_counter_right_ir_->tick();
    }
    if (ir_frame) {
      ir_frame = processIrFrameFilter(ir_frame);
      if (ir_frame) {
        frame_set->pushFrame(ir_frame);
      }
    }
    if (depth_registration_ && align_filter_ && depth_frame) {
      publishRawDepthImage(depth_frame);
      auto target_frame_type = STREAM_TYPE_TO_FRAME_TYPE.at(align_target_stream_);
      if (!frame_set->getFrame(target_frame_type) || !color_frame) {
        RCLCPP_DEBUG_STREAM(
            logger_, "Depth registration requires depth and color frames, skip software alignment");
      } else {
        auto align_color_frame = color_frame;
        if (align_target_stream_ == OB_STREAM_DEPTH) {
          ob::FormatConvertFilter align_color_format_convert_filter;
          switch (color_frame->getFormat()) {
            case OB_FORMAT_YUYV:
              align_color_format_convert_filter.setFormatConvertType(FORMAT_YUYV_TO_RGB888);
              align_color_frame = align_color_format_convert_filter.process(color_frame);
              break;
            case OB_FORMAT_UYVY:
              align_color_format_convert_filter.setFormatConvertType(FORMAT_UYVY_TO_RGB888);
              align_color_frame = align_color_format_convert_filter.process(color_frame);
              break;
            case OB_FORMAT_MJPG:
              align_color_format_convert_filter.setFormatConvertType(FORMAT_MJPEG_TO_RGB888);
              align_color_frame = align_color_format_convert_filter.process(color_frame);
              break;
            default:
              break;
          }
          if (!align_color_frame) {
            RCLCPP_ERROR_STREAM(logger_, "Failed to convert color frame for C2D alignment");
          } else if (align_color_frame != color_frame) {
            color_frame = align_color_frame;
            frame_set->pushFrame(color_frame);
          }
        }
        if (align_color_frame) {
          if (auto new_frame = align_filter_->process(frame_set)) {
            auto new_frame_set = new_frame->as<ob::FrameSet>();
            CHECK_NOTNULL(new_frame_set.get());
            frame_set = new_frame_set;
            depth_frame = frame_set->getFrame(OB_FRAME_DEPTH);
            color_frame = frame_set->getFrame(OB_FRAME_COLOR);
          } else {
            RCLCPP_ERROR(logger_, "Failed to align depth frame to color frame");
            return;
          }
        }
      }
    } else {
      RCLCPP_DEBUG_ONCE(logger_,
                        "Depth registration is disabled or align filter is null or depth frame is "
                        "null or color frame is null");
    }

    if (enable_enhanced_depth_.load()) {
      frame_set = processEnhancedDepthFilter(frame_set);
      depth_frame = frame_set->getFrame(OB_FRAME_DEPTH);
      color_frame = frame_set->getFrame(OB_FRAME_COLOR);
    }

    // Refresh frame from current frameset before logging to reflect post-filter/alignment output.
    for (const auto &stream_index : IMAGE_STREAMS) {
      if (!enable_stream_[stream_index]) {
        continue;
      }
      auto frame_type = STREAM_TYPE_TO_FRAME_TYPE.at(stream_index.first);
      auto updated_frame = frame_set->getFrame(frame_type);
      if (!updated_frame || !updated_frame->is<ob::VideoFrame>()) {
        continue;
      }
      auto updated_video = updated_frame->as<ob::VideoFrame>();

      // For D2C, avoid logging an early unaligned depth frame before align target is available.
      if (stream_index == DEPTH && depth_registration_) {
        auto align_target_frame_type = STREAM_TYPE_TO_FRAME_TYPE.at(align_target_stream_);
        auto align_target_frame = frame_set->getFrame(align_target_frame_type);
        if (!align_target_frame || !align_target_frame->is<ob::VideoFrame>()) {
          continue;
        }
        auto target_video = align_target_frame->as<ob::VideoFrame>();
        if (updated_video->getWidth() != target_video->getWidth() ||
            updated_video->getHeight() != target_video->getHeight()) {
          continue;
        }
      }

      logFrameInfoOnce(stream_index, updated_video);
    }

    if (enable_stream_[COLOR] && color_frame) {
      std::unique_lock<std::mutex> lock(color_frame_queue_lock_);
      color_frame_queue_.push(frame_set);
      color_frame_queue_cv_.notify_all();
    } else {
      publishPointCloud(frame_set);
    }

    if (enable_stream_[COLOR_LEFT] && left_color_frame) {
      std::unique_lock<std::mutex> lock(left_color_frame_queue_lock_);
      left_color_frame_queue_.push(frame_set);
      left_color_frame_queue_cv_.notify_all();
    }
    if (enable_stream_[COLOR_RIGHT] && right_color_frame) {
      std::unique_lock<std::mutex> lock(right_color_frame_queue_lock_);
      right_color_frame_queue_.push(frame_set);
      right_color_frame_queue_cv_.notify_all();
    }

    for (const auto &stream_index : IMAGE_STREAMS) {
      if (enable_stream_[stream_index]) {
        auto frame_type = STREAM_TYPE_TO_FRAME_TYPE.at(stream_index.first);
        if (frame_type == OB_FRAME_COLOR || frame_type == OB_FRAME_COLOR_LEFT ||
            frame_type == OB_FRAME_COLOR_RIGHT) {
          continue;
        }

        auto frame = frame_set->getFrame(frame_type);
        if (frame == nullptr) {
          continue;
        }
        onNewFrameCallback(frame, stream_index);
      }
    }
  } catch (const ob::Error &e) {
    RCLCPP_ERROR_STREAM(
        logger_, "onNewFrameSetCallback error: " << orbbec_camera::formatObErrorWithStatus(e));
  } catch (const std::exception &e) {
    RCLCPP_ERROR_STREAM(logger_, "onNewFrameSetCallback error: " << e.what());
  } catch (...) {
    RCLCPP_ERROR_STREAM(logger_, "onNewFrameSetCallback error: unknown error");
  }
}

void OBCameraNode::logFrameInfoOnce(const stream_index_pair &stream_index,
                                    const std::shared_ptr<ob::VideoFrame> &video_frame) {
  if (!video_frame) {
    return;
  }

  {
    std::lock_guard<std::mutex> lock(frame_info_logged_mutex_);
    auto iter = frame_info_logged_.find(stream_index);
    if (iter != frame_info_logged_.end() && iter->second) {
      return;
    }
    frame_info_logged_[stream_index] = true;
  }

  RCLCPP_INFO_STREAM(logger_, stream_name_[stream_index]
                                  << " Frame - Width: " << video_frame->getWidth() << " Height: "
                                  << video_frame->getHeight() << " fps: " << fps_[stream_index]
                                  << " Format: " << video_frame->getFormat());
}

void OBCameraNode::onNewColorFrameCallback() {
  while (enable_stream_[COLOR] && rclcpp::ok() && is_running_.load() &&
         !stop_color_frame_threads_.load()) {
    std::unique_lock<std::mutex> lock(color_frame_queue_lock_);
    color_frame_queue_cv_.wait(lock, [this]() {
      return !color_frame_queue_.empty() || !(is_running_.load()) ||
             stop_color_frame_threads_.load();
    });

    if (!rclcpp::ok() || !is_running_.load() || stop_color_frame_threads_.load()) {
      break;
    }
    std::shared_ptr<ob::FrameSet> frameSet = color_frame_queue_.front();
    is_color_frame_decoded_ = decodeColorFrameToBuffer(frameSet->colorFrame(), rgb_buffer_);
    onNewFrameCallback(frameSet->colorFrame(), COLOR);
    publishPointCloud(frameSet);
    color_frame_queue_.pop();
  }

  RCLCPP_DEBUG_STREAM(logger_, "Color frame thread exited");
}

void OBCameraNode::onNewLeftColorFrameCallback() {
  while (enable_stream_[COLOR_LEFT] && rclcpp::ok() && is_running_.load() &&
         !stop_color_frame_threads_.load()) {
    std::unique_lock<std::mutex> lock(left_color_frame_queue_lock_);
    left_color_frame_queue_cv_.wait(lock, [this]() {
      return !left_color_frame_queue_.empty() || !(is_running_.load()) ||
             stop_color_frame_threads_.load();
    });

    if (!rclcpp::ok() || !is_running_.load() || stop_color_frame_threads_.load()) {
      break;
    }
    std::shared_ptr<ob::FrameSet> frameSet = left_color_frame_queue_.front();
    is_left_color_frame_decoded_ =
        decodeColorFrameToBuffer(frameSet->getFrame(OB_FRAME_COLOR_LEFT), rgb_buffer_left_);
    onNewFrameCallback(frameSet->getFrame(OB_FRAME_COLOR_LEFT), COLOR_LEFT);
    left_color_frame_queue_.pop();
  }
  RCLCPP_DEBUG_STREAM(logger_, "Left color frame thread exited");
}

void OBCameraNode::onNewRightColorFrameCallback() {
  while (enable_stream_[COLOR_RIGHT] && rclcpp::ok() && is_running_.load() &&
         !stop_color_frame_threads_.load()) {
    std::unique_lock<std::mutex> lock(right_color_frame_queue_lock_);
    right_color_frame_queue_cv_.wait(lock, [this]() {
      return !right_color_frame_queue_.empty() || !(is_running_.load()) ||
             stop_color_frame_threads_.load();
    });

    if (!rclcpp::ok() || !is_running_.load() || stop_color_frame_threads_.load()) {
      break;
    }
    std::shared_ptr<ob::FrameSet> frameSet = right_color_frame_queue_.front();
    is_right_color_frame_decoded_ =
        decodeColorFrameToBuffer(frameSet->getFrame(OB_FRAME_COLOR_RIGHT), rgb_buffer_right_);
    onNewFrameCallback(frameSet->getFrame(OB_FRAME_COLOR_RIGHT), COLOR_RIGHT);
    right_color_frame_queue_.pop();
  }
  RCLCPP_DEBUG_STREAM(logger_, "Right color frame thread exited");
}

std::shared_ptr<ob::Frame> OBCameraNode::softwareDecodeColorFrame(
    const std::shared_ptr<ob::Frame> &frame, const stream_index_pair &stream_index) {
  if (frame == nullptr) {
    return nullptr;
  }
  if (frame->getFormat() == OB_FORMAT_RGB || frame->getFormat() == OB_FORMAT_BGR) {
    return frame;
  }
  if (frame->getFormat() == OB_FORMAT_RGBA || frame->getFormat() == OB_FORMAT_BGRA) {
    return frame;
  }
  if (frame->getFormat() == OB_FORMAT_Y16 || frame->getFormat() == OB_FORMAT_Y8) {
    return frame;
  }

  ob::FormatConvertFilter *filter = &format_convert_filter_;
  if (stream_index == COLOR_LEFT) {
    filter = &format_convert_filter_left_;
  } else if (stream_index == COLOR_RIGHT) {
    filter = &format_convert_filter_right_;
  }

  if (!setupFormatConvertType(frame->getFormat(), *filter)) {
    RCLCPP_ERROR(logger_, "Unsupported color format: %d", frame->getFormat());
    return nullptr;
  }

  std::shared_ptr<ob::Frame> color_frame;
  try {
    color_frame = filter->process(frame);
  } catch (const ob::Error &e) {
    RCLCPP_ERROR_STREAM(logger_,
                        "Format convert failed: " << orbbec_camera::formatObErrorWithStatus(e));
    return nullptr;
  } catch (const std::exception &e) {
    RCLCPP_ERROR_STREAM(logger_, "Format convert failed: " << e.what());
    return nullptr;
  } catch (...) {
    RCLCPP_ERROR(logger_, "Format convert failed: unknown error");
    return nullptr;
  }
  if (color_frame == nullptr) {
    RCLCPP_ERROR_SKIPFIRST_THROTTLE(logger_, *(node_->get_clock()), 1000,
                                    "Failed to convert frame to RGB format");
    return nullptr;
  }
  return color_frame;
}

bool OBCameraNode::isColorFrameDecodeRequired(const std::shared_ptr<ob::Frame> &frame) const {
  if (frame == nullptr) {
    return false;
  }
  const auto format = frame->getFormat();
  if (format == OB_FORMAT_RGB || format == OB_FORMAT_BGR || format == OB_FORMAT_RGB888 ||
      format == OB_FORMAT_RGBA || format == OB_FORMAT_BGRA || format == OB_FORMAT_Y16 ||
      format == OB_FORMAT_Y8) {
    return false;
  }
  return true;
}

bool OBCameraNode::decodeColorFrameToBuffer(const std::shared_ptr<ob::Frame> &frame,
                                            uint8_t *buffer) {
  if (frame == nullptr) {
    return false;
  }
  if (!buffer) {
    return false;
  }

  if (!isColorFrameDecodeRequired(frame)) {
    return true;
  }

  stream_index_pair stream_index = COLOR;
  switch (frame->getType()) {
    case OB_FRAME_COLOR:
      stream_index = COLOR;
      break;
    case OB_FRAME_COLOR_LEFT:
      stream_index = COLOR_LEFT;
      break;
    case OB_FRAME_COLOR_RIGHT:
      stream_index = COLOR_RIGHT;
      break;
    default:
      stream_index = COLOR;
      break;
  }

  bool has_subscriber = false;
  if (image_publishers_.count(stream_index) && image_publishers_.at(stream_index)) {
    has_subscriber = image_publishers_.at(stream_index)->get_subscription_count() > 0;
  }
  if (save_images_[stream_index]) {
    has_subscriber = true;
  }

  if (frame->getType() == OB_FRAME_COLOR && enable_colored_point_cloud_ &&
      depth_registration_cloud_pub_ &&
      depth_registration_cloud_pub_->get_subscription_count() > 0) {
    has_subscriber = true;
  }

  if (!has_subscriber) {
    return false;
  }

  std::shared_ptr<JPEGDecoder> decoder;
  if (stream_index == COLOR_LEFT) {
    decoder = jpeg_decoder_left_;
  } else if (stream_index == COLOR_RIGHT) {
    decoder = jpeg_decoder_right_;
  } else {
    decoder = jpeg_decoder_;
  }
  bool is_decoded = false;
  if (!frame) {
    return false;
  }

#if defined(USE_RK_HW_DECODER) || defined(USE_NV_HW_DECODER)
  if (frame && frame->getFormat() != OB_FORMAT_RGB888) {
    if (frame->getFormat() == OB_FORMAT_MJPG && decoder) {
      CHECK_NOTNULL(decoder.get());
      CHECK_NOTNULL(buffer);
      auto video_frame = frame->as<ob::ColorFrame>();
      bool ret = false;
      if (video_frame && width_.count(stream_index) && height_.count(stream_index) &&
          static_cast<int>(video_frame->getWidth()) == width_[stream_index] &&
          static_cast<int>(video_frame->getHeight()) == height_[stream_index]) {
        ret = decoder->decode(video_frame, buffer);
      }
      if (!ret) {
        RCLCPP_ERROR_STREAM(logger_, "Decode frame failed");
        is_decoded = false;

      } else {
        is_decoded = true;
      }
    }
  }
#endif
  if (!is_decoded) {
    auto video_frame = softwareDecodeColorFrame(frame, stream_index);
    if (!video_frame) {
      RCLCPP_ERROR_STREAM(logger_, "Failed to convert frame to video frame");
      return false;
    }
    uint8_t **target_buffer = nullptr;
    size_t *target_buffer_size = nullptr;
    if (stream_index == COLOR_LEFT) {
      target_buffer = &rgb_buffer_left_;
      target_buffer_size = &rgb_buffer_left_size_;
    } else if (stream_index == COLOR_RIGHT) {
      target_buffer = &rgb_buffer_right_;
      target_buffer_size = &rgb_buffer_right_size_;
    } else {
      target_buffer = &rgb_buffer_;
      target_buffer_size = &rgb_buffer_size_;
    }
    if (video_frame->getDataSize() > *target_buffer_size) {
      delete[] (*target_buffer);
      *target_buffer_size = video_frame->getDataSize();
      *target_buffer = new uint8_t[*target_buffer_size];
      buffer = *target_buffer;
    }
    CHECK_NOTNULL(buffer);
    memcpy(buffer, video_frame->getData(), video_frame->getDataSize());
    return true;
  }
  return true;
}

std::shared_ptr<ob::Frame> OBCameraNode::decodeIRMJPGFrame(
    const std::shared_ptr<ob::Frame> &frame) {
  if (frame == nullptr) {
    return nullptr;
  }
  if (frame->getFormat() == OB_FORMAT_MJPEG &&
      (frame->getType() == OB_FRAME_IR || frame->getType() == OB_FRAME_IR_LEFT ||
       frame->getType() == OB_FRAME_IR_RIGHT)) {
    auto video_frame = frame->as<ob::IRFrame>();

    cv::Mat mjpgMat(1, video_frame->getDataSize(), CV_8UC1, video_frame->getData());
    cv::Mat irRawMat = cv::imdecode(mjpgMat, cv::IMREAD_GRAYSCALE);

    std::shared_ptr<ob::Frame> irFrame =
        ob::FrameFactory::createVideoFrame(video_frame->getType(), video_frame->getFormat(),
                                           video_frame->getWidth(), video_frame->getHeight(), 0);

    uint32_t buffer_size = irRawMat.rows * irRawMat.cols * irRawMat.channels();

    if (buffer_size > irFrame->getDataSize()) {
      RCLCPP_ERROR_STREAM(logger_,
                          "Insufficient buffer size allocation,failed to decode ir mjpg frame!");
      return nullptr;
    }

    memcpy(irFrame->getData(), irRawMat.data, buffer_size);
    ob::FrameHelper::setFrameDeviceTimestamp(irFrame, video_frame->getTimeStampUs());
    ob::FrameHelper::setFrameDeviceTimestampUs(irFrame, video_frame->getTimeStampUs());
    ob::FrameHelper::setFrameSystemTimestamp(irFrame, video_frame->getSystemTimeStampUs());
    return irFrame;
  }

  return frame;
}

void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame> &frame,
                                      const stream_index_pair &stream_index) {
  if (frame == nullptr) {
    return;
  }
  FrameTimestampCsvLogger *timestamp_csv_logger = nullptr;
  if (stream_index == COLOR) {
    timestamp_csv_logger = frame_timestamp_csv_logger_ ? frame_timestamp_csv_logger_.get()
                                                       : color_timestamp_csv_logger_.get();
  } else if (stream_index == DEPTH) {
    timestamp_csv_logger = frame_timestamp_csv_logger_ ? frame_timestamp_csv_logger_.get()
                                                       : depth_timestamp_csv_logger_.get();
  }
  const auto record_image_publish_skipped = [&]() {
    if (timestamp_csv_logger && timestamp_csv_logger->enabled()) {
      timestamp_csv_logger->recordImagePublishSkipped(stream_index.first, frame);
    }
  };
  CHECK_NOTNULL(image_publishers_[stream_index]);
  const bool has_raw_image_subscriber =
      image_publishers_[stream_index]->get_subscription_count() > 0;
  const bool has_compressed_image_subscriber = hasCompressedImageSubscriber(stream_index);
  bool has_subscriber =
      has_raw_image_subscriber || has_compressed_image_subscriber || save_images_[stream_index];
  has_subscriber =
      has_subscriber || camera_info_publishers_[stream_index]->get_subscription_count() > 0;
  has_subscriber =
      has_subscriber || (metadata_publishers_.count(stream_index) &&
                         metadata_publishers_[stream_index]->get_subscription_count() > 0);
  if (!has_subscriber) {
    record_image_publish_skipped();
    return;
  }
  std::shared_ptr<ob::VideoFrame> video_frame;
  if (frame->getType() == OB_FRAME_COLOR || frame->getType() == OB_FRAME_COLOR_LEFT ||
      frame->getType() == OB_FRAME_COLOR_RIGHT) {
    video_frame = frame->as<ob::ColorFrame>();
  } else if (frame->getType() == OB_FRAME_DEPTH) {
    video_frame = frame->as<ob::DepthFrame>();
  } else if (frame->getType() == OB_FRAME_IR || frame->getType() == OB_FRAME_IR_LEFT ||
             frame->getType() == OB_FRAME_IR_RIGHT) {
    video_frame = frame->as<ob::IRFrame>();

    // interleave filter speckle or flood ir
    if (interleave_frame_enable_ && interleave_skip_enable_) {
      RCLCPP_DEBUG(logger_, "interleave filter skip interleave_skip_index_: %d",
                   interleave_skip_index_);
      if (video_frame->getMetadataValue(OB_FRAME_METADATA_TYPE_HDR_SEQUENCE_INDEX) ==
          interleave_skip_index_) {
        RCLCPP_DEBUG(logger_, "interleave filter skip frame type: %d", frame->getType());
        return;
      }
    }
  } else {
    RCLCPP_ERROR(logger_, "Unsupported frame type: %d", frame->getType());
    return;
  }
  if (!video_frame) {
    RCLCPP_ERROR(logger_, "Failed to convert frame to video frame");
    record_image_publish_skipped();
    return;
  }
  int width = static_cast<int>(video_frame->getWidth());
  int height = static_cast<int>(video_frame->getHeight());
  auto frame_timestamp = getFrameTimestampUs(frame);
  auto timestamp = fromUsToROSTime(frame_timestamp);
  if (!device_) {
    RCLCPP_ERROR_STREAM(logger_, "device is null in onNewFrameCallback");
    record_image_publish_skipped();
    return;
  }
  auto device_info = device_->getDeviceInfo();
  if (!device_info || !device_info.get()) {
    RCLCPP_ERROR_STREAM(logger_, "device_info is null in onNewFrameCallback");
    record_image_publish_skipped();
    return;
  }
  OBCameraIntrinsic intrinsic;
  OBCameraDistortion distortion;
  auto stream_profile = frame->getStreamProfile();
  CHECK_NOTNULL(stream_profile);
  auto video_stream_profile = stream_profile->as<ob::VideoStreamProfile>();
  CHECK_NOTNULL(video_stream_profile);
  intrinsic = video_stream_profile->getIntrinsic();
  distortion = video_stream_profile->getDistortion();
  if (pid_ == DABAI_MAX_PID) {
    auto camera_params = pipeline_->getCameraParam();
    // use color param
    intrinsic = camera_params.rgbIntrinsic;
    distortion = camera_params.rgbDistortion;
  }
  std::string frame_id = optical_frame_id_[stream_index];
  if (depth_registration_ && stream_index == DEPTH) {
    frame_id = depth_aligned_frame_id_[stream_index];
  }
  sensor_msgs::msg::CameraInfo camera_info{};
  if (color_info_manager_ && color_info_manager_->isCalibrated() && stream_index == COLOR) {
    camera_info = color_info_manager_->getCameraInfo();
    camera_info.header.stamp = timestamp;
    camera_info.header.frame_id = frame_id;
    camera_info.width = width;
    camera_info.height = height;
  } else if (ir_info_manager_ && ir_info_manager_->isCalibrated() &&
             (stream_index == INFRA1 || stream_index == INFRA2 || stream_index == DEPTH)) {
    camera_info = ir_info_manager_->getCameraInfo();
    camera_info.header.stamp = timestamp;
    camera_info.header.frame_id = frame_id;
    camera_info.width = width;
    camera_info.height = height;
  } else {
    camera_info = convertToCameraInfo(intrinsic, distortion, width);
    camera_info.header.stamp = timestamp;
    camera_info.header.frame_id = frame_id;
    camera_info.width = width;
    camera_info.height = height;
  }
  auto &image = images_[stream_index];
  if (frame->getType() == OB_FRAME_IR_RIGHT && enable_stream_[INFRA1]) {
    auto stream_profile = frame->getStreamProfile();
    CHECK_NOTNULL(stream_profile);
    auto video_stream_profile = stream_profile->as<ob::VideoStreamProfile>();
    CHECK_NOTNULL(video_stream_profile);
    auto left_video_profile = stream_profile_[INFRA1]->as<ob::VideoStreamProfile>();
    CHECK_NOTNULL(left_video_profile);
    auto ex = video_stream_profile->getExtrinsicTo(left_video_profile);
    float fx = camera_info.k.at(0);
    float fy = camera_info.k.at(4);
    camera_info.p.at(3) = -fx * ex.trans[0] / 1000.0 + 0.0;
    camera_info.p.at(7) = -fy * ex.trans[1] / 1000.0 + 0.0;
  }
  if ((stream_index == COLOR || stream_index == COLOR_LEFT || stream_index == COLOR_RIGHT) &&
      frame->getFormat() == OB_FORMAT_MJPG && has_compressed_image_subscriber) {
    if (!has_raw_image_subscriber && stream_index == COLOR && timestamp_csv_logger &&
        timestamp_csv_logger->enabled()) {
      timestamp_csv_logger->recordPreImagePublish(stream_index.first, frame, getSystemNowUs(),
                                                  getSteadyNowUs());
    }
    publishCompressedColorImage(frame, stream_index, timestamp, frame_id);
    if (!has_raw_image_subscriber && stream_index == COLOR) {
      fps_delay_status_color_->tick(frame_timestamp);
    }
  }

  if (!has_raw_image_subscriber && !save_images_[stream_index]) {
    CHECK(camera_info_publishers_.count(stream_index) > 0);
    camera_info_publishers_[stream_index]->publish(camera_info);
    publishMetadata(frame, stream_index, camera_info.header);
    record_image_publish_skipped();
    return;
  }

  CHECK_NOTNULL(image_publishers_[stream_index]);
  if (image.empty() || image.cols != width || image.rows != height) {
    image.create(height, width, image_format_[stream_index]);
  }
  if (isColorFrameDecodeRequired(frame) &&
      (has_raw_image_subscriber || save_images_[stream_index])) {
    if (frame->getType() == OB_FRAME_COLOR && !is_color_frame_decoded_) {
      RCLCPP_ERROR(logger_, "color frame is not decoded");
      record_image_publish_skipped();
      return;
    }
    if (frame->getType() == OB_FRAME_COLOR_LEFT && !is_left_color_frame_decoded_) {
      RCLCPP_ERROR(logger_, "left color frame is not decoded");
      return;
    }
    if (frame->getType() == OB_FRAME_COLOR_RIGHT && !is_right_color_frame_decoded_) {
      RCLCPP_ERROR(logger_, "right color frame is not decoded");
      return;
    }
    if (frame->getType() == OB_FRAME_COLOR) {
      memcpy(image.data, rgb_buffer_, video_frame->getWidth() * video_frame->getHeight() * 3);
    } else if (frame->getType() == OB_FRAME_COLOR_LEFT) {
      memcpy(image.data, rgb_buffer_left_, video_frame->getWidth() * video_frame->getHeight() * 3);
    } else if (frame->getType() == OB_FRAME_COLOR_RIGHT) {
      memcpy(image.data, rgb_buffer_right_, video_frame->getWidth() * video_frame->getHeight() * 3);
    }
  } else {
    memcpy(image.data, video_frame->getData(), video_frame->getDataSize());
  }

  CHECK(camera_info_publishers_.count(stream_index) > 0);
  camera_info_publishers_[stream_index]->publish(camera_info);
  publishMetadata(frame, stream_index, camera_info.header);

  if (!has_raw_image_subscriber && !save_images_[stream_index]) {
    return;
  }
  if (stream_index == DEPTH) {
    auto depth_scale = video_frame->as<ob::DepthFrame>()->getValueScale();
    image = image * depth_scale;
  }
  CHECK(image_publishers_.count(stream_index) > 0);
  if (has_raw_image_subscriber || save_images_[stream_index]) {
    sensor_msgs::msg::Image::UniquePtr image_msg(new sensor_msgs::msg::Image());
    cv_bridge::CvImage(std_msgs::msg::Header(), encoding_[stream_index], image)
        .toImageMsg(*image_msg);
    CHECK_NOTNULL(image_msg.get());
    image_msg->header.stamp = timestamp;
    image_msg->is_bigendian = false;
    image_msg->step = width * unit_step_size_[stream_index];
    image_msg->header.frame_id = frame_id;
    saveImageToFile(stream_index, image, *image_msg);
    if (!has_raw_image_subscriber) {
      record_image_publish_skipped();
      return;
    }
    if (timestamp_csv_logger && timestamp_csv_logger->enabled()) {
      timestamp_csv_logger->recordPreImagePublish(stream_index.first, frame, getSystemNowUs(),
                                                  getSteadyNowUs());
    }
    if (stream_index == COLOR) {
      fps_delay_status_color_->tick(frame_timestamp);
    } else if (stream_index == DEPTH) {
      fps_delay_status_depth_->tick(frame_timestamp);
    }
    image_publishers_[stream_index]->publish(std::move(image_msg));
  }
}

bool OBCameraNode::hasCompressedImageSubscriber(const stream_index_pair &stream_index) const {
  auto it = compressed_image_publishers_.find(stream_index);
  return it != compressed_image_publishers_.end() && it->second &&
         it->second->get_subscription_count() > 0;
}

void OBCameraNode::publishCompressedColorImage(const std::shared_ptr<ob::Frame> &frame,
                                               const stream_index_pair &stream_index,
                                               const rclcpp::Time &timestamp,
                                               const std::string &frame_id) {
  auto it = compressed_image_publishers_.find(stream_index);
  if (it == compressed_image_publishers_.end() || !it->second) {
    return;
  }
  sensor_msgs::msg::CompressedImage msg;
  msg.header.stamp = timestamp;
  msg.header.frame_id = frame_id;
  msg.format = encoding_[stream_index] + "; jpeg compressed " + encoding_[stream_index];
  const auto *data = static_cast<const uint8_t *>(frame->getData());
  msg.data.assign(data, data + frame->getDataSize());
  it->second->publish(std::move(msg));
}

void OBCameraNode::publishMetadata(const std::shared_ptr<ob::Frame> &frame,
                                   const stream_index_pair &stream_index,
                                   const std_msgs::msg::Header &header) {
  if (metadata_publishers_.count(stream_index) == 0) {
    return;
  }
  auto metadata_publisher = metadata_publishers_[stream_index];
  if (metadata_publisher->get_subscription_count() == 0) {
    return;
  }
  orbbec_camera_msgs::msg::Metadata metadata_msg;
  metadata_msg.header = header;
  nlohmann::json json_data;

  for (int i = 0; i < OB_FRAME_METADATA_TYPE_COUNT; i++) {
    auto meta_data_type = static_cast<OBFrameMetadataType>(i);
    std::string field_name = metaDataTypeToString(meta_data_type);
    if (!frame->hasMetadata(meta_data_type)) {
      continue;
    }
    int64_t value = frame->getMetadataValue(meta_data_type);
    json_data[field_name] = value;
  }
  metadata_msg.json_data = json_data.dump(2);
  metadata_publisher->publish(metadata_msg);
}

void OBCameraNode::saveImageToFile(const stream_index_pair &stream_index, const cv::Mat &image,
                                   const sensor_msgs::msg::Image &image_msg) {
  if (save_images_[stream_index]) {
    auto now = std::chrono::system_clock::now();
    auto in_time_t = std::chrono::system_clock::to_time_t(now);
    auto us =
        std::chrono::duration_cast<std::chrono::microseconds>(now.time_since_epoch()) % 1000000;

    std::stringstream ss;
    ss << std::put_time(std::localtime(&in_time_t), "%Y%m%d_%H%M%S");
    ss << "_" << std::setw(6) << std::setfill('0') << us.count();
    auto current_path = std::filesystem::current_path().string();
    auto fps = fps_[stream_index];
    int index = save_images_count_[stream_index];
    std::string file_suffix = stream_index == COLOR ? ".png" : ".raw";
    std::string filename = current_path + "/image/" + stream_name_[stream_index] + "_" +
                           std::to_string(image_msg.width) + "x" +
                           std::to_string(image_msg.height) + "_" + std::to_string(fps) + "hz_" +
                           ss.str() + "_" + std::to_string(index) + file_suffix;
    if (!std::filesystem::exists(current_path + "/image")) {
      std::filesystem::create_directory(current_path + "/image");
    }
    RCLCPP_INFO_STREAM(logger_, "Saving image to " << filename);
    if (stream_index.first == OB_STREAM_COLOR) {
      auto image_to_save =
          cv_bridge::toCvCopy(image_msg, sensor_msgs::image_encodings::BGR8)->image;
      cv::imwrite(filename, image_to_save);
    } else if (stream_index.first == OB_STREAM_IR || stream_index.first == OB_STREAM_IR_LEFT ||
               stream_index.first == OB_STREAM_IR_RIGHT || stream_index.first == OB_STREAM_DEPTH) {
      std::ofstream ofs(filename, std::ios::out | std::ios::binary);
      if (!ofs.is_open()) {
        RCLCPP_ERROR_STREAM(logger_, "Failed to open file: " << filename);
        return;
      }
      if (image.isContinuous()) {
        ofs.write(reinterpret_cast<const char *>(image.data), image.total() * image.elemSize());
      } else {
        int rows = image.rows;
        int cols = image.cols * image.channels();
        for (int r = 0; r < rows; ++r) {
          ofs.write(reinterpret_cast<const char *>(image.ptr<uchar>(r)), cols);
        }
      }
      ofs.close();
    } else {
      RCLCPP_ERROR_STREAM(logger_, "Unsupported stream type: " << stream_index.first);
    }
    if (++save_images_count_[stream_index] >= max_save_images_count_) {
      save_images_[stream_index] = false;
    }
  }
}

void OBCameraNode::onNewIMUFrameSyncOutputCallback(const std::shared_ptr<ob::Frame> &accelframe,
                                                   const std::shared_ptr<ob::Frame> &gryoframe,
                                                   int64_t arrival_system_us) {
  if (!is_camera_node_initialized_.load() || !rclcpp::ok()) {
    return;
  }
  const auto record_timestamps = [&](std::optional<int64_t> publish_system_us) {
    if (arrival_system_us != 0 && imu_timestamp_csv_logger_ &&
        imu_timestamp_csv_logger_->enabled()) {
      imu_timestamp_csv_logger_->recordFrameSet(accelframe, gryoframe, arrival_system_us,
                                                publish_system_us);
    }
  };
  if (!imu_gyro_accel_publisher_) {
    record_timestamps(std::nullopt);
    RCLCPP_ERROR_STREAM(logger_, "stream Accel Gryo publisher not initialized");
    return;
  }
  bool has_subscriber = imu_gyro_accel_publisher_->get_subscription_count() > 0;
  has_subscriber = has_subscriber || imu_info_publishers_[GYRO]->get_subscription_count() > 0;
  has_subscriber = has_subscriber || imu_info_publishers_[ACCEL]->get_subscription_count() > 0;
  if (!has_subscriber) {
    record_timestamps(std::nullopt);
    return;
  }
  auto imu_msg = sensor_msgs::msg::Imu();
  setDefaultIMUMessage(imu_msg);

  imu_msg.header.frame_id = optical_frame_id_[GYRO];
  auto frame_timestamp = getFrameTimestampUs(accelframe);
  auto timestamp = fromUsToROSTime(frame_timestamp);
  imu_msg.header.stamp = timestamp;

  auto gyro_info = createIMUInfo(GYRO);
  gyro_info.header = imu_msg.header;
  imu_info_publishers_[GYRO]->publish(gyro_info);

  auto accel_info = createIMUInfo(ACCEL);
  imu_msg.header.frame_id = optical_frame_id_[ACCEL];
  accel_info.header = imu_msg.header;
  imu_info_publishers_[ACCEL]->publish(accel_info);

  imu_msg.header.frame_id = accel_gyro_frame_id_;

  auto gyro_frame = gryoframe->as<ob::GyroFrame>();
  auto gyroData = gyro_frame->getValue();
  imu_msg.angular_velocity.x = gyroData.x - gyro_info.bias[0];
  imu_msg.angular_velocity.y = gyroData.y - gyro_info.bias[1];
  imu_msg.angular_velocity.z = gyroData.z - gyro_info.bias[2];

  auto accel_frame = accelframe->as<ob::AccelFrame>();
  auto accelData = accel_frame->getValue();
  imu_msg.linear_acceleration.x = accelData.x - accel_info.bias[0];
  imu_msg.linear_acceleration.y = accelData.y - accel_info.bias[1];
  imu_msg.linear_acceleration.z = accelData.z - accel_info.bias[2];

  const auto publish_system_us = getSystemNowUs();
  imu_gyro_accel_publisher_->publish(imu_msg);
  record_timestamps(publish_system_us);
}

void OBCameraNode::onNewIMUFrameCallback(const std::shared_ptr<ob::Frame> &frame,
                                         const stream_index_pair &stream_index) {
  if (!is_camera_node_initialized_.load() || !rclcpp::ok()) {
    return;
  }
  auto *timestamp_csv_logger =
      stream_index == ACCEL ? accel_timestamp_csv_logger_.get() : gyro_timestamp_csv_logger_.get();
  const bool log_imu_timestamps = timestamp_csv_logger && timestamp_csv_logger->enabled();
  const auto arrival_system_us = log_imu_timestamps ? getSystemNowUs() : 0;
  const auto record_timestamps = [&](std::optional<int64_t> publish_system_us) {
    if (log_imu_timestamps) {
      timestamp_csv_logger->recordStandaloneFrame(stream_index.first, frame, arrival_system_us,
                                                  publish_system_us);
    }
  };
  if (!imu_publishers_.count(stream_index)) {
    record_timestamps(std::nullopt);
    RCLCPP_ERROR_STREAM(logger_,
                        "stream " << stream_name_[stream_index] << " publisher not initialized");
    return;
  }
  bool has_subscriber = imu_publishers_[stream_index]->get_subscription_count() > 0;
  has_subscriber =
      has_subscriber || imu_info_publishers_[stream_index]->get_subscription_count() > 0;
  if (!has_subscriber) {
    record_timestamps(std::nullopt);
    return;
  }
  auto imu_msg = sensor_msgs::msg::Imu();
  setDefaultIMUMessage(imu_msg);

  imu_msg.header.frame_id = optical_frame_id_[stream_index];
  auto timestamp = fromUsToROSTime(frame->getTimeStampUs());
  imu_msg.header.stamp = timestamp;

  auto imu_info = createIMUInfo(stream_index);
  imu_info.header = imu_msg.header;
  imu_info_publishers_[stream_index]->publish(imu_info);

  if (frame->getType() == OB_FRAME_GYRO) {
    auto gyro_frame = frame->as<ob::GyroFrame>();
    auto data = gyro_frame->getValue();
    imu_msg.angular_velocity.x = data.x - imu_info.bias[0];
    imu_msg.angular_velocity.y = data.y - imu_info.bias[1];
    imu_msg.angular_velocity.z = data.z - imu_info.bias[2];
  } else if (frame->getType() == OB_FRAME_ACCEL) {
    auto accel_frame = frame->as<ob::AccelFrame>();
    auto data = accel_frame->getValue();
    imu_msg.linear_acceleration.x = data.x - imu_info.bias[0];
    imu_msg.linear_acceleration.y = data.y - imu_info.bias[1];
    imu_msg.linear_acceleration.z = data.z - imu_info.bias[2];
  } else {
    record_timestamps(std::nullopt);
    RCLCPP_ERROR(logger_, "Unsupported IMU frame type");
    return;
  }
  const auto publish_system_us = getSystemNowUs();
  imu_publishers_[stream_index]->publish(imu_msg);
  record_timestamps(publish_system_us);
}

void OBCameraNode::setDefaultIMUMessage(sensor_msgs::msg::Imu &imu_msg) {
  imu_msg.header.frame_id = "imu_link";
  imu_msg.orientation.x = 0.0;
  imu_msg.orientation.y = 0.0;
  imu_msg.orientation.z = 0.0;
  imu_msg.orientation.w = 1.0;

  imu_msg.orientation_covariance = {-1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
  imu_msg.linear_acceleration_covariance = {
      linear_accel_cov_, 0.0, 0.0, 0.0, linear_accel_cov_, 0.0, 0.0, 0.0, linear_accel_cov_};
  imu_msg.angular_velocity_covariance = {
      angular_vel_cov_, 0.0, 0.0, 0.0, angular_vel_cov_, 0.0, 0.0, 0.0, angular_vel_cov_};
}

sensor_msgs::msg::Imu OBCameraNode::createUnitIMUMessage(const IMUData &accel_data,
                                                         const IMUData &gyro_data) {
  sensor_msgs::msg::Imu imu_msg;
  rclcpp::Time timestamp(gyro_data.timestamp_);
  imu_msg.header.stamp = timestamp;
  imu_msg.angular_velocity.x = gyro_data.data_.x();
  imu_msg.angular_velocity.y = gyro_data.data_.y();
  imu_msg.angular_velocity.z = gyro_data.data_.z();

  imu_msg.linear_acceleration.x = accel_data.data_.x();
  imu_msg.linear_acceleration.y = accel_data.data_.y();
  imu_msg.linear_acceleration.z = accel_data.data_.z();
  return imu_msg;
}

std::optional<OBCameraParam> OBCameraNode::findDefaultCameraParam() {
  auto camera_params = device_->getCalibrationCameraParamList();
  for (size_t i = 0; i < camera_params->count(); i++) {
    auto param = camera_params->getCameraParam(i);
    int depth_w = param.depthIntrinsic.width;
    int depth_h = param.depthIntrinsic.height;
    int color_w = param.rgbIntrinsic.width;
    int color_h = param.rgbIntrinsic.height;
    if ((depth_w * height_[DEPTH] == depth_h * width_[DEPTH]) &&
        (color_w * height_[COLOR] == color_h * width_[COLOR])) {
      return param;
    }
  }
  return {};
}

std::optional<OBCameraParam> OBCameraNode::getDepthCameraParam() {
  auto camera_params = device_->getCalibrationCameraParamList();
  for (size_t i = 0; i < camera_params->count(); i++) {
    auto param = camera_params->getCameraParam(i);
    int depth_w = param.depthIntrinsic.width;
    int depth_h = param.depthIntrinsic.height;
    if (depth_w == width_[DEPTH] && depth_h == height_[DEPTH]) {
      RCLCPP_INFO_STREAM(logger_, "getCameraDepthParam w: " << depth_w << ",h:" << depth_h);
      return param;
    }
  }

  for (size_t i = 0; i < camera_params->count(); i++) {
    auto param = camera_params->getCameraParam(i);
    int depth_w = param.depthIntrinsic.width;
    int depth_h = param.depthIntrinsic.height;
    if (depth_w * height_[DEPTH] == depth_h * width_[DEPTH]) {
      RCLCPP_INFO_STREAM(logger_, "getCameraDepthParam w: " << depth_w << ",h:" << depth_h);
      return param;
    }
  }
  return {};
}

std::optional<OBCameraParam> OBCameraNode::getColorCameraParam() {
  auto camera_params = device_->getCalibrationCameraParamList();
  for (size_t i = 0; i < camera_params->count(); i++) {
    auto param = camera_params->getCameraParam(i);
    int color_w = param.rgbIntrinsic.width;
    int color_h = param.rgbIntrinsic.height;
    if (color_w == width_[COLOR] && color_h == height_[COLOR]) {
      RCLCPP_INFO_STREAM(logger_, "getColorCameraParam w: " << color_w << ",h:" << color_h);
      return param;
    }
  }

  for (size_t i = 0; i < camera_params->count(); i++) {
    auto param = camera_params->getCameraParam(i);
    int color_w = param.rgbIntrinsic.width;
    int color_h = param.rgbIntrinsic.height;
    if (color_w * height_[COLOR] == color_h * width_[COLOR]) {
      RCLCPP_INFO_STREAM(logger_, "getColorCameraParam w: " << color_w << ",h:" << color_h);
      return param;
    }
  }
  return {};
}

void OBCameraNode::publishStaticTF(const rclcpp::Time &t, const tf2::Vector3 &trans,
                                   const tf2::Quaternion &q, const std::string &from,
                                   const std::string &to) {
  geometry_msgs::msg::TransformStamped msg;
  msg.header.stamp = t;
  msg.header.frame_id = from;
  msg.child_frame_id = to;
  msg.transform.translation.x = trans[2] / 1000.0;
  msg.transform.translation.y = -trans[0] / 1000.0;
  msg.transform.translation.z = -trans[1] / 1000.0;
  msg.transform.rotation.x = q.getX();
  msg.transform.rotation.y = q.getY();
  msg.transform.rotation.z = q.getZ();
  msg.transform.rotation.w = q.getW();
  static_tf_msgs_.push_back(msg);
}

void OBCameraNode::calcAndPublishStaticTransform() {
  tf2::Quaternion quaternion_optical, zero_rot;
  zero_rot.setRPY(0.0, 0.0, 0.0);
  quaternion_optical.setRPY(-M_PI / 2, 0.0, -M_PI / 2);
  tf2::Vector3 zero_trans(0, 0, 0);
  auto base_stream_profile = stream_profile_[base_stream_];
  auto device_info = device_->getDeviceInfo();
  CHECK_NOTNULL(device_info);
  if (!base_stream_profile) {
    RCLCPP_ERROR_STREAM(logger_, "Failed to get base stream profile");
    return;
  }
  CHECK_NOTNULL(base_stream_profile.get());
  for (const auto &item : stream_profile_) {
    auto stream_index = item.first;

    auto stream_profile = item.second;
    if (!stream_profile) {
      continue;
    }
    OBExtrinsic ex;
    try {
      ex = stream_profile->getExtrinsicTo(base_stream_profile);
    } catch (const ob::Error &e) {
      RCLCPP_ERROR_STREAM(logger_, "Failed to get " << stream_name_[stream_index] << " extrinsic: "
                                                    << orbbec_camera::formatObErrorWithStatus(e));
      ex = OBExtrinsic({{1, 0, 0, 0, 1, 0, 0, 0, 1}, {0, 0, 0}});
    }

    auto Q = rotationMatrixToQuaternion(ex.rot);
    Q = quaternion_optical * Q * quaternion_optical.inverse();
    tf2::Vector3 trans(ex.trans[0], ex.trans[1], ex.trans[2]);
    auto timestamp = node_->now();
    if (stream_index.first != base_stream_.first) {
      if (stream_index.first == OB_STREAM_IR_RIGHT && base_stream_.first == OB_STREAM_DEPTH) {
        trans[0] = std::abs(trans[0]);  // because left and right ir calibration is error
      }
      publishStaticTF(timestamp, trans, Q, frame_id_[base_stream_], frame_id_[stream_index]);
    }
    publishStaticTF(timestamp, zero_trans, quaternion_optical, frame_id_[stream_index],
                    optical_frame_id_[stream_index]);
    RCLCPP_DEBUG_STREAM(logger_, "Publishing static transform from " << stream_name_[stream_index]
                                                                     << " to "
                                                                     << stream_name_[base_stream_]);
    RCLCPP_DEBUG_STREAM(logger_,
                        "Translation " << trans[0] << ", " << trans[1] << ", " << trans[2]);
    RCLCPP_DEBUG_STREAM(logger_, "Rotation " << Q.getX() << ", " << Q.getY() << ", " << Q.getZ()
                                             << ", " << Q.getW());
  }

  if ((pid_ == FEMTO_BOLT_PID || pid_ == FEMTO_MEGA_PID) && enable_stream_[DEPTH] &&
      enable_stream_[COLOR]) {
    // calc depth to color

    CHECK_NOTNULL(stream_profile_[COLOR]);
    auto depth_to_color_extrinsics = base_stream_profile->getExtrinsicTo(stream_profile_[COLOR]);
    auto Q = rotationMatrixToQuaternion(depth_to_color_extrinsics.rot);
    Q = quaternion_optical * Q * quaternion_optical.inverse();
    publishStaticTF(node_->now(), zero_trans, Q, camera_link_frame_id_, frame_id_[base_stream_]);
  } else {
    publishStaticTF(node_->now(), zero_trans, zero_rot, camera_link_frame_id_,
                    frame_id_[base_stream_]);
  }

  if (enable_stream_[DEPTH] && enable_stream_[COLOR] && enable_publish_extrinsic_) {
    static const char *frame_id = "depth_to_color_extrinsics";
    OBExtrinsic ex;
    try {
      ex = base_stream_profile->getExtrinsicTo(stream_profile_[COLOR]);
    } catch (const ob::Error &e) {
      RCLCPP_ERROR_STREAM(logger_, "Failed to get " << frame_id << " extrinsic: "
                                                    << orbbec_camera::formatObErrorWithStatus(e));
      ex = OBExtrinsic({{1, 0, 0, 0, 1, 0, 0, 0, 1}, {0, 0, 0}});
    }
    depth_to_other_extrinsics_[COLOR] = ex;
    auto ex_msg = obExtrinsicsToMsg(ex, frame_id);
    CHECK_NOTNULL(depth_to_other_extrinsics_publishers_[COLOR]);
    depth_to_other_extrinsics_publishers_[COLOR]->publish(ex_msg);
  }

  if (enable_stream_[DEPTH] && enable_stream_[INFRA0] && enable_publish_extrinsic_) {
    static const char *frame_id = "depth_to_ir_extrinsics";
    OBExtrinsic ex;
    try {
      ex = base_stream_profile->getExtrinsicTo(stream_profile_[INFRA0]);
    } catch (const ob::Error &e) {
      RCLCPP_ERROR_STREAM(logger_, "Failed to get " << frame_id << " extrinsic: "
                                                    << orbbec_camera::formatObErrorWithStatus(e));
      ex = OBExtrinsic({{1, 0, 0, 0, 1, 0, 0, 0, 1}, {0, 0, 0}});
    }
    depth_to_other_extrinsics_[INFRA0] = ex;
    auto ex_msg = obExtrinsicsToMsg(ex, frame_id);
    CHECK_NOTNULL(depth_to_other_extrinsics_publishers_[INFRA0]);
    depth_to_other_extrinsics_publishers_[INFRA0]->publish(ex_msg);
  }
  if (enable_stream_[DEPTH] && enable_stream_[INFRA1] && enable_publish_extrinsic_) {
    static const char *frame_id = "depth_to_left_ir_extrinsics";
    OBExtrinsic ex;
    try {
      ex = base_stream_profile->getExtrinsicTo(stream_profile_[INFRA1]);
    } catch (const ob::Error &e) {
      RCLCPP_ERROR_STREAM(logger_, "Failed to get " << frame_id << " extrinsic: "
                                                    << orbbec_camera::formatObErrorWithStatus(e));
      ex = OBExtrinsic({{1, 0, 0, 0, 1, 0, 0, 0, 1}, {0, 0, 0}});
    }
    depth_to_other_extrinsics_[INFRA1] = ex;
    auto ex_msg = obExtrinsicsToMsg(ex, frame_id);
    CHECK_NOTNULL(depth_to_other_extrinsics_publishers_[INFRA1]);
    depth_to_other_extrinsics_publishers_[INFRA1]->publish(ex_msg);
  }
  if (enable_stream_[DEPTH] && enable_stream_[INFRA2] && enable_publish_extrinsic_) {
    static const char *frame_id = "depth_to_right_ir_extrinsics";
    OBExtrinsic ex;
    try {
      ex = base_stream_profile->getExtrinsicTo(stream_profile_[INFRA2]);
    } catch (const ob::Error &e) {
      RCLCPP_ERROR_STREAM(logger_, "Failed to get " << frame_id << " extrinsic: "
                                                    << orbbec_camera::formatObErrorWithStatus(e));
      ex = OBExtrinsic({{1, 0, 0, 0, 1, 0, 0, 0, 1}, {0, 0, 0}});
    }
    ex.trans[0] = -std::abs(ex.trans[0]);
    depth_to_other_extrinsics_[INFRA2] = ex;
    auto ex_msg = obExtrinsicsToMsg(ex, frame_id);
    CHECK_NOTNULL(depth_to_other_extrinsics_publishers_[INFRA2]);
    depth_to_other_extrinsics_publishers_[INFRA2]->publish(ex_msg);
  }
  if (enable_stream_[DEPTH] && enable_stream_[ACCEL] && enable_publish_extrinsic_) {
    static const char *frame_id = "depth_to_accel_extrinsics";
    OBExtrinsic ex;
    try {
      ex = base_stream_profile->getExtrinsicTo(stream_profile_[ACCEL]);
    } catch (const ob::Error &e) {
      RCLCPP_ERROR_STREAM(logger_, "Failed to get " << frame_id << " extrinsic: "
                                                    << orbbec_camera::formatObErrorWithStatus(e));
      ex = OBExtrinsic({{1, 0, 0, 0, 1, 0, 0, 0, 1}, {0, 0, 0}});
    }
    depth_to_other_extrinsics_[ACCEL] = ex;
    auto ex_msg = obExtrinsicsToMsg(ex, frame_id);
    CHECK_NOTNULL(depth_to_other_extrinsics_publishers_[ACCEL]);
    depth_to_other_extrinsics_publishers_[ACCEL]->publish(ex_msg);
  }
  if (enable_stream_[DEPTH] && enable_stream_[GYRO] && enable_publish_extrinsic_) {
    static const char *frame_id = "depth_to_gyro_extrinsics";
    OBExtrinsic ex;
    try {
      ex = base_stream_profile->getExtrinsicTo(stream_profile_[GYRO]);
    } catch (const ob::Error &e) {
      RCLCPP_ERROR_STREAM(logger_, "Failed to get " << frame_id << " extrinsic: "
                                                    << orbbec_camera::formatObErrorWithStatus(e));
      ex = OBExtrinsic({{1, 0, 0, 0, 1, 0, 0, 0, 1}, {0, 0, 0}});
    }
    depth_to_other_extrinsics_[GYRO] = ex;
    auto ex_msg = obExtrinsicsToMsg(ex, frame_id);
    CHECK_NOTNULL(depth_to_other_extrinsics_publishers_[GYRO]);
    depth_to_other_extrinsics_publishers_[GYRO]->publish(ex_msg);
  }
  if (enable_stream_[COLOR_LEFT] && enable_stream_[COLOR_RIGHT] && enable_publish_extrinsic_) {
    static const char *frame_id = "left_color_to_right_color_extrinsics";
    OBExtrinsic ex;
    try {
      ex = stream_profile_[COLOR_LEFT]->getExtrinsicTo(stream_profile_[COLOR_RIGHT]);
    } catch (const ob::Error &e) {
      RCLCPP_ERROR_STREAM(logger_, "Failed to get " << frame_id << " extrinsic: "
                                                    << orbbec_camera::formatObErrorWithStatus(e));
      ex = OBExtrinsic({{1, 0, 0, 0, 1, 0, 0, 0, 1}, {0, 0, 0}});
    }
    depth_to_other_extrinsics_[COLOR_LEFT] = ex;
    auto ex_msg = obExtrinsicsToMsg(ex, frame_id);
    CHECK_NOTNULL(depth_to_other_extrinsics_publishers_[COLOR_LEFT]);
    depth_to_other_extrinsics_publishers_[COLOR_LEFT]->publish(ex_msg);
  }
  if (enable_sync_output_accel_gyro_) {
    tf2::Quaternion zero_rot;
    zero_rot.setRPY(0.0, 0.0, 0.0);
    tf2::Vector3 zero_trans(0, 0, 0);
    publishStaticTF(node_->now(), zero_trans, zero_rot, optical_frame_id_[GYRO],
                    accel_gyro_frame_id_);
  }
}

void OBCameraNode::publishStaticTransforms() {
  if (!publish_tf_) {
    return;
  }
  static_tf_broadcaster_ = std::make_shared<tf2_ros::StaticTransformBroadcaster>(*node_);
  dynamic_tf_broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(*node_);
  calcAndPublishStaticTransform();
  if (tf_publish_rate_ > 0) {
    tf_thread_ = std::make_shared<std::thread>([this]() { publishDynamicTransforms(); });
  } else {
    static_tf_broadcaster_->sendTransform(static_tf_msgs_);
  }
}

void OBCameraNode::publishDynamicTransforms() {
  RCLCPP_WARN(logger_, "Publishing dynamic camera transforms (/tf) at %g Hz", tf_publish_rate_);
  std::mutex mu;
  std::unique_lock<std::mutex> lock(mu);
  while (rclcpp::ok() && is_running_) {
    tf_cv_.wait_for(lock, std::chrono::milliseconds((int)(1000.0 / tf_publish_rate_)),
                    [this] { return (!(is_running_)); });
    {
      rclcpp::Time t = node_->now();
      for (auto &msg : static_tf_msgs_) {
        msg.header.stamp = t;
      }
      dynamic_tf_broadcaster_->sendTransform(static_tf_msgs_);
    }
  }
}

template <typename T>
T lerp(const T &a, const T &b, const double t) {
  return a * (1.0 - t) + b * t;
}

void OBCameraNode::FillImuDataLinearInterpolation(const IMUData &imu_data,
                                                  std::deque<sensor_msgs::msg::Imu> &imu_msgs) {
  imu_history_.push_back(imu_data);
  stream_index_pair steam_index(imu_data.stream_);
  imu_msgs.clear();
  std::deque<IMUData> gyros_data;
  IMUData accel0, accel1, current_imu;
  while (!imu_history_.empty()) {
    current_imu = imu_history_.front();
    imu_history_.pop_front();
    if (accel0.isSet() && current_imu.stream_ == ACCEL) {
      accel0 = current_imu;
    } else if (accel0.isSet() && current_imu.stream_ == ACCEL) {
      accel1 = current_imu;
      const double dt = accel1.timestamp_ - accel0.timestamp_;
      while (!gyros_data.empty()) {
        auto current_gyro = gyros_data.front();
        gyros_data.pop_front();
        const double alpha = (current_gyro.timestamp_ - accel0.timestamp_) / dt;
        IMUData current_accel(ACCEL, lerp(accel0.data_, accel1.data_, alpha),
                              current_gyro.timestamp_);
        imu_msgs.push_back((createUnitIMUMessage(current_accel, current_gyro)));
      }
      accel0 = accel1;
    } else if (accel0.isSet() && current_imu.timestamp_ >= accel0.timestamp_ &&
               current_imu.stream_ == GYRO) {
      gyros_data.push_back(current_imu);
    }
  }
  imu_history_.push_back(current_imu);
}

void OBCameraNode::FillImuDataCopy(const IMUData &imu_data,
                                   std::deque<sensor_msgs::msg::Imu> &imu_msgs) {
  stream_index_pair steam_index(imu_data.stream_);
  if (steam_index == ACCEL) {
    accel_data_ = imu_data;
    return;
  }
  if (accel_data_.isSet()) {
    return;
  }
  imu_msgs.push_back(createUnitIMUMessage(accel_data_, imu_data));
}

bool OBCameraNode::setupFormatConvertType(OBFormat format) {
  return setupFormatConvertType(format, format_convert_filter_);
}

bool OBCameraNode::setupFormatConvertType(OBFormat format, ob::FormatConvertFilter &filter) {
  switch (format) {
    case OB_FORMAT_RGB888:
      return true;
    case OB_FORMAT_I420:
      filter.setFormatConvertType(FORMAT_I420_TO_RGB888);
      break;
    case OB_FORMAT_MJPG:
      filter.setFormatConvertType(FORMAT_MJPEG_TO_RGB888);
      break;
    case OB_FORMAT_YUYV:
      filter.setFormatConvertType(FORMAT_YUYV_TO_RGB888);
      break;
    case OB_FORMAT_NV21:
      filter.setFormatConvertType(FORMAT_NV21_TO_RGB888);
      break;
    case OB_FORMAT_NV12:
      filter.setFormatConvertType(FORMAT_NV12_TO_RGB888);
      break;
    case OB_FORMAT_UYVY:
      filter.setFormatConvertType(FORMAT_UYVY_TO_RGB888);
      break;
    default:
      return false;
  }
  return true;
}

bool OBCameraNode::isGemini335PID(uint32_t pid) {
  return pid == GEMINI_335_PID || pid == GEMINI_330_PID || pid == GEMINI_336_PID ||
         pid == GEMINI_335L_PID || pid == GEMINI_330L_PID || pid == GEMINI_336L_PID ||
         pid == GEMINI_335LG_PID || pid == GEMINI_336LG_PID || pid == GEMINI_335LE_PID ||
         pid == GEMINI_336LE_PID || pid == CUSTOM_ADVANTECH_GEMINI_336_PID ||
         pid == CUSTOM_ADVANTECH_GEMINI_336L_PID || pid == GEMINI_338_PID ||
         pid == GEMINI_338L_PID || pid == GEMINI_338LE_PID || pid == GEMINI_338LG_PID;
}

bool OBCameraNode::isGemini435LePID(uint32_t pid) { return pid == GEMINI_435Le_PID; }
bool OBCameraNode::isPublishMetaData(uint32_t pid) {
  return isGemini335PID(pid) || isGemini435LePID(pid) || isGemini305SeriesPID(pid);
}

bool OBCameraNode::isDabaiASeriesForHwD2C(uint32_t pid) {
  return pid == DABAI_A_PID || pid == DABAI_AL_PID || pid == GEMINI_345_PID ||
         pid == GEMINI_345LG_PID;
}

bool OBCameraNode::isDepthWorkModeDevices(uint32_t pid) { return pid == GEMINI_435Le_PID; }

bool OBCameraNode::isnotLaserDevices(uint32_t pid) { return isGemini305SeriesPID(pid); }

orbbec_camera_msgs::msg::IMUInfo OBCameraNode::createIMUInfo(
    const stream_index_pair &stream_index) {
  orbbec_camera_msgs::msg::IMUInfo imu_info;
  imu_info.header.frame_id = optical_frame_id_[stream_index];
  imu_info.header.stamp = node_->now();

  if (stream_index == GYRO) {
    auto gyro_profile = stream_profile_[stream_index]->as<ob::GyroStreamProfile>();
    auto gyro_intrinsics = gyro_profile->getIntrinsic();
    imu_info.noise_density = gyro_intrinsics.noiseDensity;
    imu_info.random_walk = gyro_intrinsics.randomWalk;
    imu_info.reference_temperature = gyro_intrinsics.referenceTemp;
    imu_info.bias = {gyro_intrinsics.bias[0], gyro_intrinsics.bias[1], gyro_intrinsics.bias[2]};
    imu_info.scale_misalignment = {
        gyro_intrinsics.scaleMisalignment[0], gyro_intrinsics.scaleMisalignment[1],
        gyro_intrinsics.scaleMisalignment[2], gyro_intrinsics.scaleMisalignment[3],
        gyro_intrinsics.scaleMisalignment[4], gyro_intrinsics.scaleMisalignment[5],
        gyro_intrinsics.scaleMisalignment[6], gyro_intrinsics.scaleMisalignment[7],
        gyro_intrinsics.scaleMisalignment[8]};
    imu_info.temperature_slope = {
        gyro_intrinsics.tempSlope[0], gyro_intrinsics.tempSlope[1], gyro_intrinsics.tempSlope[2],
        gyro_intrinsics.tempSlope[3], gyro_intrinsics.tempSlope[4], gyro_intrinsics.tempSlope[5],
        gyro_intrinsics.tempSlope[6], gyro_intrinsics.tempSlope[7], gyro_intrinsics.tempSlope[8]};
  } else if (stream_index == ACCEL) {
    auto accel_profile = stream_profile_[stream_index]->as<ob::AccelStreamProfile>();
    auto accel_intrinsics = accel_profile->getIntrinsic();
    imu_info.noise_density = accel_intrinsics.noiseDensity;
    imu_info.random_walk = accel_intrinsics.randomWalk;
    imu_info.reference_temperature = accel_intrinsics.referenceTemp;
    imu_info.bias = {accel_intrinsics.bias[0], accel_intrinsics.bias[1], accel_intrinsics.bias[2]};
    imu_info.gravity = {accel_intrinsics.gravity[0], accel_intrinsics.gravity[1],
                        accel_intrinsics.gravity[2]};
    imu_info.scale_misalignment = {
        accel_intrinsics.scaleMisalignment[0], accel_intrinsics.scaleMisalignment[1],
        accel_intrinsics.scaleMisalignment[2], accel_intrinsics.scaleMisalignment[3],
        accel_intrinsics.scaleMisalignment[4], accel_intrinsics.scaleMisalignment[5],
        accel_intrinsics.scaleMisalignment[6], accel_intrinsics.scaleMisalignment[7],
        accel_intrinsics.scaleMisalignment[8]};
    imu_info.temperature_slope = {accel_intrinsics.tempSlope[0], accel_intrinsics.tempSlope[1],
                                  accel_intrinsics.tempSlope[2], accel_intrinsics.tempSlope[3],
                                  accel_intrinsics.tempSlope[4], accel_intrinsics.tempSlope[5],
                                  accel_intrinsics.tempSlope[6], accel_intrinsics.tempSlope[7],
                                  accel_intrinsics.tempSlope[8]};
  }

  return imu_info;
}

void OBCameraNode::updateDepthFilterEnabledCache(const std::string &filter_name, bool enabled) {
  const auto normalized_filter_name = normalizeDepthFilterName(filter_name);
  if (normalized_filter_name == "DecimationFilter") {
    enable_decimation_filter_ = enabled;
  } else if (normalized_filter_name == "HDRMerge") {
    enable_hdr_merge_ = enabled;
  } else if (normalized_filter_name == "SequenceIdFilter") {
    enable_sequence_id_filter_ = enabled;
  } else if (normalized_filter_name == "ThresholdFilter") {
    enable_threshold_filter_ = enabled;
  } else if (normalized_filter_name == "SpatialAdvancedFilter") {
    enable_spatial_filter_ = enabled;
  } else if (normalized_filter_name == "TemporalFilter") {
    enable_temporal_filter_ = enabled;
  } else if (normalized_filter_name == "HoleFillingFilter") {
    enable_hole_filling_filter_ = enabled;
  } else if (normalized_filter_name == "EdgeNoiseRemovalFilter") {
    enable_edge_noise_removal_filter_ = enabled;
  } else if (normalized_filter_name == "SpatialFastFilter") {
    enable_spatial_fast_filter_ = enabled;
  } else if (normalized_filter_name == "SpatialModerateFilter") {
    enable_spatial_moderate_filter_ = enabled;
  } else if (normalized_filter_name == "FalsePositiveFilter") {
    enable_false_positive_filter_ = enabled;
  } else if (normalized_filter_name == "MgcNoiseRemovalFilter") {
    enable_mgc_noise_removal_filter_ = enabled;
  } else if (normalized_filter_name == "LutNoiseRemovalFilter") {
    enable_lut_noise_removal_filter_ = enabled;
  } else if (normalized_filter_name == "NoiseRemovalFilter") {
    enable_noise_removal_filter_ = enabled;
  } else if (normalized_filter_name == "HardwareNoiseRemovalFilter") {
    enable_hardware_noise_removal_filter_ = enabled;
  } else if (normalized_filter_name == "DispOutliersFilter") {
    enable_disp_outliers_filter_ = enabled;
  }
}

bool OBCameraNode::applyNamedDepthFilterConfig(
    const std::string &filter_name, bool enabled,
    const std::vector<orbbec_camera_msgs::msg::DepthFilterParam> &params, std::string &message) {
  const auto normalized_filter_name = normalizeDepthFilterName(filter_name);
  std::unordered_set<std::string> requested_param_names;
  auto check_duplicate_param = [&requested_param_names, &message](const std::string &param_name) {
    if (param_name.empty()) {
      message = "Filter config param name is empty";
      return false;
    }
    if (!requested_param_names.insert(param_name).second) {
      message = "Duplicate filter config param '" + param_name + "'";
      return false;
    }
    return true;
  };

  if (normalized_filter_name == "NoiseRemovalFilter") {
    const bool supported =
        device_->isPropertySupported(OB_PROP_DEPTH_SOFT_FILTER_BOOL, OB_PERMISSION_READ_WRITE) ||
        device_->isPropertySupported(OB_PROP_DEPTH_MAX_DIFF_INT, OB_PERMISSION_WRITE) ||
        device_->isPropertySupported(OB_PROP_DEPTH_MAX_SPECKLE_SIZE_INT, OB_PERMISSION_WRITE);
    if (!supported) {
      message = "Filter '" + normalized_filter_name + "' is not supported by this device";
      return false;
    }

    bool has_min_diff = false;
    bool has_max_size = false;
    int min_diff = 0;
    int max_size = 0;
    for (const auto &param : params) {
      const auto param_name = getDepthFilterConfigParamName(normalized_filter_name, param.name);
      if (!check_duplicate_param(param_name)) {
        return false;
      }

      double parsed_value = 0.0;
      if (!parseFilterConfigDouble(param.value, parsed_value, message)) {
        return false;
      }
      if (std::floor(parsed_value) != parsed_value) {
        message = "Filter config '" + param_name + "' expects an integer value";
        return false;
      }

      if (param_name == "min_diff") {
        if (!device_->isPropertySupported(OB_PROP_DEPTH_MAX_DIFF_INT, OB_PERMISSION_WRITE)) {
          message = "Filter config 'min_diff' is not supported by this device";
          return false;
        }
        has_min_diff = true;
        min_diff = static_cast<int>(parsed_value);
      } else if (param_name == "max_size") {
        if (!device_->isPropertySupported(OB_PROP_DEPTH_MAX_SPECKLE_SIZE_INT,
                                          OB_PERMISSION_WRITE)) {
          message = "Filter config 'max_size' is not supported by this device";
          return false;
        }
        has_max_size = true;
        max_size = static_cast<int>(parsed_value);
      } else {
        message = "Unknown filter config '" + param.name + "' for " + normalized_filter_name;
        return false;
      }
    }

    if (device_->isPropertySupported(OB_PROP_DEPTH_SOFT_FILTER_BOOL, OB_PERMISSION_READ_WRITE)) {
      device_->setBoolProperty(OB_PROP_DEPTH_SOFT_FILTER_BOOL, enabled);
    }
    if (has_min_diff) {
      device_->setIntProperty(OB_PROP_DEPTH_MAX_DIFF_INT, min_diff);
      noise_removal_filter_min_diff_ = min_diff;
    }
    if (has_max_size) {
      device_->setIntProperty(OB_PROP_DEPTH_MAX_SPECKLE_SIZE_INT, max_size);
      noise_removal_filter_max_size_ = max_size;
    }
    updateDepthFilterEnabledCache(normalized_filter_name, enabled);
    return true;
  }

  if (normalized_filter_name == "HardwareNoiseRemovalFilter") {
    const bool supported =
        device_->isPropertySupported(OB_PROP_HW_NOISE_REMOVE_FILTER_ENABLE_BOOL,
                                     OB_PERMISSION_READ_WRITE) ||
        device_->isPropertySupported(OB_PROP_HW_NOISE_REMOVE_FILTER_THRESHOLD_FLOAT,
                                     OB_PERMISSION_READ_WRITE);
    if (!supported) {
      message = "Filter '" + normalized_filter_name + "' is not supported by this device";
      return false;
    }

    bool has_threshold = false;
    double threshold = 0.0;
    for (const auto &param : params) {
      const auto param_name = getDepthFilterConfigParamName(normalized_filter_name, param.name);
      if (!check_duplicate_param(param_name)) {
        return false;
      }
      if (param_name != "threshold") {
        message = "Unknown filter config '" + param.name + "' for " + normalized_filter_name;
        return false;
      }
      if (!device_->isPropertySupported(OB_PROP_HW_NOISE_REMOVE_FILTER_THRESHOLD_FLOAT,
                                        OB_PERMISSION_READ_WRITE)) {
        message = "Filter config 'threshold' is not supported by this device";
        return false;
      }
      if (!parseFilterConfigDouble(param.value, threshold, message)) {
        return false;
      }
      has_threshold = true;
    }

    if (device_->isPropertySupported(OB_PROP_HW_NOISE_REMOVE_FILTER_ENABLE_BOOL,
                                     OB_PERMISSION_READ_WRITE)) {
      device_->setBoolProperty(OB_PROP_HW_NOISE_REMOVE_FILTER_ENABLE_BOOL, enabled);
    }
    if (has_threshold) {
      device_->setFloatProperty(OB_PROP_HW_NOISE_REMOVE_FILTER_THRESHOLD_FLOAT,
                                static_cast<float>(threshold));
      hardware_noise_removal_filter_threshold_ = static_cast<float>(threshold);
    }
    updateDepthFilterEnabledCache(normalized_filter_name, enabled);
    return true;
  }

  if (normalized_filter_name == "DispOutliersFilter") {
    const bool supported =
        device_->isPropertySupported(OB_PROP_DEPTH_OUTLIERS_FILTER_BOOL,
                                     OB_PERMISSION_READ_WRITE) ||
        device_->isPropertySupported(OB_PROP_DEPTH_OUTLIERS_FILTER_SEARCH_MODE_INT,
                                     OB_PERMISSION_READ_WRITE);
    if (!supported) {
      message = "Filter '" + normalized_filter_name + "' is not supported by this device";
      return false;
    }

    bool has_search_mode = false;
    int search_mode = 0;
    for (const auto &param : params) {
      const auto param_name = getDepthFilterConfigParamName(normalized_filter_name, param.name);
      if (!check_duplicate_param(param_name)) {
        return false;
      }

      if (param_name != "search_mode") {
        message = "Unknown filter config '" + param.name + "' for " + normalized_filter_name;
        return false;
      }
      if (!device_->isPropertySupported(OB_PROP_DEPTH_OUTLIERS_FILTER_SEARCH_MODE_INT,
                                        OB_PERMISSION_READ_WRITE)) {
        message = "Filter config 'search_mode' is not supported by this device";
        return false;
      }

      if (!parseDispOutliersSearchMode(param.value, search_mode, message)) {
        return false;
      }
      has_search_mode = true;
    }

    if (device_->isPropertySupported(OB_PROP_DEPTH_OUTLIERS_FILTER_BOOL,
                                     OB_PERMISSION_READ_WRITE)) {
      device_->setBoolProperty(OB_PROP_DEPTH_OUTLIERS_FILTER_BOOL, enabled);
    }
    if (has_search_mode) {
      device_->setIntProperty(OB_PROP_DEPTH_OUTLIERS_FILTER_SEARCH_MODE_INT, search_mode);
      disp_outliers_filter_search_mode_ = search_mode;
    }
    updateDepthFilterEnabledCache(normalized_filter_name, enabled);
    return true;
  }

  std::unique_lock<std::mutex> depth_filter_lock(depth_filter_mutex_);
  auto is_same_filter = [&normalized_filter_name](const std::shared_ptr<ob::Filter> &filter) {
    if (!filter) {
      return false;
    }
    return normalizeDepthFilterName(filter->getName()) == normalized_filter_name ||
           normalizeDepthFilterName(filter->type()) == normalized_filter_name;
  };

  auto first_match_it =
      std::find_if(depth_filter_list_.begin(), depth_filter_list_.end(),
                   [&is_same_filter](const auto &filter) { return is_same_filter(filter); });
  if (first_match_it == depth_filter_list_.end() || !(*first_match_it)) {
    message = "Filter '" + normalized_filter_name + "' is not supported by this device";
    return false;
  }

  const auto &existing_filter = *first_match_it;
  std::vector<OBFilterConfigSchemaItem> schema_vec;
  std::unordered_map<std::string, OBFilterConfigSchemaItem> schema_by_name;
  if (!params.empty()) {
    schema_vec = existing_filter->getConfigSchemaVec();
    for (const auto &schema : schema_vec) {
      if (schema.name == nullptr || schema.name[0] == '\0') {
        continue;
      }
      schema_by_name.emplace(schema.name, schema);
    }
  }

  std::vector<std::pair<std::string, double>> parsed_params;
  parsed_params.reserve(params.size());
  for (const auto &param : params) {
    const auto param_name = getDepthFilterConfigParamName(normalized_filter_name, param.name);
    if (!check_duplicate_param(param_name)) {
      return false;
    }

    const auto schema_it = schema_by_name.find(param_name);
    if (schema_it == schema_by_name.end()) {
      message = "Unknown filter config '" + param.name + "' for " + normalized_filter_name;
      return false;
    }

    double parsed_value = 0.0;
    if (!parseFilterConfigValue(schema_it->second, param.value, parsed_value, message)) {
      return false;
    }
    parsed_params.emplace_back(param_name, parsed_value);
  }

  existing_filter->enable(enabled);
  for (const auto &parsed_param : parsed_params) {
    existing_filter->setConfigValue(parsed_param.first, parsed_param.second);
    RCLCPP_INFO_STREAM(logger_, "Set " << normalized_filter_name << " config " << parsed_param.first
                                       << " to " << parsed_param.second);
  }
  updateDepthFilterEnabledCache(normalized_filter_name, enabled);
  return true;
}

bool OBCameraNode::applyEnhancedDepthFilterConfig(
    bool enabled, const std::vector<float> &positional_params,
    const std::vector<orbbec_camera_msgs::msg::DepthFilterParam> &named_params,
    std::string &message) {
  if (positional_params.size() > 1) {
    message = "EnhancedDepthFilter only supports one positional parameter";
    return false;
  }
  if (!positional_params.empty() && !named_params.empty()) {
    message = "filter_param and filter_config cannot be used at the same time";
    return false;
  }

  bool has_confidence_threshold = false;
  int confidence_threshold = enhanced_depth_confidence_threshold_;
  auto parse_confidence_threshold = [&message](double value, int &threshold) {
    if (std::floor(value) != value || value < 0.0 || value > 255.0) {
      message =
          "EnhancedDepthFilter confidence_threshold expects an integer value in range 0 - 255";
      return false;
    }
    threshold = static_cast<int>(value);
    return true;
  };
  if (!positional_params.empty()) {
    if (!parse_confidence_threshold(positional_params[0], confidence_threshold)) {
      return false;
    }
    has_confidence_threshold = true;
  }
  for (const auto &param : named_params) {
    if (param.name != "confidence_threshold") {
      message = "Unknown filter config '" + param.name + "' for EnhancedDepthFilter";
      return false;
    }
    double parsed_value = 0.0;
    if (!parseFilterConfigDouble(param.value, parsed_value, message)) {
      return false;
    }
    if (!parse_confidence_threshold(parsed_value, confidence_threshold)) {
      return false;
    }
    has_confidence_threshold = true;
  }

  std::string validate_message;
  if (enabled && !validateEnhancedDepthFilterConfig(validate_message)) {
    message = validate_message;
    return false;
  }

  const int previous_threshold = enhanced_depth_confidence_threshold_;
  if (has_confidence_threshold) {
    enhanced_depth_confidence_threshold_ = confidence_threshold;
  }

  if (enabled) {
    if (!ensureEnhancedDepthFilter(message)) {
      enhanced_depth_confidence_threshold_ = previous_threshold;
      return false;
    }
  } else if (has_confidence_threshold && enhanced_depth_filter_) {
    try {
      applyEnhancedDepthConfidenceThreshold();
    } catch (const std::exception &e) {
      enhanced_depth_confidence_threshold_ = previous_threshold;
      message = e.what();
      return false;
    }
  }

  {
    std::lock_guard<std::mutex> lock(enhanced_depth_filter_mutex_);
    enable_enhanced_depth_.store(enabled);
  }
  filter_status_["EnhancedDepthFilter"] = enabled;
  publishDepthFiltersStatus();
  return true;
}

void OBCameraNode::setFilterCallback(const std::shared_ptr<SetFilter ::Request> &request,
                                     std::shared_ptr<SetFilter ::Response> &response) {
  try {
    response->success = false;
    response->message.clear();
    auto fail = [&response](const std::string &msg) {
      response->success = false;
      response->message = msg;
    };
    const auto normalized_request_filter_name = normalizeDepthFilterName(request->filter_name);
    const bool is_noise_removal_filter = normalized_request_filter_name == "NoiseRemovalFilter";
    const bool is_hardware_noise_removal_filter =
        normalized_request_filter_name == "HardwareNoiseRemovalFilter";
    const bool is_disp_outliers_filter = normalized_request_filter_name == "DispOutliersFilter";
    bool is_supported_by_property = false;
    if (is_noise_removal_filter) {
      is_supported_by_property =
          device_->isPropertySupported(OB_PROP_DEPTH_SOFT_FILTER_BOOL, OB_PERMISSION_READ_WRITE) ||
          device_->isPropertySupported(OB_PROP_DEPTH_MAX_DIFF_INT, OB_PERMISSION_WRITE) ||
          device_->isPropertySupported(OB_PROP_DEPTH_MAX_SPECKLE_SIZE_INT, OB_PERMISSION_WRITE);
    } else if (is_hardware_noise_removal_filter) {
      is_supported_by_property =
          device_->isPropertySupported(OB_PROP_HW_NOISE_REMOVE_FILTER_ENABLE_BOOL,
                                       OB_PERMISSION_READ_WRITE) ||
          device_->isPropertySupported(OB_PROP_HW_NOISE_REMOVE_FILTER_THRESHOLD_FLOAT,
                                       OB_PERMISSION_READ_WRITE);
    } else if (is_disp_outliers_filter) {
      is_supported_by_property =
          device_->isPropertySupported(OB_PROP_DEPTH_OUTLIERS_FILTER_BOOL,
                                       OB_PERMISSION_READ_WRITE) ||
          device_->isPropertySupported(OB_PROP_DEPTH_OUTLIERS_FILTER_SEARCH_MODE_INT,
                                       OB_PERMISSION_READ_WRITE);
    }

    RCLCPP_INFO_STREAM(logger_, "filter_name: " << request->filter_name << "  filter_enable: "
                                                << (request->filter_enable ? "true" : "false"));
    const bool has_positional_params = !request->filter_param.empty();
    const bool has_named_params = !request->filter_config.empty();
    if (has_positional_params && has_named_params) {
      fail("filter_param and filter_config cannot be used at the same time");
      return;
    }
    if (is_disp_outliers_filter && has_positional_params) {
      fail("DispOutliersFilter search_mode expects filter_config value FULL or OFFSET_80");
      return;
    }

    if (normalized_request_filter_name == "EnhancedDepthFilter") {
      std::string message;
      if (!applyEnhancedDepthFilterConfig(request->filter_enable, request->filter_param,
                                          request->filter_config, message)) {
        fail(message);
        return;
      }
      response->success = true;
      return;
    }

    if (has_named_params || !has_positional_params) {
      std::string message;
      if (!applyNamedDepthFilterConfig(normalized_request_filter_name, request->filter_enable,
                                       request->filter_config, message)) {
        fail(message);
        return;
      }

      filter_status_[normalized_request_filter_name] = static_cast<bool>(request->filter_enable);
      publishDepthFiltersStatus();

      response->success = true;
      return;
    }

    if (is_noise_removal_filter || is_hardware_noise_removal_filter || is_disp_outliers_filter) {
      if (!is_supported_by_property) {
        fail("Filter '" + normalized_request_filter_name + "' is not supported by this device");
        return;
      }
      if (is_noise_removal_filter) {
        if (device_->isPropertySupported(OB_PROP_DEPTH_SOFT_FILTER_BOOL,
                                         OB_PERMISSION_READ_WRITE)) {
          device_->setBoolProperty(OB_PROP_DEPTH_SOFT_FILTER_BOOL, request->filter_enable);
          RCLCPP_INFO_STREAM(logger_, "enable_noise_removal_filter:" << request->filter_enable);
        }
        if (request->filter_param.size() > 1) {
          if (device_->isPropertySupported(OB_PROP_DEPTH_MAX_DIFF_INT, OB_PERMISSION_WRITE)) {
            device_->setIntProperty(OB_PROP_DEPTH_MAX_DIFF_INT, request->filter_param[0]);
            auto new_noise_removal_filter_min_diff =
                device_->getIntProperty(OB_PROP_DEPTH_MAX_DIFF_INT);
            RCLCPP_INFO_STREAM(logger_, "Set noise_removal_filter_min_diff: "
                                            << new_noise_removal_filter_min_diff);
            noise_removal_filter_min_diff_ = request->filter_param[0];
          }
          if (device_->isPropertySupported(OB_PROP_DEPTH_MAX_SPECKLE_SIZE_INT,
                                           OB_PERMISSION_WRITE)) {
            device_->setIntProperty(OB_PROP_DEPTH_MAX_SPECKLE_SIZE_INT, request->filter_param[1]);
            auto new_noise_removal_filter_max_size =
                device_->getIntProperty(OB_PROP_DEPTH_MAX_SPECKLE_SIZE_INT);
            RCLCPP_INFO_STREAM(logger_, "Set noise_removal_filter_max_size: "
                                            << new_noise_removal_filter_max_size);
            noise_removal_filter_max_size_ = request->filter_param[1];
          }
        }
        enable_noise_removal_filter_ = request->filter_enable;
      } else if (is_hardware_noise_removal_filter) {
        if (device_->isPropertySupported(OB_PROP_HW_NOISE_REMOVE_FILTER_ENABLE_BOOL,
                                         OB_PERMISSION_READ_WRITE)) {
          device_->setBoolProperty(OB_PROP_HW_NOISE_REMOVE_FILTER_ENABLE_BOOL,
                                   request->filter_enable);
          RCLCPP_INFO_STREAM(logger_,
                             "Set hardware_noise_removal_filter:" << request->filter_enable);
          if (request->filter_param.size() > 0 &&
              device_->isPropertySupported(OB_PROP_HW_NOISE_REMOVE_FILTER_THRESHOLD_FLOAT,
                                           OB_PERMISSION_READ_WRITE)) {
            if (request->filter_enable) {
              device_->setFloatProperty(OB_PROP_HW_NOISE_REMOVE_FILTER_THRESHOLD_FLOAT,
                                        request->filter_param[0]);
              RCLCPP_INFO_STREAM(logger_, "Set hardware_noise_removal_filter_threshold :"
                                              << request->filter_param[0]);
              hardware_noise_removal_filter_threshold_ = request->filter_param[0];
            }
          } else {
            fail(
                "The filter switch setting is successful, but the filter parameter setting "
                "fails");
            return;
          }
        }
        enable_hardware_noise_removal_filter_ = request->filter_enable;
      }
    } else {
      std::unique_lock<std::mutex> depth_filter_lock(depth_filter_mutex_);
      auto is_same_filter =
          [&normalized_request_filter_name](const std::shared_ptr<ob::Filter> &filter) {
            if (!filter) {
              return false;
            }
            return normalizeDepthFilterName(filter->getName()) == normalized_request_filter_name ||
                   normalizeDepthFilterName(filter->type()) == normalized_request_filter_name;
          };

      auto first_match_it =
          std::find_if(depth_filter_list_.begin(), depth_filter_list_.end(),
                       [&is_same_filter](const auto &filter) { return is_same_filter(filter); });
      if (first_match_it == depth_filter_list_.end()) {
        fail("Filter '" + normalized_request_filter_name + "' is not supported by this device");
        return;
      }
      const auto &existing_filter = *first_match_it;
      if (!existing_filter) {
        fail("Filter '" + normalized_request_filter_name + "' is not supported by this device");
        return;
      }

      if (normalized_request_filter_name == "DecimationFilter") {
        auto decimation_filter = existing_filter->as<ob::DecimationFilter>();
        decimation_filter->enable(request->filter_enable);
        if (request->filter_param.size() > 0) {
          auto range = decimation_filter->getScaleRange();
          auto decimation_filter_scale = request->filter_param[0];
          if (decimation_filter_scale <= range.max && decimation_filter_scale >= range.min) {
            RCLCPP_INFO_STREAM(logger_,
                               "Set decimation filter scale value to " << decimation_filter_scale);
            decimation_filter->setScaleValue(decimation_filter_scale);
          }
          if (decimation_filter_scale != -1 &&
              (decimation_filter_scale < range.min || decimation_filter_scale > range.max)) {
            RCLCPP_ERROR_STREAM(logger_, "Decimation filter scale value is out of range "
                                             << range.min << " - " << range.max);
            fail("Decimation filter scale value is out of range");
            return;
          }
          if (decimation_filter_scale <= range.max && decimation_filter_scale >= range.min) {
            decimation_filter_scale_ = decimation_filter_scale;
          }
        } else {
          fail("The filter switch setting is successful, but the filter parameter setting fails");
          return;
        }
        enable_decimation_filter_ = request->filter_enable;
      } else if (normalized_request_filter_name == "HDRMerge") {
        auto hdr_merge_filter = existing_filter->as<ob::HdrMerge>();
        hdr_merge_filter->enable(request->filter_enable);
        if (request->filter_param.size() > 3) {
          auto config = OBHdrConfig();
          config.enable = true;
          config.exposure_1 = request->filter_param[0];
          config.gain_1 = request->filter_param[1];
          config.exposure_2 = request->filter_param[2];
          config.gain_2 = request->filter_param[3];
          device_->setStructuredData(OB_STRUCT_DEPTH_HDR_CONFIG,
                                     reinterpret_cast<const uint8_t *>(&config), sizeof(config));
          RCLCPP_INFO_STREAM(logger_, "Set HDR merge filter params: "
                                          << "\nexposure_1: " << request->filter_param[0]
                                          << "\ngain_1: " << request->filter_param[1]
                                          << "\nexposure_2: " << request->filter_param[2]
                                          << "\ngain_2: " << request->filter_param[3]);
          hdr_merge_exposure_1_ = request->filter_param[0];
          hdr_merge_gain_1_ = request->filter_param[1];
          hdr_merge_exposure_2_ = request->filter_param[2];
          hdr_merge_gain_2_ = request->filter_param[3];
        } else {
          fail("The filter switch setting is successful, but the filter parameter setting fails");
          return;
        }
        enable_hdr_merge_ = request->filter_enable;
      } else if (normalized_request_filter_name == "SequenceIdFilter") {
        auto sequenced_filter = existing_filter->as<ob::SequenceIdFilter>();
        sequenced_filter->enable(request->filter_enable);
        if (request->filter_param.size() > 0) {
          sequenced_filter->selectSequenceId(request->filter_param[0]);
          RCLCPP_INFO_STREAM(logger_, "Set sequenced filter selectSequenceId value to "
                                          << request->filter_param[0]);
          sequence_id_filter_id_ = request->filter_param[0];
        } else {
          fail("The filter switch setting is successful, but the filter parameter setting fails");
          return;
        }
        enable_sequence_id_filter_ = request->filter_enable;
      } else if (normalized_request_filter_name == "ThresholdFilter") {
        auto threshold_filter = existing_filter->as<ob::ThresholdFilter>();
        threshold_filter->enable(request->filter_enable);
        if (request->filter_param.size() > 1) {
          auto threshold_filter_min = request->filter_param[0];
          auto threshold_filter_max = request->filter_param[1];
          threshold_filter->setValueRange(threshold_filter_min, threshold_filter_max);
          RCLCPP_INFO_STREAM(logger_, "Set threshold filter value range to "
                                          << threshold_filter_min << " - " << threshold_filter_max);
          threshold_filter_min_ = threshold_filter_min;
          threshold_filter_max_ = threshold_filter_max;
        } else {
          fail("The filter switch setting is successful, but the filter parameter setting fails");
          return;
        }
        enable_threshold_filter_ = request->filter_enable;
      } else if (normalized_request_filter_name == "SpatialAdvancedFilter") {
        auto spatial_filter = existing_filter->as<ob::SpatialAdvancedFilter>();
        spatial_filter->enable(request->filter_enable);
        if (request->filter_param.size() > 3) {
          OBSpatialAdvancedFilterParams params{};
          params.alpha = request->filter_param[0];
          params.disp_diff = request->filter_param[1];
          params.magnitude = request->filter_param[2];
          params.radius = request->filter_param[3];
          spatial_filter->setFilterParams(params);
          RCLCPP_INFO_STREAM(logger_, "Set SpatialFilter params: "
                                          << "\nalpha:" << params.alpha
                                          << "\ndisp_diff:" << params.disp_diff
                                          << "\nmagnitude:" << static_cast<int>(params.magnitude)
                                          << "\nradius:" << params.radius);
          spatial_filter_alpha_ = params.alpha;
          spatial_filter_diff_threshold_ = params.disp_diff;
          spatial_filter_magnitude_ = params.magnitude;
          spatial_filter_radius_ = params.radius;
        } else {
          fail("The filter switch setting is successful, but the filter parameter setting fails");
          return;
        }
        enable_spatial_filter_ = request->filter_enable;
      } else if (normalized_request_filter_name == "TemporalFilter") {
        auto temporal_filter = existing_filter->as<ob::TemporalFilter>();
        temporal_filter->enable(request->filter_enable);
        if (request->filter_param.size() > 1) {
          temporal_filter->setDiffScale(request->filter_param[0]);
          temporal_filter->setWeight(request->filter_param[1]);
          RCLCPP_INFO_STREAM(
              logger_, "Set TemporalFilter params: " << "\ndiff_scale:" << request->filter_param[0]
                                                     << "\nweight:" << request->filter_param[1]);
          temporal_filter_diff_threshold_ = request->filter_param[0];
          temporal_filter_weight_ = request->filter_param[1];
        } else {
          fail("The filter switch setting is successful, but the filter parameter setting fails");
          return;
        }
        enable_temporal_filter_ = request->filter_enable;
      } else if (normalized_request_filter_name == "SpatialFastFilter") {
        auto spatial_fast_filter = existing_filter->as<ob::SpatialFastFilter>();
        spatial_fast_filter->enable(request->filter_enable);
        if (request->filter_param.size() > 0) {
          OBSpatialFastFilterParams params{};
          params.radius = request->filter_param[0];
          spatial_fast_filter->setFilterParams(params);
          RCLCPP_INFO_STREAM(logger_,
                             "Set SpatialFastFilter radius to " << static_cast<int>(params.radius));
          spatial_fast_filter_radius_ = params.radius;
        } else {
          fail("The filter switch setting is successful, but the filter parameter setting fails");
          return;
        }
        enable_spatial_fast_filter_ = request->filter_enable;
      } else if (normalized_request_filter_name == "SpatialModerateFilter") {
        auto spatial_moderate_filter = existing_filter->as<ob::SpatialModerateFilter>();
        spatial_moderate_filter->enable(request->filter_enable);
        if (request->filter_param.size() > 2) {
          OBSpatialModerateFilterParams params{};
          params.disp_diff = request->filter_param[0];
          params.magnitude = request->filter_param[1];
          params.radius = request->filter_param[2];
          spatial_moderate_filter->setFilterParams(params);
          RCLCPP_INFO_STREAM(logger_, "Set SpatialModerateFilter params: "
                                          << "\ndisp_diff:" << params.disp_diff
                                          << "\nmagnitude:" << static_cast<int>(params.magnitude)
                                          << "\nradius:" << static_cast<int>(params.radius));
          spatial_moderate_filter_diff_threshold_ = params.disp_diff;
          spatial_moderate_filter_magnitude_ = params.magnitude;
          spatial_moderate_filter_radius_ = params.radius;
        } else {
          fail("The filter switch setting is successful, but the filter parameter setting fails");
          return;
        }
        enable_spatial_moderate_filter_ = request->filter_enable;
      } else if (normalized_request_filter_name == "FalsePositiveFilter") {
        auto false_positive_filter = existing_filter->as<ob::FalsePositiveFilter>();
        false_positive_filter->enable(request->filter_enable);
        enable_false_positive_filter_ = request->filter_enable;
      } else if (normalized_request_filter_name == "MgcNoiseRemovalFilter") {
        auto mgc_filter = existing_filter->as<ob::MgcNoiseRemovalFilter>();
        mgc_filter->enable(request->filter_enable);
        enable_mgc_noise_removal_filter_ = request->filter_enable;
      } else if (normalized_request_filter_name == "LutNoiseRemovalFilter") {
        auto lut_filter = existing_filter->as<ob::LutNoiseRemovalFilter>();
        lut_filter->enable(request->filter_enable);
        enable_lut_noise_removal_filter_ = request->filter_enable;
      } else if (normalized_request_filter_name == "EdgeNoiseRemovalFilter") {
        existing_filter->enable(request->filter_enable);
        enable_edge_noise_removal_filter_ = request->filter_enable;
      } else {
        fail(normalized_request_filter_name + " cannot be set");
        return;
      }
    }
    filter_status_[normalized_request_filter_name] = static_cast<bool>(request->filter_enable);
    publishDepthFiltersStatus();
    response->success = true;
  } catch (const ob::Error &e) {
    response->success = false;
    response->message = "Failed to set filter: " + orbbec_camera::formatObErrorWithStatus(e);
    RCLCPP_ERROR_STREAM(logger_,
                        "Failed to set filter: " << orbbec_camera::formatObErrorWithStatus(e));
  } catch (const std::exception &e) {
    response->success = false;
    response->message = std::string("Failed to set filter: ") + e.what();
    RCLCPP_ERROR_STREAM(logger_, "Failed to set filter: " << e.what());
  } catch (...) {
    response->success = false;
    response->message = "unknown error";
    RCLCPP_ERROR_STREAM(logger_, "unknown error");
  }
}
bool OBCameraNode::isWriteCustomerDataSuccess() const {
  return write_customer_data_success_.load();
}
}  // namespace orbbec_camera