diff --git a/.agents/skills/add-unit-tests/SKILL.md b/.agents/skills/add-unit-tests/SKILL.md index 7d1d3b582..2ab4234fe 100644 --- a/.agents/skills/add-unit-tests/SKILL.md +++ b/.agents/skills/add-unit-tests/SKILL.md @@ -114,7 +114,11 @@ For `rclpy.node.Node` subclasses use a real dummy base class instead of a ### 3. Write the thin proxy in tests/robot/ -Create `tests/robot///test_.py`: +Create `tests/robot///test_.py`. Use the shared +`reexport_unit_tests` + `repo_path` helpers from `tests/conftest.py` so the proxy +stays a two-call shim and the cross-tree path is anchored on `AIRSTACK_ROOT` +(exported by CI, defaults to the repo root locally) — **never** count +`Path(__file__).parents[N]` or hardcode `sys.path` walks in the proxy: ```python # Copyright (c) 2024 Carnegie Mellon University @@ -127,37 +131,27 @@ Unit test logic lives co-located with the package source (ROS 2 / colcon convent This file makes those tests discoverable by ``pytest tests/`` (CI) and ``airstack test -m unit`` without any changes to the CI workflow. """ -import importlib.util -import sys -from pathlib import Path - -_repo_root = Path(__file__).resolve().parents[N] # adjust N so this resolves to repo root -_pkg_test = _repo_root / "robot/ros_ws/src///test" -_real_file = _pkg_test / "test_.py" - -# If the test imports from a package module, ensure the package root is on sys.path. -# Example: _pkg_root = _pkg_test.parent; sys.path.insert(0, str(_pkg_root)) +from conftest import reexport_unit_tests, repo_path -# Load the real module under a unique name to avoid the circular import that -# would occur if we used `from test_ import *` (this file has the same -# name, and pytest adds its directory to sys.path at collection time). -_spec = importlib.util.spec_from_file_location("__unit_tests", _real_file) -_real = importlib.util.module_from_spec(_spec) -_spec.loader.exec_module(_real) - -# Re-export every test_* symbol so pytest collects them from this proxy. -for _name in dir(_real): - if _name.startswith("test_"): - globals()[_name] = getattr(_real, _name) +reexport_unit_tests( + globals(), + repo_path("robot/ros_ws/src///test"), + "test_.py", # pass several filenames to fold multiple modules into one proxy +) ``` -**Counting `parents[N]` to reach the repo root:** - -| Proxy location | `parents[N]` for repo root | -|---|---| -| `tests/robot///` | `parents[4]` | -| `tests/sim//` | `parents[3]` | -| `tests/gcs//` | `parents[3]` | +`reexport_unit_tests` (in `tests/conftest.py`) execs each co-located module with +`importlib` under a unique name (avoiding the same-filename circular import), puts +both the test dir and its parent (the package root) on `sys.path` so the source +can import its package and sibling helpers, and tags every re-exported `test_*` +with `pytest.mark.unit`. Because the root `conftest` is imported before any proxy +is collected, `from conftest import ...` resolves in both CI and local runs. + +For a **direct** `pytest /test/` (or `colcon test`) run — which bypasses +the proxies — add a tiny `conftest.py` in the package `test/` dir that puts the +package/extension root on `sys.path` (see +`simulation/.../optitrack.natnet.emulator/test/conftest.py`). The test source +then stays free of `sys.path` boilerplate. ### 4. Ensure the tests/ directory structure exists diff --git a/.agents/skills/optitrack-development/SKILL.md b/.agents/skills/optitrack-development/SKILL.md new file mode 100644 index 000000000..4120510c4 --- /dev/null +++ b/.agents/skills/optitrack-development/SKILL.md @@ -0,0 +1,213 @@ +--- +name: optitrack-development +description: Develop and integrate OptiTrack NatNet in AirStack — robot client (natnet_ros2), Isaac Sim Motive emulator, wire-protocol handshake, and libNatNet 4.4 unicast behavior. Use when working on natnet_ros2, optitrack.natnet.emulator, LAUNCH_NATNET, or NatNet UDP protocol compatibility. +license: Apache-2.0 +metadata: + author: AirLab CMU + repository: AirStack +--- + +# Skill: OptiTrack / NatNet Development + +## When to Use + +- Implementing or debugging the **Motive emulator** in Isaac Sim + (`simulation/isaac-sim/extensions/optitrack.natnet.emulator/`) +- Integrating or testing **`natnet_ros2`** on the robot stack +- Understanding **NatNet wire protocol** (connect, model def, frame streaming) +- Capturing what **`libNatNet.so`** actually sends on the network +- Enabling OptiTrack in sim: `LAUNCH_NATNET=true`, `natnet_config.yaml`, Docker IPs + +## Architecture in AirStack + +```mermaid +flowchart LR + subgraph sim ["Isaac Sim (172.31.0.200)"] + Emulator["optitrack.natnet.emulator\n(NatNet UDP server)"] + end + subgraph robot ["Robot container"] + Node["natnet_ros2_node"] + SDK["libNatNet.so client"] + Node --> SDK + end + SDK -->|"UDP 1510 (unicast: cmd + frames)"| Emulator + Node --> Topics["/{ROBOT_NAME}/perception/optitrack/..."] +``` + +| Component | Path | Role | +|-----------|------|------| +| Robot client | [`robot/ros_ws/src/perception/natnet_ros2/`](../../../robot/ros_ws/src/perception/natnet_ros2/) | ROS 2 node; uses **official NatNet SDK** (`NatNetClient::Connect`) | +| SDK install | `natnet_ros2/lib/libNatNet.so`, `include/natnet/` | Download via `airstack setup --natnet` (proprietary, not in git) | +| Emulator (WIP) | [`simulation/isaac-sim/extensions/optitrack.natnet.emulator/`](../../../simulation/isaac-sim/extensions/optitrack.natnet.emulator/) | Python NatNet **server** for sim / integration tests | +| Integration tests | [`tests/integration/natnet/README.md`](../../../tests/integration/natnet/README.md) | End-to-end UDP tests against real SDK parser (marks: `integration`, `natnet`) | + +**Enable on robot:** `LAUNCH_NATNET=true` in `.env` → [`perception.launch.xml`](../../../robot/ros_ws/src/perception/perception_bringup/launch/perception.launch.xml) includes `natnet_ros2.launch.py`. + +**Enable in sim:** the same `LAUNCH_NATNET=true` (passed into the isaac-sim container via [`docker-compose.yaml`](../../../simulation/isaac-sim/docker/docker-compose.yaml)) makes the Pegasus example launch scripts author a `/World/NatNetInterface` prim with one rigid body per drone `base_link` and **auto-start the emulator on load** (no UI). Helper: [`isaac/scene_setup.py`](../../../simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/scene_setup.py) (`start_drone_natnet_server`). Body name: single = `Drone`; multi = `Drone` (id `i`). Override the name prefix with `NATNET_BODY_NAME`. + +**Default client config:** unicast, `server_ip` → Motive/emulator (use `172.31.0.200` for Isaac container), ports 1510/1511 — see [`natnet_config.yaml`](../../../robot/ros_ws/src/perception/natnet_ros2/config/natnet_config.yaml). + +## NatNet: Two UDP Channels + +| Port (server default) | Channel | Direction | +|----------------------|---------|-----------| +| **1510** | Command | Client → server: `NAT_CONNECT`, `NAT_REQUEST_MODELDEF`, keepalives. Server → client: `NAT_SERVERINFO`, `NAT_MODELDEF`, `NAT_RESPONSE` | +| **1511** | Data | Server → client: `NAT_FRAMEOFDATA` (mocap frames). Multicast group `239.255.42.99` when using multicast. **The server must send frames from a socket bound to the data port** (source port == `data_port`); see below. | + +**Critical rules (verified against the real `libNatNet.so` 4.4 unicast + `NatNet_SetLogCallback`):** + +- Command **responses** go to the client's endpoint from `recvfrom` on the server command listener (`1510`), sent via the **command** socket. +- **Frames must be sent from the server's DATA socket** (bound to `data_port`, e.g. `1511`) so the datagram **source port == `data_port`**. libNatNet routes inbound unicast datagrams by source port: frames from the **command** port are treated as command traffic and **silently dropped** (no error, no callback). This was the single biggest gotcha. +- **libNatNet 4.4 unicast uses one client UDP socket** (one ephemeral local port for command send/recv and frame recv). The client receives frames there regardless of the server's source port — but libNatNet only **dispatches** them to the frame callback when they came from the server's data port. Do **not** assume `data_port = cmd_port + 1`. +- **Every `NAT_FRAMEOFDATA` must end with a 4-byte end-of-data tag** (after the frame `params`). libNatNet's unpacker reads it; without it the unpacked length mismatches `nDataBytes` and the SDK drops the whole frame. (The lenient Python `NatNetClient` does not require it — always validate against the C SDK.) +- The **269-byte `NAT_CONNECT` payload does not include** the client port; the port is learned from the datagram **source address** on `NAT_CONNECT`. +- Do **not** trust `/proc`/`ss` alone for the client port — extra bound sockets may appear that do not match wire traffic. **`NAT_CONNECT` source `(ip, port)` is ground truth.** +- Do **not** parse connect payloads with in-memory `sNatNetClientConnectParams` (contains pointers). Use on-wire layouts below. + +## libNatNet 4.4 `NAT_CONNECT` (verified 2025-06) + +Observed against `127.0.0.1:1510` with the same unicast params as [`natnet_client_adapter.cpp`](../../../robot/ros_ws/src/perception/natnet_ros2/src/natnet_client_adapter.cpp). + +### What the client sends + +| Field | Observed value | +|-------|----------------| +| Message | `NAT_CONNECT` (0), `nDataBytes = 269`, total datagram 273 bytes | +| Payload layout | `sSender` (264 B) + `sConnectionOptions` (5 B) | +| `sSender.szName` | `"NatNetLib"` | +| `sSender.Version` | `[4, 4, 0, 0]` | +| `sSender.NatNetVersion` | `[4, 4, 0, 0]` | +| `subscribedDataOnly` | `0` | +| `BitstreamVersion` | `[0, 0, 0, 0]` → client defers to server version | +| Trailing port bytes | **None** (exactly 269 bytes; not PacketClient's optional +4) | +| UDP source port | Ephemeral (e.g. `41449`) — **client command + data port (same socket)** | + +Example hex (payload only, after 4-byte header): + +``` +NatNetLib\0 ... (256-byte name field) +04 04 00 00 (Version) +04 04 00 00 (NatNetVersion) +00 (subscribedDataOnly) +00 00 00 00 (BitstreamVersion) +``` + +## libNatNet 4.4 unicast: single client socket (verified 2025-06) + +Confirmed with wire capture on server `:1510`/`:1511`, `strace` on a minimal `NatNetClient::Connect()` binary, and `/proc//net/udp` cross-checks against the same `libNatNet.so` used by `natnet_ros2`. + +### What we observed + +| Signal | Result | +|--------|--------| +| Wire capture on server `:1510` | All client packets (`NAT_CONNECT`, `NAT_KEEPALIVE`, `NAT_REQUEST_MODELDEF`) from **one** source port | +| Wire capture on server `:1511` | **No** inbound packets from the client | +| strace on minimal client | **One** `bind()`, **one** fd for all `sendto` → server `:1510` and `recvfrom` ← server `:1510` | +| `NAT_CONNECT` payload | **No** trailing client port bytes (269 B total) | + +### Emulator rule (unicast + `natnet_ros2`) + +For libNatNet 4.4 unicast, treat the client as **single-endpoint**: + +```text +On NAT_CONNECT → store client_endpoint = (ip, port) from recvfrom +NAT_SERVERINFO → sendto(command_socket, client_endpoint) # source port = command_port +NAT_MODELDEF → sendto(command_socket, client_endpoint) # source port = command_port +NAT_FRAMEOFDATA → sendto(data_socket, client_endpoint) # source port = data_port (REQUIRED) +NAT_KEEPALIVE → no reply (client -> server only) +``` + +The client always learns its endpoint from the **`NAT_CONNECT` source address** (the +client uses a single socket), so the **destination** of frames is that endpoint. The +**source** of frames, however, must be the server's data port — bind a dedicated +`data_socket` to `('', data_port)` and `sendto` frames from it. + +`ConnectionDataPort = 1511` in `NAT_SERVERINFO` is required (the SDK uses it to +recognize the data channel — i.e. which source port valid frames arrive from). + +### When two client ports may still apply + +- **Multicast** clients (separate multicast data listener on `239.255.42.99:1511`) +- **PacketClient-style** samples that open explicit command + data sockets (optional +4 port bytes in connect) +- Other NatNet client implementations — always verify with protocol capture before assuming a two-socket model + +Do **not** assume `data_port = cmd_port + 1` for any client without capture. + +### What the server must reply (for `Connect()` + `GetServerDescription()`) + +1. **`NAT_SERVERINFO` (1)** on the **command port** to the connect datagram source. +2. Payload: packed **`sSender_Server`** (279 B), **not** `sServerDescription`. libNatNet + parses the `NAT_SERVERINFO` payload as `sSender_Server`; sending the larger + `sServerDescription` makes it misread the version/host. Fields: + - `Common.szName = "Motive"` (256-byte field) + - `Common.Version = {3, 1, 0, 0}` (Motive app), `Common.NatNetVersion = {4, 4, 0, 0}` + - `HighResClockFrequency`, `DataPort = 1511`, `IsMulticast = 0` (unicast) + +Pre-built in emulator: [`NatNetServer._build_connect_response_payload()`](../../../simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_server.py). + +### After connect (required for `natnet_ros2` topics) + +| SDK call | Server must handle | +|----------|-------------------| +| `GetDataDescriptionList()` | `NAT_REQUEST_MODELDEF` → `NAT_MODELDEF` with rigid body name/ID (e.g. `"Drone"`) | +| Frame callback | Stream `NAT_FRAMEOFDATA` to **`NAT_CONNECT` source `(ip, port)`** from the server **data socket** (source port = `data_port`); end each frame with the 4-byte EOD tag; set `rb.params & 0x01` (tracking valid) | +| Unicast keepalive | Accept `NAT_KEEPALIVE` on command port; **send no reply** | + +Verified end-to-end against the real `libNatNet.so` with a C probe that registers +`SetFrameReceivedCallback` + `NatNet_SetLogCallback`: with the data-port source, +EOD tag, `sSender_Server` reply, and no keepalive reply, the probe reports +`Server: Motive 3.1.0.0 NatNet 4.4.0.0`, `data descriptions: 1`, and ~74 Hz callbacks. + +## Wire format reference (do not confuse) + +| Client type | Connect payload | +|-------------|-----------------| +| **`libNatNet` / `natnet_ros2`** | `sSender` + `sConnectionOptions` (269 B observed) | +| **PacketClient sample** | Same + optional 4 trailing bytes (often zero in sample) | +| **Python NatNetClient sample** | Legacy 270-byte `"Ping"` blob — **not** used by `natnet_ros2` | + +API struct `sNatNetClientConnectParams` ([`NatNetTypes.h`](../../../simulation/isaac-sim/extensions/optitrack.natnet.emulator/NatNetClientSDK/NatNetSDK/include/NatNetTypes.h)) is for `Connect()` in process memory only — **not** the on-wire layout. + +## Protocol capture (optional, for debugging) + +Not part of the repo. If you need to re-verify wire behavior or debug a new client/server pairing, build a **minimal out-of-band harness**: + +1. **Minimal C++ client** — tiny binary linking `libNatNet.so` from `natnet_ros2`; call `NatNetClient::Connect()` with the same params as [`natnet_client_adapter.cpp`](../../../robot/ros_ws/src/perception/natnet_ros2/src/natnet_client_adapter.cpp). Optional: `GetDataDescriptionList()`, frame callback, `--hold-seconds` sleep. +2. **Python UDP stub server** — bind `:1510` (and optionally `:1511`); reply to `NAT_CONNECT` with canned `NAT_SERVERINFO`, to `NAT_REQUEST_MODELDEF` with `NAT_MODELDEF`, to `NAT_KEEPALIVE` with ack; log every `(ip, port)` and message id. +3. **Connect capture** — run the client against the stub; hex-dump the first datagram; confirm 269-byte `sSender` + `sConnectionOptions` payload and ephemeral source port. +4. **Endpoint discovery** — during a full connect + model-def fetch: + - `tcpdump -i any udp and host ` or the stub's packet log + - `strace -e trace=bind,sendto,recvfrom` on the client binary + - `/proc//net/udp` or `ss -uapn` (treat **`NAT_CONNECT` source port** as ground truth if they disagree) +5. **Frame delivery check** — confirm the client's frame callback fires. Register both `SetFrameReceivedCallback` **and** `NatNet_SetLogCallback` (the log callback surfaces silent drops). Frames must be sent from the server **data socket** (source port = `data_port`) and end with the 4-byte EOD tag, or the SDK drops them with no callback. + +Use the SDK's `NatNetTypes.h` and `PacketClient.cpp` for on-wire layouts — not in-memory `sNatNetClientConnectParams`. + +## Emulator implementation checklist + +1. **Command listener** on `0.0.0.0:1510` +2. **`NAT_CONNECT`** → register `client_endpoint` from `recvfrom`; reply `NAT_SERVERINFO` +3. **`NAT_REQUEST_MODELDEF`** → reply `NAT_MODELDEF` (match `body_name` in config) +4. **Frame loop** → `NAT_FRAMEOFDATA` to `client_endpoint` **from the data socket** (source port = `data_port`); end each frame with the 4-byte EOD tag +5. **Isaac integration** → sample drone pose → `sFrameOfMocapData` → `enqueue_mocap_data()` +6. **Docker** → emulator on `172.31.0.200`; robot `server_ip` points there + +## Testing levels + +| Level | Approach | Validates | +|-------|----------|-----------| +| Unit (no network) | `test_natnet_logic.cpp`, `FakeNatNetClient` | Negotiation logic, topic names | +| Protocol capture | Minimal client + UDP stub (see above) | Wire-format `NAT_CONNECT`, client endpoint model | +| Integration | `tests/integration/natnet/` | Full SDK parser + `natnet_ros2_node` (marks: `integration`, `natnet`) | +| System (future) | `airstack test -m sensors` | Topic Hz on `/perception/optitrack/...` | + +```bash +# Unit tests (robot container) +docker exec airstack-robot-desktop-1 bash -c "sws && colcon test --packages-select natnet_ros2 --event-handlers console_direct+" +``` + +## References + +- OptiTrack NatNet docs: https://docs.optitrack.com/developer-tools/natnet-sdk/natnet-4.0 +- SDK samples (wire format): `NatNet_SDK_*/Samples/PacketClient/`, `PythonClient/` (legacy connect in Python only) +- Integration test: [`tests/integration/natnet/README.md`](../../../tests/integration/natnet/README.md) diff --git a/.env b/.env index 82cc01ccb..2633b1f8e 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.3" +VERSION="0.19.0-alpha.4" # 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. @@ -51,6 +51,11 @@ URDF_FILE="robot_descriptions/iris/urdf/iris_with_sensors.pegasus.robot.urdf" DEBUG_RVIZ="false" # "true" or "false". If true, launches RViz alongside the robot via desktop_bringup/robot.launch.xml. +LAUNCH_NATNET="false" +# PX4 SITL param profile for isaac-sim (simulation/isaac-sim/docker/px4-profiles/). +# Use "vision" with LAUNCH_NATNET + MAVROS vision_pose; "default" for plain Pegasus SITL. +PX4_PARAM_PROFILE="default" + # offboard API streaming out. this is so that ports don't conflict for multi-agent FCU communication. OFFBOARD_BASE_PORT=14540 ONBOARD_BASE_PORT=14580 diff --git a/AGENTS.md b/AGENTS.md index b53069e75..fa00915a4 100644 --- a/AGENTS.md +++ b/AGENTS.md @@ -89,6 +89,7 @@ For detailed step-by-step instructions, refer to the **`.agents/skills/`** direc | [write-launch-file](.agents/skills/write-launch-file) | Authoring ROS 2 launch files with AirStack conventions (ROBOT_NAME namespacing, topic remapping, allow_substs) | | [write-isaac-sim-scene](.agents/skills/write-isaac-sim-scene) | Creating custom simulation scenes | | [visualize-in-foxglove](.agents/skills/visualize-in-foxglove) | Adding topic visualization to Foxglove/GCS | +| [optitrack-development](.agents/skills/optitrack-development) | OptiTrack NatNet emulator, natnet_ros2, wire-protocol handshake, connect-packet sniff | | [attach-gossip-payload](.agents/skills/attach-gossip-payload) | Broadcasting custom ROS messages to peers via PeerProfile gossip payloads | | [debug-module](.agents/skills/debug-module) | Autonomous debugging of ROS 2 modules | | [update-documentation](.agents/skills/update-documentation) | Documenting new modules and updating mkdocs | @@ -196,7 +197,7 @@ docker exec airstack-robot-desktop-1 bash -c "ros2 topic echo --onc - Verify module behavior in isolation - Test with synthetic data - Located in module's `test/` directory - - **Run in the robot container** with `colcon test` (after `bws`), not via `airstack test -m unit`. The root [`tests/`](tests/) suite does **not** register a `unit` pytest mark; `airstack test -m ` only selects marks declared in [`tests/pytest.ini`](tests/pytest.ini) (`build_docker`, `build_packages`, `liveliness`, `sensors`, `takeoff_hover_land`). + - **Run in the robot container** with `colcon test` (after `bws`), not via `airstack test -m unit`. The root [`tests/`](tests/) suite does **not** register a `unit` pytest mark; `airstack test -m ` only selects marks declared in [`tests/pytest.ini`](tests/pytest.ini) (`unit`, `build_docker`, `build_packages`, `liveliness`, `sensors`, `takeoff_hover_land`, `integration`, `natnet`). ```bash docker exec airstack-robot-desktop-1 bash -c "sws && colcon test --packages-select natnet_ros2 --event-handlers console_direct+" @@ -204,7 +205,9 @@ docker exec airstack-robot-desktop-1 bash -c "ros2 topic echo --onc 2. **Unit tests (`pytest`, `unit` mark):** Fast, hermetic checks. Test **source** lives co-located with each ROS 2 package in `/test/` (standard colcon convention). Thin **proxy** files in [`tests/robot/`](tests/robot/) and [`tests/sim/`](tests/sim/) re-export those tests so `pytest tests/` discovers them. Unit tests run as part of the `system-tests.yml` suite. Example: `airstack test -m unit -v`. See `add-unit-tests` skill. -3. **System Level (`tests/system/`):** Full simulation tests (Isaac Sim or Microsoft AirSim legacy) +3. **Integration (`pytest`, `integration` mark, [`tests/integration/`](tests/integration/)):** Wire a few real components together — the robot autonomy container plus a host-side component — **without** a sim or GPU. The shared `robot_autonomy_stack` fixture reuses a running `robot-desktop` container or brings one up only when `--run-integration` is passed (else skips), so a plain `pytest tests/` never spins up Docker. First resident: `tests/integration/natnet/` (host NatNet emulator → `natnet_ros2` pose Hz; marks `integration`, `natnet`). Example: `airstack test -m integration --run-integration -v`. + +4. **System Level (`tests/system/`):** Full simulation tests (Isaac Sim or Microsoft AirSim legacy) - End-to-end autonomy stack testing - Real sensor simulation - Multi-robot scenarios diff --git a/docs/development/intermediate/testing/unit_testing.md b/docs/development/intermediate/testing/unit_testing.md index f69056008..2b97c787d 100644 --- a/docs/development/intermediate/testing/unit_testing.md +++ b/docs/development/intermediate/testing/unit_testing.md @@ -184,8 +184,8 @@ The `build_packages` CI job (`tests/system/test_build_packages.py`) also runs ## Extending to sim and GCS The proxy pattern extends to other components. As sim-side Python logic (e.g. the -[Motive emulator](../../../../tests/sim/motive_emulator/README.md)) and GCS modules -acquire unit-testable code, follow the same layout: +[NatNet emulator](../../../../simulation/isaac-sim/extensions/optitrack.natnet.emulator/README.md)) +and GCS modules acquire unit-testable code, follow the same layout: ``` simulation/...//test/test_.py ← source diff --git a/docs/simulation/isaac_sim/pegasus_scene_setup.md b/docs/simulation/isaac_sim/pegasus_scene_setup.md index da4293dc7..72b3bdc97 100644 --- a/docs/simulation/isaac_sim/pegasus_scene_setup.md +++ b/docs/simulation/isaac_sim/pegasus_scene_setup.md @@ -165,6 +165,22 @@ PX4_RENDERING_HZ="30" - **`PX4_PHYSICS_HZ`** — Sets `physics_dt = 1 / PX4_PHYSICS_HZ` in Isaac Sim's physics scene, and automatically syncs PX4's `IMU_INTEG_RATE` parameter to the same value via `PX4LaunchTool` → `px4-rc.simulator`. Patched within the Docker image to read the environment variable and set the IMU_INTEG_RATE parameter. - **`PX4_RENDERING_HZ`** — Sets the rendering frame rate independently of physics. 30 Hz rendering has no effect on physics accuracy or PX4 behavior, but does slightly affect performance due to resource usage. +### PX4 parameter profiles (external vision) + +Isaac Sim loads a layered PX4 SITL parameter profile from `simulation/isaac-sim/docker/px4-profiles/`. Select the profile with `PX4_PARAM_PROFILE` in the top-level `.env`: + +```bash +# Plain Pegasus SITL +PX4_PARAM_PROFILE="default" + +# NatNet / mocap → MAVROS vision_pose → EKF2 external vision +PX4_PARAM_PROFILE="vision" +``` + +Profiles inject `PX4_PARAM_*` variables into the isaac-sim container; PX4 applies them at boot via `init.d-posix/rcS`. Pair `vision` with robot-side `LAUNCH_NATNET=true` and `publish_to_mavros: true` in `natnet_config.yaml`. + +Convenience bundle: `airstack up --env-file overrides/isaac-natnet-vision.env`. + ### Valid values PX4's documented presets for `IMU_INTEG_RATE` are **100, 200, 250, 400 Hz**. The minimum is **100 Hz** — the EKF2 estimator runs at 100 Hz (10 ms period) and `IMU_INTEG_RATE` must be at least this fast. Values below 100 Hz are accepted by the firmware but cause attitude controller oscillation and are not recommended. diff --git a/overrides/isaac-natnet-vision.env b/overrides/isaac-natnet-vision.env new file mode 100644 index 000000000..2cebb6468 --- /dev/null +++ b/overrides/isaac-natnet-vision.env @@ -0,0 +1,7 @@ +# Isaac Sim + NatNet mocap with PX4 external-vision fusion. +# Usage: airstack up --env-file overrides/isaac-natnet-vision.env + +LAUNCH_NATNET=true +PX4_PARAM_PROFILE=vision +ISAAC_SIM_USE_STANDALONE=true +ISAAC_SIM_SCRIPT_NAME=example_multi_px4_pegasus_launch_script.py diff --git a/robot/docker/Dockerfile.robot b/robot/docker/Dockerfile.robot index 894874961..f6e48b150 100644 --- a/robot/docker/Dockerfile.robot +++ b/robot/docker/Dockerfile.robot @@ -96,6 +96,7 @@ RUN python3 -m pip install --no-cache-dir --break-system-packages --ignore-insta RUN apt update -y && apt install -y --no-install-recommends \ ros-dev-tools \ ros-${ROS_DISTRO}-mavros \ + ros-${ROS_DISTRO}-mavros-extras \ ros-${ROS_DISTRO}-tf2* \ ros-${ROS_DISTRO}-stereo-image-proc \ ros-${ROS_DISTRO}-image-view \ @@ -328,6 +329,7 @@ RUN python3 -m pip install --no-cache-dir --break-system-packages --ignore-insta RUN apt update -y && apt install -y --no-install-recommends \ ros-dev-tools \ ros-${ROS_DISTRO}-mavros \ + ros-${ROS_DISTRO}-mavros-extras \ ros-${ROS_DISTRO}-tf2* \ ros-${ROS_DISTRO}-stereo-image-proc \ ros-${ROS_DISTRO}-image-view \ diff --git a/robot/ros_ws/src/perception/natnet_ros2/CMakeLists.txt b/robot/ros_ws/src/perception/natnet_ros2/CMakeLists.txt index ff47a00da..86342cb7c 100644 --- a/robot/ros_ws/src/perception/natnet_ros2/CMakeLists.txt +++ b/robot/ros_ws/src/perception/natnet_ros2/CMakeLists.txt @@ -48,8 +48,7 @@ if(EXISTS "${_NATNET_LIB}" AND EXISTS "${_NATNET_INC}") # Install libNatNet.so alongside the node and register an environment hook so # that sourcing the workspace adds lib/natnet_ros2/ to LD_LIBRARY_PATH. - # Use PROGRAMS (not FILES) to preserve the execute bit — shared libraries - # must be executable for the dynamic linker to map them. + # Use PROGRAMS (not FILES) to preserve the execute bit install(PROGRAMS "${_NATNET_LIB}" DESTINATION lib/${PROJECT_NAME}) @@ -65,10 +64,11 @@ else() endif() # --------------------------------------------------------------------------- -# Python nodes (vision_pose_converter remains Python) +# Python nodes # --------------------------------------------------------------------------- install(PROGRAMS src/vision_pose_converter_node.py + src/mavros_gp_origin_node.py DESTINATION lib/${PROJECT_NAME}) # --------------------------------------------------------------------------- diff --git a/robot/ros_ws/src/perception/natnet_ros2/README.md b/robot/ros_ws/src/perception/natnet_ros2/README.md index eb44763b4..2a36a6280 100644 --- a/robot/ros_ws/src/perception/natnet_ros2/README.md +++ b/robot/ros_ws/src/perception/natnet_ros2/README.md @@ -29,9 +29,11 @@ NatNet ROS 2 Node ├→ /robot_1/perception/optitrack/{body_name} (PoseStamped, optional) ├→ /robot_1/perception/optitrack/{body_name}/pose_cov (PoseWithCovarianceStamped, always) └→ (Optional, publish_to_mavros: true) + mavros_gp_origin_node + └→ /robot_1/interface/mavros/global_position/set_gp_origin vision_pose_converter_node - ├→ /robot_1/mavros/vision_pose/pose - └→ /robot_1/mavros/vision_pose/pose_cov + ├→ /robot_1/interface/mavros/vision_pose/pose + └→ /robot_1/interface/mavros/vision_pose/pose_cov ``` ## Interfaces @@ -62,9 +64,23 @@ For each tracked rigid body `{body_name}` from Motive: When `publish_to_mavros: true`, `vision_pose_converter_node` subscribes to `pose_cov` and republishes for PX4: -- **Topic**: `/{ROBOT_NAME}/mavros/vision_pose/pose` — `geometry_msgs/PoseStamped` (pose extracted from the covariance message) -- **Topic**: `/{ROBOT_NAME}/mavros/vision_pose/pose_cov` — `geometry_msgs/PoseWithCovarianceStamped` (full message, quaternion optionally canonicalized) +- **Topic**: `/{ROBOT_NAME}/interface/mavros/vision_pose/pose` — `geometry_msgs/PoseStamped` (pose extracted from the covariance message) +- **Topic**: `/{ROBOT_NAME}/interface/mavros/vision_pose/pose_cov` — `geometry_msgs/PoseWithCovarianceStamped` (full message, quaternion optionally canonicalized) - **Enabled by**: `publish_to_mavros: true` in config +- **PX4 side**: set `PX4_PARAM_PROFILE=vision` in `.env` so Isaac SITL loads EKF2 external-vision params from `simulation/isaac-sim/docker/px4-profiles/vision.env` + +##### Synthetic GPS origin (mocap / no-GNSS arming) + +With GNSS disabled (`EKF2_GPS_CTRL=0`), PX4 fused EKF has **no global position**. This fails preflight checks and refuse to arm. When `publish_to_mavros: true`, +`mavros_gp_origin_node` publishes a synthetic origin once at startup: + +- **Topic**: `/{ROBOT_NAME}/interface/mavros/global_position/set_gp_origin` — `geographic_msgs/GeoPointStamped` +- **Guarded**: waits for `mavros/state.connected`, then publishes only if no + origin already exists (it watches `…/global_position/gp_origin`), so a + GNSS-equipped vehicle is left untouched. +- **Params** (`config/mavros_gp_origin.yaml`): `enabled` (default `true`), + `latitude/longitude/altitude` (default PX4 SITL home, Zurich), `settle_sec`. + Set `enabled: false` to rely on real GNSS. ## Configuration @@ -147,6 +163,10 @@ Each container instance gets its own `ROBOT_NAME` and `ROS_DOMAIN_ID`: - Invalid/malformed packets are skipped with debug logging - Lost connectivity logs warnings; gracefully recovers when stream resumes - Covariance in config allows tuning uncertainty per deployment +- **Connect retry:** the initial handshake is retried every 2 s until it + succeeds, so the node tolerates the NatNet server starting *after* the robot + (e.g. a Motive PC powered on later, or the Isaac Sim NatNet emulator which only + binds ~100 s into sim boot). The retry timer cancels itself on first success. ## Testing diff --git a/robot/ros_ws/src/perception/natnet_ros2/config/mavros_gp_origin.yaml b/robot/ros_ws/src/perception/natnet_ros2/config/mavros_gp_origin.yaml new file mode 100644 index 000000000..b5edc3f70 --- /dev/null +++ b/robot/ros_ws/src/perception/natnet_ros2/config/mavros_gp_origin.yaml @@ -0,0 +1,18 @@ +# Synthetic GPS origin for mocap / no-GNSS flight via MAVROS. +# Loaded by mavros_gp_origin.launch.xml when publish_to_mavros is enabled. + +/**: + ros__parameters: + # With GNSS disabled, PX4 has no global position, so modes that require one + # (e.g. AUTO.LOITER) refuse to arm. Setting an origin lets PX4 derive a + # global position from the fused vision estimate. Guarded: skipped if an + # origin already exists (e.g. on a GNSS-equipped vehicle). + enabled: true + # Default matches PX4 SITL's default home (Zurich); shared by all robots so + # they share one global datum. + latitude: 47.397742 + longitude: 8.545594 + altitude: 488.0 + # Wait this long after MAVROS connects (listening for an existing origin) + # before publishing the synthetic one. + settle_sec: 5.0 diff --git a/robot/ros_ws/src/perception/natnet_ros2/config/natnet_config.yaml b/robot/ros_ws/src/perception/natnet_ros2/config/natnet_config.yaml index 69fc11d1c..1e0ad6916 100644 --- a/robot/ros_ws/src/perception/natnet_ros2/config/natnet_config.yaml +++ b/robot/ros_ws/src/perception/natnet_ros2/config/natnet_config.yaml @@ -7,8 +7,8 @@ /**: ros__parameters: # IP address of the PC running Motive (OptiTrack server). - # Change this to match your local network before launching NatNet. - server_ip: "192.168.1.100" + # Defaults to the Isaac Sim container on the airstack_network (172.31.0.200), + server_ip: "$(env NATNET_SERVER_IP 172.31.0.200)" # Motive learns unicast destination from outbound UDP source IP — bind explicitly when you have # multiple NICs (e.g. Docker 172.17.* vs LAN). client_ip: "0.0.0.0" @@ -41,11 +41,11 @@ debug: false position_covariance: - [0.1, 0.0, 0.0, - 0.0, 0.1, 0.0, - 0.0, 0.0, 0.1] + [1.0e-6, 0.0, 0.0, # Covariances for sub 0.1 mm precision from optitrack + 0.0, 1.0e-6, 0.0, + 0.0, 0.0, 1.0e-6] orientation_covariance: - [0.01, 0.0, 0.0, - 0.0, 0.01, 0.0, - 0.0, 0.0, 0.01] + [3.0e-6, 0.0, 0.0, # Covariances for sub 0.1 degree precision from optitrack + 0.0, 3.0e-6, 0.0, + 0.0, 0.0, 3.0e-6] diff --git a/robot/ros_ws/src/perception/natnet_ros2/launch/mavros_gp_origin.launch.xml b/robot/ros_ws/src/perception/natnet_ros2/launch/mavros_gp_origin.launch.xml new file mode 100644 index 000000000..4cb1f785b --- /dev/null +++ b/robot/ros_ws/src/perception/natnet_ros2/launch/mavros_gp_origin.launch.xml @@ -0,0 +1,34 @@ + + + + + + + + + + + + + + + + + + + diff --git a/robot/ros_ws/src/perception/natnet_ros2/launch/natnet_ros2.launch.py b/robot/ros_ws/src/perception/natnet_ros2/launch/natnet_ros2.launch.py index cb7c178d8..98a8b237c 100644 --- a/robot/ros_ws/src/perception/natnet_ros2/launch/natnet_ros2.launch.py +++ b/robot/ros_ws/src/perception/natnet_ros2/launch/natnet_ros2.launch.py @@ -43,14 +43,17 @@ def generate_launch_description() -> LaunchDescription: pkg_share = get_package_share_directory('natnet_ros2') default_natnet_yaml = os.path.join(pkg_share, 'config', 'natnet_config.yaml') default_vp_yaml = os.path.join(pkg_share, 'config', 'vision_pose_converter.yaml') + default_gp_origin_yaml = os.path.join(pkg_share, 'config', 'mavros_gp_origin.yaml') config_file = LaunchConfiguration('config_file') vision_pose_config_file = LaunchConfiguration('vision_pose_config_file') + gp_origin_config_file = LaunchConfiguration('gp_origin_config_file') use_sim_time = LaunchConfiguration('use_sim_time') def launch_setup(context, *_args, **_kwargs): cfg_path = config_file.perform(context) vp_path = vision_pose_config_file.perform(context) + gp_path = gp_origin_config_file.perform(context) ust = use_sim_time.perform(context) ros_params = _ros_params_from_file(cfg_path) @@ -80,6 +83,17 @@ def launch_setup(context, *_args, **_kwargs): ] if publish_mavros: + actions.append( + IncludeLaunchDescription( + FrontendLaunchDescriptionSource( + os.path.join(pkg_share, 'launch', 'mavros_gp_origin.launch.xml'), + ), + launch_arguments=[ + ('config_file', gp_path), + ('use_sim_time', ust), + ], + ), + ) actions.append( IncludeLaunchDescription( FrontendLaunchDescriptionSource( @@ -107,10 +121,15 @@ def launch_setup(context, *_args, **_kwargs): default_value=default_vp_yaml, description='vision_pose_converter parameter YAML.', ), + DeclareLaunchArgument( + 'gp_origin_config_file', + default_value=default_gp_origin_yaml, + description='mavros_gp_origin parameter YAML.', + ), DeclareLaunchArgument( 'use_sim_time', default_value='false', - description='Forwarded to vision_pose_converter.launch.xml.', + description='Forwarded to MAVROS bridge launch files.', ), OpaqueFunction(function=launch_setup), ], diff --git a/robot/ros_ws/src/perception/natnet_ros2/launch/vision_pose_converter.launch.xml b/robot/ros_ws/src/perception/natnet_ros2/launch/vision_pose_converter.launch.xml index aad9c4474..209b22059 100644 --- a/robot/ros_ws/src/perception/natnet_ros2/launch/vision_pose_converter.launch.xml +++ b/robot/ros_ws/src/perception/natnet_ros2/launch/vision_pose_converter.launch.xml @@ -13,8 +13,8 @@ /{ROBOT_NAME}/perception/optitrack/{body_name}/pose_cov Output (consumed by MAVROS): - /{ROBOT_NAME}/mavros/vision_pose/pose - /{ROBOT_NAME}/mavros/vision_pose/pose_cov + /{ROBOT_NAME}/interface/mavros/vision_pose/pose + /{ROBOT_NAME}/interface/mavros/vision_pose/pose_cov --> + to="/$(env ROBOT_NAME robot_1)/interface/mavros/vision_pose/pose"/> + to="/$(env ROBOT_NAME robot_1)/interface/mavros/vision_pose/pose_cov"/> diff --git a/robot/ros_ws/src/perception/natnet_ros2/package.xml b/robot/ros_ws/src/perception/natnet_ros2/package.xml index f9632b0f7..621469145 100644 --- a/robot/ros_ws/src/perception/natnet_ros2/package.xml +++ b/robot/ros_ws/src/perception/natnet_ros2/package.xml @@ -26,6 +26,7 @@ mavros_msgs + geographic_msgs ament_index_python launch diff --git a/robot/ros_ws/src/perception/natnet_ros2/src/mavros_gp_origin_node.py b/robot/ros_ws/src/perception/natnet_ros2/src/mavros_gp_origin_node.py new file mode 100755 index 000000000..d9b1c3754 --- /dev/null +++ b/robot/ros_ws/src/perception/natnet_ros2/src/mavros_gp_origin_node.py @@ -0,0 +1,123 @@ +#!/usr/bin/env python3 + +""" +MAVROS GPS Origin Node + +Publishes a synthetic GPS origin to MAVROS once at startup for mocap / no-GNSS +flight. With GNSS disabled, PX4 fuses vision into a valid local position but +has no global position, so modes that require one (e.g. AUTO.LOITER) refuse to +arm. Setting an origin lets PX4 derive global position from the fused estimate. + +The publish is guarded: it waits for MAVROS to connect, watches for an existing +origin, and only publishes if none is present — GNSS-equipped vehicles are left +untouched. +""" + +import rclpy +from rclpy.node import Node +from geographic_msgs.msg import GeoPointStamped +from mavros_msgs.msg import State + + +class MavrosGpOriginNode(Node): + """One-shot synthetic GPS origin publisher for MAVROS / PX4.""" + + def __init__(self): + super().__init__('mavros_gp_origin') + + self.declare_parameter('enabled', True) + self.declare_parameter('latitude', 47.397742) + self.declare_parameter('longitude', 8.545594) + self.declare_parameter('altitude', 488.0) + # Seconds to wait after MAVROS connects (listening for an existing + # origin) before publishing our synthetic one. + self.declare_parameter('settle_sec', 5.0) + + self._enabled = self.get_parameter('enabled').value + if not self._enabled: + self.get_logger().info('Synthetic GPS origin disabled (enabled=false).') + return + + self._lat = self.get_parameter('latitude').value + self._lon = self.get_parameter('longitude').value + self._alt = self.get_parameter('altitude').value + self._settle_sec = self.get_parameter('settle_sec').value + + self._done = False + self._origin_exists = False + self._connected_since = None + self._publish_count = 0 + + self._set_origin_pub = self.create_publisher( + GeoPointStamped, 'set_gps_origin', 10 + ) + self._origin_sub = self.create_subscription( + GeoPointStamped, 'current_gps_origin', self._on_existing_origin, 10 + ) + self._state_sub = self.create_subscription( + State, 'mavros_state', self._on_mavros_state, 10 + ) + self._timer = self.create_timer(1.0, self._tick) + + self.get_logger().info( + f'MAVROS GPS origin node started ' + f'(lat={self._lat}, lon={self._lon}, alt={self._alt}, ' + f'settle_sec={self._settle_sec})' + ) + + def _on_existing_origin(self, _msg: GeoPointStamped): + """An origin already exists (e.g. from GNSS) — never override it.""" + if not self._origin_exists and not self._done: + self.get_logger().info( + 'Existing GPS origin detected; skipping synthetic origin.' + ) + self._origin_exists = True + + def _on_mavros_state(self, msg: State): + if msg.connected and self._connected_since is None: + self._connected_since = self.get_clock().now() + + def _tick(self): + if self._done: + return + if self._origin_exists: + self._done = True + self._timer.cancel() + return + if self._connected_since is None: + return + elapsed = (self.get_clock().now() - self._connected_since).nanoseconds * 1e-9 + if elapsed < self._settle_sec: + return + + msg = GeoPointStamped() + msg.header.stamp = self.get_clock().now().to_msg() + msg.position.latitude = self._lat + msg.position.longitude = self._lon + msg.position.altitude = self._alt + self._set_origin_pub.publish(msg) + self._publish_count += 1 + self.get_logger().info( + f'Published synthetic GPS origin ' + f'(lat={self._lat}, lon={self._lon}, alt={self._alt}) ' + f'[{self._publish_count}/3]' + ) + # Publish a few times in case MAVROS subscribed late, then stop. + if self._publish_count >= 3: + self._done = True + self._timer.cancel() + + +def main(args=None): + rclpy.init(args=args) + try: + node = MavrosGpOriginNode() + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/robot/ros_ws/src/perception/natnet_ros2/src/natnet_ros2_node.cpp b/robot/ros_ws/src/perception/natnet_ros2/src/natnet_ros2_node.cpp index 65059f659..c93890e0d 100644 --- a/robot/ros_ws/src/perception/natnet_ros2/src/natnet_ros2_node.cpp +++ b/robot/ros_ws/src/perception/natnet_ros2/src/natnet_ros2_node.cpp @@ -117,7 +117,14 @@ class NatNetROS2Node : public rclcpp::Node // Production client — NatNetClientAdapter wraps the SDK client_ = std::make_unique(); - connect_and_setup(connect_cfg); + connect_cfg_ = connect_cfg; + + // Try to connect now; keep retrying. + if (!connect_and_setup(connect_cfg_)) { + connect_timer_ = this->create_wall_timer( + std::chrono::seconds(2), + std::bind(&NatNetROS2Node::retry_connect, this)); + } refresh_timer_ = this->create_wall_timer( std::chrono::seconds(1), @@ -200,14 +207,15 @@ class NatNetROS2Node : public rclcpp::Node private: // ----------------------------------------------------------------------- - void connect_and_setup(const natnet_ros2::ConnectConfig & cfg) + // Returns true once the handshake succeeds. + bool connect_and_setup(const natnet_ros2::ConnectConfig & cfg) { const natnet_ros2::NegotiationResult neg = natnet_ros2::negotiate(*client_, cfg); if (!neg.ok) { - RCLCPP_ERROR(get_logger(), "%s", neg.log_message.c_str()); - return; + RCLCPP_WARN(get_logger(), "%s", neg.log_message.c_str()); + return false; } if (neg.server_info.host_present) { @@ -221,6 +229,24 @@ class NatNetROS2Node : public rclcpp::Node client_->set_frame_callback( [this](const natnet_ros2::FrameSample & f) { on_frame(f); }); RCLCPP_INFO(get_logger(), "Frame callback registered — receiving mocap data."); + connected_ = true; + return true; + } + + // ----------------------------------------------------------------------- + // Timer-driven reconnect. + void retry_connect() + { + if (connected_) { + if (connect_timer_) { connect_timer_->cancel(); } + return; + } + RCLCPP_INFO(get_logger(), + "NatNet not connected — retrying handshake to %s ...", + connect_cfg_.server_ip.c_str()); + if (connect_and_setup(connect_cfg_) && connect_timer_) { + connect_timer_->cancel(); + } } // ----------------------------------------------------------------------- @@ -304,6 +330,8 @@ class NatNetROS2Node : public rclcpp::Node std::array covariance_6x6_{}; std::unique_ptr client_; + natnet_ros2::ConnectConfig connect_cfg_; + bool connected_ = false; std::mutex pub_mutex_; std::unordered_map body_names_; @@ -311,6 +339,7 @@ class NatNetROS2Node : public rclcpp::Node std::atomic needs_description_refresh_{false}; rclcpp::TimerBase::SharedPtr refresh_timer_; + rclcpp::TimerBase::SharedPtr connect_timer_; }; diff --git a/simulation/isaac-sim/docker/Dockerfile.isaac-ros b/simulation/isaac-sim/docker/Dockerfile.isaac-ros index 0dca11fb7..c563bf2c8 100644 --- a/simulation/isaac-sim/docker/Dockerfile.isaac-ros +++ b/simulation/isaac-sim/docker/Dockerfile.isaac-ros @@ -145,6 +145,10 @@ ENV ACCEPT_EULA="Y" # ENV ISAACSIM_PYTHON=/isaac-sim/python.sh RUN /isaac-sim/python.sh -m pip install --no-cache-dir -e /isaac-sim/.local/share/ov/data/documents/Kit/shared/exts/pegasus.simulator +# OptiTrack NatNet emulator extension (in-repo; compose mount overrides at runtime) +COPY extensions/optitrack.natnet.emulator /isaac-sim/.local/share/ov/data/documents/Kit/shared/exts/optitrack.natnet.emulator +RUN /isaac-sim/python.sh -m pip install --no-cache-dir -e /isaac-sim/.local/share/ov/data/documents/Kit/shared/exts/optitrack.natnet.emulator + # Install PX4 things RUN git clone --branch ${PX4_VERSION} --recursive https://github.com/PX4/PX4-Autopilot.git diff --git a/simulation/isaac-sim/docker/docker-compose.yaml b/simulation/isaac-sim/docker/docker-compose.yaml index dfd699aa4..4ba91f58a 100644 --- a/simulation/isaac-sim/docker/docker-compose.yaml +++ b/simulation/isaac-sim/docker/docker-compose.yaml @@ -33,6 +33,7 @@ services: ipv4_address: 172.31.0.200 # required to not conflict with other default docker networks on the host machine env_file: - ./omni_pass.env + - ./px4-profiles/${PX4_PARAM_PROFILE:-default}.env environment: - AUTOLAUNCH=${AUTOLAUNCH:-'false'} - DISPLAY=${DISPLAY} @@ -42,6 +43,11 @@ services: - NUM_ROBOTS=${NUM_ROBOTS:-1} - ENABLE_LIDAR=${ENABLE_LIDAR:-false} - ISAAC_SIM_HEADLESS=${ISAAC_SIM_HEADLESS:-false} + # OptiTrack/NatNet emulator: mirror the robot-side LAUNCH_NATNET gate so the + # example launch scripts author a NatNet interface (one rigid body per drone + # base_link) and start the server on load when enabled. + - LAUNCH_NATNET=${LAUNCH_NATNET:-false} + - NATNET_BODY_NAME=${NATNET_BODY_NAME:-Drone} # Pegasus physics tuning — read by pegasus/simulator/params.py - PX4_PHYSICS_HZ=${PX4_PHYSICS_HZ:-100} - ARDUPILOT_PHYSICS_HZ=${ARDUPILOT_PHYSICS_HZ:-800} @@ -68,9 +74,11 @@ services: - $HOME/docker/isaac-sim/pkg:/isaac-sim/.local/share/ov/pkg:rw \ # pegasus integration - ../extensions/PegasusSimulator/extensions/pegasus.simulator:/isaac-sim/.local/share/ov/data/documents/Kit/shared/exts/pegasus.simulator/:rw + # optitrack natnet emulator + - ../extensions/optitrack.natnet.emulator:/isaac-sim/.local/share/ov/data/documents/Kit/shared/exts/optitrack.natnet.emulator/:rw # omniverse - ./omniverse.toml:/isaac-sim/.nvidia-omniverse/config/omniverse.toml:rw - - ./user.config.json:/isaac-sim/.local/share/ov/data/Kit/Isaac-Sim Full/5.1/user.config.json:rw # enables pegasus extension; IMPORTANT: set the version number without the trailing .0 + - ./user.config.json:/isaac-sim/.local/share/ov/data/Kit/Isaac-Sim Full/5.1/user.config.json:rw # enables pegasus + optitrack extensions; IMPORTANT: set the version number without the trailing .0 # developer stuff - .dev:/isaac-sim/.dev:rw # developer config - .bashrc:/isaac-sim/.bashrc:rw # bash config @@ -143,6 +151,7 @@ services: - $HOME/docker/isaac-sim/data:/isaac-sim/.local/share/ov/data:rw - $HOME/docker/isaac-sim/pkg:/isaac-sim/.local/share/ov/pkg:rw - ../extensions/PegasusSimulator/extensions/pegasus.simulator:/isaac-sim/.local/share/ov/data/documents/Kit/shared/exts/pegasus.simulator/:rw + - ../extensions/optitrack.natnet.emulator:/isaac-sim/.local/share/ov/data/documents/Kit/shared/exts/optitrack.natnet.emulator/:rw - ./omniverse.toml:/isaac-sim/.nvidia-omniverse/config/omniverse.toml:rw - ./user.config.json:/isaac-sim/.local/share/ov/data/Kit/Isaac-Sim Full/5.1/user.config.json:rw - .dev:/isaac-sim/.dev:rw diff --git a/simulation/isaac-sim/docker/px4-profiles/default.env b/simulation/isaac-sim/docker/px4-profiles/default.env new file mode 100644 index 000000000..7d00f82cf --- /dev/null +++ b/simulation/isaac-sim/docker/px4-profiles/default.env @@ -0,0 +1,5 @@ +# PX4 SITL parameter profile: default (no overrides beyond airframe + image patches). +# +# Selected by PX4_PARAM_PROFILE in the top-level .env (compose interpolation). +# Variables here are injected into the isaac-sim container and applied at PX4 boot +# via PX4_PARAM_* → param set in ROMFS/px4fmu_common/init.d-posix/rcS. \ No newline at end of file diff --git a/simulation/isaac-sim/docker/px4-profiles/vision.env b/simulation/isaac-sim/docker/px4-profiles/vision.env new file mode 100644 index 000000000..3a38ed65e --- /dev/null +++ b/simulation/isaac-sim/docker/px4-profiles/vision.env @@ -0,0 +1,35 @@ +# PX4 SITL parameter profile: external vision or state estimation. +# +# Data path: +# Isaac NatNet emulator → natnet_ros2 → vision_pose_converter +# → /{robot}/interface/mavros/vision_pose/* → PX4 EKF2 +# +# Pair with robot-side publish_to_mavros: true in natnet_config.yaml. +# Set PX4_PARAM_PROFILE=vision in .env (or overrides/isaac-natnet-vision.env). + +# Fuse horizontal position, vertical position, and yaw from VISION_POSITION_ESTIMATE. +# Bit 2 (velocity) omitted — pose_cov has no twist; MAVROS vision_pose is pose-only. +PX4_PARAM_EKF2_EV_CTRL=11 + +# Use vision as the height reference (indoor mocap; no GNSS in Isaac SITL). +PX4_PARAM_EKF2_HGT_REF=3 + +# No GPS receiver in sim; avoid waiting for GNSS aiding. +PX4_PARAM_EKF2_GPS_CTRL=0 + +# Trust covariance from vision_pose_cov (natnet_config position/orientation matrices). +PX4_PARAM_EKF2_EV_NOISE_MD=0 + +# End-to-end delay: NatNet frame + ROS + MAVROS (milliseconds). Tune if fusion lags. +# PX4_PARAM_EKF2_EV_DELAY=20 + +# Mocap tracks base_link at the vehicle CG — no lever arm. +PX4_PARAM_EKF2_EV_POS_X=0 +PX4_PARAM_EKF2_EV_POS_Y=0 +PX4_PARAM_EKF2_EV_POS_Z=0 + +# Allow arming without GPS fix when fusing external vision only. +PX4_PARAM_COM_ARM_WO_GPS=1 + +# Indoor vision yaw — don't fuse simulated magnetometer +PX4_PARAM_EKF2_MAG_TYPE=5 diff --git a/simulation/isaac-sim/docker/user_TEMPLATE.config.json b/simulation/isaac-sim/docker/user_TEMPLATE.config.json index 0ce7c11f0..26b6093c5 100755 --- a/simulation/isaac-sim/docker/user_TEMPLATE.config.json +++ b/simulation/isaac-sim/docker/user_TEMPLATE.config.json @@ -544,7 +544,8 @@ }, "exts": { "enabled": { - "0": "pegasus.simulator-5.1.0" + "0": "pegasus.simulator-5.1.0", + "1": "optitrack.natnet.emulator-0.1.0" } }, "window": { 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..3f6e890cd --- /dev/null +++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/.gitignore @@ -0,0 +1,16 @@ +# OptiTrack SDK archives and build artifacts (reference tree may exist locally) +NatNetClientSDK/*.zip +NatNetClientSDK/**/x64/ +NatNetClientSDK/**/Release/ +NatNetClientSDK/**/Debug/ +**/*.obj +**/*.pdb +**/*.exe +**/*.iobj +**/*.ipdb +**/*.tlog/ +**/__pycache__/ +**/*.pyc +*.egg-info/ +dist/ +build/ 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..17d99f52d --- /dev/null +++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/README.md @@ -0,0 +1,65 @@ +# 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/). + +## Layout + +``` +optitrack.natnet.emulator/ +├── config/extension.toml # Isaac Sim extension manifest (stub) +├── setup.py +├── optitrack/natnet/emulator/ +│ ├── defaults.py # Test / Isaac reference constants (not used by server) +│ └── server/ # NatNet UDP server (transport + protocol) +│ ├── natnet_server.py +│ ├── natnet_unicast_server.py +│ ├── natnet_data_types.py +│ ├── natnet_model_types.py +│ └── natnet_server_types.py +└── NatNetClientSDK/ # Reference SDK only (not shipped in wheel) +``` + +## Responsibilities + +| Layer | Role | +|-------|------| +| **Server** (`optitrack.natnet.emulator.server`) | UDP transport, `NAT_CONNECT` / `NAT_SERVERINFO`, MODELDEF **wire cache**, frame relay via `enqueue_mocap_data()` | +| **Isaac wrapper** (planned) | Build catalog from scene config, `set_model_def_payload(catalog.pack())`, sample prims → `enqueue_mocap_data()` | +| **`defaults.py`** | Hardcoded Drone → `/World/base_link` binding for tests and future Isaac wrapper; **not imported by the server** | + +The server stores MODELDEF as packed **bytes** (`_model_def_payload`). It does not own prim-path bindings or ctypes catalog copies. The Isaac wrapper (or integration tests) call `set_model_def_payload()` after building `sDataDescriptions`. + +## Usage (development) + +```python +from optitrack.natnet.emulator import NatNetUnicastServer, TransmissionType, make_default_drone_catalog + +server = NatNetUnicastServer(local_interface="172.31.0.200") +# Default Drone MODELDEF is loaded on init; override from Isaac wrapper: +# server.set_model_def_payload(make_default_drone_catalog().pack()) +server.start() +# Wrapper enqueues pre-built frames: +# server.enqueue_mocap_data(frame) +``` + +Default Docker sim IP: `172.31.0.200` (Isaac container on AirStack bridge network). + +## Protocol notes + +For libNatNet 4.4 **unicast**, the client uses a single UDP socket; server replies and frames go to the client's `NAT_CONNECT` source endpoint on the **command port** (1510). Handlers: `NAT_CONNECT`, `NAT_REQUEST_MODELDEF`, `NAT_KEEPALIVE`. See [optitrack-development skill](../../../../.agents/skills/optitrack-development/SKILL.md). + +## Tests + +| Tier | Mark | Location | +|------|------|----------| +| Unit (serializers, protocol, payload cache) | `unit` | `test/` + proxy in `tests/sim/optitrack_natnet_emulator/` | +| Integration (natnet_ros2 loopback Hz) | `integration`, `natnet` | `tests/integration/natnet/test_natnet_integration.py` | + +```bash +pytest tests/sim/optitrack_natnet_emulator/ -m unit -v +pytest tests/integration/natnet/ -m natnet -v # requires robot container + NatNet SDK +``` + +## Reference material + +Wire-format reference lives in [`NatNetClientSDK/`](NatNetClientSDK/README.md) (OptiTrack SDK samples; not redistributed by AirStack). diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/config/extension.toml b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/config/extension.toml new file mode 100644 index 000000000..a5df394da --- /dev/null +++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/config/extension.toml @@ -0,0 +1,23 @@ +[package] +version = "0.1.0" +title = "OptiTrack NatNet Emulator" +description = "NatNet UDP server emulator for Isaac Sim integration with natnet_ros2" +category = "Simulation" +keywords = ["optitrack", "natnet", "mocap", "simulation"] + +[dependencies] +"omni.isaac.core" = {} +"omni.usd" = {} +"omni.ui" = {} +"omni.kit.menu.utils" = {} + +# Pure transport/types package (no Kit UI; safe to import anywhere). +[[python.module]] +name = "optitrack.natnet.emulator" + +# Kit UI entry point: NatNetEmulatorExtension (menu + config-prim authoring window). +[[python.module]] +name = "optitrack.natnet.emulator.isaac.ui_extension" + +[python.build-system] +requires = ["setuptools"] diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/docs/natnet_interface_prim_design.md b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/docs/natnet_interface_prim_design.md new file mode 100644 index 000000000..8f05ef495 --- /dev/null +++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/docs/natnet_interface_prim_design.md @@ -0,0 +1,633 @@ +# Design: NatNet Interface Prim (stage-driven emulator) + +> Status: **proposed** (design only — no implementation yet). +> Supersedes the hardcoded `defaults.py` `TrackedBodyBinding` approach for the +> Isaac wrapper. See [`../README.md`](../README.md) for the shipped server. +> Delivery is staged across four commits — see +> [Implementation roadmap](#implementation-roadmap-staged-commits). + +## Goal + +Drive the NatNet emulator entirely from the **USD stage** instead of launch-script +constants. A config prim in the scene declares *that* a NatNet interface should +exist, *what* rigid bodies it streams, and *how* the server is configured. The +extension scans the stage for these prims and manages a server per interface. + +Because the configuration lives as authored USD, **the catalog persists between +runs for free** — it serializes with the `.usd`/`.usda` file. This is the answer +to "how do we maintain the catalog between runs." + +## Why a prim + +- **Persistence:** authored attributes save with the scene; reopening the stage + restores the full catalog and server config. +- **Discoverability / authoring:** appears in the Stage tree and Property panel; + editable by hand or by tooling without touching Python. +- **Live control:** toggling an attribute can start/stop the server at runtime. +- **Multi-interface:** N config prims are supported naturally (multi-robot, or a + unicast + multicast pair). + +## Schema (USD applied API schema) + +Use **applied API schemas** so the attributes are typed and surface cleanly in the +Property panel. Implement them as **codeless schemas**: author `schema.usda` with +`skipCodeGeneration = true`, run `usdGenSchema` **once** to emit +`generatedSchema.usda` + `plugInfo.json` (no C++/Python classes are generated — +only the registry files), and load that plugin from the extension. USD then reads +the typed fallbacks at runtime. See [Risks & open questions](#risks--open-questions) +— schema registration inside Kit is the main unknown and has a fallback. + +Two schemas: + +### `NatNetInterfaceAPI` (single-apply) + +Applied to a holder prim (e.g. a `Scope` or `Xform`, conventionally +`/World/NatNetInterface`). Carries server-level config: + +| Attribute | Type | Maps to `NatNetUnicastServer` | +|-----------|------|-------------------------------| +| `natnet:serverEnabled` | `bool` | start / stop (see Lifecycle) | +| `natnet:serverIp` | `string` | `local_interface` | +| `natnet:mode` | `token` (`unicast`\|`multicast`) | `transmission_type` | +| `natnet:multicastAddr` | `string` | `multicast_address` | +| `natnet:commandPort` | `int` | `command_port` | +| `natnet:dataPort` | `int` | `data_port` | +| `natnet:publishRate` | `float` | `publish_rate` | +| `natnet:natnetVersion` | `string` (`"4.4.0.0"`) | `natnet_version` | + +### `NatNetBodyBindingAPI` (multiple-apply) + +The **whole catalog lives on the interface prim** — a dictionary of body entries — +so the extension reads one prim to get the full mapping and never traverses the +stage to *find* bindings. A **multiple-apply** API schema expresses that dictionary +natively: one instance per body, keyed by an opaque instance token (``). + +The Motive rigid body name is an **explicit attribute** (`rigidBodyName`), not the +instance token, so it can be typed/edited as a normal text field in the UI without +renaming a schema instance. Each instance carries the per-body metadata plus a +**single-target relationship** to the tracked prim. Relationship targets are +**path-aware** — USD rewrites them automatically when the tracked prim is renamed or +reparented — which a literal path-string dict (e.g. `customData` `VtDictionary`) +would not survive. + +| Member (per instance ``) | Type | UI label | Maps to | +|-------------------------------|------|----------|---------| +| `natnet:body::rigidBodyName` | `string` | "Motive Rigid Body Name" | `sRigidBodyDescription.szName` | +| `natnet:body::streamingId` | `int` | "Streaming ID" | `sRigidBodyDescription.ID` | +| `natnet:body::parentId` | `int` | "Parent ID" | `sRigidBodyDescription.parentID` (default `-1`) | +| `natnet:body::target` | `rel` → prim | "Tracked Prim" | pose source | + +> Alternative considered: a single relationship `natnet:bodies` with N targets plus +> parallel `int[]` arrays (`streamingIds`, `parentIds`) indexed alongside the +> targets. Functionally equivalent and slightly more compact, but the multi-apply +> instances are typed, individually editable in the Property panel, and self-naming +> (instance name = body name). Rejected the per-tracked-prim single-apply approach +> from the prior draft because it forced a stage traversal to discover bindings. + +### Example (`.usda` sketch) + +```usda +def Scope "NatNetInterface" ( + prepend apiSchemas = ["NatNetInterfaceAPI", + "NatNetBodyBindingAPI:body0"] +) +{ + bool natnet:serverEnabled = true + string natnet:serverIp = "172.31.0.200" + token natnet:mode = "unicast" + int natnet:commandPort = 1510 + int natnet:dataPort = 1511 + float natnet:publishRate = 100 + string natnet:natnetVersion = "4.4.0.0" + + # one body entry (key "body0"); add more by applying NatNetBodyBindingAPI: + string natnet:body:body0:rigidBodyName = "Drone" + int natnet:body:body0:streamingId = 1 + int natnet:body:body0:parentId = -1 + rel natnet:body:body0:target = +} +``` + +## Catalog assembly + +For each enabled interface, read its `NatNetBodyBindingAPI` instances directly off +the interface prim (no stage traversal), and build one `sRigidBodyDescription` per +instance (`szName` = `rigidBodyName`, `ID` = streamingId, `parentID` = parentId) +into an `sDataDescriptions`, then `server.set_model_def_payload(catalog.pack())`. +This is the data-driven generalization of the current `make_default_drone_catalog()` +in [`server/natnet_model_types.py`](../optitrack/natnet/emulator/server/natnet_model_types.py). + +Resolve each instance's `target` relationship to a `Usd.Prim` handle **once** at +this point and **cache** the `(prim, bodyName, streamingId)` tuples on the manager +entry. The per-frame loop then reuses those handles — it never re-reads the schema +or searches the stage. Rebuild the cache only on enable or when the interface's +binding members change. + +Each interface owns its own dictionary, so multi-interface scoping is inherent — +no cross-interface grouping key is needed. + +## Python API (authoring + scripting) + +The prim is the persisted source of truth, but authoring/reading it should never +require hand-writing `CreateAttribute` / `ApplyAPI` / relationship calls. A thin +facade in the package wraps the schema so scripts can create, edit, and read an +interface — and so the same dataclasses are reused by the runtime manager. + +### Dataclasses (in-memory form) + +```python +@dataclass +class BodyBinding: + rigid_body_name: str # Motive rigid body name (szName) + target_prim: str # USD path of the tracked prim + streaming_id: int = 1 + parent_id: int = -1 + +@dataclass +class NatNetInterfaceConfig: + server_enabled: bool = True + server_ip: str = "172.31.0.200" + mode: str = "unicast" # "unicast" | "multicast" + multicast_addr: str = "239.255.42.99" + command_port: int = 1510 + data_port: int = 1511 + publish_rate: float = 100.0 + natnet_version: str = "4.4.0.0" + bodies: list[BodyBinding] = field(default_factory=list) + + @classmethod + def from_dict(cls, data: dict) -> "NatNetInterfaceConfig": ... + def to_dict(self) -> dict: ... +``` + +`from_dict` accepts the dictionary the user passes in a script. `bodies` may be a +list of dicts **or** a `{prim_path: {...}}` mapping (the "dictionary of prims → +rigid body names and stuff"), normalized into `BodyBinding` objects. + +### Authoring / reading helpers + +```python +def author_interface(stage, prim_path, config) -> Usd.Prim: + """Create/overwrite the interface prim: apply NatNetInterfaceAPI, set server + attrs, and apply one NatNetBodyBindingAPI: instance per body with its + target relationship. `config` may be a NatNetInterfaceConfig or a plain dict.""" + +def read_interface(prim) -> NatNetInterfaceConfig: + """Reverse of author_interface — reconstruct the dataclass from authored USD.""" + +def find_interfaces(stage) -> list[Usd.Prim]: + """All prims carrying NatNetInterfaceAPI (used by the manager and tooling).""" +``` + +### Script usage (standalone launch) + +Author from a dict and bring the server up in one place — no GUI required: + +```python +from optitrack.natnet.emulator.isaac import author_interface, NatNetServerManager + +CONFIG = { + "server_enabled": True, + "server_ip": "172.31.0.200", + "mode": "unicast", + "publish_rate": 100, + "bodies": { + "/World/base_link": {"rigid_body_name": "Drone", "streaming_id": 1}, + }, +} + +author_interface(stage, "/World/NatNetInterface", CONFIG) # persists with the stage + +manager = NatNetServerManager(stage) # discovers interfaces, honors serverEnabled +manager.start() # spins up servers + per-frame pose sampling +# manager.shutdown() on teardown +``` + +Because `author_interface` writes the same attributes the GUI/manager read, a +script-authored interface is indistinguishable from a hand-authored one and saves +into the `.usd`. A convenience `author_and_start(...)` can wrap the two calls for +the common single-interface script case. + +> The dataclasses (`BodyBinding`, `NatNetInterfaceConfig`) are the single in-memory +> representation shared by: `from_dict` (script input) → `author_interface` (USD +> write) → `read_interface` (USD read) → catalog assembly + pose-sampling cache. +> This is also the natural successor to `defaults.py` — its constants become a +> `NatNetInterfaceConfig` default. + +## Lifecycle (manager) + +A manager owns a map `{Sdf.Path -> NatNetUnicastServer}` keyed by interface prim +path, and reacts to a `Usd.Notice.ObjectsChanged` listener plus stage-open events. + +- `natnet:serverEnabled` → **true**: construct a **fresh** `NatNetUnicastServer` from + the prim's attributes, build + set the MODELDEF payload, `start()`, store in the map. +- `natnet:serverEnabled` → **false**: `server.shutdown()` (closes both sockets, joins + all child threads), remove from the map. +- Server-config attr changes while enabled (IP, ports, mode): treat as shutdown + + fresh start so the new socket binding takes effect. +- Binding-member changes while enabled (add/remove a body, edit a `target`, + streamingId, or parentId): rebuild the cached `(prim, bodyName, streamingId)` + list and refresh the MODELDEF via `set_model_def_payload()` — **no server restart + needed**, since the transport/sockets are unchanged. + +**Fresh instance per enable (chosen):** the manager always builds a new server +object on enable and discards the old one. The existing server is not designed to +be re-`start()`ed in place — `shutdown()` closes sockets without recreating them and +`start()` appends to `self.threads` without resetting — so reusing an instance across +a disable→enable cycle would re-run dead threads with `shutdown_event` still set. +A fresh instance avoids all of that with no server-side changes. Note this is +**thread** teardown within the Kit process — not an OS process kill: `shutdown()` +ends the server's daemon threads and closes its sockets: + +```text +shutdown(): running=False; shutdown_event.set(); close command_socket; close data_socket; join threads +``` + +## Pose sampling + +A per-frame Kit update callback iterates the **cached** `(prim, bodyName, +streamingId)` tuples (resolved once during catalog assembly), reads each prim's +world transform (`UsdGeom.Xformable.ComputeLocalToWorldTransform` via a per-frame +`UsdGeomXformCache`, or the physics view pose), converts it into an +`sFrameOfMocapData` rigid body (params `& 0x01` = tracking valid), and calls +`server.enqueue_mocap_data(frame)`. No schema reads or stage searches happen in the +hot path — only direct pose pulls off the cached handles, exactly as intended. + +**Frame convention (`up_axis` option):** Isaac/USD is Z-up; Motive exposes an +"Up Axis" setting. The reference `natnet_ros2` driver (and ours) require it set to +**Z** and do **no** axis conversion — `rb_to_pose` is an identity copy and +`vision_pose_converter_node` only normalizes quaternion sign. So the interface +config carries an `up_axis` (default **`"Z"`**) and the sampler streams the USD +world pose **as-is** by default, matching the rest of the stack. Setting +`up_axis="Y"` emulates a default Y-up Motive by rotating each streamed pose -90° about +X (`(x,y,z) -> (x,z,-y)`, quaternion vector part the same, scalar kept) — a proper +right-handed change of basis. The conversion is the pure, unit-tested +`frames.to_motive_pose`; the manager re-reads `up_axis` on every resync and applies +it in `sample_once`. Authored on the prim as `natnet:upAxis`, editable in the UI +("Up axis" dropdown) and via `build_drone_config(..., up_axis=...)`. + +## Packaging notes + +- **Codeless applied schemas** keep this dependency-light: `schema.usda` defines + `NatNetInterfaceAPI` / `NatNetBodyBindingAPI` with `skipCodeGeneration = true`; + `usdGenSchema` emits `generatedSchema.usda` + `plugInfo.json` (registry only, no + compiled classes); the extension loads the plugin on startup. Registration + reliability inside Kit is the main risk — see below. +- **Standalone + Kit parity:** put "scan stage + manage servers + sample poses" in + a plain module callable from both the Kit `omni.ext.IExt` (GUI/live toggle) and + the standalone launch scripts (`launch_scripts/example_*_pegasus_launch_script.py`), + which have no extension lifecycle. +- **Persistence layer:** author interface/binding attributes on the stage's + root/edit layer (not the session layer) so they save with the scene. + +## Implementation roadmap (staged commits) + +Each stage is independently reviewable and ends with a concrete manual check. +Stages build on each other **in order** — each depends on the previous: + +1. **Commit 1 — Schema + config prim spawn + authoring UI** (no deps) +2. **Commit 2 — Detection + parameter read** (needs the schema + facade from 1) +3. **Commit 3 — Catalog parse + server start** (needs detection/read from 2) +4. **Commit 4 — Enable/disable lifecycle** (needs the start path from 3) + +Unit tests are added **within each commit** (co-located in the extension `test/` +dir with a proxy under `tests/sim/`), so every stage lands already covered — see +[Testing & CI integration](#testing--ci-integration) for the mechanism. + +> **Design for testability:** keep a pure-Python **config model** (dataclasses, +> `from_dict`/`to_dict`, the `natnet:body::…` attribute-name builder, +> validation) separate from a thin **USD binding** layer (apply/read against a +> `Usd.Stage`). Most logic is then hermetically testable with no `pxr`/Isaac, and +> the USD shell stays small and `pxr`-guarded. + +### Commit 1 — Schema + config prim spawn + authoring UI + +Stand up the data model and let a user create/edit an interface by hand. No +scanning, no server. + +- **Schema:** `schema.usda` + `plugInfo.json` defining the codeless + `NatNetInterfaceAPI` (single-apply) and `NatNetBodyBindingAPI` (multiple-apply); + registered/loaded via `config/extension.toml`. +- **Authoring facade:** `BodyBinding` / `NatNetInterfaceConfig` dataclasses, + `author_interface()`, and add/remove-body helpers (no `read_*`/manager yet). +- **UI (Kit extension):** a menu entry (e.g. *Create ▸ NatNet Interface*) that + spawns `/World/NatNetInterface`; a panel to edit the server attributes and to + **add a tracked body from the current stage selection**, typing *Motive Rigid + Body Name*, *Streaming ID*, *Parent ID* (the `target` relationship is set from + the selected prim). +- 🧪 **Tests** (`test/test_config_model.py`, hermetic): `from_dict`/`to_dict` + round-trips; `bodies` normalization from a list **and** a `{prim_path: {...}}` + mapping; the `natnet:body::…` attribute-name builder; validation + (mode ∈ {unicast, multicast}, distinct command/data ports). Plus + `test/test_authoring_usd.py` (`pytest.importorskip("pxr")`): + `author_interface()` → `read_interface()` round-trip on + `Usd.Stage.CreateInMemory()`, asserting attrs, the multi-apply instances, and the + `target` relationship. +- ✅ **Manual verify:** create the prim from the menu, select a prim, fill the + fields, **save and reopen** the stage — the attributes and the `target` + relationship persist and match what was entered. + +### Commit 2 — Detection + parameter read + +Make the extension *aware* of interfaces without acting on them. + +- `NatNetServerManager` skeleton: `find_interfaces()`, `read_interface() -> + NatNetInterfaceConfig`, a `Usd.Notice.ObjectsChanged` listener, and stage-open + hookup. Logs each detected interface and its parsed config (server params + body + list). No server, no pose sampling. +- 🧪 **Tests** (`test/test_discovery.py`): pure `reconcile(old, new) -> actions` + diff logic (hermetic) deciding start/stop/rebuild from config deltas; + `find_interfaces()` on an in-memory stage with/without the API applied + (`importorskip("pxr")`). +- ✅ **Manual verify:** open a stage containing an interface → logs show the prim + path and fully parsed config; adding/removing a body or editing a field updates + the logged readout live. + +### Commit 5 — Data enqueue + dynamic catalog + scripting + UI readouts ✅ IMPLEMENTED + +> Rolled the deferred data path together with live updates, scripting, and UI: +> - **`isaac/frames.py`** (pure): `BodySample`, `make_rigid_body_data`, `build_frame`. +> Sets `params` bit `0x01` (tracking valid — the client *skips* bodies without it) +> and frame bit `0x02` (model list changed). **Frame convention resolved: emit the +> USD world pose as-is, no Z-up→Y-up swap** — `natnet_ros2`'s `rb_to_pose` is an +> identity copy and nothing downstream re-axes, so swapping would desync the pose. +> - **`usd_bindings.read_world_pose(prim)`** — `ComputeLocalToWorldTransform` → +> `((x,y,z),(qx,qy,qz,qw))` (the pose *stored in the USD stage*). +> - **Manager**: a **dirty flag** (`_needs_resync`) — set on any NatNet prim change +> (`_on_objects_changed`) or `mark_dirty()`, cleared by the next sample. The +> physics-step subscription (`subscribe_physics_step_events`) calls `sample_once`, +> which resyncs the catalog/target cache when dirty (so **bodies added/removed live +> are picked up**, refreshing the server MODELDEF and flagging `model_list_changed`), +> then samples every body's world pose and `enqueue_mocap_data`s one frame. Missing +> prims stream as lost (NaN, valid bit clear). +> - **Scripting**: `NatNetServerManager.start_from_stage()` (author the prim, then +> call it). `start_server` / `apply_enabled` remain the programmatic enable path. +> - **UI**: each tracked-body row has a live readout — status dot (green=streaming, +> grey=idle, red=missing) + world position pulled from USD, refreshed ~6 Hz. +> - **Tests**: `test_frames.py` (pure: valid bit, NaN-lost, order, model-change bit, +> pack decode), `test_pose_sampling.py` (pxr: read_world_pose, sample no/one/missing, +> moving prim, **body-added-live resync + model_list_changed**), `test_pose_streaming.py` +> (pxr loopback: streamed `NAT_FRAMEOFDATA` carries the sampled USD position). Proxies +> added under `tests/sim/optitrack_natnet_emulator/`. + +### Commit 6 — Example-script auto-start (load-up integration) ✅ IMPLEMENTED + +> The Pegasus example launch scripts stand the emulator up on scene load — no UI. +> - **`isaac/scene_setup.py`** — `build_drone_config(drones, ...)` (pure: one +> `BodyBinding` per `(rigid_body_name, streaming_id, target_prim)`, validated) and +> `start_drone_natnet_server(stage, drones, ...)` (authors the `/World/NatNetInterface` +> prim, then creates a physics-subscribed `NatNetServerManager` and starts it). +> Exported from `optitrack.natnet.emulator.isaac`. +> - **Launch scripts** (`launch_scripts/example_one*.py`, `example_multi*.py`): after +> spawning drones, if `LAUNCH_NATNET=true` they author one rigid body per drone +> `base_link` and start the server. Single: `("Drone", 1, /World/base_link)`. +> Multi: `("", i, /World/drone/base_link)` for each drone. +> Failures are caught (never kill the sim); the manager is torn down on close. +> - **Import path**: scripts add `../extensions/optitrack.natnet.emulator` to +> `sys.path` so the package imports without enabling the Kit UI extension (keeps a +> single, script-owned manager — no duplicate physics/USD subscriptions). +> - **Wiring**: `LAUNCH_NATNET` / `NATNET_BODY_NAME` are passed into the isaac-sim +> container via `simulation/isaac-sim/docker/docker-compose.yaml`, mirroring the +> robot-side `natnet_ros2` gate. Server binds the container IP `172.31.0.200` +> (the config default), which the robot `server_ip` already points at. +> - **Tests**: `test_scene_setup.py` (pure: single/multi mapping, server-param +> forwarding, duplicate name/id rejection, empty catalog). Proxy under +> `tests/sim/optitrack_natnet_emulator/`. + +### Commit 3 — Catalog parse + server start ✅ IMPLEMENTED + +> **Status (combined with Commit 4):** catalog parse + server start/stop lifecycle +> landed together. What shipped: +> - `isaac/catalog.py`: `build_catalog(config) → sDataDescriptions` (pure ctypes, +> no USD) + `find_duplicate_targets(config)`. +> - `usd_bindings.resolve_targets(stage, config) → (existing, missing)` diagnostics. +> - `NatNetServerManager` owns **one** server via an injectable +> `server_factory` (default `default_server_factory`, unicast only). Methods: +> `start_server` (idempotent — builds catalog, `set_model_def_payload`, `start`), +> `stop_server`, `toggle_server`, `apply_enabled` (reconcile to `serverEnabled`), +> `is_running`, `log_target_diagnostics`. `on_shutdown` stops the server. +> - UI: **Create Interface** (prim), plus a live **Start/Stop Server** toggle button +> + RUNNING/stopped status. The button starts/stops directly from the on-stage +> prim, **regardless** of `serverEnabled` (per request). Auto-reconcile of +> `serverEnabled` on scan is intentionally *not* wired (would fight the button); +> `apply_enabled` is available + tested for future auto-driving. +> - Tests: `test_catalog.py` (no/single/multiple bodies, field fidelity, name +> truncation, MAX_MODELS guard, duplicate targets), `test_server_lifecycle.py` +> (mock factory: create+start exactly once, idempotent, toggle, reconcile, +> on_shutdown, multicast rejected), `test_target_resolution.py` (pxr-guarded: +> missing/empty/duplicate/mixed), `test_server_from_config.py` (loopback: served +> MODELDEF == `build_catalog(config)`, clean restart rebinds ports). Proxies added +> under `tests/sim/optitrack_natnet_emulator/`. + +Turn a detected config into a running server. + +- Build `sDataDescriptions` from the config, resolve each `target` to a `Usd.Prim` + and cache `(prim, rigidBodyName, streamingId)`, construct a `NatNetUnicastServer` + from the server attrs, `set_model_def_payload()`, `start()`, and run the per-frame + pose-sampling callback → `enqueue_mocap_data()`. +- Initial `serverEnabled` is honored at load (start only if true); **live toggling + is deferred to Commit 4.** +- 🧪 **Tests** (hermetic): `test/test_catalog_from_config.py` — a + `NatNetInterfaceConfig` with N bodies packs into an `sDataDescriptions` with the + right `szName`/`ID`/`parentID` per body (extends the existing catalog tests); + `test/test_pose_to_frame.py` — pure `pose → sFrameOfMocapData` conversion incl. + the **Z-up → Y-up** frame convention (feed a known transform, assert position + + quaternion). Plus a loopback case extending `test_unicast_protocol.py`: a server + built from a config serves that MODELDEF and streams enqueued frames. +- ✅ **Manual verify:** with `serverEnabled = true` at load, the robot + `natnet_ros2` connects and `/{ROBOT_NAME}/perception/optitrack/` publishes + at ~`publishRate`; moving the tracked prim moves the published pose. (A Python + loopback client can stand in for `natnet_ros2` if the SDK isn't built.) + +### Commit 4 — Enable/disable lifecycle ✅ FOLDED INTO COMMIT 3 + +> The fresh-instance start/stop state machine (`start_server`/`stop_server`/ +> `toggle_server`/`apply_enabled`) and the restart-cleanliness loopback +> (`test_server_from_config.py::test_stop_frees_port_for_restart`) shipped with +> Commit 3 above. Remaining for a follow-up: rebuild-on-config-delta while running, +> and (optionally) auto-reconciling `serverEnabled` edits from the Property panel. + +React to `serverEnabled` flipping at runtime. + +- On `serverEnabled` **false** → `server.shutdown()` (closes sockets, joins all + child threads), drop from the map. On **true** → build a **fresh** + `NatNetUnicastServer` and `start()` again. (Server-config and binding live-edits + per the [Lifecycle](#lifecycle-manager) section land here or as an immediate + follow-up.) +- 🧪 **Tests** (`test/test_lifecycle.py`, hermetic): the manager state machine + (`serverEnabled` true→false→true yields stop then fresh-start actions); and a + restart-cleanliness loopback test — `start()` then `shutdown()` joins all threads + and closes both sockets, and a **fresh** instance rebinds the same ports and + serves again (proves no thread/socket leak across cycles). +- ✅ **Manual verify:** toggle `serverEnabled` in the UI repeatedly — the server + threads die and respawn cleanly, the pose topic stops and resumes, and there is + no thread/socket leak across cycles (guaranteed by the fresh-instance approach). + +## Testing & CI integration + +Follows AirStack's **co-location + proxy** unit-test pattern (see the +[`add-unit-tests`](../../../../../.agents/skills/add-unit-tests/SKILL.md) skill). +The extension already uses it today — test source in +[`test/`](../test/), thin proxies under +[`tests/sim/optitrack_natnet_emulator/`](../../../../../tests/sim/optitrack_natnet_emulator/). + +**Mechanism (per new test file):** + +1. Write the test co-located in + `simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_.py`, + decorated `@pytest.mark.unit`. +2. Add a one-line proxy + `tests/sim/optitrack_natnet_emulator/test_.py` that calls + `reexport_unit_tests(globals(), repo_path(".../optitrack.natnet.emulator/test"), "test_.py")`. +3. It is then discovered automatically by `pytest tests/ -m unit`, + `airstack test -m unit`, and CI `system-tests.yml` (PR-open runs `pytest tests/` + with no `-m` filter) — **no CI YAML change needed.** + +**Two specifics for this feature:** + +- **`usd-core` dependency.** The `pxr`-guarded tests (`author_interface` / + `read_interface` / `find_interfaces` against an in-memory stage) only execute if + `pxr` is importable; otherwise `pytest.importorskip("pxr")` skips them. To make CI + actually exercise them, add **`usd-core`** to + [`tests/requirements.txt`](../../../../../tests/requirements.txt) (installed into + the venv by `system-tests.yml`). Pure config-model / catalog / pose / lifecycle + tests need no new deps (`numpy` is already present). +- **Not a colcon package.** This extension is a sim-side Python extension, **not** an + ament/colcon package, so it does **not** go in + [`tests/colcon_unit_test_packages.yaml`](../../../../../tests/colcon_unit_test_packages.yaml). + Its CI coverage is the pytest proxy route only (the `build_packages` colcon route + does not apply). + +**Per-commit test artifacts:** + +| Commit | Co-located test file(s) | `pxr`? | Proxy to add | +|--------|-------------------------|--------|--------------| +| 1 | `test_config_model.py`, `test_authoring_usd.py` | model: no · authoring: yes | one proxy each | +| 2 | `test_discovery.py` | `find_interfaces`: yes · reconcile: no | one proxy | +| 3 | `test_catalog_from_config.py`, `test_pose_to_frame.py` (+ extend `test_unicast_protocol.py`) | no | one proxy each | +| 4 | `test_lifecycle.py` | no | one proxy | + +Update [`tests/sim/README.md`](../../../../../tests/sim/README.md) as proxies are +added. Run locally with `airstack test -m unit -v` or +`pytest simulation/isaac-sim/extensions/optitrack.natnet.emulator/test -m unit -v`. + +## Risks & open questions + +Ordered roughly by impact. The first two change *how* Commit 1 / Commit 3 are +sequenced; the rest are "note and handle during implementation." + +1. **Schema registration inside Kit (highest unknown).** Codeless schemas still + require `usdGenSchema`-emitted `generatedSchema.usda` + `plugInfo.json` to be + discovered by USD's plugin system (search paths, `HasAPI`/`ApplyAPI`, typed + fallbacks) within the Kit runtime. **Mitigation:** make a **schema-registration + spike the first task of Commit 1**, before any UI. **Fallback:** if codeless + registration is painful in this Kit/USD version, drop to plain namespaced custom + attributes (`prim.CreateAttribute("natnet:…")`) — the Python facade hides which + backing is used, so runtime/UI code is unaffected. + +2. **Commit 1 front-loads risk with no streaming payoff.** Schema + facade + custom + multi-apply UI is heavy and moves no poses. **Option:** prove + schema + config model + facade + headless manager/server via the **script path + + unit tests first**, then build the Kit GUI on that tested foundation. Keeps the + "manually verify the UI" deliverable but rests it on green tests. *(Sequencing + choice — not yet decided.)* + +3. **Multi-apply + relationship UI is the heavy part.** The relationship picker and + "add body from selection" are the real cost in Commit 1. Consider shipping v1 + with a **single body** (or the relationship + parallel-arrays variant) to close + the end-to-end loop first, then generalize the catalog UI. + +4. **Frame convention is a correctness landmine (not a footnote).** Isaac Z-up / + meters vs Motive Y-up will silently produce wrong poses, and `natnet_ros2` + *already* converts — so the risk is **double-converting**. **Make pinning the + exact wire convention a first-class task in Commit 3**, validated against the + existing verified `libNatNet` contract and `tests/integration/natnet/`. + +5. **Pose-sampling timing.** The world-transform read must happen **after PhysX + write-back** (the correct post-physics / world callback), or streamed poses lag + or read stale. Confirm which callback the per-frame loop hooks into. + +6. **Rapid enable toggling can race the socket rebind.** Fresh-instance + + `shutdown()` join (1 s) then re-`start()` on the same port can collide if the old + socket has not fully closed. `SO_REUSEADDR` is set and the restart-cleanliness + test (Commit 4) guards this; consider also **debouncing** rapid toggles. + +7. **Standalone vs Kit parity for the live toggle.** `serverEnabled` live-toggling + relies on the `ObjectsChanged` loop, which only runs under Kit. Standalone + scripts use the **initial** state unless they pump USD notices — document this. + +8. **`usd-core` ≠ Isaac's bundled USD.** The CI `pxr` from `usd-core` may differ in + version from Isaac's USD. Fine for hermetic schema/Sdf unit tests; just be aware + behavior can differ slightly from the runtime. + +9. **Release gating.** Docker image content changes here (extension install), so bump + `.env` `VERSION` + CHANGELOG before merge (see `bump-version-and-release`). + +## Relationship to existing code + +- Replaces `defaults.py` `TrackedBodyBinding` (each `NatNetBodyBindingAPI` instance + is the authored form of that dataclass; `prim_path` becomes the instance's + `target` relationship). +- Server (`NatNetUnicastServer`) is unchanged — it stays transport-only and + consumes `set_model_def_payload()` + `enqueue_mocap_data()` exactly as today. + +## Debugging discoveries (in-sim end-to-end bring-up) + +Hard-won findings from getting the liveliness sentinel green (Isaac emulator → +`natnet_ros2` on the robot). Captured so the next agent does not re-derive them. + +1. **GIL starvation kills the background data thread inside Kit.** The server's + `_data_update_loop` daemon (which `time.sleep`s then sends frames) is reliably + starved by Kit's render/physics main loop holding the GIL — frames get enqueued + but never transmitted, so the client handshakes but receives nothing. Fix: + `NatNetServer.auto_stream` (default `True`) gates that loop; the Isaac wrapper + sets `auto_stream = False` in `manager.start_server()` and calls + `server.pump_once()` **synchronously from the physics-step callback** + (`manager.sample_once()`). Outside Kit (host integration tests, sidecar) the + default `auto_stream=True` path still works. Don't "optimize" the pump back into + the daemon thread. + +2. **The liveliness/system path runs the *standalone Pegasus script*, not a saved + scene USD.** `tests/conftest.py` sets `ISAAC_SIM_USE_STANDALONE=true` **and + `ISAAC_SIM_SCRIPT_NAME=example_multi_px4_pegasus_launch_script.py`** — even for + `--num-robots 1`. So the NatNet auto-start wiring **must** live in + `example_multi_…` (it does); `example_one_…` and the default + `airstack up isaac-sim` (which uses `ISAAC_SIM_USE_STANDALONE=false` → + `run_isaacsim.launch.py` opening `simple_pegasus.scene.usd`) do **not** exercise + it. To reproduce the harness manually you must pass all of + `AUTOLAUNCH=true ISAAC_SIM_USE_STANDALONE=true + ISAAC_SIM_SCRIPT_NAME=example_multi_px4_pegasus_launch_script.py + LAUNCH_NATNET=true PLAY_SIM_ON_START=true`. + +3. **Single-agent body/prim contract.** For `NUM_ROBOTS=1` the multi script names + the body `"Drone"` (bare) → target `/World/drone1/base_link` (the multi script's + drone prim path is always `drone{i}`, even for one drone). This matches the robot + `natnet_config.yaml` body and the liveliness sentinel topic + `/robot_1/perception/optitrack/Drone/pose_cov`. + +4. **Target prims appear *after* server start.** The Pegasus `base_link` is created + on the first Play tick — after `start_server()`. `_sample_cache` therefore stores + target *paths* and re-resolves `stage.GetPrimAtPath()` every `sample_once`, so a + body goes from "lost" to valid as soon as its prim exists (no `mark_dirty`). + +5. **`ss` is unreliable in the Isaac image; use `/proc/net/udp`.** Confirm the + server is bound by looking for ports `1510`/`1511` as hex (`05E6`/`05E7`) in + `/proc/net/udp` rather than trusting `ss -ulnp` (returns empty here). + +6. **First-message budget.** Cold Isaac boots compile shaders; the NatNet sentinel + first-message timeout was raised to 120s (`_NATNET_FIRST_MSG_TIMEOUT`) to match + the generous `/clock` (600s) / sentinel-node (300s) budgets. Once streaming, the + topic runs at ~98–140 Hz (100 Hz `publishRate`). + +7. **The client must retry the connect — the server starts ~100s after the robot.** + This was the actual liveliness blocker. `natnet_ros2_node` originally called + `connect_and_setup()` **once** in its constructor; on `NetworkError` it logged and + gave up. In the standalone path the Isaac emulator only binds 1510/1511 ~100s into + sim boot, long after the robot's node starts, so the one-shot connect failed + permanently (topic advertised, zero frames). Fix: `connect_and_setup()` returns + `bool` and a 2s `connect_timer_` retries the handshake until the first success, + then cancels itself (also correct for real Motive powered on after the drone). + Verified by starting Isaac and the node together: the node retries for ~90s, then + logs "Frame callback registered" the instant the server binds. **To reproduce the + race manually, start the robot node first, then Isaac** — bringing the robot up + *after* Isaac is ready hides the bug. 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..2a0afd935 --- /dev/null +++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/defaults.py @@ -0,0 +1,28 @@ +"""Reference tracked-body defaults for tests and the future Isaac Sim wrapper. + +Not imported by NatNetServer — the server stores MODELDEF as packed bytes only. +""" + +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 Pegasus scenes (example_one_px4_pegasus_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/isaac/__init__.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/__init__.py new file mode 100644 index 000000000..9bb35e108 --- /dev/null +++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/__init__.py @@ -0,0 +1,55 @@ +# Copyright (c) 2024 Carnegie Mellon University +# MIT License - see LICENSE in the repository root for full text. +"""Isaac Sim integration for the NatNet emulator (stage-driven config prim). + +``config`` is pure Python (no USD). ``usd_bindings`` imports ``pxr`` lazily, so +importing this package is safe in non-Isaac environments; the USD functions only +require ``pxr`` when actually called. +""" + +from .config import ( + BodyBinding, + NatNetInterfaceConfig, + body_attr_name, + make_instance_key, +) +from .catalog import build_catalog, find_duplicate_targets +from .frames import BodySample, build_frame, make_rigid_body_data +from .manager import NatNetServerManager, default_server_factory, format_interface +from .scene_setup import ( + DEFAULT_INTERFACE_PATH, + build_drone_config, + start_drone_natnet_server, +) +from .usd_bindings import ( + author_interface, + find_interfaces, + is_interface, + read_interface, + read_world_pose, + resolve_targets, +) + +__all__ = [ + "DEFAULT_INTERFACE_PATH", + "BodyBinding", + "BodySample", + "NatNetInterfaceConfig", + "NatNetServerManager", + "author_interface", + "body_attr_name", + "build_catalog", + "build_drone_config", + "build_frame", + "default_server_factory", + "find_duplicate_targets", + "find_interfaces", + "format_interface", + "is_interface", + "make_instance_key", + "make_rigid_body_data", + "read_interface", + "read_world_pose", + "resolve_targets", + "start_drone_natnet_server", +] diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/catalog.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/catalog.py new file mode 100644 index 000000000..f83a0eb51 --- /dev/null +++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/catalog.py @@ -0,0 +1,54 @@ +# Copyright (c) 2024 Carnegie Mellon University +# MIT License - see LICENSE in the repository root for full text. +"""Commit 3 — catalog parse. + +Turn a :class:`NatNetInterfaceConfig` into the server's MODELDEF catalog +(``sDataDescriptions`` of rigid bodies). Pure Python + ctypes (the ``server`` +package is stdlib-only), so this is hermetically unit-testable — no USD, no Kit. +""" + +from __future__ import annotations + +from ..server.natnet_common import ModelLimits +from ..server.natnet_model_types import DataDescriptors, sDataDescriptions +from .config import NatNetInterfaceConfig + +# szName is null-terminated on the wire; reserve one byte for the terminator. +_MAX_NAME_BYTES = int(ModelLimits.MAX_NAMELENGTH) - 1 +_MAX_MODELS = int(ModelLimits.MAX_MODELS) + + +def build_catalog(config: NatNetInterfaceConfig) -> sDataDescriptions: + """Build an ``sDataDescriptions`` rigid-body catalog from the config bodies. + + No bodies -> an empty catalog (``nDataDescriptions == 0``). Names longer than + the NatNet name field are truncated. Raises ``ValueError`` if there are more + bodies than the protocol allows. + """ + bodies = config.bodies + if len(bodies) > _MAX_MODELS: + raise ValueError( + f"Too many bodies for one catalog: {len(bodies)} > {_MAX_MODELS} (MAX_MODELS)" + ) + + descriptions = sDataDescriptions() + descriptions.nDataDescriptions = len(bodies) + for i, body in enumerate(bodies): + desc = descriptions.arrDataDescriptions[i] + desc.type = int(DataDescriptors.Descriptor_RigidBody) + rb = desc.RigidBodyDescription + rb.szName = body.rigid_body_name.encode("utf-8")[:_MAX_NAME_BYTES] + rb.ID = int(body.streaming_id) + rb.parentID = int(body.parent_id) + rb.offsetqw = 1.0 # identity quaternion offset + rb.nMarkers = 0 + return descriptions + + +def find_duplicate_targets(config: NatNetInterfaceConfig) -> list[str]: + """Return target prim paths referenced by more than one body (empties ignored).""" + counts: dict[str, int] = {} + for body in config.bodies: + if body.target_prim: + counts[body.target_prim] = counts.get(body.target_prim, 0) + 1 + return [path for path, count in counts.items() if count > 1] diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/config.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/config.py new file mode 100644 index 000000000..b0c18b555 --- /dev/null +++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/config.py @@ -0,0 +1,236 @@ +# Copyright (c) 2024 Carnegie Mellon University +# MIT License - see LICENSE in the repository root for full text. +"""Pure-Python config model for the stage-driven NatNet interface. + +No USD / Isaac imports here — this module is hermetically unit-testable. The USD +binding layer (author/read against a ``Usd.Stage``) lives in ``usd_bindings.py`` +and depends on this model. Keeping the two split is the design's "design for +testability" rule: nearly all logic stays free of ``pxr``. + +Attribute names follow the multi-apply schema convention +(``natnet:body::``) so the custom-attribute backing used today is a +drop-in for a future typed applied schema. +""" + +from __future__ import annotations + +from dataclasses import dataclass, field +from typing import Any, Iterable, Mapping + +# --- attribute name constants (USD property names) ----------------------------- + +ATTR_NAMESPACE = "natnet" +MARKER_ATTR = "natnet:isInterface" + +ATTR_SERVER_ENABLED = "natnet:serverEnabled" +ATTR_SERVER_IP = "natnet:serverIp" +ATTR_MODE = "natnet:mode" +ATTR_MULTICAST_ADDR = "natnet:multicastAddr" +ATTR_COMMAND_PORT = "natnet:commandPort" +ATTR_DATA_PORT = "natnet:dataPort" +ATTR_PUBLISH_RATE = "natnet:publishRate" +ATTR_NATNET_VERSION = "natnet:natnetVersion" +ATTR_UP_AXIS = "natnet:upAxis" + +ATTR_POSE_NOISE_ENABLED = "natnet:poseNoiseEnabled" +ATTR_POSE_NOISE_STD_METERS = "natnet:poseNoiseStdMeters" +ATTR_POSE_NOISE_ROTATION_DEG = "natnet:poseNoiseRotationDeg" + +BODY_PREFIX = "natnet:body:" +BODY_FIELD_RIGID_BODY_NAME = "rigidBodyName" +BODY_FIELD_STREAMING_ID = "streamingId" +BODY_FIELD_PARENT_ID = "parentId" +BODY_FIELD_TARGET = "target" + +VALID_MODES = ("unicast", "multicast") + +# Streamed up-axis. Motive exposes an "Up Axis" setting; the reference natnet_ros2 +# driver requires it set to Z and passes coordinates through untouched. Isaac Sim / +# USD is natively Z-up, so "Z" is a pass-through that matches the rest of the +# AirStack stack (default). "Y" emulates a default (Y-up) Motive by rotating the +# streamed pose -90deg about X. See ``frames.to_motive_pose``. +VALID_UP_AXES = ("Y", "Z") + +# defaults shared with NatNetUnicastServer +DEFAULT_SERVER_IP = "172.31.0.200" +DEFAULT_MULTICAST_ADDR = "239.255.42.99" +DEFAULT_COMMAND_PORT = 1510 +DEFAULT_DATA_PORT = 1511 +DEFAULT_PUBLISH_RATE = 100.0 +DEFAULT_NATNET_VERSION = "4.4.0.0" +DEFAULT_UP_AXIS = "Z" +DEFAULT_POSE_NOISE_ENABLED = True +DEFAULT_POSE_NOISE_STD_METERS = 0.0005 +DEFAULT_POSE_NOISE_ROTATION_DEG = 0.05 + + +def body_attr_name(key: str, field_name: str) -> str: + """USD property name for a body-binding field on the given instance key.""" + return f"{BODY_PREFIX}{key}:{field_name}" + + +def make_instance_key(name: str, used: set[str]) -> str: + """Derive a valid, unique multi-apply instance token from a rigid body name. + + USD property/instance tokens must be identifier-like; sanitize non-alnum chars + to underscores and disambiguate collisions with a numeric suffix. + """ + sanitized = "".join(c if c.isalnum() else "_" for c in name).strip("_") + if not sanitized: + sanitized = "body" + if sanitized[0].isdigit(): + sanitized = f"b_{sanitized}" + key = sanitized + i = 1 + while key in used: + key = f"{sanitized}_{i}" + i += 1 + used.add(key) + return key + + +@dataclass +class BodyBinding: + """One tracked rigid body: a Motive name/ID mapped to a USD prim path.""" + + rigid_body_name: str + target_prim: str + streaming_id: int = 1 + parent_id: int = -1 + + @classmethod + def from_dict(cls, data: Mapping[str, Any], *, target_prim: str | None = None) -> "BodyBinding": + d = dict(data) + resolved_target = target_prim if target_prim is not None else d.get("target_prim") + if not resolved_target: + raise ValueError("BodyBinding requires a target_prim (USD path of the tracked prim)") + if "rigid_body_name" not in d: + raise ValueError("BodyBinding requires a rigid_body_name") + return cls( + rigid_body_name=str(d["rigid_body_name"]), + target_prim=str(resolved_target), + streaming_id=int(d.get("streaming_id", 1)), + parent_id=int(d.get("parent_id", -1)), + ) + + def to_dict(self) -> dict[str, Any]: + return { + "rigid_body_name": self.rigid_body_name, + "target_prim": self.target_prim, + "streaming_id": self.streaming_id, + "parent_id": self.parent_id, + } + + +def _normalize_bodies(bodies: Any) -> list[BodyBinding]: + """Accept a list of dicts/BodyBindings, or a ``{prim_path: {...}}`` mapping.""" + if bodies is None: + return [] + out: list[BodyBinding] = [] + if isinstance(bodies, Mapping): + for prim_path, body in bodies.items(): + out.append(BodyBinding.from_dict(body, target_prim=prim_path)) + return out + if isinstance(bodies, Iterable): + for body in bodies: + if isinstance(body, BodyBinding): + out.append(body) + else: + out.append(BodyBinding.from_dict(body)) + return out + raise ValueError(f"`bodies` must be a list or a mapping, got {type(bodies).__name__}") + + +@dataclass +class NatNetInterfaceConfig: + """Server-level config plus the body catalog for one NatNet interface prim.""" + + server_enabled: bool = True + server_ip: str = DEFAULT_SERVER_IP + mode: str = "unicast" + multicast_addr: str = DEFAULT_MULTICAST_ADDR + command_port: int = DEFAULT_COMMAND_PORT + data_port: int = DEFAULT_DATA_PORT + publish_rate: float = DEFAULT_PUBLISH_RATE + natnet_version: str = DEFAULT_NATNET_VERSION + up_axis: str = DEFAULT_UP_AXIS + pose_noise_enabled: bool = DEFAULT_POSE_NOISE_ENABLED + pose_noise_std_meters: float = DEFAULT_POSE_NOISE_STD_METERS + pose_noise_rotation_deg: float = DEFAULT_POSE_NOISE_ROTATION_DEG + bodies: list[BodyBinding] = field(default_factory=list) + + @classmethod + def from_dict(cls, data: Mapping[str, Any]) -> "NatNetInterfaceConfig": + d = dict(data) + return cls( + server_enabled=bool(d.get("server_enabled", True)), + server_ip=str(d.get("server_ip", DEFAULT_SERVER_IP)), + mode=str(d.get("mode", "unicast")), + multicast_addr=str(d.get("multicast_addr", DEFAULT_MULTICAST_ADDR)), + command_port=int(d.get("command_port", DEFAULT_COMMAND_PORT)), + data_port=int(d.get("data_port", DEFAULT_DATA_PORT)), + publish_rate=float(d.get("publish_rate", DEFAULT_PUBLISH_RATE)), + natnet_version=str(d.get("natnet_version", DEFAULT_NATNET_VERSION)), + up_axis=str(d.get("up_axis", DEFAULT_UP_AXIS)).upper(), + pose_noise_enabled=bool(d.get("pose_noise_enabled", DEFAULT_POSE_NOISE_ENABLED)), + pose_noise_std_meters=float(d.get("pose_noise_std_meters", DEFAULT_POSE_NOISE_STD_METERS)), + pose_noise_rotation_deg=float(d.get("pose_noise_rotation_deg", DEFAULT_POSE_NOISE_ROTATION_DEG)), + bodies=_normalize_bodies(d.get("bodies")), + ) + + def to_dict(self) -> dict[str, Any]: + return { + "server_enabled": self.server_enabled, + "server_ip": self.server_ip, + "mode": self.mode, + "multicast_addr": self.multicast_addr, + "command_port": self.command_port, + "data_port": self.data_port, + "publish_rate": self.publish_rate, + "natnet_version": self.natnet_version, + "up_axis": self.up_axis, + "pose_noise_enabled": self.pose_noise_enabled, + "pose_noise_std_meters": self.pose_noise_std_meters, + "pose_noise_rotation_deg": self.pose_noise_rotation_deg, + "bodies": [b.to_dict() for b in self.bodies], + } + + def validate(self) -> "NatNetInterfaceConfig": + """Raise ``ValueError`` (aggregating all problems) if the config is invalid.""" + errors: list[str] = [] + if self.mode not in VALID_MODES: + errors.append(f"mode must be one of {VALID_MODES}, got {self.mode!r}") + if str(self.up_axis).upper() not in VALID_UP_AXES: + errors.append(f"up_axis must be one of {VALID_UP_AXES}, got {self.up_axis!r}") + for port_name, port in (("command_port", self.command_port), ("data_port", self.data_port)): + if not (0 < port < 65536): + errors.append(f"{port_name} must be in 1..65535, got {port}") + if self.command_port == self.data_port: + errors.append("command_port and data_port must differ") + if self.publish_rate <= 0: + errors.append(f"publish_rate must be > 0, got {self.publish_rate}") + if self.pose_noise_std_meters < 0: + errors.append( + f"pose_noise_std_meters must be >= 0, got {self.pose_noise_std_meters}" + ) + if self.pose_noise_rotation_deg < 0: + errors.append( + f"pose_noise_rotation_deg must be >= 0, got {self.pose_noise_rotation_deg}" + ) + for i, body in enumerate(self.bodies): + if not body.rigid_body_name: + errors.append(f"body[{i}] rigid_body_name must be non-empty") + names = [b.rigid_body_name for b in self.bodies] + if len(set(names)) != len(names): + errors.append("rigid_body_name values must be unique across bodies") + ids = [b.streaming_id for b in self.bodies] + if len(set(ids)) != len(ids): + errors.append("streaming_id values must be unique across bodies") + if errors: + raise ValueError("Invalid NatNetInterfaceConfig: " + "; ".join(errors)) + return self + + def assign_instance_keys(self) -> list[tuple[str, BodyBinding]]: + """Pair each body with a deterministic, unique multi-apply instance key.""" + used: set[str] = set() + return [(make_instance_key(b.rigid_body_name, used), b) for b in self.bodies] diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/frames.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/frames.py new file mode 100644 index 000000000..cadcc95be --- /dev/null +++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/frames.py @@ -0,0 +1,131 @@ +# Copyright (c) 2024 Carnegie Mellon University +# MIT License - see LICENSE in the repository root for full text. +"""Pose -> NatNet frame conversion (the data-enqueue path). + +Pure Python + ctypes (no USD, no Kit), so it's hermetically unit-testable. Sampled +prim world poses become an ``sFrameOfMocapData`` of rigid bodies that the server +streams to ``natnet_ros2``. + +**Frame convention:** Motive exposes an "Up Axis" setting. AirStack's ``natnet_ros2`` +(and the reference driver) require it set to **Z** and copy the rigid-body pose +straight through (``rb_to_pose`` is an identity copy). Isaac Sim / USD is natively +Z-up, so the default ``up_axis="Z"`` emits the prim's USD world pose **as-is** and +stays in sync with the rest of the stack. ``up_axis="Y"`` emulates a default (Y-up) +Motive — see :func:`to_motive_pose`. + +**params bits** (must match the client's ``is_tracking_valid`` / ``model_list_changed``): +``0x01`` on a rigid body marks tracking valid — the client *skips* bodies without it. +``0x02`` on the frame signals the model list changed so the client re-requests MODELDEF +(set the frame after the catalog changes, e.g. a body added live). +""" + +from __future__ import annotations + +import math +import numpy as np +from dataclasses import dataclass +from scipy.spatial.transform import Rotation +from ..server.natnet_data_types import sFrameOfMocapData, sRigidBodyData + +TRACKING_VALID = 0x01 +MODEL_LIST_CHANGED = 0x02 + + +@dataclass +class BodySample: + """One sampled rigid body: streaming ID + world pose, or an invalid (lost) body.""" + + streaming_id: int + position: tuple[float, float, float] = (0.0, 0.0, 0.0) + orientation: tuple[float, float, float, float] = (0.0, 0.0, 0.0, 1.0) # qx,qy,qz,qw + valid: bool = True + + @classmethod + def lost(cls, streaming_id: int) -> "BodySample": + """An untracked body (missing prim): NaN position, tracking-invalid bit clear.""" + nan = float("nan") + return cls(streaming_id, (nan, nan, nan), (0.0, 0.0, 0.0, 1.0), valid=False) + + +def to_motive_pose(position: tuple[float, float, float], orientation: tuple[float, float, float, float], up_axis: str = "Z"): + """Re-express an Isaac (Z-up) world pose in Motive's streamed up-axis frame. + + Returns ``(position, orientation)`` re-axed for the given ``up_axis``: + + - ``"Z"`` (default) — identity pass-through. Isaac/USD is Z-up and the + reference Motive setup streams Z-up, so the pose flows through unchanged and + matches ``natnet_ros2`` (which does no axis conversion). + - ``"Y"`` — emulate a default Y-up Motive by rotating the pose -90 deg about X + (Isaac ``+Z`` -> Motive ``+Y``): ``(x, y, z) -> (x, z, -y)``. This is a + proper right-handed -> right-handed change of basis (det = +1), so the + quaternion's vector part takes the same swap and the scalar part is + unchanged: ``(qx, qy, qz, qw) -> (qx, qz, -qy, qw)``. + + Non-finite components (a lost body's NaN position) pass through unchanged. + """ + if str(up_axis).upper() != "Y": + return position, orientation + x, y, z = position + qx, qy, qz, qw = orientation + return (x, z, -y), (qx, qz, -qy, qw) + + +def make_rigid_body_data(sample: BodySample) -> sRigidBodyData: + """Build one ``sRigidBodyData`` from a sample (sets the tracking-valid bit).""" + rb = sRigidBodyData() + rb.ID = int(sample.streaming_id) + x, y, z = sample.position + qx, qy, qz, qw = sample.orientation + rb.x, rb.y, rb.z = float(x), float(y), float(z) + rb.qx, rb.qy, rb.qz, rb.qw = float(qx), float(qy), float(qz), float(qw) + rb.MeanError = 0.0 + rb.params = TRACKING_VALID if sample.valid else 0 + return rb + + +def build_frame( + frame_number: int, + samples, + *, + timestamp: float = 0.0, + model_list_changed: bool = False, +) -> sFrameOfMocapData: + """Assemble an ``sFrameOfMocapData`` of rigid bodies from samples.""" + frame = sFrameOfMocapData() + frame.iFrame = int(frame_number) + samples = list(samples) + frame.nRigidBodies = len(samples) + for i, sample in enumerate(samples): + frame.RigidBodies[i] = make_rigid_body_data(sample) + frame.fTimestamp = float(timestamp) + frame.params = MODEL_LIST_CHANGED if model_list_changed else 0 + return frame + + +def is_finite_pose(sample: BodySample) -> bool: + """True if all position/orientation components are finite (no NaN/inf).""" + return all(math.isfinite(v) for v in (*sample.position, *sample.orientation)) + + +def apply_pose_noise( + position: tuple[float, float, float], + orientation: tuple[float, float, float, float], + pose_noise_std_meters: float, + pose_noise_rotation_deg: float, +) -> tuple[tuple[float, float, float], tuple[float, float, float, float]]: + """Add independent Gaussian noise to position (m) and orientation (deg, XYZ euler).""" + + x, y, z = position + if pose_noise_std_meters > 0.0: + x += np.random.normal(0, pose_noise_std_meters) + y += np.random.normal(0, pose_noise_std_meters) + z += np.random.normal(0, pose_noise_std_meters) + + roll, pitch, yaw = Rotation.from_quat(orientation).as_euler("xyz", degrees=True) + if pose_noise_rotation_deg > 0.0: + roll += np.random.normal(0, pose_noise_rotation_deg) + pitch += np.random.normal(0, pose_noise_rotation_deg) + yaw += np.random.normal(0, pose_noise_rotation_deg) + + qx, qy, qz, qw = Rotation.from_euler("xyz", (roll, pitch, yaw), degrees=True).as_quat() + return (x, y, z), (float(qx), float(qy), float(qz), float(qw)) diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/manager.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/manager.py new file mode 100644 index 000000000..09f7b0891 --- /dev/null +++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/manager.py @@ -0,0 +1,416 @@ +# Copyright (c) 2024 Carnegie Mellon University +# MIT License - see LICENSE in the repository root for full text. +"""Commits 2-4 — detection, parameter read, and server start/stop lifecycle. + +``NatNetServerManager`` detects interface prims, prints their parsed config +(Commit 2), and owns a **single** server instance it can start and stop (Commits +3-4). On each enable it builds a MODELDEF catalog from the config and constructs a +fresh server via an injectable factory (so unit tests can mock it and assert the +server is created/started exactly once, without binding real sockets). Pose +sampling / frame publishing is intentionally left for a later commit. + +``format_interface`` is pure (no USD). The server factory and lifecycle are +USD-free too (they take a ``NatNetInterfaceConfig``); only the stage-driven +helpers touch ``pxr`` / ``omni`` (lazily), so importing this module stays hermetic. +""" + +from __future__ import annotations + +from .catalog import build_catalog, find_duplicate_targets +from .config import DEFAULT_UP_AXIS, NatNetInterfaceConfig +from .frames import BodySample, apply_pose_noise, build_frame, to_motive_pose +from .usd_bindings import find_interfaces, read_interface, read_world_pose, resolve_targets + + +def _catalog_signature(config: NatNetInterfaceConfig): + """Identity of the catalog (body id/name set) — changes trigger a MODELDEF refresh.""" + return tuple((b.streaming_id, b.rigid_body_name) for b in config.bodies) + + +def _parse_version(version_str: str) -> tuple[int, int, int, int]: + try: + parts = tuple(int(x) for x in str(version_str).split(".")) + except ValueError: + parts = () + return (parts + (0, 0, 0, 0))[:4] + + +def default_server_factory(config: NatNetInterfaceConfig): + """Construct (but do not start) a ``NatNetUnicastServer`` from a config.""" + from ..server import NatNetUnicastServer, TransmissionType + + if config.mode != "unicast": + raise NotImplementedError( + f"mode {config.mode!r} is not supported yet (unicast only)" + ) + server = NatNetUnicastServer( + local_interface=config.server_ip, + transmission_type=TransmissionType.UNICAST, + multicast_address=None, + command_port=config.command_port, + data_port=config.data_port, + ) + server.publish_rate = config.publish_rate + server.natnet_version = _parse_version(config.natnet_version) + return server + + +def format_interface(prim_path: str, cfg: NatNetInterfaceConfig) -> str: + """Render a human-readable multi-line summary of one interface config.""" + lines = [f"[natnet] Interface @ {prim_path}"] + lines.append(f" serverEnabled : {cfg.server_enabled}") + lines.append(f" serverIp : {cfg.server_ip}") + lines.append(f" mode : {cfg.mode}") + if cfg.mode == "multicast": + lines.append(f" multicastAddr : {cfg.multicast_addr}") + lines.append(f" commandPort : {cfg.command_port}") + lines.append(f" dataPort : {cfg.data_port}") + lines.append(f" publishRate : {cfg.publish_rate}") + lines.append(f" natnetVersion : {cfg.natnet_version}") + lines.append(f" upAxis : {cfg.up_axis}") + lines.append(f" poseNoise : enabled={cfg.pose_noise_enabled}") + lines.append( + f" std={cfg.pose_noise_std_meters} m, rot={cfg.pose_noise_rotation_deg} deg" + ) + if cfg.bodies: + lines.append(f" bodies ({len(cfg.bodies)}):") + for b in cfg.bodies: + target = b.target_prim or "" + lines.append( + f" - {b.rigid_body_name} (id={b.streaming_id}, parent={b.parent_id}) -> {target}" + ) + else: + lines.append(" bodies : (none)") + return "\n".join(lines) + + +class NatNetServerManager: + """Detects interface prims, prints config, and owns one server instance.""" + + def __init__(self, server_factory=None): + self._stage_event_sub = None + self._usd_listener = None + self._scan_tick_sub = None + self._scan_pending = False + self._server = None + self._server_factory = server_factory or default_server_factory + # Sampling state. ``_needs_resync`` is the "latest config has been read" + # flag inverted: a NatNet prim edit sets it True (stale); the next physics + # sample re-reads the catalog/targets and clears it. ``_sample_cache`` holds + # the resolved (streaming_id, name, prim) tuples sampled every step. + self._needs_resync = False + self._sample_cache: list = [] + self._frame_counter = 0 + self._catalog_signature = None + self._physx_sub = None + # Streamed up-axis (from the interface config; re-read on every resync). + # "Z" (default) streams the Isaac/USD world pose as-is; "Y" re-axes it to + # emulate a default Y-up Motive. See frames.to_motive_pose. + self._up_axis = DEFAULT_UP_AXIS + # Pose noise. + self._pose_noise_enabled = False + self._pose_noise_std_meters = 0.0 + self._pose_noise_rotation_deg = 0.0 + + # --- lifecycle ------------------------------------------------------------- + + def on_startup(self): + import omni.usd + + usd_context = omni.usd.get_context() + self._stage_event_sub = usd_context.get_stage_event_stream().create_subscription_to_pop( + self._on_stage_event, name="natnet_manager_stage_events" + ) + self._register_usd_listener() + self._subscribe_physics() + print("[natnet] NatNetServerManager initialized") + self.scan_and_print() + + def on_shutdown(self): + self.stop_server() + self._physx_sub = None + self._stage_event_sub = None + self._scan_tick_sub = None + self._scan_pending = False + self._revoke_usd_listener() + + def _subscribe_physics(self): + # Sample + enqueue poses on every physics step (only fires while playing). + try: + import omni.physx + + self._physx_sub = omni.physx.get_physx_interface().subscribe_physics_step_events( + self._on_physics_step + ) + except Exception as exc: # pragma: no cover - Kit/physx only + print(f"[natnet] Physics step subscription unavailable: {exc}") + self._physx_sub = None + + def _on_physics_step(self, _dt): + if self._server is not None: + self.sample_once() + + # --- scanning -------------------------------------------------------------- + + def scan_and_print(self, *_): + """Find every interface prim and print its parsed config.""" + stage = self._get_stage() + if stage is None: + return + interfaces = find_interfaces(stage) + if not interfaces: + print("[natnet] Scan: no NatNetInterface prims on stage.") + return + print(f"[natnet] Scan: {len(interfaces)} interface(s) detected.") + for prim in interfaces: + cfg = read_interface(prim) + print(format_interface(prim.GetPath().pathString, cfg)) + + # --- server lifecycle (single instance; USD-free, factory-injectable) ------ + + @property + def is_running(self) -> bool: + return self._server is not None + + @property + def server(self): + return self._server + + def start_server(self, config: NatNetInterfaceConfig) -> bool: + """Build the catalog, construct a fresh server, and start it — once. + + Idempotent: if a server is already running this is a no-op returning False. + Returns True when a new server was created and started. + """ + if self._server is not None: + print("[natnet] start_server ignored: a server is already running.") + return False + catalog = build_catalog(config) + server = self._server_factory(config) + server.set_model_def_payload(catalog.pack()) + # Pump frames from our sample_once (physics-step) thread rather than the + # server's background timer: inside the Isaac Sim process that daemon thread + # is starved by the render/physics main loop, so frames never get sent. + if hasattr(server, "auto_stream"): + server.auto_stream = False + server.start() + self._server = server + # Force a resync on the first sampled frame so the prim->pose cache is built + # from the live stage (and the catalog signature is seeded). + self._needs_resync = True + self._frame_counter = 0 + # None so the first resync reports "changed" and the first streamed frame + # flags model_list_changed (nudging the client to (re)read MODELDEF). + self._catalog_signature = None + print( + f"[natnet] Server started on {config.server_ip} " + f"(cmd {config.command_port} / data {config.data_port}) " + f"with {len(config.bodies)} body(ies)." + ) + return True + + def stop_server(self) -> bool: + """Shut down the running server (fresh instance is built on next start). + + Idempotent: returns False if nothing was running. + """ + if self._server is None: + return False + try: + self._server.shutdown() + finally: + self._server = None + self._sample_cache = [] + self._needs_resync = False + print("[natnet] Server stopped.") + return True + + def toggle_server(self, config: NatNetInterfaceConfig) -> bool: + """Start if stopped, stop if running. Returns the resulting running state.""" + if self.is_running: + self.stop_server() + else: + self.start_server(config) + return self.is_running + + def apply_enabled(self, config: NatNetInterfaceConfig) -> None: + """Reconcile running state to ``config.server_enabled`` (start/stop).""" + if config.server_enabled and not self.is_running: + self.start_server(config) + elif not config.server_enabled and self.is_running: + self.stop_server() + + def log_target_diagnostics(self, config: NatNetInterfaceConfig) -> None: + """Warn about missing target prims and duplicate targets (best-effort).""" + stage = self._get_stage() + if stage is not None: + _existing, missing = resolve_targets(stage, config) + for body in missing: + print( + f"[natnet] WARNING: body '{body.rigid_body_name}' target prim " + f"missing or empty: {body.target_prim or ''}" + ) + for path in find_duplicate_targets(config): + print(f"[natnet] WARNING: multiple bodies target the same prim: {path}") + + # --- scripting entry point ------------------------------------------------- + + def start_from_stage(self) -> bool: + """Find the interface prim on the current stage, read it, and start. + + Convenience for scripts/Pegasus launchers: author the prim (see + ``author_interface``) then call this. Returns False if nothing to start. + """ + stage = self._get_stage() + if stage is None: + print("[natnet] start_from_stage: no active stage.") + return False + interfaces = find_interfaces(stage) + if not interfaces: + print("[natnet] start_from_stage: no NatNetInterface prim found.") + return False + config = read_interface(interfaces[0]) + self.log_target_diagnostics(config) + return self.start_server(config) + + # --- pose sampling + dynamic catalog (the data-enqueue path) --------------- + + def mark_dirty(self) -> None: + """Flag that the on-stage config changed; next sample re-reads the catalog.""" + self._needs_resync = True + + def _resync(self, stage) -> bool: + """Re-read the interface config, rebuild the catalog, and re-resolve targets. + + Returns True if the catalog (body id/name set) actually changed, so the next + frame can flag ``model_list_changed`` and the client re-requests MODELDEF. + """ + interfaces = find_interfaces(stage) + if not interfaces: + self._sample_cache = [] + return False + config = read_interface(interfaces[0]) + self._up_axis = config.up_axis + self._pose_noise_enabled = config.pose_noise_enabled + self._pose_noise_std_meters = config.pose_noise_std_meters + self._pose_noise_rotation_deg = config.pose_noise_rotation_deg + if self._server is not None: + self._server.set_model_def_payload(build_catalog(config).pack()) + # Cache target *paths* (not prim handles): the prim is re-resolved every + # sample so bodies whose target is created *after* the server starts — e.g. + # a Pegasus drone base_link spawned on the first Play tick — start streaming + # a valid pose as soon as the prim appears (instead of being stuck "lost"). + self._sample_cache = [ + (body.streaming_id, body.rigid_body_name, body.target_prim) + for body in config.bodies + ] + signature = _catalog_signature(config) + changed = signature != self._catalog_signature + self._catalog_signature = signature + return changed + + def sample_once(self, stage=None): + """Sample every body's USD world pose and enqueue one frame to the server. + + Resyncs the catalog first if the config is dirty (so bodies added/removed + live are picked up). Returns the enqueued frame (or None if nothing to do). + """ + if self._server is None: + return None + if stage is None: + stage = self._get_stage() + if stage is None: + return None + + model_changed = False + if self._needs_resync: + model_changed = self._resync(stage) + self._needs_resync = False + + samples = [] + for streaming_id, _name, target_path in self._sample_cache: + prim = stage.GetPrimAtPath(target_path) if target_path else None + pose = read_world_pose(prim) if prim is not None else None + if pose is None: + samples.append(BodySample.lost(streaming_id)) + else: + position, orientation = to_motive_pose(*pose, up_axis=self._up_axis) + if self._pose_noise_enabled: + position, orientation = apply_pose_noise(position, orientation, self._pose_noise_std_meters, self._pose_noise_rotation_deg) + samples.append(BodySample(streaming_id, position, orientation, valid=True)) + + frame = build_frame( + self._frame_counter, samples, model_list_changed=model_changed + ) + self._frame_counter += 1 + self._server.enqueue_mocap_data(frame) + # Send synchronously from this (physics-step) thread; the server's background + # data thread is unreliable inside the GIL-bound Isaac Sim process. + pump = getattr(self._server, "pump_once", None) + if callable(pump): + pump() + return frame + + # --- stage / USD notifications -------------------------------------------- + + def _get_stage(self): + import omni.usd + + return omni.usd.get_context().get_stage() + + def _on_stage_event(self, event): + import omni.usd + + if event.type == int(omni.usd.StageEventType.OPENED): + self._register_usd_listener() + self.scan_and_print() + + def _register_usd_listener(self): + from pxr import Tf, Usd + + stage = self._get_stage() + if stage is None: + return + self._revoke_usd_listener() + self._usd_listener = Tf.Notice.Register( + Usd.Notice.ObjectsChanged, self._on_objects_changed, stage + ) + + def _revoke_usd_listener(self): + if self._usd_listener is not None: + self._usd_listener.Revoke() + self._usd_listener = None + + def _on_objects_changed(self, notice, sender): + # Only re-scan when something NatNet-related changed, so we don't spam the + # console on every transform update while the sim is playing. + try: + paths = list(notice.GetResyncedPaths()) + list(notice.GetChangedInfoOnlyPaths()) + except Exception: # pragma: no cover - defensive + paths = [] + if any(("NatNetInterface" in str(p)) or ("natnet:" in str(p)) for p in paths): + # A NatNet prim changed (e.g. a body added/retargeted while live): mark + # the sampler dirty so the next physics step re-reads the catalog and + # nudges the client to refresh MODELDEF. + self._needs_resync = True + # A single author_interface() (Create/Save) emits many notices — one per + # attribute/relationship op. Debounce them into one scan on the next + # update tick so we print the final state once, not once per op. + self._request_scan() + + def _request_scan(self): + if self._scan_pending: + return + self._scan_pending = True + import omni.kit.app + + self._scan_tick_sub = ( + omni.kit.app.get_app() + .get_update_event_stream() + .create_subscription_to_pop(self._on_scan_tick, name="natnet_manager_scan_tick") + ) + + def _on_scan_tick(self, _event): + self._scan_pending = False + self._scan_tick_sub = None + self.scan_and_print() diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/scene_setup.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/scene_setup.py new file mode 100644 index 000000000..524c7f9b9 --- /dev/null +++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/scene_setup.py @@ -0,0 +1,115 @@ +# Copyright (c) 2024 Carnegie Mellon University +# MIT License - see LICENSE in the repository root for full text. +"""Standalone-launch helpers: stand up a drone NatNet interface on scene load. + +Used by the Pegasus example launch scripts so a Motive-compatible NatNet server +comes up automatically with one rigid body per drone ``base_link`` — no UI clicks. + +Two layers, mirroring the rest of the package: + +- ``build_drone_config`` is **pure** (no USD / Kit), so it unit-tests hermetically. +- ``start_drone_natnet_server`` authors the interface prim and owns a + :class:`~optitrack.natnet.emulator.isaac.manager.NatNetServerManager` that samples + poses on each physics step. It imports ``pxr``/``omni`` lazily (only when called). +""" + +from __future__ import annotations + +from typing import Iterable, Sequence, Tuple + +from .config import ( + DEFAULT_COMMAND_PORT, + DEFAULT_DATA_PORT, + DEFAULT_POSE_NOISE_ENABLED, + DEFAULT_POSE_NOISE_ROTATION_DEG, + DEFAULT_POSE_NOISE_STD_METERS, + DEFAULT_PUBLISH_RATE, + DEFAULT_SERVER_IP, + DEFAULT_UP_AXIS, + BodyBinding, + NatNetInterfaceConfig, +) + +# Where the example scripts author the single interface prim. +DEFAULT_INTERFACE_PATH = "/World/NatNetInterface" + +# (rigid_body_name, streaming_id, target_prim_path) +DroneSpec = Tuple[str, int, str] + + +def build_drone_config( + drones: Iterable[DroneSpec], + *, + server_ip: str = DEFAULT_SERVER_IP, + mode: str = "unicast", + command_port: int = DEFAULT_COMMAND_PORT, + data_port: int = DEFAULT_DATA_PORT, + publish_rate: float = DEFAULT_PUBLISH_RATE, + server_enabled: bool = True, + up_axis: str = DEFAULT_UP_AXIS, + pose_noise_enabled: bool = DEFAULT_POSE_NOISE_ENABLED, + pose_noise_std_meters: float = DEFAULT_POSE_NOISE_STD_METERS, + pose_noise_rotation_deg: float = DEFAULT_POSE_NOISE_ROTATION_DEG, +) -> NatNetInterfaceConfig: + """Build a validated config with one rigid body per drone. + + ``drones`` is an iterable of ``(rigid_body_name, streaming_id, target_prim)`` + tuples — typically one per spawned drone, with ``target_prim`` pointing at the + drone's ``base_link``. Raises ``ValueError`` (via ``validate``) on duplicate + names/ids or bad ports. + """ + bodies = [ + BodyBinding( + rigid_body_name=str(name), + target_prim=str(target), + streaming_id=int(streaming_id), + ) + for name, streaming_id, target in drones + ] + cfg = NatNetInterfaceConfig( + server_enabled=server_enabled, + server_ip=server_ip, + mode=mode, + command_port=command_port, + data_port=data_port, + publish_rate=publish_rate, + up_axis=up_axis, + pose_noise_enabled=pose_noise_enabled, + pose_noise_std_meters=pose_noise_std_meters, + pose_noise_rotation_deg=pose_noise_rotation_deg, + bodies=bodies, + ) + cfg.validate() + return cfg + + +def start_drone_natnet_server( + stage, + drones: Sequence[DroneSpec], + *, + prim_path: str = DEFAULT_INTERFACE_PATH, + start: bool = True, + **config_kwargs, +): + """Author a NatNet interface prim from ``drones`` and return a running manager. + + Authors ``prim_path`` (overwriting any existing interface) with one rigid body + per drone, then creates a :class:`NatNetServerManager` that subscribes to physics + steps and starts the server. Pump the sim (``timeline.play()`` / ``world.step``) + to stream poses. **Keep a reference to the returned manager** so it isn't + garbage-collected (which would tear down the physics subscription and server). + + Returns the ``NatNetServerManager``. If ``start`` is False (or the config is + authored disabled), the manager is created but the server is left stopped. + """ + from .manager import NatNetServerManager + from .usd_bindings import author_interface + + cfg = build_drone_config(drones, **config_kwargs) + author_interface(stage, prim_path, cfg) + + manager = NatNetServerManager() + manager.on_startup() + if start and cfg.server_enabled: + manager.start_from_stage() + return manager diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/ui_extension.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/ui_extension.py new file mode 100644 index 000000000..e83d742b6 --- /dev/null +++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/ui_extension.py @@ -0,0 +1,471 @@ +# Copyright (c) 2024 Carnegie Mellon University +# MIT License - see LICENSE in the repository root for full text. +"""Kit extension entry: docked editor for the NatNet interface config prim. + +Commit 1 scope — create/manage ``/World/NatNetInterface``. The window docks to the +bottom-right (alongside the Property panel, like Pegasus) so it's easy to find. + +Sync model is **explicit and user-driven** via the top button row: + +* **Load from Stage** — read the prim into the window (or reset to defaults if none). +* **Save** — write the window's current values to the prim (validate + author). +* **Create Interface** — ensure the interface prim exists (authored from the current + values) and select it. +* **Start/Stop Server** — start/stop the live UDP server directly (the button is the + authority, independent of ``serverEnabled``). While running, poses are sampled from + the USD stage on every physics step and streamed to ``natnet_ros2``. + +Each tracked-body row shows a live readout (status dot + world position pulled from +the USD stage) so you can see at a glance what each body is publishing. + +Edits in the window mutate an in-memory working copy only; nothing touches the prim +until you Save / Create, so you are in charge of keeping things synced. + +All ``omni.*`` imports live inside methods so this module never imports Kit at load +time outside a running Kit process. Authoring goes through the tested, USD-only +:mod:`optitrack.natnet.emulator.isaac.usd_bindings` facade. +""" + +from __future__ import annotations + +import omni.ext + +from .config import VALID_MODES, VALID_UP_AXES, BodyBinding, NatNetInterfaceConfig +from .manager import NatNetServerManager +from .usd_bindings import author_interface, find_interfaces, read_interface, read_world_pose + +_DEFAULT_PRIM_PATH = "/World/NatNetInterface" +_LABEL_WIDTH = 140 +_POS_REFRESH_PERIOD = 1.0 / 6.0 # seconds between live USD position reads + +_COLOR_LIVE = 0xFF33CC33 # green: prim resolves and server is streaming +_COLOR_IDLE = 0xFFAAAAAA # grey: prim resolves but server not running +_COLOR_LOST = 0xFF3333FF # red: no prim / NaN + + +class NatNetEmulatorExtension(omni.ext.IExt): + """Registers the Window menu entry + the docked editor panel.""" + + def on_startup(self, ext_id): # noqa: D401 - Kit lifecycle hook + self._window = None + self._bodies_frame = None + self._cfg = NatNetInterfaceConfig() + self._row_readouts = {} + self._pos_refresh_sub = None + self._last_pos_refresh = 0.0 + self._manager = NatNetServerManager() + self._manager.on_startup() + self._add_menu() + self._subscribe_position_refresh() + + def on_shutdown(self): + self._remove_menu() + self._pos_refresh_sub = None + self._row_readouts = {} + if self._manager is not None: + self._manager.on_shutdown() + self._manager = None + if self._window is not None: + self._window.destroy() + self._window = None + + # --- live position readout ------------------------------------------------- + + def _subscribe_position_refresh(self): + try: + import omni.kit.app + except Exception: # pragma: no cover - Kit only + return + self._pos_refresh_sub = ( + omni.kit.app.get_app() + .get_update_event_stream() + .create_subscription_to_pop(self._on_pos_refresh, name="natnet_ui_pos_refresh") + ) + + def _on_pos_refresh(self, _event): + import time + + if self._window is None or not self._window.visible or not self._row_readouts: + return + now = time.monotonic() + if now - self._last_pos_refresh < _POS_REFRESH_PERIOD: + return + self._last_pos_refresh = now + stage = self._get_stage() + running = self._manager is not None and self._manager.is_running + for idx, (status_label, pos_label) in list(self._row_readouts.items()): + if not (0 <= idx < len(self._cfg.bodies)): + continue + target = self._cfg.bodies[idx].target_prim + symbol, color, text = self._row_readout(stage, target, running) + status_label.text = symbol + status_label.style = {"color": color} + pos_label.text = text + pos_label.style = {"color": color} + + def _row_readout(self, stage, target, running): + if not target: + return "\u25cb", _COLOR_IDLE, "no target prim" + prim = stage.GetPrimAtPath(target) if stage is not None else None + pose = read_world_pose(prim) if prim is not None else None + if pose is None: + return "\u2717", _COLOR_LOST, "NaN (prim missing)" + (x, y, z), _quat = pose + text = f"{x:+.3f}, {y:+.3f}, {z:+.3f}" + if running: + return "\u25cf", _COLOR_LIVE, text + return "\u25cf", _COLOR_IDLE, text + + # --- menu ------------------------------------------------------------------ + + def _add_menu(self): + try: + import omni.kit.menu.utils as menu_utils + from omni.kit.menu.utils import MenuItemDescription + except Exception: # pragma: no cover - Kit only + return + self._menu_entries = [ + MenuItemDescription(name="NatNet Interface", onclick_fn=self._toggle_window) + ] + menu_utils.add_menu_items(self._menu_entries, "Window") + + def _remove_menu(self): + try: + import omni.kit.menu.utils as menu_utils + except Exception: # pragma: no cover - Kit only + return + if getattr(self, "_menu_entries", None): + menu_utils.remove_menu_items(self._menu_entries, "Window") + self._menu_entries = None + + # --- window ---------------------------------------------------------------- + + def _toggle_window(self, *_): + import omni.ui as ui + + if self._window is None: + self._window = ui.Window("NatNet Interface", width=400, height=600) + self._window.frame.set_build_fn(self._build_window) + # Dock bottom-right next to the Property panel, like Pegasus. + self._window.deferred_dock_in("Property", ui.DockPolicy.CURRENT_WINDOW_IS_ACTIVE) + self._window.visible = True + return + self._window.visible = not self._window.visible + + def _refresh(self, *_): + if self._window is not None: + self._window.frame.rebuild() + + def _build_window(self): + import omni.ui as ui + + with ui.ScrollingFrame(): + with ui.VStack(spacing=6, height=0): + ui.Label("NatNet interface", height=0, style={"font_size": 16}) + + with ui.HStack(height=28, spacing=6): + ui.Button("Create Interface", clicked_fn=self._create_server) + ui.Button("Save", clicked_fn=self._save) + ui.Button("Load from Stage", clicked_fn=self._load_from_stage) + ui.Button("Print config", clicked_fn=self._print_config) + + running = self._manager is not None and self._manager.is_running + with ui.HStack(height=28, spacing=6): + ui.Button( + "Stop Server" if running else "Start Server", + clicked_fn=self._toggle_server, + ) + ui.Label( + f"Server: {'RUNNING' if running else 'stopped'}", + width=0, + style={"color": 0xFF33CC33 if running else 0xFF888888}, + ) + + ui.Label( + "\u26a0 Remember to save after each edit", + height=0, + word_wrap=True, + style={"color": 0xFF33CCFF, "font_size": 14}, + ) + + ui.Label(self._status_text(), height=0, word_wrap=True) + + ui.Separator(height=6) + self._bool_row(ui, "Server enabled", "server_enabled", self._cfg.server_enabled) + self._bool_row(ui, "Pose noise enabled", "pose_noise_enabled", self._cfg.pose_noise_enabled) + self._float_row(ui, "Pose noise std meters", "pose_noise_std_meters", self._cfg.pose_noise_std_meters) + self._float_row(ui, "Pose noise rotation deg", "pose_noise_rotation_deg", self._cfg.pose_noise_rotation_deg) + self._str_row(ui, "Server IP", "server_ip", self._cfg.server_ip) + self._combo_row(ui, "Mode", "mode", self._cfg.mode, VALID_MODES) + self._int_row(ui, "Command port", "command_port", self._cfg.command_port) + self._int_row(ui, "Data port", "data_port", self._cfg.data_port) + self._float_row(ui, "Publish rate (Hz)", "publish_rate", self._cfg.publish_rate) + self._combo_row(ui, "Up axis", "up_axis", self._cfg.up_axis, VALID_UP_AXES) + + ui.Separator(height=6) + ui.Label("Tracked bodies", height=0, style={"font_size": 14}) + self._bodies_frame = ui.Frame(height=0) + self._bodies_frame.set_build_fn(self._build_bodies) + with ui.HStack(height=0, spacing=6): + ui.Button("Add body (from selection)", clicked_fn=self._add_body) + + def _status_text(self): + prim = self._find_interface() + if prim is None: + return "No prim on stage yet — Save or Create Server to author one." + return f"Prim on stage: {prim.GetPath().pathString} (Save to push edits, Load to pull)" + + # --- server field rows (edit the working copy only) ------------------------ + + def _bool_row(self, ui, label, key, value): + with ui.HStack(height=0): + ui.Label(label, width=_LABEL_WIDTH) + cb = ui.CheckBox() + cb.model.set_value(bool(value)) + cb.model.add_value_changed_fn( + lambda m, k=key: self._set_cfg_field(k, m.get_value_as_bool()) + ) + + def _str_row(self, ui, label, key, value): + with ui.HStack(height=0): + ui.Label(label, width=_LABEL_WIDTH) + model = ui.StringField().model + model.set_value(str(value)) + model.add_value_changed_fn( + lambda m, k=key: self._set_cfg_field(k, m.get_value_as_string()) + ) + + def _int_row(self, ui, label, key, value): + with ui.HStack(height=0): + ui.Label(label, width=_LABEL_WIDTH) + model = ui.IntField().model + model.set_value(int(value)) + model.add_value_changed_fn( + lambda m, k=key: self._set_cfg_field(k, m.get_value_as_int()) + ) + + def _float_row(self, ui, label, key, value): + with ui.HStack(height=0): + ui.Label(label, width=_LABEL_WIDTH) + model = ui.FloatField().model + model.set_value(float(value)) + model.add_value_changed_fn( + lambda m, k=key: self._set_cfg_field(k, m.get_value_as_float()) + ) + + def _combo_row(self, ui, label, key, value, choices): + with ui.HStack(height=0): + ui.Label(label, width=_LABEL_WIDTH) + index = choices.index(value) if value in choices else 0 + combo = ui.ComboBox(index, *choices) + combo.model.get_item_value_model().add_value_changed_fn( + lambda m, k=key, c=choices: self._set_cfg_field(k, c[m.get_value_as_int()]) + ) + + def _set_cfg_field(self, attr, value): + setattr(self._cfg, attr, value) + + # --- bodies ---------------------------------------------------------------- + + def _rebuild_bodies(self, *_): + if self._bodies_frame is not None: + self._bodies_frame.rebuild() + + def _build_bodies(self): + import omni.ui as ui + + self._row_readouts = {} + with ui.VStack(spacing=6, height=0): + if not self._cfg.bodies: + ui.Label(" (no bodies — select a prim and click Add body)", height=0) + return + with ui.HStack(height=0, spacing=4): + ui.Label("Rigid body name", width=ui.Fraction(1)) + ui.Label("ID", width=40) + ui.Label("Parent", width=50) + ui.Label("Target prim", width=ui.Fraction(2)) + ui.Spacer(width=98) + for idx, body in enumerate(self._cfg.bodies): + self._build_body_row(ui, idx, body) + + def _build_body_row(self, ui, idx, body): + with ui.VStack(height=0, spacing=2): + with ui.HStack(height=0, spacing=4): + name = ui.StringField(width=ui.Fraction(1)).model + name.set_value(body.rigid_body_name) + name.add_value_changed_fn( + lambda m, i=idx: self._set_body_field(i, "rigid_body_name", m.get_value_as_string()) + ) + + sid = ui.IntField(width=40).model + sid.set_value(body.streaming_id) + sid.add_value_changed_fn( + lambda m, i=idx: self._set_body_field(i, "streaming_id", m.get_value_as_int()) + ) + + parent = ui.IntField(width=50).model + parent.set_value(body.parent_id) + parent.add_value_changed_fn( + lambda m, i=idx: self._set_body_field(i, "parent_id", m.get_value_as_int()) + ) + + target = ui.StringField(width=ui.Fraction(2), tooltip="USD path of the tracked prim").model + target.set_value(body.target_prim) + target.add_value_changed_fn( + lambda m, i=idx: self._set_body_field(i, "target_prim", m.get_value_as_string()) + ) + + ui.Button("set target", width=70, clicked_fn=lambda i=idx: self._retarget_body(i)) + ui.Button("x", width=24, clicked_fn=lambda i=idx: self._remove_body_at(i)) + + # Live readout: status dot + world position pulled from the USD stage. + stage = self._get_stage() + running = self._manager is not None and self._manager.is_running + symbol, color, text = self._row_readout(stage, body.target_prim, running) + with ui.HStack(height=0, spacing=6): + ui.Spacer(width=4) + status_label = ui.Label(symbol, width=14, style={"color": color}) + ui.Label("pos:", width=30, style={"color": _COLOR_IDLE}) + pos_label = ui.Label(text, width=ui.Fraction(1), style={"color": color}) + self._row_readouts[idx] = (status_label, pos_label) + + def _set_body_field(self, index, attr, value): + if 0 <= index < len(self._cfg.bodies): + setattr(self._cfg.bodies[index], attr, value) + + def _add_body(self): + next_id = max((b.streaming_id for b in self._cfg.bodies), default=0) + 1 + target = self._selected_target_path(self._find_interface()) + name = target.rsplit("/", 1)[-1] if target else f"Body{next_id}" + existing = {b.rigid_body_name for b in self._cfg.bodies} + while name in existing: + name = f"{name}_{next_id}" + self._cfg.bodies.append(BodyBinding(rigid_body_name=name, target_prim=target, streaming_id=next_id)) + self._rebuild_bodies() + + def _remove_body_at(self, index): + if 0 <= index < len(self._cfg.bodies): + self._cfg.bodies.pop(index) + self._rebuild_bodies() + + def _retarget_body(self, index): + import carb + + path = self._selected_target_path(self._find_interface()) + if not path: + carb.log_warn("[natnet] Select a prim in the viewport to retarget this body.") + return + if 0 <= index < len(self._cfg.bodies): + self._cfg.bodies[index].target_prim = path + self._rebuild_bodies() + + # --- stage helpers --------------------------------------------------------- + + def _get_stage(self): + import omni.usd + + return omni.usd.get_context().get_stage() + + def _find_interface(self): + stage = self._get_stage() + if stage is None: + return None + interfaces = find_interfaces(stage) + return interfaces[0] if interfaces else None + + def _interface_path(self): + prim = self._find_interface() + return prim.GetPath().pathString if prim is not None else _DEFAULT_PRIM_PATH + + def _select(self, prim_path): + import omni.usd + + omni.usd.get_context().get_selection().set_selected_prim_paths([prim_path], True) + + def _selected_target_path(self, interface_prim): + import omni.usd + + sel = omni.usd.get_context().get_selection().get_selected_prim_paths() + iface_path = interface_prim.GetPath().pathString if interface_prim else None + for path in sel: + if path != iface_path: + return path + return "" + + # --- explicit sync actions ------------------------------------------------- + + def _save(self): + import carb + + stage = self._get_stage() + if stage is None: + carb.log_error("[natnet] No active stage.") + return + try: + self._cfg.validate() + except ValueError as exc: + carb.log_error(f"[natnet] Not saved: {exc}") + return + path = self._interface_path() + author_interface(stage, path, self._cfg) + carb.log_info(f"[natnet] Saved interface to {path} ({len(self._cfg.bodies)} bodies).") + self._refresh() + + def _load_from_stage(self): + import carb + + prim = self._find_interface() + if prim is None: + self._cfg = NatNetInterfaceConfig() + carb.log_warn("[natnet] No interface on stage — reset to defaults.") + else: + self._cfg = read_interface(prim) + carb.log_info(f"[natnet] Loaded interface from {prim.GetPath().pathString}.") + self._refresh() + + def _print_config(self): + # Print whatever is authored on the stage (the source of truth). + if self._manager is not None: + self._manager.scan_and_print() + + def _toggle_server(self): + # Start/stop the live server at the click of this button, regardless of the + # serverEnabled attribute. Builds from the prim that's actually on the stage. + import carb + + if self._manager is None: + return + if not self._manager.is_running: + if self._find_interface() is None: + carb.log_warn("[natnet] No interface on stage — Create/Save one first.") + return + try: + self._manager.start_from_stage() + except Exception as exc: # noqa: BLE001 - surface to the user + carb.log_error(f"[natnet] Could not start server: {exc}") + else: + self._manager.stop_server() + self._refresh() + + def _create_server(self): + import carb + + stage = self._get_stage() + if stage is None: + carb.log_error("[natnet] No active stage.") + return + prim = self._find_interface() + if prim is None: + try: + self._cfg.validate() + except ValueError as exc: + carb.log_error(f"[natnet] Cannot create: {exc}") + return + author_interface(stage, _DEFAULT_PRIM_PATH, self._cfg) + path = _DEFAULT_PRIM_PATH + carb.log_info(f"[natnet] Created interface prim at {path}. (Server start: later commit.)") + else: + path = prim.GetPath().pathString + carb.log_info(f"[natnet] Interface already exists at {path}. (Server start: later commit.)") + self._select(path) + self._refresh() diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/usd_bindings.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/usd_bindings.py new file mode 100644 index 000000000..870534252 --- /dev/null +++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/usd_bindings.py @@ -0,0 +1,222 @@ +# Copyright (c) 2024 Carnegie Mellon University +# MIT License - see LICENSE in the repository root for full text. +"""USD binding layer: author / read / find NatNet interface prims on a stage. + +``pxr`` is imported **lazily inside each function** so importing this module never +requires USD — that keeps ``optitrack.natnet.emulator.isaac`` importable in plain +(non-Isaac) test environments. The pure config logic lives in ``config.py``. + +Backing today is **plain namespaced custom attributes + relationships** (the +registration-free fallback from the design's Risk #1). Property names follow the +multi-apply schema convention (``natnet:body::``), so swapping to a +typed codeless applied schema later requires no change to readers/writers. +""" + +from __future__ import annotations + +from typing import Any + +from .config import ( + ATTR_COMMAND_PORT, + ATTR_DATA_PORT, + ATTR_MODE, + ATTR_MULTICAST_ADDR, + ATTR_NATNET_VERSION, + ATTR_POSE_NOISE_ENABLED, + ATTR_POSE_NOISE_ROTATION_DEG, + ATTR_POSE_NOISE_STD_METERS, + ATTR_PUBLISH_RATE, + ATTR_SERVER_ENABLED, + ATTR_SERVER_IP, + ATTR_UP_AXIS, + BODY_FIELD_PARENT_ID, + BODY_FIELD_RIGID_BODY_NAME, + BODY_FIELD_STREAMING_ID, + BODY_FIELD_TARGET, + BODY_PREFIX, + DEFAULT_COMMAND_PORT, + DEFAULT_DATA_PORT, + DEFAULT_MULTICAST_ADDR, + DEFAULT_NATNET_VERSION, + DEFAULT_POSE_NOISE_ENABLED, + DEFAULT_POSE_NOISE_ROTATION_DEG, + DEFAULT_POSE_NOISE_STD_METERS, + DEFAULT_PUBLISH_RATE, + DEFAULT_SERVER_IP, + DEFAULT_UP_AXIS, + MARKER_ATTR, + BodyBinding, + NatNetInterfaceConfig, + body_attr_name, +) + + +def author_interface(stage, prim_path: str, config: Any) -> Any: + """Create/overwrite a NatNet interface prim at ``prim_path`` from ``config``. + + ``config`` may be a :class:`NatNetInterfaceConfig` or a plain ``dict`` (passed + through ``from_dict``). Returns the ``Usd.Prim``. + """ + from pxr import Sdf + + cfg = config if isinstance(config, NatNetInterfaceConfig) else NatNetInterfaceConfig.from_dict(config) + cfg.validate() + + prim = stage.DefinePrim(prim_path, "Scope") + + # Overwrite semantics: drop any previously-authored body properties so removed + # bodies don't linger across re-authoring. + _clear_body_properties(prim) + + _set(prim, MARKER_ATTR, Sdf.ValueTypeNames.Bool, True) + _set(prim, ATTR_SERVER_ENABLED, Sdf.ValueTypeNames.Bool, cfg.server_enabled) + _set(prim, ATTR_SERVER_IP, Sdf.ValueTypeNames.String, cfg.server_ip) + _set(prim, ATTR_MODE, Sdf.ValueTypeNames.Token, cfg.mode) + _set(prim, ATTR_MULTICAST_ADDR, Sdf.ValueTypeNames.String, cfg.multicast_addr) + _set(prim, ATTR_COMMAND_PORT, Sdf.ValueTypeNames.Int, cfg.command_port) + _set(prim, ATTR_DATA_PORT, Sdf.ValueTypeNames.Int, cfg.data_port) + _set(prim, ATTR_PUBLISH_RATE, Sdf.ValueTypeNames.Float, cfg.publish_rate) + _set(prim, ATTR_NATNET_VERSION, Sdf.ValueTypeNames.String, cfg.natnet_version) + _set(prim, ATTR_UP_AXIS, Sdf.ValueTypeNames.Token, cfg.up_axis) + _set(prim, ATTR_POSE_NOISE_ENABLED, Sdf.ValueTypeNames.Bool, cfg.pose_noise_enabled) + _set(prim, ATTR_POSE_NOISE_STD_METERS, Sdf.ValueTypeNames.Float, cfg.pose_noise_std_meters) + _set(prim, ATTR_POSE_NOISE_ROTATION_DEG, Sdf.ValueTypeNames.Float, cfg.pose_noise_rotation_deg) + + for key, body in cfg.assign_instance_keys(): + _set(prim, body_attr_name(key, BODY_FIELD_RIGID_BODY_NAME), Sdf.ValueTypeNames.String, body.rigid_body_name) + _set(prim, body_attr_name(key, BODY_FIELD_STREAMING_ID), Sdf.ValueTypeNames.Int, body.streaming_id) + _set(prim, body_attr_name(key, BODY_FIELD_PARENT_ID), Sdf.ValueTypeNames.Int, body.parent_id) + rel = prim.CreateRelationship(body_attr_name(key, BODY_FIELD_TARGET), False) + # Empty target is allowed (e.g. a freshly added body to be pointed in the + # Property panel); leave the relationship target-less rather than authoring + # an invalid empty Sdf.Path. + rel.SetTargets([Sdf.Path(body.target_prim)] if body.target_prim else []) + + return prim + + +def read_interface(prim) -> NatNetInterfaceConfig: + """Reconstruct a :class:`NatNetInterfaceConfig` from an authored interface prim.""" + return NatNetInterfaceConfig( + server_enabled=bool(_get(prim, ATTR_SERVER_ENABLED, True)), + server_ip=str(_get(prim, ATTR_SERVER_IP, DEFAULT_SERVER_IP)), + mode=str(_get(prim, ATTR_MODE, "unicast")), + multicast_addr=str(_get(prim, ATTR_MULTICAST_ADDR, DEFAULT_MULTICAST_ADDR)), + command_port=int(_get(prim, ATTR_COMMAND_PORT, DEFAULT_COMMAND_PORT)), + data_port=int(_get(prim, ATTR_DATA_PORT, DEFAULT_DATA_PORT)), + publish_rate=float(_get(prim, ATTR_PUBLISH_RATE, DEFAULT_PUBLISH_RATE)), + natnet_version=str(_get(prim, ATTR_NATNET_VERSION, DEFAULT_NATNET_VERSION)), + up_axis=str(_get(prim, ATTR_UP_AXIS, DEFAULT_UP_AXIS)), + pose_noise_enabled=bool(_get(prim, ATTR_POSE_NOISE_ENABLED, DEFAULT_POSE_NOISE_ENABLED)), + pose_noise_std_meters=float(_get(prim, ATTR_POSE_NOISE_STD_METERS, DEFAULT_POSE_NOISE_STD_METERS)), + pose_noise_rotation_deg=float(_get(prim, ATTR_POSE_NOISE_ROTATION_DEG, DEFAULT_POSE_NOISE_ROTATION_DEG)), + bodies=_read_bodies(prim), + ) + + +def find_interfaces(stage) -> list: + """Return every prim on the stage marked as a NatNet interface.""" + interfaces = [] + for prim in stage.Traverse(): + attr = prim.GetAttribute(MARKER_ATTR) + if attr and attr.HasAuthoredValue() and bool(attr.Get()): + interfaces.append(prim) + return interfaces + + +def is_interface(prim) -> bool: + attr = prim.GetAttribute(MARKER_ATTR) + return bool(attr and attr.HasAuthoredValue() and bool(attr.Get())) + + +def read_world_pose(prim): + """Return ``((x, y, z), (qx, qy, qz, qw))`` from a prim's USD world transform. + + Reads the position/orientation **stored in the USD stage** (the local-to-world + transform), which is what the physics step writes back each frame. Returns + ``None`` for an invalid/non-xformable prim so callers can mark the body lost. + """ + from pxr import Usd, UsdGeom + + if prim is None or not prim.IsValid(): + return None + xformable = UsdGeom.Xformable(prim) + if not xformable: + return None + matrix = xformable.ComputeLocalToWorldTransform(Usd.TimeCode.Default()) + translation = matrix.ExtractTranslation() + quat = matrix.ExtractRotationQuat() # Gf.Quatd, normalized + imaginary = quat.GetImaginary() + position = (float(translation[0]), float(translation[1]), float(translation[2])) + orientation = ( + float(imaginary[0]), + float(imaginary[1]), + float(imaginary[2]), + float(quat.GetReal()), + ) + return position, orientation + + +def resolve_targets(stage, config): + """Split a config's bodies into (existing, missing) by target prim presence. + + A body whose ``target_prim`` is empty or points at a non-existent prim lands in + ``missing``. Returns two lists of :class:`BodyBinding`. + """ + existing = [] + missing = [] + for body in config.bodies: + prim = stage.GetPrimAtPath(body.target_prim) if body.target_prim else None + if prim is not None and prim.IsValid(): + existing.append(body) + else: + missing.append(body) + return existing, missing + + +# --- internal helpers ---------------------------------------------------------- + + +def _set(prim, name, type_name, value): + attr = prim.CreateAttribute(name, type_name) + attr.Set(value) + return attr + + +def _get(prim, name, default): + attr = prim.GetAttribute(name) + if attr and attr.HasAuthoredValue(): + return attr.Get() + return default + + +def _clear_body_properties(prim) -> None: + for name in list(prim.GetPropertyNames()): + if name.startswith(BODY_PREFIX): + prim.RemoveProperty(name) + + +def _read_bodies(prim) -> list[BodyBinding]: + suffix = f":{BODY_FIELD_RIGID_BODY_NAME}" + keys = [ + name[len(BODY_PREFIX): -len(suffix)] + for name in prim.GetPropertyNames() + if name.startswith(BODY_PREFIX) and name.endswith(suffix) + ] + + bodies: list[BodyBinding] = [] + for key in keys: + rel = prim.GetRelationship(body_attr_name(key, BODY_FIELD_TARGET)) + targets = rel.GetTargets() if rel else [] + bodies.append( + BodyBinding( + rigid_body_name=str(_get(prim, body_attr_name(key, BODY_FIELD_RIGID_BODY_NAME), "")), + target_prim=str(targets[0]) if targets else "", + streaming_id=int(_get(prim, body_attr_name(key, BODY_FIELD_STREAMING_ID), 1)), + parent_id=int(_get(prim, body_attr_name(key, BODY_FIELD_PARENT_ID), -1)), + ) + ) + + # Stable, deterministic order (independent of USD property iteration order). + bodies.sort(key=lambda b: (b.streaming_id, b.rigid_body_name)) + return bodies 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..53e342bd5 --- /dev/null +++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_data_types.py @@ -0,0 +1,263 @@ +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 rather than fixed 256 over network + 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 (PacketClient UnpackDataSize).""" + 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..1c1503b3a --- /dev/null +++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_server.py @@ -0,0 +1,345 @@ +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 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): + # We uniquely identify a client session by their IP and their unique command port. + # Why? A single machine could technically run multiple separate NatNet clients + # simultaneously, and they would share an IP but have unique command ports. + 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) pumps frames synchronously via ``pump_once`` — inside the Isaac + # Sim process the daemon send thread is starved by the render/physics main + # loop holding the GIL, so frames must be sent from the callback thread. + 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_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; frames + # arriving from the command port are treated as command traffic and dropped. + 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.") + + 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 so their source port matches the advertised data port. + """ + 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 (e.g. unicast will need to manage a list of connected clients and send packets directly to their IPs, while multicast will just send to the multicast group address) + # Loop to update mocap data and send packets at regular intervals (e.g. 100Hz) + pass + + def _send_data_packet(self, client: Client, data_message: DataMessages.sFrameOfMocapData): + # Serialize frame payload and send via the data socket (unicast libNatNet 4.4). + 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 (e.g. unicast will need to manage a list of connected clients and send packets directly to their IPs, while 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 (e.g. unicast will need to manage a list of connected clients and send packets directly to their IPs, while 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..f863ca087 --- /dev/null +++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_unicast_server.py @@ -0,0 +1,177 @@ +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.pump_once() + + def pump_once(self): + """Send the latest (or last) mocap frame to every connected client, once. + + Safe to call from any thread; the Isaac wrapper calls this from its + physics-step callback so frame delivery does not depend on the background + data thread getting scheduled inside the GIL-bound Isaac Sim process. + """ + 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: + data_messages = self._get_last_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): + # Keep-alive is client -> server only; real Motive sends no reply. + # Receiving it 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/schema/schema.usda b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/schema/schema.usda new file mode 100644 index 000000000..5f922469e --- /dev/null +++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/schema/schema.usda @@ -0,0 +1,100 @@ +#usda 1.0 +( + """ + NatNet emulator applied API schemas (CODELESS). + + Risk #1 spike (see docs/natnet_interface_prim_design.md): these schemas give the + interface prim typed, Property-panel-friendly attributes WITHOUT compiled + classes. They are codeless — `skipCodeGeneration = true` below — but still need + USD's plugin system to discover the generated registry. + + To produce the registry files (run once, in an env with USD tooling): + + usdGenSchema schema/schema.usda schema/ + + That emits `schema/generatedSchema.usda` and `schema/plugInfo.json`. The Kit + extension then registers the plugin dir on startup (Plug.Registry().RegisterPlugins). + + Until that registration is verified inside Kit, `optitrack.natnet.emulator.isaac` + authors the SAME attribute names as plain namespaced custom attributes (the + registration-free fallback), so nothing here is required for the facade to work. + """ + subLayers = [ + @usd/schema.usda@, + @usdGeom/schema.usda@ + ] +) +{ +} + +over "GLOBAL" ( + customData = { + bool skipCodeGeneration = true + string libraryName = "optitrackNatNet" + string libraryPath = "." + string libraryPrefix = "OptiTrackNatNet" + } +) +{ +} + +class "NatNetInterfaceAPI" ( + inherits = + customData = { + token apiSchemaType = "singleApply" + } + doc = "Marks a prim as a NatNet emulator interface and holds server-level config." +) +{ + bool natnet:isInterface = true ( + doc = "Discovery marker — find_interfaces() scans for prims with this set true." + ) + bool natnet:serverEnabled = true ( + doc = "When true the manager keeps a server running; toggling restarts it." + ) + string natnet:serverIp = "172.31.0.200" ( + doc = "Server interface IP (NatNetUnicastServer.local_interface)." + ) + token natnet:mode = "unicast" ( + allowedTokens = ["unicast", "multicast"] + doc = "Transmission mode." + ) + string natnet:multicastAddr = "239.255.42.99" ( + doc = "Multicast group (only used when mode = multicast)." + ) + int natnet:commandPort = 1510 ( + doc = "NatNet command port." + ) + int natnet:dataPort = 1511 ( + doc = "NatNet data port (frames stream from this source port)." + ) + float natnet:publishRate = 100 ( + doc = "Frame publish rate in Hz." + ) + string natnet:natnetVersion = "4.4.0.0" ( + doc = "Advertised NatNet protocol version." + ) +} + +class "NatNetBodyBindingAPI" ( + inherits = + customData = { + token apiSchemaType = "multipleApply" + token propertyNamespacePrefix = "natnet:body" + } + doc = "One tracked rigid body entry on a NatNet interface prim (apply once per body)." +) +{ + string natnet:body:__INSTANCE_NAME__:rigidBodyName = "" ( + doc = "Motive rigid body name (sRigidBodyDescription.szName)." + ) + int natnet:body:__INSTANCE_NAME__:streamingId = 1 ( + doc = "Streaming ID (sRigidBodyDescription.ID)." + ) + int natnet:body:__INSTANCE_NAME__:parentId = -1 ( + doc = "Parent rigid body ID (-1 if none)." + ) + rel natnet:body:__INSTANCE_NAME__:target ( + doc = "Tracked prim whose world pose is streamed for this body." + ) +} 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..4bf6d4f70 --- /dev/null +++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/setup.py @@ -0,0 +1,20 @@ +"""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=[], + packages=find_packages(where="."), + package_dir={"": "."}, + zip_safe=False, +) diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/conftest.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/conftest.py new file mode 100644 index 000000000..1e4eeec76 --- /dev/null +++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/conftest.py @@ -0,0 +1,26 @@ +# Copyright (c) 2024 Carnegie Mellon University +# MIT License - see LICENSE in the repository root for full text. +"""Make the emulator package importable when running these tests directly. + +The emulator is an Isaac Sim extension, not a pip-installed package, so a direct +``pytest test/`` (or ``colcon test``) needs the extension root on ``sys.path``. +pytest auto-loads this conftest before collecting any test in this directory, so +the test modules themselves stay free of ``sys.path`` boilerplate. (Runs via +``pytest tests/`` use the proxies under ``tests/`` instead, which set this up in +``tests/conftest.py``.) +""" + +import sys +from pathlib import Path + +_EXT_ROOT = Path(__file__).resolve().parents[1] +if str(_EXT_ROOT) not in sys.path: + sys.path.insert(0, str(_EXT_ROOT)) + + +def pytest_configure(config): + # Mirror the `unit` marker from tests/pytest.ini so direct `pytest test/` + # runs don't emit PytestUnknownMarkWarning and `-m unit` works here too. + config.addinivalue_line( + "markers", "unit: Fast hermetic unit test (no Docker stack)." + ) 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_catalog.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_catalog.py new file mode 100644 index 000000000..407618965 --- /dev/null +++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_catalog.py @@ -0,0 +1,115 @@ +# Copyright (c) 2024 Carnegie Mellon University +# MIT License - see LICENSE in the repository root for full text. +"""Commit 3 — hermetic catalog-builder tests (no USD, no Kit). + +Covers no / single / multiple bodies, field fidelity on the wire, name truncation, +the MAX_MODELS guard, and duplicate-target detection. +""" + +from __future__ import annotations + +import struct + +import pytest + +from optitrack.natnet.emulator.isaac.catalog import build_catalog, find_duplicate_targets +from optitrack.natnet.emulator.isaac.config import BodyBinding, NatNetInterfaceConfig +from optitrack.natnet.emulator.server import natnet_model_types as mt +from optitrack.natnet.emulator.server.natnet_common import ModelLimits + +pytestmark = pytest.mark.unit + + +def _unpack_bodies(payload: bytes): + """Decode a packed sDataDescriptions into [(name, id, parentID), ...].""" + (n,) = struct.unpack_from("frame builder tests (no USD, no Kit).""" + +from __future__ import annotations + +import math +import struct + +import pytest + +from optitrack.natnet.emulator.isaac.frames import ( + MODEL_LIST_CHANGED, + TRACKING_VALID, + BodySample, + apply_pose_noise, + build_frame, + make_rigid_body_data, + to_motive_pose, +) + +pytestmark = pytest.mark.unit + + +def test_to_motive_pose_z_is_identity(): + pos = (1.0, 2.0, 3.0) + quat = (0.1, 0.2, 0.3, 0.9) + assert to_motive_pose(pos, quat, up_axis="Z") == (pos, quat) + # Case-insensitive and default to Z. + assert to_motive_pose(pos, quat, up_axis="z") == (pos, quat) + assert to_motive_pose(pos, quat) == (pos, quat) + + +def test_to_motive_pose_y_swaps_axes_and_quat(): + # (x, y, z) -> (x, z, -y); quat vector part takes the same swap, scalar kept. + pos, quat = to_motive_pose((1.0, 2.0, 3.0), (0.1, 0.2, 0.3, 0.9), up_axis="Y") + assert pos == (1.0, 3.0, -2.0) + assert quat == (0.1, 0.3, -0.2, 0.9) + + +def test_to_motive_pose_y_maps_isaac_up_to_motive_up(): + # Isaac +Z (up) must become Motive +Y (up) under the Y-up emulation. + pos, _ = to_motive_pose((0.0, 0.0, 1.0), (0.0, 0.0, 0.0, 1.0), up_axis="y") + assert pos == (0.0, 1.0, 0.0) + + +def test_make_rigid_body_data_copies_pose_and_sets_valid_bit(): + rb = make_rigid_body_data( + BodySample(7, (1.0, 2.0, 3.0), (0.0, 0.0, 0.7071068, 0.7071068), valid=True) + ) + assert rb.ID == 7 + assert (rb.x, rb.y, rb.z) == (1.0, 2.0, 3.0) + assert rb.qw == pytest.approx(0.7071068) + assert rb.params & TRACKING_VALID # client requires this bit or it skips the body + + +def test_lost_sample_clears_valid_bit_and_is_nan(): + rb = make_rigid_body_data(BodySample.lost(3)) + assert rb.ID == 3 + assert rb.params & TRACKING_VALID == 0 + assert math.isnan(rb.x) and math.isnan(rb.y) and math.isnan(rb.z) + + +def test_build_frame_no_bodies(): + frame = build_frame(0, []) + assert frame.iFrame == 0 + assert frame.nRigidBodies == 0 + assert frame.params == 0 + + +def test_apply_pose_noise_zero_std_is_identity(): + position = (1.0, 2.0, 3.0) + orientation = (0.0, 0.0, 0.0, 1.0) + pos_out, quat_out = apply_pose_noise(position, orientation, 0.0, 0.0) + assert pos_out == position + assert quat_out == pytest.approx(orientation) + + +def test_apply_pose_noise_preserves_y_position(): + np = pytest.importorskip("numpy") + np.random.seed(0) + position = (0.0, 1.5, 0.0) + orientation = (0.0, 0.0, 0.0, 1.0) + pos_out, _ = apply_pose_noise(position, orientation, 0.001, 0.0) + # Before the euler-yaw shadowing bug, y collapsed to ~0 instead of staying near 1.5. + assert pos_out[1] == pytest.approx(1.5, abs=0.01) + + +def test_apply_pose_noise_adds_position_jitter(): + np = pytest.importorskip("numpy") + np.random.seed(1) + position = (0.0, 0.0, 0.0) + orientation = (0.0, 0.0, 0.0, 1.0) + pos_out, _ = apply_pose_noise(position, orientation, 0.001, 0.0) + assert pos_out != position + + +def test_build_frame_multiple_bodies_preserve_order(): + samples = [ + BodySample(1, (1.0, 0.0, 0.0)), + BodySample(2, (0.0, 2.0, 0.0)), + BodySample(5, (0.0, 0.0, 3.0)), + ] + frame = build_frame(42, samples) + assert frame.iFrame == 42 + assert frame.nRigidBodies == 3 + assert frame.RigidBodies[0].ID == 1 and frame.RigidBodies[0].x == 1.0 + assert frame.RigidBodies[1].ID == 2 and frame.RigidBodies[1].y == 2.0 + assert frame.RigidBodies[2].ID == 5 and frame.RigidBodies[2].z == 3.0 + + +def test_model_list_changed_sets_frame_param_bit(): + assert build_frame(0, [], model_list_changed=True).params & MODEL_LIST_CHANGED + assert build_frame(0, [], model_list_changed=False).params & MODEL_LIST_CHANGED == 0 + + +def test_frame_packs_and_rigid_body_section_decodes(): + frame = build_frame(9, [BodySample(4, (1.5, -2.5, 3.5), (0.0, 0.0, 0.0, 1.0))]) + payload = frame.pack(natnet_major=4, natnet_minor=4) + + # iFrame, then 4.4 counted sections (count+size each) for markersets & other markers. + (iframe,) = struct.unpack_from(" author -> read is stable + author_interface(stage, "/World/NatNetInterface", cfg) + assert read_interface(find_interfaces(stage)[0]) == cfg + + +def test_reauthoring_removes_stale_bodies(): + stage = _new_stage() + author_interface(stage, "/World/NatNetInterface", _CONFIG) + + single = NatNetInterfaceConfig.from_dict( + {"bodies": [{"rigid_body_name": "Drone", "target_prim": "/World/base_link", "streaming_id": 1}]} + ) + author_interface(stage, "/World/NatNetInterface", single) + + cfg = read_interface(find_interfaces(stage)[0]) + assert [b.rigid_body_name for b in cfg.bodies] == ["Drone"] + + +def test_up_axis_authors_and_reads_back(): + stage = _new_stage() + # Default (absent) -> Z. + author_interface(stage, "/World/NatNetInterface", _CONFIG) + assert read_interface(find_interfaces(stage)[0]).up_axis == "Z" + + # Explicit Y survives the USD round trip. + cfg = NatNetInterfaceConfig.from_dict({**_CONFIG, "up_axis": "Y"}) + author_interface(stage, "/World/NatNetInterface", cfg) + assert read_interface(find_interfaces(stage)[0]).up_axis == "Y" + + +def test_pose_noise_authors_and_reads_back(): + stage = _new_stage() + cfg = NatNetInterfaceConfig.from_dict( + { + **_CONFIG, + "pose_noise_enabled": False, + "pose_noise_std_meters": 0.001, + "pose_noise_rotation_deg": 0.1, + } + ) + author_interface(stage, "/World/NatNetInterface", cfg) + read = read_interface(find_interfaces(stage)[0]) + assert read.pose_noise_enabled is False + assert read.pose_noise_std_meters == pytest.approx(0.001) + assert read.pose_noise_rotation_deg == pytest.approx(0.1) + + +def test_empty_target_round_trips(): + # The UI's "Add body" can create a body with no target yet (set later in the + # Property panel); it must author and read back cleanly with an empty target. + stage = _new_stage() + cfg = NatNetInterfaceConfig(bodies=[BodyBinding("Drone", "", 1)]) + author_interface(stage, "/World/NatNetInterface", cfg) + read = read_interface(find_interfaces(stage)[0]) + assert read.bodies[0].rigid_body_name == "Drone" + assert read.bodies[0].target_prim == "" + + +def test_invalid_config_raises_before_authoring(): + stage = _new_stage() + with pytest.raises(ValueError): + author_interface(stage, "/World/NatNetInterface", {"mode": "bogus"}) + assert find_interfaces(stage) == [] diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_interface_config.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_interface_config.py new file mode 100644 index 000000000..9247c5f12 --- /dev/null +++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_interface_config.py @@ -0,0 +1,186 @@ +# Copyright (c) 2024 Carnegie Mellon University +# MIT License - see LICENSE in the repository root for full text. +"""Hermetic unit tests for the pure-Python NatNet interface config model. + +No USD / Isaac imports — exercises dataclasses, dict normalization, the attribute +name builder, instance-key generation, and validation. +""" + +from __future__ import annotations + +import pytest + +from optitrack.natnet.emulator.isaac.config import ( + DEFAULT_POSE_NOISE_ENABLED, + DEFAULT_POSE_NOISE_ROTATION_DEG, + DEFAULT_POSE_NOISE_STD_METERS, + BodyBinding, + NatNetInterfaceConfig, + body_attr_name, + make_instance_key, +) + +pytestmark = pytest.mark.unit + + +def test_defaults_match_server_expectations(): + cfg = NatNetInterfaceConfig() + assert cfg.server_enabled is True + assert cfg.server_ip == "172.31.0.200" + assert cfg.mode == "unicast" + assert cfg.command_port == 1510 + assert cfg.data_port == 1511 + assert cfg.up_axis == "Z" # Isaac/USD native; matches the reference Motive setup + assert cfg.pose_noise_enabled is DEFAULT_POSE_NOISE_ENABLED + assert cfg.pose_noise_std_meters == DEFAULT_POSE_NOISE_STD_METERS + assert cfg.pose_noise_rotation_deg == DEFAULT_POSE_NOISE_ROTATION_DEG + assert cfg.bodies == [] + + +def test_up_axis_from_dict_normalizes_case(): + assert NatNetInterfaceConfig.from_dict({"up_axis": "y"}).up_axis == "Y" + assert NatNetInterfaceConfig.from_dict({"up_axis": "z"}).up_axis == "Z" + # Absent -> default Z. + assert NatNetInterfaceConfig.from_dict({}).up_axis == "Z" + + +def test_up_axis_survives_round_trip(): + cfg = NatNetInterfaceConfig.from_dict({"up_axis": "Y"}) + assert NatNetInterfaceConfig.from_dict(cfg.to_dict()).up_axis == "Y" + + +def test_pose_noise_survives_round_trip(): + cfg = NatNetInterfaceConfig.from_dict( + { + "pose_noise_enabled": False, + "pose_noise_std_meters": 0.001, + "pose_noise_rotation_deg": 0.1, + } + ) + restored = NatNetInterfaceConfig.from_dict(cfg.to_dict()) + assert restored.pose_noise_enabled is False + assert restored.pose_noise_std_meters == 0.001 + assert restored.pose_noise_rotation_deg == 0.1 + + +def test_from_dict_with_bodies_as_list(): + cfg = NatNetInterfaceConfig.from_dict( + { + "server_ip": "10.0.0.5", + "bodies": [ + {"rigid_body_name": "Drone", "target_prim": "/World/base_link", "streaming_id": 1}, + ], + } + ) + assert cfg.server_ip == "10.0.0.5" + assert len(cfg.bodies) == 1 + assert cfg.bodies[0] == BodyBinding("Drone", "/World/base_link", 1, -1) + + +def test_from_dict_with_bodies_as_prim_mapping(): + # The "dictionary of prims -> rigid body names and stuff" form. + cfg = NatNetInterfaceConfig.from_dict( + { + "bodies": { + "/World/base_link": {"rigid_body_name": "Drone", "streaming_id": 1}, + "/World/target": {"rigid_body_name": "Target", "streaming_id": 2}, + } + } + ) + by_name = {b.rigid_body_name: b for b in cfg.bodies} + assert by_name["Drone"].target_prim == "/World/base_link" + assert by_name["Target"].target_prim == "/World/target" + assert by_name["Target"].streaming_id == 2 + + +def test_to_dict_round_trip(): + cfg = NatNetInterfaceConfig.from_dict( + { + "mode": "multicast", + "publish_rate": 120, + "bodies": [{"rigid_body_name": "Drone", "target_prim": "/World/base_link"}], + } + ) + restored = NatNetInterfaceConfig.from_dict(cfg.to_dict()) + assert restored == cfg + + +def test_body_from_dict_requires_target_and_name(): + with pytest.raises(ValueError): + BodyBinding.from_dict({"rigid_body_name": "Drone"}) # no target_prim + with pytest.raises(ValueError): + BodyBinding.from_dict({"target_prim": "/World/base_link"}) # no name + + +def test_body_attr_name_builder(): + assert body_attr_name("Drone", "streamingId") == "natnet:body:Drone:streamingId" + + +def test_make_instance_key_sanitizes_and_dedupes(): + used: set[str] = set() + assert make_instance_key("Drone 1", used) == "Drone_1" + # collision after sanitization -> numeric suffix + assert make_instance_key("Drone-1", used) == "Drone_1_1" + # leading digit gets a safe prefix + assert make_instance_key("3PO", used).startswith("b_") + + +def test_assign_instance_keys_are_unique(): + cfg = NatNetInterfaceConfig( + bodies=[ + BodyBinding("Drone", "/World/a", 1), + BodyBinding("Drone", "/World/b", 2), # duplicate display name + ] + ) + keys = [k for k, _ in cfg.assign_instance_keys()] + assert len(set(keys)) == 2 + + +@pytest.mark.parametrize( + "overrides", + [ + {"mode": "bogus"}, + {"command_port": 0}, + {"data_port": 70000}, + {"command_port": 1510, "data_port": 1510}, + {"publish_rate": 0}, + {"up_axis": "X"}, + {"up_axis": "bogus"}, + {"pose_noise_std_meters": -0.001}, + {"pose_noise_rotation_deg": -0.1}, + ], +) +def test_validate_rejects_bad_server_config(overrides): + cfg = NatNetInterfaceConfig(**overrides) + with pytest.raises(ValueError): + cfg.validate() + + +def test_validate_rejects_duplicate_streaming_ids(): + cfg = NatNetInterfaceConfig( + bodies=[ + BodyBinding("A", "/World/a", 1), + BodyBinding("B", "/World/b", 1), + ] + ) + with pytest.raises(ValueError): + cfg.validate() + + +def test_validate_rejects_blank_rigid_body_name(): + cfg = NatNetInterfaceConfig(bodies=[BodyBinding("", "/World/a", 1)]) + with pytest.raises(ValueError): + cfg.validate() + + +def test_validate_allows_empty_target(): + # An empty target is valid: a freshly added body to be pointed in the UI/Property panel. + cfg = NatNetInterfaceConfig(bodies=[BodyBinding("Drone", "", 1)]) + assert cfg.validate() is cfg + + +def test_validate_accepts_good_config(): + cfg = NatNetInterfaceConfig( + bodies=[BodyBinding("Drone", "/World/base_link", 1)] + ) + assert cfg.validate() is cfg diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_pose_sampling.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_pose_sampling.py new file mode 100644 index 000000000..fa13a7eb2 --- /dev/null +++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_pose_sampling.py @@ -0,0 +1,231 @@ +# Copyright (c) 2024 Carnegie Mellon University +# MIT License - see LICENSE in the repository root for full text. +"""Pose sampling + dynamic catalog (dirty/resync) tests against an in-memory stage. + +Guarded by ``importorskip("pxr")``. Uses a fake server (records enqueued frames and +MODELDEF payloads) so no sockets are bound — we exercise the manager's sampling and +resync logic, not the wire. +""" + +from __future__ import annotations + +import math + +import pytest + +pytest.importorskip("pxr") + +from pxr import Gf, Usd, UsdGeom # noqa: E402 + +from optitrack.natnet.emulator.isaac.config import BodyBinding, NatNetInterfaceConfig # noqa: E402 +from optitrack.natnet.emulator.isaac.frames import MODEL_LIST_CHANGED, TRACKING_VALID # noqa: E402 +from optitrack.natnet.emulator.isaac.manager import NatNetServerManager # noqa: E402 +from optitrack.natnet.emulator.isaac.usd_bindings import author_interface, read_world_pose # noqa: E402 + +pytestmark = pytest.mark.unit + + +class FakeServer: + def __init__(self): + self.frames = [] + self.payloads = [] + + def set_model_def_payload(self, payload): + self.payloads.append(payload) + + def start(self): + pass + + def shutdown(self): + pass + + def enqueue_mocap_data(self, frame): + self.frames.append(frame) + + +def _xform(stage, path, translate=(0.0, 0.0, 0.0)): + xform = UsdGeom.Xform.Define(stage, path) + xform.AddTranslateOp().Set(Gf.Vec3d(*translate)) + return xform + + +def _manager_with_fake(): + fake = FakeServer() + mgr = NatNetServerManager(server_factory=lambda cfg: fake) + return mgr, fake + + +# --- read_world_pose ------------------------------------------------------------- + + +def test_read_world_pose_returns_translation(): + stage = Usd.Stage.CreateInMemory() + _xform(stage, "/World/base_link", translate=(1.0, 2.0, 3.0)) + pose = read_world_pose(stage.GetPrimAtPath("/World/base_link")) + assert pose is not None + (x, y, z), (qx, qy, qz, qw) = pose + assert (round(x, 3), round(y, 3), round(z, 3)) == (1.0, 2.0, 3.0) + assert qw == pytest.approx(1.0) + + +def test_read_world_pose_invalid_prim_is_none(): + stage = Usd.Stage.CreateInMemory() + assert read_world_pose(stage.GetPrimAtPath("/World/nope")) is None + + +# --- sample_once ----------------------------------------------------------------- + + +def test_sample_once_no_bodies(): + stage = Usd.Stage.CreateInMemory() + author_interface(stage, "/World/NatNetInterface", NatNetInterfaceConfig()) + mgr, fake = _manager_with_fake() + mgr.start_server(NatNetInterfaceConfig(server_ip="127.0.0.1")) + frame = mgr.sample_once(stage) + assert frame is not None and frame.nRigidBodies == 0 + + +def test_sample_once_streams_world_pose(): + stage = Usd.Stage.CreateInMemory() + _xform(stage, "/World/base_link", translate=(4.0, 5.0, 6.0)) + cfg = NatNetInterfaceConfig( + server_ip="127.0.0.1", bodies=[BodyBinding("Drone", "/World/base_link", 1)] + ) + author_interface(stage, "/World/NatNetInterface", cfg) + mgr, fake = _manager_with_fake() + mgr.start_server(cfg) + + frame = mgr.sample_once(stage) + assert frame.nRigidBodies == 1 + rb = frame.RigidBodies[0] + assert rb.ID == 1 + assert (round(rb.x, 3), round(rb.y, 3), round(rb.z, 3)) == (4.0, 5.0, 6.0) + assert rb.params & TRACKING_VALID + # First frame after start resyncs -> client should be told the model list changed. + assert frame.params & MODEL_LIST_CHANGED + + +def test_sample_once_missing_prim_is_lost(): + stage = Usd.Stage.CreateInMemory() + cfg = NatNetInterfaceConfig( + server_ip="127.0.0.1", bodies=[BodyBinding("Ghost", "/World/missing", 9)] + ) + author_interface(stage, "/World/NatNetInterface", cfg) + mgr, fake = _manager_with_fake() + mgr.start_server(cfg) + + frame = mgr.sample_once(stage) + rb = frame.RigidBodies[0] + assert rb.ID == 9 + assert rb.params & TRACKING_VALID == 0 + assert math.isnan(rb.x) + + +def test_moving_prim_updates_streamed_position(): + stage = Usd.Stage.CreateInMemory() + xform = _xform(stage, "/World/base_link", translate=(0.0, 0.0, 0.0)) + cfg = NatNetInterfaceConfig( + server_ip="127.0.0.1", bodies=[BodyBinding("Drone", "/World/base_link", 1)] + ) + author_interface(stage, "/World/NatNetInterface", cfg) + mgr, fake = _manager_with_fake() + mgr.start_server(cfg) + + mgr.sample_once(stage) + xform.GetOrderedXformOps()[0].Set(Gf.Vec3d(10.0, 0.0, 0.0)) + frame = mgr.sample_once(stage) + assert round(frame.RigidBodies[0].x, 3) == 10.0 + + +def test_up_axis_z_streams_isaac_pose_as_is(): + stage = Usd.Stage.CreateInMemory() + _xform(stage, "/World/base_link", translate=(1.0, 2.0, 3.0)) + cfg = NatNetInterfaceConfig( + server_ip="127.0.0.1", up_axis="Z", bodies=[BodyBinding("Drone", "/World/base_link", 1)] + ) + author_interface(stage, "/World/NatNetInterface", cfg) + mgr, _fake = _manager_with_fake() + mgr.start_server(cfg) + + rb = mgr.sample_once(stage).RigidBodies[0] + assert (round(rb.x, 3), round(rb.y, 3), round(rb.z, 3)) == (1.0, 2.0, 3.0) + + +def test_up_axis_y_reaxes_streamed_pose(): + # Y-up Motive emulation: Isaac (x, y, z) streams as (x, z, -y). + stage = Usd.Stage.CreateInMemory() + _xform(stage, "/World/base_link", translate=(1.0, 2.0, 3.0)) + cfg = NatNetInterfaceConfig( + server_ip="127.0.0.1", up_axis="Y", bodies=[BodyBinding("Drone", "/World/base_link", 1)] + ) + author_interface(stage, "/World/NatNetInterface", cfg) + mgr, _fake = _manager_with_fake() + mgr.start_server(cfg) + + rb = mgr.sample_once(stage).RigidBodies[0] + assert (round(rb.x, 3), round(rb.y, 3), round(rb.z, 3)) == (1.0, 3.0, -2.0) + + +def test_body_added_while_live_is_picked_up_on_resync(): + stage = Usd.Stage.CreateInMemory() + _xform(stage, "/World/a", translate=(1.0, 0.0, 0.0)) + _xform(stage, "/World/b", translate=(0.0, 2.0, 0.0)) + cfg1 = NatNetInterfaceConfig( + server_ip="127.0.0.1", bodies=[BodyBinding("A", "/World/a", 1)] + ) + author_interface(stage, "/World/NatNetInterface", cfg1) + mgr, fake = _manager_with_fake() + mgr.start_server(cfg1) + + first = mgr.sample_once(stage) + assert first.nRigidBodies == 1 + + # Add a second body live: re-author the prim, then mark dirty (the UI/USD-notice + # path calls mark_dirty for us in Kit). + cfg2 = NatNetInterfaceConfig( + server_ip="127.0.0.1", + bodies=[BodyBinding("A", "/World/a", 1), BodyBinding("B", "/World/b", 2)], + ) + author_interface(stage, "/World/NatNetInterface", cfg2) + mgr.mark_dirty() + + second = mgr.sample_once(stage) + assert second.nRigidBodies == 2 + assert second.params & MODEL_LIST_CHANGED # catalog grew -> tell the client + ids = {second.RigidBodies[i].ID for i in range(second.nRigidBodies)} + assert ids == {1, 2} + # MODELDEF payload was refreshed on the server for the new catalog. + assert len(fake.payloads) >= 2 + + +def test_target_prim_created_after_start_becomes_valid(): + """A body whose target prim is spawned *after* the server starts (e.g. a Pegasus + drone base_link created on the first Play tick) must start streaming a valid pose + as soon as the prim appears — no mark_dirty/resync required, because the target + path is re-resolved every sample.""" + stage = Usd.Stage.CreateInMemory() + cfg = NatNetInterfaceConfig( + server_ip="127.0.0.1", bodies=[BodyBinding("Drone", "/World/drone1/base_link", 1)] + ) + author_interface(stage, "/World/NatNetInterface", cfg) + mgr, _fake = _manager_with_fake() + mgr.start_server(cfg) + + # Prim does not exist yet -> lost. + first = mgr.sample_once(stage) + assert first.RigidBodies[0].params & TRACKING_VALID == 0 + + # Spawn the target prim later (simulating the Play-tick drone creation). + _xform(stage, "/World/drone1/base_link", translate=(7.0, 8.0, 9.0)) + + # Next sample re-resolves the path -> valid pose, with no mark_dirty(). + second = mgr.sample_once(stage) + rb = second.RigidBodies[0] + assert rb.params & TRACKING_VALID + assert (round(rb.x, 3), round(rb.y, 3), round(rb.z, 3)) == (7.0, 8.0, 9.0) + + +def test_sample_once_noop_without_server(): + stage = Usd.Stage.CreateInMemory() + mgr, _fake = _manager_with_fake() + assert mgr.sample_once(stage) is None diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_pose_streaming.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_pose_streaming.py new file mode 100644 index 000000000..84e2f9896 --- /dev/null +++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_pose_streaming.py @@ -0,0 +1,87 @@ +# Copyright (c) 2024 Carnegie Mellon University +# MIT License - see LICENSE in the repository root for full text. +"""End-to-end: manager samples a USD prim and streams its pose over the wire. + +Real server + real UDP sockets (loopback) + an in-memory USD stage. Proves the full +data-enqueue path: ``sample_once`` reads the prim's world pose, builds a frame, the +server relays it as ``NAT_FRAMEOFDATA``, and the bytes carry the sampled position. +Guarded by ``importorskip("pxr")``. +""" + +from __future__ import annotations + +import socket +import struct +import time + +import pytest + +pytest.importorskip("pxr") + +from pxr import Gf, Usd, UsdGeom # noqa: E402 + +from natnet_test_helpers import NatNetTestClient, ephemeral_udp_port # noqa: E402 + +from optitrack.natnet.emulator.isaac.config import BodyBinding, NatNetInterfaceConfig # noqa: E402 +from optitrack.natnet.emulator.isaac.manager import NatNetServerManager # noqa: E402 +from optitrack.natnet.emulator.isaac.usd_bindings import author_interface # noqa: E402 +from optitrack.natnet.emulator.server import natnet_server_types as st # noqa: E402 + +pytestmark = pytest.mark.unit + + +def _decode_first_rigid_body(payload: bytes): + # iFrame(4) + markersets(count4+size4) + othermarkers(count4+size4) = 20, then + # rigid bodies: count(4) + size(4) at 20, first body at 28: id + xyz. + rb_count, _rb_size = struct.unpack_from(" 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/simulation/isaac-sim/launch_scripts/example_multi_px4_pegasus_launch_script.py b/simulation/isaac-sim/launch_scripts/example_multi_px4_pegasus_launch_script.py index 0c9c7b082..7baca6ee3 100644 --- a/simulation/isaac-sim/launch_scripts/example_multi_px4_pegasus_launch_script.py +++ b/simulation/isaac-sim/launch_scripts/example_multi_px4_pegasus_launch_script.py @@ -39,6 +39,10 @@ sys.path.insert(0, os.path.normpath(os.path.join(os.path.dirname(os.path.abspath(__file__)), "..", "utils"))) from scene_prep import scale_stage_prim, add_colliders, add_dome_light, save_scene_as_contained_usd +# Make the OptiTrack NatNet emulator package importable without enabling the Kit +# UI extension (keeps a single, script-owned server manager). +sys.path.insert(0, os.path.normpath(os.path.join(os.path.dirname(os.path.abspath(__file__)), "..", "extensions", "optitrack.natnet.emulator"))) + # --------------------- CONFIGURATION --------------------- ENV_URL = SIMULATION_ENVIRONMENTS["Default Environment"] @@ -48,6 +52,14 @@ NUM_ROBOTS = int(os.environ.get("NUM_ROBOTS", "1")) ENABLE_LIDAR = os.environ.get("ENABLE_LIDAR", "false").lower() == "true" + +# OptiTrack/NatNet emulator: when LAUNCH_NATNET=true, author a NatNet interface +# prim with every drone's base_link as a tracked rigid body and start the server on +# load. Mirrors the robot-side LAUNCH_NATNET gate. Each drone i is streamed as +# "" (streaming id i); with the default natnet_config.yaml +# (body_id=-1) each robot publishes all bodies, so narrow per robot with body_id. +LAUNCH_NATNET = os.environ.get("LAUNCH_NATNET", "false").lower() in ("1", "true", "yes", "on") +NATNET_BODY_NAME = os.environ.get("NATNET_BODY_NAME", "Drone") # --------------------------------------------------------- @@ -172,8 +184,45 @@ def __init__(self): for i in range(1, NUM_ROBOTS + 1): spawn_drone(i) + # ----- OptiTrack / NatNet emulator ----- + # Author a NatNet interface prim with every drone's base_link as a tracked + # rigid body and start the Motive-compatible server (gated on LAUNCH_NATNET). + self.natnet_manager = None + if LAUNCH_NATNET: + self._setup_natnet(stage) + self.play_on_start = os.environ.get("PLAY_SIM_ON_START", "true").lower() == "true" + def _setup_natnet(self, stage): + """Author the NatNet interface prim (one body per drone) and start the server.""" + try: + from optitrack.natnet.emulator.isaac import start_drone_natnet_server + + # Single agent: stream the bare body name ("Drone") so it matches the + # robot natnet_config body_name, the MAVROS vision-pose bridge, and the + # liveliness sentinel. Multi-drone uses unique indexed names (future: + # narrow each robot to its own body via body_id). + def _body_name(i: int) -> str: + return NATNET_BODY_NAME if NUM_ROBOTS == 1 else f"{NATNET_BODY_NAME}{i}" + + drones = [ + (_body_name(i), i, f"/World/drone{i}/base_link/body") + for i in range(1, NUM_ROBOTS + 1) + ] + self.natnet_manager = start_drone_natnet_server( + stage, + drones, + **{ + "pose_noise_enabled": True, + "pose_noise_std_meters": 0.0005, # 0.5 mm so that 95% of the time the noise is less than 1 mm + "pose_noise_rotation_deg": 0.05, # 0.05 deg so that 95% of the time the noise is less than 0.1 deg + } + ) + carb.log_warn(f"[natnet] Emulator started with {len(drones)} body(ies).") + except Exception as exc: # noqa: BLE001 - never let NatNet kill the sim + carb.log_error(f"[natnet] Failed to start emulator: {exc}") + self.natnet_manager = None + def run(self): if self.play_on_start: self.timeline.play() @@ -192,6 +241,8 @@ def run(self): app.update() carb.log_warn("Closing simulation.") + if self.natnet_manager is not None: + self.natnet_manager.on_shutdown() self.timeline.stop() simulation_app.close() diff --git a/simulation/isaac-sim/launch_scripts/example_one_px4_pegasus_launch_script.py b/simulation/isaac-sim/launch_scripts/example_one_px4_pegasus_launch_script.py index 11819fc2f..fadbc0682 100755 --- a/simulation/isaac-sim/launch_scripts/example_one_px4_pegasus_launch_script.py +++ b/simulation/isaac-sim/launch_scripts/example_one_px4_pegasus_launch_script.py @@ -97,11 +97,22 @@ sys.path.insert(0, os.path.normpath(os.path.join(os.path.dirname(os.path.abspath(__file__)), "..", "utils"))) from scene_prep import scale_stage_prim, add_colliders, add_dome_light, save_scene_as_contained_usd +# Make the OptiTrack NatNet emulator package importable without enabling the Kit +# UI extension (keeps a single, script-owned server manager). +sys.path.insert(0, os.path.normpath(os.path.join(os.path.dirname(os.path.abspath(__file__)), "..", "extensions", "optitrack.natnet.emulator"))) + # --------------------- CONFIGURATION --------------------- # Environment to load. Swap this URL/key for any other scene. ENV_URL = SIMULATION_ENVIRONMENTS["Default Environment"] +# OptiTrack/NatNet emulator: when LAUNCH_NATNET=true, author a NatNet interface +# prim with the drone's base_link as a tracked rigid body and start the server on +# load. Mirrors the robot-side LAUNCH_NATNET gate (natnet_ros2 client). The body +# name defaults to "Drone" to match natnet_config.yaml / the liveliness sentinel. +LAUNCH_NATNET = os.environ.get("LAUNCH_NATNET", "false").lower() in ("1", "true", "yes", "on") +NATNET_BODY_NAME = os.environ.get("NATNET_BODY_NAME", "Drone") + # Scale applied to /World/stage. 0.01 converts cm→m for Nucleus assets. # Set to 1.0 if the environment is already in meters. STAGE_SCALE = 1.0 @@ -244,8 +255,37 @@ def __init__(self): min_range=0.75, ) + # ----- OptiTrack / NatNet emulator ----- + # Author a NatNet interface prim with the drone's base_link as a tracked + # rigid body and start the Motive-compatible server (gated on LAUNCH_NATNET). + self.natnet_manager = None + if LAUNCH_NATNET: + self._setup_natnet(stage) + self.play_on_start = os.environ.get("PLAY_SIM_ON_START", "true").lower() == "true" + def _setup_natnet(self, stage): + """Author the NatNet interface prim and start the emulator server.""" + try: + from optitrack.natnet.emulator.isaac import start_drone_natnet_server + + drones = [(NATNET_BODY_NAME, 1, "/World/base_link/body")] + self.natnet_manager = start_drone_natnet_server( + stage, + drones, + **{ + "pose_noise_enabled": True, + "pose_noise_std_meters": 0.0005, # 0.5 mm so that 95% of the time the noise is less than 1 mm + "pose_noise_rotation_deg": 0.05, # 0.05 deg so that 95% of the time the noise is less than 0.1 deg + } + ) + carb.log_warn( + f"[natnet] Emulator started with 1 body ('{NATNET_BODY_NAME}' -> /World/base_link)." + ) + except Exception as exc: # noqa: BLE001 - never let NatNet kill the sim + carb.log_error(f"[natnet] Failed to start emulator: {exc}") + self.natnet_manager = None + def run(self): if self.play_on_start: @@ -267,6 +307,8 @@ def run(self): app.update() carb.log_warn("Closing simulation.") + if self.natnet_manager is not None: + self.natnet_manager.on_shutdown() self.timeline.stop() simulation_app.close() diff --git a/tests/README.md b/tests/README.md index f10942ff7..e8c5b1e89 100644 --- a/tests/README.md +++ b/tests/README.md @@ -1,10 +1,11 @@ # Testing (`tests/`) -AirStack's **pytest** tree under `tests/` has three roles: +AirStack's **pytest** tree under `tests/` has four roles: 1. **`tests/system/`** — Docker stack tests (sim + robot + GCS): liveliness, sensor Hz, takeoff/hover/land, image/workspace builds. -2. **`tests/robot/`** — Fast **unit** tests that mirror `robot/ros_ws/src/` (`behavior`, `global`, `interface`, `local`, `perception`, `sensors`). Mark: `unit`. -3. **`tests/sim/`** — Unit tests for simulation-side helpers (e.g. Motive / NatNet emulator). Mark: `unit`. +2. **`tests/integration/`** — Cross-component tests that need the robot container plus a host-side component, but **no sim/GPU** (e.g. the NatNet emulator). Mark: `integration`. +3. **`tests/robot/`** — Fast **unit** tests that mirror `robot/ros_ws/src/` (`behavior`, `global`, `interface`, `local`, `perception`, `sensors`). Mark: `unit`. +4. **`tests/sim/`** — Unit tests for simulation-side helpers (e.g. NatNet emulator). Mark: `unit`. Shared fixtures live in `tests/conftest.py`. Use `airstack test -m unit -v` for hermetic tests only, or the marks below for the full stack. @@ -24,6 +25,27 @@ Shared fixtures live in `tests/conftest.py`. Use `airstack test -m unit -v` for | [`system/test_sensors.py`](system/test_sensors.py) | `sensors` | After liveliness in collection order: sim + robot stereo/depth Hz (**Isaac:** batched ``ros2 topic hz`` to avoid bridge overload; **ms-airsim:** single batch), filtered LiDAR via ``echo --once`` + cloud sanity (isaacsim), sim RTF, ``test_sensor_streams_stable`` | Docker daemon, GPU, sim license | | [`system/test_takeoff_hover_land.py`](system/test_takeoff_hover_land.py) | `takeoff_hover_land` | End-to-end flight: PX4 readiness gate, takeoff to 10 m, hover stability, land — one chain per (sim, num_robots, iteration, velocity) | Docker daemon, GPU, sim license | +### Integration tests (`tests/integration/`) + +The **integration** tier wires a few real components together — typically the +robot autonomy container plus a host-side component — without a simulator or GPU. +See [`integration/README.md`](integration/README.md). + +| Scenario | Mark(s) | What it tests | Hardware required | +|----------|---------|---------------|-------------------| +| [`integration/natnet/`](integration/natnet/) | `integration`, `natnet` | Host NatNet emulator → `natnet_ros2` → `/{ROBOT_NAME}/perception/optitrack/Drone` Hz | Docker daemon (no GPU/sim) | + +The shared `robot_autonomy_stack` fixture (in [`conftest.py`](conftest.py)) +reuses a running `robot-desktop` container, or brings one up only when +`--run-integration` is passed (otherwise the test skips). So a plain +`pytest tests/` never spins up Docker for this tier. + +```bash +# Reuse a running container, or bring one up on demand: +pytest tests/integration/ -m integration -v # reuse-or-skip +pytest tests/integration/ -m integration --run-integration -v # bring up / tear down +``` + ### Unit tests (`tests/robot/`, `tests/sim/`) Hermetic tests use `@pytest.mark.unit` (see [`pytest.ini`](pytest.ini)). @@ -217,6 +239,7 @@ pytest tests/ -m sensors \ | `--stable-interval` | `10` | Seconds between polls in those stability tests | | `--gui` | off | Show simulator GUI (disables headless mode) | | `--takeoff-velocities` | `0.5,1,2` | Takeoff/land speeds in m/s | +| `--run-integration` | off | Let `tests/integration/` tests bring up the robot container themselves (else reuse-or-skip) | --- diff --git a/tests/conftest.py b/tests/conftest.py index 31fd29076..d7ff96f4c 100644 --- a/tests/conftest.py +++ b/tests/conftest.py @@ -1,3 +1,4 @@ +import importlib.util import json import logging import os @@ -51,6 +52,43 @@ ) +def repo_path(*parts: str) -> Path: + """Resolve a path relative to the repository root. + + ``AIRSTACK_ROOT`` is exported by CI and defaults to the repo root locally, + so this is the single source of truth for cross-tree paths — no test or + proxy file should hardcode ``Path(__file__).parents[N]`` walks. + """ + return Path(AIRSTACK_ROOT).joinpath(*parts) + + +def reexport_unit_tests(target_globals: dict, test_dir: Path, *module_files: str) -> None: + """Exec co-located unit-test modules and re-export their ``test_*`` callables. + + Lets the thin proxy files under ``tests/`` expose package-co-located unit + tests to ``pytest tests/`` (CI) and ``airstack test -m unit`` without + per-file ``sys.path`` boilerplate. The package root (``test_dir.parent``) + and the test dir are placed on ``sys.path`` so the exec'd modules can import + the package under test and any sibling helper modules. Every re-exported + test is tagged ``unit`` so selection works regardless of whether the source + used a module-level ``pytestmark`` or per-function marks. + """ + for path in (test_dir, test_dir.parent): + entry = str(path) + if entry not in sys.path: + sys.path.insert(0, entry) + prefix = re.sub(r"\W", "_", test_dir.parent.name) + for module_file in module_files: + real_file = test_dir / module_file + module_name = f"_unit_{prefix}_{Path(module_file).stem}" + spec = importlib.util.spec_from_file_location(module_name, real_file) + module = importlib.util.module_from_spec(spec) + spec.loader.exec_module(module) + for name in dir(module): + if name.startswith("test_"): + target_globals[name] = pytest.mark.unit(getattr(module, name)) + + def load_colcon_unit_test_config(workspace="robot"): """Load colcon test package list and pytest args from tests/colcon_unit_test_packages.yaml.""" if not COLCON_UNIT_TEST_PACKAGES_YAML.is_file(): @@ -122,15 +160,32 @@ def pytest_addoption(parser): parser.addoption("--takeoff-velocities", default="0.5", help="Comma-separated takeoff/land velocities (m/s) to " "sweep in test_takeoff_hover_land. Default: 0.5,1,2") + parser.addoption("--run-integration", action="store_true", default=False, + help="Let integration tests (tests/integration/) bring up " + "the robot container themselves. Without it they reuse " + "an already-running container or skip. Keeps a plain " + "`pytest tests/` from spinning up Docker.") + + +def _chmod_world_writable(path: Path) -> None: + """Best-effort: allow Docker and host users to share tests/results.""" + try: + path.chmod(0o777) + except OSError: + pass def pytest_configure(config): global RUN_DIR, LOGS_DIR timestamp = datetime.now().strftime("%Y-%m-%d_%H-%M-%S") results_root = Path(AIRSTACK_ROOT) / "tests" / "results" + results_root.mkdir(parents=True, exist_ok=True) + _chmod_world_writable(results_root) RUN_DIR = results_root / timestamp LOGS_DIR = RUN_DIR / "logs" LOGS_DIR.mkdir(parents=True, exist_ok=True) + _chmod_world_writable(RUN_DIR) + _chmod_world_writable(LOGS_DIR) config.option.xmlpath = str(RUN_DIR / "results.xml") @@ -802,4 +857,57 @@ def airstack_env(request): airstack_cmd("down", timeout=120, log_name=log) down_duration_s = round(time.time() - t3, 2) logger.info("Teardown finished in %.2fs", down_duration_s) - m.record(tid, "airstack_down_duration_s", down_duration_s, unit="s") \ No newline at end of file + m.record(tid, "airstack_down_duration_s", down_duration_s, unit="s") + + +# ── integration tier (tests/integration/) ───────────────────────────────── + +_INTEGRATION_ROBOT_PATTERN = "robot.*desktop" +# Robot-only bring-up: autonomy stack on, no sim profile, single robot. +_INTEGRATION_ENV = { + "AUTOLAUNCH": "true", + "NUM_ROBOTS": "1", + "COMPOSE_PROFILES": "desktop", +} + + +@pytest.fixture(scope="module") +def robot_autonomy_stack(request): + """Robot-desktop container for integration tests (no sim, no GPU). + + Yields ``{"container": , "brought_up": bool}``. Reuses an already + running container (fast local iteration, left running afterward); otherwise + runs ``airstack up robot-desktop`` only when ``--run-integration`` is passed + (and tears it down). Without the flag and with nothing running, the test is + skipped, so a plain ``pytest tests/`` never spins up Docker for this tier. + """ + existing = find_container(_INTEGRATION_ROBOT_PATTERN) + if existing and container_running(existing): + yield {"container": existing, "brought_up": False} + return + + if not request.config.getoption("--run-integration"): + pytest.skip( + "No running robot-desktop container. Start one " + "(`AUTOLAUNCH=false airstack up robot-desktop`) or pass " + "`--run-integration` to let the harness bring it up." + ) + + log = "robot_autonomy_stack" + with logger_to(log): + missing = missing_images(env=_INTEGRATION_ENV) + if missing: + pytest.skip("robot-desktop image not built locally: " + ", ".join(missing)) + airstack_cmd("down", timeout=120, log_name=log) + result = airstack_cmd("up", "robot-desktop", + env_overrides=_INTEGRATION_ENV, timeout=180, log_name=log) + assert result.returncode == 0, \ + f"`airstack up robot-desktop` failed:\n{read_log_tail(log)}" + + container = wait_for_container(_INTEGRATION_ROBOT_PATTERN, timeout=120) + assert container, "robot-desktop container not Running after 120s" + try: + yield {"container": container, "brought_up": True} + finally: + with logger_to(log): + airstack_cmd("down", timeout=120, log_name=log) \ No newline at end of file diff --git a/tests/docker/docker-compose.yaml b/tests/docker/docker-compose.yaml index 4e392d284..28bde3275 100644 --- a/tests/docker/docker-compose.yaml +++ b/tests/docker/docker-compose.yaml @@ -13,5 +13,10 @@ services: environment: - AIRSTACK_ROOT=${AIRSTACK_PATH} - DISPLAY=${DISPLAY:-} + # Forward the OptiTrack/NatNet toggle so the in-runner `airstack up` + # (which does os.environ.copy()) brings the stack up with NatNet enabled and + # the test_natnet_pose_alive sentinel runs instead of skipping. + - LAUNCH_NATNET=${LAUNCH_NATNET:-false} + - NATNET_BODY_NAME=${NATNET_BODY_NAME:-Drone} working_dir: ${AIRSTACK_PATH}/tests network_mode: host diff --git a/tests/integration/README.md b/tests/integration/README.md new file mode 100644 index 000000000..8db63f3d1 --- /dev/null +++ b/tests/integration/README.md @@ -0,0 +1,53 @@ +# Integration tests (`tests/integration/`) + +The **integration** tier sits between **unit** and **system**: + +| Tier | Lives in | Brings up | Hardware | Mark | +|------|----------|-----------|----------|------| +| unit | `/test/` + proxies in `tests/robot/`, `tests/sim/`, `tests/gcs/` | nothing | none | `unit` | +| **integration** | **`tests/integration//`** | **robot container + a host-side component** | **Docker (no sim/GPU)** | **`integration`** (+ a specific mark) | +| system | `tests/system/` | full sim + robot + GCS | Docker + GPU + sim license | `liveliness`, `sensors`, `takeoff_hover_land` | + +An integration test wires a few **real** components together — for example the +robot autonomy stack plus a host-side NatNet server — without paying for a full +simulator or GPU. + +## The harness: `robot_autonomy_stack` + +The shared bring-up fixture lives in the root [`../conftest.py`](../conftest.py) +(alongside `airstack_env`, matching the repo convention that fixtures are +defined there). Request it instead of hand-rolling `airstack up`: + +- **Reuses** an already-running `robot-desktop` container (fast local iteration; + left running afterward). +- Otherwise brings one up (`airstack up robot-desktop`, autonomy on, no sim) + **only when `--run-integration` is passed**, and tears it down after. +- Otherwise **skips** — so a plain `pytest tests/` never spins up Docker. + +## Running + +```bash +# Reuse a container you already started: +AUTOLAUNCH=false airstack up robot-desktop +pytest tests/integration/ -m integration -v + +# Or let the harness bring the robot container up/down itself: +pytest tests/integration/ -m integration --run-integration -v + +# On CI / a PR, on-demand: +# /pytest -m integration --run-integration +``` + +## Adding a scenario + +1. Create `tests/integration//test_*.py`. +2. `pytestmark = [pytest.mark.integration, pytest.mark.]` — the + `integration` umbrella selects the whole tier; the specific mark targets one. +3. Request the `robot_autonomy_stack` fixture for the container. +4. Register the specific mark in [`../pytest.ini`](../pytest.ini). + +## Residents + +| Scenario | Mark | What it verifies | +|----------|------|------------------| +| [`natnet/`](natnet/) | `natnet` | Host NatNet emulator → `natnet_ros2` → pose topic Hz | diff --git a/tests/integration/natnet/README.md b/tests/integration/natnet/README.md new file mode 100644 index 000000000..7e8919aa3 --- /dev/null +++ b/tests/integration/natnet/README.md @@ -0,0 +1,104 @@ +# NatNet ↔ robot autonomy integration + +Drives the Python NatNet wire-protocol emulator against `natnet_ros2_node` +running with the robot autonomy stack. First resident of the +[`integration`](../README.md) tier. + +Marks: `integration` (tier) + `natnet` (scenario). + +## What it verifies + +Two variants, both ending at `/{ROBOT_NAME}/perception/optitrack/Drone` ≥ 5 Hz: + +1. **Raw server** (`test_natnet_ros2_receives_drone_pose_hz`) — hand-built frames + via `NatNetUnicastServer` (no USD): the minimal end-to-end wire check. +2. **Isaac wrapper** (`test_natnet_ros2_receives_isaac_wrapper_pose_hz`) — the full + new data path: `NatNetServerManager` builds the catalog from a + `NatNetInterfaceConfig` and samples a moving prim's world pose off an in-memory + USD stage (`sample_once`, exactly what the in-sim physics-step callback does), + streaming real frames to the robot client. Skips without `usd-core` (`pxr`). + Exact pose-value fidelity is covered hermetically by the package's + `test_pose_streaming.py` loopback. + +## Requirements + +- A NatNet UDP server (`NatNetUnicastServer`) on the host — started by the test. +- `natnet_ros2_node` (OptiTrack NatNet SDK) built in the robot container. +- Docker bridge routing from container → host gateway. + +The robot container is provided by the `robot_autonomy_stack` fixture (see the +[tier README](../README.md)). The test **skips** when `natnet_ros2_node` is not +built (the SDK is license-gated, fetched via `airstack setup --natnet`). + +## Running + +```bash +# Reuse an existing robot container: +AUTOLAUNCH=false airstack up robot-desktop +pytest tests/integration/natnet/ -m natnet -v + +# Or let the harness bring the container up/down: +pytest tests/integration/natnet/ -m natnet --run-integration -v +``` + +## Architecture + +``` +┌─────────────────────────────────────────────────────────────┐ +│ Host (pytest) │ +│ NatNetUnicastServer — MODELDEF bytes cache + frame queue │ +└───────────────────────────┬─────────────────────────────────┘ + │ UDP (docker bridge gateway IP) +┌───────────────────────────▼─────────────────────────────────┐ +│ Robot container (autonomy stack) │ +│ natnet_ros2_node (libNatNet 4.4 unicast) │ +│ → /{ROBOT_NAME}/perception/optitrack/Drone │ +└─────────────────────────────────────────────────────────────┘ +``` + +**Catalog ownership:** The server holds a MODELDEF **wire cache** only. Scene +semantics (prim paths, body names/IDs) belong in the future Isaac Sim wrapper, +which calls `set_model_def_payload()`. See +[`defaults.py`](../../../simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/defaults.py) +for hardcoded Drone reference constants used in tests. + +## Liveliness sentinel (sim end-to-end) + +The integration tier drives the emulator **host-side** (no sim/GPU). The matching +in-sim check is a conditional sentinel in +[`../../system/test_liveliness.py`](../../system/test_liveliness.py): +`TestLiveliness::test_natnet_pose_alive` asserts +`/{robot}/perception/optitrack/` is live per robot. It is **gated on +`LAUNCH_NATNET=true`** (skips otherwise), so normal liveliness runs are unaffected. +Override the body name with `NATNET_BODY_NAME` (default `Drone`). + +> The sentinel passes once the Isaac emulator actually streams in-sim — i.e. a +> Pegasus launch script authors a `NatNetInterface` prim and calls +> `NatNetServerManager.start_from_stage()` (the scripting entry point). Until that +> sim auto-start is wired, run the **integration** variants above for robot-level +> coverage. + +## 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. + +Rebuild `natnet_ros2` in the robot container after any adapter changes: +`docker exec airstack-robot-desktop-1 bash -lc 'bws --packages-select natnet_ros2'` diff --git a/tests/integration/natnet/test_natnet_integration.py b/tests/integration/natnet/test_natnet_integration.py new file mode 100644 index 000000000..6a98551c1 --- /dev/null +++ b/tests/integration/natnet/test_natnet_integration.py @@ -0,0 +1,287 @@ +# Copyright (c) 2024 Carnegie Mellon University +# MIT License - see LICENSE in the repository root for full text. +"""NatNet emulator ↔ robot autonomy integration test. + +Wide-scale integration: a host-side NatNet server (the Python emulator) streams +dummy Drone frames to ``natnet_ros2`` running against the robot autonomy stack, +and we assert the pose topic stays alive at a stable rate. + +This is the first resident of the ``integration`` tier. Today the NatNet +*server* is the host emulator; once the Isaac-sim emulator wrapper emits NatNet +frames, an Isaac-wrapped variant will be added here, and the gated pose-rate +check can additionally surface in ``system/test_liveliness.py``. +""" + +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 package (host-side) is not pip-installed; expose it + its test +# helpers via the AIRSTACK_ROOT-anchored repo_path() (works in CI and locally). +_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, pytest.mark.natnet] + +_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) + + +def _make_drone_frame(frame_num: int) -> dt.sFrameOfMocapData: + frame = dt.sFrameOfMocapData() + frame.iFrame = frame_num + frame.nRigidBodies = 1 + rb = frame.RigidBodies[0] + rb.ID = 1 + 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) -> None: + frame_num = 0 + interval = 1.0 / server.publish_rate + while not stop_event.is_set(): + server.enqueue_mocap_data(_make_drone_frame(frame_num)) + frame_num += 1 + time.sleep(interval) + + +def _launch_natnet_node(container, host_ip, command_port, domain_id): + """Start natnet_ros2_node in the container pointed at the host emulator.""" + 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_name:=Drone " + f"-p body_id:=1 " + f"-p publish_to_mavros:=false " + f"-p publish_direct_optitrack:=true'" + ) + 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): + """Wait for the pose topic then assert a sustained rate >= _MIN_HZ.""" + pose_topic = f"/{robot_name}/perception/optitrack/Drone" + pose_cov_topic = f"{pose_topic}/pose_cov" + + time.sleep(_WARMUP_S) + first_msg_s = wait_for_first_message( + container, pose_cov_topic, domain_id, _ROBOT_SETUP, timeout=int(_STREAM_HOLD_S) + ) + assert first_msg_s is not None, ( + f"No messages on {pose_cov_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): + """Emulator on host streams dummy Drone frames while natnet_ros2_node publishes. + + Raw-server variant: hand-built frames via ``NatNetUnicastServer`` (no Isaac + wrapper, no USD) — the minimal end-to-end wire check. + """ + 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 variant: the full new data path drives natnet_ros2. + + ``NatNetServerManager`` builds the catalog from a ``NatNetInterfaceConfig``, + samples a (moving) prim's world pose off an in-memory USD stage via + ``sample_once`` — exactly what the physics-step callback does in-sim — and + streams real frames. We assert ``natnet_ros2`` connects and publishes the pose + at a stable rate. (Exact pose-value fidelity is covered hermetically by the + package's ``test_pose_streaming.py`` loopback; here we prove the wrapper feeds + the *real* robot client end to end without a sim/GPU.) + """ + 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() diff --git a/tests/pytest.ini b/tests/pytest.ini index a664ccbf3..f5220f93a 100644 --- a/tests/pytest.ini +++ b/tests/pytest.ini @@ -6,6 +6,8 @@ markers = liveliness: Container and process health (Docker, tmux, sentinel ROS 2 nodes) sensors: Sim and robot sensor topic rates, LiDAR validation, sim RTF takeoff_hover_land: End-to-end takeoff / hover / land action tests + integration: Cross-component integration tests (robot container + a host-side component; no sim/GPU) + natnet: NatNet emulator ↔ natnet_ros2 integration (robot container + UDP server) testpaths = . addopts = -v --durations=0 cache_dir = /tmp/.pytest_cache diff --git a/tests/requirements.txt b/tests/requirements.txt index a4b43e6bb..c752ba8c4 100644 --- a/tests/requirements.txt +++ b/tests/requirements.txt @@ -6,3 +6,4 @@ tabulate psutil pandas numpy +usd-core diff --git a/tests/robot/perception/natnet_ros2/test_natnet_ros2.py b/tests/robot/perception/natnet_ros2/test_natnet_ros2.py index fc9a78bde..aa1f2023d 100644 --- a/tests/robot/perception/natnet_ros2/test_natnet_ros2.py +++ b/tests/robot/perception/natnet_ros2/test_natnet_ros2.py @@ -11,22 +11,10 @@ gtests and ament linters. """ -import importlib.util -import sys -from pathlib import Path +from conftest import reexport_unit_tests, repo_path -_repo_root = Path(__file__).resolve().parents[4] -_pkg_test = _repo_root / "robot/ros_ws/src/perception/natnet_ros2/test" -_real_file = _pkg_test / "test_natnet_ros2.py" - -# Load the real module under a unique name to avoid the circular-import that -# would occur if we used `from test_natnet_ros2 import *` (this file has the -# same name and pytest adds its directory to sys.path at collection time). -_spec = importlib.util.spec_from_file_location("_natnet_ros2_unit_tests", _real_file) -_real = importlib.util.module_from_spec(_spec) -_spec.loader.exec_module(_real) - -# Re-export every test_* symbol so pytest collects them from this proxy. -for _name in dir(_real): - if _name.startswith("test_"): - globals()[_name] = getattr(_real, _name) +reexport_unit_tests( + globals(), + repo_path("robot/ros_ws/src/perception/natnet_ros2/test"), + "test_natnet_ros2.py", +) diff --git a/tests/robot/sensors/lidar_point_cloud_filter/test_validation_core.py b/tests/robot/sensors/lidar_point_cloud_filter/test_validation_core.py index e7babf9d4..7af5ab631 100644 --- a/tests/robot/sensors/lidar_point_cloud_filter/test_validation_core.py +++ b/tests/robot/sensors/lidar_point_cloud_filter/test_validation_core.py @@ -11,28 +11,10 @@ the ament linters. """ -import importlib.util -import sys -from pathlib import Path +from conftest import reexport_unit_tests, repo_path -_repo_root = Path(__file__).resolve().parents[4] -_pkg_test = _repo_root / "robot/ros_ws/src/sensors/lidar_point_cloud_filter/test" -_pkg_root = _pkg_test.parent # adds lidar_point_cloud_filter/ package to sys.path -_real_file = _pkg_test / "test_validation_core.py" - -# Make the package module importable so the real test can do -# `from lidar_point_cloud_filter.validation_core import ...` -if str(_pkg_root) not in sys.path: - sys.path.insert(0, str(_pkg_root)) - -# Load the real module under a unique name to avoid the circular-import that -# would occur if we used `from test_validation_core import *` (this file has -# the same name and pytest adds its directory to sys.path at collection time). -_spec = importlib.util.spec_from_file_location("_lidar_validation_unit_tests", _real_file) -_real = importlib.util.module_from_spec(_spec) -_spec.loader.exec_module(_real) - -# Re-export every test_* symbol so pytest collects them from this proxy. -for _name in dir(_real): - if _name.startswith("test_"): - globals()[_name] = getattr(_real, _name) +reexport_unit_tests( + globals(), + repo_path("robot/ros_ws/src/sensors/lidar_point_cloud_filter/test"), + "test_validation_core.py", +) diff --git a/tests/sim/README.md b/tests/sim/README.md index 09f45f6a6..ceae2ebbd 100644 --- a/tests/sim/README.md +++ b/tests/sim/README.md @@ -6,9 +6,11 @@ AirSim bridge utilities). Mark fast, hermetic checks with `@pytest.mark.unit`. Tests that require a GPU, full sim, or Docker belong in [`tests/system/`](../system/) instead. +Cross-component tests that need the robot container (but not a sim) belong in +[`tests/integration/`](../integration/) (mark: `integration`). Suggested layout: | Directory | Purpose | |-----------|---------| -| `motive_emulator/` | Motive / NatNet protocol emulation / parsing | +| `optitrack_natnet_emulator/` | NatNet emulator unit tests (proxy; mark: `unit`). Includes pure protocol/serializer/config-model checks plus `pxr`-guarded USD interface-authoring tests (skip without `usd-core`). | diff --git a/tests/sim/motive_emulator/README.md b/tests/sim/motive_emulator/README.md deleted file mode 100644 index 0e682c448..000000000 --- a/tests/sim/motive_emulator/README.md +++ /dev/null @@ -1,61 +0,0 @@ -# Motive / NatNet Emulator - -This directory is the future home of **integration tests** that drive a real -NatNet wire-protocol mock server against `natnet_ros2_node`. - -## Why here, not in the package test/ dir? - -Unit tests for pure logic live in -`tests/robot/perception/natnet_ros2/test_natnet_logic.cpp` and run via `colcon -test` with no network or SDK required (uses `FakeNatNetClient`). - -The emulator tests here will require an actual UDP server that speaks the NatNet -protocol, so they belong in the `sensors` mark of the system test suite alongside -other topic-streaming tests. - -## Planned implementation - -The mock server should: - -1. Open a UDP socket on the NatNet command port (default 1510). -2. Respond to `NAT_CONNECT` (message type 0) with a `NAT_SERVERINFO` (type 1) - packet containing a canned `sServerDescription`. -3. Respond to `NAT_REQUEST_MODELDEF` (type 4) with a `NAT_MODELDEF` (type 5) - packet describing one or more rigid bodies. -4. Stream `NAT_FRAMEOFDATA` (type 7) packets to the client's data port at a - configurable rate with synthetic pose data. - -### Reference - -The NatNet wire format is documented in the NatNet SDK developer notes and the -`PacketClient` example shipped with the SDK (available inside the robot Docker -container after `airstack setup --natnet`). - -## Relationship to `FakeNatNetClient` - -``` - ┌──────────────────────────────────────┐ - │ Test boundary │ - colcon gtest │ FakeNatNetClient (in-process) │ ← unit tests (no network) - │ test_natnet_logic.cpp │ - └──────────────────────────────────────┘ - - ┌──────────────────────────────────────┐ - │ Network boundary │ - pytest sensors │ MotiveEmulator (UDP server, Python) │ ← integration tests - │ NatNetClientAdapter → NatNetClient │ - │ natnet_ros2_node (full ROS node) │ - └──────────────────────────────────────┘ -``` - -The `FakeNatNetClient` seam (already implemented) lets unit tests verify all -connection-outcome logic paths. The emulator here will verify the full -end-to-end path including the NatNet SDK's own parser. - -## When to add this - -Implement the emulator when: -- The OptiTrack emulator service is placed under `simulation/optitrack-emulator/` - or `tests/sim/motive_emulator/` -- The `sensors` test mark is extended to include `natnet_ros2` topic checks -- CI has access to the robot container with the NatNet SDK installed diff --git a/tests/sim/optitrack_natnet_emulator/test_catalog.py b/tests/sim/optitrack_natnet_emulator/test_catalog.py new file mode 100644 index 000000000..0d19bf34b --- /dev/null +++ b/tests/sim/optitrack_natnet_emulator/test_catalog.py @@ -0,0 +1,11 @@ +# Copyright (c) 2024 Carnegie Mellon University +# MIT License - see LICENSE in the repository root for full text. +"""Proxy: re-exposes optitrack.natnet.emulator Commit 3 catalog-builder tests.""" + +from conftest import reexport_unit_tests, repo_path + +reexport_unit_tests( + globals(), + repo_path("simulation/isaac-sim/extensions/optitrack.natnet.emulator/test"), + "test_catalog.py", +) diff --git a/tests/sim/optitrack_natnet_emulator/test_defaults_and_catalog.py b/tests/sim/optitrack_natnet_emulator/test_defaults_and_catalog.py new file mode 100644 index 000000000..0dca746d2 --- /dev/null +++ b/tests/sim/optitrack_natnet_emulator/test_defaults_and_catalog.py @@ -0,0 +1,12 @@ +# Copyright (c) 2024 Carnegie Mellon University +# MIT License - see LICENSE in the repository root for full text. +"""Proxy: re-exposes optitrack.natnet.emulator default/catalog unit tests for pytest tests/.""" + +from conftest import reexport_unit_tests, repo_path + +reexport_unit_tests( + globals(), + repo_path("simulation/isaac-sim/extensions/optitrack.natnet.emulator/test"), + "test_defaults.py", + "test_server_catalog.py", +) diff --git a/tests/sim/optitrack_natnet_emulator/test_discovery.py b/tests/sim/optitrack_natnet_emulator/test_discovery.py new file mode 100644 index 000000000..b1d43de2b --- /dev/null +++ b/tests/sim/optitrack_natnet_emulator/test_discovery.py @@ -0,0 +1,11 @@ +# Copyright (c) 2024 Carnegie Mellon University +# MIT License - see LICENSE in the repository root for full text. +"""Proxy: re-exposes optitrack.natnet.emulator Commit 2 detection unit tests.""" + +from conftest import reexport_unit_tests, repo_path + +reexport_unit_tests( + globals(), + repo_path("simulation/isaac-sim/extensions/optitrack.natnet.emulator/test"), + "test_discovery.py", +) diff --git a/tests/sim/optitrack_natnet_emulator/test_frames.py b/tests/sim/optitrack_natnet_emulator/test_frames.py new file mode 100644 index 000000000..493abb722 --- /dev/null +++ b/tests/sim/optitrack_natnet_emulator/test_frames.py @@ -0,0 +1,11 @@ +# Copyright (c) 2024 Carnegie Mellon University +# MIT License - see LICENSE in the repository root for full text. +"""Proxy: re-exposes optitrack.natnet.emulator pose->frame builder unit tests.""" + +from conftest import reexport_unit_tests, repo_path + +reexport_unit_tests( + globals(), + repo_path("simulation/isaac-sim/extensions/optitrack.natnet.emulator/test"), + "test_frames.py", +) diff --git a/tests/sim/optitrack_natnet_emulator/test_interface_authoring.py b/tests/sim/optitrack_natnet_emulator/test_interface_authoring.py new file mode 100644 index 000000000..9c8271bd9 --- /dev/null +++ b/tests/sim/optitrack_natnet_emulator/test_interface_authoring.py @@ -0,0 +1,15 @@ +# Copyright (c) 2024 Carnegie Mellon University +# MIT License - see LICENSE in the repository root for full text. +"""Proxy: re-exposes optitrack.natnet.emulator USD authoring unit tests. + +The underlying tests `pytest.importorskip("pxr")`, so they skip unless USD Python +(`usd-core` in tests/requirements.txt, or Isaac's bundled USD) is available. +""" + +from conftest import reexport_unit_tests, repo_path + +reexport_unit_tests( + globals(), + repo_path("simulation/isaac-sim/extensions/optitrack.natnet.emulator/test"), + "test_interface_authoring.py", +) diff --git a/tests/sim/optitrack_natnet_emulator/test_interface_config.py b/tests/sim/optitrack_natnet_emulator/test_interface_config.py new file mode 100644 index 000000000..5e2b139b6 --- /dev/null +++ b/tests/sim/optitrack_natnet_emulator/test_interface_config.py @@ -0,0 +1,11 @@ +# Copyright (c) 2024 Carnegie Mellon University +# MIT License - see LICENSE in the repository root for full text. +"""Proxy: re-exposes optitrack.natnet.emulator interface config-model unit tests.""" + +from conftest import reexport_unit_tests, repo_path + +reexport_unit_tests( + globals(), + repo_path("simulation/isaac-sim/extensions/optitrack.natnet.emulator/test"), + "test_interface_config.py", +) diff --git a/tests/sim/optitrack_natnet_emulator/test_pose_sampling.py b/tests/sim/optitrack_natnet_emulator/test_pose_sampling.py new file mode 100644 index 000000000..0ad8dc5e6 --- /dev/null +++ b/tests/sim/optitrack_natnet_emulator/test_pose_sampling.py @@ -0,0 +1,11 @@ +# Copyright (c) 2024 Carnegie Mellon University +# MIT License - see LICENSE in the repository root for full text. +"""Proxy: re-exposes optitrack.natnet.emulator pose-sampling / resync unit tests.""" + +from conftest import reexport_unit_tests, repo_path + +reexport_unit_tests( + globals(), + repo_path("simulation/isaac-sim/extensions/optitrack.natnet.emulator/test"), + "test_pose_sampling.py", +) diff --git a/tests/sim/optitrack_natnet_emulator/test_pose_streaming.py b/tests/sim/optitrack_natnet_emulator/test_pose_streaming.py new file mode 100644 index 000000000..f33b6d251 --- /dev/null +++ b/tests/sim/optitrack_natnet_emulator/test_pose_streaming.py @@ -0,0 +1,11 @@ +# Copyright (c) 2024 Carnegie Mellon University +# MIT License - see LICENSE in the repository root for full text. +"""Proxy: re-exposes optitrack.natnet.emulator end-to-end pose-streaming test.""" + +from conftest import reexport_unit_tests, repo_path + +reexport_unit_tests( + globals(), + repo_path("simulation/isaac-sim/extensions/optitrack.natnet.emulator/test"), + "test_pose_streaming.py", +) diff --git a/tests/sim/optitrack_natnet_emulator/test_scene_setup.py b/tests/sim/optitrack_natnet_emulator/test_scene_setup.py new file mode 100644 index 000000000..4666c644a --- /dev/null +++ b/tests/sim/optitrack_natnet_emulator/test_scene_setup.py @@ -0,0 +1,11 @@ +# Copyright (c) 2024 Carnegie Mellon University +# MIT License - see LICENSE in the repository root for full text. +"""Proxy: re-exposes optitrack.natnet.emulator launch-helper (scene_setup) tests.""" + +from conftest import reexport_unit_tests, repo_path + +reexport_unit_tests( + globals(), + repo_path("simulation/isaac-sim/extensions/optitrack.natnet.emulator/test"), + "test_scene_setup.py", +) diff --git a/tests/sim/optitrack_natnet_emulator/test_serializers.py b/tests/sim/optitrack_natnet_emulator/test_serializers.py new file mode 100644 index 000000000..22226cfff --- /dev/null +++ b/tests/sim/optitrack_natnet_emulator/test_serializers.py @@ -0,0 +1,11 @@ +# Copyright (c) 2024 Carnegie Mellon University +# MIT License - see LICENSE in the repository root for full text. +"""Proxy: re-exposes optitrack.natnet.emulator serializer unit tests for pytest tests/.""" + +from conftest import reexport_unit_tests, repo_path + +reexport_unit_tests( + globals(), + repo_path("simulation/isaac-sim/extensions/optitrack.natnet.emulator/test"), + "test_serializers.py", +) diff --git a/tests/sim/optitrack_natnet_emulator/test_server_from_config.py b/tests/sim/optitrack_natnet_emulator/test_server_from_config.py new file mode 100644 index 000000000..f84c2e2a0 --- /dev/null +++ b/tests/sim/optitrack_natnet_emulator/test_server_from_config.py @@ -0,0 +1,11 @@ +# Copyright (c) 2024 Carnegie Mellon University +# MIT License - see LICENSE in the repository root for full text. +"""Proxy: re-exposes optitrack.natnet.emulator Commit 3 server-from-config tests.""" + +from conftest import reexport_unit_tests, repo_path + +reexport_unit_tests( + globals(), + repo_path("simulation/isaac-sim/extensions/optitrack.natnet.emulator/test"), + "test_server_from_config.py", +) diff --git a/tests/sim/optitrack_natnet_emulator/test_server_lifecycle.py b/tests/sim/optitrack_natnet_emulator/test_server_lifecycle.py new file mode 100644 index 000000000..9cf4a45d6 --- /dev/null +++ b/tests/sim/optitrack_natnet_emulator/test_server_lifecycle.py @@ -0,0 +1,11 @@ +# Copyright (c) 2024 Carnegie Mellon University +# MIT License - see LICENSE in the repository root for full text. +"""Proxy: re-exposes optitrack.natnet.emulator Commits 3-4 lifecycle unit tests.""" + +from conftest import reexport_unit_tests, repo_path + +reexport_unit_tests( + globals(), + repo_path("simulation/isaac-sim/extensions/optitrack.natnet.emulator/test"), + "test_server_lifecycle.py", +) diff --git a/tests/sim/optitrack_natnet_emulator/test_target_resolution.py b/tests/sim/optitrack_natnet_emulator/test_target_resolution.py new file mode 100644 index 000000000..380b60e9b --- /dev/null +++ b/tests/sim/optitrack_natnet_emulator/test_target_resolution.py @@ -0,0 +1,11 @@ +# Copyright (c) 2024 Carnegie Mellon University +# MIT License - see LICENSE in the repository root for full text. +"""Proxy: re-exposes optitrack.natnet.emulator Commit 3 target-resolution tests.""" + +from conftest import reexport_unit_tests, repo_path + +reexport_unit_tests( + globals(), + repo_path("simulation/isaac-sim/extensions/optitrack.natnet.emulator/test"), + "test_target_resolution.py", +) diff --git a/tests/sim/optitrack_natnet_emulator/test_unicast_protocol.py b/tests/sim/optitrack_natnet_emulator/test_unicast_protocol.py new file mode 100644 index 000000000..e70a25064 --- /dev/null +++ b/tests/sim/optitrack_natnet_emulator/test_unicast_protocol.py @@ -0,0 +1,11 @@ +# Copyright (c) 2024 Carnegie Mellon University +# MIT License - see LICENSE in the repository root for full text. +"""Proxy: re-exposes optitrack.natnet.emulator unicast-protocol unit tests for pytest tests/.""" + +from conftest import reexport_unit_tests, repo_path + +reexport_unit_tests( + globals(), + repo_path("simulation/isaac-sim/extensions/optitrack.natnet.emulator/test"), + "test_unicast_protocol.py", +) diff --git a/tests/system/test_liveliness.py b/tests/system/test_liveliness.py index 342e5f49a..b465726d5 100644 --- a/tests/system/test_liveliness.py +++ b/tests/system/test_liveliness.py @@ -7,6 +7,7 @@ Sensor topic rates, bridge stereo Hz, LiDAR echo/sanity, and sim RTF live in ``system/test_sensors.py`` (``@pytest.mark.sensors``), ordered after this module. """ +import os import time import pytest @@ -21,6 +22,7 @@ logger, ros2_exec, sample_compute_usage, + sample_hz, wait_for_first_message, ) @@ -31,6 +33,19 @@ "/robot_{N}/trajectory_controller/trajectory_control_node", ] +# Optional OptiTrack/NatNet liveliness sentinel — only meaningful when the robot +# launches natnet_ros2 (LAUNCH_NATNET=true) and the sim streams NatNet frames +# (Isaac emulator wrapper). Off by default, so normal runs skip it cleanly. +_NATNET_ENABLED = os.environ.get("LAUNCH_NATNET", "").strip().lower() in ("1", "true", "yes", "on") +_NATNET_BODY_NAME = os.environ.get("NATNET_BODY_NAME", "Drone") +_NATNET_MIN_HZ = 5.0 +# Generous first-message budget: on a cold Isaac boot the standalone Pegasus +# script must finish loading, start Play (physics steps drive the emulator's +# synchronous frame pump), and the robot natnet_ros2 client must connect over +# UDP before pose frames appear. 60s was too tight vs the 600s /clock and 300s +# sentinel-node budgets. +_NATNET_FIRST_MSG_TIMEOUT = 120 + def _parse_panes(raw): """Return (crashed, active_count). Input lines: 'session:window|pane_pid|title|kids'. @@ -251,6 +266,51 @@ def ready(): fail_msg=lambda: f"sentinel nodes not ready after 300s: {last_msg[0]}", ) + @pytest.mark.skipif( + not _NATNET_ENABLED, + reason="LAUNCH_NATNET not enabled — OptiTrack/NatNet pose sentinel skipped", + ) + @pytest.mark.dependency(depends=["sim_ready", "nodes"]) + def test_natnet_pose_alive(self, airstack_env): + """When NatNet is enabled, the OptiTrack pose topic must publish per robot. + + Closes the loop the integration tier opens host-side: with the Isaac + emulator wrapper streaming in-sim and ``natnet_ros2`` running on each robot + (LAUNCH_NATNET=true), ``/{robot}/perception/optitrack//pose_cov`` must + be live. We probe ``/pose_cov`` (PoseWithCovarianceStamped) because it is + published unconditionally for every body, whereas the bare ```` + PoseStamped is gated on the ``publish_direct_optitrack`` param. + """ + cfg = airstack_env["cfg"] + robot_containers = get_robot_containers(airstack_env["robot_pattern"]) + m = get_metrics() + tid = current_test_id() + + failures = [] + for n in range(1, airstack_env["num_robots"] + 1): + container = robot_containers[n - 1] + topic = f"/robot_{n}/perception/optitrack/{_NATNET_BODY_NAME}/pose_cov" + first = wait_for_first_message( + container, + topic, + domain_id=n, + setup_bash=cfg["robot_setup_bash"], + timeout=_NATNET_FIRST_MSG_TIMEOUT, + ) + if first is None: + failures.append( + f"robot_{n}: no message on {topic} within {_NATNET_FIRST_MSG_TIMEOUT}s" + ) + continue + hz = sample_hz( + container, topic, domain_id=n, setup_bash=cfg["robot_setup_bash"], duration=5, window=20 + ) + m.record(tid, f"natnet_pose_hz_robot_{n}", hz if hz is not None else "none", unit="Hz") + if hz is None or hz < _NATNET_MIN_HZ: + failures.append(f"robot_{n}: {topic} at {hz} Hz (< {_NATNET_MIN_HZ})") + + assert not failures, "NatNet pose sentinel failed: " + "; ".join(failures) + @pytest.mark.dependency(depends=["sim_ready", "nodes", "tmux"]) def test_stable(self, airstack_env, request): """Poll infra only: tmux, sentinel nodes, compute (no sensor topic Hz)."""