37 #ifndef LASER_SCAN_SHADOWS_FILTER_H 38 #define LASER_SCAN_SHADOWS_FILTER_H 44 #include <sensor_msgs/LaserScan.h> 71 ROS_ERROR(
"Error: ShadowsFilter was not given min_angle.\n");
76 ROS_ERROR(
"Error: ShadowsFilter was not given min_angle.\n");
81 ROS_ERROR(
"Error: ShadowsFilter was not given window.\n");
87 ROS_INFO(
"Error: ShadowsFilter was not given neighbors.\n");
92 ROS_ERROR(
"min_angle must be 0 <= min_angle. Forcing min_angle = 0.\n");
97 ROS_ERROR(
"min_angle must be min_angle <= 90. Forcing min_angle = 90.\n");
102 ROS_ERROR(
"max_angle must be 90 <= max_angle. Forcing max_angle = 90.\n");
105 if (180 < min_angle_)
107 ROS_ERROR(
"max_angle must be max_angle <= 180. Forcing max_angle = 180.\n");
130 bool update(
const sensor_msgs::LaserScan& scan_in, sensor_msgs::LaserScan& scan_out)
135 std::set<int> indices_to_delete;
137 for (
unsigned int i = 0; i < scan_in.ranges.size(); i++)
139 for (
int y = -window_;
y < window_ + 1;
y++)
142 if (j < 0 || j >= (
int)scan_in.ranges.size() || (int)i == j)
148 scan_in.ranges[i], scan_in.ranges[j], y * scan_in.angle_increment))
150 for (
int index = std::max<int>(i - neighbors_, 0); index <= std::min<int>(i +
neighbors_, (int)scan_in.ranges.size() - 1); index++)
152 if (scan_in.ranges[i] < scan_in.ranges[index])
154 indices_to_delete.insert(index);
161 ROS_DEBUG(
"ScanShadowsFilter removing %d Points from scan with min angle: %.2f, max angle: %.2f, neighbors: %d, and window: %d",
163 for (std::set<int>::iterator it = indices_to_delete.begin(); it != indices_to_delete.end(); ++it)
165 scan_out.ranges[*it] = std::numeric_limits<float>::quiet_NaN();
174 #endif // LASER_SCAN_SHADOWS_FILTER_H ScanShadowDetector shadow_detector_
void configure(const float min_angle, const float max_angle)
LaserScanMaskFilter removes points on directions defined in a mask from a laser scan.
TFSIMD_FORCE_INLINE const tfScalar & y() const
static double from_degrees(double degrees)
bool update(const sensor_msgs::LaserScan &scan_in, sensor_msgs::LaserScan &scan_out)
Filter shadow points based on 3 global parameters: min_angle, max_angle and window. {min,max}_angle specify the allowed angle interval (in degrees) between the created lines (see getAngleWithViewPoint). Window specifies how many consecutive measurements to take into account for one point.
bool isShadow(const float r1, const float r2, const float included_angle)
virtual ~ScanShadowsFilter()