From 8c8fe7ff6540d5d4976ff8872409c68e604e3ca8 Mon Sep 17 00:00:00 2001 From: John Date: Thu, 23 Jul 2026 14:15:14 -0400 Subject: [PATCH 1/3] feat(sim): add NatNet server emulator (protocol core) + register unit tests MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The pure-Python NatNet server that emulates an OptiTrack Motive server so natnet_ros2 can be driven without hardware. USD/Isaac-free — this is the protocol + server core (unicast server, data/model/server types, serializers, default catalogs). The Isaac wrapper that maps a USD scene onto this server lands next. Registers the emulator package's co-located unit tests via a `sim:` entry in tests/colcon_unit_test_packages.yaml (base's simulation/**//test glob). The root conftest now puts each unit-test package's import root on sys.path so co-located tests import their package without a per-package conftest.py. Co-Authored-By: Claude Opus 4.8 --- .../optitrack.natnet.emulator/.gitignore | 9 + .../optitrack.natnet.emulator/README.md | 160 ++++++++ .../optitrack/__init__.py | 1 + .../optitrack/natnet/__init__.py | 1 + .../optitrack/natnet/emulator/__init__.py | 20 + .../optitrack/natnet/emulator/defaults.py | 25 ++ .../natnet/emulator/server/__init__.py | 11 + .../natnet/emulator/server/natnet_common.py | 27 ++ .../emulator/server/natnet_data_types.py | 261 ++++++++++++ .../emulator/server/natnet_model_types.py | 134 +++++++ .../natnet/emulator/server/natnet_server.py | 353 ++++++++++++++++ .../emulator/server/natnet_server_types.py | 156 ++++++++ .../emulator/server/natnet_unicast_server.py | 172 ++++++++ .../optitrack.natnet.emulator/setup.py | 23 ++ .../test/natnet_test_helpers.py | 105 +++++ .../test/test_defaults.py | 26 ++ .../test/test_serializers.py | 341 ++++++++++++++++ .../test/test_server_catalog.py | 78 ++++ .../test/test_unicast_protocol.py | 284 +++++++++++++ tests/colcon_unit_test_packages.yaml | 6 + tests/conftest.py | 10 + tests/integration/natnet/README.md | 151 +++++++ .../natnet/test_natnet_integration.py | 376 ++++++++++++++++++ 23 files changed, 2730 insertions(+) create mode 100644 simulation/isaac-sim/extensions/optitrack.natnet.emulator/.gitignore create mode 100644 simulation/isaac-sim/extensions/optitrack.natnet.emulator/README.md create mode 100644 simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/__init__.py create mode 100644 simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/__init__.py create mode 100644 simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/__init__.py create mode 100644 simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/defaults.py create mode 100644 simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/__init__.py create mode 100644 simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_common.py create mode 100644 simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_data_types.py create mode 100644 simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_model_types.py create mode 100644 simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_server.py create mode 100644 simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_server_types.py create mode 100644 simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_unicast_server.py create mode 100644 simulation/isaac-sim/extensions/optitrack.natnet.emulator/setup.py create mode 100644 simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/natnet_test_helpers.py create mode 100644 simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_defaults.py create mode 100644 simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_serializers.py create mode 100644 simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_server_catalog.py create mode 100644 simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_unicast_protocol.py create mode 100644 tests/integration/natnet/README.md create mode 100644 tests/integration/natnet/test_natnet_integration.py diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/.gitignore b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/.gitignore new file mode 100644 index 000000000..0db35ef6e --- /dev/null +++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/.gitignore @@ -0,0 +1,9 @@ +# OptiTrack SDK archives and build artifacts (reference tree may exist locally) +**/*.obj +**/*.pdb +**/*.exe +**/*.iobj +**/*.ipdb +**/*.tlog/ +**/__pycache__/ +**/*.pyc diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/README.md b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/README.md new file mode 100644 index 000000000..3bb05f0b6 --- /dev/null +++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/README.md @@ -0,0 +1,160 @@ +# OptiTrack NatNet Emulator (Isaac Sim Extension) + +Python NatNet **server** emulator for AirStack simulation and integration testing with [`natnet_ros2`](../../../../robot/ros_ws/src/perception/natnet_ros2/). + +The extension has two layers: + +1. **Transport + protocol** (`optitrack.natnet.emulator.server`) — UDP NatNet server, ctypes wire types, MODELDEF cache, frame streaming. Importable outside Isaac Sim (unit tests, host-side integration). +2. **Isaac integration** (`optitrack.natnet.emulator.isaac`) — stage-driven `/World/NatNetInterface` config prim, pose sampling on physics steps, Kit UI editor, and Pegasus launch-script helpers. + +## Layout + +``` +optitrack.natnet.emulator/ +├── config/extension.toml # Kit manifest (server module + UI entry point) +├── schema/schema.usda # Typed NatNet interface attribute definitions +├── setup.py +├── docs/ # (legacy design notes — see docs/simulation/isaac_sim/natnet_emulator.md) +├── test/ # Co-located unit tests (proxied by tests/sim/) +└── optitrack/natnet/emulator/ + ├── defaults.py # Reference Drone → prim bindings for tests + ├── server/ # NatNet UDP server (transport + protocol) + │ ├── natnet_server.py # Base server, queue, MODELDEF cache + │ ├── natnet_unicast_server.py + │ ├── natnet_data_types.py + │ ├── natnet_model_types.py + │ └── natnet_server_types.py + └── isaac/ # Isaac Sim wrapper (Kit + USD) + ├── config.py # Pure-Python NatNetInterfaceConfig model + ├── usd_bindings.py # Author/read interface prims on a stage + ├── catalog.py # Config → sDataDescriptions (MODELDEF) + ├── frames.py # Prim poses → sFrameOfMocapData + ├── manager.py # NatNetServerManager (lifecycle + sampling) + ├── scene_setup.py # Pegasus launch helpers (start_drone_natnet_server) + └── ui_extension.py # Docked editor panel (NatNetEmulatorExtension) +``` + +## Responsibilities + +| Layer | Role | +|-------|------| +| **Server** | UDP transport; `NAT_CONNECT` / `NAT_SERVERINFO`; `NAT_REQUEST_MODELDEF`; `NAT_KEEPALIVE`; `NAT_ECHOREQUEST` / `NAT_ECHORESPONSE`; `NAT_FRAMEOFDATA` on the **data port** (1511). MODELDEF stored as packed bytes via `set_model_def_payload()`. Frames enqueued with `enqueue_mocap_data()`. | +| **Isaac wrapper** | Authors and reads the NatNet interface config prim; builds MODELDEF from scene config; samples tracked prim world poses each physics step; calls `flush_mocap_data()` synchronously (background timer disabled — see below). | +| **`defaults.py`** | Hardcoded `Drone` → `/World/base_link` binding for legacy tests; production paths use the stage prim via `scene_setup.build_drone_config()`. | + +The server does **not** own prim-path bindings. The Isaac layer calls `set_model_def_payload(catalog.pack())` after building `sDataDescriptions` from the interface config. + +## Stage-driven config prim + +Configuration lives on a USD prim (conventionally `/World/NatNetInterface`) with `natnet:*` attributes: + +- Server: IP, unicast/multicast mode, command/data ports, publish rate, NatNet version, up-axis, optional pose noise. +- Bodies: multi-apply `natnet:body::*` fields mapping rigid-body name / streaming ID → target prim path. + +`NatNetServerManager` scans the stage, resyncs the catalog when the prim changes, and streams one rigid body per configured target. Missing prims emit **lost** bodies (NaN position, tracking-invalid bit clear) until the target appears — important for Pegasus drones spawned on first Play. + +**Up axis:** default `Z` passes Isaac/USD world poses through unchanged (matches `natnet_ros2`). Set `Y` to emulate a Y-up Motive room. + +## Streaming model (Isaac) + +Inside Kit, the server's background `_data_update_loop` is **disabled** (`auto_stream = False`) because the GIL-starved daemon thread does not reliably transmit frames. Instead, each physics step: + +1. `NatNetServerManager.sample_once()` reads prim poses and `enqueue_mocap_data(frame)`. +2. `NatNetUnicastServer.flush_mocap_data()` sends immediately on the physics-step thread. + +Outside Isaac (host unit tests), `auto_stream=True` uses the timer-driven loop. + +Default Docker sim IP: **`172.31.0.200`** (Isaac container on the AirStack bridge network). + +## Enabling in AirStack + +**Robot:** `LAUNCH_NATNET=true` in `.env` → `natnet_ros2` in perception bringup. Configure Motive/emulator IP in [`natnet_config.yaml`](../../../../robot/ros_ws/src/perception/natnet_ros2/config/natnet_config.yaml). + +**Isaac Sim:** set `ISAAC_SIM_SCRIPT_NAME` to a NatNet launch script (NatNet always starts — no `LAUNCH_NATNET` gate in the script): + +| Script | Use | +|--------|-----| +| `example_one_px4_pegasus_natnet_launch_script.py` | Single drone + static `Target` | +| `example_multi_px4_pegasus_natnet_launch_script.py` | `NUM_ROBOTS` drones + shared `Target` (system tests with NatNet use this even for `NUM_ROBOTS=1`) | + +Baseline Pegasus scripts (`example_one_px4_pegasus_launch_script.py`, `example_multi_px4_pegasus_launch_script.py`) have **no** NatNet integration. + +Convenience bundle for NatNet + external-vision PX4 SITL: + +```bash +airstack up --env-file overrides/isaac-natnet-vision.env +``` + +See [optitrack-development skill](../../../../.agents/skills/optitrack-development/SKILL.md) for wire-protocol details, libNatNet 4.4 unicast quirks, and debugging. + +## Usage + +### Server only (no Kit) + +```python +from optitrack.natnet.emulator import NatNetUnicastServer, make_default_drone_catalog +from optitrack.natnet.emulator.isaac.frames import BodySample, build_frame + +server = NatNetUnicastServer(local_interface="172.31.0.200") +server.set_model_def_payload(make_default_drone_catalog().pack()) +server.start() + +frame = build_frame(0, [BodySample(1, (0, 0, 1), (0, 0, 0, 1))]) +server.enqueue_mocap_data(frame) +server.flush_mocap_data() +``` + +### Isaac launch script + +```python +from optitrack.natnet.emulator.isaac import start_drone_natnet_server + +# Keep a reference to the manager for the sim lifetime. +manager = start_drone_natnet_server( + stage, + drones=[("Drone", 1, "/World/drone1/base_link")], + server_ip="172.31.0.200", +) +``` + +### Kit UI + +The extension registers **Window → NatNet Emulator** — a docked panel to create/edit the interface prim, start/stop the server, and view live body readouts. The same `NatNetServerManager` backs both the UI and launch-script paths. + +## Protocol notes (unicast, libNatNet 4.4) + +| Port | Traffic | +|------|---------| +| **1510** | Command: `NAT_CONNECT`, `NAT_REQUEST_MODELDEF`, keepalives, echo | +| **1511** | Data: `NAT_FRAMEOFDATA` — **must** be sent from a socket bound to the data port | + +Frames sent from the command socket are silently dropped by libNatNet. Every frame payload must include the 4-byte end-of-data tag expected by the C SDK unpacker. + +Full handshake layouts and sniffing workflow: [optitrack-development skill](../../../../.agents/skills/optitrack-development/SKILL.md). + +## Tests + +| Tier | Mark | What | +|------|------|------| +| Unit | `unit` | Serializers, protocol, config, USD authoring, catalog, pose sampling, server lifecycle, scene setup | +| Integration | `integration` | Host emulator → robot `natnet_ros2` pose Hz | + +Co-located tests live in `test/`. Pytest discovers them via thin proxies in [`tests/sim/optitrack_natnet_emulator/`](../../../../tests/sim/optitrack_natnet_emulator/). + +```bash +# Unit (no Docker / no SDK) +pytest tests/sim/optitrack_natnet_emulator/ -m unit -v + +# Integration (robot container + NatNet SDK) +pytest tests/integration/natnet/ -m integration -v +``` + +Representative unit modules: `test_unicast_protocol.py`, `test_pose_streaming.py`, `test_interface_authoring.py`, `test_server_lifecycle.py`, `test_scene_setup.py`. + +## Reference material + +- User guide: [`docs/simulation/isaac_sim/natnet_emulator.md`](../../../../docs/simulation/isaac_sim/natnet_emulator.md) +- Robot client: [`natnet_ros2/README.md`](../../../../robot/ros_ws/src/perception/natnet_ros2/README.md) +- Integration tier: [`tests/integration/natnet/README.md`](../../../../tests/integration/natnet/README.md) + +OptiTrack SDK sample headers may exist locally under `NatNetClientSDK/` for wire-format reference; they are **not** redistributed by AirStack (proprietary license). diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/__init__.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/__init__.py new file mode 100644 index 000000000..39ed38144 --- /dev/null +++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/__init__.py @@ -0,0 +1 @@ +"""OptiTrack NatNet packages for AirStack Isaac Sim integration.""" diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/__init__.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/__init__.py new file mode 100644 index 000000000..b19da2cbc --- /dev/null +++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/__init__.py @@ -0,0 +1 @@ +"""NatNet simulation components.""" diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/__init__.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/__init__.py new file mode 100644 index 000000000..e819d8b1b --- /dev/null +++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/__init__.py @@ -0,0 +1,20 @@ +"""OptiTrack Motive NatNet emulator for Isaac Sim.""" + +from .defaults import ( + DEFAULT_DRONE_BINDING, + DEFAULT_TRACKED_BODY_BINDINGS, + TrackedBodyBinding, +) +from .server import Client, NatNetServer, NatNetUnicastServer, TransmissionType +from .server.natnet_model_types import make_default_drone_catalog + +__all__ = [ + "Client", + "DEFAULT_DRONE_BINDING", + "DEFAULT_TRACKED_BODY_BINDINGS", + "NatNetServer", + "NatNetUnicastServer", + "TrackedBodyBinding", + "TransmissionType", + "make_default_drone_catalog", +] diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/defaults.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/defaults.py new file mode 100644 index 000000000..2c1915264 --- /dev/null +++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/defaults.py @@ -0,0 +1,25 @@ +"""Reference tracked-body defaults for tests and the future Isaac Sim wrapper.""" + +from __future__ import annotations + +from dataclasses import dataclass + + +@dataclass(frozen=True) +class TrackedBodyBinding: + """Maps a NatNet rigid body to a USD prim path (not sent on the NatNet wire).""" + + name: str + id: int + prim_path: str + parent_id: int = -1 + + +# Single-drone NatNet Pegasus scenes (example_one_px4_pegasus_natnet_launch_script.py). +DEFAULT_DRONE_BINDING = TrackedBodyBinding( + name="Drone", + id=1, + prim_path="/World/base_link", +) + +DEFAULT_TRACKED_BODY_BINDINGS: tuple[TrackedBodyBinding, ...] = (DEFAULT_DRONE_BINDING,) diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/__init__.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/__init__.py new file mode 100644 index 000000000..c84c1fe4b --- /dev/null +++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/__init__.py @@ -0,0 +1,11 @@ +"""NatNet UDP server implementation (unicast; multicast planned).""" + +from .natnet_server import Client, NatNetServer, TransmissionType +from .natnet_unicast_server import NatNetUnicastServer + +__all__ = [ + "Client", + "NatNetServer", + "NatNetUnicastServer", + "TransmissionType", +] diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_common.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_common.py new file mode 100644 index 000000000..1eb32177c --- /dev/null +++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_common.py @@ -0,0 +1,27 @@ +from enum import IntEnum +import ctypes + +class ModelLimits(IntEnum): + MAX_MODELS = 2000 # maximum number of total models (data descriptions) + MAX_MARKERSETS = 1000 # maximum number of MarkerSets + MAX_RIGIDBODIES = 1000 # maximum number of RigidBodies + MAX_ASSETS = 1000 # Maximum number of Assets + MAX_NAMELENGTH = 256 # maximum length for strings + MAX_MARKERS = 200 # maximum number of markers per MarkerSet + MAX_RBMARKERS = 20 # maximum number of markers per RigidBody + MAX_SKELETONS = 100 # maximum number of skeletons + MAX_SKELRIGIDBODIES = 200 # maximum number of RididBodies per Skeleton + MAX_LABELED_MARKERS = 1000 # maximum number of labeled markers per frame + MAX_UNLABELED_MARKERS = 1000 # maximum number of unlabeled (other) markers per frame + + MAX_FORCEPLATES = 100 # maximum number of force plate 'bundles' + MAX_DEVICES = 100 # maximum number of peripheral device 'bundles' + MAX_ANALOG_CHANNELS = 32 # maximum number of data channels (signals) per analog/force plate device + MAX_ANALOG_SUBFRAMES = 30 # maximum number of analog/force plate frames per mocap frame + + MAX_PACKETSIZE = 65503 # max size of packet in bytes (actual packet size is dynamic) + # (65535 byte IP limit - 20 byte IP header - 8 byte UDP header - 4 byte sPacket header = 65503 bytes) + + + +MarkerData = ctypes.c_float * 3 diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_data_types.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_data_types.py new file mode 100644 index 000000000..91e23a412 --- /dev/null +++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_data_types.py @@ -0,0 +1,261 @@ +import ctypes +import struct +from .natnet_common import ModelLimits, MarkerData + +class sMarker(ctypes.Structure): + _pack_ = 1 + _fields_ = [ + ("ID", ctypes.c_int32), + ("x", ctypes.c_float), + ("y", ctypes.c_float), + ("z", ctypes.c_float), + ("size", ctypes.c_float), + ("params", ctypes.c_int16), + ("residual", ctypes.c_float) + ] + + def pack(self) -> bytes: + return struct.pack(' bytes: + # szName is null-terminated on the wire. + name_bytes = self.szName.rstrip(b'\x00') + b'\x00' + payload = bytearray(name_bytes) + payload += struct.pack(' bytes: + return struct.pack(' bytes: + payload = bytearray(struct.pack(' bytes: + payload = bytearray(struct.pack(' bytes: + payload = bytearray(struct.pack(' bytes: + payload = bytearray(struct.pack(' bytes: + payload = bytearray(struct.pack(' bytes: + """NatNet 4.1+ prefixes each collection with a 4-byte byte count.""" + payload = bytearray(struct.pack(' 0) or natnet_major > 4: + payload += struct.pack(' bytes: + payload = bytearray() + + payload += struct.pack(' bytes: + # szName is null-terminated on the wire, not fixed MAX_NAMELENGTH. + name_bytes = self.szName.rstrip(b"\x00") + b"\x00" + payload = bytearray(name_bytes) + payload += struct.pack( + " bytes: + if self.type == int(DataDescriptors.Descriptor_RigidBody): + body = self.RigidBodyDescription.pack() + else: + raise ValueError(f"Unsupported data description type: {self.type}") + payload = bytearray(struct.pack(" bytes: + payload = bytearray(struct.pack(" sDataDescriptions: + """Build the default single-body catalog (Drone id=1) for natnet_ros2.""" + descriptions = sDataDescriptions() + descriptions.nDataDescriptions = 1 + desc = descriptions.arrDataDescriptions[0] + desc.type = int(DataDescriptors.Descriptor_RigidBody) + rb = desc.RigidBodyDescription + rb.szName = b"Drone" + rb.ID = 1 + rb.parentID = -1 + rb.offsetqw = 1.0 + rb.nMarkers = 0 + return descriptions diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_server.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_server.py new file mode 100644 index 000000000..5ef19009e --- /dev/null +++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_server.py @@ -0,0 +1,353 @@ +from . import natnet_data_types as DataMessages +from . import natnet_server_types as ServerMessages +from . import natnet_model_types as ModelTypes +from enum import Enum +import socket +import threading +import queue +import signal +import ctypes +import time +import typing + + +class TransmissionType(str, Enum): + UNICAST = "unicast" + MULTICAST = "multicast" + +class Client: + def __init__(self, ip: str, port: int, version: typing.Tuple[int, int, int, int] = (4, 4, 0, 0)): + self.ip = ip + self.port = port + self.version = version + self.subscribed_assets = set() + self.socket_lock = threading.Lock() + + def __hash__(self): + # Uniquely identify a client session by their IP and their unique command port. + return hash((self.ip, self.port)) + + def __eq__(self, other): + return (isinstance(other, Client) and + self.ip == other.ip and + self.port == other.port) + + +class NatNetServer: + def __init__(self, + local_interface : str = "172.31.0.200", + transmission_type: TransmissionType = TransmissionType.MULTICAST, + multicast_address : str = "239.255.42.99", + command_port: int = 1510, + data_port : int = 1511, + motive_app_version : typing.Tuple[int, int, int, int]=(3, 1, 0, 0), + natnet_version : typing.Tuple[int, int, int, int]=(4, 4, 0, 0), + high_res_clock_freq : int = 1_000_000_000, + publish_rate : int = 100 # Hz (default 100Hz) + ): + + self.local_interface = local_interface + self.transmission_type = transmission_type + self.multicast_address = multicast_address + self.command_port = command_port + self.data_port = data_port + self.motive_app_version = motive_app_version + self.natnet_version = natnet_version + self.high_res_clock_freq = high_res_clock_freq + self.publish_rate = publish_rate + + self._validate_init_params() + + self.server_description = self._build_server_description() + # Initialize synchronously safe data structures for server state and mocap data + + # Thread-safe queue for Mocp frames + self.mocap_data_queue = queue.Queue(maxsize=100) + self._last_mocap_frame: DataMessages.sFrameOfMocapData | None = None + self._last_mocap_lock = threading.Lock() + + # Thread list and shutdown event + self.threads = [] + self.shutdown_event = threading.Event() + + # Connected clients for unicast mode + self.connected_clients : typing.Set[Client] = set() + self.clients_lock : threading.Lock = threading.Lock() + + # MODELDEF wire cache (Isaac wrapper updates via set_model_def_payload) + self._model_def_lock = threading.Lock() + self._model_def_payload: bytes = ModelTypes.make_default_drone_catalog().pack() + + # Sockets + self.command_socket : socket.socket | None = None + self.data_socket : socket.socket | None = None + + self.running = False + + # When True (default), the background data loop streams frames on its own timer. + # Set False when an external driver (the Isaac wrapper's physics-step callback) + # sends frames synchronously via ``flush_mocap_data``. + self.auto_stream = True + + # start() launches two daemon threads: a command listener (handshake / MODELDEF / keepalive) + # and a data loop that streams mocap frames. The transmission-specific behavior lives in the unicast/multicast subclass. + + def _signal_handler(self, signum, frame): + print(f"\n[NatNetServer] Received interrupt signal {signum}. Initiating shutdown...") + self.shutdown() + + def enqueue_mocap_data(self, new_data: DataMessages.sFrameOfMocapData): + # Thread-safe method to push new physics frames (called by Isaac-Sim extension) + if self.mocap_data_queue.full(): + try: + # Drop oldest frame if falling behind + self.mocap_data_queue.get_nowait() + except queue.Empty: + pass + self.mocap_data_queue.put(new_data) + with self._last_mocap_lock: + self._last_mocap_frame = new_data + + def _get_last_known_mocap_frame(self) -> DataMessages.sFrameOfMocapData | None: + with self._last_mocap_lock: + return self._last_mocap_frame + + def set_model_def_payload(self, payload: bytes) -> None: + """Replace MODELDEF body served on NAT_REQUEST_MODELDEF (Isaac wrapper calls this).""" + with self._model_def_lock: + self._model_def_payload = payload + + def set_model_def_from_descriptions( + self, descriptions: ModelTypes.sDataDescriptions + ) -> None: + """Pack descriptions once and store as the MODELDEF wire cache.""" + self.set_model_def_payload(descriptions.pack()) + + def _get_model_def_payload(self) -> bytes: + """Return cached MODELDEF bytes (command thread only).""" + with self._model_def_lock: + return self._model_def_payload + + def start(self): + # Bind sockets and launch worker threads automatically on init + + # Register signal handlers for graceful shutdown (Catches Ctrl+C and kill) + try: + signal.signal(signal.SIGINT, self._signal_handler) + signal.signal(signal.SIGTERM, self._signal_handler) + except ValueError: + pass # Safe fallback if not called from the main thread + + # 1. Setup Command Socket (Receives connection/discovery requests) + self.command_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM, socket.IPPROTO_UDP) + self.command_socket.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) + self.command_socket.bind(('', self.command_port)) + + # 2. Setup Data Socket (Sends outward Mocap frames). + # Bind to the data port so frames leave with source port == data_port. + # libNatNet routes unicast NAT_FRAMEOFDATA by the server's data port + self.data_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM, socket.IPPROTO_UDP) + self.data_socket.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) + self.data_socket.bind(('', self.data_port)) + if self.transmission_type == TransmissionType.MULTICAST: + self.data_socket.setsockopt(socket.IPPROTO_IP, socket.IP_MULTICAST_IF, socket.inet_aton(self.local_interface)) + + # 3. Launch Threads + cmd_thread = threading.Thread(target=self._command_listener_loop, daemon=True) + data_thread = threading.Thread(target=self._data_update_loop, daemon=True) + + self.threads.extend([cmd_thread, data_thread]) + + for t in self.threads: + t.start() + + self.running = True + + def shutdown(self): + # Cleanly shutdown threads and close sockets + self.running = False + self.shutdown_event.set() + + if self.command_socket: + self.command_socket.close() + + if self.data_socket: + self.data_socket.close() + + for t in self.threads: + if t.is_alive(): + t.join(timeout=1.0) + + def _validate_init_params(self): + + # Validate the local_interface is a valid IP address + if not self.local_interface or not isinstance(self.local_interface, str) or self.local_interface.count('.') != 3: + raise ValueError(f"Invalid local interface IP address: {self.local_interface}") + + # Validate between transmission types and address requirements + if self.transmission_type not in TransmissionType: + raise ValueError(f"Invalid transmission type: {self.transmission_type}. Must be 'unicast' or 'multicast'.") + + if self.transmission_type == TransmissionType.MULTICAST and not self.multicast_address: + raise ValueError("Multicast address must be provided for multicast transmission type.") + + if self.transmission_type == TransmissionType.UNICAST and self.multicast_address: + raise ValueError("Multicast address should not be provided for unicast transmission type.") + + if not (0 < self.command_port < 65536): + raise ValueError(f"Invalid command port: {self.command_port}. Must be between 1 and 65535.") + + if not (0 < self.data_port < 65536): + raise ValueError(f"Invalid data port: {self.data_port}. Must be between 1 and 65535.") + + if self.command_port == self.data_port: + raise ValueError("Command port and data port must be different.") + + if self.motive_app_version and (not isinstance(self.motive_app_version, tuple) or len(self.motive_app_version) != 4): + raise ValueError(f"Invalid Motive app version: {self.motive_app_version}. Must be a tuple of 4 integers (major, minor, build, revision).") + + if self.natnet_version and (not isinstance(self.natnet_version, tuple) or len(self.natnet_version) != 4): + raise ValueError(f"Invalid NatNet version: {self.natnet_version}. Must be a tuple of 4 integers (major, minor, build, revision).") + + if self.motive_app_version and not self.motive_app_version[0] == 3: + raise ValueError(f"Unsupported Motive app version: {self.motive_app_version}. Minimum supported version is 3.0.0.0. Recommended to use 3.1.0.0") + + if not self.natnet_version[0] == 4: + raise ValueError(f"Unsupported NatNet version: {self.natnet_version}. Minimum supported version is 4.0.0.0. Recommended to use 4.4.0.0") + + if self.high_res_clock_freq <= 0: + raise ValueError( + f"Invalid high resolution clock frequency: {self.high_res_clock_freq}. Must be a positive integer representing the frequency in Hz." + ) + + if self.publish_rate <= 0: + raise ValueError( + f"Invalid publish rate: {self.publish_rate}. Must be a positive number representing Hz." + ) + def _get_latest_mocap_packet(self) -> DataMessages.sFrameOfMocapData | None: + # Thread-safe method to retrieve the latest mocap data to be sent + try: + return self.mocap_data_queue.get_nowait() + except queue.Empty: + return None + + @staticmethod + def _pad_fixed_string(value: bytes) -> bytes: + """Null-pad a byte string to MAX_NAMELENGTH for fixed-size NatNet name fields.""" + truncated = value[: ServerMessages.MAX_NAMELENGTH - 1] + return truncated + b"\x00" * (ServerMessages.MAX_NAMELENGTH - len(truncated)) + + @staticmethod + def _assign_version_bytes(field: ctypes.Array, version: typing.Tuple[int, int, int, int]) -> None: + for index, component in enumerate(version): + field[index] = component + + @staticmethod + def _assign_ipv4_bytes(field: ctypes.Array, address: str | bytes) -> None: + octets = socket.inet_aton(address) if isinstance(address, str) else address + for index, octet in enumerate(octets): + field[index] = octet + + def _build_server_description(self) -> ServerMessages.sServerDescription: + # Helper to build the server description struct with current server info (e.g. on startup or in response to command request) + description = ServerMessages.sServerDescription() + description.HostPresent = True + description.szHostComputerName = self._pad_fixed_string( + socket.gethostname().encode("utf-8") + ) + self._assign_ipv4_bytes(description.HostComputerAddress, self.local_interface) + description.szHostApp = self._pad_fixed_string(b"Motive") + self._assign_version_bytes(description.HostAppVersion, self.motive_app_version) + self._assign_version_bytes(description.NatNetVersion, self.natnet_version) + description.HighResClockFrequency = self.high_res_clock_freq + description.bConnectionInfoValid = True + description.ConnectionDataPort = self.data_port + description.ConnectionMulticast = self.transmission_type == TransmissionType.MULTICAST + + if self.transmission_type == TransmissionType.MULTICAST: + self._assign_ipv4_bytes(description.ConnectionMulticastAddress, self.multicast_address) + else: + self._assign_ipv4_bytes(description.ConnectionMulticastAddress, b"\x00\x00\x00\x00") + + return description + + def _build_connect_response_payload(self) -> bytes: + """NAT_CONNECT reply: libNatNet parses NAT_SERVERINFO payload as sSender_Server.""" + sender = ServerMessages.sSender_Server() + sender.Common.szName = self._pad_fixed_string(b"Motive") + self._assign_version_bytes(sender.Common.Version, self.motive_app_version) + self._assign_version_bytes(sender.Common.NatNetVersion, self.natnet_version) + sender.HighResClockFrequency = self.high_res_clock_freq + sender.DataPort = self.data_port + sender.IsMulticast = self.transmission_type == TransmissionType.MULTICAST + if self.transmission_type == TransmissionType.MULTICAST: + self._assign_ipv4_bytes(sender.MulticastGroupAddress, self.multicast_address) + else: + self._assign_ipv4_bytes(sender.MulticastGroupAddress, b"\x00\x00\x00\x00") + return sender.pack() + + def _send_packet_to_client( + self, + client: Client, + message_id: ServerMessages.MessageId | int, + payload: bytes, + sock: socket.socket | None = None, + ) -> None: + """Send a NatNet packet to a unicast client (libNatNet 4.4). + + Command replies go out the command socket; mocap frames go out the data socket. + """ + if self.shutdown_event.is_set(): + return + sock = sock or self.command_socket + if not sock: + raise ValueError("[NatNetServer] Socket not initialized. Cannot send packet.") + + header = ServerMessages.sPacketHeader( + iMessage=int(message_id), + nDataBytes=len(payload), + ) + packet = header.pack() + payload + try: + with client.socket_lock: + sock.sendto(packet, (client.ip, client.port)) + except OSError as e: + raise ValueError( + f"[NatNetServer] Error sending message {int(message_id)} to " + f"client {client.ip}:{client.port}: {e}" + ) from e + + def _data_update_loop(self): # Stub: Different betweeen multicast and unicast server implementations, as they will need to handle client connections differently (multicast will just send to the multicast group address) + # Loop to update mocap data and send packets at regular intervals. + pass + + def _send_data_packet(self, client: Client, data_message: DataMessages.sFrameOfMocapData): + # Serialize frame payload and send via the data socket. + # + # Stamp the transmit time in the server's high-resolution clock domain so + # the client can recover per-message transit latency via + # NatNetClient::SecondsSinceHostTimestamp(TransmitTimestamp). This must match + # the clock used in the NAT_ECHORESPONSE handshake (time.time() nanoseconds) + # and the advertised HighResClockFrequency (defaults to 1e9 ticks/s), so the + # SDK's server-clock estimate and this timestamp share one timeline. + data_message.TransmitTimestamp = int(time.time() * 1_000_000_000) + try: + packet_bytes = data_message.pack() + except Exception as e: + raise ValueError(f"[NatNetServer] Error serializing data message: {e}") from e + + self._send_packet_to_client( + client, + ServerMessages.MessageId.NAT_FRAMEOFDATA, + packet_bytes, + sock=self.data_socket, + ) + + def _command_listener_loop(self): # Stub: Different betweeen multicast and unicast server implementations, as they will need to handle client connections differently (multicast will just send to the multicast group address) + # Loop to listen for and handle incoming command requests (e.g. from client apps) + pass + + def _handle_command_request(self, request_data: bytes): # Stub: Different betweeen multicast and unicast server implementations, as they will need to handle client connections differently (multicast will just send to the multicast group address) + # Parse incoming command request, perform requested action, and send response if needed + pass + diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_server_types.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_server_types.py new file mode 100644 index 000000000..3083f1a6e --- /dev/null +++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_server_types.py @@ -0,0 +1,156 @@ +import ctypes +import struct +from enum import IntEnum + +MAX_NAMELENGTH = 256 +MAX_PACKETSIZE = 65503 + +# NatNet SDK sServerDescription uses default struct alignment (#pragma pack(pop)), not pack(1). +SERVER_DESCRIPTION_WIRE_SIZE = 552 +# NAT_CONNECT / NAT_SERVERINFO reply uses packed sSender_Server (#pragma pack(1) in NatNetTypes.h). +SENDER_SERVER_WIRE_SIZE = 256 + 4 + 4 + 8 + 2 + 1 + 4 # 279 + +# Client/server message ids +class MessageId(IntEnum): + NAT_CONNECT = 0 + NAT_SERVERINFO = 1 + NAT_REQUEST = 2 + NAT_RESPONSE = 3 + NAT_REQUEST_MODELDEF = 4 + NAT_MODELDEF = 5 + NAT_REQUEST_FRAMEOFDATA = 6 + NAT_FRAMEOFDATA = 7 + NAT_MESSAGESTRING = 8 + NAT_DISCONNECT = 9 + NAT_KEEPALIVE = 10 + NAT_DISCONNECTBYTIMEOUT = 11 + NAT_ECHOREQUEST = 12 + NAT_ECHORESPONSE = 13 + NAT_DISCOVERY = 14 + NAT_UNRECOGNIZED_REQUEST = 100 + +# Server/Sender configuration and info +def _fixed_name(field: ctypes.Array) -> bytes: + raw = bytes(field).split(b"\x00", 1)[0] + b"\x00" + if len(raw) > MAX_NAMELENGTH: + raw = raw[: MAX_NAMELENGTH - 1] + b"\x00" + return raw + b"\x00" * (MAX_NAMELENGTH - len(raw)) + + +class sSender(ctypes.Structure): + _pack_ = 1 + _fields_ = [ + ("szName", ctypes.c_char * MAX_NAMELENGTH), # host app's name + ("Version", ctypes.c_uint8 * 4), # host app's version [major.minor.build.revision] + ("NatNetVersion", ctypes.c_uint8 * 4) # host app's NatNet version + ] + + def pack(self) -> bytes: + payload = bytearray() + payload += _fixed_name(self.szName) + payload += bytes(self.Version) + payload += bytes(self.NatNetVersion) + return bytes(payload) + +class sSender_Server(ctypes.Structure): + _pack_ = 1 + _fields_ = [ + ("Common", sSender), + ("HighResClockFrequency", ctypes.c_uint64), + ("DataPort", ctypes.c_uint16), + ("IsMulticast", ctypes.c_bool), + ("MulticastGroupAddress", ctypes.c_uint8 * 4) + ] + + def pack(self) -> bytes: + payload = bytearray(self.Common.pack()) + payload += struct.pack(" bytes: + # Wire layout matches NatNet SDK on x86-64 (3 pad bytes before HighResClockFrequency). + payload = bytearray() + payload.append(1 if self.HostPresent else 0) + payload += _fixed_name(self.szHostComputerName) + payload += bytes(self.HostComputerAddress) + payload += _fixed_name(self.szHostApp) + payload += bytes(self.HostAppVersion) + payload += bytes(self.NatNetVersion) + while len(payload) % 8: + payload.append(0) + payload += struct.pack(" bytes: + return bytes(self) + +# Connection types enum matching NatNet SDK rules +class ConnectionType(IntEnum): + ConnectionType_Multicast = 0 + ConnectionType_Unicast = 1 + +class sNatNetClientConnectParams(ctypes.Structure): + """ + Python ctypes translation of the C++ sNatNetClientConnectParams struct. + Enforces a packed structure byte alignment matching the NatNet binary network protocol. + """ + _pack_ = 1 + _fields_ = [ + ("connectionType", ctypes.c_int32), # 4 bytes (mapping to standard ConnectionType enum) + ("serverCommandPort", ctypes.c_uint16), # 2 bytes + ("serverDataPort", ctypes.c_uint16), # 2 bytes + + # NOTE: Represented as void pointers (c_void_p) to safely match the host system's native bit size (e.g., 8 bytes on 64-bit) without string data unpacking overhead. + ("serverAddress", ctypes.c_void_p), + ("localAddress", ctypes.c_void_p), + ("multicastAddress", ctypes.c_void_p), + + ("subscribedDataOnly", ctypes.c_bool), # 1 byte + ("BitstreamVersion", ctypes.c_uint8 * 4) # 4 bytes: [Major, Minor, Build, Revision] + ] + + def pack(self) -> bytes: + return bytes(self) \ No newline at end of file diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_unicast_server.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_unicast_server.py new file mode 100644 index 000000000..d9c0a1e22 --- /dev/null +++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_unicast_server.py @@ -0,0 +1,172 @@ +import ctypes +import time + +from . import natnet_server_types as ServerTypes +from .natnet_server import TransmissionType, Client, NatNetServer + + +class NatNetUnicastServer(NatNetServer): + def __init__(self, + local_interface="172.31.0.200", + transmission_type: TransmissionType = TransmissionType.UNICAST, + multicast_address=None, + command_port=1510, + data_port=1511 + ): + + if not transmission_type == TransmissionType.UNICAST: + raise ValueError("Transmission type 'MULTICAST' is not supported in NatNetUnicastServer. Please use NatNetMulticastServer instead.") + + super().__init__(local_interface, transmission_type, multicast_address, command_port, data_port) + + def _data_update_loop(self): + # Loop to update mocap data and send packets at regular intervals. + # When auto_stream is False the frames are pumped externally (Isaac physics step), + # so this thread only idles — but stays alive for clean shutdown. + while not self.shutdown_event.is_set(): + time.sleep(1 / self.publish_rate) + if not self.auto_stream: + continue + self.flush_mocap_data() + + def flush_mocap_data(self): + """Send the latest (or last) mocap frame to every connected client, once.""" + with self.clients_lock: + clients = list(self.connected_clients) + if not clients: + return + + data_messages = self._get_latest_mocap_packet() + + if data_messages is None: # If the server stops producing frames, use the last known frame. + data_messages = self._get_last_known_mocap_frame() + if data_messages is None: + return + + for client in clients: + try: + self._send_data_packet(client, data_messages) + except ValueError as e: + print(str(e)) + continue + + def _command_listener_loop(self): + # Listens on UDP command socket for incoming command requests from clients. + # Handles incoming client handshakes and teardown. + + print(f"[Command Listener] Command listener thread started. Listening for incoming client command requests on UDP address:port {self.local_interface}:{self.command_port}...") + + while not self.shutdown_event.is_set(): + try: + data, addr = self.command_socket.recvfrom(1024) # Buffer size of 1024 bytes should be sufficient for command requests + if not data: + continue + self._handle_command_request(data, addr) + except Exception as e: + if self.shutdown_event.is_set(): + break + print(f"[Command Listener] Error receiving command request: {e}") + time.sleep(0.1) # Sleep briefly to avoid tight loop on errors + + def _handle_command_request(self, request_data: bytes, client_address: tuple): + """ + Processes standard binary headers and registers unicast endpoints. + """ + header_size = ctypes.sizeof(ServerTypes.sPacketHeader) + if len(request_data) < header_size: + return + + # Parse the header via ctypes + header = ServerTypes.sPacketHeader.from_buffer_copy(request_data[:header_size]) + + # Handle Connection Handshake + if header.iMessage == int(ServerTypes.MessageId.NAT_CONNECT): + client_requested_version = self.natnet_version # Fallback to server's version. Version handshaking not supported in this extension. + + client_ip, client_port = client_address + + # Create and store a new client object + new_client = Client(client_ip, client_port, version=client_requested_version) + try: + with self.clients_lock: + self.connected_clients.discard(new_client) # Remove any existing client with the same IP and port + self.connected_clients.add(new_client) # Add the new client to the connected clients list + print(f"[Command Handler] Added client {new_client.ip}:{new_client.port} to connected clients list.") + except Exception as e: + print(f"[Command Handler] Error adding client {new_client.ip}:{new_client.port} to connected clients list: {e}") + return + + try: + self._send_packet_to_client( + new_client, + ServerTypes.MessageId.NAT_SERVERINFO, + self._build_connect_response_payload(), + ) + except ValueError as e: + raise ValueError( + f"[Command Handler] Error sending server description to client {client_address}: {e}" + ) from e + print( + f"[Command Handler] Sent server description to client address " + f"through its port {client_address}." + ) + return + + # Non-handshake commands require a prior NAT_CONNECT from this endpoint. + client_ip, client_port = client_address + client = self._find_client(client_ip, client_port) + if client is None: + print( + f"[Command Handler] Ignoring message {header.iMessage} from " + f"unregistered client {client_address}." + ) + return + + if header.iMessage == int(ServerTypes.MessageId.NAT_REQUEST_MODELDEF): + try: + self._send_packet_to_client( + client, + ServerTypes.MessageId.NAT_MODELDEF, + self._get_model_def_payload(), + ) + except ValueError as e: + print( + f"[Command Handler] Error sending MODELDEF to client " + f"{client_address}: {e}" + ) + return + + if header.iMessage == int(ServerTypes.MessageId.NAT_KEEPALIVE): + # Receiving a keepalive refreshes the client's liveness; nothing to send back. + return + + if header.iMessage == int(ServerTypes.MessageId.NAT_ECHOREQUEST): + echo_payload = request_data[header_size : header_size + header.nDataBytes] + # libNatNet expects clientRequestTimestamp + hostReceivedTimestamp (8 + 8 bytes). + host_ts = int(time.time() * 1_000_000_000).to_bytes(8, "little", signed=False) + response_payload = echo_payload[:8].ljust(8, b"\x00") + host_ts + try: + self._send_packet_to_client( + client, + ServerTypes.MessageId.NAT_ECHORESPONSE, + response_payload, + ) + except ValueError as e: + print( + f"[Command Handler] Error sending ECHORESPONSE to client " + f"{client_address}: {e}" + ) + return + + print( + f"[Command Handler] Unhandled message id {header.iMessage} from " + f"registered client {client_address}." + ) + + def _find_client(self, ip: str, port: int) -> Client | None: + target = Client(ip, port) + with self.clients_lock: + for client in self.connected_clients: + if client == target: + return client + return None diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/setup.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/setup.py new file mode 100644 index 000000000..1a1c153fc --- /dev/null +++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/setup.py @@ -0,0 +1,23 @@ +"""Isaac Sim extension install metadata for the OptiTrack NatNet emulator.""" + +import os + +from setuptools import find_packages, setup + +EXTENSION_PATH = os.path.dirname(os.path.realpath(__file__)) + +setup( + name="optitrack-natnet-emulator", + version="0.1.0", + description="NatNet UDP server emulator for Isaac Sim and natnet_ros2 integration", + license="MIT", + include_package_data=True, + python_requires=">=3.10", + install_requires=[ + "numpy", + "scipy", + ], + packages=find_packages(where="."), + package_dir={"": "."}, + zip_safe=False, +) diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/natnet_test_helpers.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/natnet_test_helpers.py new file mode 100644 index 000000000..07d6c8a5e --- /dev/null +++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/natnet_test_helpers.py @@ -0,0 +1,105 @@ +"""Shared helpers for optitrack.natnet.emulator unit tests.""" + +from __future__ import annotations + +import socket +import struct +import time +from contextlib import contextmanager + +from optitrack.natnet.emulator import NatNetUnicastServer, TransmissionType +from optitrack.natnet.emulator.server import natnet_server_types as st + + +def ephemeral_udp_port(host: str = "127.0.0.1") -> int: + """Return a free UDP port on *host* by binding and releasing a probe socket.""" + with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as probe: + probe.bind((host, 0)) + return probe.getsockname()[1] + + +class NatNetTestClient: + """Minimal UDP client for NatNet command-port protocol tests.""" + + def __init__(self, host: str = "127.0.0.1", timeout: float = 2.0) -> None: + self._host = host + self._sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + self._sock.bind((host, 0)) + self._sock.settimeout(timeout) + + @property + def local_port(self) -> int: + return self._sock.getsockname()[1] + + def send_message( + self, + server_port: int, + message_id: st.MessageId | int, + payload: bytes = b"", + server_host: str | None = None, + ) -> None: + header = st.sPacketHeader( + iMessage=int(message_id), + nDataBytes=len(payload), + ) + self.send_raw(header.pack() + payload, server_port, server_host) + + def send_raw( + self, + data: bytes, + server_port: int, + server_host: str | None = None, + ) -> None: + """Send a raw UDP datagram (for malformed / malicious packet tests).""" + self._sock.sendto(data, (server_host or self._host, server_port)) + + def send_header_only( + self, + server_port: int, + message_id: st.MessageId | int, + declared_payload_len: int, + server_host: str | None = None, + ) -> None: + """Send a header whose nDataBytes does not match any trailing payload.""" + header = struct.pack(" tuple[int, bytes, tuple[str, int]]: + data, addr = self._sock.recvfrom(65535) + message_id, payload_len = struct.unpack(" None: + self._sock.close() + + +@contextmanager +def running_unicast_server( + command_port: int | None = None, + local_interface: str = "127.0.0.1", + publish_rate: int = 100, +): + """Start NatNetUnicastServer on ephemeral (or fixed) command + data ports. + + Both ports are ephemeral by default so concurrent/sequential tests never + collide on the well-known 1510/1511 pair. + """ + port = command_port if command_port is not None else ephemeral_udp_port(local_interface) + data_port = ephemeral_udp_port(local_interface) + while data_port == port: + data_port = ephemeral_udp_port(local_interface) + server = NatNetUnicastServer( + local_interface=local_interface, + transmission_type=TransmissionType.UNICAST, + multicast_address=None, + command_port=port, + data_port=data_port, + ) + server.publish_rate = publish_rate + server.start() + time.sleep(0.05) + try: + yield server, port + finally: + server.shutdown() diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_defaults.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_defaults.py new file mode 100644 index 000000000..7cc72b036 --- /dev/null +++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_defaults.py @@ -0,0 +1,26 @@ +# Copyright (c) 2024 Carnegie Mellon University +# MIT License - see LICENSE in the repository root for full text. +"""Unit tests for hardcoded tracked-body defaults.""" + +from __future__ import annotations + +import pytest + +from optitrack.natnet.emulator.defaults import ( + DEFAULT_DRONE_BINDING, + DEFAULT_TRACKED_BODY_BINDINGS, +) + + +pytestmark = pytest.mark.unit + + +def test_default_drone_binding_matches_natnet_ros2_config(): + assert DEFAULT_DRONE_BINDING.name == "Drone" + assert DEFAULT_DRONE_BINDING.id == 1 + assert DEFAULT_DRONE_BINDING.parent_id == -1 + assert DEFAULT_DRONE_BINDING.prim_path == "/World/base_link" + + +def test_default_tracked_body_bindings_contains_drone_only(): + assert DEFAULT_TRACKED_BODY_BINDINGS == (DEFAULT_DRONE_BINDING,) diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_serializers.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_serializers.py new file mode 100644 index 000000000..0db5a34df --- /dev/null +++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_serializers.py @@ -0,0 +1,341 @@ +# Copyright (c) 2024 Carnegie Mellon University +# MIT License - see LICENSE in the repository root for full text. +"""Unit tests for NatNet wire serializers (no network).""" + +from __future__ import annotations + +import ctypes +import struct + +import pytest + +from optitrack.natnet.emulator import NatNetUnicastServer, TransmissionType +from optitrack.natnet.emulator.server import natnet_data_types as dt +from optitrack.natnet.emulator.server import natnet_model_types as mt +from optitrack.natnet.emulator.server import natnet_server_types as st +from optitrack.natnet.emulator.server.natnet_common import ModelLimits + + +pytestmark = pytest.mark.unit + + +# ============================================================================= +# natnet_server_types — transport / handshake +# ============================================================================= + + +def test_packet_header_pack_size_and_endianness(): + header = st.sPacketHeader( + iMessage=int(st.MessageId.NAT_FRAMEOFDATA), + nDataBytes=42, + ) + packed = header.pack() + + assert len(packed) == ctypes.sizeof(st.sPacketHeader) == 4 + message_id, payload_len = struct.unpack(" server only; real Motive sends no reply. An echo + # reply makes libNatNet log "Received unrecognized message Message=10". + with running_unicast_server() as (server, command_port): + client = NatNetTestClient(timeout=0.5) + try: + client.send_message(command_port, st.MessageId.NAT_CONNECT) + client.recv_message() + + client.send_message(command_port, st.MessageId.NAT_KEEPALIVE) + with pytest.raises(socket.timeout): + client.recv_message() + + # Client stays registered and keeps receiving frames. + assert len(server.connected_clients) == 1 + finally: + client.close() + + +# ============================================================================= +# Malformed datagrams — registered client & recovery +# ============================================================================= + + +def test_unknown_message_from_registered_client_gets_no_reply(): + with running_unicast_server() as (server, command_port): + client = NatNetTestClient(timeout=0.5) + try: + client.send_message(command_port, st.MessageId.NAT_CONNECT) + client.recv_message() + + client.send_message(command_port, 999) + with pytest.raises(socket.timeout): + client.recv_message() + + assert len(server.connected_clients) == 1 + finally: + client.close() + + +def test_server_survives_malformed_burst_then_valid_connect(): + with running_unicast_server() as (server, command_port): + client = NatNetTestClient(timeout=2.0) + try: + client.send_raw(b"", command_port) + client.send_raw(b"\xff", command_port) + client.send_header_only(command_port, 999, declared_payload_len=50000) + client.send_message(command_port, st.MessageId.NAT_REQUEST_MODELDEF) + + client.send_message(command_port, st.MessageId.NAT_CONNECT) + message_id, _payload, _addr = client.recv_message() + finally: + client.close() + + assert message_id == int(st.MessageId.NAT_SERVERINFO) + assert len(server.connected_clients) == 1 diff --git a/tests/colcon_unit_test_packages.yaml b/tests/colcon_unit_test_packages.yaml index 65cdd38cc..e7a204e40 100644 --- a/tests/colcon_unit_test_packages.yaml +++ b/tests/colcon_unit_test_packages.yaml @@ -13,3 +13,9 @@ robot: - lidar_point_cloud_filter # Skips ament_copyright / flake8 / pep257 on Python packages; run linters separately. pytest_args: "-m not linter" + +# Simulation-side extensions (globbed under simulation/**//test). Collected by +# `pytest tests/` on the host runner; not part of the robot colcon workspace. +sim: + packages: + - optitrack.natnet.emulator diff --git a/tests/conftest.py b/tests/conftest.py index 47a668e65..49bb83a14 100644 --- a/tests/conftest.py +++ b/tests/conftest.py @@ -46,6 +46,16 @@ def pytest_configure(config): run_dir = session.init_run_dir(AIRSTACK_ROOT) config.option.xmlpath = str(run_dir / "results.xml") + # Co-located unit tests import their own package (e.g. `optitrack.natnet.emulator`, + # `lidar_point_cloud_filter.validation_core`). Put each package/extension import + # root (the parent of its test/ dir) on sys.path so they resolve without a + # per-package conftest.py — a second conftest.py collides with this root one as + # module `conftest` under --import-mode=importlib and breaks `from conftest import`. + for d in unit_test_dirs(): + root = str(d.parent) + if root not in sys.path: + sys.path.insert(0, root) + # Collect co-located unit tests: their files live outside tests/, so add the # explicit non-linter test files to the collection args. Skip when an explicit # path was given on the CLI (args_source == ARGS) so `pytest tests/system/foo.py` diff --git a/tests/integration/natnet/README.md b/tests/integration/natnet/README.md new file mode 100644 index 000000000..4e9330c32 --- /dev/null +++ b/tests/integration/natnet/README.md @@ -0,0 +1,151 @@ +# NatNet ↔ robot autonomy integration + +Host-side NatNet wire-protocol tests that drive the Python emulator against +`natnet_ros2_node` in a real robot container. First resident of the +[`integration`](../README.md) tier (no sim, no GPU). + +Mark: `integration`. Filter this scenario with `tests/integration/natnet/`. + +For the **in-sim** end-to-end check (Isaac emulator + full stack), see +[Liveliness sentinel](#liveliness-sentinel-sim-end-to-end) below and +[`tests/system/test_liveliness.py`](../../system/test_liveliness.py). + +## What it verifies + +Three variants in [`test_natnet_integration.py`](test_natnet_integration.py). +All start a host-side `NatNetUnicastServer`, launch `natnet_ros2_node` in the +robot container pointed at the Docker bridge gateway, and assert a sustained +pose stream at **≥ 5 Hz** on the configured topic(s), e.g.: + +- `/{ROBOT_NAME}/perception/optitrack/drone/pose_cov` (wait for first message) +- `/{ROBOT_NAME}/perception/optitrack/drone` (Hz sample) + +| Test | Path | +|------|------| +| **`test_natnet_ros2_receives_drone_pose_hz`** | Hand-built `sFrameOfMocapData` frames enqueued on a raw `NatNetUnicastServer` (no USD). Minimal wire + SDK check. | +| **`test_natnet_ros2_receives_isaac_wrapper_pose_hz`** | Full Isaac data path: in-memory USD stage, `NatNetInterfaceConfig`, `author_interface`, `NatNetServerManager.sample_once()` on a moving prim — same sampling logic as the in-sim physics-step callback. Skips without `usd-core` (`pxr`). Pose-value fidelity is covered hermetically by the emulator's `test_pose_streaming.py` loopback. | +| **`test_natnet_ros2_multi_body_drone_and_target`** | Two bodies (drone id 1 + target id 100) with distinct relative topics; asserts both pose streams and that the target's `pose_cov` topic is **absent** (`body_pose_cov=false`). Exercises the multi-body profile + per-body `pose`/`pose_cov` toggles. | + +These tests **do not** start the full perception bringup or `LAUNCH_NATNET`; they +exec `natnet_ros2_node` directly with the flattened per-body params +(`body_names`/`body_ids`/`body_topics`/`body_pose`/`body_pose_cov`) and no MAVROS bridge. + +## Requirements + +- Docker daemon (robot-desktop container reachable from pytest). +- **`natnet_ros2_node` built** in the robot image (OptiTrack NatNet SDK is + license-gated — run `airstack setup --natnet`, then + `bws --packages-select natnet_ros2` in the container). Tests **skip** if the + node binary is missing. +- Host-side emulator package on `PYTHONPATH` (the test adds + `simulation/isaac-sim/extensions/optitrack.natnet.emulator` — not pip-installed + on the host). +- Ephemeral UDP ports on the host gateway IP (Docker default route as seen from + inside the container). + +The robot container comes from the shared **`robot_autonomy_stack`** fixture in +[`tests/conftest.py`](../../conftest.py) (see the [integration tier README](../README.md)). + +## Running + +```bash +# 1. One-time: NatNet SDK + build natnet_ros2 in the robot image +airstack setup --natnet # or NATNET_ACCEPT_LICENSE=1 airstack setup --natnet +docker exec airstack-robot-desktop-1 bash -lc 'bws --packages-select natnet_ros2' + +# 2a. Reuse an existing robot container (fast local iteration): +AUTOLAUNCH=false airstack up robot-desktop +pytest tests/integration/natnet/ -m integration -v + +# 2b. Let the harness bring the container up/down: +pytest tests/integration/natnet/ -m integration -v +``` +On CI / PR (write access): `/pytest -m integration` + +## Architecture + +``` +┌──────────────────────────────────────────────────────────────┐ +│ Host (pytest) │ +│ NatNetUnicastServer @ docker bridge gateway IP │ +│ • raw variant: hand-built frame queue │ +│ • Isaac variant: NatNetServerManager.sample_once(USD) │ +└────────────────────────────┬─────────────────────────────────┘ + │ UDP unicast (cmd + data ports) +┌────────────────────────────▼─────────────────────────────────┐ +│ Robot container (robot-desktop) │ +│ natnet_ros2_node (libNatNet 4.4 client) │ +│ → /{ROBOT_NAME}/{body topic}[/pose_cov] per configured body │ +└──────────────────────────────────────────────────────────────┘ +``` + +**In sim (liveliness tier):** the server runs inside the Isaac Sim container +(`172.31.0.200` by default). Use a NatNet Pegasus launch script +(`example_one_px4_pegasus_natnet_launch_script.py` or +`example_multi_px4_pegasus_natnet_launch_script.py`); `natnet_ros2` in the +robot stack connects via `natnet_config.yaml` (`server_ip` → emulator IP). + +**Catalog / MODELDEF:** The server holds a MODELDEF **wire cache** only +(`set_model_def_payload()`). Scene semantics (body names, streaming IDs, target +prim paths) come from the Isaac layer (`NatNetInterfaceConfig`, USD interface +prim, or launch-script `build_drone_config`). See the +[emulator README](../../../simulation/isaac-sim/extensions/optitrack.natnet.emulator/README.md). + +## Liveliness sentinel (sim end-to-end) + +The integration tier proves **robot client + host emulator** without Isaac. +The matching **system** check is +`TestLiveliness::test_natnet_pose_alive` in +[`test_liveliness.py`](../../system/test_liveliness.py): + +- **Gated on `LAUNCH_NATNET=true`** (skipped otherwise — normal liveliness runs + are unaffected). +- Asserts `/{robot_n}/{natnet pose topic}/pose_cov` ≥ 5 Hz per robot (the drone + body's configured topic — default `perception/optitrack/drone`). +- Override the checked topic with `NATNET_POSE_TOPIC` (default + `perception/optitrack/drone`). The sim body name (`NATNET_BODY_NAME`, default + `Drone`) is decoupled from the published topic, which the robot profile sets. + +Sim auto-start: set `ISAAC_SIM_SCRIPT_NAME` to a NatNet launch script and +`LAUNCH_NATNET=true` on the robot. Convenience bundle: +`airstack up --env-file overrides/isaac-natnet-vision.env` (NatNet script + +PX4 external-vision SITL profile). + +## libNatNet 4.4 unicast — verified wire contract + +The emulator is validated against the **real `libNatNet.so`** (not just the Python +`NatNetClient`) with a minimal C probe that registers `SetFrameReceivedCallback` +and `NatNet_SetLogCallback`. All of the following must hold for the SDK to deliver +frames to the callback: + +| Requirement | Why | +|-------------|-----| +| `NAT_CONNECT` → `sSender_Server` (279 B), name `Motive` | libNatNet reads `Motive 3.1 / NatNet 4.4` | +| `NAT_ECHOREQUEST` → `NAT_ECHORESPONSE` (16 B) | Prevents libNatNet assert | +| Frame ends with a **4-byte end-of-data tag** after `params` | libNatNet's frame unpacker reads it; without it the unpacked size mismatches `nDataBytes` and **every frame is silently dropped** | +| `NAT_FRAMEOFDATA` sent from the **data port** (source port == `data_port`) | libNatNet routes unicast frames by the server's data port. Frames sent from the **command** port are treated as command traffic and dropped — no error, no callback | +| `NAT_KEEPALIVE` gets **no reply** | An echo reply makes libNatNet log `Received unrecognized message Message=10` | + +With these in place the C probe reports `Server: Motive 3.1.0.0 NatNet 4.4.0.0`, +`data descriptions: 1`, and **~74 Hz** of frame callbacks. + +> The lenient Python `NatNetClient` accepts frames *without* the end-of-data tag +> and *on the command port*, which is why it appeared to work while libNatNet did +> not. Always validate against the C SDK. + +Full handshake notes and sniffing workflow: +[optitrack-development skill](../../../.agents/skills/optitrack-development/SKILL.md). + +## After changing natnet_ros2 or the emulator + +Rebuild in the robot container: + +```bash +docker exec airstack-robot-desktop-1 bash -lc 'bws --packages-select natnet_ros2' +``` + +Unit tests (protocol, serializers, Isaac wrapper loopback): + +```bash +pytest tests/sim/optitrack_natnet_emulator/ -m unit -v +``` diff --git a/tests/integration/natnet/test_natnet_integration.py b/tests/integration/natnet/test_natnet_integration.py new file mode 100644 index 000000000..43fa4a7f3 --- /dev/null +++ b/tests/integration/natnet/test_natnet_integration.py @@ -0,0 +1,376 @@ +# Copyright (c) 2024 Carnegie Mellon University +# MIT License - see LICENSE in the repository root for full text. +"""NatNet - robot autonomy integration tests. + +Host-side variants stream frames to ``natnet_ros2_node`` in the robot container and +assert pose topics stay alive at >= 5 Hz: (1) raw ``NatNetUnicastServer`` hand-built +single-body frames; (2) ``NatNetServerManager`` sampling an in-memory USD stage +(Isaac wrapper path, no sim/GPU); (3) a multi-body profile (drone + target) that +exercises per-body topic overrides and the pose / pose_cov toggles. + +The node is parameterised with the flattened per-body arrays +(``body_names`` / ``body_ids`` / ``body_topics`` / ``body_pose`` / ``body_pose_cov``) +that natnet_ros2.launch.py derives from a robot's natnet_config.yaml profile. + +Multi-robot (NUM_ROBOTS=3, per-robot profiles) is exercised in-sim by +``tests/system/test_liveliness.py::test_natnet_pose_alive``. +""" + +from __future__ import annotations + +import subprocess +import sys +import threading +import time + +import pytest + +from conftest import ( # noqa: E402 — pytest adds tests/ to sys.path + docker_exec, + repo_path, + ros2_env, + sample_hz, + wait_for_first_message, +) + +# Emulator is not pip-installed on the host; add extension root + test helpers. +_EXT_ROOT = repo_path("simulation/isaac-sim/extensions/optitrack.natnet.emulator") +for _path in (_EXT_ROOT, _EXT_ROOT / "test"): + if str(_path) not in sys.path: + sys.path.insert(0, str(_path)) + +from optitrack.natnet.emulator import NatNetUnicastServer, TransmissionType # noqa: E402 +from optitrack.natnet.emulator.server import natnet_data_types as dt # noqa: E402 +from natnet_test_helpers import ephemeral_udp_port # noqa: E402 + +pytestmark = pytest.mark.integration + +_ROBOT_SETUP = "/root/AirStack/robot/ros_ws/install/setup.bash" +_NATNET_NODE = "/root/AirStack/robot/ros_ws/install/natnet_ros2/lib/natnet_ros2/natnet_ros2_node" +_WARMUP_S = 2.0 +_STREAM_HOLD_S = 12.0 +_MIN_HZ = 5.0 + +# Robot image has route/netstat but not `ip`; /proc/net/route is always present. +_DEFAULT_GATEWAY_CMD = ( + """awk '$2 == "00000000" { printf "%d.%d.%d.%d\\n", """ + """"0x" substr($3,7,2), "0x" substr($3,5,2), "0x" substr($3,3,2), "0x" substr($3,1,2); exit }' """ + """/proc/net/route""" +) + + +def _docker_default_gateway(container: str) -> str: + result = docker_exec(container, _DEFAULT_GATEWAY_CMD, timeout=10) + gateway = result.stdout.strip() + if not gateway: + pytest.skip(f"Could not resolve default gateway inside {container}") + return gateway + + +def _container_env(container: str, var: str, default: str) -> str: + # ROBOT_NAME / ROS_DOMAIN_ID are set in .bashrc (login shell), not container ENV. + # .bashrc may print "Sourcing ..." to stdout; take the last line as the value. + result = docker_exec(container, f"bash -lc 'echo ${var}'") + lines = [line.strip() for line in result.stdout.splitlines() if line.strip()] + value = lines[-1] if lines else "" + return value if value else default + + +def _natnet_node_available(container: str) -> bool: + result = docker_exec(container, f"test -x {_NATNET_NODE} && echo yes || echo no") + return "yes" in result.stdout + + +def _stop_stale_natnet_nodes(container: str) -> None: + docker_exec(container, "pkill -f natnet_ros2_node || true") + time.sleep(0.5) + + +# Each body: (streaming_id, rigid_body_name). The raw server frame carries ids only; +# the node maps ids → topics via its body_* params. +_DRONE_BODY = (1, "Drone") +_TARGET_BODY = (100, "Target") + + +def _make_frame(frame_num: int, body_ids) -> dt.sFrameOfMocapData: + frame = dt.sFrameOfMocapData() + frame.iFrame = frame_num + frame.nRigidBodies = len(body_ids) + for slot, body_id in enumerate(body_ids): + rb = frame.RigidBodies[slot] + rb.ID = body_id + rb.qw = 1.0 + # Bit 0 = tracking valid; natnet_ros2 skips bodies without it (natnet_logic.hpp). + rb.params = 1 + return frame + + +def _frame_publisher( + server: NatNetUnicastServer, stop_event: threading.Event, body_ids=(1,) +) -> None: + frame_num = 0 + interval = 1.0 / server.publish_rate + while not stop_event.is_set(): + server.enqueue_mocap_data(_make_frame(frame_num, body_ids)) + frame_num += 1 + time.sleep(interval) + + +def _launch_natnet_node(container, host_ip, command_port, domain_id, bodies=None): + """Start natnet_ros2_node in the container pointed at the host emulator. + + ``bodies`` is a list of (id, name, topic, pose, pose_cov); defaults to a single + Drone body on topic ``perception/optitrack/drone`` (the shipped config default). + """ + if bodies is None: + bodies = [(1, "Drone", "perception/optitrack/drone", "true", "true")] + ids = ",".join(str(b[0]) for b in bodies) + names = ",".join(b[1] for b in bodies) + topics = ",".join(b[2] for b in bodies) + pose = ",".join(b[3] for b in bodies) + pose_cov = ",".join(b[4] for b in bodies) + launch_cmd = ( + f"bash -lc '{ros2_env(_ROBOT_SETUP, domain_id)} && " + f"exec {_NATNET_NODE} --ros-args " + f"-p server_ip:={host_ip} " + f"-p command_port:={command_port} " + f"-p body_names:=[{names}] " + f"-p body_ids:=[{ids}] " + f"-p body_topics:=[{topics}] " + f"-p body_pose:=[{pose}] " + f"-p body_pose_cov:=[{pose_cov}]'" + ) + return subprocess.Popen( + ["docker", "exec", container, "bash", "-c", launch_cmd], + stdout=subprocess.PIPE, + stderr=subprocess.STDOUT, + text=True, + ) + + +def _assert_pose_stream( + container, robot_name, domain_id, topic="perception/optitrack/drone", pose_cov=True +): + """Wait for the pose topic then assert a sustained rate >= _MIN_HZ. + + A body configured with ``body_pose_cov=false`` never publishes the ``/pose_cov`` + variant, so detect the first message on whichever topic the body actually emits. + """ + pose_topic = f"/{robot_name}/{topic}" + detect_topic = f"{pose_topic}/pose_cov" if pose_cov else pose_topic + + time.sleep(_WARMUP_S) + first_msg_s = wait_for_first_message( + container, detect_topic, domain_id, _ROBOT_SETUP, timeout=int(_STREAM_HOLD_S) + ) + assert first_msg_s is not None, ( + f"No messages on {detect_topic} within {_STREAM_HOLD_S}s " + "(NatNet connect or frame stream failed)" + ) + hz = sample_hz( + container, + pose_topic, + domain_id, + _ROBOT_SETUP, + duration=min(8, int(_STREAM_HOLD_S - first_msg_s)), + window=20, + ) + assert hz is not None, f"No sustained stream on {pose_topic}" + assert hz >= _MIN_HZ, f"Expected >= {_MIN_HZ} Hz on {pose_topic}, got {hz}" + + +def _terminate(proc) -> None: + if proc is None: + return + proc.terminate() + try: + proc.wait(timeout=5) + except subprocess.TimeoutExpired: + proc.kill() + + +def test_natnet_ros2_receives_drone_pose_hz(robot_autonomy_stack): + """Raw-server path: hand-built frames on NatNetUnicastServer.""" + container = robot_autonomy_stack["container"] + + if not _natnet_node_available(container): + pytest.skip( + "natnet_ros2_node not built — run airstack setup (NatNet SDK) and " + "bws --packages-select natnet_ros2 in the robot container" + ) + + _stop_stale_natnet_nodes(container) + + host_ip = _docker_default_gateway(container) + command_port = ephemeral_udp_port(host_ip) + robot_name = _container_env(container, "ROBOT_NAME", "robot_1") + domain_id = int(_container_env(container, "ROS_DOMAIN_ID", "0")) + + server = NatNetUnicastServer( + local_interface=host_ip, + transmission_type=TransmissionType.UNICAST, + multicast_address=None, + command_port=command_port, + ) + server.publish_rate = 50 + + stop_event = threading.Event() + publisher = threading.Thread( + target=_frame_publisher, args=(server, stop_event), daemon=True + ) + + node_proc: subprocess.Popen[str] | None = None + try: + # Seed dummy frames before the client connects; keep streaming the whole window. + publisher.start() + time.sleep(0.1) + server.start() + node_proc = _launch_natnet_node(container, host_ip, command_port, domain_id) + _assert_pose_stream(container, robot_name, domain_id) + finally: + stop_event.set() + publisher.join(timeout=2.0) + _terminate(node_proc) + server.shutdown() + + +def test_natnet_ros2_receives_isaac_wrapper_pose_hz(robot_autonomy_stack): + """Isaac-wrapper path: NatNetServerManager.sample_once on a moving USD prim. + + Tests that the wrapper feeds the real robot client end-to-end. Pose-value fidelity + is covered by test_pose_streaming.py loopback. + """ + pytest.importorskip("pxr") + import math + + from pxr import Gf, Usd, UsdGeom + + from optitrack.natnet.emulator.isaac import ( + BodyBinding, + NatNetInterfaceConfig, + NatNetServerManager, + author_interface, + ) + + container = robot_autonomy_stack["container"] + if not _natnet_node_available(container): + pytest.skip("natnet_ros2_node not built — run airstack setup (NatNet SDK)") + + _stop_stale_natnet_nodes(container) + + host_ip = _docker_default_gateway(container) + command_port = ephemeral_udp_port(host_ip) + data_port = ephemeral_udp_port(host_ip) + while data_port == command_port: + data_port = ephemeral_udp_port(host_ip) + robot_name = _container_env(container, "ROBOT_NAME", "robot_1") + domain_id = int(_container_env(container, "ROS_DOMAIN_ID", "0")) + + stage = Usd.Stage.CreateInMemory() + xform = UsdGeom.Xform.Define(stage, "/World/base_link") + translate_op = xform.AddTranslateOp() + translate_op.Set(Gf.Vec3d(0.0, 0.0, 1.0)) + cfg = NatNetInterfaceConfig( + server_ip=host_ip, + command_port=command_port, + data_port=data_port, + publish_rate=50.0, + bodies=[BodyBinding("Drone", "/World/base_link", streaming_id=1)], + ) + author_interface(stage, "/World/NatNetInterface", cfg) + + manager = NatNetServerManager(server_factory=None) # real server factory + stop_event = threading.Event() + + def _sampler(): + # Stand in for the in-sim physics-step callback: move the prim and sample. + interval = 1.0 / cfg.publish_rate + t = 0.0 + while not stop_event.is_set(): + translate_op.Set(Gf.Vec3d(math.sin(t), 0.0, 1.0)) + manager.sample_once(stage) + t += interval + time.sleep(interval) + + sampler = threading.Thread(target=_sampler, daemon=True) + + node_proc: subprocess.Popen[str] | None = None + try: + assert manager.start_server(cfg) is True + sampler.start() + time.sleep(0.1) + node_proc = _launch_natnet_node(container, host_ip, command_port, domain_id) + _assert_pose_stream(container, robot_name, domain_id) + finally: + stop_event.set() + sampler.join(timeout=2.0) + _terminate(node_proc) + manager.stop_server() + + +def test_natnet_ros2_multi_body_drone_and_target(robot_autonomy_stack): + """Multi-body profile: one robot tracks a drone + a static target. + + Streams two bodies (drone id 1, target id 100) and configures the node like a + robot profile with two bodies and distinct relative topics. Asserts: the drone + pose streams >= 5 Hz on its custom topic; the target pose streams on its own + topic; and the target's pose_cov topic is absent (body_pose_cov=false). + """ + container = robot_autonomy_stack["container"] + + if not _natnet_node_available(container): + pytest.skip("natnet_ros2_node not built — run airstack setup (NatNet SDK)") + + _stop_stale_natnet_nodes(container) + + host_ip = _docker_default_gateway(container) + command_port = ephemeral_udp_port(host_ip) + robot_name = _container_env(container, "ROBOT_NAME", "robot_1") + domain_id = int(_container_env(container, "ROS_DOMAIN_ID", "0")) + + server = NatNetUnicastServer( + local_interface=host_ip, + transmission_type=TransmissionType.UNICAST, + multicast_address=None, + command_port=command_port, + ) + server.publish_rate = 50 + + bodies = [ + (_DRONE_BODY[0], _DRONE_BODY[1], "perception/optitrack/drone", "true", "true"), + (_TARGET_BODY[0], _TARGET_BODY[1], "perception/optitrack/target", "true", "false"), + ] + body_ids = (_DRONE_BODY[0], _TARGET_BODY[0]) + + stop_event = threading.Event() + publisher = threading.Thread( + target=_frame_publisher, args=(server, stop_event, body_ids), daemon=True + ) + + node_proc: subprocess.Popen[str] | None = None + try: + publisher.start() + time.sleep(0.1) + server.start() + node_proc = _launch_natnet_node(container, host_ip, command_port, domain_id, bodies) + # Drone (pose + pose_cov) and target (pose only) both stream. + _assert_pose_stream(container, robot_name, domain_id, "perception/optitrack/drone") + _assert_pose_stream( + container, robot_name, domain_id, "perception/optitrack/target", pose_cov=False + ) + + # body_pose_cov=false → the target pose_cov publisher must not exist. + target_cov = f"/{robot_name}/perception/optitrack/target/pose_cov" + topics = docker_exec( + container, + f"bash -lc '{ros2_env(_ROBOT_SETUP, domain_id)} && ros2 topic list'", + timeout=15, + ).stdout + assert target_cov not in topics.split(), ( + f"{target_cov} should not exist when body_pose_cov=false; topics:\n{topics}" + ) + finally: + stop_event.set() + publisher.join(timeout=2.0) + _terminate(node_proc) + server.shutdown() From 090b6fdd097ac5afbf2a1ecfe3cfaca797d9531b Mon Sep 17 00:00:00 2001 From: John Date: Thu, 23 Jul 2026 14:15:14 -0400 Subject: [PATCH 2/3] =?UTF-8?q?test(natnet):=20host=20integration=20tests?= =?UTF-8?q?=20=E2=80=94=20emulator=20server=20=E2=86=92=20natnet=5Fros2?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Drive the real natnet_ros2 client from the host NatNet server emulator and check the drone pose reaches ROS at rate (single-body and multi-body profiles). No sim, no GPU — uses the base's `robot_autonomy_stack` fixture + `integration` mark. The Isaac-wrapper variant lands with the Isaac wrapper PR. Co-Authored-By: Claude Opus 4.8 --- .../natnet/test_natnet_integration.py | 74 ------------------- 1 file changed, 74 deletions(-) diff --git a/tests/integration/natnet/test_natnet_integration.py b/tests/integration/natnet/test_natnet_integration.py index 43fa4a7f3..e386fd1ee 100644 --- a/tests/integration/natnet/test_natnet_integration.py +++ b/tests/integration/natnet/test_natnet_integration.py @@ -234,80 +234,6 @@ def test_natnet_ros2_receives_drone_pose_hz(robot_autonomy_stack): server.shutdown() -def test_natnet_ros2_receives_isaac_wrapper_pose_hz(robot_autonomy_stack): - """Isaac-wrapper path: NatNetServerManager.sample_once on a moving USD prim. - - Tests that the wrapper feeds the real robot client end-to-end. Pose-value fidelity - is covered by test_pose_streaming.py loopback. - """ - pytest.importorskip("pxr") - import math - - from pxr import Gf, Usd, UsdGeom - - from optitrack.natnet.emulator.isaac import ( - BodyBinding, - NatNetInterfaceConfig, - NatNetServerManager, - author_interface, - ) - - container = robot_autonomy_stack["container"] - if not _natnet_node_available(container): - pytest.skip("natnet_ros2_node not built — run airstack setup (NatNet SDK)") - - _stop_stale_natnet_nodes(container) - - host_ip = _docker_default_gateway(container) - command_port = ephemeral_udp_port(host_ip) - data_port = ephemeral_udp_port(host_ip) - while data_port == command_port: - data_port = ephemeral_udp_port(host_ip) - robot_name = _container_env(container, "ROBOT_NAME", "robot_1") - domain_id = int(_container_env(container, "ROS_DOMAIN_ID", "0")) - - stage = Usd.Stage.CreateInMemory() - xform = UsdGeom.Xform.Define(stage, "/World/base_link") - translate_op = xform.AddTranslateOp() - translate_op.Set(Gf.Vec3d(0.0, 0.0, 1.0)) - cfg = NatNetInterfaceConfig( - server_ip=host_ip, - command_port=command_port, - data_port=data_port, - publish_rate=50.0, - bodies=[BodyBinding("Drone", "/World/base_link", streaming_id=1)], - ) - author_interface(stage, "/World/NatNetInterface", cfg) - - manager = NatNetServerManager(server_factory=None) # real server factory - stop_event = threading.Event() - - def _sampler(): - # Stand in for the in-sim physics-step callback: move the prim and sample. - interval = 1.0 / cfg.publish_rate - t = 0.0 - while not stop_event.is_set(): - translate_op.Set(Gf.Vec3d(math.sin(t), 0.0, 1.0)) - manager.sample_once(stage) - t += interval - time.sleep(interval) - - sampler = threading.Thread(target=_sampler, daemon=True) - - node_proc: subprocess.Popen[str] | None = None - try: - assert manager.start_server(cfg) is True - sampler.start() - time.sleep(0.1) - node_proc = _launch_natnet_node(container, host_ip, command_port, domain_id) - _assert_pose_stream(container, robot_name, domain_id) - finally: - stop_event.set() - sampler.join(timeout=2.0) - _terminate(node_proc) - manager.stop_server() - - def test_natnet_ros2_multi_body_drone_and_target(robot_autonomy_stack): """Multi-body profile: one robot tracks a drone + a static target. From 4e19fba391b0d477a5775235e928ca174df7d85e Mon Sep 17 00:00:00 2001 From: John Date: Thu, 23 Jul 2026 14:15:42 -0400 Subject: [PATCH 3/3] chore: bump version to 0.19.0-alpha.10 Version-increment gate for the NatNet server PR (base optitrack-autonomy alpha.9). Co-Authored-By: Claude Opus 4.8 --- .env | 2 +- CHANGELOG.md | 1 + 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/.env b/.env index c9527e1f8..020228fa2 100644 --- a/.env +++ b/.env @@ -12,7 +12,7 @@ PROJECT_NAME="airstack" # If you've run ./airstack.sh setup, then this will auto-generate from the git commit hash every time a change is made # to a Dockerfile or docker-compose.yaml file. Otherwise this can also be set explicitly to make a release version. # auto-generated from git commit hash -VERSION="0.19.0-alpha.9" +VERSION="0.19.0-alpha.10" # Choose "dev" or "prebuilt". "dev" is for mounted code that must be built live. "prebuilt" is for built ros_ws baked into the image DOCKER_IMAGE_BUILD_MODE="dev" # Where to push and pull images from. Can replace with your docker hub username if using docker hub. diff --git a/CHANGELOG.md b/CHANGELOG.md index 34554edd1..ab1c1ef0d 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -15,6 +15,7 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0 - `overrides/l4t-px4-realrobot.env` — site-agnostic deployment override for a single real PX4 robot on a Jetson (aarch64/l4t) - `integration` test tier (`tests/integration/`, `integration` mark) with a shared `robot_autonomy_stack` fixture (robot container, no sim/GPU) - Real-robot PX4 external-vision fusion in `natnet_ros2` (OptiTrack mocap → EKF2): `mavros_gp_origin` (geoid-corrected synthetic GPS origin so `local_position.z` == OptiTrack z, fixing the ~36 m boot offset), `vision_pose_converter`, and a PX4 param **checker** (`px4_param_setter`, `auto_set` off by default; `on_mismatch` warn/halt) — setup guide at `docs/robot/px4_external_vision.md` +- NatNet server emulator (`optitrack.natnet.emulator`, protocol core) — pure-Python OptiTrack Motive server emulation so `natnet_ros2` can be driven without hardware; host integration tests (`tests/integration/natnet/`) wire it to the robot client ### Changed