()
| 42 | |
| 43 | |
| 44 | def main(): |
| 45 | parser = argparse.ArgumentParser(description="ArUco Marker Detection Demo") |
| 46 | parser.add_argument("--ip", type=str, required=True, help="Vision Pro IP address") |
| 47 | args = parser.parse_args() |
| 48 | |
| 49 | print(f"[INFO] Connecting to Vision Pro at {args.ip}...") |
| 50 | streamer = VisionProStreamer(ip=args.ip, verbose=True) |
| 51 | |
| 52 | print("[INFO] Connected! Waiting for marker detection data...") |
| 53 | print("[INFO] Make sure 'Marker Detection' is enabled in the Vision Pro Settings panel.") |
| 54 | print("[INFO] Use the 'Fix' button on Vision Pro to freeze a marker's pose.") |
| 55 | print() |
| 56 | print("Status Legend: 📌=Fixed, 🔴=Live, 🟢=Tracked, ⚪=Not tracked") |
| 57 | print("-" * 75) |
| 58 | |
| 59 | last_print_time = 0 |
| 60 | print_interval = 0.5 # Print every 0.5 seconds |
| 61 | |
| 62 | try: |
| 63 | while True: |
| 64 | markers = streamer.get_markers() |
| 65 | current_time = time.time() |
| 66 | |
| 67 | if markers and current_time - last_print_time >= print_interval: |
| 68 | last_print_time = current_time |
| 69 | |
| 70 | # Count states |
| 71 | n_fixed = sum(1 for m in markers.values() if m.get("is_fixed", False)) |
| 72 | n_tracked = sum(1 for m in markers.values() if m.get("is_tracked", False)) |
| 73 | |
| 74 | print(f"\n[{time.strftime('%H:%M:%S')}] {len(markers)} marker(s) " |
| 75 | f"| {n_tracked} tracked | {n_fixed} fixed") |
| 76 | |
| 77 | for marker_id, info in sorted(markers.items()): |
| 78 | pose = info["pose"] # 4x4 homogeneous transform |
| 79 | position = pose[:3, 3] # XYZ translation |
| 80 | is_fixed = info.get("is_fixed", False) |
| 81 | is_tracked = info.get("is_tracked", False) |
| 82 | |
| 83 | # Visual indicator for state |
| 84 | status = get_status_icon(is_fixed, is_tracked) |
| 85 | fixed_str = "FIXED" if is_fixed else "live " |
| 86 | tracked_str = "TRACKED" if is_tracked else "lost " |
| 87 | |
| 88 | print(f" {status} ID {marker_id}: [{position[0]:6.3f}, {position[1]:6.3f}, {position[2]:6.3f}] m " |
| 89 | f"({fixed_str}, {tracked_str})") |
| 90 | |
| 91 | elif not markers: |
| 92 | if current_time - last_print_time >= 2.0: |
| 93 | last_print_time = current_time |
| 94 | print(f"[{time.strftime('%H:%M:%S')}] No markers detected. Point camera at ArUco markers.") |
| 95 | |
| 96 | time.sleep(0.05) # 20 Hz polling |
| 97 | |
| 98 | except KeyboardInterrupt: |
| 99 | print("\n[INFO] Stopping...") |
| 100 | |
| 101 |
no test coverage detected