mirror of
https://github.com/Rulez777/FramePicoTracker.git
synced 2026-10-17 14:01:15 +02:00
129 lines
4.2 KiB
Python
129 lines
4.2 KiB
Python
# SPDX-License-Identifier: GPL-3.0-or-later
|
|
# Copyright (C) 2026 L8OLOOE and contributors. See CREDITS.md and NOTICE.md.
|
|
"""Main bridge server: receives Pico Motion Tracker frames and publishes to SteamVR VMT.
|
|
|
|
Usage:
|
|
python main.py [--port 39500] [--vmt-port 39570] [--calib calibration.json]
|
|
"""
|
|
|
|
from __future__ import annotations
|
|
|
|
import argparse
|
|
import logging
|
|
import sys
|
|
import time
|
|
from pathlib import Path
|
|
from typing import Optional
|
|
|
|
from calibration import SpaceTransform
|
|
from coords import unity_to_openvr_pos, unity_to_openvr_quat
|
|
from protocol import FramePayload, HelloPayload
|
|
from receiver import StreamReceiver
|
|
from vmt_client import VMTClient
|
|
|
|
logging.basicConfig(
|
|
level=logging.INFO,
|
|
format="%(asctime)s [%(levelname)s] %(name)s: %(message)s",
|
|
datefmt="%H:%M:%S",
|
|
)
|
|
logger = logging.getLogger("pico_bridge")
|
|
|
|
|
|
class BridgeApp:
|
|
def __init__(
|
|
self,
|
|
tcp_port: int = 39500,
|
|
vmt_port: int = 39570,
|
|
calib_file: Optional[Path] = None,
|
|
) -> None:
|
|
self.vmt = VMTClient(port=vmt_port)
|
|
self.calib = (
|
|
SpaceTransform.load(calib_file)
|
|
if calib_file and calib_file.exists()
|
|
else SpaceTransform.identity()
|
|
)
|
|
if calib_file and calib_file.exists():
|
|
logger.info("Loaded space calibration from %s", calib_file)
|
|
else:
|
|
logger.info("Using identity space transform (no calibration file loaded)")
|
|
|
|
self.receiver = StreamReceiver(
|
|
port=tcp_port,
|
|
on_hello=self.handle_hello,
|
|
on_frame=self.handle_frame,
|
|
)
|
|
|
|
self._frame_count = 0
|
|
self._last_report_time = time.monotonic()
|
|
self._last_reported_frame = 0
|
|
|
|
def handle_hello(self, hello: HelloPayload) -> None:
|
|
logger.info("Pico Headset Connected: Device=%s, SDK=%s, App=%s", hello.device, hello.sdk, hello.app)
|
|
|
|
def handle_frame(self, frame: FramePayload) -> None:
|
|
self._frame_count += 1
|
|
now = time.monotonic()
|
|
|
|
# Update FPS statistics every 2 seconds
|
|
dt = now - self._last_report_time
|
|
if dt >= 2.0:
|
|
fps = (self._frame_count - self._last_reported_frame) / dt
|
|
valid_count = sum(1 for t in frame.trackers if t.valid)
|
|
logger.info(
|
|
"Streaming @ %.1f Hz | Trackers active: %d/%d | Frame ID: %d",
|
|
fps,
|
|
valid_count,
|
|
len(frame.trackers),
|
|
frame.frame,
|
|
)
|
|
self._last_report_time = now
|
|
self._last_reported_frame = self._frame_count
|
|
|
|
# Map each tracker to a VMT device index
|
|
for idx, tr in enumerate(frame.trackers):
|
|
if not tr.valid:
|
|
continue
|
|
|
|
# 1. Convert Unity (left-handed) to OpenVR (right-handed)
|
|
vr_pos = unity_to_openvr_pos(tr.pos)
|
|
vr_rot = unity_to_openvr_quat(tr.rot)
|
|
|
|
# 2. Apply space calibration (Pico space -> Steam Frame space)
|
|
calib_pos = self.calib.apply_pos(vr_pos)
|
|
calib_rot = self.calib.apply_rot(vr_rot)
|
|
|
|
# 3. Publish to SteamVR via VMT
|
|
self.vmt.send_pose(
|
|
index=idx,
|
|
pos=calib_pos,
|
|
quat=calib_rot,
|
|
enabled=True,
|
|
time_offset=0.0,
|
|
)
|
|
|
|
# 4. Optional battery report
|
|
if tr.battery >= 0.0:
|
|
self.vmt.send_diag(idx, f"bat: {int(tr.battery * 100)}%")
|
|
|
|
def run(self) -> None:
|
|
try:
|
|
self.receiver.start()
|
|
except KeyboardInterrupt:
|
|
logger.info("Shutting down bridge...")
|
|
self.receiver.stop()
|
|
|
|
|
|
def main() -> None:
|
|
parser = argparse.ArgumentParser(description="Pico Motion Tracker -> SteamVR Bridge")
|
|
parser.add_argument("--port", type=int, default=39500, help="TCP listen port (default 39500)")
|
|
parser.add_argument("--vmt-port", type=int, default=39570, help="VMT OSC port (default 39570)")
|
|
parser.add_argument("--calib", type=Path, default=None, help="Path to space calibration JSON file")
|
|
args = parser.parse_args()
|
|
|
|
app = BridgeApp(tcp_port=args.port, vmt_port=args.vmt_port, calib_file=args.calib)
|
|
app.run()
|
|
|
|
|
|
if __name__ == "__main__":
|
|
main()
|