37 #ifndef LASER_SCAN_FOOTOBJECT_FILTER_H 38 #define LASER_SCAN_FOOTOBJECT_FILTER_H 43 #include <sensor_msgs/LaserScan.h> 72 ROS_ERROR(
"Error: FootObjectFilter was not given min_radius.\n");
79 ROS_INFO(
"Error: FootObjectFilter was not given min_points.\n");
93 bool update(
const sensor_msgs::LaserScan& scan_in, sensor_msgs::LaserScan& scan_out)
98 std::set<int> indices_to_publish;
100 std::vector<std::vector<int> > range_blobs;
101 std::vector<int> range_blob;
102 for (
unsigned int i = 0; i < scan_in.ranges.size (); i++)
104 scan_out.ranges[i] = -1.0 * fabs(scan_in.ranges[i]);
105 if ( scan_in.ranges[i] < 0 || std::isnan(scan_in.ranges[i])) {
107 range_blobs.push_back(range_blob);
111 range_blob.push_back(i);
115 range_blobs.push_back(range_blob);
118 for (
unsigned int i = 0; i < range_blobs.size(); i++) {
119 int size = range_blobs[i].size();
121 double center_x = 0, center_y = 0;
122 for (
unsigned int j = 0; j < size; j++) {
123 double x = scan_in.ranges[range_blobs[i][j]];
124 double y = scan_in.ranges[range_blobs[i][j]] * scan_in.angle_increment;
133 for (
unsigned int j = 0; j < size; j++) {
134 double x = scan_in.ranges[range_blobs[i][j]];
135 double y = scan_in.ranges[range_blobs[i][j]] * scan_in.angle_increment;
136 if ( radius < fabs(center_x - x) ) radius = fabs(center_x - x) ;
137 if ( radius < fabs(center_y - y) ) radius = fabs(center_y - y) ;
140 ROS_DEBUG_STREAM(
"blob center " << center_x <<
" " << center_y <<
", radius " << radius <<
", num of ponits " << size);
141 if ( radius < max_radius_ ) {
142 indices_to_publish.insert(range_blobs[i][0] + size/2);
145 ROS_DEBUG(
"ScanFootObjectFilter %d Points from scan with min radius: %.2f, num of pints: %d", (
int)indices_to_publish.size(),
max_radius_,
min_points_);
146 for ( std::set<int>::iterator it = indices_to_publish.begin(); it != indices_to_publish.end(); ++it)
148 scan_out.ranges[*it] = fabs(scan_in.ranges[*it]);
158 #endif //LASER_SCAN_FOOTOBJECT_FILTER_H
TFSIMD_FORCE_INLINE const tfScalar & y() const
TFSIMD_FORCE_INLINE const tfScalar & x() const
#define ROS_DEBUG_STREAM(args)