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_SHADOWS_FILTER_H
00038 #define LASER_SCAN_SHADOWS_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 ScanShadowsFilter : public filters::FilterBase<sensor_msgs::LaserScan>
00052 {
00053 public:
00054
00055 double laser_max_range_;
00056 double min_angle_, max_angle_;
00057 int window_, neighbors_;
00058
00059
00061 ScanShadowsFilter ()
00062 {
00063
00064
00065 }
00066
00068 bool configure()
00069 {
00070 if (!filters::FilterBase<sensor_msgs::LaserScan>::getParam(std::string("min_angle"), min_angle_))
00071 {
00072 ROS_ERROR("Error: ShadowsFilter was not given min_angle.\n");
00073 return false;
00074 }
00075 if (!filters::FilterBase<sensor_msgs::LaserScan>::getParam(std::string("max_angle"), max_angle_))
00076 {
00077 ROS_ERROR("Error: ShadowsFilter was not given min_angle.\n");
00078 return false;
00079 }
00080 if (!filters::FilterBase<sensor_msgs::LaserScan>::getParam(std::string("window"), window_))
00081 {
00082 ROS_ERROR("Error: ShadowsFilter was not given window.\n");
00083 return false;
00084 }
00085 neighbors_ = 0;
00086 if (!filters::FilterBase<sensor_msgs::LaserScan>::getParam(std::string("neighbors"), neighbors_))
00087 {
00088 ROS_INFO("Error: ShadowsFilter was not given neighbors.\n");
00089 }
00090
00091 return true;
00092 }
00093
00095 virtual ~ScanShadowsFilter () { }
00096
00099 inline double getAngleWithViewpoint(float r1, float r2, float included_angle)
00100 {
00101 return atan2(r2 * sin(included_angle), r1 - r2 * cos(included_angle));
00102 }
00103
00104
00106
00113 bool update(const sensor_msgs::LaserScan& scan_in, sensor_msgs::LaserScan& scan_out)
00114 {
00115
00116 scan_out = scan_in;
00117
00118 std::set<int> indices_to_delete;
00119
00120 for (unsigned int i = 0; i < scan_in.ranges.size (); i++)
00121 {
00122 for (int y = -window_; y < window_ + 1; y++)
00123 {
00124 int j = i + y;
00125 if ( j < 0 || j >= (int)scan_in.ranges.size () || (int)i == j ){
00126 continue;
00127 }
00128
00129 double angle = abs(angles::to_degrees(getAngleWithViewpoint (scan_in.ranges[i],scan_in.ranges[j], y * scan_in.angle_increment)));
00130 if (angle < min_angle_ || angle > max_angle_)
00131 {
00132 for (int index = std::max<int>(i - neighbors_, 0); index <= std::min<int>(i+neighbors_, (int)scan_in.ranges.size()-1); index++)
00133 {
00134 if (scan_in.ranges[i] < scan_in.ranges[index])
00135 indices_to_delete.insert(index);
00136 }
00137 }
00138
00139 }
00140 }
00141
00142 ROS_DEBUG("ScanShadowsFilter removing %d Points from scan with min angle: %.2f, max angle: %.2f, neighbors: %d, and window: %d", (int)indices_to_delete.size(), min_angle_, max_angle_, neighbors_, window_);
00143 for ( std::set<int>::iterator it = indices_to_delete.begin(); it != indices_to_delete.end(); ++it)
00144 {
00145 scan_out.ranges[*it] = -1.0 * fabs(scan_in.ranges[*it]);
00146 }
00147 return true;
00148 }
00149
00151
00152 } ;
00153 }
00154
00155 #endif //LASER_SCAN_SHADOWS_FILTER_H