update

reachy_mini_greetings/main.py

@@ -12,119 +12,174 @@ import time
 
 
 class GreetingsApp(ReachyMiniApp):
-    def run(self, reachy_mini: ReachyMini, stop_event: threading.Event):
-        head_tracker = HeadTracker()
-        emotions = RecordedMoves("pollen-robotics/reachy-mini-emotions-library")
-
-        moves = [
-            "proud1",
-            "proud2",
-            "success1",
-            "cheerful1",
-            "welcoming1",
-            "loving1",
-            "surprised1",
-        ]
-        is_scared = True
-        # did_one_turn = False
-        # tracking = False
-        # was_tracking = False
-        # tracking_lost_since = time.time()
-
-        t0 = time.time()
-        while not stop_event.is_set():
-            frame = reachy_mini.media.get_frame()
-            # cv2.imshow("frame", frame)
-            # cv2.waitKey(1)
-
-            # if did_one_turn:
-            #     eye_center, roll = head_tracker.get_head_position(frame)
-            #     if eye_center is not None:
-            #         tracking = True
-            #     else:
-            #         tracking_lost_since = time.time()
-
-            #     if time.time() - tracking_lost_since > 2:
-            #         tracking = False
-
-            #     if eye_center is not None and tracking:
-            #         h, w, _ = frame.shape
-            #         eye_center = (eye_center + 1) / 2
-            #         eye_center[0] *= w
-            #         eye_center[1] *= h
-            #         reachy_mini.look_at_image(*eye_center, duration=0.0)
-
-            # if not did_one_turn and not tracking:
-            pose = create_head_pose(0, 0, 0, 0, 0, 0)
-            reachy_mini.goto_target(
-                pose, duration=1, antennas=[0, 0], method="ease_in_out"
-            )
-            time.sleep(0.5)
-
-            if is_scared:
-                reachy_mini.media.play_sound("impatient1.wav")
-                pose = create_head_pose(-0.01, 0, -0.035, 0, 15, 0)
-                reachy_mini.goto_target(
-                    pose, duration=0.3, antennas=[-1.5, 1.5], method="ease_in_out"
-                )
-                time.sleep(0.2)
-
-                pose = create_head_pose(0, 0, -0.02, 0, 0, 0)
-                reachy_mini.goto_target(
-                    pose, duration=2, antennas=[-1, 1], method="ease_in_out"
-                )
-                time.sleep(0.5)
-                is_scared = False
-
-            pose = create_head_pose(0.0, 0.02, -0.02, 20, 0, 0)
+    head_tracker = HeadTracker()
+    emotions = RecordedMoves("pollen-robotics/reachy-mini-emotions-library")
+
+    moves = [
+        "proud1",
+        "proud2",
+        "success1",
+        "cheerful1",
+        "welcoming1",
+        "loving1",
+        "surprised1",
+    ]
+    first_start = True
+    is_tracking = False
+    last_track_time = time.time()
+    lost_track_timeout = 2.0  # s
+    stop_tracking_timeout = 15.0  # s
+
+    def wake_up_sequence(self, reachy_mini: ReachyMini):
+        print("Start wake up sequence")
+        pose = create_head_pose(0, 0, 0, 0, 0, 0)
+        reachy_mini.goto_target(pose, duration=1, antennas=[0, 0], method="ease_in_out")
+        time.sleep(0.05)
+
+        if self.first_start:
+            reachy_mini.media.play_sound("impatient1.wav")
+            pose = create_head_pose(-0.01, 0, -0.035, 0, 15, 0)
             reachy_mini.goto_target(
-                pose, duration=0.5, antennas=[-1.5, 0], method="ease_in_out"
+                pose, duration=0.3, antennas=[-1.5, 1.5], method="ease_in_out"
             )
             time.sleep(0.2)
 
-            pose = create_head_pose(0.0, -0.02, -0.02, -20, 0, 0)
+            pose = create_head_pose(0, 0, -0.02, 0, 0, 0)
             reachy_mini.goto_target(
-                pose, duration=0.5, antennas=[0, 1.6], method="ease_in_out"
-            )
-            time.sleep(0.2)
-
-            pose = create_head_pose(0, 0, 0, 0, 0, 0)
-            reachy_mini.goto_target(
-                pose, duration=0.5, antennas=[0, 0], method="ease_in_out"
-            )
-            reachy_mini.play_move(
-                emotions.get(np.random.choice(moves)), initial_goto_duration=1.0
-            )
-            reachy_mini.media.play_sound("confused1.wav")
-
-            pose = create_head_pose(0, 0, 0.02, 0, -20, -140)
-            reachy_mini.goto_target(
-                pose,
-                body_yaw=np.deg2rad(-100),
-                duration=1,
-                antennas=[-1.5, 0],
-                method="ease_in_out",
+                pose, duration=2, antennas=[-1, 1], method="ease_in_out"
             )
             time.sleep(0.5)
 
-            pose = create_head_pose(0, 0, 0, 0, 0, 0)
-            reachy_mini.goto_target(
-                pose, duration=1, antennas=[0, 0], method="ease_in_out"
-            )
+        pose = create_head_pose(0.0, 0.02, -0.02, 20, 0, 0)
+        reachy_mini.goto_target(
+            pose, duration=0.5, antennas=[-1.5, 0], method="ease_in_out"
+        )
+        if not self.first_start:
+            for _ in range(10):
+                frame = reachy_mini.media.get_frame()
+                ret = self.track_head(reachy_mini, frame)
+                if ret and np.random.rand() < 0.5:
+                    reachy_mini.goto_target(body_yaw=0, duration=0.3)
+                    return
+        time.sleep(0.2)
+
+        pose = create_head_pose(0.0, -0.02, -0.02, -20, 0, 0)
+        reachy_mini.goto_target(
+            pose, duration=0.5, antennas=[0, 1.6], method="ease_in_out"
+        )
+        if not self.first_start:
+            for _ in range(10):
+                frame = reachy_mini.media.get_frame()
+                ret = self.track_head(reachy_mini, frame)
+                if ret and np.random.rand() < 0.5:
+                    reachy_mini.goto_target(body_yaw=0, duration=0.3)
+                    return
+        time.sleep(0.2)
+
+        pose = create_head_pose(0, 0, 0, 0, 0, 0)
+        reachy_mini.goto_target(
+            pose, duration=0.5, antennas=[0, 0], method="ease_in_out"
+        )
+        reachy_mini.play_move(
+            self.emotions.get(np.random.choice(self.moves)), initial_goto_duration=1.0
+        )
+        reachy_mini.media.play_sound("confused1.wav")
+
+        if not self.first_start:
+            for _ in range(10):
+                frame = reachy_mini.media.get_frame()
+                ret = self.track_head(reachy_mini, frame)
+                if ret and np.random.rand() < 0.5:
+                    reachy_mini.goto_target(body_yaw=0, duration=0.3)
+                    return
+
+        pose = create_head_pose(0, 0, 0.02, 0, -20, -140)
+        reachy_mini.goto_target(
+            pose,
+            body_yaw=np.deg2rad(-100),
+            duration=1,
+            antennas=[-1.5, 0],
+            method="ease_in_out",
+        )
+        if not self.first_start:
+            for _ in range(10):
+                frame = reachy_mini.media.get_frame()
+                ret = self.track_head(reachy_mini, frame)
+                if ret and np.random.rand() < 0.5:
+                    reachy_mini.goto_target(body_yaw=0, duration=0.3)
+                    return
+        time.sleep(0.5)
+
+        pose = create_head_pose(0, 0, 0, 0, 0, 0)
+        reachy_mini.goto_target(pose, duration=1, antennas=[0, 0], method="ease_in_out")
+
+        if np.random.rand() < 0.2:
+            reachy_mini.media.play_sound("dance1.wav")
+        pose = create_head_pose(0, 0, 0.02, 0, -30, 100)
+        reachy_mini.goto_target(
+            pose,
+            body_yaw=np.deg2rad(80),
+            duration=1,
+            antennas=[0, 1.5],
+            method="ease_in_out",
+        )
+        if not self.first_start:
+            for _ in range(10):
+                frame = reachy_mini.media.get_frame()
+                ret = self.track_head(reachy_mini, frame)
+                if ret and np.random.rand() < 0.5:
+                    reachy_mini.goto_target(body_yaw=0, duration=0.3)
+                    return
+        time.sleep(0.5)
+        pose = create_head_pose(0, 0, 0, 0, 0, 0)
+        reachy_mini.goto_target(pose, duration=1, antennas=[0, 0], method="ease_in_out")
+
+        self.first_start = False
+
+    def track_head(self, reachy_mini: ReachyMini, frame: np.ndarray):
+        eye_center, roll = self.head_tracker.get_head_position(frame)
+        if eye_center is None:
+            if (
+                self.is_tracking
+                and time.time() - self.last_tracking_time > self.lost_track_timeout
+            ):
+                self.is_tracking = False
+                print("Lost track of the face")
+            return False
+
+        if not self.is_tracking:
+            self.start_tracking_time = time.time()
+        self.is_tracking = True
+        self.last_tracking_time = time.time()
+        h, w, _ = frame.shape
+        eye_center = (eye_center + 1) / 2
+        eye_center[0] *= w
+        eye_center[1] *= h
+        reachy_mini.look_at_image(*eye_center, duration=0.0)
+
+        if time.time() - self.start_tracking_time > self.stop_tracking_timeout:
+            self.is_tracking = False
+            print("Stop tracking the face")
+            reachy_mini.goto_target(body_yaw=0, duration=0.5)
+            return False
+        return True
 
-            if np.random.rand() < 0.2:
-                reachy_mini.media.play_sound("dance1.wav")
-            pose = create_head_pose(0, 0, 0.02, 0, -30, 100)
-            reachy_mini.goto_target(
-                pose,
-                body_yaw=np.deg2rad(80),
-                duration=1,
-                antennas=[0, 1.5],
-                method="ease_in_out",
-            )
-            time.sleep(0.5)
-
-            print("reset")
+    def run(self, reachy_mini: ReachyMini, stop_event: threading.Event):
+        t0 = time.time()
+        while not stop_event.is_set():
+            frame = reachy_mini.media.get_frame()
+            if self.first_start:
+                self.wake_up_sequence(reachy_mini)
+            else:
+                if not self.is_tracking:
+                    for _ in range(10):
+                        self.track_head(reachy_mini, frame)
+                        frame = reachy_mini.media.get_frame()
+                else:
+                    ret = self.track_head(reachy_mini, frame)
+                # print("found face" if ret else "lost face")
+                if not self.is_tracking:
+                    self.wake_up_sequence(reachy_mini)
 
             time.sleep(0.02)