diff --git a/CMakeLists.txt b/CMakeLists.txt index 521ec03c30..afc7ca3ee9 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -71,6 +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 wire codec library (Teleop::remote_plugin_session)" 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/src/plugins/CMakeLists.txt b/src/plugins/CMakeLists.txt index c26d9384c2..60c15ae223 100644 --- a/src/plugins/CMakeLists.txt +++ b/src/plugins/CMakeLists.txt @@ -27,6 +27,10 @@ add_subdirectory(manus) add_subdirectory(manus_calibration) add_subdirectory(haptikos) +if(BUILD_PLUGIN_REMOTE_DEVICE_BRIDGE) + add_subdirectory(remote_device_bridge) +endif() + if(BUILD_PLUGIN_NOITOM_MOCAP) add_subdirectory(noitom_mocap) endif() diff --git a/src/plugins/remote_device_bridge/AGENTS.md b/src/plugins/remote_device_bridge/AGENTS.md new file mode 100644 index 0000000000..12e70cd249 --- /dev/null +++ b/src/plugins/remote_device_bridge/AGENTS.md @@ -0,0 +1,13 @@ + + +# Agent notes — `src/plugins/remote_device_bridge` + +**CRITICAL:** Complete the mandatory `AGENTS.md` preflight in [`../../../AGENTS.md`](../../../AGENTS.md) before editing here. + +- `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. +- 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. +- 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. diff --git a/src/plugins/remote_device_bridge/CMakeLists.txt b/src/plugins/remote_device_bridge/CMakeLists.txt new file mode 100644 index 0000000000..f7c312b75a --- /dev/null +++ b/src/plugins/remote_device_bridge/CMakeLists.txt @@ -0,0 +1,4 @@ +# SPDX-FileCopyrightText: Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +add_subdirectory(session) diff --git a/src/plugins/remote_device_bridge/session/CMakeLists.txt b/src/plugins/remote_device_bridge/session/CMakeLists.txt new file mode 100644 index 0000000000..c2f76d671f --- /dev/null +++ b/src/plugins/remote_device_bridge/session/CMakeLists.txt @@ -0,0 +1,22 @@ +# SPDX-FileCopyrightText: Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +add_library(remote_plugin_session STATIC + protocol_frame.cpp + protocol_payloads.cpp + protocol_io.hpp + inc/remote_plugin_session/protocol_frame.hpp + inc/remote_plugin_session/protocol_payloads.hpp +) + +target_include_directories(remote_plugin_session + INTERFACE + $ +) + +target_link_libraries(remote_plugin_session + PUBLIC + pusherio::pusherio +) + +add_library(Teleop::remote_plugin_session ALIAS remote_plugin_session) diff --git a/src/plugins/remote_device_bridge/session/inc/remote_plugin_session/protocol_frame.hpp b/src/plugins/remote_device_bridge/session/inc/remote_plugin_session/protocol_frame.hpp new file mode 100644 index 0000000000..c727e0cdb8 --- /dev/null +++ b/src/plugins/remote_device_bridge/session/inc/remote_plugin_session/protocol_frame.hpp @@ -0,0 +1,95 @@ +// SPDX-FileCopyrightText: Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include +#include +#include +#include +#include + +namespace remote_plugin_protocol +{ + +/*! + * Remote plugin protocol v1: fixed little-endian frames over one connection per + * plugin session. Frame = header + payload; payload layouts live in + * protocol_payloads.hpp. kProtocolVersion versions the whole format: any change + * to a header, payload or frame type number bumps it, and decode_header rejects + * every other version before it reads the type or anything after it. + */ + +constexpr uint32_t kMagic = 0x30535052u; // "RPS0" +constexpr uint8_t kProtocolVersion = 1; +//! Header byte offsets: magic u32, type u8, version u8, channel u16, seq u32, payload size u32, t_send i64. +constexpr size_t kTypeOffset = sizeof(kMagic); +constexpr size_t kVersionOffset = kTypeOffset + sizeof(uint8_t); +constexpr size_t kChannelOffset = kVersionOffset + sizeof(uint8_t); +constexpr size_t kSeqOffset = kChannelOffset + sizeof(uint16_t); +constexpr size_t kPayloadSizeOffset = kSeqOffset + sizeof(uint32_t); +constexpr size_t kSendTimeOffset = kPayloadSizeOffset + sizeof(uint32_t); +constexpr size_t kHeaderSize = kSendTimeOffset + sizeof(int64_t); +constexpr uint16_t kSessionChannelId = 0; +constexpr uint32_t kMaxPayloadSize = 16u * 1024u * 1024u; // large tactile/sensor payloads; still bounded + +//! Direction is c->s (plugin to bridge) or s->c. OPEN_PULL, CLOSE, HEARTBEAT and PING have empty payloads. +enum class FrameType : uint8_t +{ + Hello = 1, // c->s + HelloAck = 2, // s->c + OpenHand = 3, // c->s + OpenSchema = 4, // c->s + OpenPull = 5, // c->s + Close = 6, // c->s + HandSample = 7, // c->s + SchemaSample = 8, // c->s + WristSample = 9, // s->c + Heartbeat = 10, // both + Error = 11, // s->c + Ping = 12, // s->c + Pong = 13, // c->s +}; + +struct FrameHeader +{ + FrameType type{}; // 0 is not a frame type + uint16_t channel_id = kSessionChannelId; + uint32_t seq = 0; + uint32_t payload_size = 0; + int64_t t_send_monotonic_ns = 0; +}; + +struct Frame +{ + FrameHeader header; + std::vector payload; +}; + +class DecodeError : public std::runtime_error +{ +public: + using std::runtime_error::runtime_error; +}; + +//! Reads the header from the first kHeaderSize bytes of `in`. Throws DecodeError, checking in this order: `in` +//! shorter than a header, bad magic, oversized payload, another protocol version, unknown type. The version is +//! checked before the type byte is interpreted, since another version may number its frame types differently. +FrameHeader decode_header(std::span in); + +//! What framing needs: the payload size from the first kHeaderSize bytes of `in`, checking only the magic and the +//! payload limit, which sit at the same offsets in every protocol version. Version and type are left to +//! decode_header, so a frame from another version still reaches the decoder and fails there with a readable reason. +//! Throws DecodeError if `in` is shorter than a header, on bad magic or an oversized payload. +uint32_t frame_payload_size(std::span in); + +//! Writes header + payload into one contiguous buffer. seq and t_send stay zero until stamp_header(), which the +//! sender calls at the moment it writes the frame: t_send is the send time PING/PONG clock exchanges use. +std::vector encode_frame(FrameType type, uint16_t channel_id, std::span payload); +//! Patches seq and t_send into an encoded frame's header in place. +void stamp_header(std::vector& frame, uint32_t seq, int64_t t_send_monotonic_ns); +//! Splits one complete message into header and payload. The payload keeps `message`'s buffer (its bytes are +//! shifted down by the header size). Throws DecodeError. +Frame split_frame(std::vector message); + +} // namespace remote_plugin_protocol diff --git a/src/plugins/remote_device_bridge/session/inc/remote_plugin_session/protocol_payloads.hpp b/src/plugins/remote_device_bridge/session/inc/remote_plugin_session/protocol_payloads.hpp new file mode 100644 index 0000000000..05558c02f9 --- /dev/null +++ b/src/plugins/remote_device_bridge/session/inc/remote_plugin_session/protocol_payloads.hpp @@ -0,0 +1,113 @@ +// SPDX-FileCopyrightText: Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include "protocol_frame.hpp" + +#include +#include + +#include +#include +#include +#include +#include +#include + +namespace remote_plugin_protocol +{ + +/*! + * Payload builders and parsers. `str` = u32 length + bytes, `pose` = 7 x f32 in + * XrPosef order (orientation x,y,z,w then position x,y,z). + */ + +struct Hello +{ + std::string app_name; + core::PluginSessionRequirements requirements; +}; + +struct HelloAck +{ + bool accepted = false; + std::string reason; +}; + +struct HandSample +{ + int64_t sample_time_local_common_clock_ns = 0; + std::array joints{}; +}; + +struct SchemaSample +{ + int64_t sample_time_local_common_clock_ns = 0; + int64_t sample_time_raw_device_clock_ns = 0; + std::vector bytes; +}; + +//! On the wire the hand is one byte, 0 = left and 1 = right, as in OPEN_HAND. +struct WristSample +{ + int64_t sample_time_local_common_clock_ns = 0; + XrHandEXT hand = XR_HAND_LEFT_EXT; + core::WristTrackingSample sample; +}; + +//! Two-way clock exchange; times are CLOCK_MONOTONIC ns of the side that took them. The PING carries no payload: +//! its send time is its header t_send. The PONG's own send time is the PONG header t_send. +struct Pong +{ + int64_t ping_sent_ns = 0; //!< bridge clock: the PING header t_send, echoed + int64_t ping_received_ns = 0; //!< client clock +}; + +constexpr size_t kPoseSize = 7 * sizeof(float); +constexpr size_t kJointSize = sizeof(uint64_t) + kPoseSize + sizeof(float); +constexpr size_t kHandSamplePayloadSize = sizeof(int64_t) + XR_HAND_JOINT_COUNT_EXT * kJointSize; +//! WRIST_SAMPLE payload: sample time i64, hand u8, valid u8, tracked u8, pose. +constexpr size_t kWristSamplePayloadSize = sizeof(int64_t) + 3 * sizeof(uint8_t) + kPoseSize; +//! SCHEMA_SAMPLE payload before the schema bytes: two sample times (i64) and the byte count (u32). +constexpr size_t kSchemaSampleFixedSize = 2 * sizeof(int64_t) + sizeof(uint32_t); + +static_assert(kJointSize == 40); +static_assert(kHandSamplePayloadSize == 1048); +static_assert(kWristSamplePayloadSize == 39); +static_assert(kSchemaSampleFixedSize == 20); + +//! One hand's joints, as IHandTrackingPushChannel::push receives them. +using HandJoints = std::span; + +std::vector encode_hello(const Hello& value); +std::vector encode_hello_ack(const HelloAck& value); +//! One byte: 0 = left, 1 = right. +std::vector encode_open_hand(XrHandEXT hand); +std::vector encode_open_schema(const core::SchemaPusherConfig& value); +std::vector encode_wrist_sample(const WristSample& value); +std::vector encode_error(const std::string& message); +std::vector encode_pong(const Pong& value); + +// HAND_SAMPLE and SCHEMA_SAMPLE frames are only built this way: the builders read the plugin's buffers directly so a +// push copies once. +std::vector encode_hand_sample_frame(uint16_t channel_id, + HandJoints joints, + int64_t sample_time_local_common_clock_ns); +std::vector encode_schema_sample_frame(uint16_t channel_id, + std::span bytes, + int64_t sample_time_local_common_clock_ns, + int64_t sample_time_raw_device_clock_ns); + +// Parsers throw DecodeError, naming the frame, on truncated, over-long or out-of-range payloads. +Hello decode_hello(std::span payload); +HelloAck decode_hello_ack(std::span payload); +XrHandEXT decode_open_hand(std::span payload); +core::SchemaPusherConfig decode_open_schema(std::span payload); +HandSample decode_hand_sample(std::span payload); +SchemaSample decode_schema_sample(std::span payload); +WristSample decode_wrist_sample(std::span payload); +std::string decode_error(std::span payload); +Pong decode_pong(std::span payload); + +} // namespace remote_plugin_protocol diff --git a/src/plugins/remote_device_bridge/session/protocol_frame.cpp b/src/plugins/remote_device_bridge/session/protocol_frame.cpp new file mode 100644 index 0000000000..57f06bd9af --- /dev/null +++ b/src/plugins/remote_device_bridge/session/protocol_frame.cpp @@ -0,0 +1,132 @@ +// SPDX-FileCopyrightText: Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "inc/remote_plugin_session/protocol_frame.hpp" + +#include "protocol_io.hpp" + +#include +#include +#include + +namespace remote_plugin_protocol +{ + +using detail::build_frame; +using detail::load_le; +using detail::store_le; +using detail::Writer; + +namespace +{ + +bool is_frame_type(uint8_t value) +{ + switch (static_cast(value)) + { + case FrameType::Hello: + case FrameType::HelloAck: + case FrameType::OpenHand: + case FrameType::OpenSchema: + case FrameType::OpenPull: + case FrameType::Close: + case FrameType::HandSample: + case FrameType::SchemaSample: + case FrameType::WristSample: + case FrameType::Heartbeat: + case FrameType::Error: + case FrameType::Ping: + case FrameType::Pong: + return true; + } + return false; +} + +} // namespace + +namespace detail +{ + +void encode_header(const FrameHeader& header, uint8_t* out) +{ + store_le(out, kMagic); + out[kTypeOffset] = static_cast(header.type); + out[kVersionOffset] = kProtocolVersion; + store_le(out + kChannelOffset, header.channel_id); + store_le(out + kSeqOffset, header.seq); + store_le(out + kPayloadSizeOffset, header.payload_size); + store_le(out + kSendTimeOffset, header.t_send_monotonic_ns); +} + +} // namespace detail + +void stamp_header(std::vector& frame, uint32_t seq, int64_t t_send_monotonic_ns) +{ + if (frame.size() < kHeaderSize) + { + throw std::invalid_argument("protocol: frame shorter than a header"); + } + store_le(frame.data() + kSeqOffset, seq); + store_le(frame.data() + kSendTimeOffset, t_send_monotonic_ns); +} + +Frame split_frame(std::vector message) +{ + Frame frame; + frame.header = decode_header(message); + if (message.size() != kHeaderSize + frame.header.payload_size) + { + throw DecodeError("protocol: message length does not match payload_size"); + } + message.erase(message.begin(), message.begin() + static_cast(kHeaderSize)); + frame.payload = std::move(message); + return frame; +} + +std::vector encode_frame(FrameType type, uint16_t channel_id, std::span payload) +{ + return build_frame(type, channel_id, payload.size(), [payload](Writer& writer) { writer.bytes(payload); }); +} + +uint32_t frame_payload_size(std::span in) +{ + if (in.size() < kHeaderSize) + { + throw DecodeError("protocol: header: truncated"); + } + if (load_le(in.data()) != kMagic) + { + throw DecodeError("protocol: bad frame magic"); + } + const auto payload_size = load_le(in.data() + kPayloadSizeOffset); + if (payload_size > kMaxPayloadSize) + { + throw DecodeError("protocol: payload size " + std::to_string(payload_size) + " exceeds limit"); + } + return payload_size; +} + +FrameHeader decode_header(std::span in) +{ + FrameHeader header; + header.payload_size = frame_payload_size(in); // length, magic and payload limit + // Version before type: another version may number its frame types differently. + if (const uint8_t version = in[kVersionOffset]; version != kProtocolVersion) + { + throw DecodeError("protocol: peer speaks protocol version " + std::to_string(version) + + ", this build speaks version " + std::to_string(kProtocolVersion) + + "; rebuild the sender and the bridge from the same source"); + } + const uint8_t type = in[kTypeOffset]; + if (!is_frame_type(type)) + { + throw DecodeError("protocol: unknown frame type " + std::to_string(type)); + } + header.type = static_cast(type); + header.channel_id = load_le(in.data() + kChannelOffset); + header.seq = load_le(in.data() + kSeqOffset); + header.t_send_monotonic_ns = load_le(in.data() + kSendTimeOffset); + return header; +} + +} // namespace remote_plugin_protocol diff --git a/src/plugins/remote_device_bridge/session/protocol_io.hpp b/src/plugins/remote_device_bridge/session/protocol_io.hpp new file mode 100644 index 0000000000..b8163f6b61 --- /dev/null +++ b/src/plugins/remote_device_bridge/session/protocol_io.hpp @@ -0,0 +1,262 @@ +// SPDX-FileCopyrightText: Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include "inc/remote_plugin_session/protocol_frame.hpp" + +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace remote_plugin_protocol +{ +namespace detail +{ + +//! Stores `value` little-endian at `out`; the byte count is sizeof(T). +template +inline void store_le(uint8_t* out, T value) +{ + static_assert(std::is_integral_v); + const auto bits = static_cast>(value); + for (size_t i = 0; i < sizeof(T); ++i) + { + out[i] = static_cast(bits >> (8 * i)); + } +} + +//! Loads a little-endian T from the sizeof(T) bytes at `in`. +template +inline T load_le(const uint8_t* in) +{ + static_assert(std::is_integral_v); + using Bits = std::make_unsigned_t; + Bits bits = 0; + for (size_t i = 0; i < sizeof(T); ++i) + { + bits = static_cast(bits | (static_cast(in[i]) << (8 * i))); + } + return static_cast(bits); +} + +//! Writes the header into the first kHeaderSize bytes of `out`. +void encode_header(const FrameHeader& header, uint8_t* out); + +//! Appends little-endian fields to a caller-owned buffer. +class Writer +{ +public: + explicit Writer(std::vector& out) : m_out(out) + { + } + + void u8(uint8_t value) + { + m_out.push_back(value); + } + + void boolean(bool value) + { + u8(value ? 1 : 0); + } + + void u32(uint32_t value) + { + append(value); + } + + void u64(uint64_t value) + { + append(value); + } + + void i64(int64_t value) + { + u64(static_cast(value)); + } + + void f32(float value) + { + uint32_t bits = 0; + std::memcpy(&bits, &value, sizeof(bits)); + u32(bits); + } + + void str(const std::string& value) + { + if (value.size() > std::numeric_limits::max()) + { + throw std::invalid_argument("protocol: string too long"); + } + u32(static_cast(value.size())); + m_out.insert(m_out.end(), value.begin(), value.end()); + } + + void bytes(std::span data) + { + m_out.insert(m_out.end(), data.begin(), data.end()); + } + + void pose(const XrPosef& value) + { + f32(value.orientation.x); + f32(value.orientation.y); + f32(value.orientation.z); + f32(value.orientation.w); + f32(value.position.x); + f32(value.position.y); + f32(value.position.z); + } + +private: + template + void append(T value) + { + const size_t at = m_out.size(); + m_out.resize(at + sizeof(T)); + store_le(m_out.data() + at, value); + } + + std::vector& m_out; +}; + +class Reader +{ +public: + //! `what` names the frame or header being read in DecodeError messages. + Reader(std::span data, const char* what) : m_data(data), m_what(what) + { + } + + uint8_t u8() + { + need(1); + return m_data[m_pos++]; + } + + bool boolean() + { + const uint8_t value = u8(); + if (value > 1) + { + throw DecodeError(std::string("protocol: ") + m_what + ": invalid boolean value " + std::to_string(value)); + } + return value == 1; + } + + uint32_t u32() + { + return take(); + } + + uint64_t u64() + { + return take(); + } + + int64_t i64() + { + return static_cast(u64()); + } + + float f32() + { + const uint32_t bits = u32(); + float value = 0.0f; + std::memcpy(&value, &bits, sizeof(value)); + return value; + } + + std::string str() + { + const uint32_t size = u32(); + need(size); + std::string value(reinterpret_cast(m_data.data() + m_pos), size); + m_pos += size; + return value; + } + + std::vector bytes(size_t size) + { + need(size); + const auto field = m_data.subspan(m_pos, size); + std::vector value(field.begin(), field.end()); + m_pos += size; + return value; + } + + XrPosef pose() + { + XrPosef value; + value.orientation.x = f32(); + value.orientation.y = f32(); + value.orientation.z = f32(); + value.orientation.w = f32(); + value.position.x = f32(); + value.position.y = f32(); + value.position.z = f32(); + return value; + } + + void finish() const + { + if (m_pos != m_data.size()) + { + throw DecodeError(std::string("protocol: ") + m_what + ": " + std::to_string(m_data.size() - m_pos) + + " trailing bytes"); + } + } + +private: + template + T take() + { + need(sizeof(T)); + const T value = load_le(m_data.data() + m_pos); + m_pos += sizeof(T); + return value; + } + + void need(size_t count) const + { + if (m_data.size() - m_pos < count) + { + throw DecodeError(std::string("protocol: ") + m_what + ": truncated"); + } + } + + std::span m_data; + const char* m_what; + size_t m_pos = 0; +}; + +//! Reserves the header, writes the payload behind it, then fills the header in with the resulting size. +template +inline std::vector build_frame(FrameType type, uint16_t channel_id, size_t payload_hint, WritePayload write_payload) +{ + std::vector out; + out.reserve(kHeaderSize + payload_hint); + out.resize(kHeaderSize); + Writer writer(out); + write_payload(writer); + if (out.size() - kHeaderSize > kMaxPayloadSize) + { + throw std::invalid_argument("protocol: payload too large"); + } + FrameHeader header; + header.type = type; + header.channel_id = channel_id; + header.payload_size = static_cast(out.size() - kHeaderSize); + encode_header(header, out.data()); + return out; +} + +} // namespace detail +} // namespace remote_plugin_protocol diff --git a/src/plugins/remote_device_bridge/session/protocol_payloads.cpp b/src/plugins/remote_device_bridge/session/protocol_payloads.cpp new file mode 100644 index 0000000000..4bd03df1e8 --- /dev/null +++ b/src/plugins/remote_device_bridge/session/protocol_payloads.cpp @@ -0,0 +1,329 @@ +// SPDX-FileCopyrightText: Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "inc/remote_plugin_session/protocol_payloads.hpp" + +#include "protocol_io.hpp" + +#include + +namespace remote_plugin_protocol +{ + +using detail::build_frame; +using detail::Reader; +using detail::Writer; + +namespace +{ + +uint8_t to_wire_hand(XrHandEXT hand) +{ + switch (hand) + { + case XR_HAND_LEFT_EXT: + return 0; + case XR_HAND_RIGHT_EXT: + return 1; + default: + throw std::invalid_argument("protocol: unsupported XrHandEXT value"); + } +} + +XrHandEXT from_wire_hand(uint8_t hand) +{ + switch (hand) + { + case 0: + return XR_HAND_LEFT_EXT; + case 1: + return XR_HAND_RIGHT_EXT; + default: + throw DecodeError("protocol: invalid hand value " + std::to_string(hand)); + } +} + +uint8_t to_wire_mode(core::WristTrackingSourceMode mode) +{ + switch (mode) + { + case core::WristTrackingSourceMode::Auto: + return 0; + case core::WristTrackingSourceMode::HandTracking: + return 1; + case core::WristTrackingSourceMode::Controller: + return 2; + } + throw std::invalid_argument("protocol: unsupported wrist tracking source mode"); +} + +core::WristTrackingSourceMode from_wire_mode(uint8_t mode) +{ + switch (mode) + { + case 0: + return core::WristTrackingSourceMode::Auto; + case 1: + return core::WristTrackingSourceMode::HandTracking; + case 2: + return core::WristTrackingSourceMode::Controller; + default: + throw DecodeError("protocol: invalid wrist tracking source mode " + std::to_string(mode)); + } +} + +void write_wrist_config(Writer& writer, const core::WristTrackingSourceConfig& value) +{ + writer.u8(to_wire_mode(value.mode)); + writer.pose(value.left_aim_to_wrist); + writer.pose(value.right_aim_to_wrist); +} + +core::WristTrackingSourceConfig read_wrist_config(Reader& reader) +{ + core::WristTrackingSourceConfig value; + value.mode = from_wire_mode(reader.u8()); + value.left_aim_to_wrist = reader.pose(); + value.right_aim_to_wrist = reader.pose(); + return value; +} + +//! Runs `write` against a fresh payload buffer and returns it. +template +std::vector build_payload(Write write) +{ + std::vector out; + Writer writer(out); + write(writer); + return out; +} + +//! Runs `read` over `payload` and rejects trailing bytes; the payload-side mirror of build_payload. +template +auto parse_payload(std::span payload, const char* what, Read read) +{ + Reader reader(payload, what); + auto value = read(reader); + reader.finish(); + return value; +} + +} // namespace + +std::vector encode_hand_sample_frame(uint16_t channel_id, + HandJoints joints, + int64_t sample_time_local_common_clock_ns) +{ + return build_frame(FrameType::HandSample, channel_id, kHandSamplePayloadSize, + [&](Writer& writer) + { + writer.i64(sample_time_local_common_clock_ns); + for (const XrHandJointLocationEXT& joint : joints) + { + writer.u64(joint.locationFlags); + writer.pose(joint.pose); + writer.f32(joint.radius); + } + }); +} + +std::vector encode_schema_sample_frame(uint16_t channel_id, + std::span bytes, + int64_t sample_time_local_common_clock_ns, + int64_t sample_time_raw_device_clock_ns) +{ + return build_frame(FrameType::SchemaSample, channel_id, kSchemaSampleFixedSize + bytes.size(), + [&](Writer& writer) + { + writer.i64(sample_time_local_common_clock_ns); + writer.i64(sample_time_raw_device_clock_ns); + writer.u32(static_cast(bytes.size())); + writer.bytes(bytes); + }); +} + +std::vector encode_hello(const Hello& value) +{ + return build_payload( + [&](Writer& writer) + { + writer.str(value.app_name); + writer.boolean(value.requirements.schema_push); + writer.boolean(value.requirements.hand_tracking_push); + writer.boolean(value.requirements.wrist_tracking_pull.has_value()); + if (value.requirements.wrist_tracking_pull) + { + write_wrist_config(writer, *value.requirements.wrist_tracking_pull); + } + }); +} + +std::vector encode_hello_ack(const HelloAck& value) +{ + return build_payload( + [&](Writer& writer) + { + writer.boolean(value.accepted); + writer.str(value.reason); + }); +} + +std::vector encode_open_hand(XrHandEXT hand) +{ + return build_payload([&](Writer& writer) { writer.u8(to_wire_hand(hand)); }); +} + +std::vector encode_open_schema(const core::SchemaPusherConfig& value) +{ + return build_payload( + [&](Writer& writer) + { + writer.str(value.collection_id); + writer.u64(static_cast(value.max_flatbuffer_size)); + writer.str(value.tensor_identifier); + writer.str(value.localized_name); + writer.str(value.app_name); + }); +} + +std::vector encode_wrist_sample(const WristSample& value) +{ + return build_payload( + [&](Writer& writer) + { + writer.i64(value.sample_time_local_common_clock_ns); + writer.u8(to_wire_hand(value.hand)); + writer.boolean(value.sample.valid); + writer.boolean(value.sample.tracked); + writer.pose(value.sample.pose); + }); +} + +std::vector encode_error(const std::string& message) +{ + return build_payload([&](Writer& writer) { writer.str(message); }); +} + +std::vector encode_pong(const Pong& value) +{ + return build_payload( + [&](Writer& writer) + { + writer.i64(value.ping_sent_ns); + writer.i64(value.ping_received_ns); + }); +} + +Hello decode_hello(std::span payload) +{ + return parse_payload(payload, "HELLO", + [](Reader& reader) + { + Hello value; + value.app_name = reader.str(); + value.requirements.schema_push = reader.boolean(); + value.requirements.hand_tracking_push = reader.boolean(); + if (reader.boolean()) + { + value.requirements.wrist_tracking_pull = read_wrist_config(reader); + } + return value; + }); +} + +HelloAck decode_hello_ack(std::span payload) +{ + return parse_payload(payload, "HELLO_ACK", + [](Reader& reader) + { + HelloAck value; + value.accepted = reader.boolean(); + value.reason = reader.str(); + return value; + }); +} + +XrHandEXT decode_open_hand(std::span payload) +{ + return parse_payload(payload, "OPEN_HAND", [](Reader& reader) { return from_wire_hand(reader.u8()); }); +} + +core::SchemaPusherConfig decode_open_schema(std::span payload) +{ + return parse_payload(payload, "OPEN_SCHEMA", + [](Reader& reader) + { + core::SchemaPusherConfig value; + value.collection_id = reader.str(); + value.max_flatbuffer_size = static_cast(reader.u64()); + value.tensor_identifier = reader.str(); + value.localized_name = reader.str(); + value.app_name = reader.str(); + return value; + }); +} + +HandSample decode_hand_sample(std::span payload) +{ + return parse_payload(payload, "HAND_SAMPLE", + [](Reader& reader) + { + HandSample value; + value.sample_time_local_common_clock_ns = reader.i64(); + for (XrHandJointLocationEXT& joint : value.joints) + { + joint.locationFlags = reader.u64(); + joint.pose = reader.pose(); + joint.radius = reader.f32(); + } + return value; + }); +} + +SchemaSample decode_schema_sample(std::span payload) +{ + return parse_payload(payload, "SCHEMA_SAMPLE", + [](Reader& reader) + { + SchemaSample value; + value.sample_time_local_common_clock_ns = reader.i64(); + value.sample_time_raw_device_clock_ns = reader.i64(); + const uint32_t size = reader.u32(); + value.bytes = reader.bytes(size); + return value; + }); +} + +WristSample decode_wrist_sample(std::span payload) +{ + return parse_payload(payload, "WRIST_SAMPLE", + [](Reader& reader) + { + WristSample value; + value.sample_time_local_common_clock_ns = reader.i64(); + value.hand = from_wire_hand(reader.u8()); + value.sample.valid = reader.boolean(); + value.sample.tracked = reader.boolean(); + value.sample.pose = reader.pose(); + return value; + }); +} + +std::string decode_error(std::span payload) +{ + return parse_payload(payload, "ERROR", [](Reader& reader) { return reader.str(); }); +} + +Pong decode_pong(std::span payload) +{ + return parse_payload(payload, "PONG", + [](Reader& reader) + { + Pong value; + value.ping_sent_ns = reader.i64(); + value.ping_received_ns = reader.i64(); + return value; + }); +} + +} // namespace remote_plugin_protocol diff --git a/tests/cpp/plugins/CMakeLists.txt b/tests/cpp/plugins/CMakeLists.txt index fd7e2d2e68..51d4666a9e 100644 --- a/tests/cpp/plugins/CMakeLists.txt +++ b/tests/cpp/plugins/CMakeLists.txt @@ -21,3 +21,7 @@ endif() if(TARGET manus_calibration_core) add_subdirectory(manus_calibration) endif() + +if(TARGET Teleop::remote_plugin_session) + add_subdirectory(remote_device_bridge) +endif() diff --git a/tests/cpp/plugins/remote_device_bridge/CMakeLists.txt b/tests/cpp/plugins/remote_device_bridge/CMakeLists.txt new file mode 100644 index 0000000000..fcac49524b --- /dev/null +++ b/tests/cpp/plugins/remote_device_bridge/CMakeLists.txt @@ -0,0 +1,13 @@ +# SPDX-FileCopyrightText: Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +add_executable(remote_plugin_session_tests + test_wire_codec.cpp +) + +target_link_libraries(remote_plugin_session_tests PRIVATE + Teleop::remote_plugin_session + Catch2::Catch2WithMain +) + +catch_discover_tests(remote_plugin_session_tests ADD_TAGS_AS_LABELS) diff --git a/tests/cpp/plugins/remote_device_bridge/test_wire_codec.cpp b/tests/cpp/plugins/remote_device_bridge/test_wire_codec.cpp new file mode 100644 index 0000000000..be5feb9c84 --- /dev/null +++ b/tests/cpp/plugins/remote_device_bridge/test_wire_codec.cpp @@ -0,0 +1,459 @@ +// SPDX-FileCopyrightText: Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +namespace protocol = remote_plugin_protocol; + +namespace +{ + +//! WRIST_SAMPLE payload: sample_time i64, then the hand byte, then `valid`. +constexpr size_t kWristHandOffset = sizeof(int64_t); +constexpr size_t kWristValidOffset = kWristHandOffset + sizeof(uint8_t); +//! SCHEMA_SAMPLE payload: two i64 sample times, then the u32 byte count. +constexpr size_t kSchemaByteCountOffset = 2 * sizeof(int64_t); + +bool same_pose(const XrPosef& a, const XrPosef& b) +{ + return std::memcmp(&a, &b, sizeof(XrPosef)) == 0; +} + +XrPosef make_pose(float seed) +{ + return { { seed, seed + 0.25f, -seed, 0.5f }, { 10.0f * seed, -20.0f * seed, 30.0f * seed } }; +} + +void put_u32(std::vector& out, uint32_t v) +{ + for (int i = 0; i < 4; ++i) + { + out.push_back(static_cast(v >> (8 * i))); + } +} + +void put_u64(std::vector& out, uint64_t v) +{ + put_u32(out, static_cast(v)); + put_u32(out, static_cast(v >> 32)); +} + +void put_f32(std::vector& out, float v) +{ + uint32_t bits = 0; + std::memcpy(&bits, &v, sizeof(bits)); + put_u32(out, bits); +} + +//! Field-by-field reference encoder, the golden for HAND_SAMPLE. +std::vector reference_hand_payload(const protocol::HandSample& sample) +{ + std::vector out; + put_u64(out, static_cast(sample.sample_time_local_common_clock_ns)); + for (const XrHandJointLocationEXT& joint : sample.joints) + { + put_u64(out, joint.locationFlags); + for (const float f : + { joint.pose.orientation.x, joint.pose.orientation.y, joint.pose.orientation.z, joint.pose.orientation.w, + joint.pose.position.x, joint.pose.position.y, joint.pose.position.z, joint.radius }) + { + put_f32(out, f); + } + } + return out; +} + +std::vector hand_payload(const protocol::HandSample& sample) +{ + return protocol::split_frame( + protocol::encode_hand_sample_frame(1, sample.joints, sample.sample_time_local_common_clock_ns)) + .payload; +} + +std::vector schema_payload(const protocol::SchemaSample& sample) +{ + return protocol::split_frame(protocol::encode_schema_sample_frame(1, sample.bytes, + sample.sample_time_local_common_clock_ns, + sample.sample_time_raw_device_clock_ns)) + .payload; +} + +} // namespace + +TEST_CASE("Frame header round-trips and validates", "[remote_device_bridge][unit]") +{ + std::vector frame = protocol::encode_frame( + protocol::FrameType::HandSample, 0xBEEF, std::vector(protocol::kHandSamplePayloadSize)); + protocol::stamp_header(frame, 0x01020304, -1234567890123LL); + uint8_t* bytes = frame.data(); + REQUIRE(bytes[0] == 0x52); // little-endian "RPS0" + REQUIRE(bytes[1] == 0x50); + REQUIRE(bytes[2] == 0x53); + REQUIRE(bytes[3] == 0x30); + + const protocol::FrameHeader decoded = protocol::decode_header({ bytes, protocol::kHeaderSize }); + REQUIRE(decoded.type == protocol::FrameType::HandSample); + REQUIRE(decoded.channel_id == 0xBEEF); + REQUIRE(decoded.seq == 0x01020304); + REQUIRE(decoded.payload_size == protocol::kHandSamplePayloadSize); + REQUIRE(decoded.t_send_monotonic_ns == -1234567890123LL); + + SECTION("bad magic is rejected") + { + bytes[0] ^= 0xFF; + REQUIRE_THROWS_AS(protocol::decode_header({ bytes, protocol::kHeaderSize }), protocol::DecodeError); + } + SECTION("unknown type is rejected") + { + bytes[protocol::kTypeOffset] = 200; + REQUIRE_THROWS_AS(protocol::decode_header({ bytes, protocol::kHeaderSize }), protocol::DecodeError); + } + SECTION("other protocol versions are rejected, naming both versions") + { + for (const uint8_t version : { 0, 2 }) + { + bytes[protocol::kVersionOffset] = version; + REQUIRE_THROWS_WITH( + protocol::decode_header({ bytes, protocol::kHeaderSize }), + Catch::Matchers::ContainsSubstring("peer speaks protocol version " + std::to_string(version) + + ", this build speaks version 1")); + } + } + SECTION("oversized payloads are rejected") + { + std::memset(bytes + protocol::kPayloadSizeOffset, 0xFF, sizeof(uint32_t)); + REQUIRE_THROWS_AS(protocol::decode_header({ bytes, protocol::kHeaderSize }), protocol::DecodeError); + REQUIRE_THROWS_AS(protocol::frame_payload_size({ bytes, protocol::kHeaderSize }), protocol::DecodeError); + } + SECTION("framing reads the size and checks only magic and limit") + { + REQUIRE(protocol::frame_payload_size({ bytes, protocol::kHeaderSize }) == protocol::kHandSamplePayloadSize); + bytes[protocol::kTypeOffset] = 200; + bytes[protocol::kVersionOffset] = 0; + REQUIRE(protocol::frame_payload_size({ bytes, protocol::kHeaderSize }) == protocol::kHandSamplePayloadSize); + REQUIRE_THROWS_AS(protocol::frame_payload_size({ bytes, protocol::kHeaderSize - 1 }), protocol::DecodeError); + bytes[0] ^= 0xFF; + REQUIRE_THROWS_AS(protocol::frame_payload_size({ bytes, protocol::kHeaderSize }), protocol::DecodeError); + } +} + +TEST_CASE("A version-0 peer's frames are rejected by version, never decoded", "[remote_device_bridge][unit]") +{ + // HELLO v2 inside a version-0 header, as the previous wire format sent it, and a version-0 PONG (type 14 there). + std::vector old_hello = { 0x52, 0x50, 0x53, 0x30, 1, 0, 0, 0, 0, 0, 0, 0, 9, 0, 0, 0 }; + old_hello.resize(protocol::kHeaderSize, 0); + for (const uint8_t b : { 2, 0, 0, 0, 0, 0, 0, 0, 0 }) + { + old_hello.push_back(b); // version u16 = 2, empty app_name, three false flags + } + REQUIRE_THROWS_WITH( + protocol::split_frame(old_hello), + Catch::Matchers::ContainsSubstring("peer speaks protocol version 0, this build speaks version 1")); + + std::vector old_pong = { 0x52, 0x50, 0x53, 0x30, 14, 0, 0, 0, 0, 0, 0, 0, 24, 0, 0, 0 }; + old_pong.resize(protocol::kHeaderSize + 24, 0); + // Framing still finds both frames' lengths, so a transport delivers them to the decoder above. + REQUIRE(protocol::frame_payload_size(old_hello) == 9); + REQUIRE(protocol::frame_payload_size(old_pong) == 24); + REQUIRE_THROWS_WITH(protocol::split_frame(old_pong), Catch::Matchers::ContainsSubstring("protocol version 0")); +} + +TEST_CASE("Wire contract is 13 frame types in a 24-byte header", "[remote_device_bridge][unit]") +{ + // Frame type numbers are part of the wire contract. + const std::vector> contract = { + { protocol::FrameType::Hello, 1 }, { protocol::FrameType::HelloAck, 2 }, + { protocol::FrameType::OpenHand, 3 }, { protocol::FrameType::OpenSchema, 4 }, + { protocol::FrameType::OpenPull, 5 }, { protocol::FrameType::Close, 6 }, + { protocol::FrameType::HandSample, 7 }, { protocol::FrameType::SchemaSample, 8 }, + { protocol::FrameType::WristSample, 9 }, { protocol::FrameType::Heartbeat, 10 }, + { protocol::FrameType::Error, 11 }, { protocol::FrameType::Ping, 12 }, + { protocol::FrameType::Pong, 13 }, + }; + for (const auto& [type, number] : contract) + { + REQUIRE(static_cast(type) == number); + } + + // magic u32 @0, type u8 @4, version u8 @5, channel u16 @6, seq u32 @8, payload size u32 @12, t_send i64 @16. + std::vector frame = protocol::encode_frame(protocol::FrameType::Pong, 0x0A0B, std::vector(16, 0)); + protocol::stamp_header(frame, 0x0C0D0E0F, 0x1112131415161718LL); + const std::vector expected = { 0x52, 0x50, 0x53, 0x30, 13, 1, 0x0B, 0x0A, 0x0F, 0x0E, 0x0D, 0x0C, + 16, 0, 0, 0, 0x18, 0x17, 0x16, 0x15, 0x14, 0x13, 0x12, 0x11 }; + REQUIRE(std::vector(frame.begin(), frame.begin() + protocol::kHeaderSize) == expected); + uint8_t* bytes = frame.data(); + + for (const uint8_t type : { 0, 14 }) + { + bytes[protocol::kTypeOffset] = type; + REQUIRE_THROWS_AS(protocol::decode_header({ bytes, protocol::kHeaderSize }), protocol::DecodeError); + } +} + +TEST_CASE("HELLO and HELLO_ACK round-trip", "[remote_device_bridge][unit]") +{ + protocol::Hello hello; + hello.app_name = "ManusHandPlugin"; + const core::WristTrackingSourceConfig wrist{ + .mode = core::WristTrackingSourceMode::Controller, + .left_aim_to_wrist = { { 0.0f, 0.0f, 0.0f, 1.0f }, { 1.0f, 2.0f, 3.0f } }, + .right_aim_to_wrist = { { 0.0f, 1.0f, 0.0f, 0.0f }, { -1.0f, 0.5f, 0.25f } } + }; + hello.requirements = { .schema_push = true, .hand_tracking_push = false, .wrist_tracking_pull = wrist }; + const protocol::Hello decoded = protocol::decode_hello(protocol::encode_hello(hello)); + REQUIRE(decoded.app_name == "ManusHandPlugin"); + REQUIRE(decoded.requirements.schema_push); + REQUIRE_FALSE(decoded.requirements.hand_tracking_push); + REQUIRE(decoded.requirements.wrist_tracking_pull); + REQUIRE(decoded.requirements.wrist_tracking_pull->mode == core::WristTrackingSourceMode::Controller); + REQUIRE(decoded.requirements.wrist_tracking_pull->left_aim_to_wrist.position.y == 2.0f); + REQUIRE(decoded.requirements.wrist_tracking_pull->right_aim_to_wrist.orientation.y == 1.0f); + + hello.requirements.wrist_tracking_pull.reset(); + REQUIRE_FALSE(protocol::decode_hello(protocol::encode_hello(hello)).requirements.wrist_tracking_pull); + + const protocol::HelloAck ack = + protocol::decode_hello_ack(protocol::encode_hello_ack(protocol::HelloAck{ false, "no runtime" })); + REQUIRE_FALSE(ack.accepted); + REQUIRE(ack.reason == "no runtime"); + + const protocol::HelloAck ok = protocol::decode_hello_ack(protocol::encode_hello_ack(protocol::HelloAck{ true, "" })); + REQUIRE(ok.accepted); + REQUIRE(ok.reason.empty()); +} + +TEST_CASE("OPEN_HAND encodes both hands and rejects others", "[remote_device_bridge][unit]") +{ + REQUIRE(protocol::encode_open_hand(XR_HAND_LEFT_EXT) == std::vector{ 0 }); + REQUIRE(protocol::encode_open_hand(XR_HAND_RIGHT_EXT) == std::vector{ 1 }); + REQUIRE(protocol::decode_open_hand(std::vector{ 0 }) == XR_HAND_LEFT_EXT); + REQUIRE(protocol::decode_open_hand(std::vector{ 1 }) == XR_HAND_RIGHT_EXT); + REQUIRE_THROWS_AS(protocol::decode_open_hand(std::vector{ 2 }), protocol::DecodeError); + REQUIRE_THROWS_AS(protocol::decode_open_hand(std::vector{}), protocol::DecodeError); + REQUIRE_THROWS_AS(protocol::decode_open_hand(std::vector{ 0, 0 }), protocol::DecodeError); +} + +TEST_CASE("OPEN_SCHEMA round-trips SchemaPusherConfig", "[remote_device_bridge][unit]") +{ + core::SchemaPusherConfig config{ .collection_id = "manus_sensors_left", + .max_flatbuffer_size = 4096, + .tensor_identifier = "joint_state", + .localized_name = "Manus Sensors Left", + .app_name = "" }; + const core::SchemaPusherConfig decoded = protocol::decode_open_schema(protocol::encode_open_schema(config)); + REQUIRE(decoded.collection_id == config.collection_id); + REQUIRE(decoded.max_flatbuffer_size == 4096); + REQUIRE(decoded.tensor_identifier == config.tensor_identifier); + REQUIRE(decoded.localized_name == config.localized_name); + REQUIRE(decoded.app_name.empty()); + + std::vector truncated = protocol::encode_open_schema(config); + truncated.pop_back(); + REQUIRE_THROWS_AS(protocol::decode_open_schema(truncated), protocol::DecodeError); +} + +TEST_CASE("HELLO rejects an unknown wrist source mode", "[remote_device_bridge][unit]") +{ + protocol::Hello hello; + hello.requirements.wrist_tracking_pull = core::WristTrackingSourceConfig{ .mode = core::WristTrackingSourceMode::Auto, + .left_aim_to_wrist = make_pose(0.1f), + .right_aim_to_wrist = make_pose(0.7f) }; + std::vector bytes = protocol::encode_hello(hello); + // app_name (u32 length, empty), three flags, then the wrist mode byte. + const size_t mode_offset = sizeof(uint32_t) + 3; + REQUIRE(bytes[mode_offset] == 0); + bytes[mode_offset] = 7; + REQUIRE_THROWS_AS(protocol::decode_hello(bytes), protocol::DecodeError); +} + +TEST_CASE("HAND_SAMPLE is a bit-exact copy of 26 joints", "[remote_device_bridge][unit]") +{ + protocol::HandSample sample; + sample.sample_time_local_common_clock_ns = 987654321012345LL; + for (size_t i = 0; i < XR_HAND_JOINT_COUNT_EXT; ++i) + { + sample.joints[i].locationFlags = 0xF000000000000000ULL | i; + sample.joints[i].pose = make_pose(0.01f * static_cast(i + 1)); + sample.joints[i].radius = 0.001f * static_cast(i); + } + + const std::vector bytes = hand_payload(sample); + REQUIRE(bytes.size() == protocol::kHandSamplePayloadSize); + + const protocol::HandSample decoded = protocol::decode_hand_sample(bytes); + REQUIRE(decoded.sample_time_local_common_clock_ns == sample.sample_time_local_common_clock_ns); + for (size_t i = 0; i < XR_HAND_JOINT_COUNT_EXT; ++i) + { + REQUIRE(decoded.joints[i].locationFlags == sample.joints[i].locationFlags); + REQUIRE(same_pose(decoded.joints[i].pose, sample.joints[i].pose)); + REQUIRE(std::memcmp(&decoded.joints[i].radius, &sample.joints[i].radius, sizeof(float)) == 0); + } + + std::vector truncated(bytes.begin(), bytes.end() - 1); + REQUIRE_THROWS_AS(protocol::decode_hand_sample(truncated), protocol::DecodeError); + std::vector extended = bytes; + extended.push_back(0); + REQUIRE_THROWS_AS(protocol::decode_hand_sample(extended), protocol::DecodeError); +} + +TEST_CASE("SCHEMA_SAMPLE round-trips bytes and timestamps", "[remote_device_bridge][unit]") +{ + protocol::SchemaSample sample; + sample.sample_time_local_common_clock_ns = 111; + sample.sample_time_raw_device_clock_ns = 222; + sample.bytes = { 0xDE, 0xAD, 0xBE, 0xEF, 0x00, 0x01 }; + + const protocol::SchemaSample decoded = protocol::decode_schema_sample(schema_payload(sample)); + REQUIRE(decoded.sample_time_local_common_clock_ns == 111); + REQUIRE(decoded.sample_time_raw_device_clock_ns == 222); + REQUIRE(decoded.bytes == sample.bytes); + + const protocol::SchemaSample empty = protocol::decode_schema_sample(schema_payload(protocol::SchemaSample{})); + REQUIRE(empty.bytes.empty()); + + std::vector lying = schema_payload(sample); + lying[kSchemaByteCountOffset] = 0xFF; // declared size larger than the remaining bytes + REQUIRE_THROWS_AS(protocol::decode_schema_sample(lying), protocol::DecodeError); +} + +TEST_CASE("WRIST_SAMPLE round-trips", "[remote_device_bridge][unit]") +{ + protocol::WristSample sample; + sample.sample_time_local_common_clock_ns = -5; + sample.hand = XR_HAND_RIGHT_EXT; + sample.sample.valid = true; + sample.sample.tracked = false; + sample.sample.pose = make_pose(0.3f); + + const std::vector bytes = protocol::encode_wrist_sample(sample); + REQUIRE(bytes.size() == protocol::kWristSamplePayloadSize); + REQUIRE(bytes[kWristHandOffset] == 1); // 0 = left, 1 = right, as in OPEN_HAND + const protocol::WristSample decoded = protocol::decode_wrist_sample(bytes); + REQUIRE(decoded.sample_time_local_common_clock_ns == -5); + REQUIRE(decoded.hand == XR_HAND_RIGHT_EXT); + REQUIRE(decoded.sample.valid); + REQUIRE_FALSE(decoded.sample.tracked); + REQUIRE(same_pose(decoded.sample.pose, sample.sample.pose)); + + std::vector bad_bool = bytes; + bad_bool[kWristValidOffset] = 2; + REQUIRE_THROWS_AS(protocol::decode_wrist_sample(bad_bool), protocol::DecodeError); +} + +TEST_CASE("ERROR carries a message and empty payloads decode", "[remote_device_bridge][unit]") +{ + REQUIRE(protocol::decode_error(protocol::encode_error("boom")) == "boom"); + REQUIRE(protocol::decode_error(protocol::encode_error("")).empty()); + REQUIRE_THROWS_AS(protocol::decode_error(std::vector{ 4, 0, 0, 0, 'a' }), protocol::DecodeError); +} + +TEST_CASE("PONG carries the PING send time and receive time; send times live in headers", "[remote_device_bridge][unit]") +{ + const std::vector payload = protocol::encode_pong(protocol::Pong{ 1, 2 }); + REQUIRE(payload.size() == 2 * sizeof(int64_t)); + const protocol::Pong pong = protocol::decode_pong(payload); + REQUIRE(pong.ping_sent_ns == 1); + REQUIRE(pong.ping_received_ns == 2); + REQUIRE_THROWS_AS(protocol::decode_pong(std::vector(17, 0)), protocol::DecodeError); + REQUIRE_THROWS_WITH(protocol::decode_pong(std::vector{ 1 }), Catch::Matchers::ContainsSubstring("PONG")); + + // PING has no payload; each side's send time is the t_send stamped when the frame is written. + std::vector ping = protocol::encode_frame(protocol::FrameType::Ping, protocol::kSessionChannelId, {}); + REQUIRE(ping.size() == protocol::kHeaderSize); + protocol::stamp_header(ping, 3, -123456789012345LL); + REQUIRE(protocol::split_frame(ping).header.t_send_monotonic_ns == -123456789012345LL); + + std::vector frame = protocol::encode_frame(protocol::FrameType::Pong, protocol::kSessionChannelId, payload); + protocol::stamp_header(frame, 7, 0x0102030405060708LL); + const protocol::Frame split = protocol::split_frame(frame); + REQUIRE(split.header.type == protocol::FrameType::Pong); + REQUIRE(split.header.t_send_monotonic_ns == 0x0102030405060708LL); + REQUIRE(split.payload == payload); +} + +TEST_CASE("Contiguous frame builders match header + payload encoder byte for byte", "[remote_device_bridge][unit]") +{ + SECTION("SCHEMA_SAMPLE against a hand-built golden frame") + { + const std::vector bytes = { 0xDE, 0xAD }; + std::vector frame = protocol::encode_schema_sample_frame(3, bytes, 0x0102030405060708LL, -1); + // clang-format off + const std::vector golden = { + 0x52, 0x50, 0x53, 0x30, // magic "RPS0" + 0x08, 0x01, // type SCHEMA_SAMPLE, version 1 + 0x03, 0x00, // channel 3 + 0x00, 0x00, 0x00, 0x00, // seq (unstamped) + 0x16, 0x00, 0x00, 0x00, // payload_size 22 + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, // t_send (unstamped) + 0x08, 0x07, 0x06, 0x05, 0x04, 0x03, 0x02, 0x01, // sample_time_local_common_clock_ns + 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, // sample_time_raw_device_clock_ns = -1 + 0x02, 0x00, 0x00, 0x00, // byte count + 0xDE, 0xAD, + }; + // clang-format on + REQUIRE(frame == golden); + + protocol::stamp_header(frame, 0x11223344u, 0x0807060504030201LL); + REQUIRE(std::vector(frame.begin() + protocol::kSeqOffset, frame.begin() + protocol::kPayloadSizeOffset) == + std::vector{ 0x44, 0x33, 0x22, 0x11 }); + REQUIRE(std::vector(frame.begin() + protocol::kSendTimeOffset, frame.begin() + protocol::kHeaderSize) == + std::vector{ 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 0x08 }); + // payload untouched + REQUIRE(std::equal(frame.begin() + protocol::kHeaderSize, frame.end(), golden.begin() + protocol::kHeaderSize)); + + const protocol::Frame split = protocol::split_frame(frame); + REQUIRE(split.header.type == protocol::FrameType::SchemaSample); + REQUIRE(split.header.channel_id == 3); + REQUIRE(split.header.seq == 0x11223344u); + REQUIRE(split.header.t_send_monotonic_ns == 0x0807060504030201LL); + REQUIRE(split.payload == std::vector(golden.begin() + protocol::kHeaderSize, golden.end())); + protocol::SchemaSample decoded = protocol::decode_schema_sample(split.payload); + REQUIRE(decoded.bytes == bytes); + REQUIRE(decoded.sample_time_raw_device_clock_ns == -1); + } + SECTION("HAND_SAMPLE against the reference payload encoder") + { + protocol::HandSample sample; + sample.sample_time_local_common_clock_ns = -42; + for (size_t i = 0; i < XR_HAND_JOINT_COUNT_EXT; ++i) + { + sample.joints[i].locationFlags = 0xA000000000000000ULL | i; + sample.joints[i].pose = make_pose(0.02f * static_cast(i + 1)); + sample.joints[i].radius = 0.001f * static_cast(i); + } + const std::vector payload = reference_hand_payload(sample); + REQUIRE(payload.size() == protocol::kHandSamplePayloadSize); + const std::vector frame = protocol::encode_hand_sample_frame(0x0102, sample.joints, -42); + REQUIRE(frame.size() == protocol::kHeaderSize + protocol::kHandSamplePayloadSize); + const protocol::FrameHeader header = protocol::decode_header(frame); + REQUIRE(header.type == protocol::FrameType::HandSample); + REQUIRE(header.channel_id == 0x0102); + REQUIRE(header.payload_size == protocol::kHandSamplePayloadSize); + REQUIRE(std::equal(frame.begin() + protocol::kHeaderSize, frame.end(), payload.begin(), payload.end())); + REQUIRE(protocol::encode_frame(protocol::FrameType::HandSample, 0x0102, payload) == frame); + } + SECTION("split_frame rejects short or inconsistent messages") + { + REQUIRE_THROWS_AS(protocol::split_frame(std::vector(10, 0)), protocol::DecodeError); + std::vector frame = protocol::encode_frame(protocol::FrameType::Heartbeat, 0, {}); + REQUIRE(frame.size() == protocol::kHeaderSize); + REQUIRE(protocol::split_frame(frame).payload.empty()); + frame.push_back(0); + REQUIRE_THROWS_AS(protocol::split_frame(frame), protocol::DecodeError); + } + SECTION("payloads above the limit are refused at build time") + { + std::vector too_big(protocol::kMaxPayloadSize + 1); + REQUIRE_THROWS_AS(protocol::encode_schema_sample_frame(1, too_big, 0, 0), std::invalid_argument); + } +}