From ed453ea2d0b405e836e3c3c890272b5da57b96c9 Mon Sep 17 00:00:00 2001 From: teundeplanque Date: Thu, 28 Jan 2021 12:21:54 +0100 Subject: [PATCH] Added support for laserscanners that spin clockwise (angle increment is negative) --- include/laser_filters/angular_bounds_filter.h | 57 +++++++++++++------ 1 file changed, 41 insertions(+), 16 deletions(-) diff --git a/include/laser_filters/angular_bounds_filter.h b/include/laser_filters/angular_bounds_filter.h index e9f88b4d..903d653a 100644 --- a/include/laser_filters/angular_bounds_filter.h +++ b/include/laser_filters/angular_bounds_filter.h @@ -73,28 +73,53 @@ namespace laser_filters unsigned int count = 0; //loop through the scan and truncate the beginning and the end of the scan as necessary for(unsigned int i = 0; i < input_scan.ranges.size(); ++i){ - //wait until we get to our desired starting angle - if(start_angle < lower_angle_){ - start_angle += input_scan.angle_increment; - current_angle += input_scan.angle_increment; - start_time += ros::Duration(input_scan.time_increment); - } - else{ - filtered_scan.ranges[count] = input_scan.ranges[i]; + if(input_scan.angle_increment > 0){ //if the laserscanner turns counterclockwise + //wait until we get to our desired starting angle + if(start_angle < lower_angle_){ + start_angle += input_scan.angle_increment; + current_angle += input_scan.angle_increment; + start_time += ros::Duration(input_scan.time_increment); + } + else{ + filtered_scan.ranges[count] = input_scan.ranges[i]; - //make sure that we don't update intensity data if its not available - if(input_scan.intensities.size() > i) - filtered_scan.intensities[count] = input_scan.intensities[i]; + //make sure that we don't update intensity data if its not available + if(input_scan.intensities.size() > i) + filtered_scan.intensities[count] = input_scan.intensities[i]; - count++; + count++; - //check if we need to break out of the loop, basically if the next increment will put us over the threshold - if(current_angle + input_scan.angle_increment > upper_angle_){ - break; + //check if we need to break out of the loop, basically if the next increment will put us over the threshold + if(current_angle + input_scan.angle_increment > upper_angle_){ + break; + } + current_angle += input_scan.angle_increment; + } + } + else{ //the laserscanner turns clockwise + //wait until we get to our desired starting angle + if(start_angle > upper_angle_){ + start_angle += input_scan.angle_increment; + current_angle += input_scan.angle_increment; + start_time += ros::Duration(input_scan.time_increment); } + else{ + filtered_scan.ranges[count] = input_scan.ranges[i]; - current_angle += input_scan.angle_increment; + //make sure that we don't update intensity data if its not available + if(input_scan.intensities.size() > i) + filtered_scan.intensities[count] = input_scan.intensities[i]; + count++; + + //check if we need to break out of the loop, basically if the next increment will put us over the threshold + if(current_angle + input_scan.angle_increment < lower_angle_){ + break; + } + + current_angle += input_scan.angle_increment; + + } } }