✨

robot/arlo.py

@@ -1,4 +1,7 @@
 from time import sleep
+
+import cv2
+
 from .robot import Robot
 
 START_VALUES = [60, 60]
@@ -78,6 +81,38 @@ class Arlo():
         sleep((angle * self.timing[3])/1000)
         self.robot.stop()
 
+    def find_aruco(self):
+        image = self.robot.take_photo()
+
+        aruco_dict = cv2.aruco.Dictionary_get(cv2.aruco.DICT_6X6_250)
+        aruco_params = cv2.aruco.DetectorParameters_create()
+        corners, ids, _ = cv2.aruco.detectMarkers(
+            image,
+            aruco_dict,
+            parameters=aruco_params
+        )
+
+        for bounding, n in zip(corners, ids):
+            bounding_box = [(int(x), int(y)) for x, y in bounding.reshape((4,2))]
+            print(bounding_box)
+
+            cv2.line(image, bounding_box[0], bounding_box[1], (0,255,0), 2)
+            cv2.line(image, bounding_box[1], bounding_box[2], (0,255,0), 2)
+            cv2.line(image, bounding_box[2], bounding_box[3], (0,255,0), 2)
+            cv2.line(image, bounding_box[3], bounding_box[0], (0,255,0), 2)
+
+            cv2.putText(
+                image,
+                str(n[0]),
+                (bounding_box[0][0] - 10, bounding_box[0][1] - 10),
+                cv2.FONT_HERSHEY_COMPLEX,
+                0.8,
+                (0,255,0),
+                2
+            )
+
+        return image
+
     def _decode_calibration_code(self) -> None:
         wheel_values_hex, timing_hex = tuple(self.calibration_code.split("_"))
         self.wheel_values = [
@@ -135,3 +170,4 @@ class Arlo():
         )
         time_hex = "-".join([format(i, "x") for i in time])
         self.calibration_code = f"{values_hex}_{time_hex}"
+        print("Calibration code:", self.calibration_code)