37 #ifndef LASER_SCAN_ANGULAR_BOUNDS_FILTER_H 38 #define LASER_SCAN_ANGULAR_BOUNDS_FILTER_H 41 #include <sensor_msgs/LaserScan.h> 56 if(!
getParam(
"lower_angle", lower_angle_) || !
getParam(
"upper_angle", upper_angle_)){
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){
77 if(start_angle < lower_angle_){
78 start_angle += input_scan.angle_increment;
79 current_angle += input_scan.angle_increment;
83 filtered_scan.ranges[count] = input_scan.ranges[i];
86 if(input_scan.intensities.size() > i)
87 filtered_scan.intensities[count] = input_scan.intensities[i];
92 if(current_angle + input_scan.angle_increment > upper_angle_){
96 current_angle += input_scan.angle_increment;
102 filtered_scan.header.frame_id = input_scan.header.frame_id;
103 filtered_scan.header.stamp = start_time;
104 filtered_scan.angle_min = start_angle;
105 filtered_scan.angle_max = current_angle;
106 filtered_scan.angle_increment = input_scan.angle_increment;
107 filtered_scan.time_increment = input_scan.time_increment;
108 filtered_scan.scan_time = input_scan.scan_time;
109 filtered_scan.range_min = input_scan.range_min;
110 filtered_scan.range_max = input_scan.range_max;
112 filtered_scan.ranges.resize(count);
114 if(input_scan.intensities.size() >= count)
115 filtered_scan.intensities.resize(count);
117 ROS_DEBUG(
"Filtered out %d points from the laser scan.", (
int)input_scan.ranges.size() - (int)count);
LaserScanMaskFilter removes points on directions defined in a mask from a laser scan.
virtual ~LaserScanAngularBoundsFilter()
bool update(const sensor_msgs::LaserScan &input_scan, sensor_msgs::LaserScan &filtered_scan)
bool getParam(const std::string &name, std::string &value)