i/OrbbecSDK_ROS2
1
0
Fork 0
mirror of https://github.com/orbbec/OrbbecSDK_ROS2.git synced 2026-09-13 03:30:18 +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/include/orbbec_camera/ob_camera_node.h
+5 -33
View File
@@ -56,6 +56,7 @@
#include "orbbec_camera_msgs/srv/set_int32.hpp"
#include "orbbec_camera_msgs/srv/get_bool.hpp"
#include "orbbec_camera_msgs/srv/set_string.hpp"
#include "orbbec_camera_msgs/srv/set_filter.hpp"
#include "orbbec_camera/constants.h"
#include "orbbec_camera/dynamic_params.h"
#include "orbbec_camera/d2c_viewer.h"
@@ -107,6 +108,7 @@ using GetString = orbbec_camera_msgs::srv::GetString;
using SetString = orbbec_camera_msgs::srv::SetString;
using SetBool = std_srvs::srv::SetBool;
using GetBool = orbbec_camera_msgs::srv::GetBool;
using SetFilter = orbbec_camera_msgs::srv::SetFilter;
typedef std::pair<ob_stream_type, int> stream_index_pair;
@@ -293,34 +295,11 @@ class OBCameraNode {
std::shared_ptr<std_srvs::srv::SetBool::Response>& response);
void setSYNCInterleaveLaserCallback(const std::shared_ptr<SetInt32 ::Request>& request,
std::shared_ptr<SetInt32 ::Response>& response);
void setFilterCallback(const std::shared_ptr<SetFilter ::Request>& request,
std::shared_ptr<SetFilter ::Response>& response);
void setSYNCHostimeCallback(const std::shared_ptr<std_srvs::srv::SetBool::Request>& request,
std::shared_ptr<std_srvs::srv::SetBool::Response>& response);
void setDecimationFilterEnableCallback(
const std::shared_ptr<std_srvs::srv::SetBool::Request>& request,
std::shared_ptr<std_srvs::srv::SetBool::Response>& response);
void setSequenceIdFilterEnableCallback(
const std::shared_ptr<std_srvs::srv::SetBool::Request>& request,
std::shared_ptr<std_srvs::srv::SetBool::Response>& response);
void setThresholdFilterEnableCallback(
const std::shared_ptr<std_srvs::srv::SetBool::Request>& request,
std::shared_ptr<std_srvs::srv::SetBool::Response>& response);
void setSpatialFilterEnableCallback(
const std::shared_ptr<std_srvs::srv::SetBool::Request>& request,
std::shared_ptr<std_srvs::srv::SetBool::Response>& response);
void setTemporalFilterEnableCallback(
const std::shared_ptr<std_srvs::srv::SetBool::Request>& request,
std::shared_ptr<std_srvs::srv::SetBool::Response>& response);
void setHoleFillingFilterEnableCallback(
const std::shared_ptr<std_srvs::srv::SetBool::Request>& request,
std::shared_ptr<std_srvs::srv::SetBool::Response>& response);
void setNoiseRemovalFilterEnableCallback(
const std::shared_ptr<std_srvs::srv::SetBool::Request>& request,
std::shared_ptr<std_srvs::srv::SetBool::Response>& response);
void setAllSoftwareFilterEnableCallback(
const std::shared_ptr<std_srvs::srv::SetBool::Request>& request,
std::shared_ptr<std_srvs::srv::SetBool::Response>& response);
bool toggleSensor(const stream_index_pair& stream_index, bool enabled, std::string& msg);
void saveImageCallback(const std::shared_ptr<std_srvs::srv::Empty::Request>& request,
@@ -474,14 +453,7 @@ class OBCameraNode {
rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr set_reset_timestamp_srv_;
rclcpp::Service<SetInt32>::SharedPtr set_interleaver_laser_sync_srv_;
rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr set_sync_host_time_srv_;
rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr set_decimation_filter_enable_srv_;
rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr set_sequence_id_filter_enable_srv_;
rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr set_threshold_filter_enable_srv_;
rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr set_spatial_filter_enable_srv_;
rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr set_temporal_filter_enable_srv_;
rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr set_hole_filling_filter_enable_srv_;
rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr set_noise_removal_filter_enable_srv_;
rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr set_all_software_filter_enable_srv_;
rclcpp::Service<SetFilter>::SharedPtr set_filter_srv_;
bool enable_sync_output_accel_gyro_ = false;
bool publish_tf_ = false;
orbbec_camera/src/ob_camera_node.cpp
+164 -1
View File
@@ -619,6 +619,11 @@ void OBCameraNode::setupDepthPostProcessFilter() {
}
}
}
set_filter_srv_ = node_->create_service<SetFilter>(
"set_filter", [this](const std::shared_ptr<SetFilter ::Request> request,
std::shared_ptr<SetFilter ::Response> response) {
setFilterCallback(request, response);
});
}
void OBCameraNode::selectBaseStream() {
@@ -1898,6 +1903,7 @@ std::shared_ptr<ob::Frame> OBCameraNode::processDepthFrameFilter(
}
void OBCameraNode::setDisparitySearchOffset() {
static bool has_run = false;
auto config = OBDispOffsetConfig();
if (has_run) {
return;
}
@@ -1905,10 +1911,12 @@ void OBCameraNode::setDisparitySearchOffset() {
if (disparity_search_offset_ >= 0 && disparity_search_offset_ <= 127) {
device_->setIntProperty(OB_PROP_DISP_SEARCH_OFFSET_INT, disparity_search_offset_);
RCLCPP_INFO_STREAM(logger_, "disparity_search_offset: " << disparity_search_offset_);
} else {
device_->setIntProperty(OB_PROP_DISP_SEARCH_OFFSET_INT, 0);
RCLCPP_INFO_STREAM(logger_, "Set default disparity_search_offset ");
}
if (offset_index0_ >= 0 && offset_index0_ <= 127 && offset_index1_ >= 0 &&
offset_index1_ <= 127) {
auto config = OBDispOffsetConfig();
config.enable = disparity_offset_config_;
config.offset0 = offset_index0_;
config.offset1 = offset_index1_;
@@ -1919,6 +1927,15 @@ void OBCameraNode::setDisparitySearchOffset() {
RCLCPP_INFO_STREAM(logger_, "disparity_offset_config: "
<< disparity_offset_config_ << " offset_index0:"
<< offset_index0_ << " offset_index1:" << offset_index1_);
} else {
config.enable = false;
config.offset0 = 0;
config.offset1 = 0;
config.reserved = 0;
device_->setStructuredData(OB_STRUCT_DISP_OFFSET_CONFIG,
reinterpret_cast<const uint8_t *>(&config), sizeof(config));
RCLCPP_INFO_STREAM(logger_, "Set default disparity_offset_config ");
}
}
has_run = true;
@@ -2901,5 +2918,151 @@ orbbec_camera_msgs::msg::IMUInfo OBCameraNode::createIMUInfo(
return imu_info;
}
void OBCameraNode::setFilterCallback(const std::shared_ptr<SetFilter ::Request> &request,
std::shared_ptr<SetFilter ::Response> &response) {
try {
RCLCPP_INFO_STREAM(logger_, "filter_name: " << request->filter_name
<< " filter_enable: " << request->filter_enable);
auto it = std::remove_if(filter_list_.begin(), filter_list_.end(),
[&request](const std::shared_ptr<ob::Filter> &filter) {
return filter->getName() == request->filter_name;
});
filter_list_.erase(it, filter_list_.end());
if (request->filter_name == "DecimationFilter") {
auto decimation_filter = std::make_shared<ob::DecimationFilter>();
decimation_filter->enable(request->filter_enable);
filter_list_.push_back(decimation_filter);
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);
}
} else if (request->filter_name == "HDRMerge") {
auto hdr_merge_filter = std::make_shared<ob::HdrMerge>();
hdr_merge_filter->enable(request->filter_enable);
filter_list_.push_back(hdr_merge_filter);
auto config = OBHdrConfig();
config.enable = true;
config.exposure_1 = request->filter_param[0];
config.gain_1 = request->filter_param[1];
config.exposure_2 = request->filter_param[2];
config.gain_2 = request->filter_param[3];
device_->setStructuredData(OB_STRUCT_DEPTH_HDR_CONFIG,
reinterpret_cast<const uint8_t *>(&config), sizeof(config));
RCLCPP_INFO_STREAM(
logger_, "Set HDR merge filter params: " << "\nexposure_1: " << request->filter_param[0]
<< "\ngain_1: " << request->filter_param[1]
<< "\nexposure_2: " << request->filter_param[2]
<< "\ngain_2: " << request->filter_param[3]);
} else if (request->filter_name == "SequencedFilter") {
auto sequenced_filter = std::make_shared<ob::SequenceIdFilter>();
sequenced_filter->enable(request->filter_enable);
filter_list_.push_back(sequenced_filter);
sequenced_filter->selectSequenceId(request->filter_param[0]);
RCLCPP_INFO_STREAM(
logger_, "Set sequenced filter selectSequenceId value to " << request->filter_param[0]);
} else if (request->filter_name == "ThresholdFilter") {
auto threshold_filter = std::make_shared<ob::ThresholdFilter>();
threshold_filter->enable(request->filter_enable);
filter_list_.push_back(threshold_filter);
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);
} else if (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);
RCLCPP_INFO_STREAM(logger_, "enable_noise_removal_filter:" << request->filter_enable);
}
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);
}
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);
}
} else if (request->filter_name == "HardwareNoiseRemoval") {
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);
}
} else if (request->filter_name == "SpatialAdvancedFilter") {
auto spatial_filter = std::make_shared<ob::SpatialAdvancedFilter>();
spatial_filter->enable(request->filter_enable);
filter_list_.push_back(spatial_filter);
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 spatial filter params: "
<< "\nalpha:" << params.alpha << "\nradius:" << params.radius
<< "\ndisp_diff:" << params.disp_diff);
} else if (request->filter_name == "TemporalFilter") {
auto temporal_filter = std::make_shared<ob::TemporalFilter>();
temporal_filter->enable(request->filter_enable);
filter_list_.push_back(temporal_filter);
temporal_filter->setDiffScale(request->filter_param[0]);
temporal_filter->setWeight(request->filter_param[1]);
RCLCPP_INFO_STREAM(logger_, "Set temporal filter value to " << request->filter_param[0]
<< " - "
<< request->filter_param[1]);
} else {
RCLCPP_INFO_STREAM(logger_,
request->filter_name
<< "Cannot be set\n"
<< "The filter_name value that can be set is "
"DecimationFilter、HDRMerge、SequencedFilter、ThresholdFilter、Nois"
"eRemovalFilter、SpatialAdvancedFilter and TemporalFilter");
}
for (auto &filter : filter_list_) {
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;
}
}
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
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
orbbec_camera_msgs/CMakeLists.txt
+1
View File
@@ -25,6 +25,7 @@ rosidl_generate_interfaces(${PROJECT_NAME}
"srv/GetCameraInfo.srv"
"srv/GetInt32.srv"
"srv/GetString.srv"
"srv/SetFilter.srv"
"srv/SetInt32.srv"
"srv/SetString.srv"
DEPENDENCIES
orbbec_camera_msgs/srv/SetFilter.srv
+6
View File
@@ -0,0 +1,6 @@
string filter_name
bool filter_enable
float32[] filter_param
---
bool success
string message