✨

find_aruco.py

@@ -1,5 +1,18 @@
+import cv2
+import robot
+
 def main():
-    pass
+    arlo = robot.Robot()
+    image = arlo.take_photo()
+
+    aruco_dict = cv2.aruco.Dictionary_get(cv2.aruco.DICT_6X6_250)
+    aruco_params = cv2.aruco.DetectorParameters_create()
+    detected = cv2.aruco.detectMarkers(
+        image,
+        aruco_dict,
+        parameters=aruco_params
+    )
+    print(detected)
 
 if __name__ == "__main__":
     main()

robot/robot.py

@@ -2,7 +2,7 @@
 
 from time import sleep
 import serial
-from picamera import PiCamera
+import cv2
 
 class Robot(object):
     """
@@ -36,6 +36,8 @@ class Robot(object):
         while not self.serial_read.isOpen():
             sleep(1)
 
+        self._init_camera()
+
         print("Waiting for serial port connection ...")
         sleep(2)
 
@@ -52,6 +54,34 @@ class Robot(object):
         print((self.send_command(cmd)))
         self.serial_read.close()
 
+    def _init_camera(self):
+        def gstreamer_pipeline(
+            capture_width=1024, capture_height=720, framerate=30):
+            """
+            Utility function for setting parameters for the gstreamer camera
+            pipeline
+            """
+            return (
+                "libcamerasrc !"
+                "video/x-raw, width=(int)%d, height=(int)%d, framerate=(fraction)%d/1 ! "
+                "videoconvert ! "
+                "appsink"
+                % (
+                    capture_width,
+                    capture_height,
+                    framerate,
+                )
+            )
+
+        self.cam = cv2.VideoCapture(
+            gstreamer_pipeline(),
+            apiPreference=cv2.CAP_GSTREAMER
+        )
+
+    def take_photo(self):
+        _, photo = self.cam.read()
+        return photo
+
     def send_command(self, cmd : str, sleep_ms: float=0.0):
         """Sends a command to the Arduino robot controller"""
         self.serial_read.write(cmd.encode('ascii'))