From a90ef9d8f96c28403c8bcd2b2b2b99d6d96a38cf Mon Sep 17 00:00:00 2001 From: NikolajDanger Date: Wed, 28 Sep 2022 13:39:25 +0200 Subject: [PATCH] :sparkles: --- drive_between_arucos.py | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/drive_between_arucos.py b/drive_between_arucos.py index f86a30f..57d7e36 100644 --- a/drive_between_arucos.py +++ b/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]),