61 dyn_server_.reset(
new dynamic_reconfigure::Server<laser_filters::SpeckleFilterConfig>(
own_mutex_, private_nh));
62 dynamic_reconfigure::Server<laser_filters::SpeckleFilterConfig>::CallbackType
f;
76 output_scan = input_scan;
77 std::vector<bool> valid_ranges(output_scan.ranges.size(),
false);
80 if (output_scan.ranges.size() <=
config_.filter_window + 1)
82 ROS_ERROR(
"Scan ranges size is too small: size = %i", output_scan.ranges.size());
86 for (
size_t idx = 0; idx < output_scan.ranges.size() -
config_.filter_window + 1; ++idx)
89 output_scan, idx,
config_.filter_window,
config_.max_range_difference);
92 for (
size_t neighbor_idx_or_self_nr = 0; neighbor_idx_or_self_nr <
config_.filter_window; ++neighbor_idx_or_self_nr)
94 size_t neighbor_idx_or_self = idx + neighbor_idx_or_self_nr;
95 if (neighbor_idx_or_self < output_scan.ranges.size())
97 bool out_of_range = output_scan.ranges[neighbor_idx_or_self] >
config_.max_range;
98 valid_ranges[neighbor_idx_or_self] = valid_ranges[neighbor_idx_or_self] || window_valid || out_of_range;
103 for (
size_t idx = 0; idx < valid_ranges.size(); ++idx)
105 if (!valid_ranges[idx])
107 output_scan.ranges[idx] = std::numeric_limits<float>::quiet_NaN();
119 case laser_filters::SpeckleFilter_RadiusOutlier:
127 case laser_filters::SpeckleFilter_Distance:
WindowValidator * validator_
virtual bool checkWindowValid(const sensor_msgs::LaserScan &scan, size_t idx, size_t window, double max_range_difference)=0
bool update(const sensor_msgs::LaserScan &input_scan, sensor_msgs::LaserScan &output_scan)
LaserScanMaskFilter removes points on directions defined in a mask from a laser scan.
const std::string & getName() const
boost::recursive_mutex own_mutex_
std::shared_ptr< dynamic_reconfigure::Server< laser_filters::SpeckleFilterConfig > > dyn_server_
~LaserScanSpeckleFilter()
bool getParam(const std::string &name, std::string &value) const
SpeckleFilterConfig config_
void reconfigureCB(laser_filters::SpeckleFilterConfig &config, uint32_t level)