✨

drive_between_arucos.py

@@ -1,4 +1,5 @@
 from time import sleep
+from math import cos, sin
 
 import cv2
 import numpy as np
@@ -38,20 +39,29 @@ def find_aruco(image):
 
 def find_arucos(arlo):
     aruco_dict = {}
+    theta = np.deg2rad(30)
+    rot = [[[cos(theta), -sin(theta)], [sin(theta), cos(theta)]]]
 
     while True:
         arucos = find_aruco(arlo.take_photo())
         if arucos != []:
             for aruco, id_ in arucos:
-                if id_ in [1,6]:
+                if id_ in [1,6] and id_ not in aruco_dict:
                     print(f"found box {id_}")
-                    aruco_dict[id_] = aruco
+                    position = cv2.aruco.estimatePoseSingleMarkers(
+                        np.array([aruco]), 14.5, CAMERA_MATRIX, DIST_COEF
+                    )[1][0][0]
+                    position = np.array([position[0], position[1]])
+                    position[0] += 22.5
+                    aruco_dict[id_] = position
 
             if len(aruco_dict) >= 2:
                 break
-        arlo.go_diff(40, 40, 1, 0)
-        sleep(0.3)
+        arlo.go_diff(POWER, POWER, 1, 0)
+        sleep((20 * TURN_T)/1000)
         arlo.stop()
+        for key, value in aruco_dict.items():
+            aruco_dict[key] = np.dot(rot, value)
 
     return list(aruco_dict.values())[:2]
 
@@ -64,6 +74,8 @@ def main():
         np.array([aruco]), 14.5, CAMERA_MATRIX, DIST_COEF
     )[1][0][0] for aruco in arucos])
 
+    print(aruco_positions)
+
     position = [
         np.average(aruco_positions[:,0]),
         np.average(aruco_positions[:,2])