✨

estimate_distance.py

@@ -0,0 +1,10 @@
+from robot import Arlo
+
+def main():
+    arlo = Arlo(False)
+    image = arlo.robot.take_photo()
+    bounding_boxes = arlo.find_aruco(image)
+    arlo.estimate_distance(bounding_boxes[0])
+
+if __name__ == "__main__":
+    main(9)

robot/arlo.py

@@ -8,7 +8,9 @@ START_VALUES = [60, 60]
 THRESHOLD = 1.05
 SLEEP_TIME = 2
 
-FOCAL_LENGTH = 0
+FOCAL_LENGTH = 1691
+
+DEFAULT_CALIBRATION_CODE = "40.40-40.40-40.40_ff-ff-ff-ff"
 
 def test_forward(arlo, l_power, r_power):
     arlo.reset_encoder_counts()
@@ -55,6 +57,9 @@ class Arlo():
         self.robot = Robot()
         if calibration_code is None:
             self._calibrate()
+        elif calibration_code is None:
+            self.calibration_code = DEFAULT_CALIBRATION_CODE
+            self._decode_calibration_code()
         else:
             self.calibration_code = calibration_code
             self._decode_calibration_code()
@@ -84,7 +89,17 @@ class Arlo():
         self.robot.stop()
 
     def estimate_distance(self, bounding_box):
-        average_left = bounding_box[0][0]
+        avg_left   = (bounding_box[0][0] + bounding_box[3][0])/2
+        avg_right  = (bounding_box[1][0] + bounding_box[2][0])/2
+        avg_up     = (bounding_box[0][1] + bounding_box[1][1])/2
+        avg_down   = (bounding_box[2][1] + bounding_box[3][1])/2
+
+        avg_width  = avg_right - avg_left
+        avg_height = avg_down - avg_up
+
+        avg_size = (avg_width + avg_height)/2
+
+        return (FOCAL_LENGTH * 15)/avg_size
 
     def find_aruco(self, image):
         aruco_dict = cv2.aruco.Dictionary_get(cv2.aruco.DICT_6X6_250)
@@ -95,7 +110,7 @@ class Arlo():
             parameters=aruco_params
         )
 
-        return zip(corners, ids)
+        return zip(corners, ids) if corners is not None else []
 
     def draw_arucos(self, image, bounding_boxes):
         for bounding, n in bounding_boxes: