diff --git a/CMakeLists.txt b/CMakeLists.txt index 33a061f55..1786629f2 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -59,6 +59,7 @@ endif() # Options option(BUILD_PLUGINS "Build plugins" ON) option(BUILD_PLUGIN_OAK_CAMERA "Build OAK camera plugin (requires vcpkg for DepthAI v3.x)" OFF) +option(BUILD_PLUGIN_OGLO "Build OGLO tactile glove plugin (BLE, Linux only; fetches SimpleBLE + nlohmann/json)" OFF) option(BUILD_EXAMPLES "Build examples" ON) option(BUILD_EXAMPLE_TELEOP_ROS2 "Build only the teleop_ros2 example (e.g. for Docker)" OFF) option(BUILD_TESTING "Build unit tests" ON) @@ -169,6 +170,9 @@ if(BUILD_PLUGINS) if(BUILD_PLUGIN_OAK_CAMERA) add_subdirectory(src/plugins/oak) endif() + if(BUILD_PLUGIN_OGLO) + add_subdirectory(src/plugins/oglo_tactile) + endif() endif() # Formatting enforcement (runs on Linux by default) diff --git a/docs/source/device/index.rst b/docs/source/device/index.rst index 8f4579af5..1beadfd41 100644 --- a/docs/source/device/index.rst +++ b/docs/source/device/index.rst @@ -23,3 +23,4 @@ See the `Plugins directory `_ (AFL-2.1, permissive), so + the build carries **no copyleft dependency** and works out of the box. The + transport is isolated behind the ``OgloBleClient`` interface + (:code-file:`src/plugins/oglo_tactile/ble/oglo_ble_client.hpp`), so an + alternative backend can be dropped in via ``make_ble_client()`` without + touching the parser, schema, or tracker. + +Usage +----- + +The plugin streams one glove (``--side``) and either records a local MCAP +(**Mode 1**) or pushes via OpenXR for a host tracker (**Mode 2**): + +.. code-block:: bash + + # Mode 1 — standalone local MCAP (no OpenXR / TeleopSession) + ./build/src/plugins/oglo_tactile/oglo_tactile_plugin --side right --mcap-filename=right.mcap + + # Mode 2 — push for a host OgloTactileTracker into a shared session MCAP + ./build/src/plugins/oglo_tactile/oglo_tactile_plugin --side right --collection-prefix=oglo + +The packet parser reads the device **Config characteristic** first and branches +on the notify ``flags`` byte (``0x04`` packed12 v5 — primary; ``0x02``/``0x01`` +legacy schema-4 fallback), so payload sizes are never hardcoded. + +Recorded data +------------- + +Channels ``oglo_left`` / ``oglo_right`` carry ``core.OgloGloveSampleRecord``: +``seq``, ``device_time_us``, ``taxels[80]`` (raw 12-bit, ``finger,row,col``), +and a 6-axis IMU, each with a ``DeviceDataTimestamp`` whose +``sample_time_local_common_clock`` is on the shared host monotonic clock — so +OGLO aligns in time with hand/head streams. + +A complete data-collection demo (MetaQuest hand/head + both gloves + a live +in-headset tactile heatmap) lives at +:code-dir:`examples/oglo_tactile`; see its ``README.md``. + +Tests +----- + +``test_oglo_packet_parser`` validates the wire decode against the firmware's own +12-bit packing reference, plus the schema-4 fallback and malformed-packet +rejection: + +.. code-block:: bash + + ctest --test-dir build -R oglo_packet_parser --output-on-failure diff --git a/examples/oglo_tactile/README.md b/examples/oglo_tactile/README.md new file mode 100644 index 000000000..573e6b66f --- /dev/null +++ b/examples/oglo_tactile/README.md @@ -0,0 +1,131 @@ + + +# OGLO + MetaQuest Data-Collection Demo + +End-to-end demo on a Linux host: + +- **MetaQuest** connects over the same WiFi via **CloudXR** → streams hand + head pose. +- **OGLO gloves** (2) connect over **BLE** → 80 taxels + 6-axis IMU per hand at 100 Hz. +- One **time-synced MCAP** records Quest hand/head + both gloves. +- A live **tactile heatmap** is composited into the operator's Quest view (TeleViz). + +``` + MetaQuest ──CloudXR/WiFi──► laptop OpenXR runtime ──► HandTracker/HeadTracker ─┐ + OGLO L/R ──BLE──► oglo_tactile plugins ──SchemaPusher──► OgloTactileTracker ──┤ + ├─► MCAP (synced) + oglo_heatmap ──► QuadLayer ──► TeleViz compositor ──CloudXR──► Quest screen ─┘ +``` + +`oglo_teleop_record.py` orchestrates everything (launches the plugins, runs the +session, records, and draws the overlay). `oglo_heatmap.py` is the renderer. + +--- + +## A. One-time setup + +1. **System deps** + + ```bash + sudo apt install libdbus-1-dev # BlueZ for the BLE plugin + pip install pillow numpy # heatmap renderer + pip install cupy-cuda12x # headset overlay GPU upload (match your CUDA) + ``` + `cupy` is only needed for the in-headset overlay; recording works without it. + +2. **Build** (plugin + viz + python wheel) + + ```bash + cd IsaacTeleop + cmake -B build -DBUILD_PLUGIN_OGLO=ON -DBUILD_VIZ=ON + cmake --build build --parallel + cmake --install build # builds the isaacteleop python wheel/package + ctest --test-dir build -R oglo_packet_parser --output-on-failure # parser sanity + ``` + The BLE backend (BlueZ via libdbus, permissive) connects out of the box. + +3. **Gloves**: confirm both run firmware `0.7.1` (schema 5), are charged, and + advertise `OGLO LEFT` / `OGLO RIGHT`. Validate each glove standalone first: + + ```bash + ./build/src/plugins/oglo_tactile/oglo_tactile_plugin --side right --mcap-filename=right.mcap + # press taxels; Ctrl+C; expect schema_ver:5, ~100Hz, seq continuous + ``` + +4. **CloudXR**: install per the IsaacTeleop Quick Start (`pip install 'isaacteleop[cloudxr]'`). + +--- + +## B. Demo scenario (run in order) + +**Terminal 1 — CloudXR runtime** +```bash +cd ~/Documents/IsaacTeleop +source scripts/setup_cloudxr_env.sh +python -m isaacteleop.cloudxr +# Note the printed web-client URL, e.g. https://:48322/ +``` + +**MetaQuest — connect** (same WiFi) +- Open the headset browser → the CloudXR web client at + `https://nvidia.github.io/IsaacTeleop/client` → enter the laptop IP → accept the + self-signed certificate → **CONNECT**. (Or run the server with `--host-client` + and open `https://:48322/client/` instead.) +- You should now be in the CloudXR view; hand/head tracking is live. + +**Terminal 2 — gloves + recording + overlay** +```bash +cd ~/Documents/IsaacTeleop +source scripts/setup_cloudxr_env.sh +source ~/.cloudxr/run/cloudxr.env # path printed by step above +cd examples/oglo_tactile +python oglo_teleop_record.py +``` +The script: connects both gloves over BLE, starts recording to +`oglo_teleop_.mcap`, and shows the **two-hand tactile heatmap** in the +headset. **Keep hands relaxed for ~1 s** at start so the baseline tares. + +Press a glove → the matching finger/taxel lights up (YlOrRd) on the Quest screen. + +**Stop**: `Ctrl+C` in Terminal 2 (flushes + closes the MCAP), then `Ctrl+C` in Terminal 1. + +--- + +## C. Verify the recording + +```bash +mcap info oglo_teleop_*.mcap +# channels: hands/{left,right}_hand, head/head, oglo_{left,right}/oglo(+_tracked) +``` +All streams share `sample_time_local_common_clock`, so Quest pose and OGLO +tactile align in time. + +--- + +## Useful flags (`oglo_teleop_record.py`) + +| Flag | Purpose | +|------|---------| +| `--plugin-bin PATH` | Path to `oglo_tactile_plugin` (auto-detected under `build/`). | +| `--mcap PATH` | Output file (default timestamped). | +| `--duration S` | Auto-stop after S seconds (0 = until Ctrl+C). | +| `--raw` | Raw-ADC heatmap (no baseline subtraction). | +| `--no-overlay` | Record only (skip headset heatmap). | +| `--overlay-fullscreen` | Fullscreen overlay (facing-safe fallback). | +| `--panel-dist / --panel-right / --panel-drop / --panel-w` | Head-locked HUD placement/size (meters); default is a small bottom-right panel. | +| `--no-headlock` (`--panel-y / --panel-z`) | Use a fixed stage-space panel instead of head-locked. | +| `--plugin-stagger S` | Delay between launching the two gloves (avoids BLE scan contention; default 8 s). | +| `--no-plugins` | Don't auto-launch plugins (you start them yourself). | + +--- + +## Troubleshooting + +- **Glove not found**: `bluetoothctl scan on` should list `OGLO LEFT/RIGHT`. Power-cycle the glove; ensure it isn't already connected elsewhere. +- **Quest view but no heatmap**: `cupy` missing/mismatched CUDA → see warning in Terminal 2; install the `cupy` build matching your CUDA. Panel off-view → adjust `--panel-right/--panel-drop/--panel-dist`, or use `--overlay-fullscreen`. +- **One hand not connecting**: usually BLE scan contention — increase `--plugin-stagger` (e.g. 12). Confirm the glove advertises via `bluetoothctl scan on`. +- **No tactile in MCAP**: confirm the plugins connected (Terminal 2 logs `Connected: side=… schema_ver:5`) and that the tracker `collection_id` (`oglo/left`,`oglo/right`) matches the plugin `--collection-prefix=oglo`. +- **Quest won't connect**: same WiFi/subnet, firewall allows the CloudXR ports, accept the self-signed cert in the headset browser. +- **Stutter**: the overlay is auxiliary — drop it with `--no-overlay` to confirm recording is unaffected; lower overlay rate if needed. diff --git a/examples/oglo_tactile/oglo_heatmap.py b/examples/oglo_tactile/oglo_heatmap.py new file mode 100644 index 000000000..c53bca246 --- /dev/null +++ b/examples/oglo_tactile/oglo_heatmap.py @@ -0,0 +1,309 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""Hand-shaped OGLO tactile heatmap renderer for the IsaacTeleop headset overlay. + +Produces an RGBA8 ``numpy`` frame (H, W, 4) ready for ``isaacteleop.viz`` ``QuadLayer.submit``. +The layout, colormap (YlOrRd) and normalization replicate the OGLO bench heatmap viewer +so the in-headset view matches the bench tool an operator already trusts. + +Taxel index convention (firmware ``sample_order = "finger,row,col"``): + idx = finger * 16 + row * 4 + col (5 fingers x 4 rows x 4 cols = 80) +Row 0 is the fingertip (distal); rows are flipped so the fingertip draws at the top. + +Standalone preview (writes a PNG you can open): + python oglo_heatmap.py +""" + +from __future__ import annotations + +from dataclasses import dataclass, field + +import numpy as np + +try: + from PIL import Image, ImageDraw, ImageFont +except ( + ImportError +) as exc: # pragma: no cover - PIL is a hard dependency of this renderer + raise ImportError("oglo_heatmap requires Pillow: pip install pillow") from exc + +# --- geometry (from the reference viewer) ------------------------------------ +NUM_FINGERS = 5 +ROWS = 4 +COLS = 4 +TAXELS_PER_FINGER = ROWS * COLS # 16 +NUM_TAXELS = NUM_FINGERS * TAXELS_PER_FINGER # 80 + +_CELL = 0.36 +_BLK = COLS * _CELL # 1.44 + +# Finger block anchor positions in "cell units" (right hand). Left hand mirrors X. +_RIGHT_POS = { + "thumb": (1.5, 2.5), + "index": (3.05, 5.3), + "middle": (4.6, 6.05), + "ring": (6.15, 5.3), + "pinky": (7.7, 4.4), +} +_XS = [p[0] for p in _RIGHT_POS.values()] +_MIRROR = min(_XS) + max(_XS) + _BLK + +CHANNELS_RIGHT = ["thumb", "index", "middle", "ring", "pinky"] +CHANNELS_LEFT = ["pinky", "ring", "middle", "index", "thumb"] + +# YlOrRd 9-stop colormap (ColorBrewer), identical to the viewer. +_STOPS = np.array( + [ + [255, 255, 255], + [255, 255, 204], + [255, 237, 160], + [254, 217, 118], + [253, 141, 60], + [252, 78, 42], + [227, 26, 28], + [189, 0, 38], + [128, 0, 38], + ], + dtype=np.float32, +) +_IDLE_RGB = (244, 244, 244) # "#f4f4f4" for ~zero pressure + + +def ylord(t: np.ndarray) -> np.ndarray: + """Map t in [0, 1] (array) to RGB uint8 via the YlOrRd 9-stop ramp.""" + t = np.clip(t, 0.0, 1.0) * (len(_STOPS) - 1) + i = np.floor(t).astype(np.int32) + i = np.minimum(i, len(_STOPS) - 2) + f = (t - i)[..., None] + rgb = _STOPS[i] * (1.0 - f) + _STOPS[i + 1] * f + return rgb.astype(np.uint8) + + +def _hand_positions(side: str) -> dict: + if side == "left": + return {k: (_MIRROR - p[0] - _BLK, p[1]) for k, p in _RIGHT_POS.items()} + return _RIGHT_POS + + +@dataclass +class Normalizer: + """Baseline subtraction + threshold + scale, matching the viewer. + + ``v = clip((raw - baseline - threshold) / scale, 0, 1)``. The baseline is the + median of the first ``baseline_frames`` samples (captured per hand). RAW mode + bypasses baseline/threshold for absolute-ADC display. + """ + + scale: float = 500.0 + threshold: float = 30.0 + baseline_frames: int = 18 + raw: bool = False + _buf: list = field(default_factory=list) + _baseline: np.ndarray | None = None + + def rebaseline(self) -> None: + self._buf.clear() + self._baseline = None + + def normalize(self, taxels: np.ndarray) -> np.ndarray: + taxels = np.asarray(taxels, dtype=np.float32).reshape(NUM_TAXELS) + if self.raw: + return np.clip(taxels / max(self.scale, 1.0), 0.0, 1.0) + if self._baseline is None: + self._buf.append(taxels.copy()) + if len(self._buf) >= self.baseline_frames: + self._baseline = np.median(np.stack(self._buf), axis=0) + base = self._buf[0] if self._baseline is None else self._baseline + else: + base = self._baseline + return np.clip((taxels - base - self.threshold) / self.scale, 0.0, 1.0) + + +@dataclass +class _CellGeom: + finger: int + row: int + col: int + taxel: int + x: float # left, viewer cell-space + y: float # top, viewer cell-space (y-up before pixel flip) + + +def _hand_cells(side: str, channels: list) -> tuple: + """Precompute per-taxel cell rects (cell-space) and the hand bounding box.""" + pos = _hand_positions(side) + cells: list = [] + xs: list = [] + ys: list = [] + for f in range(NUM_FINGERS): + name = channels[f] if f < len(channels) else CHANNELS_RIGHT[f] + bx, by = pos[name] + for r in range(ROWS): + dr = ROWS - 1 - r # FLIP_ROWS: fingertip (row 0) at the top + for c in range(COLS): + taxel = f * TAXELS_PER_FINGER + r * COLS + c + # Viewer: px=ofx+(bx+c*CELL)*s, py=ofy-(by+BLK-dr*CELL)*s. + x = bx + c * _CELL + y = -(by + _BLK - dr * _CELL) + cells.append(_CellGeom(f, r, c, taxel, x, y)) + xs += [x, x + _CELL] + ys += [y, y + _CELL] + bbox = (min(xs), min(ys), max(xs), max(ys)) + label_anchors = [] + for f in range(NUM_FINGERS): + name = channels[f] if f < len(channels) else CHANNELS_RIGHT[f] + bx, by = pos[name] + label_anchors.append((name, bx + _BLK / 2.0, -(by + _BLK + 0.2))) + return cells, bbox, label_anchors + + +class TactileHeatmapRenderer: + """Renders both gloves into one RGBA8 HUD panel for the headset overlay.""" + + def __init__( + self, + width: int = 1024, + height: int = 512, + bg_rgba: tuple = (16, 18, 22, 200), + channels_left: list | None = None, + channels_right: list | None = None, + ) -> None: + self.width = width + self.height = height + self.bg_rgba = bg_rgba + self._left = _hand_cells("left", channels_left or CHANNELS_LEFT) + self._right = _hand_cells("right", channels_right or CHANNELS_RIGHT) + try: + self._font = ImageFont.truetype( + "DejaVuSans-Bold.ttf", max(11, height // 36) + ) + except OSError: + self._font = ImageFont.load_default() + + def render( + self, left_norm: np.ndarray | None, right_norm: np.ndarray | None + ) -> np.ndarray: + """Return an (H, W, 4) uint8 RGBA frame. Pass normalized taxels in [0, 1].""" + img = Image.new("RGBA", (self.width, self.height), self.bg_rgba) + draw = ImageDraw.Draw(img, "RGBA") + + half = self.width // 2 + margin = int(self.height * 0.06) + self._draw_hand( + draw, + self._left, + left_norm, + "LEFT", + (margin, margin, half - margin, self.height - margin), + ) + self._draw_hand( + draw, + self._right, + right_norm, + "RIGHT", + (half + margin, margin, self.width - margin, self.height - margin), + ) + self._draw_legend(draw) + return np.asarray(img, dtype=np.uint8) + + def _draw_hand(self, draw, hand, norm, title, region) -> None: + cells, bbox, labels = hand + rx0, ry0, rx1, ry1 = region + rw, rh = rx1 - rx0, ry1 - ry0 + x0, y0, x1, y1 = bbox + span_x, span_y = (x1 - x0) or 1.0, (y1 - y0) or 1.0 + s = 0.86 * min(rw / span_x, rh / span_y) + ofx = rx0 + (rw - span_x * s) / 2.0 - x0 * s + ofy = ry0 + (rh - span_y * s) / 2.0 - y0 * s + sz = _CELL * s + + values = ( + None + if norm is None + else np.clip( + np.asarray(norm, dtype=np.float32).reshape(NUM_TAXELS), 0.0, 1.0 + ) + ) + for cell in cells: + px = ofx + cell.x * s + py = ofy + cell.y * s + if values is None: + color = (90, 92, 96, 255) # disconnected: neutral gray + else: + v = float(values[cell.taxel]) + color = ( + (*_IDLE_RGB, 255) + if v < 0.015 + else (*tuple(int(x) for x in ylord(np.array(v))), 255) + ) + draw.rectangle( + [px + 0.6, py + 0.6, px + sz - 1.2, py + sz - 1.2], + fill=color, + outline=(60, 62, 66, 255), + ) + + for name, lx, ly in labels: + px = ofx + lx * s + py = ofy + ly * s + draw.text( + (px, py), + name.capitalize(), + fill=(210, 212, 216, 255), + font=self._font, + anchor="mb", + ) + + draw.text( + (rx0, ry0 - int(self.height * 0.05)), + title, + fill=(235, 236, 240, 255), + font=self._font, + anchor="lt", + ) + + def _draw_legend(self, draw) -> None: + bar_w = int(self.width * 0.22) + bar_h = max(8, int(self.height * 0.018)) + x0 = (self.width - bar_w) // 2 + y0 = self.height - bar_h - int(self.height * 0.035) + ramp = ylord(np.linspace(0.0, 1.0, bar_w)) + for i in range(bar_w): + r, g, b = (int(ramp[i][0]), int(ramp[i][1]), int(ramp[i][2])) + draw.line([(x0 + i, y0), (x0 + i, y0 + bar_h)], fill=(r, g, b, 255)) + draw.text( + (x0, y0 - 2), "low", fill=(200, 200, 205, 255), font=self._font, anchor="rb" + ) + draw.text( + (x0 + bar_w, y0 - 2), + "high", + fill=(200, 200, 205, 255), + font=self._font, + anchor="lb", + ) + + +def _demo() -> None: + """Render a synthetic frame (a press on the right index fingertip) to PNG.""" + rng = np.random.default_rng(0) + left = rng.integers(40, 90, NUM_TAXELS).astype(np.float32) # idle noise + right = rng.integers(40, 90, NUM_TAXELS).astype(np.float32) + # Press: right index (finger 1), fingertip rows, strong center taxels. + for r in range(2): + for c in range(1, 3): + right[1 * 16 + r * 4 + c] = 3500 + # Press: left thumb (finger 4 in left channel order) lighter. + left[4 * 16 + 0 * 4 + 1] = 1800 + + norm = Normalizer(raw=True, scale=4000.0) # raw mode for a deterministic demo + renderer = TactileHeatmapRenderer(width=1024, height=512) + frame = renderer.render(norm.normalize(left), norm.normalize(right)) + assert frame.shape == (512, 1024, 4) and frame.dtype == np.uint8 + out = "oglo_heatmap_demo.png" + Image.fromarray(frame, "RGBA").save(out) + nonzero = int((frame[..., :3] > 0).any(axis=-1).sum()) + print(f"wrote {out} shape={frame.shape} non-background px={nonzero}") + + +if __name__ == "__main__": + _demo() diff --git a/examples/oglo_tactile/oglo_teleop_record.py b/examples/oglo_tactile/oglo_teleop_record.py new file mode 100755 index 000000000..8db8e8434 --- /dev/null +++ b/examples/oglo_tactile/oglo_teleop_record.py @@ -0,0 +1,371 @@ +#!/usr/bin/env python3 +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""End-to-end OGLO + MetaQuest data-collection demo. + +Runs on the Linux host. It: + 1. Creates a TeleViz XR session (CloudXR runtime) shared with a DeviceIOSession. + 2. Launches the two ``oglo_tactile`` BLE plugins (one per hand) which push + tactile/IMU tensors into the shared runtime. + 3. Records Quest hand + head pose and both gloves to one time-synced MCAP. + 4. Draws a live tactile heatmap into the operator's Quest view via a QuadLayer. + +Prereqs (see README): CloudXR running + MetaQuest connected; gloves powered and +advertising; the project built with -DBUILD_PLUGIN_OGLO=ON -DBUILD_VIZ=ON and +the Python wheel installed. ``cupy`` is required for the headset overlay +(recording still works without it). + + python oglo_teleop_record.py --plugin-bin /oglo_tactile_plugin +""" + +from __future__ import annotations + +import argparse +import signal +import subprocess +import sys +import time +from collections.abc import Sequence +from datetime import datetime +from pathlib import Path +from types import FrameType + +import numpy as np + +import isaacteleop.deviceio as deviceio +import isaacteleop.viz as viz + +from oglo_heatmap import NUM_TAXELS, Normalizer, TactileHeatmapRenderer + +OVERLAY_W, OVERLAY_H = 1024, 512 +COLLECTION_PREFIX = "oglo" + +_stop = False + + +def _on_signal(signum: int, frame: FrameType | None) -> None: + global _stop + _stop = True + + +def _default_plugin_bin() -> str: + here = Path(__file__).resolve() + root = here.parents[2] # repo root + for cand in ( + root / "build/src/plugins/oglo_tactile/oglo_tactile_plugin", + root / "install/plugins/oglo_tactile/oglo_tactile_plugin", + ): + if cand.exists(): + return str(cand) + return "oglo_tactile_plugin" # rely on PATH + + +def _popen_plugin(plugin_bin: str, side: str) -> subprocess.Popen: + cmd = [plugin_bin, "--side", side, f"--collection-prefix={COLLECTION_PREFIX}"] + print(f"launching: {' '.join(cmd)}", flush=True) + return subprocess.Popen(cmd) + + +def _qrot(q_wxyz: Sequence[float], v: Sequence[float]) -> np.ndarray: + """Rotate vector v=(x,y,z) by quaternion q=(w,x,y,z).""" + w, x, y, z = q_wxyz + u = np.array([x, y, z], dtype=float) + vv = np.array(v, dtype=float) + t = 2.0 * np.cross(u, vv) + return vv + w * t + np.cross(u, t) + + +def _head_locked_placement( + head_tracker: "deviceio.HeadTracker", + session: "deviceio.DeviceIOSession", + args: argparse.Namespace, +) -> "viz.QuadLayerPlacement | None": + """Place the panel as a first-person HUD: `panel_dist` ahead of the head, + offset `panel_right` to the right and `panel_drop` down, facing the operator + (bottom-right of view so it never blocks the center). None if no head pose.""" + h = head_tracker.get_head(session) + if h is None or h.data is None or h.data.pose is None: + return None + p = h.data.pose.position + o = h.data.pose.orientation + q = (o.w, o.x, o.y, o.z) # schema quaternion is (x,y,z,w); Pose3D wants (w,x,y,z) + pos = np.array([p.x, p.y, p.z], dtype=float) + fwd = _qrot(q, (0.0, 0.0, -1.0)) + up = _qrot(q, (0.0, 1.0, 0.0)) + right = _qrot(q, (1.0, 0.0, 0.0)) + center = ( + pos + fwd * args.panel_dist + right * args.panel_right - up * args.panel_drop + ) + pose = viz.Pose3D( + position=(float(center[0]), float(center[1]), float(center[2])), orientation=q + ) + return viz.QuadLayerPlacement( + pose=pose, size_meters=(args.panel_w, args.panel_w * OVERLAY_H / OVERLAY_W) + ) + + +def _make_xr_session(trackers, xr_wait_s: float) -> viz.VizSession: + cfg = viz.VizSessionConfig() + cfg.mode = viz.DisplayMode.kXr + cfg.app_name = "OgloTeleopRecord" + cfg.xr_near_z = 0.05 + cfg.xr_far_z = 100.0 + # Wait this long for the MetaQuest to connect via CloudXR before giving up + # (otherwise xrGetSystem fails immediately with FORM_FACTOR_UNAVAILABLE). + cfg.xr_system_wait_seconds = int(xr_wait_s) + # The XrInstance must advertise the extensions the trackers need. + cfg.required_extensions = deviceio.DeviceIOSession.get_required_extensions(trackers) + return viz.VizSession.create(cfg) + + +def _make_overlay_layer( + session: viz.VizSession, args: argparse.Namespace +) -> viz.QuadLayer: + layer_cfg = viz.QuadLayerConfig() + layer_cfg.name = "oglo_tactile_overlay" + layer_cfg.resolution = viz.Resolution(OVERLAY_W, OVERLAY_H) + layer_cfg.format = viz.PixelFormat.kRGBA8 + if not args.overlay_fullscreen: + # Fixed panel in stage space, in front of and slightly below eye line. + # Tune at demo time with --panel-y / --panel-z if placement looks off; + # --overlay-fullscreen is the guaranteed-visible fallback. + pose = viz.Pose3D( + position=(0.0, args.panel_y, args.panel_z), orientation=(1.0, 0.0, 0.0, 0.0) + ) + layer_cfg.placement = viz.QuadLayerPlacement( + pose=pose, size_meters=(args.panel_w, args.panel_w * OVERLAY_H / OVERLAY_W) + ) + return session.add_quad_layer(layer_cfg) + + +def _taxels(tracked) -> np.ndarray | None: + if tracked is None or tracked.data is None: + return None + t = tracked.data.taxels + if not t or len(t) < NUM_TAXELS: + return None + return np.asarray(t, dtype=np.float32)[:NUM_TAXELS] + + +def main(argv: list[str] | None = None) -> int: + parser = argparse.ArgumentParser(description="OGLO + Quest data-collection demo") + parser.add_argument( + "--plugin-bin", + default=_default_plugin_bin(), + help="path to oglo_tactile_plugin", + ) + parser.add_argument( + "--mcap", default=None, help="output MCAP path (default: timestamped)" + ) + parser.add_argument( + "--duration", + type=float, + default=0.0, + help="seconds to record (0 = until Ctrl+C)", + ) + parser.add_argument( + "--no-plugins", + action="store_true", + help="do not launch plugins (launch them yourself)", + ) + parser.add_argument( + "--no-overlay", + action="store_true", + help="record only; skip the headset heatmap", + ) + parser.add_argument( + "--raw", action="store_true", help="raw ADC heatmap (no baseline subtraction)" + ) + parser.add_argument( + "--overlay-fullscreen", + action="store_true", + help="fullscreen overlay (facing-safe fallback)", + ) + parser.add_argument( + "--no-headlock", + action="store_true", + help="fixed panel instead of head-locked overlay", + ) + parser.add_argument( + "--panel-dist", + type=float, + default=0.7, + help="head-locked panel distance ahead (m)", + ) + parser.add_argument( + "--panel-right", + type=float, + default=0.28, + help="head-locked panel rightward offset (m)", + ) + parser.add_argument( + "--panel-drop", + type=float, + default=0.24, + help="head-locked panel drop below eye line (m)", + ) + parser.add_argument( + "--panel-y", + type=float, + default=0.0, + help="fixed-panel height (m, --no-headlock)", + ) + parser.add_argument( + "--panel-z", + type=float, + default=-1.0, + help="fixed-panel distance (m, -Z forward, --no-headlock)", + ) + parser.add_argument( + "--panel-w", type=float, default=0.42, help="overlay panel width (m)" + ) + parser.add_argument( + "--xr-wait", + type=float, + default=60.0, + help="seconds to wait for the MetaQuest to connect", + ) + parser.add_argument( + "--plugin-stagger", + type=float, + default=8.0, + help="seconds between launching the two gloves (avoids BLE scan contention)", + ) + args = parser.parse_args(argv) + + signal.signal(signal.SIGINT, _on_signal) + signal.signal(signal.SIGTERM, _on_signal) + + mcap_path = ( + args.mcap or f"oglo_teleop_{datetime.now().strftime('%Y%m%d_%H%M%S')}.mcap" + ) + + # --- trackers: Quest hand/head + both gloves -------------------------------- + hand = deviceio.HandTracker() + head = deviceio.HeadTracker() + oglo_left = deviceio.OgloTactileTracker(f"{COLLECTION_PREFIX}/left") + oglo_right = deviceio.OgloTactileTracker(f"{COLLECTION_PREFIX}/right") + trackers = [hand, head, oglo_left, oglo_right] + + # --- shared XR session (CloudXR) + DeviceIOSession with recording ----------- + print( + f"Creating XR session (waiting up to {args.xr_wait:.0f}s for MetaQuest via CloudXR)...", + flush=True, + ) + print("→ Now press CONNECT in the Quest web client if you haven't.", flush=True) + viz_session = _make_xr_session(trackers, args.xr_wait) + handles = deviceio.OpenXRSessionHandles(*viz_session.get_oxr_handles()) + + # Launch the gloves staggered: the two plugins share one BLE adapter, and a + # simultaneous BlueZ StartDiscovery from both makes one fail (DBus NoReply). + # Start the right glove now; the left is launched after --plugin-stagger + # seconds from inside the render loop (so the headset keeps rendering). + plugins = [] + left_launched = args.no_plugins + if not args.no_plugins: + plugins.append(_popen_plugin(args.plugin_bin, "right")) + + # Overlay setup (optional / requires cupy for GPU upload). + overlay = None + cp = None + if not args.no_overlay: + try: + import cupy as _cp + + _cp.asarray( + np.zeros((1, 1, 4), dtype=np.uint8) + ) # verify cupy actually works + cp = _cp + overlay = _make_overlay_layer(viz_session, args) + except Exception as e: + print( + f"WARNING: headset overlay disabled — cupy unusable ({e}). Recording continues.", + flush=True, + ) + print( + " Fix: pip uninstall -y cupy-cuda11x cupy-cuda12x && pip install cupy-cuda11x", + flush=True, + ) + overlay = None + cp = None + + renderer = TactileHeatmapRenderer(width=OVERLAY_W, height=OVERLAY_H) + norm_l = Normalizer(raw=args.raw) + norm_r = Normalizer(raw=args.raw) + + recording = deviceio.McapRecordingConfig( + mcap_path, + [ + (hand, "hands"), + (head, "head"), + (oglo_left, "oglo_left"), + (oglo_right, "oglo_right"), + ], + ) + + print(f"Recording → {mcap_path}", flush=True) + print("Keep hands relaxed for ~1s so the tactile baseline can tare.", flush=True) + + try: + with deviceio.DeviceIOSession.run(trackers, handles, recording) as session: + start = time.time() + left_due = start + args.plugin_stagger + frames = 0 + while not _stop: + # Staggered launch of the left glove (avoids BLE scan contention). + if not left_launched and time.time() >= left_due: + plugins.append(_popen_plugin(args.plugin_bin, "left")) + left_launched = True + + session.update() + + if overlay is not None: + try: + if not args.no_headlock: + placement = _head_locked_placement(head, session, args) + if placement is not None: + overlay.set_placement(placement) + left = _taxels(oglo_left.get_glove_data(session)) + right = _taxels(oglo_right.get_glove_data(session)) + frame = renderer.render( + norm_l.normalize(left) if left is not None else None, + norm_r.normalize(right) if right is not None else None, + ) + overlay.submit( + cp.asarray(frame) + ) # host->device for the GPU compositor + except Exception as e: + # Overlay is auxiliary — never let it kill the recording. + print( + f"WARNING: overlay error ({e}); disabling overlay, recording continues.", + flush=True, + ) + overlay = None + + viz_session.render() + + frames += 1 + if frames % 90 == 0: + print(f"[{time.time() - start:5.1f}s] frames={frames}", flush=True) + + if viz_session.should_close(): + break + if args.duration and (time.time() - start) >= args.duration: + break + finally: + for p in plugins: + p.terminate() + for p in plugins: + try: + p.wait(timeout=3) + except subprocess.TimeoutExpired: + p.kill() + viz_session.destroy() + + print(f"Done. Saved {mcap_path}", flush=True) + return 0 + + +if __name__ == "__main__": + sys.exit(main()) diff --git a/src/core/deviceio_base/cpp/inc/deviceio_base/oglo_tactile_tracker_base.hpp b/src/core/deviceio_base/cpp/inc/deviceio_base/oglo_tactile_tracker_base.hpp new file mode 100644 index 000000000..846010742 --- /dev/null +++ b/src/core/deviceio_base/cpp/inc/deviceio_base/oglo_tactile_tracker_base.hpp @@ -0,0 +1,20 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include "tracker.hpp" + +namespace core +{ + +struct OgloGloveSampleTrackedT; + +// Abstract base interface for OgloTactileTracker implementations. +class IOgloTactileTrackerImpl : public ITrackerImpl +{ +public: + virtual const OgloGloveSampleTrackedT& get_data() const = 0; +}; + +} // namespace core diff --git a/src/core/deviceio_trackers/cpp/CMakeLists.txt b/src/core/deviceio_trackers/cpp/CMakeLists.txt index 48b460d71..9a311a216 100644 --- a/src/core/deviceio_trackers/cpp/CMakeLists.txt +++ b/src/core/deviceio_trackers/cpp/CMakeLists.txt @@ -10,6 +10,7 @@ add_library(deviceio_trackers STATIC controller_tracker.cpp message_channel_tracker.cpp generic_3axis_pedal_tracker.cpp + oglo_tactile_tracker.cpp frame_metadata_tracker_oak.cpp full_body_tracker_pico.cpp inc/deviceio_trackers/head_tracker.hpp @@ -18,6 +19,7 @@ add_library(deviceio_trackers STATIC inc/deviceio_trackers/message_channel_tracker.hpp inc/deviceio_trackers/full_body_tracker_pico.hpp inc/deviceio_trackers/generic_3axis_pedal_tracker.hpp + inc/deviceio_trackers/oglo_tactile_tracker.hpp inc/deviceio_trackers/frame_metadata_tracker_oak.hpp ) diff --git a/src/core/deviceio_trackers/cpp/inc/deviceio_trackers/oglo_tactile_tracker.hpp b/src/core/deviceio_trackers/cpp/inc/deviceio_trackers/oglo_tactile_tracker.hpp new file mode 100644 index 000000000..4dabd81bf --- /dev/null +++ b/src/core/deviceio_trackers/cpp/inc/deviceio_trackers/oglo_tactile_tracker.hpp @@ -0,0 +1,79 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include +#include + +#include +#include + +namespace core +{ + +/*! + * @brief Facade for one OGLO tactile glove exposed as ``OgloGloveSampleTrackedT``. + * + * Reads a tensor collection pushed by the ``oglo_tactile`` plugin (Mode 2, + * ``--collection-prefix``). One tracker per hand: construct with the matching + * ``collection_id`` (e.g. ``"oglo/left"`` / ``"oglo/right"``). After each + * ``ITrackerSession::update()`` that includes this tracker, ``get_data(session)`` + * reflects the latest decoded sample; ``data`` is null until the first sample + * arrives or when the collection is unavailable. + * + * Usage: + * @code + * auto glove = std::make_shared("oglo/right"); + * // ... register with a session, then each tick: ... + * session->update(); + * const auto& tracked = glove->get_data(*session); + * if (tracked.data) { auto& taxels = tracked.data->taxels; ... } + * @endcode + */ +class OgloTactileTracker : public ITracker +{ +public: + //! Default maximum FlatBuffer size for OgloGloveSample messages + //! (80 taxels x 2B + 6 IMU x 2B + table overhead). + static constexpr size_t DEFAULT_MAX_FLATBUFFER_SIZE = 512; + + /*! + * @brief Constructs an OgloTactileTracker. + * @param collection_id Tensor collection identifier; must match the + * ``oglo_tactile`` plugin's ``--collection-prefix`` + "/" + side. + * @param max_flatbuffer_size Upper bound for serialized payloads (default 512). + */ + explicit OgloTactileTracker(const std::string& collection_id, + size_t max_flatbuffer_size = DEFAULT_MAX_FLATBUFFER_SIZE); + + std::string_view get_name() const override + { + return TRACKER_NAME; + } + + /*! + * @brief Glove snapshot from the session's implementation. + * @c tracked.data is null when no valid sample is available; when non-null, + * @c data->taxels (80 values) and the IMU fields are safe to read. + */ + const OgloGloveSampleTrackedT& get_data(const ITrackerSession& session) const; + + const std::string& collection_id() const + { + return collection_id_; + } + + size_t max_flatbuffer_size() const + { + return max_flatbuffer_size_; + } + +private: + static constexpr const char* TRACKER_NAME = "OgloTactileTracker"; + + std::string collection_id_; + size_t max_flatbuffer_size_; +}; + +} // namespace core diff --git a/src/core/deviceio_trackers/cpp/oglo_tactile_tracker.cpp b/src/core/deviceio_trackers/cpp/oglo_tactile_tracker.cpp new file mode 100644 index 000000000..851911d15 --- /dev/null +++ b/src/core/deviceio_trackers/cpp/oglo_tactile_tracker.cpp @@ -0,0 +1,23 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "inc/deviceio_trackers/oglo_tactile_tracker.hpp" + +namespace core +{ + +// ============================================================================ +// OgloTactileTracker +// ============================================================================ + +OgloTactileTracker::OgloTactileTracker(const std::string& collection_id, size_t max_flatbuffer_size) + : collection_id_(collection_id), max_flatbuffer_size_(max_flatbuffer_size) +{ +} + +const OgloGloveSampleTrackedT& OgloTactileTracker::get_data(const ITrackerSession& session) const +{ + return static_cast(session.get_tracker_impl(*this)).get_data(); +} + +} // namespace core diff --git a/src/core/deviceio_trackers/python/deviceio_trackers_init.py b/src/core/deviceio_trackers/python/deviceio_trackers_init.py index f867e8f54..580e1e045 100644 --- a/src/core/deviceio_trackers/python/deviceio_trackers_init.py +++ b/src/core/deviceio_trackers/python/deviceio_trackers_init.py @@ -12,6 +12,7 @@ MessageChannelTracker, FrameMetadataTrackerOak, Generic3AxisPedalTracker, + OgloTactileTracker, FullBodyTrackerPico, ITrackerSession, NUM_JOINTS, @@ -28,6 +29,7 @@ "FrameMetadataTrackerOak", "FullBodyTrackerPico", "Generic3AxisPedalTracker", + "OgloTactileTracker", "HandTracker", "HeadTracker", "ITracker", diff --git a/src/core/deviceio_trackers/python/tracker_bindings.cpp b/src/core/deviceio_trackers/python/tracker_bindings.cpp index 601c7db06..7147847d5 100644 --- a/src/core/deviceio_trackers/python/tracker_bindings.cpp +++ b/src/core/deviceio_trackers/python/tracker_bindings.cpp @@ -8,6 +8,7 @@ #include #include #include +#include #include #include #include @@ -150,6 +151,18 @@ PYBIND11_MODULE(_deviceio_trackers, m) { return self.get_data(session); }, py::arg("session"), "Get the current foot pedal tracked state (data is None when no data available)"); + py::class_>( + m, "OgloTactileTracker") + .def(py::init(), py::arg("collection_id"), + py::arg("max_flatbuffer_size") = core::OgloTactileTracker::DEFAULT_MAX_FLATBUFFER_SIZE, + "Construct an OgloTactileTracker for the given tensor collection ID " + "(e.g. 'oglo/left' / 'oglo/right', matching the oglo_tactile plugin's --collection-prefix)") + .def( + "get_glove_data", + [](const core::OgloTactileTracker& self, const core::ITrackerSession& session) -> core::OgloGloveSampleTrackedT + { return self.get_data(session); }, + py::arg("session"), "Get the current tactile glove tracked state (data is None when no data available)"); + py::class_>( m, "FullBodyTrackerPico") .def(py::init<>()) diff --git a/src/core/live_trackers/cpp/CMakeLists.txt b/src/core/live_trackers/cpp/CMakeLists.txt index 23d105b7d..d9826d3c2 100644 --- a/src/core/live_trackers/cpp/CMakeLists.txt +++ b/src/core/live_trackers/cpp/CMakeLists.txt @@ -12,6 +12,7 @@ add_library(live_trackers STATIC live_message_channel_tracker_impl.cpp live_full_body_tracker_pico_impl.cpp live_generic_3axis_pedal_tracker_impl.cpp + live_oglo_tactile_tracker_impl.cpp live_frame_metadata_tracker_oak_impl.cpp inc/live_trackers/schema_tracker_base.hpp inc/live_trackers/schema_tracker.hpp @@ -22,6 +23,7 @@ add_library(live_trackers STATIC live_message_channel_tracker_impl.hpp live_full_body_tracker_pico_impl.hpp live_generic_3axis_pedal_tracker_impl.hpp + live_oglo_tactile_tracker_impl.hpp live_frame_metadata_tracker_oak_impl.hpp ) diff --git a/src/core/live_trackers/cpp/inc/live_trackers/live_deviceio_factory.hpp b/src/core/live_trackers/cpp/inc/live_trackers/live_deviceio_factory.hpp index 7d6b5c4f9..9cff04674 100644 --- a/src/core/live_trackers/cpp/inc/live_trackers/live_deviceio_factory.hpp +++ b/src/core/live_trackers/cpp/inc/live_trackers/live_deviceio_factory.hpp @@ -30,6 +30,8 @@ class FullBodyTrackerPico; class IFullBodyTrackerPicoImpl; class Generic3AxisPedalTracker; class IGeneric3AxisPedalTrackerImpl; +class OgloTactileTracker; +class IOgloTactileTrackerImpl; class HandTracker; class IHandTrackerImpl; class HeadTracker; @@ -62,6 +64,7 @@ class LiveDeviceIOFactory std::unique_ptr create_full_body_tracker_pico_impl(const FullBodyTrackerPico* tracker); std::unique_ptr create_generic_3axis_pedal_tracker_impl( const Generic3AxisPedalTracker* tracker); + std::unique_ptr create_oglo_tactile_tracker_impl(const OgloTactileTracker* tracker); std::unique_ptr create_frame_metadata_tracker_oak_impl( const FrameMetadataTrackerOak* tracker); diff --git a/src/core/live_trackers/cpp/live_deviceio_factory.cpp b/src/core/live_trackers/cpp/live_deviceio_factory.cpp index 2c304480c..cefad07ea 100644 --- a/src/core/live_trackers/cpp/live_deviceio_factory.cpp +++ b/src/core/live_trackers/cpp/live_deviceio_factory.cpp @@ -10,6 +10,7 @@ #include "live_hand_tracker_impl.hpp" #include "live_head_tracker_impl.hpp" #include "live_message_channel_tracker_impl.hpp" +#include "live_oglo_tactile_tracker_impl.hpp" #include #include @@ -18,6 +19,7 @@ #include #include #include +#include #include #include @@ -85,6 +87,12 @@ std::unique_ptr try_create_oak_impl(LiveDeviceIOFactory& factory, return typed ? factory.create_frame_metadata_tracker_oak_impl(typed) : nullptr; } +std::unique_ptr try_create_oglo_impl(LiveDeviceIOFactory& factory, const ITracker& tracker) +{ + auto* typed = dynamic_cast(&tracker); + return typed ? factory.create_oglo_tactile_tracker_impl(typed) : nullptr; +} + using CollectExtensionsFn = bool (*)(const ITracker&, std::set&); using TryCreateFn = std::unique_ptr (*)(LiveDeviceIOFactory&, const ITracker&); @@ -103,6 +111,7 @@ inline const TrackerDispatchEntry k_tracker_dispatch[] = { { &try_add_extensions, &try_create_full_body_pico_impl }, { &try_add_extensions, &try_create_generic_pedal_impl }, { &try_add_extensions, &try_create_oak_impl }, + { &try_add_extensions, &try_create_oglo_impl }, }; } // namespace @@ -244,6 +253,17 @@ std::unique_ptr LiveDeviceIOFactory::create_gener return std::make_unique(handles_, tracker, std::move(channels)); } +std::unique_ptr LiveDeviceIOFactory::create_oglo_tactile_tracker_impl( + const OgloTactileTracker* tracker) +{ + std::unique_ptr channels; + if (should_record(tracker)) + { + channels = LiveOgloTactileTrackerImpl::create_mcap_channels(*writer_, get_name(tracker)); + } + return std::make_unique(handles_, tracker, std::move(channels)); +} + std::unique_ptr LiveDeviceIOFactory::create_frame_metadata_tracker_oak_impl( const FrameMetadataTrackerOak* tracker) { diff --git a/src/core/live_trackers/cpp/live_oglo_tactile_tracker_impl.cpp b/src/core/live_trackers/cpp/live_oglo_tactile_tracker_impl.cpp new file mode 100644 index 000000000..eb7fa2def --- /dev/null +++ b/src/core/live_trackers/cpp/live_oglo_tactile_tracker_impl.cpp @@ -0,0 +1,63 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "live_oglo_tactile_tracker_impl.hpp" + +#include +#include + +namespace core +{ + +namespace +{ + +SchemaTrackerConfig make_oglo_tensor_config(const OgloTactileTracker* tracker) +{ + SchemaTrackerConfig cfg; + cfg.collection_id = tracker->collection_id(); + cfg.max_flatbuffer_size = tracker->max_flatbuffer_size(); + cfg.tensor_identifier = "oglo_tactile"; + cfg.localized_name = "OgloTactileTracker"; + return cfg; +} + +} // namespace + +// ============================================================================ +// LiveOgloTactileTrackerImpl +// ============================================================================ + +std::unique_ptr LiveOgloTactileTrackerImpl::create_mcap_channels(mcap::McapWriter& writer, + std::string_view base_name) +{ + return std::make_unique(writer, base_name, OgloRecordingTraits::schema_name, + std::vector(OgloRecordingTraits::recording_channels.begin(), + OgloRecordingTraits::recording_channels.end())); +} + +LiveOgloTactileTrackerImpl::LiveOgloTactileTrackerImpl(const OpenXRSessionHandles& handles, + const OgloTactileTracker* tracker, + std::unique_ptr mcap_channels) + : mcap_channels_(std::move(mcap_channels)), + m_schema_reader(handles, + make_oglo_tensor_config(tracker), + mcap_channels_.get(), + /*mcap_channel_index=*/0, + /*mcap_channel_tracked_index=*/1) +{ +} + +void LiveOgloTactileTrackerImpl::update(int64_t /*monotonic_time_ns*/) +{ + // SchemaTracker throws on critical OpenXR/tensor failures; missing collection + // and "no new data" are non-fatal. + m_schema_reader.update(m_tracked.data); +} + +const OgloGloveSampleTrackedT& LiveOgloTactileTrackerImpl::get_data() const +{ + return m_tracked; +} + +} // namespace core diff --git a/src/core/live_trackers/cpp/live_oglo_tactile_tracker_impl.hpp b/src/core/live_trackers/cpp/live_oglo_tactile_tracker_impl.hpp new file mode 100644 index 000000000..57e3362c9 --- /dev/null +++ b/src/core/live_trackers/cpp/live_oglo_tactile_tracker_impl.hpp @@ -0,0 +1,51 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include "inc/live_trackers/schema_tracker.hpp" + +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace core +{ + +using OgloMcapChannels = McapTrackerChannels; +using OgloSchemaTracker = SchemaTracker; + +class LiveOgloTactileTrackerImpl : public IOgloTactileTrackerImpl +{ +public: + static std::vector required_extensions() + { + return SchemaTrackerBase::get_required_extensions(); + } + static std::unique_ptr create_mcap_channels(mcap::McapWriter& writer, std::string_view base_name); + + LiveOgloTactileTrackerImpl(const OpenXRSessionHandles& handles, + const OgloTactileTracker* tracker, + std::unique_ptr mcap_channels); + + LiveOgloTactileTrackerImpl(const LiveOgloTactileTrackerImpl&) = delete; + LiveOgloTactileTrackerImpl& operator=(const LiveOgloTactileTrackerImpl&) = delete; + LiveOgloTactileTrackerImpl(LiveOgloTactileTrackerImpl&&) = delete; + LiveOgloTactileTrackerImpl& operator=(LiveOgloTactileTrackerImpl&&) = delete; + + void update(int64_t monotonic_time_ns) override; + const OgloGloveSampleTrackedT& get_data() const override; + +private: + std::unique_ptr mcap_channels_; + OgloSchemaTracker m_schema_reader; + OgloGloveSampleTrackedT m_tracked; +}; + +} // namespace core diff --git a/src/core/mcap/cpp/inc/mcap/recording_traits.hpp b/src/core/mcap/cpp/inc/mcap/recording_traits.hpp index 8eb960396..75ff99058 100644 --- a/src/core/mcap/cpp/inc/mcap/recording_traits.hpp +++ b/src/core/mcap/cpp/inc/mcap/recording_traits.hpp @@ -52,6 +52,13 @@ struct PedalRecordingTraits static constexpr std::array replay_channels = { "pedals_tracked" }; }; +struct OgloRecordingTraits +{ + static constexpr std::string_view schema_name = "core.OgloGloveSampleRecord"; + static constexpr std::array recording_channels = { "oglo", "oglo_tracked" }; + static constexpr std::array replay_channels = { "oglo_tracked" }; +}; + struct OakRecordingTraits { static constexpr std::string_view schema_name = "core.FrameMetadataOakRecord"; diff --git a/src/core/python/deviceio_init.py b/src/core/python/deviceio_init.py index ea4a5aafe..e4b855207 100644 --- a/src/core/python/deviceio_init.py +++ b/src/core/python/deviceio_init.py @@ -17,6 +17,7 @@ MessageChannelTracker, FrameMetadataTrackerOak, Generic3AxisPedalTracker, + OgloTactileTracker, FullBodyTrackerPico, NUM_JOINTS, JOINT_PALM, @@ -42,6 +43,7 @@ StreamType, FrameMetadataOak, Generic3AxisPedalOutput, + OgloGloveSample, ) __all__ = [ @@ -52,6 +54,7 @@ "StreamType", "FrameMetadataOak", "Generic3AxisPedalOutput", + "OgloGloveSample", "ITracker", "HandTracker", "HeadTracker", @@ -60,6 +63,7 @@ "MessageChannelTracker", "FrameMetadataTrackerOak", "Generic3AxisPedalTracker", + "OgloTactileTracker", "FullBodyTrackerPico", "OpenXRSessionHandles", "DeviceIOSession", diff --git a/src/core/schema/fbs/oglo_tactile.fbs b/src/core/schema/fbs/oglo_tactile.fbs new file mode 100644 index 000000000..99e68fa89 --- /dev/null +++ b/src/core/schema/fbs/oglo_tactile.fbs @@ -0,0 +1,54 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +include "timestamp.fbs"; + +namespace core; + +// One tactile + IMU sample from a single OGLO glove (one hand). +// +// OGLO is a 5-finger Velostat tactile glove: each finger has a 4x4 taxel matrix +// (5 x 4 x 4 = 80 taxels). The hand side (left/right) is conveyed by the +// channel / collection name, mirroring how hand poses use left/right channels. +// +// Source device: OGLO-MT-RDR-02 reader, firmware >= 0.7.0 (CONFIG_SCHEMA_VER 5), +// BLE "packed12 v5" stream at 100 Hz (see the OGLO firmware packet-format spec). +table OgloGloveSample { + // Global sample sequence number (from the device). Monotonic; gaps indicate + // dropped BLE notifications. + seq: uint32 (id: 0); + + // Device timestamp of this sample in microseconds (device clock; wraps ~71 min). + device_time_us: uint32 (id: 1); + + // 80 taxel readings in "finger,row,col" order (5 fingers x 4 rows x 4 cols). + // Raw 12-bit ADC counts in [0, 4095]; higher == more force. The anatomical + // finger order is side-aware and is reported by the device Config characteristic + // (recorded as MCAP metadata at connection time). + taxels: [uint16] (id: 2); + + // 6-axis IMU, raw LSB (no on-device fusion in schema 5). + // Accelerometer (~0.000244 g/LSB at +/-8 g). + accel_x: int16 (id: 3); + accel_y: int16 (id: 4); + accel_z: int16 (id: 5); + // Gyroscope (raw; +/-2000 dps full scale). + gyro_x: int16 (id: 6); + gyro_y: int16 (id: 7); + gyro_z: int16 (id: 8); +} + +// Tracked wrapper for the in-memory tracker API (data is null when no glove +// sample is available, e.g. the glove is disconnected). +table OgloGloveSampleTracked { + data: OgloGloveSample (id: 0); +} + +// MCAP recording wrapper. Record types are the root types written to MCAP +// channels; trackers serialize into Record types in serialize_all(). +table OgloGloveSampleRecord { + data: OgloGloveSample (id: 0); + timestamp: DeviceDataTimestamp (id: 1); +} + +root_type OgloGloveSampleRecord; diff --git a/src/core/schema/python/CMakeLists.txt b/src/core/schema/python/CMakeLists.txt index d948e1417..5a3b98700 100644 --- a/src/core/schema/python/CMakeLists.txt +++ b/src/core/schema/python/CMakeLists.txt @@ -8,6 +8,7 @@ pybind11_add_module(schema_py hand_bindings.h head_bindings.h message_channel_bindings.h + oglo_tactile_bindings.h pedals_bindings.h pose_bindings.h schema_module.cpp diff --git a/src/core/schema/python/oglo_tactile_bindings.h b/src/core/schema/python/oglo_tactile_bindings.h new file mode 100644 index 000000000..1ef75ab91 --- /dev/null +++ b/src/core/schema/python/oglo_tactile_bindings.h @@ -0,0 +1,79 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +// Python bindings for the OGLO tactile glove FlatBuffer schema. +// Types: OgloGloveSample (table), OgloGloveSampleRecord, OgloGloveSampleTrackedT. + +#pragma once + +#include +#include +#include +#include + +#include + +namespace py = pybind11; + +namespace core +{ + +inline void bind_oglo_tactile(py::module& m) +{ + py::class_>(m, "OgloGloveSample") + .def(py::init([]() { return std::make_shared(); })) + .def_property( + "seq", [](const OgloGloveSampleT& s) { return s.seq; }, [](OgloGloveSampleT& s, uint32_t v) { s.seq = v; }) + .def_property( + "device_time_us", [](const OgloGloveSampleT& s) { return s.device_time_us; }, + [](OgloGloveSampleT& s, uint32_t v) { s.device_time_us = v; }) + .def_property( + "taxels", [](const OgloGloveSampleT& s) { return s.taxels; }, + [](OgloGloveSampleT& s, std::vector v) { s.taxels = std::move(v); }, + "80 raw 12-bit taxels (0..4095) in finger,row,col order") + .def_property( + "accel_x", [](const OgloGloveSampleT& s) { return s.accel_x; }, + [](OgloGloveSampleT& s, int16_t v) { s.accel_x = v; }) + .def_property( + "accel_y", [](const OgloGloveSampleT& s) { return s.accel_y; }, + [](OgloGloveSampleT& s, int16_t v) { s.accel_y = v; }) + .def_property( + "accel_z", [](const OgloGloveSampleT& s) { return s.accel_z; }, + [](OgloGloveSampleT& s, int16_t v) { s.accel_z = v; }) + .def_property( + "gyro_x", [](const OgloGloveSampleT& s) { return s.gyro_x; }, + [](OgloGloveSampleT& s, int16_t v) { s.gyro_x = v; }) + .def_property( + "gyro_y", [](const OgloGloveSampleT& s) { return s.gyro_y; }, + [](OgloGloveSampleT& s, int16_t v) { s.gyro_y = v; }) + .def_property( + "gyro_z", [](const OgloGloveSampleT& s) { return s.gyro_z; }, + [](OgloGloveSampleT& s, int16_t v) { s.gyro_z = v; }) + .def("__repr__", + [](const OgloGloveSampleT& s) + { + return "OgloGloveSample(seq=" + std::to_string(s.seq) + + ", device_time_us=" + std::to_string(s.device_time_us) + + ", taxels=" + std::to_string(s.taxels.size()) + ")"; + }); + + py::class_>(m, "OgloGloveSampleRecord") + .def(py::init<>()) + .def_property_readonly( + "data", [](const OgloGloveSampleRecordT& self) -> std::shared_ptr { return self.data; }) + .def_readonly("timestamp", &OgloGloveSampleRecordT::timestamp) + .def("__repr__", [](const OgloGloveSampleRecordT& self) + { return "OgloGloveSampleRecord(data=" + std::string(self.data ? "OgloGloveSample(...)" : "None") + ")"; }); + + py::class_>(m, "OgloGloveSampleTrackedT") + .def(py::init<>()) + .def_property_readonly( + "data", [](const OgloGloveSampleTrackedT& self) -> std::shared_ptr { return self.data; }) + .def("__repr__", + [](const OgloGloveSampleTrackedT& self) { + return std::string("OgloGloveSampleTrackedT(data=") + (self.data ? "OgloGloveSample(...)" : "None") + + ")"; + }); +} + +} // namespace core diff --git a/src/core/schema/python/schema_init.py b/src/core/schema/python/schema_init.py index 3f3aeb108..2b1aceeea 100644 --- a/src/core/schema/python/schema_init.py +++ b/src/core/schema/python/schema_init.py @@ -35,6 +35,10 @@ Generic3AxisPedalOutput, Generic3AxisPedalOutputTrackedT, Generic3AxisPedalOutputRecord, + # OGLO tactile glove types. + OgloGloveSample, + OgloGloveSampleTrackedT, + OgloGloveSampleRecord, # Message channel types. MessageChannelMessages, MessageChannelMessagesTrackedT, @@ -82,6 +86,10 @@ "Generic3AxisPedalOutput", "Generic3AxisPedalOutputTrackedT", "Generic3AxisPedalOutputRecord", + # OGLO tactile glove types. + "OgloGloveSample", + "OgloGloveSampleTrackedT", + "OgloGloveSampleRecord", # Message channel types. "MessageChannelMessages", "MessageChannelMessagesTrackedT", diff --git a/src/core/schema/python/schema_module.cpp b/src/core/schema/python/schema_module.cpp index e20dae586..59e497e50 100644 --- a/src/core/schema/python/schema_module.cpp +++ b/src/core/schema/python/schema_module.cpp @@ -12,6 +12,7 @@ #include "head_bindings.h" #include "message_channel_bindings.h" #include "oak_bindings.h" +#include "oglo_tactile_bindings.h" #include "pedals_bindings.h" #include "pose_bindings.h" #include "timestamp_bindings.h" @@ -40,6 +41,9 @@ PYBIND11_MODULE(_schema, m) // Bind pedals types (Generic3AxisPedalOutput table). core::bind_pedals(m); + // Bind OGLO tactile glove types (OgloGloveSample table). + core::bind_oglo_tactile(m); + // Bind message channel types (MessageChannelMessages table). core::bind_message_channel(m); diff --git a/src/core/schema_tests/python/test_oglo_tactile.py b/src/core/schema_tests/python/test_oglo_tactile.py new file mode 100644 index 000000000..0ce96bd00 --- /dev/null +++ b/src/core/schema_tests/python/test_oglo_tactile.py @@ -0,0 +1,66 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""Unit tests for OgloGloveSample types in isaacteleop.schema. + +Tests the following FlatBuffers types: +- OgloGloveSample: tactile glove sample (seq, device_time_us, 80 taxels, 6-axis IMU) +- OgloGloveSampleRecord: record wrapper carrying DeviceDataTimestamp +- OgloGloveSampleTrackedT: tracked wrapper (data is None when inactive) +""" + +from isaacteleop.schema import ( + OgloGloveSample, + OgloGloveSampleRecord, + OgloGloveSampleTrackedT, +) + +NUM_TAXELS = 80 + + +class TestOgloGloveSample: + """Tests for the OgloGloveSample table.""" + + def test_default_construction(self): + s = OgloGloveSample() + assert s.seq == 0 + assert s.device_time_us == 0 + assert list(s.taxels) == [] + + def test_field_round_trip(self): + s = OgloGloveSample() + s.seq = 12345 + s.device_time_us = 6_000_000 + s.taxels = list(range(NUM_TAXELS)) + s.accel_x, s.accel_y, s.accel_z = 100, -200, 4000 + s.gyro_x, s.gyro_y, s.gyro_z = 1, -2, 3 + + assert s.seq == 12345 + assert s.device_time_us == 6_000_000 + assert len(s.taxels) == NUM_TAXELS + assert s.taxels[0] == 0 and s.taxels[-1] == NUM_TAXELS - 1 + assert (s.accel_x, s.accel_y, s.accel_z) == (100, -200, 4000) + assert (s.gyro_x, s.gyro_y, s.gyro_z) == (1, -2, 3) + + def test_repr(self): + assert "OgloGloveSample" in repr(OgloGloveSample()) + + +class TestOgloGloveSampleTrackedT: + """Tests for the tracked wrapper.""" + + def test_default_construction_inactive(self): + assert OgloGloveSampleTrackedT().data is None + + def test_repr_inactive(self): + assert "None" in repr(OgloGloveSampleTrackedT()) + + +class TestOgloGloveSampleRecord: + """Tests for the MCAP record wrapper.""" + + def test_default_construction(self): + assert OgloGloveSampleRecord().data is None + + def test_repr(self): + assert "OgloGloveSampleRecord" in repr(OgloGloveSampleRecord()) diff --git a/src/plugins/oglo_tactile/CMakeLists.txt b/src/plugins/oglo_tactile/CMakeLists.txt new file mode 100644 index 000000000..7bfe87607 --- /dev/null +++ b/src/plugins/oglo_tactile/CMakeLists.txt @@ -0,0 +1,84 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +# ============================================================================== +# OGLO Tactile Glove Plugin (BLE) +# ============================================================================== +# Reads OGLO tactile gloves (80 taxels + 6-axis IMU per hand) over BLE and either +# records a local MCAP (Mode 1) or pushes via OpenXR SchemaPusher for a host +# tracker (Mode 2). Linux only (BlueZ). +# +# Enable with: cmake -B build -DBUILD_PLUGIN_OGLO=ON +# +# BLE backend: the plugin talks to BlueZ directly over libdbus (AFL-2.1, +# permissive) — no copyleft dependency, works out of the box on any Linux host +# with the BlueZ daemon. The transport sits behind the OgloBleClient interface, +# so an alternative backend can be dropped in via make_ble_client() if needed. +# +# Prerequisites (Linux): BlueZ daemon + libdbus development headers +# sudo apt install libdbus-1-dev +# ============================================================================== + +if(NOT CMAKE_SYSTEM_NAME STREQUAL "Linux") + message(STATUS "Skipping oglo_tactile plugin (Linux only)") + return() +endif() + +include(FetchContent) + +# ------------------------------------------------------------------------------ +# nlohmann/json — parse the device Config characteristic (MIT, header-only). +# ------------------------------------------------------------------------------ +FetchContent_Declare( + nlohmann_json + GIT_REPOSITORY https://github.com/nlohmann/json.git + GIT_TAG v3.11.3 + GIT_SHALLOW TRUE +) +set(JSON_BuildTests OFF CACHE INTERNAL "") +FetchContent_MakeAvailable(nlohmann_json) + +# ------------------------------------------------------------------------------ +# BLE backend: BlueZ over the system libdbus (AFL-2.1, permissive). +# ------------------------------------------------------------------------------ +find_package(PkgConfig REQUIRED) +pkg_check_modules(DBUS REQUIRED IMPORTED_TARGET dbus-1) + +add_executable(oglo_tactile_plugin + main.cpp + oglo_tactile_plugin.cpp + oglo_glove_sink.cpp + oglo_config.cpp + oglo_packet_parser.cpp + ble/oglo_ble_client.cpp + ble/oglo_ble_client_bluez.cpp +) + +target_include_directories(oglo_tactile_plugin PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}) + +target_link_libraries(oglo_tactile_plugin PRIVATE + pusherio::pusherio + oxr::oxr_core + isaacteleop_schema + mcap::mcap + nlohmann_json::nlohmann_json + PkgConfig::DBUS +) + +set_target_properties(oglo_tactile_plugin PROPERTIES INSTALL_RPATH "$ORIGIN") + +install(TARGETS oglo_tactile_plugin RUNTIME DESTINATION plugins/oglo_tactile) +install(FILES plugin.yaml README.md DESTINATION plugins/oglo_tactile) + +# ------------------------------------------------------------------------------ +# Parser unit test (dependency-free; validates the packed12 v5 wire decode +# against the firmware's packing reference). +# ------------------------------------------------------------------------------ +if(BUILD_TESTING) + add_executable(test_oglo_packet_parser + tests/test_oglo_packet_parser.cpp + oglo_packet_parser.cpp + ) + target_include_directories(test_oglo_packet_parser PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}) + add_test(NAME oglo_packet_parser COMMAND test_oglo_packet_parser) +endif() diff --git a/src/plugins/oglo_tactile/README.md b/src/plugins/oglo_tactile/README.md new file mode 100644 index 000000000..50173b11e --- /dev/null +++ b/src/plugins/oglo_tactile/README.md @@ -0,0 +1,92 @@ + + +# OGLO Tactile Glove Plugin + +C++ plugin that streams **OGLO** tactile gloves over BLE and records them into the +IsaacTeleop pipeline. Each glove (one BLE device per hand) provides **80 tactile +taxels** (5 fingers × 4×4) plus a **6-axis IMU** at 100 Hz. + +It follows the standard [Add a New Device](../../../docs/source/device/add_device.rst) +pattern and is modeled on the OAK camera plugin, including its two recording modes. + +## Data path + +``` +OGLO glove (BLE notify, packed12 v5) + → OgloBleClient (BlueZ over libdbus) [BLE thread] + → PacketParser (config-driven decode) [BLE thread] + → queue → IGloveSink [single consumer thread] + ├─ Mode 1: local MCAP file (mcap::McapWriter) + └─ Mode 2: SchemaPusher → host OgloTactileTracker → shared TeleopSession MCAP +``` + +The parser reads the device **Config characteristic** first and branches on the +notify `flags` byte (`0x04` packed12 v5 — primary; `0x02`/`0x01` schema-4 fallback), +so packet sizes are never hardcoded (wire spec: the OGLO firmware packed12-v5 packet format). + +## Build + +Linux only (BlueZ). Prerequisite: + +```bash +sudo apt install libdbus-1-dev +``` + +```bash +cmake -B build -DBUILD_PLUGIN_OGLO=ON +cmake --build build --target oglo_tactile_plugin --parallel +``` + +`nlohmann/json` (MIT) is fetched automatically via FetchContent. + +> **BLE backend.** The plugin talks to BlueZ directly over **libdbus** (AFL-2.1, +> permissive) — no copyleft dependency, works out of the box on any Linux host +> with the BlueZ daemon. The transport is isolated behind the `OgloBleClient` +> interface (`ble/oglo_ble_client.hpp`), so an alternative backend can be dropped +> in via `make_ble_client()` without touching the parser, schema, or tracker. + +## Usage + +```bash +# Mode 1 — standalone local MCAP (no OpenXR/TeleopSession needed) +./build/src/plugins/oglo_tactile/oglo_tactile_plugin --side right --mcap-filename=right.mcap +./build/src/plugins/oglo_tactile/oglo_tactile_plugin --side left --mcap-filename=left.mcap + +# Mode 2 — push for a host tracker into a shared TeleopSession MCAP +./build/src/plugins/oglo_tactile/oglo_tactile_plugin --side right --collection-prefix=oglo +``` + +| Option | Description | +|--------|-------------| +| `--side left\|right` | **Required.** Selects the `OGLO LEFT` / `OGLO RIGHT` device. | +| `--mcap-filename=PATH` | Mode 1: local MCAP output (mutually exclusive with `--collection-prefix`). | +| `--collection-prefix=PREFIX` | Mode 2: OpenXR collection prefix (`PREFIX/left`, `PREFIX/right`). | +| `--device-name=NAME` | Pin an exact advertised BLE name (multiple gloves nearby). | +| `--scan-timeout-ms=N` | BLE scan timeout (default 15000). | + +## MCAP output + +- Channel `oglo_left` / `oglo_right`, schema `core.OgloGloveSampleRecord` (flatbuffer). +- Each message: `seq`, `device_time_us`, `taxels[80]` (raw 12-bit, `finger,row,col`), + `accel_x/y/z`, `gyro_x/y/z` (raw IMU LSB), plus a `DeviceDataTimestamp` + (`sample_time_local_common_clock` is on the shared host monotonic clock, so OGLO + aligns with Quest hand/head streams). +- A `Metadata` record (`oglo_device_`) stores the verbatim device Config + (side, schema, rate, finger `channels`, serial, fw_rev), making each dataset + self-describing. + +## Tests + +`test_oglo_packet_parser` validates the wire decode against the firmware's own +12-bit packing reference (round-trip), plus the schema-4 fallback and +malformed-packet rejection: + +```bash +ctest --test-dir build -R oglo_packet_parser --output-on-failure +# or standalone, no IsaacTeleop deps: +cd src/plugins/oglo_tactile/tests +g++ -std=c++20 -I.. test_oglo_packet_parser.cpp ../oglo_packet_parser.cpp -o t && ./t +``` diff --git a/src/plugins/oglo_tactile/ble/oglo_ble_client.cpp b/src/plugins/oglo_tactile/ble/oglo_ble_client.cpp new file mode 100644 index 000000000..07882a4a3 --- /dev/null +++ b/src/plugins/oglo_tactile/ble/oglo_ble_client.cpp @@ -0,0 +1,28 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +// Backend-independent OgloBleClient helpers (always compiled, no BLE deps). + +#include "oglo_ble_client.hpp" + +namespace plugins +{ +namespace oglo_tactile +{ + +std::string advertised_name_for(Side side) +{ + switch (side) + { + case Side::Left: + return "OGLO LEFT"; + case Side::Right: + return "OGLO RIGHT"; + case Side::Unknown: + default: + return "OGLO"; + } +} + +} // namespace oglo_tactile +} // namespace plugins diff --git a/src/plugins/oglo_tactile/ble/oglo_ble_client.hpp b/src/plugins/oglo_tactile/ble/oglo_ble_client.hpp new file mode 100644 index 000000000..e568d365c --- /dev/null +++ b/src/plugins/oglo_tactile/ble/oglo_ble_client.hpp @@ -0,0 +1,75 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include "oglo_config.hpp" + +#include +#include +#include +#include +#include +#include + +namespace plugins +{ +namespace oglo_tactile +{ + +// OGLO BLE GATT identifiers (firmware OGLO-MT-RDR-02, all schema revisions). +namespace ble_uuids +{ +constexpr const char* kService = "4652535f-424c-4500-0000-000000000001"; +constexpr const char* kNotify = "4652535f-424c-4500-0001-000000000001"; // tactile + IMU stream +constexpr const char* kConfig = "4652535f-424c-4500-0002-000000000001"; // device manifest (JSON) +constexpr const char* kCommand = "4652535f-424c-4500-0003-000000000001"; // commands (write) +} // namespace ble_uuids + +//! Advertised BLE name for a hand, e.g. "OGLO LEFT". +std::string advertised_name_for(Side side); + +//! Abstract transport for one OGLO glove. +//! +//! The interface is deliberately backend-agnostic so the concrete BLE library +//! can be swapped without touching the parser/plugin (the upstream choice is a +//! licensing decision — see the plugin README). Notifications are delivered on +//! the backend's own thread; the @c NotifyCallback must therefore be +//! thread-safe and must not block. +class OgloBleClient +{ +public: + using NotifyCallback = std::function; + using StateCallback = std::function; + + virtual ~OgloBleClient() = default; + + //! Scan for the glove matching @p side (by advertised name, LE transport), + //! connect, and read the Config characteristic. + //! @return the Config JSON payload. + //! @throws std::runtime_error on scan/connect/read failure or timeout. + virtual std::string connect(Side side, std::chrono::milliseconds timeout) = 0; + + //! Subscribe to the notify characteristic. @p cb runs on the BLE thread. + virtual void subscribe(NotifyCallback cb) = 0; + + //! Register an optional connection-state observer (connect / drop events). + virtual void on_state_change(StateCallback cb) = 0; + + virtual bool is_connected() const = 0; + + //! Disconnect and quiesce notifications: after this returns, the backend + //! must not invoke the @c NotifyCallback again until the next subscribe(). + //! This lets the caller safely re-read geometry and re-subscribe on reconnect. + virtual void disconnect() = 0; +}; + +//! Construct the BLE backend (BlueZ over libdbus). +//! +//! @param device_name_override If non-empty, scan for this exact advertised +//! name instead of the side-derived default (useful when several gloves +//! are nearby and need to be pinned by name). +std::unique_ptr make_ble_client(std::string device_name_override = ""); + +} // namespace oglo_tactile +} // namespace plugins diff --git a/src/plugins/oglo_tactile/ble/oglo_ble_client_bluez.cpp b/src/plugins/oglo_tactile/ble/oglo_ble_client_bluez.cpp new file mode 100644 index 000000000..0d62b41a8 --- /dev/null +++ b/src/plugins/oglo_tactile/ble/oglo_ble_client_bluez.cpp @@ -0,0 +1,619 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +// Default OGLO BLE backend: talks to BlueZ directly over its D-Bus API using +// libdbus. libdbus is licensed under AFL-2.1 OR GPL-2.0; we use it under the +// permissive AFL-2.1, so the plugin carries no copyleft dependency. The only +// system requirement is the BlueZ D-Bus daemon plus libdbus-1 (apt: +// libdbus-1-dev), both already standard on any Linux host running Bluetooth. +// +// BlueZ exposes adapters, devices and GATT attributes as D-Bus objects under the +// well-known name org.bluez. The connection flow used here mirrors `bluetoothctl`: +// 1. find an adapter (org.bluez.Adapter1) and StartDiscovery +// 2. scan ObjectManager for a Device1 whose name matches the target glove +// 3. Device1.Connect(), wait for ServicesResolved +// 4. resolve the notify/config GATT characteristics by UUID +// 5. read the Config characteristic, then StartNotify and dispatch the +// PropertiesChanged(Value) signals on a dedicated thread. + +#include "oglo_ble_client.hpp" + +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace plugins +{ +namespace oglo_tactile +{ + +namespace +{ + +constexpr const char* kBluez = "org.bluez"; +constexpr const char* kObjMgr = "org.freedesktop.DBus.ObjectManager"; +constexpr const char* kProps = "org.freedesktop.DBus.Properties"; +constexpr const char* kAdapterIface = "org.bluez.Adapter1"; +constexpr const char* kDeviceIface = "org.bluez.Device1"; +constexpr const char* kGattCharIface = "org.bluez.GattCharacteristic1"; + +std::string to_upper(std::string s) +{ + std::transform(s.begin(), s.end(), s.begin(), [](unsigned char c) { return static_cast(std::toupper(c)); }); + return s; +} + +bool name_matches(const std::string& identifier, const std::string& target) +{ + return to_upper(identifier).find(to_upper(target)) != std::string::npos; +} + +bool uuid_equals(const std::string& a, const std::string& b) +{ + return to_upper(a) == to_upper(b); +} + +//! RAII wrapper for a DBusError so callers never leak the message string. +struct ScopedError +{ + DBusError err; + ScopedError() + { + dbus_error_init(&err); + } + ~ScopedError() + { + dbus_error_free(&err); + } + bool is_set() const + { + return dbus_error_is_set(&err); + } + std::string message() const + { + return err.message ? err.message : "unknown D-Bus error"; + } +}; + +//! RAII wrapper for a DBusMessage* reply. +struct ScopedMessage +{ + DBusMessage* msg = nullptr; + explicit ScopedMessage(DBusMessage* m = nullptr) : msg(m) + { + } + ~ScopedMessage() + { + if (msg) + dbus_message_unref(msg); + } + ScopedMessage(const ScopedMessage&) = delete; + ScopedMessage& operator=(const ScopedMessage&) = delete; + explicit operator bool() const + { + return msg != nullptr; + } +}; + +//! Send a method call and block for the reply (BlueZ object under org.bluez). +//! @throws std::runtime_error with the D-Bus error message on failure. +DBusMessage* call_blocking(DBusConnection* conn, + const std::string& path, + const char* iface, + const char* method, + DBusMessage* with_args = nullptr, + int timeout_ms = 20000) +{ + DBusMessage* msg = with_args ? with_args : dbus_message_new_method_call(kBluez, path.c_str(), iface, method); + if (!msg) + throw std::runtime_error("OGLO BlueZ: out of memory building D-Bus call"); + + ScopedError err; + DBusMessage* reply = dbus_connection_send_with_reply_and_block(conn, msg, timeout_ms, &err.err); + dbus_message_unref(msg); + if (err.is_set() || !reply) + throw std::runtime_error("OGLO BlueZ: " + std::string(method) + " failed: " + err.message()); + return reply; +} + +//! Append an empty a{sv} options dict (required by ReadValue / StartNotify args). +void append_empty_options(DBusMessage* msg) +{ + DBusMessageIter args, dict; + dbus_message_iter_init_append(msg, &args); + dbus_message_iter_open_container(&args, DBUS_TYPE_ARRAY, "{sv}", &dict); + dbus_message_iter_close_container(&args, &dict); +} + +//! Pull a fixed byte array (ay) out of the iterator currently pointing at it. +void read_byte_array(DBusMessageIter* arr_iter, std::vector& out) +{ + out.clear(); + if (dbus_message_iter_get_arg_type(arr_iter) != DBUS_TYPE_ARRAY) + return; + int n = 0; + const uint8_t* data = nullptr; + dbus_message_iter_get_fixed_array(arr_iter, &data, &n); + if (data && n > 0) + out.assign(data, data + n); +} + +//! Within an a{sv} props iterator, find @p key and leave @p value_out recursed +//! into its variant payload. Returns false if the key is absent. +bool find_prop_variant(DBusMessageIter props_iter, const char* key, DBusMessageIter* value_out) +{ + while (dbus_message_iter_get_arg_type(&props_iter) == DBUS_TYPE_DICT_ENTRY) + { + DBusMessageIter entry; + dbus_message_iter_recurse(&props_iter, &entry); + const char* name = nullptr; + if (dbus_message_iter_get_arg_type(&entry) == DBUS_TYPE_STRING) + dbus_message_iter_get_basic(&entry, &name); + dbus_message_iter_next(&entry); // -> variant + if (name && std::strcmp(name, key) == 0) + { + dbus_message_iter_recurse(&entry, value_out); // into the variant payload + return true; + } + dbus_message_iter_next(&props_iter); + } + return false; +} + +bool prop_get_string(DBusMessageIter props_iter, const char* key, std::string& out) +{ + DBusMessageIter v; + if (!find_prop_variant(props_iter, key, &v) || dbus_message_iter_get_arg_type(&v) != DBUS_TYPE_STRING) + return false; + const char* s = nullptr; + dbus_message_iter_get_basic(&v, &s); + out = s ? s : ""; + return true; +} + +bool prop_get_bool(DBusMessageIter props_iter, const char* key, bool& out) +{ + DBusMessageIter v; + if (!find_prop_variant(props_iter, key, &v) || dbus_message_iter_get_arg_type(&v) != DBUS_TYPE_BOOLEAN) + return false; + dbus_bool_t b = FALSE; + dbus_message_iter_get_basic(&v, &b); + out = b == TRUE; + return true; +} + +//! Visit every (object_path, interface_name, props-iter) from GetManagedObjects. +//! The props iterator passed to @p visit is positioned at the start of the +//! interface's a{sv} and may be copied freely by the visitor. +template +void walk_managed_objects(DBusConnection* conn, const Visitor& visit) +{ + ScopedMessage reply(call_blocking(conn, "/", kObjMgr, "GetManagedObjects")); + DBusMessageIter root; // a{oa{sa{sv}}} + if (!dbus_message_iter_init(reply.msg, &root) || dbus_message_iter_get_arg_type(&root) != DBUS_TYPE_ARRAY) + return; + + DBusMessageIter objs; + dbus_message_iter_recurse(&root, &objs); + while (dbus_message_iter_get_arg_type(&objs) == DBUS_TYPE_DICT_ENTRY) + { + DBusMessageIter obj; // { o, a{sa{sv}} } + dbus_message_iter_recurse(&objs, &obj); + const char* obj_path = nullptr; + if (dbus_message_iter_get_arg_type(&obj) == DBUS_TYPE_OBJECT_PATH) + dbus_message_iter_get_basic(&obj, &obj_path); + dbus_message_iter_next(&obj); // -> a{sa{sv}} (interfaces) + + DBusMessageIter ifaces; + dbus_message_iter_recurse(&obj, &ifaces); + while (dbus_message_iter_get_arg_type(&ifaces) == DBUS_TYPE_DICT_ENTRY) + { + DBusMessageIter iface; // { s, a{sv} } + dbus_message_iter_recurse(&ifaces, &iface); + const char* iface_name = nullptr; + if (dbus_message_iter_get_arg_type(&iface) == DBUS_TYPE_STRING) + dbus_message_iter_get_basic(&iface, &iface_name); + dbus_message_iter_next(&iface); // -> a{sv} (props) + + DBusMessageIter props; + dbus_message_iter_recurse(&iface, &props); + if (obj_path && iface_name) + visit(std::string(obj_path), std::string(iface_name), props); + + dbus_message_iter_next(&ifaces); + } + dbus_message_iter_next(&objs); + } +} + +class BlueZClient final : public OgloBleClient +{ +public: + explicit BlueZClient(std::string device_name_override) : m_name_override(std::move(device_name_override)) + { + dbus_threads_init_default(); // backend touches the connection from two threads (never concurrently) + } + + ~BlueZClient() override + { + try + { + disconnect(); + } + catch (...) + { + } + if (m_conn) + { + dbus_connection_close(m_conn); + dbus_connection_unref(m_conn); + } + } + + std::string connect(Side side, std::chrono::milliseconds timeout) override + { + const std::string target = m_name_override.empty() ? advertised_name_for(side) : m_name_override; + ensure_connection(); + + m_adapter_path = find_adapter(); + if (m_adapter_path.empty()) + throw std::runtime_error("OGLO BlueZ: no Bluetooth adapter (org.bluez.Adapter1) found"); + + set_le_discovery_filter(); + start_discovery(); + + m_device_path = scan_for_device(target, timeout); + stop_discovery(); + if (m_device_path.empty()) + throw std::runtime_error("OGLO BlueZ: '" + target + "' not found within scan timeout"); + + connect_device(timeout); + wait_services_resolved(timeout); + resolve_characteristics(); + if (m_notify_char_path.empty() || m_config_char_path.empty()) + throw std::runtime_error("OGLO BlueZ: notify/config GATT characteristics not found on device"); + + std::vector config = read_characteristic(m_config_char_path); + notify_state(true); + return std::string(reinterpret_cast(config.data()), config.size()); + } + + void subscribe(NotifyCallback cb) override + { + if (!m_conn || m_notify_char_path.empty()) + throw std::runtime_error("OGLO BlueZ: subscribe before connect"); + + m_notify_cb = std::move(cb); + + // Receive PropertiesChanged for our notify characteristic and device. + ScopedError err; + const std::string rule = + "type='signal',sender='org.bluez',interface='" + std::string(kProps) + "',member='PropertiesChanged'"; + dbus_bus_add_match(m_conn, rule.c_str(), &err.err); + if (err.is_set()) + throw std::runtime_error("OGLO BlueZ: add_match failed: " + err.message()); + if (!dbus_connection_add_filter(m_conn, &BlueZClient::signal_filter, this, nullptr)) + throw std::runtime_error("OGLO BlueZ: add_filter failed (out of memory)"); + m_match_rule = rule; + dbus_connection_flush(m_conn); + + { + ScopedMessage start(call_blocking(m_conn, m_notify_char_path, kGattCharIface, "StartNotify")); + } + + // Dispatch incoming notify signals on a dedicated thread so the consumer + // thread (run()) is free to drain the queue and run the watchdog. + m_dispatch_run.store(true, std::memory_order_relaxed); + m_dispatch_thread = std::thread([this] { dispatch_loop(); }); + } + + void on_state_change(StateCallback cb) override + { + m_state_cb = std::move(cb); + } + + bool is_connected() const override + { + return m_connected.load(std::memory_order_relaxed); + } + + void disconnect() override + { + // Quiescence contract: stop and join the dispatch thread first so the + // NotifyCallback can never fire again, then tear the link down. After + // this returns the connection is touched by this thread only. + m_dispatch_run.store(false, std::memory_order_relaxed); + if (m_dispatch_thread.joinable()) + m_dispatch_thread.join(); + + if (m_conn) + { + dbus_connection_remove_filter(m_conn, &BlueZClient::signal_filter, this); + if (!m_match_rule.empty()) + { + ScopedError err; + dbus_bus_remove_match(m_conn, m_match_rule.c_str(), &err.err); + m_match_rule.clear(); + } + stop_notify_best_effort(); + disconnect_device_best_effort(); + } + m_notify_char_path.clear(); + m_config_char_path.clear(); + m_connected.store(false, std::memory_order_relaxed); + } + +private: + void ensure_connection() + { + if (m_conn) + return; + ScopedError err; + m_conn = dbus_bus_get_private(DBUS_BUS_SYSTEM, &err.err); + if (err.is_set() || !m_conn) + throw std::runtime_error("OGLO BlueZ: cannot reach the system D-Bus / BlueZ daemon: " + err.message()); + // We dispatch the connection ourselves; don't let libdbus exit the process. + dbus_connection_set_exit_on_disconnect(m_conn, FALSE); + } + + std::string find_adapter() + { + std::string adapter; + walk_managed_objects(m_conn, + [&](const std::string& path, const std::string& iface, DBusMessageIter) + { + if (adapter.empty() && iface == kAdapterIface) + adapter = path; + }); + return adapter; + } + + void set_le_discovery_filter() + { + // Restrict discovery to LE so we converge faster and don't pick up BR/EDR + // duplicates. Best-effort: ignore failures (older BlueZ may reject keys). + try + { + DBusMessage* msg = + dbus_message_new_method_call(kBluez, m_adapter_path.c_str(), kAdapterIface, "SetDiscoveryFilter"); + if (!msg) + return; + DBusMessageIter args, dict, entry, var; + dbus_message_iter_init_append(msg, &args); + dbus_message_iter_open_container(&args, DBUS_TYPE_ARRAY, "{sv}", &dict); + dbus_message_iter_open_container(&dict, DBUS_TYPE_DICT_ENTRY, nullptr, &entry); + const char* key = "Transport"; + const char* val = "le"; + dbus_message_iter_append_basic(&entry, DBUS_TYPE_STRING, &key); + dbus_message_iter_open_container(&entry, DBUS_TYPE_VARIANT, "s", &var); + dbus_message_iter_append_basic(&var, DBUS_TYPE_STRING, &val); + dbus_message_iter_close_container(&entry, &var); + dbus_message_iter_close_container(&dict, &entry); + dbus_message_iter_close_container(&args, &dict); + ScopedMessage reply(call_blocking(m_conn, m_adapter_path, kAdapterIface, "SetDiscoveryFilter", msg)); + } + catch (const std::exception&) + { + // Non-fatal; discovery still works with the default filter. + } + } + + void start_discovery() + { + ScopedMessage reply(call_blocking(m_conn, m_adapter_path, kAdapterIface, "StartDiscovery")); + } + + void stop_discovery() + { + try + { + ScopedMessage reply(call_blocking(m_conn, m_adapter_path, kAdapterIface, "StopDiscovery")); + } + catch (const std::exception&) + { + // Already stopped / adapter busy — harmless. + } + } + + std::string scan_for_device(const std::string& target, std::chrono::milliseconds timeout) + { + const auto deadline = std::chrono::steady_clock::now() + timeout; + while (std::chrono::steady_clock::now() < deadline) + { + std::string match; + walk_managed_objects(m_conn, + [&](const std::string& path, const std::string& iface, DBusMessageIter props) + { + if (!match.empty() || iface != kDeviceIface) + return; + std::string name; + if ((prop_get_string(props, "Alias", name) || prop_get_string(props, "Name", name)) && + name_matches(name, target)) + match = path; + }); + if (!match.empty()) + return match; + std::this_thread::sleep_for(std::chrono::milliseconds(200)); + } + return ""; + } + + void connect_device(std::chrono::milliseconds timeout) + { + const int timeout_ms = static_cast(std::max(timeout.count(), 20000)); + ScopedMessage reply(call_blocking(m_conn, m_device_path, kDeviceIface, "Connect", nullptr, timeout_ms)); + } + + void wait_services_resolved(std::chrono::milliseconds timeout) + { + const auto deadline = std::chrono::steady_clock::now() + timeout; + while (std::chrono::steady_clock::now() < deadline) + { + bool resolved = false; + walk_managed_objects(m_conn, + [&](const std::string& path, const std::string& iface, DBusMessageIter props) + { + if (path == m_device_path && iface == kDeviceIface) + prop_get_bool(props, "ServicesResolved", resolved); + }); + if (resolved) + return; + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + throw std::runtime_error("OGLO BlueZ: GATT services did not resolve before timeout"); + } + + void resolve_characteristics() + { + m_notify_char_path.clear(); + m_config_char_path.clear(); + walk_managed_objects(m_conn, + [&](const std::string& path, const std::string& iface, DBusMessageIter props) + { + if (iface != kGattCharIface || path.rfind(m_device_path, 0) != 0) + return; + std::string uuid; + if (!prop_get_string(props, "UUID", uuid)) + return; + if (uuid_equals(uuid, ble_uuids::kNotify)) + m_notify_char_path = path; + else if (uuid_equals(uuid, ble_uuids::kConfig)) + m_config_char_path = path; + }); + } + + std::vector read_characteristic(const std::string& char_path) + { + DBusMessage* msg = dbus_message_new_method_call(kBluez, char_path.c_str(), kGattCharIface, "ReadValue"); + if (!msg) + throw std::runtime_error("OGLO BlueZ: out of memory building ReadValue"); + append_empty_options(msg); + ScopedMessage reply(call_blocking(m_conn, char_path, kGattCharIface, "ReadValue", msg)); + + DBusMessageIter it; + std::vector out; + if (dbus_message_iter_init(reply.msg, &it)) + read_byte_array(&it, out); + return out; + } + + void stop_notify_best_effort() + { + if (m_notify_char_path.empty()) + return; + try + { + ScopedMessage reply(call_blocking(m_conn, m_notify_char_path, kGattCharIface, "StopNotify")); + } + catch (const std::exception&) + { + } + } + + void disconnect_device_best_effort() + { + if (m_device_path.empty()) + return; + try + { + ScopedMessage reply(call_blocking(m_conn, m_device_path, kDeviceIface, "Disconnect")); + } + catch (const std::exception&) + { + } + } + + void notify_state(bool connected) + { + m_connected.store(connected, std::memory_order_relaxed); + if (m_state_cb) + m_state_cb(connected); + } + + void dispatch_loop() + { + // Pump the private connection: read_write_dispatch drives the filter, + // which forwards notify payloads. 100 ms wakeups keep shutdown responsive. + while (m_dispatch_run.load(std::memory_order_relaxed)) + { + if (!dbus_connection_read_write_dispatch(m_conn, 100)) + break; // connection closed + } + } + + //! D-Bus filter (runs on the dispatch thread). Forwards notify-characteristic + //! Value updates to the NotifyCallback and tracks device disconnects. + static DBusHandlerResult signal_filter(DBusConnection*, DBusMessage* msg, void* user) + { + auto* self = static_cast(user); + if (!dbus_message_is_signal(msg, kProps, "PropertiesChanged")) + return DBUS_HANDLER_RESULT_NOT_YET_HANDLED; + + const char* path = dbus_message_get_path(msg); + if (!path) + return DBUS_HANDLER_RESULT_NOT_YET_HANDLED; + + DBusMessageIter it; // (s interface, a{sv} changed, as invalidated) + if (!dbus_message_iter_init(msg, &it) || dbus_message_iter_get_arg_type(&it) != DBUS_TYPE_STRING) + return DBUS_HANDLER_RESULT_NOT_YET_HANDLED; + const char* iface = nullptr; + dbus_message_iter_get_basic(&it, &iface); + dbus_message_iter_next(&it); // -> a{sv} changed + if (dbus_message_iter_get_arg_type(&it) != DBUS_TYPE_ARRAY) + return DBUS_HANDLER_RESULT_NOT_YET_HANDLED; + DBusMessageIter changed; + dbus_message_iter_recurse(&it, &changed); + + if (iface && std::strcmp(iface, kGattCharIface) == 0 && self->m_notify_char_path == path) + { + DBusMessageIter value; + if (find_prop_variant(changed, "Value", &value)) + { + std::vector bytes; + read_byte_array(&value, bytes); + if (!bytes.empty() && self->m_notify_cb) + self->m_notify_cb(bytes.data(), bytes.size()); + } + return DBUS_HANDLER_RESULT_HANDLED; + } + + if (iface && std::strcmp(iface, kDeviceIface) == 0 && self->m_device_path == path) + { + bool connected = true; + if (prop_get_bool(changed, "Connected", connected) && !connected) + self->notify_state(false); + } + return DBUS_HANDLER_RESULT_NOT_YET_HANDLED; + } + + std::string m_name_override; + DBusConnection* m_conn = nullptr; + std::string m_adapter_path; + std::string m_device_path; + std::string m_notify_char_path; + std::string m_config_char_path; + std::string m_match_rule; + + NotifyCallback m_notify_cb; + StateCallback m_state_cb; + std::atomic m_connected{ false }; + + std::thread m_dispatch_thread; + std::atomic m_dispatch_run{ false }; +}; + +} // namespace + +std::unique_ptr make_ble_client(std::string device_name_override) +{ + return std::make_unique(std::move(device_name_override)); +} + +} // namespace oglo_tactile +} // namespace plugins diff --git a/src/plugins/oglo_tactile/main.cpp b/src/plugins/oglo_tactile/main.cpp new file mode 100644 index 000000000..3af4c4800 --- /dev/null +++ b/src/plugins/oglo_tactile/main.cpp @@ -0,0 +1,136 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "oglo_tactile_plugin.hpp" + +#include +#include +#include +#include +#include + +using namespace plugins::oglo_tactile; + +namespace +{ + +std::atomic g_stop{ false }; + +void signal_handler(int signal) +{ + if (signal == SIGINT || signal == SIGTERM) + g_stop.store(true, std::memory_order_relaxed); +} + +void print_usage(const char* prog) +{ + std::cout << "Usage: " << prog << " --side left|right [options]\n" + << "\nRequired:\n" + << " --side left|right Hand to connect (selects OGLO LEFT/RIGHT)\n" + << "\nOutput (exactly one):\n" + << " --mcap-filename=PATH Record to a local MCAP file (Mode 1)\n" + << " --collection-prefix=PREFIX Push via OpenXR for a host tracker (Mode 2)\n" + << "\nOptional:\n" + << " --device-name=NAME Pin an exact advertised BLE name\n" + << " --scan-timeout-ms=N Scan timeout (default 15000)\n" + << " --help Show this help\n"; +} + +} // namespace + +int main(int argc, char** argv) +try +{ + OgloTactilePlugin::Options opts; + bool side_set = false; + + for (int i = 1; i < argc; ++i) + { + const std::string arg = argv[i]; + if (arg == "--help" || arg == "-h") + { + print_usage(argv[0]); + return 0; + } + else if (arg.rfind("--side=", 0) == 0 || arg == "--side") + { + const std::string val = (arg == "--side") ? (i + 1 < argc ? argv[++i] : "") : arg.substr(7); + opts.side = side_from_string(val); + side_set = true; + } + else if (arg.rfind("--mcap-filename=", 0) == 0) + { + opts.mcap_filename = arg.substr(16); + } + else if (arg.rfind("--collection-prefix=", 0) == 0) + { + opts.collection_prefix = arg.substr(20); + } + else if (arg.rfind("--device-name=", 0) == 0) + { + opts.device_name_override = arg.substr(14); + } + else if (arg.rfind("--scan-timeout-ms=", 0) == 0) + { + const std::string val = arg.substr(18); + int ms = 0; + try + { + std::size_t parsed = 0; + ms = std::stoi(val, &parsed); + if (parsed != val.size()) + ms = -1; // reject trailing garbage, e.g. "15000ms" + } + catch (const std::exception&) + { + ms = -1; // force the validation error below + } + if (ms <= 0) + { + std::cerr << "Error: --scan-timeout-ms expects a positive integer (got '" << val << "')." << std::endl; + print_usage(argv[0]); + return 1; + } + opts.scan_timeout = std::chrono::milliseconds(ms); + } + else if (arg.rfind("--plugin-root-id=", 0) == 0) + { + // Injected by the PluginManager; not needed here. + } + else + { + std::cerr << "Unknown option: " << arg << std::endl; + print_usage(argv[0]); + return 1; + } + } + + if (!side_set || opts.side == Side::Unknown) + { + std::cerr << "Error: --side left|right is required." << std::endl; + print_usage(argv[0]); + return 1; + } + + std::signal(SIGINT, signal_handler); + std::signal(SIGTERM, signal_handler); + + std::cout << "============================================================\n" + << "OGLO Tactile Glove Plugin (" << to_string(opts.side) << ")\n" + << "============================================================" << std::endl; + + OgloTactilePlugin plugin(std::move(opts)); + plugin.run(g_stop); + + 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/oglo_tactile/oglo_config.cpp b/src/plugins/oglo_tactile/oglo_config.cpp new file mode 100644 index 000000000..48c150f7c --- /dev/null +++ b/src/plugins/oglo_tactile/oglo_config.cpp @@ -0,0 +1,87 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "oglo_config.hpp" + +#include + +#include + +namespace plugins +{ +namespace oglo_tactile +{ + +const char* to_string(Side side) noexcept +{ + switch (side) + { + case Side::Left: + return "left"; + case Side::Right: + return "right"; + case Side::Unknown: + default: + return "unknown"; + } +} + +Side side_from_string(const std::string& s) noexcept +{ + if (s == "left") + return Side::Left; + if (s == "right") + return Side::Right; + return Side::Unknown; +} + +namespace +{ + +// Tolerant getters: the Config schema evolves across firmware revisions, so a +// missing or wrong-typed field falls back to the default rather than throwing. +template +T get_or(const nlohmann::json& j, const char* key, T fallback) +{ + auto it = j.find(key); + if (it == j.end() || it->is_null()) + return fallback; + try + { + return it->get(); + } + catch (const nlohmann::json::exception&) + { + return fallback; + } +} + +} // namespace + +OgloDeviceConfig OgloDeviceConfig::parse(const std::string& json) +{ + nlohmann::json j = nlohmann::json::parse(json); // throws on malformed JSON + + OgloDeviceConfig cfg; + cfg.raw_json = json; + cfg.schema_ver = get_or(j, "schema_ver", 0); + cfg.packet_format = get_or(j, "packet_format", ""); + cfg.values_per_sample = get_or(j, "values_per_sample", 80); + cfg.samples_per_packet = get_or(j, "samples_per_packet", 0); + cfg.rate_hz = get_or(j, "rate_hz", 0); + cfg.sample_order = get_or(j, "sample_order", ""); + cfg.sample_shape = get_or>(j, "sample_shape", {}); + cfg.channels = get_or>(j, "channels", {}); + cfg.side = side_from_string(get_or(j, "side", "")); + cfg.serial = get_or(j, "serial", ""); + cfg.fw_rev = get_or(j, "fw_rev", ""); + cfg.device_id = get_or(j, "device_id", ""); + + if (cfg.values_per_sample <= 0) + throw std::runtime_error("OGLO Config: invalid values_per_sample"); + + return cfg; +} + +} // namespace oglo_tactile +} // namespace plugins diff --git a/src/plugins/oglo_tactile/oglo_config.hpp b/src/plugins/oglo_tactile/oglo_config.hpp new file mode 100644 index 000000000..4427d416d --- /dev/null +++ b/src/plugins/oglo_tactile/oglo_config.hpp @@ -0,0 +1,53 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include +#include + +namespace plugins +{ +namespace oglo_tactile +{ + +//! Hand side of a glove, as reported by the device Config characteristic. +enum class Side +{ + Unknown, + Left, + Right, +}; + +const char* to_string(Side side) noexcept; +Side side_from_string(const std::string& s) noexcept; + +//! Decoded contents of the OGLO Config characteristic (JSON). +//! +//! The firmware exposes the packet geometry here so the host never hardcodes +//! sizes (per the OGLO firmware packet-format spec). The raw JSON is retained so it can be +//! attached to the MCAP recording, making each dataset self-describing. +struct OgloDeviceConfig +{ + int schema_ver = 0; + std::string packet_format; //!< e.g. "packed12_v5" + int values_per_sample = 80; //!< taxel count per sample + int samples_per_packet = 0; //!< samples batched per BLE notify + int rate_hz = 0; //!< nominal sample rate + std::string sample_order; //!< e.g. "finger,row,col" + std::vector sample_shape; //!< e.g. [5, 4, 4] + std::vector channels; //!< side-aware finger order (thumb..pinky) + Side side = Side::Unknown; //!< left / right + std::string serial; + std::string fw_rev; + std::string device_id; + + std::string raw_json; //!< verbatim Config payload, for MCAP metadata. + + //! Parse a Config-characteristic JSON payload. Missing fields keep their + //! defaults; malformed JSON throws std::runtime_error. + static OgloDeviceConfig parse(const std::string& json); +}; + +} // namespace oglo_tactile +} // namespace plugins diff --git a/src/plugins/oglo_tactile/oglo_glove_sink.cpp b/src/plugins/oglo_tactile/oglo_glove_sink.cpp new file mode 100644 index 000000000..783621356 --- /dev/null +++ b/src/plugins/oglo_tactile/oglo_glove_sink.cpp @@ -0,0 +1,192 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#define MCAP_IMPLEMENTATION +#include "oglo_glove_sink.hpp" + +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +namespace plugins +{ +namespace oglo_tactile +{ + +namespace +{ + +// 80 taxels * 2B + 6 IMU * 2B + table overhead. 512 is comfortable headroom. +constexpr size_t kMaxFlatbufferSize = 512; + +//! Serialize a GloveSample into the schema's native packing type. +core::OgloGloveSampleT to_native(const GloveSample& s) +{ + core::OgloGloveSampleT out; + out.seq = s.seq; + out.device_time_us = s.device_time_us; + out.taxels.assign(s.taxels.begin(), s.taxels.end()); + out.accel_x = s.imu.ax; + out.accel_y = s.imu.ay; + out.accel_z = s.imu.az; + out.gyro_x = s.imu.gx; + out.gyro_y = s.imu.gy; + out.gyro_z = s.imu.gz; + return out; +} + +// ============================================================================= +// Mode 1: local MCAP file (self-contained; no OpenXR / TeleopSession) +// ============================================================================= + +class McapGloveSink final : public IGloveSink +{ +public: + McapGloveSink(Side side, const OgloDeviceConfig& config, const std::string& mcap_filename) + { + mcap::McapWriterOptions options("oglo_tactile"); + options.compression = mcap::Compression::None; + + auto status = m_writer.open(mcap_filename, options); + if (!status.ok()) + throw std::runtime_error("OGLO MCAP: failed to open " + mcap_filename + ": " + status.message); + + mcap::Schema schema("core.OgloGloveSampleRecord", "flatbuffer", + std::string(reinterpret_cast(core::OgloGloveSampleRecordBinarySchema::data()), + core::OgloGloveSampleRecordBinarySchema::size())); + m_writer.addSchema(schema); + + const std::string channel_name = std::string("oglo_") + to_string(side); + mcap::Channel channel(channel_name, "flatbuffer", schema.id); + m_writer.addChannel(channel); + m_channel_id = channel.id; + + write_device_metadata(side, config); + + std::cout << "MCAP recording '" << channel_name << "' to: " << mcap_filename << std::endl; + } + + ~McapGloveSink() override + { + m_writer.close(); + std::cout << "MCAP closed with " << m_message_count << " messages" << std::endl; + } + + void on_sample(const GloveSample& sample, int64_t local_ns, int64_t raw_ns) override + { + const int64_t now_ns = core::os_monotonic_now_ns(); + + flatbuffers::FlatBufferBuilder builder(kMaxFlatbufferSize); + auto native = to_native(sample); + auto data_offset = core::OgloGloveSample::Pack(builder, &native); + core::DeviceDataTimestamp timestamp(now_ns, local_ns, raw_ns); + core::OgloGloveSampleRecordBuilder record(builder); + record.add_data(data_offset); + record.add_timestamp(×tamp); + builder.Finish(record.Finish()); + + mcap::Message msg; + msg.channelId = m_channel_id; + msg.logTime = static_cast(now_ns); + msg.publishTime = static_cast(local_ns); + msg.sequence = sample.seq; + msg.data = reinterpret_cast(builder.GetBufferPointer()); + msg.dataSize = builder.GetSize(); + + auto status = m_writer.write(msg); + if (!status.ok()) + std::cerr << "OGLO MCAP: write failed: " << status.message << std::endl; + ++m_message_count; + } + +private: + //! Attach the device Config + geometry so the dataset is self-describing. + void write_device_metadata(Side side, const OgloDeviceConfig& config) + { + mcap::Metadata meta; + meta.name = std::string("oglo_device_") + to_string(side); + meta.metadata["side"] = to_string(side); + meta.metadata["schema_ver"] = std::to_string(config.schema_ver); + meta.metadata["packet_format"] = config.packet_format; + meta.metadata["rate_hz"] = std::to_string(config.rate_hz); + meta.metadata["values_per_sample"] = std::to_string(config.values_per_sample); + meta.metadata["sample_order"] = config.sample_order; + meta.metadata["serial"] = config.serial; + meta.metadata["fw_rev"] = config.fw_rev; + meta.metadata["device_id"] = config.device_id; + std::string channels; + for (size_t i = 0; i < config.channels.size(); ++i) + channels += (i ? "," : "") + config.channels[i]; + meta.metadata["channels"] = channels; + meta.metadata["config_json"] = config.raw_json; + + auto status = m_writer.write(meta); + if (!status.ok()) + std::cerr << "OGLO MCAP: metadata write failed: " << status.message << std::endl; + } + + mcap::McapWriter m_writer; + mcap::ChannelId m_channel_id = 0; + uint64_t m_message_count = 0; +}; + +// ============================================================================= +// Mode 2: OpenXR SchemaPusher (read by a host tracker into a shared MCAP) +// ============================================================================= + +class SchemaPusherGloveSink final : public IGloveSink +{ +public: + SchemaPusherGloveSink(Side side, const std::string& collection_prefix) + : m_session(std::make_shared( + "OgloTactilePlugin", core::SchemaPusher::get_required_extensions())), + m_pusher(m_session->get_handles(), + core::SchemaPusherConfig{ .collection_id = collection_prefix + "/" + to_string(side), + .max_flatbuffer_size = kMaxFlatbufferSize, + .tensor_identifier = "oglo_tactile", + .localized_name = "OGLO Tactile Glove", + .app_name = "OgloTactilePlugin" }) + { + std::cout << "Pushing collection: " << collection_prefix << "/" << to_string(side) << std::endl; + } + + void on_sample(const GloveSample& sample, int64_t local_ns, int64_t raw_ns) override + { + flatbuffers::FlatBufferBuilder builder(kMaxFlatbufferSize); + auto native = to_native(sample); + auto offset = core::OgloGloveSample::Pack(builder, &native); + builder.Finish(offset); + m_pusher.push_buffer(builder.GetBufferPointer(), builder.GetSize(), local_ns, raw_ns); + } + +private: + std::shared_ptr m_session; + core::SchemaPusher m_pusher; +}; + +} // namespace + +std::unique_ptr create_glove_sink(Side side, + const OgloDeviceConfig& config, + const std::string& mcap_filename, + const std::string& collection_prefix) +{ + if (!mcap_filename.empty() && !collection_prefix.empty()) + throw std::runtime_error("Specify only one of --mcap-filename or --collection-prefix"); + if (mcap_filename.empty() && collection_prefix.empty()) + throw std::runtime_error("One of --mcap-filename or --collection-prefix is required"); + + if (!mcap_filename.empty()) + return std::make_unique(side, config, mcap_filename); + return std::make_unique(side, collection_prefix); +} + +} // namespace oglo_tactile +} // namespace plugins diff --git a/src/plugins/oglo_tactile/oglo_glove_sink.hpp b/src/plugins/oglo_tactile/oglo_glove_sink.hpp new file mode 100644 index 000000000..4cae643a1 --- /dev/null +++ b/src/plugins/oglo_tactile/oglo_glove_sink.hpp @@ -0,0 +1,47 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include "oglo_config.hpp" +#include "oglo_packet_parser.hpp" + +#include +#include +#include + +namespace plugins +{ +namespace oglo_tactile +{ + +//! Consumes decoded glove samples and records / publishes them. +//! +//! Mirrors the OAK plugin's two recording modes: a local MCAP file +//! (self-contained, no OpenXR) or an OpenXR SchemaPusher collection that a host +//! tracker reads into a shared TeleopSession MCAP. +class IGloveSink +{ +public: + virtual ~IGloveSink() = default; + + //! Record one sample. Timestamps are nanoseconds: @p local_ns on the host + //! monotonic clock (shared across devices), @p raw_ns on the device clock. + //! Called from the plugin's single push thread. + virtual void on_sample(const GloveSample& sample, int64_t local_ns, int64_t raw_ns) = 0; +}; + +//! Selects the sink from the two mutually exclusive output modes. +//! +//! @param side Glove side (left/right), used in channel/collection names. +//! @param config Device Config, attached as MCAP metadata for provenance. +//! @param mcap_filename Mode 1: local MCAP path (empty to disable). +//! @param collection_prefix Mode 2: OpenXR collection prefix (empty to disable). +//! @throws std::runtime_error if both or neither mode is given, or on open failure. +std::unique_ptr create_glove_sink(Side side, + const OgloDeviceConfig& config, + const std::string& mcap_filename, + const std::string& collection_prefix); + +} // namespace oglo_tactile +} // namespace plugins diff --git a/src/plugins/oglo_tactile/oglo_packet_parser.cpp b/src/plugins/oglo_tactile/oglo_packet_parser.cpp new file mode 100644 index 000000000..d35475752 --- /dev/null +++ b/src/plugins/oglo_tactile/oglo_packet_parser.cpp @@ -0,0 +1,197 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "oglo_packet_parser.hpp" + +namespace plugins +{ +namespace oglo_tactile +{ + +namespace +{ + +// Little-endian readers (no alignment assumptions on the BLE payload). +inline uint16_t rd_u16(const uint8_t* p) noexcept +{ + return static_cast(p[0]) | static_cast(static_cast(p[1]) << 8); +} + +inline int16_t rd_i16(const uint8_t* p) noexcept +{ + return static_cast(rd_u16(p)); +} + +inline uint32_t rd_u32(const uint8_t* p) noexcept +{ + return static_cast(p[0]) | (static_cast(p[1]) << 8) | (static_cast(p[2]) << 16) | + (static_cast(p[3]) << 24); +} + +// Wire-format constants (see the OGLO firmware packed12-v5 packet specification). +constexpr uint8_t kFlagMethodB = 0x01; // schema 4, per-sample IMU +constexpr uint8_t kFlagMethodC = 0x02; // schema 4, one packet-level IMU +constexpr uint8_t kFlagPacked12 = 0x04; // schema 5, packed 12-bit + per-sample IMU + +// packed12 v5 layout. +constexpr size_t kV5HeaderBytes = 10; // count, flags, seq_base(u32), t_base_us(u32) +constexpr size_t kV5TaxelBytes = 120; // 80 x 12-bit +constexpr size_t kV5ImuBytes = 12; // 6 x i16 +constexpr size_t kV5SampleStride = 2 + kV5TaxelBytes + kV5ImuBytes; // dt_us(u16) + taxels + imu = 134 + +// schema-4 (Method B/C) layout. +constexpr size_t kS4HeaderBytes = 6; // count, flags, base_ts_us(u32) +constexpr size_t kS4ImuBytes = 17; // roll,pitch,ax,ay,az,gx,gy,gz (8 x i16) + ok(u8) + +} // namespace + +PacketFormat format_from_flags(uint8_t flags) noexcept +{ + if ((flags & kFlagPacked12) != 0) + return PacketFormat::Packed12V5; + if ((flags & kFlagMethodC) != 0) + return PacketFormat::MethodC; + if ((flags & kFlagMethodB) != 0) + return PacketFormat::MethodB; + return PacketFormat::Unknown; +} + +void PacketParser::unpack_taxels12(const uint8_t* packed120, std::array& out) noexcept +{ + // 80 taxels packed as 40 triplets (2 taxels per 3 bytes). + for (int k = 0; k < kNumTaxels / 2; ++k) + { + const uint8_t b0 = packed120[3 * k + 0]; + const uint8_t b1 = packed120[3 * k + 1]; + const uint8_t b2 = packed120[3 * k + 2]; + out[2 * k + 0] = static_cast((static_cast(b0) << 4) | (b1 >> 4)); + out[2 * k + 1] = static_cast((static_cast(b1 & 0x0F) << 8) | b2); + } +} + +bool PacketParser::parse_packed12_v5(const uint8_t* data, size_t len, std::vector& out) +{ + if (len < kV5HeaderBytes) + return false; + + const uint8_t count = data[0]; + const uint32_t seq_base = rd_u32(data + 2); + const uint32_t t_base_us = rd_u32(data + 6); + + if (len < kV5HeaderBytes + static_cast(count) * kV5SampleStride) + return false; + + const size_t start = out.size(); + out.reserve(start + count); + for (uint8_t i = 0; i < count; ++i) + { + const uint8_t* slot = data + kV5HeaderBytes + static_cast(i) * kV5SampleStride; + GloveSample s; + s.seq = seq_base + i; + s.device_time_us = t_base_us + rd_u16(slot); // dt_us + unpack_taxels12(slot + 2, s.taxels); + + const uint8_t* imu = slot + 2 + kV5TaxelBytes; + s.imu.ax = rd_i16(imu + 0); + s.imu.ay = rd_i16(imu + 2); + s.imu.az = rd_i16(imu + 4); + s.imu.gx = rd_i16(imu + 6); + s.imu.gy = rd_i16(imu + 8); + s.imu.gz = rd_i16(imu + 10); + out.push_back(s); + } + return true; +} + +bool PacketParser::parse_schema4(const uint8_t* data, + size_t len, + int values_per_sample, + bool per_sample_imu, + bool packet_imu, + std::vector& out) +{ + if (len < kS4HeaderBytes || values_per_sample <= 0) + return false; + + const uint8_t count = data[0]; + const uint32_t base_ts_us = rd_u32(data + 2); + + const size_t taxel_bytes = static_cast(values_per_sample) * 2; + const size_t imu_len = per_sample_imu ? kS4ImuBytes : 0; + const size_t stride = taxel_bytes + imu_len; + + // Taxels must be fully present; a trailing Method-C IMU is best-effort. + if (len < kS4HeaderBytes + static_cast(count) * stride) + return false; + + const int n_taxels = values_per_sample < kNumTaxels ? values_per_sample : kNumTaxels; + + const size_t start = out.size(); + out.reserve(start + count); + for (uint8_t i = 0; i < count; ++i) + { + const uint8_t* base = data + kS4HeaderBytes + static_cast(i) * stride; + GloveSample s; + s.seq = i; // schema 4 carries no per-sample sequence; plugin rebases. + s.device_time_us = base_ts_us; + for (int t = 0; t < n_taxels; ++t) + s.taxels[static_cast(t)] = rd_u16(base + static_cast(t) * 2); + + // Method B: per-sample 17B IMU block is roll,pitch,ax,ay,az,gx,gy,gz,ok. + // Our schema keeps only the raw 6-axis values (offsets 4..15). + if (per_sample_imu) + { + const uint8_t* imu = base + taxel_bytes; + s.imu.ax = rd_i16(imu + 4); + s.imu.ay = rd_i16(imu + 6); + s.imu.az = rd_i16(imu + 8); + s.imu.gx = rd_i16(imu + 10); + s.imu.gy = rd_i16(imu + 12); + s.imu.gz = rd_i16(imu + 14); + } + out.push_back(s); + } + + // Method C: one packet-level IMU after all taxel samples; apply to every + // sample in this packet (best-effort, only if the bytes are present). + if (packet_imu) + { + const size_t imu_off = kS4HeaderBytes + static_cast(count) * stride; + if (len >= imu_off + kS4ImuBytes) + { + const uint8_t* imu = data + imu_off; + ImuSample shared; + shared.ax = rd_i16(imu + 4); + shared.ay = rd_i16(imu + 6); + shared.az = rd_i16(imu + 8); + shared.gx = rd_i16(imu + 10); + shared.gy = rd_i16(imu + 12); + shared.gz = rd_i16(imu + 14); + for (size_t j = start; j < out.size(); ++j) + out[j].imu = shared; + } + } + return true; +} + +bool PacketParser::parse(const uint8_t* data, size_t len, int values_per_sample, std::vector& out) +{ + if (data == nullptr || len < 2) + return false; + + switch (format_from_flags(data[1])) + { + case PacketFormat::Packed12V5: + return parse_packed12_v5(data, len, out); + case PacketFormat::MethodC: + return parse_schema4(data, len, values_per_sample, /*per_sample_imu=*/false, /*packet_imu=*/true, out); + case PacketFormat::MethodB: + return parse_schema4(data, len, values_per_sample, /*per_sample_imu=*/true, /*packet_imu=*/false, out); + case PacketFormat::Unknown: + default: + return false; + } +} + +} // namespace oglo_tactile +} // namespace plugins diff --git a/src/plugins/oglo_tactile/oglo_packet_parser.hpp b/src/plugins/oglo_tactile/oglo_packet_parser.hpp new file mode 100644 index 000000000..c0f08f8f3 --- /dev/null +++ b/src/plugins/oglo_tactile/oglo_packet_parser.hpp @@ -0,0 +1,90 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include +#include +#include +#include + +namespace plugins +{ +namespace oglo_tactile +{ + +// OGLO tactile matrix geometry (fixed by the OGLO-MT-RDR-02 sensor flex). +constexpr int kNumFingers = 5; +constexpr int kRowsPerFinger = 4; +constexpr int kColsPerFinger = 4; +constexpr int kTaxelsPerFinger = kRowsPerFinger * kColsPerFinger; // 16 +constexpr int kNumTaxels = kNumFingers * kTaxelsPerFinger; // 80 + +//! Raw 6-axis IMU sample (device LSB units, no on-device fusion). +struct ImuSample +{ + int16_t ax = 0, ay = 0, az = 0; + int16_t gx = 0, gy = 0, gz = 0; +}; + +//! One decoded tactile + IMU sample for a single glove. +//! +//! @c seq and @c device_time_us are taken from the wire when present +//! (packed12 v5). For the legacy schema-4 fallbacks the wire carries no +//! per-sample sequence/timestamp, so @c seq is the index within the packet and +//! @c device_time_us is the packet base timestamp; the plugin layer rebases +//! these against a host counter / sample rate. +struct GloveSample +{ + uint32_t seq = 0; + uint32_t device_time_us = 0; + std::array taxels{}; // raw 12-bit ADC, [0, 4095] + ImuSample imu{}; +}; + +//! Wire framing, selected by the notify @c flags byte. The host MUST read the +//! Config characteristic first and never hardcode sizes; the parser branches on +//! @c flags so a firmware that switches framing (SET BLEFMT) keeps working. +enum class PacketFormat +{ + Unknown, + Packed12V5, //!< flags bit2 (0x04): 12-bit taxels + per-sample 6-axis raw IMU (FW >= 0.7.0). + MethodC, //!< flags bit1 (0x02): schema-4, 16-bit taxels-only samples + one trailing IMU. + MethodB, //!< flags bit0 (0x01): schema-4, 16-bit taxels + per-sample 17B IMU. +}; + +PacketFormat format_from_flags(uint8_t flags) noexcept; + +//! Stateless parser for a single BLE notify payload. +//! +//! All multi-byte fields are little-endian. The parser is intentionally free of +//! any BLE / FlatBuffers dependency so it can be unit-tested against the +//! firmware reference vectors (the OGLO packed12-v5 packing reference). +class PacketParser +{ +public: + //! Parse @p data (@p len bytes) and append the decoded samples to @p out. + //! + //! @param values_per_sample Taxel count from the Config characteristic + //! (normally 80). Used to size the legacy schema-4 sample stride. + //! @return true if the packet was well-formed and fully decoded; false if it + //! was truncated or had an unknown framing (in which case @p out is + //! left unmodified). + static bool parse(const uint8_t* data, size_t len, int values_per_sample, std::vector& out); + + //! Unpack 80 12-bit taxels from a 120-byte packed block (packed12 v5). + //! Triplet (b0,b1,b2) -> even=(b0<<4)|(b1>>4), odd=((b1&0x0F)<<8)|b2. + static void unpack_taxels12(const uint8_t* packed120, std::array& out) noexcept; + +private: + static bool parse_packed12_v5(const uint8_t* data, size_t len, std::vector& out); + static bool parse_schema4(const uint8_t* data, + size_t len, + int values_per_sample, + bool per_sample_imu, + bool packet_imu, + std::vector& out); +}; + +} // namespace oglo_tactile +} // namespace plugins diff --git a/src/plugins/oglo_tactile/oglo_tactile_plugin.cpp b/src/plugins/oglo_tactile/oglo_tactile_plugin.cpp new file mode 100644 index 000000000..e8465217b --- /dev/null +++ b/src/plugins/oglo_tactile/oglo_tactile_plugin.cpp @@ -0,0 +1,169 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "oglo_tactile_plugin.hpp" + +#include + +#include +#include +#include +#include + +namespace plugins +{ +namespace oglo_tactile +{ + +namespace +{ +constexpr int64_t kNsPerUs = 1000; +constexpr uint64_t kDeviceUsWrap = (1ull << 32); // device_time_us is uint32 +} // namespace + +OgloTactilePlugin::OgloTactilePlugin(Options options) : m_opts(std::move(options)) +{ + m_ble = make_ble_client(m_opts.device_name_override); + m_ble->on_state_change([this](bool connected) { m_connected.store(connected, std::memory_order_relaxed); }); +} + +OgloTactilePlugin::~OgloTactilePlugin() +{ + if (m_ble) + m_ble->disconnect(); +} + +int64_t OgloTactilePlugin::extend_device_ns(uint32_t device_time_us) +{ + // Promote the wrapping 32-bit microsecond counter to a monotonic 64-bit ns + // value so recorded raw-device timestamps don't jump backwards every ~71 min. + if (m_have_last_us && device_time_us < m_last_device_us && (m_last_device_us - device_time_us) > (kDeviceUsWrap / 2)) + { + m_device_wrap_offset_ns += static_cast(kDeviceUsWrap) * kNsPerUs; + } + m_last_device_us = device_time_us; + m_have_last_us = true; + return m_device_wrap_offset_ns + static_cast(device_time_us) * kNsPerUs; +} + +void OgloTactilePlugin::connect_and_subscribe() +{ + std::cout << "Scanning for " << advertised_name_for(m_opts.side) << "..." << std::endl; + const std::string config_json = m_ble->connect(m_opts.side, m_opts.scan_timeout); + m_config = OgloDeviceConfig::parse(config_json); + m_values_per_sample.store(m_config.values_per_sample, std::memory_order_relaxed); + + // The device's microsecond clock restarts on reconnect, so reset the wrap + // tracker; otherwise a stale m_last_device_us would spuriously trip the wrap + // detector on the first packet of the new session. Safe to touch here: the + // BLE notify thread is quiesced by disconnect() before we re-subscribe below. + m_have_last_us = false; + m_last_device_us = 0; + m_device_wrap_offset_ns = 0; + + std::cout << "Connected: side=" << to_string(m_config.side) << " schema_ver=" << m_config.schema_ver + << " format=" << m_config.packet_format << " rate=" << m_config.rate_hz << "Hz" + << " serial=" << m_config.serial << std::endl; + + // The sink is created once (first connect); reconnects reuse it so the MCAP + // file / OpenXR collection stays continuous across drops. + if (!m_sink) + m_sink = create_glove_sink(m_opts.side, m_config, m_opts.mcap_filename, m_opts.collection_prefix); + + m_ble->subscribe([this](const uint8_t* data, std::size_t len) { on_notify(data, len); }); + m_last_notify_ns.store(core::os_monotonic_now_ns(), std::memory_order_relaxed); +} + +void OgloTactilePlugin::on_notify(const uint8_t* data, std::size_t len) +{ + // Runs on the BLE backend thread: parse + timestamp + enqueue only. + const int64_t arrival_ns = core::os_monotonic_now_ns(); + m_last_notify_ns.store(arrival_ns, std::memory_order_relaxed); + + std::vector samples; + if (!PacketParser::parse(data, len, m_values_per_sample.load(std::memory_order_relaxed), samples) || samples.empty()) + return; + + // Anchor the newest sample of the batch to arrival time and back-date the + // earlier samples by their intra-batch device delta (no wrap within a batch). + const uint32_t newest_us = samples.back().device_time_us; + for (const auto& s : samples) + { + const int64_t intra_batch_ns = static_cast(newest_us - s.device_time_us) * kNsPerUs; + QueuedSample q; + q.sample = s; + q.local_ns = arrival_ns - intra_batch_ns; + q.raw_ns = extend_device_ns(s.device_time_us); + enqueue(q); + } +} + +void OgloTactilePlugin::enqueue(const QueuedSample& q) +{ + { + std::lock_guard lock(m_mutex); + m_queue.push_back(q); + } + m_cv.notify_one(); +} + +void OgloTactilePlugin::run(std::atomic& stop) +{ + connect_and_subscribe(); + + while (!stop.load(std::memory_order_relaxed)) + { + std::deque batch; + { + std::unique_lock lock(m_mutex); + m_cv.wait_for(lock, std::chrono::milliseconds(100), [&] { return !m_queue.empty(); }); + batch.swap(m_queue); + } + + for (const auto& q : batch) + { + m_sink->on_sample(q.sample, q.local_ns, q.raw_ns); + ++m_total_samples; + } + + // Stall / disconnect watchdog: if no notifications have arrived within + // the stall window, force a reconnect (the BLE link may have dropped + // silently). Recording continues into the same sink. + const int64_t now_ns = core::os_monotonic_now_ns(); + const int64_t since_notify_ms = (now_ns - m_last_notify_ns.load(std::memory_order_relaxed)) / 1'000'000; + const bool stalled = since_notify_ms > m_opts.stall_timeout.count(); + if ((stalled || !m_connected.load(std::memory_order_relaxed)) && !stop.load(std::memory_order_relaxed)) + { + std::cerr << "OGLO: link stalled/dropped (" << since_notify_ms << " ms), reconnecting..." << std::endl; + try + { + m_ble->disconnect(); + connect_and_subscribe(); + } + catch (const std::exception& e) + { + std::cerr << "OGLO: reconnect failed: " << e.what() << " — retrying." << std::endl; + std::this_thread::sleep_for(std::chrono::milliseconds(500)); + } + } + } + + // Quiesce the producer, then drain anything the BLE thread queued before we + // observed `stop`, so the tail of a recording is never silently dropped. + m_ble->disconnect(); + std::deque tail; + { + std::lock_guard lock(m_mutex); + tail.swap(m_queue); + } + for (const auto& q : tail) + { + m_sink->on_sample(q.sample, q.local_ns, q.raw_ns); + ++m_total_samples; + } + + std::cout << "OGLO: stopped after " << m_total_samples << " samples." << std::endl; +} + +} // namespace oglo_tactile +} // namespace plugins diff --git a/src/plugins/oglo_tactile/oglo_tactile_plugin.hpp b/src/plugins/oglo_tactile/oglo_tactile_plugin.hpp new file mode 100644 index 000000000..f84c8d216 --- /dev/null +++ b/src/plugins/oglo_tactile/oglo_tactile_plugin.hpp @@ -0,0 +1,90 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include "ble/oglo_ble_client.hpp" +#include "oglo_config.hpp" +#include "oglo_glove_sink.hpp" +#include "oglo_packet_parser.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace plugins +{ +namespace oglo_tactile +{ + +//! Drives one OGLO glove end to end: BLE connect -> Config -> notify parse -> +//! record / push. BLE notifications are parsed on the backend thread and queued; +//! a single consumer thread (run()) drains the queue and owns the sink so all +//! OpenXR / MCAP I/O happens on one thread. +class OgloTactilePlugin +{ +public: + struct Options + { + Side side = Side::Unknown; + std::string mcap_filename; //!< Mode 1 (XOR collection_prefix) + std::string collection_prefix; //!< Mode 2 (XOR mcap_filename) + std::string device_name_override; //!< pin a specific advertised name + std::chrono::milliseconds scan_timeout{ 15000 }; + std::chrono::milliseconds stall_timeout{ 3000 }; //!< no-notify -> reconnect + }; + + explicit OgloTactilePlugin(Options options); + ~OgloTactilePlugin(); + + //! Connect, then run until @p stop is set. Reconnects automatically on drop. + void run(std::atomic& stop); + +private: + struct QueuedSample + { + GloveSample sample; + int64_t local_ns; + int64_t raw_ns; + }; + + void connect_and_subscribe(); + void on_notify(const uint8_t* data, std::size_t len); // BLE thread + void enqueue(const QueuedSample& q); + + //! Extend the device's 32-bit microsecond clock to a wrap-free 64-bit ns. + int64_t extend_device_ns(uint32_t device_time_us); + + Options m_opts; + OgloDeviceConfig m_config; + // Written by the consumer thread on (re)connect, read by the BLE thread in + // on_notify(); atomic so the geometry handoff across threads is race-free. + std::atomic m_values_per_sample{ kNumTaxels }; + + std::unique_ptr m_ble; + std::unique_ptr m_sink; + + // BLE-thread -> consumer-thread sample queue. + std::mutex m_mutex; + std::condition_variable m_cv; + std::deque m_queue; + + std::atomic m_last_notify_ns{ 0 }; + std::atomic m_connected{ false }; + + // Device-clock extension state (consumer side: only touched on BLE thread). + bool m_have_last_us = false; + uint32_t m_last_device_us = 0; + int64_t m_device_wrap_offset_ns = 0; + + uint64_t m_total_samples = 0; +}; + +} // namespace oglo_tactile +} // namespace plugins diff --git a/src/plugins/oglo_tactile/plugin.yaml b/src/plugins/oglo_tactile/plugin.yaml new file mode 100644 index 000000000..4ba4dd3ec --- /dev/null +++ b/src/plugins/oglo_tactile/plugin.yaml @@ -0,0 +1,14 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +name: oglo_tactile +description: "OGLO tactile glove (80 taxels + 6-axis IMU per hand) over BLE" +command: "./oglo_tactile_plugin" +version: "1.0.0" +devices: + - path: "/glove/oglo_left" + type: "oglo_tactile" + description: "Left OGLO tactile glove (OGLO LEFT)" + - path: "/glove/oglo_right" + type: "oglo_tactile" + description: "Right OGLO tactile glove (OGLO RIGHT)" diff --git a/src/plugins/oglo_tactile/tests/test_oglo_packet_parser.cpp b/src/plugins/oglo_tactile/tests/test_oglo_packet_parser.cpp new file mode 100644 index 000000000..9b61704c8 --- /dev/null +++ b/src/plugins/oglo_tactile/tests/test_oglo_packet_parser.cpp @@ -0,0 +1,200 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +// Standalone unit test for the OGLO packet parser. It builds packets using the +// firmware's own 12-bit packing reference and asserts that +// the parser is the exact inverse, so a decode bug cannot pass silently. +// +// Build & run standalone (no IsaacTeleop deps): +// g++ -std=c++20 -I.. test_oglo_packet_parser.cpp ../oglo_packet_parser.cpp -o t && ./t + +#include "oglo_packet_parser.hpp" + +#include +#include +#include +#include +#include + +using namespace plugins::oglo_tactile; + +namespace +{ + +void put_u16(std::vector& b, uint16_t v) +{ + b.push_back(static_cast(v & 0xFF)); + b.push_back(static_cast((v >> 8) & 0xFF)); +} + +void put_u32(std::vector& b, uint32_t v) +{ + b.push_back(static_cast(v & 0xFF)); + b.push_back(static_cast((v >> 8) & 0xFF)); + b.push_back(static_cast((v >> 16) & 0xFF)); + b.push_back(static_cast((v >> 24) & 0xFF)); +} + +void put_i16(std::vector& b, int16_t v) +{ + put_u16(b, static_cast(v)); +} + +// Firmware-side 12-bit pack reference (OGLO packed12-v5): +// b0 = a >> 4; b1 = ((a & 0x0F) << 4) | (b >> 8); b2 = b & 0xFF +void pack_taxels12(const uint16_t taxels[kNumTaxels], std::vector& b) +{ + for (int k = 0; k < kNumTaxels / 2; ++k) + { + const uint16_t a = taxels[2 * k + 0] & 0x0FFF; + const uint16_t c = taxels[2 * k + 1] & 0x0FFF; + b.push_back(static_cast(a >> 4)); + b.push_back(static_cast(((a & 0x0F) << 4) | (c >> 8))); + b.push_back(static_cast(c & 0xFF)); + } +} + +// Deterministic synthetic taxel pattern spanning the full 12-bit range. +uint16_t synth_taxel(int sample, int idx) +{ + return static_cast((idx * 53 + sample * 911 + 7) % 4096); +} + +int g_checks = 0; +void check(bool cond, const char* msg) +{ + ++g_checks; + if (!cond) + { + std::fprintf(stderr, "FAIL: %s\n", msg); + std::abort(); + } +} + +// ---- packed12 v5 round-trip -------------------------------------------------- +void test_packed12_v5() +{ + const uint8_t count = 3; + const uint32_t seq_base = 1000; + const uint32_t t_base_us = 5'000'000; + const uint16_t dt[3] = { 0, 10000, 20000 }; + + std::vector pkt; + pkt.push_back(count); + pkt.push_back(0x04); // flags: packed12 v5 + put_u32(pkt, seq_base); + put_u32(pkt, t_base_us); + + uint16_t expected_taxels[3][kNumTaxels]; + for (int s = 0; s < count; ++s) + { + put_u16(pkt, dt[s]); + for (int i = 0; i < kNumTaxels; ++i) + expected_taxels[s][i] = synth_taxel(s, i); + pack_taxels12(expected_taxels[s], pkt); + // IMU raw: ax,ay,az,gx,gy,gz + put_i16(pkt, static_cast(100 + s)); + put_i16(pkt, static_cast(-200 - s)); + put_i16(pkt, static_cast(4096 - s)); + put_i16(pkt, static_cast(s)); + put_i16(pkt, static_cast(-s)); + put_i16(pkt, static_cast(s * 7)); + } + + check(pkt.size() == 10u + 3u * 134u, "v5 packet size == 412"); + + std::vector out; + check(PacketParser::parse(pkt.data(), pkt.size(), kNumTaxels, out), "v5 parse ok"); + check(out.size() == count, "v5 sample count"); + + for (int s = 0; s < count; ++s) + { + check(out[s].seq == seq_base + static_cast(s), "v5 seq"); + check(out[s].device_time_us == t_base_us + dt[s], "v5 device_time_us"); + for (int i = 0; i < kNumTaxels; ++i) + check(out[s].taxels[i] == expected_taxels[s][i], "v5 taxel value"); + check(out[s].imu.ax == static_cast(100 + s), "v5 imu ax"); + check(out[s].imu.ay == static_cast(-200 - s), "v5 imu ay"); + check(out[s].imu.az == static_cast(4096 - s), "v5 imu az"); + check(out[s].imu.gz == static_cast(s * 7), "v5 imu gz"); + } +} + +// ---- packed12 12-bit unpack edge values -------------------------------------- +void test_unpack_edges() +{ + uint16_t taxels[kNumTaxels]; + for (int i = 0; i < kNumTaxels; ++i) + taxels[i] = (i % 2 == 0) ? 4095 : 0; // alternating max / min + + std::vector packed; + pack_taxels12(taxels, packed); + check(packed.size() == 120u, "packed block == 120 bytes"); + + std::array got{}; + PacketParser::unpack_taxels12(packed.data(), got); + for (int i = 0; i < kNumTaxels; ++i) + check(got[i] == taxels[i], "unpack edge value"); +} + +// ---- schema-4 Method B fallback --------------------------------------------- +void test_method_b() +{ + const uint8_t count = 2; + const uint32_t base_ts = 1234; + + std::vector pkt; + pkt.push_back(count); + pkt.push_back(0x01); // flags: Method B + put_u32(pkt, base_ts); + + for (int s = 0; s < count; ++s) + { + for (int i = 0; i < kNumTaxels; ++i) + put_u16(pkt, static_cast((i + s) % 4096)); + // 17B IMU: roll,pitch,ax,ay,az,gx,gy,gz (i16) + ok(u8) + put_i16(pkt, 11); // roll (ignored) + put_i16(pkt, 22); // pitch (ignored) + put_i16(pkt, static_cast(s)); // ax + put_i16(pkt, static_cast(-s)); // ay + put_i16(pkt, 333); // az + put_i16(pkt, 1); // gx + put_i16(pkt, 2); // gy + put_i16(pkt, 3); // gz + pkt.push_back(1); // ok + } + + std::vector out; + check(PacketParser::parse(pkt.data(), pkt.size(), kNumTaxels, out), "methodB parse ok"); + check(out.size() == count, "methodB sample count"); + check(out[0].taxels[0] == 0 && out[1].taxels[0] == 1, "methodB taxel"); + check(out[0].imu.az == 333 && out[0].imu.gz == 3, "methodB imu (roll/pitch skipped)"); +} + +// ---- robustness: truncated / unknown ----------------------------------------- +void test_malformed() +{ + std::vector out; + const uint8_t too_short[] = { 0x03 }; + check(!PacketParser::parse(too_short, sizeof(too_short), kNumTaxels, out), "reject 1-byte packet"); + + const uint8_t unknown_flags[] = { 0x01, 0x00, 0, 0, 0, 0 }; + check(!PacketParser::parse(unknown_flags, sizeof(unknown_flags), kNumTaxels, out), "reject unknown flags"); + + // v5 header claims 3 samples but body is missing. + std::vector truncated = { 0x03, 0x04, 0, 0, 0, 0, 0, 0, 0, 0 }; + check(!PacketParser::parse(truncated.data(), truncated.size(), kNumTaxels, out), "reject truncated v5 body"); + check(out.empty(), "out untouched on failure"); +} + +} // namespace + +int main() +{ + test_unpack_edges(); + test_packed12_v5(); + test_method_b(); + test_malformed(); + std::printf("OK: all %d checks passed\n", g_checks); + return 0; +}