guard.py

import os
import time
import cv2
import numpy as np
import requests
import subprocess
import platform
from reachy_mini import ReachyMini
from reachy_mini.utils import create_head_pose

SCRIPT_DIR = os.path.dirname(os.path.abspath(__file__))
RECORD_DIR = os.path.join(SCRIPT_DIR, "record")
os.makedirs(RECORD_DIR, exist_ok=True)


# ----------------- 加入 Monkey Patch 解決遠端連線問題 -----------------
from reachy_mini.media.media_manager import MediaManager
if not hasattr(MediaManager, "_is_patched"):
    original_media_manager_init = MediaManager.__init__
    def mocked_media_manager_init(self, *args, **kwargs):
        # 如果傳入的信令伺服器不是 localhost，強制改為 localhost 以便走 SSH Tunnel
        if kwargs.get("signalling_host") not in (None, "localhost", "127.0.0.1"):
            kwargs["signalling_host"] = "localhost"
        original_media_manager_init(self, *args, **kwargs)
    # 替換原有的初始化函數
    MediaManager.__init__ = mocked_media_manager_init
    MediaManager._is_patched = True
# ---------------------------------------------------------------------

def capture_and_check(mini, image_path, prompt):
    frame = mini.media.get_frame()
    while frame is None:
        print("Waiting for camera frame...")
        time.sleep(0.1)
        frame = mini.media.get_frame()
        
    frame_bgr = cv2.cvtColor(frame, cv2.COLOR_RGB2BGR)
    cv2.imwrite(image_path, frame_bgr)
    print(f"Image saved as {image_path}")

    try:
        from image_check import check_image
        return check_image(image_path, prompt)
    except Exception as e:
        print(f"Error executing image check: {e}")
        return None


def warning_action(mini, count=10):
    for _ in range(count):
        mini.goto_target(
            head=create_head_pose(y=0.0, yaw=0, mm=True),
            antennas=np.deg2rad([30, -30]),
            body_yaw=np.deg2rad(0),
            duration=0.3,
            method="cartoon"
        )
        time.sleep(0.1)

        mini.goto_target(
            head=create_head_pose(y=0.0, yaw=0, mm=True),
            antennas=np.deg2rad([-30, 30]),
            body_yaw=np.deg2rad(0),
            duration=0.3,
            method="cartoon"
        )
        time.sleep(0.1)

def run_guard_workflow(mini):
    print("Connected successfully!")
    
    # Enable motors (Torque ON) so the robot can move
    print("Enabling motors (Torque ON)...")
    mini.enable_motors()
    time.sleep(0.5)
    
    print("Moving robot up (goto_target)...")
    mini.goto_target(
        head=create_head_pose(z=10, mm=True),    # Up 5mm
        antennas=np.deg2rad([0, 0]),           # Antennas out
        body_yaw=np.deg2rad(0),                 # Turn body 30 degrees
        duration=2.0,                            # Slow movement
        method="minjerk"                         # Smooth movement
    )
    time.sleep(0.5)
    
    def process_check_result(result_text):
        if not result_text:
            print("No text result returned from check_image.")
            return False
        try:
            import ast
            result_json = ast.literal_eval(result_text)
            print(result_json)
            if isinstance(result_json, dict) and result_json.get('danger') == 1:
                mini.media.play_sound(os.path.join(os.path.dirname(os.path.abspath(__file__)), "asset", "warning_song.mp3"))
                warning_action(mini)
                mini.media.stop_playing()
                return True
        except Exception as e:
            print(f"Failed to parse result JSON: {result_text}, error: {e}")
        return False

    for _ in range(1):

        mini.goto_target(
            head=create_head_pose(y=0.0, yaw=0, mm=True),    # Up 5mm
            antennas=np.deg2rad([0, 0]),           # Antennas out
            body_yaw=np.deg2rad(0),                 # Turn body 30 degrees
            duration=3,                            # Slow movement
            method="minjerk"                         # Smooth movement
        )
        time.sleep(0.1)
        result_text = capture_and_check(mini, os.path.join(RECORD_DIR, "image.jpg"), "What is in the photo? Please output the result in the following format: {'danger': 0, 'reason': 'text_reason'}. If it is a dangerous creature, like a snake, output 1 for 'danger'; otherwise, output 0, and explain the reason in 'reason'. Remember to enclose JSON values in quotes.")
        if process_check_result(result_text):
            continue

        mini.goto_target(
            head=create_head_pose(y=0.0, yaw=60, mm=True),    # Up 5mm
            antennas=np.deg2rad([0, 0]),           # Antennas out
            body_yaw=np.deg2rad(60),                 # Turn body 30 degrees
            duration=3,                            # Slow movement
            method="minjerk"                         # Smooth movement
        )
        time.sleep(0.1)
        result_text = capture_and_check(mini, os.path.join(RECORD_DIR, "image.jpg"), "What is in the photo? Please output the result in the following format: {'danger': 0, 'reason': 'text_reason'}. If it is a dangerous creature, like a snake, output 1 for 'danger'; otherwise, output 0, and explain the reason in 'reason'. Remember to enclose JSON values in quotes.")
        if process_check_result(result_text):
            continue

        mini.goto_target(
            head=create_head_pose(y=0.0, yaw=0, mm=True),    # Up 5mm
            antennas=np.deg2rad([0, 0]),           # Antennas out
            body_yaw=np.deg2rad(0),                 # Turn body 30 degrees
            duration=3,                            # Slow movement
            method="minjerk"                         # Smooth movement
        )
        time.sleep(0.1)
        result_text = capture_and_check(mini, os.path.join(RECORD_DIR, "image.jpg"), "What is in the photo? Please output the result in the following format: {'danger': 0, 'reason': 'text_reason'}. If it is a dangerous creature, like a snake, output 1 for 'danger'; otherwise, output 0, and explain the reason in 'reason'. Remember to enclose JSON values in quotes.")
        if process_check_result(result_text):
            continue

        mini.goto_target(
            head=create_head_pose(y=0.0, yaw=-60, mm=True),    # Up 5mm
            antennas=np.deg2rad([0, 0]),           # Antennas out
            body_yaw=np.deg2rad(-60),                 # Turn body 30 degrees
            duration=3,                            # Slow movement
            method="minjerk"                         # Smooth movement
        )
        time.sleep(0.1)
        result_text = capture_and_check(mini, os.path.join(RECORD_DIR, "image.jpg"), "What is in the photo? Please output the result in the following format: {'danger': 0, 'reason': 'text_reason'}. If it is a dangerous creature, like a snake, output 1 for 'danger'; otherwise, output 0, and explain the reason in 'reason'. Remember to enclose JSON values in quotes.")
        process_check_result(result_text)


    
    print("Moving robot down (goto_target)...")
    mini.goto_target(
        head=create_head_pose(z=-15, mm=True),    # Back down to 0mm
        antennas=np.deg2rad([0, 0]),             # Antennas back to 0
        body_yaw=np.deg2rad(0),                  # Turn body back to 0
        duration=2.0,                            # Slow movement
        method="minjerk"                         # Smooth movement
    )
    print("Down movement completed!")
    time.sleep(0.5)
    
    # Disable motors (Torque OFF) to protect motors and make it compliant
    print("Disabling motors (Torque OFF - making robot limp)...")
    mini.disable_motors()
    print("Done!")

def apply_safe_goto(mini):
    original_goto = mini.goto_target
    if not hasattr(original_goto, "_is_safe"):
        def safe_goto(*args, **kwargs):
            try:
                return original_goto(*args, **kwargs)
            except Exception as e:
                if "timeout" in str(e).lower() or "TimeoutError" in type(e).__name__:
                    print(f"⚠️ [網路延遲] 動作發送超時但可能已在執行中，繼續下一個工作... ({e})")
                else:
                    raise e
        safe_goto._is_safe = True
        mini.goto_target = safe_goto

def main():
    print("Connecting to Reachy Mini...")
    shared_mini = None
    try:
        from app import robot_manager
        if robot_manager and robot_manager.mini is not None:
            shared_mini = robot_manager.mini
    except Exception:
        pass

    if shared_mini is not None:
        print("Reusing existing ReachyMini connection from RobotManager.")
        apply_safe_goto(shared_mini)
        run_guard_workflow(shared_mini)
    else:
        # Standard standalone execution
        with ReachyMini(connection_mode="network", host="localhost") as mini:
            apply_safe_goto(mini)
            run_guard_workflow(mini)

if __name__ == "__main__":
    main()