37 #ifndef LASER_SCAN_SHADOWS_FILTER_H 38 #define LASER_SCAN_SHADOWS_FILTER_H 44 #include <sensor_msgs/LaserScan.h> 72 ROS_ERROR(
"Error: ShadowsFilter was not given min_angle.\n");
77 ROS_ERROR(
"Error: ShadowsFilter was not given min_angle.\n");
82 ROS_ERROR(
"Error: ShadowsFilter was not given window.\n");
88 ROS_INFO(
"Error: ShadowsFilter was not given neighbors.\n");
90 remove_shadow_start_point_ =
false;
92 ROS_INFO(
"Remove shadow start point: %s", remove_shadow_start_point_ ?
"true" :
"false");
96 ROS_ERROR(
"min_angle must be 0 <= min_angle. Forcing min_angle = 0.\n");
101 ROS_ERROR(
"min_angle must be min_angle <= 90. Forcing min_angle = 90.\n");
106 ROS_ERROR(
"max_angle must be 90 <= max_angle. Forcing max_angle = 90.\n");
109 if (180 < min_angle_)
111 ROS_ERROR(
"max_angle must be max_angle <= 180. Forcing max_angle = 180.\n");
134 bool update(
const sensor_msgs::LaserScan& scan_in, sensor_msgs::LaserScan& scan_out)
139 std::set<int> indices_to_delete;
141 for (
unsigned int i = 0; i < scan_in.ranges.size(); i++)
143 for (
int y = -window_;
y < window_ + 1;
y++)
146 if (j < 0 || j >= (
int)scan_in.ranges.size() || (int)i == j)
152 scan_in.ranges[i], scan_in.ranges[j], y * scan_in.angle_increment))
154 for (
int index = std::max<int>(i - neighbors_, 0); index <= std::min<int>(i +
neighbors_, (int)scan_in.ranges.size() - 1); index++)
156 if (scan_in.ranges[i] < scan_in.ranges[index])
158 indices_to_delete.insert(index);
161 if (remove_shadow_start_point_)
163 indices_to_delete.insert(i);
169 ROS_DEBUG(
"ScanShadowsFilter removing %d Points from scan with min angle: %.2f, max angle: %.2f, neighbors: %d, and window: %d",
171 for (std::set<int>::iterator it = indices_to_delete.begin(); it != indices_to_delete.end(); ++it)
173 scan_out.ranges[*it] = std::numeric_limits<float>::quiet_NaN();
182 #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
bool getParam(const std::string &name, std::string &value)
bool remove_shadow_start_point_
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()