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 #include <math.h>
00036 #include <ros/ros.h>
00037 #include <sensor_msgs/LaserScan.h>
00038 #include <tf/transform_listener.h>
00039
00040 class LaserFootprintFilter
00041 {
00042 public:
00043 LaserFootprintFilter()
00044 : nh_("~"), listener_(ros::Duration(10))
00045 {
00046 scan_filtered_pub_ = nh_.advertise<sensor_msgs::LaserScan>("/scan_filtered", 1);
00047 scan_sub_ = nh_.subscribe("/scan", 1000, &LaserFootprintFilter::update, this);
00048
00049 nh_.param<double>("footprint_inscribed_radius", inscribed_radius_, 0.16495*1.1);
00050 nh_.param<std::string>("base_frame", base_frame_, "/base_link");
00051 }
00052
00053 void update(const sensor_msgs::LaserScan& input_scan)
00054 {
00055 sensor_msgs::LaserScan filtered_scan;
00056 filtered_scan = input_scan;
00057
00058 double angle = filtered_scan.angle_min - filtered_scan.angle_increment;
00059 geometry_msgs::PointStamped p_input;
00060 p_input.header = input_scan.header;
00061
00062 for(size_t i=0; i < filtered_scan.ranges.size(); i++)
00063 {
00064 angle += filtered_scan.angle_increment;
00065 if(filtered_scan.ranges[i] >= filtered_scan.range_max) continue;
00066
00067 p_input.point.x = cos(angle) * filtered_scan.ranges[i];
00068 p_input.point.y = sin(angle) * filtered_scan.ranges[i];
00069
00070 geometry_msgs::PointStamped p_transformed;
00071 try{
00072 listener_.transformPoint(base_frame_, p_input, p_transformed);
00073 }catch(tf::TransformException &ex){
00074 ROS_ERROR("Received an exception trying to transform a point: %s", ex.what());
00075 return;
00076 }
00077
00078 if( inFootprint(p_transformed) )
00079 {
00080 filtered_scan.ranges[i] = filtered_scan.range_max + 1.0;
00081 }
00082
00083 }
00084
00085 scan_filtered_pub_.publish(filtered_scan);
00086 }
00087
00088
00089 bool inFootprint(const geometry_msgs::PointStamped& scan_pt)
00090 {
00091
00092 if (sqrt(scan_pt.point.x*scan_pt.point.x + scan_pt.point.y*scan_pt.point.y) > inscribed_radius_)
00093 return false;
00094 return true;
00095 }
00096
00097 private:
00098 ros::NodeHandle nh_;
00099 tf::TransformListener listener_;
00100 double inscribed_radius_;
00101 std::string base_frame_;
00102 ros::Publisher scan_filtered_pub_;
00103 ros::Publisher debug_pub_;
00104 ros::Subscriber scan_sub_;
00105 };
00106
00107 int main(int argc, char** argv)
00108 {
00109 ros::init(argc, argv, "laser_footprint_filter");
00110
00111 LaserFootprintFilter filter;
00112 ros::spin();
00113 }
00114