From 58861a8c204091d4359d8c73c401b232fadef503 Mon Sep 17 00:00:00 2001 From: jenny_yu Date: Mon, 17 Aug 2026 16:37:50 +0800 Subject: [PATCH] feat(vive_se3_tracker): stream VIVE Ultimate Trackers as per-tracker SE3 poses MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Add a standalone plugin plus record/replay examples that stream each VIVE Ultimate Tracker as its own generic SE3 (6-DoF) pose collection (se3_tracker.fbs from PR #778) — one tensor collection per device. Plugin (src/plugins/vive_se3_tracker): - Reads live poses from the VIVEHub tracker_server via the VUT SDK client (on a receiver thread) and pushes Se3TrackerPose through a per-device SchemaPusher keyed on Se3Tracker::TENSOR_IDENTIFIER / DEFAULT_MAX_FLATBUFFER_SIZE. - Timestamps pass through untranslated: VUT pose timestamps are host CLOCK_MONOTONIC, the same clock SchemaPusher documents as the local common clock, so the VUT sample time is forwarded verbatim (no push-loop bias). - Collections are named by the tracker's physical serial number, which rides on the pose wire (VUT SDK, VIVEHub >= 1.0.1; Pose::serial), fixed on the first pose (collections can't rename); an empty serial falls back to vive_tracker_. The serial is the stable identity; mapping a tracker to a role belongs downstream. - Push policy per device: valid at the VUT sample time on a new sample, skip when unchanged (no duplicate timestamps in MCAP), is_valid=false + identity filler when stale. - Advertises live collection ids to VIVE_SE3_COLLECTIONS_FILE (default /tmp/vive_se3_collections.txt) so readers auto-discover them; the tensor-list enumeration used by SchemaTracker is C++-only, so this is the Python-side discovery hook. - Requires the VUT SDK: pass -DVUT_SDK_DIR=/path/to/VIVEHub-Linux/sdk at configure time; if unset the plugin is skipped (like the other vendor-SDK plugins), so a plain source or wheel build still configures. - VIVE_SE3_SYNTHETIC=1 fakes trackers for a no-hardware smoke test. Examples (examples/mcap_record_replay/python): - record_se3_vive.py records N seconds (default 10) of every advertised collection, no arguments needed. - replay_se3_vive.py replays the newest recording, auto-discovers collections from the MCAP channel names, derives the playback rate from the recording's own timestamps, and renders each tracker as a viser 3D coordinate frame. Verified on real hardware (VIVEHub 1.0.1): live trackers -> per-serial collections -> se3_printer / no-arg MCAP record + replay round-trip. Signed-off-by: jenny_yu --- CMakeLists.txt | 1 + .../python/record_se3_vive.py | 151 +++++++ .../python/replay_se3_vive.py | 264 ++++++++++++ src/plugins/vive_se3_tracker/CMakeLists.txt | 41 ++ src/plugins/vive_se3_tracker/README.md | 119 +++++ src/plugins/vive_se3_tracker/main.cpp | 61 +++ src/plugins/vive_se3_tracker/plugin.yaml | 11 + .../vive_se3_tracker_plugin.cpp | 407 ++++++++++++++++++ .../vive_se3_tracker_plugin.hpp | 155 +++++++ 9 files changed, 1210 insertions(+) create mode 100644 examples/mcap_record_replay/python/record_se3_vive.py create mode 100644 examples/mcap_record_replay/python/replay_se3_vive.py create mode 100644 src/plugins/vive_se3_tracker/CMakeLists.txt create mode 100644 src/plugins/vive_se3_tracker/README.md create mode 100644 src/plugins/vive_se3_tracker/main.cpp create mode 100644 src/plugins/vive_se3_tracker/plugin.yaml create mode 100644 src/plugins/vive_se3_tracker/vive_se3_tracker_plugin.cpp create mode 100644 src/plugins/vive_se3_tracker/vive_se3_tracker_plugin.hpp diff --git a/CMakeLists.txt b/CMakeLists.txt index a2f74a8f4f..844f1e11ed 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -184,6 +184,7 @@ if(BUILD_PLUGINS) add_subdirectory(src/plugins/plugin_utils) add_subdirectory(src/plugins/controller_se3_tracker) + add_subdirectory(src/plugins/vive_se3_tracker) add_subdirectory(src/plugins/controller_synthetic_hands) add_subdirectory(src/plugins/generic_3axis_pedal) add_subdirectory(src/plugins/so101_leader) diff --git a/examples/mcap_record_replay/python/record_se3_vive.py b/examples/mcap_record_replay/python/record_se3_vive.py new file mode 100644 index 0000000000..3e19b47c7d --- /dev/null +++ b/examples/mcap_record_replay/python/record_se3_vive.py @@ -0,0 +1,151 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 HTC Corporation. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +""" +Record live VIVE Ultimate Tracker SE3 pose streams to an MCAP file — headset-free. + +Reads the per-tracker "vive_tracker_" tensor collections pushed by the +vive_se3_tracker plugin (one core.Se3Tracker per collection) and records each +one to its own MCAP channel pair ( / _tracked) via the standard +Se3TrackerRecordingTraits. Replay with replay_se3_vive.py. + +Prerequisites (separate terminals): + 1. CloudXR runtime: python -m isaacteleop.cloudxr + 2. the pusher: ./vive_se3_tracker_plugin + (VIVEHub tracker_server running, or VIVE_SE3_SYNTHETIC=1 for a smoke test) + +With no arguments it records 10 s of every collection the pusher currently +advertises (see --collections), to a timestamped file under ../recordings/. + +Usage: + source ~/.cloudxr/run/cloudxr.env + uv run record_se3_vive.py [duration_s] [output.mcap] [--collections a,b,c] +""" + +import argparse +import os +import sys +import time +from datetime import datetime +from pathlib import Path + +from isaacteleop.deviceio_session import DeviceIOSession, McapRecordingConfig +from isaacteleop.deviceio_trackers import Se3Tracker +from isaacteleop.oxr import OpenXRSession + + +def _collections_file() -> str: + """Where the pusher advertises live collection ids (one per line). + + Must mirror resolve_collections_file() in the plugin: VIVE_SE3_COLLECTIONS_FILE + overrides wholesale; otherwise the per-user $XDG_RUNTIME_DIR (systemd's + mode-0700 dir); otherwise a private per-user dir under /tmp + (vive_se3_tracker-) — never a fixed world-writable /tmp path. + """ + env = os.environ.get("VIVE_SE3_COLLECTIONS_FILE") + if env: + return env + xdg = os.environ.get("XDG_RUNTIME_DIR") + if xdg: + return os.path.join(xdg, "vive_se3_collections.txt") + base = os.path.join("/tmp", f"vive_se3_tracker-{os.getuid()}") # noqa: S108 - per-user 0700 dir, matches plugin + return os.path.join(base, "vive_se3_collections.txt") + + +DEFAULT_COLLECTIONS_FILE = _collections_file() + + +def discover_collections() -> list[str]: + """Read the collection ids the running pusher advertises, in file order.""" + path = Path(DEFAULT_COLLECTIONS_FILE) + if not path.is_file(): + return [] + seen = {} + for line in path.read_text().splitlines(): + cid = line.strip() + if cid: + seen.setdefault(cid, None) + return list(seen) + + +def main(argv: list[str]) -> int: + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument( + "duration", nargs="?", type=float, default=10.0, help="Recording duration (s)" + ) + parser.add_argument("output", nargs="?", help="Output .mcap path") + parser.add_argument( + "--collections", + default=None, + help="Comma-separated collection ids (default: auto-discover from the running pusher)", + ) + args = parser.parse_args(argv[1:]) + + if args.output: + mcap_path = Path(args.output) + mcap_path.parent.mkdir(parents=True, exist_ok=True) + else: + out_dir = Path(__file__).resolve().parent.parent / "recordings" + out_dir.mkdir(exist_ok=True) + mcap_path = out_dir / f"se3_vive_{datetime.now():%Y%m%d_%H%M%S}.mcap" + + if args.collections: + collections = [c.strip() for c in args.collections.split(",") if c.strip()] + else: + collections = discover_collections() + if not collections: + print( + "[record-se3] no live collections found " + f"({DEFAULT_COLLECTIONS_FILE} missing or empty). " + "Is the vive_se3_tracker pusher running and a tracker streaming? " + "Or pass --collections explicitly.", + file=sys.stderr, + ) + return 1 + print(f"[record-se3] discovered collections: {', '.join(collections)}") + + trackers = {cid: Se3Tracker(cid) for cid in collections} + + # MCAP channel base name = collection id (channels "" + "_tracked"). + recording = McapRecordingConfig( + str(mcap_path), tracker_names=[(t, cid) for cid, t in trackers.items()] + ) + + print(f"[record-se3] writing {mcap_path} for {args.duration:.1f}s") + for cid in collections: + print(f"[record-se3] collection '{cid}'") + + tracker_list = list(trackers.values()) + extensions = DeviceIOSession.get_required_extensions(tracker_list) + with OpenXRSession("McapSe3ViveRecord", extensions) as oxr_session: + with DeviceIOSession.run( + tracker_list, oxr_session.get_handles(), recording + ) as session: + start = time.time() + frame = 0 + while time.time() - start < args.duration: + session.update() + if frame % 60 == 0: + parts = [] + for cid, tracker in trackers.items(): + data = tracker.get_data(session) + if data.data is None: + parts.append(f"{cid}: -") + elif not data.data.is_valid: + parts.append(f"{cid}: lost") + else: + p = data.data.pose.position + parts.append(f"{cid}: [{p.x:+.2f} {p.y:+.2f} {p.z:+.2f}]") + print( + f"[record-se3] t={time.time() - start:5.2f}s " + + " ".join(parts) + ) + frame += 1 + time.sleep(1 / 90) + + print(f"[record-se3] done — {mcap_path}") + return 0 + + +if __name__ == "__main__": + sys.exit(main(sys.argv)) diff --git a/examples/mcap_record_replay/python/replay_se3_vive.py b/examples/mcap_record_replay/python/replay_se3_vive.py new file mode 100644 index 0000000000..be05b5b9f3 --- /dev/null +++ b/examples/mcap_record_replay/python/replay_se3_vive.py @@ -0,0 +1,264 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 HTC Corporation. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +""" +Replay VIVE Ultimate Tracker SE3 pose streams from an MCAP file — no runtime needed. + +Opens a recording made by record_se3_vive.py, replays each per-tracker channel +through ReplaySession + core.Se3Tracker (ReplaySe3TrackerImpl), and shows every +tracker as a coordinate frame in a viser 3D view (browser). Replay needs no +OpenXR runtime and no hardware. + +With no arguments it replays the most recent recording under ../recordings/, +auto-discovers its collections, and plays back at the recording's own capture +rate. Open the printed viser URL to see the trackers in 3D. + +Usage: + uv run replay_se3_vive.py [recording.mcap] [--loop] [--rate N] [--no-viz] \ + [--collections a,b,c] +""" + +import argparse +import sys +import time +from pathlib import Path + +from mcap.reader import make_reader + +from isaacteleop.deviceio_session import McapReplayConfig, ReplaySession +from isaacteleop.deviceio_trackers import Se3Tracker + +# SE3 recordings write two channels per collection: "/se3_tracker" and +# "/se3_tracker_tracked". Strip either suffix to recover the collection id. +_CHANNEL_SUFFIXES = ("/se3_tracker_tracked", "/se3_tracker") + + +def _summary(mcap_path: Path): + with open(mcap_path, "rb") as f: + return make_reader(f).get_summary() + + +def resolve_mcap(path_arg: str | None) -> Path: + """Use the given path, or the newest .mcap under ../recordings/.""" + if path_arg: + return Path(path_arg) + recordings = Path(__file__).resolve().parent.parent / "recordings" + candidates = list(recordings.glob("se3_vive_*.mcap")) or list( + recordings.glob("*.mcap") + ) + if not candidates: + sys.exit( + f"[replay-se3] no .mcap files in {recordings}. Run record_se3_vive.py first." + ) + return max(candidates, key=lambda p: p.stat().st_mtime) + + +def discover_collections(mcap_path: Path) -> list[str]: + """Return the SE3 collection ids present in an MCAP, in first-seen order.""" + seen = {} + summary = _summary(mcap_path) + channels = summary.channels.values() if summary else [] + for ch in channels: + for suffix in _CHANNEL_SUFFIXES: + if ch.topic.endswith(suffix): + seen.setdefault(ch.topic[: -len(suffix)], None) + break + return list(seen) + + +def capture_rate_hz(mcap_path: Path, default: float = 30.0) -> float: + """Estimate playback rate from the per-tick channel replay actually consumes. + + Replay advances one frame per ReplaySession.update() and reads the coalesced + "/se3_tracker_tracked" channel (one message per recording tick). Rate is + derived from that channel's message count -- not the raw "/se3_tracker" sample + channel, whose count can exceed the tick count when the producer bursts + several samples per tick (which would make playback run too fast). + """ + summary = _summary(mcap_path) + if not summary or not summary.statistics: + return default + stats = summary.statistics + span_ns = stats.message_end_time - stats.message_start_time + if span_ns <= 0: + return default + tracked_ids = [ + cid + for cid, ch in summary.channels.items() + if ch.topic.endswith("/se3_tracker_tracked") + ] + if not tracked_ids: + return default + counts = stats.channel_message_counts + best = max((counts.get(cid, 0) for cid in tracked_ids), default=0) + if best <= 1: + return default + return best / (span_ns / 1e9) + + +# Fixed distinct axis-label colors per tracker slot (viser label background). +TRACKER_COLORS = [ + (230, 60, 60), + (60, 180, 75), + (65, 105, 225), + (240, 160, 30), + (150, 90, 200), +] + + +class Se3Viz: + """One coordinate frame + name label per tracker in a viser scene.""" + + def __init__(self, server, collections: list[str]): + import viser # noqa: F401 (import here so --no-viz runs without viser) + + server.scene.set_up_direction("+y") + server.scene.add_grid(name="/grid", width=2.0, height=2.0, cell_size=0.1) + self._frames = {} + self._labels = {} + for i, cid in enumerate(collections): + self._frames[cid] = server.scene.add_frame( + f"/trackers/{cid}", axes_length=0.15, axes_radius=0.006, visible=False + ) + self._labels[cid] = server.scene.add_label( + f"/trackers/{cid}/label", text=cid.removeprefix("vive_tracker_") + ) + + def update(self, cid: str, pose, valid: bool) -> None: + frame = self._frames[cid] + if not valid or pose is None: + frame.visible = False + return + p, q = pose.position, pose.orientation + frame.position = (p.x, p.y, p.z) + # schema quaternion is (x, y, z, w); viser expects (w, x, y, z). + frame.wxyz = (q.w, q.x, q.y, q.z) + frame.visible = True + + +def run_once(session, trackers, viz, rate_hz: float) -> tuple[int, dict]: + frames = 0 + samples = {cid: 0 for cid in trackers} + none_streak = 0 + tick = 1.0 / rate_hz if rate_hz > 0 else 0.0 + # update() is void; per the replay contract tracker data goes null at + # end-of-file, so stop after a run of all-null frames. + while none_streak < 30: + session.update() + frames += 1 + any_data = False + parts = [] + for cid, tracker in trackers.items(): + data = tracker.get_data(session) + if data.data is None: + parts.append(f"{cid}: -") + if viz: + viz.update(cid, None, False) + continue + any_data = True + samples[cid] += 1 + valid = bool(data.data.is_valid) + if viz: + viz.update(cid, data.data.pose, valid) + if not valid: + parts.append(f"{cid}: lost") + else: + p = data.data.pose.position + parts.append(f"{cid}: [{p.x:+.2f} {p.y:+.2f} {p.z:+.2f}]") + none_streak = 0 if any_data else none_streak + 1 + if frames % 60 == 1: + print(f"[replay-se3] frame={frames:5d} " + " ".join(parts)) + if tick: + time.sleep(tick) + return frames, samples + + +def main(argv: list[str]) -> int: + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument( + "mcap", + nargs="?", + help="Recording to replay (default: newest under ../recordings/)", + ) + parser.add_argument( + "--collections", + default=None, + help="Comma-separated collection ids to replay (default: auto-discover from the MCAP)", + ) + parser.add_argument( + "--loop", action="store_true", help="Replay in a loop until Ctrl+C" + ) + parser.add_argument( + "--rate", + type=float, + default=None, + help="Playback tick rate in Hz (default: the recording's own capture rate); 0 = as fast as possible", + ) + parser.add_argument( + "--no-viz", action="store_true", help="Text output only, no viser" + ) + parser.add_argument("--host", default="127.0.0.1", help="Viser HTTP bind address") + parser.add_argument("--port", type=int, default=8080, help="Viser HTTP port") + args = parser.parse_args(argv[1:]) + + mcap_path = resolve_mcap(args.mcap) + if not mcap_path.is_file(): + print(f"[replay-se3] no such file: {mcap_path}", file=sys.stderr) + return 1 + + if args.collections: + collections = [c.strip() for c in args.collections.split(",") if c.strip()] + else: + collections = discover_collections(mcap_path) + if not collections: + print( + f"[replay-se3] no SE3 tracker channels found in {mcap_path}", + file=sys.stderr, + ) + return 1 + print(f"[replay-se3] discovered collections: {', '.join(collections)}") + + rate = args.rate if args.rate is not None else capture_rate_hz(mcap_path) + if args.rate is None: + print(f"[replay-se3] playback rate {rate:.1f} Hz (from recording)") + + viz = None + if not args.no_viz: + import viser + + server = viser.ViserServer(host=args.host, port=args.port) + viz = Se3Viz(server, collections) + print(f"[replay-se3] viser running at http://localhost:{args.port}") + + print(f"[replay-se3] replaying {mcap_path}") + + while True: + # Fresh trackers + session per pass (replay consumes the file front-to-back). + trackers = {cid: Se3Tracker(cid) for cid in collections} + config = McapReplayConfig( + str(mcap_path), tracker_names=[(t, cid) for cid, t in trackers.items()] + ) + with ReplaySession.run(config) as session: + frames, samples = run_once(session, trackers, viz, rate) + print(f"[replay-se3] done — {frames} frames") + for cid in collections: + print(f"[replay-se3] {cid}: {samples[cid]} frames with data") + if not args.loop: + break + print("[replay-se3] looping…") + + if viz: + print( + "[replay-se3] viser still up at " + f"http://localhost:{args.port} — Ctrl+C to exit" + ) + try: + while True: + time.sleep(1.0) + except KeyboardInterrupt: + pass + return 0 + + +if __name__ == "__main__": + sys.exit(main(sys.argv)) diff --git a/src/plugins/vive_se3_tracker/CMakeLists.txt b/src/plugins/vive_se3_tracker/CMakeLists.txt new file mode 100644 index 0000000000..5046eaeb37 --- /dev/null +++ b/src/plugins/vive_se3_tracker/CMakeLists.txt @@ -0,0 +1,41 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 HTC Corporation. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +cmake_minimum_required(VERSION 3.20) + +# VIVEHub VUT tracker SDK (static archive + headers), shipped with VIVEHub-Linux. +# Requires SDK >= 1.0.1 (Pose::serial on the wire). Point VUT_SDK_DIR at your +# VIVEHub-Linux/sdk directory (contains include/ and lib/libvut_sdk.a): +# cmake ... -DVUT_SDK_DIR=/path/to/VIVEHub-Linux/sdk +# If it is not set / not found, the plugin is skipped (matches how the other +# vendor-SDK plugins behave), so a plain source or wheel build still configures. +set(VUT_SDK_DIR "" CACHE PATH "Path to the VIVEHub VUT SDK (contains include/ and lib/libvut_sdk.a)") +# Derive the lib path directly from VUT_SDK_DIR. +set(_vut_lib "${VUT_SDK_DIR}/lib/libvut_sdk.a") +if(NOT VUT_SDK_DIR OR NOT EXISTS "${_vut_lib}") + message(STATUS "VIVEHub VUT SDK not found (set -DVUT_SDK_DIR=/path/to/VIVEHub-Linux/sdk) " + "— skipping vive_se3_tracker plugin build.") + return() +endif() + +find_package(Threads REQUIRED) + +add_executable(vive_se3_tracker_plugin + main.cpp + vive_se3_tracker_plugin.cpp + vive_se3_tracker_plugin.hpp +) + +target_include_directories(vive_se3_tracker_plugin PRIVATE "${VUT_SDK_DIR}/include") + +target_link_libraries(vive_se3_tracker_plugin PRIVATE + deviceio::deviceio_trackers + pusherio::pusherio + oxr::oxr_core + isaacteleop_schema + "${_vut_lib}" + Threads::Threads +) + +install(TARGETS vive_se3_tracker_plugin RUNTIME DESTINATION plugins/vive_se3_tracker) +install(FILES plugin.yaml README.md DESTINATION plugins/vive_se3_tracker) diff --git a/src/plugins/vive_se3_tracker/README.md b/src/plugins/vive_se3_tracker/README.md new file mode 100644 index 0000000000..4ff1b95f57 --- /dev/null +++ b/src/plugins/vive_se3_tracker/README.md @@ -0,0 +1,119 @@ + + +# Vive SE3 Tracker Plugin + +Pushes each VIVE Ultimate Tracker as its own generic SE3 (6-DoF) pose stream +(`se3_tracker.fbs`), one tensor collection per tracker — raw per-tracker poses +for the consumer to use directly. + +## Data flow + +``` +Ultimate Trackers -> dongle -> VIVEHub tracker_server -> /tmp/vut.sock (VUT SDK) + -> this plugin -> Se3TrackerPose per device -> tensor collections + "vive_tracker_" (e.g. vive_tracker_SN0001) + -> core::Se3Tracker(collection_id) readers / MCAP recording / replay +``` + +Collections are created lazily on the first pose from each device and named by +the tracker's physical **serial number** — the only stable identity. See +"Identifying trackers" below. + +## Reference frame + +Poses are forwarded verbatim in the frame the VIVEHub daemon reports: the +SteamVR/OpenVR right-handed convention (X right, Y up, -Z forward), origin +defined by the tracker_server's own calibration. Per `se3_tracker.fbs` this is a +producer-defined reference frame (this producer is not XR-sourced); axis +conventions match OpenXR, but the origin is **not** the OpenXR session base +space — align downstream if cross-device consistency is needed. + +## Timestamps + +VUT pose timestamps are host `CLOCK_MONOTONIC` nanoseconds (`vut_types.h`) — the +same clock `SchemaPusher` documents as the local common clock — so valid samples +carry the VUT sample time verbatim (no push-loop resampling bias). The VUT SDK +does not expose the dongle's raw clock; the local common clock value is passed +as the documented best-effort substitute. + +Push policy per device, per ~90 Hz tick: + +| condition | action | +|---|---| +| new sample since last push | push `is_valid=true` at the VUT sample time | +| unchanged & fresh | push nothing (readers retain last-known; no duplicate timestamps in MCAP) | +| stale (> `VIVE_SE3_STALE_MS`) | push `is_valid=false` + identity filler every tick, stamped now | + +## Configuration (environment) + +| variable | default | meaning | +|---|---|---| +| `VIVE_VUT_SOCKET` | `/tmp/vut.sock` | VIVEHub VUT daemon socket path | +| `VIVE_SE3_STALE_MS` | `250` (ms; 1..3600000, else default) | staleness threshold before a tracker is pushed invalid | +| `VIVE_SE3_COLLECTIONS_FILE` | `$XDG_RUNTIME_DIR/vive_se3_collections.txt` (else `/tmp/…`) | where live collection ids are advertised | +| `VIVE_SE3_SYNTHETIC` | (unset) | `1` = fake trackers, no VIVEHub (smoke test) | + +No role or naming knobs: collections are always named by serial. + +## Identifying trackers (serial from the wire) + +Requires the **VUT SDK (VIVEHub >= 1.0.1)**, whose pose event carries the +tracker's serial (`Pose::serial`). The plugin names each collection by that +serial: + +- pose with a serial → `vive_tracker_` (e.g. `vive_tracker_SN0001`); +- pose with an empty serial (`""`) → `vive_tracker_` (logged fallback). + +The name is fixed on the **first** pose from a device (a live tensor collection +cannot be renamed). The serial is the stable physical identity, so an MCAP +recorded today still identifies exactly which tracker each channel came from. +Mapping a tracker to a role (which one is which) belongs downstream, wherever the +poses are consumed. + +> This plugin builds against the VUT SDK; VIVEHub **1.0.1** is the first release +> shipping it (1.0.0 predates the rename and will not build or connect). Run +> VIVEHub 1.0.1 or newer. + +## Running + +```bash +# v1.0.1 (or newer) VIVEHub tracker_server running + trackers paired; CloudXR runtime up. +./vive_se3_tracker_plugin +``` + +Verify with the stock SE3 reader (per collection): + +```bash +./se3_printer vive_tracker_SN0001 # a serial the plugin prints on startup + # (or read one from the collections file) +``` + +## Collection auto-discovery + +While running, the plugin writes its live collection ids (one per line) to a +per-user file under `$XDG_RUNTIME_DIR` (falling back to `/tmp`), overridable via +`VIVE_SE3_COLLECTIONS_FILE`. It is rewritten as collections appear and removed on +exit (including on Ctrl+C / SIGTERM). This lets readers attach without knowing +device_ids in advance — `record_se3_vive.py` resolves the same path so it can run +with no arguments. Stale files from a crashed run are cleared on the next startup. + +## Recording / replay (Python examples) + +Both run with no arguments once the plugin is up (from the example directory, so +`uv` picks up its `pyproject.toml`): + +```bash +cd examples/mcap_record_replay/python +# records 10 s of every advertised collection to ../recordings/.mcap +uv run record_se3_vive.py +# replays the newest recording, auto-discovering collections + capture rate, +# and serves a viser 3D view at http://localhost:8080 +uv run replay_se3_vive.py +``` + +Under the hood this is just the standard MCAP tooling — a `core::Se3Tracker(cid)` +per collection registered with a `DeviceIOSession`; the live/replay factories +handle serialization (`Se3TrackerRecordingTraits`, `ReplaySe3TrackerImpl`). diff --git a/src/plugins/vive_se3_tracker/main.cpp b/src/plugins/vive_se3_tracker/main.cpp new file mode 100644 index 0000000000..957faf03ea --- /dev/null +++ b/src/plugins/vive_se3_tracker/main.cpp @@ -0,0 +1,61 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 HTC Corporation. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "vive_se3_tracker_plugin.hpp" + +#include +#include +#include +#include +#include +#include + +using namespace plugins::vive_se3_tracker; + +namespace +{ +std::atomic g_stop{ false }; +void on_signal(int) +{ + g_stop.store(true); +} +} // namespace + +int main(int, char** argv) +try +{ + std::cout << "Vive SE3 Tracker Plugin (one collection per Ultimate Tracker)" << std::endl; + + // Stop cleanly on Ctrl+C / kill so ~ViveSe3TrackerPlugin runs and removes the + // collections advertisement file (else readers see stale collection ids). + std::signal(SIGINT, on_signal); + std::signal(SIGTERM, on_signal); + + ViveSe3TrackerPlugin plugin; + + // Poll/push at 90 Hz; valid samples carry the VUT sample time, so the loop + // rate only bounds delivery latency, not timestamp accuracy. + const auto frame_duration = std::chrono::nanoseconds(1000000000 / 90); + const auto program_start = std::chrono::steady_clock::now(); + std::size_t frame_count = 0; + + while (!g_stop.load()) + { + plugin.update(); + frame_count++; + std::this_thread::sleep_until(program_start + frame_duration * frame_count); + } + + std::cout << "Vive SE3 Tracker Plugin: shutting down." << std::endl; + return 0; +} +catch (const std::exception& e) +{ + std::cerr << argv[0] << ": " << e.what() << std::endl; + return 1; +} +catch (...) +{ + std::cerr << argv[0] << ": Unknown error" << std::endl; + return 1; +} diff --git a/src/plugins/vive_se3_tracker/plugin.yaml b/src/plugins/vive_se3_tracker/plugin.yaml new file mode 100644 index 0000000000..2331be93e3 --- /dev/null +++ b/src/plugins/vive_se3_tracker/plugin.yaml @@ -0,0 +1,11 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 HTC Corporation. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +name: vive_se3_tracker +description: "Vive Ultimate Trackers -> per-tracker SE3 pose streams, pushed via OpenXR" +command: "./vive_se3_tracker_plugin" +version: "0.1.0" +devices: + - path: "/se3_tracker/vive" + type: "se3_tracker" + description: "SE3 (6-DoF) pose of each VIVE Ultimate Tracker (via VIVEHub); one collection per tracker, named by serial" diff --git a/src/plugins/vive_se3_tracker/vive_se3_tracker_plugin.cpp b/src/plugins/vive_se3_tracker/vive_se3_tracker_plugin.cpp new file mode 100644 index 0000000000..fad33c5535 --- /dev/null +++ b/src/plugins/vive_se3_tracker/vive_se3_tracker_plugin.cpp @@ -0,0 +1,407 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 HTC Corporation. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "vive_se3_tracker_plugin.hpp" + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace plugins +{ +namespace vive_se3_tracker +{ + +namespace +{ + +int64_t env_int(const char* name, int64_t fallback) +{ + const char* v = std::getenv(name); + if (!v || !*v) + return fallback; + return std::strtoll(v, nullptr, 10); +} + +// A directory is safe to place the advertisement file in only if it is a real +// directory (not a symlink) that we own with no group/other write access — so no +// other local user can pre-plant a symlink and redirect our truncating open. +bool is_secure_dir(const std::string& dir) +{ + struct stat st + { + }; + if (::lstat(dir.c_str(), &st) != 0) + return false; + if (!S_ISDIR(st.st_mode)) + return false; // e.g. a symlink or a regular file planted at that path + if (st.st_uid != ::geteuid()) + return false; // not owned by us + if (st.st_mode & (S_IWGRP | S_IWOTH)) + return false; // group/other-writable + return true; +} + +// Resolve where to write the collection-id advertisement file, securely: +// 1. VIVE_SE3_COLLECTIONS_FILE, if set, wins verbatim (caller's responsibility). +// 2. else $XDG_RUNTIME_DIR (systemd gives a per-user, mode-0700 dir). +// 3. else a private /tmp/vive_se3_tracker- we create 0700 and verify we own. +// Falls through to the plain /tmp file only if the private dir can't be secured, +// and warns — never silently write to a world-writable fixed path. +// Keep in sync with record_se3_vive.py. +std::string resolve_collections_file() +{ + const char* env_cf = std::getenv("VIVE_SE3_COLLECTIONS_FILE"); + if (env_cf && *env_cf) + return env_cf; + + const char* xdg = std::getenv("XDG_RUNTIME_DIR"); + if (xdg && *xdg && is_secure_dir(xdg)) + return std::string(xdg) + "/" + kCollectionsFileName; + + const std::string dir = "/tmp/vive_se3_tracker-" + std::to_string(::geteuid()); + ::mkdir(dir.c_str(), 0700); // ignore EEXIST; is_secure_dir does the real check + if (is_secure_dir(dir)) + return dir + "/" + kCollectionsFileName; + + std::cerr << "[vive_se3_tracker] could not secure a per-user directory for the collections file; " + "set VIVE_SE3_COLLECTIONS_FILE to a private path." + << std::endl; + return dir + "/" + kCollectionsFileName; // best effort; open still O_TRUNC-guarded below +} + +core::SchemaPusherConfig make_pusher_config(const std::string& collection_id) +{ + // Wire rendezvous (tensor identifier + buffer size) comes from the Se3Tracker facade — + // the single source of truth shared with LiveSe3TrackerImpl; a mismatch is silent no-data. + return core::SchemaPusherConfig{ .collection_id = collection_id, + .max_flatbuffer_size = core::Se3Tracker::DEFAULT_MAX_FLATBUFFER_SIZE, + .tensor_identifier = std::string(core::Se3Tracker::TENSOR_IDENTIFIER), + .localized_name = "Vive Ultimate Tracker SE3", + .app_name = "ViveSe3TrackerPlugin" }; +} + +} // namespace + +ViveSe3TrackerPlugin::ViveSe3TrackerPlugin() + : session_( + std::make_shared("ViveSe3TrackerPlugin", core::SchemaPusher::get_required_extensions())), + start_time_ns_(core::os_monotonic_now_ns()) +{ + // Clamp VIVE_SE3_STALE_MS before scaling to ns: a non-numeric env yields 0 + // (env_int), which would mark every sample stale, and a huge value would + // overflow the * 1'000'000. Reject out-of-range values, use the default. + const int64_t stale_ms = env_int("VIVE_SE3_STALE_MS", kDefaultStaleMs); + if (stale_ms <= 0 || stale_ms > kMaxStaleMs) + { + if (std::getenv("VIVE_SE3_STALE_MS")) + std::cerr << "[vive_se3_tracker] ignoring invalid VIVE_SE3_STALE_MS=" << stale_ms << "; using " + << kDefaultStaleMs << " ms" << std::endl; + stale_ns_ = kDefaultStaleMs * 1000000; + } + else + { + stale_ns_ = stale_ms * 1000000; + } + + // Advertisement file, placed in a per-user directory (never a fixed + // world-writable /tmp path — see resolve_collections_file). + collections_file_ = resolve_collections_file(); + // Remove any stale advertisement from a previous run; recreated lazily as + // collections come up. Prevents readers from subscribing to dead streams. + std::remove(collections_file_.c_str()); + + const char* synth = std::getenv("VIVE_SE3_SYNTHETIC"); + synthetic_mode_ = (synth && (synth[0] == '1' || synth[0] == 't' || synth[0] == 'T')); + if (synthetic_mode_) + { + std::cout << "[vive_se3_tracker] SYNTHETIC mode (VIVE_SE3_SYNTHETIC=1); not connecting to VIVEHub." << std::endl; + return; + } + + const char* env_sock = std::getenv("VIVE_VUT_SOCKET"); + const std::string socket_path = (env_sock && *env_sock) ? env_sock : kDefaultVutSocket; + + std::cout << "[vive_se3_tracker] connecting to VIVEHub VUT daemon at " << socket_path << " (stale threshold " + << (stale_ns_ / 1000000) << " ms)" << std::endl; + + vut_client_ = std::make_unique(socket_path); + vut_client_->set_auto_reconnect(true, 200, 2000); + vut_client_->on_connection_change( + [](bool up) + { std::cout << "[vive_se3_tracker] VIVEHub daemon " << (up ? "CONNECTED" : "disconnected") << std::endl; }); + vut_client_->on_pose([this](const vut::Pose& p) { on_vut_pose(p); }); + + // Non-fatal: with auto-reconnect the supervisor keeps retrying if the daemon + // isn't up yet. Poses simply won't flow until it connects and a tracker runs. + if (vut_client_->connect("ViveSe3TrackerPlugin", vut::SUB_POSE) != 0) + std::cout << "[vive_se3_tracker] daemon not up yet; retrying in background." << std::endl; + + // Best-effort device inventory (may be empty until trackers pair). The serial + // — and thus the collection name — only arrives on the pose wire, so it is + // resolved on the first pose, not here (list_devices carries no serial). + std::vector devs; + if (vut_client_->list_devices(devs, 500) == 0 && !devs.empty()) + { + std::cout << "[vive_se3_tracker] devices reported by daemon:" << std::endl; + for (const auto& d : devs) + std::cout << " id=" << d.device_id << " state=" << d.state << " name=\"" << d.name << "\"" << std::endl; + } +} + +ViveSe3TrackerPlugin::~ViveSe3TrackerPlugin() +{ + // disconnect() stops the VUT SDK's I/O thread and JOINS it before returning + // (SDK contract; verified: it does shutdown(fd) + std::thread::join()). The + // pose callback runs only on that thread, so once this returns on_vut_pose() + // can no longer fire and the members it touches are destroyed safely + // afterwards. We never call disconnect() from inside the callback, so there + // is no self-join. + if (vut_client_) + vut_client_->disconnect(); + if (!collections_file_.empty()) + std::remove(collections_file_.c_str()); +} + +// Sanitize a serial taken straight off the VUT wire before it becomes a +// collection id and a line in the advertisement file that record_se3_vive.py +// trusts. Bounds the read to the fixed buffer (no reliance on NUL-termination) +// and keeps only filename-safe characters — alphanumerics and '-' '_' '.', +// dropping everything else (newlines and separators included) so a malformed +// serial cannot inject or split lines. +static std::string sanitize_serial(const char* s, size_t maxlen) +{ + const size_t n = ::strnlen(s, maxlen); + std::string out; + out.reserve(n); + for (size_t i = 0; i < n; ++i) + { + const char c = s[i]; + const bool ok = (c >= 'A' && c <= 'Z') || (c >= 'a' && c <= 'z') || (c >= '0' && c <= '9') || c == '-' || + c == '_' || c == '.'; + if (ok) + out.push_back(c); + } + return out; +} + +// Collection id for a tracker: its physical serial when present (the stable +// identity), else a device_id fallback for a pose that arrived with no serial. +static std::string make_collection_id(uint32_t device_id, const std::string& serial) +{ + if (!serial.empty()) + return kCollectionPrefix + serial; + return kCollectionPrefix + std::to_string(device_id); +} + +void ViveSe3TrackerPlugin::on_vut_pose(const vut::Pose& p) +{ + // Runs on the VUT receiver thread — copy only, no blocking. Pushing happens + // on the update() thread; SchemaPusher is never touched from here. + bool first = false; + std::string serial; + { + std::lock_guard lock(pose_mutex_); + LatestPose& lp = latest_poses_[p.device_id]; + first = !lp.have; + lp.pos[0] = p.position[0]; + lp.pos[1] = p.position[1]; + lp.pos[2] = p.position[2]; + lp.quat[0] = p.orientation[0]; + lp.quat[1] = p.orientation[1]; + lp.quat[2] = p.orientation[2]; + lp.quat[3] = p.orientation[3]; + // Serial arrives on the pose wire (VUT SDK >= 1.0.1), "" if unknown. + // Sanitized + length-bounded here (see sanitize_serial) before it names a + // collection. Fixed on the first pose — collections can't be renamed later. + lp.serial = sanitize_serial(p.serial, sizeof(p.serial)); + lp.ts_ns = static_cast(p.timestamp_ns); + lp.have = true; + serial = lp.serial; + } + + // Log outside the lock so the push thread never blocks on this I/O. + if (first) + { + std::cout << "[vive_se3_tracker] first pose from device_id=" << p.device_id; + if (serial.empty()) + std::cout << " (no serial on wire; naming by device_id)"; + std::cout << " -> collection '" << make_collection_id(p.device_id, serial) << "'" << std::endl; + } +} + +ViveSe3TrackerPlugin::DeviceStream& ViveSe3TrackerPlugin::stream_for(uint32_t device_id, const std::string& serial) +{ + auto it = streams_.find(device_id); + if (it != streams_.end()) + { + // Same device_id but a different serial means the daemon reassigned this + // id to another physical tracker (re-pair / dongle reset). Rebuild so the + // new tracker gets its own collection instead of inheriting the old name. + if (!serial.empty() && !it->second.serial.empty() && it->second.serial != serial) + { + std::cerr << "[vive_se3_tracker] device_id=" << device_id << " serial changed from '" << it->second.serial + << "' to '" << serial << "'; creating a new collection" << std::endl; + streams_.erase(it); + } + else + { + return it->second; + } + } + + DeviceStream stream; + stream.serial = serial; + stream.collection_id = make_collection_id(device_id, serial); + stream.pusher = + std::make_unique(session_->get_handles(), make_pusher_config(stream.collection_id)); + std::cout << "[vive_se3_tracker] created tensor collection '" << stream.collection_id + << "' for device_id=" << device_id << std::endl; + DeviceStream& ref = streams_.emplace(device_id, std::move(stream)).first->second; + write_collections_file(); // advertise the updated live set to readers + return ref; +} + +void ViveSe3TrackerPlugin::write_collections_file() const +{ + if (collections_file_.empty()) + return; + // Write to a temp then rename so a concurrent reader never sees a partial file. + const std::string tmp = collections_file_ + ".tmp"; + { + std::ofstream out(tmp, std::ios::trunc); + if (!out) + { + std::cerr << "[vive_se3_tracker] could not write collections file " << tmp << std::endl; + return; + } + for (const auto& kv : streams_) + out << kv.second.collection_id << "\n"; + } + if (std::rename(tmp.c_str(), collections_file_.c_str()) != 0) + { + std::cerr << "[vive_se3_tracker] could not update collections file " << collections_file_ << std::endl; + std::remove(tmp.c_str()); // don't leave the temp behind + } +} + +void ViveSe3TrackerPlugin::generate_synthetic_poses(int64_t now_ns) +{ + // Slow circle per device, phase-offset by device_id; orientation spins about Y. + const double t = static_cast(now_ns - start_time_ns_) / 1e9; + + // Fake a trio (device_ids 1,2,3) with synthetic serials so the collections + // are named as they would be live — enough to smoke-test the + // push->record->replay path with no hardware. + std::lock_guard lock(pose_mutex_); + for (uint32_t dev = 1; dev <= 3; ++dev) + { + const double phase = t * 0.5 + dev * 2.0; + LatestPose& lp = latest_poses_[dev]; + lp.pos[0] = static_cast(0.3 * std::cos(phase)); + lp.pos[1] = 0.2f * dev; + lp.pos[2] = static_cast(0.3 * std::sin(phase)); + const double half = phase * 0.5; + lp.quat[0] = 0.f; + lp.quat[1] = static_cast(std::sin(half)); + lp.quat[2] = 0.f; + lp.quat[3] = static_cast(std::cos(half)); + lp.serial = "SYNTH0000000" + std::to_string(dev); + lp.ts_ns = now_ns; + lp.have = true; + } +} + +void ViveSe3TrackerPlugin::update() +{ + if (synthetic_mode_) + generate_synthetic_poses(core::os_monotonic_now_ns()); + + // Snapshot under the lock, push outside it (pushes go through OpenXR). + std::unordered_map snapshot; + { + std::lock_guard lock(pose_mutex_); + snapshot = latest_poses_; + } + + const int64_t now_ns = core::os_monotonic_now_ns(); + + for (const auto& kv : snapshot) + { + const uint32_t device_id = kv.first; + const LatestPose& lp = kv.second; + if (!lp.have) + continue; + + DeviceStream& stream = stream_for(device_id, lp.serial); + const bool stale = (now_ns - lp.ts_ns > stale_ns_); + + core::Se3TrackerPoseT out; + int64_t sample_time_ns; + + if (!stale) + { + if (lp.ts_ns == stream.last_pushed_ts_ns) + continue; // no new sample; live readers retain last-known + + // VUT forwards poses in the SteamVR/OpenVR right-handed frame (X right, + // Y up, -Z forward) with the daemon's own origin — a producer-defined + // reference frame per se3_tracker.fbs (documented in README.md); passed + // verbatim. Origin alignment vs the consumer's frame is downstream tuning. + out.pose = std::make_shared(core::Point(lp.pos[0], lp.pos[1], lp.pos[2]), + core::Quaternion(lp.quat[0], lp.quat[1], lp.quat[2], lp.quat[3])); + out.is_valid = true; + // VUT timestamps are host CLOCK_MONOTONIC — the local common clock — + // so the sample time passes through without conversion. No raw dongle + // clock is exposed; same value as the documented substitute. + sample_time_ns = lp.ts_ns; + stream.last_pushed_ts_ns = lp.ts_ns; + if (stream.pushed_invalid) + { + stream.pushed_invalid = false; + std::cout << "[vive_se3_tracker] device_id=" << device_id << " tracking recovered" << std::endl; + } + } + else + { + // Identity pose is a filler consistent with "pose contents unspecified + // when is_valid == false" (se3_tracker.fbs) — consumers gate on is_valid, + // never on pose values. Pushed every tick while stale, stamped with the + // current time. + out.pose = + std::make_shared(core::Point(0.0f, 0.0f, 0.0f), core::Quaternion(0.0f, 0.0f, 0.0f, 1.0f)); + out.is_valid = false; + sample_time_ns = now_ns; + if (!stream.pushed_invalid) + { + stream.pushed_invalid = true; + std::cout << "[vive_se3_tracker] device_id=" << device_id << " stale (last sample " + << ((now_ns - lp.ts_ns) / 1000000) << " ms ago) -> pushing is_valid=false" << std::endl; + } + } + + flatbuffers::FlatBufferBuilder builder(core::Se3Tracker::DEFAULT_MAX_FLATBUFFER_SIZE); + auto offset = core::Se3TrackerPose::Pack(builder, &out); + builder.Finish(offset); + + stream.pusher->push_buffer(builder.GetBufferPointer(), builder.GetSize(), sample_time_ns, sample_time_ns); + } +} + +} // namespace vive_se3_tracker +} // namespace plugins diff --git a/src/plugins/vive_se3_tracker/vive_se3_tracker_plugin.hpp b/src/plugins/vive_se3_tracker/vive_se3_tracker_plugin.hpp new file mode 100644 index 0000000000..687b39c187 --- /dev/null +++ b/src/plugins/vive_se3_tracker/vive_se3_tracker_plugin.hpp @@ -0,0 +1,155 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 HTC Corporation. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace core +{ +class OpenXRSession; +} + +namespace plugins +{ +namespace vive_se3_tracker +{ + +// Collection ids are "", one collection per tracker. The reader +// (core::Se3Tracker) must be constructed with the same collection_id; the tensor +// identifier inside the collection is fixed to core::Se3Tracker::TENSOR_IDENTIFIER. +constexpr const char* kCollectionPrefix = "vive_tracker_"; + +// Default VIVEHub VUT daemon socket (README: /tmp/vut.sock, per-user). +constexpr const char* kDefaultVutSocket = "/tmp/vut.sock"; + +// Basename of the file the plugin advertises its live collection ids in (one per +// line) so readers (e.g. record_se3_vive.py) can auto-discover them. It lives in +// $XDG_RUNTIME_DIR (per-user, mode-0700) when set; otherwise in a private +// per-user dir created under /tmp (see resolve_collections_file) — never a fixed +// world-writable /tmp path, which would be open to symlink pre-planting. +// Overridable wholesale via VIVE_SE3_COLLECTIONS_FILE. Rewritten on each new +// collection, removed on exit. Keep the resolution in sync with record_se3_vive.py. +constexpr const char* kCollectionsFileName = "vive_se3_collections.txt"; + +// A tracker pose older than this (vs the pose's own monotonic timestamp) is +// treated as stale -> pushed with is_valid=false. Overridable via +// VIVE_SE3_STALE_MS. +constexpr int64_t kDefaultStaleMs = 250; + +// Upper bound for VIVE_SE3_STALE_MS (1 hour). Values <= 0 or above this are +// rejected in favor of the default: 0 (e.g. a non-numeric env) would mark every +// sample stale, and a huge value would overflow the * 1'000'000 to nanoseconds. +constexpr int64_t kMaxStaleMs = 3600000; + +// Synthetic mode (VIVE_SE3_SYNTHETIC=1): generate slow circular motion for a +// few fake trackers instead of connecting to VIVEHub — no-hardware smoke test +// of the push -> read path (pair with se3_printer). + +/*! + * @brief Reads live Vive Ultimate Tracker poses from the VIVEHub `tracker_server` + * daemon (via the VUT SDK client) and pushes each tracker as its own SE3 + * tracker stream (se3_tracker.fbs) — one tensor collection per device. + * + * Data flow: tracker_server (dongle -> trackers) -> /tmp/vut.sock -> vut::Client + * -> on_vut_pose() (receiver thread) -> latest-pose store -> update() (push + * thread) -> Se3TrackerPose -> per-device tensor collection -> core::Se3Tracker. + * + * Naming: each collection is named by the tracker's physical serial number — + * "" (e.g. "vive_tracker_SN0001"), the only stable + * identity. The serial arrives on the pose wire (VUT SDK >= 1.0.1, Pose::serial); + * a pose with an empty serial falls back to "". Mapping a + * tracker to a role is a downstream concern, keyed by serial wherever the poses + * are consumed. + * + * Timestamps: VUT pose timestamps are host CLOCK_MONOTONIC (vut_types.h), the + * same clock SchemaPusher documents as the local common clock, so the VUT + * sample time is passed through verbatim — no push-time resampling bias. The + * dongle's raw clock is not exposed by the VUT SDK, so the same value is used + * as the documented best-effort substitute. + * + * Per-device push policy (per ~90 Hz tick): + * - new sample since last push -> push is_valid=true at the VUT sample time + * - unchanged & fresh -> push nothing (live readers retain last-known; + * avoids duplicate-timestamp samples in MCAP) + * - stale -> push is_valid=false + identity filler at the + * current time (explicit invalidity beats + * silence) + */ +class ViveSe3TrackerPlugin +{ +public: + ViveSe3TrackerPlugin(); + ~ViveSe3TrackerPlugin(); + + ViveSe3TrackerPlugin(const ViveSe3TrackerPlugin&) = delete; + ViveSe3TrackerPlugin& operator=(const ViveSe3TrackerPlugin&) = delete; + ViveSe3TrackerPlugin(ViveSe3TrackerPlugin&&) = delete; + ViveSe3TrackerPlugin& operator=(ViveSe3TrackerPlugin&&) = delete; + + // Push pending tracker samples. Call at the desired rate (~90 Hz). + void update(); + +private: + // VUT receiver-thread callback: store the latest pose for a device_id. + void on_vut_pose(const vut::Pose& p); + + // Synthetic pose generator: writes fake latest_poses_ entries (timestamps = + // now, so they are never stale). + void generate_synthetic_poses(int64_t now_ns); + + // Rewrite collections_file_ with the current set of live collection ids so + // readers can auto-discover them without knowing serials in advance. + void write_collections_file() const; + + // Per-device push state (pusher created lazily on first pose). + struct DeviceStream + { + std::unique_ptr pusher; + std::string collection_id; // "vive_tracker_" (or device_id fallback) + std::string serial; // serial the collection was named from + int64_t last_pushed_ts_ns = -1; + bool pushed_invalid = false; // logged transition into stale + }; + // Create (or fetch) the stream for a device. serial comes from the pose wire + // (new VUT SDK); an empty serial falls back to device_id naming. The name is + // fixed at creation from the first pose's serial (collections can't rename); + // if the daemon later reassigns this device_id to a different serial, the + // stream is rebuilt so samples never land in the previous tracker's collection. + DeviceStream& stream_for(uint32_t device_id, const std::string& serial); + + std::shared_ptr session_; + + // --- VIVEHub VUT client --- + std::unique_ptr vut_client_; + bool synthetic_mode_ = false; + int64_t start_time_ns_ = 0; + int64_t stale_ns_ = kDefaultStaleMs * 1000000; + std::string collections_file_; // live collection-id advertisement path + + // Latest pose per device_id, written by the VUT receiver thread, snapshotted + // by the push thread. Guarded by pose_mutex_. + struct LatestPose + { + float pos[3] = { 0.f, 0.f, 0.f }; + float quat[4] = { 0.f, 0.f, 0.f, 1.f }; + std::string serial; // tracker serial from the pose wire ("" if unknown) + int64_t ts_ns = 0; + bool have = false; + }; + std::mutex pose_mutex_; + std::unordered_map latest_poses_; + + std::unordered_map streams_; +}; + +} // namespace vive_se3_tracker +} // namespace plugins