lookat image

reachy_mini_red_light_green_light/main.py

@@ -11,28 +11,27 @@ from reachy_mini.reachy_mini import (
     SLEEP_HEAD_JOINT_POSITIONS,
     SLEEP_ANTENNAS_JOINT_POSITIONS,
 )
-import cv2
-import mediapipe as mp
 from importlib.resources import files
 import reachy_mini_red_light_green_light
 from reachy_mini_red_light_green_light.people_detector import PeopleDetector
 
 UPPER_BODY_LANDMARKS = [11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22]
 
+
 class RedLightGreenLightApp(ReachyMiniApp):
     def run(self, reachy_mini: ReachyMini, stop_event: threading.Event):
         # cap = find_camera()
-        cap = cv2.VideoCapture(0) # debug
+        cap = cv2.VideoCapture(0)  # debug
         if cap is None:
             raise RuntimeError("No camera found. Please connect a camera.")
 
         # arbitrary 3 maximum people are tracked. Maybe make it a parameter?
         pose_landmarker_path: str = str(
-            files(reachy_mini_red_light_green_light).joinpath("pose_landmarker_full.task")
-        )
-        people_detector = PeopleDetector(
-            model_path=pose_landmarker_path, max_poses=3
+            files(reachy_mini_red_light_green_light).joinpath(
+                "pose_landmarker_full.task"
+            )
         )
+        people_detector = PeopleDetector(model_path=pose_landmarker_path, max_poses=3)
 
         def sequence():
             time.sleep(1)
@@ -113,7 +112,6 @@ class RedLightGreenLightApp(ReachyMiniApp):
                 if len(buffer) > 0:
                     if time.time() - last_watched >= (scan_for / (len(buffer) + 2)):
                         print("LOOK")
-                        # TODO replace with reachy_mini.look_at_image
                         selected_to_watch = np.random.randint(0, len(buffer))
                         left_shoulder = buffer[selected_to_watch][-1][0]
                         r_shoulder = buffer[selected_to_watch][-1][1]
@@ -121,23 +119,26 @@ class RedLightGreenLightApp(ReachyMiniApp):
                             (left_shoulder[0] + r_shoulder[0]) // 2,
                             (left_shoulder[1] + r_shoulder[1]) // 2,
                         )
-
-                        normalized_look_at = (
-                            ((look_at_px[0] / frame.shape[1]) - 0.5) * 2,
-                            ((look_at_px[1] / frame.shape[0]) - 0.5) * 2,
-                        )  # Normalize to [-1, 1]
-                        yaw_range = [-80, 80]
-                        target_yaw = (
-                            normalized_look_at[0] * (yaw_range[1] - yaw_range[0]) / 2
-                            + (yaw_range[1] + yaw_range[0]) / 2
-                        )
-                        pose = create_head_pose(
-                            yaw=-target_yaw,
-                        )
-                        reachy_mini.goto_target(
-                            head=pose, antennas=[0.5, 0.0], duration=0.3
+                        reachy_mini.look_at_image(
+                            look_at_px[0], look_at_px[1], duration=0.3
                         )
 
+                        # normalized_look_at = (
+                        #     ((look_at_px[0] / frame.shape[1]) - 0.5) * 2,
+                        #     ((look_at_px[1] / frame.shape[0]) - 0.5) * 2,
+                        # )  # Normalize to [-1, 1]
+                        # yaw_range = [-80, 80]
+                        # target_yaw = (
+                        #     normalized_look_at[0] * (yaw_range[1] - yaw_range[0]) / 2
+                        #     + (yaw_range[1] + yaw_range[0]) / 2
+                        # )
+                        # pose = create_head_pose(
+                        #     yaw=-target_yaw,
+                        # )
+                        # reachy_mini.goto_target(
+                        #     head=pose, antennas=[0.5, 0.0], duration=0.3
+                        # )
+
                         last_watched = time.time()
                         buffer = []
                         moved = []