37 #ifndef LASER_SCAN_ANGULAR_BOUNDS_FILTER_H
38 #define LASER_SCAN_ANGULAR_BOUNDS_FILTER_H
41 #include <sensor_msgs/LaserScan.h>
57 ROS_ERROR(
"Both the lower_angle and upper_angle parameters must be set to use this filter.");
66 bool update(
const sensor_msgs::LaserScan& input_scan, sensor_msgs::LaserScan& filtered_scan){
67 filtered_scan.ranges.resize(input_scan.ranges.size());
68 filtered_scan.intensities.resize(input_scan.intensities.size());
70 double start_angle = input_scan.angle_min;
71 double current_angle = input_scan.angle_min;
72 ros::Time start_time = input_scan.header.stamp;
73 unsigned int count = 0;
75 for(
unsigned int i = 0; i < input_scan.ranges.size(); ++i){
76 if(input_scan.angle_increment > 0){
79 start_angle += input_scan.angle_increment;
80 current_angle += input_scan.angle_increment;
84 filtered_scan.ranges[count] = input_scan.ranges[i];
87 if(input_scan.intensities.size() > i)
88 filtered_scan.intensities[count] = input_scan.intensities[i];
93 if(current_angle + input_scan.angle_increment >
upper_angle_){
96 current_angle += input_scan.angle_increment;
102 start_angle += input_scan.angle_increment;
103 current_angle += input_scan.angle_increment;
107 filtered_scan.ranges[count] = input_scan.ranges[i];
110 if(input_scan.intensities.size() > i)
111 filtered_scan.intensities[count] = input_scan.intensities[i];
116 if(current_angle + input_scan.angle_increment <
lower_angle_){
120 current_angle += input_scan.angle_increment;
127 filtered_scan.header.frame_id = input_scan.header.frame_id;
128 filtered_scan.header.stamp = start_time;
129 filtered_scan.angle_min = start_angle;
130 filtered_scan.angle_max = current_angle;
131 filtered_scan.angle_increment = input_scan.angle_increment;
132 filtered_scan.time_increment = input_scan.time_increment;
133 filtered_scan.scan_time = input_scan.scan_time;
134 filtered_scan.range_min = input_scan.range_min;
135 filtered_scan.range_max = input_scan.range_max;
137 filtered_scan.ranges.resize(count);
139 if(input_scan.intensities.size() >= count)
140 filtered_scan.intensities.resize(count);
142 ROS_DEBUG(
"Filtered out %d points from the laser scan.", (
int)input_scan.ranges.size() - (
int)count);