diff --git a/CMakeLists.txt b/CMakeLists.txt index 87b0079b2d..06d1047a6d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -71,7 +71,7 @@ option(BUILD_PLUGIN_NOITOM_MOCAP "Build Noitom mocap plugin (downloads MocapApi option(BUILD_PLUGIN_OGLO "Build OGLO tactile glove plugin (BLE, Linux only; fetches SimpleBLE + nlohmann/json)" OFF) option(BUILD_PLUGIN_WUJI_GLOVE "Build Wuji glove plugin (requires the wuji_sdk C SDK)" OFF) option(BUILD_PLUGIN_SHARPA_AVATAR "Build Sharpa AvatarExo Glove plugin (Linux; skips if the Avatar SDK is missing)" OFF) -option(BUILD_PLUGIN_REMOTE_DEVICE_BRIDGE "Build the remote device bridge (remote_device_bridge app and server) and the RemotePluginSession library (POSIX sockets, skipped on Windows)" ON) +option(BUILD_PLUGIN_REMOTE_DEVICE_BRIDGE "Build the remote device bridge (app and server), the RemotePluginSession library and the synthetic sender; enables manus --remote (POSIX sockets, skipped on Windows)" ON) option(BUILD_EXAMPLES "Build examples" ON) option(BUILD_EXAMPLE_TELEOP_ROS2 "Build only the teleop_ros2 ROS 2 reference integration (e.g. for Docker)" OFF) option(BUILD_TESTING "Build unit tests" ON) diff --git a/docs/source/device/hand_tracking/manus.rst b/docs/source/device/hand_tracking/manus.rst index a012922758..776b93bedc 100644 --- a/docs/source/device/hand_tracking/manus.rst +++ b/docs/source/device/hand_tracking/manus.rst @@ -195,6 +195,15 @@ haptic read. Restrict datasets with ``--datasets=`` (comma-separated): ./install/plugins/manus/manus_hand_plugin --datasets=human # skeleton only ./install/plugins/manus/manus_hand_plugin --datasets=sensors # flex sensors only +To run the plugin on a machine without an OpenXR runtime, point it at a +``remote_device_bridge`` running next to the runtime with ``--remote=`` +(``haptic`` is not available over the remote session): + +.. code-block:: bash + + ./install/plugins/manus/manus_hand_plugin --remote= # human,sensors only + ./install/plugins/manus/manus_hand_plugin --remote=ws:// # bridge started with --ws-listen + 4. Load glove calibration files (optional) ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ diff --git a/src/plugins/CMakeLists.txt b/src/plugins/CMakeLists.txt index 60c15ae223..1419da534a 100644 --- a/src/plugins/CMakeLists.txt +++ b/src/plugins/CMakeLists.txt @@ -23,13 +23,13 @@ add_subdirectory(gamepad) add_subdirectory(xsens_full_body) add_subdirectory(so101_leader) add_subdirectory(rebot_devarm_leader) -add_subdirectory(manus) -add_subdirectory(manus_calibration) -add_subdirectory(haptikos) - +# Before manus: its app links Teleop::remote_plugin_session when the target exists. if(BUILD_PLUGIN_REMOTE_DEVICE_BRIDGE) add_subdirectory(remote_device_bridge) endif() +add_subdirectory(manus) +add_subdirectory(manus_calibration) +add_subdirectory(haptikos) if(BUILD_PLUGIN_NOITOM_MOCAP) add_subdirectory(noitom_mocap) diff --git a/src/plugins/manus/app/CMakeLists.txt b/src/plugins/manus/app/CMakeLists.txt index 7aa5d6fea3..7956a39a53 100644 --- a/src/plugins/manus/app/CMakeLists.txt +++ b/src/plugins/manus/app/CMakeLists.txt @@ -10,6 +10,13 @@ target_link_libraries(manus_hand_plugin manus_plugin_core ) +# --remote= needs the RemotePluginSession library (BUILD_PLUGIN_REMOTE_DEVICE_BRIDGE, non-Windows); +# without it the flag is rejected at startup. +if(TARGET Teleop::remote_plugin_session) + target_link_libraries(manus_hand_plugin PRIVATE Teleop::remote_plugin_session) + target_compile_definitions(manus_hand_plugin PRIVATE MANUS_HAVE_REMOTE_PLUGIN_SESSION) +endif() + set_target_properties(manus_hand_plugin PROPERTIES CXX_VISIBILITY_PRESET hidden VISIBILITY_INLINES_HIDDEN YES diff --git a/src/plugins/manus/app/main.cpp b/src/plugins/manus/app/main.cpp index c97426c698..1eb3f6a132 100644 --- a/src/plugins/manus/app/main.cpp +++ b/src/plugins/manus/app/main.cpp @@ -3,6 +3,10 @@ #include #include +#if defined(MANUS_HAVE_REMOTE_PLUGIN_SESSION) +# include +# include +#endif #include #include @@ -11,6 +15,8 @@ #include #include #include +#include +#include #include #include #include @@ -108,10 +114,18 @@ XrPosef parse_aim_to_wrist(const std::string& arg, spdlog::logger& logger) return pose; } -ManusPluginConfig parse_args(int argc, char** argv) +struct AppArgs { ManusPluginConfig config; - std::string datasets_arg = "human,sensors,haptic"; + //! host[:port] or ws://host[:port][/path] of a remote_device_bridge; empty selects the local OpenXR session. + std::string remote_endpoint; +}; + +AppArgs parse_args(int argc, char** argv) +{ + AppArgs args; + ManusPluginConfig& config = args.config; + std::optional datasets_arg; auto logger = isaaccapture::Logger::get("isaaccapture.plugins.manus.main"); for (int i = 1; i < argc; ++i) @@ -121,6 +135,10 @@ ManusPluginConfig parse_args(int argc, char** argv) { datasets_arg = arg.substr(std::string("--datasets=").size()); } + else if (starts_with(arg, "--remote=")) + { + args.remote_endpoint = arg.substr(std::string("--remote=").size()); + } else if (starts_with(arg, "--left-calibration=")) { config.left_calibration_file = arg.substr(std::string("--left-calibration=").size()); @@ -150,7 +168,7 @@ ManusPluginConfig parse_args(int argc, char** argv) config.human = false; config.sensors = false; config.haptic = false; - for (const auto& ds : split_csv(datasets_arg)) + for (const auto& ds : split_csv(datasets_arg.value_or("human,sensors,haptic"))) { if (ds == "human") { @@ -175,7 +193,20 @@ ManusPluginConfig parse_args(int argc, char** argv) throw std::runtime_error("ManusHandPlugin: --datasets must enable at least one of human,sensors,haptic"); } - return config; + // RemotePluginSession's pull channel exposes no trackers, so the haptic reader cannot run over --remote. + if (!args.remote_endpoint.empty() && config.haptic) + { + if (datasets_arg) + { + throw std::invalid_argument( + "ManusHandPlugin: --remote cannot be combined with the 'haptic' data set; " + "use --datasets=human,sensors"); + } + config.haptic = false; + logger->info("ManusHandPlugin: --remote given; defaulting to --datasets=human,sensors"); + } + + return args; } } // namespace @@ -186,12 +217,29 @@ try auto logger = isaaccapture::Logger::get("isaaccapture.plugins.manus.main"); logger->info("Manus Hand Plugin starting..."); - const ManusPluginConfig config = parse_args(argc, argv); + const AppArgs args = parse_args(argc, argv); + const ManusPluginConfig& config = args.config; + + // Empty for the local path: ManusTracker then opens its own OpenXR session, so this executable links no loader. + core::PluginSessionFactory remote_session_factory; + if (!args.remote_endpoint.empty()) + { +#if defined(MANUS_HAVE_REMOTE_PLUGIN_SESSION) + // Parsed here so a malformed --remote fails at startup, not when ManusTracker opens its session. + remote_session_factory = remote_plugin_session::make_remote_plugin_session_factory( + "ManusHandPlugin", remote_plugin_session::parse_bridge_remote(args.remote_endpoint)); + logger->info("Using remote plugin session at {}", args.remote_endpoint); +#else + throw std::invalid_argument( + "ManusHandPlugin: --remote requires a build with BUILD_PLUGIN_REMOTE_DEVICE_BRIDGE=ON"); +#endif + } std::signal(SIGINT, signal_handler); std::signal(SIGTERM, signal_handler); - auto& tracker = ManusTracker::instance(config); + auto& tracker = remote_session_factory ? ManusTracker::instance(config, remote_session_factory) : + ManusTracker::instance(config); logger->info("Plugin running. Press Ctrl+C to stop."); diff --git a/src/plugins/manus/core/inc/manus/manus_hand_tracking_plugin.hpp b/src/plugins/manus/core/inc/manus/manus_hand_tracking_plugin.hpp index 9147333d85..331fcec2aa 100644 --- a/src/plugins/manus/core/inc/manus/manus_hand_tracking_plugin.hpp +++ b/src/plugins/manus/core/inc/manus/manus_hand_tracking_plugin.hpp @@ -125,6 +125,8 @@ class __attribute__((visibility("default"))) ManusTracker // Hand-publishing methods void inject_hand_data(); + void log_status(); + // -- Member Variables -- ManusPluginConfig m_config; @@ -144,6 +146,11 @@ class __attribute__((visibility("default"))) ManusTracker std::vector m_right_calibration_file; std::array m_calibration_failed{ { false, false } }; bool is_connected = false; + // Last landscape as the SDK reported it (before the one-left/one-right filter); for log_status(). + uint32_t m_sdk_glove_count = 0; + std::optional m_license_ok; + static constexpr std::chrono::seconds kStatusInterval{ 5 }; + std::chrono::steady_clock::time_point m_next_status = std::chrono::steady_clock::now() + kStatusInterval; // Haptic state — the per-side log-once flags use std::atomic to stay // quiet when many frames in a row fail (e.g. the glove was disconnected diff --git a/src/plugins/manus/core/manus_hand_tracking_plugin.cpp b/src/plugins/manus/core/manus_hand_tracking_plugin.cpp index 89d475ab93..a5b2eb8ab6 100644 --- a/src/plugins/manus/core/manus_hand_tracking_plugin.cpp +++ b/src/plugins/manus/core/manus_hand_tracking_plugin.cpp @@ -141,6 +141,8 @@ ManusTracker& ManusTracker::instance(const ConstructionArgs* args) void ManusTracker::update() { + log_status(); + if (!m_plugin_session) { // The configured plugin session is unavailable, so no session-backed @@ -616,6 +618,22 @@ void ManusTracker::OnSkeletonStream(const SkeletonStreamInfo* skeleton_stream_in } } +//! Operator liveness line for the gloves and the Manus license, once per kStatusInterval. +void ManusTracker::log_status() +{ + const auto now = std::chrono::steady_clock::now(); + if (now < m_next_status) + { + return; + } + m_next_status = now + kStatusInterval; + std::lock_guard lock(landscape_mutex); + m_logger->info("{}s: gloves={} license={}", kStatusInterval.count(), m_sdk_glove_count, + !m_license_ok ? "unknown" : + *m_license_ok ? "ok" : + "missing"); +} + void ManusTracker::OnLandscapeStream(const Landscape* landscape) { auto& tracker = instance(nullptr); @@ -628,6 +646,8 @@ void ManusTracker::OnLandscapeStream(const Landscape* landscape) const auto& gloves = landscape->gloveDevices; std::lock_guard landscape_lock(tracker.landscape_mutex); + tracker.m_sdk_glove_count = gloves.gloveCount; + tracker.m_license_ok = landscape->settings.license.sdk || landscape->settings.license.integrated; // We only support one left and one right glove if (gloves.gloveCount > 2) diff --git a/src/plugins/remote_device_bridge/AGENTS.md b/src/plugins/remote_device_bridge/AGENTS.md index 64c1456c36..e49800ebbf 100644 --- a/src/plugins/remote_device_bridge/AGENTS.md +++ b/src/plugins/remote_device_bridge/AGENTS.md @@ -10,11 +10,12 @@ SPDX-License-Identifier: Apache-2.0 - `session/protocol_frame.*`, `session/protocol_payloads.*` and the `Reader`/`Writer` in `session/protocol_io.hpp` are the only serialization points. Every frame is encoded and decoded there and nowhere else; changing the wire format means changing them and their codec tests together. Sessions hold an `ITransport` (`session/inc/remote_plugin_session/transport.hpp`) and only call `send/receive/shutdown` on complete frames; TCP framing lives in `TcpTransport`. - Wire format v1 (`kProtocolVersion` = 1, the only version number): 24-byte header `RPS0` magic, type u8, version u8, channel u16, seq u32, payload size u32, `t_send` i64; frame types 1..13 with no gaps; the hand byte is 0 = left, 1 = right in every payload; PING has no payload and PONG carries the PING's header `t_send` and the client's receive time. Any change to a header, payload or type number bumps `kProtocolVersion`; `decode_header` rejects other versions before reading the type, so old senders and bridges fail with a readable reason instead of mis-decoding. - A frame's `t_send` is its send time for clock exchange: call `stamp_header` where the frame is written to the transport, never when it is queued. -- WebSocket is a second transport, not a second protocol: `WebSocketTransport` carries exactly one wire frame per binary message (no TLS; terminate `wss://` in a proxy). Clients pick it with a `ws://host:port[/path]` address via `parse_remote`/`connect_transport`. The bridge serves it on a separate `--ws-listen` port sharing the fork-per-client loop and `--max-clients`; never sniff the protocol on the TCP port. The server-side HTTP upgrade runs in the first `receive()` so an accept loop never blocks on a client. +- WebSocket is a second transport, not a second protocol: `WebSocketTransport` carries exactly one wire frame per binary message (no TLS; terminate `wss://` in a proxy). Clients pick it with `--remote=ws://host:port[/path]` via `parse_remote`/`connect_transport`. The bridge serves it on a separate `--ws-listen` port sharing the fork-per-client loop and `--max-clients`; never sniff the protocol on the TCP port. The server-side HTTP upgrade runs in the first `receive()` so an accept loop never blocks on a client. - Never add `OpenXR::openxr_loader`, `Teleop::plugin_utils` or DeviceIO session libraries to `Teleop::remote_plugin_session`: a plugin that talks to a bridge through this library must run without an OpenXR runtime. - `Teleop::remote_device_bridge_server` (`server/`) gets each client's session from a `SessionFactory`; only `app/main.cpp` builds the OpenXR session, with `plugin_utils::make_openxr_plugin_session_factory`. Keep the OpenXR loader and `plugin_utils` out of `server/` so `tests/cpp/plugins/remote_device_bridge_server` can run the real fork-per-client bridge against a fake session. - ERROR on channel 0 ends the session; ERROR on a channel id refuses only that OPEN (schema above `--max-schema-bytes`, a hand someone else owns) and the session and its other channels continue. Refuse per channel whenever the session itself is still sound. - `RemotePluginSession` never throws on the sample path for transport failures (`push`, `push_buffer`, `query`, `update`): after one they become no-ops; only channel creation and the constructor throw. Caller bugs (null joints or buffer) still throw `std::invalid_argument`. - The bridge (`server/`) talks to OpenXR from one thread per client (the thread running that `ClientSession`). Socket reading happens on a helper thread that only parses frames into a queue. Keep it that way; `OpenXRPluginSession` and its channels are not thread-safe. +- Plugins are not modified for remoting: only the composition root (`main.cpp`) chooses the `core::PluginSessionFactory` (`remote_plugin_session::make_remote_plugin_session_factory` or `plugin_utils::make_openxr_plugin_session_factory`). Do not add transport branches inside plugin classes. - Timestamps cross the wire in the sender's `CLOCK_MONOTONIC`; the receiver maps them with `ClockOffsetEstimator`. The bridge's offset comes from PING/PONG (the minimum-RTT recent exchange, `--clock-source=two-way`); the one-way minimum of `local_recv - remote_send` hides the minimum network delay and is only the fallback (no PONG in time) or the `--clock-source=one-way` A/B baseline. Do not push raw remote timestamps into OpenXR. - One bridge process serves N clients. The accept loop never creates threads or OpenXR objects so it may `fork()` a child per client (the isolation of N local plugin processes). Do not serve clients on threads: the OpenXR loader allows one `XrInstance` per process (a second one fails with -10). State the children must share (hand ownership) lives in a `MAP_SHARED` page mapped before the first fork; the parent clears a dead child's entries when it reaps it. Sample paths are lossy by design: per-channel latest-wins queues on the client, coalescing and a staleness guard on the bridge; control frames are never dropped or reordered. diff --git a/src/plugins/remote_device_bridge/CMakeLists.txt b/src/plugins/remote_device_bridge/CMakeLists.txt index 3df03b6500..b5389ea01b 100644 --- a/src/plugins/remote_device_bridge/CMakeLists.txt +++ b/src/plugins/remote_device_bridge/CMakeLists.txt @@ -9,6 +9,7 @@ endif() add_subdirectory(session) add_subdirectory(server) add_subdirectory(app) +add_subdirectory(tools) install(FILES plugin.yaml DESTINATION plugins/remote_device_bridge diff --git a/src/plugins/remote_device_bridge/tools/CMakeLists.txt b/src/plugins/remote_device_bridge/tools/CMakeLists.txt new file mode 100644 index 0000000000..1353c1235e --- /dev/null +++ b/src/plugins/remote_device_bridge/tools/CMakeLists.txt @@ -0,0 +1,22 @@ +# SPDX-FileCopyrightText: Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +include(GNUInstallDirs) + +add_executable(remote_hand_sender_synthetic + remote_hand_sender_synthetic.cpp +) + +target_link_libraries(remote_hand_sender_synthetic + PRIVATE + Teleop::remote_plugin_session + log_bridge::log_bridge_core +) + +set_target_properties(remote_hand_sender_synthetic PROPERTIES + RUNTIME_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/bin" +) + +install(TARGETS remote_hand_sender_synthetic + RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} +) diff --git a/src/plugins/remote_device_bridge/tools/latency_breakdown.py b/src/plugins/remote_device_bridge/tools/latency_breakdown.py new file mode 100755 index 0000000000..929be89a54 --- /dev/null +++ b/src/plugins/remote_device_bridge/tools/latency_breakdown.py @@ -0,0 +1,299 @@ +#!/usr/bin/env python3 +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""Capture-to-consumer latency breakdown from a bridge --stats-csv and a latency_probe_xrclient.py CSV. + +Both CSVs must come from the same box (one CLOCK_MONOTONIC). The consumer stage is the lag of the +probe's curl behind the bridge's pushed curl, so it needs a step test (fast finger flexes). (c) and (d) +are only reported when enough windows correlate (--min-xcorr) and enough step edges pair (--min-edges). +""" + +from __future__ import annotations + +import argparse +import csv +import sys +from dataclasses import dataclass + +import numpy as np + +GRID_NS = 1_000_000 # resampling step for the curl signals +WINDOW_NS = 10_000_000_000 +WINDOW_STEP_NS = 5_000_000_000 +MAX_LAG_NS = 500_000_000 +MIN_LAG_NS = -50_000_000 +EDGE_SMOOTH_NS = 10_000_000 +EDGE_MIN_GAP_NS = 200_000_000 +EDGE_THRESHOLD = 0.3 # fraction of the largest |d curl/dt| +EDGE_MAD_K = 3.0 # edge pairs outside median +- K * MAD are discarded +DEFAULT_MIN_XCORR = 0.6 +DEFAULT_MIN_EDGES = 3 +MIN_OVERLAP_NS = 2_000_000_000 +NOT_ESTIMATED = "consumer lag: not estimated (low correlation / too few edges)" + + +@dataclass(frozen=True) +class Breakdown: + """Stage latencies in ms; consumer_p50 and total_p50 are NaN unless `estimated`.""" + + network_p50: float + network_p99: float + bridge_p50: float + bridge_p99: float + consumer_p50: float + consumer_windows: int + windows_total: int + xcorr_median: float + edge_delays_ms: list[float] + edges_paired: int + edges_discarded: int + edge_p50: float + estimated: bool + offset_ms: float + total_p50: float + samples: int + + +class InputError(Exception): + """Input the breakdown cannot use (no rows, no finite curl, too little overlap).""" + + +def load_bridge(path: str, side: str) -> dict[str, np.ndarray]: + with open(path, newline="") as f: + rows = [r for r in csv.DictReader(f) if r["side"] == side] + cols = ("push_mono_ns", "recv_mono_ns", "capture_sender_ns", "capture_mapped_ns") + out = {c: np.array([int(r[c]) for r in rows], dtype=np.int64) for c in cols} + out["curl"] = np.array([float(r["curl"]) for r in rows]) + return out + + +def load_probe(path: str, side: str) -> dict[str, np.ndarray]: + with open(path, newline="") as f: + rows = [r for r in csv.DictReader(f) if r["side"] == side and r["curl"] != ""] + out = { + "mono_ns": np.array([int(r["mono_ns"]) for r in rows], dtype=np.int64), + "curl": np.array([float(r["curl"]) for r in rows]), + } + finite = np.isfinite(out["curl"]) + return {k: v[finite] for k, v in out.items()} + + +def check_inputs(bridge: dict[str, np.ndarray], probe: dict[str, np.ndarray]) -> None: + if len(bridge["push_mono_ns"]) == 0: + raise InputError("no bridge rows for this side") + if not np.isfinite(bridge["curl"]).any(): + raise InputError("bridge curl is all NaN for this side") + if len(probe["mono_ns"]) == 0: + raise InputError("no probe rows with curl for this side") + overlap_ns = min(bridge["push_mono_ns"].max(), probe["mono_ns"].max()) - max( + bridge["push_mono_ns"].min(), probe["mono_ns"].min() + ) + if overlap_ns < MIN_OVERLAP_NS: + raise InputError( + f"bridge/probe overlap is {max(overlap_ns, 0) / 1e9:.2f} s, need {MIN_OVERLAP_NS / 1e9:.0f} s" + ) + + +def hold(t_ns: np.ndarray, values: np.ndarray, grid_ns: np.ndarray) -> np.ndarray: + """Zero-order hold of (t, value) onto grid; grid points before the first sample take the first value.""" + order = np.argsort(t_ns, kind="stable") + idx = np.searchsorted(t_ns[order], grid_ns, side="right") - 1 + return values[order][np.clip(idx, 0, len(order) - 1)] + + +def xcorr_lag_ns(ref: np.ndarray, sig: np.ndarray) -> tuple[float, float] | None: + """(lag ns, peak score) of sig behind ref maximising normalized cross-correlation; None for a flat window.""" + lags = np.arange(MIN_LAG_NS // GRID_NS, MAX_LAG_NS // GRID_NS + 1) + n = len(ref) - lags.max() + min(lags.min(), 0) + if n <= 0: + return None + start = -min(lags.min(), 0) + a = ref[start : start + n] - ref[start : start + n].mean() + if a.std() == 0: + return None + best, best_score = None, -np.inf + for lag in lags: + b = sig[start + lag : start + lag + n] + b = b - b.mean() + denom = np.sqrt((a * a).sum() * (b * b).sum()) + if denom == 0: + continue + score = (a * b).sum() / denom + if score > best_score: + best, best_score = lag, score + return None if best is None else (float(best * GRID_NS), float(best_score)) + + +def edges(grid_ns: np.ndarray, signal: np.ndarray) -> tuple[np.ndarray, np.ndarray]: + """Times and signs of |d curl/dt| peaks (smoothed derivative above EDGE_THRESHOLD of its max).""" + k = max(1, EDGE_SMOOTH_NS // GRID_NS) + deriv = (signal[k:] - signal[:-k]) / k + mag = np.abs(deriv) + if len(mag) == 0 or mag.max() == 0: + return np.array([], dtype=np.int64), np.array([]) + above = mag > EDGE_THRESHOLD * mag.max() + times, signs = [], [] + i = 0 + while i < len(mag): + if not above[i]: + i += 1 + continue + j = i + while j < len(mag) and above[j]: + j += 1 + peak = i + int(np.argmax(mag[i:j])) + t = grid_ns[peak] + (k * GRID_NS) // 2 + if not times or t - times[-1] >= EDGE_MIN_GAP_NS: + times.append(t) + signs.append(np.sign(deriv[peak])) + i = j + return np.array(times, dtype=np.int64), np.array(signs) + + +def pct(values: np.ndarray, q: float) -> float: + return float(np.percentile(values, q)) if len(values) else float("nan") + + +def breakdown( + bridge: dict[str, np.ndarray], + probe: dict[str, np.ndarray], + min_xcorr: float = DEFAULT_MIN_XCORR, + min_edges: int = DEFAULT_MIN_EDGES, +) -> Breakdown: + """Raises InputError for unusable input.""" + check_inputs(bridge, probe) + network_ms = (bridge["recv_mono_ns"] - bridge["capture_mapped_ns"]) / 1e6 + bridge_ms = (bridge["push_mono_ns"] - bridge["recv_mono_ns"]) / 1e6 + offset_ms = (bridge["capture_mapped_ns"] - bridge["capture_sender_ns"]) / 1e6 + + t0 = max(bridge["push_mono_ns"].min(), probe["mono_ns"].min()) + t1 = min(bridge["push_mono_ns"].max(), probe["mono_ns"].max()) + grid = np.arange(t0, t1, GRID_NS, dtype=np.int64) + finite = np.isfinite(bridge["curl"]) + ref = hold(bridge["push_mono_ns"][finite], bridge["curl"][finite], grid) + sig = hold(probe["mono_ns"], probe["curl"], grid) + + window = WINDOW_NS // GRID_NS + step = WINDOW_STEP_NS // GRID_NS + window_lags, scores = [], [] + windows_total = 0 + for s in range(0, max(1, len(grid) - window + 1), step): + windows_total += 1 + result = xcorr_lag_ns(ref[s : s + window], sig[s : s + window]) + if result is None: + continue + scores.append(result[1]) + if result[1] >= min_xcorr: + window_lags.append(result[0] / 1e6) + + ref_t, ref_sign = edges(grid, ref) + sig_t, sig_sign = edges(grid, sig) + edge_delays = [] + for t, sign in zip(ref_t, ref_sign): + candidates = ( + (sig_sign == sign) & (sig_t >= t + MIN_LAG_NS) & (sig_t <= t + MAX_LAG_NS) + ) + if candidates.any(): + delays = sig_t[candidates] - t + edge_delays.append(delays[np.argmin(np.abs(delays))] / 1e6) + + # The MAD floor is the probe period: edge times are quantized to it. + edge_ms = np.array(edge_delays) + edges_paired = len(edge_ms) + if edges_paired: + probe_period_ms = float(np.median(np.diff(np.unique(probe["mono_ns"])))) / 1e6 + med = np.median(edge_ms) + mad = max(float(np.median(np.abs(edge_ms - med))), probe_period_ms) + edge_ms = edge_ms[np.abs(edge_ms - med) <= EDGE_MAD_K * mad] + + estimated = len(window_lags) > 0 and len(edge_ms) >= min_edges + consumer_ms = np.array(window_lags) + consumer_p50 = pct(consumer_ms, 50) if estimated else float("nan") + return Breakdown( + network_p50=pct(network_ms, 50), + network_p99=pct(network_ms, 99), + bridge_p50=pct(bridge_ms, 50), + bridge_p99=pct(bridge_ms, 99), + consumer_p50=consumer_p50, + consumer_windows=len(window_lags), + windows_total=windows_total, + xcorr_median=pct(np.array(scores), 50), + edge_delays_ms=edge_ms.tolist(), + edges_paired=edges_paired, + edges_discarded=edges_paired - len(edge_ms), + edge_p50=pct(edge_ms, 50), + estimated=estimated, + offset_ms=pct(offset_ms, 50), + total_p50=pct(network_ms, 50) + pct(bridge_ms, 50) + consumer_p50, + samples=len(bridge["push_mono_ns"]), + ) + + +def main(argv: list[str] | None = None) -> int: + parser = argparse.ArgumentParser(description=__doc__.splitlines()[0]) + parser.add_argument( + "--bridge", required=True, help="remote_device_bridge --stats-csv output" + ) + parser.add_argument( + "--probe", required=True, help="latency_probe_xrclient.py output" + ) + parser.add_argument("--side", default="right", choices=("left", "right")) + parser.add_argument( + "--min-xcorr", + type=float, + default=DEFAULT_MIN_XCORR, + help="windows whose correlation peak is lower are not used for (c) (default %(default)s)", + ) + parser.add_argument( + "--min-edges", + type=int, + default=DEFAULT_MIN_EDGES, + help="step edges that must pair for (c) to be reported (default %(default)s)", + ) + args = parser.parse_args(argv) + + try: + r = breakdown( + load_bridge(args.bridge, args.side), + load_probe(args.probe, args.side), + args.min_xcorr, + args.min_edges, + ) + except InputError as e: + print(f"latency_breakdown: side={args.side}: {e}", file=sys.stderr) + return 2 + edges_ms = ", ".join(f"{d:.0f}" for d in r.edge_delays_ms[:20]) + print( + f"side={args.side} bridge samples={r.samples} offset_ms={r.offset_ms:.3f} (capture_mapped - capture_sender)" + ) + print(f"{'stage':<40}{'p50 ms':>10}{'p99 ms':>10}") + print( + f"{'(a) network+queue recv - capture_mapped':<40}{r.network_p50:>10.1f}{r.network_p99:>10.1f}" + ) + print( + f"{'(b) bridge push - recv':<40}{r.bridge_p50:>10.1f}{r.bridge_p99:>10.1f}" + ) + print( + f"xcorr windows used {r.consumer_windows}/{r.windows_total} (peak >= {args.min_xcorr:.2f})," + f" median peak {r.xcorr_median:.2f}" + ) + print( + f"step edges paired {r.edges_paired}, discarded {r.edges_discarded} (outside median +- {EDGE_MAD_K:g} MAD)," + f" kept {len(r.edge_delays_ms)} (need {args.min_edges})" + ) + if not r.estimated: + print(NOT_ESTIMATED) + return 0 + print( + f"{'(c) consumer xcorr probe vs bridge':<40}{r.consumer_p50:>10.1f}{'':>10} ({r.consumer_windows} x {WINDOW_NS / 1e9:g} s windows)" + ) + print( + f"{' step edges probe - bridge':<40}{r.edge_p50:>10.1f}{'':>10} n={len(r.edge_delays_ms)}: {edges_ms}" + ) + print(f"{'(d) total (a)+(b)+(c)':<40}{r.total_p50:>10.1f}") + return 0 + + +if __name__ == "__main__": + sys.exit(main()) diff --git a/src/plugins/remote_device_bridge/tools/latency_probe_xrclient.py b/src/plugins/remote_device_bridge/tools/latency_probe_xrclient.py new file mode 100755 index 0000000000..dc3b1dcdf5 --- /dev/null +++ b/src/plugins/remote_device_bridge/tools/latency_probe_xrclient.py @@ -0,0 +1,109 @@ +#!/usr/bin/env python3 +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""Sample the hand as a consumer sees it (gr00t XRClient) into a CSV for latency_breakdown.py. + +Runs inside the gr00t venv. mono_ns is time.monotonic_ns(), i.e. CLOCK_MONOTONIC, the clock of the +bridge's --stats-csv. curl is |slot 9 - slot 0| of get_hand_fingertips(): XRSnapshot slot = OpenXR +joint - 1 (OXR_TO_FINGERTIP_SLOT), so these are XR_HAND_JOINT_WRIST_EXT (1) and +XR_HAND_JOINT_INDEX_TIP_EXT (10), the joints of the bridge's index_curl_m. +""" + +from __future__ import annotations + +import argparse +import csv +import math +import time + +import numpy as np + +WRIST_SLOT = 0 +INDEX_TIP_SLOT = 9 + + +def fingertips_curl(joints: np.ndarray | None) -> tuple[float, int]: + """(curl in m or NaN, number of non-identity slots) of one get_hand_fingertips() array. + + An identity slot is one the server did not fill. The wrist is the exception: without a headset + wrist pose the Manus root is the origin, so a tracked wrist is exactly eye(4). Like the bridge's + index_curl_m, curl then only needs the index tip. + """ + if joints is None: + return math.nan, 0 + is_set = ~np.all(joints == np.eye(4), axis=(1, 2)) + if not is_set[INDEX_TIP_SLOT]: + return math.nan, int(is_set.sum()) + curl = float( + np.linalg.norm(joints[INDEX_TIP_SLOT, :3, 3] - joints[WRIST_SLOT, :3, 3]) + ) + return curl if math.isfinite(curl) else math.nan, int(is_set.sum()) + + +def main() -> None: + parser = argparse.ArgumentParser(description=__doc__.splitlines()[0]) + parser.add_argument( + "--out", required=True, help="CSV path (mono_ns,side,curl,n_set)" + ) + parser.add_argument( + "--hz", type=float, default=200.0, help="sample rate (default %(default)s)" + ) + parser.add_argument( + "--seconds", type=float, default=60.0, help="duration (default %(default)s)" + ) + parser.add_argument( + "--sides", default="left,right", help="comma list (default %(default)s)" + ) + parser.add_argument( + "--host", default="127.0.0.1", help="xr_server host (default %(default)s)" + ) + parser.add_argument( + "--port", type=int, default=None, help="xr_server port (default XRClient's)" + ) + args = parser.parse_args() + + from groot.core.robotics.teleop.xr.xr_client import XRClient + + sides = [side for side in args.sides.split(",") if side] + client = ( + XRClient(host=args.host) + if args.port is None + else XRClient(host=args.host, port=args.port) + ) + client.start_streaming() + period_ns = int(1e9 / args.hz) + rows = 0 + with_curl = 0 + try: + with open(args.out, "w", newline="") as out: + writer = csv.writer(out) + writer.writerow(["mono_ns", "side", "curl", "n_set"]) + start = time.monotonic_ns() + next_ns = start + end = start + int(args.seconds * 1e9) + while next_ns < end: + time.sleep(max(0.0, (next_ns - time.monotonic_ns()) / 1e9)) + for side in sides: + mono_ns = time.monotonic_ns() + joints = client.get_hand_fingertips(side) + curl, n_set = fingertips_curl(joints) + if not math.isnan(curl): + with_curl += 1 + writer.writerow( + [ + mono_ns, + side, + "" if math.isnan(curl) else f"{curl:.5f}", + n_set, + ] + ) + rows += 1 + next_ns += period_ns + finally: + client.close() + print(f"latency_probe_xrclient: {rows} rows ({with_curl} with curl) -> {args.out}") + + +if __name__ == "__main__": + main() diff --git a/src/plugins/remote_device_bridge/tools/remote_hand_sender_synthetic.cpp b/src/plugins/remote_device_bridge/tools/remote_hand_sender_synthetic.cpp new file mode 100644 index 0000000000..4a83856ada --- /dev/null +++ b/src/plugins/remote_device_bridge/tools/remote_hand_sender_synthetic.cpp @@ -0,0 +1,402 @@ +// SPDX-FileCopyrightText: Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +// Stands in for a hand plugin: streams two synthetic hands over RemotePluginSession +// so the bridge loopback can be exercised without glove hardware. --schema-bytes adds +// a schema channel pushing fixed-size payloads at its own rate (bandwidth/rate load). +// --hands=0 / --wrist=0 drop the hand push channels / wrist pull so N clients can carry +// distinct device types (the runtime allows one push-device owner per hand). +// +// Its terminal output is the product (connect banner, the 1 s "[sender]" line, the stop summary), so it stays on +// std::cout like manus_hand_tracker_printer; warnings and the fatal handlers go to the logger. + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace +{ + +static_assert(ATOMIC_BOOL_LOCK_FREE == 2, "lock-free atomic bool is required for signal safety"); + +std::atomic g_stop_requested{ false }; + +std::shared_ptr logger() +{ + return isaaccapture::Logger::get("isaaccapture.plugins.remote_device_bridge.remote_hand_sender_synthetic"); +} + +void signal_handler(int signal) +{ + if (signal == SIGINT || signal == SIGTERM) + { + g_stop_requested.store(true, std::memory_order_relaxed); + } +} + +struct Options +{ + remote_plugin_session::RemoteAddress remote{ { "127.0.0.1", remote_plugin_session::kDefaultBridgePort } }; + int rate_hz = 90; + double duration_s = 0.0; // 0 = run until Ctrl-C + size_t schema_bytes = 0; // 0 = no schema channel + int schema_rate_hz = 180; + bool hands = true; + bool wrist = true; +}; + +bool parse_bool(const std::string& arg, const std::string& value) +{ + if (value != "0" && value != "1") + { + throw std::invalid_argument(arg + " expects 0 or 1"); + } + return value == "1"; +} + +std::string mode_string(const Options& options) +{ + std::string mode; + int count = 0; + for (const auto& [on, name] : { std::pair{ options.hands, "hands" }, std::pair{ options.wrist, "wrist" }, + std::pair{ options.schema_bytes > 0, "schema" } }) + { + if (on) + { + mode += (count++ == 0 ? "" : "+") + std::string(name); + } + } + return count == 1 ? mode + "-only" : mode; +} + +Options parse_args(int argc, char** argv) +{ + Options options; + for (int i = 1; i < argc; ++i) + { + const std::string arg = argv[i]; + if (arg.starts_with("--remote=")) + { + options.remote = remote_plugin_session::parse_bridge_remote(arg.substr(std::string("--remote=").size())); + } + else if (arg.starts_with("--rate-hz=")) + { + options.rate_hz = std::stoi(arg.substr(std::string("--rate-hz=").size())); + } + else if (arg.starts_with("--duration-s=")) + { + options.duration_s = std::stod(arg.substr(std::string("--duration-s=").size())); + } + else if (arg.starts_with("--schema-bytes=")) + { + options.schema_bytes = std::stoul(arg.substr(std::string("--schema-bytes=").size())); + } + else if (arg.starts_with("--schema-rate-hz=")) + { + options.schema_rate_hz = std::stoi(arg.substr(std::string("--schema-rate-hz=").size())); + } + else if (arg.starts_with("--hands=")) + { + options.hands = parse_bool("--hands", arg.substr(std::string("--hands=").size())); + } + else if (arg.starts_with("--wrist=")) + { + options.wrist = parse_bool("--wrist", arg.substr(std::string("--wrist=").size())); + } + else if (arg == "--help" || arg == "-h") + { + std::cout + << "Usage: remote_hand_sender_synthetic [--remote=|ws://[/path]] [--rate-hz=] " + "[--duration-s=] [--schema-bytes=] [--schema-rate-hz=] [--hands=0|1] " + "[--wrist=0|1]\n"; + std::exit(0); + } + else + { + throw std::invalid_argument("unknown argument '" + arg + "'"); + } + } + if (options.rate_hz <= 0 || options.schema_rate_hz <= 0) + { + throw std::invalid_argument("--rate-hz and --schema-rate-hz must be positive"); + } + if (!options.hands && !options.wrist && options.schema_bytes == 0) + { + throw std::invalid_argument("nothing to send: enable --hands=1, --wrist=1 or --schema-bytes="); + } + return options; +} + +XrQuaternionf quat_multiply(const XrQuaternionf& a, const XrQuaternionf& b) +{ + return { a.w * b.x + a.x * b.w + a.y * b.z - a.z * b.y, a.w * b.y - a.x * b.z + a.y * b.w + a.z * b.x, + a.w * b.z + a.x * b.y - a.y * b.x + a.z * b.w, a.w * b.w - a.x * b.x - a.y * b.y - a.z * b.z }; +} + +XrVector3f quat_rotate(const XrQuaternionf& q, const XrVector3f& v) +{ + const float t1 = 2.0f * (q.y * v.z - q.z * v.y); + const float t2 = 2.0f * (q.z * v.x - q.x * v.z); + const float t3 = 2.0f * (q.x * v.y - q.y * v.x); + return { v.x + q.w * t1 + (q.y * t3 - q.z * t2), v.y + q.w * t2 + (q.z * t1 - q.x * t3), + v.z + q.w * t3 + (q.x * t2 - q.y * t1) }; +} + +XrQuaternionf quat_from_axis_angle(const XrVector3f& axis, float angle) +{ + const float half = 0.5f * angle; + const float s = std::sin(half); + return { axis.x * s, axis.y * s, axis.z * s, std::cos(half) }; +} + +XrPosef compose(const XrPosef& parent, const XrPosef& local) +{ + const XrVector3f p = quat_rotate(parent.orientation, local.position); + return { quat_multiply(parent.orientation, local.orientation), + { parent.position.x + p.x, parent.position.y + p.y, parent.position.z + p.z } }; +} + +// Resting hand in the wrist frame (OpenXR: +X right of the left palm, +Y back of hand, -Z fingers). +// Each finger is a chain of segments along -Z; curl pitches every segment about +X towards the palm. +struct FingerLayout +{ + int first_joint; // the finger's metacarpal joint index in XrHandJointEXT order + int segments; // number of segments after the first joint + XrVector3f base; + std::array lengths; + float yaw; // spread about +Y, radians +}; + +constexpr std::array kFingers = { { + { XR_HAND_JOINT_THUMB_METACARPAL_EXT, 3, { 0.020f, -0.005f, -0.020f }, { 0.040f, 0.032f, 0.028f, 0.0f }, 0.9f }, + { XR_HAND_JOINT_INDEX_METACARPAL_EXT, 4, { 0.018f, 0.0f, -0.020f }, { 0.060f, 0.040f, 0.025f, 0.022f }, 0.12f }, + { XR_HAND_JOINT_MIDDLE_METACARPAL_EXT, 4, { 0.005f, 0.0f, -0.020f }, { 0.062f, 0.044f, 0.028f, 0.024f }, 0.0f }, + { XR_HAND_JOINT_RING_METACARPAL_EXT, 4, { -0.010f, 0.0f, -0.020f }, { 0.058f, 0.040f, 0.026f, 0.022f }, -0.10f }, + { XR_HAND_JOINT_LITTLE_METACARPAL_EXT, 4, { -0.024f, 0.0f, -0.022f }, { 0.050f, 0.032f, 0.020f, 0.020f }, -0.22f }, +} }; + +float joint_radius(int joint) +{ + if (joint <= XR_HAND_JOINT_WRIST_EXT) + { + return 0.015f; + } + switch (joint) + { + case XR_HAND_JOINT_THUMB_TIP_EXT: + case XR_HAND_JOINT_INDEX_TIP_EXT: + case XR_HAND_JOINT_MIDDLE_TIP_EXT: + case XR_HAND_JOINT_RING_TIP_EXT: + case XR_HAND_JOINT_LITTLE_TIP_EXT: + return 0.006f; + default: + return 0.009f; + } +} + +void generate_hand(XrHandJointLocationEXT* joints, const XrPosef& wrist, bool is_left, float curl) +{ + const float mirror = is_left ? 1.0f : -1.0f; + const XrPosef identity{ { 0.0f, 0.0f, 0.0f, 1.0f }, { 0.0f, 0.0f, 0.0f } }; + + std::array local; + local.fill(identity); + local[XR_HAND_JOINT_PALM_EXT].position = { 0.0f, 0.0f, -0.045f }; + + for (const FingerLayout& finger : kFingers) + { + const bool is_thumb = finger.first_joint == XR_HAND_JOINT_THUMB_METACARPAL_EXT; + XrPosef cursor{ quat_from_axis_angle({ 0.0f, 1.0f, 0.0f }, mirror * finger.yaw), + { mirror * finger.base.x, finger.base.y, finger.base.z } }; + local[finger.first_joint] = cursor; + for (int s = 0; s < finger.segments; ++s) + { + // Curl is applied progressively along the chain so tips bend most. + const float pitch = is_thumb ? 0.35f * curl : (0.55f + 0.20f * static_cast(s)) * curl; + const XrQuaternionf bend = quat_from_axis_angle({ 1.0f, 0.0f, 0.0f }, -pitch); + cursor = compose(cursor, XrPosef{ bend, { 0.0f, 0.0f, -finger.lengths[static_cast(s)] } }); + local[finger.first_joint + 1 + s] = cursor; + } + } + + for (int j = 0; j < XR_HAND_JOINT_COUNT_EXT; ++j) + { + joints[j].pose = compose(wrist, local[static_cast(j)]); + joints[j].radius = joint_radius(j); + joints[j].locationFlags = XR_SPACE_LOCATION_ORIENTATION_VALID_BIT | XR_SPACE_LOCATION_POSITION_VALID_BIT | + XR_SPACE_LOCATION_ORIENTATION_TRACKED_BIT | XR_SPACE_LOCATION_POSITION_TRACKED_BIT; + } +} + +} // namespace + +int main(int argc, char** argv) +try +{ + const Options options = parse_args(argc, argv); + + std::signal(SIGINT, signal_handler); + std::signal(SIGTERM, signal_handler); + + std::cout << "remote_hand_sender_synthetic: connecting to " << (options.remote.ws_path ? "ws://" : "") + << options.remote.endpoint.host << ":" << options.remote.endpoint.port + << options.remote.ws_path.value_or("") << std::endl; + remote_plugin_session::RemotePluginSession session( + remote_plugin_session::connect_transport(options.remote), "remote_hand_sender_synthetic", + core::PluginSessionRequirements{ + .schema_push = options.schema_bytes > 0, + .hand_tracking_push = options.hands, + .wrist_tracking_pull = options.wrist ? std::optional(core::WristTrackingSourceConfig{}) : std::nullopt }); + + // Session-owned; null when --wrist=0. + core::IPluginPullChannel* pull_channel = options.wrist ? &session.pull_channel() : nullptr; + core::IWristTrackingSource* wrist_source = pull_channel ? &pull_channel->wrist_tracking_source() : nullptr; + std::unique_ptr left_pusher; + std::unique_ptr right_pusher; + if (options.hands) + { + left_pusher = session.create_hand_tracking_push_channel(XR_HAND_LEFT_EXT); + right_pusher = session.create_hand_tracking_push_channel(XR_HAND_RIGHT_EXT); + } + std::unique_ptr schema_channel; + std::vector schema_payload(options.schema_bytes, 0x5A); + if (options.schema_bytes > 0) + { + core::SchemaPusherConfig config; + config.collection_id = "synthetic_tactile"; + config.max_flatbuffer_size = options.schema_bytes; + if (options.schema_bytes > remote_plugin_session::kDefaultMaxSchemaBytes) + { + logger()->warn( + "--schema-bytes={} exceeds the bridge's default cap ({} bytes); expect a 'bridge error: ... " + "channel refused' disconnect", + options.schema_bytes, remote_plugin_session::kDefaultMaxSchemaBytes); + } + schema_channel = session.create_schema_push_channel(config); + } + std::cout << "connected; mode=" << mode_string(options) << ", tick " << options.rate_hz << " Hz"; + if (schema_channel) + { + std::cout << " + " << options.schema_bytes << " B schema samples at " << options.schema_rate_hz << " Hz"; + } + std::cout << std::endl; + + const auto period = std::chrono::nanoseconds(1'000'000'000 / options.rate_hz); + const auto schema_period = std::chrono::nanoseconds(1'000'000'000 / options.schema_rate_hz); + const auto start = std::chrono::steady_clock::now(); + auto next_tick = start; + auto next_schema = start; + auto next_status = start + std::chrono::seconds(1); + uint64_t frames_sent = 0; + uint64_t schema_sent = 0; + int exit_code = 0; + + XrHandJointLocationEXT joints[XR_HAND_JOINT_COUNT_EXT]; + while (!g_stop_requested.load(std::memory_order_relaxed)) + { + const auto now = std::chrono::steady_clock::now(); + const double elapsed_s = std::chrono::duration(now - start).count(); + if (options.duration_s > 0.0 && elapsed_s >= options.duration_s) + { + break; + } + if (!session.connected()) + { + logger()->error("session disconnected; exiting"); + exit_code = 1; + break; + } + if (schema_channel && now >= next_schema) + { + std::memcpy(schema_payload.data(), &schema_sent, std::min(sizeof(schema_sent), schema_payload.size())); + const int64_t t_ns = core::os_monotonic_now_ns(); + schema_channel->push_buffer(schema_payload.data(), schema_payload.size(), t_ns, t_ns); + ++schema_sent; + next_schema += schema_period; + } + if (now < next_tick) + { + std::this_thread::sleep_until(schema_channel ? std::min(next_tick, next_schema) : next_tick); + continue; + } + + if (pull_channel) + { + pull_channel->update(); + } + const int64_t sample_time_local_common_clock_ns = core::os_monotonic_now_ns(); + // Slow sine (4 s period) so a viewer sees the fingers open and close. + const float curl = 0.5f + 0.5f * static_cast(std::sin(elapsed_s * 2.0 * std::numbers::pi / 4.0)); + + core::WristTrackingSample wrist[2]; + for (const bool is_left : { true, false }) + { + core::WristTrackingSample& sample = wrist[is_left ? 0 : 1]; + sample = wrist_source ? wrist_source->query(is_left ? XR_HAND_LEFT_EXT : XR_HAND_RIGHT_EXT, + sample_time_local_common_clock_ns) : + core::WristTrackingSample{}; + if (!options.hands) + { + continue; + } + // Fallback when the bridge has no wrist pose yet: hands a little apart in front of the user. + const XrPosef pose = sample.valid ? + sample.pose : + XrPosef{ { 0.0f, 0.0f, 0.0f, 1.0f }, { is_left ? -0.15f : 0.15f, 1.2f, -0.4f } }; + generate_hand(joints, pose, is_left, curl); + (is_left ? left_pusher : right_pusher)->push(joints, sample_time_local_common_clock_ns); + } + frames_sent += options.hands ? 1 : 0; + + if (now >= next_status) + { + std::cout << "[sender] mode=" << mode_string(options) << " frames=" << frames_sent + << " schema=" << schema_sent; + if (wrist_source) + { + std::cout << " wrist L(valid=" << wrist[0].valid << ",tracked=" << wrist[0].tracked + << ") R(valid=" << wrist[1].valid << ",tracked=" << wrist[1].tracked << ")"; + } + else + { + std::cout << " wrist=off"; + } + std::cout << " offset_ns=" << session.clock_offset_ns() << std::endl; + next_status += std::chrono::seconds(1); + } + + next_tick += period; + } + + std::cout << "stopping after " << frames_sent << " frames, " << schema_sent << " schema samples" << std::endl; + return exit_code; +} +catch (const std::exception& e) +{ + logger()->error("{}: {}", argv[0], e.what()); + return 1; +} +catch (...) +{ + logger()->error("{}: Unknown error occurred", argv[0]); + return 1; +} diff --git a/tests/AGENTS.md b/tests/AGENTS.md index 207effceb4..281ae3f302 100644 --- a/tests/AGENTS.md +++ b/tests/AGENTS.md @@ -24,7 +24,7 @@ Organize by **product area**, mirroring `src/` and `examples/`: |------|----------|-------------| | core | `tests/cpp/core//` | `tests/python/core//` | | viz | `tests/cpp/viz//` | `tests/python/viz/` | -| plugins | `tests/cpp/plugins//` | — | +| plugins | `tests/cpp/plugins//` | `tests/python/plugins//` (plugin `tools/` scripts) | | examples | — | `tests/python/examples//` | Shared C++ fixtures (not executables) live under `tests/cpp/viz/support/` diff --git a/tests/python/CMakeLists.txt b/tests/python/CMakeLists.txt index f2609a4e58..3df100d54c 100644 --- a/tests/python/CMakeLists.txt +++ b/tests/python/CMakeLists.txt @@ -18,6 +18,10 @@ if(BUILD_EXAMPLES AND BUILD_PYTHON_BINDINGS) add_subdirectory(examples/teleop) endif() +if(TARGET Teleop::remote_plugin_session) + add_subdirectory(plugins/remote_device_bridge) +endif() + if(BUILD_EXAMPLE_TELEOP_ROS2) add_subdirectory(examples/teleop_ros2) endif() diff --git a/tests/python/plugins/remote_device_bridge/CMakeLists.txt b/tests/python/plugins/remote_device_bridge/CMakeLists.txt new file mode 100644 index 0000000000..271f892b51 --- /dev/null +++ b/tests/python/plugins/remote_device_bridge/CMakeLists.txt @@ -0,0 +1,18 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +# remote_device_bridge tools/ (latency_breakdown.py, latency_probe_xrclient.py); the tests load the tool SOURCE files. +file(GLOB TEST_FILES + RELATIVE "${CMAKE_CURRENT_SOURCE_DIR}" + CONFIGURE_DEPENDS + "${CMAKE_CURRENT_SOURCE_DIR}/test_*.py" +) + +foreach(test_file ${TEST_FILES}) + get_filename_component(test_name "${test_file}" NAME_WE) + add_test( + NAME "remote_device_bridge_${test_name}" + COMMAND uv run --python ${ISAAC_TELEOP_PYTHON_VERSION} --extra dev pytest -v --tb=short "${test_file}" + WORKING_DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}" + ) +endforeach() diff --git a/tests/python/plugins/remote_device_bridge/conftest.py b/tests/python/plugins/remote_device_bridge/conftest.py new file mode 100644 index 0000000000..8b8869d3d1 --- /dev/null +++ b/tests/python/plugins/remote_device_bridge/conftest.py @@ -0,0 +1,17 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +# The tests import the remote_device_bridge tools/ scripts from their in-tree source. + +import sys +from pathlib import Path + +_tests_python = Path(__file__).resolve().parents[2] +if str(_tests_python) not in sys.path: + sys.path.insert(0, str(_tests_python)) + +from repo_paths import repo_root # noqa: E402 + +sys.path.insert( + 0, str(repo_root() / "src" / "plugins" / "remote_device_bridge" / "tools") +) diff --git a/tests/python/plugins/remote_device_bridge/pyproject.toml b/tests/python/plugins/remote_device_bridge/pyproject.toml new file mode 100644 index 0000000000..f4fbbbbecd --- /dev/null +++ b/tests/python/plugins/remote_device_bridge/pyproject.toml @@ -0,0 +1,18 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +[project] +name = "remote-device-bridge-tests" +version = "0.0.0" # Internal tests - not versioned +description = "remote_device_bridge tools unit tests" +requires-python = ">=3.11,<3.14" +dependencies = ["numpy"] + +[project.optional-dependencies] +dev = [ + "pytest", +] + +[tool.pytest.ini_options] +python_files = ["test_*.py"] +norecursedirs = [".git", ".venv", "build", "dist", "*.egg", "__pycache__"] diff --git a/tests/python/plugins/remote_device_bridge/test_latency_breakdown.py b/tests/python/plugins/remote_device_bridge/test_latency_breakdown.py new file mode 100644 index 0000000000..7f46bcfb73 --- /dev/null +++ b/tests/python/plugins/remote_device_bridge/test_latency_breakdown.py @@ -0,0 +1,252 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""latency_breakdown.py recovers known stage delays from synthetic bridge/probe CSV pairs and refuses to +report the consumer stage when the signals do not correlate.""" + +from __future__ import annotations + +import contextlib +import csv +import io +import tempfile +import unittest +from pathlib import Path + +import numpy as np + +import latency_breakdown + +MS = 1_000_000 +NETWORK_MS, BRIDGE_MS, CONSUMER_MS = 70, 3, 40 +OFFSET_NS = 5_000_000 * MS # bridge clock - sender clock +BRIDGE_HEADER = [ + "push_mono_ns", + "recv_mono_ns", + "capture_sender_ns", + "capture_mapped_ns", + "age_ms", + "side", + "seq", + "curl", +] + + +def write_csvs( + directory: Path, + capture_mapped: np.ndarray, + recv: np.ndarray, + push: np.ndarray, + curl: np.ndarray, + probe_t: np.ndarray, + probe_curl: np.ndarray, +) -> tuple[Path, Path]: + """Right-hand rows only; capture_sender = capture_mapped - OFFSET_NS.""" + directory.mkdir(parents=True, exist_ok=True) + bridge = directory / "bridge.csv" + with open(bridge, "w", newline="") as f: + w = csv.writer(f) + w.writerow(BRIDGE_HEADER) + for i in range(len(push)): + age = (push[i] - capture_mapped[i]) / 1e6 + w.writerow( + [ + push[i], + recv[i], + capture_mapped[i] - OFFSET_NS, + capture_mapped[i], + f"{age:.3f}", + "right", + i, + f"{curl[i]:.5f}", + ] + ) + probe = directory / "probe.csv" + with open(probe, "w", newline="") as f: + w = csv.writer(f) + w.writerow(["mono_ns", "side", "curl", "n_set"]) + for t, c in zip(probe_t, probe_curl): + w.writerow([t, "right", f"{c:.5f}", 25]) + return bridge, probe + + +def write_case( + directory: Path, + network_ms: float, + bridge_ms: float, + consumer_ms: float, + curl_fn, + probe_noise: bool = False, + seconds: float = 60.0, +) -> tuple[Path, Path]: + """90 Hz bridge rows with 10 ms network jitter; a 200 Hz probe (0-2 ms jitter) sees curl_fn + consumer_ms after the push, or independent noise when probe_noise.""" + rng = np.random.default_rng(35) + capture_s = np.arange(0.0, seconds, 1 / 90) + capture_mapped = (2_000 * 1e9 + capture_s * 1e9).astype(np.int64) + recv = capture_mapped + ( + network_ms * MS + rng.normal(0, 10 * MS, len(capture_s)) + ).astype(np.int64) + push = recv + int(bridge_ms * MS) + curl = curl_fn(capture_s, rng) + probe_t = np.arange(push.min(), push.max(), 5 * MS, dtype=np.int64) + probe_t += rng.integers(0, 2 * MS, len(probe_t)) + if probe_noise: + probe_curl = curl_fn(np.zeros(len(probe_t)), rng) + else: + probe_curl = latency_breakdown.hold(push, curl, probe_t - int(consumer_ms * MS)) + return write_csvs(directory, capture_mapped, recv, push, curl, probe_t, probe_curl) + + +def three_steps(t_s: np.ndarray, rng) -> np.ndarray: + return 0.07 + 0.06 * (np.searchsorted([12.3, 27.8, 44.1], t_s) % 2) + + +def static_noise(t_s: np.ndarray, rng) -> np.ndarray: + return 0.09 + rng.normal(0, 0.0005, len(t_s)) # 0.5 mm + + +def flat(t_s: np.ndarray, rng) -> np.ndarray: + return np.full(len(t_s), 0.09) + + +def run_cli(bridge: Path, probe: Path, side: str = "right") -> tuple[int, str, str]: + out, err = io.StringIO(), io.StringIO() + with contextlib.redirect_stdout(out), contextlib.redirect_stderr(err): + rc = latency_breakdown.main( + ["--bridge", str(bridge), "--probe", str(probe), "--side", side] + ) + return rc, out.getvalue(), err.getvalue() + + +def write_pair(directory: Path) -> tuple[Path, Path]: + """Steps of random length between two curl levels, sampled at 90 Hz and probed at 200 Hz.""" + rng = np.random.default_rng(32) + switch_s = np.cumsum(rng.uniform(0.4, 1.2, size=80)) + level = lambda t_s: 0.08 + 0.05 * (np.searchsorted(switch_s, t_s) % 2) # noqa: E731 + + capture_s = np.arange(0.0, 60.0, 1 / 90) + capture_sender = (1_000 * 1e9 + capture_s * 1e9).astype(np.int64) + capture_mapped = capture_sender + OFFSET_NS + recv = capture_mapped + ( + NETWORK_MS * MS + rng.normal(0, 2 * MS, len(capture_s)) + ).astype(np.int64) + push = recv + BRIDGE_MS * MS + curl = level(capture_s) + + bridge = directory / "bridge.csv" + with open(bridge, "w", newline="") as f: + w = csv.writer(f) + w.writerow(BRIDGE_HEADER) + for i in range(len(capture_s)): + age = (push[i] - capture_mapped[i]) / 1e6 + w.writerow( + [ + push[i], + recv[i], + capture_sender[i], + capture_mapped[i], + f"{age:.3f}", + "right", + i, + f"{curl[i]:.5f}", + ] + ) + + probe = directory / "probe.csv" + probe_t = np.arange(push[0], push[-1], 5 * MS, dtype=np.int64) + seen = latency_breakdown.hold(push, curl, probe_t - CONSUMER_MS * MS) + with open(probe, "w", newline="") as f: + w = csv.writer(f) + w.writerow(["mono_ns", "side", "curl", "n_set"]) + for t, c in zip(probe_t, seen): + w.writerow([t, "right", f"{c:.5f}", 25]) + w.writerow([t, "left", "", 0]) + return bridge, probe + + +class LatencyBreakdownTest(unittest.TestCase): + def test_recovers_70_3_40_ms(self) -> None: + with tempfile.TemporaryDirectory() as tmp: + bridge_csv, probe_csv = write_pair(Path(tmp)) + r = latency_breakdown.breakdown( + latency_breakdown.load_bridge(str(bridge_csv), "right"), + latency_breakdown.load_probe(str(probe_csv), "right"), + ) + self.assertAlmostEqual(r.network_p50, NETWORK_MS, delta=5) + self.assertAlmostEqual(r.bridge_p50, BRIDGE_MS, delta=5) + self.assertAlmostEqual(r.consumer_p50, CONSUMER_MS, delta=5) + self.assertAlmostEqual(r.edge_p50, CONSUMER_MS, delta=5) + self.assertGreater(len(r.edge_delays_ms), 40) + self.assertAlmostEqual( + r.total_p50, NETWORK_MS + BRIDGE_MS + CONSUMER_MS, delta=5 + ) + self.assertAlmostEqual(r.offset_ms, OFFSET_NS / 1e6, delta=1e-3) + self.assertTrue(r.estimated) + + def test_recovers_110_2_25_ms_with_three_edges(self) -> None: + with tempfile.TemporaryDirectory() as tmp: + bridge_csv, probe_csv = write_case(Path(tmp), 110, 2, 25, three_steps) + r = latency_breakdown.breakdown( + latency_breakdown.load_bridge(str(bridge_csv), "right"), + latency_breakdown.load_probe(str(probe_csv), "right"), + ) + self.assertTrue(r.estimated) + self.assertEqual(len(r.edge_delays_ms), 3) + self.assertAlmostEqual(r.network_p50, 110, delta=5) + self.assertAlmostEqual(r.bridge_p50, 2, delta=5) + self.assertAlmostEqual(r.consumer_p50, 25, delta=5) + self.assertAlmostEqual(r.edge_p50, 25, delta=5) + self.assertAlmostEqual(r.total_p50, 137, delta=5) + + def test_static_hand_with_uncorrelated_noise_is_not_estimated(self) -> None: + with tempfile.TemporaryDirectory() as tmp: + rc, out, _ = run_cli( + *write_case(Path(tmp), 110, 2, 25, static_noise, probe_noise=True) + ) + self.assertEqual(rc, 0) + self.assertIn(latency_breakdown.NOT_ESTIMATED, out) + self.assertIn("(a) network", out) + self.assertNotIn("(c)", out) + self.assertNotIn("(d)", out) + + def test_flat_curl_is_not_estimated(self) -> None: + with tempfile.TemporaryDirectory() as tmp: + rc, out, _ = run_cli(*write_case(Path(tmp), 110, 2, 25, flat)) + self.assertEqual(rc, 0) + self.assertIn(latency_breakdown.NOT_ESTIMATED, out) + self.assertNotIn("(c)", out) + + def test_unusable_input_exits_2_without_traceback(self) -> None: + with tempfile.TemporaryDirectory() as tmp: + cases = { + "empty side": ( + write_case(Path(tmp) / "a", 110, 2, 25, three_steps), + "left", + ), + "all-NaN curl": ( + write_case( + Path(tmp) / "b", + 110, + 2, + 25, + lambda t, rng: np.full(len(t), np.nan), + ), + "right", + ), + "short overlap": ( + write_case(Path(tmp) / "c", 110, 2, 25, three_steps, seconds=1.5), + "right", + ), + } + for name, ((bridge_csv, probe_csv), side) in cases.items(): + with self.subTest(name): + rc, out, err = run_cli(bridge_csv, probe_csv, side) + self.assertEqual(rc, 2) + self.assertEqual(out, "") + self.assertEqual(len(err.strip().splitlines()), 1) + self.assertNotIn("Traceback", err) + + +if __name__ == "__main__": + unittest.main() diff --git a/tests/python/plugins/remote_device_bridge/test_latency_probe_xrclient.py b/tests/python/plugins/remote_device_bridge/test_latency_probe_xrclient.py new file mode 100644 index 0000000000..aca140892b --- /dev/null +++ b/tests/python/plugins/remote_device_bridge/test_latency_probe_xrclient.py @@ -0,0 +1,69 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""latency_probe_xrclient.fingertips_curl on get_hand_fingertips() arrays shaped like the isaac_teleop +backend's (identity = slot not filled).""" + +from __future__ import annotations + +import math +import unittest + +import numpy as np + +import latency_probe_xrclient as probe + +TIP = np.array([0.03, -0.02, 0.17]) + + +def hand(wrist_at_origin: bool) -> np.ndarray: + joints = np.tile(np.eye(4), (25, 1, 1)) + for slot in range(25): + joints[slot, :3, 3] = [0.0, 0.0, 0.01 * (slot + 1)] + joints[probe.INDEX_TIP_SLOT, :3, 3] = TIP + if wrist_at_origin: + joints[probe.WRIST_SLOT] = np.eye(4) + return joints + + +class FingertipsCurlTest(unittest.TestCase): + def test_wrist_at_origin_still_gives_curl(self): + # Manus without a headset wrist pose: the wrist slot is exactly eye(4). + curl, n_set = probe.fingertips_curl(hand(wrist_at_origin=True)) + self.assertEqual(n_set, 24) + self.assertAlmostEqual(curl, float(np.linalg.norm(TIP))) + + def test_curl_is_relative_to_wrist(self): + joints = hand(wrist_at_origin=False) + joints[probe.WRIST_SLOT, :3, 3] = [0.5, 0.0, 0.0] + curl, n_set = probe.fingertips_curl(joints) + self.assertEqual(n_set, 25) + self.assertAlmostEqual( + curl, float(np.linalg.norm(TIP - np.array([0.5, 0.0, 0.0]))) + ) + + def test_unfilled_index_tip_has_no_curl(self): + joints = hand(wrist_at_origin=False) + joints[probe.INDEX_TIP_SLOT] = np.eye(4) + curl, n_set = probe.fingertips_curl(joints) + self.assertTrue(math.isnan(curl)) + self.assertEqual(n_set, 24) + + def test_no_hand(self): + curl, n_set = probe.fingertips_curl(None) + self.assertTrue(math.isnan(curl)) + self.assertEqual(n_set, 0) + + def test_all_identity(self): + curl, n_set = probe.fingertips_curl(np.tile(np.eye(4), (25, 1, 1))) + self.assertTrue(math.isnan(curl)) + self.assertEqual(n_set, 0) + + def test_non_finite_tip(self): + joints = hand(wrist_at_origin=True) + joints[probe.INDEX_TIP_SLOT, 0, 3] = np.nan + self.assertTrue(math.isnan(probe.fingertips_curl(joints)[0])) + + +if __name__ == "__main__": + unittest.main()