From 32232c8929fd1fe657c98823100e8c21b77a9442 Mon Sep 17 00:00:00 2001 From: ob-yalian Date: Wed, 8 Apr 2026 21:24:32 +0800 Subject: [PATCH] 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