diff --git a/.agents/skills/add-unit-tests/SKILL.md b/.agents/skills/add-unit-tests/SKILL.md index 7d1d3b582..ea47f7b75 100644 --- a/.agents/skills/add-unit-tests/SKILL.md +++ b/.agents/skills/add-unit-tests/SKILL.md @@ -35,11 +35,12 @@ robot/ros_ws/src/// └── CMakeLists.txt # wires ament_add_gtest under BUILD_TESTING tests/robot/// -└── test_.py # ← thin PROXY (re-exports tests from above) +└── test_.py # ← thin PROXY (registers tests from above) ``` -The **proxy** is a one-file shim that loads the real test module with `importlib` -and re-exports every `test_*` function. This means: +The **proxy** is a one-file shim that calls ``register_unit_tests()`` to load the +real test module with ``importlib`` and expose every ``test_*`` function to pytest. +This means: | Invocation | What runs | |---|---| @@ -48,6 +49,39 @@ and re-exports every `test_*` function. This means: | CI `system-tests.yml` (PR open / approved) | Same path via `pytest tests/` | | `colcon test --packages-select ` | Real test in `package/test/` directly | +### `register_unit_tests` (in `tests/conftest.py`) + +Proxies call this helper; they do not duplicate test logic. Signature: + +```python +register_unit_tests(target_globals, test_dir, *module_files) +``` + +| Argument | Meaning | +|---|---| +| `target_globals` | Pass ``globals()`` from the proxy module — pytest collects ``test_*`` names injected here | +| `test_dir` | Co-located test directory, usually ``repo_path("robot/ros_ws/src///test")`` | +| `*module_files` | One or more filenames under ``test_dir`` (e.g. ``"test_foo.py"``); fold several into one proxy | + +**What it does (in order):** + +1. Prepends ``test_dir`` and ``test_dir.parent`` (package or extension root) to + ``sys.path`` so loaded modules can import production code and sibling helpers. +2. Loads each file via ``importlib.util.spec_from_file_location`` under a + synthetic name (``_unit__``) so proxy and source can share the + same basename without circular imports. +3. Copies every ``test_*`` callable from the loaded module into ``target_globals``. +4. Wraps each with ``pytest.mark.unit`` so ``pytest tests/ -m unit`` selects them + even if the source omitted ``pytestmark``. + +**What it does not do:** run tests, install packages, or replace ``colcon test``. +For local iteration against source only, run ``pytest /test/`` (add a tiny +``conftest.py`` in that dir for ``sys.path`` — see the emulator example below). + +Pair with ``repo_path()`` from the same conftest — paths are anchored on +``AIRSTACK_ROOT`` (CI export, repo root locally). **Never** use +``Path(__file__).parents[N]`` in a proxy. + ## Step-by-Step: Adding a Python Unit Test ### 1. Identify pure-Python logic to test @@ -114,50 +148,34 @@ 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 +``register_unit_tests`` + ``repo_path`` from ``tests/conftest.py`` so the proxy +stays a two-call shim: ```python # Copyright (c) 2024 Carnegie Mellon University # MIT License - see LICENSE in the repository root for full text. -"""Proxy: re-exposes unit tests from the package source tree. +"""Proxy: registers unit tests from the package source tree. -Unit test logic lives co-located with the package source (ROS 2 / colcon convention): +Unit test logic lives co-located with the package (ROS 2 / colcon convention): robot/ros_ws/src///test/test_.py -This file makes those tests discoverable by ``pytest tests/`` (CI) and -``airstack test -m unit`` without any changes to the CI workflow. +Discoverable by ``pytest tests/`` (CI) and ``airstack test -m unit``. """ -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" +from conftest import register_unit_tests, repo_path -# 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)) - -# 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) +register_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]` | +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 @@ -257,16 +275,16 @@ there are listed in [`tests/colcon_unit_test_packages.yaml`](../../../tests/colc The same proxy pattern applies verbatim: -**Sim-side Python** (e.g. motive emulator protocol logic): +**Sim-side Python** (e.g. OptiTrack NatNet emulator): ``` simulation/...//test/test_.py ← source -tests/sim//test_.py ← proxy (parents[3] = repo root) +tests/sim//test_.py ← proxy (register_unit_tests + repo_path) ``` **GCS modules**: ``` gcs/...//test/test_.py ← source -tests/gcs//test_.py ← proxy (parents[3] = repo root) +tests/gcs//test_.py ← proxy (register_unit_tests + repo_path) ``` `pytest tests/ -m unit` discovers them through the proxy without any @@ -280,7 +298,8 @@ pytest.ini or CI changes needed. |---|---| | Where does test source live? | `/…//test/` (co-located with the package) | | Where does pytest discover tests? | `tests/robot/` (or `tests/sim/`, `tests/gcs/`) via thin proxy | -| How does the proxy avoid circular import? | `importlib.util.spec_from_file_location` with a unique module name | +| How does the proxy register tests? | ``register_unit_tests(globals(), repo_path(...), "test_*.py")`` in `tests/conftest.py` | +| How does the proxy avoid circular import? | `importlib.util.spec_from_file_location` with a unique synthetic module name | | What mark do all unit tests use? | `@pytest.mark.unit` | | What CI workflow runs them? | `system-tests.yml` — runs `pytest tests/` which includes unit tests | | When does that workflow trigger? | PR opened, `/pytest` comment, `workflow_dispatch` | @@ -302,8 +321,9 @@ Corresponding proxies: `tests/robot/perception/natnet_ros2/test_natnet_ros2.py`, ## Files to Know - `.github/workflows/system-tests.yml` — CI workflow (runs `pytest tests/` including unit tests) +- `tests/conftest.py` — `register_unit_tests`, `repo_path`, shared fixtures - `tests/pytest.ini` — mark registration (`unit`, `build_docker`, etc.) - `tests/robot/` — proxy layer mirroring `robot/ros_ws/src/` -- `tests/sim/` — proxy layer for sim-side code (future) +- `tests/sim/` — proxy layer for sim-side extensions and tools - `tests/gcs/` — proxy layer for GCS code (future) - `tests/README.md` — full test harness reference diff --git a/.agents/skills/optitrack-development/SKILL.md b/.agents/skills/optitrack-development/SKILL.md new file mode 100644 index 000000000..fcfe66e87 --- /dev/null +++ b/.agents/skills/optitrack-development/SKILL.md @@ -0,0 +1,220 @@ +--- +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 (mark: `integration`) | + +**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:** set ``ISAAC_SIM_SCRIPT_NAME`` to a NatNet Pegasus launch script (no env gate in the script — NatNet always starts): + +| Script | Use | +|--------|-----| +| [`example_one_px4_pegasus_natnet_launch_script.py`](../../../simulation/isaac-sim/launch_scripts/example_one_px4_pegasus_natnet_launch_script.py) | Single drone + static ``Target`` | +| [`example_multi_px4_pegasus_natnet_launch_script.py`](../../../simulation/isaac-sim/launch_scripts/example_multi_px4_pegasus_natnet_launch_script.py) | ``NUM_ROBOTS`` drones + shared ``Target`` (pair with 3-profile ``natnet_config.yaml``) | + +Helpers: [`isaac/scene_setup.py`](../../../simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/scene_setup.py) (`start_drone_natnet_server`, `author_static_target`). Drone body: single = ``Drone``; multi = ``Drone`` (id ``i``); target = ``Target`` (id 100). Override names with ``NATNET_BODY_NAME`` / ``NATNET_TARGET_NAME``. Baseline Pegasus scripts (no NatNet) remain ``example_one_px4_pegasus_launch_script.py`` / ``example_multi_px4_pegasus_launch_script.py``. + +**Default client config:** unicast, `server_ip` → Motive/emulator (use `172.31.0.200` for Isaac container), ports 1510/1511. The config is per-robot: each `robots[$ROBOT_NAME]` profile lists the bodies it tracks (each a `rigid_body_name` + `id` mapped to a relative `topic`, with `pose`/`pose_cov` toggles and per-body covariance) and an optional `vision_pose` block that drives the MAVROS bridge. 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` (mark: `integration`) | +| 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/.agents/skills/run-system-tests/SKILL.md b/.agents/skills/run-system-tests/SKILL.md index 453bbc953..6cf7fbaa7 100644 --- a/.agents/skills/run-system-tests/SKILL.md +++ b/.agents/skills/run-system-tests/SKILL.md @@ -25,7 +25,7 @@ This skill is about the **test harness itself** — pytest marks, fixtures, the The suite lives at `tests/` (repo root) and is fully pytest-based. Configuration is in `tests/pytest.ini` and shared infrastructure in `tests/conftest.py`. - **`tests/system/`** — Docker stack integration tests. Marks: `build_docker`, `build_packages`, `liveliness`, `sensors`, `takeoff_hover_land`. -- **`tests/robot/`** and **`tests/sim/`** — Hermetic **unit** tests (`@pytest.mark.unit`). These are **thin proxy files** that re-export tests from each ROS 2 package's own `test/` directory (co-located with the source, the ROS 2 / colcon convention). The proxy pattern keeps test source next to the code it tests while making tests discoverable by `pytest tests/`. +- **`tests/robot/`** and **`tests/sim/`** — Hermetic **unit** tests (`@pytest.mark.unit`). These are **thin proxy files** that call ``register_unit_tests()`` to register tests from each package's co-located `test/` directory so `pytest tests/` discovers them. ### Unit tests vs system tests diff --git a/.env b/.env index 82cc01ccb..0d994bc51 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. diff --git a/AGENTS.md b/AGENTS.md index b53069e75..dbb70f09a 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,15 +197,17 @@ 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`, `integration`, `liveliness`, `sensors`, `takeoff_hover_land`). ```bash docker exec airstack-robot-desktop-1 bash -c "sws && colcon test --packages-select natnet_ros2 --event-handlers console_direct+" ``` -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. +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/) call `register_unit_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. Collection order runs integration after `build_docker` and `build_packages`. First resident: `tests/integration/natnet/` (host NatNet emulator → `natnet_ros2` pose Hz). 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/index.md b/docs/development/intermediate/testing/index.md index 0769e2b09..2a61b2e34 100644 --- a/docs/development/intermediate/testing/index.md +++ b/docs/development/intermediate/testing/index.md @@ -12,8 +12,8 @@ hardware requirement: ## Unit tests (`pytest -m unit`) Fast, hermetic Python tests that run in seconds with no Docker or GPU. Test source -lives **co-located with its ROS 2 package** (`/test/`) and is re-exported -through thin proxy files in `tests/robot/` for centralized discovery. +lives **co-located with its ROS 2 package** (`/test/`) and is registered +through thin proxy files in `tests/robot/` via `register_unit_tests()`. ```bash airstack test -m unit -v diff --git a/docs/development/intermediate/testing/unit_testing.md b/docs/development/intermediate/testing/unit_testing.md index f69056008..6d0a1b220 100644 --- a/docs/development/intermediate/testing/unit_testing.md +++ b/docs/development/intermediate/testing/unit_testing.md @@ -5,7 +5,7 @@ AirStack unit tests are **fast, hermetic, and purely Python** — no Docker stac ## Design principles - **Co-located with source.** Test files live in `/test/` alongside the code they test. This is the standard ROS 2 / colcon convention and ensures tests are discovered by both `colcon test` and `pytest`. -- **Proxy for centralized discovery.** A thin shim in `tests/robot///` re-exports the test functions so `pytest tests/` (the CI command) and `airstack test -m unit` discover them without any changes to the CI workflow. +- **Proxy for centralized discovery.** A thin shim in `tests/robot///` calls `register_unit_tests()` (in `tests/conftest.py`) to register co-located `test_*` functions so `pytest tests/` and `airstack test -m unit` discover them. See the `add-unit-tests` skill for the full contract. - **`@pytest.mark.unit` on every test.** The `unit` mark is the filter that keeps unit tests isolated from system tests that need Docker, GPUs, and sim licenses. ## Repository layout @@ -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/docker.md b/docs/simulation/isaac_sim/docker.md index 74edf49f8..8956aab34 100644 --- a/docs/simulation/isaac_sim/docker.md +++ b/docs/simulation/isaac_sim/docker.md @@ -12,6 +12,9 @@ simulation/isaac-sim/docker/ ├── fastdds.xml # DDS configuration ├── omni_pass.env # Omniverse credentials (git-ignored) ├── omni_pass_TEMPLATE.env # Template for credentials +├── sitl-files/ # SITL env bundles (PX4_PARAM_* via compose) +│ ├── default.env # Plain Pegasus SITL (no param overrides) +│ └── px4-vision.env # External vision / NatNet → MAVROS → EKF2 ├── omniverse.toml # Omniverse settings ├── user.config.json # Isaac Sim configuration (enables extensions) └── user_TEMPLATE.config.json # Template configuration @@ -116,11 +119,14 @@ Key variables for Isaac Sim configuration: | `ISAAC_SIM_SCRIPT_NAME` | Standalone script filename | - | | `PX4_PHYSICS_HZ` | Physics step rate for PX4 SITL — also sets PX4 `IMU_INTEG_RATE` | `250` | | `PX4_RENDERING_HZ` | Rendering frame rate for PX4 profiles (independent of physics) | `60` | +| `SITL_PARAM_PROFILE` | SITL env bundle stem under `sitl-files/` (e.g. `default`, `px4-vision`) | `default` | | `ARDUPILOT_PHYSICS_HZ` | Physics step rate for ArduPilot SITL | `800` | | `ARDUPILOT_RENDERING_HZ` | Rendering frame rate for ArduPilot profiles | `120` | `PX4_PHYSICS_HZ` and `PX4_RENDERING_HZ` are set in the top-level `.env`. Pegasus defaults to 250 Hz but AirStack runs PX4 at **100 Hz** for near-real-time performance. See [Pegasus Scene Setup → Physics Rate](pegasus_scene_setup.md) for valid values and the full configuration flow. +SITL parameter bundles live in `sitl-files/` (`default.env`, `px4-vision.env`). Compose loads `sitl-files/${SITL_PARAM_PROFILE}.env` into the container; see [Pegasus Scene Setup → PX4 SITL env bundles](pegasus_scene_setup.md#px4-sitl-env-bundles-external-vision). + **Example overrides:** ```bash diff --git a/docs/simulation/isaac_sim/pegasus_scene_setup.md b/docs/simulation/isaac_sim/pegasus_scene_setup.md index da4293dc7..8fe10efb1 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 SITL env bundles (external vision) + +Isaac Sim loads optional PX4 SITL parameter bundles from `simulation/isaac-sim/docker/sitl-files/`. Select the bundle with `SITL_PARAM_PROFILE` in the top-level `.env` (compose maps this to `sitl-files/.env`): + +```bash +# Plain Pegasus SITL (default.env — comments only, no PX4_PARAM_* overrides) +SITL_PARAM_PROFILE="default" + +# NatNet / mocap → MAVROS vision_pose → EKF2 external vision (px4-vision.env) +SITL_PARAM_PROFILE="px4-vision" +``` + +Bundles inject `PX4_PARAM_*` variables into the isaac-sim container; PX4 applies them at boot via `init.d-posix/rcS`. Pair `px4-vision` with robot-side `LAUNCH_NATNET=true` and `vision_pose.enabled: true` in the robot's `natnet_config.yaml` profile. + +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/docs/simulation/isaac_sim/spawning_drones.md b/docs/simulation/isaac_sim/spawning_drones.md index f5919ecec..e7baa6a17 100644 --- a/docs/simulation/isaac_sim/spawning_drones.md +++ b/docs/simulation/isaac_sim/spawning_drones.md @@ -5,8 +5,10 @@ The reference launch scripts under `simulation/isaac-sim/launch_scripts/` cover | Script | Purpose | |---|---| | `barebones_pegasus_launch.py` | Minimal Pegasus boilerplate. Single drone, default environment, no scene import. Use as a template for new launch scripts. | -| `example_one_px4_pegasus_launch_script.py` | One PX4 drone with the standard sensor stack (ZED stereo, optional Ouster lidar) in the default environment. | +| `example_one_px4_pegasus_launch_script.py` | One PX4 drone with the standard sensor stack (ZED stereo, RTX lidar) in the default environment. | | `example_multi_px4_pegasus_launch_script.py` | `NUM_ROBOTS` drones spawned in a row in the default environment. Each drone gets its own ROS domain id (`1..N`). | +| `example_one_px4_pegasus_natnet_launch_script.py` | Same as the one-drone script, plus OptiTrack NatNet streaming (drone + static target). Pair with `LAUNCH_NATNET=true` on the robot. | +| `example_multi_px4_pegasus_natnet_launch_script.py` | Same as the multi-drone script, plus NatNet streaming (one body per drone + shared `Target`). Use with the 3-profile `natnet_config.yaml` scaffolding for shared-target demos. | | `example_multi_drone_scene_import.py` | `NUM_ROBOTS` drones in an **imported scene** (USD from a Nucleus server) with per-drone GPS homes set via `gps_utils.set_gps_origins`. Use this as the starting point for any custom scene. | The first three are vanilla Pegasus patterns; this page focuses on the multi-drone + custom-scene case where you also need correct GPS homes. diff --git a/overrides/isaac-natnet-vision.env b/overrides/isaac-natnet-vision.env new file mode 100644 index 000000000..8650e8926 --- /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 +SITL_PARAM_PROFILE=px4-vision +ISAAC_SIM_USE_STANDALONE=true +ISAAC_SIM_SCRIPT_NAME=example_one_px4_pegasus_natnet_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..c34202b18 100644 --- a/robot/ros_ws/src/perception/natnet_ros2/README.md +++ b/robot/ros_ws/src/perception/natnet_ros2/README.md @@ -15,8 +15,9 @@ This module provides a bridge between OptiTrack Motive motion capture systems an - Receives **NatNet UDP packets** from an external Motive PC (configurable IP/port) - **Decodes motion capture frames** containing rigid body positions and orientations - **Publishes pose data** to the AirStack perception layer in standard ROS 2 formats -- **Supports multi-robot** via ROBOT_NAME namespacing -- **Optionally bridges** to MAVROS for PX4 external pose feedback +- **Tracks multiple rigid bodies per robot** (e.g. a drone for state estimation plus a separate target), each mapped to its own topic +- **Supports multi-robot** via per-robot profiles selected by `ROBOT_NAME` +- **Optionally bridges** to MAVROS for PX4 external pose feedback (per-robot) - **Respects OptiTrack licensing** by keeping the NatNet SDK external (host-side download with explicit consent) ## Architecture @@ -25,13 +26,16 @@ This module provides a bridge between OptiTrack Motive motion capture systems an Motive (External PC) ↓ NatNet UDP (port 1511) ↓ -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) - vision_pose_converter_node - ├→ /robot_1/mavros/vision_pose/pose - └→ /robot_1/mavros/vision_pose/pose_cov +NatNet ROS 2 Node (loads the ROBOT_NAME profile from natnet_config.yaml) + │ per configured body (one or more): + ├→ /{ROBOT_NAME}/{topic} (PoseStamped, when pose: true) + ├→ /{ROBOT_NAME}/{topic}/pose_cov (PoseWithCovarianceStamped, when pose_cov: true) + └→ (Optional, vision_pose.enabled: true) + mavros_gp_origin_node + └→ /{ROBOT_NAME}/interface/mavros/global_position/set_gp_origin + vision_pose_converter_node (reads input/output topics from the profile) + ├→ /{ROBOT_NAME}/interface/mavros/vision_pose/pose + └→ /{ROBOT_NAME}/interface/mavros/vision_pose/pose_cov ``` ## Interfaces @@ -39,63 +43,97 @@ NatNet ROS 2 Node ### Inputs - **Network**: NatNet UDP stream from Motive PC (external network) -- **Configuration**: `natnet_config.yaml` with server IP, ports, `body_name`, and covariance +- **Configuration**: `natnet_config.yaml` — generic `server` settings plus a `robots` map of per-robot profiles (body list + optional MAVROS `vision_pose` block). The launch file selects the profile matching `ROBOT_NAME`. ### Outputs -For each tracked rigid body `{body_name}` from Motive: +For each rigid body in the robot's profile, `topic` is a **relative** leaf namespaced +under `/{ROBOT_NAME}/` (it defaults to `perception/optitrack/{rigid_body_name}` when +omitted): -#### Direct OptiTrack pose (optional) +#### Direct OptiTrack pose -- **Topic**: `/{ROBOT_NAME}/perception/optitrack/{body_name}` +- **Topic**: `/{ROBOT_NAME}/{topic}` - **Type**: `geometry_msgs/PoseStamped` - **Description**: Position and orientation only (no covariance) -- **Enabled by**: `publish_direct_optitrack: true` in config (default: `true`) +- **Enabled by**: `pose: true` on that body (per body) -#### Pose with covariance (always) +#### Pose with covariance -- **Topic**: `/{ROBOT_NAME}/perception/optitrack/{body_name}/pose_cov` +- **Topic**: `/{ROBOT_NAME}/{topic}/pose_cov` - **Type**: `geometry_msgs/PoseWithCovarianceStamped` -- **Description**: Same pose as above plus a 6×6 covariance matrix (`position_covariance` and `orientation_covariance` from config). Published whenever the rigid body is tracked — independent of `publish_direct_optitrack` and `publish_to_mavros`. +- **Description**: Same pose plus a 6×6 covariance matrix from that body's `position_covariance` / `orientation_covariance`. +- **Enabled by**: `pose_cov: true` on that body (per body) -#### MAVROS vision pose bridge (optional) +#### MAVROS vision pose bridge (optional, per robot) -When `publish_to_mavros: true`, `vision_pose_converter_node` subscribes to `pose_cov` and republishes for PX4: +When the robot's `vision_pose.enabled: true`, `vision_pose_converter_node` subscribes to the configured `input_topic` (a body's `pose_cov`) and republishes for PX4 on the configured outputs: -- **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) -- **Enabled by**: `publish_to_mavros: true` in config +- **Topic** (`output_pose_topic`): `/{ROBOT_NAME}/interface/mavros/vision_pose/pose` — `geometry_msgs/PoseStamped` (pose extracted from the covariance message) +- **Topic** (`output_pose_cov_topic`): `/{ROBOT_NAME}/interface/mavros/vision_pose/pose_cov` — `geometry_msgs/PoseWithCovarianceStamped` (full message, quaternion optionally canonicalized) +- **Enabled by**: `vision_pose.enabled: true` in the robot's profile +- **Retargetable**: change `input_topic` / `output_pose_topic` / `output_pose_cov_topic` (relative, namespaced) to bridge to other middleware +- **PX4 side**: set `SITL_PARAM_PROFILE=px4-vision` in `.env` so Isaac SITL loads EKF2 external-vision params from `simulation/isaac-sim/docker/sitl-files/px4-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 `vision_pose.enabled: 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 -Edit `config/natnet_config.yaml`: +`config/natnet_config.yaml` uses a custom `natnet:` schema (not a flat ROS 2 param +file): generic `server` settings shared by every agent, then a `robots` map of +per-robot profiles. The launch file parses it, selects the profile matching the +container's `ROBOT_NAME`, flattens the body list into node parameters, and brings up +the MAVROS bridge only when that robot's `vision_pose.enabled` is true. ```yaml -/**: - ros__parameters: - server_ip: "192.168.1.100" # IP of the Motive PC +natnet: + server: # generic across all agents + server_ip: "$(env NATNET_SERVER_IP 172.31.0.200)" client_ip: "0.0.0.0" command_port: 1510 data_port: 1511 - connection_type: "unicast" # or "multicast" - - body_name: "Drone" # rigid body name in Motive (case-sensitive) - body_id: -1 # -1 = publish all bodies in the frame - - publish_direct_optitrack: true # PoseStamped on …/optitrack/{body_name} - publish_to_mavros: false # include vision_pose_converter → MAVROS - + connection_type: "unicast" # or "multicast" + multicast_address: "239.255.42.99" frame_id: "world" - - position_covariance: [0.1, 0.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.1] - orientation_covariance: [0.01, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.01] + debug: false + robots: + robot_1: + vision_pose: # per-robot MAVROS bridge (omit/false to skip) + enabled: true + input_topic: "perception/optitrack/drone/pose_cov" + output_pose_topic: "interface/mavros/vision_pose/pose" + output_pose_cov_topic: "interface/mavros/vision_pose/pose_cov" + bodies: # one or more tracked rigid bodies + - rigid_body_name: "Drone" # Motive name (case-sensitive) + id: 1 # Motive streaming id + topic: "perception/optitrack/drone" # relative → /{ROBOT_NAME}/ + pose: true # publish PoseStamped + pose_cov: true # publish PoseWithCovarianceStamped + position_covariance: [1.0e-6, 0, 0, 0, 1.0e-6, 0, 0, 0, 1.0e-6] + orientation_covariance: [3.0e-6, 0, 0, 0, 3.0e-6, 0, 0, 0, 3.0e-6] ``` +To track an additional body (e.g. a target) for a robot, add another entry under that +robot's `bodies`. To add a robot, add a new key under `robots`. The shipped file +includes commented scaffolding for a 3-drone fleet where `robot_1` and `robot_2` also +track a shared `Target` body and `robot_3` tracks only its drone. + ## Launch ### Basic launch -Parameters come from `config/natnet_config.yaml` (network, body, covariance). Optional overrides: +Parameters come from `config/natnet_config.yaml` (server + the `ROBOT_NAME` profile). Optional overrides: ```bash ros2 launch natnet_ros2 natnet_ros2.launch.py \ @@ -106,7 +144,7 @@ ros2 launch natnet_ros2 natnet_ros2.launch.py \ ### MAVROS bridge -Set `publish_to_mavros: true` in `natnet_config.yaml`. The launch file reads `publish_to_mavros` and `body_name` from that YAML to decide whether to include `vision_pose_converter.launch.xml`. +Set `vision_pose.enabled: true` in the robot's profile. The launch file includes `vision_pose_converter.launch.xml` (and `mavros_gp_origin.launch.xml`) and forwards the profile's `input_topic` / `output_pose_topic` / `output_pose_cov_topic`. ### From perception bringup @@ -140,13 +178,18 @@ The SDK will be installed into `robot/ros_ws/src/perception/natnet_ros2/lib/` an ### Multi-Robot Support Each container instance gets its own `ROBOT_NAME` and `ROS_DOMAIN_ID`: -- Topics: `/{ROBOT_NAME}/perception/optitrack/{body_name}` and `/{ROBOT_NAME}/perception/optitrack/{body_name}/pose_cov` -- Supported via launch file argument forwarding +- The node loads the `robots[$ROBOT_NAME]` profile, so each robot tracks only the bodies (and runs the MAVROS bridge) configured for it. +- Topics are namespaced under `/{ROBOT_NAME}/` from each body's relative `topic`. +- Set `NUM_ROBOTS=N`; each replica resolves its own `ROBOT_NAME` (via `resolve_robot_name.py`) and auto-selects its profile — no per-robot env overrides. ### Error Handling - 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 @@ -157,9 +200,9 @@ Each container instance gets its own `ROBOT_NAME` and `ROS_DOMAIN_ID`: ```bash ros2 launch natnet_ros2 natnet_ros2.launch.py ``` -4. Verify topics: +4. Verify topics (default profile maps the `Drone` body to `perception/optitrack/drone`): ```bash - ros2 topic echo /robot_1/perception/optitrack/Drone/pose_cov + ros2 topic echo /robot_1/perception/optitrack/drone/pose_cov ``` ### Without Real Hardware (Mock) @@ -167,7 +210,7 @@ TODO: Implement Motive simulator in Isaac Sim to generate fake NatNet packets ## Known Limitations -- When `body_id: -1`, all rigid bodies in the Motive frame get publishers; filter by subscribing to the `{body_name}` you care about +- The node publishes only bodies listed in the robot's profile (matched by `id`); bodies streamed by Motive but absent from the profile are ignored. - MAVROS bridge applies frame_id override and quaternion canonicalization; full PX4 frame alignment may still need tuning per airframe - No support for skeleton tracking or labeled markers yet (future enhancement) 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..09d2ac3e5 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 @@ -1,51 +1,108 @@ -# NatNet ROS 2 parameters — loaded by natnet_ros2.launch.py (NatNet node + MAVROS gate). -# publish_to_mavros / body_name are read by the launch file to decide vision_pose_converter include. +# NatNet ROS 2 configuration — parsed by natnet_ros2.launch.py. # -# Use /** so parameters apply regardless of namespace (e.g. /robot_1/perception/natnet_ros2_node). -# See: https://docs.ros.org/en/humble/Tutorials/Beginner-CLI-Tools/Understanding-ROS2-Parameters.html - -/**: - 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" - # Motive learns unicast destination from outbound UDP source IP — bind explicitly when you have - # multiple NICs (e.g. Docker 172.17.* vs LAN). +# Unlike a plain ROS 2 parameter file, this uses a custom `natnet:` schema so a +# single file can describe multiple robots, each tracking multiple rigid bodies. +# The launch file selects the profile matching the container's ROBOT_NAME, flattens +# it into node parameters, and (optionally) brings up the MAVROS vision_pose bridge. +# +# Schema: +# natnet: +# server: generic connection settings shared by every agent +# robots: one profile per ROBOT_NAME +# : +# vision_pose: whether/how to forward a body to MAVROS (optional) +# bodies: rigid bodies this robot subscribes to (one or more) + +natnet: + + # --- Connection settings (generic across all agents) ----------------------- + server: + # IP of the PC running Motive (OptiTrack server). 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 the unicast destination from the outbound UDP source IP — bind + # explicitly when the client has multiple NICs (e.g. Docker 172.17.* vs LAN). client_ip: "0.0.0.0" command_port: 1510 data_port: 1511 - # "unicast" — point-to-point; Motive streams directly to this machine's IP. - # Requires Motive unicast streaming enabled and client_ip set - # to the correct NIC when multiple interfaces are present. - # "multicast" — Motive broadcasts to a multicast group; any machine on the - # subnet that joins the group receives all body data. - # Use for multi-robot setups where every robot receives the - # full frame and filters by body_id. + # "unicast" — point-to-point; Motive streams directly to this machine (default). + # "multicast" — Motive broadcasts to a group; every robot receives the full frame + # and filters by the body ids in its profile. Use for multi-robot. connection_type: "unicast" - - # Only used when connection_type = "multicast". - # Must match Motive > Edit > Preferences > Data Streaming > Multicast Interface. - # OptiTrack default is 239.255.42.99. + # Only used when connection_type = "multicast" (OptiTrack default 239.255.42.99). multicast_address: "239.255.42.99" - # Name of the rigid body as defined in Motive. Must match exactly (case-sensitive). - body_name: "Drone" - body_id: -1 - - publish_direct_optitrack: true - publish_to_mavros: true - + # Frame applied to every published pose. debug enables per-frame logging. frame_id: "world" debug: false - position_covariance: - [0.1, 0.0, 0.0, - 0.0, 0.1, 0.0, - 0.0, 0.0, 0.1] + # --- Per-robot profiles (selected by ROBOT_NAME) --------------------------- + robots: + + robot_1: + # MAVROS vision_pose bridge. enabled=false skips the converter entirely. + # input_topic is one of this robot's body pose_cov topics; the two outputs map + # to MAVROS's vision_pose/pose (PoseStamped) and vision_pose/pose_cov + # (PoseWithCovarianceStamped) subscribers. Topics are relative and namespaced + # under /{ROBOT_NAME}/ — redirect them to interface with other middleware. + vision_pose: + enabled: true + input_topic: "perception/optitrack/drone/pose_cov" + output_pose_topic: "interface/mavros/vision_pose/pose" + output_pose_cov_topic: "interface/mavros/vision_pose/pose_cov" + + # Rigid bodies this robot tracks. rigid_body_name must match Motive exactly + # (case-sensitive). topic is a relative leaf namespaced under /{ROBOT_NAME}/; + # pose / pose_cov toggle the PoseStamped and PoseWithCovarianceStamped variants. + bodies: + - rigid_body_name: "Drone" + id: 1 + topic: "perception/optitrack/drone" + pose: true + pose_cov: true + # Covariances are per-body. Sub-0.1 mm / sub-0.1 deg for OptiTrack precision. + position_covariance: + [1.0e-6, 0.0, 0.0, + 0.0, 1.0e-6, 0.0, + 0.0, 0.0, 1.0e-6] + orientation_covariance: + [3.0e-6, 0.0, 0.0, + 0.0, 3.0e-6, 0.0, + 0.0, 0.0, 3.0e-6] - orientation_covariance: - [0.01, 0.0, 0.0, - 0.0, 0.01, 0.0, - 0.0, 0.0, 0.01] + # --- Scaffolding: a 3-drone fleet where robot_1 & robot_2 also track a shared + # "Target" body and robot_3 tracks only its drone. Uncomment + set NUM_ROBOTS=3 + # (and stream the matching bodies from example_multi_px4_pegasus_natnet_launch_script.py). + # + # robot_2: + # vision_pose: + # enabled: true + # input_topic: "perception/optitrack/drone/pose_cov" + # output_pose_topic: "interface/mavros/vision_pose/pose" + # output_pose_cov_topic: "interface/mavros/vision_pose/pose_cov" + # bodies: + # - rigid_body_name: "Drone2" + # id: 2 + # topic: "perception/optitrack/drone" + # pose: true + # pose_cov: true + # - rigid_body_name: "Target" # shared target — also tracked by robot_1 + # id: 100 + # topic: "perception/optitrack/target" + # pose: true + # pose_cov: false + # + # robot_3: + # vision_pose: + # enabled: true + # input_topic: "perception/optitrack/drone/pose_cov" + # output_pose_topic: "interface/mavros/vision_pose/pose" + # output_pose_cov_topic: "interface/mavros/vision_pose/pose_cov" + # bodies: + # - rigid_body_name: "Drone3" + # id: 3 + # topic: "perception/optitrack/drone" + # pose: true + # pose_cov: true diff --git a/robot/ros_ws/src/perception/natnet_ros2/include/natnet_ros2/natnet_logic.hpp b/robot/ros_ws/src/perception/natnet_ros2/include/natnet_ros2/natnet_logic.hpp index f23216565..a0cf8927f 100644 --- a/robot/ros_ws/src/perception/natnet_ros2/include/natnet_ros2/natnet_logic.hpp +++ b/robot/ros_ws/src/perception/natnet_ros2/include/natnet_ros2/natnet_logic.hpp @@ -34,6 +34,7 @@ #pragma once +#include #include #include #include @@ -91,6 +92,34 @@ inline std::string optitrack_pose_cov_topic( return optitrack_topic_base(robot_name, body_name) + "/pose_cov"; } +/// Namespace a relative topic leaf under /{robot_name}/. +/// +/// Leading slashes in \p relative are stripped so the result always has exactly +/// one. Used for the per-body ``topic`` overrides in natnet_config.yaml, which are +/// relative and namespaced by the node at runtime. +inline std::string namespaced_topic( + const std::string & robot_name, + const std::string & relative) +{ + const std::size_t start = relative.find_first_not_of('/'); + const std::string leaf = + (start == std::string::npos) ? std::string{} : relative.substr(start); + return "/" + robot_name + "/" + leaf; +} + +/// Topic base for one configured body: the per-body relative override when set, +/// otherwise the default /{robot_name}/perception/optitrack/{body_name}. +inline std::string body_topic_base( + const std::string & robot_name, + const std::string & body_name, + const std::string & relative_override) +{ + if (relative_override.empty()) { + return optitrack_topic_base(robot_name, body_name); + } + return namespaced_topic(robot_name, relative_override); +} + // =========================================================================== // 3. Connection-configuration helpers @@ -199,6 +228,16 @@ inline bool should_publish_body(int32_t filter_id, int32_t rb_id) return filter_id < 0 || rb_id == filter_id; } +/// Returns true when rb_id is one of the configured body ids. +/// +/// Multi-body variant of should_publish_body(): the node tracks a fixed set of +/// ids from natnet_config.yaml and publishes only those (empty set → nothing). +inline bool body_is_configured(const std::vector & configured_ids, int32_t rb_id) +{ + return std::find(configured_ids.begin(), configured_ids.end(), rb_id) + != configured_ids.end(); +} + /// Double-precision pose extracted from a RigidBodySample. struct PoseData { 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..6e49fcf88 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 @@ -1,5 +1,10 @@ #!/usr/bin/env python3 -"""Bring up NatNet node; optionally MAVROS bridge per natnet_config.yaml. +"""Bring up the NatNet node from natnet_config.yaml; optionally the MAVROS bridge. + +The config uses a custom ``natnet:`` schema (server settings + per-robot profiles), +so this launch file parses it, selects the profile matching ``ROBOT_NAME``, flattens +the body list into node parameters, and — when the robot's ``vision_pose`` block is +enabled — includes the MAVROS GP-origin + vision_pose_converter bridges. natnet_ros2_node is a C++ executable that requires the OptiTrack NatNet SDK. If the SDK was not installed (``airstack setup`` not run) and the workspace @@ -10,8 +15,9 @@ from __future__ import annotations import os +import re from pathlib import Path -from typing import cast +from typing import Any, cast import yaml from ament_index_python.packages import get_package_share_directory @@ -20,11 +26,28 @@ from launch.launch_description_sources import FrontendLaunchDescriptionSource from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node -from launch_ros.parameter_descriptions import ParameterFile +# Per-body covariance fallback when a body omits its own (sub-0.1 mm / sub-0.1 deg). +_DEFAULT_POSITION_COVARIANCE = [1.0e-6, 0.0, 0.0, 0.0, 1.0e-6, 0.0, 0.0, 0.0, 1.0e-6] +_DEFAULT_ORIENTATION_COVARIANCE = [3.0e-6, 0.0, 0.0, 0.0, 3.0e-6, 0.0, 0.0, 0.0, 3.0e-6] + +_ENV_SUBST = re.compile(r"\$\(env\s+(\w+)(?:\s+([^)]*))?\)") + + +def _expand_env(value: Any) -> Any: + """Expand ``$(env VAR default)`` tokens in a string using os.environ.""" + if not isinstance(value, str): + return value + + def _replace(match: re.Match) -> str: + var, default = match.group(1), match.group(2) + return os.environ.get(var, default if default is not None else "") -def _ros_params_from_file(config_path: str) -> dict: - """Parse /** / ros__parameters block from a ROS 2 parameter YAML.""" + return _ENV_SUBST.sub(_replace, value) + + +def _load_natnet_config(config_path: str) -> dict: + """Parse the ``natnet:`` block from the config YAML.""" path = Path(config_path) if not path.is_file(): return {} @@ -32,34 +55,110 @@ def _ros_params_from_file(config_path: str) -> dict: data = yaml.safe_load(f) if not isinstance(data, dict): return {} - block = data.get('/**') - if not isinstance(block, dict): - return {} - params = block.get('ros__parameters', {}) - return cast(dict, params) if isinstance(params, dict) else {} + natnet = data.get('natnet', {}) + return cast(dict, natnet) if isinstance(natnet, dict) else {} + + +def _flatten_covariance(values: Any, fallback: list[float]) -> list[float]: + """Coerce a 9-element covariance block to floats, falling back when absent.""" + if not isinstance(values, (list, tuple)) or len(values) == 0: + return list(fallback) + return [float(v) for v in values] + + +def _build_node_params(server: dict, profile: dict) -> dict: + """Flatten the server block + a robot's body list into node parameters.""" + bodies = profile.get('bodies', []) or [] + + params: dict[str, Any] = { + 'server_ip': str(_expand_env(server.get('server_ip', '172.31.0.200'))), + 'client_ip': str(_expand_env(server.get('client_ip', '0.0.0.0'))), + 'command_port': int(server.get('command_port', 1510)), + 'data_port': int(server.get('data_port', 1511)), + 'connection_type': str(server.get('connection_type', 'unicast')), + 'multicast_address': str(server.get('multicast_address', '239.255.42.99')), + 'frame_id': str(server.get('frame_id', 'world')), + 'debug': bool(server.get('debug', False)), + } + + body_names: list[str] = [] + body_ids: list[int] = [] + body_topics: list[str] = [] + body_pose: list[bool] = [] + body_pose_cov: list[bool] = [] + body_position_covariance: list[float] = [] + body_orientation_covariance: list[float] = [] + + for body in bodies: + body_names.append(str(body.get('rigid_body_name', ''))) + body_ids.append(int(body.get('id', -1))) + body_topics.append(str(body.get('topic', ''))) + body_pose.append(bool(body.get('pose', True))) + body_pose_cov.append(bool(body.get('pose_cov', True))) + body_position_covariance.extend( + _flatten_covariance(body.get('position_covariance'), _DEFAULT_POSITION_COVARIANCE) + ) + body_orientation_covariance.extend( + _flatten_covariance(body.get('orientation_covariance'), _DEFAULT_ORIENTATION_COVARIANCE) + ) + + params.update( + { + 'body_names': body_names, + 'body_ids': body_ids, + 'body_topics': body_topics, + 'body_pose': body_pose, + 'body_pose_cov': body_pose_cov, + 'body_position_covariance': body_position_covariance, + 'body_orientation_covariance': body_orientation_covariance, + } + ) + return params + + +def _namespaced(robot_name: str, relative: str) -> str: + """Namespace a relative topic under /{robot_name}/.""" + return '/' + robot_name + '/' + relative.lstrip('/') 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) - publish_mavros = bool(ros_params.get('publish_to_mavros', False)) - body_name = str(ros_params.get('body_name', 'robot_1')) + robot_name = os.environ.get('ROBOT_NAME', 'robot_1') + natnet = _load_natnet_config(cfg_path) + server = natnet.get('server', {}) if isinstance(natnet, dict) else {} + robots = natnet.get('robots', {}) if isinstance(natnet, dict) else {} + profile = robots.get(robot_name, {}) if isinstance(robots, dict) else {} + + if not profile: + print( + f"[natnet_ros2.launch] WARNING: no profile for ROBOT_NAME='{robot_name}' " + f"in {cfg_path}; node will start with no tracked bodies." + ) + + node_params = _build_node_params(server, profile) + # launch_ros / rclpy cannot infer the type of an empty-list parameter, so drop + # any empty arrays; the node declares matching empty defaults and tracks nothing. + node_params = { + k: v for k, v in node_params.items() if not (isinstance(v, list) and len(v) == 0) + } # pkg_share = /share/natnet_ros2 → go up two levels to reach , # then down into lib/natnet_ros2/ where colcon installs executables. - pkg_share = get_package_share_directory('natnet_ros2') node_path = Path(pkg_share).parent.parent / 'lib' / 'natnet_ros2' / 'natnet_ros2_node' if not node_path.exists(): raise RuntimeError( @@ -75,11 +174,34 @@ def launch_setup(context, *_args, **_kwargs): executable='natnet_ros2_node', name='natnet_ros2_node', output='screen', - parameters=[ParameterFile(config_file, allow_substs=True)], + parameters=[node_params], ), ] - if publish_mavros: + vision_pose = profile.get('vision_pose', {}) if isinstance(profile, dict) else {} + if vision_pose.get('enabled', False): + input_topic = _namespaced( + robot_name, str(vision_pose.get('input_topic', 'perception/optitrack/drone/pose_cov')) + ) + output_pose_topic = _namespaced( + robot_name, str(vision_pose.get('output_pose_topic', 'interface/mavros/vision_pose/pose')) + ) + output_pose_cov_topic = _namespaced( + robot_name, + str(vision_pose.get('output_pose_cov_topic', 'interface/mavros/vision_pose/pose_cov')), + ) + + 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( @@ -87,7 +209,9 @@ def launch_setup(context, *_args, **_kwargs): ), launch_arguments=[ ('config_file', vp_path), - ('body_name', body_name), + ('input_topic', input_topic), + ('output_pose_topic', output_pose_topic), + ('output_pose_cov_topic', output_pose_cov_topic), ('use_sim_time', ust), ], ), @@ -99,18 +223,23 @@ def launch_setup(context, *_args, **_kwargs): DeclareLaunchArgument( 'config_file', default_value=default_natnet_yaml, - description='NatNet parameter YAML (/** ros__parameters). ' - 'publish_to_mavros and body_name control MAVROS include.', + description='NatNet config YAML (natnet: server + per-robot profiles). ' + 'The robot profile selected by ROBOT_NAME drives bodies + MAVROS include.', ), DeclareLaunchArgument( 'vision_pose_config_file', default_value=default_vp_yaml, - description='vision_pose_converter parameter YAML.', + description='vision_pose_converter parameter YAML (frame_id, canonical_quaternion).', + ), + 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..803cbdad7 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 @@ -3,26 +3,31 @@ - + + + - - - + + + 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..d754a1cef 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 @@ -2,25 +2,27 @@ // // ROS 2 NatNet SDK node for OptiTrack Motive integration. // -// Published topics (per tracked rigid body): -// /{robot_name}/perception/optitrack/{body_name} → PoseStamped -// /{robot_name}/perception/optitrack/{body_name}/pose_cov → PoseWithCovarianceStamped +// Published topics (per configured rigid body): +// /{robot_name}/{topic} → PoseStamped (when pose=true) +// /{robot_name}/{topic}/pose_cov → PoseWithCovarianceStamped (when pose_cov=true) +// where {topic} defaults to perception/optitrack/{rigid_body_name} when unset. // -// Parameters (see config/natnet_config.yaml): -// server_ip, client_ip, command_port, data_port, -// body_name, body_id (-1 = all), publish_direct_optitrack, -// frame_id, debug, position_covariance, orientation_covariance +// Parameters are flattened from config/natnet_config.yaml by natnet_ros2.launch.py: +// server_ip, client_ip, command_port, data_port, connection_type, +// multicast_address, frame_id, debug, and parallel per-body arrays: +// body_names[], body_ids[], body_topics[], body_pose[], body_pose_cov[], +// body_position_covariance[] / body_orientation_covariance[] (9·N, sliced per body). // // ROBOT_NAME is read from the environment variable set by AirStack's // robot_name_map resolver at container startup. +#include #include -#include #include #include -#include #include #include +#include // ROS 2 #include "rclcpp/rclcpp.hpp" @@ -41,8 +43,14 @@ class NatNetROS2Node : public rclcpp::Node using PoseStamped = geometry_msgs::msg::PoseStamped; using PoseWithCovarianceStamped = geometry_msgs::msg::PoseWithCovarianceStamped; - struct BodyPublishers + struct BodyConfig { + int32_t id = -1; + std::string rigid_body_name; + std::string topic_base; + bool publish_pose = true; + bool publish_pose_cov = true; + std::array covariance{}; rclcpp::Publisher::SharedPtr pose_pub; rclcpp::Publisher::SharedPtr pose_cov_pub; }; @@ -52,24 +60,23 @@ class NatNetROS2Node : public rclcpp::Node : Node("natnet_ros2_node") { // ----- Parameters -------------------------------------------------- - this->declare_parameter("server_ip", "192.168.1.1"); - this->declare_parameter("client_ip", "0.0.0.0"); - this->declare_parameter("command_port", 1510); - this->declare_parameter("data_port", 1511); - this->declare_parameter("connection_type", std::string("unicast")); - this->declare_parameter("multicast_address", std::string("239.255.42.99")); - this->declare_parameter("body_name", "robot_1"); - this->declare_parameter("body_id", -1); - this->declare_parameter("publish_direct_optitrack", true); - this->declare_parameter("publish_to_mavros", false); - this->declare_parameter("frame_id", "world"); - this->declare_parameter("debug", false); - this->declare_parameter( - "position_covariance", - std::vector{0.1,0.,0., 0.,0.1,0., 0.,0.,0.1}); - this->declare_parameter( - "orientation_covariance", - std::vector{0.01,0.,0., 0.,0.01,0., 0.,0.,0.01}); + this->declare_parameter("server_ip", "192.168.1.1"); + this->declare_parameter("client_ip", "0.0.0.0"); + this->declare_parameter("command_port", 1510); + this->declare_parameter("data_port", 1511); + this->declare_parameter("connection_type", std::string("unicast")); + this->declare_parameter("multicast_address", std::string("239.255.42.99")); + this->declare_parameter("frame_id", "world"); + this->declare_parameter("debug", false); + + // Parallel per-body arrays (flattened from natnet_config.yaml by the launch file). + this->declare_parameter("body_names", std::vector{}); + this->declare_parameter("body_ids", std::vector{}); + this->declare_parameter("body_topics", std::vector{}); + this->declare_parameter("body_pose", std::vector{}); + this->declare_parameter("body_pose_cov", std::vector{}); + this->declare_parameter("body_position_covariance", std::vector{}); + this->declare_parameter("body_orientation_covariance", std::vector{}); // ----- Read parameters --------------------------------------------- const auto connect_cfg = natnet_ros2::make_connect_config( @@ -88,19 +95,14 @@ class NatNetROS2Node : public rclcpp::Node this->get_parameter("connection_type").as_string().c_str()); } - body_name_ = this->get_parameter("body_name").as_string(); - body_id_ = static_cast(this->get_parameter("body_id").as_int()); - publish_direct_ = this->get_parameter("publish_direct_optitrack").as_bool(); - frame_id_ = this->get_parameter("frame_id").as_string(); - debug_ = this->get_parameter("debug").as_bool(); - - covariance_6x6_ = natnet_ros2::build_covariance_6x6( - this->get_parameter("position_covariance").as_double_array(), - this->get_parameter("orientation_covariance").as_double_array()); + frame_id_ = this->get_parameter("frame_id").as_string(); + debug_ = this->get_parameter("debug").as_bool(); const char * rn = std::getenv("ROBOT_NAME"); robot_name_ = rn ? rn : "robot_1"; + build_body_configs(); + RCLCPP_INFO(get_logger(), "========================================="); RCLCPP_INFO(get_logger(), "NatNet ROS 2 Node"); RCLCPP_INFO(get_logger(), " robot_name: %s", robot_name_.c_str()); @@ -110,18 +112,19 @@ class NatNetROS2Node : public rclcpp::Node if (natnet_ros2::is_multicast(connect_cfg)) { RCLCPP_INFO(get_logger(), " multicast_addr: %s", connect_cfg.multicast_address.c_str()); } - RCLCPP_INFO(get_logger(), " body_id: %d (%s)", - static_cast(body_id_), - (body_id_ < 0) ? "track all" : "single body"); + RCLCPP_INFO(get_logger(), " tracked bodies: %zu", bodies_.size()); RCLCPP_INFO(get_logger(), "========================================="); // Production client — NatNetClientAdapter wraps the SDK client_ = std::make_unique(); - connect_and_setup(connect_cfg); + connect_cfg_ = connect_cfg; - refresh_timer_ = this->create_wall_timer( - std::chrono::seconds(1), - std::bind(&NatNetROS2Node::refresh_descriptions_if_needed, this)); + // 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)); + } } // ----------------------------------------------------------------------- @@ -132,14 +135,10 @@ class NatNetROS2Node : public rclcpp::Node // ----------------------------------------------------------------------- // Called from the NatNetClientAdapter's frame trampoline. - // publish() and Clock::now() are thread-safe; pub_mutex_ guards map access. + // publish() and Clock::now() are thread-safe; bodies_ is immutable after init. // ----------------------------------------------------------------------- void on_frame(const natnet_ros2::FrameSample & frame) { - if (natnet_ros2::model_list_changed(frame.params)) { - needs_description_refresh_.store(true, std::memory_order_relaxed); - } - if (debug_) { RCLCPP_DEBUG(get_logger(), "Frame %d: %zu rigid bodies, ts=%.4f s", frame.frame_num, frame.bodies.size(), static_cast(frame.timestamp)); @@ -154,20 +153,14 @@ class NatNetROS2Node : public rclcpp::Node } continue; } - if (!natnet_ros2::should_publish_body(body_id_, rb.id)) { continue; } - - std::lock_guard lock(pub_mutex_); - const auto pub_it = publishers_.find(rb.id); - if (pub_it == publishers_.end()) { - needs_description_refresh_.store(true, std::memory_order_relaxed); - continue; - } + const auto it = bodies_.find(rb.id); + if (it == bodies_.end()) { continue; } // not configured for this robot const natnet_ros2::PoseData pose = natnet_ros2::rb_to_pose(rb); - const BodyPublishers & bp = pub_it->second; + const BodyConfig & body = it->second; - if (publish_direct_ && bp.pose_pub) { + if (body.publish_pose && body.pose_pub) { PoseStamped msg; msg.header.frame_id = frame_id_; msg.header.stamp = stamp; @@ -178,10 +171,10 @@ class NatNetROS2Node : public rclcpp::Node msg.pose.orientation.y = pose.qy; msg.pose.orientation.z = pose.qz; msg.pose.orientation.w = pose.qw; - bp.pose_pub->publish(msg); + body.pose_pub->publish(msg); } - if (bp.pose_cov_pub) { + if (body.publish_pose_cov && body.pose_cov_pub) { PoseWithCovarianceStamped cov_msg; cov_msg.header.frame_id = frame_id_; cov_msg.header.stamp = stamp; @@ -192,22 +185,23 @@ class NatNetROS2Node : public rclcpp::Node cov_msg.pose.pose.orientation.y = pose.qy; cov_msg.pose.pose.orientation.z = pose.qz; cov_msg.pose.pose.orientation.w = pose.qw; - cov_msg.pose.covariance = covariance_6x6_; - bp.pose_cov_pub->publish(cov_msg); + cov_msg.pose.covariance = body.covariance; + body.pose_cov_pub->publish(cov_msg); } } } 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) { @@ -216,103 +210,119 @@ class NatNetROS2Node : public rclcpp::Node RCLCPP_WARN(get_logger(), "%s", neg.log_message.c_str()); } - refresh_descriptions_locked(); - 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; } // ----------------------------------------------------------------------- - void refresh_descriptions_if_needed() + // Timer-driven reconnect. + void retry_connect() { - if (!needs_description_refresh_.exchange(false, std::memory_order_relaxed)) { + if (connected_) { + if (connect_timer_) { connect_timer_->cancel(); } return; } - RCLCPP_INFO(get_logger(), "Model list change detected — refreshing data descriptions."); - std::lock_guard lock(pub_mutex_); - refresh_descriptions_locked(); + 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(); + } } // ----------------------------------------------------------------------- - // Must be called with pub_mutex_ held (or from single-threaded init). + // Build the per-body config map + publishers from the parallel param arrays. + // Publishers are created up front (config-driven), so streaming begins as soon + // as frames arrive — no dependency on Motive's data-description handshake. // ----------------------------------------------------------------------- - void refresh_descriptions_locked() + void build_body_configs() { - if (!client_) { return; } - - // Always ensure the statically-configured body has a publisher - if (body_id_ >= 0) { - ensure_publisher_locked(body_id_, body_name_); + const auto names = this->get_parameter("body_names").as_string_array(); + const auto ids = this->get_parameter("body_ids").as_integer_array(); + const auto topics = this->get_parameter("body_topics").as_string_array(); + const auto pose = this->get_parameter("body_pose").as_bool_array(); + const auto pose_cov = this->get_parameter("body_pose_cov").as_bool_array(); + const auto pos_cov = this->get_parameter("body_position_covariance").as_double_array(); + const auto ori_cov = this->get_parameter("body_orientation_covariance").as_double_array(); + + const std::size_t n = std::min(names.size(), ids.size()); + if (names.size() != ids.size()) { + RCLCPP_WARN(get_logger(), + "body_names (%zu) and body_ids (%zu) length mismatch — using %zu.", + names.size(), ids.size(), n); } - const auto bodies = client_->get_body_descriptors(); - int newly_created = 0; - for (const auto & bd : bodies) { - // Store name for every body (including skeleton bones) - body_names_[bd.id] = bd.name; + for (std::size_t i = 0; i < n; ++i) { + BodyConfig body; + body.id = static_cast(ids[i]); + body.rigid_body_name = names[i]; + body.publish_pose = (i < pose.size()) ? pose[i] : true; + body.publish_pose_cov = (i < pose_cov.size()) ? pose_cov[i] : true; - // Skip skeleton bones (parent_id >= 0) - if (bd.parent_id >= 0) { continue; } + const std::string relative = (i < topics.size()) ? topics[i] : std::string{}; + body.topic_base = + natnet_ros2::body_topic_base(robot_name_, body.rigid_body_name, relative); - // When tracking a single body, skip others - if (!natnet_ros2::should_publish_body(body_id_, bd.id)) { continue; } + body.covariance = natnet_ros2::build_covariance_6x6( + cov_slice(pos_cov, i, _DEFAULT_POSITION_COVARIANCE), + cov_slice(ori_cov, i, _DEFAULT_ORIENTATION_COVARIANCE)); - if (ensure_publisher_locked(bd.id, bd.name)) { ++newly_created; } - } + if (body.publish_pose) { + body.pose_pub = this->create_publisher(body.topic_base, 10); + } + if (body.publish_pose_cov) { + body.pose_cov_pub = this->create_publisher( + body.topic_base + "/pose_cov", 10); + } - if (newly_created > 0) { RCLCPP_INFO(get_logger(), - "Data descriptions refreshed: %d new publisher(s) created.", newly_created); - } else { - RCLCPP_DEBUG(get_logger(), "Data descriptions refreshed: no new publishers."); + "Tracking body id=%d name='%s' → %s (pose=%d pose_cov=%d)", + static_cast(body.id), body.rigid_body_name.c_str(), + body.topic_base.c_str(), + static_cast(body.publish_pose), + static_cast(body.publish_pose_cov)); + + bodies_.emplace(body.id, std::move(body)); } } // ----------------------------------------------------------------------- - bool ensure_publisher_locked(int32_t id, const std::string & name) + // Return the i-th 9-element covariance block from a flattened array, or the + // built-in default when the slice is missing. + static std::vector cov_slice( + const std::vector & flat, std::size_t i, const std::vector & fallback) { - if (publishers_.count(id)) { return false; } - - const std::string topic_base = - natnet_ros2::optitrack_topic_base(robot_name_, name); - - BodyPublishers bp; - if (publish_direct_) { - bp.pose_pub = this->create_publisher(topic_base, 10); - } - bp.pose_cov_pub = this->create_publisher( - natnet_ros2::optitrack_pose_cov_topic(robot_name_, name), 10); - - publishers_.emplace(id, std::move(bp)); - - RCLCPP_INFO(get_logger(), - "Publisher registered: id=%d name='%s' → %s[/pose_cov]", - static_cast(id), name.c_str(), topic_base.c_str()); - return true; + const std::size_t start = i * 9; + if (flat.size() < start + 9) { return fallback; } + return std::vector(flat.begin() + start, flat.begin() + start + 9); } // ----------------------------------------------------------------------- // Parameters / state - std::string body_name_; - int32_t body_id_ = -1; - bool publish_direct_ = true; std::string frame_id_; - bool debug_ = false; + bool debug_ = false; std::string robot_name_; - std::array covariance_6x6_{}; - std::unique_ptr client_; + natnet_ros2::ConnectConfig connect_cfg_; + bool connected_ = false; + + std::unordered_map bodies_; - std::mutex pub_mutex_; - std::unordered_map body_names_; - std::unordered_map publishers_; + rclcpp::TimerBase::SharedPtr connect_timer_; - std::atomic needs_description_refresh_{false}; - rclcpp::TimerBase::SharedPtr refresh_timer_; + static const std::vector _DEFAULT_POSITION_COVARIANCE; + static const std::vector _DEFAULT_ORIENTATION_COVARIANCE; }; +const std::vector NatNetROS2Node::_DEFAULT_POSITION_COVARIANCE = + {1.0e-6, 0., 0., 0., 1.0e-6, 0., 0., 0., 1.0e-6}; +const std::vector NatNetROS2Node::_DEFAULT_ORIENTATION_COVARIANCE = + {3.0e-6, 0., 0., 0., 3.0e-6, 0., 0., 0., 3.0e-6}; + // --------------------------------------------------------------------------- int main(int argc, char ** argv) diff --git a/robot/ros_ws/src/perception/natnet_ros2/src/vision_pose_converter_node.py b/robot/ros_ws/src/perception/natnet_ros2/src/vision_pose_converter_node.py index 9a36f879d..81d2df3dd 100755 --- a/robot/ros_ws/src/perception/natnet_ros2/src/vision_pose_converter_node.py +++ b/robot/ros_ws/src/perception/natnet_ros2/src/vision_pose_converter_node.py @@ -7,6 +7,12 @@ for PX4 external pose estimation and state fusion. Converts from NatNet coordinate frame to a frame suitable for MAVROS. + +Topics are configurable so the bridge can be retargeted to other middleware. +``input_topic`` / ``output_pose_topic`` / ``output_pose_cov_topic`` default to the +relative names ``input_pose`` / ``output_pose`` / ``output_pose_cov`` (remappable), +but natnet_ros2.launch.py overrides them with the absolute, ROBOT_NAME-namespaced +topics from the robot's ``vision_pose`` block in natnet_config.yaml. """ import rclpy @@ -28,15 +34,23 @@ def __init__(self): self.declare_parameter('frame_id', 'world') self.declare_parameter('child_frame_id', 'base_link') self.declare_parameter('canonical_quaternion', True) + # Topic names — overridden by the launch file from the per-robot + # vision_pose block; defaults are the historical remappable relative names. + self.declare_parameter('input_topic', 'input_pose') + self.declare_parameter('output_pose_topic', 'output_pose') + self.declare_parameter('output_pose_cov_topic', 'output_pose_cov') self.frame_id = self.get_parameter('frame_id').value self.child_frame_id = self.get_parameter('child_frame_id').value self.canonical_quaternion = self.get_parameter('canonical_quaternion').value + input_topic = self.get_parameter('input_topic').value + output_pose_topic = self.get_parameter('output_pose_topic').value + output_pose_cov_topic = self.get_parameter('output_pose_cov_topic').value # Subscribers self.pose_sub = self.create_subscription( PoseWithCovarianceStamped, - 'input_pose', + input_topic, self._on_pose, 10 ) @@ -44,19 +58,21 @@ def __init__(self): # Publishers self.pose_pub = self.create_publisher( PoseStamped, - 'output_pose', + output_pose_topic, 10 ) self.pose_cov_pub = self.create_publisher( PoseWithCovarianceStamped, - 'output_pose_cov', + output_pose_cov_topic, 10 ) self.get_logger().info( f'Vision pose converter started ' f'(frame_id={self.frame_id!r}, child_frame_id={self.child_frame_id!r}, ' - f'canonical_quaternion={self.canonical_quaternion})' + f'canonical_quaternion={self.canonical_quaternion}, ' + f'input_topic={input_topic!r}, output_pose_topic={output_pose_topic!r}, ' + f'output_pose_cov_topic={output_pose_cov_topic!r})' ) @staticmethod diff --git a/robot/ros_ws/src/perception/natnet_ros2/test/test_natnet_logic.cpp b/robot/ros_ws/src/perception/natnet_ros2/test/test_natnet_logic.cpp index 7a144ef9b..5796c121b 100644 --- a/robot/ros_ws/src/perception/natnet_ros2/test/test_natnet_logic.cpp +++ b/robot/ros_ws/src/perception/natnet_ros2/test/test_natnet_logic.cpp @@ -136,6 +136,46 @@ TEST(TopicNames, LeadingSlashPresent) EXPECT_EQ(optitrack_topic_base("robot_1", "Body")[0], '/'); } +TEST(TopicNames, NamespacedTopicStripsLeadingSlashes) +{ + EXPECT_EQ(namespaced_topic("robot_1", "perception/optitrack/drone"), + "/robot_1/perception/optitrack/drone"); + EXPECT_EQ(namespaced_topic("robot_1", "/perception/optitrack/drone"), + "/robot_1/perception/optitrack/drone"); + EXPECT_EQ(namespaced_topic("robot_2", "///a/b"), "/robot_2/a/b"); +} + +TEST(TopicNames, BodyTopicBaseUsesOverrideWhenSet) +{ + // Empty override → default perception/optitrack/{name} + EXPECT_EQ(body_topic_base("robot_1", "Drone", ""), + "/robot_1/perception/optitrack/Drone"); + // Non-empty override → namespaced relative leaf (decoupled from body name) + EXPECT_EQ(body_topic_base("robot_1", "Drone", "perception/optitrack/drone"), + "/robot_1/perception/optitrack/drone"); + EXPECT_EQ(body_topic_base("robot_3", "Target", "perception/optitrack/target"), + "/robot_3/perception/optitrack/target"); +} + +// =========================================================================== +// Multi-body filtering — body_is_configured +// =========================================================================== + +TEST(BodyIsConfigured, MatchesConfiguredIds) +{ + const std::vector ids = {1, 100}; + EXPECT_TRUE(body_is_configured(ids, 1)); + EXPECT_TRUE(body_is_configured(ids, 100)); + EXPECT_FALSE(body_is_configured(ids, 2)); +} + +TEST(BodyIsConfigured, EmptySetMatchesNothing) +{ + const std::vector ids = {}; + EXPECT_FALSE(body_is_configured(ids, 0)); + EXPECT_FALSE(body_is_configured(ids, 1)); +} + // =========================================================================== // Server negotiation — validate_connection_type diff --git a/robot/ros_ws/src/perception/natnet_ros2/test/test_natnet_ros2.py b/robot/ros_ws/src/perception/natnet_ros2/test/test_natnet_ros2.py index cba5a3444..37526cd18 100644 --- a/robot/ros_ws/src/perception/natnet_ros2/test/test_natnet_ros2.py +++ b/robot/ros_ws/src/perception/natnet_ros2/test/test_natnet_ros2.py @@ -1,20 +1,15 @@ # Copyright (c) 2024 Carnegie Mellon University # MIT License - see LICENSE in the repository root for full text. -"""Unit tests for natnet_ros2 Python source code. +"""Unit tests for natnet_ros2 Python helpers (no ROS install required). -These tests import the actual production source files and stub out ROS at the -import boundary so no ROS installation is required. +Stubs rclpy/launch at import time. Covers ``VisionPoseConverterNode`` quaternion +canonicalisation, configurable-topic wiring, and ``natnet_ros2.launch.py`` +profile-flattening helpers (server + per-body arrays, env expansion, namespacing). -Coverage here: - vision_pose_converter_node.py → VisionPoseConverterNode._canonical_quaternion() - → VisionPoseConverterNode._on_pose() frame_id assignment - -NOT covered here (C++ — requires colcon build + gtest): - natnet_ros2_node.cpp → build_covariance_6x6(), topic name construction, - connection_type validation, SDK frame callback logic. - These live in test_natnet_logic.cpp in the same test/ directory. +C++ logic (``natnet_logic.hpp``) is tested in ``test_natnet_logic.cpp`` via colcon. """ +import importlib.util import sys from pathlib import Path from types import SimpleNamespace @@ -28,22 +23,31 @@ # metaclass machinery returns a Mock for attribute access instead of running # __init_subclass__ / defining methods). We supply a real dummy base class # so the actual class body — including _canonical_quaternion — is defined. +# +# The fake also records declared params and created sub/pub topics so the +# configurable-topic wiring can be asserted without a ROS install. # --------------------------------------------------------------------------- class _FakeNode: + # Per-test parameter overrides keyed by name; consulted by declare_parameter so + # values survive the node's super().__init__ (which resets per-instance state). + _overrides: dict = {} + def __init__(self, name: str): - pass + self._params: dict = {} + self.created_subscriptions: list = [] + self.created_publishers: list = [] def get_logger(self): return MagicMock() - def declare_parameter(self, *args, **kwargs): - pass + def declare_parameter(self, name, default=None): + self._params[name] = self._overrides.get(name, default) def get_parameter(self, name): - m = MagicMock() - m.value = MagicMock() - return m - def create_subscription(self, *args, **kwargs): + return SimpleNamespace(value=self._params.get(name)) + def create_subscription(self, msg_type, topic, callback, qos): + self.created_subscriptions.append(topic) return MagicMock() - def create_publisher(self, *args, **kwargs): + def create_publisher(self, msg_type, topic, qos): + self.created_publishers.append(topic) return MagicMock() @@ -62,6 +66,36 @@ def create_publisher(self, *args, **kwargs): from vision_pose_converter_node import VisionPoseConverterNode # noqa: E402 +# --------------------------------------------------------------------------- +# Load natnet_ros2.launch.py with its heavy launch/ROS deps stubbed, so the +# pure flattening helpers can be unit-tested without a ROS install. +# --------------------------------------------------------------------------- + +for _mod in ( + "ament_index_python", + "ament_index_python.packages", + "launch", + "launch.actions", + "launch.launch_description_sources", + "launch.substitutions", + "launch_ros", + "launch_ros.actions", +): + sys.modules.setdefault(_mod, MagicMock()) + +# yaml is only needed by _load_natnet_config (not the flattening helpers); stub it +# if PyYAML is absent so the launch module still imports in a minimal unit env. +try: + import yaml # noqa: F401 +except ImportError: + sys.modules.setdefault("yaml", MagicMock()) + +_launch_path = Path(__file__).resolve().parent.parent / "launch" / "natnet_ros2.launch.py" +_spec = importlib.util.spec_from_file_location("natnet_ros2_launch_under_test", _launch_path) +natnet_launch = importlib.util.module_from_spec(_spec) +_spec.loader.exec_module(natnet_launch) + + # --------------------------------------------------------------------------- # Helpers # --------------------------------------------------------------------------- @@ -150,3 +184,104 @@ def test_canonical_quaternion_dual_sign_produces_same_result(): assert out_pos.x == pytest.approx(out_neg.x) assert out_pos.y == pytest.approx(out_neg.y) assert out_pos.z == pytest.approx(out_neg.z) + + +# --------------------------------------------------------------------------- +# VisionPoseConverterNode — configurable input/output topics +# --------------------------------------------------------------------------- + +@pytest.mark.unit +def test_vision_pose_converter_default_topics(): + """Defaults reproduce the historical relative (remappable) topic names.""" + node = VisionPoseConverterNode() + assert node.created_subscriptions == ["input_pose"] + assert node.created_publishers == ["output_pose", "output_pose_cov"] + + +@pytest.mark.unit +def test_vision_pose_converter_topic_overrides_applied(): + """When the topic params are set, sub/pub use those exact names.""" + _FakeNode._overrides = { + "input_topic": "/robot_2/perception/optitrack/drone/pose_cov", + "output_pose_topic": "/robot_2/custom/vision/pose", + "output_pose_cov_topic": "/robot_2/custom/vision/pose_cov", + } + try: + node = VisionPoseConverterNode() + finally: + _FakeNode._overrides = {} + assert node.created_subscriptions == ["/robot_2/perception/optitrack/drone/pose_cov"] + assert node.created_publishers == [ + "/robot_2/custom/vision/pose", + "/robot_2/custom/vision/pose_cov", + ] + + +# --------------------------------------------------------------------------- +# natnet_ros2.launch.py — pure config-flattening helpers +# --------------------------------------------------------------------------- + +@pytest.mark.unit +def test_expand_env_uses_default_when_unset(monkeypatch): + monkeypatch.delenv("NATNET_SERVER_IP", raising=False) + assert natnet_launch._expand_env("$(env NATNET_SERVER_IP 172.31.0.200)") == "172.31.0.200" + + +@pytest.mark.unit +def test_expand_env_uses_environment_value(monkeypatch): + monkeypatch.setenv("NATNET_SERVER_IP", "10.0.0.5") + assert natnet_launch._expand_env("$(env NATNET_SERVER_IP 172.31.0.200)") == "10.0.0.5" + + +@pytest.mark.unit +def test_namespaced_strips_and_prefixes(): + assert natnet_launch._namespaced("robot_1", "perception/optitrack/drone") == \ + "/robot_1/perception/optitrack/drone" + assert natnet_launch._namespaced("robot_2", "/already/abs") == "/robot_2/already/abs" + + +@pytest.mark.unit +def test_build_node_params_flattens_bodies(): + server = {"server_ip": "1.2.3.4", "command_port": 1510, "connection_type": "unicast"} + profile = { + "bodies": [ + { + "rigid_body_name": "Drone", + "id": 1, + "topic": "perception/optitrack/drone", + "pose": True, + "pose_cov": True, + "position_covariance": [9.0] * 9, + "orientation_covariance": [8.0] * 9, + }, + { + "rigid_body_name": "Target", + "id": 100, + "topic": "perception/optitrack/target", + "pose": True, + "pose_cov": False, + }, + ] + } + params = natnet_launch._build_node_params(server, profile) + + assert params["server_ip"] == "1.2.3.4" + assert params["body_names"] == ["Drone", "Target"] + assert params["body_ids"] == [1, 100] + assert params["body_topics"] == ["perception/optitrack/drone", "perception/optitrack/target"] + assert params["body_pose"] == [True, True] + assert params["body_pose_cov"] == [True, False] + # 9 floats per body, flattened in body order. + assert len(params["body_position_covariance"]) == 18 + assert params["body_position_covariance"][:9] == [9.0] * 9 + # Target omitted its covariance → built-in default fills its slice. + assert params["body_position_covariance"][9:] == natnet_launch._DEFAULT_POSITION_COVARIANCE + + +@pytest.mark.unit +def test_build_node_params_empty_profile(): + """A robot with no profile yields empty body arrays (node tracks nothing).""" + params = natnet_launch._build_node_params({}, {}) + assert params["body_names"] == [] + assert params["body_ids"] == [] + assert params["body_position_covariance"] == [] diff --git a/robot/ros_ws/src/perception/perception_bringup/launch/perception.launch.xml b/robot/ros_ws/src/perception/perception_bringup/launch/perception.launch.xml index e433e94d4..354eb3c88 100644 --- a/robot/ros_ws/src/perception/perception_bringup/launch/perception.launch.xml +++ b/robot/ros_ws/src/perception/perception_bringup/launch/perception.launch.xml @@ -73,7 +73,8 @@ - + 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..ea1693155 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 + - ./sitl-files/${SITL_PARAM_PROFILE:-default}.env environment: - AUTOLAUNCH=${AUTOLAUNCH:-'false'} - DISPLAY=${DISPLAY} @@ -42,6 +43,8 @@ services: - NUM_ROBOTS=${NUM_ROBOTS:-1} - ENABLE_LIDAR=${ENABLE_LIDAR:-false} - ISAAC_SIM_HEADLESS=${ISAAC_SIM_HEADLESS:-false} + - 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 +71,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 +148,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/sitl-files/default.env b/simulation/isaac-sim/docker/sitl-files/default.env new file mode 100644 index 000000000..aac0ea21d --- /dev/null +++ b/simulation/isaac-sim/docker/sitl-files/default.env @@ -0,0 +1,7 @@ +# SITL env bundle: default (no PX4_PARAM_* overrides beyond airframe + image patches). +# +# Selected by SITL_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. +# +# See px4-vision.env for external-vision / NatNet → MAVROS fusion tuning. diff --git a/simulation/isaac-sim/docker/sitl-files/px4-vision.env b/simulation/isaac-sim/docker/sitl-files/px4-vision.env new file mode 100644 index 000000000..39a6ebf67 --- /dev/null +++ b/simulation/isaac-sim/docker/sitl-files/px4-vision.env @@ -0,0 +1,35 @@ +# PX4 SITL env bundle: external vision / mocap state estimation. +# +# Data path: +# Isaac NatNet emulator → natnet_ros2 → vision_pose_converter +# → /{robot}/interface/mavros/vision_pose/* → PX4 EKF2 +# +# Pair with robot-side vision_pose.enabled: true in the robot's natnet_config.yaml profile. +# Set SITL_PARAM_PROFILE=px4-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..0db35ef6e --- /dev/null +++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/.gitignore @@ -0,0 +1,9 @@ +# OptiTrack SDK archives and build artifacts (reference tree may exist locally) +**/*.obj +**/*.pdb +**/*.exe +**/*.iobj +**/*.ipdb +**/*.tlog/ +**/__pycache__/ +**/*.pyc diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/README.md b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/README.md new file mode 100644 index 000000000..407c150b3 --- /dev/null +++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/README.md @@ -0,0 +1,161 @@ +# OptiTrack NatNet Emulator (Isaac Sim Extension) + +Python NatNet **server** emulator for AirStack simulation and integration testing with [`natnet_ros2`](../../../../robot/ros_ws/src/perception/natnet_ros2/). + +The extension has two layers: + +1. **Transport + protocol** (`optitrack.natnet.emulator.server`) — UDP NatNet server, ctypes wire types, MODELDEF cache, frame streaming. Importable outside Isaac Sim (unit tests, host-side integration). +2. **Isaac integration** (`optitrack.natnet.emulator.isaac`) — stage-driven `/World/NatNetInterface` config prim, pose sampling on physics steps, Kit UI editor, and Pegasus launch-script helpers. + +## Layout + +``` +optitrack.natnet.emulator/ +├── config/extension.toml # Kit manifest (server module + UI entry point) +├── schema/schema.usda # Typed NatNet interface attribute definitions +├── setup.py +├── docs/ +│ └── natnet_interface_prim_design.md # Design notes + implementation history +├── test/ # Co-located unit tests (proxied by tests/sim/) +└── optitrack/natnet/emulator/ + ├── defaults.py # Reference Drone → prim bindings for tests + ├── server/ # NatNet UDP server (transport + protocol) + │ ├── natnet_server.py # Base server, queue, MODELDEF cache + │ ├── natnet_unicast_server.py + │ ├── natnet_data_types.py + │ ├── natnet_model_types.py + │ └── natnet_server_types.py + └── isaac/ # Isaac Sim wrapper (Kit + USD) + ├── config.py # Pure-Python NatNetInterfaceConfig model + ├── usd_bindings.py # Author/read interface prims on a stage + ├── catalog.py # Config → sDataDescriptions (MODELDEF) + ├── frames.py # Prim poses → sFrameOfMocapData + ├── manager.py # NatNetServerManager (lifecycle + sampling) + ├── scene_setup.py # Pegasus launch helpers (start_drone_natnet_server) + └── ui_extension.py # Docked editor panel (NatNetEmulatorExtension) +``` + +## Responsibilities + +| Layer | Role | +|-------|------| +| **Server** | UDP transport; `NAT_CONNECT` / `NAT_SERVERINFO`; `NAT_REQUEST_MODELDEF`; `NAT_KEEPALIVE`; `NAT_ECHOREQUEST` / `NAT_ECHORESPONSE`; `NAT_FRAMEOFDATA` on the **data port** (1511). MODELDEF stored as packed bytes via `set_model_def_payload()`. Frames enqueued with `enqueue_mocap_data()`. | +| **Isaac wrapper** | Authors and reads the NatNet interface config prim; builds MODELDEF from scene config; samples tracked prim world poses each physics step; calls `flush_mocap_data()` synchronously (background timer disabled — see below). | +| **`defaults.py`** | Hardcoded `Drone` → `/World/base_link` binding for legacy tests; production paths use the stage prim via `scene_setup.build_drone_config()`. | + +The server does **not** own prim-path bindings. The Isaac layer calls `set_model_def_payload(catalog.pack())` after building `sDataDescriptions` from the interface config. + +## Stage-driven config prim + +Configuration lives on a USD prim (conventionally `/World/NatNetInterface`) with `natnet:*` attributes: + +- Server: IP, unicast/multicast mode, command/data ports, publish rate, NatNet version, up-axis, optional pose noise. +- Bodies: multi-apply `natnet:body::*` fields mapping rigid-body name / streaming ID → target prim path. + +`NatNetServerManager` scans the stage, resyncs the catalog when the prim changes, and streams one rigid body per configured target. Missing prims emit **lost** bodies (NaN position, tracking-invalid bit clear) until the target appears — important for Pegasus drones spawned on first Play. + +**Up axis:** default `Z` passes Isaac/USD world poses through unchanged (matches `natnet_ros2`). Set `Y` to emulate a Y-up Motive room. + +## Streaming model (Isaac) + +Inside Kit, the server's background `_data_update_loop` is **disabled** (`auto_stream = False`) because the GIL-starved daemon thread does not reliably transmit frames. Instead, each physics step: + +1. `NatNetServerManager.sample_once()` reads prim poses and `enqueue_mocap_data(frame)`. +2. `NatNetUnicastServer.flush_mocap_data()` sends immediately on the physics-step thread. + +Outside Isaac (host unit tests), `auto_stream=True` uses the timer-driven loop. + +Default Docker sim IP: **`172.31.0.200`** (Isaac container on the AirStack bridge network). + +## Enabling in AirStack + +**Robot:** `LAUNCH_NATNET=true` in `.env` → `natnet_ros2` in perception bringup. Configure Motive/emulator IP in [`natnet_config.yaml`](../../../../robot/ros_ws/src/perception/natnet_ros2/config/natnet_config.yaml). + +**Isaac Sim:** set `ISAAC_SIM_SCRIPT_NAME` to a NatNet launch script (NatNet always starts — no `LAUNCH_NATNET` gate in the script): + +| Script | Use | +|--------|-----| +| `example_one_px4_pegasus_natnet_launch_script.py` | Single drone + static `Target` | +| `example_multi_px4_pegasus_natnet_launch_script.py` | `NUM_ROBOTS` drones + shared `Target` (system tests with NatNet use this even for `NUM_ROBOTS=1`) | + +Baseline Pegasus scripts (`example_one_px4_pegasus_launch_script.py`, `example_multi_px4_pegasus_launch_script.py`) have **no** NatNet integration. + +Convenience bundle for NatNet + external-vision PX4 SITL: + +```bash +airstack up --env-file overrides/isaac-natnet-vision.env +``` + +See [optitrack-development skill](../../../../.agents/skills/optitrack-development/SKILL.md) for wire-protocol details, libNatNet 4.4 unicast quirks, and debugging. + +## Usage + +### Server only (no Kit) + +```python +from optitrack.natnet.emulator import NatNetUnicastServer, make_default_drone_catalog +from optitrack.natnet.emulator.isaac.frames import BodySample, build_frame + +server = NatNetUnicastServer(local_interface="172.31.0.200") +server.set_model_def_payload(make_default_drone_catalog().pack()) +server.start() + +frame = build_frame(0, [BodySample(1, (0, 0, 1), (0, 0, 0, 1))]) +server.enqueue_mocap_data(frame) +server.flush_mocap_data() +``` + +### Isaac launch script + +```python +from optitrack.natnet.emulator.isaac import start_drone_natnet_server + +# Keep a reference to the manager for the sim lifetime. +manager = start_drone_natnet_server( + stage, + drones=[("Drone", 1, "/World/drone1/base_link")], + server_ip="172.31.0.200", +) +``` + +### Kit UI + +The extension registers **Window → NatNet Emulator** — a docked panel to create/edit the interface prim, start/stop the server, and view live body readouts. The same `NatNetServerManager` backs both the UI and launch-script paths. + +## Protocol notes (unicast, libNatNet 4.4) + +| Port | Traffic | +|------|---------| +| **1510** | Command: `NAT_CONNECT`, `NAT_REQUEST_MODELDEF`, keepalives, echo | +| **1511** | Data: `NAT_FRAMEOFDATA` — **must** be sent from a socket bound to the data port | + +Frames sent from the command socket are silently dropped by libNatNet. Every frame payload must include the 4-byte end-of-data tag expected by the C SDK unpacker. + +Full handshake layouts and sniffing workflow: [optitrack-development skill](../../../../.agents/skills/optitrack-development/SKILL.md). + +## Tests + +| Tier | Mark | What | +|------|------|------| +| Unit | `unit` | Serializers, protocol, config, USD authoring, catalog, pose sampling, server lifecycle, scene setup | +| Integration | `integration` | Host emulator → robot `natnet_ros2` pose Hz | + +Co-located tests live in `test/`. Pytest discovers them via thin proxies in [`tests/sim/optitrack_natnet_emulator/`](../../../../tests/sim/optitrack_natnet_emulator/). + +```bash +# Unit (no Docker / no SDK) +pytest tests/sim/optitrack_natnet_emulator/ -m unit -v + +# Integration (robot container + NatNet SDK) +pytest tests/integration/natnet/ -m integration --run-integration -v +``` + +Representative unit modules: `test_unicast_protocol.py`, `test_pose_streaming.py`, `test_interface_authoring.py`, `test_server_lifecycle.py`, `test_scene_setup.py`. + +## Reference material + +- Design doc: [`docs/natnet_interface_prim_design.md`](docs/natnet_interface_prim_design.md) +- Robot client: [`natnet_ros2/README.md`](../../../../robot/ros_ws/src/perception/natnet_ros2/README.md) +- Integration tier: [`tests/integration/natnet/README.md`](../../../../tests/integration/natnet/README.md) + +OptiTrack SDK sample headers may exist locally under `NatNetClientSDK/` for wire-format reference; they are **not** redistributed by AirStack (proprietary license). diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/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..39b4eed7a --- /dev/null +++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/docs/natnet_interface_prim_design.md @@ -0,0 +1,638 @@ +# 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** (NatNet line: `launch_scripts/example_one_px4_pegasus_natnet_launch_script.py`, +> `example_multi_px4_pegasus_natnet_launch_script.py`): after spawning drones, author one +> rigid body per drone `base_link`, a static `/World/target` body (via `author_static_target`), +> and start the server. Single: `("Drone", 1, /World/base_link/body)` + +> `("Target", 100, /World/target)`. Multi: `("", i, /World/drone/base_link/body)` +> per drone + one shared `("Target", 100, /World/target)`. Each robot's `natnet_config.yaml` +> profile selects which bodies it subscribes to. Failures are caught (never kill the sim); +> the manager is torn down on close. +> Baseline Pegasus scripts (`example_one_px4_pegasus_launch_script.py`, +> `example_multi_px4_pegasus_launch_script.py`) have no NatNet code. +> - **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:** set `ISAAC_SIM_SCRIPT_NAME` to a NatNet launch script; `NATNET_BODY_NAME` / +> `NATNET_TARGET_NAME` optional. Robot-side `LAUNCH_NATNET=true` brings up `natnet_ros2`. +> Server binds the container IP `172.31.0.200` (the config default). +> - **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 + `register_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.flush_mocap_data()` **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. **NatNet liveliness uses a NatNet Pegasus script, not the baseline scripts.** + Default system tests (`tests/conftest.py`) use + `ISAAC_SIM_SCRIPT_NAME=example_multi_px4_pegasus_launch_script.py` (no NatNet). + For the NatNet pose sentinel, set robot `LAUNCH_NATNET=true` **and** point sim at + `example_multi_px4_pegasus_natnet_launch_script.py` (or the one-drone NatNet + script). Convenience bundle: `overrides/isaac-natnet-vision.env`. + + Manual repro: + `AUTOLAUNCH=true ISAAC_SIM_USE_STANDALONE=true + ISAAC_SIM_SCRIPT_NAME=example_multi_px4_pegasus_natnet_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). The robot's + `natnet_config.yaml` profile maps the `Drone` body (id 1) to the relative topic + `perception/optitrack/drone`, so the liveliness sentinel topic is + `/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..2c1915264 --- /dev/null +++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/defaults.py @@ -0,0 +1,25 @@ +"""Reference tracked-body defaults for tests and the future Isaac Sim wrapper.""" + +from __future__ import annotations + +from dataclasses import dataclass + + +@dataclass(frozen=True) +class TrackedBodyBinding: + """Maps a NatNet rigid body to a USD prim path (not sent on the NatNet wire).""" + + name: str + id: int + prim_path: str + parent_id: int = -1 + + +# Single-drone NatNet Pegasus scenes (example_one_px4_pegasus_natnet_launch_script.py). +DEFAULT_DRONE_BINDING = TrackedBodyBinding( + name="Drone", + id=1, + prim_path="/World/base_link", +) + +DEFAULT_TRACKED_BODY_BINDINGS: tuple[TrackedBodyBinding, ...] = (DEFAULT_DRONE_BINDING,) diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/__init__.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/__init__.py new file mode 100644 index 000000000..ffd1b9e93 --- /dev/null +++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/__init__.py @@ -0,0 +1,62 @@ +# 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. ``usd_bindings`` imports ``pxr`` lazily, so +importing this package is safe in non-Isaac environments. +""" + +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, + DEFAULT_TARGET_PATH, + DEFAULT_TARGET_POSITION, + DEFAULT_TARGET_STREAMING_ID, + author_static_target, + 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", + "DEFAULT_TARGET_PATH", + "DEFAULT_TARGET_POSITION", + "DEFAULT_TARGET_STREAMING_ID", + "BodyBinding", + "BodySample", + "NatNetInterfaceConfig", + "NatNetServerManager", + "author_interface", + "author_static_target", + "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..9c3bfaa35 --- /dev/null +++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/catalog.py @@ -0,0 +1,53 @@ +# Copyright (c) 2024 Carnegie Mellon University +# MIT License - see LICENSE in the repository root for full text. +""" +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..56c866ab1 --- /dev/null +++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/config.py @@ -0,0 +1,234 @@ +# 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. + +The USD binding layer (author/read against a ``Usd.Stage``) +lives in ``usd_bindings.py`` and depends on this model. + +Attribute names follow the multi-apply schema convention +(``natnet:body::``). +The custom-attribute backing is 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. +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..576969b80 --- /dev/null +++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/frames.py @@ -0,0 +1,129 @@ +# 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. Sampled prim world poses become an +``sFrameOfMocapData`` of rigid bodies that the server +streams to the client. + +**Frame convention:** Motive exposes an "Up Axis" setting. AirStack's ``natnet_ros2`` +requires it set to **Z** and copy the rigid-body pose straight through +(``rb_to_pose`` is an identity copy). Isaac Sim is Z-up, so the default ``up_axis="Z"`` +emits the prim's USD world pose **as-is**. ``up_axis="Y"`` emulates a default (Y-up) +Motive by rotating the pose -90 deg about X. + +**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..398d761eb --- /dev/null +++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/manager.py @@ -0,0 +1,403 @@ +# Copyright (c) 2024 Carnegie Mellon University +# MIT License - see LICENSE in the repository root for full text. +""" +``NatNetServerManager`` detects interface prims, samples poses from the stage, +and owns a **single** server instance it can start and stop. +On each enable it builds a MODELDEF catalog from the config and constructs a +fresh server via an injectable factory. +""" + +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: # 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. + flush_mocap_data = getattr(self._server, "flush_mocap_data", None) + if callable(flush_mocap_data): + flush_mocap_data() + 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 + try: + paths = list(notice.GetResyncedPaths()) + list(notice.GetChangedInfoOnlyPaths()) + except Exception: + paths = [] + if any(("NatNetInterface" in str(p)) or ("natnet:" in str(p)) for p in paths): + # A NatNet prim changed: mark the sampler dirty so the next physics step re-reads the catalog. + self._needs_resync = True + # Debounce author_interface() calls into one scan on the next update tick. + 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..d7e804c61 --- /dev/null +++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/scene_setup.py @@ -0,0 +1,142 @@ +# 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" + +# Default world prim + position for the demo "target" body (a static placeholder +# the example scripts stream alongside the drones so a tracked target is available). +DEFAULT_TARGET_PATH = "/World/target" +DEFAULT_TARGET_POSITION = (2.0, 0.0, 1.0) +DEFAULT_TARGET_STREAMING_ID = 100 + +# (rigid_body_name, streaming_id, target_prim_path) +DroneSpec = Tuple[str, int, str] + + +def author_static_target( + stage, + prim_path: str = DEFAULT_TARGET_PATH, + position: Sequence[float] = DEFAULT_TARGET_POSITION, +): + """Author a static ``Xform`` prim to act as a NatNet-tracked target. + + Creates ``prim_path`` (a plain transform with a single translate op) at + ``position`` so the emulator can sample it like any other tracked body. The + prim is static — no physics, no animation — representing a fixed point of + interest that drones can be commanded toward. Imports ``pxr`` lazily so this + module stays importable outside Isaac. Returns ``prim_path``. + """ + from pxr import Gf, UsdGeom + + xform = UsdGeom.Xform.Define(stage, prim_path) + xform.AddTranslateOp().Set(Gf.Vec3d(float(position[0]), float(position[1]), float(position[2]))) + return prim_path + + +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. Play the sim to stream poses. Keep a reference to + the returned manager so it isn't garbage-collected (which would tear + down the physics subscription and stop the 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..50a581551 --- /dev/null +++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/ui_extension.py @@ -0,0 +1,453 @@ +# 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. + +Create/manage the ``/World/NatNetInterface`` prim. 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. +""" + +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..262216ee2 --- /dev/null +++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/usd_bindings.py @@ -0,0 +1,216 @@ +# 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 doesn't +require USD. + +Backing today is plain namespaced custom attributes + relationships. Property names +follow the multi-apply schema convention (``natnet:body::``). +""" + +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) + 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..91e23a412 --- /dev/null +++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_data_types.py @@ -0,0 +1,261 @@ +import ctypes +import struct +from .natnet_common import ModelLimits, MarkerData + +class sMarker(ctypes.Structure): + _pack_ = 1 + _fields_ = [ + ("ID", ctypes.c_int32), + ("x", ctypes.c_float), + ("y", ctypes.c_float), + ("z", ctypes.c_float), + ("size", ctypes.c_float), + ("params", ctypes.c_int16), + ("residual", ctypes.c_float) + ] + + def pack(self) -> bytes: + return struct.pack(' bytes: + # szName is null-terminated on the wire. + name_bytes = self.szName.rstrip(b'\x00') + b'\x00' + payload = bytearray(name_bytes) + payload += struct.pack(' bytes: + return struct.pack(' bytes: + payload = bytearray(struct.pack(' bytes: + payload = bytearray(struct.pack(' bytes: + payload = bytearray(struct.pack(' bytes: + payload = bytearray(struct.pack(' bytes: + payload = bytearray(struct.pack(' bytes: + """NatNet 4.1+ prefixes each collection with a 4-byte byte count.""" + payload = bytearray(struct.pack(' 0) or natnet_major > 4: + payload += struct.pack(' bytes: + payload = bytearray() + + payload += struct.pack(' bytes: + # szName is null-terminated on the wire, not fixed MAX_NAMELENGTH. + name_bytes = self.szName.rstrip(b"\x00") + b"\x00" + payload = bytearray(name_bytes) + payload += struct.pack( + " bytes: + if self.type == int(DataDescriptors.Descriptor_RigidBody): + body = self.RigidBodyDescription.pack() + else: + raise ValueError(f"Unsupported data description type: {self.type}") + payload = bytearray(struct.pack(" bytes: + payload = bytearray(struct.pack(" sDataDescriptions: + """Build the default single-body catalog (Drone id=1) for natnet_ros2.""" + descriptions = sDataDescriptions() + descriptions.nDataDescriptions = 1 + desc = descriptions.arrDataDescriptions[0] + desc.type = int(DataDescriptors.Descriptor_RigidBody) + rb = desc.RigidBodyDescription + rb.szName = b"Drone" + rb.ID = 1 + rb.parentID = -1 + rb.offsetqw = 1.0 + rb.nMarkers = 0 + return descriptions diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_server.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_server.py new file mode 100644 index 000000000..05b3c0a2e --- /dev/null +++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_server.py @@ -0,0 +1,338 @@ +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): + # Uniquely identify a client session by their IP and their unique command port. + return hash((self.ip, self.port)) + + def __eq__(self, other): + return (isinstance(other, Client) and + self.ip == other.ip and + self.port == other.port) + + +class NatNetServer: + def __init__(self, + local_interface : str = "172.31.0.200", + transmission_type: TransmissionType = TransmissionType.MULTICAST, + multicast_address : str = "239.255.42.99", + command_port: int = 1510, + data_port : int = 1511, + motive_app_version : typing.Tuple[int, int, int, int]=(3, 1, 0, 0), + natnet_version : typing.Tuple[int, int, int, int]=(4, 4, 0, 0), + high_res_clock_freq : int = 1_000_000_000, + publish_rate : int = 100 # Hz (default 100Hz) + ): + + self.local_interface = local_interface + self.transmission_type = transmission_type + self.multicast_address = multicast_address + self.command_port = command_port + self.data_port = data_port + self.motive_app_version = motive_app_version + self.natnet_version = natnet_version + self.high_res_clock_freq = high_res_clock_freq + self.publish_rate = publish_rate + + self._validate_init_params() + + self.server_description = self._build_server_description() + # Initialize synchronously safe data structures for server state and mocap data + + # Thread-safe queue for Mocp frames + self.mocap_data_queue = queue.Queue(maxsize=100) + self._last_mocap_frame: DataMessages.sFrameOfMocapData | None = None + self._last_mocap_lock = threading.Lock() + + # Thread list and shutdown event + self.threads = [] + self.shutdown_event = threading.Event() + + # Connected clients for unicast mode + self.connected_clients : typing.Set[Client] = set() + self.clients_lock : threading.Lock = threading.Lock() + + # MODELDEF wire cache (Isaac wrapper updates via set_model_def_payload) + self._model_def_lock = threading.Lock() + self._model_def_payload: bytes = ModelTypes.make_default_drone_catalog().pack() + + # Sockets + self.command_socket : socket.socket | None = None + self.data_socket : socket.socket | None = None + + self.running = False + + # When True (default), the background data loop streams frames on its own timer. + # Set False when an external driver (the Isaac wrapper's physics-step callback) + # sends frames synchronously via ``flush_mocap_data``. + self.auto_stream = True + + # start() launches two daemon threads: a command listener (handshake / MODELDEF / keepalive) + # and a data loop that streams mocap frames. The transmission-specific behavior lives in the unicast/multicast subclass. + + def _signal_handler(self, signum, frame): + print(f"\n[NatNetServer] Received interrupt signal {signum}. Initiating shutdown...") + self.shutdown() + + def enqueue_mocap_data(self, new_data: DataMessages.sFrameOfMocapData): + # Thread-safe method to push new physics frames (called by Isaac-Sim extension) + if self.mocap_data_queue.full(): + try: + # Drop oldest frame if falling behind + self.mocap_data_queue.get_nowait() + except queue.Empty: + pass + self.mocap_data_queue.put(new_data) + with self._last_mocap_lock: + self._last_mocap_frame = new_data + + def _get_last_known_mocap_frame(self) -> DataMessages.sFrameOfMocapData | None: + with self._last_mocap_lock: + return self._last_mocap_frame + + def set_model_def_payload(self, payload: bytes) -> None: + """Replace MODELDEF body served on NAT_REQUEST_MODELDEF (Isaac wrapper calls this).""" + with self._model_def_lock: + self._model_def_payload = payload + + def set_model_def_from_descriptions( + self, descriptions: ModelTypes.sDataDescriptions + ) -> None: + """Pack descriptions once and store as the MODELDEF wire cache.""" + self.set_model_def_payload(descriptions.pack()) + + def _get_model_def_payload(self) -> bytes: + """Return cached MODELDEF bytes (command thread only).""" + with self._model_def_lock: + return self._model_def_payload + + def start(self): + # Bind sockets and launch worker threads automatically on init + + # Register signal handlers for graceful shutdown (Catches Ctrl+C and kill) + try: + signal.signal(signal.SIGINT, self._signal_handler) + signal.signal(signal.SIGTERM, self._signal_handler) + except ValueError: + pass # Safe fallback if not called from the main thread + + # 1. Setup Command Socket (Receives connection/discovery requests) + self.command_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM, socket.IPPROTO_UDP) + self.command_socket.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) + self.command_socket.bind(('', self.command_port)) + + # 2. Setup Data Socket (Sends outward Mocap frames). + # Bind to the data port so frames leave with source port == data_port. + # libNatNet routes unicast NAT_FRAMEOFDATA by the server's data port + self.data_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM, socket.IPPROTO_UDP) + self.data_socket.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) + self.data_socket.bind(('', self.data_port)) + if self.transmission_type == TransmissionType.MULTICAST: + self.data_socket.setsockopt(socket.IPPROTO_IP, socket.IP_MULTICAST_IF, socket.inet_aton(self.local_interface)) + + # 3. Launch Threads + cmd_thread = threading.Thread(target=self._command_listener_loop, daemon=True) + data_thread = threading.Thread(target=self._data_update_loop, daemon=True) + + self.threads.extend([cmd_thread, data_thread]) + + for t in self.threads: + t.start() + + self.running = True + + def shutdown(self): + # Cleanly shutdown threads and close sockets + self.running = False + self.shutdown_event.set() + + if self.command_socket: + self.command_socket.close() + + if self.data_socket: + self.data_socket.close() + + for t in self.threads: + if t.is_alive(): + t.join(timeout=1.0) + + def _validate_init_params(self): + + # Validate the local_interface is a valid IP address + if not self.local_interface or not isinstance(self.local_interface, str) or self.local_interface.count('.') != 3: + raise ValueError(f"Invalid local interface IP address: {self.local_interface}") + + # Validate between transmission types and address requirements + if self.transmission_type not in TransmissionType: + raise ValueError(f"Invalid transmission type: {self.transmission_type}. Must be 'unicast' or 'multicast'.") + + if self.transmission_type == TransmissionType.MULTICAST and not self.multicast_address: + raise ValueError("Multicast address must be provided for multicast transmission type.") + + if self.transmission_type == TransmissionType.UNICAST and self.multicast_address: + raise ValueError("Multicast address should not be provided for unicast transmission type.") + + if not (0 < self.command_port < 65536): + raise ValueError(f"Invalid command port: {self.command_port}. Must be between 1 and 65535.") + + if not (0 < self.data_port < 65536): + raise ValueError(f"Invalid data port: {self.data_port}. Must be between 1 and 65535.") + + if self.command_port == self.data_port: + raise ValueError("Command port and data port must be different.") + + if self.motive_app_version and (not isinstance(self.motive_app_version, tuple) or len(self.motive_app_version) != 4): + raise ValueError(f"Invalid Motive app version: {self.motive_app_version}. Must be a tuple of 4 integers (major, minor, build, revision).") + + if self.natnet_version and (not isinstance(self.natnet_version, tuple) or len(self.natnet_version) != 4): + raise ValueError(f"Invalid NatNet version: {self.natnet_version}. Must be a tuple of 4 integers (major, minor, build, revision).") + + if self.motive_app_version and not self.motive_app_version[0] == 3: + raise ValueError(f"Unsupported Motive app version: {self.motive_app_version}. Minimum supported version is 3.0.0.0. Recommended to use 3.1.0.0") + + if not self.natnet_version[0] == 4: + raise ValueError(f"Unsupported NatNet version: {self.natnet_version}. Minimum supported version is 4.0.0.0. Recommended to use 4.4.0.0") + + if self.high_res_clock_freq <= 0: + raise ValueError(f"Invalid high resolution clock frequency: {self.high_res_clock_freq}. Must be a positive integer representing the frequency in Hz.") + + def _get_latest_mocap_packet(self) -> DataMessages.sFrameOfMocapData | None: + # Thread-safe method to retrieve the latest mocap data to be sent + try: + return self.mocap_data_queue.get_nowait() + except queue.Empty: + return None + + @staticmethod + def _pad_fixed_string(value: bytes) -> bytes: + """Null-pad a byte string to MAX_NAMELENGTH for fixed-size NatNet name fields.""" + truncated = value[: ServerMessages.MAX_NAMELENGTH - 1] + return truncated + b"\x00" * (ServerMessages.MAX_NAMELENGTH - len(truncated)) + + @staticmethod + def _assign_version_bytes(field: ctypes.Array, version: typing.Tuple[int, int, int, int]) -> None: + for index, component in enumerate(version): + field[index] = component + + @staticmethod + def _assign_ipv4_bytes(field: ctypes.Array, address: str | bytes) -> None: + octets = socket.inet_aton(address) if isinstance(address, str) else address + for index, octet in enumerate(octets): + field[index] = octet + + def _build_server_description(self) -> ServerMessages.sServerDescription: + # Helper to build the server description struct with current server info (e.g. on startup or in response to command request) + description = ServerMessages.sServerDescription() + description.HostPresent = True + description.szHostComputerName = self._pad_fixed_string( + socket.gethostname().encode("utf-8") + ) + self._assign_ipv4_bytes(description.HostComputerAddress, self.local_interface) + description.szHostApp = self._pad_fixed_string(b"Motive") + self._assign_version_bytes(description.HostAppVersion, self.motive_app_version) + self._assign_version_bytes(description.NatNetVersion, self.natnet_version) + description.HighResClockFrequency = self.high_res_clock_freq + description.bConnectionInfoValid = True + description.ConnectionDataPort = self.data_port + description.ConnectionMulticast = self.transmission_type == TransmissionType.MULTICAST + + if self.transmission_type == TransmissionType.MULTICAST: + self._assign_ipv4_bytes(description.ConnectionMulticastAddress, self.multicast_address) + else: + self._assign_ipv4_bytes(description.ConnectionMulticastAddress, b"\x00\x00\x00\x00") + + return description + + def _build_connect_response_payload(self) -> bytes: + """NAT_CONNECT reply: libNatNet parses NAT_SERVERINFO payload as sSender_Server.""" + sender = ServerMessages.sSender_Server() + sender.Common.szName = self._pad_fixed_string(b"Motive") + self._assign_version_bytes(sender.Common.Version, self.motive_app_version) + self._assign_version_bytes(sender.Common.NatNetVersion, self.natnet_version) + sender.HighResClockFrequency = self.high_res_clock_freq + sender.DataPort = self.data_port + sender.IsMulticast = self.transmission_type == TransmissionType.MULTICAST + if self.transmission_type == TransmissionType.MULTICAST: + self._assign_ipv4_bytes(sender.MulticastGroupAddress, self.multicast_address) + else: + self._assign_ipv4_bytes(sender.MulticastGroupAddress, b"\x00\x00\x00\x00") + return sender.pack() + + def _send_packet_to_client( + self, + client: Client, + message_id: ServerMessages.MessageId | int, + payload: bytes, + sock: socket.socket | None = None, + ) -> None: + """Send a NatNet packet to a unicast client (libNatNet 4.4). + + Command replies go out the command socket; mocap frames go out the data socket. + """ + if self.shutdown_event.is_set(): + return + sock = sock or self.command_socket + if not sock: + raise ValueError("[NatNetServer] Socket not initialized. Cannot send packet.") + + header = ServerMessages.sPacketHeader( + iMessage=int(message_id), + nDataBytes=len(payload), + ) + packet = header.pack() + payload + try: + with client.socket_lock: + sock.sendto(packet, (client.ip, client.port)) + except OSError as e: + raise ValueError( + f"[NatNetServer] Error sending message {int(message_id)} to " + f"client {client.ip}:{client.port}: {e}" + ) from e + + def _data_update_loop(self): # Stub: Different betweeen multicast and unicast server implementations, as they will need to handle client connections differently (multicast will just send to the multicast group address) + # Loop to update mocap data and send packets at regular intervals. + pass + + def _send_data_packet(self, client: Client, data_message: DataMessages.sFrameOfMocapData): + # Serialize frame payload and send via the data socket. + try: + packet_bytes = data_message.pack() + except Exception as e: + raise ValueError(f"[NatNetServer] Error serializing data message: {e}") from e + + self._send_packet_to_client( + client, + ServerMessages.MessageId.NAT_FRAMEOFDATA, + packet_bytes, + sock=self.data_socket, + ) + + def _command_listener_loop(self): # Stub: Different betweeen multicast and unicast server implementations, as they will need to handle client connections differently (multicast will just send to the multicast group address) + # Loop to listen for and handle incoming command requests (e.g. from client apps) + pass + + def _handle_command_request(self, request_data: bytes): # Stub: Different betweeen multicast and unicast server implementations, as they will need to handle client connections differently (multicast will just send to the multicast group address) + # Parse incoming command request, perform requested action, and send response if needed + pass + diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_server_types.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_server_types.py new file mode 100644 index 000000000..3083f1a6e --- /dev/null +++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_server_types.py @@ -0,0 +1,156 @@ +import ctypes +import struct +from enum import IntEnum + +MAX_NAMELENGTH = 256 +MAX_PACKETSIZE = 65503 + +# NatNet SDK sServerDescription uses default struct alignment (#pragma pack(pop)), not pack(1). +SERVER_DESCRIPTION_WIRE_SIZE = 552 +# NAT_CONNECT / NAT_SERVERINFO reply uses packed sSender_Server (#pragma pack(1) in NatNetTypes.h). +SENDER_SERVER_WIRE_SIZE = 256 + 4 + 4 + 8 + 2 + 1 + 4 # 279 + +# Client/server message ids +class MessageId(IntEnum): + NAT_CONNECT = 0 + NAT_SERVERINFO = 1 + NAT_REQUEST = 2 + NAT_RESPONSE = 3 + NAT_REQUEST_MODELDEF = 4 + NAT_MODELDEF = 5 + NAT_REQUEST_FRAMEOFDATA = 6 + NAT_FRAMEOFDATA = 7 + NAT_MESSAGESTRING = 8 + NAT_DISCONNECT = 9 + NAT_KEEPALIVE = 10 + NAT_DISCONNECTBYTIMEOUT = 11 + NAT_ECHOREQUEST = 12 + NAT_ECHORESPONSE = 13 + NAT_DISCOVERY = 14 + NAT_UNRECOGNIZED_REQUEST = 100 + +# Server/Sender configuration and info +def _fixed_name(field: ctypes.Array) -> bytes: + raw = bytes(field).split(b"\x00", 1)[0] + b"\x00" + if len(raw) > MAX_NAMELENGTH: + raw = raw[: MAX_NAMELENGTH - 1] + b"\x00" + return raw + b"\x00" * (MAX_NAMELENGTH - len(raw)) + + +class sSender(ctypes.Structure): + _pack_ = 1 + _fields_ = [ + ("szName", ctypes.c_char * MAX_NAMELENGTH), # host app's name + ("Version", ctypes.c_uint8 * 4), # host app's version [major.minor.build.revision] + ("NatNetVersion", ctypes.c_uint8 * 4) # host app's NatNet version + ] + + def pack(self) -> bytes: + payload = bytearray() + payload += _fixed_name(self.szName) + payload += bytes(self.Version) + payload += bytes(self.NatNetVersion) + return bytes(payload) + +class sSender_Server(ctypes.Structure): + _pack_ = 1 + _fields_ = [ + ("Common", sSender), + ("HighResClockFrequency", ctypes.c_uint64), + ("DataPort", ctypes.c_uint16), + ("IsMulticast", ctypes.c_bool), + ("MulticastGroupAddress", ctypes.c_uint8 * 4) + ] + + def pack(self) -> bytes: + payload = bytearray(self.Common.pack()) + payload += struct.pack(" bytes: + # Wire layout matches NatNet SDK on x86-64 (3 pad bytes before HighResClockFrequency). + payload = bytearray() + payload.append(1 if self.HostPresent else 0) + payload += _fixed_name(self.szHostComputerName) + payload += bytes(self.HostComputerAddress) + payload += _fixed_name(self.szHostApp) + payload += bytes(self.HostAppVersion) + payload += bytes(self.NatNetVersion) + while len(payload) % 8: + payload.append(0) + payload += struct.pack(" bytes: + return bytes(self) + +# Connection types enum matching NatNet SDK rules +class ConnectionType(IntEnum): + ConnectionType_Multicast = 0 + ConnectionType_Unicast = 1 + +class sNatNetClientConnectParams(ctypes.Structure): + """ + Python ctypes translation of the C++ sNatNetClientConnectParams struct. + Enforces a packed structure byte alignment matching the NatNet binary network protocol. + """ + _pack_ = 1 + _fields_ = [ + ("connectionType", ctypes.c_int32), # 4 bytes (mapping to standard ConnectionType enum) + ("serverCommandPort", ctypes.c_uint16), # 2 bytes + ("serverDataPort", ctypes.c_uint16), # 2 bytes + + # NOTE: Represented as void pointers (c_void_p) to safely match the host system's native bit size (e.g., 8 bytes on 64-bit) without string data unpacking overhead. + ("serverAddress", ctypes.c_void_p), + ("localAddress", ctypes.c_void_p), + ("multicastAddress", ctypes.c_void_p), + + ("subscribedDataOnly", ctypes.c_bool), # 1 byte + ("BitstreamVersion", ctypes.c_uint8 * 4) # 4 bytes: [Major, Minor, Build, Revision] + ] + + def pack(self) -> bytes: + return bytes(self) \ No newline at end of file diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_unicast_server.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_unicast_server.py new file mode 100644 index 000000000..d9c0a1e22 --- /dev/null +++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_unicast_server.py @@ -0,0 +1,172 @@ +import ctypes +import time + +from . import natnet_server_types as ServerTypes +from .natnet_server import TransmissionType, Client, NatNetServer + + +class NatNetUnicastServer(NatNetServer): + def __init__(self, + local_interface="172.31.0.200", + transmission_type: TransmissionType = TransmissionType.UNICAST, + multicast_address=None, + command_port=1510, + data_port=1511 + ): + + if not transmission_type == TransmissionType.UNICAST: + raise ValueError("Transmission type 'MULTICAST' is not supported in NatNetUnicastServer. Please use NatNetMulticastServer instead.") + + super().__init__(local_interface, transmission_type, multicast_address, command_port, data_port) + + def _data_update_loop(self): + # Loop to update mocap data and send packets at regular intervals. + # When auto_stream is False the frames are pumped externally (Isaac physics step), + # so this thread only idles — but stays alive for clean shutdown. + while not self.shutdown_event.is_set(): + time.sleep(1 / self.publish_rate) + if not self.auto_stream: + continue + self.flush_mocap_data() + + def flush_mocap_data(self): + """Send the latest (or last) mocap frame to every connected client, once.""" + with self.clients_lock: + clients = list(self.connected_clients) + if not clients: + return + + data_messages = self._get_latest_mocap_packet() + + if data_messages is None: # If the server stops producing frames, use the last known frame. + data_messages = self._get_last_known_mocap_frame() + if data_messages is None: + return + + for client in clients: + try: + self._send_data_packet(client, data_messages) + except ValueError as e: + print(str(e)) + continue + + def _command_listener_loop(self): + # Listens on UDP command socket for incoming command requests from clients. + # Handles incoming client handshakes and teardown. + + print(f"[Command Listener] Command listener thread started. Listening for incoming client command requests on UDP address:port {self.local_interface}:{self.command_port}...") + + while not self.shutdown_event.is_set(): + try: + data, addr = self.command_socket.recvfrom(1024) # Buffer size of 1024 bytes should be sufficient for command requests + if not data: + continue + self._handle_command_request(data, addr) + except Exception as e: + if self.shutdown_event.is_set(): + break + print(f"[Command Listener] Error receiving command request: {e}") + time.sleep(0.1) # Sleep briefly to avoid tight loop on errors + + def _handle_command_request(self, request_data: bytes, client_address: tuple): + """ + Processes standard binary headers and registers unicast endpoints. + """ + header_size = ctypes.sizeof(ServerTypes.sPacketHeader) + if len(request_data) < header_size: + return + + # Parse the header via ctypes + header = ServerTypes.sPacketHeader.from_buffer_copy(request_data[:header_size]) + + # Handle Connection Handshake + if header.iMessage == int(ServerTypes.MessageId.NAT_CONNECT): + client_requested_version = self.natnet_version # Fallback to server's version. Version handshaking not supported in this extension. + + client_ip, client_port = client_address + + # Create and store a new client object + new_client = Client(client_ip, client_port, version=client_requested_version) + try: + with self.clients_lock: + self.connected_clients.discard(new_client) # Remove any existing client with the same IP and port + self.connected_clients.add(new_client) # Add the new client to the connected clients list + print(f"[Command Handler] Added client {new_client.ip}:{new_client.port} to connected clients list.") + except Exception as e: + print(f"[Command Handler] Error adding client {new_client.ip}:{new_client.port} to connected clients list: {e}") + return + + try: + self._send_packet_to_client( + new_client, + ServerTypes.MessageId.NAT_SERVERINFO, + self._build_connect_response_payload(), + ) + except ValueError as e: + raise ValueError( + f"[Command Handler] Error sending server description to client {client_address}: {e}" + ) from e + print( + f"[Command Handler] Sent server description to client address " + f"through its port {client_address}." + ) + return + + # Non-handshake commands require a prior NAT_CONNECT from this endpoint. + client_ip, client_port = client_address + client = self._find_client(client_ip, client_port) + if client is None: + print( + f"[Command Handler] Ignoring message {header.iMessage} from " + f"unregistered client {client_address}." + ) + return + + if header.iMessage == int(ServerTypes.MessageId.NAT_REQUEST_MODELDEF): + try: + self._send_packet_to_client( + client, + ServerTypes.MessageId.NAT_MODELDEF, + self._get_model_def_payload(), + ) + except ValueError as e: + print( + f"[Command Handler] Error sending MODELDEF to client " + f"{client_address}: {e}" + ) + return + + if header.iMessage == int(ServerTypes.MessageId.NAT_KEEPALIVE): + # Receiving a keepalive refreshes the client's liveness; nothing to send back. + return + + if header.iMessage == int(ServerTypes.MessageId.NAT_ECHOREQUEST): + echo_payload = request_data[header_size : header_size + header.nDataBytes] + # libNatNet expects clientRequestTimestamp + hostReceivedTimestamp (8 + 8 bytes). + host_ts = int(time.time() * 1_000_000_000).to_bytes(8, "little", signed=False) + response_payload = echo_payload[:8].ljust(8, b"\x00") + host_ts + try: + self._send_packet_to_client( + client, + ServerTypes.MessageId.NAT_ECHORESPONSE, + response_payload, + ) + except ValueError as e: + print( + f"[Command Handler] Error sending ECHORESPONSE to client " + f"{client_address}: {e}" + ) + return + + print( + f"[Command Handler] Unhandled message id {header.iMessage} from " + f"registered client {client_address}." + ) + + def _find_client(self, ip: str, port: int) -> Client | None: + target = Client(ip, port) + with self.clients_lock: + for client in self.connected_clients: + if client == target: + return client + return None diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/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..6a15e5183 --- /dev/null +++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/conftest.py @@ -0,0 +1,21 @@ +# Copyright (c) 2024 Carnegie Mellon University +# MIT License - see LICENSE in the repository root for full text. +"""Put the extension root on sys.path for direct pytest test/ runs. + +CI uses proxies under tests/sim/optitrack_natnet_emulator/ instead. +""" + +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..a3cc8cd34 --- /dev/null +++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_catalog.py @@ -0,0 +1,111 @@ +# Copyright (c) 2024 Carnegie Mellon University +# MIT License - see LICENSE in the repository root for full text. +"""Catalog builder: body counts, wire fidelity, truncation, MAX_MODELS, duplicate targets.""" + +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..9f01ce80b --- /dev/null +++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_pose_sampling.py @@ -0,0 +1,226 @@ +# Copyright (c) 2024 Carnegie Mellon University +# MIT License - see LICENSE in the repository root for full text. +"""Pose sampling and catalog resync against an in-memory USD stage (fake server, no sockets).""" + +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..f93d761ab --- /dev/null +++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_pose_streaming.py @@ -0,0 +1,84 @@ +# Copyright (c) 2024 Carnegie Mellon University +# MIT License - see LICENSE in the repository root for full text. +"""Loopback: sample_once on a USD prim → NAT_FRAMEOFDATA with sampled position. + +Real server + UDP sockets + in-memory stage. Requires 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_natnet_launch_script.py b/simulation/isaac-sim/launch_scripts/example_multi_px4_pegasus_natnet_launch_script.py new file mode 100644 index 000000000..12b4799d1 --- /dev/null +++ b/simulation/isaac-sim/launch_scripts/example_multi_px4_pegasus_natnet_launch_script.py @@ -0,0 +1,256 @@ +#!/usr/bin/env python +""" +Multi-drone PX4 Pegasus launcher with OptiTrack NatNet mocap streaming. + +Same scene prep and sensor stack as ``example_multi_px4_pegasus_launch_script.py``, +plus a Motive-compatible NatNet server that always streams one rigid body per drone +and a shared static ``Target`` (id 100) at ``/World/target``. + +Body naming: + - ``NUM_ROBOTS=1``: drone body ``Drone`` (id 1) + - ``NUM_ROBOTS>1``: ``Drone1``, ``Drone2``, … (ids 1..N) + +Intended multi-robot profile pairing (see ``natnet_config.yaml`` commented scaffolding): + - ``robot_1``: tracks its drone + the shared ``Target`` + - ``robot_2``: tracks its drone + the shared ``Target`` + - ``robot_3``: tracks its drone only (no Target in profile) + +Set ``NUM_ROBOTS=3`` on both sim and robot stacks; each container picks its profile +via ``ROBOT_NAME``. + +Env: + - ``NUM_ROBOTS`` (default 1) + - ``ENABLE_LIDAR`` (default false) + - ``PLAY_SIM_ON_START`` (default true) + - ``NATNET_BODY_NAME`` / ``NATNET_TARGET_NAME`` (optional name overrides) +""" + +import asyncio +import os +import sys +import time + +import carb +from isaacsim import SimulationApp + +_headless = os.environ.get("ISAAC_SIM_HEADLESS", "false").lower() == "true" +simulation_app = SimulationApp({"headless": _headless}) + +import omni.kit.app +import omni.timeline +import omni.usd + +from omni.isaac.core.world import World + +from pegasus.simulator.params import SIMULATION_ENVIRONMENTS +from pegasus.simulator.logic.interface.pegasus_interface import PegasusInterface +from pegasus.simulator.ogn.api.spawn_multirotor import spawn_px4_multirotor_node +from pegasus.simulator.ogn.api.spawn_zed_camera import add_zed_stereo_camera_subgraph +from pegasus.simulator.ogn.api.spawn_rtx_lidar import add_rtx_lidar_subgraph + +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 + +from optitrack.natnet.emulator.isaac import ( + DEFAULT_TARGET_PATH, + DEFAULT_TARGET_POSITION, + DEFAULT_TARGET_STREAMING_ID, + author_static_target, + start_drone_natnet_server, +) + +# --------------------- CONFIGURATION --------------------- +ENV_URL = SIMULATION_ENVIRONMENTS["Default Environment"] +STAGE_SCALE = 1.0 +SAVE_SCENE_TO = None +DRONE_USD = "~/.local/share/ov/data/documents/Kit/shared/exts/pegasus.simulator/pegasus/simulator/assets/Robots/Iris/iris.usd" + +NUM_ROBOTS = int(os.environ.get("NUM_ROBOTS", "1")) +ENABLE_LIDAR = os.environ.get("ENABLE_LIDAR", "false").lower() == "true" +NATNET_BODY_NAME = os.environ.get("NATNET_BODY_NAME", "Drone") +NATNET_TARGET_NAME = os.environ.get("NATNET_TARGET_NAME", "Target") + +_NATNET_SERVER_KWARGS = { + "pose_noise_enabled": True, + "pose_noise_std_meters": 0.0005, + "pose_noise_rotation_deg": 0.05, +} +# --------------------------------------------------------- + + +ext_manager = omni.kit.app.get_app().get_extension_manager() +for ext in [ + "omni.graph.core", + "omni.graph.action", + "omni.graph.action_nodes", + "isaacsim.core.nodes", + "omni.graph.ui", + "omni.graph.visualization.nodes", + "omni.graph.scriptnode", + "omni.graph.window.action", + "omni.graph.window.generic", + "omni.graph.ui_nodes", + "pegasus.simulator", +]: + if not ext_manager.is_extension_enabled(ext): + ext_manager.set_extension_enabled_immediate(ext, True) + + +def wait_for_stage(stage, timeout_s: float = 10.0): + for _ in range(int(timeout_s / 0.1)): + omni.kit.app.get_app().update() + world_prim = stage.GetPrimAtPath("/World") + if world_prim.IsValid(): + non_physics = [c for c in world_prim.GetChildren() if c.GetName() != "PhysicsScene"] + if non_physics: + return True + time.sleep(0.1) + return False + + +def _drone_body_name(index: int) -> str: + """Single agent uses bare ``Drone``; multi uses ``Drone1``, ``Drone2``, …""" + return NATNET_BODY_NAME if NUM_ROBOTS == 1 else f"{NATNET_BODY_NAME}{index}" + + +def spawn_drone(index: int): + robot_name = f"robot_{index}" + drone_prim = f"/World/drone{index}/base_link" + init_x = 2.0 * (index - 1) - 2.0 * (NUM_ROBOTS - 1) / 2.0 + + graph_handle = spawn_px4_multirotor_node( + pegasus_node_name=f"PX4Multirotor_{index}", + drone_prim=drone_prim, + robot_name=robot_name, + vehicle_id=index, + domain_id=index, + usd_file=DRONE_USD, + init_pos=[init_x, 0.0, 0.07], + init_orient=[0.0, 0.0, 0.0, 1.0], + ) + + add_zed_stereo_camera_subgraph( + parent_graph_handle=graph_handle, + drone_prim=drone_prim, + robot_name=robot_name, + camera_name="ZEDCamera", + camera_offset=[0.2, 0.0, -0.05], + camera_rotation_offset=[0.0, 0.0, 0.0], + ) + + if ENABLE_LIDAR: + add_rtx_lidar_subgraph( + parent_graph_handle=graph_handle, + drone_prim=drone_prim, + robot_name=robot_name, + lidar_config="ouster_os1", + lidar_topic_name="point_cloud_raw", + lidar_offset=[0.0, 0.0, 0.025], + lidar_rotation_offset=[0.0, 0.0, 0.0], + min_range=0.75, + ) + + +class PegasusApp: + + def __init__(self): + self.timeline = omni.timeline.get_timeline_interface() + self.natnet_manager = None + + self.pg = PegasusInterface() + self.pg._world = World(**self.pg._world_settings) + self.world = self.pg.world + self.timeline.stop() + + self.pg.load_environment(ENV_URL) + + stage = omni.usd.get_context().get_stage() + if stage is None: + raise RuntimeError("Stage failed to load") + + if not wait_for_stage(stage): + carb.log_warn("Stage load timed out — continuing anyway.") + + stage_prim = stage.GetPrimAtPath("/World/stage") + if stage_prim.IsValid(): + scale_stage_prim(stage, "/World/stage", STAGE_SCALE) + add_colliders(stage_prim) + for _ in range(10): + omni.kit.app.get_app().update() + else: + carb.log_warn("/World/stage not found — skipping scale and collision.") + + add_dome_light(stage) + + if SAVE_SCENE_TO: + import tempfile + tmp_usd = os.path.join(tempfile.gettempdir(), "prepared_scene.usd") + success, error = asyncio.get_event_loop().run_until_complete( + omni.usd.get_context().export_as_stage_async(tmp_usd) + ) + if success: + os.makedirs(SAVE_SCENE_TO, exist_ok=True) + save_scene_as_contained_usd(tmp_usd, SAVE_SCENE_TO) + os.remove(tmp_usd) + else: + carb.log_error(f"Scene export failed: {error}") + + print(f"[example_multi_natnet] Spawning {NUM_ROBOTS} drone(s), lidar={'on' if ENABLE_LIDAR else 'off'}") + for i in range(1, NUM_ROBOTS + 1): + spawn_drone(i) + + self._setup_natnet(stage) + self.play_on_start = os.environ.get("PLAY_SIM_ON_START", "true").lower() == "true" + + def _setup_natnet(self, stage): + """Author NatNet bodies: one per drone plus one shared static target.""" + try: + author_static_target(stage, DEFAULT_TARGET_PATH, DEFAULT_TARGET_POSITION) + bodies = [ + (_drone_body_name(i), i, f"/World/drone{i}/base_link/body") + for i in range(1, NUM_ROBOTS + 1) + ] + bodies.append((NATNET_TARGET_NAME, DEFAULT_TARGET_STREAMING_ID, DEFAULT_TARGET_PATH)) + + self.natnet_manager = start_drone_natnet_server( + stage, bodies, **_NATNET_SERVER_KWARGS + ) + carb.log_warn( + f"[natnet] Emulator started with {NUM_ROBOTS} drone body(ies) " + f"and shared target '{NATNET_TARGET_NAME}' (robot_1/robot_2 subscribe via " + f"natnet_config; robot_3 omits Target)." + ) + 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() + else: + self.timeline.stop() + + app = omni.kit.app.get_app() + while simulation_app.is_running(): + world = World.instance() + if world is not None and hasattr(world, '_scene'): + world.step(render=True) + if world is not self.world: + self.world = world + self.pg._world = world + else: + 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() + + +def main(): + PegasusApp().run() + + +if __name__ == "__main__": + main() diff --git a/simulation/isaac-sim/launch_scripts/example_one_px4_pegasus_natnet_launch_script.py b/simulation/isaac-sim/launch_scripts/example_one_px4_pegasus_natnet_launch_script.py new file mode 100644 index 000000000..f2957108b --- /dev/null +++ b/simulation/isaac-sim/launch_scripts/example_one_px4_pegasus_natnet_launch_script.py @@ -0,0 +1,250 @@ +#!/usr/bin/env python +""" +Single-drone PX4 Pegasus launcher with OptiTrack NatNet mocap streaming. + +Same scene prep and sensor stack as ``example_one_px4_pegasus_launch_script.py``, plus +a Motive-compatible NatNet server that always streams: + + - ``Drone`` (id 1) from the Pegasus ``body`` prim under ``/World/base_link`` + - ``Target`` (id 100) from a static ``/World/target`` prim + +Pair with robot-side ``LAUNCH_NATNET=true`` and a matching ``natnet_config.yaml`` +profile. To consume the target on the robot, add a Target body to the profile +(see the commented scaffolding in ``natnet_config.yaml``). + +Override rigid-body names with ``NATNET_BODY_NAME`` / ``NATNET_TARGET_NAME``. +""" + +import os +import sys +import time +import asyncio + +import carb +from isaacsim import SimulationApp + +_LIVESTREAM = os.environ.get("ISAAC_SIM_LIVESTREAM", "").lower() == "true" + +if _LIVESTREAM: + _SIM_APP_CONFIG = { + "width": 1280, + "height": 720, + "window_width": 1920, + "window_height": 1080, + "headless": True, + "hide_ui": False, + "renderer": "RaytracedLighting", + "display_options": 3286, + } +else: + _SIM_APP_CONFIG = {"headless": False} + +simulation_app = SimulationApp(launch_config=_SIM_APP_CONFIG) + +if _LIVESTREAM: + from isaacsim.core.utils.extensions import enable_extension + simulation_app.set_setting("/app/window/drawMouse", True) + simulation_app.set_setting("/app/livestream/enabled", True) + LIVESTREAM_UDP_PORT = int(os.environ.get("ISAAC_SIM_LIVESTREAM_UDP_PORT", "49099")) + simulation_app.set_setting("/app/livestream/fixedHostPort", LIVESTREAM_UDP_PORT) + simulation_app.set_setting("/app/livestream/minHostPort", LIVESTREAM_UDP_PORT) + simulation_app.set_setting("/app/livestream/maxHostPort", LIVESTREAM_UDP_PORT) + enable_extension("omni.kit.livestream.webrtc") + +import omni.kit.app +import omni.timeline +import omni.usd + +from omni.isaac.core.world import World + +from pegasus.simulator.params import SIMULATION_ENVIRONMENTS +from pegasus.simulator.logic.interface.pegasus_interface import PegasusInterface +from pegasus.simulator.ogn.api.spawn_multirotor import spawn_px4_multirotor_node +from pegasus.simulator.ogn.api.spawn_zed_camera import add_zed_stereo_camera_subgraph +from pegasus.simulator.ogn.api.spawn_rtx_lidar import add_rtx_lidar_subgraph + +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 + +from optitrack.natnet.emulator.isaac import ( + DEFAULT_TARGET_PATH, + DEFAULT_TARGET_POSITION, + DEFAULT_TARGET_STREAMING_ID, + author_static_target, + start_drone_natnet_server, +) + +# --------------------- CONFIGURATION --------------------- +ENV_URL = SIMULATION_ENVIRONMENTS["Default Environment"] +STAGE_SCALE = 1.0 +SAVE_SCENE_TO = None +DRONE_USD = "~/.local/share/ov/data/documents/Kit/shared/exts/pegasus.simulator/pegasus/simulator/assets/Robots/Iris/iris.usd" + +NATNET_BODY_NAME = os.environ.get("NATNET_BODY_NAME", "Drone") +NATNET_TARGET_NAME = os.environ.get("NATNET_TARGET_NAME", "Target") + +_NATNET_SERVER_KWARGS = { + "pose_noise_enabled": True, + "pose_noise_std_meters": 0.0005, + "pose_noise_rotation_deg": 0.05, +} +# --------------------------------------------------------- + + +ext_manager = omni.kit.app.get_app().get_extension_manager() +for ext in [ + "omni.graph.core", + "omni.graph.action", + "omni.graph.action_nodes", + "isaacsim.core.nodes", + "omni.graph.ui", + "omni.graph.visualization.nodes", + "omni.graph.scriptnode", + "omni.graph.window.action", + "omni.graph.window.generic", + "omni.graph.ui_nodes", + "pegasus.simulator", +]: + if not ext_manager.is_extension_enabled(ext): + ext_manager.set_extension_enabled_immediate(ext, True) + + +def wait_for_stage(stage, timeout_s: float = 10.0): + for _ in range(int(timeout_s / 0.1)): + omni.kit.app.get_app().update() + world_prim = stage.GetPrimAtPath("/World") + if world_prim.IsValid(): + non_physics = [c for c in world_prim.GetChildren() if c.GetName() != "PhysicsScene"] + if non_physics: + return True + time.sleep(0.1) + return False + + +class PegasusApp: + + def __init__(self): + self.timeline = omni.timeline.get_timeline_interface() + self.natnet_manager = None + + self.pg = PegasusInterface() + self.pg._world = World(**self.pg._world_settings) + self.world = self.pg.world + self.timeline.stop() + + self.pg.load_environment(ENV_URL) + + stage = omni.usd.get_context().get_stage() + if stage is None: + raise RuntimeError("Stage failed to load") + + if not wait_for_stage(stage): + carb.log_warn("Stage load timed out — continuing anyway.") + + stage_prim = stage.GetPrimAtPath("/World/stage") + if stage_prim.IsValid(): + scale_stage_prim(stage, "/World/stage", STAGE_SCALE) + add_colliders(stage_prim) + for _ in range(10): + omni.kit.app.get_app().update() + else: + carb.log_warn("/World/stage not found — skipping scale and collision.") + + add_dome_light(stage) + + if SAVE_SCENE_TO: + import tempfile + tmp_usd = os.path.join(tempfile.gettempdir(), "prepared_scene.usd") + success, error = asyncio.get_event_loop().run_until_complete( + omni.usd.get_context().export_as_stage_async(tmp_usd) + ) + if success: + os.makedirs(SAVE_SCENE_TO, exist_ok=True) + save_scene_as_contained_usd(tmp_usd, SAVE_SCENE_TO) + os.remove(tmp_usd) + else: + carb.log_error(f"Scene export failed: {error}") + + graph_handle = spawn_px4_multirotor_node( + pegasus_node_name="PX4Multirotor", + drone_prim="/World/base_link", + robot_name="robot_1", + vehicle_id=1, + domain_id=1, + usd_file=DRONE_USD, + init_pos=[0.0, 0.0, 0.07], + init_orient=[0.0, 0.0, 0.0, 1.0], + ) + + add_zed_stereo_camera_subgraph( + parent_graph_handle=graph_handle, + drone_prim="/World/base_link", + robot_name="robot_1", + camera_name="ZEDCamera", + camera_offset=[0.2, 0.0, -0.05], + camera_rotation_offset=[0.0, 0.0, 0.0], + ) + + add_rtx_lidar_subgraph( + parent_graph_handle=graph_handle, + drone_prim="/World/base_link", + robot_name="robot_1", + lidar_config="ouster_os1", + lidar_topic_name="point_cloud_raw", + lidar_offset=[0.0, 0.0, 0.025], + lidar_rotation_offset=[0.0, 0.0, 0.0], + min_range=0.75, + ) + + 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 (drone + static target) and start the server.""" + try: + author_static_target(stage, DEFAULT_TARGET_PATH, DEFAULT_TARGET_POSITION) + bodies = [ + (NATNET_BODY_NAME, 1, "/World/base_link/body"), + (NATNET_TARGET_NAME, DEFAULT_TARGET_STREAMING_ID, DEFAULT_TARGET_PATH), + ] + self.natnet_manager = start_drone_natnet_server( + stage, bodies, **_NATNET_SERVER_KWARGS + ) + carb.log_warn( + f"[natnet] Emulator started: '{NATNET_BODY_NAME}' (-> /World/base_link/body), " + f"'{NATNET_TARGET_NAME}' (-> {DEFAULT_TARGET_PATH})." + ) + 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() + else: + self.timeline.stop() + + app = omni.kit.app.get_app() + while simulation_app.is_running(): + world = World.instance() + if world is not None and hasattr(world, '_scene'): + world.step(render=True) + if world is not self.world: + self.world = world + self.pg._world = world + else: + 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() + + +def main(): + PegasusApp().run() + + +if __name__ == "__main__": + main() diff --git a/tests/README.md b/tests/README.md index f10942ff7..7a992ac7d 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,14 +25,36 @@ 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` | Host NatNet emulator → `natnet_ros2` → `/{ROBOT_NAME}/perception/optitrack/Drone` Hz | Docker daemon (no GPU/sim); runs after `build_docker` / `build_packages` in collection order | + +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)). **Co-location + proxy pattern:** test source lives alongside its ROS 2 package at `robot/ros_ws/src///test/test_*.py` (the ROS 2 / colcon convention). -Files in `tests/robot/` are thin proxies that re-export those tests so that -`pytest tests/` discovers them. Both `airstack test -m unit` and +Files in `tests/robot/` are thin proxies that call ``register_unit_tests()`` (in +[`conftest.py`](conftest.py)) to register co-located tests so ``pytest tests/`` +discovers them. Both `airstack test -m unit` and `colcon test --packages-select ` run the same test source. Example: `robot/ros_ws/src/sensors/lidar_point_cloud_filter/test/test_validation_core.py` @@ -217,6 +240,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..a8889f1e8 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,62 @@ ) +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 register_unit_tests(target_globals: dict, test_dir: Path, *module_files: str) -> None: + """Register co-located unit tests with a thin proxy module under ``tests/``. + + AirStack keeps unit test **source** next to each package (colcon convention), + but CI runs pytest tests/. Each proxy file calls this helper so pytest + discovers the real tests. + + For each module_file in test_dir: + + 1. Prepend test_dir and test_dir.parent (package/extension root) to + sys.path so the loaded module can import production code and sibling + helpers (e.g. natnet_test_helpers). + 2. Load the file with importlib.util.spec_from_file_location under a + synthetic module name (_unit__) to avoid circular + imports when proxy and source share the same basename. + 3. Copy every test_* callable from the loaded module into + target_globals (typically the proxy's globals()) so pytest + collects them as if they lived under tests/. + 4. Wrap each callable with pytest.mark.unit so -m unit works even + when the source module omits pytestmark. + + Args: + target_globals: Namespace to populate (pass globals() from the proxy). + test_dir: Directory containing co-located test modules + (e.g. repo_path("robot/ros_ws/src//test")). + *module_files: One or more filenames under test_dir; multiple files + may be folded into a single proxy. + + pytest /test/ or colcon test bypasses proxies; use a local conftest.py in the package test/ dir for sys.path setup instead. + """ + 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(): @@ -84,9 +141,9 @@ def colcon_test_robot_command(workspace="robot"): cmd += f' --pytest-args "{pytest_args}"' return cmd # Unit tests live co-located with their ROS 2 packages in robot/ros_ws/src/. -# Thin proxy files under tests/robot/ re-export those tests so that -# `pytest tests/` and `airstack test -m unit` discover them without any -# sys.path manipulation here. Each proxy file sets up its own paths. +# Thin proxy files under tests/robot/ (etc.) call register_unit_tests() so +# `pytest tests/` and `airstack test -m unit` discover them without sys.path +# boilerplate in this root conftest. RUN_DIR = None LOGS_DIR = None ROS_DISTRO_SETUP = "/opt/ros/jazzy/setup.bash" @@ -122,15 +179,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") @@ -196,16 +270,17 @@ def pytest_generate_tests(metafunc): # Run cheap/fast-fail tests first so real problems surface early: -# docker image builds → colcon workspace builds → liveliness (infra) → sensors -# (ROS topic streams) → autonomy flight tests. +# unit → docker image builds → colcon builds → integration (robot container) → +# liveliness (full stack infra) → sensors → autonomy flight tests. _MODULE_ORDER = [ # Unit tests first — fast, hermetic, no Docker. Any module whose dotted # name starts with "robot." or "sim." is a proxy for a package-level unit # test and sorts into this leading slot via the prefix check below. "__unit__", - # System tests follow in dependency order. "system.test_build_docker", "system.test_build_packages", + # Integration needs robot-desktop image + colcon-built packages (e.g. natnet_ros2). + "__integration__", "system.test_liveliness", "system.test_sensors", "system.test_takeoff_hover_land", @@ -228,12 +303,14 @@ def _rank(name, order): def _module_key(item): """Return the ordering key for an item. - Unit-test proxies live under ``robot/``, ``sim/``, or ``gcs/`` and are - identified by their nodeid prefix. Everything else uses the dotted module - ``__name__`` looked up against ``_MODULE_ORDER``. + Unit-test proxies live under robot/, sim/, or gcs/. + Integration tests live under integration/. + Everything else uses the dotted module __name__ against _MODULE_ORDER. """ if item.nodeid.startswith(("robot/", "sim/", "gcs/")): return _rank("__unit__", _MODULE_ORDER) + if item.nodeid.startswith("integration/"): + return _rank("__integration__", _MODULE_ORDER) return _rank(getattr(item.module, "__name__", ""), _MODULE_ORDER) @@ -802,4 +879,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..841788917 100644 --- a/tests/docker/docker-compose.yaml +++ b/tests/docker/docker-compose.yaml @@ -13,5 +13,7 @@ services: environment: - AIRSTACK_ROOT=${AIRSTACK_PATH} - DISPLAY=${DISPLAY:-} + - 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..14189245b --- /dev/null +++ b/tests/integration/README.md @@ -0,0 +1,56 @@ +# 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`** | +| 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 using a full simulator or GPU. + +**Collection order** (see `pytest_collection_modifyitems` in [`../conftest.py`](../conftest.py)): + +## 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 + +# One scenario by path: +pytest tests/integration/natnet/ -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. Set `pytestmark = pytest.mark.integration`. +3. Request the `robot_autonomy_stack` fixture for the container. +4. Filter by path (`tests/integration//`) when you only want that scenario. + +## Residents + +| Scenario | What it verifies | +|----------|------------------| +| [`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..d0085a63f --- /dev/null +++ b/tests/integration/natnet/README.md @@ -0,0 +1,151 @@ +# NatNet ↔ robot autonomy integration + +Host-side NatNet wire-protocol tests that drive the Python emulator against +`natnet_ros2_node` in a real robot container. First resident of the +[`integration`](../README.md) tier (no sim, no GPU). + +Mark: `integration`. Filter this scenario with `tests/integration/natnet/`. + +For the **in-sim** end-to-end check (Isaac emulator + full stack), see +[Liveliness sentinel](#liveliness-sentinel-sim-end-to-end) below and +[`tests/system/test_liveliness.py`](../../system/test_liveliness.py). + +## What it verifies + +Three variants in [`test_natnet_integration.py`](test_natnet_integration.py). +All start a host-side `NatNetUnicastServer`, launch `natnet_ros2_node` in the +robot container pointed at the Docker bridge gateway, and assert a sustained +pose stream at **≥ 5 Hz** on the configured topic(s), e.g.: + +- `/{ROBOT_NAME}/perception/optitrack/drone/pose_cov` (wait for first message) +- `/{ROBOT_NAME}/perception/optitrack/drone` (Hz sample) + +| Test | Path | +|------|------| +| **`test_natnet_ros2_receives_drone_pose_hz`** | Hand-built `sFrameOfMocapData` frames enqueued on a raw `NatNetUnicastServer` (no USD). Minimal wire + SDK check. | +| **`test_natnet_ros2_receives_isaac_wrapper_pose_hz`** | Full Isaac data path: in-memory USD stage, `NatNetInterfaceConfig`, `author_interface`, `NatNetServerManager.sample_once()` on a moving prim — same sampling logic as the in-sim physics-step callback. Skips without `usd-core` (`pxr`). Pose-value fidelity is covered hermetically by the emulator's `test_pose_streaming.py` loopback. | +| **`test_natnet_ros2_multi_body_drone_and_target`** | Two bodies (drone id 1 + target id 100) with distinct relative topics; asserts both pose streams and that the target's `pose_cov` topic is **absent** (`body_pose_cov=false`). Exercises the multi-body profile + per-body `pose`/`pose_cov` toggles. | + +These tests **do not** start the full perception bringup or `LAUNCH_NATNET`; they +exec `natnet_ros2_node` directly with the flattened per-body params +(`body_names`/`body_ids`/`body_topics`/`body_pose`/`body_pose_cov`) and no MAVROS bridge. + +## Requirements + +- Docker daemon (robot-desktop container reachable from pytest). +- **`natnet_ros2_node` built** in the robot image (OptiTrack NatNet SDK is + license-gated — run `airstack setup --natnet`, then + `bws --packages-select natnet_ros2` in the container). Tests **skip** if the + node binary is missing. +- Host-side emulator package on `PYTHONPATH` (the test adds + `simulation/isaac-sim/extensions/optitrack.natnet.emulator` — not pip-installed + on the host). +- Ephemeral UDP ports on the host gateway IP (Docker default route as seen from + inside the container). + +The robot container comes from the shared **`robot_autonomy_stack`** fixture in +[`tests/conftest.py`](../../conftest.py) (see the [integration tier README](../README.md)). + +## Running + +```bash +# 1. One-time: NatNet SDK + build natnet_ros2 in the robot image +airstack setup --natnet # or NATNET_ACCEPT_LICENSE=1 airstack setup --natnet +docker exec airstack-robot-desktop-1 bash -lc 'bws --packages-select natnet_ros2' + +# 2a. Reuse an existing robot container (fast local iteration): +AUTOLAUNCH=false airstack up robot-desktop +pytest tests/integration/natnet/ -m integration -v + +# 2b. Let the harness bring the container up/down: +pytest tests/integration/natnet/ -m integration --run-integration -v +``` +On CI / PR (write access): `/pytest -m integration --run-integration` + +## Architecture + +``` +┌──────────────────────────────────────────────────────────────┐ +│ Host (pytest) │ +│ NatNetUnicastServer @ docker bridge gateway IP │ +│ • raw variant: hand-built frame queue │ +│ • Isaac variant: NatNetServerManager.sample_once(USD) │ +└────────────────────────────┬─────────────────────────────────┘ + │ UDP unicast (cmd + data ports) +┌────────────────────────────▼─────────────────────────────────┐ +│ Robot container (robot-desktop) │ +│ natnet_ros2_node (libNatNet 4.4 client) │ +│ → /{ROBOT_NAME}/{body topic}[/pose_cov] per configured body │ +└──────────────────────────────────────────────────────────────┘ +``` + +**In sim (liveliness tier):** the server runs inside the Isaac Sim container +(`172.31.0.200` by default). Use a NatNet Pegasus launch script +(`example_one_px4_pegasus_natnet_launch_script.py` or +`example_multi_px4_pegasus_natnet_launch_script.py`); `natnet_ros2` in the +robot stack connects via `natnet_config.yaml` (`server_ip` → emulator IP). + +**Catalog / MODELDEF:** The server holds a MODELDEF **wire cache** only +(`set_model_def_payload()`). Scene semantics (body names, streaming IDs, target +prim paths) come from the Isaac layer (`NatNetInterfaceConfig`, USD interface +prim, or launch-script `build_drone_config`). See the +[emulator README](../../../simulation/isaac-sim/extensions/optitrack.natnet.emulator/README.md). + +## Liveliness sentinel (sim end-to-end) + +The integration tier proves **robot client + host emulator** without Isaac. +The matching **system** check is +`TestLiveliness::test_natnet_pose_alive` in +[`test_liveliness.py`](../../system/test_liveliness.py): + +- **Gated on `LAUNCH_NATNET=true`** (skipped otherwise — normal liveliness runs + are unaffected). +- Asserts `/{robot_n}/{natnet pose topic}/pose_cov` ≥ 5 Hz per robot (the drone + body's configured topic — default `perception/optitrack/drone`). +- Override the checked topic with `NATNET_POSE_TOPIC` (default + `perception/optitrack/drone`). The sim body name (`NATNET_BODY_NAME`, default + `Drone`) is decoupled from the published topic, which the robot profile sets. + +Sim auto-start: set `ISAAC_SIM_SCRIPT_NAME` to a NatNet launch script and +`LAUNCH_NATNET=true` on the robot. Convenience bundle: +`airstack up --env-file overrides/isaac-natnet-vision.env` (NatNet script + +PX4 external-vision SITL profile). + +## libNatNet 4.4 unicast — verified wire contract + +The emulator is validated against the **real `libNatNet.so`** (not just the Python +`NatNetClient`) with a minimal C probe that registers `SetFrameReceivedCallback` +and `NatNet_SetLogCallback`. All of the following must hold for the SDK to deliver +frames to the callback: + +| Requirement | Why | +|-------------|-----| +| `NAT_CONNECT` → `sSender_Server` (279 B), name `Motive` | libNatNet reads `Motive 3.1 / NatNet 4.4` | +| `NAT_ECHOREQUEST` → `NAT_ECHORESPONSE` (16 B) | Prevents libNatNet assert | +| Frame ends with a **4-byte end-of-data tag** after `params` | libNatNet's frame unpacker reads it; without it the unpacked size mismatches `nDataBytes` and **every frame is silently dropped** | +| `NAT_FRAMEOFDATA` sent from the **data port** (source port == `data_port`) | libNatNet routes unicast frames by the server's data port. Frames sent from the **command** port are treated as command traffic and dropped — no error, no callback | +| `NAT_KEEPALIVE` gets **no reply** | An echo reply makes libNatNet log `Received unrecognized message Message=10` | + +With these in place the C probe reports `Server: Motive 3.1.0.0 NatNet 4.4.0.0`, +`data descriptions: 1`, and **~74 Hz** of frame callbacks. + +> The lenient Python `NatNetClient` accepts frames *without* the end-of-data tag +> and *on the command port*, which is why it appeared to work while libNatNet did +> not. Always validate against the C SDK. + +Full handshake notes and sniffing workflow: +[optitrack-development skill](../../../.agents/skills/optitrack-development/SKILL.md). + +## After changing natnet_ros2 or the emulator + +Rebuild in the robot container: + +```bash +docker exec airstack-robot-desktop-1 bash -lc 'bws --packages-select natnet_ros2' +``` + +Unit tests (protocol, serializers, Isaac wrapper loopback): + +```bash +pytest tests/sim/optitrack_natnet_emulator/ -m unit -v +``` diff --git a/tests/integration/natnet/test_natnet_integration.py b/tests/integration/natnet/test_natnet_integration.py new file mode 100644 index 000000000..2ac769501 --- /dev/null +++ b/tests/integration/natnet/test_natnet_integration.py @@ -0,0 +1,368 @@ +# Copyright (c) 2024 Carnegie Mellon University +# MIT License - see LICENSE in the repository root for full text. +"""NatNet - robot autonomy integration tests. + +Host-side variants stream frames to ``natnet_ros2_node`` in the robot container and +assert pose topics stay alive at >= 5 Hz: (1) raw ``NatNetUnicastServer`` hand-built +single-body frames; (2) ``NatNetServerManager`` sampling an in-memory USD stage +(Isaac wrapper path, no sim/GPU); (3) a multi-body profile (drone + target) that +exercises per-body topic overrides and the pose / pose_cov toggles. + +The node is parameterised with the flattened per-body arrays +(``body_names`` / ``body_ids`` / ``body_topics`` / ``body_pose`` / ``body_pose_cov``) +that natnet_ros2.launch.py derives from a robot's natnet_config.yaml profile. + +Multi-robot (NUM_ROBOTS=3, per-robot profiles) is exercised in-sim by +``tests/system/test_liveliness.py::test_natnet_pose_alive``. +""" + +from __future__ import annotations + +import subprocess +import sys +import threading +import time + +import pytest + +from conftest import ( # noqa: E402 — pytest adds tests/ to sys.path + docker_exec, + repo_path, + ros2_env, + sample_hz, + wait_for_first_message, +) + +# Emulator is not pip-installed on the host; add extension root + test helpers. +_EXT_ROOT = repo_path("simulation/isaac-sim/extensions/optitrack.natnet.emulator") +for _path in (_EXT_ROOT, _EXT_ROOT / "test"): + if str(_path) not in sys.path: + sys.path.insert(0, str(_path)) + +from optitrack.natnet.emulator import NatNetUnicastServer, TransmissionType # noqa: E402 +from optitrack.natnet.emulator.server import natnet_data_types as dt # noqa: E402 +from natnet_test_helpers import ephemeral_udp_port # noqa: E402 + +pytestmark = pytest.mark.integration + +_ROBOT_SETUP = "/root/AirStack/robot/ros_ws/install/setup.bash" +_NATNET_NODE = "/root/AirStack/robot/ros_ws/install/natnet_ros2/lib/natnet_ros2/natnet_ros2_node" +_WARMUP_S = 2.0 +_STREAM_HOLD_S = 12.0 +_MIN_HZ = 5.0 + +# Robot image has route/netstat but not `ip`; /proc/net/route is always present. +_DEFAULT_GATEWAY_CMD = ( + """awk '$2 == "00000000" { printf "%d.%d.%d.%d\\n", """ + """"0x" substr($3,7,2), "0x" substr($3,5,2), "0x" substr($3,3,2), "0x" substr($3,1,2); exit }' """ + """/proc/net/route""" +) + + +def _docker_default_gateway(container: str) -> str: + result = docker_exec(container, _DEFAULT_GATEWAY_CMD, timeout=10) + gateway = result.stdout.strip() + if not gateway: + pytest.skip(f"Could not resolve default gateway inside {container}") + return gateway + + +def _container_env(container: str, var: str, default: str) -> str: + # ROBOT_NAME / ROS_DOMAIN_ID are set in .bashrc (login shell), not container ENV. + # .bashrc may print "Sourcing ..." to stdout; take the last line as the value. + result = docker_exec(container, f"bash -lc 'echo ${var}'") + lines = [line.strip() for line in result.stdout.splitlines() if line.strip()] + value = lines[-1] if lines else "" + return value if value else default + + +def _natnet_node_available(container: str) -> bool: + result = docker_exec(container, f"test -x {_NATNET_NODE} && echo yes || echo no") + return "yes" in result.stdout + + +def _stop_stale_natnet_nodes(container: str) -> None: + docker_exec(container, "pkill -f natnet_ros2_node || true") + time.sleep(0.5) + + +# Each body: (streaming_id, rigid_body_name). The raw server frame carries ids only; +# the node maps ids → topics via its body_* params. +_DRONE_BODY = (1, "Drone") +_TARGET_BODY = (100, "Target") + + +def _make_frame(frame_num: int, body_ids) -> dt.sFrameOfMocapData: + frame = dt.sFrameOfMocapData() + frame.iFrame = frame_num + frame.nRigidBodies = len(body_ids) + for slot, body_id in enumerate(body_ids): + rb = frame.RigidBodies[slot] + rb.ID = body_id + rb.qw = 1.0 + # Bit 0 = tracking valid; natnet_ros2 skips bodies without it (natnet_logic.hpp). + rb.params = 1 + return frame + + +def _frame_publisher( + server: NatNetUnicastServer, stop_event: threading.Event, body_ids=(1,) +) -> None: + frame_num = 0 + interval = 1.0 / server.publish_rate + while not stop_event.is_set(): + server.enqueue_mocap_data(_make_frame(frame_num, body_ids)) + frame_num += 1 + time.sleep(interval) + + +def _launch_natnet_node(container, host_ip, command_port, domain_id, bodies=None): + """Start natnet_ros2_node in the container pointed at the host emulator. + + ``bodies`` is a list of (id, name, topic, pose, pose_cov); defaults to a single + Drone body on topic ``perception/optitrack/drone`` (the shipped config default). + """ + if bodies is None: + bodies = [(1, "Drone", "perception/optitrack/drone", "true", "true")] + ids = ",".join(str(b[0]) for b in bodies) + names = ",".join(b[1] for b in bodies) + topics = ",".join(b[2] for b in bodies) + pose = ",".join(b[3] for b in bodies) + pose_cov = ",".join(b[4] for b in bodies) + launch_cmd = ( + f"bash -lc '{ros2_env(_ROBOT_SETUP, domain_id)} && " + f"exec {_NATNET_NODE} --ros-args " + f"-p server_ip:={host_ip} " + f"-p command_port:={command_port} " + f"-p body_names:=[{names}] " + f"-p body_ids:=[{ids}] " + f"-p body_topics:=[{topics}] " + f"-p body_pose:=[{pose}] " + f"-p body_pose_cov:=[{pose_cov}]'" + ) + return subprocess.Popen( + ["docker", "exec", container, "bash", "-c", launch_cmd], + stdout=subprocess.PIPE, + stderr=subprocess.STDOUT, + text=True, + ) + + +def _assert_pose_stream(container, robot_name, domain_id, topic="perception/optitrack/drone"): + """Wait for the pose topic then assert a sustained rate >= _MIN_HZ.""" + pose_topic = f"/{robot_name}/{topic}" + 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): + """Raw-server path: hand-built frames on NatNetUnicastServer.""" + container = robot_autonomy_stack["container"] + + if not _natnet_node_available(container): + pytest.skip( + "natnet_ros2_node not built — run airstack setup (NatNet SDK) and " + "bws --packages-select natnet_ros2 in the robot container" + ) + + _stop_stale_natnet_nodes(container) + + host_ip = _docker_default_gateway(container) + command_port = ephemeral_udp_port(host_ip) + robot_name = _container_env(container, "ROBOT_NAME", "robot_1") + domain_id = int(_container_env(container, "ROS_DOMAIN_ID", "0")) + + server = NatNetUnicastServer( + local_interface=host_ip, + transmission_type=TransmissionType.UNICAST, + multicast_address=None, + command_port=command_port, + ) + server.publish_rate = 50 + + stop_event = threading.Event() + publisher = threading.Thread( + target=_frame_publisher, args=(server, stop_event), daemon=True + ) + + node_proc: subprocess.Popen[str] | None = None + try: + # Seed dummy frames before the client connects; keep streaming the whole window. + publisher.start() + time.sleep(0.1) + server.start() + node_proc = _launch_natnet_node(container, host_ip, command_port, domain_id) + _assert_pose_stream(container, robot_name, domain_id) + finally: + stop_event.set() + publisher.join(timeout=2.0) + _terminate(node_proc) + server.shutdown() + + +def test_natnet_ros2_receives_isaac_wrapper_pose_hz(robot_autonomy_stack): + """Isaac-wrapper path: NatNetServerManager.sample_once on a moving USD prim. + + Tests that the wrapper feeds the real robot client end-to-end. Pose-value fidelity + is covered by test_pose_streaming.py loopback. + """ + pytest.importorskip("pxr") + import math + + from pxr import Gf, Usd, UsdGeom + + from optitrack.natnet.emulator.isaac import ( + BodyBinding, + NatNetInterfaceConfig, + NatNetServerManager, + author_interface, + ) + + container = robot_autonomy_stack["container"] + if not _natnet_node_available(container): + pytest.skip("natnet_ros2_node not built — run airstack setup (NatNet SDK)") + + _stop_stale_natnet_nodes(container) + + host_ip = _docker_default_gateway(container) + command_port = ephemeral_udp_port(host_ip) + data_port = ephemeral_udp_port(host_ip) + while data_port == command_port: + data_port = ephemeral_udp_port(host_ip) + robot_name = _container_env(container, "ROBOT_NAME", "robot_1") + domain_id = int(_container_env(container, "ROS_DOMAIN_ID", "0")) + + stage = Usd.Stage.CreateInMemory() + xform = UsdGeom.Xform.Define(stage, "/World/base_link") + translate_op = xform.AddTranslateOp() + translate_op.Set(Gf.Vec3d(0.0, 0.0, 1.0)) + cfg = NatNetInterfaceConfig( + server_ip=host_ip, + command_port=command_port, + data_port=data_port, + publish_rate=50.0, + bodies=[BodyBinding("Drone", "/World/base_link", streaming_id=1)], + ) + author_interface(stage, "/World/NatNetInterface", cfg) + + manager = NatNetServerManager(server_factory=None) # real server factory + stop_event = threading.Event() + + def _sampler(): + # Stand in for the in-sim physics-step callback: move the prim and sample. + interval = 1.0 / cfg.publish_rate + t = 0.0 + while not stop_event.is_set(): + translate_op.Set(Gf.Vec3d(math.sin(t), 0.0, 1.0)) + manager.sample_once(stage) + t += interval + time.sleep(interval) + + sampler = threading.Thread(target=_sampler, daemon=True) + + node_proc: subprocess.Popen[str] | None = None + try: + assert manager.start_server(cfg) is True + sampler.start() + time.sleep(0.1) + node_proc = _launch_natnet_node(container, host_ip, command_port, domain_id) + _assert_pose_stream(container, robot_name, domain_id) + finally: + stop_event.set() + sampler.join(timeout=2.0) + _terminate(node_proc) + manager.stop_server() + + +def test_natnet_ros2_multi_body_drone_and_target(robot_autonomy_stack): + """Multi-body profile: one robot tracks a drone + a static target. + + Streams two bodies (drone id 1, target id 100) and configures the node like a + robot profile with two bodies and distinct relative topics. Asserts: the drone + pose streams >= 5 Hz on its custom topic; the target pose streams on its own + topic; and the target's pose_cov topic is absent (body_pose_cov=false). + """ + container = robot_autonomy_stack["container"] + + if not _natnet_node_available(container): + pytest.skip("natnet_ros2_node not built — run airstack setup (NatNet SDK)") + + _stop_stale_natnet_nodes(container) + + host_ip = _docker_default_gateway(container) + command_port = ephemeral_udp_port(host_ip) + robot_name = _container_env(container, "ROBOT_NAME", "robot_1") + domain_id = int(_container_env(container, "ROS_DOMAIN_ID", "0")) + + server = NatNetUnicastServer( + local_interface=host_ip, + transmission_type=TransmissionType.UNICAST, + multicast_address=None, + command_port=command_port, + ) + server.publish_rate = 50 + + bodies = [ + (_DRONE_BODY[0], _DRONE_BODY[1], "perception/optitrack/drone", "true", "true"), + (_TARGET_BODY[0], _TARGET_BODY[1], "perception/optitrack/target", "true", "false"), + ] + body_ids = (_DRONE_BODY[0], _TARGET_BODY[0]) + + stop_event = threading.Event() + publisher = threading.Thread( + target=_frame_publisher, args=(server, stop_event, body_ids), daemon=True + ) + + node_proc: subprocess.Popen[str] | None = None + try: + publisher.start() + time.sleep(0.1) + server.start() + node_proc = _launch_natnet_node(container, host_ip, command_port, domain_id, bodies) + # Drone (pose + pose_cov) and target (pose only) both stream. + _assert_pose_stream(container, robot_name, domain_id, "perception/optitrack/drone") + _assert_pose_stream(container, robot_name, domain_id, "perception/optitrack/target") + + # body_pose_cov=false → the target pose_cov publisher must not exist. + target_cov = f"/{robot_name}/perception/optitrack/target/pose_cov" + topics = docker_exec( + container, + f"bash -lc '{ros2_env(_ROBOT_SETUP, domain_id)} && ros2 topic list'", + timeout=15, + ).stdout + assert target_cov not in topics.split(), ( + f"{target_cov} should not exist when body_pose_cov=false; topics:\n{topics}" + ) + finally: + stop_event.set() + publisher.join(timeout=2.0) + _terminate(node_proc) + server.shutdown() diff --git a/tests/pytest.ini b/tests/pytest.ini index a664ccbf3..6e7bd7c6b 100644 --- a/tests/pytest.ini +++ b/tests/pytest.ini @@ -3,6 +3,7 @@ markers = unit: Fast hermetic tests (no Docker stack; numpy / pure Python) build_docker: Docker image build tests build_packages: Colcon workspace build tests + integration: Cross-component integration tests (robot container + a host-side component; no sim/GPU) 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 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/README.md b/tests/robot/README.md index a4409c4b1..2c7806367 100644 --- a/tests/robot/README.md +++ b/tests/robot/README.md @@ -20,13 +20,13 @@ convention): robot/ros_ws/src///test/test_.py ← source of truth ``` -**This directory** contains thin proxy files that load the real test module via -`importlib` and re-export its `test_*` functions, making them discoverable by -`pytest tests/` and `airstack test -m unit` without any changes to the CI -workflow. Each proxy is ~15 lines. +**This directory** contains thin proxy files that call ``register_unit_tests()`` +(from [`tests/conftest.py`](../conftest.py)) to register co-located ``test_*`` +functions for ``pytest tests/`` and ``airstack test -m unit``. Each proxy is +~10 lines. ``` -tests/robot///test_.py ← proxy (re-exports above) +tests/robot///test_.py ← proxy (registers above) ``` Both `airstack test -m unit` (pytest path via proxy) and diff --git a/tests/robot/perception/natnet_ros2/test_natnet_ros2.py b/tests/robot/perception/natnet_ros2/test_natnet_ros2.py index fc9a78bde..a8bcb1dfb 100644 --- a/tests/robot/perception/natnet_ros2/test_natnet_ros2.py +++ b/tests/robot/perception/natnet_ros2/test_natnet_ros2.py @@ -1,32 +1,11 @@ # Copyright (c) 2024 Carnegie Mellon University # MIT License - see LICENSE in the repository root for full text. -"""Proxy: re-exposes natnet_ros2 unit tests from the package source tree. +"""Proxy: registers natnet_ros2 Python unit tests for pytest tests/.""" -Unit test logic lives co-located with its package (ROS 2 / colcon convention): - robot/ros_ws/src/perception/natnet_ros2/test/test_natnet_ros2.py +from conftest import register_unit_tests, repo_path -This file makes those tests discoverable by ``pytest tests/`` (CI) and -``airstack test -m unit`` without any changes to the CI workflow. -Run ``colcon test --packages-select natnet_ros2`` to also execute the C++ -gtests and ament linters. -""" - -import importlib.util -import sys -from pathlib import 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) +register_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..677680092 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 @@ -1,6 +1,6 @@ # Copyright (c) 2024 Carnegie Mellon University # MIT License - see LICENSE in the repository root for full text. -"""Proxy: re-exposes validation_core unit tests from the package source tree. +"""Proxy: registers validation_core unit tests from the package source tree. Unit test logic lives co-located with its package (ROS 2 / colcon convention): robot/ros_ws/src/sensors/lidar_point_cloud_filter/test/test_validation_core.py @@ -11,28 +11,10 @@ the ament linters. """ -import importlib.util -import sys -from pathlib import Path +from conftest import register_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) +register_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..8bb6b4730 --- /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: registers NatNet emulator catalog-builder unit tests.""" + +from conftest import register_unit_tests, repo_path + +register_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..c598d25f0 --- /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: registers optitrack.natnet.emulator default/catalog unit tests for pytest tests/.""" + +from conftest import register_unit_tests, repo_path + +register_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..d0559b68f --- /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: registers NatNet emulator format_interface unit tests.""" + +from conftest import register_unit_tests, repo_path + +register_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..7a95f783e --- /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: registers optitrack.natnet.emulator pose->frame builder unit tests.""" + +from conftest import register_unit_tests, repo_path + +register_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..56a4d410c --- /dev/null +++ b/tests/sim/optitrack_natnet_emulator/test_interface_authoring.py @@ -0,0 +1,11 @@ +# Copyright (c) 2024 Carnegie Mellon University +# MIT License - see LICENSE in the repository root for full text. +"""Proxy: registers optitrack.natnet.emulator USD authoring unit tests (skips without ``pxr``).""" + +from conftest import register_unit_tests, repo_path + +register_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..a1e894bdc --- /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: registers optitrack.natnet.emulator interface config-model unit tests.""" + +from conftest import register_unit_tests, repo_path + +register_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..241b58e83 --- /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: registers optitrack.natnet.emulator pose-sampling / resync unit tests.""" + +from conftest import register_unit_tests, repo_path + +register_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..1d5480bf7 --- /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: registers optitrack.natnet.emulator end-to-end pose-streaming test.""" + +from conftest import register_unit_tests, repo_path + +register_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..8f124ee4d --- /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: registers optitrack.natnet.emulator launch-helper (scene_setup) tests.""" + +from conftest import register_unit_tests, repo_path + +register_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..f64cf3b35 --- /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: registers optitrack.natnet.emulator serializer unit tests for pytest tests/.""" + +from conftest import register_unit_tests, repo_path + +register_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..69cf75d96 --- /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: registers NatNet emulator server-from-config unit tests.""" + +from conftest import register_unit_tests, repo_path + +register_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..f1eddeed9 --- /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: registers optitrack.natnet.emulator server lifecycle unit tests.""" + +from conftest import register_unit_tests, repo_path + +register_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..803d21939 --- /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: registers optitrack.natnet.emulator target-resolution unit tests.""" + +from conftest import register_unit_tests, repo_path + +register_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..b163407cc --- /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: registers optitrack.natnet.emulator unicast-protocol unit tests for pytest tests/.""" + +from conftest import register_unit_tests, repo_path + +register_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..ce406444d 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,15 @@ "/robot_{N}/trajectory_controller/trajectory_control_node", ] +# NatNet pose sentinel — only when LAUNCH_NATNET=true (robot natnet_ros2 + sim emulator). +_NATNET_ENABLED = os.environ.get("LAUNCH_NATNET", "").strip().lower() in ("1", "true", "yes", "on") +# Relative pose topic from the robot's natnet_config.yaml profile (drone body), namespaced +# per robot below. Each profile's drone uses this same leaf, so it holds for multi-robot too. +_NATNET_POSE_TOPIC = os.environ.get("NATNET_POSE_TOPIC", "perception/optitrack/drone") +_NATNET_MIN_HZ = 5.0 +# Cold Isaac boot: Pegasus load, Play, UDP connect — 60s was too tight vs other budgets. +_NATNET_FIRST_MSG_TIMEOUT = 120 + def _parse_panes(raw): """Return (crashed, active_count). Input lines: 'session:window|pane_pid|title|kids'. @@ -251,6 +262,47 @@ 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): + """With LAUNCH_NATNET=true, /{robot}/{natnet_pose_topic}/pose_cov >= 5 Hz. + + In-sim counterpart to the host-side integration tests. Checks the pose_cov + variant of the drone body's configured topic (natnet_config.yaml profile). + """ + 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}/{_NATNET_POSE_TOPIC}/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)."""