Added support for laserscanners that spin clockwise (angle increment …

…is negative)
milvusrobotics · Jan 28, 2021 · ed453ea · ed453ea
1 parent 6b95207
commit ed453ea
Showing 1 changed file with 41 additions and 16 deletions.
diff --git 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;
+
+            }
           }
         }