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/.env b/.env index 020228fa2..a66f9354a 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.10" +VERSION="0.19.0-alpha.11" # Choose "dev" or "prebuilt". "dev" is for mounted code that must be built live. "prebuilt" is for built ros_ws baked into the image DOCKER_IMAGE_BUILD_MODE="dev" # Where to push and pull images from. Can replace with your docker hub username if using docker hub. diff --git a/CHANGELOG.md b/CHANGELOG.md index ab1c1ef0d..658849ded 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -16,6 +16,7 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0 - `integration` test tier (`tests/integration/`, `integration` mark) with a shared `robot_autonomy_stack` fixture (robot container, no sim/GPU) - Real-robot PX4 external-vision fusion in `natnet_ros2` (OptiTrack mocap → EKF2): `mavros_gp_origin` (geoid-corrected synthetic GPS origin so `local_position.z` == OptiTrack z, fixing the ~36 m boot offset), `vision_pose_converter`, and a PX4 param **checker** (`px4_param_setter`, `auto_set` off by default; `on_mismatch` warn/halt) — setup guide at `docs/robot/px4_external_vision.md` - NatNet server emulator (`optitrack.natnet.emulator`, protocol core) — pure-Python OptiTrack Motive server emulation so `natnet_ros2` can be driven without hardware; host integration tests (`tests/integration/natnet/`) wire it to the robot client +- Isaac wrapper for the NatNet emulator (USD scene → server) + natnet Pegasus launch scripts, and a dedicated OptiTrack sim e2e test (`optitrack` mark, `tests/system/test_optitrack_e2e.py`) that validates emulator → `natnet_ros2` → PX4 EV fusion in one Isaac bring-up ### Changed diff --git a/docs/simulation/isaac_sim/natnet_emulator.md b/docs/simulation/isaac_sim/natnet_emulator.md new file mode 100644 index 000000000..2bda072ee --- /dev/null +++ b/docs/simulation/isaac_sim/natnet_emulator.md @@ -0,0 +1,277 @@ +# NatNet Emulator (OptiTrack Simulation) + +The `optitrack.natnet.emulator` Isaac Sim extension lets you test the full +[`natnet_ros2`](../../../robot/ros_ws/src/perception/natnet_ros2/README.md) +perception stack in simulation without a physical OptiTrack system. It runs a +Motive-compatible NatNet UDP server inside Isaac Sim, streams rigid-body poses +sampled from USD prim world transforms, and presents the same wire protocol +that real Motive software uses — so `natnet_ros2` cannot tell the difference. + +## How it works + +``` +Isaac Sim (physics step) + ↓ sample prim world pose +NatNetServerManager (/World/NatNetInterface USD prim) + ↓ encode sFrameOfMocapData (NatNet 4.1 wire format) +NatNetUnicastServer ──UDP 1510/1511──► natnet_ros2_node (robot container) + ↓ + /robot_N/perception/optitrack/{body} + /robot_N/interface/mavros/vision_pose/pose +``` + +Configuration lives on a `/World/NatNetInterface` USD prim with `natnet:*` +attributes. Because it is USD, the config **persists when you save the stage** — +re-opening a `.usd` file restores the catalog and server settings without +re-running any script. + +Each physics step the extension: + +1. Reads the world transform of each tracked prim. +2. Packs a `sFrameOfMocapData` frame (one `sRigidBodyData` entry per body). +3. Flushes the frame immediately on the physics-step thread (no background timer). + +Bodies whose target prim is missing emit a **lost** frame (NaN position, +tracking-invalid bit clear) until the prim appears — this handles Pegasus drones +that are spawned on the first Play tick. + +Optional **sensor noise** (`pose_noise_std_meters`, `pose_noise_rotation_deg`) +adds Gaussian position and orientation perturbation to simulate real OptiTrack +measurement uncertainty. + +--- + +## Using the pre-built launch scripts + +The easiest way to start is with the provided Pegasus launch scripts. Set +`ISAAC_SIM_SCRIPT_NAME` in your environment or use the convenience override: + +```bash +# Multi-drone NatNet + PX4 external-vision (3 robots, vision-pose mode) +airstack up --env-file overrides/isaac-natnet-vision.env +``` + +`overrides/isaac-natnet-vision.env` sets: + +| Variable | Value | +|---|---| +| `NUM_ROBOTS` | `3` | +| `LAUNCH_NATNET` | `true` | +| `SITL_PARAM_PROFILE` | `px4-vision` | +| `ISAAC_SIM_SCRIPT_NAME` | `example_multi_px4_pegasus_natnet_launch_script.py` | + +### Available NatNet launch scripts + +| Script | Use case | +|---|---| +| `example_one_px4_pegasus_natnet_launch_script.py` | Single drone + static `Target` body | +| `example_multi_px4_pegasus_natnet_launch_script.py` | `NUM_ROBOTS` drones + shared `Target` body | + +Both scripts set up GPS origins (via `gps_utils.py`) so the GCS datum matches +PX4, start the NatNet emulator, and play the simulation automatically. + +!!! note "Baseline scripts have no NatNet" + `example_one_px4_pegasus_launch_script.py` and + `example_multi_px4_pegasus_launch_script.py` do **not** include NatNet. Use + the `*_natnet_*` variants above when you need mocap simulation. + +### Body naming + +| `NUM_ROBOTS` | Body names streamed | +|---|---| +| 1 | `Drone`, `Target` | +| N > 1 | `Drone1`, `Drone2`, …, `DroneN`, `Target` | + +Override the base name with `NATNET_BODY_NAME` (default `Drone`) and +`NATNET_TARGET_NAME` (default `Target`). These must match the +`rigid_body_name` entries in your robot's +[`natnet_config.yaml`](../../../robot/ros_ws/src/perception/natnet_ros2/config/natnet_config.yaml) +profile. + +### What the robot container needs + +Set `LAUNCH_NATNET=true` (already included in `isaac-natnet-vision.env`). +`natnet_ros2` connects to the emulator at `172.31.0.200` (the Isaac Sim +container's address on the AirStack bridge network) — the default in +`natnet_config.yaml`. + +After bringup, verify the stream is flowing: + +```bash +# Drone pose arriving from emulator +docker exec airstack-robot-desktop-1 bash -lc \ + "ros2 topic hz /robot_1/perception/optitrack/drone" + +# Vision pose forwarded to MAVROS / PX4 +docker exec airstack-robot-desktop-1 bash -lc \ + "ros2 topic hz /robot_1/interface/mavros/vision_pose/pose" +``` + +--- + +## Adding NatNet to your own launch script + +Call `start_drone_natnet_server` after your Pegasus drones are spawned: + +```python +import sys, os +sys.path.insert(0, os.path.join(os.path.dirname(__file__), "..", "utils")) +from optitrack.natnet.emulator.isaac import ( + start_drone_natnet_server, + author_static_target, + DEFAULT_TARGET_PATH, + DEFAULT_TARGET_STREAMING_ID, +) + +stage = omni.usd.get_context().get_stage() + +# Optional: add a static target body the robot can navigate toward. +author_static_target(stage, DEFAULT_TARGET_PATH, position=(2.0, 0.0, 1.0)) + +# One entry per drone: (rigid_body_name, streaming_id, target_prim_path) +drones = [ + ("Drone", 1, "/World/drone1/base_link/body"), +] + +# Keep a reference for the sim lifetime — dropping it stops the server. +self.natnet_manager = start_drone_natnet_server( + stage, + drones=drones, + server_ip="172.31.0.200", # Isaac container IP on AirStack bridge network + pose_noise_enabled=True, + pose_noise_std_meters=0.0005, + pose_noise_rotation_deg=0.05, +) +``` + +For multiple drones, add one tuple per drone: + +```python +drones = [ + ("Drone1", 1, "/World/drone1/base_link/body"), + ("Drone2", 2, "/World/drone2/base_link/body"), + ("Drone3", 3, "/World/drone3/base_link/body"), +] +``` + +`start_drone_natnet_server` also accepts the static target as a body — include +it explicitly if you want it: + +```python +from optitrack.natnet.emulator.isaac import DEFAULT_TARGET_STREAMING_ID, DEFAULT_TARGET_PATH + +drones = [ + ("Drone", 1, "/World/drone1/base_link/body"), + ("Target", DEFAULT_TARGET_STREAMING_ID, DEFAULT_TARGET_PATH), +] +``` + +--- + +## Using the Kit UI panel + +The extension registers a docked panel under **Window → NatNet Interface** in +the Isaac Sim menu bar (appears alongside the Pegasus panel). + +### Opening the panel + +Open Isaac Sim, load your scene, then go to **Window → NatNet Interface**. The +panel docks next to the Property panel in the bottom-right. + +### Panel controls + +| Button | Action | +|---|---| +| **Create Interface** | Author a fresh `/World/NatNetInterface` prim with current settings | +| **Save** | Push the form fields into the USD prim on the stage | +| **Load from Stage** | Pull the existing prim's values back into the form | +| **Print config** | Log the current config to the console | +| **Start Server / Stop Server** | Toggle the NatNet UDP server | + +!!! warning "Always Save before Start" + The server reads its configuration from the USD prim, not the form. Press + **Save** after every edit, then **Start Server**. + +### Server settings + +| Field | Default | Description | +|---|---|---| +| Server enabled | `true` | Uncheck to prevent auto-start on stage open | +| Server IP | `172.31.0.200` | IP the UDP socket binds to (Isaac container address) | +| Mode | `unicast` | `unicast` for direct; `multicast` for broadcast | +| Command port | `1510` | NatNet command channel | +| Data port | `1511` | NatNet data channel (frame stream) | +| Publish rate (Hz) | `120` | Target frame rate | +| Up axis | `Z` | `Z` passes poses through unchanged; `Y` re-axes for Y-up Motive | +| Pose noise enabled | `true` | Add Gaussian noise to simulate real sensor uncertainty | +| Pose noise std (m) | `0.0005` | Position noise std dev (0.5 mm, matching OptiTrack spec) | +| Pose noise rotation (deg) | `0.05` | Orientation noise std dev | + +### Adding tracked bodies + +1. In the Stage tree, **select the prim** you want to track (e.g. `/World/drone1/base_link/body`). +2. Click **Add body (from selection)** in the panel. +3. Fill in the **rigid body name** (must match the `rigid_body_name` in `natnet_config.yaml`) and **streaming ID**. +4. Click **Save**, then **Start Server**. + +Each body row shows a live readout of the prim's current world position with a +colour-coded status indicator: + +- 🟢 Green dot — server running, prim found, pose valid +- ⚫ Grey dot — prim found but server not running +- ✗ Red — prim missing or NaN position + +### Persistence + +After configuring the panel, save your USD stage (**File → Save**). The +`natnet:*` attributes are written into the `.usd` file. Re-opening the stage +restores the full catalog automatically — no script or panel interaction needed +unless you want to change the config. + +--- + +## Configuration reference + +`start_drone_natnet_server` and `build_drone_config` accept these keyword +arguments (all optional): + +| Parameter | Default | Description | +|---|---|---| +| `server_ip` | `"172.31.0.200"` | IP to bind the UDP server to | +| `mode` | `"unicast"` | `"unicast"` or `"multicast"` | +| `command_port` | `1510` | NatNet command port | +| `data_port` | `1511` | NatNet data port | +| `publish_rate` | `120.0` | Frame streaming rate (Hz) | +| `up_axis` | `"Z"` | Axis convention (`"Z"` or `"Y"`) | +| `pose_noise_enabled` | `True` | Enable sensor noise | +| `pose_noise_std_meters` | `0.0005` | Position noise std dev (m) | +| `pose_noise_rotation_deg` | `0.05` | Orientation noise std dev (degrees) | + +--- + +## Troubleshooting + +**`natnet_ros2` connects but no pose topics appear** + +- Check that `rigid_body_name` in `natnet_config.yaml` matches exactly what the + emulator is streaming (case-sensitive). Run `ros2 topic list` inside the robot + container and look for `/robot_N/perception/optitrack/...`. + +**Server starts but no data arrives in `natnet_ros2`** + +- Confirm the server IP matches the Isaac container's address (`172.31.0.200` + on the AirStack bridge). Check with `docker network inspect airstack_network`. +- The NatNet data port (1511) must be bound to the *data* socket — frames sent + from the command socket are silently dropped by libNatNet 4.4. + +**Emulator streams but `vision_pose` is empty** + +- `vision_pose` forwarding requires `vision_pose.enabled: true` in the robot's + `natnet_config.yaml` profile and `SITL_PARAM_PROFILE=px4-vision` so PX4 + accepts external vision instead of GPS. + +**Body shows ✗ red / NaN in the UI panel** + +- The target prim doesn't exist yet. This is normal before pressing Play (Pegasus + spawns the drone `base_link` prim on the first physics tick). After Play the + indicator should turn green within one frame. 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/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/schema/schema.usda b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/schema/schema.usda new file mode 100644 index 000000000..55defc731 --- /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). + + 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/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..25db5ad96 --- /dev/null +++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_pose_sampling.py @@ -0,0 +1,238 @@ +# 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", + pose_noise_enabled=False, + 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", + pose_noise_enabled=False, + 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", + pose_noise_enabled=False, + 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", + pose_noise_enabled=False, + 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", + pose_noise_enabled=False, + 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..bc526c5b5 --- /dev/null +++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_pose_streaming.py @@ -0,0 +1,86 @@ +# 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("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_launch_script.py b/simulation/isaac-sim/launch_scripts/example_one_px4_pegasus_launch_script.py index 11819fc2f..53c975e09 100755 --- a/simulation/isaac-sim/launch_scripts/example_one_px4_pegasus_launch_script.py +++ b/simulation/isaac-sim/launch_scripts/example_one_px4_pegasus_launch_script.py @@ -97,6 +97,12 @@ 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 +# gps_utils lives in this launch_scripts directory. +_LAUNCH_SCRIPTS_DIR = os.path.dirname(os.path.abspath(__file__)) +if _LAUNCH_SCRIPTS_DIR not in sys.path: + sys.path.insert(0, _LAUNCH_SCRIPTS_DIR) +from gps_utils import set_gps_origins, DEFAULT_WORLD_ORIGIN + # --------------------- CONFIGURATION --------------------- # Environment to load. Swap this URL/key for any other scene. @@ -111,6 +117,18 @@ SAVE_SCENE_TO = None # e.g. os.path.expanduser("~/AirStack/my_scene/") DRONE_USD = "~/.local/share/ov/data/documents/Kit/shared/exts/pegasus.simulator/pegasus/simulator/assets/Robots/Iris/iris.usd" + +# GPS world anchor: what world (0, 0, 0) maps to in real GPS coordinates. Must +# match the GCS origin (gcs_visualizer/gcs_utils.py) and the robot's +# natnet_ros2 mavros_gp_origin.yaml, otherwise PX4 SITL defaults to Zurich and +# Foxglove waypoints pick up a ~1.8e6 m boot-ENU offset (wrong-way navigation). +WORLD_GPS_ORIGIN = DEFAULT_WORLD_ORIGIN + +# Single drone spawned at the world origin. domain_id / spawn must match the +# spawn_px4_multirotor_node call below. +DRONE_CONFIGS = [ + {"domain_id": 1, "x_m": 0.0, "y_m": 0.0, "z_m": 0.07}, +] # --------------------------------------------------------- @@ -149,6 +167,10 @@ def wait_for_stage(stage, timeout_s: float = 10.0): class PegasusApp: def __init__(self): + # Write GPS home before spawning so the drone's global position shares + # the GCS datum. Must run before the PX4 SITL subprocess starts. + set_gps_origins(DRONE_CONFIGS, world_origin=WORLD_GPS_ORIGIN) + self.timeline = omni.timeline.get_timeline_interface() # Start Pegasus interface + world 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..12fa2213f --- /dev/null +++ b/simulation/isaac-sim/launch_scripts/example_one_px4_pegasus_natnet_launch_script.py @@ -0,0 +1,273 @@ +#!/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 + +# gps_utils lives in this launch_scripts directory. +_LAUNCH_SCRIPTS_DIR = os.path.dirname(os.path.abspath(__file__)) +if _LAUNCH_SCRIPTS_DIR not in sys.path: + sys.path.insert(0, _LAUNCH_SCRIPTS_DIR) +from gps_utils import set_gps_origins, DEFAULT_WORLD_ORIGIN + +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" + +# GPS world anchor: what world (0, 0, 0) maps to in real GPS coordinates. Must +# match the GCS origin (gcs_visualizer/gcs_utils.py) and the robot's +# natnet_ros2 mavros_gp_origin.yaml. In vision/mocap mode the robot-side +# mavros_gp_origin node is the authoritative datum; this keeps PX4's SITL home +# consistent with it so the two never disagree. +WORLD_GPS_ORIGIN = DEFAULT_WORLD_ORIGIN + +# Single drone spawned at the world origin. domain_id / spawn must match the +# spawn_px4_multirotor_node call below. +DRONE_CONFIGS = [ + {"domain_id": 1, "x_m": 0.0, "y_m": 0.0, "z_m": 0.07}, +] + +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): + # Write GPS home before spawning so the drone's global position shares + # the GCS datum. Must run before the PX4 SITL subprocess starts. + set_gps_origins(DRONE_CONFIGS, world_origin=WORLD_GPS_ORIGIN) + + 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/harness/collection.py b/tests/harness/collection.py index aa20c9f1e..cca487d07 100644 --- a/tests/harness/collection.py +++ b/tests/harness/collection.py @@ -24,6 +24,8 @@ "system.test_sensors", "system.test_takeoff_hover_land", "system.test_fixed_trajectory", + # OptiTrack e2e: its own single NatNet bring-up, runs after the flight suites. + "system.test_optitrack_e2e", ] # Within test_takeoff_hover_land, each (env, velocity) runs phases in this chain order. diff --git a/tests/integration/natnet/test_natnet_integration.py b/tests/integration/natnet/test_natnet_integration.py index e386fd1ee..43fa4a7f3 100644 --- a/tests/integration/natnet/test_natnet_integration.py +++ b/tests/integration/natnet/test_natnet_integration.py @@ -234,6 +234,80 @@ def test_natnet_ros2_receives_drone_pose_hz(robot_autonomy_stack): server.shutdown() +def test_natnet_ros2_receives_isaac_wrapper_pose_hz(robot_autonomy_stack): + """Isaac-wrapper path: NatNetServerManager.sample_once on a moving USD prim. + + Tests that the wrapper feeds the real robot client end-to-end. Pose-value fidelity + is covered by test_pose_streaming.py loopback. + """ + pytest.importorskip("pxr") + import math + + from pxr import Gf, Usd, UsdGeom + + from optitrack.natnet.emulator.isaac import ( + BodyBinding, + NatNetInterfaceConfig, + NatNetServerManager, + author_interface, + ) + + container = robot_autonomy_stack["container"] + if not _natnet_node_available(container): + pytest.skip("natnet_ros2_node not built — run airstack setup (NatNet SDK)") + + _stop_stale_natnet_nodes(container) + + host_ip = _docker_default_gateway(container) + command_port = ephemeral_udp_port(host_ip) + data_port = ephemeral_udp_port(host_ip) + while data_port == command_port: + data_port = ephemeral_udp_port(host_ip) + robot_name = _container_env(container, "ROBOT_NAME", "robot_1") + domain_id = int(_container_env(container, "ROS_DOMAIN_ID", "0")) + + stage = Usd.Stage.CreateInMemory() + xform = UsdGeom.Xform.Define(stage, "/World/base_link") + translate_op = xform.AddTranslateOp() + translate_op.Set(Gf.Vec3d(0.0, 0.0, 1.0)) + cfg = NatNetInterfaceConfig( + server_ip=host_ip, + command_port=command_port, + data_port=data_port, + publish_rate=50.0, + bodies=[BodyBinding("Drone", "/World/base_link", streaming_id=1)], + ) + author_interface(stage, "/World/NatNetInterface", cfg) + + manager = NatNetServerManager(server_factory=None) # real server factory + stop_event = threading.Event() + + def _sampler(): + # Stand in for the in-sim physics-step callback: move the prim and sample. + interval = 1.0 / cfg.publish_rate + t = 0.0 + while not stop_event.is_set(): + translate_op.Set(Gf.Vec3d(math.sin(t), 0.0, 1.0)) + manager.sample_once(stage) + t += interval + time.sleep(interval) + + sampler = threading.Thread(target=_sampler, daemon=True) + + node_proc: subprocess.Popen[str] | None = None + try: + assert manager.start_server(cfg) is True + sampler.start() + time.sleep(0.1) + node_proc = _launch_natnet_node(container, host_ip, command_port, domain_id) + _assert_pose_stream(container, robot_name, domain_id) + finally: + stop_event.set() + sampler.join(timeout=2.0) + _terminate(node_proc) + manager.stop_server() + + def test_natnet_ros2_multi_body_drone_and_target(robot_autonomy_stack): """Multi-body profile: one robot tracks a drone + a static target. diff --git a/tests/pytest.ini b/tests/pytest.ini index dc8c939de..dd8b0e2c7 100644 --- a/tests/pytest.ini +++ b/tests/pytest.ini @@ -8,6 +8,7 @@ markers = sensors: Sim and robot sensor topic rates, LiDAR validation, sim RTF takeoff_hover_land: End-to-end takeoff / hover / land action tests autonomy: Fixed-pattern trajectory path-tracker benchmark (test_fixed_trajectory.py) + optitrack: OptiTrack NatNet end-to-end (sim emulator → natnet_ros2 → PX4 EV fusion) testpaths = . addopts = -v --durations=0 --import-mode=importlib cache_dir = /tmp/.pytest_cache diff --git a/tests/requirements.txt b/tests/requirements.txt index a4b43e6bb..757d1ebdb 100644 --- a/tests/requirements.txt +++ b/tests/requirements.txt @@ -6,3 +6,5 @@ tabulate psutil pandas numpy +scipy +usd-core diff --git a/tests/system/test_optitrack_e2e.py b/tests/system/test_optitrack_e2e.py new file mode 100644 index 000000000..87cafebfd --- /dev/null +++ b/tests/system/test_optitrack_e2e.py @@ -0,0 +1,135 @@ +"""OptiTrack NatNet end-to-end (sim). + +A single dedicated bring-up that exercises the whole OptiTrack path in Isaac Sim: +the in-sim NatNet **emulator** streams rigid-body poses → ``natnet_ros2`` publishes +the drone pose → the ``vision_pose`` bridge feeds MAVROS → PX4 EKF2 fuses it. + +This intentionally does NOT add ``isaacsim_natnet`` as a third parametrized sim in +the ``airstack_env`` matrix (that would re-run the entire liveliness/sensors/flight +suite under NatNet for two assertions). Instead we bring the NatNet stack up **once** +here and assert only the NatNet-specific chain. The cheap, GPU-free half of this +(host emulator → ``natnet_ros2`` Hz) lives in ``tests/integration/natnet/``. + +Mark: ``optitrack``. Needs Docker + GPU + Isaac Sim license; skips cleanly when the +isaac-sim image isn't built locally. +""" +import os +import time + +import pytest + +from conftest import ( # noqa: E402 — pytest adds tests/ to sys.path + airstack_cmd, + container_running, + find_container, + get_metrics, + get_robot_containers, + logger, + missing_images, + read_log_tail, + sample_hz, + wait_for_container, + wait_for_first_message, +) + +pytestmark = pytest.mark.optitrack + +# Single-drone NatNet Isaac stack: the natnet Pegasus script spawns the emulator +# alongside PX4, and LAUNCH_NATNET=true brings up natnet_ros2 + the vision_pose / +# gp_origin / param bridges on the robot. +_E2E_ENV = { + "NUM_ROBOTS": "1", + "COMPOSE_PROFILES": "desktop,isaac-sim", + "AUTOLAUNCH": "true", + "ISAAC_SIM_USE_STANDALONE": "true", + "ISAAC_SIM_SCRIPT_NAME": "example_one_px4_pegasus_natnet_launch_script.py", + "PLAY_SIM_ON_START": "true", + "LAUNCH_NATNET": "true", + # Headless: no X on the CI runner. + "QT_QPA_PLATFORM": "offscreen", +} + +_ROBOT_PATTERN = "robot.*desktop" +_ROBOT_SETUP_BASH = "/root/AirStack/robot/ros_ws/install/setup.bash" +_ROBOT_DOMAIN = 1 +# Drone body's relative pose topic from natnet_config.yaml, namespaced per robot. +_NATNET_POSE_TOPIC = os.environ.get("NATNET_POSE_TOPIC", "perception/optitrack/drone") +_NATNET_MIN_HZ = 5.0 +# PX4 fused local position (proves EKF2 accepted the external vision). +_PX4_LOCAL_POSE_TOPIC = "interface/mavros/local_position/pose" +# Cold Isaac boot: Pegasus load + Play + emulator UDP connect. +_FIRST_MSG_TIMEOUT = 180 + + +@pytest.fixture(scope="module") +def optitrack_sim_stack(request): + """Bring the NatNet Isaac stack up once for the module; tear it down after. + + Reuses an already-running robot-desktop container (fast local iteration); + otherwise brings the stack up. Skips when the isaac-sim image isn't built. + """ + existing = find_container(_ROBOT_PATTERN) + if existing and container_running(existing): + yield {"container": existing, "brought_up": False} + return + + missing = missing_images(env=_E2E_ENV) + if missing: + pytest.skip("isaac-sim / robot image not built locally: " + ", ".join(missing)) + + airstack_cmd("down", timeout=120, log_name="optitrack_e2e") + result = airstack_cmd("up", env_overrides=_E2E_ENV, timeout=300, log_name="optitrack_e2e") + if result.returncode != 0: + pytest.fail(f"`airstack up` (natnet isaac) failed:\n{read_log_tail('optitrack_e2e')}") + + container = wait_for_container(_ROBOT_PATTERN, timeout=180) + assert container, "robot-desktop container not Running after 180s" + try: + yield {"container": container, "brought_up": True} + finally: + airstack_cmd("down", timeout=120, log_name="optitrack_e2e") + + +def _robot_container(stack): + # robot_1 lives on the first (index-1) replica. + return get_robot_containers(_ROBOT_PATTERN)[0] if not stack["brought_up"] \ + else wait_for_container(_ROBOT_PATTERN, timeout=60) + + +class TestOptitrackE2E: + + @pytest.mark.dependency(name="natnet_pose") + def test_natnet_pose_alive(self, optitrack_sim_stack): + """Emulator → natnet_ros2 → vision_pose: the drone pose_cov streams >= 5 Hz.""" + container = _robot_container(optitrack_sim_stack) + topic = f"/robot_{_ROBOT_DOMAIN}/{_NATNET_POSE_TOPIC}/pose_cov" + + first = wait_for_first_message( + container, topic, domain_id=_ROBOT_DOMAIN, + setup_bash=_ROBOT_SETUP_BASH, timeout=_FIRST_MSG_TIMEOUT, + ) + assert first is not None, ( + f"no NatNet pose on {topic} within {_FIRST_MSG_TIMEOUT}s " + "(emulator → natnet_ros2 path down)" + ) + hz = sample_hz(container, topic, domain_id=_ROBOT_DOMAIN, + setup_bash=_ROBOT_SETUP_BASH, duration=5, window=20) + get_metrics().record("test_optitrack_e2e.natnet_pose_hz", + "natnet_pose_hz", hz if hz is not None else "none", unit="Hz") + assert hz is not None and hz >= _NATNET_MIN_HZ, \ + f"{topic} at {hz} Hz (< {_NATNET_MIN_HZ})" + + @pytest.mark.dependency(depends=["natnet_pose"]) + def test_px4_fuses_vision(self, optitrack_sim_stack): + """PX4 EKF2 fuses the external vision: local_position/pose streams.""" + container = _robot_container(optitrack_sim_stack) + topic = f"/robot_{_ROBOT_DOMAIN}/{_PX4_LOCAL_POSE_TOPIC}" + + first = wait_for_first_message( + container, topic, domain_id=_ROBOT_DOMAIN, + setup_bash=_ROBOT_SETUP_BASH, timeout=_FIRST_MSG_TIMEOUT, + ) + assert first is not None, ( + f"no PX4 local_position on {topic} within {_FIRST_MSG_TIMEOUT}s " + "(EKF2 not fusing external vision)" + )