35 #ifndef LASER_SCAN_FOOTPRINT_FILTER_H
36 #define LASER_SCAN_FOOTPRINT_FILTER_H
46 #include "sensor_msgs/LaserScan.h"
48 #include "sensor_msgs/PointCloud.h"
64 ROS_ERROR(
"LaserScanFootprintFilter needs inscribed_radius to be set");
75 bool update(
const sensor_msgs::LaserScan& input_scan, sensor_msgs::LaserScan& filtered_scan)
77 filtered_scan = input_scan ;
78 sensor_msgs::PointCloud laser_cloud;
95 if (c_idx == -1 || laser_cloud.channels[c_idx].values.size () == 0){
96 ROS_ERROR(
"We need an index channel to be able to filter out the footprint");
100 for (
unsigned int i=0; i < laser_cloud.points.size(); i++)
103 int index = laser_cloud.channels[c_idx].values[i];
104 filtered_scan.ranges[index] = std::numeric_limits<float>::quiet_NaN();
112 int indexChannel(
const sensor_msgs::PointCloud& scan_cloud){
114 for (
unsigned int d = 0;
d < scan_cloud.channels.size ();
d++)
116 if (scan_cloud.channels[d].name ==
"index")
125 bool inFootprint(
const geometry_msgs::Point32& scan_pt){
140 #endif // LASER_SCAN_FOOTPRINT_FILTER_H