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 "laser_filters/scan_shadow_detector.h"
00044 #include <sensor_msgs/LaserScan.h>
00045 #include <angles/angles.h>
00046
00047 namespace laser_filters
00048 {
00052 class ScanShadowsFilter : public filters::FilterBase<sensor_msgs::LaserScan>
00053 {
00054 public:
00055 double laser_max_range_;
00056 double min_angle_, max_angle_;
00057 int window_, neighbors_;
00058
00059 ScanShadowDetector shadow_detector_;
00060
00062 ScanShadowsFilter()
00063 {
00064 }
00065
00067 bool configure()
00068 {
00069 if (!filters::FilterBase<sensor_msgs::LaserScan>::getParam(std::string("min_angle"), min_angle_))
00070 {
00071 ROS_ERROR("Error: ShadowsFilter was not given min_angle.\n");
00072 return false;
00073 }
00074 if (!filters::FilterBase<sensor_msgs::LaserScan>::getParam(std::string("max_angle"), max_angle_))
00075 {
00076 ROS_ERROR("Error: ShadowsFilter was not given min_angle.\n");
00077 return false;
00078 }
00079 if (!filters::FilterBase<sensor_msgs::LaserScan>::getParam(std::string("window"), window_))
00080 {
00081 ROS_ERROR("Error: ShadowsFilter was not given window.\n");
00082 return false;
00083 }
00084 neighbors_ = 0;
00085 if (!filters::FilterBase<sensor_msgs::LaserScan>::getParam(std::string("neighbors"), neighbors_))
00086 {
00087 ROS_INFO("Error: ShadowsFilter was not given neighbors.\n");
00088 }
00089
00090 if (min_angle_ < 0)
00091 {
00092 ROS_ERROR("min_angle must be 0 <= min_angle. Forcing min_angle = 0.\n");
00093 min_angle_ = 0.0;
00094 }
00095 if (90 < min_angle_)
00096 {
00097 ROS_ERROR("min_angle must be min_angle <= 90. Forcing min_angle = 90.\n");
00098 min_angle_ = 90.0;
00099 }
00100 if (max_angle_ < 90)
00101 {
00102 ROS_ERROR("max_angle must be 90 <= max_angle. Forcing max_angle = 90.\n");
00103 max_angle_ = 90.0;
00104 }
00105 if (180 < min_angle_)
00106 {
00107 ROS_ERROR("max_angle must be max_angle <= 180. Forcing max_angle = 180.\n");
00108 max_angle_ = 180.0;
00109 }
00110 shadow_detector_.configure(
00111 angles::from_degrees(min_angle_),
00112 angles::from_degrees(max_angle_));
00113
00114 return true;
00115 }
00116
00118 virtual ~ScanShadowsFilter()
00119 {
00120 }
00121
00123
00130 bool update(const sensor_msgs::LaserScan& scan_in, sensor_msgs::LaserScan& scan_out)
00131 {
00132
00133 scan_out = scan_in;
00134
00135 std::set<int> indices_to_delete;
00136
00137 for (unsigned int i = 0; i < scan_in.ranges.size(); i++)
00138 {
00139 for (int y = -window_; y < window_ + 1; y++)
00140 {
00141 int j = i + y;
00142 if (j < 0 || j >= (int)scan_in.ranges.size() || (int)i == j)
00143 {
00144 continue;
00145 }
00146
00147 if (shadow_detector_.isShadow(
00148 scan_in.ranges[i], scan_in.ranges[j], y * scan_in.angle_increment))
00149 {
00150 for (int index = std::max<int>(i - neighbors_, 0); index <= std::min<int>(i + neighbors_, (int)scan_in.ranges.size() - 1); index++)
00151 {
00152 if (scan_in.ranges[i] < scan_in.ranges[index])
00153 {
00154 indices_to_delete.insert(index);
00155 }
00156 }
00157 }
00158 }
00159 }
00160
00161 ROS_DEBUG("ScanShadowsFilter removing %d Points from scan with min angle: %.2f, max angle: %.2f, neighbors: %d, and window: %d",
00162 (int)indices_to_delete.size(), min_angle_, max_angle_, neighbors_, window_);
00163 for (std::set<int>::iterator it = indices_to_delete.begin(); it != indices_to_delete.end(); ++it)
00164 {
00165 scan_out.ranges[*it] = std::numeric_limits<float>::quiet_NaN();
00166 }
00167 return true;
00168 }
00169
00171 };
00172 }
00173
00174 #endif // LASER_SCAN_SHADOWS_FILTER_H