lidar error handling

modules/detection/detection.py

@@ -60,6 +60,8 @@ def run(
             return False, None
 
         distance, angle = self.lidar.wait_for_reading(self.lidar.serial_port)
+        if distance == -1:
+            return False, None
 
         result, lidar_detection = detection_and_odometry.LidarDetection.create(distance, angle)
         if not result:

modules/detection/lidar_driver.py

@@ -271,12 +271,18 @@ def wait_for_reading(self, port, timeout=1):
         distance_in_cm = response[4] << 0 | response[5] << 8
         distance_in_metres = distance_in_cm / 100.0
 
+        if distance_in_metres < 0 or distance_in_metres > 50:
+            return -1, 0
+
         yaw_angle = response[6] << 0 | response[7] << 8
         if yaw_angle > 32000:
             yaw_angle = yaw_angle - 65535
 
         yaw_angle /= 100.0
 
+        if yaw_angle <= self.MIN_LOW_ANGLE or yaw_angle >= self.MAX_HIGH_ANGLE:
+            return -1, 0
+
         return distance_in_metres, yaw_angle
 
     def set_speed(self, port, value):

tests/integration/test_lidar_driver.py

@@ -22,7 +22,8 @@ def output_scan(lidar, duration):
     while time.time() < t_end:
         distance, angle = lidar.wait_for_reading(lidar.serial_port)
 
-        assert distance != -1
+        if distance == -1:
+            continue
 
         print(f"{distance} m {angle} deg")