35 #ifndef LASER_SCAN_FOOTPRINT_FILTER_H 36 #define LASER_SCAN_FOOTPRINT_FILTER_H 47 #include "sensor_msgs/PointCloud.h" 57 ROS_WARN(
"PointCloudFootprintFilter has been deprecated. Please use PR2PointCloudFootprintFilter instead.\n");
64 ROS_ERROR(
"PointCloudFootprintFilter needs inscribed_radius to be set");
75 bool update(
const sensor_msgs::PointCloud& input_scan, sensor_msgs::PointCloud& filtered_scan)
77 if(&input_scan == &filtered_scan){
78 ROS_ERROR(
"This filter does not currently support in place copying");
81 sensor_msgs::PointCloud laser_cloud;
87 ROS_ERROR(
"Transform unavailable %s", ex.what());
91 filtered_scan.header = input_scan.header;
92 filtered_scan.points.resize (input_scan.points.size());
93 filtered_scan.channels.resize (input_scan.channels.size());
94 for (
unsigned int d = 0;
d < input_scan.channels.size ();
d++){
95 filtered_scan.channels[
d].values.resize (input_scan.points.size());
96 filtered_scan.channels[
d].name = input_scan.channels[
d].name;
100 for (
unsigned int i=0; i < laser_cloud.points.size(); i++)
103 filtered_scan.points[num_pts] = input_scan.points[i];
104 for (
unsigned int d = 0;
d < filtered_scan.channels.size ();
d++)
105 filtered_scan.channels[
d].values[num_pts] = input_scan.channels[
d].values[i];
111 filtered_scan.points.resize (num_pts);
112 for (
unsigned int d = 0;
d < filtered_scan.channels.size ();
d++)
113 filtered_scan.channels[
d].values.resize (num_pts);
133 #endif // LASER_SCAN_FOOTPRINT_FILTER_H
LaserScanMaskFilter removes points on directions defined in a mask from a laser scan.
bool getParam(const std::string &name, std::string &value)