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
00036
00037 #ifndef LASER_SCAN_FOOTOBJECT_FILTER_H
00038 #define LASER_SCAN_FOOTOBJECT_FILTER_H
00039
00040 #include <set>
00041
00042 #include "filters/filter_base.h"
00043 #include <sensor_msgs/LaserScan.h>
00044 #include "angles/angles.h"
00045
00046 namespace laser_filters{
00047
00051 class ScanFootObjectFilter : public filters::FilterBase<sensor_msgs::LaserScan>
00052 {
00053 public:
00054
00055 double max_radius_;
00056 int min_points_;
00057
00058
00060 ScanFootObjectFilter ()
00061 {
00062
00063
00064 }
00065
00067 bool configure()
00068 {
00069 max_radius_ = 0.1;
00070 if (!filters::FilterBase<sensor_msgs::LaserScan>::getParam(std::string("max_radius"), max_radius_))
00071 {
00072 ROS_ERROR("Error: FootObjectFilter was not given min_radius.\n");
00073 return false;
00074 }
00075
00076 min_points_ = 5;
00077 if (!filters::FilterBase<sensor_msgs::LaserScan>::getParam(std::string("min_points"), min_points_))
00078 {
00079 ROS_INFO("Error: FootObjectFilter was not given min_points.\n");
00080 return false;
00081 }
00082 return true;
00083 }
00084
00086 virtual ~ScanFootObjectFilter () { }
00087
00089
00093 bool update(const sensor_msgs::LaserScan& scan_in, sensor_msgs::LaserScan& scan_out)
00094 {
00095
00096 scan_out = scan_in;
00097
00098 std::set<int> indices_to_publish;
00099
00100 std::vector<std::vector<int> > range_blobs;
00101 std::vector<int> range_blob;
00102 for (unsigned int i = 0; i < scan_in.ranges.size (); i++)
00103 {
00104 scan_out.ranges[i] = -1.0 * fabs(scan_in.ranges[i]);
00105 if ( scan_in.ranges[i] < 0 || std::isnan(scan_in.ranges[i])) {
00106 if ( range_blob.size() > min_points_ ) {
00107 range_blobs.push_back(range_blob);
00108 }
00109 range_blob.clear();
00110 }else{
00111 range_blob.push_back(i);
00112 }
00113 }
00114 if ( range_blob.size() > min_points_ ) {
00115 range_blobs.push_back(range_blob);
00116 }
00117
00118 for (unsigned int i = 0; i < range_blobs.size(); i++) {
00119 int size = range_blobs[i].size();
00120
00121 double center_x = 0, center_y = 0;
00122 for (unsigned int j = 0; j < size; j++) {
00123 double x = scan_in.ranges[range_blobs[i][j]];
00124 double y = scan_in.ranges[range_blobs[i][j]] * scan_in.angle_increment;
00125 center_x += x;
00126 center_y += y;
00127 }
00128 center_x /= size;
00129 center_y /= size;
00130
00131
00132 double radius = 0;
00133 for (unsigned int j = 0; j < size; j++) {
00134 double x = scan_in.ranges[range_blobs[i][j]];
00135 double y = scan_in.ranges[range_blobs[i][j]] * scan_in.angle_increment;
00136 if ( radius < fabs(center_x - x) ) radius = fabs(center_x - x) ;
00137 if ( radius < fabs(center_y - y) ) radius = fabs(center_y - y) ;
00138 }
00139
00140 ROS_DEBUG_STREAM("blob center " << center_x << " " << center_y << ", radius " << radius << ", num of ponits " << size);
00141 if ( radius < max_radius_ ) {
00142 indices_to_publish.insert(range_blobs[i][0] + size/2);
00143 }
00144 }
00145 ROS_DEBUG("ScanFootObjectFilter %d Points from scan with min radius: %.2f, num of pints: %d", (int)indices_to_publish.size(), max_radius_, min_points_);
00146 for ( std::set<int>::iterator it = indices_to_publish.begin(); it != indices_to_publish.end(); ++it)
00147 {
00148 scan_out.ranges[*it] = fabs(scan_in.ranges[*it]);
00149 }
00150 return true;
00151 }
00152
00154
00155 } ;
00156 }
00157
00158 #endif //LASER_SCAN_FOOTOBJECT_FILTER_H