Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035 #ifndef LASER_SCAN_FOOTPRINT_FILTER_H
00036 #define LASER_SCAN_FOOTPRINT_FILTER_H
00037
00045 #include "filters/filter_base.h"
00046 #include "sensor_msgs/LaserScan.h"
00047 #include "tf/transform_listener.h"
00048 #include "sensor_msgs/PointCloud.h"
00049 #include "ros/ros.h"
00050 #include "laser_geometry/laser_geometry.h"
00051
00052 namespace laser_filters
00053 {
00054
00055 class LaserScanFootprintFilter : public filters::FilterBase<sensor_msgs::LaserScan>
00056 {
00057 public:
00058 LaserScanFootprintFilter(): up_and_running_(false)
00059 {
00060 ROS_WARN("LaserScanFootprintFilter has been deprecated. Please use PR2LaserScanFootprintFilter instead.\n");
00061 }
00062
00063 bool configure()
00064 {
00065 if(!getParam("inscribed_radius", inscribed_radius_))
00066 {
00067 ROS_ERROR("LaserScanFootprintFilter needs inscribed_radius to be set");
00068 return false;
00069 }
00070 return true;
00071 }
00072
00073 virtual ~LaserScanFootprintFilter()
00074 {
00075
00076 }
00077
00078 bool update(const sensor_msgs::LaserScan& input_scan, sensor_msgs::LaserScan& filtered_scan)
00079 {
00080 filtered_scan = input_scan ;
00081 sensor_msgs::PointCloud laser_cloud;
00082
00083 try{
00084 projector_.transformLaserScanToPointCloud("base_link", input_scan, laser_cloud, tf_);
00085 }
00086 catch(tf::TransformException& ex){
00087 if(up_and_running_){
00088 ROS_WARN_THROTTLE(1, "Dropping Scan: Transform unavailable %s", ex.what());
00089 }
00090 else {
00091 ROS_INFO_THROTTLE(.3, "Ignoring Scan: Waiting for TF");
00092 }
00093 return false;
00094 }
00095
00096 int c_idx = indexChannel(laser_cloud);
00097
00098 if (c_idx == -1 || laser_cloud.channels[c_idx].values.size () == 0){
00099 ROS_ERROR("We need an index channel to be able to filter out the footprint");
00100 return false;
00101 }
00102
00103 for (unsigned int i=0; i < laser_cloud.points.size(); i++)
00104 {
00105 if (inFootprint(laser_cloud.points[i])){
00106 int index = laser_cloud.channels[c_idx].values[i];
00107 filtered_scan.ranges[index] = filtered_scan.range_max + 1.0 ;
00108 }
00109 }
00110
00111 up_and_running_ = true;
00112 return true;
00113 }
00114
00115 int indexChannel(const sensor_msgs::PointCloud& scan_cloud){
00116 int c_idx = -1;
00117 for (unsigned int d = 0; d < scan_cloud.channels.size (); d++)
00118 {
00119 if (scan_cloud.channels[d].name == "index")
00120 {
00121 c_idx = d;
00122 break;
00123 }
00124 }
00125 return c_idx;
00126 }
00127
00128 bool inFootprint(const geometry_msgs::Point32& scan_pt){
00129 if(scan_pt.x < -1.0 * inscribed_radius_ || scan_pt.x > inscribed_radius_ || scan_pt.y < -1.0 * inscribed_radius_ || scan_pt.y > inscribed_radius_)
00130 return false;
00131 return true;
00132 }
00133
00134 private:
00135 tf::TransformListener tf_;
00136 laser_geometry::LaserProjection projector_;
00137 double inscribed_radius_;
00138 bool up_and_running_;
00139 } ;
00140
00141 }
00142
00143 #endif // LASER_SCAN_FOOTPRINT_FILTER_H