mirror of
https://github.com/Rulez777/FramePicoTracker.git
synced 2026-10-18 07:01:14 +02:00
69 lines
3.0 KiB
Python
69 lines
3.0 KiB
Python
# SPDX-License-Identifier: GPL-3.0-or-later
|
|
# Copyright (C) 2026 L8OLOOE and contributors. See CREDITS.md and NOTICE.md.
|
|
"""Versioned loopback IPC for measured OpenVR world poses (PTP2).
|
|
|
|
Time is Linux CLOCK_MONOTONIC on Frame, never a remote PC clock. The producer
|
|
must convert calibrated optical measurements to OpenVR standing space first.
|
|
"""
|
|
from dataclasses import dataclass
|
|
import math
|
|
import socket
|
|
import struct
|
|
import time
|
|
|
|
WIRE = struct.Struct('<4sIIQQ15f')
|
|
MAX_AGE_NS = 250_000_000
|
|
|
|
|
|
@dataclass(frozen=True)
|
|
class PoseFrame:
|
|
chip_id: int
|
|
sequence: int
|
|
source_time_ns: int
|
|
position: tuple[float, float, float]
|
|
quaternion: tuple[float, float, float, float]
|
|
linear_velocity: tuple[float, float, float] = (0., 0., 0.)
|
|
angular_velocity: tuple[float, float, float] = (0., 0., 0.)
|
|
connected: bool = False
|
|
pose_valid: bool = False
|
|
quality: float = 0.
|
|
battery_percent: float | None = None
|
|
|
|
def encode(self, now_ns=None):
|
|
now_ns = time.monotonic_ns() if now_ns is None else now_ns
|
|
if (type(self.chip_id) is not int or not 0 < self.chip_id <= 0xffffffff
|
|
or type(self.sequence) is not int or not 0 <= self.sequence < 2**64
|
|
or type(self.source_time_ns) is not int or self.source_time_ns <= 0
|
|
or not 0 <= now_ns-self.source_time_ns <= MAX_AGE_NS):
|
|
raise ValueError('Invalid pose identity, sequence or source time')
|
|
if (len(self.position),len(self.quaternion),len(self.linear_velocity),
|
|
len(self.angular_velocity)) != (3,4,3,3):
|
|
raise ValueError('Invalid pose dimensions')
|
|
values = (*self.position,*self.quaternion,*self.linear_velocity,*self.angular_velocity)
|
|
if not all(math.isfinite(x) and abs(x) <= 1e6 for x in values):
|
|
raise ValueError('Invalid pose values')
|
|
norm = sum(x*x for x in self.quaternion)
|
|
if not .81 <= norm <= 1.21:
|
|
raise ValueError('Invalid quaternion norm')
|
|
if not math.isfinite(self.quality) or not 0 <= self.quality <= 1:
|
|
raise ValueError('Invalid tracking quality')
|
|
if self.pose_valid and (not self.connected or self.quality <= 0):
|
|
raise ValueError('Valid pose requires connection and measurement quality')
|
|
if self.battery_percent is not None and (
|
|
not math.isfinite(self.battery_percent) or not 0 <= self.battery_percent <= 100):
|
|
raise ValueError('Invalid verified battery percent')
|
|
flags = int(self.connected) | (int(self.pose_valid)<<1) | (int(self.battery_percent is not None)<<2)
|
|
return WIRE.pack(b'PTP2',self.chip_id,flags,self.sequence,self.source_time_ns,
|
|
*values,-1 if self.battery_percent is None else self.battery_percent,self.quality)
|
|
|
|
|
|
class NativePosePublisher:
|
|
def __init__(self):
|
|
self.socket = socket.socket(socket.AF_INET,socket.SOCK_DGRAM)
|
|
|
|
def send(self, frame: PoseFrame):
|
|
self.socket.sendto(frame.encode(),('127.0.0.1',39571))
|
|
|
|
def close(self):
|
|
self.socket.close()
|