From 32232c8929fd1fe657c98823100e8c21b77a9442 Mon Sep 17 00:00:00 2001 From: ob-yalian Date: Wed, 8 Apr 2026 21:24:32 +0800 Subject: [PATCH 01/11] feat: add depth filter message types and implement depth filter status publishing --- .../include/orbbec_camera/ob_camera_node.h | 15 ++ orbbec_camera/src/ob_camera_node.cpp | 149 ++++++++++++++++++ orbbec_camera_msgs/CMakeLists.txt | 3 + orbbec_camera_msgs/msg/DepthFilterParam.msg | 2 + orbbec_camera_msgs/msg/DepthFilterState.msg | 3 + orbbec_camera_msgs/msg/DepthFiltersStatus.msg | 2 + 6 files changed, 174 insertions(+) create mode 100644 orbbec_camera_msgs/msg/DepthFilterParam.msg create mode 100644 orbbec_camera_msgs/msg/DepthFilterState.msg create mode 100644 orbbec_camera_msgs/msg/DepthFiltersStatus.msg diff --git a/orbbec_camera/include/orbbec_camera/ob_camera_node.h b/orbbec_camera/include/orbbec_camera/ob_camera_node.h index e1b67f3a..8bac9b55 100644 --- a/orbbec_camera/include/orbbec_camera/ob_camera_node.h +++ b/orbbec_camera/include/orbbec_camera/ob_camera_node.h @@ -47,6 +47,9 @@ #include "libobsensor/ObSensor.hpp" #include "orbbec_camera_msgs/msg/device_info.hpp" +#include "orbbec_camera_msgs/msg/depth_filter_param.hpp" +#include "orbbec_camera_msgs/msg/depth_filter_state.hpp" +#include "orbbec_camera_msgs/msg/depth_filters_status.hpp" #include "orbbec_camera_msgs/srv/get_device_info.hpp" #include "orbbec_camera_msgs/msg/extrinsics.hpp" #include "orbbec_camera_msgs/msg/metadata.hpp" @@ -117,6 +120,8 @@ using SetFilter = orbbec_camera_msgs::srv::SetFilter; using SetArrays = orbbec_camera_msgs::srv::SetArrays; using SetUserCalibParams = orbbec_camera_msgs::srv::SetUserCalibParams; using GetUserCalibParams = orbbec_camera_msgs::srv::GetUserCalibParams; +using DepthFilterState = orbbec_camera_msgs::msg::DepthFilterState; +using DepthFiltersStatus = orbbec_camera_msgs::msg::DepthFiltersStatus; typedef std::pair stream_index_pair; @@ -256,6 +261,15 @@ class OBCameraNode { void setupPublishers(); + void publishDepthFiltersStatus(); + + DepthFilterState buildDepthFilterState(const std::string &filter_name, bool enabled) const; + + static std::string normalizeDepthFilterName(const std::string &filter_name); + + static void appendDepthFilterParam(DepthFilterState &filter_state, const std::string &name, + const std::string &value); + void setupCameraInfo(); void publishStaticTF(const rclcpp::Time& t, const tf2::Vector3& trans, const tf2::Quaternion& q, @@ -822,6 +836,7 @@ class OBCameraNode { int spatial_moderate_filter_diff_threshold_ = -1; int spatial_moderate_filter_magnitude_ = -1; rclcpp::Publisher::SharedPtr filter_status_pub_; + rclcpp::Publisher::SharedPtr depth_filters_status_pub_; nlohmann::json filter_status_; std::string align_mode_ = "HW"; std::unique_ptr diagnostic_updater_ = nullptr; diff --git a/orbbec_camera/src/ob_camera_node.cpp b/orbbec_camera/src/ob_camera_node.cpp index 70fa9811..0d5f33c4 100644 --- a/orbbec_camera/src/ob_camera_node.cpp +++ b/orbbec_camera/src/ob_camera_node.cpp @@ -38,6 +38,115 @@ namespace orbbec_camera { using namespace std::chrono_literals; +std::string OBCameraNode::normalizeDepthFilterName(const std::string &filter_name) { + if (filter_name == "HardwareNoiseRemoval") { + return "HardwareNoiseRemovalFilter"; + } + return filter_name; +} + +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 { + const auto normalized_filter_name = normalizeDepthFilterName(filter_name); + DepthFilterState filter_state; + filter_state.filter_name = 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 == "DecimationFilter") { + appendDepthFilterParam(filter_state, "decimation_filter_scale", to_param_value(decimation_filter_scale_)); + } else if (normalized_filter_name == "HDRMerge") { + appendDepthFilterParam(filter_state, "hdr_merge_exposure_1", to_param_value(hdr_merge_exposure_1_)); + appendDepthFilterParam(filter_state, "hdr_merge_gain_1", to_param_value(hdr_merge_gain_1_)); + appendDepthFilterParam(filter_state, "hdr_merge_exposure_2", to_param_value(hdr_merge_exposure_2_)); + appendDepthFilterParam(filter_state, "hdr_merge_gain_2", to_param_value(hdr_merge_gain_2_)); + } else if (normalized_filter_name == "SequenceIdFilter") { + appendDepthFilterParam(filter_state, "sequence_id_filter_id", to_param_value(sequence_id_filter_id_)); + } else if (normalized_filter_name == "ThresholdFilter") { + appendDepthFilterParam(filter_state, "threshold_filter_min", to_param_value(threshold_filter_min_)); + appendDepthFilterParam(filter_state, "threshold_filter_max", to_param_value(threshold_filter_max_)); + } else if (normalized_filter_name == "NoiseRemovalFilter") { + appendDepthFilterParam(filter_state, "noise_removal_filter_min_diff", + to_param_value(noise_removal_filter_min_diff_)); + appendDepthFilterParam(filter_state, "noise_removal_filter_max_size", + to_param_value(noise_removal_filter_max_size_)); + } else if (normalized_filter_name == "HardwareNoiseRemovalFilter") { + appendDepthFilterParam(filter_state, "hardware_noise_removal_filter_threshold", + to_param_value(hardware_noise_removal_filter_threshold_)); + } else if (normalized_filter_name == "SpatialAdvancedFilter") { + appendDepthFilterParam(filter_state, "spatial_filter_alpha", to_param_value(spatial_filter_alpha_)); + appendDepthFilterParam(filter_state, "spatial_filter_diff_threshold", + to_param_value(spatial_filter_diff_threshold_)); + appendDepthFilterParam(filter_state, "spatial_filter_magnitude", + to_param_value(spatial_filter_magnitude_)); + appendDepthFilterParam(filter_state, "spatial_filter_radius", to_param_value(spatial_filter_radius_)); + } else if (normalized_filter_name == "TemporalFilter") { + appendDepthFilterParam(filter_state, "temporal_filter_diff_threshold", + to_param_value(temporal_filter_diff_threshold_)); + appendDepthFilterParam(filter_state, "temporal_filter_weight", to_param_value(temporal_filter_weight_)); + } else if (normalized_filter_name == "HoleFillingFilter") { + appendDepthFilterParam(filter_state, "hole_filling_filter_mode", hole_filling_filter_mode_); + } else if (normalized_filter_name == "DisparityTransform") { + appendDepthFilterParam(filter_state, "disparity_to_depth_mode", disparity_to_depth_mode_); + } else if (normalized_filter_name == "SpatialFastFilter") { + appendDepthFilterParam(filter_state, "spatial_fast_filter_radius", + to_param_value(spatial_fast_filter_radius_)); + } else if (normalized_filter_name == "SpatialModerateFilter") { + appendDepthFilterParam(filter_state, "spatial_moderate_filter_diff_threshold", + to_param_value(spatial_moderate_filter_diff_threshold_)); + appendDepthFilterParam(filter_state, "spatial_moderate_filter_magnitude", + to_param_value(spatial_moderate_filter_magnitude_)); + appendDepthFilterParam(filter_state, "spatial_moderate_filter_radius", + to_param_value(spatial_moderate_filter_radius_)); + } + + return filter_state; +} + +void OBCameraNode::publishDepthFiltersStatus() { + if (!depth_filters_status_pub_) { + return; + } + DepthFiltersStatus msg; + msg.header.stamp = node_->now(); + msg.header.frame_id = camera_name_; + + const std::vector> filter_states = { + {"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_}, + {"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_}, + {"NoiseRemovalFilter", enable_noise_removal_filter_}, + {"HardwareNoiseRemovalFilter", enable_hardware_noise_removal_filter_}, + }; + msg.filters.reserve(filter_states.size()); + for (const auto &filter_state : filter_states) { + msg.filters.push_back(buildDepthFilterState(filter_state.first, filter_state.second)); + } + depth_filters_status_pub_->publish(msg); +} + OBCameraNode::OBCameraNode(rclcpp::Node *node, std::shared_ptr device, std::shared_ptr parameters, bool use_intra_process) : node_(node), @@ -2722,6 +2831,9 @@ void OBCameraNode::setupPublishers() { std_msgs::msg::String msg; msg.data = filter_status_.dump(2); filter_status_pub_->publish(msg); + depth_filters_status_pub_ = + node_->create_publisher("depth_filters/status", extrinsics_qos); + publishDepthFiltersStatus(); } void OBCameraNode::publishPointCloud(const std::shared_ptr &frame_set) { @@ -4547,11 +4659,15 @@ void OBCameraNode::setFilterCallback(const std::shared_ptr RCLCPP_ERROR_STREAM(logger_, "Decimation filter scale value is out of range " << range.min << " - " << range.max); } + if (decimation_filter_scale <= range.max && decimation_filter_scale >= range.min) { + decimation_filter_scale_ = decimation_filter_scale; + } } else { response->message = "The filter switch setting is successful, but the filter parameter setting fails"; return; } + enable_decimation_filter_ = request->filter_enable; } else if (request->filter_name == "HDRMerge") { auto hdr_merge_filter = std::make_shared(); @@ -4571,11 +4687,16 @@ void OBCameraNode::setFilterCallback(const std::shared_ptr << "\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 { response->message = "The filter switch setting is successful, but the filter parameter setting fails"; return; } + enable_hdr_merge_ = request->filter_enable; } else if (request->filter_name == "SequenceIdFilter") { auto sequenced_filter = std::make_shared(); @@ -4585,11 +4706,13 @@ void OBCameraNode::setFilterCallback(const std::shared_ptr 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 { response->message = "The filter switch setting is successful, but the filter parameter setting fails"; return; } + enable_sequence_id_filter_ = request->filter_enable; } else if (request->filter_name == "ThresholdFilter") { auto threshold_filter = std::make_shared(); @@ -4601,11 +4724,14 @@ void OBCameraNode::setFilterCallback(const std::shared_ptr 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 { response->message = "The filter switch setting is successful, but the filter parameter setting fails"; return; } + enable_threshold_filter_ = request->filter_enable; } else if (request->filter_name == "NoiseRemovalFilter") { if (device_->isPropertySupported(OB_PROP_DEPTH_SOFT_FILTER_BOOL, OB_PERMISSION_READ_WRITE)) { @@ -4622,6 +4748,7 @@ void OBCameraNode::setFilterCallback(const std::shared_ptr device_->getIntProperty(OB_PROP_DEPTH_MAX_DIFF_INT); RCLCPP_INFO_STREAM(logger_, "after 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)) { auto default_noise_removal_filter_max_size = @@ -4633,12 +4760,14 @@ void OBCameraNode::setFilterCallback(const std::shared_ptr device_->getIntProperty(OB_PROP_DEPTH_MAX_SPECKLE_SIZE_INT); RCLCPP_INFO_STREAM(logger_, "after set noise removal filter max size: " << new_noise_removal_filter_max_size); + noise_removal_filter_max_size_ = request->filter_param[1]; } } else { response->message = "The filter switch setting is successful, but the filter parameter setting fails"; return; } + enable_noise_removal_filter_ = request->filter_enable; } else if (request->filter_name == "HardwareNoiseRemoval") { if (device_->isPropertySupported(OB_PROP_HW_NOISE_REMOVE_FILTER_ENABLE_BOOL, OB_PERMISSION_READ_WRITE)) { @@ -4652,6 +4781,7 @@ void OBCameraNode::setFilterCallback(const std::shared_ptr request->filter_param[0]); RCLCPP_INFO_STREAM(logger_, "Setting hardware noise removal filter threshold :" << request->filter_param[0]); + hardware_noise_removal_filter_threshold_ = request->filter_param[0]; } } else { response->message = @@ -4659,6 +4789,7 @@ void OBCameraNode::setFilterCallback(const std::shared_ptr return; } } + enable_hardware_noise_removal_filter_ = request->filter_enable; } else if (request->filter_name == "SpatialAdvancedFilter") { auto spatial_filter = std::make_shared(); spatial_filter->enable(request->filter_enable); @@ -4675,11 +4806,16 @@ void OBCameraNode::setFilterCallback(const std::shared_ptr << "\ndisp_diff:" << params.disp_diff << "\nmagnitude:" << static_cast(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 { response->message = "The filter switch setting is successful, but the filter parameter setting fails"; return; } + enable_spatial_filter_ = request->filter_enable; } else if (request->filter_name == "TemporalFilter") { auto temporal_filter = std::make_shared(); temporal_filter->enable(request->filter_enable); @@ -4690,11 +4826,14 @@ void OBCameraNode::setFilterCallback(const std::shared_ptr 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 { response->message = "The filter switch setting is successful, but the filter parameter setting fails"; return; } + enable_temporal_filter_ = request->filter_enable; } else if (request->filter_name == "SpatialFastFilter") { auto spatial_fast_filter = std::make_shared(); spatial_fast_filter->enable(request->filter_enable); @@ -4705,11 +4844,13 @@ void OBCameraNode::setFilterCallback(const std::shared_ptr spatial_fast_filter->setFilterParams(params); RCLCPP_INFO_STREAM(logger_, "Set SpatialFastFilter radius to " << static_cast(params.radius)); + spatial_fast_filter_radius_ = params.radius; } else { response->message = "The filter switch setting is successful, but the filter parameter setting fails"; return; } + enable_spatial_fast_filter_ = request->filter_enable; } else if (request->filter_name == "SpatialModerateFilter") { auto spatial_moderate_filter = std::make_shared(); @@ -4725,23 +4866,30 @@ void OBCameraNode::setFilterCallback(const std::shared_ptr << "\ndisp_diff:" << params.disp_diff << "\nmagnitude:" << static_cast(params.magnitude) << "\nradius:" << static_cast(params.radius)); + spatial_moderate_filter_diff_threshold_ = params.disp_diff; + spatial_moderate_filter_magnitude_ = params.magnitude; + spatial_moderate_filter_radius_ = params.radius; } else { response->message = "The filter switch setting is successful, but the filter parameter setting fails"; return; } + enable_spatial_moderate_filter_ = request->filter_enable; } else if (request->filter_name == "FalsePositiveFilter") { auto false_positive_filter = std::make_shared(); false_positive_filter->enable(request->filter_enable); depth_filter_list_.push_back(false_positive_filter); + enable_false_positive_filter_ = request->filter_enable; } else if (request->filter_name == "MgcNoiseRemovalFilter") { auto mgc_filter = std::make_shared(); mgc_filter->enable(request->filter_enable); depth_filter_list_.push_back(mgc_filter); + enable_mgc_noise_removal_filter_ = request->filter_enable; } else if (request->filter_name == "LutNoiseRemovalFilter") { auto lut_filter = std::make_shared(); lut_filter->enable(request->filter_enable); depth_filter_list_.push_back(lut_filter); + enable_lut_noise_removal_filter_ = request->filter_enable; } else { RCLCPP_INFO_STREAM(logger_, request->filter_name @@ -4770,6 +4918,7 @@ void OBCameraNode::setFilterCallback(const std::shared_ptr msg.data = filter_status_.dump(2); filter_status_pub_->publish(msg); } + publishDepthFiltersStatus(); response->success = true; } catch (const ob::Error &e) { response->message = orbbec_camera::formatObErrorWithStatus(e); diff --git a/orbbec_camera_msgs/CMakeLists.txt b/orbbec_camera_msgs/CMakeLists.txt index a862e984..09dfbf50 100644 --- a/orbbec_camera_msgs/CMakeLists.txt +++ b/orbbec_camera_msgs/CMakeLists.txt @@ -18,6 +18,9 @@ rosidl_generate_interfaces( ${PROJECT_NAME} "msg/DeviceInfo.msg" "msg/DeviceStatus.msg" + "msg/DepthFilterParam.msg" + "msg/DepthFilterState.msg" + "msg/DepthFiltersStatus.msg" "msg/Extrinsics.msg" "msg/Metadata.msg" "msg/IMUInfo.msg" diff --git a/orbbec_camera_msgs/msg/DepthFilterParam.msg b/orbbec_camera_msgs/msg/DepthFilterParam.msg new file mode 100644 index 00000000..5a766001 --- /dev/null +++ b/orbbec_camera_msgs/msg/DepthFilterParam.msg @@ -0,0 +1,2 @@ +string name +string value diff --git a/orbbec_camera_msgs/msg/DepthFilterState.msg b/orbbec_camera_msgs/msg/DepthFilterState.msg new file mode 100644 index 00000000..11b26636 --- /dev/null +++ b/orbbec_camera_msgs/msg/DepthFilterState.msg @@ -0,0 +1,3 @@ +string filter_name +bool enabled +DepthFilterParam[] params diff --git a/orbbec_camera_msgs/msg/DepthFiltersStatus.msg b/orbbec_camera_msgs/msg/DepthFiltersStatus.msg new file mode 100644 index 00000000..73c71219 --- /dev/null +++ b/orbbec_camera_msgs/msg/DepthFiltersStatus.msg @@ -0,0 +1,2 @@ +std_msgs/Header header +DepthFilterState[] filters From 741b7ad250d8fcd43fa8159dbb9ccbc72988d4cd Mon Sep 17 00:00:00 2001 From: ob-yalian Date: Thu, 9 Apr 2026 16:07:43 +0800 Subject: [PATCH 02/11] feat: enhance depth filter status management and improve filter handling logic --- orbbec_camera/src/ob_camera_node.cpp | 234 ++++++++++++++++++++++----- 1 file changed, 193 insertions(+), 41 deletions(-) diff --git a/orbbec_camera/src/ob_camera_node.cpp b/orbbec_camera/src/ob_camera_node.cpp index 0d5f33c4..3ed347bd 100644 --- a/orbbec_camera/src/ob_camera_node.cpp +++ b/orbbec_camera/src/ob_camera_node.cpp @@ -119,30 +119,176 @@ void OBCameraNode::publishDepthFiltersStatus() { if (!depth_filters_status_pub_) { return; } + + auto find_depth_filter = [this](const std::string &filter_name) -> std::shared_ptr { + const auto normalized_name = normalizeDepthFilterName(filter_name); + auto it = std::find_if(depth_filter_list_.begin(), depth_filter_list_.end(), + [&normalized_name](const auto &filter) { + return normalizeDepthFilterName(filter->type()) == + normalized_name || + normalizeDepthFilterName(filter->getName()) == + normalized_name; + }); + if (it == depth_filter_list_.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("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 (auto filter = find_depth_filter("DecimationFilter")) { + try { + decimation_filter_scale_ = static_cast(filter->as()->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()->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(filter->getConfigValue("min")); + threshold_filter_max_ = static_cast(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()->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(filter->getConfigValue("diff_scale")); + temporal_filter_weight_ = static_cast(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()->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()->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 std::vector> filter_states = { - {"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_}, - {"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_}, - {"NoiseRemovalFilter", enable_noise_removal_filter_}, - {"HardwareNoiseRemovalFilter", enable_hardware_noise_removal_filter_}, - }; - msg.filters.reserve(filter_states.size()); - for (const auto &filter_state : filter_states) { - msg.filters.push_back(buildDepthFilterState(filter_state.first, filter_state.second)); + std::vector ordered_filter_names; + ordered_filter_names.reserve(depth_filter_list_.size()); + for (const auto &filter : depth_filter_list_) { + if (!filter) { + continue; + } + const auto normalized_name = normalizeDepthFilterName(filter->type()); + if (std::find(ordered_filter_names.begin(), ordered_filter_names.end(), normalized_name) == + ordered_filter_names.end()) { + ordered_filter_names.push_back(normalized_name); + } + } + + msg.filters.reserve(ordered_filter_names.size()); + for (const auto &filter_name : ordered_filter_names) { + bool enabled = false; + if (auto filter = find_depth_filter(filter_name)) { + try { + enabled = filter->isEnabled(); + } catch (const std::exception &) { + // Keep default value when runtime querying fails. + } + } + msg.filters.push_back(buildDepthFilterState(filter_name, enabled)); } depth_filters_status_pub_->publish(msg); } @@ -4609,13 +4755,16 @@ orbbec_camera_msgs::msg::IMUInfo OBCameraNode::createIMUInfo( void OBCameraNode::setFilterCallback(const std::shared_ptr &request, std::shared_ptr &response) { try { + const auto normalized_request_filter_name = normalizeDepthFilterName(request->filter_name); const bool in_recommended_filter_list = std::find_if(depth_filter_list_.begin(), depth_filter_list_.end(), - [&request](const auto &filter) { - return filter->type() == request->filter_name; + [&normalized_request_filter_name](const auto &filter) { + return normalizeDepthFilterName(filter->type()) == + normalized_request_filter_name; }) != depth_filter_list_.end(); - const bool is_noise_removal_filter = request->filter_name == "NoiseRemovalFilter"; - const bool is_hardware_noise_removal = request->filter_name == "HardwareNoiseRemoval"; + const bool is_noise_removal_filter = normalized_request_filter_name == "NoiseRemovalFilter"; + const bool is_hardware_noise_removal = + normalized_request_filter_name == "HardwareNoiseRemovalFilter"; const bool noise_removal_property_writable = device_->isPropertySupported(OB_PROP_DEPTH_SOFT_FILTER_BOOL, OB_PERMISSION_READ_WRITE) || device_->isPropertySupported(OB_PROP_DEPTH_MAX_DIFF_INT, OB_PERMISSION_WRITE) || @@ -4638,11 +4787,14 @@ void OBCameraNode::setFilterCallback(const std::shared_ptr RCLCPP_INFO_STREAM(logger_, "filter_name: " << request->filter_name << " filter_enable: " << (request->filter_enable ? "true" : "false")); auto it = std::remove_if(depth_filter_list_.begin(), depth_filter_list_.end(), - [&request](const std::shared_ptr &filter) { - return filter->getName() == request->filter_name; + [&normalized_request_filter_name](const std::shared_ptr &filter) { + return normalizeDepthFilterName(filter->getName()) == + normalized_request_filter_name || + normalizeDepthFilterName(filter->type()) == + normalized_request_filter_name; }); depth_filter_list_.erase(it, depth_filter_list_.end()); - if (request->filter_name == "DecimationFilter") { + if (normalized_request_filter_name == "DecimationFilter") { auto decimation_filter = std::make_shared(); decimation_filter->enable(request->filter_enable); depth_filter_list_.push_back(decimation_filter); @@ -4669,7 +4821,7 @@ void OBCameraNode::setFilterCallback(const std::shared_ptr } enable_decimation_filter_ = request->filter_enable; - } else if (request->filter_name == "HDRMerge") { + } else if (normalized_request_filter_name == "HDRMerge") { auto hdr_merge_filter = std::make_shared(); hdr_merge_filter->enable(request->filter_enable); depth_filter_list_.push_back(hdr_merge_filter); @@ -4698,7 +4850,7 @@ void OBCameraNode::setFilterCallback(const std::shared_ptr } enable_hdr_merge_ = request->filter_enable; - } else if (request->filter_name == "SequenceIdFilter") { + } else if (normalized_request_filter_name == "SequenceIdFilter") { auto sequenced_filter = std::make_shared(); sequenced_filter->enable(request->filter_enable); depth_filter_list_.push_back(sequenced_filter); @@ -4714,7 +4866,7 @@ void OBCameraNode::setFilterCallback(const std::shared_ptr } enable_sequence_id_filter_ = request->filter_enable; - } else if (request->filter_name == "ThresholdFilter") { + } else if (normalized_request_filter_name == "ThresholdFilter") { auto threshold_filter = std::make_shared(); threshold_filter->enable(request->filter_enable); depth_filter_list_.push_back(threshold_filter); @@ -4733,7 +4885,7 @@ void OBCameraNode::setFilterCallback(const std::shared_ptr } enable_threshold_filter_ = request->filter_enable; - } else if (request->filter_name == "NoiseRemovalFilter") { + } else if (normalized_request_filter_name == "NoiseRemovalFilter") { if (device_->isPropertySupported(OB_PROP_DEPTH_SOFT_FILTER_BOOL, OB_PERMISSION_READ_WRITE)) { device_->setBoolProperty(OB_PROP_DEPTH_SOFT_FILTER_BOOL, request->filter_enable); } @@ -4768,7 +4920,7 @@ void OBCameraNode::setFilterCallback(const std::shared_ptr return; } enable_noise_removal_filter_ = request->filter_enable; - } else if (request->filter_name == "HardwareNoiseRemoval") { + } else if (normalized_request_filter_name == "HardwareNoiseRemovalFilter") { 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, @@ -4790,7 +4942,7 @@ void OBCameraNode::setFilterCallback(const std::shared_ptr } } enable_hardware_noise_removal_filter_ = request->filter_enable; - } else if (request->filter_name == "SpatialAdvancedFilter") { + } else if (normalized_request_filter_name == "SpatialAdvancedFilter") { auto spatial_filter = std::make_shared(); spatial_filter->enable(request->filter_enable); depth_filter_list_.push_back(spatial_filter); @@ -4816,7 +4968,7 @@ void OBCameraNode::setFilterCallback(const std::shared_ptr return; } enable_spatial_filter_ = request->filter_enable; - } else if (request->filter_name == "TemporalFilter") { + } else if (normalized_request_filter_name == "TemporalFilter") { auto temporal_filter = std::make_shared(); temporal_filter->enable(request->filter_enable); depth_filter_list_.push_back(temporal_filter); @@ -4834,7 +4986,7 @@ void OBCameraNode::setFilterCallback(const std::shared_ptr return; } enable_temporal_filter_ = request->filter_enable; - } else if (request->filter_name == "SpatialFastFilter") { + } else if (normalized_request_filter_name == "SpatialFastFilter") { auto spatial_fast_filter = std::make_shared(); spatial_fast_filter->enable(request->filter_enable); depth_filter_list_.push_back(spatial_fast_filter); @@ -4852,7 +5004,7 @@ void OBCameraNode::setFilterCallback(const std::shared_ptr } enable_spatial_fast_filter_ = request->filter_enable; - } else if (request->filter_name == "SpatialModerateFilter") { + } else if (normalized_request_filter_name == "SpatialModerateFilter") { auto spatial_moderate_filter = std::make_shared(); spatial_moderate_filter->enable(request->filter_enable); depth_filter_list_.push_back(spatial_moderate_filter); @@ -4875,17 +5027,17 @@ void OBCameraNode::setFilterCallback(const std::shared_ptr return; } enable_spatial_moderate_filter_ = request->filter_enable; - } else if (request->filter_name == "FalsePositiveFilter") { + } else if (normalized_request_filter_name == "FalsePositiveFilter") { auto false_positive_filter = std::make_shared(); false_positive_filter->enable(request->filter_enable); depth_filter_list_.push_back(false_positive_filter); enable_false_positive_filter_ = request->filter_enable; - } else if (request->filter_name == "MgcNoiseRemovalFilter") { + } else if (normalized_request_filter_name == "MgcNoiseRemovalFilter") { auto mgc_filter = std::make_shared(); mgc_filter->enable(request->filter_enable); depth_filter_list_.push_back(mgc_filter); enable_mgc_noise_removal_filter_ = request->filter_enable; - } else if (request->filter_name == "LutNoiseRemovalFilter") { + } else if (normalized_request_filter_name == "LutNoiseRemovalFilter") { auto lut_filter = std::make_shared(); lut_filter->enable(request->filter_enable); depth_filter_list_.push_back(lut_filter); @@ -4896,7 +5048,7 @@ void OBCameraNode::setFilterCallback(const std::shared_ptr << "Cannot be set\n" << "The filter_name value that can be set is " "DecimationFilter, HDRMerge, SequenceIdFilter, ThresholdFilter, " - "NoiseRemovalFilter, HardwareNoiseRemoval, SpatialAdvancedFilter, " + "NoiseRemovalFilter, HardwareNoiseRemoval/HardwareNoiseRemovalFilter, SpatialAdvancedFilter, " "SpatialFastFilter, SpatialModerateFilter, FalsePositiveFilter and " "TemporalFilter, MgcNoiseRemovalFilter and " "LutNoiseRemovalFilter"); @@ -4912,7 +5064,7 @@ void OBCameraNode::setFilterCallback(const std::shared_ptr << ", " << configSchema.def << ", " << configSchema.desc << "}" << std::endl; } } - filter_status_[request->filter_name] = request->filter_enable; + filter_status_[normalized_request_filter_name] = request->filter_enable; if (filter_status_pub_) { std_msgs::msg::String msg; msg.data = filter_status_.dump(2); From 38d9dc7e4bf9799e08e1a1d31c281f4ebbcaa987 Mon Sep 17 00:00:00 2001 From: ob-yalian Date: Mon, 13 Apr 2026 14:04:37 +0800 Subject: [PATCH 03/11] refactor: align depth filter status params with SDK names --- orbbec_camera/src/ob_camera_node.cpp | 52 ++++++++++++---------------- 1 file changed, 22 insertions(+), 30 deletions(-) diff --git a/orbbec_camera/src/ob_camera_node.cpp b/orbbec_camera/src/ob_camera_node.cpp index 3ed347bd..2dcc951d 100644 --- a/orbbec_camera/src/ob_camera_node.cpp +++ b/orbbec_camera/src/ob_camera_node.cpp @@ -66,50 +66,42 @@ DepthFilterState OBCameraNode::buildDepthFilterState(const std::string &filter_n }; if (normalized_filter_name == "DecimationFilter") { - appendDepthFilterParam(filter_state, "decimation_filter_scale", to_param_value(decimation_filter_scale_)); + appendDepthFilterParam(filter_state, "decimate", to_param_value(decimation_filter_scale_)); } else if (normalized_filter_name == "HDRMerge") { - appendDepthFilterParam(filter_state, "hdr_merge_exposure_1", to_param_value(hdr_merge_exposure_1_)); - appendDepthFilterParam(filter_state, "hdr_merge_gain_1", to_param_value(hdr_merge_gain_1_)); - appendDepthFilterParam(filter_state, "hdr_merge_exposure_2", to_param_value(hdr_merge_exposure_2_)); - appendDepthFilterParam(filter_state, "hdr_merge_gain_2", to_param_value(hdr_merge_gain_2_)); + appendDepthFilterParam(filter_state, "exposure_1", to_param_value(hdr_merge_exposure_1_)); + appendDepthFilterParam(filter_state, "gain_1", to_param_value(hdr_merge_gain_1_)); + appendDepthFilterParam(filter_state, "exposure_2", to_param_value(hdr_merge_exposure_2_)); + appendDepthFilterParam(filter_state, "gain_2", to_param_value(hdr_merge_gain_2_)); } else if (normalized_filter_name == "SequenceIdFilter") { - appendDepthFilterParam(filter_state, "sequence_id_filter_id", to_param_value(sequence_id_filter_id_)); + appendDepthFilterParam(filter_state, "sequenceid", to_param_value(sequence_id_filter_id_)); } else if (normalized_filter_name == "ThresholdFilter") { - appendDepthFilterParam(filter_state, "threshold_filter_min", to_param_value(threshold_filter_min_)); - appendDepthFilterParam(filter_state, "threshold_filter_max", to_param_value(threshold_filter_max_)); + appendDepthFilterParam(filter_state, "min", to_param_value(threshold_filter_min_)); + appendDepthFilterParam(filter_state, "max", to_param_value(threshold_filter_max_)); } else if (normalized_filter_name == "NoiseRemovalFilter") { - appendDepthFilterParam(filter_state, "noise_removal_filter_min_diff", - to_param_value(noise_removal_filter_min_diff_)); - appendDepthFilterParam(filter_state, "noise_removal_filter_max_size", - to_param_value(noise_removal_filter_max_size_)); + 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, "hardware_noise_removal_filter_threshold", + appendDepthFilterParam(filter_state, "threshold", to_param_value(hardware_noise_removal_filter_threshold_)); } else if (normalized_filter_name == "SpatialAdvancedFilter") { - appendDepthFilterParam(filter_state, "spatial_filter_alpha", to_param_value(spatial_filter_alpha_)); - appendDepthFilterParam(filter_state, "spatial_filter_diff_threshold", - to_param_value(spatial_filter_diff_threshold_)); - appendDepthFilterParam(filter_state, "spatial_filter_magnitude", - to_param_value(spatial_filter_magnitude_)); - appendDepthFilterParam(filter_state, "spatial_filter_radius", to_param_value(spatial_filter_radius_)); + appendDepthFilterParam(filter_state, "alpha", to_param_value(spatial_filter_alpha_)); + appendDepthFilterParam(filter_state, "disp_diff", to_param_value(spatial_filter_diff_threshold_)); + appendDepthFilterParam(filter_state, "magnitude", to_param_value(spatial_filter_magnitude_)); + appendDepthFilterParam(filter_state, "radius", to_param_value(spatial_filter_radius_)); } else if (normalized_filter_name == "TemporalFilter") { - appendDepthFilterParam(filter_state, "temporal_filter_diff_threshold", + appendDepthFilterParam(filter_state, "diff_scale", to_param_value(temporal_filter_diff_threshold_)); - appendDepthFilterParam(filter_state, "temporal_filter_weight", to_param_value(temporal_filter_weight_)); + appendDepthFilterParam(filter_state, "weight", to_param_value(temporal_filter_weight_)); } else if (normalized_filter_name == "HoleFillingFilter") { - appendDepthFilterParam(filter_state, "hole_filling_filter_mode", hole_filling_filter_mode_); - } else if (normalized_filter_name == "DisparityTransform") { - appendDepthFilterParam(filter_state, "disparity_to_depth_mode", disparity_to_depth_mode_); + appendDepthFilterParam(filter_state, "hole_filling_mode", hole_filling_filter_mode_); } else if (normalized_filter_name == "SpatialFastFilter") { - appendDepthFilterParam(filter_state, "spatial_fast_filter_radius", - to_param_value(spatial_fast_filter_radius_)); + appendDepthFilterParam(filter_state, "radius", to_param_value(spatial_fast_filter_radius_)); } else if (normalized_filter_name == "SpatialModerateFilter") { - appendDepthFilterParam(filter_state, "spatial_moderate_filter_diff_threshold", + appendDepthFilterParam(filter_state, "disp_diff", to_param_value(spatial_moderate_filter_diff_threshold_)); - appendDepthFilterParam(filter_state, "spatial_moderate_filter_magnitude", + appendDepthFilterParam(filter_state, "magnitude", to_param_value(spatial_moderate_filter_magnitude_)); - appendDepthFilterParam(filter_state, "spatial_moderate_filter_radius", - to_param_value(spatial_moderate_filter_radius_)); + appendDepthFilterParam(filter_state, "radius", to_param_value(spatial_moderate_filter_radius_)); } return filter_state; From c5c5787e5679baf42b1c0772a5b2fe3279cb0682 Mon Sep 17 00:00:00 2001 From: ob-yalian Date: Mon, 13 Apr 2026 14:05:20 +0800 Subject: [PATCH 04/11] feat: publish property-based depth filter states --- orbbec_camera/src/ob_camera_node.cpp | 35 +++++++++++++++++++++++----- 1 file changed, 29 insertions(+), 6 deletions(-) diff --git a/orbbec_camera/src/ob_camera_node.cpp b/orbbec_camera/src/ob_camera_node.cpp index 2dcc951d..57e43140 100644 --- a/orbbec_camera/src/ob_camera_node.cpp +++ b/orbbec_camera/src/ob_camera_node.cpp @@ -257,22 +257,45 @@ void OBCameraNode::publishDepthFiltersStatus() { 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); + std::vector ordered_filter_names; - ordered_filter_names.reserve(depth_filter_list_.size()); + ordered_filter_names.reserve(depth_filter_list_.size() + 2); + 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_filter_list_) { if (!filter) { continue; } - const auto normalized_name = normalizeDepthFilterName(filter->type()); - if (std::find(ordered_filter_names.begin(), ordered_filter_names.end(), normalized_name) == - ordered_filter_names.end()) { - ordered_filter_names.push_back(normalized_name); - } + 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"); } msg.filters.reserve(ordered_filter_names.size()); for (const auto &filter_name : ordered_filter_names) { bool enabled = false; + if (filter_name == "NoiseRemovalFilter") { + enabled = enable_noise_removal_filter_; + } else if (filter_name == "HardwareNoiseRemovalFilter") { + enabled = enable_hardware_noise_removal_filter_; + } if (auto filter = find_depth_filter(filter_name)) { try { enabled = filter->isEnabled(); From f1ee63028eb65db7c99e9e9b5369754106e21fad Mon Sep 17 00:00:00 2001 From: ob-yalian Date: Mon, 13 Apr 2026 14:12:28 +0800 Subject: [PATCH 05/11] fix: synchronize depth filter list access --- .../include/orbbec_camera/ob_camera_node.h | 1 + orbbec_camera/src/ob_camera_node.cpp | 148 +++++++++++++++--- 2 files changed, 129 insertions(+), 20 deletions(-) diff --git a/orbbec_camera/include/orbbec_camera/ob_camera_node.h b/orbbec_camera/include/orbbec_camera/ob_camera_node.h index 8bac9b55..fbd8b1d9 100644 --- a/orbbec_camera/include/orbbec_camera/ob_camera_node.h +++ b/orbbec_camera/include/orbbec_camera/ob_camera_node.h @@ -880,6 +880,7 @@ class OBCameraNode { bool has_first_color_frame_ = false; bool use_intra_process_ = false; std::string cloud_frame_id_; + std::mutex depth_filter_mutex_; std::vector> depth_filter_list_; std::vector> color_filter_list_; std::vector> left_color_filter_list_; diff --git a/orbbec_camera/src/ob_camera_node.cpp b/orbbec_camera/src/ob_camera_node.cpp index 57e43140..e70b2650 100644 --- a/orbbec_camera/src/ob_camera_node.cpp +++ b/orbbec_camera/src/ob_camera_node.cpp @@ -112,16 +112,23 @@ void OBCameraNode::publishDepthFiltersStatus() { return; } - auto find_depth_filter = [this](const std::string &filter_name) -> std::shared_ptr { + std::vector> depth_filters_snapshot; + { + std::lock_guard depth_filter_lock(depth_filter_mutex_); + depth_filters_snapshot = depth_filter_list_; + } + + auto find_depth_filter = + [&depth_filters_snapshot, this](const std::string &filter_name) -> std::shared_ptr { const auto normalized_name = normalizeDepthFilterName(filter_name); - auto it = std::find_if(depth_filter_list_.begin(), depth_filter_list_.end(), + 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_filter_list_.end()) { + if (it == depth_filters_snapshot.end()) { return nullptr; } return *it; @@ -268,14 +275,14 @@ void OBCameraNode::publishDepthFiltersStatus() { OB_PERMISSION_READ_WRITE); std::vector ordered_filter_names; - ordered_filter_names.reserve(depth_filter_list_.size() + 2); + ordered_filter_names.reserve(depth_filters_snapshot.size() + 2); 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_filter_list_) { + for (const auto &filter : depth_filters_snapshot) { if (!filter) { continue; } @@ -3381,6 +3388,7 @@ std::shared_ptr OBCameraNode::processDepthFrameFilter( if (frame == nullptr || frame->getType() != OB_FRAME_DEPTH) { return nullptr; } + std::lock_guard 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()); @@ -4771,12 +4779,16 @@ void OBCameraNode::setFilterCallback(const std::shared_ptr std::shared_ptr &response) { try { const auto normalized_request_filter_name = normalizeDepthFilterName(request->filter_name); - const bool in_recommended_filter_list = - std::find_if(depth_filter_list_.begin(), depth_filter_list_.end(), - [&normalized_request_filter_name](const auto &filter) { - return normalizeDepthFilterName(filter->type()) == - normalized_request_filter_name; - }) != depth_filter_list_.end(); + bool in_recommended_filter_list = false; + { + std::lock_guard depth_filter_lock(depth_filter_mutex_); + in_recommended_filter_list = + std::find_if(depth_filter_list_.begin(), depth_filter_list_.end(), + [&normalized_request_filter_name](const auto &filter) { + return normalizeDepthFilterName(filter->type()) == + normalized_request_filter_name; + }) != depth_filter_list_.end(); + } const bool is_noise_removal_filter = normalized_request_filter_name == "NoiseRemovalFilter"; const bool is_hardware_noise_removal = normalized_request_filter_name == "HardwareNoiseRemovalFilter"; @@ -4801,15 +4813,16 @@ void OBCameraNode::setFilterCallback(const std::shared_ptr RCLCPP_INFO_STREAM(logger_, "filter_name: " << request->filter_name << " filter_enable: " << (request->filter_enable ? "true" : "false")); - auto it = std::remove_if(depth_filter_list_.begin(), depth_filter_list_.end(), - [&normalized_request_filter_name](const std::shared_ptr &filter) { - return normalizeDepthFilterName(filter->getName()) == - normalized_request_filter_name || - normalizeDepthFilterName(filter->type()) == - normalized_request_filter_name; - }); - depth_filter_list_.erase(it, depth_filter_list_.end()); if (normalized_request_filter_name == "DecimationFilter") { + std::unique_lock depth_filter_lock(depth_filter_mutex_); + auto it = std::remove_if(depth_filter_list_.begin(), depth_filter_list_.end(), + [&normalized_request_filter_name](const std::shared_ptr &filter) { + return normalizeDepthFilterName(filter->getName()) == + normalized_request_filter_name || + normalizeDepthFilterName(filter->type()) == + normalized_request_filter_name; + }); + depth_filter_list_.erase(it, depth_filter_list_.end()); auto decimation_filter = std::make_shared(); decimation_filter->enable(request->filter_enable); depth_filter_list_.push_back(decimation_filter); @@ -4837,6 +4850,15 @@ void OBCameraNode::setFilterCallback(const std::shared_ptr enable_decimation_filter_ = request->filter_enable; } else if (normalized_request_filter_name == "HDRMerge") { + std::unique_lock depth_filter_lock(depth_filter_mutex_); + auto it = std::remove_if(depth_filter_list_.begin(), depth_filter_list_.end(), + [&normalized_request_filter_name](const std::shared_ptr &filter) { + return normalizeDepthFilterName(filter->getName()) == + normalized_request_filter_name || + normalizeDepthFilterName(filter->type()) == + normalized_request_filter_name; + }); + depth_filter_list_.erase(it, depth_filter_list_.end()); auto hdr_merge_filter = std::make_shared(); hdr_merge_filter->enable(request->filter_enable); depth_filter_list_.push_back(hdr_merge_filter); @@ -4866,6 +4888,15 @@ void OBCameraNode::setFilterCallback(const std::shared_ptr enable_hdr_merge_ = request->filter_enable; } else if (normalized_request_filter_name == "SequenceIdFilter") { + std::unique_lock depth_filter_lock(depth_filter_mutex_); + auto it = std::remove_if(depth_filter_list_.begin(), depth_filter_list_.end(), + [&normalized_request_filter_name](const std::shared_ptr &filter) { + return normalizeDepthFilterName(filter->getName()) == + normalized_request_filter_name || + normalizeDepthFilterName(filter->type()) == + normalized_request_filter_name; + }); + depth_filter_list_.erase(it, depth_filter_list_.end()); auto sequenced_filter = std::make_shared(); sequenced_filter->enable(request->filter_enable); depth_filter_list_.push_back(sequenced_filter); @@ -4882,6 +4913,15 @@ void OBCameraNode::setFilterCallback(const std::shared_ptr enable_sequence_id_filter_ = request->filter_enable; } else if (normalized_request_filter_name == "ThresholdFilter") { + std::unique_lock depth_filter_lock(depth_filter_mutex_); + auto it = std::remove_if(depth_filter_list_.begin(), depth_filter_list_.end(), + [&normalized_request_filter_name](const std::shared_ptr &filter) { + return normalizeDepthFilterName(filter->getName()) == + normalized_request_filter_name || + normalizeDepthFilterName(filter->type()) == + normalized_request_filter_name; + }); + depth_filter_list_.erase(it, depth_filter_list_.end()); auto threshold_filter = std::make_shared(); threshold_filter->enable(request->filter_enable); depth_filter_list_.push_back(threshold_filter); @@ -4958,6 +4998,15 @@ void OBCameraNode::setFilterCallback(const std::shared_ptr } enable_hardware_noise_removal_filter_ = request->filter_enable; } else if (normalized_request_filter_name == "SpatialAdvancedFilter") { + std::unique_lock depth_filter_lock(depth_filter_mutex_); + auto it = std::remove_if(depth_filter_list_.begin(), depth_filter_list_.end(), + [&normalized_request_filter_name](const std::shared_ptr &filter) { + return normalizeDepthFilterName(filter->getName()) == + normalized_request_filter_name || + normalizeDepthFilterName(filter->type()) == + normalized_request_filter_name; + }); + depth_filter_list_.erase(it, depth_filter_list_.end()); auto spatial_filter = std::make_shared(); spatial_filter->enable(request->filter_enable); depth_filter_list_.push_back(spatial_filter); @@ -4984,6 +5033,15 @@ void OBCameraNode::setFilterCallback(const std::shared_ptr } enable_spatial_filter_ = request->filter_enable; } else if (normalized_request_filter_name == "TemporalFilter") { + std::unique_lock depth_filter_lock(depth_filter_mutex_); + auto it = std::remove_if(depth_filter_list_.begin(), depth_filter_list_.end(), + [&normalized_request_filter_name](const std::shared_ptr &filter) { + return normalizeDepthFilterName(filter->getName()) == + normalized_request_filter_name || + normalizeDepthFilterName(filter->type()) == + normalized_request_filter_name; + }); + depth_filter_list_.erase(it, depth_filter_list_.end()); auto temporal_filter = std::make_shared(); temporal_filter->enable(request->filter_enable); depth_filter_list_.push_back(temporal_filter); @@ -5002,6 +5060,15 @@ void OBCameraNode::setFilterCallback(const std::shared_ptr } enable_temporal_filter_ = request->filter_enable; } else if (normalized_request_filter_name == "SpatialFastFilter") { + std::unique_lock depth_filter_lock(depth_filter_mutex_); + auto it = std::remove_if(depth_filter_list_.begin(), depth_filter_list_.end(), + [&normalized_request_filter_name](const std::shared_ptr &filter) { + return normalizeDepthFilterName(filter->getName()) == + normalized_request_filter_name || + normalizeDepthFilterName(filter->type()) == + normalized_request_filter_name; + }); + depth_filter_list_.erase(it, depth_filter_list_.end()); auto spatial_fast_filter = std::make_shared(); spatial_fast_filter->enable(request->filter_enable); depth_filter_list_.push_back(spatial_fast_filter); @@ -5020,6 +5087,15 @@ void OBCameraNode::setFilterCallback(const std::shared_ptr enable_spatial_fast_filter_ = request->filter_enable; } else if (normalized_request_filter_name == "SpatialModerateFilter") { + std::unique_lock depth_filter_lock(depth_filter_mutex_); + auto it = std::remove_if(depth_filter_list_.begin(), depth_filter_list_.end(), + [&normalized_request_filter_name](const std::shared_ptr &filter) { + return normalizeDepthFilterName(filter->getName()) == + normalized_request_filter_name || + normalizeDepthFilterName(filter->type()) == + normalized_request_filter_name; + }); + depth_filter_list_.erase(it, depth_filter_list_.end()); auto spatial_moderate_filter = std::make_shared(); spatial_moderate_filter->enable(request->filter_enable); depth_filter_list_.push_back(spatial_moderate_filter); @@ -5043,16 +5119,43 @@ void OBCameraNode::setFilterCallback(const std::shared_ptr } enable_spatial_moderate_filter_ = request->filter_enable; } else if (normalized_request_filter_name == "FalsePositiveFilter") { + std::unique_lock depth_filter_lock(depth_filter_mutex_); + auto it = std::remove_if(depth_filter_list_.begin(), depth_filter_list_.end(), + [&normalized_request_filter_name](const std::shared_ptr &filter) { + return normalizeDepthFilterName(filter->getName()) == + normalized_request_filter_name || + normalizeDepthFilterName(filter->type()) == + normalized_request_filter_name; + }); + depth_filter_list_.erase(it, depth_filter_list_.end()); auto false_positive_filter = std::make_shared(); false_positive_filter->enable(request->filter_enable); depth_filter_list_.push_back(false_positive_filter); enable_false_positive_filter_ = request->filter_enable; } else if (normalized_request_filter_name == "MgcNoiseRemovalFilter") { + std::unique_lock depth_filter_lock(depth_filter_mutex_); + auto it = std::remove_if(depth_filter_list_.begin(), depth_filter_list_.end(), + [&normalized_request_filter_name](const std::shared_ptr &filter) { + return normalizeDepthFilterName(filter->getName()) == + normalized_request_filter_name || + normalizeDepthFilterName(filter->type()) == + normalized_request_filter_name; + }); + depth_filter_list_.erase(it, depth_filter_list_.end()); auto mgc_filter = std::make_shared(); mgc_filter->enable(request->filter_enable); depth_filter_list_.push_back(mgc_filter); enable_mgc_noise_removal_filter_ = request->filter_enable; } else if (normalized_request_filter_name == "LutNoiseRemovalFilter") { + std::unique_lock depth_filter_lock(depth_filter_mutex_); + auto it = std::remove_if(depth_filter_list_.begin(), depth_filter_list_.end(), + [&normalized_request_filter_name](const std::shared_ptr &filter) { + return normalizeDepthFilterName(filter->getName()) == + normalized_request_filter_name || + normalizeDepthFilterName(filter->type()) == + normalized_request_filter_name; + }); + depth_filter_list_.erase(it, depth_filter_list_.end()); auto lut_filter = std::make_shared(); lut_filter->enable(request->filter_enable); depth_filter_list_.push_back(lut_filter); @@ -5069,7 +5172,12 @@ void OBCameraNode::setFilterCallback(const std::shared_ptr "LutNoiseRemovalFilter"); return; } - for (auto &filter : depth_filter_list_) { + std::vector> depth_filters_snapshot; + { + std::lock_guard depth_filter_lock(depth_filter_mutex_); + depth_filters_snapshot = depth_filter_list_; + } + for (auto &filter : depth_filters_snapshot) { std::cout << " - " << filter->getName() << ": " << (filter->isEnabled() ? "enabled" : "disabled") << std::endl; auto configSchemaVec = filter->getConfigSchemaVec(); From d195044756233e0cddecafba69a2374d219bf5ed Mon Sep 17 00:00:00 2001 From: ob-yalian Date: Mon, 13 Apr 2026 14:27:26 +0800 Subject: [PATCH 06/11] fix: align set_filter response handling --- orbbec_camera/src/ob_camera_node.cpp | 18 +++++++++++++----- 1 file changed, 13 insertions(+), 5 deletions(-) diff --git a/orbbec_camera/src/ob_camera_node.cpp b/orbbec_camera/src/ob_camera_node.cpp index e70b2650..d2422cad 100644 --- a/orbbec_camera/src/ob_camera_node.cpp +++ b/orbbec_camera/src/ob_camera_node.cpp @@ -4778,6 +4778,12 @@ orbbec_camera_msgs::msg::IMUInfo OBCameraNode::createIMUInfo( void OBCameraNode::setFilterCallback(const std::shared_ptr &request, std::shared_ptr &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); bool in_recommended_filter_list = false; { @@ -4806,8 +4812,7 @@ void OBCameraNode::setFilterCallback(const std::shared_ptr (is_hardware_noise_removal && hardware_noise_removal_property_writable); if (!in_recommended_filter_list && !supported_by_writable_property) { - response->success = false; - response->message = "Filter '" + request->filter_name + "' is not supported by this device"; + fail("Filter '" + request->filter_name + "' is not supported by this device"); return; } @@ -5196,14 +5201,17 @@ void OBCameraNode::setFilterCallback(const std::shared_ptr publishDepthFiltersStatus(); response->success = true; } catch (const ob::Error &e) { - response->message = orbbec_camera::formatObErrorWithStatus(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->message = e.what(); 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->message = "unknown error"; response->success = false; + response->message = "unknown error"; + RCLCPP_ERROR_STREAM(logger_, "unknown error"); } } bool OBCameraNode::isWriteCustomerDataSuccess() const { From 86bda2ab6dbc8ad46b302a82c656f46e01cec45f Mon Sep 17 00:00:00 2001 From: ob-yalian Date: Mon, 13 Apr 2026 14:28:33 +0800 Subject: [PATCH 07/11] fix: align property-backed set_filter behavior --- orbbec_camera/src/ob_camera_node.cpp | 161 +++++++++++++-------------- 1 file changed, 76 insertions(+), 85 deletions(-) diff --git a/orbbec_camera/src/ob_camera_node.cpp b/orbbec_camera/src/ob_camera_node.cpp index d2422cad..3252f258 100644 --- a/orbbec_camera/src/ob_camera_node.cpp +++ b/orbbec_camera/src/ob_camera_node.cpp @@ -4785,40 +4785,88 @@ void OBCameraNode::setFilterCallback(const std::shared_ptr response->message = msg; }; const auto normalized_request_filter_name = normalizeDepthFilterName(request->filter_name); - bool in_recommended_filter_list = false; - { - std::lock_guard depth_filter_lock(depth_filter_mutex_); - in_recommended_filter_list = - std::find_if(depth_filter_list_.begin(), depth_filter_list_.end(), - [&normalized_request_filter_name](const auto &filter) { - return normalizeDepthFilterName(filter->type()) == - normalized_request_filter_name; - }) != depth_filter_list_.end(); - } const bool is_noise_removal_filter = normalized_request_filter_name == "NoiseRemovalFilter"; - const bool is_hardware_noise_removal = + const bool is_hardware_noise_removal_filter = normalized_request_filter_name == "HardwareNoiseRemovalFilter"; - const bool noise_removal_property_writable = - 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_property_writable = - 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 supported_by_writable_property = - (is_noise_removal_filter && noise_removal_property_writable) || - (is_hardware_noise_removal && hardware_noise_removal_property_writable); - - if (!in_recommended_filter_list && !supported_by_writable_property) { - fail("Filter '" + request->filter_name + "' is not supported by this device"); - return; + 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); } RCLCPP_INFO_STREAM(logger_, "filter_name: " << request->filter_name << " filter_enable: " << (request->filter_enable ? "true" : "false")); - if (normalized_request_filter_name == "DecimationFilter") { + if (is_noise_removal_filter || is_hardware_noise_removal_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)) { + auto default_noise_removal_filter_min_diff = + device_->getIntProperty(OB_PROP_DEPTH_MAX_DIFF_INT); + RCLCPP_INFO_STREAM(logger_, "default_noise_removal_filter_min_diff: " + << default_noise_removal_filter_min_diff); + 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_, "after 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)) { + auto default_noise_removal_filter_max_size = + device_->getIntProperty(OB_PROP_DEPTH_MAX_SPECKLE_SIZE_INT); + RCLCPP_INFO_STREAM(logger_, "default_noise_removal_filter_max_size: " + << default_noise_removal_filter_max_size); + 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_, "after 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_, + "Setting 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_, "Setting 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 if (normalized_request_filter_name == "DecimationFilter") { std::unique_lock depth_filter_lock(depth_filter_mutex_); auto it = std::remove_if(depth_filter_list_.begin(), depth_filter_list_.end(), [&normalized_request_filter_name](const std::shared_ptr &filter) { @@ -4945,63 +4993,6 @@ void OBCameraNode::setFilterCallback(const std::shared_ptr } enable_threshold_filter_ = request->filter_enable; - } else if (normalized_request_filter_name == "NoiseRemovalFilter") { - if (device_->isPropertySupported(OB_PROP_DEPTH_SOFT_FILTER_BOOL, OB_PERMISSION_READ_WRITE)) { - device_->setBoolProperty(OB_PROP_DEPTH_SOFT_FILTER_BOOL, request->filter_enable); - } - if (request->filter_param.size() > 1) { - if (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); - RCLCPP_INFO_STREAM(logger_, "default noise removal filter min diff: " - << default_noise_removal_filter_min_diff); - 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_, "after 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)) { - auto default_noise_removal_filter_max_size = - device_->getIntProperty(OB_PROP_DEPTH_MAX_SPECKLE_SIZE_INT); - RCLCPP_INFO_STREAM(logger_, "default noise removal filter max size: " - << default_noise_removal_filter_max_size); - 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_, "after set noise removal filter max size: " - << new_noise_removal_filter_max_size); - noise_removal_filter_max_size_ = request->filter_param[1]; - } - } else { - response->message = - "The filter switch setting is successful, but the filter parameter setting fails"; - return; - } - enable_noise_removal_filter_ = request->filter_enable; - } else if (normalized_request_filter_name == "HardwareNoiseRemovalFilter") { - 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); - 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_, "Setting hardware noise removal filter threshold :" - << request->filter_param[0]); - hardware_noise_removal_filter_threshold_ = request->filter_param[0]; - } - } else { - response->message = - "The filter switch setting is successful, but the filter parameter setting fails"; - return; - } - } - enable_hardware_noise_removal_filter_ = request->filter_enable; } else if (normalized_request_filter_name == "SpatialAdvancedFilter") { std::unique_lock depth_filter_lock(depth_filter_mutex_); auto it = std::remove_if(depth_filter_list_.begin(), depth_filter_list_.end(), From 6253a39c8b347441c443ee156af025ace93b9e6c Mon Sep 17 00:00:00 2001 From: ob-yalian Date: Mon, 13 Apr 2026 14:33:32 +0800 Subject: [PATCH 08/11] fix: preserve depth filter order in set_filter --- orbbec_camera/src/ob_camera_node.cpp | 520 ++++++++++++--------------- 1 file changed, 220 insertions(+), 300 deletions(-) diff --git a/orbbec_camera/src/ob_camera_node.cpp b/orbbec_camera/src/ob_camera_node.cpp index 3252f258..7f18c7ce 100644 --- a/orbbec_camera/src/ob_camera_node.cpp +++ b/orbbec_camera/src/ob_camera_node.cpp @@ -4866,307 +4866,227 @@ void OBCameraNode::setFilterCallback(const std::shared_ptr } enable_hardware_noise_removal_filter_ = request->filter_enable; } - } else if (normalized_request_filter_name == "DecimationFilter") { - std::unique_lock depth_filter_lock(depth_filter_mutex_); - auto it = std::remove_if(depth_filter_list_.begin(), depth_filter_list_.end(), - [&normalized_request_filter_name](const std::shared_ptr &filter) { - return normalizeDepthFilterName(filter->getName()) == - normalized_request_filter_name || - normalizeDepthFilterName(filter->type()) == - normalized_request_filter_name; - }); - depth_filter_list_.erase(it, depth_filter_list_.end()); - auto decimation_filter = std::make_shared(); - decimation_filter->enable(request->filter_enable); - depth_filter_list_.push_back(decimation_filter); - 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); - } - if (decimation_filter_scale <= range.max && decimation_filter_scale >= range.min) { - decimation_filter_scale_ = decimation_filter_scale; - } - } else { - response->message = - "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") { - std::unique_lock depth_filter_lock(depth_filter_mutex_); - auto it = std::remove_if(depth_filter_list_.begin(), depth_filter_list_.end(), - [&normalized_request_filter_name](const std::shared_ptr &filter) { - return normalizeDepthFilterName(filter->getName()) == - normalized_request_filter_name || - normalizeDepthFilterName(filter->type()) == - normalized_request_filter_name; - }); - depth_filter_list_.erase(it, depth_filter_list_.end()); - auto hdr_merge_filter = std::make_shared(); - hdr_merge_filter->enable(request->filter_enable); - depth_filter_list_.push_back(hdr_merge_filter); - 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(&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 { - response->message = - "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") { - std::unique_lock depth_filter_lock(depth_filter_mutex_); - auto it = std::remove_if(depth_filter_list_.begin(), depth_filter_list_.end(), - [&normalized_request_filter_name](const std::shared_ptr &filter) { - return normalizeDepthFilterName(filter->getName()) == - normalized_request_filter_name || - normalizeDepthFilterName(filter->type()) == - normalized_request_filter_name; - }); - depth_filter_list_.erase(it, depth_filter_list_.end()); - auto sequenced_filter = std::make_shared(); - sequenced_filter->enable(request->filter_enable); - depth_filter_list_.push_back(sequenced_filter); - 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 { - response->message = - "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") { - std::unique_lock depth_filter_lock(depth_filter_mutex_); - auto it = std::remove_if(depth_filter_list_.begin(), depth_filter_list_.end(), - [&normalized_request_filter_name](const std::shared_ptr &filter) { - return normalizeDepthFilterName(filter->getName()) == - normalized_request_filter_name || - normalizeDepthFilterName(filter->type()) == - normalized_request_filter_name; - }); - depth_filter_list_.erase(it, depth_filter_list_.end()); - auto threshold_filter = std::make_shared(); - threshold_filter->enable(request->filter_enable); - depth_filter_list_.push_back(threshold_filter); - 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 { - response->message = - "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") { - std::unique_lock depth_filter_lock(depth_filter_mutex_); - auto it = std::remove_if(depth_filter_list_.begin(), depth_filter_list_.end(), - [&normalized_request_filter_name](const std::shared_ptr &filter) { - return normalizeDepthFilterName(filter->getName()) == - normalized_request_filter_name || - normalizeDepthFilterName(filter->type()) == - normalized_request_filter_name; - }); - depth_filter_list_.erase(it, depth_filter_list_.end()); - auto spatial_filter = std::make_shared(); - spatial_filter->enable(request->filter_enable); - depth_filter_list_.push_back(spatial_filter); - 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 SpatialAdvancedFilter params: " - << "\nalpha:" << params.alpha - << "\ndisp_diff:" << params.disp_diff - << "\nmagnitude:" << static_cast(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 { - response->message = - "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") { - std::unique_lock depth_filter_lock(depth_filter_mutex_); - auto it = std::remove_if(depth_filter_list_.begin(), depth_filter_list_.end(), - [&normalized_request_filter_name](const std::shared_ptr &filter) { - return normalizeDepthFilterName(filter->getName()) == - normalized_request_filter_name || - normalizeDepthFilterName(filter->type()) == - normalized_request_filter_name; - }); - depth_filter_list_.erase(it, depth_filter_list_.end()); - auto temporal_filter = std::make_shared(); - temporal_filter->enable(request->filter_enable); - depth_filter_list_.push_back(temporal_filter); - 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 { - response->message = - "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") { - std::unique_lock depth_filter_lock(depth_filter_mutex_); - auto it = std::remove_if(depth_filter_list_.begin(), depth_filter_list_.end(), - [&normalized_request_filter_name](const std::shared_ptr &filter) { - return normalizeDepthFilterName(filter->getName()) == - normalized_request_filter_name || - normalizeDepthFilterName(filter->type()) == - normalized_request_filter_name; - }); - depth_filter_list_.erase(it, depth_filter_list_.end()); - auto spatial_fast_filter = std::make_shared(); - spatial_fast_filter->enable(request->filter_enable); - depth_filter_list_.push_back(spatial_fast_filter); - 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(params.radius)); - spatial_fast_filter_radius_ = params.radius; - } else { - response->message = - "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") { - std::unique_lock depth_filter_lock(depth_filter_mutex_); - auto it = std::remove_if(depth_filter_list_.begin(), depth_filter_list_.end(), - [&normalized_request_filter_name](const std::shared_ptr &filter) { - return normalizeDepthFilterName(filter->getName()) == - normalized_request_filter_name || - normalizeDepthFilterName(filter->type()) == - normalized_request_filter_name; - }); - depth_filter_list_.erase(it, depth_filter_list_.end()); - auto spatial_moderate_filter = std::make_shared(); - spatial_moderate_filter->enable(request->filter_enable); - depth_filter_list_.push_back(spatial_moderate_filter); - 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(params.magnitude) - << "\nradius:" << static_cast(params.radius)); - spatial_moderate_filter_diff_threshold_ = params.disp_diff; - spatial_moderate_filter_magnitude_ = params.magnitude; - spatial_moderate_filter_radius_ = params.radius; - } else { - response->message = - "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") { - std::unique_lock depth_filter_lock(depth_filter_mutex_); - auto it = std::remove_if(depth_filter_list_.begin(), depth_filter_list_.end(), - [&normalized_request_filter_name](const std::shared_ptr &filter) { - return normalizeDepthFilterName(filter->getName()) == - normalized_request_filter_name || - normalizeDepthFilterName(filter->type()) == - normalized_request_filter_name; - }); - depth_filter_list_.erase(it, depth_filter_list_.end()); - auto false_positive_filter = std::make_shared(); - false_positive_filter->enable(request->filter_enable); - depth_filter_list_.push_back(false_positive_filter); - enable_false_positive_filter_ = request->filter_enable; - } else if (normalized_request_filter_name == "MgcNoiseRemovalFilter") { - std::unique_lock depth_filter_lock(depth_filter_mutex_); - auto it = std::remove_if(depth_filter_list_.begin(), depth_filter_list_.end(), - [&normalized_request_filter_name](const std::shared_ptr &filter) { - return normalizeDepthFilterName(filter->getName()) == - normalized_request_filter_name || - normalizeDepthFilterName(filter->type()) == - normalized_request_filter_name; - }); - depth_filter_list_.erase(it, depth_filter_list_.end()); - auto mgc_filter = std::make_shared(); - mgc_filter->enable(request->filter_enable); - depth_filter_list_.push_back(mgc_filter); - enable_mgc_noise_removal_filter_ = request->filter_enable; - } else if (normalized_request_filter_name == "LutNoiseRemovalFilter") { - std::unique_lock depth_filter_lock(depth_filter_mutex_); - auto it = std::remove_if(depth_filter_list_.begin(), depth_filter_list_.end(), - [&normalized_request_filter_name](const std::shared_ptr &filter) { - return normalizeDepthFilterName(filter->getName()) == - normalized_request_filter_name || - normalizeDepthFilterName(filter->type()) == - normalized_request_filter_name; - }); - depth_filter_list_.erase(it, depth_filter_list_.end()); - auto lut_filter = std::make_shared(); - lut_filter->enable(request->filter_enable); - depth_filter_list_.push_back(lut_filter); - enable_lut_noise_removal_filter_ = request->filter_enable; } else { - RCLCPP_INFO_STREAM(logger_, - request->filter_name - << "Cannot be set\n" - << "The filter_name value that can be set is " - "DecimationFilter, HDRMerge, SequenceIdFilter, ThresholdFilter, " - "NoiseRemovalFilter, HardwareNoiseRemoval/HardwareNoiseRemovalFilter, SpatialAdvancedFilter, " - "SpatialFastFilter, SpatialModerateFilter, FalsePositiveFilter and " - "TemporalFilter, MgcNoiseRemovalFilter and " - "LutNoiseRemovalFilter"); - return; + std::unique_lock depth_filter_lock(depth_filter_mutex_); + auto is_same_filter = + [&normalized_request_filter_name](const std::shared_ptr &filter) { + 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; + } + std::size_t filter_insert_pos = + static_cast(std::distance(depth_filter_list_.begin(), first_match_it)); + + auto it = std::remove_if(depth_filter_list_.begin(), depth_filter_list_.end(), + [&is_same_filter](const std::shared_ptr &filter) { + return is_same_filter(filter); + }); + depth_filter_list_.erase(it, depth_filter_list_.end()); + + auto add_or_replace_filter = [&filter_insert_pos, + this](const std::shared_ptr &filter) { + if (!filter) { + return; + } + if (filter_insert_pos <= depth_filter_list_.size()) { + depth_filter_list_.insert( + depth_filter_list_.begin() + static_cast(filter_insert_pos), filter); + } else { + depth_filter_list_.push_back(filter); + } + }; + + if (normalized_request_filter_name == "DecimationFilter") { + auto decimation_filter = std::make_shared(); + decimation_filter->enable(request->filter_enable); + add_or_replace_filter(decimation_filter); + 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 = std::make_shared(); + hdr_merge_filter->enable(request->filter_enable); + add_or_replace_filter(hdr_merge_filter); + 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(&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 = std::make_shared(); + sequenced_filter->enable(request->filter_enable); + add_or_replace_filter(sequenced_filter); + 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 = std::make_shared(); + threshold_filter->enable(request->filter_enable); + add_or_replace_filter(threshold_filter); + 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 = std::make_shared(); + spatial_filter->enable(request->filter_enable); + add_or_replace_filter(spatial_filter); + 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 SpatialAdvancedFilter params: " + << "\nalpha:" << params.alpha + << "\ndisp_diff:" << params.disp_diff + << "\nmagnitude:" << static_cast(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 = std::make_shared(); + temporal_filter->enable(request->filter_enable); + add_or_replace_filter(temporal_filter); + 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 = std::make_shared(); + spatial_fast_filter->enable(request->filter_enable); + add_or_replace_filter(spatial_fast_filter); + 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(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 = std::make_shared(); + spatial_moderate_filter->enable(request->filter_enable); + add_or_replace_filter(spatial_moderate_filter); + 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(params.magnitude) + << "\nradius:" << static_cast(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 = std::make_shared(); + false_positive_filter->enable(request->filter_enable); + add_or_replace_filter(false_positive_filter); + enable_false_positive_filter_ = request->filter_enable; + } else if (normalized_request_filter_name == "MgcNoiseRemovalFilter") { + auto mgc_filter = std::make_shared(); + mgc_filter->enable(request->filter_enable); + add_or_replace_filter(mgc_filter); + enable_mgc_noise_removal_filter_ = request->filter_enable; + } else if (normalized_request_filter_name == "LutNoiseRemovalFilter") { + auto lut_filter = std::make_shared(); + lut_filter->enable(request->filter_enable); + add_or_replace_filter(lut_filter); + enable_lut_noise_removal_filter_ = request->filter_enable; + } else { + fail(normalized_request_filter_name + " cannot be set"); + return; + } } std::vector> depth_filters_snapshot; { From 0965559ffa429d62d472bf096fdd3c0a84eb8f24 Mon Sep 17 00:00:00 2001 From: ob-yalian Date: Mon, 13 Apr 2026 14:34:36 +0800 Subject: [PATCH 09/11] chore: remove set_filter debug dump --- orbbec_camera/src/ob_camera_node.cpp | 17 +---------------- 1 file changed, 1 insertion(+), 16 deletions(-) diff --git a/orbbec_camera/src/ob_camera_node.cpp b/orbbec_camera/src/ob_camera_node.cpp index 7f18c7ce..55309436 100644 --- a/orbbec_camera/src/ob_camera_node.cpp +++ b/orbbec_camera/src/ob_camera_node.cpp @@ -5088,22 +5088,7 @@ void OBCameraNode::setFilterCallback(const std::shared_ptr return; } } - std::vector> depth_filters_snapshot; - { - std::lock_guard depth_filter_lock(depth_filter_mutex_); - depth_filters_snapshot = depth_filter_list_; - } - for (auto &filter : depth_filters_snapshot) { - std::cout << " - " << filter->getName() << ": " - << (filter->isEnabled() ? "enabled" : "disabled") << std::endl; - auto configSchemaVec = filter->getConfigSchemaVec(); - for (auto &configSchema : configSchemaVec) { - std::cout << " - {" << configSchema.name << ", " << configSchema.type << ", " - << configSchema.min << ", " << configSchema.max << ", " << configSchema.step - << ", " << configSchema.def << ", " << configSchema.desc << "}" << std::endl; - } - } - filter_status_[normalized_request_filter_name] = request->filter_enable; + filter_status_[normalized_request_filter_name] = static_cast(request->filter_enable); if (filter_status_pub_) { std_msgs::msg::String msg; msg.data = filter_status_.dump(2); From 8ea74e9bf6648960ac4e7f3305a2ceab2664acc4 Mon Sep 17 00:00:00 2001 From: ob-yalian Date: Mon, 13 Apr 2026 14:50:26 +0800 Subject: [PATCH 10/11] feat: expose dynamic depth filter params in status --- .../include/orbbec_camera/ob_camera_node.h | 3 +- orbbec_camera/src/ob_camera_node.cpp | 40 +++++++++++++++++-- 2 files changed, 38 insertions(+), 5 deletions(-) diff --git a/orbbec_camera/include/orbbec_camera/ob_camera_node.h b/orbbec_camera/include/orbbec_camera/ob_camera_node.h index fbd8b1d9..14d30f85 100644 --- a/orbbec_camera/include/orbbec_camera/ob_camera_node.h +++ b/orbbec_camera/include/orbbec_camera/ob_camera_node.h @@ -263,7 +263,8 @@ class OBCameraNode { void publishDepthFiltersStatus(); - DepthFilterState buildDepthFilterState(const std::string &filter_name, bool enabled) const; + DepthFilterState buildDepthFilterState(const std::string &filter_name, bool enabled, + const std::shared_ptr &filter) const; static std::string normalizeDepthFilterName(const std::string &filter_name); diff --git a/orbbec_camera/src/ob_camera_node.cpp b/orbbec_camera/src/ob_camera_node.cpp index e70b2650..5ca3684a 100644 --- a/orbbec_camera/src/ob_camera_node.cpp +++ b/orbbec_camera/src/ob_camera_node.cpp @@ -53,8 +53,8 @@ void OBCameraNode::appendDepthFilterParam(DepthFilterState &filter_state, const filter_state.params.push_back(param); } -DepthFilterState OBCameraNode::buildDepthFilterState(const std::string &filter_name, - bool enabled) const { +DepthFilterState OBCameraNode::buildDepthFilterState(const std::string &filter_name, bool enabled, + const std::shared_ptr &filter) const { const auto normalized_filter_name = normalizeDepthFilterName(filter_name); DepthFilterState filter_state; filter_state.filter_name = normalized_filter_name; @@ -104,6 +104,37 @@ DepthFilterState OBCameraNode::buildDepthFilterState(const std::string &filter_n appendDepthFilterParam(filter_state, "radius", to_param_value(spatial_moderate_filter_radius_)); } + if (filter_state.params.empty() && filter) { + 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(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, 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; } @@ -298,19 +329,20 @@ void OBCameraNode::publishDepthFiltersStatus() { msg.filters.reserve(ordered_filter_names.size()); for (const auto &filter_name : ordered_filter_names) { 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_; } - if (auto filter = find_depth_filter(filter_name)) { + if (filter) { try { enabled = filter->isEnabled(); } catch (const std::exception &) { // Keep default value when runtime querying fails. } } - msg.filters.push_back(buildDepthFilterState(filter_name, enabled)); + msg.filters.push_back(buildDepthFilterState(filter_name, enabled, filter)); } depth_filters_status_pub_->publish(msg); } From 6eae5cda13ba487df9f8f786c0b5fe59bc925e75 Mon Sep 17 00:00:00 2001 From: ob-yalian Date: Tue, 14 Apr 2026 15:43:02 +0800 Subject: [PATCH 11/11] refactor: streamline depth filter parameter handling in buildDepthFilterState --- orbbec_camera/src/ob_camera_node.cpp | 33 +--------------------------- 1 file changed, 1 insertion(+), 32 deletions(-) diff --git a/orbbec_camera/src/ob_camera_node.cpp b/orbbec_camera/src/ob_camera_node.cpp index a6501c82..cc0f5847 100644 --- a/orbbec_camera/src/ob_camera_node.cpp +++ b/orbbec_camera/src/ob_camera_node.cpp @@ -65,43 +65,12 @@ DepthFilterState OBCameraNode::buildDepthFilterState(const std::string &filter_n return ss.str(); }; - if (normalized_filter_name == "DecimationFilter") { - appendDepthFilterParam(filter_state, "decimate", to_param_value(decimation_filter_scale_)); - } else if (normalized_filter_name == "HDRMerge") { - appendDepthFilterParam(filter_state, "exposure_1", to_param_value(hdr_merge_exposure_1_)); - appendDepthFilterParam(filter_state, "gain_1", to_param_value(hdr_merge_gain_1_)); - appendDepthFilterParam(filter_state, "exposure_2", to_param_value(hdr_merge_exposure_2_)); - appendDepthFilterParam(filter_state, "gain_2", to_param_value(hdr_merge_gain_2_)); - } else if (normalized_filter_name == "SequenceIdFilter") { - appendDepthFilterParam(filter_state, "sequenceid", to_param_value(sequence_id_filter_id_)); - } else if (normalized_filter_name == "ThresholdFilter") { - appendDepthFilterParam(filter_state, "min", to_param_value(threshold_filter_min_)); - appendDepthFilterParam(filter_state, "max", to_param_value(threshold_filter_max_)); - } else if (normalized_filter_name == "NoiseRemovalFilter") { + 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 == "SpatialAdvancedFilter") { - appendDepthFilterParam(filter_state, "alpha", to_param_value(spatial_filter_alpha_)); - appendDepthFilterParam(filter_state, "disp_diff", to_param_value(spatial_filter_diff_threshold_)); - appendDepthFilterParam(filter_state, "magnitude", to_param_value(spatial_filter_magnitude_)); - appendDepthFilterParam(filter_state, "radius", to_param_value(spatial_filter_radius_)); - } else if (normalized_filter_name == "TemporalFilter") { - appendDepthFilterParam(filter_state, "diff_scale", - to_param_value(temporal_filter_diff_threshold_)); - appendDepthFilterParam(filter_state, "weight", to_param_value(temporal_filter_weight_)); - } else if (normalized_filter_name == "HoleFillingFilter") { - appendDepthFilterParam(filter_state, "hole_filling_mode", hole_filling_filter_mode_); - } else if (normalized_filter_name == "SpatialFastFilter") { - appendDepthFilterParam(filter_state, "radius", to_param_value(spatial_fast_filter_radius_)); - } else if (normalized_filter_name == "SpatialModerateFilter") { - appendDepthFilterParam(filter_state, "disp_diff", - to_param_value(spatial_moderate_filter_diff_threshold_)); - appendDepthFilterParam(filter_state, "magnitude", - to_param_value(spatial_moderate_filter_magnitude_)); - appendDepthFilterParam(filter_state, "radius", to_param_value(spatial_moderate_filter_radius_)); } if (filter_state.params.empty() && filter) {