重构xr teleop

2026-03-23 22:06:13 +08:00
parent d756e3ae0b
commit 05a146bca6
23 changed files with 3195 additions and 761 deletions
--- a/scripts/test_sim_streamer.py
+++ b/scripts/test_sim_streamer.py
@@ -0,0 +1,97 @@
+#!/usr/bin/env python3
+"""Standalone test for SimVideoStreamer: generates a color-cycling SBS test pattern
+and streams it to a TCP server (e.g. PICO VR headset).
+
+Usage:
+    # 1. On PICO: select SIM-Stereo, click Listen
+    # 2. On PC:
+    python scripts/test_sim_streamer.py --host 172.20.103.171 --port 12345
+
+    The PICO should display a side-by-side test pattern:
+    left half = cycling red, right half = cycling blue.
+"""
+
+import argparse
+import time
+
+import numpy as np
+
+
+def main():
+    parser = argparse.ArgumentParser(description="Test SimVideoStreamer with synthetic frames")
+    parser.add_argument("--host", type=str, required=True, help="PICO IP address (TCP server)")
+    parser.add_argument("--port", type=int, default=12345, help="TCP port")
+    parser.add_argument("--width", type=int, default=960, help="Eye width")
+    parser.add_argument("--height", type=int, default=540, help="Eye height")
+    parser.add_argument("--fps", type=int, default=30, help="Target FPS")
+    parser.add_argument("--duration", type=int, default=30, help="Stream duration in seconds")
+    args = parser.parse_args()
+
+    # Direct import to avoid pulling in the full mindbot/isaaclab dependency chain
+    import importlib.util, pathlib
+    _spec = importlib.util.spec_from_file_location(
+        "sim_video_streamer",
+        pathlib.Path(__file__).resolve().parents[1] / "source/mindbot/mindbot/utils/sim_video_streamer.py",
+    )
+    _mod = importlib.util.module_from_spec(_spec)
+    _spec.loader.exec_module(_mod)
+    SimVideoStreamer = _mod.SimVideoStreamer
+
+    streamer = SimVideoStreamer(
+        host=args.host,
+        port=args.port,
+        width=args.width,
+        height=args.height,
+        fps=args.fps,
+    )
+
+    print(f"Connecting to {args.host}:{args.port}...")
+    if not streamer.connect():
+        print("Failed to connect. Is PICO listening?")
+        return
+
+    print(f"Streaming {args.width*2}x{args.height} SBS test pattern for {args.duration}s...")
+    frame_interval = 1.0 / args.fps
+    start_time = time.time()
+    frame_count = 0
+
+    try:
+        while time.time() - start_time < args.duration:
+            t = time.time() - start_time
+
+            # Generate test pattern: cycling colors
+            # Left eye: red gradient with horizontal bars
+            left = np.zeros((args.height, args.width, 3), dtype=np.uint8)
+            red_val = int(128 + 127 * np.sin(t * 2.0))
+            left[:, :, 0] = red_val  # R channel
+            # Add horizontal position indicator
+            bar_y = int((args.height - 20) * (0.5 + 0.5 * np.sin(t * 1.5)))
+            left[bar_y:bar_y+20, :, :] = 255
+
+            # Right eye: blue gradient with horizontal bars
+            right = np.zeros((args.height, args.width, 3), dtype=np.uint8)
+            blue_val = int(128 + 127 * np.sin(t * 2.0 + 1.0))
+            right[:, :, 2] = blue_val  # B channel
+            right[bar_y:bar_y+20, :, :] = 255  # same bar position for stereo reference
+
+            if not streamer.send_frame(left, right):
+                print("Connection lost.")
+                break
+
+            frame_count += 1
+            # Maintain frame rate
+            elapsed = time.time() - start_time
+            expected = frame_count * frame_interval
+            if expected > elapsed:
+                time.sleep(expected - elapsed)
+
+    except KeyboardInterrupt:
+        print("\nStopped by user.")
+
+    elapsed = time.time() - start_time
+    print(f"Sent {frame_count} frames in {elapsed:.1f}s ({frame_count/elapsed:.1f} fps)")
+    streamer.close()
+
+
+if __name__ == "__main__":
+    main()