61 dyn_server_.reset(
new dynamic_reconfigure::Server<laser_filters::SpeckleFilterConfig>(
own_mutex_, private_nh));
62 dynamic_reconfigure::Server<laser_filters::SpeckleFilterConfig>::CallbackType
f;
63 f = [
this](
auto& config,
auto level){
reconfigureCB(config, level); };
76 boost::recursive_mutex::scoped_lock lock(
own_mutex_);
78 output_scan = input_scan;
83 if (output_scan.ranges.size() <=
config_.filter_window + 1)
85 ROS_ERROR(
"Scan ranges size is too small: size = %ld", output_scan.ranges.size());
90 size_t i_max = input_scan.ranges.size();
93 bool out_of_range = output_scan.ranges[i] >
config_.max_range;
94 valid_ranges.push_back(out_of_range);
99 i_max = input_scan.ranges.size() -
config_.filter_window + 1;
102 output_scan, i,
config_.filter_window,
config_.max_range_difference
105 size_t j = i, j_max = i +
config_.filter_window;
107 valid_ranges[j++] =
true;
114 i_max = valid_ranges.size();
116 if (!valid_ranges[i]) {
117 output_scan.ranges[i] = std::numeric_limits<float>::quiet_NaN();
130 case laser_filters::SpeckleFilter_RadiusOutlier:
138 case laser_filters::SpeckleFilter_Distance: