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

Add service switch and parameter setting interface for DecimationFilter, HDRMerge, SequencedFilter, ThresholdFilter, NoiseRemovalFilter, HardwareNoiseRemoval, SpatialAdvancedFilter and TemporalFilter

Browse Source
This commit is contained in:
jj
2025-02-11 17:22:07 +08:00
parent 141bebb7ac
commit 49cfd33689
5 changed files with 177 additions and 399 deletions
Download Patch File Download Diff File Expand all files Collapse all files
orbbec_camera/src/ros_service.cpp
+1 -365
View File
@@ -184,46 +184,6 @@ void OBCameraNode::setupCameraCtrlServices() {
std::shared_ptr<SetBool::Response> response) {
setSYNCHostimeCallback(request, response);
});
set_decimation_filter_enable_srv_ = node_->create_service<SetBool>(
"set_decimation_filter_enable", [this](const std::shared_ptr<SetBool::Request> request,
std::shared_ptr<SetBool::Response> response) {
setDecimationFilterEnableCallback(request, response);
});
set_sequence_id_filter_enable_srv_ = node_->create_service<SetBool>(
"set_sequence_id_filter_enable", [this](const std::shared_ptr<SetBool::Request> request,
std::shared_ptr<SetBool::Response> response) {
setSequenceIdFilterEnableCallback(request, response);
});
set_threshold_filter_enable_srv_ = node_->create_service<SetBool>(
"set_threshold_filter_enable", [this](const std::shared_ptr<SetBool::Request> request,
std::shared_ptr<SetBool::Response> response) {
setThresholdFilterEnableCallback(request, response);
});
set_spatial_filter_enable_srv_ = node_->create_service<SetBool>(
"set_spatial_filter_enable", [this](const std::shared_ptr<SetBool::Request> request,
std::shared_ptr<SetBool::Response> response) {
setSpatialFilterEnableCallback(request, response);
});
set_temporal_filter_enable_srv_ = node_->create_service<SetBool>(
"set_temporal_filter_enable", [this](const std::shared_ptr<SetBool::Request> request,
std::shared_ptr<SetBool::Response> response) {
setTemporalFilterEnableCallback(request, response);
});
set_hole_filling_filter_enable_srv_ = node_->create_service<SetBool>(
"set_hole_filling_filter_enable", [this](const std::shared_ptr<SetBool::Request> request,
std::shared_ptr<SetBool::Response> response) {
setHoleFillingFilterEnableCallback(request, response);
});
set_noise_removal_filter_enable_srv_ = node_->create_service<SetBool>(
"set_noise_removal_filter_enable", [this](const std::shared_ptr<SetBool::Request> request,
std::shared_ptr<SetBool::Response> response) {
setNoiseRemovalFilterEnableCallback(request, response);
});
set_all_software_filter_enable_srv_ = node_->create_service<SetBool>(
"set_all_software_filter_enable", [this](const std::shared_ptr<SetBool::Request> request,
std::shared_ptr<SetBool::Response> response) {
setAllSoftwareFilterEnableCallback(request, response);
});
}
void OBCameraNode::setExposureCallback(const std::shared_ptr<SetInt32::Request>& request,
@@ -876,329 +836,5 @@ void OBCameraNode::setSYNCHostimeCallback(
response->success = false;
}
}
void OBCameraNode::setDecimationFilterEnableCallback(
const std::shared_ptr<std_srvs::srv::SetBool::Request>& request,
std::shared_ptr<std_srvs::srv::SetBool::Response>& response) {
try {
enable_decimation_filter_ = request->data;
if (enable_decimation_filter_) {
decimation_filter_scale_ = node_->get_parameter("decimation_filter_scale").as_int();
if (decimation_filter_scale_ == -1) {
RCLCPP_WARN(logger_, "Please configure the parameter 'decimation_filter_scale'");
return;
}
} else {
decimation_filter_scale_ = -1;
node_->set_parameter(rclcpp::Parameter("decimation_filter_scale", decimation_filter_scale_));
}
setupDepthPostProcessFilter();
node_->set_parameter(rclcpp::Parameter("enable_decimation_filter", enable_decimation_filter_));
response->success = true;
} catch (const ob::Error& e) {
response->message = e.getMessage();
response->success = false;
} catch (const std::exception& e) {
response->message = e.what();
response->success = false;
} catch (...) {
response->message = "unknown error";
response->success = false;
}
}
void OBCameraNode::setSequenceIdFilterEnableCallback(
const std::shared_ptr<std_srvs::srv::SetBool::Request>& request,
std::shared_ptr<std_srvs::srv::SetBool::Response>& response) {
try {
enable_sequence_id_filter_ = request->data;
if (enable_sequence_id_filter_) {
sequence_id_filter_id_ = node_->get_parameter("sequence_id_filter_id").as_int();
if (sequence_id_filter_id_ == -1) {
RCLCPP_WARN(logger_, "Please configure the parameter 'sequence_id_filter_id'");
return;
}
} else {
sequence_id_filter_id_ = -1;
node_->set_parameter(rclcpp::Parameter("sequence_id_filter_id", sequence_id_filter_id_));
}
setupDepthPostProcessFilter();
node_->set_parameter(
rclcpp::Parameter("enable_sequence_id_filter", enable_sequence_id_filter_));
response->success = true;
} catch (const ob::Error& e) {
response->message = e.getMessage();
response->success = false;
} catch (const std::exception& e) {
response->message = e.what();
response->success = false;
} catch (...) {
response->message = "unknown error";
response->success = false;
}
}
void OBCameraNode::setThresholdFilterEnableCallback(
const std::shared_ptr<std_srvs::srv::SetBool::Request>& request,
std::shared_ptr<std_srvs::srv::SetBool::Response>& response) {
try {
enable_threshold_filter_ = request->data;
if (enable_threshold_filter_) {
threshold_filter_max_ = node_->get_parameter("threshold_filter_max").as_int();
threshold_filter_min_ = node_->get_parameter("threshold_filter_min").as_int();
if (threshold_filter_max_ == -1 || threshold_filter_min_ == -1) {
RCLCPP_WARN(
logger_,
"Please configure the parameter 'threshold_filter_max' and 'threshold_filter_min'");
return;
}
} else {
threshold_filter_max_ = -1;
threshold_filter_min_ = -1;
node_->set_parameter(rclcpp::Parameter("threshold_filter_max", threshold_filter_max_));
node_->set_parameter(rclcpp::Parameter("threshold_filter_min", threshold_filter_min_));
}
setupDepthPostProcessFilter();
node_->set_parameter(rclcpp::Parameter("enable_threshold_filter", enable_threshold_filter_));
response->success = true;
} catch (const ob::Error& e) {
response->message = e.getMessage();
response->success = false;
} catch (const std::exception& e) {
response->message = e.what();
response->success = false;
} catch (...) {
response->message = "unknown error";
response->success = false;
}
}
void OBCameraNode::setSpatialFilterEnableCallback(
const std::shared_ptr<std_srvs::srv::SetBool::Request>& request,
std::shared_ptr<std_srvs::srv::SetBool::Response>& response) {
try {
enable_spatial_filter_ = request->data;
if (enable_spatial_filter_) {
spatial_filter_alpha_ =
static_cast<float>(node_->get_parameter("spatial_filter_alpha").as_double());
spatial_filter_diff_threshold_ =
node_->get_parameter("spatial_filter_diff_threshold").as_int();
spatial_filter_magnitude_ = node_->get_parameter("spatial_filter_magnitude").as_int();
spatial_filter_radius_ = node_->get_parameter("spatial_filter_radius").as_int();
if (spatial_filter_alpha_ == -1.0 || spatial_filter_diff_threshold_ == -1 ||
spatial_filter_magnitude_ == -1 || spatial_filter_radius_ == -1) {
return;
}
} else {
spatial_filter_alpha_ = -1.0;
spatial_filter_diff_threshold_ = -1;
spatial_filter_magnitude_ = -1;
spatial_filter_radius_ = -1;
node_->set_parameter(rclcpp::Parameter("spatial_filter_alpha", spatial_filter_alpha_));
node_->set_parameter(
rclcpp::Parameter("spatial_filter_diff_threshold", spatial_filter_diff_threshold_));
node_->set_parameter(
rclcpp::Parameter("spatial_filter_magnitude", spatial_filter_magnitude_));
node_->set_parameter(rclcpp::Parameter("spatial_filter_radius", spatial_filter_radius_));
}
setupDepthPostProcessFilter();
node_->set_parameter(rclcpp::Parameter("enable_spatial_filter", enable_spatial_filter_));
response->success = true;
} catch (const ob::Error& e) {
response->message = e.getMessage();
response->success = false;
} catch (const std::exception& e) {
response->message = e.what();
response->success = false;
} catch (...) {
response->message = "unknown error";
response->success = false;
}
}
void OBCameraNode::setTemporalFilterEnableCallback(
const std::shared_ptr<std_srvs::srv::SetBool::Request>& request,
std::shared_ptr<std_srvs::srv::SetBool::Response>& response) {
try {
enable_temporal_filter_ = request->data;
if (enable_temporal_filter_) {
temporal_filter_diff_threshold_ =
static_cast<float>(node_->get_parameter("temporal_filter_diff_threshold").as_double());
temporal_filter_weight_ =
static_cast<float>(node_->get_parameter("temporal_filter_weight").as_double());
if (temporal_filter_diff_threshold_ == -1.0 || temporal_filter_weight_ == -1.0) {
return;
}
} else {
temporal_filter_diff_threshold_ = -1.0;
temporal_filter_weight_ = -1.0;
node_->set_parameter(
rclcpp::Parameter("temporal_filter_diff_threshold", temporal_filter_diff_threshold_));
node_->set_parameter(rclcpp::Parameter("temporal_filter_weight", temporal_filter_weight_));
}
setupDepthPostProcessFilter();
node_->set_parameter(rclcpp::Parameter("enable_temporal_filter", enable_temporal_filter_));
response->success = true;
} catch (const ob::Error& e) {
response->message = e.getMessage();
response->success = false;
} catch (const std::exception& e) {
response->message = e.what();
response->success = false;
} catch (...) {
response->message = "unknown error";
response->success = false;
}
}
void OBCameraNode::setHoleFillingFilterEnableCallback(
const std::shared_ptr<std_srvs::srv::SetBool::Request>& request,
std::shared_ptr<std_srvs::srv::SetBool::Response>& response) {
try {
enable_hole_filling_filter_ = request->data;
if (enable_hole_filling_filter_) {
hole_filling_filter_mode_ = node_->get_parameter("hole_filling_filter_mode").as_string();
if (hole_filling_filter_mode_.empty()) {
return;
}
} else {
hole_filling_filter_mode_ = "";
node_->set_parameter(
rclcpp::Parameter("hole_filling_filter_mode", hole_filling_filter_mode_));
}
setupDepthPostProcessFilter();
node_->set_parameter(
rclcpp::Parameter("enable_hole_filling_filter", enable_hole_filling_filter_));
response->success = true;
} catch (const ob::Error& e) {
response->message = e.getMessage();
response->success = false;
} catch (const std::exception& e) {
response->message = e.what();
response->success = false;
} catch (...) {
response->message = "unknown error";
response->success = false;
}
}
void OBCameraNode::setNoiseRemovalFilterEnableCallback(
const std::shared_ptr<std_srvs::srv::SetBool::Request>& request,
std::shared_ptr<std_srvs::srv::SetBool::Response>& response) {
try {
enable_noise_removal_filter_ = request->data;
if (enable_noise_removal_filter_) {
noise_removal_filter_min_diff_ =
node_->get_parameter("noise_removal_filter_min_diff").as_int();
noise_removal_filter_max_size_ =
node_->get_parameter("noise_removal_filter_max_size").as_int();
if (noise_removal_filter_min_diff_ == -1 || noise_removal_filter_max_size_ == -1) {
return;
}
} else {
noise_removal_filter_min_diff_ = -1;
noise_removal_filter_max_size_ = -1;
node_->set_parameter(
rclcpp::Parameter("noise_removal_filter_min_diff", noise_removal_filter_min_diff_));
node_->set_parameter(
rclcpp::Parameter("noise_removal_filter_max_size", noise_removal_filter_max_size_));
}
setupDepthPostProcessFilter();
node_->set_parameter(
rclcpp::Parameter("enable_noise_removal_filter", enable_noise_removal_filter_));
response->success = true;
} catch (const ob::Error& e) {
response->message = e.getMessage();
response->success = false;
} catch (const std::exception& e) {
response->message = e.what();
response->success = false;
} catch (...) {
response->message = "unknown error";
response->success = false;
}
}
void OBCameraNode::setAllSoftwareFilterEnableCallback(
const std::shared_ptr<std_srvs::srv::SetBool::Request>& request,
std::shared_ptr<std_srvs::srv::SetBool::Response>& response) {
try {
if (request->data) {
decimation_filter_scale_ = node_->get_parameter("decimation_filter_scale").as_int();
sequence_id_filter_id_ = node_->get_parameter("sequence_id_filter_id").as_int();
threshold_filter_max_ = node_->get_parameter("threshold_filter_max").as_int();
threshold_filter_min_ = node_->get_parameter("threshold_filter_min").as_int();
spatial_filter_alpha_ =
static_cast<float>(node_->get_parameter("spatial_filter_alpha").as_double());
spatial_filter_diff_threshold_ =
node_->get_parameter("spatial_filter_diff_threshold").as_int();
spatial_filter_magnitude_ = node_->get_parameter("spatial_filter_magnitude").as_int();
spatial_filter_radius_ = node_->get_parameter("spatial_filter_radius").as_int();
noise_removal_filter_min_diff_ =
node_->get_parameter("noise_removal_filter_min_diff").as_int();
noise_removal_filter_max_size_ =
node_->get_parameter("noise_removal_filter_max_size").as_int();
temporal_filter_diff_threshold_ =
static_cast<float>(node_->get_parameter("temporal_filter_diff_threshold").as_double());
temporal_filter_weight_ =
static_cast<float>(node_->get_parameter("temporal_filter_weight").as_double());
hole_filling_filter_mode_ = node_->get_parameter("hole_filling_filter_mode").as_string();
if (decimation_filter_scale_ == -1 || sequence_id_filter_id_ == -1 ||
threshold_filter_max_ == -1 || threshold_filter_min_ == -1 ||
spatial_filter_alpha_ == -1.0 || spatial_filter_diff_threshold_ == -1 ||
spatial_filter_magnitude_ == -1 || spatial_filter_radius_ == -1 ||
noise_removal_filter_min_diff_ == -1 || noise_removal_filter_max_size_ == -1 ||
temporal_filter_diff_threshold_ == -1.0 || temporal_filter_weight_ == -1.0 ||
hole_filling_filter_mode_.empty()) {
return;
} else {
enable_decimation_filter_ = enable_sequence_id_filter_ = enable_threshold_filter_ =
enable_noise_removal_filter_ = enable_spatial_filter_ = enable_temporal_filter_ =
enable_hole_filling_filter_ = true;
}
} else {
enable_decimation_filter_ = enable_sequence_id_filter_ = enable_threshold_filter_ =
enable_noise_removal_filter_ = enable_spatial_filter_ = enable_temporal_filter_ =
enable_hole_filling_filter_ = false;
decimation_filter_scale_ = sequence_id_filter_id_ = threshold_filter_max_ =
threshold_filter_min_ = spatial_filter_diff_threshold_ = spatial_filter_magnitude_ =
spatial_filter_radius_ = noise_removal_filter_min_diff_ =
noise_removal_filter_max_size_ = -1;
spatial_filter_alpha_ = temporal_filter_diff_threshold_ = temporal_filter_weight_ = -1.0;
hole_filling_filter_mode_ = "";
node_->set_parameter(rclcpp::Parameter("decimation_filter_scale", decimation_filter_scale_));
node_->set_parameter(rclcpp::Parameter("sequence_id_filter_id", sequence_id_filter_id_));
node_->set_parameter(rclcpp::Parameter("threshold_filter_max", threshold_filter_max_));
node_->set_parameter(rclcpp::Parameter("threshold_filter_min", threshold_filter_min_));
node_->set_parameter(
rclcpp::Parameter("spatial_filter_diff_threshold", spatial_filter_diff_threshold_));
node_->set_parameter(
rclcpp::Parameter("spatial_filter_magnitude", spatial_filter_magnitude_));
node_->set_parameter(rclcpp::Parameter("spatial_filter_radius", spatial_filter_radius_));
node_->set_parameter(rclcpp::Parameter("spatial_filter_alpha", spatial_filter_alpha_));
node_->set_parameter(
rclcpp::Parameter("noise_removal_filter_min_diff", noise_removal_filter_min_diff_));
node_->set_parameter(
rclcpp::Parameter("noise_removal_filter_max_size", noise_removal_filter_max_size_));
node_->set_parameter(
rclcpp::Parameter("temporal_filter_diff_threshold", temporal_filter_diff_threshold_));
node_->set_parameter(rclcpp::Parameter("temporal_filter_weight", temporal_filter_weight_));
node_->set_parameter(
rclcpp::Parameter("hole_filling_filter_mode", hole_filling_filter_mode_));
}
setupDepthPostProcessFilter();
node_->set_parameter(rclcpp::Parameter("enable_decimation_filter", enable_decimation_filter_));
node_->set_parameter(
rclcpp::Parameter("enable_sequence_id_filter", enable_sequence_id_filter_));
node_->set_parameter(rclcpp::Parameter("enable_threshold_filter", enable_threshold_filter_));
node_->set_parameter(rclcpp::Parameter("enable_spatial_filter", enable_spatial_filter_));
node_->set_parameter(
rclcpp::Parameter("enable_noise_removal_filter", enable_noise_removal_filter_));
node_->set_parameter(rclcpp::Parameter("enable_temporal_filter", enable_temporal_filter_));
node_->set_parameter(
rclcpp::Parameter("enable_hole_filling_filter", enable_hole_filling_filter_));
response->success = true;
} catch (const ob::Error& e) {
response->message = e.getMessage();
response->success = false;
} catch (const std::exception& e) {
response->message = e.what();
response->success = false;
} catch (...) {
response->message = "unknown error";
response->success = false;
}
}
} // namespace orbbec_camera