diff --git a/CMakeLists.txt b/CMakeLists.txt index 0dcde99227..1c5333e40a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -187,6 +187,7 @@ if(BUILD_PLUGINS) add_subdirectory(src/plugins/vive_se3_tracker) add_subdirectory(src/plugins/controller_synthetic_hands) add_subdirectory(src/plugins/generic_3axis_pedal) + add_subdirectory(src/plugins/xsens_full_body) add_subdirectory(src/plugins/so101_leader) add_subdirectory(src/plugins/rebot_devarm_leader) add_subdirectory(src/plugins/manus) diff --git a/src/core/live_trackers/cpp/CMakeLists.txt b/src/core/live_trackers/cpp/CMakeLists.txt index a3ec5ab5a1..2d65a8dace 100644 --- a/src/core/live_trackers/cpp/CMakeLists.txt +++ b/src/core/live_trackers/cpp/CMakeLists.txt @@ -12,6 +12,7 @@ add_library(live_trackers STATIC live_message_channel_tracker_impl.cpp live_full_body_tracker_pico_impl.cpp live_full_body_tracker_noitom_impl.cpp + live_full_body_tracker_xsens_impl.cpp live_tensor_push_tracker_impl.cpp live_haptic_command_reader_tracker_impl.cpp ${GENERATED_TRACKER_LIVE_SOURCES} @@ -24,6 +25,7 @@ add_library(live_trackers STATIC live_message_channel_tracker_impl.hpp live_full_body_tracker_pico_impl.hpp live_full_body_tracker_noitom_impl.hpp + live_full_body_tracker_xsens_impl.hpp live_tensor_push_tracker_impl.hpp live_haptic_command_reader_tracker_impl.hpp ) diff --git a/src/core/live_trackers/cpp/inc/live_trackers/live_deviceio_factory.hpp b/src/core/live_trackers/cpp/inc/live_trackers/live_deviceio_factory.hpp index 6f8d52aae1..a2c57aa9e4 100644 --- a/src/core/live_trackers/cpp/inc/live_trackers/live_deviceio_factory.hpp +++ b/src/core/live_trackers/cpp/inc/live_trackers/live_deviceio_factory.hpp @@ -84,6 +84,7 @@ class LiveDeviceIOFactory std::unique_ptr create_message_channel_tracker_impl(const MessageChannelTracker* tracker); std::unique_ptr create_full_body_tracker_pico_impl(const FullBodyTracker* tracker); std::unique_ptr create_full_body_tracker_noitom_impl(const FullBodyTracker* tracker); + std::unique_ptr create_full_body_tracker_xsens_impl(const FullBodyTracker* tracker); std::unique_ptr create_tensor_push_tracker_impl(const TensorPushTracker* tracker); std::unique_ptr create_haptic_command_reader_tracker_impl( const HapticCommandReaderTracker* tracker); diff --git a/src/core/live_trackers/cpp/live_deviceio_factory.cpp b/src/core/live_trackers/cpp/live_deviceio_factory.cpp index c11104e395..000a1b5bce 100644 --- a/src/core/live_trackers/cpp/live_deviceio_factory.cpp +++ b/src/core/live_trackers/cpp/live_deviceio_factory.cpp @@ -7,6 +7,7 @@ #include "live_controller_tracker_impl.hpp" #include "live_full_body_tracker_noitom_impl.hpp" #include "live_full_body_tracker_pico_impl.hpp" +#include "live_full_body_tracker_xsens_impl.hpp" #include "live_hand_tracker_impl.hpp" #include "live_haptic_command_reader_tracker_impl.hpp" #include "live_head_tracker_impl.hpp" @@ -92,6 +93,12 @@ std::unique_ptr try_create_full_body_noitom_impl(LiveDeviceIOFacto return typed ? factory.create_full_body_tracker_noitom_impl(typed) : nullptr; } +std::unique_ptr try_create_full_body_xsens_impl(LiveDeviceIOFactory& factory, const ITracker& tracker) +{ + auto* typed = dynamic_cast(&tracker); + return typed ? factory.create_full_body_tracker_xsens_impl(typed) : nullptr; +} + std::unique_ptr try_create_tensor_push_impl(LiveDeviceIOFactory& factory, const ITracker& tracker) { auto* typed = dynamic_cast(&tracker); @@ -144,6 +151,8 @@ inline const TrackerDispatchEntry k_tracker_dispatch[] = { make_dispatch_entry(&try_create_full_body_pico_impl, "body.pico-xr"), make_dispatch_entry( &try_create_full_body_noitom_impl, LiveFullBodyTrackerNoitomImpl::VENDOR_ID), + make_dispatch_entry( + &try_create_full_body_xsens_impl, LiveFullBodyTrackerXsensImpl::VENDOR_ID), make_dispatch_entry(&try_create_tensor_push_impl), make_dispatch_entry( &try_create_haptic_command_reader_impl), @@ -276,6 +285,10 @@ void validate_vendor_selections(const std::vector LiveDeviceIOFactory::create_full_body_trac return std::make_unique(handles_, *vendor, std::move(channels)); } +std::unique_ptr LiveDeviceIOFactory::create_full_body_tracker_xsens_impl(const FullBodyTracker* tracker) +{ + std::unique_ptr channels; + if (should_record(tracker)) + { + channels = LiveFullBodyTrackerPicoImpl::create_mcap_channels(*writer_, get_name(tracker)); + } + + const TrackerVendor* vendor = find_vendor(tracker); + assert(vendor && vendor->id == LiveFullBodyTrackerXsensImpl::VENDOR_ID); + return std::make_unique(handles_, *vendor, std::move(channels)); +} + std::unique_ptr LiveDeviceIOFactory::create_tensor_push_tracker_impl(const TensorPushTracker* tracker) { return std::make_unique(handles_, tracker); diff --git a/src/core/live_trackers/cpp/live_full_body_tracker_xsens_impl.cpp b/src/core/live_trackers/cpp/live_full_body_tracker_xsens_impl.cpp new file mode 100644 index 0000000000..f6927a26a0 --- /dev/null +++ b/src/core/live_trackers/cpp/live_full_body_tracker_xsens_impl.cpp @@ -0,0 +1,97 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 Xsens Technologies B.V. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "live_full_body_tracker_xsens_impl.hpp" + +#include +#include +#include +#include +#include + +namespace core +{ + +namespace +{ + +constexpr std::string_view COLLECTION_ID_PARAM = "collection_id"; +constexpr std::string_view MAX_FLATBUFFER_SIZE_PARAM = "max_flatbuffer_size"; + +SchemaTrackerConfig make_xsens_tensor_config(const TrackerVendor& vendor) +{ + if (vendor.id != LiveFullBodyTrackerXsensImpl::VENDOR_ID) + { + throw std::invalid_argument("Xsens full-body vendor id must be '" + + std::string(LiveFullBodyTrackerXsensImpl::VENDOR_ID) + "'"); + } + + for (const auto& [key, value] : vendor.params) + { + (void)value; + if (key != COLLECTION_ID_PARAM && key != MAX_FLATBUFFER_SIZE_PARAM) + { + throw std::invalid_argument("Xsens full-body vendor does not support parameter '" + key + "'"); + } + } + + SchemaTrackerConfig config; + config.collection_id = std::string(LiveFullBodyTrackerXsensImpl::DEFAULT_COLLECTION_ID); + config.max_flatbuffer_size = LiveFullBodyTrackerXsensImpl::DEFAULT_MAX_FLATBUFFER_SIZE; + config.tensor_identifier = std::string(LiveFullBodyTrackerXsensImpl::TENSOR_IDENTIFIER); + config.localized_name = "Xsens Full Body"; + + if (auto it = vendor.params.find(std::string(COLLECTION_ID_PARAM)); it != vendor.params.end()) + { + // A collection_id mismatch between pusher and reader fails SILENTLY and forever -- the + // two sides rendezvous on this string and simply never connect. An empty one is always a + // configuration error, so reject it loudly here rather than hang later. + if (it->second.empty()) + { + throw std::invalid_argument("Xsens full-body collection_id must not be empty"); + } + config.collection_id = it->second; + } + + if (auto it = vendor.params.find(std::string(MAX_FLATBUFFER_SIZE_PARAM)); it != vendor.params.end()) + { + size_t parsed = 0; + const char* begin = it->second.data(); + const char* end = begin + it->second.size(); + const auto [ptr, error] = std::from_chars(begin, end, parsed); + if (error != std::errc{} || ptr != end || parsed == 0) + { + throw std::invalid_argument("Xsens full-body max_flatbuffer_size must be a positive integer"); + } + config.max_flatbuffer_size = parsed; + } + + return config; +} + +} // namespace + +void LiveFullBodyTrackerXsensImpl::validate_vendor(const TrackerVendor& vendor) +{ + (void)make_xsens_tensor_config(vendor); +} + +LiveFullBodyTrackerXsensImpl::LiveFullBodyTrackerXsensImpl(const OpenXRSessionHandles& handles, + const TrackerVendor& vendor, + std::unique_ptr mcap_channels) + : mcap_channels_(std::move(mcap_channels)), + schema_reader_(handles, make_xsens_tensor_config(vendor), mcap_channels_.get(), /*mcap_channel_index=*/0) +{ +} + +void LiveFullBodyTrackerXsensImpl::update(int64_t /*monotonic_time_ns*/) +{ + schema_reader_.update(tracked_); +} + +const Serialized& LiveFullBodyTrackerXsensImpl::get_body_pose() const +{ + return tracked_; +} + +} // namespace core diff --git a/src/core/live_trackers/cpp/live_full_body_tracker_xsens_impl.hpp b/src/core/live_trackers/cpp/live_full_body_tracker_xsens_impl.hpp new file mode 100644 index 0000000000..8243b1a140 --- /dev/null +++ b/src/core/live_trackers/cpp/live_full_body_tracker_xsens_impl.hpp @@ -0,0 +1,75 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 Xsens Technologies B.V. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include "inc/live_trackers/schema_tracker.hpp" + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace core +{ + +using FullBodyXsensMcapChannels = McapTrackerChannels; +using FullBodyXsensSchemaTracker = SchemaTracker; + +/*! + * @brief Full-body reader for the Xsens MVN stream pushed by the `xsens_full_body` plugin. + * + * Xsens MVN Studio converts its own 23-segment skeleton to the vendor-neutral 24-joint + * XR_BD_body_tracking layout inside MVN Studio itself (Options -> Network Streamer, preset + * "Isaac Teleop"), so what arrives here is already a `FullBodyPose` FlatBuffer -- byte-identical + * to what any other full-body vendor produces. This reader therefore adds nothing but the + * tensor-collection plumbing, exactly like the other pushed-collection full-body vendors. + * + * Two joints are always flagged invalid: MVN has no finger tracking, so LEFT_HAND (22) and + * RIGHT_HAND (23) carry a copy of the wrist pose with `is_valid = false`, and + * `all_joint_poses_tracked` is consequently always false. 22 of 24 valid is correct here, not a + * fault -- consult the per-joint flags. + */ +class LiveFullBodyTrackerXsensImpl : public IFullBodyTrackerImpl +{ +public: + static constexpr std::string_view VENDOR_ID = "body.xsens"; + static constexpr std::string_view DEFAULT_COLLECTION_ID = "xsens_full_body"; + static constexpr std::string_view TENSOR_IDENTIFIER = "full_body_pose"; + //! One pose is 16 B of table/vtable + 24 x 32 B of joint structs = 784 B. 4 KiB leaves room + //! without over-allocating; the pusher must be configured to match or both sides throw. + static constexpr size_t DEFAULT_MAX_FLATBUFFER_SIZE = 4 * 1024; + + static std::vector required_extensions() + { + return SchemaTrackerBase::get_required_extensions(); + } + + static void validate_vendor(const TrackerVendor& vendor); + + LiveFullBodyTrackerXsensImpl(const OpenXRSessionHandles& handles, + const TrackerVendor& vendor, + std::unique_ptr mcap_channels); + + LiveFullBodyTrackerXsensImpl(const LiveFullBodyTrackerXsensImpl&) = delete; + LiveFullBodyTrackerXsensImpl& operator=(const LiveFullBodyTrackerXsensImpl&) = delete; + LiveFullBodyTrackerXsensImpl(LiveFullBodyTrackerXsensImpl&&) = delete; + LiveFullBodyTrackerXsensImpl& operator=(LiveFullBodyTrackerXsensImpl&&) = delete; + + void update(int64_t monotonic_time_ns) override; + const Serialized& get_body_pose() const override; + +private: + std::unique_ptr mcap_channels_; + FullBodyXsensSchemaTracker schema_reader_; + Serialized tracked_; +}; + +} // namespace core diff --git a/src/plugins/xsens_full_body/CMakeLists.txt b/src/plugins/xsens_full_body/CMakeLists.txt new file mode 100644 index 0000000000..99f6434f31 --- /dev/null +++ b/src/plugins/xsens_full_body/CMakeLists.txt @@ -0,0 +1,48 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 Xsens Technologies B.V. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +if(NOT CMAKE_SYSTEM_NAME STREQUAL "Linux") + message(STATUS "Skipping xsens_full_body plugin (Linux only)") + add_custom_target(xsens_full_body_plugin + COMMAND ${CMAKE_COMMAND} -E echo "Skipping xsens_full_body: Linux only") + return() +endif() + +# No vendor SDK: MVN Studio performs the skeleton conversion itself and emits the schema this +# repo already defines, so the plugin needs nothing beyond the core libraries. +add_executable(xsens_full_body_plugin + main.cpp + xsens_full_body_plugin.cpp + frame_decision.cpp + plugin_options.cpp +) + +target_link_libraries(xsens_full_body_plugin PRIVATE + pusherio::pusherio + oxr::oxr_core + isaacteleop_schema +) + +install(TARGETS xsens_full_body_plugin RUNTIME DESTINATION plugins/xsens_full_body) +install(FILES plugin.yaml README.md DESTINATION plugins/xsens_full_body) + +# Unit tests for the two pieces the plugin can be taken apart into: what it decides about a +# datagram, and what it decides about a command line. Both link their unit directly and need no +# OpenXR runtime, so they run anywhere. +if(BUILD_TESTING) + add_executable(test_xsens_frame_decision + tests/test_frame_decision.cpp + frame_decision.cpp + ) + target_include_directories(test_xsens_frame_decision PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}) + target_link_libraries(test_xsens_frame_decision PRIVATE isaacteleop_schema) + add_test(NAME xsens_frame_decision COMMAND test_xsens_frame_decision) + + # Lighter still than the above: argument parsing pulls in neither flatbuffers nor the schema. + add_executable(test_xsens_plugin_options + tests/test_plugin_options.cpp + plugin_options.cpp + ) + target_include_directories(test_xsens_plugin_options PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}) + add_test(NAME xsens_plugin_options COMMAND test_xsens_plugin_options) +endif() diff --git a/src/plugins/xsens_full_body/README.md b/src/plugins/xsens_full_body/README.md new file mode 100644 index 0000000000..6c582b0ed8 --- /dev/null +++ b/src/plugins/xsens_full_body/README.md @@ -0,0 +1,191 @@ + + +# xsens_full_body plugin + +Receives Xsens MVN Studio's **Isaac Teleop** UDP stream and republishes it as an Isaac Teleop +tensor collection, readable through the `body.xsens` vendor. + +``` +MVN Studio --UDP 9764 (XTLP)--> xsens_full_body_plugin --> collection "xsens_full_body" + tensor "full_body_pose" +``` + +## Why it is this small + +MVN Studio does the skeleton conversion itself: its network-streamer preset converts the +23-segment MVN skeleton to the vendor-neutral 24-joint `XR_BD_body_tracking` layout and emits a +`core::FullBodyPose` FlatBuffer — the same schema this repository defines. So the plugin needs no +vendor SDK, and **forwards the payload bytes verbatim**; re-serializing would risk silently +changing a pose and buys nothing. + +## Running + +```bash +source ~/.cloudxr/run/cloudxr.env # in this shell and every consumer's +./xsens_full_body_plugin \ + --collection-id=xsens_full_body \ + --address=0.0.0.0 \ + --port=9764 \ + --max-flatbuffer-size=4096 +``` + +Every flag shown is its default, so bare `./xsens_full_body_plugin` is the same invocation; the +defaults match MVN's "Isaac Teleop" preset. `--address` picks the interface to bind — the default +accepts the stream on any of them, and `--address=127.0.0.1` confines the pusher to loopback. + +Then in MVN Studio: **Options → Network Streamer → preset "Isaac Teleop"**, tick the destination +row, and move in the suit or press **Play** on a recording. + +Reader side: + +```python +tracker = deviceio.FullBodyTracker() +vendor = deviceio.VendorConfig([(tracker, deviceio.TrackerVendor("body.xsens", { + "collection_id": "xsens_full_body", "max_flatbuffer_size": "4096"}))]) +# NB: pass `vendor` to get_required_extensions() as well, or the pushed-tensor extensions are +# never requested and the session silently never sees the collection. +``` + +## Things that will bite + +- **Order matters.** The pusher creates the collection, so a consumer started first reports "no + collection found" forever. +- **A `collection_id` mismatch fails silently and forever** — the two sides rendezvous on that + string. A `max_flatbuffer_size` mismatch, by contrast, throws loudly and names the right value. + So when a reader sees nothing, suspect the collection id first. +- **`--address` has the same silent failure signature.** Bind to one interface and point MVN at a + *different* local address, and the datagrams are simply never delivered — no error on either + side, identical to a collection-id mismatch. A bad address is at least rejected at startup: only + a literal IPv4 is accepted, hostnames included, because an unresolvable name would otherwise + surface much later as an opaque `EADDRNOTAVAIL`. +- **22 of 24 joints valid is correct.** MVN has no finger tracking, so `LEFT_HAND` (22) and + `RIGHT_HAND` (23) carry a copy of the wrist pose flagged invalid, and `all_joint_poses_tracked` + is structurally always false. Consult the per-joint flags. +- **A sequence gap is not proof of packet loss, and the stats line reports two figures for it.** + `gapEvents` is how many times continuity broke; `seqSkipped` is how many sequence numbers went + missing in total. One 500-frame dropout and 500 single-frame losses are the same `seqSkipped` + and very different faults, which is why neither number is reported alone. + + Neither is a link-quality figure on its own. A number goes missing when MVN declines to build + that frame (`seq` is sent-only: invalid pose, fewer than 23 segments, any non-finite + component), when a datagram is genuinely lost in transit, and when the pusher itself rejects + one — the verifier runs before the sequence machine, so a payload we refuse leaves a hole + exactly like a dropped datagram. A long session outage also produces them, because the pusher + stops draining the socket while it retries. A step back to 0 is a new MVN session, not a gap. +- **Timestamps are two different clocks.** The header's `sample_time_ns` is on MVN's send-host + clock and is deliberately *not* published as the local common clock; the plugin stamps that + itself at publish time and forwards MVN's device clock verbatim alongside it. +- **Nothing is ever dropped on a timestamp.** Both timestamp rules are diagnostics: a sample time + that is not a whole millisecond (`nonWholeMs`) did not come from MVN's solver, and one that + moves backwards (`rewinds`) is a scrub or a recording restart. Neither rejects the frame — the + pose has already passed the structural verifier, and nothing downstream decodes the sample + time. Dropping on either would discard good poses for a metadata fault: rejecting rewinds once + cost 1315 frames of a looped playback, and rejecting sub-millisecond times would stop the robot + the moment MVN's clock granularity changed. `seq` is the authority on frame identity, not the + clock. + +## Outages it survives on its own + +Both of the plugin's dependencies can disappear underneath it and come back. Neither is fatal, and +neither needs an operator: `update()` recovers on a bounded backoff and only throws once a retry +budget is exhausted — at which point the process exits 1 *after* printing its counters, because +those are usually the only record of what led up to it. + +| What goes away | Retry budget | Counter | +|-|-|-| +| The UDP socket fails hard (`recv` errors with anything other than `EAGAIN` / `EWOULDBLOCK` / `EINTR`) | 5 re-binds of the same port over ~2.3 s | `socketRecoveries`, `socketRecoveryFailures` | +| The CloudXR runtime is restarted, so `push_buffer` throws | 8 session re-establishes over ~23.5 s | `pushFailures`, `sessionRecoveries` | + +Two details worth knowing: + +- **A restarted runtime needs a full re-create, not a re-connect.** Once its IPC pipe breaks, the + pusher's collection handle is dead, so recovery tears down the pusher *and* the session before + rebuilding both. A dead runtime arrives as `XR_ERROR_RUNTIME_FAILURE`, not as anything + session-shaped. +- **Stream state deliberately survives an outage.** Neither recovery path resets `seq` tracking: + the outage reappears as a sequence gap or a session reset, and both are already handled. Expect + `seqSkipped` to climb across a long one — the socket is not being drained while the pusher + retries, so the kernel discards what arrives. + +`SIGINT`/`SIGTERM` are observed *inside* the backoffs, so Ctrl-C during a 23-second outage stops +the pusher promptly rather than waiting the budget out. + +**Testing the socket path.** A real hard `recv` error needs the interface to fail underneath you, +so `XSENS_TELEOP_INJECT_RECV_ERRORS=[:]` forces the next *n* receives to fail +(`ENOTCONN` by default). Unset in production; it announces itself loudly when set. + +```bash +# three forced hard errors: expect socketRecoveries=3 and an unbroken stream +XSENS_TELEOP_INJECT_RECV_ERRORS=3 ./xsens_full_body_plugin --address=127.0.0.1 +``` + +The session path needs no hook: `kill` the CloudXR runtime while streaming, and restart it inside +~23.5 s to watch it recover, or leave it down to watch it give up. + +Log lines are throttled per category — first occurrence and every hundredth, tagged `(xN)` — so a +lossy link or an operator scrubbing a recording cannot flood the console from inside the receive +loop. Stream diagnostics go to stdout; problems (bad sender, broken socket, dead runtime) go to +stderr. + +## Tests + +`frame_decision.{hpp,cpp}` holds everything the pusher *decides* about a datagram — framing, the +size check, the structural verifier, the sequence state machine and the timestamp rules — with +none of the I/O it decides it for. It is split out so it can be tested: the plugin itself owns a +socket and an OpenXR session and cannot be constructed without a running CloudXR runtime, so this +logic was previously reachable only through a live end-to-end run. + +`plugin_options.{hpp,cpp}` is split out for the same reason and holds everything the pusher +decides about a *command line*: which form was used, what each flag means, and what is rejected +before anything binds. + +```bash +# from the IsaacTeleop checkout, once configured with -DBUILD_TESTING=ON (see apply.sh) +ctest --test-dir build-py312 -R 'xsens_' --output-on-failure + +# or, with no build tree at all -- neither unit needs one +g++ -std=c++20 -I. -I -I \ + tests/test_frame_decision.cpp frame_decision.cpp -lflatbuffers -o /tmp/t && /tmp/t +g++ -std=c++20 -I. tests/test_plugin_options.cpp plugin_options.cpp -o /tmp/o && /tmp/o +``` + +114 and 120 assertions respectively, no runtime, no socket, no suit, milliseconds to run. The +frame-decision suite is mutation-checked: inverting the stale comparison, the rewind comparison, +the whole-millisecond flag, the oversize check or the verifier, dropping the timeline clear on +session reset, putting the duplicate-`seq`-0 defect back, or collapsing the skipped-sequence +count back to a flag, each make it fail. + +Three frame-decision behaviours it pins that are easy to get wrong by accident: + +- **The verifier runs before the sequence machine, so a payload it rejects never consumes its + sequence number** and the next frame reads as a gap. That is deliberate: an unverified payload + must not be able to advance stream state at all. Everything that gets past the verifier does + consume its number, delivered or not. +- **A session reset clears the timeline**, so the new session's first timestamp is not reported + as a rewind. +- **A repeat of `seq` 0 is a duplicate, not a new session.** Only a step back to 0 from a + *non-zero* seq is a session boundary. The decider therefore holds the last seq it saw rather + than the next one it expects: the two differ by one, and writing the reset test against the + expected value is what produced the defect this test now guards (a duplicate at seq 0 counted + as a reset, cleared the timeline, and got pushed to the robot). + +## Wire format + +One frame per datagram: a 36-byte little-endian header, then the payload. See `teleop_wire.hpp`; +the authority is `bsn_stack/mvn/mvn_studio/src/picofullbody_core/teleop_wire.h`. + +| offset | size | field | +|---|---|---| +| 0 | 4 | magic `"XTLP"` | +| 4 | 2 | version = 1 | +| 6 | 2 | reserved — v1 receivers MUST ignore | +| 8 | 8 | `seq` (uint64) | +| 16 | 8 | `sample_time_ns` (int64, MVN send-host clock) | +| 24 | 8 | `raw_device_time_ns` (int64, MVN device clock) | +| 32 | 4 | `payload_len` (uint32) | + +A live MVN pose is 820 bytes: 36 B header + 784 B payload (16 B table/vtable + 24 × 32 B joint +structs). Meters, **Y-up** (OpenXR), quaternions **xyzw**, absolute global poses. diff --git a/src/plugins/xsens_full_body/frame_decision.cpp b/src/plugins/xsens_full_body/frame_decision.cpp new file mode 100644 index 0000000000..3ab07a9fdb --- /dev/null +++ b/src/plugins/xsens_full_body/frame_decision.cpp @@ -0,0 +1,119 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 Xsens Technologies B.V. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "frame_decision.hpp" + +#include "teleop_wire.hpp" + +#include +#include + +namespace plugins +{ +namespace xsens_full_body +{ + +namespace +{ + +constexpr int64_t NS_PER_MS = 1000000; + +/*! + * @brief Full FlatBuffers bounds check on a payload before it is trusted. + * + * Mandatory, and easy to skip by accident: `deserialize_teleop_frame` validates *framing* only + * and never looks inside the payload. It also cannot use a generated `VerifyFullBodyPoseBuffer`, + * because the schema's `root_type` is `FullBodyPoseRecord` and flatc emits no verifier for a + * non-root table -- hence `VerifyBuffer` by hand. + */ +bool verify_full_body_payload(const uint8_t* data, size_t size) +{ + if (data == nullptr || size == 0) + { + return false; + } + flatbuffers::Verifier verifier(data, size); + return verifier.VerifyBuffer(nullptr); +} + +} // namespace + +FrameOutcome FrameDecider::classify(const uint8_t* datagram, size_t size) +{ + FrameOutcome outcome; + + const auto frame = deserialize_teleop_frame(datagram, size); + if (!frame) + { + outcome.verdict = FrameVerdict::DroppedMalformed; + return outcome; + } + + if (frame->payload_size > max_flatbuffer_size_) + { + outcome.verdict = FrameVerdict::DroppedOversize; + outcome.observed_payload_size = frame->payload_size; + return outcome; + } + + if (!verify_full_body_payload(frame->payload, frame->payload_size)) + { + outcome.verdict = FrameVerdict::DroppedUnverified; + return outcome; + } + + // seq is per-emitter, monotonic and sent-only. It resets to 0 when MVN starts a new session, + // so a step backwards is a session boundary rather than an error. Note a gap is NOT reliable + // evidence of transport loss: MVN also skips seq for frames it declines to build. + if (have_seq_) + { + if (frame->seq == 0 && last_seq_ != 0) + { + // A step back to 0 from a non-zero seq is MVN starting a new session. A repeat of + // seq 0 is NOT that -- it is a duplicate, and falls through to the stale test below. + outcome.session_reset = true; + last_sample_time_ns_ = 0; + } + else if (frame->seq <= last_seq_) + { + outcome.verdict = FrameVerdict::DroppedStale; + return outcome; // duplicate or reordered datagram; state deliberately unchanged + } + else if (frame->seq > last_seq_ + 1) + { + outcome.sequence_numbers_skipped = frame->seq - last_seq_ - 1; + } + } + have_seq_ = true; + last_seq_ = frame->seq; + + // MVN solves at millisecond resolution, so a sub-millisecond sample time means the frame did + // not come from MVN's solver. Flagged, NOT dropped: the timestamp is metadata and the pose + // has already passed the verifier, so rejecting on it would throw away a good pose the + // moment MVN's clock granularity changed. + if (frame->sample_time_ns % NS_PER_MS != 0) + { + outcome.non_whole_ms = true; + } + + // A sample time that moves BACKWARDS is a timeline discontinuity, not a stale frame, and it + // must not be dropped: scrubbing or restarting a recording rewinds MVN's clock while `seq` + // keeps climbing, so rejecting these silently throws away every frame of a looped playback + // (measured: 1315 frames lost in one run before this was fixed). Duplicates and reordering + // are already handled by the `seq` check above, which is the authority on frame identity. + if (frame->sample_time_ns < last_sample_time_ns_) + { + outcome.timeline_rewind = true; + } + last_sample_time_ns_ = frame->sample_time_ns; + + outcome.verdict = FrameVerdict::Deliver; + outcome.payload = frame->payload; + outcome.payload_size = frame->payload_size; + outcome.seq = frame->seq; + outcome.raw_device_time_ns = frame->raw_device_time_ns; + return outcome; +} + +} // namespace xsens_full_body +} // namespace plugins diff --git a/src/plugins/xsens_full_body/frame_decision.hpp b/src/plugins/xsens_full_body/frame_decision.hpp new file mode 100644 index 0000000000..244adbe096 --- /dev/null +++ b/src/plugins/xsens_full_body/frame_decision.hpp @@ -0,0 +1,125 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 Xsens Technologies B.V. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +// Everything the pusher decides about a datagram, with none of the I/O it decides it for. +// +// Split out of the plugin deliberately: the plugin owns a socket and an OpenXR session and +// cannot be constructed without a running CloudXR runtime, so nothing inside it can be unit +// tested. This class needs neither -- bytes in, verdict out -- so the sequence state machine +// and the timestamp rules are covered by `tests/test_frame_decision.cpp`, which runs in +// milliseconds with no runtime, no socket and no suit. +// +// That matters more than usual here. These are branch decisions whose wrong answers look +// entirely plausible: a frame that should have been dropped gets pushed, a session boundary is +// read as a duplicate, a rewind is read as staleness. None of them crash, and none of them show +// up in a live run unless you go looking for the exact sequence that triggers them. + +#include +#include + +namespace plugins +{ +namespace xsens_full_body +{ + +//! What should happen to one datagram. Everything but `Deliver` means "drop it". +enum class FrameVerdict +{ + Deliver, + //! Framing failed: too short, wrong magic, unknown version, or a declared payload length + //! that overruns the datagram. + DroppedMalformed, + //! Payload is bigger than the collection was created for. `push_buffer` would reject it + //! anyway; catching it here lets the caller name the real number. + DroppedOversize, + //! Payload failed the FlatBuffers structural verifier. + DroppedUnverified, + //! Sequence regression: a duplicate or reordered datagram. + DroppedStale, +}; + +//! One classification result. The event flags are not rejections -- a delivered frame can carry +//! a session reset or a timeline rewind -- so the caller counts and logs them independently of +//! the verdict. +struct FrameOutcome +{ + FrameVerdict verdict = FrameVerdict::DroppedMalformed; + + bool session_reset = false; //!< `seq` stepped back to 0: MVN started a new session. + /*! + * @brief How many sequence numbers this frame skipped past; 0 for a contiguous frame. + * + * NOT a transport-loss figure, and it must not be reported as one. A number goes missing + * whenever MVN declines to build that frame (`seq` is sent-only), whenever a datagram is + * genuinely lost in transit, and whenever this decider itself rejects one -- the verifier + * runs before the sequence machine, so a payload we refuse leaves a hole exactly like a + * dropped datagram. It is "sequence numbers that never reached the delivered stream", from + * any cause. + */ + uint64_t sequence_numbers_skipped = 0; + bool timeline_rewind = false; //!< Sample time moved backwards: a scrub or a restart. + /*! + * @brief Sample time is not a whole millisecond, so it did not come from MVN's solver. + * + * A diagnostic, not a rejection. The timestamp is metadata: the pose itself has already + * passed the structural verifier, and nothing downstream decodes the sample time. Dropping + * on it would mean any change to MVN's clock granularity -- a faster solver, a resampled + * playback -- silently stopping the robot with a perfectly good pose on the wire. + */ + bool non_whole_ms = false; + + //! Set when `verdict == Deliver`. `payload` points into the caller's buffer. + const uint8_t* payload = nullptr; + size_t payload_size = 0; + uint64_t seq = 0; + int64_t raw_device_time_ns = 0; + + //! Set when `verdict == DroppedOversize`, so the caller can report the offending size. + size_t observed_payload_size = 0; +}; + +/*! + * @brief The per-datagram decision pipeline, and the stream state it needs to make it. + * + * Not thread-safe and not meant to be: one instance per receiving stream. + */ +class FrameDecider +{ +public: + explicit FrameDecider(size_t max_flatbuffer_size) : max_flatbuffer_size_(max_flatbuffer_size) + { + } + + /*! + * @brief Classify one datagram, advancing stream state. + * + * Order is load-bearing and matches the shipped pusher exactly: framing, then the size + * check, then the structural verifier, then the sequence state machine, then the timestamp + * rules. The verifier runs *before* any state change, so a hostile payload can never + * advance the stream; and the whole-millisecond check runs *after* the sequence machine, so + * a frame rejected on its timestamp has still consumed its sequence number. + * + * @param datagram Raw bytes as received. May be null only when \a size is 0. + * @return The verdict, plus any events observed on the way to it. + */ + FrameOutcome classify(const uint8_t* datagram, size_t size); + +private: + size_t max_flatbuffer_size_; + bool have_seq_ = false; + //! The last sequence number that got through the sequence machine -- NOT the next one + //! expected. Held this way on purpose: the reset, stale and gap tests are all naturally + //! expressed against the last seq, and holding `expected = last + 1` instead is what + //! produced the duplicate-seq-0 defect, where `expected != 0` was written meaning + //! "we have seen a non-zero seq" but was already true after a single frame at seq 0. + //! + //! Note "got through", not "was delivered": a frame rejected on its timestamp has already + //! passed this point and consumed its number. See classify(). + uint64_t last_seq_ = 0; + int64_t last_sample_time_ns_ = 0; +}; + +} // namespace xsens_full_body +} // namespace plugins diff --git a/src/plugins/xsens_full_body/main.cpp b/src/plugins/xsens_full_body/main.cpp new file mode 100644 index 0000000000..827b8c034f --- /dev/null +++ b/src/plugins/xsens_full_body/main.cpp @@ -0,0 +1,140 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 Xsens Technologies B.V. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "plugin_options.hpp" +#include "xsens_full_body_plugin.hpp" + +#include +#include +#include +#include +#include + +using namespace plugins::xsens_full_body; + +namespace +{ + +constexpr uint64_t STATS_EVERY = 250; + +//! Atomic rather than volatile sig_atomic_t: the plugin observes this from inside its recovery +//! backoffs, so a stop request lands during an outage instead of waiting the budget out. +std::atomic g_stop{ false }; + +extern "C" void handle_signal(int) +{ + g_stop.store(true, std::memory_order_relaxed); +} + +void usage(const char* argv0) +{ + const XsensFullBodyOptions defaults; + std::cerr << "Usage: " << argv0 << " [options]\n\n" + << "Receives Xsens MVN Studio's Isaac Teleop UDP stream and republishes it as an\n" + << "Isaac Teleop tensor collection, readable via the `body.xsens` vendor.\n\n" + << "Options:\n" + << " --collection-id=ID Tensor collection id (default: " << defaults.collection_id << ")\n" + << " --address=ADDR Interface to bind, literal IPv4 (default: " << defaults.bind_address + << ")\n" + << " --port=N UDP port to listen on (default: " << defaults.udp_port << ")\n" + << " --max-flatbuffer-size=N Max serialized frame size, must match the reader\n" + << " (default: " << defaults.max_flatbuffer_size << ")\n" + << " --help Show this message\n\n" + << "The defaults match MVN's \"Isaac Teleop\" preset. `--address=127.0.0.1` confines the\n" + << "pusher to loopback; the default accepts the stream on every interface.\n\n" + << "Deprecated: the positional form `[collection_id] [udp_port] [max_flatbuffer_size]`\n" + << "is still accepted, but cannot be mixed with the flags above.\n"; +} + +//! Every counter, on one line. Printed periodically and once more on exit -- including the +//! unrecoverable-outage exit, so a pusher that gave up still says what it saw first. +void print_stats(const XsensFullBodyStats& s, const char* prefix) +{ + std::cout << prefix << "delivered=" << s.delivered << " seq=" << s.last_seq << " size=" << s.last_size + << " fnv1a64=0x" << std::hex << s.last_hash << std::dec << " malformed=" << s.dropped_malformed + << " unverified=" << s.dropped_unverified << " stale=" << s.dropped_stale + << " truncated=" << s.dropped_truncated << " gapEvents=" << s.sequence_gap_events + << " seqSkipped=" << s.sequence_numbers_skipped << " resets=" << s.session_resets + << " rewinds=" << s.timeline_rewinds << " nonWholeMs=" << s.non_whole_ms_samples + << " socketRecoveries=" << s.socket_recoveries << " socketRecoveryFailures=" << s.socket_recovery_failures + << " pushFailures=" << s.push_failures << " sessionRecoveries=" << s.session_recoveries << std::endl; +} + +} // namespace + +int main(int argc, char** argv) +try +{ + XsensFullBodyOptions options; + std::string parse_error; + switch (parse_options(argc, argv, options, parse_error)) + { + case ParseOutcome::HelpRequested: + usage(argv[0]); + return 0; + case ParseOutcome::Error: + std::cerr << argv[0] << ": " << parse_error << "\n\n"; + usage(argv[0]); + return 1; + case ParseOutcome::Ok: + break; + } + + if (options.used_legacy_positionals) + { + std::cerr << argv[0] << ": warning: the positional argument form is deprecated; use" + << " --collection-id=, --port= and --max-flatbuffer-size= instead" << std::endl; + } + + std::signal(SIGINT, handle_signal); + std::signal(SIGTERM, handle_signal); + + std::cout << "Xsens Full Body Pusher (collection: " << options.collection_id << ", tensor: full_body_pose" + << ", udp: " << options.bind_address << ":" << options.udp_port + << ", max_flatbuffer_size: " << options.max_flatbuffer_size << ")" << std::endl; + + XsensFullBodyPlugin plugin(options); + + std::cout << "listening on " << options.bind_address << ":" << options.udp_port << " -> push_buffer" << std::endl; + std::cout << "In MVN Studio: Options -> Network Streamer -> preset \"Isaac Teleop\", tick the row, press Play." + << std::endl; + + // A broken socket or a restarted CloudXR runtime is recovered inside update(). It only throws + // once a retry budget is exhausted, which is the operator-restart case -- and even then the + // counters go out, because they are usually the only record of what led up to it. + int exit_code = 0; + uint64_t since_stats = 0; + try + { + while (!g_stop.load(std::memory_order_relaxed)) + { + if (!plugin.update(g_stop)) + { + continue; + } + if (++since_stats >= STATS_EVERY) + { + since_stats = 0; + print_stats(plugin.stats(), "[XsensFullBody] "); + } + } + } + catch (const std::exception& e) + { + std::cerr << "\n" << argv[0] << ": " << e.what() << std::endl; + exit_code = 1; + } + + print_stats(plugin.stats(), "\nstopped: "); + return exit_code; +} +catch (const std::exception& e) +{ + std::cerr << argv[0] << ": " << e.what() << std::endl; + return 1; +} +catch (...) +{ + std::cerr << argv[0] << ": Unknown error" << std::endl; + return 1; +} diff --git a/src/plugins/xsens_full_body/plugin.yaml b/src/plugins/xsens_full_body/plugin.yaml new file mode 100644 index 0000000000..f277952324 --- /dev/null +++ b/src/plugins/xsens_full_body/plugin.yaml @@ -0,0 +1,17 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 Xsens Technologies B.V. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +name: xsens_full_body +description: "Xsens MVN full-body stream over UDP, republished as an OpenXR tensor collection" +command: "./xsens_full_body_plugin" +version: "0.1.0" +# No --address: the deployed default binds every interface, as it always has. Add +# `--address=127.0.0.1` (or a specific NIC) to narrow which interface accepts the suit stream. +args: + - "--collection-id=xsens_full_body" + - "--port=9764" + - "--max-flatbuffer-size=4096" +devices: + - path: "/mocap/xsens" + type: "xsens_full_body" + description: "Xsens MVN 24-joint full-body pose (hands untracked)" diff --git a/src/plugins/xsens_full_body/plugin_options.cpp b/src/plugins/xsens_full_body/plugin_options.cpp new file mode 100644 index 0000000000..ade8ca9a27 --- /dev/null +++ b/src/plugins/xsens_full_body/plugin_options.cpp @@ -0,0 +1,199 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 Xsens Technologies B.V. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "plugin_options.hpp" + +#include +#include +#include + +#include +#include +#include + +namespace plugins +{ +namespace xsens_full_body +{ + +namespace +{ + +constexpr std::string_view ROOT_ID_FLAG = "--plugin-root-id"; +constexpr size_t MAX_POSITIONALS = 3; + +bool starts_with(std::string_view text, std::string_view prefix) +{ + return text.size() >= prefix.size() && text.compare(0, prefix.size(), prefix) == 0; +} + +//! Strict: rejects trailing garbage ("9764x") and zero, both of which `strtoul` would accept. +bool parse_size(std::string_view text, size_t& out) +{ + const char* begin = text.data(); + const char* end = begin + text.size(); + const auto [ptr, error] = std::from_chars(begin, end, out); + return error == std::errc{} && ptr == end && out > 0; +} + +bool parse_port(std::string_view text, uint16_t& out) +{ + size_t parsed = 0; + if (!parse_size(text, parsed) || parsed > 65535) + { + return false; + } + out = static_cast(parsed); + return true; +} + +//! Literal dotted-quad only -- deliberately not `getaddrinfo`. The socket is AF_INET, a bind +//! address should name an interface rather than a name to look up, and resolving a hostname to +//! an address this host does not own only defers the failure to a confusing EADDRNOTAVAIL. +bool valid_ipv4(const std::string& text) +{ + in_addr parsed{}; + return ::inet_pton(AF_INET, text.c_str(), &parsed) == 1; +} + +ParseOutcome bad_value(const std::string& label, const std::string& value, std::string& error) +{ + error = "invalid " + label + " '" + value + "'"; + return ParseOutcome::Error; +} + +ParseOutcome parse_collection_id(const std::string& value, XsensFullBodyOptions& out, std::string& error) +{ + // An empty rendezvous string would leave the reader hunting a collection that cannot be + // named -- the silent-no-data failure, arrived at from the pusher side. + if (value.empty()) + { + error = "collection_id must not be empty"; + return ParseOutcome::Error; + } + out.collection_id = value; + return ParseOutcome::Ok; +} + +ParseOutcome parse_flag_form(const std::vector& args, XsensFullBodyOptions& out, std::string& error) +{ + for (const std::string& arg : args) + { + if (!starts_with(arg, "--")) + { + error = "unexpected argument '" + arg + "' -- flags and the deprecated positional form cannot be mixed"; + return ParseOutcome::Error; + } + + if (starts_with(arg, "--collection-id=")) + { + if (parse_collection_id(arg.substr(16), out, error) != ParseOutcome::Ok) + { + return ParseOutcome::Error; + } + } + else if (starts_with(arg, "--address=")) + { + const std::string value = arg.substr(10); + if (!valid_ipv4(value)) + { + error = + "invalid --address '" + value + "' -- expected a literal IPv4 address such as 0.0.0.0 or 127.0.0.1"; + return ParseOutcome::Error; + } + out.bind_address = value; + } + else if (starts_with(arg, "--port=")) + { + if (!parse_port(arg.substr(7), out.udp_port)) + { + return bad_value("--port", arg.substr(7), error); + } + } + else if (starts_with(arg, "--max-flatbuffer-size=")) + { + if (!parse_size(arg.substr(22), out.max_flatbuffer_size)) + { + return bad_value("--max-flatbuffer-size", arg.substr(22), error); + } + } + else + { + error = "unknown option '" + arg + "'"; + return ParseOutcome::Error; + } + } + return ParseOutcome::Ok; +} + +ParseOutcome parse_positional_form(const std::vector& args, XsensFullBodyOptions& out, std::string& error) +{ + if (args.size() > MAX_POSITIONALS) + { + error = "expected at most 3 positional arguments -- use the flag form for anything more"; + return ParseOutcome::Error; + } + + out.used_legacy_positionals = true; + + if (parse_collection_id(args[0], out, error) != ParseOutcome::Ok) + { + return ParseOutcome::Error; + } + if (args.size() > 1 && !parse_port(args[1], out.udp_port)) + { + return bad_value("udp_port", args[1], error); + } + if (args.size() > 2 && !parse_size(args[2], out.max_flatbuffer_size)) + { + return bad_value("max_flatbuffer_size", args[2], error); + } + return ParseOutcome::Ok; +} + +} // namespace + +ParseOutcome parse_options(int argc, const char* const* argv, XsensFullBodyOptions& out, std::string& error) +{ + out = XsensFullBodyOptions{}; + error.clear(); + + // --plugin-root-id is stripped first, in both spellings, so neither form below has to think + // about it. The launcher injects it ahead of plugin.yaml's own arguments (see + // core/plugin_manager/cpp/plugin.cpp), so a plugin that does not swallow it here sees a + // stray argument and exits before it ever binds. + std::vector args; + for (int i = 1; i < argc; ++i) + { + const std::string_view arg = argv[i]; + + if (arg == ROOT_ID_FLAG) + { + if (i + 1 >= argc) + { + error = "--plugin-root-id requires a value"; + return ParseOutcome::Error; + } + ++i; + continue; + } + if (starts_with(arg, "--plugin-root-id=")) + { + continue; + } + if (arg == "--help" || arg == "-h") + { + return ParseOutcome::HelpRequested; + } + args.emplace_back(arg); + } + + if (args.empty()) + { + return ParseOutcome::Ok; + } + return starts_with(args.front(), "--") ? parse_flag_form(args, out, error) : parse_positional_form(args, out, error); +} + +} // namespace xsens_full_body +} // namespace plugins diff --git a/src/plugins/xsens_full_body/plugin_options.hpp b/src/plugins/xsens_full_body/plugin_options.hpp new file mode 100644 index 0000000000..e98207db83 --- /dev/null +++ b/src/plugins/xsens_full_body/plugin_options.hpp @@ -0,0 +1,59 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 Xsens Technologies B.V. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include +#include +#include + +namespace plugins +{ +namespace xsens_full_body +{ + +//! Everything the pusher is configured with, and the defaults it runs on when told nothing. +//! +//! Split out from `main.cpp` so the parser below can be unit tested: the plugin itself owns a +//! socket and an OpenXR session and cannot be constructed without a running CloudXR runtime, +//! which previously made argument handling reachable only through a live end-to-end run. Same +//! reason `frame_decision.{hpp,cpp}` is its own unit. +struct XsensFullBodyOptions +{ + //! The string the pusher and the reader rendezvous on. A mismatch fails silently and + //! forever, which is why the parser refuses an empty one. + std::string collection_id = "xsens_full_body"; + //! Interface to bind the receive socket to. `0.0.0.0` accepts the stream on every interface; + //! a literal address narrows it to one, which is also how a run is confined to loopback. + std::string bind_address = "0.0.0.0"; + uint16_t udp_port = 9764; + //! Must match the reader's `max_flatbuffer_size`. A mismatch throws loudly on both sides -- + //! which is the good case; a `collection_id` mismatch instead fails silently and forever. + size_t max_flatbuffer_size = 4096; + + //! Set when the deprecated positional form was used. Parse metadata rather than + //! configuration: the parser stays free of I/O, and the caller decides how to warn. + bool used_legacy_positionals = false; +}; + +enum class ParseOutcome +{ + Ok, + HelpRequested, + Error +}; + +/*! + * @brief Parse the command line into \a out. + * + * Accepts the flag form (`--collection-id=`, `--address=`, `--port=`, `--max-flatbuffer-size=`) + * and, deprecated, the original positional form `[collection_id] [udp_port] + * [max_flatbuffer_size]`. The two cannot be mixed. `--plugin-root-id` is swallowed in both its + * spellings: the plugin launcher injects it ahead of `plugin.yaml`'s own arguments. + * + * @param error filled with an operator-facing message when ParseOutcome::Error is returned. + */ +ParseOutcome parse_options(int argc, const char* const* argv, XsensFullBodyOptions& out, std::string& error); + +} // namespace xsens_full_body +} // namespace plugins diff --git a/src/plugins/xsens_full_body/teleop_wire.hpp b/src/plugins/xsens_full_body/teleop_wire.hpp new file mode 100644 index 0000000000..499f6dfb8e --- /dev/null +++ b/src/plugins/xsens_full_body/teleop_wire.hpp @@ -0,0 +1,125 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 Xsens Technologies B.V. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +// The "XTLP" wire format MVN Studio emits on its Isaac Teleop network-streamer preset. +// +// One frame per UDP datagram: a fixed 36-byte little-endian header followed by `payload_len` +// bytes of a bare `core::FullBodyPose` FlatBuffer. Mirrors MVN's own authority, +// bsn_stack/mvn/mvn_studio/src/picofullbody_core/teleop_wire.h. +// +// offset size field +// 0 4 magic = "XTLP" +// 4 2 version = 1 +// 6 2 reserved -- v1 receivers MUST ignore; any future use needs a version bump +// 8 8 seq (uint64) +// 16 8 sample_time_ns (int64), MVN's send-host clock -- a DIFFERENT clock domain +// 24 8 raw_device_time_ns (int64), MVN's device clock +// 32 4 payload_len (uint32) +// +// Deliberately hand-decoded field by field rather than memcpy'd into a packed struct: the wire +// is little-endian regardless of host, and a struct would silently disagree on a big-endian +// build. Framing only -- this says nothing about whether the payload is a valid FlatBuffer, +// which is why the plugin runs a flatbuffers::Verifier before pushing anything. + +#include +#include +#include +#include + +namespace plugins +{ +namespace xsens_full_body +{ + +constexpr uint32_t TELEOP_WIRE_MAGIC = 0x504C5458u; // 'X','T','L','P' little-endian +constexpr uint16_t TELEOP_WIRE_VERSION = 1; +constexpr size_t TELEOP_WIRE_HEADER_SIZE = 36; + +struct TeleopFrame +{ + uint64_t seq = 0; + int64_t sample_time_ns = 0; + int64_t raw_device_time_ns = 0; + const uint8_t* payload = nullptr; + size_t payload_size = 0; +}; + +namespace detail +{ + +inline uint16_t read_u16_le(const uint8_t* p) +{ + return static_cast(p[0]) | static_cast(static_cast(p[1]) << 8); +} + +inline uint32_t read_u32_le(const uint8_t* p) +{ + return static_cast(p[0]) | (static_cast(p[1]) << 8) | (static_cast(p[2]) << 16) | + (static_cast(p[3]) << 24); +} + +inline uint64_t read_u64_le(const uint8_t* p) +{ + uint64_t value = 0; + for (int i = 7; i >= 0; --i) + { + value = (value << 8) | static_cast(p[static_cast(i)]); + } + return value; +} + +} // namespace detail + +/*! + * @brief Decode the XTLP header and locate the payload inside `data`. + * + * @return The frame, or nullopt when the datagram is not a well-formed XTLP v1 frame. The + * returned `payload` points into `data` and does not outlive it. + */ +inline std::optional deserialize_teleop_frame(const uint8_t* data, size_t size) +{ + if (data == nullptr || size < TELEOP_WIRE_HEADER_SIZE) + { + return std::nullopt; + } + if (detail::read_u32_le(data) != TELEOP_WIRE_MAGIC) + { + return std::nullopt; + } + if (detail::read_u16_le(data + 4) != TELEOP_WIRE_VERSION) + { + return std::nullopt; + } + // bytes [6:8] are `reserved` and are deliberately not read. + + const uint32_t payload_len = detail::read_u32_le(data + 32); + if (TELEOP_WIRE_HEADER_SIZE + static_cast(payload_len) > size) + { + return std::nullopt; + } + + TeleopFrame frame; + frame.seq = detail::read_u64_le(data + 8); + frame.sample_time_ns = static_cast(detail::read_u64_le(data + 16)); + frame.raw_device_time_ns = static_cast(detail::read_u64_le(data + 24)); + frame.payload = data + TELEOP_WIRE_HEADER_SIZE; + frame.payload_size = payload_len; + return frame; +} + +//! FNV-1a over the payload, for cross-checking a frame against another receiver bit for bit. +inline uint64_t fnv1a64(const uint8_t* data, size_t size) +{ + uint64_t hash = 0xcbf29ce484222325ull; + for (size_t i = 0; i < size; ++i) + { + hash ^= static_cast(data[i]); + hash *= 0x100000001b3ull; + } + return hash; +} + +} // namespace xsens_full_body +} // namespace plugins diff --git a/src/plugins/xsens_full_body/tests/test_frame_decision.cpp b/src/plugins/xsens_full_body/tests/test_frame_decision.cpp new file mode 100644 index 0000000000..6f453ca318 --- /dev/null +++ b/src/plugins/xsens_full_body/tests/test_frame_decision.cpp @@ -0,0 +1,529 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 Xsens Technologies B.V. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +// Standalone unit test for the Xsens full-body frame decision pipeline. It builds real XTLP +// datagrams around real `core::FullBodyPose` payloads and asserts what the pusher decides about +// each one, so a sequence-machine or timestamp bug cannot pass silently. +// +// Needs no CloudXR runtime, no socket and no suit -- which is the point: the plugin itself +// cannot be constructed without an OpenXR session, so this logic was previously reachable only +// through a live end-to-end run. +// +// Build & run standalone: +// g++ -std=c++20 -I.. -I -I +// test_frame_decision.cpp ../frame_decision.cpp -lflatbuffers -o t && ./t + +#include "frame_decision.hpp" +#include "teleop_wire.hpp" + +#include + +#include +#include +#include +#include +#include +#include + +using namespace plugins::xsens_full_body; + +namespace +{ + +int g_checks = 0; + +#define CHECK(cond) \ + do \ + { \ + ++g_checks; \ + if (!(cond)) \ + { \ + std::fprintf(stderr, "FAIL %s:%d: %s\n", __FILE__, __LINE__, #cond); \ + std::abort(); \ + } \ + } while (0) + +// --------------------------------------------------------------------------------------------- +// Fixtures: a real 784 B FullBodyPose, and the XTLP framing MVN puts around it. +// --------------------------------------------------------------------------------------------- + +std::vector valid_pose_payload() +{ + flatbuffers::FlatBufferBuilder fbb(1024); + core::BodyJointPose joints[24]; + for (int i = 0; i < 24; ++i) + { + const core::Point pos(0.0f, 0.96f, 0.0f); + const core::Quaternion quat(0.0f, 0.0f, 0.0f, 1.0f); // xyzw identity + // MVN has no finger tracking: LEFT_HAND (22) and RIGHT_HAND (23) are flagged invalid. + joints[i] = core::BodyJointPose(core::Pose(pos, quat), i < 22); + } + const core::BodyJoints body(flatbuffers::span(joints, 24)); + fbb.Finish(core::CreateFullBodyPose(fbb, &body, false)); + return std::vector(fbb.GetBufferPointer(), fbb.GetBufferPointer() + fbb.GetSize()); +} + +void put_u16(std::vector& b, uint16_t v) +{ + b.push_back(uint8_t(v)); + b.push_back(uint8_t(v >> 8)); +} +void put_u32(std::vector& b, uint32_t v) +{ + for (int i = 0; i < 4; ++i) + b.push_back(uint8_t(v >> (8 * i))); +} +void put_u64(std::vector& b, uint64_t v) +{ + for (int i = 0; i < 8; ++i) + b.push_back(uint8_t(v >> (8 * i))); +} + +struct FrameOpts +{ + uint64_t seq = 0; + int64_t sample_time_ns = 0; + int64_t raw_device_time_ns = 0; + uint32_t magic = TELEOP_WIRE_MAGIC; + uint16_t version = TELEOP_WIRE_VERSION; + //! Overrides the declared payload_len without changing the bytes actually appended. + bool lie_about_length = false; + uint32_t declared_length = 0; +}; + +std::vector frame_it(const std::vector& payload, const FrameOpts& o) +{ + std::vector f; + put_u32(f, o.magic); + put_u16(f, o.version); + put_u16(f, 0); // reserved + put_u64(f, o.seq); + put_u64(f, uint64_t(o.sample_time_ns)); + put_u64(f, uint64_t(o.raw_device_time_ns)); + put_u32(f, o.lie_about_length ? o.declared_length : uint32_t(payload.size())); + f.insert(f.end(), payload.begin(), payload.end()); + return f; +} + +//! One well-formed frame at `seq`, timestamped on a whole millisecond. +std::vector good_frame(uint64_t seq, int64_t sample_ms = -1) +{ + FrameOpts o; + o.seq = seq; + o.sample_time_ns = (sample_ms < 0 ? int64_t(seq) * 20 : sample_ms) * 1000000LL; + o.raw_device_time_ns = o.sample_time_ns; + return frame_it(valid_pose_payload(), o); +} + +FrameOutcome classify(FrameDecider& d, const std::vector& f) +{ + return d.classify(f.data(), f.size()); +} + +constexpr size_t MAX_FB = 4096; + +// --------------------------------------------------------------------------------------------- +// Framing +// --------------------------------------------------------------------------------------------- + +void test_framing() +{ + FrameDecider d(MAX_FB); + const std::vector payload = valid_pose_payload(); + CHECK(payload.size() == 784); // the size the vendor row documents; a change here is a schema change + + // A well-formed frame is delivered, and carries the fields the pusher needs. + const std::vector f = good_frame(0); + const FrameOutcome ok = classify(d, f); + CHECK(ok.verdict == FrameVerdict::Deliver); + CHECK(ok.payload_size == 784); + CHECK(ok.seq == 0); + CHECK(ok.raw_device_time_ns == 0); + CHECK(ok.payload == f.data() + TELEOP_WIRE_HEADER_SIZE); // borrowed, not copied + CHECK(!ok.session_reset && ok.sequence_numbers_skipped == 0 && !ok.timeline_rewind); + + // Null and empty. + FrameDecider d2(MAX_FB); + CHECK(d2.classify(nullptr, 0).verdict == FrameVerdict::DroppedMalformed); + CHECK(d2.classify(f.data(), 0).verdict == FrameVerdict::DroppedMalformed); + + // Shorter than the 36-byte header, and exactly one byte short of it. + CHECK(d2.classify(f.data(), 1).verdict == FrameVerdict::DroppedMalformed); + CHECK(d2.classify(f.data(), TELEOP_WIRE_HEADER_SIZE - 1).verdict == FrameVerdict::DroppedMalformed); + + // Header only, declaring a zero-length payload: frames fine, fails the verifier. + FrameOpts empty; + empty.declared_length = 0; + empty.lie_about_length = true; + const std::vector hdr_only = frame_it({}, empty); + CHECK(hdr_only.size() == TELEOP_WIRE_HEADER_SIZE); + CHECK(classify(d2, hdr_only).verdict == FrameVerdict::DroppedUnverified); + + // Wrong magic, wrong version. + FrameOpts bad_magic; + bad_magic.magic = 0xDEADBEEFu; + CHECK(classify(d2, frame_it(payload, bad_magic)).verdict == FrameVerdict::DroppedMalformed); + FrameOpts bad_version; + bad_version.version = 2; + CHECK(classify(d2, frame_it(payload, bad_version)).verdict == FrameVerdict::DroppedMalformed); + + // A declared length that overruns the datagram must not be trusted. + FrameOpts overrun; + overrun.lie_about_length = true; + overrun.declared_length = uint32_t(payload.size() + 1); + CHECK(classify(d2, frame_it(payload, overrun)).verdict == FrameVerdict::DroppedMalformed); + + // Trailing garbage after a truthful length is tolerated: the payload still frames. + std::vector padded = good_frame(0); + padded.push_back(0xFF); + FrameDecider d3(MAX_FB); + CHECK(classify(d3, padded).verdict == FrameVerdict::Deliver); +} + +void test_oversize_and_verify() +{ + // A payload larger than the collection was created for is rejected, and the real size is + // reported so the operator can raise the right number on both sides. + FrameDecider small(256); + const FrameOutcome big = classify(small, good_frame(0)); + CHECK(big.verdict == FrameVerdict::DroppedOversize); + CHECK(big.observed_payload_size == 784); + + // Correctly framed but structurally invalid payload bytes: the verifier is the trust + // boundary, and it must reject before anything downstream can root the buffer. + FrameDecider d(MAX_FB); + const std::vector garbage(784, 0xAB); + FrameOpts o; + CHECK(classify(d, frame_it(garbage, o)).verdict == FrameVerdict::DroppedUnverified); + + // A rejected payload must not have advanced the stream: the next frame at seq 0 is treated + // as a fresh mid-stream attach, not as a duplicate or a session reset. + const FrameOutcome next = classify(d, good_frame(0)); + CHECK(next.verdict == FrameVerdict::Deliver); + CHECK(!next.session_reset && next.sequence_numbers_skipped == 0); +} + +// --------------------------------------------------------------------------------------------- +// Sequence state machine +// --------------------------------------------------------------------------------------------- + +void test_sequence() +{ + // Mid-stream attach: the first verified frame opens the stream at whatever seq it carries. + FrameDecider d(MAX_FB); + const FrameOutcome first = classify(d, good_frame(5000)); + CHECK(first.verdict == FrameVerdict::Deliver); + CHECK(first.sequence_numbers_skipped == 0 && !first.session_reset); + + // Contiguous frames are unremarkable. + for (uint64_t s = 5001; s < 5005; ++s) + { + const FrameOutcome o = classify(d, good_frame(s)); + CHECK(o.verdict == FrameVerdict::Deliver); + CHECK(o.sequence_numbers_skipped == 0 && !o.session_reset && !o.timeline_rewind); + } + + // A forward jump is a gap: counted, but still delivered -- the pose is current. 5005..5009 + // never arrived, so five sequence numbers were skipped. + const FrameOutcome gap = classify(d, good_frame(5010)); + CHECK(gap.verdict == FrameVerdict::Deliver); + CHECK(gap.sequence_numbers_skipped == 5); + + // A regression is stale: dropped, and it must not move the stream on. + CHECK(classify(d, good_frame(5009)).verdict == FrameVerdict::DroppedStale); + const FrameOutcome resumed = classify(d, good_frame(5011)); + CHECK(resumed.verdict == FrameVerdict::Deliver); + CHECK(resumed.sequence_numbers_skipped == 0); // 5011 follows 5010: the stale frame changed nothing + + // An exact duplicate of the last delivered frame is stale too. + CHECK(classify(d, good_frame(5011)).verdict == FrameVerdict::DroppedStale); +} + +void test_session_reset() +{ + FrameDecider d(MAX_FB); + CHECK(classify(d, good_frame(500)).verdict == FrameVerdict::Deliver); + + // seq stepping back to 0 after a non-zero run is MVN starting a new session, not an error: + // flagged, and delivered. + const FrameOutcome reset = classify(d, good_frame(0)); + CHECK(reset.verdict == FrameVerdict::Deliver); + CHECK(reset.session_reset); + CHECK(reset.sequence_numbers_skipped == 0); // a reset is not a gap + + // The new session continues normally from 1. + const FrameOutcome after = classify(d, good_frame(1)); + CHECK(after.verdict == FrameVerdict::Deliver); + CHECK(!after.session_reset && after.sequence_numbers_skipped == 0); + + // A reset also clears the timeline, so the new session's first timestamp -- which is far + // behind the old session's -- is not reported as a rewind. + FrameDecider d2(MAX_FB); + CHECK(classify(d2, good_frame(500, 10000)).verdict == FrameVerdict::Deliver); + const FrameOutcome fresh = classify(d2, good_frame(0, 0)); + CHECK(fresh.verdict == FrameVerdict::Deliver); + CHECK(fresh.session_reset); + CHECK(!fresh.timeline_rewind); +} + +void test_duplicate_seq_zero_is_stale_not_a_reset() +{ + FrameDecider d(MAX_FB); + CHECK(classify(d, good_frame(0, 100)).verdict == FrameVerdict::Deliver); + + const FrameOutcome dup = classify(d, good_frame(0, 100)); + CHECK(dup.verdict == FrameVerdict::DroppedStale); + CHECK(!dup.session_reset); + + // The duplicate must not have disturbed the stream: the next frame follows on normally, + // with no gap and -- because the timeline was never cleared -- no spurious rewind. + const FrameOutcome next = classify(d, good_frame(1, 120)); + CHECK(next.verdict == FrameVerdict::Deliver); + CHECK(next.sequence_numbers_skipped == 0 && !next.session_reset && !next.timeline_rewind); + + // Several duplicates in a row are all stale, not a burst of session resets. + FrameDecider d2(MAX_FB); + CHECK(classify(d2, good_frame(0)).verdict == FrameVerdict::Deliver); + for (int i = 0; i < 3; ++i) + { + const FrameOutcome o = classify(d2, good_frame(0)); + CHECK(o.verdict == FrameVerdict::DroppedStale); + CHECK(!o.session_reset); + } + + // A genuine reset -- a non-zero run stepping back to 0 -- still reads as a reset. This is + // the case that always worked, which is why the defect never showed up in a live run. + FrameDecider d3(MAX_FB); + CHECK(classify(d3, good_frame(1)).verdict == FrameVerdict::Deliver); + CHECK(classify(d3, good_frame(2)).verdict == FrameVerdict::Deliver); + const FrameOutcome real_reset = classify(d3, good_frame(0)); + CHECK(real_reset.verdict == FrameVerdict::Deliver); + CHECK(real_reset.session_reset); + + // And a reset back to 0 from a run that started at 0 is still a reset -- the distinguishing + // fact is the seq we last saw, not where the session began. + FrameDecider d4(MAX_FB); + CHECK(classify(d4, good_frame(0)).verdict == FrameVerdict::Deliver); + CHECK(classify(d4, good_frame(1)).verdict == FrameVerdict::Deliver); + CHECK(classify(d4, good_frame(0)).session_reset); +} + +// --------------------------------------------------------------------------------------------- +// Timestamp rules +// --------------------------------------------------------------------------------------------- + +void test_whole_millisecond_rule() +{ + // A sub-millisecond sample time means the frame did not come from MVN's solver. It is + // FLAGGED, not dropped: the timestamp is metadata, the pose has already passed the + // verifier, and rejecting on it would stop the robot the moment MVN's clock granularity + // changed (a faster solver, a resampled playback) with a good pose on the wire. + FrameDecider d(MAX_FB); + CHECK(classify(d, good_frame(0)).verdict == FrameVerdict::Deliver); + + FrameOpts o; + o.seq = 1; + o.sample_time_ns = 20 * 1000000LL + 1; // one nanosecond off a whole millisecond + const FrameOutcome odd = classify(d, frame_it(valid_pose_payload(), o)); + CHECK(odd.verdict == FrameVerdict::Deliver); + CHECK(odd.non_whole_ms); + CHECK(odd.payload_size == 784); // and it carries a real pose, which is the whole point + + // A whole-millisecond frame is not flagged. + const FrameOutcome even = classify(d, good_frame(2)); + CHECK(even.verdict == FrameVerdict::Deliver); + CHECK(!even.non_whole_ms); + CHECK(even.sequence_numbers_skipped == 0); // and seq 1 was consumed normally + + // Zero is a whole number of milliseconds; the very first frame of a session must not be + // flagged just for being at t=0. + FrameDecider d2(MAX_FB); + const FrameOutcome first = classify(d2, good_frame(0, 0)); + CHECK(first.verdict == FrameVerdict::Deliver); + CHECK(!first.non_whole_ms); + + // Because a flagged frame is delivered, its timestamp becomes the new reference like any + // other -- so a later, earlier frame is judged against it and reads as a rewind. + FrameDecider d3(MAX_FB); + CHECK(classify(d3, good_frame(0, 100)).verdict == FrameVerdict::Deliver); + FrameOpts ahead; + ahead.seq = 1; + ahead.sample_time_ns = 500 * 1000000LL + 7; // t = 500 ms + 7 ns, flagged but delivered + const FrameOutcome flagged = classify(d3, frame_it(valid_pose_payload(), ahead)); + CHECK(flagged.verdict == FrameVerdict::Deliver); + CHECK(flagged.non_whole_ms); + CHECK(!flagged.timeline_rewind); // 500 > 100 + const FrameOutcome back = classify(d3, good_frame(2, 200)); // t = 200 ms, behind the 500 + CHECK(back.verdict == FrameVerdict::Deliver); + CHECK(back.timeline_rewind); + + // A frame can be flagged and a rewind at the same time; the two are independent. + FrameDecider d4(MAX_FB); + CHECK(classify(d4, good_frame(0, 1000)).verdict == FrameVerdict::Deliver); + FrameOpts both; + both.seq = 1; + both.sample_time_ns = 500 * 1000000LL + 3; + const FrameOutcome b = classify(d4, frame_it(valid_pose_payload(), both)); + CHECK(b.verdict == FrameVerdict::Deliver); + CHECK(b.non_whole_ms); + CHECK(b.timeline_rewind); +} + +void test_timeline_rewind() +{ + FrameDecider d(MAX_FB); + CHECK(classify(d, good_frame(0, 1000)).verdict == FrameVerdict::Deliver); + + // Scrubbing or restarting a recording rewinds MVN's clock while seq keeps climbing. The + // frame is current and MUST be delivered -- dropping these lost 1315 frames of a looped + // playback in one measured run. + const FrameOutcome rewind = classify(d, good_frame(1, 500)); + CHECK(rewind.verdict == FrameVerdict::Deliver); + CHECK(rewind.timeline_rewind); + CHECK(!rewind.session_reset); + + // The timeline follows the rewind rather than latching at the old high-water mark, so + // continuing forward from the new position is unremarkable. + const FrameOutcome after = classify(d, good_frame(2, 520)); + CHECK(after.verdict == FrameVerdict::Deliver); + CHECK(!after.timeline_rewind); + + // An identical timestamp is not a rewind (the comparison is strictly less-than). + const FrameOutcome same = classify(d, good_frame(3, 520)); + CHECK(same.verdict == FrameVerdict::Deliver); + CHECK(!same.timeline_rewind); +} + +void test_gap_accounting() +{ + // The two figures answer different questions, and this is the case that shows why one + // number cannot do both: the same total loss, in two very different shapes. + struct Tally + { + int events = 0; + uint64_t skipped = 0; + void operator+=(const FrameOutcome& o) + { + if (o.sequence_numbers_skipped > 0) + { + ++events; + skipped += o.sequence_numbers_skipped; + } + } + }; + + // One long dropout: the link died for a moment and came back. + Tally burst; + FrameDecider d1(MAX_FB); + burst += classify(d1, good_frame(0)); + burst += classify(d1, good_frame(9)); + CHECK(burst.events == 1); + CHECK(burst.skipped == 8); + + // Steady every-other-frame loss: the link is up but lossy. Same order of frames missing, + // an entirely different fault. + Tally lossy; + FrameDecider d2(MAX_FB); + for (uint64_t seq = 0; seq <= 16; seq += 2) + lossy += classify(d2, good_frame(seq)); + CHECK(lossy.events == 8); + CHECK(lossy.skipped == 8); + + // Identical under "frames missing", four-to-one apart under "how often continuity broke". + // Reporting only one of these is what let the same field name mean different things. + CHECK(burst.skipped == lossy.skipped); + CHECK(burst.events != lossy.events); + + // A frame this decider rejects itself also leaves a hole, because the verifier runs before + // the sequence machine. The count is honest about what it is -- sequence numbers that never + // reached the delivered stream -- but that is NOT the same as transport loss, and it must + // not be reported as a link figure. + FrameDecider d3(MAX_FB); + CHECK(classify(d3, good_frame(0)).verdict == FrameVerdict::Deliver); + FrameOpts corrupt; + corrupt.seq = 1; + corrupt.sample_time_ns = 20 * 1000000LL; + CHECK(classify(d3, frame_it(std::vector(784, 0x5A), corrupt)).verdict == FrameVerdict::DroppedUnverified); + const FrameOutcome after = classify(d3, good_frame(2)); + CHECK(after.verdict == FrameVerdict::Deliver); + CHECK(after.sequence_numbers_skipped == 1); // seq 1: we saw it, we refused it, it still counts + + // By contrast a frame that is merely *flagged* -- a non-whole-millisecond sample time -- is + // delivered, so it consumes its number and leaves no hole at all. + FrameDecider d4(MAX_FB); + CHECK(classify(d4, good_frame(0)).verdict == FrameVerdict::Deliver); + FrameOpts odd; + odd.seq = 1; + odd.sample_time_ns = 20 * 1000000LL + 1; + const FrameOutcome flagged = classify(d4, frame_it(valid_pose_payload(), odd)); + CHECK(flagged.verdict == FrameVerdict::Deliver); + CHECK(flagged.non_whole_ms); + CHECK(classify(d4, good_frame(2)).sequence_numbers_skipped == 0); +} + +void test_a_realistic_session() +{ + // One pass with everything at once: attach mid-stream, lose some frames, get a bad datagram, + // scrub backwards, then have MVN restart. Nothing here should reject a usable pose. + FrameDecider d(MAX_FB); + int delivered = 0, gap_events = 0, resets = 0, rewinds = 0, dropped = 0; + uint64_t skipped = 0; + + const auto feed = [&](const std::vector& f) + { + const FrameOutcome o = classify(d, f); + if (o.verdict == FrameVerdict::Deliver) + ++delivered; + else + ++dropped; + if (o.sequence_numbers_skipped > 0) + { + ++gap_events; + skipped += o.sequence_numbers_skipped; + } + resets += o.session_reset; + rewinds += o.timeline_rewind; + }; + + for (uint64_t s = 900; s < 905; ++s) + feed(good_frame(s, int64_t(s) * 20)); + feed(good_frame(920, 920 * 20)); // gap: frames lost in transit + FrameOpts corrupt; + corrupt.seq = 921; + corrupt.sample_time_ns = 921 * 20 * 1000000LL; + feed(frame_it(std::vector(784, 0x00), corrupt)); // payload fails the verifier + feed(good_frame(922, 400)); // operator scrubbed backwards + feed(good_frame(0, 0)); // MVN restarted: new session + feed(good_frame(1, 20)); + + CHECK(delivered == 9); + CHECK(dropped == 1); // only the corrupt payload + // Two gaps, not one -- and the second is worth understanding. The verifier runs BEFORE the + // sequence machine, so the corrupt frame at 921 never consumed its sequence number and 922 + // reads as a jump. A frame rejected on its *timestamp* behaves the opposite way (see + // test_whole_millisecond_rule), because that check runs after. The asymmetry is deliberate: + // an unverified payload must not be allowed to advance stream state at all. + CHECK(gap_events == 2); + // 905..919 is fifteen, plus the single number the corrupt frame at 921 left behind. + CHECK(skipped == 16); + CHECK(resets == 1); + CHECK(rewinds == 1); +} + +} // namespace + +int main() +{ + test_framing(); + test_oversize_and_verify(); + test_sequence(); + test_session_reset(); + test_duplicate_seq_zero_is_stale_not_a_reset(); + test_whole_millisecond_rule(); + test_timeline_rewind(); + test_gap_accounting(); + test_a_realistic_session(); + + std::printf("test_frame_decision: %d checks passed\n", g_checks); + return 0; +} diff --git a/src/plugins/xsens_full_body/tests/test_plugin_options.cpp b/src/plugins/xsens_full_body/tests/test_plugin_options.cpp new file mode 100644 index 0000000000..b0e99b7940 --- /dev/null +++ b/src/plugins/xsens_full_body/tests/test_plugin_options.cpp @@ -0,0 +1,275 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 Xsens Technologies B.V. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +// Standalone unit test for the Xsens full-body pusher's argument parsing. It asserts what each +// command line resolves to, so a defaulting, validation or precedence bug cannot pass silently. +// +// Needs no CloudXR runtime, no socket and no suit -- which is the point: the plugin itself +// cannot be constructed without an OpenXR session, so argument handling was previously +// reachable only through a live end-to-end run. Same reason test_frame_decision.cpp exists. +// +// Build & run standalone: +// g++ -std=c++20 -I.. test_plugin_options.cpp ../plugin_options.cpp -o t && ./t + +#include "plugin_options.hpp" + +#include +#include +#include +#include +#include + +using namespace plugins::xsens_full_body; + +namespace +{ + +int g_checks = 0; + +#define CHECK(cond) \ + do \ + { \ + ++g_checks; \ + if (!(cond)) \ + { \ + std::fprintf(stderr, "FAIL %s:%d: %s\n", __FILE__, __LINE__, #cond); \ + std::abort(); \ + } \ + } while (0) + +// --------------------------------------------------------------------------------------------- +// Harness: argv[0] is prepended for us, because a real one is always present and forgetting it +// silently shifts every argument by one. +// --------------------------------------------------------------------------------------------- + +struct Parsed +{ + ParseOutcome outcome; + XsensFullBodyOptions options; + std::string error; +}; + +Parsed parse(std::initializer_list args) +{ + std::vector argv{ "xsens_full_body_plugin" }; + argv.insert(argv.end(), args.begin(), args.end()); + + Parsed result; + result.outcome = parse_options(static_cast(argv.size()), argv.data(), result.options, result.error); + return result; +} + +//! An error, and one whose message names \a needle -- an operator who cannot see which argument +//! was rejected has to bisect the command line by hand. +void check_error_naming(const Parsed& p, const std::string& needle) +{ + CHECK(p.outcome == ParseOutcome::Error); + CHECK(p.error.find(needle) != std::string::npos); +} + +// --------------------------------------------------------------------------------------------- + +void test_defaults() +{ + const Parsed p = parse({}); + CHECK(p.outcome == ParseOutcome::Ok); + CHECK(p.options.collection_id == "xsens_full_body"); + CHECK(p.options.bind_address == "0.0.0.0"); + CHECK(p.options.udp_port == 9764); + CHECK(p.options.max_flatbuffer_size == 4096); + CHECK(!p.options.used_legacy_positionals); + CHECK(p.error.empty()); +} + +void test_each_flag_alone() +{ + const Parsed collection = parse({ "--collection-id=other" }); + CHECK(collection.outcome == ParseOutcome::Ok); + CHECK(collection.options.collection_id == "other"); + // The untouched fields keep their defaults rather than being reset alongside. + CHECK(collection.options.bind_address == "0.0.0.0"); + CHECK(collection.options.udp_port == 9764); + + const Parsed address = parse({ "--address=127.0.0.1" }); + CHECK(address.outcome == ParseOutcome::Ok); + CHECK(address.options.bind_address == "127.0.0.1"); + + const Parsed port = parse({ "--port=1" }); + CHECK(port.outcome == ParseOutcome::Ok); + CHECK(port.options.udp_port == 1); + + const Parsed size = parse({ "--max-flatbuffer-size=16384" }); + CHECK(size.outcome == ParseOutcome::Ok); + CHECK(size.options.max_flatbuffer_size == 16384); +} + +void test_all_flags_together() +{ + const Parsed p = + parse({ "--collection-id=rig2", "--address=10.1.2.3", "--port=65535", "--max-flatbuffer-size=8192" }); + CHECK(p.outcome == ParseOutcome::Ok); + CHECK(p.options.collection_id == "rig2"); + CHECK(p.options.bind_address == "10.1.2.3"); + CHECK(p.options.udp_port == 65535); + CHECK(p.options.max_flatbuffer_size == 8192); + CHECK(!p.options.used_legacy_positionals); +} + +//! Regression: the launcher injects --plugin-root-id ahead of plugin.yaml's own arguments, and +//! a plugin that does not swallow it exits before it ever binds. That is the bug this fixes, +//! so every position it can arrive in is pinned. +void test_plugin_root_id_is_swallowed() +{ + const Parsed alone = parse({ "--plugin-root-id=abc" }); + CHECK(alone.outcome == ParseOutcome::Ok); + CHECK(alone.options.collection_id == "xsens_full_body"); + CHECK(!alone.options.used_legacy_positionals); + + // The real launcher shape: injected first, then the yaml args. + const Parsed launcher = parse({ "--plugin-root-id=abc", "--collection-id=rig", "--port=9000" }); + CHECK(launcher.outcome == ParseOutcome::Ok); + CHECK(launcher.options.collection_id == "rig"); + CHECK(launcher.options.udp_port == 9000); + + const Parsed interleaved = parse({ "--collection-id=rig", "--plugin-root-id=abc", "--port=9000" }); + CHECK(interleaved.outcome == ParseOutcome::Ok); + CHECK(interleaved.options.collection_id == "rig"); + CHECK(interleaved.options.udp_port == 9000); + + // Space-separated form: the value must not leak through as a positional. + const Parsed spaced = parse({ "--plugin-root-id", "abc", "--port=9000" }); + CHECK(spaced.outcome == ParseOutcome::Ok); + CHECK(spaced.options.udp_port == 9000); + CHECK(!spaced.options.used_legacy_positionals); + + // Ahead of the deprecated positional form, it must not be taken for the collection id. + const Parsed with_positionals = parse({ "--plugin-root-id=abc", "rig", "9000", "4096" }); + CHECK(with_positionals.outcome == ParseOutcome::Ok); + CHECK(with_positionals.options.collection_id == "rig"); + CHECK(with_positionals.options.udp_port == 9000); + CHECK(with_positionals.options.used_legacy_positionals); + + check_error_naming(parse({ "--plugin-root-id" }), "--plugin-root-id"); +} + +void test_legacy_positionals() +{ + const Parsed one = parse({ "rig" }); + CHECK(one.outcome == ParseOutcome::Ok); + CHECK(one.options.collection_id == "rig"); + CHECK(one.options.udp_port == 9764); + CHECK(one.options.max_flatbuffer_size == 4096); + CHECK(one.options.used_legacy_positionals); + + const Parsed two = parse({ "rig", "9000" }); + CHECK(two.outcome == ParseOutcome::Ok); + CHECK(two.options.udp_port == 9000); + CHECK(two.options.max_flatbuffer_size == 4096); + + // The invocation the README and run/pipeline.sh have always used. + const Parsed three = parse({ "xsens_full_body", "9764", "4096" }); + CHECK(three.outcome == ParseOutcome::Ok); + CHECK(three.options.collection_id == "xsens_full_body"); + CHECK(three.options.udp_port == 9764); + CHECK(three.options.max_flatbuffer_size == 4096); + CHECK(three.options.used_legacy_positionals); + // No positional ever sets the bind address -- that is flag-only by design. + CHECK(three.options.bind_address == "0.0.0.0"); + + CHECK(parse({ "rig", "9764", "4096", "extra" }).outcome == ParseOutcome::Error); +} + +//! Mixing is an error rather than a precedence rule: a silently ignored argument here is a +//! pusher listening somewhere the operator did not ask for. +void test_flags_and_positionals_do_not_mix() +{ + check_error_naming(parse({ "--port=9000", "rig" }), "rig"); + // Leading positional wins the form, so the flag lands in the positional parser as a fourth + // argument or an unparsable port -- either way it must not be silently applied. + CHECK(parse({ "rig", "--port=9000" }).outcome == ParseOutcome::Error); + CHECK(parse({ "rig", "9000", "4096", "--address=127.0.0.1" }).outcome == ParseOutcome::Error); +} + +void test_invalid_port() +{ + for (const char* bad : { "--port=0", "--port=65536", "--port=abc", "--port=9764x", "--port=-1", "--port=" }) + { + check_error_naming(parse({ bad }), "--port"); + } + check_error_naming(parse({ "rig", "70000" }), "udp_port"); +} + +void test_invalid_max_flatbuffer_size() +{ + for (const char* bad : { "--max-flatbuffer-size=0", "--max-flatbuffer-size=abc", "--max-flatbuffer-size=4096b", + "--max-flatbuffer-size=" }) + { + check_error_naming(parse({ bad }), "--max-flatbuffer-size"); + } + check_error_naming(parse({ "rig", "9764", "0" }), "max_flatbuffer_size"); +} + +//! An unvalidated bind address fails as silent no-data, the same signature as a collection_id +//! mismatch -- so every near-miss shape is rejected at parse time instead. +void test_invalid_address() +{ + for (const char* bad : + { "--address=999.1.1.1", "--address=localhost", "--address=", "--address=::1", "--address=127.0.0.1.1", + "--address=127.0.0", "--address=127.0.0.1:9764", "--address=1.2.3.4 " }) + { + check_error_naming(parse({ bad }), "--address"); + } + + // Accepted shapes, including the two that matter operationally. + for (const char* good : { "--address=0.0.0.0", "--address=127.0.0.1", "--address=255.255.255.255" }) + { + CHECK(parse({ good }).outcome == ParseOutcome::Ok); + } +} + +void test_empty_collection_id() +{ + check_error_naming(parse({ "--collection-id=" }), "collection_id"); + check_error_naming(parse({ "" }), "collection_id"); +} + +void test_unknown_flag() +{ + check_error_naming(parse({ "--frobnicate" }), "--frobnicate"); + check_error_naming(parse({ "--port=9000", "--frobnicate=1" }), "--frobnicate"); + // A near miss on a real flag is still unknown, not a partial match. + check_error_naming(parse({ "--adress=127.0.0.1" }), "--adress"); + check_error_naming(parse({ "--port" }), "--port"); +} + +void test_help() +{ + CHECK(parse({ "--help" }).outcome == ParseOutcome::HelpRequested); + CHECK(parse({ "-h" }).outcome == ParseOutcome::HelpRequested); + CHECK(parse({ "--port=9000", "--help" }).outcome == ParseOutcome::HelpRequested); + CHECK(parse({ "rig", "--help" }).outcome == ParseOutcome::HelpRequested); + // Help wins over an otherwise-fatal command line: asking what the flags are is exactly what + // an operator does after getting one wrong. + CHECK(parse({ "--frobnicate", "--help" }).outcome == ParseOutcome::HelpRequested); +} + +} // namespace + +int main() +{ + test_defaults(); + test_each_flag_alone(); + test_all_flags_together(); + test_plugin_root_id_is_swallowed(); + test_legacy_positionals(); + test_flags_and_positionals_do_not_mix(); + test_invalid_port(); + test_invalid_max_flatbuffer_size(); + test_invalid_address(); + test_empty_collection_id(); + test_unknown_flag(); + test_help(); + + std::printf("test_plugin_options: %d checks passed\n", g_checks); + return 0; +} diff --git a/src/plugins/xsens_full_body/xsens_full_body_plugin.cpp b/src/plugins/xsens_full_body/xsens_full_body_plugin.cpp new file mode 100644 index 0000000000..eef4c18436 --- /dev/null +++ b/src/plugins/xsens_full_body/xsens_full_body_plugin.cpp @@ -0,0 +1,432 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 Xsens Technologies B.V. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "xsens_full_body_plugin.hpp" + +#include "frame_decision.hpp" +#include "teleop_wire.hpp" + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace plugins +{ +namespace xsens_full_body +{ + +namespace +{ + +constexpr std::string_view TENSOR_IDENTIFIER = "full_body_pose"; +//! Bounded so a stalled MVN cannot wedge the loop; the caller just gets a false from update(). +constexpr int RECV_TIMEOUT_MS = 250; +//! MVN's datagram is 820 B. Larger than the 65507 B maximum IPv4 UDP payload, so a valid +//! datagram is never truncated and the MSG_TRUNC check below is belt and braces. +constexpr size_t RECV_BUFFER_SIZE = 64 * 1024; + +//! Log every event of a category on its 1st and every 100th occurrence. +constexpr uint64_t LOG_EVERY = 100; + +//! Socket re-bind budget: ~2.3 s total. A hard recv error is almost always transient (an +//! interface bounce); anything longer than this is a machine problem, not a blip. +constexpr int SOCKET_BACKOFF_MS[] = { 100, 200, 400, 800, 800 }; +constexpr int SOCKET_RECOVERY_ATTEMPTS = static_cast(std::size(SOCKET_BACKOFF_MS)); + +//! Session re-establish budget: ~23.5 s total. Sized for a CloudXR runtime restart, which is +//! slow -- the runtime has to come up and re-advertise its extensions before we can bind again. +constexpr int SESSION_BACKOFF_MS[] = { 500, 1000, 2000, 4000, 4000, 4000, 4000, 4000 }; +constexpr int SESSION_RECOVERY_ATTEMPTS = static_cast(std::size(SESSION_BACKOFF_MS)); + +//! Backoffs are slept in slices so a stop request during a long outage lands promptly. +constexpr int BACKOFF_SLICE_MS = 50; + +} // namespace + +XsensFullBodyPlugin::XsensFullBodyPlugin(const XsensFullBodyOptions& options) + : collection_id_(options.collection_id), + max_flatbuffer_size_(options.max_flatbuffer_size), + buffer_(RECV_BUFFER_SIZE), + decider_(options.max_flatbuffer_size) +{ + read_recv_error_injection(); + + // Session before socket: if the CloudXR runtime is absent we fail before taking the port, + // so a retried start does not collide with itself. + establish_session(); + open_socket(options.bind_address, options.udp_port); +} + +void XsensFullBodyPlugin::read_recv_error_injection() +{ + const char* spec = std::getenv("XSENS_TELEOP_INJECT_RECV_ERRORS"); + if (spec == nullptr) + { + return; + } + // "" or ":"; ENOTCONN by default because it is unambiguously hard. + char* rest = nullptr; + const long count = std::strtol(spec, &rest, 10); + inject_recv_errors_left_ = (count > 0) ? static_cast(count) : 0; + inject_recv_errno_ = + (rest != nullptr && *rest == ':') ? static_cast(std::strtol(rest + 1, nullptr, 10)) : ENOTCONN; + if (inject_recv_errors_left_ > 0) + { + std::cerr << "[XsensFullBody] TEST HOOK: forcing the next " << inject_recv_errors_left_ + << " recv call(s) to fail with errno " << inject_recv_errno_ << " (" + << std::strerror(inject_recv_errno_) << ")" << std::endl; + } +} + +XsensFullBodyPlugin::~XsensFullBodyPlugin() +{ + close_socket(); +} + +void XsensFullBodyPlugin::establish_session() +{ + // Torn down first, and in this order: once the runtime's IPC pipe breaks, the pusher's + // collection handle is dead and only a full re-create works. + pusher_.reset(); + session_.reset(); + + session_ = + std::make_shared("XsensFullBodyPlugin", core::SchemaPusher::get_required_extensions()); + pusher_.emplace( + session_->get_handles(), core::SchemaPusherConfig{ .collection_id = collection_id_, + .max_flatbuffer_size = max_flatbuffer_size_, + .tensor_identifier = std::string(TENSOR_IDENTIFIER), + .localized_name = "Xsens Full Body", + .app_name = "XsensFullBodyPlugin" }); +} + +void XsensFullBodyPlugin::open_socket(const std::string& address, uint16_t port) +{ + in_addr bind_addr{}; + if (::inet_pton(AF_INET, address.c_str(), &bind_addr) != 1) + { + // Validated at parse time (see plugin_options.cpp), so reaching here is a programmer + // error rather than operator input. + throw std::runtime_error("XsensFullBodyPlugin: '" + address + "' is not a literal IPv4 address"); + } + + socket_fd_ = ::socket(AF_INET, SOCK_DGRAM, 0); + if (socket_fd_ < 0) + { + throw std::runtime_error(std::string("XsensFullBodyPlugin: socket() failed: ") + std::strerror(errno)); + } + + int reuse = 1; + if (::setsockopt(socket_fd_, SOL_SOCKET, SO_REUSEADDR, &reuse, sizeof(reuse)) < 0) + { + close_socket(); + throw std::runtime_error(std::string("XsensFullBodyPlugin: SO_REUSEADDR failed: ") + std::strerror(errno)); + } + + timeval timeout{}; + timeout.tv_sec = RECV_TIMEOUT_MS / 1000; + timeout.tv_usec = (RECV_TIMEOUT_MS % 1000) * 1000; + if (::setsockopt(socket_fd_, SOL_SOCKET, SO_RCVTIMEO, &timeout, sizeof(timeout)) < 0) + { + close_socket(); + throw std::runtime_error(std::string("XsensFullBodyPlugin: SO_RCVTIMEO failed: ") + std::strerror(errno)); + } + + sockaddr_in endpoint{}; + endpoint.sin_family = AF_INET; + endpoint.sin_addr = bind_addr; + endpoint.sin_port = htons(port); + if (::bind(socket_fd_, reinterpret_cast(&endpoint), sizeof(endpoint)) < 0) + { + // Two failures dominate here and they have opposite fixes, so the hint follows errno + // rather than guessing. With a non-default --address, the second is the common one. + const int reason_errno = errno; + const std::string reason = std::strerror(reason_errno); + const char* hint = ""; + if (reason_errno == EADDRINUSE) + { + hint = " (is another pusher already running?)"; + } + else if (reason_errno == EADDRNOTAVAIL) + { + hint = " (no interface on this host has that address)"; + } + close_socket(); + throw std::runtime_error("XsensFullBodyPlugin: bind to UDP " + address + ":" + std::to_string(port) + + " failed: " + reason + hint); + } + bind_address_ = address; + port_ = port; +} + +void XsensFullBodyPlugin::close_socket() +{ + if (socket_fd_ >= 0) + { + ::close(socket_fd_); + socket_fd_ = -1; + } +} + +void XsensFullBodyPlugin::log_rate_limited(LogCategory category, const std::string& message, std::ostream& out) +{ + const uint64_t n = ++log_counts_[category]; + if (n == 1 || (n % LOG_EVERY) == 0) + { + out << "[XsensFullBody] " << message; + if (n > 1) + { + out << " (x" << n << ")"; + } + out << std::endl; + } +} + +bool XsensFullBodyPlugin::wait_unless_stopped(int total_ms, const std::atomic& stop) +{ + for (int remaining = total_ms; remaining > 0; remaining -= BACKOFF_SLICE_MS) + { + if (stop.load(std::memory_order_relaxed)) + { + return false; + } + std::this_thread::sleep_for( + std::chrono::milliseconds(remaining < BACKOFF_SLICE_MS ? remaining : BACKOFF_SLICE_MS)); + } + return !stop.load(std::memory_order_relaxed); +} + +bool XsensFullBodyPlugin::recover_socket(const std::atomic& stop) +{ + // Without a port, re-binding would take an ephemeral one the sender cannot reach -- which + // looks like a working pusher that never receives anything. Refuse instead. Zero here means + // the first bind never succeeded, since both members are assigned only on success. + if (port_ == 0) + { + ++stats_.socket_recovery_failures; + return false; + } + + for (int attempt = 0; attempt < SOCKET_RECOVERY_ATTEMPTS; ++attempt) + { + if (!wait_unless_stopped(SOCKET_BACKOFF_MS[attempt], stop)) + { + return false; + } + + close_socket(); // the dead fd, before asking for a new one + try + { + open_socket(bind_address_, port_); + ++stats_.socket_recoveries; + log_rate_limited( + LC_SOCKET_RECOVERED, + "socket re-bound to UDP " + bind_address_ + ":" + std::to_string(port_) + ", receive loop continuing", + std::cerr); + return true; + } + catch (const std::exception& e) + { + // Expected while the interface is down; the caller reports the give-up. + if (attempt == SOCKET_RECOVERY_ATTEMPTS - 1) + { + std::cerr << "[XsensFullBody] final socket re-bind attempt failed: " << e.what() << std::endl; + } + } + } + + ++stats_.socket_recovery_failures; + return false; +} + +bool XsensFullBodyPlugin::recover_session(const std::atomic& stop) +{ + for (int attempt = 0; attempt < SESSION_RECOVERY_ATTEMPTS; ++attempt) + { + if (!wait_unless_stopped(SESSION_BACKOFF_MS[attempt], stop)) + { + return false; + } + + try + { + establish_session(); + ++stats_.session_recoveries; + std::cout << "[XsensFullBody] OpenXR session re-established after " << (attempt + 1) + << " attempt(s); resuming push" << std::endl; + return true; + } + catch (const std::exception& e) + { + // Expected while the runtime is down. Log only the first and last attempt, so a long + // outage cannot flood the console with one line per retry. + if (attempt == 0 || attempt == SESSION_RECOVERY_ATTEMPTS - 1) + { + std::cerr << "[XsensFullBody] session re-establish attempt " << (attempt + 1) << "/" + << SESSION_RECOVERY_ATTEMPTS << " failed: " << e.what() << std::endl; + } + } + } + return false; +} + +bool XsensFullBodyPlugin::update(const std::atomic& stop) +{ + ssize_t received; + if (inject_recv_errors_left_ > 0) // test hook; see read_recv_error_injection() + { + --inject_recv_errors_left_; + errno = inject_recv_errno_; + received = -1; + } + else + { + // MSG_TRUNC makes recv report the datagram's real length even when it overflowed the + // buffer, so an over-large datagram is dropped rather than processed from a partial one. + received = ::recv(socket_fd_, buffer_.data(), buffer_.size(), MSG_TRUNC); + } + if (received < 0) + { + // The idle path: the receive timeout expired, or a signal landed. Not an error, and it is + // what paces this loop while MVN is not streaming. + if (errno == EAGAIN || errno == EWOULDBLOCK || errno == EINTR) + { + return false; + } + + // Anything else is a hard socket error. Without this branch it would be indistinguishable + // from "MVN is not streaming yet" and would spin the caller's loop at full CPU. + const std::string reason = std::strerror(errno); + log_rate_limited( + LC_SOCKET_ERROR, "recv failed: " + reason + " -- re-binding UDP port " + std::to_string(port_), std::cerr); + if (!recover_socket(stop)) + { + if (stop.load(std::memory_order_relaxed)) + { + return false; // stopping anyway; let the caller's loop exit normally + } + throw std::runtime_error("XsensFullBodyPlugin: UDP socket unrecoverable after " + + std::to_string(SOCKET_RECOVERY_ATTEMPTS) + + " re-bind attempts (last error: " + reason + ")"); + } + return false; + } + if (received == 0) + { + return false; // empty datagram + } + if (static_cast(received) > buffer_.size()) + { + ++stats_.dropped_truncated; + log_rate_limited(LC_TRUNCATED, "dropped over-large datagram (" + std::to_string(received) + " B)", std::cerr); + return false; + } + + const FrameOutcome outcome = decider_.classify(buffer_.data(), static_cast(received)); + + // Events are independent of the verdict: a delivered frame can still carry a session reset + // or a rewind, so these are counted before the verdict is acted on. + if (outcome.session_reset) + { + ++stats_.session_resets; + log_rate_limited(LC_SESSION_RESET, "sequence reset -> new MVN session", std::cout); + } + if (outcome.sequence_numbers_skipped > 0) + { + ++stats_.sequence_gap_events; + stats_.sequence_numbers_skipped += outcome.sequence_numbers_skipped; + } + if (outcome.non_whole_ms) + { + ++stats_.non_whole_ms_samples; + log_rate_limited(LC_NON_WHOLE_MS, + "sample time is not a whole millisecond -- not from MVN's solver; delivering anyway", std::cout); + } + if (outcome.timeline_rewind) + { + ++stats_.timeline_rewinds; + log_rate_limited(LC_TIMELINE_REWIND, + "sample time moved backwards -- timeline rewind (scrub or recording restart)", std::cout); + } + + switch (outcome.verdict) + { + case FrameVerdict::Deliver: + break; + case FrameVerdict::DroppedOversize: + // Counted as malformed, like any other unusable frame; logged separately because the + // fix is specific and the number is the whole diagnosis. + ++stats_.dropped_malformed; + log_rate_limited(LC_OVERSIZE_PAYLOAD, + "payload " + std::to_string(outcome.observed_payload_size) + " B exceeds max_flatbuffer_size " + + std::to_string(max_flatbuffer_size_) + " -- raise it on BOTH pusher and reader", + std::cerr); + return false; + case FrameVerdict::DroppedMalformed: + ++stats_.dropped_malformed; + return false; + case FrameVerdict::DroppedUnverified: + ++stats_.dropped_unverified; + return false; + case FrameVerdict::DroppedStale: + ++stats_.dropped_stale; + return false; + } + + // The header's sample_time_ns is on MVN's SEND-HOST clock -- a different domain from ours, + // so it must not be published as the local common clock. Stamp the common clock here, at + // publish time, and forward MVN's device clock verbatim alongside it. + // + // push_buffer THROWS on failure, and a dead CloudXR runtime arrives as + // XR_ERROR_RUNTIME_FAILURE rather than anything session-shaped. Uncaught, a runtime restart + // would take the whole pusher down with it. + try + { + pusher_->push_buffer( + outcome.payload, outcome.payload_size, core::os_monotonic_now_ns(), outcome.raw_device_time_ns); + } + catch (const std::exception& e) + { + ++stats_.push_failures; + log_rate_limited(LC_PUSH_FAILED, + "push failed at seq=" + std::to_string(outcome.seq) + ": " + e.what() + + " -- re-establishing the OpenXR session", + std::cerr); + if (!recover_session(stop)) + { + if (stop.load(std::memory_order_relaxed)) + { + return false; // stopping anyway; let the caller's loop exit normally + } + throw std::runtime_error("XsensFullBodyPlugin: OpenXR session unrecoverable after " + + std::to_string(SESSION_RECOVERY_ATTEMPTS) + + " re-establish attempts -- restart the CloudXR runtime, then restart " + "this pusher"); + } + return false; // recovered, but this frame is now stale; the next one goes out + } + + ++stats_.delivered; + stats_.last_seq = outcome.seq; + stats_.last_size = outcome.payload_size; + stats_.last_hash = fnv1a64(outcome.payload, outcome.payload_size); + return true; +} + +} // namespace xsens_full_body +} // namespace plugins diff --git a/src/plugins/xsens_full_body/xsens_full_body_plugin.hpp b/src/plugins/xsens_full_body/xsens_full_body_plugin.hpp new file mode 100644 index 0000000000..88bb81d9d3 --- /dev/null +++ b/src/plugins/xsens_full_body/xsens_full_body_plugin.hpp @@ -0,0 +1,191 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 Xsens Technologies B.V. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include "frame_decision.hpp" +#include "plugin_options.hpp" + +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace core +{ +class OpenXRSession; +} + +namespace plugins +{ +namespace xsens_full_body +{ + +//! Counters reported on the periodic stats line; also the acceptance signal for a live run. +struct XsensFullBodyStats +{ + uint64_t delivered = 0; + uint64_t dropped_malformed = 0; + uint64_t dropped_unverified = 0; + uint64_t dropped_stale = 0; + //! Datagram larger than the receive buffer. Unreachable for IPv4 UDP (the buffer is bigger + //! than the 65507 B maximum), counted so an over-large datagram is dropped rather than + //! processed from a partial buffer. + uint64_t dropped_truncated = 0; + uint64_t timeline_rewinds = 0; + //! Frames whose sample time was not a whole millisecond. A warning, not a drop -- these are + //! counted in `delivered` too. + uint64_t non_whole_ms_samples = 0; + //! Two different questions, deliberately answered separately -- one number cannot do both, + //! and reporting only one is how the same figure came to mean different things on the two + //! branches. `gap_events` is how many times the stream jumped (a controller cares how often + //! continuity broke); `sequence_numbers_skipped` is how many frames' worth went missing in + //! total (a link report cares about volume). One 500-frame gap and 500 single-frame gaps are + //! very different faults and are indistinguishable under either number alone. + //! + //! Neither is a transport-loss figure on its own -- see FrameOutcome::sequence_numbers_skipped. + uint64_t sequence_gap_events = 0; + uint64_t sequence_numbers_skipped = 0; + uint64_t session_resets = 0; + //! Hard `recv` errors survived by re-binding the port, and recovery episodes that gave up. + uint64_t socket_recoveries = 0; + uint64_t socket_recovery_failures = 0; + //! `push_buffer` throws, and OpenXR sessions re-established in response. + uint64_t push_failures = 0; + uint64_t session_recoveries = 0; + uint64_t last_seq = 0; + uint64_t last_hash = 0; + size_t last_size = 0; +}; + +/*! + * @brief Receives Xsens MVN's Isaac Teleop UDP stream and republishes it as a tensor collection. + * + * MVN Studio already converts its 23-segment skeleton to the vendor-neutral 24-joint + * XR_BD_body_tracking layout (see the "Isaac Teleop" network-streamer preset), so the payload + * arriving here is a `core::FullBodyPose` FlatBuffer -- byte-identical to what any other + * full-body vendor produces. This plugin therefore **forwards the payload bytes verbatim** and + * never re-serializes: re-packing would risk silently changing a pose, and buys nothing. + * + * Pairs with the `body.xsens` vendor on the reader side: + * + * deviceio.VendorConfig([(tracker, deviceio.TrackerVendor("body.xsens", { + * "collection_id": "xsens_full_body", "max_flatbuffer_size": "4096"}))]) + * + * **Both of this plugin's dependencies can go away underneath it and come back.** The UDP socket + * can fail hard, and the CloudXR runtime backing the OpenXR session can be restarted. Neither is + * fatal: `update()` re-binds the socket and re-establishes the session on bounded backoffs, and + * only throws once a retry budget is exhausted. See `recover_socket()` and `recover_session()`. + */ +class XsensFullBodyPlugin +{ +public: + explicit XsensFullBodyPlugin(const XsensFullBodyOptions& options); + ~XsensFullBodyPlugin(); + + XsensFullBodyPlugin(const XsensFullBodyPlugin&) = delete; + XsensFullBodyPlugin& operator=(const XsensFullBodyPlugin&) = delete; + + /*! + * @brief Block for one datagram (up to the socket timeout) and push it if it is valid. + * + * @param stop Observed during a recovery backoff so SIGINT/SIGTERM still stops the pusher + * promptly during an outage rather than waiting the whole budget out. + * @return true if a frame was delivered, false on timeout, a rejected datagram, or a + * recovered outage. + * @throws std::runtime_error when the socket or the OpenXR session could not be recovered + * within its retry budget. That is the operator-restart case; the caller should + * report the counters and exit. + */ + bool update(const std::atomic& stop); + + const XsensFullBodyStats& stats() const + { + return stats_; + } + +private: + /*! + * @brief Per-category log throttle. + * + * A lossy link or a scrubbing operator can produce one event per frame, and these messages + * are emitted from inside the receive loop. Each category therefore logs only its first + * occurrence and every hundredth after that, so a flood cannot drown the console. + */ + enum LogCategory + { + LC_TRUNCATED = 0, + LC_OVERSIZE_PAYLOAD, + LC_SESSION_RESET, + LC_TIMELINE_REWIND, + LC_NON_WHOLE_MS, + LC_SOCKET_ERROR, + LC_SOCKET_RECOVERED, + LC_PUSH_FAILED, + LC_COUNT + }; + + //! Read the XSENS_TELEOP_INJECT_RECV_ERRORS test hook. No-op unless it is set. + void read_recv_error_injection(); + + void open_socket(const std::string& address, uint16_t port); + void close_socket(); + + //! Create the OpenXR session and the pusher on it. Also the recovery path: both are torn + //! down first, because once the runtime's IPC pipe breaks only a full re-create works. + void establish_session(); + + //! Re-bind the same address and port after a hard `recv` error, on a bounded backoff. Stream state is + //! deliberately left alone: the outage reappears as a sequence gap or a session reset, and + //! both are already handled. \return false if \a stop was set or the budget ran out. + bool recover_socket(const std::atomic& stop); + + //! Re-establish the OpenXR session after `push_buffer` threw, on a bounded backoff. + //! \return false if \a stop was set or the budget ran out. + bool recover_session(const std::atomic& stop); + + //! Sleep \a total_ms in slices, returning early (false) if \a stop is set. + static bool wait_unless_stopped(int total_ms, const std::atomic& stop); + + //! Emit \a message for \a category only on its first and every hundredth occurrence. + //! Stream diagnostics go to stdout; anything an operator would call a problem -- a bad + //! sender, a broken socket, a dead runtime -- goes to stderr, so the outage narrative reads + //! in one place. + void log_rate_limited(LogCategory category, const std::string& message, std::ostream& out); + + std::string collection_id_; + size_t max_flatbuffer_size_ = 0; + + int socket_fd_ = -1; + //! The address and port actually bound, assigned only once `bind()` has succeeded -- so a + //! zero `port_` still means "never successfully bound", and recovery cannot drift onto a + //! different interface than the one the operator asked for. + std::string bind_address_; + uint16_t port_ = 0; + //! Recovery-matrix test hook, set from XSENS_TELEOP_INJECT_RECV_ERRORS=[:]. Forces + //! the next n recv calls to fail hard, which is the only practical way to exercise + //! recover_socket() -- a real hard error needs the interface to fail underneath you. Zero, + //! and therefore inert, unless the variable is set. + int inject_recv_errors_left_ = 0; + int inject_recv_errno_ = 0; + std::vector buffer_; + XsensFullBodyStats stats_; + //! Everything decided per datagram. Separate so it can be unit tested without a runtime. + FrameDecider decider_; + //! Occurrences seen per LogCategory, for rate limiting. + uint64_t log_counts_[LC_COUNT] = {}; + + std::shared_ptr session_; + //! Optional because SchemaPusher is neither copyable nor movable, and recovery has to + //! destroy and re-create it in place. + std::optional pusher_; +}; + +} // namespace xsens_full_body +} // namespace plugins diff --git a/tests/cpp/core/live_trackers/test_vendor_validation.cpp b/tests/cpp/core/live_trackers/test_vendor_validation.cpp index 8053fe2b21..cad155b9b2 100644 --- a/tests/cpp/core/live_trackers/test_vendor_validation.cpp +++ b/tests/cpp/core/live_trackers/test_vendor_validation.cpp @@ -103,6 +103,22 @@ TEST_CASE("vendor validation: accepted configurations resolve extensions", "[liv { "max_flatbuffer_size", "32768" } } } } }; REQUIRE_NOTHROW(core::DeviceIOSession::get_required_extensions(trackers, core::VendorConfig{ vendors })); } + + SECTION("Xsens vendor selects tensor data extensions") + { + VendorList vendors{ { body.get(), core::TrackerVendor{ "body.xsens" } } }; + const auto extensions = core::DeviceIOSession::get_required_extensions(trackers, core::VendorConfig{ vendors }); + REQUIRE(contains(extensions, "XR_NVX1_tensor_data")); + REQUIRE_FALSE(contains(extensions, "XR_BD_body_tracking")); + } + + SECTION("Xsens vendor accepts collection and sample-size parameters") + { + VendorList vendors{ { body.get(), core::TrackerVendor{ "body.xsens", + { { "collection_id", "custom_xsens" }, + { "max_flatbuffer_size", "32768" } } } } }; + REQUIRE_NOTHROW(core::DeviceIOSession::get_required_extensions(trackers, core::VendorConfig{ vendors })); + } } TEST_CASE("vendor validation: invalid configurations are rejected", "[live_trackers][vendor]") @@ -156,6 +172,32 @@ TEST_CASE("vendor validation: invalid configurations are rejected", "[live_track REQUIRE_THAT(vendor_validation_error(trackers, vendors), ContainsSubstring("must be a positive integer")); } + SECTION("an unsupported Xsens vendor parameter is rejected") + { + VendorList vendors{ + { body.get(), core::TrackerVendor{ "body.xsens", { { "unsupported", "value" } } } }, + }; + REQUIRE_THAT(vendor_validation_error(trackers, vendors), ContainsSubstring("does not support parameter")); + } + + SECTION("an invalid Xsens max flatbuffer size is rejected") + { + VendorList vendors{ + { body.get(), core::TrackerVendor{ "body.xsens", { { "max_flatbuffer_size", "0" } } } }, + }; + REQUIRE_THAT(vendor_validation_error(trackers, vendors), ContainsSubstring("must be a positive integer")); + } + + SECTION("an empty Xsens collection_id is rejected rather than silently never connecting") + { + // The pusher and reader rendezvous on collection_id, so a mismatch fails silently and + // forever. An empty one is always a configuration error and must be caught up front. + VendorList vendors{ + { body.get(), core::TrackerVendor{ "body.xsens", { { "collection_id", "" } } } }, + }; + REQUIRE_THAT(vendor_validation_error(trackers, vendors), ContainsSubstring("must not be empty")); + } + SECTION("a duplicate selection for the same tracker is rejected") { VendorList vendors{