From 9165cb9551a536cd9c387d8e9aca08156ff51ce2 Mon Sep 17 00:00:00 2001 From: Davide Depau Date: Sun, 11 Aug 2024 22:13:34 +0200 Subject: [PATCH] Fix code for new ArUcO markers --- bot.py | 37 ++++++++++++++++++++++++------------- 1 file changed, 24 insertions(+), 13 deletions(-) diff --git a/bot.py b/bot.py index c43b9f6..95dffa2 100644 --- a/bot.py +++ b/bot.py @@ -39,7 +39,7 @@ class Bot: self.tapo = Tapo(self.camera_ip, self.camera_user, self.camera_password) self.executor = ThreadPoolExecutor(max_workers=len(os.sched_getaffinity(0))) - self.last_aruco_corners = {} + self.last_aruco_corners = [None] * 4 def _get_presets(self): presets = self.tapo.getPresets() @@ -82,6 +82,9 @@ class Bot: vcap = cv2.VideoCapture( f"rtsp://{self.camera_user}:{self.camera_password}@{self.camera_ip}:554/stream1" ) + if chat_action_fn: + chat_action_fn() + print("Taking color image...") ret, pretty_image = vcap.read() while pretty_image is None: @@ -94,22 +97,31 @@ class Bot: self.tapo.setDayNightMode("on") # Iterate until we find all 4 aruco markers or timeout - aruco_corners = {} + aruco_corners = [None] * 4 + + def found_all(corners): + return all(c is not None for c in corners) + annotated_image = None aruco_dict = aruco.getPredefinedDictionary(aruco.DICT_4X4_50) aruco_params = aruco.DetectorParameters() + if chat_action_fn: + chat_action_fn() + t0 = time.time() last_chat_action = 0 print("Taking image with ArUco markers...") - while len(aruco_corners) < 4: + while not found_all(aruco_corners): delta = time.time() - t0 if delta > timeout: a = self.last_aruco_corners.copy() - a.update(aruco_corners) + for i in range(4): + if aruco_corners[i] is not None: + a[i] = aruco_corners[i] aruco_corners = a - if len(aruco_corners) == 4: + if found_all(aruco_corners): print("Timeout waiting for ArUco markers, using cached corners") break @@ -146,17 +158,15 @@ class Bot: self.tapo.setDayNightMode("auto") self.tapo.setPrivacyMode(privacy_mode) - corners = [aruco_corners[i] for i in range(1, 5)] - ids = np.array([[i] for i in range(1, 5)]) - aruco.drawDetectedMarkers(annotated_image, corners, ids) + aruco.drawDetectedMarkers(annotated_image, aruco_corners, np.array(range(4))) # Annotate the image with the detected markers and apply the perspective transform to the pretty image # Get the outermost points of each marker - bl_marker = corners[0].squeeze()[3] # bottom left marker - tl_marker = corners[1].squeeze()[0] # top left marker - tr_marker = corners[2].squeeze()[1] # top right marker - bc_marker = corners[3].squeeze()[3] # bottom center marker + tl_marker = aruco_corners[0].squeeze()[0] # top left marker + tr_marker = aruco_corners[1].squeeze()[1] # top right marker + bl_marker = aruco_corners[2].squeeze()[3] # bottom left marker + bc_marker = aruco_corners[3].squeeze()[2] # bottom center marker # Calculate the fourth point by computing the line through the bottom markers and intersecting with the vertical # line through the top right marker @@ -202,9 +212,10 @@ class Bot: await chat_action_fn() + loop = asyncio.get_event_loop() photos = await self.take_photo( adjust_perspective=adjust_perspective, - chat_action_fn=lambda: asyncio.run(chat_action_fn()) + chat_action_fn=lambda: loop.create_task(chat_action_fn()), ) jpegs = [cv2.imencode(".jpg", photo)[1].tobytes() for photo in photos]