✨

drive_between_arucos.py

@@ -37,19 +37,21 @@ def find_aruco(image):
     return [(box[0], ids[i]) for i, box in enumerate(corners)]
 
 def find_arucos(arlo):
-    aruco_list = []
+    aruco_dict = {}
 
     while True:
         arucos = find_aruco(arlo.take_photo())
         if arucos != []:
-            aruco_list += arucos
-            if len(aruco_list) >= 2:
+            for aruco, id_ in arucos:
+                aruco_dict[id_] = aruco
+
+            if len(aruco_dict) >= 2:
                 break
         arlo.go_diff(40, 40, 1, 0)
         sleep(0.3)
         arlo.stop()
 
-    return arucos[:2]
+    return list(arucos.items())[:2]
 
 def main():
     arlo = Robot()
@@ -57,7 +59,7 @@ def main():
 
     aruco_positions = np.array([cv2.aruco.estimatePoseSingleMarkers(
         np.array([aruco[0]]), 14.5, CAMERA_MATRIX, DIST_COEF
-    )[1][0][0] for aruco in arucos])
+    )[0][0] for aruco in arucos])
 
     position = [
         np.average(aruco_positions[:,0]),