37 #ifndef LASER_SCAN_ANGULAR_BOUNDS_FILTER_IN_PLACE_H 38 #define LASER_SCAN_ANGULAR_BOUNDS_FILTER_IN_PLACE_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 = input_scan;
69 double current_angle = input_scan.angle_min;
70 unsigned int count = 0;
72 for(
unsigned int i = 0; i < input_scan.ranges.size(); ++i){
73 if((current_angle > lower_angle_) && (current_angle <
upper_angle_)){
74 filtered_scan.ranges[i] = input_scan.range_max + 1.0;
75 if(i < filtered_scan.intensities.size()){
76 filtered_scan.intensities[i] = 0.0;
80 current_angle += input_scan.angle_increment;
83 ROS_DEBUG(
"Filtered out %u points from the laser scan.", count);
LaserScanMaskFilter removes points on directions defined in a mask from a laser scan.
bool getParam(const std::string &name, std::string &value)
bool update(const sensor_msgs::LaserScan &input_scan, sensor_msgs::LaserScan &filtered_scan)
virtual ~LaserScanAngularBoundsFilterInPlace()