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()