i/OrbbecSDK_ROS2
1
0
Fork 0
mirror of https://github.com/orbbec/OrbbecSDK_ROS2.git synced 2026-10-03 19:47:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

feat: add depth filter message types and implement depth filter status publishing

Browse Source
This commit is contained in:
ob-yalian
2026-04-08 21:24:32 +08:00
parent 31baab180c
commit 32232c8929
6 changed files with 174 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
orbbec_camera/include/orbbec_camera/ob_camera_node.h
+15
View File
@@ -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<ob_stream_type, int> 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<std_msgs::msg::String>::SharedPtr filter_status_pub_;
rclcpp::Publisher<DepthFiltersStatus>::SharedPtr depth_filters_status_pub_;
nlohmann::json filter_status_;
std::string align_mode_ = "HW";
std::unique_ptr<diagnostic_updater::Updater> diagnostic_updater_ = nullptr;
orbbec_camera/src/ob_camera_node.cpp
+149
View File
@@ -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<std::pair<std::string, bool>> 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<ob::Device> device,
std::shared_ptr<Parameters> 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<DepthFiltersStatus>("depth_filters/status", extrinsics_qos);
publishDepthFiltersStatus();
}
void OBCameraNode::publishPointCloud(const std::shared_ptr<ob::FrameSet> &frame_set) {
@@ -4547,11 +4659,15 @@ void OBCameraNode::setFilterCallback(const std::shared_ptr<SetFilter ::Request>
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<ob::HdrMerge>();
@@ -4571,11 +4687,16 @@ void OBCameraNode::setFilterCallback(const std::shared_ptr<SetFilter ::Request>
<< "\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<ob::SequenceIdFilter>();
@@ -4585,11 +4706,13 @@ void OBCameraNode::setFilterCallback(const std::shared_ptr<SetFilter ::Request>
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<ob::ThresholdFilter>();
@@ -4601,11 +4724,14 @@ void OBCameraNode::setFilterCallback(const std::shared_ptr<SetFilter ::Request>
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<SetFilter ::Request>
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<SetFilter ::Request>
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<SetFilter ::Request>
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<SetFilter ::Request>
return;
}
}
enable_hardware_noise_removal_filter_ = request->filter_enable;
} else if (request->filter_name == "SpatialAdvancedFilter") {
auto spatial_filter = std::make_shared<ob::SpatialAdvancedFilter>();
spatial_filter->enable(request->filter_enable);
@@ -4675,11 +4806,16 @@ void OBCameraNode::setFilterCallback(const std::shared_ptr<SetFilter ::Request>
<< "\ndisp_diff:" << params.disp_diff
<< "\nmagnitude:" << static_cast<int>(params.magnitude)
<< "\nradius:" << params.radius);
spatial_filter_alpha_ = params.alpha;
spatial_filter_diff_threshold_ = params.disp_diff;
spatial_filter_magnitude_ = params.magnitude;
spatial_filter_radius_ = params.radius;
} else {
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<ob::TemporalFilter>();
temporal_filter->enable(request->filter_enable);
@@ -4690,11 +4826,14 @@ void OBCameraNode::setFilterCallback(const std::shared_ptr<SetFilter ::Request>
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<ob::SpatialFastFilter>();
spatial_fast_filter->enable(request->filter_enable);
@@ -4705,11 +4844,13 @@ void OBCameraNode::setFilterCallback(const std::shared_ptr<SetFilter ::Request>
spatial_fast_filter->setFilterParams(params);
RCLCPP_INFO_STREAM(logger_,
"Set SpatialFastFilter radius to " << static_cast<int>(params.radius));
spatial_fast_filter_radius_ = params.radius;
} else {
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<ob::SpatialModerateFilter>();
@@ -4725,23 +4866,30 @@ void OBCameraNode::setFilterCallback(const std::shared_ptr<SetFilter ::Request>
<< "\ndisp_diff:" << params.disp_diff
<< "\nmagnitude:" << static_cast<int>(params.magnitude)
<< "\nradius:" << static_cast<int>(params.radius));
spatial_moderate_filter_diff_threshold_ = params.disp_diff;
spatial_moderate_filter_magnitude_ = params.magnitude;
spatial_moderate_filter_radius_ = params.radius;
} else {
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<ob::FalsePositiveFilter>();
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<ob::MgcNoiseRemovalFilter>();
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<ob::LutNoiseRemovalFilter>();
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<SetFilter ::Request>
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);