48 dynamic_reconfigure::Server<SectorFilterConfig>::CallbackType
f;
49 f = [
this](
auto& config,
auto level){
reconfigureCB(config, level); };
72 bool clear_inside =
config_.clear_inside;
75 clear_inside = invert ? false : clear_inside;
82 filtered_scan = input_scan;
84 double angle_min =
config_.angle_min;
85 double angle_max =
config_.angle_max;
86 double range_min =
config_.range_min;
87 double range_max =
config_.range_max;
90 double angle_delta = angle_max - angle_min;
91 if (angle_max < angle_min)
93 angle_delta += M_PI * 2;
96 double current_angle = input_scan.angle_min;
97 unsigned int count = 0;
99 for (
size_t i = 0; i < input_scan.ranges.size(); ++i)
101 current_angle = (i == 0) ? current_angle : (current_angle + input_scan.angle_increment);
103 double current_range = input_scan.ranges[i];
104 double current_angle_delta = current_angle - angle_min;
105 if ((angle_max < angle_min) && (current_angle_delta < 0))
107 current_angle_delta += M_PI * 2;
110 if (clear_inside != ((current_angle_delta > 0)
111 && (current_angle_delta < angle_delta)
112 && (current_range > range_min)
113 && (current_range < range_max)))
118 filtered_scan.ranges[i] = input_scan.range_max + 1.0;
119 if (i < filtered_scan.intensities.size())
121 filtered_scan.intensities[i] = 0.0;
126 ROS_DEBUG(
"Filtered out %u points from the laser scan.", count);