diff --git a/.agents/skills/add-unit-tests/SKILL.md b/.agents/skills/add-unit-tests/SKILL.md index 7d1d3b582..ea47f7b75 100644 --- a/.agents/skills/add-unit-tests/SKILL.md +++ b/.agents/skills/add-unit-tests/SKILL.md @@ -35,11 +35,12 @@ robot/ros_ws/src/// └── CMakeLists.txt # wires ament_add_gtest under BUILD_TESTING tests/robot/// -└── test_.py # ← thin PROXY (re-exports tests from above) +└── test_.py # ← thin PROXY (registers tests from above) ``` -The **proxy** is a one-file shim that loads the real test module with `importlib` -and re-exports every `test_*` function. This means: +The **proxy** is a one-file shim that calls ``register_unit_tests()`` to load the +real test module with ``importlib`` and expose every ``test_*`` function to pytest. +This means: | Invocation | What runs | |---|---| @@ -48,6 +49,39 @@ and re-exports every `test_*` function. This means: | CI `system-tests.yml` (PR open / approved) | Same path via `pytest tests/` | | `colcon test --packages-select ` | Real test in `package/test/` directly | +### `register_unit_tests` (in `tests/conftest.py`) + +Proxies call this helper; they do not duplicate test logic. Signature: + +```python +register_unit_tests(target_globals, test_dir, *module_files) +``` + +| Argument | Meaning | +|---|---| +| `target_globals` | Pass ``globals()`` from the proxy module — pytest collects ``test_*`` names injected here | +| `test_dir` | Co-located test directory, usually ``repo_path("robot/ros_ws/src///test")`` | +| `*module_files` | One or more filenames under ``test_dir`` (e.g. ``"test_foo.py"``); fold several into one proxy | + +**What it does (in order):** + +1. Prepends ``test_dir`` and ``test_dir.parent`` (package or extension root) to + ``sys.path`` so loaded modules can import production code and sibling helpers. +2. Loads each file via ``importlib.util.spec_from_file_location`` under a + synthetic name (``_unit__``) so proxy and source can share the + same basename without circular imports. +3. Copies every ``test_*`` callable from the loaded module into ``target_globals``. +4. Wraps each with ``pytest.mark.unit`` so ``pytest tests/ -m unit`` selects them + even if the source omitted ``pytestmark``. + +**What it does not do:** run tests, install packages, or replace ``colcon test``. +For local iteration against source only, run ``pytest /test/`` (add a tiny +``conftest.py`` in that dir for ``sys.path`` — see the emulator example below). + +Pair with ``repo_path()`` from the same conftest — paths are anchored on +``AIRSTACK_ROOT`` (CI export, repo root locally). **Never** use +``Path(__file__).parents[N]`` in a proxy. + ## Step-by-Step: Adding a Python Unit Test ### 1. Identify pure-Python logic to test @@ -114,50 +148,34 @@ For `rclpy.node.Node` subclasses use a real dummy base class instead of a ### 3. Write the thin proxy in tests/robot/ -Create `tests/robot///test_.py`: +Create `tests/robot///test_.py`. Use +``register_unit_tests`` + ``repo_path`` from ``tests/conftest.py`` so the proxy +stays a two-call shim: ```python # Copyright (c) 2024 Carnegie Mellon University # MIT License - see LICENSE in the repository root for full text. -"""Proxy: re-exposes unit tests from the package source tree. +"""Proxy: registers unit tests from the package source tree. -Unit test logic lives co-located with the package source (ROS 2 / colcon convention): +Unit test logic lives co-located with the package (ROS 2 / colcon convention): robot/ros_ws/src///test/test_.py -This file makes those tests discoverable by ``pytest tests/`` (CI) and -``airstack test -m unit`` without any changes to the CI workflow. +Discoverable by ``pytest tests/`` (CI) and ``airstack test -m unit``. """ -import importlib.util -import sys -from pathlib import Path - -_repo_root = Path(__file__).resolve().parents[N] # adjust N so this resolves to repo root -_pkg_test = _repo_root / "robot/ros_ws/src///test" -_real_file = _pkg_test / "test_.py" +from conftest import register_unit_tests, repo_path -# If the test imports from a package module, ensure the package root is on sys.path. -# Example: _pkg_root = _pkg_test.parent; sys.path.insert(0, str(_pkg_root)) - -# Load the real module under a unique name to avoid the circular import that -# would occur if we used `from test_ import *` (this file has the same -# name, and pytest adds its directory to sys.path at collection time). -_spec = importlib.util.spec_from_file_location("__unit_tests", _real_file) -_real = importlib.util.module_from_spec(_spec) -_spec.loader.exec_module(_real) - -# Re-export every test_* symbol so pytest collects them from this proxy. -for _name in dir(_real): - if _name.startswith("test_"): - globals()[_name] = getattr(_real, _name) +register_unit_tests( + globals(), + repo_path("robot/ros_ws/src///test"), + "test_.py", # pass several filenames to fold multiple modules into one proxy +) ``` -**Counting `parents[N]` to reach the repo root:** - -| Proxy location | `parents[N]` for repo root | -|---|---| -| `tests/robot///` | `parents[4]` | -| `tests/sim//` | `parents[3]` | -| `tests/gcs//` | `parents[3]` | +For a **direct** `pytest /test/` (or `colcon test`) run — which bypasses +the proxies — add a tiny `conftest.py` in the package `test/` dir that puts the +package/extension root on `sys.path` (see +`simulation/.../optitrack.natnet.emulator/test/conftest.py`). The test source +then stays free of `sys.path` boilerplate. ### 4. Ensure the tests/ directory structure exists @@ -257,16 +275,16 @@ there are listed in [`tests/colcon_unit_test_packages.yaml`](../../../tests/colc The same proxy pattern applies verbatim: -**Sim-side Python** (e.g. motive emulator protocol logic): +**Sim-side Python** (e.g. OptiTrack NatNet emulator): ``` simulation/...//test/test_.py ← source -tests/sim//test_.py ← proxy (parents[3] = repo root) +tests/sim//test_.py ← proxy (register_unit_tests + repo_path) ``` **GCS modules**: ``` gcs/...//test/test_.py ← source -tests/gcs//test_.py ← proxy (parents[3] = repo root) +tests/gcs//test_.py ← proxy (register_unit_tests + repo_path) ``` `pytest tests/ -m unit` discovers them through the proxy without any @@ -280,7 +298,8 @@ pytest.ini or CI changes needed. |---|---| | Where does test source live? | `/…//test/` (co-located with the package) | | Where does pytest discover tests? | `tests/robot/` (or `tests/sim/`, `tests/gcs/`) via thin proxy | -| How does the proxy avoid circular import? | `importlib.util.spec_from_file_location` with a unique module name | +| How does the proxy register tests? | ``register_unit_tests(globals(), repo_path(...), "test_*.py")`` in `tests/conftest.py` | +| How does the proxy avoid circular import? | `importlib.util.spec_from_file_location` with a unique synthetic module name | | What mark do all unit tests use? | `@pytest.mark.unit` | | What CI workflow runs them? | `system-tests.yml` — runs `pytest tests/` which includes unit tests | | When does that workflow trigger? | PR opened, `/pytest` comment, `workflow_dispatch` | @@ -302,8 +321,9 @@ Corresponding proxies: `tests/robot/perception/natnet_ros2/test_natnet_ros2.py`, ## Files to Know - `.github/workflows/system-tests.yml` — CI workflow (runs `pytest tests/` including unit tests) +- `tests/conftest.py` — `register_unit_tests`, `repo_path`, shared fixtures - `tests/pytest.ini` — mark registration (`unit`, `build_docker`, etc.) - `tests/robot/` — proxy layer mirroring `robot/ros_ws/src/` -- `tests/sim/` — proxy layer for sim-side code (future) +- `tests/sim/` — proxy layer for sim-side extensions and tools - `tests/gcs/` — proxy layer for GCS code (future) - `tests/README.md` — full test harness reference diff --git a/.agents/skills/docker-build-profiles/SKILL.md b/.agents/skills/docker-build-profiles/SKILL.md index 64b579a01..7e54abbb8 100644 --- a/.agents/skills/docker-build-profiles/SKILL.md +++ b/.agents/skills/docker-build-profiles/SKILL.md @@ -7,12 +7,14 @@ Summary When to use - When adding or updating a `docker-compose` profile that passes `PYTHON_VERSION`, `ROS_DISTRO`, or other numeric-like build args. - When an automated agent needs to verify a new profile will produce a correct `PYTHONPATH` and avoid YAML float-parsing bugs. +- When adding or reviewing platform-specific robot builds that need a non-default ROS library architecture suffix in `LD_LIBRARY_PATH`. Actions the agent can perform 1. Validate `docker-compose.yaml` args are quoted when numeric-like (e.g. `PYTHON_VERSION: "3.10"`). 2. Insert a build-time validation `RUN` into `robot/docker/Dockerfile.robot` to fail early when the ROS Python path does not exist. -3. Add or update a short test in documentation showing how to build the `builder` stage and check `ament_package` import. -4. Suggest `network: host` under `build:` for L4T/Jetson profiles only when necessary (kernel iptables workarounds). +3. Ensure `TARGET_ARCH` is passed as a `build.args` value, not a runtime `environment` value, when a profile needs a non-default ROS library path (default is `x86_64`; arm64 targets such as VOXL and L4T/Jetson should use `aarch64`). +4. Add or update a short test in documentation showing how to build the `builder` stage and check `ament_package` import. +5. Suggest `network: host` under `build:` for L4T/Jetson profiles only when necessary (kernel iptables workarounds). Snippets (copyable) @@ -43,12 +45,14 @@ docker run --rm -it airstack-builder-test:local bash -c "python3 -c 'import amen Guidance for agents when editing the repo - Prefer making minimal, reversible changes: add the `RUN test -d ...` check early in the Dockerfile and gate it with informative message text. - When updating `docker-compose.yaml`, only quote the numeric-like values; do not change unrelated fields. +- Put `TARGET_ARCH` under `build.args`; setting it under `environment` is too late because `LD_LIBRARY_PATH` is baked into the image by `Dockerfile.robot`. - If creating PRs, include a short note in the PR description instructing maintainers to run the builder-stage sanity build on both an amd64 desktop profile and an arm64 L4T profile. Troubleshooting notes - YAML quirk: unquoted `3.10` may be parsed as float `3.1` — this changes path strings and breaks imports (e.g., `python3.1` instead of `python3.10`). - Jetson/L4T builds may require `network: host` during the build to avoid kernel iptables/raw table missing-module errors. - Jetson **`robot-l4t`** builds from **`robot-l4t-stack-base`** (`robot/docker/Dockerfile.l4t-stack-base`), not raw dustynv, so **`Dockerfile.robot` stays Ubuntu-shaped.** `airstack image-build --profile l4t robot-l4t` triggers **`robot-l4t-stack-base`** first (`airstack.sh`); bare `compose build robot-l4t` can still parallelize badly, so list stack-base explicitly if not using AirStack CLI. +- `Dockerfile.robot` defaults `TARGET_ARCH=x86_64` for desktop builds. Arm64 services (for example, VOXL and Jetson/L4T) must pass `TARGET_ARCH: aarch64` in `build.args` so the ROS library path includes `/opt/ros/${ROS_DISTRO}/lib/aarch64-linux-gnu`. Examples of agent prompts - "Check `robot/docker/docker-compose.yaml` for `PYTHON_VERSION` entries and quote any unquoted numeric values; open a PR with the fixes and include a test log from a builder-stage build." @@ -77,6 +81,7 @@ This section shows the minimal, recommended steps an agent or maintainer should - Add a service block in `robot/docker/docker-compose.yaml` (or an override file) and set `build.args` for the profile. - Always quote `PYTHON_VERSION` values (e.g. `"3.10"`) so YAML does not convert them to floats. + - Leave `TARGET_ARCH` unset for x86-64 desktop builds. Set `TARGET_ARCH: aarch64` under `build.args` for arm64 builds so `Dockerfile.robot` composes the correct ROS `LD_LIBRARY_PATH`. Example snippet to add: @@ -89,6 +94,7 @@ This section shows the minimal, recommended steps an agent or maintainer should BASE_IMAGE: nvcr.io/nvidia/l4t-jetpack:r36.4.0 ROS_DISTRO: humble PYTHON_VERSION: "3.10" + TARGET_ARCH: aarch64 REAL_ROBOT: true SKIP_MACVO: true # for L4T builds only when necessary diff --git a/.agents/skills/optitrack-development/SKILL.md b/.agents/skills/optitrack-development/SKILL.md new file mode 100644 index 000000000..fcfe66e87 --- /dev/null +++ b/.agents/skills/optitrack-development/SKILL.md @@ -0,0 +1,220 @@ +--- +name: optitrack-development +description: Develop and integrate OptiTrack NatNet in AirStack — robot client (natnet_ros2), Isaac Sim Motive emulator, wire-protocol handshake, and libNatNet 4.4 unicast behavior. Use when working on natnet_ros2, optitrack.natnet.emulator, LAUNCH_NATNET, or NatNet UDP protocol compatibility. +license: Apache-2.0 +metadata: + author: AirLab CMU + repository: AirStack +--- + +# Skill: OptiTrack / NatNet Development + +## When to Use + +- Implementing or debugging the **Motive emulator** in Isaac Sim + (`simulation/isaac-sim/extensions/optitrack.natnet.emulator/`) +- Integrating or testing **`natnet_ros2`** on the robot stack +- Understanding **NatNet wire protocol** (connect, model def, frame streaming) +- Capturing what **`libNatNet.so`** actually sends on the network +- Enabling OptiTrack in sim: `LAUNCH_NATNET=true`, `natnet_config.yaml`, Docker IPs + +## Architecture in AirStack + +```mermaid +flowchart LR + subgraph sim ["Isaac Sim (172.31.0.200)"] + Emulator["optitrack.natnet.emulator\n(NatNet UDP server)"] + end + subgraph robot ["Robot container"] + Node["natnet_ros2_node"] + SDK["libNatNet.so client"] + Node --> SDK + end + SDK -->|"UDP 1510 (unicast: cmd + frames)"| Emulator + Node --> Topics["/{ROBOT_NAME}/perception/optitrack/..."] +``` + +| Component | Path | Role | +|-----------|------|------| +| Robot client | [`robot/ros_ws/src/perception/natnet_ros2/`](../../../robot/ros_ws/src/perception/natnet_ros2/) | ROS 2 node; uses **official NatNet SDK** (`NatNetClient::Connect`) | +| SDK install | `natnet_ros2/lib/libNatNet.so`, `include/natnet/` | Download via `airstack setup --natnet` (proprietary, not in git) | +| Emulator (WIP) | [`simulation/isaac-sim/extensions/optitrack.natnet.emulator/`](../../../simulation/isaac-sim/extensions/optitrack.natnet.emulator/) | Python NatNet **server** for sim / integration tests | +| Integration tests | [`tests/integration/natnet/README.md`](../../../tests/integration/natnet/README.md) | End-to-end UDP tests against real SDK parser (mark: `integration`) | + +**Enable on robot:** `LAUNCH_NATNET=true` in `.env` → [`perception.launch.xml`](../../../robot/ros_ws/src/perception/perception_bringup/launch/perception.launch.xml) includes `natnet_ros2.launch.py`. + +**Enable in sim:** set ``ISAAC_SIM_SCRIPT_NAME`` to a NatNet Pegasus launch script (no env gate in the script — NatNet always starts): + +| Script | Use | +|--------|-----| +| [`example_one_px4_pegasus_natnet_launch_script.py`](../../../simulation/isaac-sim/launch_scripts/example_one_px4_pegasus_natnet_launch_script.py) | Single drone + static ``Target`` | +| [`example_multi_px4_pegasus_natnet_launch_script.py`](../../../simulation/isaac-sim/launch_scripts/example_multi_px4_pegasus_natnet_launch_script.py) | ``NUM_ROBOTS`` drones + shared ``Target`` (pair with 3-profile ``natnet_config.yaml``) | + +Helpers: [`isaac/scene_setup.py`](../../../simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/scene_setup.py) (`start_drone_natnet_server`, `author_static_target`). Drone body: single = ``Drone``; multi = ``Drone`` (id ``i``); target = ``Target`` (id 100). Override names with ``NATNET_BODY_NAME`` / ``NATNET_TARGET_NAME``. Baseline Pegasus scripts (no NatNet) remain ``example_one_px4_pegasus_launch_script.py`` / ``example_multi_px4_pegasus_launch_script.py``. + +**Default client config:** unicast, `server_ip` → Motive/emulator (use `172.31.0.200` for Isaac container), ports 1510/1511. The config is per-robot: each `robots[$ROBOT_NAME]` profile lists the bodies it tracks (each a `rigid_body_name` + `id` mapped to a relative `topic`, with `pose`/`pose_cov` toggles and per-body covariance) and an optional `vision_pose` block that drives the MAVROS bridge. See [`natnet_config.yaml`](../../../robot/ros_ws/src/perception/natnet_ros2/config/natnet_config.yaml). + +## NatNet: Two UDP Channels + +| Port (server default) | Channel | Direction | +|----------------------|---------|-----------| +| **1510** | Command | Client → server: `NAT_CONNECT`, `NAT_REQUEST_MODELDEF`, keepalives. Server → client: `NAT_SERVERINFO`, `NAT_MODELDEF`, `NAT_RESPONSE` | +| **1511** | Data | Server → client: `NAT_FRAMEOFDATA` (mocap frames). Multicast group `239.255.42.99` when using multicast. **The server must send frames from a socket bound to the data port** (source port == `data_port`); see below. | + +**Critical rules (verified against the real `libNatNet.so` 4.4 unicast + `NatNet_SetLogCallback`):** + +- Command **responses** go to the client's endpoint from `recvfrom` on the server command listener (`1510`), sent via the **command** socket. +- **Frames must be sent from the server's DATA socket** (bound to `data_port`, e.g. `1511`) so the datagram **source port == `data_port`**. libNatNet routes inbound unicast datagrams by source port: frames from the **command** port are treated as command traffic and **silently dropped** (no error, no callback). This was the single biggest gotcha. +- **libNatNet 4.4 unicast uses one client UDP socket** (one ephemeral local port for command send/recv and frame recv). The client receives frames there regardless of the server's source port — but libNatNet only **dispatches** them to the frame callback when they came from the server's data port. Do **not** assume `data_port = cmd_port + 1`. +- **Every `NAT_FRAMEOFDATA` must end with a 4-byte end-of-data tag** (after the frame `params`). libNatNet's unpacker reads it; without it the unpacked length mismatches `nDataBytes` and the SDK drops the whole frame. (The lenient Python `NatNetClient` does not require it — always validate against the C SDK.) +- The **269-byte `NAT_CONNECT` payload does not include** the client port; the port is learned from the datagram **source address** on `NAT_CONNECT`. +- Do **not** trust `/proc`/`ss` alone for the client port — extra bound sockets may appear that do not match wire traffic. **`NAT_CONNECT` source `(ip, port)` is ground truth.** +- Do **not** parse connect payloads with in-memory `sNatNetClientConnectParams` (contains pointers). Use on-wire layouts below. + +## libNatNet 4.4 `NAT_CONNECT` (verified 2025-06) + +Observed against `127.0.0.1:1510` with the same unicast params as [`natnet_client_adapter.cpp`](../../../robot/ros_ws/src/perception/natnet_ros2/src/natnet_client_adapter.cpp). + +### What the client sends + +| Field | Observed value | +|-------|----------------| +| Message | `NAT_CONNECT` (0), `nDataBytes = 269`, total datagram 273 bytes | +| Payload layout | `sSender` (264 B) + `sConnectionOptions` (5 B) | +| `sSender.szName` | `"NatNetLib"` | +| `sSender.Version` | `[4, 4, 0, 0]` | +| `sSender.NatNetVersion` | `[4, 4, 0, 0]` | +| `subscribedDataOnly` | `0` | +| `BitstreamVersion` | `[0, 0, 0, 0]` → client defers to server version | +| Trailing port bytes | **None** (exactly 269 bytes; not PacketClient's optional +4) | +| UDP source port | Ephemeral (e.g. `41449`) — **client command + data port (same socket)** | + +Example hex (payload only, after 4-byte header): + +``` +NatNetLib\0 ... (256-byte name field) +04 04 00 00 (Version) +04 04 00 00 (NatNetVersion) +00 (subscribedDataOnly) +00 00 00 00 (BitstreamVersion) +``` + +## libNatNet 4.4 unicast: single client socket (verified 2025-06) + +Confirmed with wire capture on server `:1510`/`:1511`, `strace` on a minimal `NatNetClient::Connect()` binary, and `/proc//net/udp` cross-checks against the same `libNatNet.so` used by `natnet_ros2`. + +### What we observed + +| Signal | Result | +|--------|--------| +| Wire capture on server `:1510` | All client packets (`NAT_CONNECT`, `NAT_KEEPALIVE`, `NAT_REQUEST_MODELDEF`) from **one** source port | +| Wire capture on server `:1511` | **No** inbound packets from the client | +| strace on minimal client | **One** `bind()`, **one** fd for all `sendto` → server `:1510` and `recvfrom` ← server `:1510` | +| `NAT_CONNECT` payload | **No** trailing client port bytes (269 B total) | + +### Emulator rule (unicast + `natnet_ros2`) + +For libNatNet 4.4 unicast, treat the client as **single-endpoint**: + +```text +On NAT_CONNECT → store client_endpoint = (ip, port) from recvfrom +NAT_SERVERINFO → sendto(command_socket, client_endpoint) # source port = command_port +NAT_MODELDEF → sendto(command_socket, client_endpoint) # source port = command_port +NAT_FRAMEOFDATA → sendto(data_socket, client_endpoint) # source port = data_port (REQUIRED) +NAT_KEEPALIVE → no reply (client -> server only) +``` + +The client always learns its endpoint from the **`NAT_CONNECT` source address** (the +client uses a single socket), so the **destination** of frames is that endpoint. The +**source** of frames, however, must be the server's data port — bind a dedicated +`data_socket` to `('', data_port)` and `sendto` frames from it. + +`ConnectionDataPort = 1511` in `NAT_SERVERINFO` is required (the SDK uses it to +recognize the data channel — i.e. which source port valid frames arrive from). + +### When two client ports may still apply + +- **Multicast** clients (separate multicast data listener on `239.255.42.99:1511`) +- **PacketClient-style** samples that open explicit command + data sockets (optional +4 port bytes in connect) +- Other NatNet client implementations — always verify with protocol capture before assuming a two-socket model + +Do **not** assume `data_port = cmd_port + 1` for any client without capture. + +### What the server must reply (for `Connect()` + `GetServerDescription()`) + +1. **`NAT_SERVERINFO` (1)** on the **command port** to the connect datagram source. +2. Payload: packed **`sSender_Server`** (279 B), **not** `sServerDescription`. libNatNet + parses the `NAT_SERVERINFO` payload as `sSender_Server`; sending the larger + `sServerDescription` makes it misread the version/host. Fields: + - `Common.szName = "Motive"` (256-byte field) + - `Common.Version = {3, 1, 0, 0}` (Motive app), `Common.NatNetVersion = {4, 4, 0, 0}` + - `HighResClockFrequency`, `DataPort = 1511`, `IsMulticast = 0` (unicast) + +Pre-built in emulator: [`NatNetServer._build_connect_response_payload()`](../../../simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_server.py). + +### After connect (required for `natnet_ros2` topics) + +| SDK call | Server must handle | +|----------|-------------------| +| `GetDataDescriptionList()` | `NAT_REQUEST_MODELDEF` → `NAT_MODELDEF` with rigid body name/ID (e.g. `"Drone"`) | +| Frame callback | Stream `NAT_FRAMEOFDATA` to **`NAT_CONNECT` source `(ip, port)`** from the server **data socket** (source port = `data_port`); end each frame with the 4-byte EOD tag; set `rb.params & 0x01` (tracking valid) | +| Unicast keepalive | Accept `NAT_KEEPALIVE` on command port; **send no reply** | + +Verified end-to-end against the real `libNatNet.so` with a C probe that registers +`SetFrameReceivedCallback` + `NatNet_SetLogCallback`: with the data-port source, +EOD tag, `sSender_Server` reply, and no keepalive reply, the probe reports +`Server: Motive 3.1.0.0 NatNet 4.4.0.0`, `data descriptions: 1`, and ~74 Hz callbacks. + +## Wire format reference (do not confuse) + +| Client type | Connect payload | +|-------------|-----------------| +| **`libNatNet` / `natnet_ros2`** | `sSender` + `sConnectionOptions` (269 B observed) | +| **PacketClient sample** | Same + optional 4 trailing bytes (often zero in sample) | +| **Python NatNetClient sample** | Legacy 270-byte `"Ping"` blob — **not** used by `natnet_ros2` | + +API struct `sNatNetClientConnectParams` ([`NatNetTypes.h`](../../../simulation/isaac-sim/extensions/optitrack.natnet.emulator/NatNetClientSDK/NatNetSDK/include/NatNetTypes.h)) is for `Connect()` in process memory only — **not** the on-wire layout. + +## Protocol capture (optional, for debugging) + +Not part of the repo. If you need to re-verify wire behavior or debug a new client/server pairing, build a **minimal out-of-band harness**: + +1. **Minimal C++ client** — tiny binary linking `libNatNet.so` from `natnet_ros2`; call `NatNetClient::Connect()` with the same params as [`natnet_client_adapter.cpp`](../../../robot/ros_ws/src/perception/natnet_ros2/src/natnet_client_adapter.cpp). Optional: `GetDataDescriptionList()`, frame callback, `--hold-seconds` sleep. +2. **Python UDP stub server** — bind `:1510` (and optionally `:1511`); reply to `NAT_CONNECT` with canned `NAT_SERVERINFO`, to `NAT_REQUEST_MODELDEF` with `NAT_MODELDEF`, to `NAT_KEEPALIVE` with ack; log every `(ip, port)` and message id. +3. **Connect capture** — run the client against the stub; hex-dump the first datagram; confirm 269-byte `sSender` + `sConnectionOptions` payload and ephemeral source port. +4. **Endpoint discovery** — during a full connect + model-def fetch: + - `tcpdump -i any udp and host ` or the stub's packet log + - `strace -e trace=bind,sendto,recvfrom` on the client binary + - `/proc//net/udp` or `ss -uapn` (treat **`NAT_CONNECT` source port** as ground truth if they disagree) +5. **Frame delivery check** — confirm the client's frame callback fires. Register both `SetFrameReceivedCallback` **and** `NatNet_SetLogCallback` (the log callback surfaces silent drops). Frames must be sent from the server **data socket** (source port = `data_port`) and end with the 4-byte EOD tag, or the SDK drops them with no callback. + +Use the SDK's `NatNetTypes.h` and `PacketClient.cpp` for on-wire layouts — not in-memory `sNatNetClientConnectParams`. + +## Emulator implementation checklist + +1. **Command listener** on `0.0.0.0:1510` +2. **`NAT_CONNECT`** → register `client_endpoint` from `recvfrom`; reply `NAT_SERVERINFO` +3. **`NAT_REQUEST_MODELDEF`** → reply `NAT_MODELDEF` (match `body_name` in config) +4. **Frame loop** → `NAT_FRAMEOFDATA` to `client_endpoint` **from the data socket** (source port = `data_port`); end each frame with the 4-byte EOD tag +5. **Isaac integration** → sample drone pose → `sFrameOfMocapData` → `enqueue_mocap_data()` +6. **Docker** → emulator on `172.31.0.200`; robot `server_ip` points there + +## Testing levels + +| Level | Approach | Validates | +|-------|----------|-----------| +| Unit (no network) | `test_natnet_logic.cpp`, `FakeNatNetClient` | Negotiation logic, topic names | +| Protocol capture | Minimal client + UDP stub (see above) | Wire-format `NAT_CONNECT`, client endpoint model | +| Integration | `tests/integration/natnet/` | Full SDK parser + `natnet_ros2_node` (mark: `integration`) | +| System (future) | `airstack test -m sensors` | Topic Hz on `/perception/optitrack/...` | + +```bash +# Unit tests (robot container) +docker exec airstack-robot-desktop-1 bash -c "sws && colcon test --packages-select natnet_ros2 --event-handlers console_direct+" +``` + +## References + +- OptiTrack NatNet docs: https://docs.optitrack.com/developer-tools/natnet-sdk/natnet-4.0 +- SDK samples (wire format): `NatNet_SDK_*/Samples/PacketClient/`, `PythonClient/` (legacy connect in Python only) +- Integration test: [`tests/integration/natnet/README.md`](../../../tests/integration/natnet/README.md) diff --git a/.agents/skills/run-system-tests/SKILL.md b/.agents/skills/run-system-tests/SKILL.md index 453bbc953..6cf7fbaa7 100644 --- a/.agents/skills/run-system-tests/SKILL.md +++ b/.agents/skills/run-system-tests/SKILL.md @@ -25,7 +25,7 @@ This skill is about the **test harness itself** — pytest marks, fixtures, the The suite lives at `tests/` (repo root) and is fully pytest-based. Configuration is in `tests/pytest.ini` and shared infrastructure in `tests/conftest.py`. - **`tests/system/`** — Docker stack integration tests. Marks: `build_docker`, `build_packages`, `liveliness`, `sensors`, `takeoff_hover_land`. -- **`tests/robot/`** and **`tests/sim/`** — Hermetic **unit** tests (`@pytest.mark.unit`). These are **thin proxy files** that re-export tests from each ROS 2 package's own `test/` directory (co-located with the source, the ROS 2 / colcon convention). The proxy pattern keeps test source next to the code it tests while making tests discoverable by `pytest tests/`. +- **`tests/robot/`** and **`tests/sim/`** — Hermetic **unit** tests (`@pytest.mark.unit`). These are **thin proxy files** that call ``register_unit_tests()`` to register tests from each package's co-located `test/` directory so `pytest tests/` discovers them. ### Unit tests vs system tests diff --git a/.env b/.env index 82cc01ccb..0d994bc51 100644 --- a/.env +++ b/.env @@ -12,7 +12,7 @@ PROJECT_NAME="airstack" # If you've run ./airstack.sh setup, then this will auto-generate from the git commit hash every time a change is made # to a Dockerfile or docker-compose.yaml file. Otherwise this can also be set explicitly to make a release version. # auto-generated from git commit hash -VERSION="0.19.0-alpha.3" +VERSION="0.19.0-alpha.4" # Choose "dev" or "prebuilt". "dev" is for mounted code that must be built live. "prebuilt" is for built ros_ws baked into the image DOCKER_IMAGE_BUILD_MODE="dev" # Where to push and pull images from. Can replace with your docker hub username if using docker hub. diff --git a/AGENTS.md b/AGENTS.md index b53069e75..0a392667b 100644 --- a/AGENTS.md +++ b/AGENTS.md @@ -89,6 +89,7 @@ For detailed step-by-step instructions, refer to the **`.agents/skills/`** direc | [write-launch-file](.agents/skills/write-launch-file) | Authoring ROS 2 launch files with AirStack conventions (ROBOT_NAME namespacing, topic remapping, allow_substs) | | [write-isaac-sim-scene](.agents/skills/write-isaac-sim-scene) | Creating custom simulation scenes | | [visualize-in-foxglove](.agents/skills/visualize-in-foxglove) | Adding topic visualization to Foxglove/GCS | +| [optitrack-development](.agents/skills/optitrack-development) | OptiTrack NatNet emulator, natnet_ros2, wire-protocol handshake, connect-packet sniff | | [attach-gossip-payload](.agents/skills/attach-gossip-payload) | Broadcasting custom ROS messages to peers via PeerProfile gossip payloads | | [debug-module](.agents/skills/debug-module) | Autonomous debugging of ROS 2 modules | | [update-documentation](.agents/skills/update-documentation) | Documenting new modules and updating mkdocs | @@ -196,15 +197,17 @@ docker exec airstack-robot-desktop-1 bash -c "ros2 topic echo --onc - Verify module behavior in isolation - Test with synthetic data - Located in module's `test/` directory - - **Run in the robot container** with `colcon test` (after `bws`), not via `airstack test -m unit`. The root [`tests/`](tests/) suite does **not** register a `unit` pytest mark; `airstack test -m ` only selects marks declared in [`tests/pytest.ini`](tests/pytest.ini) (`build_docker`, `build_packages`, `liveliness`, `sensors`, `takeoff_hover_land`). + - **Run in the robot container** with `colcon test` (after `bws`), not via `airstack test -m unit`. The root [`tests/`](tests/) suite does **not** register a `unit` pytest mark; `airstack test -m ` only selects marks declared in [`tests/pytest.ini`](tests/pytest.ini) (`unit`, `build_docker`, `build_packages`, `integration`, `liveliness`, `sensors`, `takeoff_hover_land`). ```bash docker exec airstack-robot-desktop-1 bash -c "sws && colcon test --packages-select natnet_ros2 --event-handlers console_direct+" ``` -2. **Unit tests (`pytest`, `unit` mark):** Fast, hermetic checks. Test **source** lives co-located with each ROS 2 package in `/test/` (standard colcon convention). Thin **proxy** files in [`tests/robot/`](tests/robot/) and [`tests/sim/`](tests/sim/) re-export those tests so `pytest tests/` discovers them. Unit tests run as part of the `system-tests.yml` suite. Example: `airstack test -m unit -v`. See `add-unit-tests` skill. +2. **Unit tests (`pytest`, `unit` mark):** Fast, hermetic checks. Test **source** lives co-located with each ROS 2 package in `/test/` (standard colcon convention). Thin **proxy** files in [`tests/robot/`](tests/robot/) and [`tests/sim/`](tests/sim/) call `register_unit_tests()` so `pytest tests/` discovers them. Unit tests run as part of the `system-tests.yml` suite. Example: `airstack test -m unit -v`. See `add-unit-tests` skill. -3. **System Level (`tests/system/`):** Full simulation tests (Isaac Sim or Microsoft AirSim legacy) +3. **Integration (`pytest`, `integration` mark, [`tests/integration/`](tests/integration/)):** Wire a few real components together — the robot autonomy container plus a host-side component — **without** a sim or GPU. The shared `robot_autonomy_stack` fixture reuses a running `robot-desktop` container or brings one up automatically (like `build_packages`), and tears it down after. Collection order runs integration after `build_docker` and `build_packages`. First resident: `tests/integration/natnet/` (host NatNet emulator → `natnet_ros2` pose Hz). Example: `airstack test -m integration -v`. + +4. **System Level (`tests/system/`):** Full simulation tests (Isaac Sim or Microsoft AirSim legacy) - End-to-end autonomy stack testing - Real sensor simulation - Multi-robot scenarios diff --git a/docs/development/intermediate/docker-build-profiles.md b/docs/development/intermediate/docker-build-profiles.md index bbd3ebff1..3148c930e 100644 --- a/docs/development/intermediate/docker-build-profiles.md +++ b/docs/development/intermediate/docker-build-profiles.md @@ -15,6 +15,7 @@ Key inputs: | `BASE_IMAGE` | OS / vendor base image | | `ROS_DISTRO` | ROS 2 distro to install (e.g. `jazzy`) | | `PYTHON_VERSION` | Must match the ROS Python path; **quote in YAML** (e.g. `"3.12"`) | +| `TARGET_ARCH` | Architecture suffix used for ROS library paths in `LD_LIBRARY_PATH` (`x86_64` by default, `aarch64` for arm64 targets such as VOXL and L4T/Jetson) | | `REAL_ROBOT` | Platform-specific content toggles | | `SKIP_MACVO` | Skip MACVO in image when set | | `SKIP_TENSORRT` | Skip TensorRT in image when set | @@ -46,6 +47,7 @@ robot-l4t: REAL_ROBOT: true SKIP_MACVO: true SKIP_TENSORRT: true + TARGET_ARCH: aarch64 ROS_DISTRO: jazzy ``` @@ -60,6 +62,7 @@ robot-myboard: BASE_IMAGE: nvcr.io/nvidia/l4t-jetpack:r36.4.0 ROS_DISTRO: jazzy PYTHON_VERSION: "3.12" + TARGET_ARCH: aarch64 # omit for default x86_64 desktop builds REAL_ROBOT: true SKIP_MACVO: true # L4T only, when needed: diff --git a/docs/development/intermediate/testing/index.md b/docs/development/intermediate/testing/index.md index 0769e2b09..2a61b2e34 100644 --- a/docs/development/intermediate/testing/index.md +++ b/docs/development/intermediate/testing/index.md @@ -12,8 +12,8 @@ hardware requirement: ## Unit tests (`pytest -m unit`) Fast, hermetic Python tests that run in seconds with no Docker or GPU. Test source -lives **co-located with its ROS 2 package** (`/test/`) and is re-exported -through thin proxy files in `tests/robot/` for centralized discovery. +lives **co-located with its ROS 2 package** (`/test/`) and is registered +through thin proxy files in `tests/robot/` via `register_unit_tests()`. ```bash airstack test -m unit -v diff --git a/docs/development/intermediate/testing/unit_testing.md b/docs/development/intermediate/testing/unit_testing.md index f69056008..6d0a1b220 100644 --- a/docs/development/intermediate/testing/unit_testing.md +++ b/docs/development/intermediate/testing/unit_testing.md @@ -5,7 +5,7 @@ AirStack unit tests are **fast, hermetic, and purely Python** — no Docker stac ## Design principles - **Co-located with source.** Test files live in `/test/` alongside the code they test. This is the standard ROS 2 / colcon convention and ensures tests are discovered by both `colcon test` and `pytest`. -- **Proxy for centralized discovery.** A thin shim in `tests/robot///` re-exports the test functions so `pytest tests/` (the CI command) and `airstack test -m unit` discover them without any changes to the CI workflow. +- **Proxy for centralized discovery.** A thin shim in `tests/robot///` calls `register_unit_tests()` (in `tests/conftest.py`) to register co-located `test_*` functions so `pytest tests/` and `airstack test -m unit` discover them. See the `add-unit-tests` skill for the full contract. - **`@pytest.mark.unit` on every test.** The `unit` mark is the filter that keeps unit tests isolated from system tests that need Docker, GPUs, and sim licenses. ## Repository layout @@ -184,8 +184,8 @@ The `build_packages` CI job (`tests/system/test_build_packages.py`) also runs ## Extending to sim and GCS The proxy pattern extends to other components. As sim-side Python logic (e.g. the -[Motive emulator](../../../../tests/sim/motive_emulator/README.md)) and GCS modules -acquire unit-testable code, follow the same layout: +[NatNet emulator](../../../../simulation/isaac-sim/extensions/optitrack.natnet.emulator/README.md)) +and GCS modules acquire unit-testable code, follow the same layout: ``` simulation/...//test/test_.py ← source diff --git a/docs/robot/docker/index.md b/docs/robot/docker/index.md index 1502fdeb6..9dc409e92 100644 --- a/docs/robot/docker/index.md +++ b/docs/robot/docker/index.md @@ -43,7 +43,7 @@ robot_base (robot-base-docker-compose.yaml) ## Platform Profiles -Build-time args (`BASE_IMAGE`, `ROS_DISTRO`, `PYTHON_VERSION`, and platform toggles) are set per service in `docker-compose.yaml`. See [Docker Build Profiles](../../development/intermediate/docker-build-profiles.md) for how those args map to image variants. +Build-time args (`BASE_IMAGE`, `ROS_DISTRO`, `PYTHON_VERSION`, `TARGET_ARCH`, and platform toggles) are set per service in `docker-compose.yaml`. `TARGET_ARCH` controls the ROS library-path suffix used in `LD_LIBRARY_PATH` (`x86_64` by default, `aarch64` for arm64 targets such as VOXL and L4T/Jetson). See [Docker Build Profiles](../../development/intermediate/docker-build-profiles.md) for how those args map to image variants. Select a profile by passing `--profile ` to `docker compose` (or via the `airstack` CLI). diff --git a/docs/simulation/isaac_sim/docker.md b/docs/simulation/isaac_sim/docker.md index 74edf49f8..8956aab34 100644 --- a/docs/simulation/isaac_sim/docker.md +++ b/docs/simulation/isaac_sim/docker.md @@ -12,6 +12,9 @@ simulation/isaac-sim/docker/ ├── fastdds.xml # DDS configuration ├── omni_pass.env # Omniverse credentials (git-ignored) ├── omni_pass_TEMPLATE.env # Template for credentials +├── sitl-files/ # SITL env bundles (PX4_PARAM_* via compose) +│ ├── default.env # Plain Pegasus SITL (no param overrides) +│ └── px4-vision.env # External vision / NatNet → MAVROS → EKF2 ├── omniverse.toml # Omniverse settings ├── user.config.json # Isaac Sim configuration (enables extensions) └── user_TEMPLATE.config.json # Template configuration @@ -116,11 +119,14 @@ Key variables for Isaac Sim configuration: | `ISAAC_SIM_SCRIPT_NAME` | Standalone script filename | - | | `PX4_PHYSICS_HZ` | Physics step rate for PX4 SITL — also sets PX4 `IMU_INTEG_RATE` | `250` | | `PX4_RENDERING_HZ` | Rendering frame rate for PX4 profiles (independent of physics) | `60` | +| `SITL_PARAM_PROFILE` | SITL env bundle stem under `sitl-files/` (e.g. `default`, `px4-vision`) | `default` | | `ARDUPILOT_PHYSICS_HZ` | Physics step rate for ArduPilot SITL | `800` | | `ARDUPILOT_RENDERING_HZ` | Rendering frame rate for ArduPilot profiles | `120` | `PX4_PHYSICS_HZ` and `PX4_RENDERING_HZ` are set in the top-level `.env`. Pegasus defaults to 250 Hz but AirStack runs PX4 at **100 Hz** for near-real-time performance. See [Pegasus Scene Setup → Physics Rate](pegasus_scene_setup.md) for valid values and the full configuration flow. +SITL parameter bundles live in `sitl-files/` (`default.env`, `px4-vision.env`). Compose loads `sitl-files/${SITL_PARAM_PROFILE}.env` into the container; see [Pegasus Scene Setup → PX4 SITL env bundles](pegasus_scene_setup.md#px4-sitl-env-bundles-external-vision). + **Example overrides:** ```bash diff --git a/docs/simulation/isaac_sim/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/docs/simulation/isaac_sim/pegasus_scene_setup.md b/docs/simulation/isaac_sim/pegasus_scene_setup.md index da4293dc7..8fe10efb1 100644 --- a/docs/simulation/isaac_sim/pegasus_scene_setup.md +++ b/docs/simulation/isaac_sim/pegasus_scene_setup.md @@ -165,6 +165,22 @@ PX4_RENDERING_HZ="30" - **`PX4_PHYSICS_HZ`** — Sets `physics_dt = 1 / PX4_PHYSICS_HZ` in Isaac Sim's physics scene, and automatically syncs PX4's `IMU_INTEG_RATE` parameter to the same value via `PX4LaunchTool` → `px4-rc.simulator`. Patched within the Docker image to read the environment variable and set the IMU_INTEG_RATE parameter. - **`PX4_RENDERING_HZ`** — Sets the rendering frame rate independently of physics. 30 Hz rendering has no effect on physics accuracy or PX4 behavior, but does slightly affect performance due to resource usage. +### PX4 SITL env bundles (external vision) + +Isaac Sim loads optional PX4 SITL parameter bundles from `simulation/isaac-sim/docker/sitl-files/`. Select the bundle with `SITL_PARAM_PROFILE` in the top-level `.env` (compose maps this to `sitl-files/.env`): + +```bash +# Plain Pegasus SITL (default.env — comments only, no PX4_PARAM_* overrides) +SITL_PARAM_PROFILE="default" + +# NatNet / mocap → MAVROS vision_pose → EKF2 external vision (px4-vision.env) +SITL_PARAM_PROFILE="px4-vision" +``` + +Bundles inject `PX4_PARAM_*` variables into the isaac-sim container; PX4 applies them at boot via `init.d-posix/rcS`. Pair `px4-vision` with robot-side `LAUNCH_NATNET=true` and `vision_pose.enabled: true` in the robot's `natnet_config.yaml` profile. + +Convenience bundle: `airstack up --env-file overrides/isaac-natnet-vision.env`. + ### Valid values PX4's documented presets for `IMU_INTEG_RATE` are **100, 200, 250, 400 Hz**. The minimum is **100 Hz** — the EKF2 estimator runs at 100 Hz (10 ms period) and `IMU_INTEG_RATE` must be at least this fast. Values below 100 Hz are accepted by the firmware but cause attitude controller oscillation and are not recommended. diff --git a/docs/simulation/isaac_sim/spawning_drones.md b/docs/simulation/isaac_sim/spawning_drones.md index f5919ecec..e7baa6a17 100644 --- a/docs/simulation/isaac_sim/spawning_drones.md +++ b/docs/simulation/isaac_sim/spawning_drones.md @@ -5,8 +5,10 @@ The reference launch scripts under `simulation/isaac-sim/launch_scripts/` cover | Script | Purpose | |---|---| | `barebones_pegasus_launch.py` | Minimal Pegasus boilerplate. Single drone, default environment, no scene import. Use as a template for new launch scripts. | -| `example_one_px4_pegasus_launch_script.py` | One PX4 drone with the standard sensor stack (ZED stereo, optional Ouster lidar) in the default environment. | +| `example_one_px4_pegasus_launch_script.py` | One PX4 drone with the standard sensor stack (ZED stereo, RTX lidar) in the default environment. | | `example_multi_px4_pegasus_launch_script.py` | `NUM_ROBOTS` drones spawned in a row in the default environment. Each drone gets its own ROS domain id (`1..N`). | +| `example_one_px4_pegasus_natnet_launch_script.py` | Same as the one-drone script, plus OptiTrack NatNet streaming (drone + static target). Pair with `LAUNCH_NATNET=true` on the robot. | +| `example_multi_px4_pegasus_natnet_launch_script.py` | Same as the multi-drone script, plus NatNet streaming (one body per drone + shared `Target`). Use with the 3-profile `natnet_config.yaml` scaffolding for shared-target demos. | | `example_multi_drone_scene_import.py` | `NUM_ROBOTS` drones in an **imported scene** (USD from a Nucleus server) with per-drone GPS homes set via `gps_utils.set_gps_origins`. Use this as the starting point for any custom scene. | The first three are vanilla Pegasus patterns; this page focuses on the multi-drone + custom-scene case where you also need correct GPS homes. diff --git a/mkdocs.yml b/mkdocs.yml index 4dc57b961..830638ef1 100644 --- a/mkdocs.yml +++ b/mkdocs.yml @@ -87,6 +87,7 @@ nav: - Docker: docs/simulation/isaac_sim/docker.md - docs/simulation/isaac_sim/pegasus_scene_setup.md - Spawning Drones: docs/simulation/isaac_sim/spawning_drones.md + - NatNet Emulator (OptiTrack): docs/simulation/isaac_sim/natnet_emulator.md - Overhead Camera: docs/simulation/isaac_sim/overhead_camera.md - docs/simulation/isaac_sim/ascent_sitl_extension.md - docs/simulation/isaac_sim/export_stages_from_unreal.md diff --git a/overrides/isaac-natnet-vision.env b/overrides/isaac-natnet-vision.env new file mode 100644 index 000000000..3c7d3663b --- /dev/null +++ b/overrides/isaac-natnet-vision.env @@ -0,0 +1,10 @@ +# Isaac Sim + NatNet mocap with PX4 external-vision fusion. +# Usage: airstack up --env-file overrides/isaac-natnet-vision.env + +NUM_ROBOTS="1" # Increase to however many agents you want +LAUNCH_NATNET=true +SITL_PARAM_PROFILE=px4-vision +ISAAC_SIM_USE_STANDALONE=true +ISAAC_SIM_SCRIPT_NAME=example_one_px4_pegasus_natnet_launch_script.py +# Switch to your specified multi agent script as desired. +# ISAAC_SIM_SCRIPT_NAME=example_multi_px4_pegasus_natnet_launch_script.py diff --git a/robot/docker/Dockerfile.robot b/robot/docker/Dockerfile.robot index 894874961..92fd116fe 100644 --- a/robot/docker/Dockerfile.robot +++ b/robot/docker/Dockerfile.robot @@ -12,6 +12,7 @@ ARG UPDATE_FLAGS="-o Acquire::AllowInsecureRepositories=true -o Acquire::AllowDo ARG INSTALL_FLAGS="-o APT::Get::AllowUnauthenticated=true" ARG SKIP_MACVO=false ARG SKIP_TENSORRT=false +ARG TARGET_ARCH=x86_64 ARG PIP_VERSION=24.0 ARG PYTHON_VERSION=3.12 @@ -65,7 +66,7 @@ RUN sudo add-apt-repository universe \ ENV AMENT_PREFIX_PATH=/opt/ros/${ROS_DISTRO} ENV COLCON_PREFIX_PATH=/opt/ros/${ROS_DISTRO} -ENV LD_LIBRARY_PATH=/opt/ros/${ROS_DISTRO}/lib/x86_64-linux-gnu:/opt/ros/${ROS_DISTRO}/lib +ENV LD_LIBRARY_PATH=/opt/ros/${ROS_DISTRO}/lib/${TARGET_ARCH}-linux-gnu:/opt/ros/${ROS_DISTRO}/lib ENV PATH=/opt/ros/${ROS_DISTRO}/bin:$PATH ENV PYTHONPATH=/opt/ros/${ROS_DISTRO}/local/lib/python${PYTHON_VERSION}/dist-packages:/opt/ros/${ROS_DISTRO}/lib/python${PYTHON_VERSION}/site-packages ENV ROS_PYTHON_VERSION=3 @@ -96,6 +97,7 @@ RUN python3 -m pip install --no-cache-dir --break-system-packages --ignore-insta RUN apt update -y && apt install -y --no-install-recommends \ ros-dev-tools \ ros-${ROS_DISTRO}-mavros \ + ros-${ROS_DISTRO}-mavros-extras \ ros-${ROS_DISTRO}-tf2* \ ros-${ROS_DISTRO}-stereo-image-proc \ ros-${ROS_DISTRO}-image-view \ @@ -245,6 +247,7 @@ ARG UPDATE_FLAGS="-o Acquire::AllowInsecureRepositories=true -o Acquire::AllowDo ARG INSTALL_FLAGS="-o APT::Get::AllowUnauthenticated=true" ARG SKIP_MACVO=false ARG SKIP_TENSORRT=false +ARG TARGET_ARCH=x86_64 ARG PIP_VERSION=24.0 ARG PYTHON_VERSION=3.12 @@ -297,7 +300,7 @@ RUN sudo add-apt-repository universe \ ENV AMENT_PREFIX_PATH=/opt/ros/${ROS_DISTRO} ENV COLCON_PREFIX_PATH=/opt/ros/${ROS_DISTRO} -ENV LD_LIBRARY_PATH=/opt/ros/${ROS_DISTRO}/lib/x86_64-linux-gnu:/opt/ros/${ROS_DISTRO}/lib +ENV LD_LIBRARY_PATH=/opt/ros/${ROS_DISTRO}/lib/${TARGET_ARCH}-linux-gnu:/opt/ros/${ROS_DISTRO}/lib ENV PATH=/opt/ros/${ROS_DISTRO}/bin:$PATH ENV PYTHONPATH=/opt/ros/${ROS_DISTRO}/local/lib/python${PYTHON_VERSION}/dist-packages:/opt/ros/${ROS_DISTRO}/lib/python${PYTHON_VERSION}/site-packages ENV ROS_PYTHON_VERSION=3 @@ -328,6 +331,7 @@ RUN python3 -m pip install --no-cache-dir --break-system-packages --ignore-insta RUN apt update -y && apt install -y --no-install-recommends \ ros-dev-tools \ ros-${ROS_DISTRO}-mavros \ + ros-${ROS_DISTRO}-mavros-extras \ ros-${ROS_DISTRO}-tf2* \ ros-${ROS_DISTRO}-stereo-image-proc \ ros-${ROS_DISTRO}-image-view \ diff --git a/robot/docker/docker-compose.yaml b/robot/docker/docker-compose.yaml index f1799f48d..71b71e425 100644 --- a/robot/docker/docker-compose.yaml +++ b/robot/docker/docker-compose.yaml @@ -122,6 +122,7 @@ services: REAL_ROBOT: true SKIP_MACVO: true SKIP_TENSORRT: true + TARGET_ARCH: aarch64 ROS_DISTRO: jazzy tags: - *voxl_image @@ -179,6 +180,7 @@ services: REAL_ROBOT: true SKIP_MACVO: true SKIP_TENSORRT: true + TARGET_ARCH: aarch64 ROS_DISTRO: jazzy tags: - *l4t_image diff --git a/robot/ros_ws/src/perception/natnet_ros2/CMakeLists.txt b/robot/ros_ws/src/perception/natnet_ros2/CMakeLists.txt index ff47a00da..86342cb7c 100644 --- a/robot/ros_ws/src/perception/natnet_ros2/CMakeLists.txt +++ b/robot/ros_ws/src/perception/natnet_ros2/CMakeLists.txt @@ -48,8 +48,7 @@ if(EXISTS "${_NATNET_LIB}" AND EXISTS "${_NATNET_INC}") # Install libNatNet.so alongside the node and register an environment hook so # that sourcing the workspace adds lib/natnet_ros2/ to LD_LIBRARY_PATH. - # Use PROGRAMS (not FILES) to preserve the execute bit — shared libraries - # must be executable for the dynamic linker to map them. + # Use PROGRAMS (not FILES) to preserve the execute bit install(PROGRAMS "${_NATNET_LIB}" DESTINATION lib/${PROJECT_NAME}) @@ -65,10 +64,11 @@ else() endif() # --------------------------------------------------------------------------- -# Python nodes (vision_pose_converter remains Python) +# Python nodes # --------------------------------------------------------------------------- install(PROGRAMS src/vision_pose_converter_node.py + src/mavros_gp_origin_node.py DESTINATION lib/${PROJECT_NAME}) # --------------------------------------------------------------------------- diff --git a/robot/ros_ws/src/perception/natnet_ros2/README.md b/robot/ros_ws/src/perception/natnet_ros2/README.md index eb44763b4..773947d72 100644 --- a/robot/ros_ws/src/perception/natnet_ros2/README.md +++ b/robot/ros_ws/src/perception/natnet_ros2/README.md @@ -15,8 +15,9 @@ This module provides a bridge between OptiTrack Motive motion capture systems an - Receives **NatNet UDP packets** from an external Motive PC (configurable IP/port) - **Decodes motion capture frames** containing rigid body positions and orientations - **Publishes pose data** to the AirStack perception layer in standard ROS 2 formats -- **Supports multi-robot** via ROBOT_NAME namespacing -- **Optionally bridges** to MAVROS for PX4 external pose feedback +- **Tracks multiple rigid bodies per robot** (e.g. a drone for state estimation plus a separate target), each mapped to its own topic +- **Supports multi-robot** via per-robot profiles selected by `ROBOT_NAME` +- **Optionally bridges** to MAVROS for PX4 external pose feedback (per-robot) - **Respects OptiTrack licensing** by keeping the NatNet SDK external (host-side download with explicit consent) ## Architecture @@ -25,13 +26,16 @@ This module provides a bridge between OptiTrack Motive motion capture systems an Motive (External PC) ↓ NatNet UDP (port 1511) ↓ -NatNet ROS 2 Node - ├→ /robot_1/perception/optitrack/{body_name} (PoseStamped, optional) - ├→ /robot_1/perception/optitrack/{body_name}/pose_cov (PoseWithCovarianceStamped, always) - └→ (Optional, publish_to_mavros: true) - vision_pose_converter_node - ├→ /robot_1/mavros/vision_pose/pose - └→ /robot_1/mavros/vision_pose/pose_cov +NatNet ROS 2 Node (loads the ROBOT_NAME profile from natnet_config.yaml) + │ per configured body (one or more): + ├→ /{ROBOT_NAME}/{topic} (PoseStamped, when pose: true) + ├→ /{ROBOT_NAME}/{topic}/pose_cov (PoseWithCovarianceStamped, when pose_cov: true) + └→ (Optional, vision_pose.enabled: true) + mavros_gp_origin_node + └→ /{ROBOT_NAME}/interface/mavros/global_position/set_gp_origin + vision_pose_converter_node (reads input/output topics from the profile) + ├→ /{ROBOT_NAME}/interface/mavros/vision_pose/pose + └→ /{ROBOT_NAME}/interface/mavros/vision_pose/pose_cov ``` ## Interfaces @@ -39,63 +43,99 @@ NatNet ROS 2 Node ### Inputs - **Network**: NatNet UDP stream from Motive PC (external network) -- **Configuration**: `natnet_config.yaml` with server IP, ports, `body_name`, and covariance +- **Configuration**: `natnet_config.yaml` — generic `server` settings plus a `robots` map of per-robot profiles (body list + optional MAVROS `vision_pose` block). The launch file selects the profile matching `ROBOT_NAME`. ### Outputs -For each tracked rigid body `{body_name}` from Motive: +For each rigid body in the robot's profile, `topic` is a **relative** leaf namespaced +under `/{ROBOT_NAME}/` (it defaults to `perception/optitrack/{rigid_body_name}` when +omitted): -#### Direct OptiTrack pose (optional) +#### Direct OptiTrack pose -- **Topic**: `/{ROBOT_NAME}/perception/optitrack/{body_name}` +- **Topic**: `/{ROBOT_NAME}/{topic}` - **Type**: `geometry_msgs/PoseStamped` - **Description**: Position and orientation only (no covariance) -- **Enabled by**: `publish_direct_optitrack: true` in config (default: `true`) +- **Enabled by**: `pose: true` on that body (per body) -#### Pose with covariance (always) +#### Pose with covariance -- **Topic**: `/{ROBOT_NAME}/perception/optitrack/{body_name}/pose_cov` +- **Topic**: `/{ROBOT_NAME}/{topic}/pose_cov` - **Type**: `geometry_msgs/PoseWithCovarianceStamped` -- **Description**: Same pose as above plus a 6×6 covariance matrix (`position_covariance` and `orientation_covariance` from config). Published whenever the rigid body is tracked — independent of `publish_direct_optitrack` and `publish_to_mavros`. +- **Description**: Same pose plus a 6×6 covariance matrix from that body's `position_covariance` / `orientation_covariance`. +- **Enabled by**: `pose_cov: true` on that body (per body) -#### MAVROS vision pose bridge (optional) +#### MAVROS vision pose bridge (optional, per robot) -When `publish_to_mavros: true`, `vision_pose_converter_node` subscribes to `pose_cov` and republishes for PX4: +When the robot's `vision_pose.enabled: true`, `vision_pose_converter_node` subscribes to the configured `input_topic` (a body's `pose_cov`) and republishes for PX4 on the configured outputs: -- **Topic**: `/{ROBOT_NAME}/mavros/vision_pose/pose` — `geometry_msgs/PoseStamped` (pose extracted from the covariance message) -- **Topic**: `/{ROBOT_NAME}/mavros/vision_pose/pose_cov` — `geometry_msgs/PoseWithCovarianceStamped` (full message, quaternion optionally canonicalized) -- **Enabled by**: `publish_to_mavros: true` in config +- **Topic** (`output_pose_topic`): `/{ROBOT_NAME}/interface/mavros/vision_pose/pose` — `geometry_msgs/PoseStamped` (pose extracted from the covariance message) +- **Topic** (`output_pose_cov_topic`): `/{ROBOT_NAME}/interface/mavros/vision_pose/pose_cov` — `geometry_msgs/PoseWithCovarianceStamped` (full message, quaternion optionally canonicalized) +- **Enabled by**: `vision_pose.enabled: true` in the robot's profile +- **Retargetable**: change `input_topic` / `output_pose_topic` / `output_pose_cov_topic` (relative, namespaced) to bridge to other middleware +- **PX4 side**: set `SITL_PARAM_PROFILE=px4-vision` in `.env` so Isaac SITL loads EKF2 external-vision params from `simulation/isaac-sim/docker/sitl-files/px4-vision.env` + +##### Synthetic GPS origin (mocap / no-GNSS arming) + +With GNSS disabled (`EKF2_GPS_CTRL=0`), PX4 fused EKF has **no global position**. This fails preflight checks and refuse to arm. When `vision_pose.enabled: true`, +`mavros_gp_origin_node` publishes a synthetic origin once at startup: + +- **Topic**: `/{ROBOT_NAME}/interface/mavros/global_position/set_gp_origin` — `geographic_msgs/GeoPointStamped` +- **Guarded**: waits for `mavros/state.connected`, then publishes only if no + origin already exists (it watches `…/global_position/gp_origin`), so a + GNSS-equipped vehicle is left untouched. +- **Params** (`config/mavros_gp_origin.yaml`): `enabled` (default `true`), + `latitude/longitude/altitude` (default Lisbon — the AirStack shared world + datum; **must match** the GCS origin in `gcs_visualizer/gcs_utils.py` and the + sim's `gps_utils.py` so Foxglove waypoints transform 1:1), `settle_sec`. + Set `enabled: false` to rely on real GNSS. ## Configuration -Edit `config/natnet_config.yaml`: +`config/natnet_config.yaml` uses a custom `natnet:` schema (not a flat ROS 2 param +file): generic `server` settings shared by every agent, then a `robots` map of +per-robot profiles. The launch file parses it, selects the profile matching the +container's `ROBOT_NAME`, flattens the body list into node parameters, and brings up +the MAVROS bridge only when that robot's `vision_pose.enabled` is true. ```yaml -/**: - ros__parameters: - server_ip: "192.168.1.100" # IP of the Motive PC +natnet: + server: # generic across all agents + server_ip: "$(env NATNET_SERVER_IP 172.31.0.200)" client_ip: "0.0.0.0" command_port: 1510 data_port: 1511 - connection_type: "unicast" # or "multicast" - - body_name: "Drone" # rigid body name in Motive (case-sensitive) - body_id: -1 # -1 = publish all bodies in the frame - - publish_direct_optitrack: true # PoseStamped on …/optitrack/{body_name} - publish_to_mavros: false # include vision_pose_converter → MAVROS - + connection_type: "unicast" # or "multicast" + multicast_address: "239.255.42.99" frame_id: "world" - - position_covariance: [0.1, 0.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.1] - orientation_covariance: [0.01, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.01] + debug: false + robots: + robot_1: + vision_pose: # per-robot MAVROS bridge (omit/false to skip) + enabled: true + input_topic: "perception/optitrack/drone/pose_cov" + output_pose_topic: "interface/mavros/vision_pose/pose" + output_pose_cov_topic: "interface/mavros/vision_pose/pose_cov" + bodies: # one or more tracked rigid bodies + - rigid_body_name: "Drone" # Motive name (case-sensitive) + id: 1 # Motive streaming id + topic: "perception/optitrack/drone" # relative → /{ROBOT_NAME}/ + pose: true # publish PoseStamped + pose_cov: true # publish PoseWithCovarianceStamped + position_covariance: [1.0e-6, 0, 0, 0, 1.0e-6, 0, 0, 0, 1.0e-6] + orientation_covariance: [3.0e-6, 0, 0, 0, 3.0e-6, 0, 0, 0, 3.0e-6] ``` +To track an additional body (e.g. a target) for a robot, add another entry under that +robot's `bodies`. To add a robot, add a new key under `robots`. The shipped file +includes commented scaffolding for a 3-drone fleet where `robot_1` and `robot_2` also +track a shared `Target` body and `robot_3` tracks only its drone. + ## Launch ### Basic launch -Parameters come from `config/natnet_config.yaml` (network, body, covariance). Optional overrides: +Parameters come from `config/natnet_config.yaml` (server + the `ROBOT_NAME` profile). Optional overrides: ```bash ros2 launch natnet_ros2 natnet_ros2.launch.py \ @@ -106,7 +146,7 @@ ros2 launch natnet_ros2 natnet_ros2.launch.py \ ### MAVROS bridge -Set `publish_to_mavros: true` in `natnet_config.yaml`. The launch file reads `publish_to_mavros` and `body_name` from that YAML to decide whether to include `vision_pose_converter.launch.xml`. +Set `vision_pose.enabled: true` in the robot's profile. The launch file includes `vision_pose_converter.launch.xml` (and `mavros_gp_origin.launch.xml`) and forwards the profile's `input_topic` / `output_pose_topic` / `output_pose_cov_topic`. ### From perception bringup @@ -140,13 +180,18 @@ The SDK will be installed into `robot/ros_ws/src/perception/natnet_ros2/lib/` an ### Multi-Robot Support Each container instance gets its own `ROBOT_NAME` and `ROS_DOMAIN_ID`: -- Topics: `/{ROBOT_NAME}/perception/optitrack/{body_name}` and `/{ROBOT_NAME}/perception/optitrack/{body_name}/pose_cov` -- Supported via launch file argument forwarding +- The node loads the `robots[$ROBOT_NAME]` profile, so each robot tracks only the bodies (and runs the MAVROS bridge) configured for it. +- Topics are namespaced under `/{ROBOT_NAME}/` from each body's relative `topic`. +- Set `NUM_ROBOTS=N`; each replica resolves its own `ROBOT_NAME` (via `resolve_robot_name.py`) and auto-selects its profile — no per-robot env overrides. ### Error Handling - Invalid/malformed packets are skipped with debug logging - Lost connectivity logs warnings; gracefully recovers when stream resumes - Covariance in config allows tuning uncertainty per deployment +- **Connect retry:** the initial handshake is retried every 2 s until it + succeeds, so the node tolerates the NatNet server starting *after* the robot + (e.g. a Motive PC powered on later, or the Isaac Sim NatNet emulator which only + binds ~100 s into sim boot). The retry timer cancels itself on first success. ## Testing @@ -157,9 +202,9 @@ Each container instance gets its own `ROBOT_NAME` and `ROS_DOMAIN_ID`: ```bash ros2 launch natnet_ros2 natnet_ros2.launch.py ``` -4. Verify topics: +4. Verify topics (default profile maps the `Drone` body to `perception/optitrack/drone`): ```bash - ros2 topic echo /robot_1/perception/optitrack/Drone/pose_cov + ros2 topic echo /robot_1/perception/optitrack/drone/pose_cov ``` ### Without Real Hardware (Mock) @@ -167,7 +212,7 @@ TODO: Implement Motive simulator in Isaac Sim to generate fake NatNet packets ## Known Limitations -- When `body_id: -1`, all rigid bodies in the Motive frame get publishers; filter by subscribing to the `{body_name}` you care about +- The node publishes only bodies listed in the robot's profile (matched by `id`); bodies streamed by Motive but absent from the profile are ignored. - MAVROS bridge applies frame_id override and quaternion canonicalization; full PX4 frame alignment may still need tuning per airframe - No support for skeleton tracking or labeled markers yet (future enhancement) diff --git a/robot/ros_ws/src/perception/natnet_ros2/config/mavros_gp_origin.yaml b/robot/ros_ws/src/perception/natnet_ros2/config/mavros_gp_origin.yaml new file mode 100644 index 000000000..bf2abab68 --- /dev/null +++ b/robot/ros_ws/src/perception/natnet_ros2/config/mavros_gp_origin.yaml @@ -0,0 +1,22 @@ +# Synthetic GPS origin for mocap / no-GNSS flight via MAVROS. +# Loaded by mavros_gp_origin.launch.xml when publish_to_mavros is enabled. + +/**: + ros__parameters: + # With GNSS disabled, PX4 has no global position, so modes that require one + # (e.g. AUTO.LOITER) refuse to arm. Setting an origin lets PX4 derive a + # global position from the fused vision estimate. Guarded: skipped if an + # origin already exists (e.g. on a GNSS-equipped vehicle). + enabled: true + # MUST match the GCS world origin so Foxglove waypoints transform 1:1: + # - gcs_visualizer/gcs_utils.py ORIGIN_LAT / ORIGIN_LON + # - sim launch_scripts/gps_utils.py DEFAULT_WORLD_ORIGIN + # If this disagrees with the GCS (e.g. the old Zurich SITL default), the + # relay computes a boot-ENU offset of ~1.8e6 m and the drone flies the + # wrong way. Default is Lisbon (the AirStack shared world datum). + latitude: 38.736832 + longitude: -9.137977 + altitude: 90.0 + # Wait this long after MAVROS connects (listening for an existing origin) + # before publishing the synthetic one. + settle_sec: 5.0 diff --git a/robot/ros_ws/src/perception/natnet_ros2/config/natnet_config.yaml b/robot/ros_ws/src/perception/natnet_ros2/config/natnet_config.yaml index 69fc11d1c..6c5a03a47 100644 --- a/robot/ros_ws/src/perception/natnet_ros2/config/natnet_config.yaml +++ b/robot/ros_ws/src/perception/natnet_ros2/config/natnet_config.yaml @@ -1,51 +1,123 @@ -# NatNet ROS 2 parameters — loaded by natnet_ros2.launch.py (NatNet node + MAVROS gate). -# publish_to_mavros / body_name are read by the launch file to decide vision_pose_converter include. +# NatNet ROS 2 configuration — parsed by natnet_ros2.launch.py. # -# Use /** so parameters apply regardless of namespace (e.g. /robot_1/perception/natnet_ros2_node). -# See: https://docs.ros.org/en/humble/Tutorials/Beginner-CLI-Tools/Understanding-ROS2-Parameters.html - -/**: - ros__parameters: - # IP address of the PC running Motive (OptiTrack server). - # Change this to match your local network before launching NatNet. - server_ip: "192.168.1.100" - # Motive learns unicast destination from outbound UDP source IP — bind explicitly when you have - # multiple NICs (e.g. Docker 172.17.* vs LAN). +# Unlike a plain ROS 2 parameter file, this uses a custom `natnet:` schema so a +# single file can describe multiple robots, each tracking multiple rigid bodies. +# The launch file selects the profile matching the container's ROBOT_NAME, flattens +# it into node parameters, and (optionally) brings up the MAVROS vision_pose bridge. +# +# Schema: +# natnet: +# server: generic connection settings shared by every agent +# robots: one profile per ROBOT_NAME +# : +# vision_pose: whether/how to forward a body to MAVROS (optional) +# bodies: rigid bodies this robot subscribes to (one or more) + +natnet: + + # --- Connection settings (generic across all agents) ----------------------- + server: + # IP of the PC running Motive (OptiTrack server). Defaults to the Isaac Sim + # container on the airstack_network (172.31.0.200). + server_ip: "$(env NATNET_SERVER_IP 172.31.0.200)" + # Motive learns the unicast destination from the outbound UDP source IP — bind + # explicitly when the client has multiple NICs (e.g. Docker 172.17.* vs LAN). client_ip: "0.0.0.0" command_port: 1510 data_port: 1511 - # "unicast" — point-to-point; Motive streams directly to this machine's IP. - # Requires Motive unicast streaming enabled and client_ip set - # to the correct NIC when multiple interfaces are present. - # "multicast" — Motive broadcasts to a multicast group; any machine on the - # subnet that joins the group receives all body data. - # Use for multi-robot setups where every robot receives the - # full frame and filters by body_id. + # "unicast" — point-to-point; Motive streams directly to this machine (default). + # "multicast" — Motive broadcasts to a group; every robot receives the full frame + # and filters by the body ids in its profile. Use for multi-robot. connection_type: "unicast" - - # Only used when connection_type = "multicast". - # Must match Motive > Edit > Preferences > Data Streaming > Multicast Interface. - # OptiTrack default is 239.255.42.99. + # Only used when connection_type = "multicast" (OptiTrack default 239.255.42.99). multicast_address: "239.255.42.99" - # Name of the rigid body as defined in Motive. Must match exactly (case-sensitive). - body_name: "Drone" - body_id: -1 - - publish_direct_optitrack: true - publish_to_mavros: true - + # Frame applied to every published pose. debug enables per-frame logging. frame_id: "world" debug: false - position_covariance: - [0.1, 0.0, 0.0, - 0.0, 0.1, 0.0, - 0.0, 0.0, 0.1] + # --- Per-robot profiles (selected by ROBOT_NAME) --------------------------- + robots: + + robot_1: + # MAVROS vision_pose bridge. enabled=false skips the converter entirely. + # input_topic is one of this robot's body pose_cov topics; the two outputs map + # to MAVROS's vision_pose/pose (PoseStamped) and vision_pose/pose_cov + # (PoseWithCovarianceStamped) subscribers. Topics are relative and namespaced + # under /{ROBOT_NAME}/ — redirect them to interface with other middleware. + vision_pose: + enabled: true + input_topic: "perception/optitrack/drone/pose_cov" + output_pose_topic: "interface/mavros/vision_pose/pose" + output_pose_cov_topic: "interface/mavros/vision_pose/pose_cov" - orientation_covariance: - [0.01, 0.0, 0.0, - 0.0, 0.01, 0.0, - 0.0, 0.0, 0.01] + # Rigid bodies this robot tracks. rigid_body_name must match Motive exactly + # (case-sensitive). topic is a relative leaf namespaced under /{ROBOT_NAME}/; + # pose / pose_cov toggle the PoseStamped and PoseWithCovarianceStamped variants. + bodies: + - rigid_body_name: "Drone1" + id: 1 + topic: "perception/optitrack/drone" + pose: true + pose_cov: true + # Covariances are per-body. Sub-0.1 mm / sub-0.1 deg for OptiTrack precision. + position_covariance: + [1.0e-6, 0.0, 0.0, + 0.0, 1.0e-6, 0.0, + 0.0, 0.0, 1.0e-6] + orientation_covariance: + [3.0e-6, 0.0, 0.0, + 0.0, 3.0e-6, 0.0, + 0.0, 0.0, 3.0e-6] + # --- Scaffolding: a 3-drone fleet where robot_1 & robot_2 also track a shared + # "Target" body and robot_3 tracks only its drone. Uncomment + set NUM_ROBOTS=3 + # (and stream the matching bodies from example_multi_px4_pegasus_natnet_launch_script.py). + # + # robot_2: + # vision_pose: + # enabled: true + # input_topic: "perception/optitrack/drone/pose_cov" + # output_pose_topic: "interface/mavros/vision_pose/pose" + # output_pose_cov_topic: "interface/mavros/vision_pose/pose_cov" + # bodies: + # - rigid_body_name: "Drone2" + # id: 2 + # topic: "perception/optitrack/drone" + # pose: true + # pose_cov: true + # - rigid_body_name: "Target" # shared target — also tracked by robot_1 + # id: 100 + # topic: "perception/optitrack/target" + # pose: true + # pose_cov: false + # position_covariance: + # [1.0e-6, 0.0, 0.0, + # 0.0, 1.0e-6, 0.0, + # 0.0, 0.0, 1.0e-6] + # orientation_covariance: + # [3.0e-6, 0.0, 0.0, + # 0.0, 3.0e-6, 0.0, + # 0.0, 0.0, 3.0e-6] + # + # robot_3: + # vision_pose: + # enabled: true + # input_topic: "perception/optitrack/drone/pose_cov" + # output_pose_topic: "interface/mavros/vision_pose/pose" + # output_pose_cov_topic: "interface/mavros/vision_pose/pose_cov" + # bodies: + # - rigid_body_name: "Drone3" + # id: 3 + # topic: "perception/optitrack/drone" + # pose: true + # pose_cov: true + # position_covariance: + # [1.0e-6, 0.0, 0.0, + # 0.0, 1.0e-6, 0.0, + # 0.0, 0.0, 1.0e-6] + # orientation_covariance: + # [3.0e-6, 0.0, 0.0, + # 0.0, 3.0e-6, 0.0, + # 0.0, 0.0, 3.0e-6] diff --git a/robot/ros_ws/src/perception/natnet_ros2/include/natnet_ros2/natnet_logic.hpp b/robot/ros_ws/src/perception/natnet_ros2/include/natnet_ros2/natnet_logic.hpp index f23216565..a0cf8927f 100644 --- a/robot/ros_ws/src/perception/natnet_ros2/include/natnet_ros2/natnet_logic.hpp +++ b/robot/ros_ws/src/perception/natnet_ros2/include/natnet_ros2/natnet_logic.hpp @@ -34,6 +34,7 @@ #pragma once +#include #include #include #include @@ -91,6 +92,34 @@ inline std::string optitrack_pose_cov_topic( return optitrack_topic_base(robot_name, body_name) + "/pose_cov"; } +/// Namespace a relative topic leaf under /{robot_name}/. +/// +/// Leading slashes in \p relative are stripped so the result always has exactly +/// one. Used for the per-body ``topic`` overrides in natnet_config.yaml, which are +/// relative and namespaced by the node at runtime. +inline std::string namespaced_topic( + const std::string & robot_name, + const std::string & relative) +{ + const std::size_t start = relative.find_first_not_of('/'); + const std::string leaf = + (start == std::string::npos) ? std::string{} : relative.substr(start); + return "/" + robot_name + "/" + leaf; +} + +/// Topic base for one configured body: the per-body relative override when set, +/// otherwise the default /{robot_name}/perception/optitrack/{body_name}. +inline std::string body_topic_base( + const std::string & robot_name, + const std::string & body_name, + const std::string & relative_override) +{ + if (relative_override.empty()) { + return optitrack_topic_base(robot_name, body_name); + } + return namespaced_topic(robot_name, relative_override); +} + // =========================================================================== // 3. Connection-configuration helpers @@ -199,6 +228,16 @@ inline bool should_publish_body(int32_t filter_id, int32_t rb_id) return filter_id < 0 || rb_id == filter_id; } +/// Returns true when rb_id is one of the configured body ids. +/// +/// Multi-body variant of should_publish_body(): the node tracks a fixed set of +/// ids from natnet_config.yaml and publishes only those (empty set → nothing). +inline bool body_is_configured(const std::vector & configured_ids, int32_t rb_id) +{ + return std::find(configured_ids.begin(), configured_ids.end(), rb_id) + != configured_ids.end(); +} + /// Double-precision pose extracted from a RigidBodySample. struct PoseData { diff --git a/robot/ros_ws/src/perception/natnet_ros2/launch/mavros_gp_origin.launch.xml b/robot/ros_ws/src/perception/natnet_ros2/launch/mavros_gp_origin.launch.xml new file mode 100644 index 000000000..4cb1f785b --- /dev/null +++ b/robot/ros_ws/src/perception/natnet_ros2/launch/mavros_gp_origin.launch.xml @@ -0,0 +1,34 @@ + + + + + + + + + + + + + + + + + + + diff --git a/robot/ros_ws/src/perception/natnet_ros2/launch/natnet_ros2.launch.py b/robot/ros_ws/src/perception/natnet_ros2/launch/natnet_ros2.launch.py index cb7c178d8..6e49fcf88 100644 --- a/robot/ros_ws/src/perception/natnet_ros2/launch/natnet_ros2.launch.py +++ b/robot/ros_ws/src/perception/natnet_ros2/launch/natnet_ros2.launch.py @@ -1,5 +1,10 @@ #!/usr/bin/env python3 -"""Bring up NatNet node; optionally MAVROS bridge per natnet_config.yaml. +"""Bring up the NatNet node from natnet_config.yaml; optionally the MAVROS bridge. + +The config uses a custom ``natnet:`` schema (server settings + per-robot profiles), +so this launch file parses it, selects the profile matching ``ROBOT_NAME``, flattens +the body list into node parameters, and — when the robot's ``vision_pose`` block is +enabled — includes the MAVROS GP-origin + vision_pose_converter bridges. natnet_ros2_node is a C++ executable that requires the OptiTrack NatNet SDK. If the SDK was not installed (``airstack setup`` not run) and the workspace @@ -10,8 +15,9 @@ from __future__ import annotations import os +import re from pathlib import Path -from typing import cast +from typing import Any, cast import yaml from ament_index_python.packages import get_package_share_directory @@ -20,11 +26,28 @@ from launch.launch_description_sources import FrontendLaunchDescriptionSource from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node -from launch_ros.parameter_descriptions import ParameterFile +# Per-body covariance fallback when a body omits its own (sub-0.1 mm / sub-0.1 deg). +_DEFAULT_POSITION_COVARIANCE = [1.0e-6, 0.0, 0.0, 0.0, 1.0e-6, 0.0, 0.0, 0.0, 1.0e-6] +_DEFAULT_ORIENTATION_COVARIANCE = [3.0e-6, 0.0, 0.0, 0.0, 3.0e-6, 0.0, 0.0, 0.0, 3.0e-6] + +_ENV_SUBST = re.compile(r"\$\(env\s+(\w+)(?:\s+([^)]*))?\)") + + +def _expand_env(value: Any) -> Any: + """Expand ``$(env VAR default)`` tokens in a string using os.environ.""" + if not isinstance(value, str): + return value + + def _replace(match: re.Match) -> str: + var, default = match.group(1), match.group(2) + return os.environ.get(var, default if default is not None else "") -def _ros_params_from_file(config_path: str) -> dict: - """Parse /** / ros__parameters block from a ROS 2 parameter YAML.""" + return _ENV_SUBST.sub(_replace, value) + + +def _load_natnet_config(config_path: str) -> dict: + """Parse the ``natnet:`` block from the config YAML.""" path = Path(config_path) if not path.is_file(): return {} @@ -32,34 +55,110 @@ def _ros_params_from_file(config_path: str) -> dict: data = yaml.safe_load(f) if not isinstance(data, dict): return {} - block = data.get('/**') - if not isinstance(block, dict): - return {} - params = block.get('ros__parameters', {}) - return cast(dict, params) if isinstance(params, dict) else {} + natnet = data.get('natnet', {}) + return cast(dict, natnet) if isinstance(natnet, dict) else {} + + +def _flatten_covariance(values: Any, fallback: list[float]) -> list[float]: + """Coerce a 9-element covariance block to floats, falling back when absent.""" + if not isinstance(values, (list, tuple)) or len(values) == 0: + return list(fallback) + return [float(v) for v in values] + + +def _build_node_params(server: dict, profile: dict) -> dict: + """Flatten the server block + a robot's body list into node parameters.""" + bodies = profile.get('bodies', []) or [] + + params: dict[str, Any] = { + 'server_ip': str(_expand_env(server.get('server_ip', '172.31.0.200'))), + 'client_ip': str(_expand_env(server.get('client_ip', '0.0.0.0'))), + 'command_port': int(server.get('command_port', 1510)), + 'data_port': int(server.get('data_port', 1511)), + 'connection_type': str(server.get('connection_type', 'unicast')), + 'multicast_address': str(server.get('multicast_address', '239.255.42.99')), + 'frame_id': str(server.get('frame_id', 'world')), + 'debug': bool(server.get('debug', False)), + } + + body_names: list[str] = [] + body_ids: list[int] = [] + body_topics: list[str] = [] + body_pose: list[bool] = [] + body_pose_cov: list[bool] = [] + body_position_covariance: list[float] = [] + body_orientation_covariance: list[float] = [] + + for body in bodies: + body_names.append(str(body.get('rigid_body_name', ''))) + body_ids.append(int(body.get('id', -1))) + body_topics.append(str(body.get('topic', ''))) + body_pose.append(bool(body.get('pose', True))) + body_pose_cov.append(bool(body.get('pose_cov', True))) + body_position_covariance.extend( + _flatten_covariance(body.get('position_covariance'), _DEFAULT_POSITION_COVARIANCE) + ) + body_orientation_covariance.extend( + _flatten_covariance(body.get('orientation_covariance'), _DEFAULT_ORIENTATION_COVARIANCE) + ) + + params.update( + { + 'body_names': body_names, + 'body_ids': body_ids, + 'body_topics': body_topics, + 'body_pose': body_pose, + 'body_pose_cov': body_pose_cov, + 'body_position_covariance': body_position_covariance, + 'body_orientation_covariance': body_orientation_covariance, + } + ) + return params + + +def _namespaced(robot_name: str, relative: str) -> str: + """Namespace a relative topic under /{robot_name}/.""" + return '/' + robot_name + '/' + relative.lstrip('/') def generate_launch_description() -> LaunchDescription: pkg_share = get_package_share_directory('natnet_ros2') default_natnet_yaml = os.path.join(pkg_share, 'config', 'natnet_config.yaml') default_vp_yaml = os.path.join(pkg_share, 'config', 'vision_pose_converter.yaml') + default_gp_origin_yaml = os.path.join(pkg_share, 'config', 'mavros_gp_origin.yaml') config_file = LaunchConfiguration('config_file') vision_pose_config_file = LaunchConfiguration('vision_pose_config_file') + gp_origin_config_file = LaunchConfiguration('gp_origin_config_file') use_sim_time = LaunchConfiguration('use_sim_time') def launch_setup(context, *_args, **_kwargs): cfg_path = config_file.perform(context) vp_path = vision_pose_config_file.perform(context) + gp_path = gp_origin_config_file.perform(context) ust = use_sim_time.perform(context) - ros_params = _ros_params_from_file(cfg_path) - publish_mavros = bool(ros_params.get('publish_to_mavros', False)) - body_name = str(ros_params.get('body_name', 'robot_1')) + robot_name = os.environ.get('ROBOT_NAME', 'robot_1') + natnet = _load_natnet_config(cfg_path) + server = natnet.get('server', {}) if isinstance(natnet, dict) else {} + robots = natnet.get('robots', {}) if isinstance(natnet, dict) else {} + profile = robots.get(robot_name, {}) if isinstance(robots, dict) else {} + + if not profile: + print( + f"[natnet_ros2.launch] WARNING: no profile for ROBOT_NAME='{robot_name}' " + f"in {cfg_path}; node will start with no tracked bodies." + ) + + node_params = _build_node_params(server, profile) + # launch_ros / rclpy cannot infer the type of an empty-list parameter, so drop + # any empty arrays; the node declares matching empty defaults and tracks nothing. + node_params = { + k: v for k, v in node_params.items() if not (isinstance(v, list) and len(v) == 0) + } # pkg_share = /share/natnet_ros2 → go up two levels to reach , # then down into lib/natnet_ros2/ where colcon installs executables. - pkg_share = get_package_share_directory('natnet_ros2') node_path = Path(pkg_share).parent.parent / 'lib' / 'natnet_ros2' / 'natnet_ros2_node' if not node_path.exists(): raise RuntimeError( @@ -75,11 +174,34 @@ def launch_setup(context, *_args, **_kwargs): executable='natnet_ros2_node', name='natnet_ros2_node', output='screen', - parameters=[ParameterFile(config_file, allow_substs=True)], + parameters=[node_params], ), ] - if publish_mavros: + vision_pose = profile.get('vision_pose', {}) if isinstance(profile, dict) else {} + if vision_pose.get('enabled', False): + input_topic = _namespaced( + robot_name, str(vision_pose.get('input_topic', 'perception/optitrack/drone/pose_cov')) + ) + output_pose_topic = _namespaced( + robot_name, str(vision_pose.get('output_pose_topic', 'interface/mavros/vision_pose/pose')) + ) + output_pose_cov_topic = _namespaced( + robot_name, + str(vision_pose.get('output_pose_cov_topic', 'interface/mavros/vision_pose/pose_cov')), + ) + + actions.append( + IncludeLaunchDescription( + FrontendLaunchDescriptionSource( + os.path.join(pkg_share, 'launch', 'mavros_gp_origin.launch.xml'), + ), + launch_arguments=[ + ('config_file', gp_path), + ('use_sim_time', ust), + ], + ), + ) actions.append( IncludeLaunchDescription( FrontendLaunchDescriptionSource( @@ -87,7 +209,9 @@ def launch_setup(context, *_args, **_kwargs): ), launch_arguments=[ ('config_file', vp_path), - ('body_name', body_name), + ('input_topic', input_topic), + ('output_pose_topic', output_pose_topic), + ('output_pose_cov_topic', output_pose_cov_topic), ('use_sim_time', ust), ], ), @@ -99,18 +223,23 @@ def launch_setup(context, *_args, **_kwargs): DeclareLaunchArgument( 'config_file', default_value=default_natnet_yaml, - description='NatNet parameter YAML (/** ros__parameters). ' - 'publish_to_mavros and body_name control MAVROS include.', + description='NatNet config YAML (natnet: server + per-robot profiles). ' + 'The robot profile selected by ROBOT_NAME drives bodies + MAVROS include.', ), DeclareLaunchArgument( 'vision_pose_config_file', default_value=default_vp_yaml, - description='vision_pose_converter parameter YAML.', + description='vision_pose_converter parameter YAML (frame_id, canonical_quaternion).', + ), + DeclareLaunchArgument( + 'gp_origin_config_file', + default_value=default_gp_origin_yaml, + description='mavros_gp_origin parameter YAML.', ), DeclareLaunchArgument( 'use_sim_time', default_value='false', - description='Forwarded to vision_pose_converter.launch.xml.', + description='Forwarded to MAVROS bridge launch files.', ), OpaqueFunction(function=launch_setup), ], diff --git a/robot/ros_ws/src/perception/natnet_ros2/launch/vision_pose_converter.launch.xml b/robot/ros_ws/src/perception/natnet_ros2/launch/vision_pose_converter.launch.xml index aad9c4474..803cbdad7 100644 --- a/robot/ros_ws/src/perception/natnet_ros2/launch/vision_pose_converter.launch.xml +++ b/robot/ros_ws/src/perception/natnet_ros2/launch/vision_pose_converter.launch.xml @@ -3,26 +3,31 @@ - + + + - - - + + + diff --git a/robot/ros_ws/src/perception/natnet_ros2/package.xml b/robot/ros_ws/src/perception/natnet_ros2/package.xml index f9632b0f7..621469145 100644 --- a/robot/ros_ws/src/perception/natnet_ros2/package.xml +++ b/robot/ros_ws/src/perception/natnet_ros2/package.xml @@ -26,6 +26,7 @@ mavros_msgs + geographic_msgs ament_index_python launch diff --git a/robot/ros_ws/src/perception/natnet_ros2/src/mavros_gp_origin_node.py b/robot/ros_ws/src/perception/natnet_ros2/src/mavros_gp_origin_node.py new file mode 100755 index 000000000..1f21464bc --- /dev/null +++ b/robot/ros_ws/src/perception/natnet_ros2/src/mavros_gp_origin_node.py @@ -0,0 +1,126 @@ +#!/usr/bin/env python3 + +""" +MAVROS GPS Origin Node + +Publishes a synthetic GPS origin to MAVROS once at startup for mocap / no-GNSS +flight. With GNSS disabled, PX4 fuses vision into a valid local position but +has no global position, so modes that require one (e.g. AUTO.LOITER) refuse to +arm. Setting an origin lets PX4 derive global position from the fused estimate. + +The publish is guarded: it waits for MAVROS to connect, watches for an existing +origin, and only publishes if none is present — GNSS-equipped vehicles are left +untouched. +""" + +import rclpy +from rclpy.node import Node +from geographic_msgs.msg import GeoPointStamped +from mavros_msgs.msg import State + + +class MavrosGpOriginNode(Node): + """One-shot synthetic GPS origin publisher for MAVROS / PX4.""" + + def __init__(self): + super().__init__('mavros_gp_origin') + + self.declare_parameter('enabled', True) + # Defaults match the AirStack shared world datum (Lisbon) used by the GCS + # (gcs_utils.py) and sim (gps_utils.py). Normally overridden by + # config/mavros_gp_origin.yaml; kept in sync to avoid a stale fallback. + self.declare_parameter('latitude', 38.736832) + self.declare_parameter('longitude', -9.137977) + self.declare_parameter('altitude', 90.0) + # Seconds to wait after MAVROS connects (listening for an existing + # origin) before publishing our synthetic one. + self.declare_parameter('settle_sec', 5.0) + + self._enabled = self.get_parameter('enabled').value + if not self._enabled: + self.get_logger().info('Synthetic GPS origin disabled (enabled=false).') + return + + self._lat = self.get_parameter('latitude').value + self._lon = self.get_parameter('longitude').value + self._alt = self.get_parameter('altitude').value + self._settle_sec = self.get_parameter('settle_sec').value + + self._done = False + self._origin_exists = False + self._connected_since = None + self._publish_count = 0 + + self._set_origin_pub = self.create_publisher( + GeoPointStamped, 'set_gps_origin', 10 + ) + self._origin_sub = self.create_subscription( + GeoPointStamped, 'current_gps_origin', self._on_existing_origin, 10 + ) + self._state_sub = self.create_subscription( + State, 'mavros_state', self._on_mavros_state, 10 + ) + self._timer = self.create_timer(1.0, self._tick) + + self.get_logger().info( + f'MAVROS GPS origin node started ' + f'(lat={self._lat}, lon={self._lon}, alt={self._alt}, ' + f'settle_sec={self._settle_sec})' + ) + + def _on_existing_origin(self, _msg: GeoPointStamped): + """An origin already exists (e.g. from GNSS) — never override it.""" + if not self._origin_exists and not self._done: + self.get_logger().info( + 'Existing GPS origin detected; skipping synthetic origin.' + ) + self._origin_exists = True + + def _on_mavros_state(self, msg: State): + if msg.connected and self._connected_since is None: + self._connected_since = self.get_clock().now() + + def _tick(self): + if self._done: + return + if self._origin_exists: + self._done = True + self._timer.cancel() + return + if self._connected_since is None: + return + elapsed = (self.get_clock().now() - self._connected_since).nanoseconds * 1e-9 + if elapsed < self._settle_sec: + return + + msg = GeoPointStamped() + msg.header.stamp = self.get_clock().now().to_msg() + msg.position.latitude = self._lat + msg.position.longitude = self._lon + msg.position.altitude = self._alt + self._set_origin_pub.publish(msg) + self._publish_count += 1 + self.get_logger().info( + f'Published synthetic GPS origin ' + f'(lat={self._lat}, lon={self._lon}, alt={self._alt}) ' + f'[{self._publish_count}/3]' + ) + # Publish a few times in case MAVROS subscribed late, then stop. + if self._publish_count >= 3: + self._done = True + self._timer.cancel() + + +def main(args=None): + rclpy.init(args=args) + try: + node = MavrosGpOriginNode() + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/robot/ros_ws/src/perception/natnet_ros2/src/natnet_ros2_node.cpp b/robot/ros_ws/src/perception/natnet_ros2/src/natnet_ros2_node.cpp index 65059f659..d754a1cef 100644 --- a/robot/ros_ws/src/perception/natnet_ros2/src/natnet_ros2_node.cpp +++ b/robot/ros_ws/src/perception/natnet_ros2/src/natnet_ros2_node.cpp @@ -2,25 +2,27 @@ // // ROS 2 NatNet SDK node for OptiTrack Motive integration. // -// Published topics (per tracked rigid body): -// /{robot_name}/perception/optitrack/{body_name} → PoseStamped -// /{robot_name}/perception/optitrack/{body_name}/pose_cov → PoseWithCovarianceStamped +// Published topics (per configured rigid body): +// /{robot_name}/{topic} → PoseStamped (when pose=true) +// /{robot_name}/{topic}/pose_cov → PoseWithCovarianceStamped (when pose_cov=true) +// where {topic} defaults to perception/optitrack/{rigid_body_name} when unset. // -// Parameters (see config/natnet_config.yaml): -// server_ip, client_ip, command_port, data_port, -// body_name, body_id (-1 = all), publish_direct_optitrack, -// frame_id, debug, position_covariance, orientation_covariance +// Parameters are flattened from config/natnet_config.yaml by natnet_ros2.launch.py: +// server_ip, client_ip, command_port, data_port, connection_type, +// multicast_address, frame_id, debug, and parallel per-body arrays: +// body_names[], body_ids[], body_topics[], body_pose[], body_pose_cov[], +// body_position_covariance[] / body_orientation_covariance[] (9·N, sliced per body). // // ROBOT_NAME is read from the environment variable set by AirStack's // robot_name_map resolver at container startup. +#include #include -#include #include #include -#include #include #include +#include // ROS 2 #include "rclcpp/rclcpp.hpp" @@ -41,8 +43,14 @@ class NatNetROS2Node : public rclcpp::Node using PoseStamped = geometry_msgs::msg::PoseStamped; using PoseWithCovarianceStamped = geometry_msgs::msg::PoseWithCovarianceStamped; - struct BodyPublishers + struct BodyConfig { + int32_t id = -1; + std::string rigid_body_name; + std::string topic_base; + bool publish_pose = true; + bool publish_pose_cov = true; + std::array covariance{}; rclcpp::Publisher::SharedPtr pose_pub; rclcpp::Publisher::SharedPtr pose_cov_pub; }; @@ -52,24 +60,23 @@ class NatNetROS2Node : public rclcpp::Node : Node("natnet_ros2_node") { // ----- Parameters -------------------------------------------------- - this->declare_parameter("server_ip", "192.168.1.1"); - this->declare_parameter("client_ip", "0.0.0.0"); - this->declare_parameter("command_port", 1510); - this->declare_parameter("data_port", 1511); - this->declare_parameter("connection_type", std::string("unicast")); - this->declare_parameter("multicast_address", std::string("239.255.42.99")); - this->declare_parameter("body_name", "robot_1"); - this->declare_parameter("body_id", -1); - this->declare_parameter("publish_direct_optitrack", true); - this->declare_parameter("publish_to_mavros", false); - this->declare_parameter("frame_id", "world"); - this->declare_parameter("debug", false); - this->declare_parameter( - "position_covariance", - std::vector{0.1,0.,0., 0.,0.1,0., 0.,0.,0.1}); - this->declare_parameter( - "orientation_covariance", - std::vector{0.01,0.,0., 0.,0.01,0., 0.,0.,0.01}); + this->declare_parameter("server_ip", "192.168.1.1"); + this->declare_parameter("client_ip", "0.0.0.0"); + this->declare_parameter("command_port", 1510); + this->declare_parameter("data_port", 1511); + this->declare_parameter("connection_type", std::string("unicast")); + this->declare_parameter("multicast_address", std::string("239.255.42.99")); + this->declare_parameter("frame_id", "world"); + this->declare_parameter("debug", false); + + // Parallel per-body arrays (flattened from natnet_config.yaml by the launch file). + this->declare_parameter("body_names", std::vector{}); + this->declare_parameter("body_ids", std::vector{}); + this->declare_parameter("body_topics", std::vector{}); + this->declare_parameter("body_pose", std::vector{}); + this->declare_parameter("body_pose_cov", std::vector{}); + this->declare_parameter("body_position_covariance", std::vector{}); + this->declare_parameter("body_orientation_covariance", std::vector{}); // ----- Read parameters --------------------------------------------- const auto connect_cfg = natnet_ros2::make_connect_config( @@ -88,19 +95,14 @@ class NatNetROS2Node : public rclcpp::Node this->get_parameter("connection_type").as_string().c_str()); } - body_name_ = this->get_parameter("body_name").as_string(); - body_id_ = static_cast(this->get_parameter("body_id").as_int()); - publish_direct_ = this->get_parameter("publish_direct_optitrack").as_bool(); - frame_id_ = this->get_parameter("frame_id").as_string(); - debug_ = this->get_parameter("debug").as_bool(); - - covariance_6x6_ = natnet_ros2::build_covariance_6x6( - this->get_parameter("position_covariance").as_double_array(), - this->get_parameter("orientation_covariance").as_double_array()); + frame_id_ = this->get_parameter("frame_id").as_string(); + debug_ = this->get_parameter("debug").as_bool(); const char * rn = std::getenv("ROBOT_NAME"); robot_name_ = rn ? rn : "robot_1"; + build_body_configs(); + RCLCPP_INFO(get_logger(), "========================================="); RCLCPP_INFO(get_logger(), "NatNet ROS 2 Node"); RCLCPP_INFO(get_logger(), " robot_name: %s", robot_name_.c_str()); @@ -110,18 +112,19 @@ class NatNetROS2Node : public rclcpp::Node if (natnet_ros2::is_multicast(connect_cfg)) { RCLCPP_INFO(get_logger(), " multicast_addr: %s", connect_cfg.multicast_address.c_str()); } - RCLCPP_INFO(get_logger(), " body_id: %d (%s)", - static_cast(body_id_), - (body_id_ < 0) ? "track all" : "single body"); + RCLCPP_INFO(get_logger(), " tracked bodies: %zu", bodies_.size()); RCLCPP_INFO(get_logger(), "========================================="); // Production client — NatNetClientAdapter wraps the SDK client_ = std::make_unique(); - connect_and_setup(connect_cfg); + connect_cfg_ = connect_cfg; - refresh_timer_ = this->create_wall_timer( - std::chrono::seconds(1), - std::bind(&NatNetROS2Node::refresh_descriptions_if_needed, this)); + // Try to connect now; keep retrying. + if (!connect_and_setup(connect_cfg_)) { + connect_timer_ = this->create_wall_timer( + std::chrono::seconds(2), + std::bind(&NatNetROS2Node::retry_connect, this)); + } } // ----------------------------------------------------------------------- @@ -132,14 +135,10 @@ class NatNetROS2Node : public rclcpp::Node // ----------------------------------------------------------------------- // Called from the NatNetClientAdapter's frame trampoline. - // publish() and Clock::now() are thread-safe; pub_mutex_ guards map access. + // publish() and Clock::now() are thread-safe; bodies_ is immutable after init. // ----------------------------------------------------------------------- void on_frame(const natnet_ros2::FrameSample & frame) { - if (natnet_ros2::model_list_changed(frame.params)) { - needs_description_refresh_.store(true, std::memory_order_relaxed); - } - if (debug_) { RCLCPP_DEBUG(get_logger(), "Frame %d: %zu rigid bodies, ts=%.4f s", frame.frame_num, frame.bodies.size(), static_cast(frame.timestamp)); @@ -154,20 +153,14 @@ class NatNetROS2Node : public rclcpp::Node } continue; } - if (!natnet_ros2::should_publish_body(body_id_, rb.id)) { continue; } - - std::lock_guard lock(pub_mutex_); - const auto pub_it = publishers_.find(rb.id); - if (pub_it == publishers_.end()) { - needs_description_refresh_.store(true, std::memory_order_relaxed); - continue; - } + const auto it = bodies_.find(rb.id); + if (it == bodies_.end()) { continue; } // not configured for this robot const natnet_ros2::PoseData pose = natnet_ros2::rb_to_pose(rb); - const BodyPublishers & bp = pub_it->second; + const BodyConfig & body = it->second; - if (publish_direct_ && bp.pose_pub) { + if (body.publish_pose && body.pose_pub) { PoseStamped msg; msg.header.frame_id = frame_id_; msg.header.stamp = stamp; @@ -178,10 +171,10 @@ class NatNetROS2Node : public rclcpp::Node msg.pose.orientation.y = pose.qy; msg.pose.orientation.z = pose.qz; msg.pose.orientation.w = pose.qw; - bp.pose_pub->publish(msg); + body.pose_pub->publish(msg); } - if (bp.pose_cov_pub) { + if (body.publish_pose_cov && body.pose_cov_pub) { PoseWithCovarianceStamped cov_msg; cov_msg.header.frame_id = frame_id_; cov_msg.header.stamp = stamp; @@ -192,22 +185,23 @@ class NatNetROS2Node : public rclcpp::Node cov_msg.pose.pose.orientation.y = pose.qy; cov_msg.pose.pose.orientation.z = pose.qz; cov_msg.pose.pose.orientation.w = pose.qw; - cov_msg.pose.covariance = covariance_6x6_; - bp.pose_cov_pub->publish(cov_msg); + cov_msg.pose.covariance = body.covariance; + body.pose_cov_pub->publish(cov_msg); } } } private: // ----------------------------------------------------------------------- - void connect_and_setup(const natnet_ros2::ConnectConfig & cfg) + // Returns true once the handshake succeeds. + bool connect_and_setup(const natnet_ros2::ConnectConfig & cfg) { const natnet_ros2::NegotiationResult neg = natnet_ros2::negotiate(*client_, cfg); if (!neg.ok) { - RCLCPP_ERROR(get_logger(), "%s", neg.log_message.c_str()); - return; + RCLCPP_WARN(get_logger(), "%s", neg.log_message.c_str()); + return false; } if (neg.server_info.host_present) { @@ -216,103 +210,119 @@ class NatNetROS2Node : public rclcpp::Node RCLCPP_WARN(get_logger(), "%s", neg.log_message.c_str()); } - refresh_descriptions_locked(); - client_->set_frame_callback( [this](const natnet_ros2::FrameSample & f) { on_frame(f); }); RCLCPP_INFO(get_logger(), "Frame callback registered — receiving mocap data."); + connected_ = true; + return true; } // ----------------------------------------------------------------------- - void refresh_descriptions_if_needed() + // Timer-driven reconnect. + void retry_connect() { - if (!needs_description_refresh_.exchange(false, std::memory_order_relaxed)) { + if (connected_) { + if (connect_timer_) { connect_timer_->cancel(); } return; } - RCLCPP_INFO(get_logger(), "Model list change detected — refreshing data descriptions."); - std::lock_guard lock(pub_mutex_); - refresh_descriptions_locked(); + RCLCPP_INFO(get_logger(), + "NatNet not connected — retrying handshake to %s ...", + connect_cfg_.server_ip.c_str()); + if (connect_and_setup(connect_cfg_) && connect_timer_) { + connect_timer_->cancel(); + } } // ----------------------------------------------------------------------- - // Must be called with pub_mutex_ held (or from single-threaded init). + // Build the per-body config map + publishers from the parallel param arrays. + // Publishers are created up front (config-driven), so streaming begins as soon + // as frames arrive — no dependency on Motive's data-description handshake. // ----------------------------------------------------------------------- - void refresh_descriptions_locked() + void build_body_configs() { - if (!client_) { return; } - - // Always ensure the statically-configured body has a publisher - if (body_id_ >= 0) { - ensure_publisher_locked(body_id_, body_name_); + const auto names = this->get_parameter("body_names").as_string_array(); + const auto ids = this->get_parameter("body_ids").as_integer_array(); + const auto topics = this->get_parameter("body_topics").as_string_array(); + const auto pose = this->get_parameter("body_pose").as_bool_array(); + const auto pose_cov = this->get_parameter("body_pose_cov").as_bool_array(); + const auto pos_cov = this->get_parameter("body_position_covariance").as_double_array(); + const auto ori_cov = this->get_parameter("body_orientation_covariance").as_double_array(); + + const std::size_t n = std::min(names.size(), ids.size()); + if (names.size() != ids.size()) { + RCLCPP_WARN(get_logger(), + "body_names (%zu) and body_ids (%zu) length mismatch — using %zu.", + names.size(), ids.size(), n); } - const auto bodies = client_->get_body_descriptors(); - int newly_created = 0; - for (const auto & bd : bodies) { - // Store name for every body (including skeleton bones) - body_names_[bd.id] = bd.name; + for (std::size_t i = 0; i < n; ++i) { + BodyConfig body; + body.id = static_cast(ids[i]); + body.rigid_body_name = names[i]; + body.publish_pose = (i < pose.size()) ? pose[i] : true; + body.publish_pose_cov = (i < pose_cov.size()) ? pose_cov[i] : true; - // Skip skeleton bones (parent_id >= 0) - if (bd.parent_id >= 0) { continue; } + const std::string relative = (i < topics.size()) ? topics[i] : std::string{}; + body.topic_base = + natnet_ros2::body_topic_base(robot_name_, body.rigid_body_name, relative); - // When tracking a single body, skip others - if (!natnet_ros2::should_publish_body(body_id_, bd.id)) { continue; } + body.covariance = natnet_ros2::build_covariance_6x6( + cov_slice(pos_cov, i, _DEFAULT_POSITION_COVARIANCE), + cov_slice(ori_cov, i, _DEFAULT_ORIENTATION_COVARIANCE)); - if (ensure_publisher_locked(bd.id, bd.name)) { ++newly_created; } - } + if (body.publish_pose) { + body.pose_pub = this->create_publisher(body.topic_base, 10); + } + if (body.publish_pose_cov) { + body.pose_cov_pub = this->create_publisher( + body.topic_base + "/pose_cov", 10); + } - if (newly_created > 0) { RCLCPP_INFO(get_logger(), - "Data descriptions refreshed: %d new publisher(s) created.", newly_created); - } else { - RCLCPP_DEBUG(get_logger(), "Data descriptions refreshed: no new publishers."); + "Tracking body id=%d name='%s' → %s (pose=%d pose_cov=%d)", + static_cast(body.id), body.rigid_body_name.c_str(), + body.topic_base.c_str(), + static_cast(body.publish_pose), + static_cast(body.publish_pose_cov)); + + bodies_.emplace(body.id, std::move(body)); } } // ----------------------------------------------------------------------- - bool ensure_publisher_locked(int32_t id, const std::string & name) + // Return the i-th 9-element covariance block from a flattened array, or the + // built-in default when the slice is missing. + static std::vector cov_slice( + const std::vector & flat, std::size_t i, const std::vector & fallback) { - if (publishers_.count(id)) { return false; } - - const std::string topic_base = - natnet_ros2::optitrack_topic_base(robot_name_, name); - - BodyPublishers bp; - if (publish_direct_) { - bp.pose_pub = this->create_publisher(topic_base, 10); - } - bp.pose_cov_pub = this->create_publisher( - natnet_ros2::optitrack_pose_cov_topic(robot_name_, name), 10); - - publishers_.emplace(id, std::move(bp)); - - RCLCPP_INFO(get_logger(), - "Publisher registered: id=%d name='%s' → %s[/pose_cov]", - static_cast(id), name.c_str(), topic_base.c_str()); - return true; + const std::size_t start = i * 9; + if (flat.size() < start + 9) { return fallback; } + return std::vector(flat.begin() + start, flat.begin() + start + 9); } // ----------------------------------------------------------------------- // Parameters / state - std::string body_name_; - int32_t body_id_ = -1; - bool publish_direct_ = true; std::string frame_id_; - bool debug_ = false; + bool debug_ = false; std::string robot_name_; - std::array covariance_6x6_{}; - std::unique_ptr client_; + natnet_ros2::ConnectConfig connect_cfg_; + bool connected_ = false; + + std::unordered_map bodies_; - std::mutex pub_mutex_; - std::unordered_map body_names_; - std::unordered_map publishers_; + rclcpp::TimerBase::SharedPtr connect_timer_; - std::atomic needs_description_refresh_{false}; - rclcpp::TimerBase::SharedPtr refresh_timer_; + static const std::vector _DEFAULT_POSITION_COVARIANCE; + static const std::vector _DEFAULT_ORIENTATION_COVARIANCE; }; +const std::vector NatNetROS2Node::_DEFAULT_POSITION_COVARIANCE = + {1.0e-6, 0., 0., 0., 1.0e-6, 0., 0., 0., 1.0e-6}; +const std::vector NatNetROS2Node::_DEFAULT_ORIENTATION_COVARIANCE = + {3.0e-6, 0., 0., 0., 3.0e-6, 0., 0., 0., 3.0e-6}; + // --------------------------------------------------------------------------- int main(int argc, char ** argv) diff --git a/robot/ros_ws/src/perception/natnet_ros2/src/vision_pose_converter_node.py b/robot/ros_ws/src/perception/natnet_ros2/src/vision_pose_converter_node.py index 9a36f879d..81d2df3dd 100755 --- a/robot/ros_ws/src/perception/natnet_ros2/src/vision_pose_converter_node.py +++ b/robot/ros_ws/src/perception/natnet_ros2/src/vision_pose_converter_node.py @@ -7,6 +7,12 @@ for PX4 external pose estimation and state fusion. Converts from NatNet coordinate frame to a frame suitable for MAVROS. + +Topics are configurable so the bridge can be retargeted to other middleware. +``input_topic`` / ``output_pose_topic`` / ``output_pose_cov_topic`` default to the +relative names ``input_pose`` / ``output_pose`` / ``output_pose_cov`` (remappable), +but natnet_ros2.launch.py overrides them with the absolute, ROBOT_NAME-namespaced +topics from the robot's ``vision_pose`` block in natnet_config.yaml. """ import rclpy @@ -28,15 +34,23 @@ def __init__(self): self.declare_parameter('frame_id', 'world') self.declare_parameter('child_frame_id', 'base_link') self.declare_parameter('canonical_quaternion', True) + # Topic names — overridden by the launch file from the per-robot + # vision_pose block; defaults are the historical remappable relative names. + self.declare_parameter('input_topic', 'input_pose') + self.declare_parameter('output_pose_topic', 'output_pose') + self.declare_parameter('output_pose_cov_topic', 'output_pose_cov') self.frame_id = self.get_parameter('frame_id').value self.child_frame_id = self.get_parameter('child_frame_id').value self.canonical_quaternion = self.get_parameter('canonical_quaternion').value + input_topic = self.get_parameter('input_topic').value + output_pose_topic = self.get_parameter('output_pose_topic').value + output_pose_cov_topic = self.get_parameter('output_pose_cov_topic').value # Subscribers self.pose_sub = self.create_subscription( PoseWithCovarianceStamped, - 'input_pose', + input_topic, self._on_pose, 10 ) @@ -44,19 +58,21 @@ def __init__(self): # Publishers self.pose_pub = self.create_publisher( PoseStamped, - 'output_pose', + output_pose_topic, 10 ) self.pose_cov_pub = self.create_publisher( PoseWithCovarianceStamped, - 'output_pose_cov', + output_pose_cov_topic, 10 ) self.get_logger().info( f'Vision pose converter started ' f'(frame_id={self.frame_id!r}, child_frame_id={self.child_frame_id!r}, ' - f'canonical_quaternion={self.canonical_quaternion})' + f'canonical_quaternion={self.canonical_quaternion}, ' + f'input_topic={input_topic!r}, output_pose_topic={output_pose_topic!r}, ' + f'output_pose_cov_topic={output_pose_cov_topic!r})' ) @staticmethod diff --git a/robot/ros_ws/src/perception/natnet_ros2/test/test_natnet_logic.cpp b/robot/ros_ws/src/perception/natnet_ros2/test/test_natnet_logic.cpp index 7a144ef9b..5796c121b 100644 --- a/robot/ros_ws/src/perception/natnet_ros2/test/test_natnet_logic.cpp +++ b/robot/ros_ws/src/perception/natnet_ros2/test/test_natnet_logic.cpp @@ -136,6 +136,46 @@ TEST(TopicNames, LeadingSlashPresent) EXPECT_EQ(optitrack_topic_base("robot_1", "Body")[0], '/'); } +TEST(TopicNames, NamespacedTopicStripsLeadingSlashes) +{ + EXPECT_EQ(namespaced_topic("robot_1", "perception/optitrack/drone"), + "/robot_1/perception/optitrack/drone"); + EXPECT_EQ(namespaced_topic("robot_1", "/perception/optitrack/drone"), + "/robot_1/perception/optitrack/drone"); + EXPECT_EQ(namespaced_topic("robot_2", "///a/b"), "/robot_2/a/b"); +} + +TEST(TopicNames, BodyTopicBaseUsesOverrideWhenSet) +{ + // Empty override → default perception/optitrack/{name} + EXPECT_EQ(body_topic_base("robot_1", "Drone", ""), + "/robot_1/perception/optitrack/Drone"); + // Non-empty override → namespaced relative leaf (decoupled from body name) + EXPECT_EQ(body_topic_base("robot_1", "Drone", "perception/optitrack/drone"), + "/robot_1/perception/optitrack/drone"); + EXPECT_EQ(body_topic_base("robot_3", "Target", "perception/optitrack/target"), + "/robot_3/perception/optitrack/target"); +} + +// =========================================================================== +// Multi-body filtering — body_is_configured +// =========================================================================== + +TEST(BodyIsConfigured, MatchesConfiguredIds) +{ + const std::vector ids = {1, 100}; + EXPECT_TRUE(body_is_configured(ids, 1)); + EXPECT_TRUE(body_is_configured(ids, 100)); + EXPECT_FALSE(body_is_configured(ids, 2)); +} + +TEST(BodyIsConfigured, EmptySetMatchesNothing) +{ + const std::vector ids = {}; + EXPECT_FALSE(body_is_configured(ids, 0)); + EXPECT_FALSE(body_is_configured(ids, 1)); +} + // =========================================================================== // Server negotiation — validate_connection_type diff --git a/robot/ros_ws/src/perception/natnet_ros2/test/test_natnet_ros2.py b/robot/ros_ws/src/perception/natnet_ros2/test/test_natnet_ros2.py index cba5a3444..37526cd18 100644 --- a/robot/ros_ws/src/perception/natnet_ros2/test/test_natnet_ros2.py +++ b/robot/ros_ws/src/perception/natnet_ros2/test/test_natnet_ros2.py @@ -1,20 +1,15 @@ # Copyright (c) 2024 Carnegie Mellon University # MIT License - see LICENSE in the repository root for full text. -"""Unit tests for natnet_ros2 Python source code. +"""Unit tests for natnet_ros2 Python helpers (no ROS install required). -These tests import the actual production source files and stub out ROS at the -import boundary so no ROS installation is required. +Stubs rclpy/launch at import time. Covers ``VisionPoseConverterNode`` quaternion +canonicalisation, configurable-topic wiring, and ``natnet_ros2.launch.py`` +profile-flattening helpers (server + per-body arrays, env expansion, namespacing). -Coverage here: - vision_pose_converter_node.py → VisionPoseConverterNode._canonical_quaternion() - → VisionPoseConverterNode._on_pose() frame_id assignment - -NOT covered here (C++ — requires colcon build + gtest): - natnet_ros2_node.cpp → build_covariance_6x6(), topic name construction, - connection_type validation, SDK frame callback logic. - These live in test_natnet_logic.cpp in the same test/ directory. +C++ logic (``natnet_logic.hpp``) is tested in ``test_natnet_logic.cpp`` via colcon. """ +import importlib.util import sys from pathlib import Path from types import SimpleNamespace @@ -28,22 +23,31 @@ # metaclass machinery returns a Mock for attribute access instead of running # __init_subclass__ / defining methods). We supply a real dummy base class # so the actual class body — including _canonical_quaternion — is defined. +# +# The fake also records declared params and created sub/pub topics so the +# configurable-topic wiring can be asserted without a ROS install. # --------------------------------------------------------------------------- class _FakeNode: + # Per-test parameter overrides keyed by name; consulted by declare_parameter so + # values survive the node's super().__init__ (which resets per-instance state). + _overrides: dict = {} + def __init__(self, name: str): - pass + self._params: dict = {} + self.created_subscriptions: list = [] + self.created_publishers: list = [] def get_logger(self): return MagicMock() - def declare_parameter(self, *args, **kwargs): - pass + def declare_parameter(self, name, default=None): + self._params[name] = self._overrides.get(name, default) def get_parameter(self, name): - m = MagicMock() - m.value = MagicMock() - return m - def create_subscription(self, *args, **kwargs): + return SimpleNamespace(value=self._params.get(name)) + def create_subscription(self, msg_type, topic, callback, qos): + self.created_subscriptions.append(topic) return MagicMock() - def create_publisher(self, *args, **kwargs): + def create_publisher(self, msg_type, topic, qos): + self.created_publishers.append(topic) return MagicMock() @@ -62,6 +66,36 @@ def create_publisher(self, *args, **kwargs): from vision_pose_converter_node import VisionPoseConverterNode # noqa: E402 +# --------------------------------------------------------------------------- +# Load natnet_ros2.launch.py with its heavy launch/ROS deps stubbed, so the +# pure flattening helpers can be unit-tested without a ROS install. +# --------------------------------------------------------------------------- + +for _mod in ( + "ament_index_python", + "ament_index_python.packages", + "launch", + "launch.actions", + "launch.launch_description_sources", + "launch.substitutions", + "launch_ros", + "launch_ros.actions", +): + sys.modules.setdefault(_mod, MagicMock()) + +# yaml is only needed by _load_natnet_config (not the flattening helpers); stub it +# if PyYAML is absent so the launch module still imports in a minimal unit env. +try: + import yaml # noqa: F401 +except ImportError: + sys.modules.setdefault("yaml", MagicMock()) + +_launch_path = Path(__file__).resolve().parent.parent / "launch" / "natnet_ros2.launch.py" +_spec = importlib.util.spec_from_file_location("natnet_ros2_launch_under_test", _launch_path) +natnet_launch = importlib.util.module_from_spec(_spec) +_spec.loader.exec_module(natnet_launch) + + # --------------------------------------------------------------------------- # Helpers # --------------------------------------------------------------------------- @@ -150,3 +184,104 @@ def test_canonical_quaternion_dual_sign_produces_same_result(): assert out_pos.x == pytest.approx(out_neg.x) assert out_pos.y == pytest.approx(out_neg.y) assert out_pos.z == pytest.approx(out_neg.z) + + +# --------------------------------------------------------------------------- +# VisionPoseConverterNode — configurable input/output topics +# --------------------------------------------------------------------------- + +@pytest.mark.unit +def test_vision_pose_converter_default_topics(): + """Defaults reproduce the historical relative (remappable) topic names.""" + node = VisionPoseConverterNode() + assert node.created_subscriptions == ["input_pose"] + assert node.created_publishers == ["output_pose", "output_pose_cov"] + + +@pytest.mark.unit +def test_vision_pose_converter_topic_overrides_applied(): + """When the topic params are set, sub/pub use those exact names.""" + _FakeNode._overrides = { + "input_topic": "/robot_2/perception/optitrack/drone/pose_cov", + "output_pose_topic": "/robot_2/custom/vision/pose", + "output_pose_cov_topic": "/robot_2/custom/vision/pose_cov", + } + try: + node = VisionPoseConverterNode() + finally: + _FakeNode._overrides = {} + assert node.created_subscriptions == ["/robot_2/perception/optitrack/drone/pose_cov"] + assert node.created_publishers == [ + "/robot_2/custom/vision/pose", + "/robot_2/custom/vision/pose_cov", + ] + + +# --------------------------------------------------------------------------- +# natnet_ros2.launch.py — pure config-flattening helpers +# --------------------------------------------------------------------------- + +@pytest.mark.unit +def test_expand_env_uses_default_when_unset(monkeypatch): + monkeypatch.delenv("NATNET_SERVER_IP", raising=False) + assert natnet_launch._expand_env("$(env NATNET_SERVER_IP 172.31.0.200)") == "172.31.0.200" + + +@pytest.mark.unit +def test_expand_env_uses_environment_value(monkeypatch): + monkeypatch.setenv("NATNET_SERVER_IP", "10.0.0.5") + assert natnet_launch._expand_env("$(env NATNET_SERVER_IP 172.31.0.200)") == "10.0.0.5" + + +@pytest.mark.unit +def test_namespaced_strips_and_prefixes(): + assert natnet_launch._namespaced("robot_1", "perception/optitrack/drone") == \ + "/robot_1/perception/optitrack/drone" + assert natnet_launch._namespaced("robot_2", "/already/abs") == "/robot_2/already/abs" + + +@pytest.mark.unit +def test_build_node_params_flattens_bodies(): + server = {"server_ip": "1.2.3.4", "command_port": 1510, "connection_type": "unicast"} + profile = { + "bodies": [ + { + "rigid_body_name": "Drone", + "id": 1, + "topic": "perception/optitrack/drone", + "pose": True, + "pose_cov": True, + "position_covariance": [9.0] * 9, + "orientation_covariance": [8.0] * 9, + }, + { + "rigid_body_name": "Target", + "id": 100, + "topic": "perception/optitrack/target", + "pose": True, + "pose_cov": False, + }, + ] + } + params = natnet_launch._build_node_params(server, profile) + + assert params["server_ip"] == "1.2.3.4" + assert params["body_names"] == ["Drone", "Target"] + assert params["body_ids"] == [1, 100] + assert params["body_topics"] == ["perception/optitrack/drone", "perception/optitrack/target"] + assert params["body_pose"] == [True, True] + assert params["body_pose_cov"] == [True, False] + # 9 floats per body, flattened in body order. + assert len(params["body_position_covariance"]) == 18 + assert params["body_position_covariance"][:9] == [9.0] * 9 + # Target omitted its covariance → built-in default fills its slice. + assert params["body_position_covariance"][9:] == natnet_launch._DEFAULT_POSITION_COVARIANCE + + +@pytest.mark.unit +def test_build_node_params_empty_profile(): + """A robot with no profile yields empty body arrays (node tracks nothing).""" + params = natnet_launch._build_node_params({}, {}) + assert params["body_names"] == [] + assert params["body_ids"] == [] + assert params["body_position_covariance"] == [] diff --git a/robot/ros_ws/src/perception/perception_bringup/launch/perception.launch.xml b/robot/ros_ws/src/perception/perception_bringup/launch/perception.launch.xml index e433e94d4..354eb3c88 100644 --- a/robot/ros_ws/src/perception/perception_bringup/launch/perception.launch.xml +++ b/robot/ros_ws/src/perception/perception_bringup/launch/perception.launch.xml @@ -73,7 +73,8 @@ - + diff --git a/simulation/isaac-sim/docker/Dockerfile.isaac-ros b/simulation/isaac-sim/docker/Dockerfile.isaac-ros index 0dca11fb7..cb1dff3c1 100644 --- a/simulation/isaac-sim/docker/Dockerfile.isaac-ros +++ b/simulation/isaac-sim/docker/Dockerfile.isaac-ros @@ -154,15 +154,23 @@ RUN sed -i \ 's|param set-default IMU_INTEG_RATE 250|param set-default IMU_INTEG_RATE ${PX4_IMU_INTEG_RATE:-250}|' \ /isaac-sim/PX4-Autopilot/ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator -# install px4 dependencies and build -RUN cd PX4-Autopilot && \ - DEBIAN_FRONTEND=noninteractive ./Tools/setup/ubuntu.sh +# Install PX4 SITL build dependencies directly +RUN apt-get update -q && \ + DEBIAN_FRONTEND=noninteractive apt-get install -y --no-install-recommends \ + ccache libssl-dev libxml2-utils rsync xxd && \ + rm -rf /var/lib/apt/lists/* +RUN pip3 install --break-system-packages -r PX4-Autopilot/Tools/setup/requirements.txt + # build px4 RUN cd PX4-Autopilot && \ make px4_sitl RUN /isaac-sim/python.sh -m pip install --no-cache-dir lark-parser +# OptiTrack NatNet emulator extension (in-repo; compose mount overrides at runtime) +COPY extensions/optitrack.natnet.emulator /isaac-sim/.local/share/ov/data/documents/Kit/shared/exts/optitrack.natnet.emulator +RUN /isaac-sim/python.sh -m pip install --no-cache-dir -e /isaac-sim/.local/share/ov/data/documents/Kit/shared/exts/optitrack.natnet.emulator + # TMux config RUN git clone --depth 1 https://github.com/tmux-plugins/tpm /isaac-sim/.tmux/plugins/tpm diff --git a/simulation/isaac-sim/docker/docker-compose.yaml b/simulation/isaac-sim/docker/docker-compose.yaml index dfd699aa4..ea1693155 100644 --- a/simulation/isaac-sim/docker/docker-compose.yaml +++ b/simulation/isaac-sim/docker/docker-compose.yaml @@ -33,6 +33,7 @@ services: ipv4_address: 172.31.0.200 # required to not conflict with other default docker networks on the host machine env_file: - ./omni_pass.env + - ./sitl-files/${SITL_PARAM_PROFILE:-default}.env environment: - AUTOLAUNCH=${AUTOLAUNCH:-'false'} - DISPLAY=${DISPLAY} @@ -42,6 +43,8 @@ services: - NUM_ROBOTS=${NUM_ROBOTS:-1} - ENABLE_LIDAR=${ENABLE_LIDAR:-false} - ISAAC_SIM_HEADLESS=${ISAAC_SIM_HEADLESS:-false} + - LAUNCH_NATNET=${LAUNCH_NATNET:-false} + - NATNET_BODY_NAME=${NATNET_BODY_NAME:-Drone} # Pegasus physics tuning — read by pegasus/simulator/params.py - PX4_PHYSICS_HZ=${PX4_PHYSICS_HZ:-100} - ARDUPILOT_PHYSICS_HZ=${ARDUPILOT_PHYSICS_HZ:-800} @@ -68,9 +71,11 @@ services: - $HOME/docker/isaac-sim/pkg:/isaac-sim/.local/share/ov/pkg:rw \ # pegasus integration - ../extensions/PegasusSimulator/extensions/pegasus.simulator:/isaac-sim/.local/share/ov/data/documents/Kit/shared/exts/pegasus.simulator/:rw + # optitrack natnet emulator + - ../extensions/optitrack.natnet.emulator:/isaac-sim/.local/share/ov/data/documents/Kit/shared/exts/optitrack.natnet.emulator/:rw # omniverse - ./omniverse.toml:/isaac-sim/.nvidia-omniverse/config/omniverse.toml:rw - - ./user.config.json:/isaac-sim/.local/share/ov/data/Kit/Isaac-Sim Full/5.1/user.config.json:rw # enables pegasus extension; IMPORTANT: set the version number without the trailing .0 + - ./user.config.json:/isaac-sim/.local/share/ov/data/Kit/Isaac-Sim Full/5.1/user.config.json:rw # enables pegasus + optitrack extensions; IMPORTANT: set the version number without the trailing .0 # developer stuff - .dev:/isaac-sim/.dev:rw # developer config - .bashrc:/isaac-sim/.bashrc:rw # bash config @@ -143,6 +148,7 @@ services: - $HOME/docker/isaac-sim/data:/isaac-sim/.local/share/ov/data:rw - $HOME/docker/isaac-sim/pkg:/isaac-sim/.local/share/ov/pkg:rw - ../extensions/PegasusSimulator/extensions/pegasus.simulator:/isaac-sim/.local/share/ov/data/documents/Kit/shared/exts/pegasus.simulator/:rw + - ../extensions/optitrack.natnet.emulator:/isaac-sim/.local/share/ov/data/documents/Kit/shared/exts/optitrack.natnet.emulator/:rw - ./omniverse.toml:/isaac-sim/.nvidia-omniverse/config/omniverse.toml:rw - ./user.config.json:/isaac-sim/.local/share/ov/data/Kit/Isaac-Sim Full/5.1/user.config.json:rw - .dev:/isaac-sim/.dev:rw diff --git a/simulation/isaac-sim/docker/sitl-files/default.env b/simulation/isaac-sim/docker/sitl-files/default.env new file mode 100644 index 000000000..aac0ea21d --- /dev/null +++ b/simulation/isaac-sim/docker/sitl-files/default.env @@ -0,0 +1,7 @@ +# SITL env bundle: default (no PX4_PARAM_* overrides beyond airframe + image patches). +# +# Selected by SITL_PARAM_PROFILE in the top-level .env (compose interpolation). +# Variables here are injected into the isaac-sim container and applied at PX4 boot +# via PX4_PARAM_* → param set in ROMFS/px4fmu_common/init.d-posix/rcS. +# +# See px4-vision.env for external-vision / NatNet → MAVROS fusion tuning. diff --git a/simulation/isaac-sim/docker/sitl-files/px4-vision.env b/simulation/isaac-sim/docker/sitl-files/px4-vision.env new file mode 100644 index 000000000..39a6ebf67 --- /dev/null +++ b/simulation/isaac-sim/docker/sitl-files/px4-vision.env @@ -0,0 +1,35 @@ +# PX4 SITL env bundle: external vision / mocap state estimation. +# +# Data path: +# Isaac NatNet emulator → natnet_ros2 → vision_pose_converter +# → /{robot}/interface/mavros/vision_pose/* → PX4 EKF2 +# +# Pair with robot-side vision_pose.enabled: true in the robot's natnet_config.yaml profile. +# Set SITL_PARAM_PROFILE=px4-vision in .env (or overrides/isaac-natnet-vision.env). + +# Fuse horizontal position, vertical position, and yaw from VISION_POSITION_ESTIMATE. +# Bit 2 (velocity) omitted — pose_cov has no twist; MAVROS vision_pose is pose-only. +PX4_PARAM_EKF2_EV_CTRL=11 + +# Use vision as the height reference (indoor mocap; no GNSS in Isaac SITL). +PX4_PARAM_EKF2_HGT_REF=3 + +# No GPS receiver in sim; avoid waiting for GNSS aiding. +PX4_PARAM_EKF2_GPS_CTRL=0 + +# Trust covariance from vision_pose_cov (natnet_config position/orientation matrices). +PX4_PARAM_EKF2_EV_NOISE_MD=0 + +# End-to-end delay: NatNet frame + ROS + MAVROS (milliseconds). Tune if fusion lags. +# PX4_PARAM_EKF2_EV_DELAY=20 + +# Mocap tracks base_link at the vehicle CG — no lever arm. +PX4_PARAM_EKF2_EV_POS_X=0 +PX4_PARAM_EKF2_EV_POS_Y=0 +PX4_PARAM_EKF2_EV_POS_Z=0 + +# Allow arming without GPS fix when fusing external vision only. +PX4_PARAM_COM_ARM_WO_GPS=1 + +# Indoor vision yaw — don't fuse simulated magnetometer +PX4_PARAM_EKF2_MAG_TYPE=5 diff --git a/simulation/isaac-sim/docker/user_TEMPLATE.config.json b/simulation/isaac-sim/docker/user_TEMPLATE.config.json index 0ce7c11f0..26b6093c5 100755 --- a/simulation/isaac-sim/docker/user_TEMPLATE.config.json +++ b/simulation/isaac-sim/docker/user_TEMPLATE.config.json @@ -544,7 +544,8 @@ }, "exts": { "enabled": { - "0": "pegasus.simulator-5.1.0" + "0": "pegasus.simulator-5.1.0", + "1": "optitrack.natnet.emulator-0.1.0" } }, "window": { diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/.gitignore b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/.gitignore new file mode 100644 index 000000000..0db35ef6e --- /dev/null +++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/.gitignore @@ -0,0 +1,9 @@ +# OptiTrack SDK archives and build artifacts (reference tree may exist locally) +**/*.obj +**/*.pdb +**/*.exe +**/*.iobj +**/*.ipdb +**/*.tlog/ +**/__pycache__/ +**/*.pyc diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/README.md b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/README.md new file mode 100644 index 000000000..3bb05f0b6 --- /dev/null +++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/README.md @@ -0,0 +1,160 @@ +# OptiTrack NatNet Emulator (Isaac Sim Extension) + +Python NatNet **server** emulator for AirStack simulation and integration testing with [`natnet_ros2`](../../../../robot/ros_ws/src/perception/natnet_ros2/). + +The extension has two layers: + +1. **Transport + protocol** (`optitrack.natnet.emulator.server`) — UDP NatNet server, ctypes wire types, MODELDEF cache, frame streaming. Importable outside Isaac Sim (unit tests, host-side integration). +2. **Isaac integration** (`optitrack.natnet.emulator.isaac`) — stage-driven `/World/NatNetInterface` config prim, pose sampling on physics steps, Kit UI editor, and Pegasus launch-script helpers. + +## Layout + +``` +optitrack.natnet.emulator/ +├── config/extension.toml # Kit manifest (server module + UI entry point) +├── schema/schema.usda # Typed NatNet interface attribute definitions +├── setup.py +├── docs/ # (legacy design notes — see docs/simulation/isaac_sim/natnet_emulator.md) +├── test/ # Co-located unit tests (proxied by tests/sim/) +└── optitrack/natnet/emulator/ + ├── defaults.py # Reference Drone → prim bindings for tests + ├── server/ # NatNet UDP server (transport + protocol) + │ ├── natnet_server.py # Base server, queue, MODELDEF cache + │ ├── natnet_unicast_server.py + │ ├── natnet_data_types.py + │ ├── natnet_model_types.py + │ └── natnet_server_types.py + └── isaac/ # Isaac Sim wrapper (Kit + USD) + ├── config.py # Pure-Python NatNetInterfaceConfig model + ├── usd_bindings.py # Author/read interface prims on a stage + ├── catalog.py # Config → sDataDescriptions (MODELDEF) + ├── frames.py # Prim poses → sFrameOfMocapData + ├── manager.py # NatNetServerManager (lifecycle + sampling) + ├── scene_setup.py # Pegasus launch helpers (start_drone_natnet_server) + └── ui_extension.py # Docked editor panel (NatNetEmulatorExtension) +``` + +## Responsibilities + +| Layer | Role | +|-------|------| +| **Server** | UDP transport; `NAT_CONNECT` / `NAT_SERVERINFO`; `NAT_REQUEST_MODELDEF`; `NAT_KEEPALIVE`; `NAT_ECHOREQUEST` / `NAT_ECHORESPONSE`; `NAT_FRAMEOFDATA` on the **data port** (1511). MODELDEF stored as packed bytes via `set_model_def_payload()`. Frames enqueued with `enqueue_mocap_data()`. | +| **Isaac wrapper** | Authors and reads the NatNet interface config prim; builds MODELDEF from scene config; samples tracked prim world poses each physics step; calls `flush_mocap_data()` synchronously (background timer disabled — see below). | +| **`defaults.py`** | Hardcoded `Drone` → `/World/base_link` binding for legacy tests; production paths use the stage prim via `scene_setup.build_drone_config()`. | + +The server does **not** own prim-path bindings. The Isaac layer calls `set_model_def_payload(catalog.pack())` after building `sDataDescriptions` from the interface config. + +## Stage-driven config prim + +Configuration lives on a USD prim (conventionally `/World/NatNetInterface`) with `natnet:*` attributes: + +- Server: IP, unicast/multicast mode, command/data ports, publish rate, NatNet version, up-axis, optional pose noise. +- Bodies: multi-apply `natnet:body::*` fields mapping rigid-body name / streaming ID → target prim path. + +`NatNetServerManager` scans the stage, resyncs the catalog when the prim changes, and streams one rigid body per configured target. Missing prims emit **lost** bodies (NaN position, tracking-invalid bit clear) until the target appears — important for Pegasus drones spawned on first Play. + +**Up axis:** default `Z` passes Isaac/USD world poses through unchanged (matches `natnet_ros2`). Set `Y` to emulate a Y-up Motive room. + +## Streaming model (Isaac) + +Inside Kit, the server's background `_data_update_loop` is **disabled** (`auto_stream = False`) because the GIL-starved daemon thread does not reliably transmit frames. Instead, each physics step: + +1. `NatNetServerManager.sample_once()` reads prim poses and `enqueue_mocap_data(frame)`. +2. `NatNetUnicastServer.flush_mocap_data()` sends immediately on the physics-step thread. + +Outside Isaac (host unit tests), `auto_stream=True` uses the timer-driven loop. + +Default Docker sim IP: **`172.31.0.200`** (Isaac container on the AirStack bridge network). + +## Enabling in AirStack + +**Robot:** `LAUNCH_NATNET=true` in `.env` → `natnet_ros2` in perception bringup. Configure Motive/emulator IP in [`natnet_config.yaml`](../../../../robot/ros_ws/src/perception/natnet_ros2/config/natnet_config.yaml). + +**Isaac Sim:** set `ISAAC_SIM_SCRIPT_NAME` to a NatNet launch script (NatNet always starts — no `LAUNCH_NATNET` gate in the script): + +| Script | Use | +|--------|-----| +| `example_one_px4_pegasus_natnet_launch_script.py` | Single drone + static `Target` | +| `example_multi_px4_pegasus_natnet_launch_script.py` | `NUM_ROBOTS` drones + shared `Target` (system tests with NatNet use this even for `NUM_ROBOTS=1`) | + +Baseline Pegasus scripts (`example_one_px4_pegasus_launch_script.py`, `example_multi_px4_pegasus_launch_script.py`) have **no** NatNet integration. + +Convenience bundle for NatNet + external-vision PX4 SITL: + +```bash +airstack up --env-file overrides/isaac-natnet-vision.env +``` + +See [optitrack-development skill](../../../../.agents/skills/optitrack-development/SKILL.md) for wire-protocol details, libNatNet 4.4 unicast quirks, and debugging. + +## Usage + +### Server only (no Kit) + +```python +from optitrack.natnet.emulator import NatNetUnicastServer, make_default_drone_catalog +from optitrack.natnet.emulator.isaac.frames import BodySample, build_frame + +server = NatNetUnicastServer(local_interface="172.31.0.200") +server.set_model_def_payload(make_default_drone_catalog().pack()) +server.start() + +frame = build_frame(0, [BodySample(1, (0, 0, 1), (0, 0, 0, 1))]) +server.enqueue_mocap_data(frame) +server.flush_mocap_data() +``` + +### Isaac launch script + +```python +from optitrack.natnet.emulator.isaac import start_drone_natnet_server + +# Keep a reference to the manager for the sim lifetime. +manager = start_drone_natnet_server( + stage, + drones=[("Drone", 1, "/World/drone1/base_link")], + server_ip="172.31.0.200", +) +``` + +### Kit UI + +The extension registers **Window → NatNet Emulator** — a docked panel to create/edit the interface prim, start/stop the server, and view live body readouts. The same `NatNetServerManager` backs both the UI and launch-script paths. + +## Protocol notes (unicast, libNatNet 4.4) + +| Port | Traffic | +|------|---------| +| **1510** | Command: `NAT_CONNECT`, `NAT_REQUEST_MODELDEF`, keepalives, echo | +| **1511** | Data: `NAT_FRAMEOFDATA` — **must** be sent from a socket bound to the data port | + +Frames sent from the command socket are silently dropped by libNatNet. Every frame payload must include the 4-byte end-of-data tag expected by the C SDK unpacker. + +Full handshake layouts and sniffing workflow: [optitrack-development skill](../../../../.agents/skills/optitrack-development/SKILL.md). + +## Tests + +| Tier | Mark | What | +|------|------|------| +| Unit | `unit` | Serializers, protocol, config, USD authoring, catalog, pose sampling, server lifecycle, scene setup | +| Integration | `integration` | Host emulator → robot `natnet_ros2` pose Hz | + +Co-located tests live in `test/`. Pytest discovers them via thin proxies in [`tests/sim/optitrack_natnet_emulator/`](../../../../tests/sim/optitrack_natnet_emulator/). + +```bash +# Unit (no Docker / no SDK) +pytest tests/sim/optitrack_natnet_emulator/ -m unit -v + +# Integration (robot container + NatNet SDK) +pytest tests/integration/natnet/ -m integration -v +``` + +Representative unit modules: `test_unicast_protocol.py`, `test_pose_streaming.py`, `test_interface_authoring.py`, `test_server_lifecycle.py`, `test_scene_setup.py`. + +## Reference material + +- User guide: [`docs/simulation/isaac_sim/natnet_emulator.md`](../../../../docs/simulation/isaac_sim/natnet_emulator.md) +- Robot client: [`natnet_ros2/README.md`](../../../../robot/ros_ws/src/perception/natnet_ros2/README.md) +- Integration tier: [`tests/integration/natnet/README.md`](../../../../tests/integration/natnet/README.md) + +OptiTrack SDK sample headers may exist locally under `NatNetClientSDK/` for wire-format reference; they are **not** redistributed by AirStack (proprietary license). diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/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/__init__.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/__init__.py new file mode 100644 index 000000000..39ed38144 --- /dev/null +++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/__init__.py @@ -0,0 +1 @@ +"""OptiTrack NatNet packages for AirStack Isaac Sim integration.""" diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/__init__.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/__init__.py new file mode 100644 index 000000000..b19da2cbc --- /dev/null +++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/__init__.py @@ -0,0 +1 @@ +"""NatNet simulation components.""" diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/__init__.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/__init__.py new file mode 100644 index 000000000..e819d8b1b --- /dev/null +++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/__init__.py @@ -0,0 +1,20 @@ +"""OptiTrack Motive NatNet emulator for Isaac Sim.""" + +from .defaults import ( + DEFAULT_DRONE_BINDING, + DEFAULT_TRACKED_BODY_BINDINGS, + TrackedBodyBinding, +) +from .server import Client, NatNetServer, NatNetUnicastServer, TransmissionType +from .server.natnet_model_types import make_default_drone_catalog + +__all__ = [ + "Client", + "DEFAULT_DRONE_BINDING", + "DEFAULT_TRACKED_BODY_BINDINGS", + "NatNetServer", + "NatNetUnicastServer", + "TrackedBodyBinding", + "TransmissionType", + "make_default_drone_catalog", +] diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/defaults.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/defaults.py new file mode 100644 index 000000000..2c1915264 --- /dev/null +++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/defaults.py @@ -0,0 +1,25 @@ +"""Reference tracked-body defaults for tests and the future Isaac Sim wrapper.""" + +from __future__ import annotations + +from dataclasses import dataclass + + +@dataclass(frozen=True) +class TrackedBodyBinding: + """Maps a NatNet rigid body to a USD prim path (not sent on the NatNet wire).""" + + name: str + id: int + prim_path: str + parent_id: int = -1 + + +# Single-drone NatNet Pegasus scenes (example_one_px4_pegasus_natnet_launch_script.py). +DEFAULT_DRONE_BINDING = TrackedBodyBinding( + name="Drone", + id=1, + prim_path="/World/base_link", +) + +DEFAULT_TRACKED_BODY_BINDINGS: tuple[TrackedBodyBinding, ...] = (DEFAULT_DRONE_BINDING,) diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/__init__.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/__init__.py new file mode 100644 index 000000000..ffd1b9e93 --- /dev/null +++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/__init__.py @@ -0,0 +1,62 @@ +# Copyright (c) 2024 Carnegie Mellon University +# MIT License - see LICENSE in the repository root for full text. +"""Isaac Sim integration for the NatNet emulator (stage-driven config prim). + +``config`` is pure Python. ``usd_bindings`` imports ``pxr`` lazily, so +importing this package is safe in non-Isaac environments. +""" + +from .config import ( + BodyBinding, + NatNetInterfaceConfig, + body_attr_name, + make_instance_key, +) +from .catalog import build_catalog, find_duplicate_targets +from .frames import BodySample, build_frame, make_rigid_body_data +from .manager import NatNetServerManager, default_server_factory, format_interface +from .scene_setup import ( + DEFAULT_INTERFACE_PATH, + DEFAULT_TARGET_PATH, + DEFAULT_TARGET_POSITION, + DEFAULT_TARGET_STREAMING_ID, + author_static_target, + build_drone_config, + start_drone_natnet_server, +) +from .usd_bindings import ( + author_interface, + find_interfaces, + is_interface, + read_interface, + read_world_pose, + resolve_targets, +) + +__all__ = [ + "DEFAULT_INTERFACE_PATH", + "DEFAULT_TARGET_PATH", + "DEFAULT_TARGET_POSITION", + "DEFAULT_TARGET_STREAMING_ID", + "BodyBinding", + "BodySample", + "NatNetInterfaceConfig", + "NatNetServerManager", + "author_interface", + "author_static_target", + "body_attr_name", + "build_catalog", + "build_drone_config", + "build_frame", + "default_server_factory", + "find_duplicate_targets", + "find_interfaces", + "format_interface", + "is_interface", + "make_instance_key", + "make_rigid_body_data", + "read_interface", + "read_world_pose", + "resolve_targets", + "start_drone_natnet_server", +] diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/catalog.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/catalog.py new file mode 100644 index 000000000..9c3bfaa35 --- /dev/null +++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/catalog.py @@ -0,0 +1,53 @@ +# Copyright (c) 2024 Carnegie Mellon University +# MIT License - see LICENSE in the repository root for full text. +""" +Turn a :class:`NatNetInterfaceConfig` into the server's MODELDEF catalog +(``sDataDescriptions`` of rigid bodies). Pure Python + ctypes (the ``server`` +package is stdlib-only), so this is hermetically unit-testable — no USD, no Kit. +""" + +from __future__ import annotations + +from ..server.natnet_common import ModelLimits +from ..server.natnet_model_types import DataDescriptors, sDataDescriptions +from .config import NatNetInterfaceConfig + +# szName is null-terminated on the wire; reserve one byte for the terminator. +_MAX_NAME_BYTES = int(ModelLimits.MAX_NAMELENGTH) - 1 +_MAX_MODELS = int(ModelLimits.MAX_MODELS) + + +def build_catalog(config: NatNetInterfaceConfig) -> sDataDescriptions: + """Build an ``sDataDescriptions`` rigid-body catalog from the config bodies. + + No bodies -> an empty catalog (``nDataDescriptions == 0``). Names longer than + the NatNet name field are truncated. Raises ``ValueError`` if there are more + bodies than the protocol allows. + """ + bodies = config.bodies + if len(bodies) > _MAX_MODELS: + raise ValueError( + f"Too many bodies for one catalog: {len(bodies)} > {_MAX_MODELS} (MAX_MODELS)" + ) + + descriptions = sDataDescriptions() + descriptions.nDataDescriptions = len(bodies) + for i, body in enumerate(bodies): + desc = descriptions.arrDataDescriptions[i] + desc.type = int(DataDescriptors.Descriptor_RigidBody) + rb = desc.RigidBodyDescription + rb.szName = body.rigid_body_name.encode("utf-8")[:_MAX_NAME_BYTES] + rb.ID = int(body.streaming_id) + rb.parentID = int(body.parent_id) + rb.offsetqw = 1.0 # identity quaternion offset + rb.nMarkers = 0 + return descriptions + + +def find_duplicate_targets(config: NatNetInterfaceConfig) -> list[str]: + """Return target prim paths referenced by more than one body (empties ignored).""" + counts: dict[str, int] = {} + for body in config.bodies: + if body.target_prim: + counts[body.target_prim] = counts.get(body.target_prim, 0) + 1 + return [path for path, count in counts.items() if count > 1] diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/config.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/config.py new file mode 100644 index 000000000..56c866ab1 --- /dev/null +++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/config.py @@ -0,0 +1,234 @@ +# Copyright (c) 2024 Carnegie Mellon University +# MIT License - see LICENSE in the repository root for full text. +"""Pure-Python config model for the stage-driven NatNet interface. + +The USD binding layer (author/read against a ``Usd.Stage``) +lives in ``usd_bindings.py`` and depends on this model. + +Attribute names follow the multi-apply schema convention +(``natnet:body::``). +The custom-attribute backing is for a future typed applied schema. +""" + +from __future__ import annotations + +from dataclasses import dataclass, field +from typing import Any, Iterable, Mapping + +# --- attribute name constants (USD property names) ----------------------------- + +ATTR_NAMESPACE = "natnet" +MARKER_ATTR = "natnet:isInterface" + +ATTR_SERVER_ENABLED = "natnet:serverEnabled" +ATTR_SERVER_IP = "natnet:serverIp" +ATTR_MODE = "natnet:mode" +ATTR_MULTICAST_ADDR = "natnet:multicastAddr" +ATTR_COMMAND_PORT = "natnet:commandPort" +ATTR_DATA_PORT = "natnet:dataPort" +ATTR_PUBLISH_RATE = "natnet:publishRate" +ATTR_NATNET_VERSION = "natnet:natnetVersion" +ATTR_UP_AXIS = "natnet:upAxis" + +ATTR_POSE_NOISE_ENABLED = "natnet:poseNoiseEnabled" +ATTR_POSE_NOISE_STD_METERS = "natnet:poseNoiseStdMeters" +ATTR_POSE_NOISE_ROTATION_DEG = "natnet:poseNoiseRotationDeg" + +BODY_PREFIX = "natnet:body:" +BODY_FIELD_RIGID_BODY_NAME = "rigidBodyName" +BODY_FIELD_STREAMING_ID = "streamingId" +BODY_FIELD_PARENT_ID = "parentId" +BODY_FIELD_TARGET = "target" + +VALID_MODES = ("unicast", "multicast") + +# Streamed up-axis. Motive exposes an "Up Axis" setting; the reference natnet_ros2 +# driver requires it set to Z and passes coordinates through untouched. +# Isaac Sim / USD is natively Z-up, so "Z" is a pass-through that matches the rest of the +# AirStack stack (default). "Y" emulates a default (Y-up) Motive by rotating the +# streamed pose -90deg about X. +VALID_UP_AXES = ("Y", "Z") + +# defaults shared with NatNetUnicastServer +DEFAULT_SERVER_IP = "172.31.0.200" +DEFAULT_MULTICAST_ADDR = "239.255.42.99" +DEFAULT_COMMAND_PORT = 1510 +DEFAULT_DATA_PORT = 1511 +DEFAULT_PUBLISH_RATE = 100.0 +DEFAULT_NATNET_VERSION = "4.4.0.0" +DEFAULT_UP_AXIS = "Z" +DEFAULT_POSE_NOISE_ENABLED = True +DEFAULT_POSE_NOISE_STD_METERS = 0.0005 +DEFAULT_POSE_NOISE_ROTATION_DEG = 0.05 + + +def body_attr_name(key: str, field_name: str) -> str: + """USD property name for a body-binding field on the given instance key.""" + return f"{BODY_PREFIX}{key}:{field_name}" + + +def make_instance_key(name: str, used: set[str]) -> str: + """Derive a valid, unique multi-apply instance token from a rigid body name. + + USD property/instance tokens must be identifier-like; sanitize non-alnum chars + to underscores and disambiguate collisions with a numeric suffix. + """ + sanitized = "".join(c if c.isalnum() else "_" for c in name).strip("_") + if not sanitized: + sanitized = "body" + if sanitized[0].isdigit(): + sanitized = f"b_{sanitized}" + key = sanitized + i = 1 + while key in used: + key = f"{sanitized}_{i}" + i += 1 + used.add(key) + return key + + +@dataclass +class BodyBinding: + """One tracked rigid body: a Motive name/ID mapped to a USD prim path.""" + + rigid_body_name: str + target_prim: str + streaming_id: int = 1 + parent_id: int = -1 + + @classmethod + def from_dict(cls, data: Mapping[str, Any], *, target_prim: str | None = None) -> "BodyBinding": + d = dict(data) + resolved_target = target_prim if target_prim is not None else d.get("target_prim") + if not resolved_target: + raise ValueError("BodyBinding requires a target_prim (USD path of the tracked prim)") + if "rigid_body_name" not in d: + raise ValueError("BodyBinding requires a rigid_body_name") + return cls( + rigid_body_name=str(d["rigid_body_name"]), + target_prim=str(resolved_target), + streaming_id=int(d.get("streaming_id", 1)), + parent_id=int(d.get("parent_id", -1)), + ) + + def to_dict(self) -> dict[str, Any]: + return { + "rigid_body_name": self.rigid_body_name, + "target_prim": self.target_prim, + "streaming_id": self.streaming_id, + "parent_id": self.parent_id, + } + + +def _normalize_bodies(bodies: Any) -> list[BodyBinding]: + """Accept a list of dicts/BodyBindings, or a ``{prim_path: {...}}`` mapping.""" + if bodies is None: + return [] + out: list[BodyBinding] = [] + if isinstance(bodies, Mapping): + for prim_path, body in bodies.items(): + out.append(BodyBinding.from_dict(body, target_prim=prim_path)) + return out + if isinstance(bodies, Iterable): + for body in bodies: + if isinstance(body, BodyBinding): + out.append(body) + else: + out.append(BodyBinding.from_dict(body)) + return out + raise ValueError(f"`bodies` must be a list or a mapping, got {type(bodies).__name__}") + + +@dataclass +class NatNetInterfaceConfig: + """Server-level config plus the body catalog for one NatNet interface prim.""" + + server_enabled: bool = True + server_ip: str = DEFAULT_SERVER_IP + mode: str = "unicast" + multicast_addr: str = DEFAULT_MULTICAST_ADDR + command_port: int = DEFAULT_COMMAND_PORT + data_port: int = DEFAULT_DATA_PORT + publish_rate: float = DEFAULT_PUBLISH_RATE + natnet_version: str = DEFAULT_NATNET_VERSION + up_axis: str = DEFAULT_UP_AXIS + pose_noise_enabled: bool = DEFAULT_POSE_NOISE_ENABLED + pose_noise_std_meters: float = DEFAULT_POSE_NOISE_STD_METERS + pose_noise_rotation_deg: float = DEFAULT_POSE_NOISE_ROTATION_DEG + bodies: list[BodyBinding] = field(default_factory=list) + + @classmethod + def from_dict(cls, data: Mapping[str, Any]) -> "NatNetInterfaceConfig": + d = dict(data) + return cls( + server_enabled=bool(d.get("server_enabled", True)), + server_ip=str(d.get("server_ip", DEFAULT_SERVER_IP)), + mode=str(d.get("mode", "unicast")), + multicast_addr=str(d.get("multicast_addr", DEFAULT_MULTICAST_ADDR)), + command_port=int(d.get("command_port", DEFAULT_COMMAND_PORT)), + data_port=int(d.get("data_port", DEFAULT_DATA_PORT)), + publish_rate=float(d.get("publish_rate", DEFAULT_PUBLISH_RATE)), + natnet_version=str(d.get("natnet_version", DEFAULT_NATNET_VERSION)), + up_axis=str(d.get("up_axis", DEFAULT_UP_AXIS)).upper(), + pose_noise_enabled=bool(d.get("pose_noise_enabled", DEFAULT_POSE_NOISE_ENABLED)), + pose_noise_std_meters=float(d.get("pose_noise_std_meters", DEFAULT_POSE_NOISE_STD_METERS)), + pose_noise_rotation_deg=float(d.get("pose_noise_rotation_deg", DEFAULT_POSE_NOISE_ROTATION_DEG)), + bodies=_normalize_bodies(d.get("bodies")), + ) + + def to_dict(self) -> dict[str, Any]: + return { + "server_enabled": self.server_enabled, + "server_ip": self.server_ip, + "mode": self.mode, + "multicast_addr": self.multicast_addr, + "command_port": self.command_port, + "data_port": self.data_port, + "publish_rate": self.publish_rate, + "natnet_version": self.natnet_version, + "up_axis": self.up_axis, + "pose_noise_enabled": self.pose_noise_enabled, + "pose_noise_std_meters": self.pose_noise_std_meters, + "pose_noise_rotation_deg": self.pose_noise_rotation_deg, + "bodies": [b.to_dict() for b in self.bodies], + } + + def validate(self) -> "NatNetInterfaceConfig": + """Raise ``ValueError`` (aggregating all problems) if the config is invalid.""" + errors: list[str] = [] + if self.mode not in VALID_MODES: + errors.append(f"mode must be one of {VALID_MODES}, got {self.mode!r}") + if str(self.up_axis).upper() not in VALID_UP_AXES: + errors.append(f"up_axis must be one of {VALID_UP_AXES}, got {self.up_axis!r}") + for port_name, port in (("command_port", self.command_port), ("data_port", self.data_port)): + if not (0 < port < 65536): + errors.append(f"{port_name} must be in 1..65535, got {port}") + if self.command_port == self.data_port: + errors.append("command_port and data_port must differ") + if self.publish_rate <= 0: + errors.append(f"publish_rate must be > 0, got {self.publish_rate}") + if self.pose_noise_std_meters < 0: + errors.append( + f"pose_noise_std_meters must be >= 0, got {self.pose_noise_std_meters}" + ) + if self.pose_noise_rotation_deg < 0: + errors.append( + f"pose_noise_rotation_deg must be >= 0, got {self.pose_noise_rotation_deg}" + ) + for i, body in enumerate(self.bodies): + if not body.rigid_body_name: + errors.append(f"body[{i}] rigid_body_name must be non-empty") + names = [b.rigid_body_name for b in self.bodies] + if len(set(names)) != len(names): + errors.append("rigid_body_name values must be unique across bodies") + ids = [b.streaming_id for b in self.bodies] + if len(set(ids)) != len(ids): + errors.append("streaming_id values must be unique across bodies") + if errors: + raise ValueError("Invalid NatNetInterfaceConfig: " + "; ".join(errors)) + return self + + def assign_instance_keys(self) -> list[tuple[str, BodyBinding]]: + """Pair each body with a deterministic, unique multi-apply instance key.""" + used: set[str] = set() + return [(make_instance_key(b.rigid_body_name, used), b) for b in self.bodies] diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/frames.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/frames.py new file mode 100644 index 000000000..576969b80 --- /dev/null +++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/frames.py @@ -0,0 +1,129 @@ +# Copyright (c) 2024 Carnegie Mellon University +# MIT License - see LICENSE in the repository root for full text. +"""Pose -> NatNet frame conversion (the data-enqueue path). + +Pure Python + ctypes. Sampled prim world poses become an +``sFrameOfMocapData`` of rigid bodies that the server +streams to the client. + +**Frame convention:** Motive exposes an "Up Axis" setting. AirStack's ``natnet_ros2`` +requires it set to **Z** and copy the rigid-body pose straight through +(``rb_to_pose`` is an identity copy). Isaac Sim is Z-up, so the default ``up_axis="Z"`` +emits the prim's USD world pose **as-is**. ``up_axis="Y"`` emulates a default (Y-up) +Motive by rotating the pose -90 deg about X. + +**params bits** (must match the client's ``is_tracking_valid`` / ``model_list_changed``): +- ``0x01`` on a rigid body marks tracking valid — the client *skips* bodies without it. +- ``0x02`` on the frame signals the model list changed so the client re-requests MODELDEF(set the frame after the catalog changes, e.g. a body added live). +""" + +from __future__ import annotations + +import math +import numpy as np +from dataclasses import dataclass +from scipy.spatial.transform import Rotation +from ..server.natnet_data_types import sFrameOfMocapData, sRigidBodyData + +TRACKING_VALID = 0x01 +MODEL_LIST_CHANGED = 0x02 + + +@dataclass +class BodySample: + """One sampled rigid body: streaming ID + world pose, or an invalid (lost) body.""" + + streaming_id: int + position: tuple[float, float, float] = (0.0, 0.0, 0.0) + orientation: tuple[float, float, float, float] = (0.0, 0.0, 0.0, 1.0) # qx,qy,qz,qw + valid: bool = True + + @classmethod + def lost(cls, streaming_id: int) -> "BodySample": + """An untracked body (missing prim): NaN position, tracking-invalid bit clear.""" + nan = float("nan") + return cls(streaming_id, (nan, nan, nan), (0.0, 0.0, 0.0, 1.0), valid=False) + + +def to_motive_pose(position: tuple[float, float, float], orientation: tuple[float, float, float, float], up_axis: str = "Z"): + """Re-express an Isaac (Z-up) world pose in Motive's streamed up-axis frame. + + Returns ``(position, orientation)`` re-axed for the given ``up_axis``: + + - ``"Z"`` (default) — identity pass-through. Isaac/USD is Z-up and the + reference Motive setup streams Z-up, so the pose flows through unchanged and + matches ``natnet_ros2`` (which does no axis conversion). + - ``"Y"`` — emulate a default Y-up Motive by rotating the pose -90 deg about X + (Isaac ``+Z`` -> Motive ``+Y``): ``(x, y, z) -> (x, z, -y)``. This is a + proper right-handed -> right-handed change of basis (det = +1), so the + quaternion's vector part takes the same swap and the scalar part is + unchanged: ``(qx, qy, qz, qw) -> (qx, qz, -qy, qw)``. + + Non-finite components (a lost body's NaN position) pass through unchanged. + """ + if str(up_axis).upper() != "Y": + return position, orientation + x, y, z = position + qx, qy, qz, qw = orientation + return (x, z, -y), (qx, qz, -qy, qw) + + +def make_rigid_body_data(sample: BodySample) -> sRigidBodyData: + """Build one ``sRigidBodyData`` from a sample (sets the tracking-valid bit).""" + rb = sRigidBodyData() + rb.ID = int(sample.streaming_id) + x, y, z = sample.position + qx, qy, qz, qw = sample.orientation + rb.x, rb.y, rb.z = float(x), float(y), float(z) + rb.qx, rb.qy, rb.qz, rb.qw = float(qx), float(qy), float(qz), float(qw) + rb.MeanError = 0.0 + rb.params = TRACKING_VALID if sample.valid else 0 + return rb + + +def build_frame( + frame_number: int, + samples, + *, + timestamp: float = 0.0, + model_list_changed: bool = False, +) -> sFrameOfMocapData: + """Assemble an ``sFrameOfMocapData`` of rigid bodies from samples.""" + frame = sFrameOfMocapData() + frame.iFrame = int(frame_number) + samples = list(samples) + frame.nRigidBodies = len(samples) + for i, sample in enumerate(samples): + frame.RigidBodies[i] = make_rigid_body_data(sample) + frame.fTimestamp = float(timestamp) + frame.params = MODEL_LIST_CHANGED if model_list_changed else 0 + return frame + + +def is_finite_pose(sample: BodySample) -> bool: + """True if all position/orientation components are finite (no NaN/inf).""" + return all(math.isfinite(v) for v in (*sample.position, *sample.orientation)) + + +def apply_pose_noise( + position: tuple[float, float, float], + orientation: tuple[float, float, float, float], + pose_noise_std_meters: float, + pose_noise_rotation_deg: float, +) -> tuple[tuple[float, float, float], tuple[float, float, float, float]]: + """Add independent Gaussian noise to position (m) and orientation (deg, XYZ euler).""" + + x, y, z = position + if pose_noise_std_meters > 0.0: + x += np.random.normal(0, pose_noise_std_meters) + y += np.random.normal(0, pose_noise_std_meters) + z += np.random.normal(0, pose_noise_std_meters) + + roll, pitch, yaw = Rotation.from_quat(orientation).as_euler("xyz", degrees=True) + if pose_noise_rotation_deg > 0.0: + roll += np.random.normal(0, pose_noise_rotation_deg) + pitch += np.random.normal(0, pose_noise_rotation_deg) + yaw += np.random.normal(0, pose_noise_rotation_deg) + + qx, qy, qz, qw = Rotation.from_euler("xyz", (roll, pitch, yaw), degrees=True).as_quat() + return (x, y, z), (float(qx), float(qy), float(qz), float(qw)) diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/manager.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/manager.py new file mode 100644 index 000000000..398d761eb --- /dev/null +++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/manager.py @@ -0,0 +1,403 @@ +# Copyright (c) 2024 Carnegie Mellon University +# MIT License - see LICENSE in the repository root for full text. +""" +``NatNetServerManager`` detects interface prims, samples poses from the stage, +and owns a **single** server instance it can start and stop. +On each enable it builds a MODELDEF catalog from the config and constructs a +fresh server via an injectable factory. +""" + +from __future__ import annotations + +from .catalog import build_catalog, find_duplicate_targets +from .config import DEFAULT_UP_AXIS, NatNetInterfaceConfig +from .frames import BodySample, apply_pose_noise, build_frame, to_motive_pose +from .usd_bindings import find_interfaces, read_interface, read_world_pose, resolve_targets + + +def _catalog_signature(config: NatNetInterfaceConfig): + """Identity of the catalog (body id/name set) — changes trigger a MODELDEF refresh.""" + return tuple((b.streaming_id, b.rigid_body_name) for b in config.bodies) + + +def _parse_version(version_str: str) -> tuple[int, int, int, int]: + try: + parts = tuple(int(x) for x in str(version_str).split(".")) + except ValueError: + parts = () + return (parts + (0, 0, 0, 0))[:4] + + +def default_server_factory(config: NatNetInterfaceConfig): + """Construct (but do not start) a ``NatNetUnicastServer`` from a config.""" + from ..server import NatNetUnicastServer, TransmissionType + + if config.mode != "unicast": + raise NotImplementedError( + f"mode {config.mode!r} is not supported yet (unicast only)" + ) + server = NatNetUnicastServer( + local_interface=config.server_ip, + transmission_type=TransmissionType.UNICAST, + multicast_address=None, + command_port=config.command_port, + data_port=config.data_port, + ) + server.publish_rate = config.publish_rate + server.natnet_version = _parse_version(config.natnet_version) + return server + + +def format_interface(prim_path: str, cfg: NatNetInterfaceConfig) -> str: + """Render a human-readable multi-line summary of one interface config.""" + lines = [f"[natnet] Interface @ {prim_path}"] + lines.append(f" serverEnabled : {cfg.server_enabled}") + lines.append(f" serverIp : {cfg.server_ip}") + lines.append(f" mode : {cfg.mode}") + if cfg.mode == "multicast": + lines.append(f" multicastAddr : {cfg.multicast_addr}") + lines.append(f" commandPort : {cfg.command_port}") + lines.append(f" dataPort : {cfg.data_port}") + lines.append(f" publishRate : {cfg.publish_rate}") + lines.append(f" natnetVersion : {cfg.natnet_version}") + lines.append(f" upAxis : {cfg.up_axis}") + lines.append(f" poseNoise : enabled={cfg.pose_noise_enabled}") + lines.append( + f" std={cfg.pose_noise_std_meters} m, rot={cfg.pose_noise_rotation_deg} deg" + ) + if cfg.bodies: + lines.append(f" bodies ({len(cfg.bodies)}):") + for b in cfg.bodies: + target = b.target_prim or "" + lines.append( + f" - {b.rigid_body_name} (id={b.streaming_id}, parent={b.parent_id}) -> {target}" + ) + else: + lines.append(" bodies : (none)") + return "\n".join(lines) + + +class NatNetServerManager: + """Detects interface prims, prints config, and owns one server instance.""" + + def __init__(self, server_factory=None): + self._stage_event_sub = None + self._usd_listener = None + self._scan_tick_sub = None + self._scan_pending = False + self._server = None + self._server_factory = server_factory or default_server_factory + # Sampling state. ``_needs_resync`` is the "latest config has been read" + # flag inverted: a NatNet prim edit sets it True (stale); the next physics + # sample re-reads the catalog/targets and clears it. ``_sample_cache`` holds + # the resolved (streaming_id, name, prim) tuples sampled every step. + self._needs_resync = False + self._sample_cache: list = [] + self._frame_counter = 0 + self._catalog_signature = None + self._physx_sub = None + # Streamed up-axis (from the interface config; re-read on every resync). + # "Z" (default) streams the Isaac/USD world pose as-is; "Y" re-axes it to + # emulate a default Y-up Motive. See frames.to_motive_pose. + self._up_axis = DEFAULT_UP_AXIS + # Pose noise. + self._pose_noise_enabled = False + self._pose_noise_std_meters = 0.0 + self._pose_noise_rotation_deg = 0.0 + + # --- lifecycle ------------------------------------------------------------- + + def on_startup(self): + import omni.usd + + usd_context = omni.usd.get_context() + self._stage_event_sub = usd_context.get_stage_event_stream().create_subscription_to_pop( + self._on_stage_event, name="natnet_manager_stage_events" + ) + self._register_usd_listener() + self._subscribe_physics() + print("[natnet] NatNetServerManager initialized") + self.scan_and_print() + + def on_shutdown(self): + self.stop_server() + self._physx_sub = None + self._stage_event_sub = None + self._scan_tick_sub = None + self._scan_pending = False + self._revoke_usd_listener() + + def _subscribe_physics(self): + # Sample + enqueue poses on every physics step (only fires while playing). + try: + import omni.physx + + self._physx_sub = omni.physx.get_physx_interface().subscribe_physics_step_events( + self._on_physics_step + ) + except Exception as exc: # Kit/physx only + print(f"[natnet] Physics step subscription unavailable: {exc}") + self._physx_sub = None + + def _on_physics_step(self, _dt): + if self._server is not None: + self.sample_once() + + # --- scanning -------------------------------------------------------------- + + def scan_and_print(self, *_): + """Find every interface prim and print its parsed config.""" + stage = self._get_stage() + if stage is None: + return + interfaces = find_interfaces(stage) + if not interfaces: + print("[natnet] Scan: no NatNetInterface prims on stage.") + return + print(f"[natnet] Scan: {len(interfaces)} interface(s) detected.") + for prim in interfaces: + cfg = read_interface(prim) + print(format_interface(prim.GetPath().pathString, cfg)) + + # --- server lifecycle (single instance; USD-free, factory-injectable) ------ + + @property + def is_running(self) -> bool: + return self._server is not None + + @property + def server(self): + return self._server + + def start_server(self, config: NatNetInterfaceConfig) -> bool: + """Build the catalog, construct a fresh server, and start it — once. + + Idempotent: if a server is already running this is a no-op returning False. + Returns True when a new server was created and started. + """ + if self._server is not None: + print("[natnet] start_server ignored: a server is already running.") + return False + catalog = build_catalog(config) + server = self._server_factory(config) + server.set_model_def_payload(catalog.pack()) + # Pump frames from our sample_once (physics-step) thread rather than the + # server's background timer: inside the Isaac Sim process that daemon thread + # is starved by the render/physics main loop, so frames never get sent. + if hasattr(server, "auto_stream"): + server.auto_stream = False + server.start() + self._server = server + # Force a resync on the first sampled frame so the prim->pose cache is built + # from the live stage (and the catalog signature is seeded). + self._needs_resync = True + self._frame_counter = 0 + # None so the first resync reports "changed" and the first streamed frame + # flags model_list_changed (nudging the client to (re)read MODELDEF). + self._catalog_signature = None + print( + f"[natnet] Server started on {config.server_ip} " + f"(cmd {config.command_port} / data {config.data_port}) " + f"with {len(config.bodies)} body(ies)." + ) + return True + + def stop_server(self) -> bool: + """Shut down the running server (fresh instance is built on next start). + + Idempotent: returns False if nothing was running. + """ + if self._server is None: + return False + try: + self._server.shutdown() + finally: + self._server = None + self._sample_cache = [] + self._needs_resync = False + print("[natnet] Server stopped.") + return True + + def toggle_server(self, config: NatNetInterfaceConfig) -> bool: + """Start if stopped, stop if running. Returns the resulting running state.""" + if self.is_running: + self.stop_server() + else: + self.start_server(config) + return self.is_running + + def apply_enabled(self, config: NatNetInterfaceConfig) -> None: + """Reconcile running state to ``config.server_enabled`` (start/stop).""" + if config.server_enabled and not self.is_running: + self.start_server(config) + elif not config.server_enabled and self.is_running: + self.stop_server() + + def log_target_diagnostics(self, config: NatNetInterfaceConfig) -> None: + """Warn about missing target prims and duplicate targets (best-effort).""" + stage = self._get_stage() + if stage is not None: + _existing, missing = resolve_targets(stage, config) + for body in missing: + print( + f"[natnet] WARNING: body '{body.rigid_body_name}' target prim " + f"missing or empty: {body.target_prim or ''}" + ) + for path in find_duplicate_targets(config): + print(f"[natnet] WARNING: multiple bodies target the same prim: {path}") + + # --- scripting entry point ------------------------------------------------- + + def start_from_stage(self) -> bool: + """Find the interface prim on the current stage, read it, and start. + + Convenience for scripts/Pegasus launchers: author the prim (see + ``author_interface``) then call this. Returns False if nothing to start. + """ + stage = self._get_stage() + if stage is None: + print("[natnet] start_from_stage: no active stage.") + return False + interfaces = find_interfaces(stage) + if not interfaces: + print("[natnet] start_from_stage: no NatNetInterface prim found.") + return False + config = read_interface(interfaces[0]) + self.log_target_diagnostics(config) + return self.start_server(config) + + # --- pose sampling + dynamic catalog (the data-enqueue path) --------------- + + def mark_dirty(self) -> None: + """Flag that the on-stage config changed; next sample re-reads the catalog.""" + self._needs_resync = True + + def _resync(self, stage) -> bool: + """Re-read the interface config, rebuild the catalog, and re-resolve targets. + + Returns True if the catalog (body id/name set) actually changed, so the next + frame can flag ``model_list_changed`` and the client re-requests MODELDEF. + """ + interfaces = find_interfaces(stage) + if not interfaces: + self._sample_cache = [] + return False + config = read_interface(interfaces[0]) + self._up_axis = config.up_axis + self._pose_noise_enabled = config.pose_noise_enabled + self._pose_noise_std_meters = config.pose_noise_std_meters + self._pose_noise_rotation_deg = config.pose_noise_rotation_deg + if self._server is not None: + self._server.set_model_def_payload(build_catalog(config).pack()) + # Cache target *paths* (not prim handles): the prim is re-resolved every + # sample so bodies whose target is created *after* the server starts — e.g. + # a Pegasus drone base_link spawned on the first Play tick — start streaming + # a valid pose as soon as the prim appears (instead of being stuck "lost"). + self._sample_cache = [ + (body.streaming_id, body.rigid_body_name, body.target_prim) + for body in config.bodies + ] + signature = _catalog_signature(config) + changed = signature != self._catalog_signature + self._catalog_signature = signature + return changed + + def sample_once(self, stage=None): + """Sample every body's USD world pose and enqueue one frame to the server. + + Resyncs the catalog first if the config is dirty (so bodies added/removed + live are picked up). Returns the enqueued frame (or None if nothing to do). + """ + if self._server is None: + return None + if stage is None: + stage = self._get_stage() + if stage is None: + return None + + model_changed = False + if self._needs_resync: + model_changed = self._resync(stage) + self._needs_resync = False + + samples = [] + for streaming_id, _name, target_path in self._sample_cache: + prim = stage.GetPrimAtPath(target_path) if target_path else None + pose = read_world_pose(prim) if prim is not None else None + if pose is None: + samples.append(BodySample.lost(streaming_id)) + else: + position, orientation = to_motive_pose(*pose, up_axis=self._up_axis) + if self._pose_noise_enabled: + position, orientation = apply_pose_noise(position, orientation, self._pose_noise_std_meters, self._pose_noise_rotation_deg) + samples.append(BodySample(streaming_id, position, orientation, valid=True)) + + frame = build_frame( + self._frame_counter, samples, model_list_changed=model_changed + ) + self._frame_counter += 1 + self._server.enqueue_mocap_data(frame) + # Send synchronously from this (physics-step) thread. + flush_mocap_data = getattr(self._server, "flush_mocap_data", None) + if callable(flush_mocap_data): + flush_mocap_data() + return frame + + # --- stage / USD notifications -------------------------------------------- + + def _get_stage(self): + import omni.usd + + return omni.usd.get_context().get_stage() + + def _on_stage_event(self, event): + import omni.usd + + if event.type == int(omni.usd.StageEventType.OPENED): + self._register_usd_listener() + self.scan_and_print() + + def _register_usd_listener(self): + from pxr import Tf, Usd + + stage = self._get_stage() + if stage is None: + return + self._revoke_usd_listener() + self._usd_listener = Tf.Notice.Register( + Usd.Notice.ObjectsChanged, self._on_objects_changed, stage + ) + + def _revoke_usd_listener(self): + if self._usd_listener is not None: + self._usd_listener.Revoke() + self._usd_listener = None + + def _on_objects_changed(self, notice, sender): + # Only re-scan when something NatNet-related changed + try: + paths = list(notice.GetResyncedPaths()) + list(notice.GetChangedInfoOnlyPaths()) + except Exception: + paths = [] + if any(("NatNetInterface" in str(p)) or ("natnet:" in str(p)) for p in paths): + # A NatNet prim changed: mark the sampler dirty so the next physics step re-reads the catalog. + self._needs_resync = True + # Debounce author_interface() calls into one scan on the next update tick. + self._request_scan() + + def _request_scan(self): + if self._scan_pending: + return + self._scan_pending = True + import omni.kit.app + + self._scan_tick_sub = ( + omni.kit.app.get_app() + .get_update_event_stream() + .create_subscription_to_pop(self._on_scan_tick, name="natnet_manager_scan_tick") + ) + + def _on_scan_tick(self, _event): + self._scan_pending = False + self._scan_tick_sub = None + self.scan_and_print() diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/scene_setup.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/scene_setup.py new file mode 100644 index 000000000..d7e804c61 --- /dev/null +++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/scene_setup.py @@ -0,0 +1,142 @@ +# Copyright (c) 2024 Carnegie Mellon University +# MIT License - see LICENSE in the repository root for full text. +"""Standalone-launch helpers: stand up a drone NatNet interface on scene load. + +Used by the Pegasus example launch scripts so a Motive-compatible NatNet server +comes up automatically with one rigid body per drone ``base_link`` — no UI clicks. + +Two layers, mirroring the rest of the package: + +- ``build_drone_config`` is **pure** (no USD / Kit), so it unit-tests hermetically. +- ``start_drone_natnet_server`` authors the interface prim and owns a + :class:`~optitrack.natnet.emulator.isaac.manager.NatNetServerManager` that samples + poses on each physics step. It imports ``pxr``/``omni`` lazily (only when called). +""" + +from __future__ import annotations + +from typing import Iterable, Sequence, Tuple + +from .config import ( + DEFAULT_COMMAND_PORT, + DEFAULT_DATA_PORT, + DEFAULT_POSE_NOISE_ENABLED, + DEFAULT_POSE_NOISE_ROTATION_DEG, + DEFAULT_POSE_NOISE_STD_METERS, + DEFAULT_PUBLISH_RATE, + DEFAULT_SERVER_IP, + DEFAULT_UP_AXIS, + BodyBinding, + NatNetInterfaceConfig, +) + +# Where the example scripts author the single interface prim. +DEFAULT_INTERFACE_PATH = "/World/NatNetInterface" + +# Default world prim + position for the demo "target" body (a static placeholder +# the example scripts stream alongside the drones so a tracked target is available). +DEFAULT_TARGET_PATH = "/World/target" +DEFAULT_TARGET_POSITION = (2.0, 0.0, 1.0) +DEFAULT_TARGET_STREAMING_ID = 100 + +# (rigid_body_name, streaming_id, target_prim_path) +DroneSpec = Tuple[str, int, str] + + +def author_static_target( + stage, + prim_path: str = DEFAULT_TARGET_PATH, + position: Sequence[float] = DEFAULT_TARGET_POSITION, +): + """Author a static ``Xform`` prim to act as a NatNet-tracked target. + + Creates ``prim_path`` (a plain transform with a single translate op) at + ``position`` so the emulator can sample it like any other tracked body. The + prim is static — no physics, no animation — representing a fixed point of + interest that drones can be commanded toward. Imports ``pxr`` lazily so this + module stays importable outside Isaac. Returns ``prim_path``. + """ + from pxr import Gf, UsdGeom + + xform = UsdGeom.Xform.Define(stage, prim_path) + xform.AddTranslateOp().Set(Gf.Vec3d(float(position[0]), float(position[1]), float(position[2]))) + return prim_path + + +def build_drone_config( + drones: Iterable[DroneSpec], + *, + server_ip: str = DEFAULT_SERVER_IP, + mode: str = "unicast", + command_port: int = DEFAULT_COMMAND_PORT, + data_port: int = DEFAULT_DATA_PORT, + publish_rate: float = DEFAULT_PUBLISH_RATE, + server_enabled: bool = True, + up_axis: str = DEFAULT_UP_AXIS, + pose_noise_enabled: bool = DEFAULT_POSE_NOISE_ENABLED, + pose_noise_std_meters: float = DEFAULT_POSE_NOISE_STD_METERS, + pose_noise_rotation_deg: float = DEFAULT_POSE_NOISE_ROTATION_DEG, +) -> NatNetInterfaceConfig: + """Build a validated config with one rigid body per drone. + + ``drones`` is an iterable of ``(rigid_body_name, streaming_id, target_prim)`` + tuples — typically one per spawned drone, with ``target_prim`` pointing at the + drone's ``base_link``. Raises ``ValueError`` (via ``validate``) on duplicate + names/ids or bad ports. + """ + bodies = [ + BodyBinding( + rigid_body_name=str(name), + target_prim=str(target), + streaming_id=int(streaming_id), + ) + for name, streaming_id, target in drones + ] + cfg = NatNetInterfaceConfig( + server_enabled=server_enabled, + server_ip=server_ip, + mode=mode, + command_port=command_port, + data_port=data_port, + publish_rate=publish_rate, + up_axis=up_axis, + pose_noise_enabled=pose_noise_enabled, + pose_noise_std_meters=pose_noise_std_meters, + pose_noise_rotation_deg=pose_noise_rotation_deg, + bodies=bodies, + ) + cfg.validate() + return cfg + + +def start_drone_natnet_server( + stage, + drones: Sequence[DroneSpec], + *, + prim_path: str = DEFAULT_INTERFACE_PATH, + start: bool = True, + **config_kwargs, +): + """Author a NatNet interface prim from ``drones`` and return a running manager. + + Authors ``prim_path`` (overwriting any existing interface) with one rigid body + per drone, then creates a :class:`NatNetServerManager` that subscribes to physics + steps and starts the server. Play the sim to stream poses. Keep a reference to + the returned manager so it isn't garbage-collected (which would tear + down the physics subscription and stop the server). + + Returns the ``NatNetServerManager``. If ``start`` is + False (or the config is authored disabled), + the manager is created but the server is left stopped. + """ + from .manager import NatNetServerManager + from .usd_bindings import author_interface + + cfg = build_drone_config(drones, **config_kwargs) + author_interface(stage, prim_path, cfg) + + manager = NatNetServerManager() + manager.on_startup() + if start and cfg.server_enabled: + manager.start_from_stage() + return manager diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/ui_extension.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/ui_extension.py new file mode 100644 index 000000000..50a581551 --- /dev/null +++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/ui_extension.py @@ -0,0 +1,453 @@ +# Copyright (c) 2024 Carnegie Mellon University +# MIT License - see LICENSE in the repository root for full text. +"""Kit extension entry: docked editor for the NatNet interface config prim. + +Create/manage the ``/World/NatNetInterface`` prim. The window docks to the +bottom-right (alongside the Property panel, like Pegasus) so it's easy to find. + +Sync model is explicit and user-driven via the top button row. +""" + +from __future__ import annotations + +import omni.ext + +from .config import VALID_MODES, VALID_UP_AXES, BodyBinding, NatNetInterfaceConfig +from .manager import NatNetServerManager +from .usd_bindings import author_interface, find_interfaces, read_interface, read_world_pose + +_DEFAULT_PRIM_PATH = "/World/NatNetInterface" +_LABEL_WIDTH = 140 +_POS_REFRESH_PERIOD = 1.0 / 6.0 # seconds between live USD position reads + +_COLOR_LIVE = 0xFF33CC33 # green: prim resolves and server is streaming +_COLOR_IDLE = 0xFFAAAAAA # grey: prim resolves but server not running +_COLOR_LOST = 0xFF3333FF # red: no prim / NaN + + +class NatNetEmulatorExtension(omni.ext.IExt): + """Registers the Window menu entry + the docked editor panel.""" + + def on_startup(self, ext_id): # noqa: D401 - Kit lifecycle hook + self._window = None + self._bodies_frame = None + self._cfg = NatNetInterfaceConfig() + self._row_readouts = {} + self._pos_refresh_sub = None + self._last_pos_refresh = 0.0 + self._manager = NatNetServerManager() + self._manager.on_startup() + self._add_menu() + self._subscribe_position_refresh() + + def on_shutdown(self): + self._remove_menu() + self._pos_refresh_sub = None + self._row_readouts = {} + if self._manager is not None: + self._manager.on_shutdown() + self._manager = None + if self._window is not None: + self._window.destroy() + self._window = None + + # --- live position readout ------------------------------------------------- + + def _subscribe_position_refresh(self): + try: + import omni.kit.app + except Exception: # pragma: no cover - Kit only + return + self._pos_refresh_sub = ( + omni.kit.app.get_app() + .get_update_event_stream() + .create_subscription_to_pop(self._on_pos_refresh, name="natnet_ui_pos_refresh") + ) + + def _on_pos_refresh(self, _event): + import time + + if self._window is None or not self._window.visible or not self._row_readouts: + return + now = time.monotonic() + if now - self._last_pos_refresh < _POS_REFRESH_PERIOD: + return + self._last_pos_refresh = now + stage = self._get_stage() + running = self._manager is not None and self._manager.is_running + for idx, (status_label, pos_label) in list(self._row_readouts.items()): + if not (0 <= idx < len(self._cfg.bodies)): + continue + target = self._cfg.bodies[idx].target_prim + symbol, color, text = self._row_readout(stage, target, running) + status_label.text = symbol + status_label.style = {"color": color} + pos_label.text = text + pos_label.style = {"color": color} + + def _row_readout(self, stage, target, running): + if not target: + return "\u25cb", _COLOR_IDLE, "no target prim" + prim = stage.GetPrimAtPath(target) if stage is not None else None + pose = read_world_pose(prim) if prim is not None else None + if pose is None: + return "\u2717", _COLOR_LOST, "NaN (prim missing)" + (x, y, z), _quat = pose + text = f"{x:+.3f}, {y:+.3f}, {z:+.3f}" + if running: + return "\u25cf", _COLOR_LIVE, text + return "\u25cf", _COLOR_IDLE, text + + # --- menu ------------------------------------------------------------------ + + def _add_menu(self): + try: + import omni.kit.menu.utils as menu_utils + from omni.kit.menu.utils import MenuItemDescription + except Exception: # pragma: no cover - Kit only + return + self._menu_entries = [ + MenuItemDescription(name="NatNet Interface", onclick_fn=self._toggle_window) + ] + menu_utils.add_menu_items(self._menu_entries, "Window") + + def _remove_menu(self): + try: + import omni.kit.menu.utils as menu_utils + except Exception: # pragma: no cover - Kit only + return + if getattr(self, "_menu_entries", None): + menu_utils.remove_menu_items(self._menu_entries, "Window") + self._menu_entries = None + + # --- window ---------------------------------------------------------------- + + def _toggle_window(self, *_): + import omni.ui as ui + + if self._window is None: + self._window = ui.Window("NatNet Interface", width=400, height=600) + self._window.frame.set_build_fn(self._build_window) + # Dock bottom-right next to the Property panel, like Pegasus. + self._window.deferred_dock_in("Property", ui.DockPolicy.CURRENT_WINDOW_IS_ACTIVE) + self._window.visible = True + return + self._window.visible = not self._window.visible + + def _refresh(self, *_): + if self._window is not None: + self._window.frame.rebuild() + + def _build_window(self): + import omni.ui as ui + + with ui.ScrollingFrame(): + with ui.VStack(spacing=6, height=0): + ui.Label("NatNet interface", height=0, style={"font_size": 16}) + + with ui.HStack(height=28, spacing=6): + ui.Button("Create Interface", clicked_fn=self._create_server) + ui.Button("Save", clicked_fn=self._save) + ui.Button("Load from Stage", clicked_fn=self._load_from_stage) + ui.Button("Print config", clicked_fn=self._print_config) + + running = self._manager is not None and self._manager.is_running + with ui.HStack(height=28, spacing=6): + ui.Button( + "Stop Server" if running else "Start Server", + clicked_fn=self._toggle_server, + ) + ui.Label( + f"Server: {'RUNNING' if running else 'stopped'}", + width=0, + style={"color": 0xFF33CC33 if running else 0xFF888888}, + ) + + ui.Label( + "\u26a0 Remember to save after each edit", + height=0, + word_wrap=True, + style={"color": 0xFF33CCFF, "font_size": 14}, + ) + + ui.Label(self._status_text(), height=0, word_wrap=True) + + ui.Separator(height=6) + self._bool_row(ui, "Server enabled", "server_enabled", self._cfg.server_enabled) + self._bool_row(ui, "Pose noise enabled", "pose_noise_enabled", self._cfg.pose_noise_enabled) + self._float_row(ui, "Pose noise std meters", "pose_noise_std_meters", self._cfg.pose_noise_std_meters) + self._float_row(ui, "Pose noise rotation deg", "pose_noise_rotation_deg", self._cfg.pose_noise_rotation_deg) + self._str_row(ui, "Server IP", "server_ip", self._cfg.server_ip) + self._combo_row(ui, "Mode", "mode", self._cfg.mode, VALID_MODES) + self._int_row(ui, "Command port", "command_port", self._cfg.command_port) + self._int_row(ui, "Data port", "data_port", self._cfg.data_port) + self._float_row(ui, "Publish rate (Hz)", "publish_rate", self._cfg.publish_rate) + self._combo_row(ui, "Up axis", "up_axis", self._cfg.up_axis, VALID_UP_AXES) + + ui.Separator(height=6) + ui.Label("Tracked bodies", height=0, style={"font_size": 14}) + self._bodies_frame = ui.Frame(height=0) + self._bodies_frame.set_build_fn(self._build_bodies) + with ui.HStack(height=0, spacing=6): + ui.Button("Add body (from selection)", clicked_fn=self._add_body) + + def _status_text(self): + prim = self._find_interface() + if prim is None: + return "No prim on stage yet — Save or Create Server to author one." + return f"Prim on stage: {prim.GetPath().pathString} (Save to push edits, Load to pull)" + + # --- server field rows (edit the working copy only) ------------------------ + + def _bool_row(self, ui, label, key, value): + with ui.HStack(height=0): + ui.Label(label, width=_LABEL_WIDTH) + cb = ui.CheckBox() + cb.model.set_value(bool(value)) + cb.model.add_value_changed_fn( + lambda m, k=key: self._set_cfg_field(k, m.get_value_as_bool()) + ) + + def _str_row(self, ui, label, key, value): + with ui.HStack(height=0): + ui.Label(label, width=_LABEL_WIDTH) + model = ui.StringField().model + model.set_value(str(value)) + model.add_value_changed_fn( + lambda m, k=key: self._set_cfg_field(k, m.get_value_as_string()) + ) + + def _int_row(self, ui, label, key, value): + with ui.HStack(height=0): + ui.Label(label, width=_LABEL_WIDTH) + model = ui.IntField().model + model.set_value(int(value)) + model.add_value_changed_fn( + lambda m, k=key: self._set_cfg_field(k, m.get_value_as_int()) + ) + + def _float_row(self, ui, label, key, value): + with ui.HStack(height=0): + ui.Label(label, width=_LABEL_WIDTH) + model = ui.FloatField().model + model.set_value(float(value)) + model.add_value_changed_fn( + lambda m, k=key: self._set_cfg_field(k, m.get_value_as_float()) + ) + + def _combo_row(self, ui, label, key, value, choices): + with ui.HStack(height=0): + ui.Label(label, width=_LABEL_WIDTH) + index = choices.index(value) if value in choices else 0 + combo = ui.ComboBox(index, *choices) + combo.model.get_item_value_model().add_value_changed_fn( + lambda m, k=key, c=choices: self._set_cfg_field(k, c[m.get_value_as_int()]) + ) + + def _set_cfg_field(self, attr, value): + setattr(self._cfg, attr, value) + + # --- bodies ---------------------------------------------------------------- + + def _rebuild_bodies(self, *_): + if self._bodies_frame is not None: + self._bodies_frame.rebuild() + + def _build_bodies(self): + import omni.ui as ui + + self._row_readouts = {} + with ui.VStack(spacing=6, height=0): + if not self._cfg.bodies: + ui.Label(" (no bodies — select a prim and click Add body)", height=0) + return + with ui.HStack(height=0, spacing=4): + ui.Label("Rigid body name", width=ui.Fraction(1)) + ui.Label("ID", width=40) + ui.Label("Parent", width=50) + ui.Label("Target prim", width=ui.Fraction(2)) + ui.Spacer(width=98) + for idx, body in enumerate(self._cfg.bodies): + self._build_body_row(ui, idx, body) + + def _build_body_row(self, ui, idx, body): + with ui.VStack(height=0, spacing=2): + with ui.HStack(height=0, spacing=4): + name = ui.StringField(width=ui.Fraction(1)).model + name.set_value(body.rigid_body_name) + name.add_value_changed_fn( + lambda m, i=idx: self._set_body_field(i, "rigid_body_name", m.get_value_as_string()) + ) + + sid = ui.IntField(width=40).model + sid.set_value(body.streaming_id) + sid.add_value_changed_fn( + lambda m, i=idx: self._set_body_field(i, "streaming_id", m.get_value_as_int()) + ) + + parent = ui.IntField(width=50).model + parent.set_value(body.parent_id) + parent.add_value_changed_fn( + lambda m, i=idx: self._set_body_field(i, "parent_id", m.get_value_as_int()) + ) + + target = ui.StringField(width=ui.Fraction(2), tooltip="USD path of the tracked prim").model + target.set_value(body.target_prim) + target.add_value_changed_fn( + lambda m, i=idx: self._set_body_field(i, "target_prim", m.get_value_as_string()) + ) + + ui.Button("set target", width=70, clicked_fn=lambda i=idx: self._retarget_body(i)) + ui.Button("x", width=24, clicked_fn=lambda i=idx: self._remove_body_at(i)) + + # Live readout: status dot + world position pulled from the USD stage. + stage = self._get_stage() + running = self._manager is not None and self._manager.is_running + symbol, color, text = self._row_readout(stage, body.target_prim, running) + with ui.HStack(height=0, spacing=6): + ui.Spacer(width=4) + status_label = ui.Label(symbol, width=14, style={"color": color}) + ui.Label("pos:", width=30, style={"color": _COLOR_IDLE}) + pos_label = ui.Label(text, width=ui.Fraction(1), style={"color": color}) + self._row_readouts[idx] = (status_label, pos_label) + + def _set_body_field(self, index, attr, value): + if 0 <= index < len(self._cfg.bodies): + setattr(self._cfg.bodies[index], attr, value) + + def _add_body(self): + next_id = max((b.streaming_id for b in self._cfg.bodies), default=0) + 1 + target = self._selected_target_path(self._find_interface()) + name = target.rsplit("/", 1)[-1] if target else f"Body{next_id}" + existing = {b.rigid_body_name for b in self._cfg.bodies} + while name in existing: + name = f"{name}_{next_id}" + self._cfg.bodies.append(BodyBinding(rigid_body_name=name, target_prim=target, streaming_id=next_id)) + self._rebuild_bodies() + + def _remove_body_at(self, index): + if 0 <= index < len(self._cfg.bodies): + self._cfg.bodies.pop(index) + self._rebuild_bodies() + + def _retarget_body(self, index): + import carb + + path = self._selected_target_path(self._find_interface()) + if not path: + carb.log_warn("[natnet] Select a prim in the viewport to retarget this body.") + return + if 0 <= index < len(self._cfg.bodies): + self._cfg.bodies[index].target_prim = path + self._rebuild_bodies() + + # --- stage helpers --------------------------------------------------------- + + def _get_stage(self): + import omni.usd + + return omni.usd.get_context().get_stage() + + def _find_interface(self): + stage = self._get_stage() + if stage is None: + return None + interfaces = find_interfaces(stage) + return interfaces[0] if interfaces else None + + def _interface_path(self): + prim = self._find_interface() + return prim.GetPath().pathString if prim is not None else _DEFAULT_PRIM_PATH + + def _select(self, prim_path): + import omni.usd + + omni.usd.get_context().get_selection().set_selected_prim_paths([prim_path], True) + + def _selected_target_path(self, interface_prim): + import omni.usd + + sel = omni.usd.get_context().get_selection().get_selected_prim_paths() + iface_path = interface_prim.GetPath().pathString if interface_prim else None + for path in sel: + if path != iface_path: + return path + return "" + + # --- explicit sync actions ------------------------------------------------- + + def _save(self): + import carb + + stage = self._get_stage() + if stage is None: + carb.log_error("[natnet] No active stage.") + return + try: + self._cfg.validate() + except ValueError as exc: + carb.log_error(f"[natnet] Not saved: {exc}") + return + path = self._interface_path() + author_interface(stage, path, self._cfg) + carb.log_info(f"[natnet] Saved interface to {path} ({len(self._cfg.bodies)} bodies).") + self._refresh() + + def _load_from_stage(self): + import carb + + prim = self._find_interface() + if prim is None: + self._cfg = NatNetInterfaceConfig() + carb.log_warn("[natnet] No interface on stage — reset to defaults.") + else: + self._cfg = read_interface(prim) + carb.log_info(f"[natnet] Loaded interface from {prim.GetPath().pathString}.") + self._refresh() + + def _print_config(self): + # Print whatever is authored on the stage (the source of truth). + if self._manager is not None: + self._manager.scan_and_print() + + def _toggle_server(self): + # Start/stop the live server at the click of this button, regardless of the + # serverEnabled attribute. Builds from the prim that's actually on the stage. + import carb + + if self._manager is None: + return + if not self._manager.is_running: + if self._find_interface() is None: + carb.log_warn("[natnet] No interface on stage — Create/Save one first.") + return + try: + self._manager.start_from_stage() + except Exception as exc: # noqa: BLE001 - surface to the user + carb.log_error(f"[natnet] Could not start server: {exc}") + else: + self._manager.stop_server() + self._refresh() + + def _create_server(self): + import carb + + stage = self._get_stage() + if stage is None: + carb.log_error("[natnet] No active stage.") + return + prim = self._find_interface() + if prim is None: + try: + self._cfg.validate() + except ValueError as exc: + carb.log_error(f"[natnet] Cannot create: {exc}") + return + author_interface(stage, _DEFAULT_PRIM_PATH, self._cfg) + path = _DEFAULT_PRIM_PATH + carb.log_info(f"[natnet] Created interface prim at {path}. (Server start: later commit.)") + else: + path = prim.GetPath().pathString + carb.log_info(f"[natnet] Interface already exists at {path}. (Server start: later commit.)") + self._select(path) + self._refresh() diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/usd_bindings.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/usd_bindings.py new file mode 100644 index 000000000..262216ee2 --- /dev/null +++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/usd_bindings.py @@ -0,0 +1,216 @@ +# Copyright (c) 2024 Carnegie Mellon University +# MIT License - see LICENSE in the repository root for full text. +"""USD binding layer: author / read / find NatNet interface prims on a stage. + +``pxr`` is imported lazily inside each function so importing this module doesn't +require USD. + +Backing today is plain namespaced custom attributes + relationships. Property names +follow the multi-apply schema convention (``natnet:body::``). +""" + +from __future__ import annotations + +from typing import Any + +from .config import ( + ATTR_COMMAND_PORT, + ATTR_DATA_PORT, + ATTR_MODE, + ATTR_MULTICAST_ADDR, + ATTR_NATNET_VERSION, + ATTR_POSE_NOISE_ENABLED, + ATTR_POSE_NOISE_ROTATION_DEG, + ATTR_POSE_NOISE_STD_METERS, + ATTR_PUBLISH_RATE, + ATTR_SERVER_ENABLED, + ATTR_SERVER_IP, + ATTR_UP_AXIS, + BODY_FIELD_PARENT_ID, + BODY_FIELD_RIGID_BODY_NAME, + BODY_FIELD_STREAMING_ID, + BODY_FIELD_TARGET, + BODY_PREFIX, + DEFAULT_COMMAND_PORT, + DEFAULT_DATA_PORT, + DEFAULT_MULTICAST_ADDR, + DEFAULT_NATNET_VERSION, + DEFAULT_POSE_NOISE_ENABLED, + DEFAULT_POSE_NOISE_ROTATION_DEG, + DEFAULT_POSE_NOISE_STD_METERS, + DEFAULT_PUBLISH_RATE, + DEFAULT_SERVER_IP, + DEFAULT_UP_AXIS, + MARKER_ATTR, + BodyBinding, + NatNetInterfaceConfig, + body_attr_name, +) + + +def author_interface(stage, prim_path: str, config: Any) -> Any: + """Create/overwrite a NatNet interface prim at ``prim_path`` from ``config``. + + ``config`` may be a :class:`NatNetInterfaceConfig` or a plain ``dict`` (passed + through ``from_dict``). Returns the ``Usd.Prim``. + """ + from pxr import Sdf + + cfg = config if isinstance(config, NatNetInterfaceConfig) else NatNetInterfaceConfig.from_dict(config) + cfg.validate() + + prim = stage.DefinePrim(prim_path, "Scope") + + # Overwrite semantics: drop any previously-authored body properties so removed + # bodies don't linger across re-authoring. + _clear_body_properties(prim) + + _set(prim, MARKER_ATTR, Sdf.ValueTypeNames.Bool, True) + _set(prim, ATTR_SERVER_ENABLED, Sdf.ValueTypeNames.Bool, cfg.server_enabled) + _set(prim, ATTR_SERVER_IP, Sdf.ValueTypeNames.String, cfg.server_ip) + _set(prim, ATTR_MODE, Sdf.ValueTypeNames.Token, cfg.mode) + _set(prim, ATTR_MULTICAST_ADDR, Sdf.ValueTypeNames.String, cfg.multicast_addr) + _set(prim, ATTR_COMMAND_PORT, Sdf.ValueTypeNames.Int, cfg.command_port) + _set(prim, ATTR_DATA_PORT, Sdf.ValueTypeNames.Int, cfg.data_port) + _set(prim, ATTR_PUBLISH_RATE, Sdf.ValueTypeNames.Float, cfg.publish_rate) + _set(prim, ATTR_NATNET_VERSION, Sdf.ValueTypeNames.String, cfg.natnet_version) + _set(prim, ATTR_UP_AXIS, Sdf.ValueTypeNames.Token, cfg.up_axis) + _set(prim, ATTR_POSE_NOISE_ENABLED, Sdf.ValueTypeNames.Bool, cfg.pose_noise_enabled) + _set(prim, ATTR_POSE_NOISE_STD_METERS, Sdf.ValueTypeNames.Float, cfg.pose_noise_std_meters) + _set(prim, ATTR_POSE_NOISE_ROTATION_DEG, Sdf.ValueTypeNames.Float, cfg.pose_noise_rotation_deg) + + for key, body in cfg.assign_instance_keys(): + _set(prim, body_attr_name(key, BODY_FIELD_RIGID_BODY_NAME), Sdf.ValueTypeNames.String, body.rigid_body_name) + _set(prim, body_attr_name(key, BODY_FIELD_STREAMING_ID), Sdf.ValueTypeNames.Int, body.streaming_id) + _set(prim, body_attr_name(key, BODY_FIELD_PARENT_ID), Sdf.ValueTypeNames.Int, body.parent_id) + rel = prim.CreateRelationship(body_attr_name(key, BODY_FIELD_TARGET), False) + rel.SetTargets([Sdf.Path(body.target_prim)] if body.target_prim else []) + + return prim + + +def read_interface(prim) -> NatNetInterfaceConfig: + """Reconstruct a :class:`NatNetInterfaceConfig` from an authored interface prim.""" + return NatNetInterfaceConfig( + server_enabled=bool(_get(prim, ATTR_SERVER_ENABLED, True)), + server_ip=str(_get(prim, ATTR_SERVER_IP, DEFAULT_SERVER_IP)), + mode=str(_get(prim, ATTR_MODE, "unicast")), + multicast_addr=str(_get(prim, ATTR_MULTICAST_ADDR, DEFAULT_MULTICAST_ADDR)), + command_port=int(_get(prim, ATTR_COMMAND_PORT, DEFAULT_COMMAND_PORT)), + data_port=int(_get(prim, ATTR_DATA_PORT, DEFAULT_DATA_PORT)), + publish_rate=float(_get(prim, ATTR_PUBLISH_RATE, DEFAULT_PUBLISH_RATE)), + natnet_version=str(_get(prim, ATTR_NATNET_VERSION, DEFAULT_NATNET_VERSION)), + up_axis=str(_get(prim, ATTR_UP_AXIS, DEFAULT_UP_AXIS)), + pose_noise_enabled=bool(_get(prim, ATTR_POSE_NOISE_ENABLED, DEFAULT_POSE_NOISE_ENABLED)), + pose_noise_std_meters=float(_get(prim, ATTR_POSE_NOISE_STD_METERS, DEFAULT_POSE_NOISE_STD_METERS)), + pose_noise_rotation_deg=float(_get(prim, ATTR_POSE_NOISE_ROTATION_DEG, DEFAULT_POSE_NOISE_ROTATION_DEG)), + bodies=_read_bodies(prim), + ) + + +def find_interfaces(stage) -> list: + """Return every prim on the stage marked as a NatNet interface.""" + interfaces = [] + for prim in stage.Traverse(): + attr = prim.GetAttribute(MARKER_ATTR) + if attr and attr.HasAuthoredValue() and bool(attr.Get()): + interfaces.append(prim) + return interfaces + + +def is_interface(prim) -> bool: + attr = prim.GetAttribute(MARKER_ATTR) + return bool(attr and attr.HasAuthoredValue() and bool(attr.Get())) + + +def read_world_pose(prim): + """Return ``((x, y, z), (qx, qy, qz, qw))`` from a prim's USD world transform. + + Reads the position/orientation **stored in the USD stage** (the local-to-world + transform), which is what the physics step writes back each frame. Returns + ``None`` for an invalid/non-xformable prim so callers can mark the body lost. + """ + from pxr import Usd, UsdGeom + + if prim is None or not prim.IsValid(): + return None + xformable = UsdGeom.Xformable(prim) + if not xformable: + return None + matrix = xformable.ComputeLocalToWorldTransform(Usd.TimeCode.Default()) + translation = matrix.ExtractTranslation() + quat = matrix.ExtractRotationQuat() # Gf.Quatd, normalized + imaginary = quat.GetImaginary() + position = (float(translation[0]), float(translation[1]), float(translation[2])) + orientation = ( + float(imaginary[0]), + float(imaginary[1]), + float(imaginary[2]), + float(quat.GetReal()), + ) + return position, orientation + + +def resolve_targets(stage, config): + """Split a config's bodies into (existing, missing) by target prim presence. + + A body whose ``target_prim`` is empty or points at a non-existent prim lands in + ``missing``. Returns two lists of :class:`BodyBinding`. + """ + existing = [] + missing = [] + for body in config.bodies: + prim = stage.GetPrimAtPath(body.target_prim) if body.target_prim else None + if prim is not None and prim.IsValid(): + existing.append(body) + else: + missing.append(body) + return existing, missing + + +# --- internal helpers ---------------------------------------------------------- + + +def _set(prim, name, type_name, value): + attr = prim.CreateAttribute(name, type_name) + attr.Set(value) + return attr + + +def _get(prim, name, default): + attr = prim.GetAttribute(name) + if attr and attr.HasAuthoredValue(): + return attr.Get() + return default + + +def _clear_body_properties(prim) -> None: + for name in list(prim.GetPropertyNames()): + if name.startswith(BODY_PREFIX): + prim.RemoveProperty(name) + + +def _read_bodies(prim) -> list[BodyBinding]: + suffix = f":{BODY_FIELD_RIGID_BODY_NAME}" + keys = [ + name[len(BODY_PREFIX): -len(suffix)] + for name in prim.GetPropertyNames() + if name.startswith(BODY_PREFIX) and name.endswith(suffix) + ] + + bodies: list[BodyBinding] = [] + for key in keys: + rel = prim.GetRelationship(body_attr_name(key, BODY_FIELD_TARGET)) + targets = rel.GetTargets() if rel else [] + bodies.append( + BodyBinding( + rigid_body_name=str(_get(prim, body_attr_name(key, BODY_FIELD_RIGID_BODY_NAME), "")), + target_prim=str(targets[0]) if targets else "", + streaming_id=int(_get(prim, body_attr_name(key, BODY_FIELD_STREAMING_ID), 1)), + parent_id=int(_get(prim, body_attr_name(key, BODY_FIELD_PARENT_ID), -1)), + ) + ) + + # Stable, deterministic order (independent of USD property iteration order). + bodies.sort(key=lambda b: (b.streaming_id, b.rigid_body_name)) + return bodies diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/__init__.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/__init__.py new file mode 100644 index 000000000..c84c1fe4b --- /dev/null +++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/__init__.py @@ -0,0 +1,11 @@ +"""NatNet UDP server implementation (unicast; multicast planned).""" + +from .natnet_server import Client, NatNetServer, TransmissionType +from .natnet_unicast_server import NatNetUnicastServer + +__all__ = [ + "Client", + "NatNetServer", + "NatNetUnicastServer", + "TransmissionType", +] diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_common.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_common.py new file mode 100644 index 000000000..1eb32177c --- /dev/null +++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_common.py @@ -0,0 +1,27 @@ +from enum import IntEnum +import ctypes + +class ModelLimits(IntEnum): + MAX_MODELS = 2000 # maximum number of total models (data descriptions) + MAX_MARKERSETS = 1000 # maximum number of MarkerSets + MAX_RIGIDBODIES = 1000 # maximum number of RigidBodies + MAX_ASSETS = 1000 # Maximum number of Assets + MAX_NAMELENGTH = 256 # maximum length for strings + MAX_MARKERS = 200 # maximum number of markers per MarkerSet + MAX_RBMARKERS = 20 # maximum number of markers per RigidBody + MAX_SKELETONS = 100 # maximum number of skeletons + MAX_SKELRIGIDBODIES = 200 # maximum number of RididBodies per Skeleton + MAX_LABELED_MARKERS = 1000 # maximum number of labeled markers per frame + MAX_UNLABELED_MARKERS = 1000 # maximum number of unlabeled (other) markers per frame + + MAX_FORCEPLATES = 100 # maximum number of force plate 'bundles' + MAX_DEVICES = 100 # maximum number of peripheral device 'bundles' + MAX_ANALOG_CHANNELS = 32 # maximum number of data channels (signals) per analog/force plate device + MAX_ANALOG_SUBFRAMES = 30 # maximum number of analog/force plate frames per mocap frame + + MAX_PACKETSIZE = 65503 # max size of packet in bytes (actual packet size is dynamic) + # (65535 byte IP limit - 20 byte IP header - 8 byte UDP header - 4 byte sPacket header = 65503 bytes) + + + +MarkerData = ctypes.c_float * 3 diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_data_types.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_data_types.py new file mode 100644 index 000000000..91e23a412 --- /dev/null +++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_data_types.py @@ -0,0 +1,261 @@ +import ctypes +import struct +from .natnet_common import ModelLimits, MarkerData + +class sMarker(ctypes.Structure): + _pack_ = 1 + _fields_ = [ + ("ID", ctypes.c_int32), + ("x", ctypes.c_float), + ("y", ctypes.c_float), + ("z", ctypes.c_float), + ("size", ctypes.c_float), + ("params", ctypes.c_int16), + ("residual", ctypes.c_float) + ] + + def pack(self) -> bytes: + return struct.pack(' bytes: + # szName is null-terminated on the wire. + name_bytes = self.szName.rstrip(b'\x00') + b'\x00' + payload = bytearray(name_bytes) + payload += struct.pack(' bytes: + return struct.pack(' bytes: + payload = bytearray(struct.pack(' bytes: + payload = bytearray(struct.pack(' bytes: + payload = bytearray(struct.pack(' bytes: + payload = bytearray(struct.pack(' bytes: + payload = bytearray(struct.pack(' bytes: + """NatNet 4.1+ prefixes each collection with a 4-byte byte count.""" + payload = bytearray(struct.pack(' 0) or natnet_major > 4: + payload += struct.pack(' bytes: + payload = bytearray() + + payload += struct.pack(' bytes: + # szName is null-terminated on the wire, not fixed MAX_NAMELENGTH. + name_bytes = self.szName.rstrip(b"\x00") + b"\x00" + payload = bytearray(name_bytes) + payload += struct.pack( + " bytes: + if self.type == int(DataDescriptors.Descriptor_RigidBody): + body = self.RigidBodyDescription.pack() + else: + raise ValueError(f"Unsupported data description type: {self.type}") + payload = bytearray(struct.pack(" bytes: + payload = bytearray(struct.pack(" sDataDescriptions: + """Build the default single-body catalog (Drone id=1) for natnet_ros2.""" + descriptions = sDataDescriptions() + descriptions.nDataDescriptions = 1 + desc = descriptions.arrDataDescriptions[0] + desc.type = int(DataDescriptors.Descriptor_RigidBody) + rb = desc.RigidBodyDescription + rb.szName = b"Drone" + rb.ID = 1 + rb.parentID = -1 + rb.offsetqw = 1.0 + rb.nMarkers = 0 + return descriptions diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_server.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_server.py new file mode 100644 index 000000000..05b3c0a2e --- /dev/null +++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_server.py @@ -0,0 +1,338 @@ +from . import natnet_data_types as DataMessages +from . import natnet_server_types as ServerMessages +from . import natnet_model_types as ModelTypes +from enum import Enum +import socket +import threading +import queue +import signal +import ctypes +import typing + + +class TransmissionType(str, Enum): + UNICAST = "unicast" + MULTICAST = "multicast" + +class Client: + def __init__(self, ip: str, port: int, version: typing.Tuple[int, int, int, int] = (4, 4, 0, 0)): + self.ip = ip + self.port = port + self.version = version + self.subscribed_assets = set() + self.socket_lock = threading.Lock() + + def __hash__(self): + # Uniquely identify a client session by their IP and their unique command port. + return hash((self.ip, self.port)) + + def __eq__(self, other): + return (isinstance(other, Client) and + self.ip == other.ip and + self.port == other.port) + + +class NatNetServer: + def __init__(self, + local_interface : str = "172.31.0.200", + transmission_type: TransmissionType = TransmissionType.MULTICAST, + multicast_address : str = "239.255.42.99", + command_port: int = 1510, + data_port : int = 1511, + motive_app_version : typing.Tuple[int, int, int, int]=(3, 1, 0, 0), + natnet_version : typing.Tuple[int, int, int, int]=(4, 4, 0, 0), + high_res_clock_freq : int = 1_000_000_000, + publish_rate : int = 100 # Hz (default 100Hz) + ): + + self.local_interface = local_interface + self.transmission_type = transmission_type + self.multicast_address = multicast_address + self.command_port = command_port + self.data_port = data_port + self.motive_app_version = motive_app_version + self.natnet_version = natnet_version + self.high_res_clock_freq = high_res_clock_freq + self.publish_rate = publish_rate + + self._validate_init_params() + + self.server_description = self._build_server_description() + # Initialize synchronously safe data structures for server state and mocap data + + # Thread-safe queue for Mocp frames + self.mocap_data_queue = queue.Queue(maxsize=100) + self._last_mocap_frame: DataMessages.sFrameOfMocapData | None = None + self._last_mocap_lock = threading.Lock() + + # Thread list and shutdown event + self.threads = [] + self.shutdown_event = threading.Event() + + # Connected clients for unicast mode + self.connected_clients : typing.Set[Client] = set() + self.clients_lock : threading.Lock = threading.Lock() + + # MODELDEF wire cache (Isaac wrapper updates via set_model_def_payload) + self._model_def_lock = threading.Lock() + self._model_def_payload: bytes = ModelTypes.make_default_drone_catalog().pack() + + # Sockets + self.command_socket : socket.socket | None = None + self.data_socket : socket.socket | None = None + + self.running = False + + # When True (default), the background data loop streams frames on its own timer. + # Set False when an external driver (the Isaac wrapper's physics-step callback) + # sends frames synchronously via ``flush_mocap_data``. + self.auto_stream = True + + # start() launches two daemon threads: a command listener (handshake / MODELDEF / keepalive) + # and a data loop that streams mocap frames. The transmission-specific behavior lives in the unicast/multicast subclass. + + def _signal_handler(self, signum, frame): + print(f"\n[NatNetServer] Received interrupt signal {signum}. Initiating shutdown...") + self.shutdown() + + def enqueue_mocap_data(self, new_data: DataMessages.sFrameOfMocapData): + # Thread-safe method to push new physics frames (called by Isaac-Sim extension) + if self.mocap_data_queue.full(): + try: + # Drop oldest frame if falling behind + self.mocap_data_queue.get_nowait() + except queue.Empty: + pass + self.mocap_data_queue.put(new_data) + with self._last_mocap_lock: + self._last_mocap_frame = new_data + + def _get_last_known_mocap_frame(self) -> DataMessages.sFrameOfMocapData | None: + with self._last_mocap_lock: + return self._last_mocap_frame + + def set_model_def_payload(self, payload: bytes) -> None: + """Replace MODELDEF body served on NAT_REQUEST_MODELDEF (Isaac wrapper calls this).""" + with self._model_def_lock: + self._model_def_payload = payload + + def set_model_def_from_descriptions( + self, descriptions: ModelTypes.sDataDescriptions + ) -> None: + """Pack descriptions once and store as the MODELDEF wire cache.""" + self.set_model_def_payload(descriptions.pack()) + + def _get_model_def_payload(self) -> bytes: + """Return cached MODELDEF bytes (command thread only).""" + with self._model_def_lock: + return self._model_def_payload + + def start(self): + # Bind sockets and launch worker threads automatically on init + + # Register signal handlers for graceful shutdown (Catches Ctrl+C and kill) + try: + signal.signal(signal.SIGINT, self._signal_handler) + signal.signal(signal.SIGTERM, self._signal_handler) + except ValueError: + pass # Safe fallback if not called from the main thread + + # 1. Setup Command Socket (Receives connection/discovery requests) + self.command_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM, socket.IPPROTO_UDP) + self.command_socket.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) + self.command_socket.bind(('', self.command_port)) + + # 2. Setup Data Socket (Sends outward Mocap frames). + # Bind to the data port so frames leave with source port == data_port. + # libNatNet routes unicast NAT_FRAMEOFDATA by the server's data port + self.data_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM, socket.IPPROTO_UDP) + self.data_socket.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) + self.data_socket.bind(('', self.data_port)) + if self.transmission_type == TransmissionType.MULTICAST: + self.data_socket.setsockopt(socket.IPPROTO_IP, socket.IP_MULTICAST_IF, socket.inet_aton(self.local_interface)) + + # 3. Launch Threads + cmd_thread = threading.Thread(target=self._command_listener_loop, daemon=True) + data_thread = threading.Thread(target=self._data_update_loop, daemon=True) + + self.threads.extend([cmd_thread, data_thread]) + + for t in self.threads: + t.start() + + self.running = True + + def shutdown(self): + # Cleanly shutdown threads and close sockets + self.running = False + self.shutdown_event.set() + + if self.command_socket: + self.command_socket.close() + + if self.data_socket: + self.data_socket.close() + + for t in self.threads: + if t.is_alive(): + t.join(timeout=1.0) + + def _validate_init_params(self): + + # Validate the local_interface is a valid IP address + if not self.local_interface or not isinstance(self.local_interface, str) or self.local_interface.count('.') != 3: + raise ValueError(f"Invalid local interface IP address: {self.local_interface}") + + # Validate between transmission types and address requirements + if self.transmission_type not in TransmissionType: + raise ValueError(f"Invalid transmission type: {self.transmission_type}. Must be 'unicast' or 'multicast'.") + + if self.transmission_type == TransmissionType.MULTICAST and not self.multicast_address: + raise ValueError("Multicast address must be provided for multicast transmission type.") + + if self.transmission_type == TransmissionType.UNICAST and self.multicast_address: + raise ValueError("Multicast address should not be provided for unicast transmission type.") + + if not (0 < self.command_port < 65536): + raise ValueError(f"Invalid command port: {self.command_port}. Must be between 1 and 65535.") + + if not (0 < self.data_port < 65536): + raise ValueError(f"Invalid data port: {self.data_port}. Must be between 1 and 65535.") + + if self.command_port == self.data_port: + raise ValueError("Command port and data port must be different.") + + if self.motive_app_version and (not isinstance(self.motive_app_version, tuple) or len(self.motive_app_version) != 4): + raise ValueError(f"Invalid Motive app version: {self.motive_app_version}. Must be a tuple of 4 integers (major, minor, build, revision).") + + if self.natnet_version and (not isinstance(self.natnet_version, tuple) or len(self.natnet_version) != 4): + raise ValueError(f"Invalid NatNet version: {self.natnet_version}. Must be a tuple of 4 integers (major, minor, build, revision).") + + if self.motive_app_version and not self.motive_app_version[0] == 3: + raise ValueError(f"Unsupported Motive app version: {self.motive_app_version}. Minimum supported version is 3.0.0.0. Recommended to use 3.1.0.0") + + if not self.natnet_version[0] == 4: + raise ValueError(f"Unsupported NatNet version: {self.natnet_version}. Minimum supported version is 4.0.0.0. Recommended to use 4.4.0.0") + + if self.high_res_clock_freq <= 0: + raise ValueError(f"Invalid high resolution clock frequency: {self.high_res_clock_freq}. Must be a positive integer representing the frequency in Hz.") + + def _get_latest_mocap_packet(self) -> DataMessages.sFrameOfMocapData | None: + # Thread-safe method to retrieve the latest mocap data to be sent + try: + return self.mocap_data_queue.get_nowait() + except queue.Empty: + return None + + @staticmethod + def _pad_fixed_string(value: bytes) -> bytes: + """Null-pad a byte string to MAX_NAMELENGTH for fixed-size NatNet name fields.""" + truncated = value[: ServerMessages.MAX_NAMELENGTH - 1] + return truncated + b"\x00" * (ServerMessages.MAX_NAMELENGTH - len(truncated)) + + @staticmethod + def _assign_version_bytes(field: ctypes.Array, version: typing.Tuple[int, int, int, int]) -> None: + for index, component in enumerate(version): + field[index] = component + + @staticmethod + def _assign_ipv4_bytes(field: ctypes.Array, address: str | bytes) -> None: + octets = socket.inet_aton(address) if isinstance(address, str) else address + for index, octet in enumerate(octets): + field[index] = octet + + def _build_server_description(self) -> ServerMessages.sServerDescription: + # Helper to build the server description struct with current server info (e.g. on startup or in response to command request) + description = ServerMessages.sServerDescription() + description.HostPresent = True + description.szHostComputerName = self._pad_fixed_string( + socket.gethostname().encode("utf-8") + ) + self._assign_ipv4_bytes(description.HostComputerAddress, self.local_interface) + description.szHostApp = self._pad_fixed_string(b"Motive") + self._assign_version_bytes(description.HostAppVersion, self.motive_app_version) + self._assign_version_bytes(description.NatNetVersion, self.natnet_version) + description.HighResClockFrequency = self.high_res_clock_freq + description.bConnectionInfoValid = True + description.ConnectionDataPort = self.data_port + description.ConnectionMulticast = self.transmission_type == TransmissionType.MULTICAST + + if self.transmission_type == TransmissionType.MULTICAST: + self._assign_ipv4_bytes(description.ConnectionMulticastAddress, self.multicast_address) + else: + self._assign_ipv4_bytes(description.ConnectionMulticastAddress, b"\x00\x00\x00\x00") + + return description + + def _build_connect_response_payload(self) -> bytes: + """NAT_CONNECT reply: libNatNet parses NAT_SERVERINFO payload as sSender_Server.""" + sender = ServerMessages.sSender_Server() + sender.Common.szName = self._pad_fixed_string(b"Motive") + self._assign_version_bytes(sender.Common.Version, self.motive_app_version) + self._assign_version_bytes(sender.Common.NatNetVersion, self.natnet_version) + sender.HighResClockFrequency = self.high_res_clock_freq + sender.DataPort = self.data_port + sender.IsMulticast = self.transmission_type == TransmissionType.MULTICAST + if self.transmission_type == TransmissionType.MULTICAST: + self._assign_ipv4_bytes(sender.MulticastGroupAddress, self.multicast_address) + else: + self._assign_ipv4_bytes(sender.MulticastGroupAddress, b"\x00\x00\x00\x00") + return sender.pack() + + def _send_packet_to_client( + self, + client: Client, + message_id: ServerMessages.MessageId | int, + payload: bytes, + sock: socket.socket | None = None, + ) -> None: + """Send a NatNet packet to a unicast client (libNatNet 4.4). + + Command replies go out the command socket; mocap frames go out the data socket. + """ + if self.shutdown_event.is_set(): + return + sock = sock or self.command_socket + if not sock: + raise ValueError("[NatNetServer] Socket not initialized. Cannot send packet.") + + header = ServerMessages.sPacketHeader( + iMessage=int(message_id), + nDataBytes=len(payload), + ) + packet = header.pack() + payload + try: + with client.socket_lock: + sock.sendto(packet, (client.ip, client.port)) + except OSError as e: + raise ValueError( + f"[NatNetServer] Error sending message {int(message_id)} to " + f"client {client.ip}:{client.port}: {e}" + ) from e + + def _data_update_loop(self): # Stub: Different betweeen multicast and unicast server implementations, as they will need to handle client connections differently (multicast will just send to the multicast group address) + # Loop to update mocap data and send packets at regular intervals. + pass + + def _send_data_packet(self, client: Client, data_message: DataMessages.sFrameOfMocapData): + # Serialize frame payload and send via the data socket. + try: + packet_bytes = data_message.pack() + except Exception as e: + raise ValueError(f"[NatNetServer] Error serializing data message: {e}") from e + + self._send_packet_to_client( + client, + ServerMessages.MessageId.NAT_FRAMEOFDATA, + packet_bytes, + sock=self.data_socket, + ) + + def _command_listener_loop(self): # Stub: Different betweeen multicast and unicast server implementations, as they will need to handle client connections differently (multicast will just send to the multicast group address) + # Loop to listen for and handle incoming command requests (e.g. from client apps) + pass + + def _handle_command_request(self, request_data: bytes): # Stub: Different betweeen multicast and unicast server implementations, as they will need to handle client connections differently (multicast will just send to the multicast group address) + # Parse incoming command request, perform requested action, and send response if needed + pass + diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_server_types.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_server_types.py new file mode 100644 index 000000000..3083f1a6e --- /dev/null +++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_server_types.py @@ -0,0 +1,156 @@ +import ctypes +import struct +from enum import IntEnum + +MAX_NAMELENGTH = 256 +MAX_PACKETSIZE = 65503 + +# NatNet SDK sServerDescription uses default struct alignment (#pragma pack(pop)), not pack(1). +SERVER_DESCRIPTION_WIRE_SIZE = 552 +# NAT_CONNECT / NAT_SERVERINFO reply uses packed sSender_Server (#pragma pack(1) in NatNetTypes.h). +SENDER_SERVER_WIRE_SIZE = 256 + 4 + 4 + 8 + 2 + 1 + 4 # 279 + +# Client/server message ids +class MessageId(IntEnum): + NAT_CONNECT = 0 + NAT_SERVERINFO = 1 + NAT_REQUEST = 2 + NAT_RESPONSE = 3 + NAT_REQUEST_MODELDEF = 4 + NAT_MODELDEF = 5 + NAT_REQUEST_FRAMEOFDATA = 6 + NAT_FRAMEOFDATA = 7 + NAT_MESSAGESTRING = 8 + NAT_DISCONNECT = 9 + NAT_KEEPALIVE = 10 + NAT_DISCONNECTBYTIMEOUT = 11 + NAT_ECHOREQUEST = 12 + NAT_ECHORESPONSE = 13 + NAT_DISCOVERY = 14 + NAT_UNRECOGNIZED_REQUEST = 100 + +# Server/Sender configuration and info +def _fixed_name(field: ctypes.Array) -> bytes: + raw = bytes(field).split(b"\x00", 1)[0] + b"\x00" + if len(raw) > MAX_NAMELENGTH: + raw = raw[: MAX_NAMELENGTH - 1] + b"\x00" + return raw + b"\x00" * (MAX_NAMELENGTH - len(raw)) + + +class sSender(ctypes.Structure): + _pack_ = 1 + _fields_ = [ + ("szName", ctypes.c_char * MAX_NAMELENGTH), # host app's name + ("Version", ctypes.c_uint8 * 4), # host app's version [major.minor.build.revision] + ("NatNetVersion", ctypes.c_uint8 * 4) # host app's NatNet version + ] + + def pack(self) -> bytes: + payload = bytearray() + payload += _fixed_name(self.szName) + payload += bytes(self.Version) + payload += bytes(self.NatNetVersion) + return bytes(payload) + +class sSender_Server(ctypes.Structure): + _pack_ = 1 + _fields_ = [ + ("Common", sSender), + ("HighResClockFrequency", ctypes.c_uint64), + ("DataPort", ctypes.c_uint16), + ("IsMulticast", ctypes.c_bool), + ("MulticastGroupAddress", ctypes.c_uint8 * 4) + ] + + def pack(self) -> bytes: + payload = bytearray(self.Common.pack()) + payload += struct.pack(" bytes: + # Wire layout matches NatNet SDK on x86-64 (3 pad bytes before HighResClockFrequency). + payload = bytearray() + payload.append(1 if self.HostPresent else 0) + payload += _fixed_name(self.szHostComputerName) + payload += bytes(self.HostComputerAddress) + payload += _fixed_name(self.szHostApp) + payload += bytes(self.HostAppVersion) + payload += bytes(self.NatNetVersion) + while len(payload) % 8: + payload.append(0) + payload += struct.pack(" bytes: + return bytes(self) + +# Connection types enum matching NatNet SDK rules +class ConnectionType(IntEnum): + ConnectionType_Multicast = 0 + ConnectionType_Unicast = 1 + +class sNatNetClientConnectParams(ctypes.Structure): + """ + Python ctypes translation of the C++ sNatNetClientConnectParams struct. + Enforces a packed structure byte alignment matching the NatNet binary network protocol. + """ + _pack_ = 1 + _fields_ = [ + ("connectionType", ctypes.c_int32), # 4 bytes (mapping to standard ConnectionType enum) + ("serverCommandPort", ctypes.c_uint16), # 2 bytes + ("serverDataPort", ctypes.c_uint16), # 2 bytes + + # NOTE: Represented as void pointers (c_void_p) to safely match the host system's native bit size (e.g., 8 bytes on 64-bit) without string data unpacking overhead. + ("serverAddress", ctypes.c_void_p), + ("localAddress", ctypes.c_void_p), + ("multicastAddress", ctypes.c_void_p), + + ("subscribedDataOnly", ctypes.c_bool), # 1 byte + ("BitstreamVersion", ctypes.c_uint8 * 4) # 4 bytes: [Major, Minor, Build, Revision] + ] + + def pack(self) -> bytes: + return bytes(self) \ No newline at end of file diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_unicast_server.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_unicast_server.py new file mode 100644 index 000000000..d9c0a1e22 --- /dev/null +++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_unicast_server.py @@ -0,0 +1,172 @@ +import ctypes +import time + +from . import natnet_server_types as ServerTypes +from .natnet_server import TransmissionType, Client, NatNetServer + + +class NatNetUnicastServer(NatNetServer): + def __init__(self, + local_interface="172.31.0.200", + transmission_type: TransmissionType = TransmissionType.UNICAST, + multicast_address=None, + command_port=1510, + data_port=1511 + ): + + if not transmission_type == TransmissionType.UNICAST: + raise ValueError("Transmission type 'MULTICAST' is not supported in NatNetUnicastServer. Please use NatNetMulticastServer instead.") + + super().__init__(local_interface, transmission_type, multicast_address, command_port, data_port) + + def _data_update_loop(self): + # Loop to update mocap data and send packets at regular intervals. + # When auto_stream is False the frames are pumped externally (Isaac physics step), + # so this thread only idles — but stays alive for clean shutdown. + while not self.shutdown_event.is_set(): + time.sleep(1 / self.publish_rate) + if not self.auto_stream: + continue + self.flush_mocap_data() + + def flush_mocap_data(self): + """Send the latest (or last) mocap frame to every connected client, once.""" + with self.clients_lock: + clients = list(self.connected_clients) + if not clients: + return + + data_messages = self._get_latest_mocap_packet() + + if data_messages is None: # If the server stops producing frames, use the last known frame. + data_messages = self._get_last_known_mocap_frame() + if data_messages is None: + return + + for client in clients: + try: + self._send_data_packet(client, data_messages) + except ValueError as e: + print(str(e)) + continue + + def _command_listener_loop(self): + # Listens on UDP command socket for incoming command requests from clients. + # Handles incoming client handshakes and teardown. + + print(f"[Command Listener] Command listener thread started. Listening for incoming client command requests on UDP address:port {self.local_interface}:{self.command_port}...") + + while not self.shutdown_event.is_set(): + try: + data, addr = self.command_socket.recvfrom(1024) # Buffer size of 1024 bytes should be sufficient for command requests + if not data: + continue + self._handle_command_request(data, addr) + except Exception as e: + if self.shutdown_event.is_set(): + break + print(f"[Command Listener] Error receiving command request: {e}") + time.sleep(0.1) # Sleep briefly to avoid tight loop on errors + + def _handle_command_request(self, request_data: bytes, client_address: tuple): + """ + Processes standard binary headers and registers unicast endpoints. + """ + header_size = ctypes.sizeof(ServerTypes.sPacketHeader) + if len(request_data) < header_size: + return + + # Parse the header via ctypes + header = ServerTypes.sPacketHeader.from_buffer_copy(request_data[:header_size]) + + # Handle Connection Handshake + if header.iMessage == int(ServerTypes.MessageId.NAT_CONNECT): + client_requested_version = self.natnet_version # Fallback to server's version. Version handshaking not supported in this extension. + + client_ip, client_port = client_address + + # Create and store a new client object + new_client = Client(client_ip, client_port, version=client_requested_version) + try: + with self.clients_lock: + self.connected_clients.discard(new_client) # Remove any existing client with the same IP and port + self.connected_clients.add(new_client) # Add the new client to the connected clients list + print(f"[Command Handler] Added client {new_client.ip}:{new_client.port} to connected clients list.") + except Exception as e: + print(f"[Command Handler] Error adding client {new_client.ip}:{new_client.port} to connected clients list: {e}") + return + + try: + self._send_packet_to_client( + new_client, + ServerTypes.MessageId.NAT_SERVERINFO, + self._build_connect_response_payload(), + ) + except ValueError as e: + raise ValueError( + f"[Command Handler] Error sending server description to client {client_address}: {e}" + ) from e + print( + f"[Command Handler] Sent server description to client address " + f"through its port {client_address}." + ) + return + + # Non-handshake commands require a prior NAT_CONNECT from this endpoint. + client_ip, client_port = client_address + client = self._find_client(client_ip, client_port) + if client is None: + print( + f"[Command Handler] Ignoring message {header.iMessage} from " + f"unregistered client {client_address}." + ) + return + + if header.iMessage == int(ServerTypes.MessageId.NAT_REQUEST_MODELDEF): + try: + self._send_packet_to_client( + client, + ServerTypes.MessageId.NAT_MODELDEF, + self._get_model_def_payload(), + ) + except ValueError as e: + print( + f"[Command Handler] Error sending MODELDEF to client " + f"{client_address}: {e}" + ) + return + + if header.iMessage == int(ServerTypes.MessageId.NAT_KEEPALIVE): + # Receiving a keepalive refreshes the client's liveness; nothing to send back. + return + + if header.iMessage == int(ServerTypes.MessageId.NAT_ECHOREQUEST): + echo_payload = request_data[header_size : header_size + header.nDataBytes] + # libNatNet expects clientRequestTimestamp + hostReceivedTimestamp (8 + 8 bytes). + host_ts = int(time.time() * 1_000_000_000).to_bytes(8, "little", signed=False) + response_payload = echo_payload[:8].ljust(8, b"\x00") + host_ts + try: + self._send_packet_to_client( + client, + ServerTypes.MessageId.NAT_ECHORESPONSE, + response_payload, + ) + except ValueError as e: + print( + f"[Command Handler] Error sending ECHORESPONSE to client " + f"{client_address}: {e}" + ) + return + + print( + f"[Command Handler] Unhandled message id {header.iMessage} from " + f"registered client {client_address}." + ) + + def _find_client(self, ip: str, port: int) -> Client | None: + target = Client(ip, port) + with self.clients_lock: + for client in self.connected_clients: + if client == target: + return client + return None diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/schema/schema.usda b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/schema/schema.usda new file mode 100644 index 000000000..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/setup.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/setup.py new file mode 100644 index 000000000..4bf6d4f70 --- /dev/null +++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/setup.py @@ -0,0 +1,20 @@ +"""Isaac Sim extension install metadata for the OptiTrack NatNet emulator.""" + +import os + +from setuptools import find_packages, setup + +EXTENSION_PATH = os.path.dirname(os.path.realpath(__file__)) + +setup( + name="optitrack-natnet-emulator", + version="0.1.0", + description="NatNet UDP server emulator for Isaac Sim and natnet_ros2 integration", + license="MIT", + include_package_data=True, + python_requires=">=3.10", + install_requires=[], + packages=find_packages(where="."), + package_dir={"": "."}, + zip_safe=False, +) diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/conftest.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/conftest.py new file mode 100644 index 000000000..6a15e5183 --- /dev/null +++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/conftest.py @@ -0,0 +1,21 @@ +# Copyright (c) 2024 Carnegie Mellon University +# MIT License - see LICENSE in the repository root for full text. +"""Put the extension root on sys.path for direct pytest test/ runs. + +CI uses proxies under tests/sim/optitrack_natnet_emulator/ instead. +""" + +import sys +from pathlib import Path + +_EXT_ROOT = Path(__file__).resolve().parents[1] +if str(_EXT_ROOT) not in sys.path: + sys.path.insert(0, str(_EXT_ROOT)) + + +def pytest_configure(config): + # Mirror the `unit` marker from tests/pytest.ini so direct `pytest test/` + # runs don't emit PytestUnknownMarkWarning and `-m unit` works here too. + config.addinivalue_line( + "markers", "unit: Fast hermetic unit test (no Docker stack)." + ) diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/natnet_test_helpers.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/natnet_test_helpers.py new file mode 100644 index 000000000..07d6c8a5e --- /dev/null +++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/natnet_test_helpers.py @@ -0,0 +1,105 @@ +"""Shared helpers for optitrack.natnet.emulator unit tests.""" + +from __future__ import annotations + +import socket +import struct +import time +from contextlib import contextmanager + +from optitrack.natnet.emulator import NatNetUnicastServer, TransmissionType +from optitrack.natnet.emulator.server import natnet_server_types as st + + +def ephemeral_udp_port(host: str = "127.0.0.1") -> int: + """Return a free UDP port on *host* by binding and releasing a probe socket.""" + with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as probe: + probe.bind((host, 0)) + return probe.getsockname()[1] + + +class NatNetTestClient: + """Minimal UDP client for NatNet command-port protocol tests.""" + + def __init__(self, host: str = "127.0.0.1", timeout: float = 2.0) -> None: + self._host = host + self._sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + self._sock.bind((host, 0)) + self._sock.settimeout(timeout) + + @property + def local_port(self) -> int: + return self._sock.getsockname()[1] + + def send_message( + self, + server_port: int, + message_id: st.MessageId | int, + payload: bytes = b"", + server_host: str | None = None, + ) -> None: + header = st.sPacketHeader( + iMessage=int(message_id), + nDataBytes=len(payload), + ) + self.send_raw(header.pack() + payload, server_port, server_host) + + def send_raw( + self, + data: bytes, + server_port: int, + server_host: str | None = None, + ) -> None: + """Send a raw UDP datagram (for malformed / malicious packet tests).""" + self._sock.sendto(data, (server_host or self._host, server_port)) + + def send_header_only( + self, + server_port: int, + message_id: st.MessageId | int, + declared_payload_len: int, + server_host: str | None = None, + ) -> None: + """Send a header whose nDataBytes does not match any trailing payload.""" + header = struct.pack(" tuple[int, bytes, tuple[str, int]]: + data, addr = self._sock.recvfrom(65535) + message_id, payload_len = struct.unpack(" None: + self._sock.close() + + +@contextmanager +def running_unicast_server( + command_port: int | None = None, + local_interface: str = "127.0.0.1", + publish_rate: int = 100, +): + """Start NatNetUnicastServer on ephemeral (or fixed) command + data ports. + + Both ports are ephemeral by default so concurrent/sequential tests never + collide on the well-known 1510/1511 pair. + """ + port = command_port if command_port is not None else ephemeral_udp_port(local_interface) + data_port = ephemeral_udp_port(local_interface) + while data_port == port: + data_port = ephemeral_udp_port(local_interface) + server = NatNetUnicastServer( + local_interface=local_interface, + transmission_type=TransmissionType.UNICAST, + multicast_address=None, + command_port=port, + data_port=data_port, + ) + server.publish_rate = publish_rate + server.start() + time.sleep(0.05) + try: + yield server, port + finally: + server.shutdown() diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_catalog.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_catalog.py new file mode 100644 index 000000000..a3cc8cd34 --- /dev/null +++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_catalog.py @@ -0,0 +1,111 @@ +# Copyright (c) 2024 Carnegie Mellon University +# MIT License - see LICENSE in the repository root for full text. +"""Catalog builder: body counts, wire fidelity, truncation, MAX_MODELS, duplicate targets.""" + +from __future__ import annotations + +import struct + +import pytest + +from optitrack.natnet.emulator.isaac.catalog import build_catalog, find_duplicate_targets +from optitrack.natnet.emulator.isaac.config import BodyBinding, NatNetInterfaceConfig +from optitrack.natnet.emulator.server import natnet_model_types as mt +from optitrack.natnet.emulator.server.natnet_common import ModelLimits + +pytestmark = pytest.mark.unit + + +def _unpack_bodies(payload: bytes): + """Decode a packed sDataDescriptions into [(name, id, parentID), ...].""" + (n,) = struct.unpack_from("frame builder tests (no USD, no Kit).""" + +from __future__ import annotations + +import math +import struct + +import pytest + +from optitrack.natnet.emulator.isaac.frames import ( + MODEL_LIST_CHANGED, + TRACKING_VALID, + BodySample, + apply_pose_noise, + build_frame, + make_rigid_body_data, + to_motive_pose, +) + +pytestmark = pytest.mark.unit + + +def test_to_motive_pose_z_is_identity(): + pos = (1.0, 2.0, 3.0) + quat = (0.1, 0.2, 0.3, 0.9) + assert to_motive_pose(pos, quat, up_axis="Z") == (pos, quat) + # Case-insensitive and default to Z. + assert to_motive_pose(pos, quat, up_axis="z") == (pos, quat) + assert to_motive_pose(pos, quat) == (pos, quat) + + +def test_to_motive_pose_y_swaps_axes_and_quat(): + # (x, y, z) -> (x, z, -y); quat vector part takes the same swap, scalar kept. + pos, quat = to_motive_pose((1.0, 2.0, 3.0), (0.1, 0.2, 0.3, 0.9), up_axis="Y") + assert pos == (1.0, 3.0, -2.0) + assert quat == (0.1, 0.3, -0.2, 0.9) + + +def test_to_motive_pose_y_maps_isaac_up_to_motive_up(): + # Isaac +Z (up) must become Motive +Y (up) under the Y-up emulation. + pos, _ = to_motive_pose((0.0, 0.0, 1.0), (0.0, 0.0, 0.0, 1.0), up_axis="y") + assert pos == (0.0, 1.0, 0.0) + + +def test_make_rigid_body_data_copies_pose_and_sets_valid_bit(): + rb = make_rigid_body_data( + BodySample(7, (1.0, 2.0, 3.0), (0.0, 0.0, 0.7071068, 0.7071068), valid=True) + ) + assert rb.ID == 7 + assert (rb.x, rb.y, rb.z) == (1.0, 2.0, 3.0) + assert rb.qw == pytest.approx(0.7071068) + assert rb.params & TRACKING_VALID # client requires this bit or it skips the body + + +def test_lost_sample_clears_valid_bit_and_is_nan(): + rb = make_rigid_body_data(BodySample.lost(3)) + assert rb.ID == 3 + assert rb.params & TRACKING_VALID == 0 + assert math.isnan(rb.x) and math.isnan(rb.y) and math.isnan(rb.z) + + +def test_build_frame_no_bodies(): + frame = build_frame(0, []) + assert frame.iFrame == 0 + assert frame.nRigidBodies == 0 + assert frame.params == 0 + + +def test_apply_pose_noise_zero_std_is_identity(): + position = (1.0, 2.0, 3.0) + orientation = (0.0, 0.0, 0.0, 1.0) + pos_out, quat_out = apply_pose_noise(position, orientation, 0.0, 0.0) + assert pos_out == position + assert quat_out == pytest.approx(orientation) + + +def test_apply_pose_noise_preserves_y_position(): + np = pytest.importorskip("numpy") + np.random.seed(0) + position = (0.0, 1.5, 0.0) + orientation = (0.0, 0.0, 0.0, 1.0) + pos_out, _ = apply_pose_noise(position, orientation, 0.001, 0.0) + # Before the euler-yaw shadowing bug, y collapsed to ~0 instead of staying near 1.5. + assert pos_out[1] == pytest.approx(1.5, abs=0.01) + + +def test_apply_pose_noise_adds_position_jitter(): + np = pytest.importorskip("numpy") + np.random.seed(1) + position = (0.0, 0.0, 0.0) + orientation = (0.0, 0.0, 0.0, 1.0) + pos_out, _ = apply_pose_noise(position, orientation, 0.001, 0.0) + assert pos_out != position + + +def test_build_frame_multiple_bodies_preserve_order(): + samples = [ + BodySample(1, (1.0, 0.0, 0.0)), + BodySample(2, (0.0, 2.0, 0.0)), + BodySample(5, (0.0, 0.0, 3.0)), + ] + frame = build_frame(42, samples) + assert frame.iFrame == 42 + assert frame.nRigidBodies == 3 + assert frame.RigidBodies[0].ID == 1 and frame.RigidBodies[0].x == 1.0 + assert frame.RigidBodies[1].ID == 2 and frame.RigidBodies[1].y == 2.0 + assert frame.RigidBodies[2].ID == 5 and frame.RigidBodies[2].z == 3.0 + + +def test_model_list_changed_sets_frame_param_bit(): + assert build_frame(0, [], model_list_changed=True).params & MODEL_LIST_CHANGED + assert build_frame(0, [], model_list_changed=False).params & MODEL_LIST_CHANGED == 0 + + +def test_frame_packs_and_rigid_body_section_decodes(): + frame = build_frame(9, [BodySample(4, (1.5, -2.5, 3.5), (0.0, 0.0, 0.0, 1.0))]) + payload = frame.pack(natnet_major=4, natnet_minor=4) + + # iFrame, then 4.4 counted sections (count+size each) for markersets & other markers. + (iframe,) = struct.unpack_from(" author -> read is stable + author_interface(stage, "/World/NatNetInterface", cfg) + assert read_interface(find_interfaces(stage)[0]) == cfg + + +def test_reauthoring_removes_stale_bodies(): + stage = _new_stage() + author_interface(stage, "/World/NatNetInterface", _CONFIG) + + single = NatNetInterfaceConfig.from_dict( + {"bodies": [{"rigid_body_name": "Drone", "target_prim": "/World/base_link", "streaming_id": 1}]} + ) + author_interface(stage, "/World/NatNetInterface", single) + + cfg = read_interface(find_interfaces(stage)[0]) + assert [b.rigid_body_name for b in cfg.bodies] == ["Drone"] + + +def test_up_axis_authors_and_reads_back(): + stage = _new_stage() + # Default (absent) -> Z. + author_interface(stage, "/World/NatNetInterface", _CONFIG) + assert read_interface(find_interfaces(stage)[0]).up_axis == "Z" + + # Explicit Y survives the USD round trip. + cfg = NatNetInterfaceConfig.from_dict({**_CONFIG, "up_axis": "Y"}) + author_interface(stage, "/World/NatNetInterface", cfg) + assert read_interface(find_interfaces(stage)[0]).up_axis == "Y" + + +def test_pose_noise_authors_and_reads_back(): + stage = _new_stage() + cfg = NatNetInterfaceConfig.from_dict( + { + **_CONFIG, + "pose_noise_enabled": False, + "pose_noise_std_meters": 0.001, + "pose_noise_rotation_deg": 0.1, + } + ) + author_interface(stage, "/World/NatNetInterface", cfg) + read = read_interface(find_interfaces(stage)[0]) + assert read.pose_noise_enabled is False + assert read.pose_noise_std_meters == pytest.approx(0.001) + assert read.pose_noise_rotation_deg == pytest.approx(0.1) + + +def test_empty_target_round_trips(): + # The UI's "Add body" can create a body with no target yet (set later in the + # Property panel); it must author and read back cleanly with an empty target. + stage = _new_stage() + cfg = NatNetInterfaceConfig(bodies=[BodyBinding("Drone", "", 1)]) + author_interface(stage, "/World/NatNetInterface", cfg) + read = read_interface(find_interfaces(stage)[0]) + assert read.bodies[0].rigid_body_name == "Drone" + assert read.bodies[0].target_prim == "" + + +def test_invalid_config_raises_before_authoring(): + stage = _new_stage() + with pytest.raises(ValueError): + author_interface(stage, "/World/NatNetInterface", {"mode": "bogus"}) + assert find_interfaces(stage) == [] diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_interface_config.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_interface_config.py new file mode 100644 index 000000000..9247c5f12 --- /dev/null +++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_interface_config.py @@ -0,0 +1,186 @@ +# Copyright (c) 2024 Carnegie Mellon University +# MIT License - see LICENSE in the repository root for full text. +"""Hermetic unit tests for the pure-Python NatNet interface config model. + +No USD / Isaac imports — exercises dataclasses, dict normalization, the attribute +name builder, instance-key generation, and validation. +""" + +from __future__ import annotations + +import pytest + +from optitrack.natnet.emulator.isaac.config import ( + DEFAULT_POSE_NOISE_ENABLED, + DEFAULT_POSE_NOISE_ROTATION_DEG, + DEFAULT_POSE_NOISE_STD_METERS, + BodyBinding, + NatNetInterfaceConfig, + body_attr_name, + make_instance_key, +) + +pytestmark = pytest.mark.unit + + +def test_defaults_match_server_expectations(): + cfg = NatNetInterfaceConfig() + assert cfg.server_enabled is True + assert cfg.server_ip == "172.31.0.200" + assert cfg.mode == "unicast" + assert cfg.command_port == 1510 + assert cfg.data_port == 1511 + assert cfg.up_axis == "Z" # Isaac/USD native; matches the reference Motive setup + assert cfg.pose_noise_enabled is DEFAULT_POSE_NOISE_ENABLED + assert cfg.pose_noise_std_meters == DEFAULT_POSE_NOISE_STD_METERS + assert cfg.pose_noise_rotation_deg == DEFAULT_POSE_NOISE_ROTATION_DEG + assert cfg.bodies == [] + + +def test_up_axis_from_dict_normalizes_case(): + assert NatNetInterfaceConfig.from_dict({"up_axis": "y"}).up_axis == "Y" + assert NatNetInterfaceConfig.from_dict({"up_axis": "z"}).up_axis == "Z" + # Absent -> default Z. + assert NatNetInterfaceConfig.from_dict({}).up_axis == "Z" + + +def test_up_axis_survives_round_trip(): + cfg = NatNetInterfaceConfig.from_dict({"up_axis": "Y"}) + assert NatNetInterfaceConfig.from_dict(cfg.to_dict()).up_axis == "Y" + + +def test_pose_noise_survives_round_trip(): + cfg = NatNetInterfaceConfig.from_dict( + { + "pose_noise_enabled": False, + "pose_noise_std_meters": 0.001, + "pose_noise_rotation_deg": 0.1, + } + ) + restored = NatNetInterfaceConfig.from_dict(cfg.to_dict()) + assert restored.pose_noise_enabled is False + assert restored.pose_noise_std_meters == 0.001 + assert restored.pose_noise_rotation_deg == 0.1 + + +def test_from_dict_with_bodies_as_list(): + cfg = NatNetInterfaceConfig.from_dict( + { + "server_ip": "10.0.0.5", + "bodies": [ + {"rigid_body_name": "Drone", "target_prim": "/World/base_link", "streaming_id": 1}, + ], + } + ) + assert cfg.server_ip == "10.0.0.5" + assert len(cfg.bodies) == 1 + assert cfg.bodies[0] == BodyBinding("Drone", "/World/base_link", 1, -1) + + +def test_from_dict_with_bodies_as_prim_mapping(): + # The "dictionary of prims -> rigid body names and stuff" form. + cfg = NatNetInterfaceConfig.from_dict( + { + "bodies": { + "/World/base_link": {"rigid_body_name": "Drone", "streaming_id": 1}, + "/World/target": {"rigid_body_name": "Target", "streaming_id": 2}, + } + } + ) + by_name = {b.rigid_body_name: b for b in cfg.bodies} + assert by_name["Drone"].target_prim == "/World/base_link" + assert by_name["Target"].target_prim == "/World/target" + assert by_name["Target"].streaming_id == 2 + + +def test_to_dict_round_trip(): + cfg = NatNetInterfaceConfig.from_dict( + { + "mode": "multicast", + "publish_rate": 120, + "bodies": [{"rigid_body_name": "Drone", "target_prim": "/World/base_link"}], + } + ) + restored = NatNetInterfaceConfig.from_dict(cfg.to_dict()) + assert restored == cfg + + +def test_body_from_dict_requires_target_and_name(): + with pytest.raises(ValueError): + BodyBinding.from_dict({"rigid_body_name": "Drone"}) # no target_prim + with pytest.raises(ValueError): + BodyBinding.from_dict({"target_prim": "/World/base_link"}) # no name + + +def test_body_attr_name_builder(): + assert body_attr_name("Drone", "streamingId") == "natnet:body:Drone:streamingId" + + +def test_make_instance_key_sanitizes_and_dedupes(): + used: set[str] = set() + assert make_instance_key("Drone 1", used) == "Drone_1" + # collision after sanitization -> numeric suffix + assert make_instance_key("Drone-1", used) == "Drone_1_1" + # leading digit gets a safe prefix + assert make_instance_key("3PO", used).startswith("b_") + + +def test_assign_instance_keys_are_unique(): + cfg = NatNetInterfaceConfig( + bodies=[ + BodyBinding("Drone", "/World/a", 1), + BodyBinding("Drone", "/World/b", 2), # duplicate display name + ] + ) + keys = [k for k, _ in cfg.assign_instance_keys()] + assert len(set(keys)) == 2 + + +@pytest.mark.parametrize( + "overrides", + [ + {"mode": "bogus"}, + {"command_port": 0}, + {"data_port": 70000}, + {"command_port": 1510, "data_port": 1510}, + {"publish_rate": 0}, + {"up_axis": "X"}, + {"up_axis": "bogus"}, + {"pose_noise_std_meters": -0.001}, + {"pose_noise_rotation_deg": -0.1}, + ], +) +def test_validate_rejects_bad_server_config(overrides): + cfg = NatNetInterfaceConfig(**overrides) + with pytest.raises(ValueError): + cfg.validate() + + +def test_validate_rejects_duplicate_streaming_ids(): + cfg = NatNetInterfaceConfig( + bodies=[ + BodyBinding("A", "/World/a", 1), + BodyBinding("B", "/World/b", 1), + ] + ) + with pytest.raises(ValueError): + cfg.validate() + + +def test_validate_rejects_blank_rigid_body_name(): + cfg = NatNetInterfaceConfig(bodies=[BodyBinding("", "/World/a", 1)]) + with pytest.raises(ValueError): + cfg.validate() + + +def test_validate_allows_empty_target(): + # An empty target is valid: a freshly added body to be pointed in the UI/Property panel. + cfg = NatNetInterfaceConfig(bodies=[BodyBinding("Drone", "", 1)]) + assert cfg.validate() is cfg + + +def test_validate_accepts_good_config(): + cfg = NatNetInterfaceConfig( + bodies=[BodyBinding("Drone", "/World/base_link", 1)] + ) + assert cfg.validate() is cfg diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_pose_sampling.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_pose_sampling.py new file mode 100644 index 000000000..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..f93d761ab --- /dev/null +++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_pose_streaming.py @@ -0,0 +1,84 @@ +# Copyright (c) 2024 Carnegie Mellon University +# MIT License - see LICENSE in the repository root for full text. +"""Loopback: sample_once on a USD prim → NAT_FRAMEOFDATA with sampled position. + +Real server + UDP sockets + in-memory stage. Requires pxr. +""" + +from __future__ import annotations + +import socket +import struct +import time + +import pytest + +pytest.importorskip("pxr") + +from pxr import Gf, Usd, UsdGeom # noqa: E402 + +from natnet_test_helpers import NatNetTestClient, ephemeral_udp_port # noqa: E402 + +from optitrack.natnet.emulator.isaac.config import BodyBinding, NatNetInterfaceConfig # noqa: E402 +from optitrack.natnet.emulator.isaac.manager import NatNetServerManager # noqa: E402 +from optitrack.natnet.emulator.isaac.usd_bindings import author_interface # noqa: E402 +from optitrack.natnet.emulator.server import natnet_server_types as st # noqa: E402 + +pytestmark = pytest.mark.unit + + +def _decode_first_rigid_body(payload: bytes): + # iFrame(4) + markersets(count4+size4) + othermarkers(count4+size4) = 20, then + # rigid bodies: count(4) + size(4) at 20, first body at 28: id + xyz. + rb_count, _rb_size = struct.unpack_from(" server only; real Motive sends no reply. An echo + # reply makes libNatNet log "Received unrecognized message Message=10". + with running_unicast_server() as (server, command_port): + client = NatNetTestClient(timeout=0.5) + try: + client.send_message(command_port, st.MessageId.NAT_CONNECT) + client.recv_message() + + client.send_message(command_port, st.MessageId.NAT_KEEPALIVE) + with pytest.raises(socket.timeout): + client.recv_message() + + # Client stays registered and keeps receiving frames. + assert len(server.connected_clients) == 1 + finally: + client.close() + + +# ============================================================================= +# Malformed datagrams — registered client & recovery +# ============================================================================= + + +def test_unknown_message_from_registered_client_gets_no_reply(): + with running_unicast_server() as (server, command_port): + client = NatNetTestClient(timeout=0.5) + try: + client.send_message(command_port, st.MessageId.NAT_CONNECT) + client.recv_message() + + client.send_message(command_port, 999) + with pytest.raises(socket.timeout): + client.recv_message() + + assert len(server.connected_clients) == 1 + finally: + client.close() + + +def test_server_survives_malformed_burst_then_valid_connect(): + with running_unicast_server() as (server, command_port): + client = NatNetTestClient(timeout=2.0) + try: + client.send_raw(b"", command_port) + client.send_raw(b"\xff", command_port) + client.send_header_only(command_port, 999, declared_payload_len=50000) + client.send_message(command_port, st.MessageId.NAT_REQUEST_MODELDEF) + + client.send_message(command_port, st.MessageId.NAT_CONNECT) + message_id, _payload, _addr = client.recv_message() + finally: + client.close() + + assert message_id == int(st.MessageId.NAT_SERVERINFO) + assert len(server.connected_clients) == 1 diff --git a/simulation/isaac-sim/launch_scripts/example_multi_px4_pegasus_natnet_launch_script.py b/simulation/isaac-sim/launch_scripts/example_multi_px4_pegasus_natnet_launch_script.py new file mode 100644 index 000000000..12b4799d1 --- /dev/null +++ b/simulation/isaac-sim/launch_scripts/example_multi_px4_pegasus_natnet_launch_script.py @@ -0,0 +1,256 @@ +#!/usr/bin/env python +""" +Multi-drone PX4 Pegasus launcher with OptiTrack NatNet mocap streaming. + +Same scene prep and sensor stack as ``example_multi_px4_pegasus_launch_script.py``, +plus a Motive-compatible NatNet server that always streams one rigid body per drone +and a shared static ``Target`` (id 100) at ``/World/target``. + +Body naming: + - ``NUM_ROBOTS=1``: drone body ``Drone`` (id 1) + - ``NUM_ROBOTS>1``: ``Drone1``, ``Drone2``, … (ids 1..N) + +Intended multi-robot profile pairing (see ``natnet_config.yaml`` commented scaffolding): + - ``robot_1``: tracks its drone + the shared ``Target`` + - ``robot_2``: tracks its drone + the shared ``Target`` + - ``robot_3``: tracks its drone only (no Target in profile) + +Set ``NUM_ROBOTS=3`` on both sim and robot stacks; each container picks its profile +via ``ROBOT_NAME``. + +Env: + - ``NUM_ROBOTS`` (default 1) + - ``ENABLE_LIDAR`` (default false) + - ``PLAY_SIM_ON_START`` (default true) + - ``NATNET_BODY_NAME`` / ``NATNET_TARGET_NAME`` (optional name overrides) +""" + +import asyncio +import os +import sys +import time + +import carb +from isaacsim import SimulationApp + +_headless = os.environ.get("ISAAC_SIM_HEADLESS", "false").lower() == "true" +simulation_app = SimulationApp({"headless": _headless}) + +import omni.kit.app +import omni.timeline +import omni.usd + +from omni.isaac.core.world import World + +from pegasus.simulator.params import SIMULATION_ENVIRONMENTS +from pegasus.simulator.logic.interface.pegasus_interface import PegasusInterface +from pegasus.simulator.ogn.api.spawn_multirotor import spawn_px4_multirotor_node +from pegasus.simulator.ogn.api.spawn_zed_camera import add_zed_stereo_camera_subgraph +from pegasus.simulator.ogn.api.spawn_rtx_lidar import add_rtx_lidar_subgraph + +sys.path.insert(0, os.path.normpath(os.path.join(os.path.dirname(os.path.abspath(__file__)), "..", "utils"))) +from scene_prep import scale_stage_prim, add_colliders, add_dome_light, save_scene_as_contained_usd + +from optitrack.natnet.emulator.isaac import ( + DEFAULT_TARGET_PATH, + DEFAULT_TARGET_POSITION, + DEFAULT_TARGET_STREAMING_ID, + author_static_target, + start_drone_natnet_server, +) + +# --------------------- CONFIGURATION --------------------- +ENV_URL = SIMULATION_ENVIRONMENTS["Default Environment"] +STAGE_SCALE = 1.0 +SAVE_SCENE_TO = None +DRONE_USD = "~/.local/share/ov/data/documents/Kit/shared/exts/pegasus.simulator/pegasus/simulator/assets/Robots/Iris/iris.usd" + +NUM_ROBOTS = int(os.environ.get("NUM_ROBOTS", "1")) +ENABLE_LIDAR = os.environ.get("ENABLE_LIDAR", "false").lower() == "true" +NATNET_BODY_NAME = os.environ.get("NATNET_BODY_NAME", "Drone") +NATNET_TARGET_NAME = os.environ.get("NATNET_TARGET_NAME", "Target") + +_NATNET_SERVER_KWARGS = { + "pose_noise_enabled": True, + "pose_noise_std_meters": 0.0005, + "pose_noise_rotation_deg": 0.05, +} +# --------------------------------------------------------- + + +ext_manager = omni.kit.app.get_app().get_extension_manager() +for ext in [ + "omni.graph.core", + "omni.graph.action", + "omni.graph.action_nodes", + "isaacsim.core.nodes", + "omni.graph.ui", + "omni.graph.visualization.nodes", + "omni.graph.scriptnode", + "omni.graph.window.action", + "omni.graph.window.generic", + "omni.graph.ui_nodes", + "pegasus.simulator", +]: + if not ext_manager.is_extension_enabled(ext): + ext_manager.set_extension_enabled_immediate(ext, True) + + +def wait_for_stage(stage, timeout_s: float = 10.0): + for _ in range(int(timeout_s / 0.1)): + omni.kit.app.get_app().update() + world_prim = stage.GetPrimAtPath("/World") + if world_prim.IsValid(): + non_physics = [c for c in world_prim.GetChildren() if c.GetName() != "PhysicsScene"] + if non_physics: + return True + time.sleep(0.1) + return False + + +def _drone_body_name(index: int) -> str: + """Single agent uses bare ``Drone``; multi uses ``Drone1``, ``Drone2``, …""" + return NATNET_BODY_NAME if NUM_ROBOTS == 1 else f"{NATNET_BODY_NAME}{index}" + + +def spawn_drone(index: int): + robot_name = f"robot_{index}" + drone_prim = f"/World/drone{index}/base_link" + init_x = 2.0 * (index - 1) - 2.0 * (NUM_ROBOTS - 1) / 2.0 + + graph_handle = spawn_px4_multirotor_node( + pegasus_node_name=f"PX4Multirotor_{index}", + drone_prim=drone_prim, + robot_name=robot_name, + vehicle_id=index, + domain_id=index, + usd_file=DRONE_USD, + init_pos=[init_x, 0.0, 0.07], + init_orient=[0.0, 0.0, 0.0, 1.0], + ) + + add_zed_stereo_camera_subgraph( + parent_graph_handle=graph_handle, + drone_prim=drone_prim, + robot_name=robot_name, + camera_name="ZEDCamera", + camera_offset=[0.2, 0.0, -0.05], + camera_rotation_offset=[0.0, 0.0, 0.0], + ) + + if ENABLE_LIDAR: + add_rtx_lidar_subgraph( + parent_graph_handle=graph_handle, + drone_prim=drone_prim, + robot_name=robot_name, + lidar_config="ouster_os1", + lidar_topic_name="point_cloud_raw", + lidar_offset=[0.0, 0.0, 0.025], + lidar_rotation_offset=[0.0, 0.0, 0.0], + min_range=0.75, + ) + + +class PegasusApp: + + def __init__(self): + self.timeline = omni.timeline.get_timeline_interface() + self.natnet_manager = None + + self.pg = PegasusInterface() + self.pg._world = World(**self.pg._world_settings) + self.world = self.pg.world + self.timeline.stop() + + self.pg.load_environment(ENV_URL) + + stage = omni.usd.get_context().get_stage() + if stage is None: + raise RuntimeError("Stage failed to load") + + if not wait_for_stage(stage): + carb.log_warn("Stage load timed out — continuing anyway.") + + stage_prim = stage.GetPrimAtPath("/World/stage") + if stage_prim.IsValid(): + scale_stage_prim(stage, "/World/stage", STAGE_SCALE) + add_colliders(stage_prim) + for _ in range(10): + omni.kit.app.get_app().update() + else: + carb.log_warn("/World/stage not found — skipping scale and collision.") + + add_dome_light(stage) + + if SAVE_SCENE_TO: + import tempfile + tmp_usd = os.path.join(tempfile.gettempdir(), "prepared_scene.usd") + success, error = asyncio.get_event_loop().run_until_complete( + omni.usd.get_context().export_as_stage_async(tmp_usd) + ) + if success: + os.makedirs(SAVE_SCENE_TO, exist_ok=True) + save_scene_as_contained_usd(tmp_usd, SAVE_SCENE_TO) + os.remove(tmp_usd) + else: + carb.log_error(f"Scene export failed: {error}") + + print(f"[example_multi_natnet] Spawning {NUM_ROBOTS} drone(s), lidar={'on' if ENABLE_LIDAR else 'off'}") + for i in range(1, NUM_ROBOTS + 1): + spawn_drone(i) + + self._setup_natnet(stage) + self.play_on_start = os.environ.get("PLAY_SIM_ON_START", "true").lower() == "true" + + def _setup_natnet(self, stage): + """Author NatNet bodies: one per drone plus one shared static target.""" + try: + author_static_target(stage, DEFAULT_TARGET_PATH, DEFAULT_TARGET_POSITION) + bodies = [ + (_drone_body_name(i), i, f"/World/drone{i}/base_link/body") + for i in range(1, NUM_ROBOTS + 1) + ] + bodies.append((NATNET_TARGET_NAME, DEFAULT_TARGET_STREAMING_ID, DEFAULT_TARGET_PATH)) + + self.natnet_manager = start_drone_natnet_server( + stage, bodies, **_NATNET_SERVER_KWARGS + ) + carb.log_warn( + f"[natnet] Emulator started with {NUM_ROBOTS} drone body(ies) " + f"and shared target '{NATNET_TARGET_NAME}' (robot_1/robot_2 subscribe via " + f"natnet_config; robot_3 omits Target)." + ) + except Exception as exc: # noqa: BLE001 - never let NatNet kill the sim + carb.log_error(f"[natnet] Failed to start emulator: {exc}") + self.natnet_manager = None + + def run(self): + if self.play_on_start: + self.timeline.play() + else: + self.timeline.stop() + + app = omni.kit.app.get_app() + while simulation_app.is_running(): + world = World.instance() + if world is not None and hasattr(world, '_scene'): + world.step(render=True) + if world is not self.world: + self.world = world + self.pg._world = world + else: + app.update() + + carb.log_warn("Closing simulation.") + if self.natnet_manager is not None: + self.natnet_manager.on_shutdown() + self.timeline.stop() + simulation_app.close() + + +def main(): + PegasusApp().run() + + +if __name__ == "__main__": + main() diff --git a/simulation/isaac-sim/launch_scripts/example_one_px4_pegasus_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/README.md b/tests/README.md index f10942ff7..93d61a067 100644 --- a/tests/README.md +++ b/tests/README.md @@ -1,10 +1,11 @@ # Testing (`tests/`) -AirStack's **pytest** tree under `tests/` has three roles: +AirStack's **pytest** tree under `tests/` has four roles: 1. **`tests/system/`** — Docker stack tests (sim + robot + GCS): liveliness, sensor Hz, takeoff/hover/land, image/workspace builds. -2. **`tests/robot/`** — Fast **unit** tests that mirror `robot/ros_ws/src/` (`behavior`, `global`, `interface`, `local`, `perception`, `sensors`). Mark: `unit`. -3. **`tests/sim/`** — Unit tests for simulation-side helpers (e.g. Motive / NatNet emulator). Mark: `unit`. +2. **`tests/integration/`** — Cross-component tests that need the robot container plus a host-side component, but **no sim/GPU** (e.g. the NatNet emulator). Mark: `integration`. +3. **`tests/robot/`** — Fast **unit** tests that mirror `robot/ros_ws/src/` (`behavior`, `global`, `interface`, `local`, `perception`, `sensors`). Mark: `unit`. +4. **`tests/sim/`** — Unit tests for simulation-side helpers (e.g. NatNet emulator). Mark: `unit`. Shared fixtures live in `tests/conftest.py`. Use `airstack test -m unit -v` for hermetic tests only, or the marks below for the full stack. @@ -24,14 +25,33 @@ Shared fixtures live in `tests/conftest.py`. Use `airstack test -m unit -v` for | [`system/test_sensors.py`](system/test_sensors.py) | `sensors` | After liveliness in collection order: sim + robot stereo/depth Hz (**Isaac:** batched ``ros2 topic hz`` to avoid bridge overload; **ms-airsim:** single batch), filtered LiDAR via ``echo --once`` + cloud sanity (isaacsim), sim RTF, ``test_sensor_streams_stable`` | Docker daemon, GPU, sim license | | [`system/test_takeoff_hover_land.py`](system/test_takeoff_hover_land.py) | `takeoff_hover_land` | End-to-end flight: PX4 readiness gate, takeoff to 10 m, hover stability, land — one chain per (sim, num_robots, iteration, velocity) | Docker daemon, GPU, sim license | +### Integration tests (`tests/integration/`) + +The **integration** tier wires a few real components together — typically the +robot autonomy container plus a host-side component — without a simulator or GPU. +See [`integration/README.md`](integration/README.md). + +| Scenario | Mark(s) | What it tests | Hardware required | +|----------|---------|---------------|-------------------| +| [`integration/natnet/`](integration/natnet/) | `integration` | Host NatNet emulator → `natnet_ros2` → `/{ROBOT_NAME}/perception/optitrack/Drone` Hz | Docker daemon (no GPU/sim); runs after `build_docker` / `build_packages` in collection order | + +The shared `robot_autonomy_stack` fixture (in [`conftest.py`](conftest.py)) +reuses a running `robot-desktop` container, or brings one up automatically and tears it down after the module. + +```bash +# Reuse a running container, or let the harness bring one up: +pytest tests/integration/ -m integration -v +``` + ### Unit tests (`tests/robot/`, `tests/sim/`) Hermetic tests use `@pytest.mark.unit` (see [`pytest.ini`](pytest.ini)). **Co-location + proxy pattern:** test source lives alongside its ROS 2 package at `robot/ros_ws/src///test/test_*.py` (the ROS 2 / colcon convention). -Files in `tests/robot/` are thin proxies that re-export those tests so that -`pytest tests/` discovers them. Both `airstack test -m unit` and +Files in `tests/robot/` are thin proxies that call ``register_unit_tests()`` (in +[`conftest.py`](conftest.py)) to register co-located tests so ``pytest tests/`` +discovers them. Both `airstack test -m unit` and `colcon test --packages-select ` run the same test source. Example: `robot/ros_ws/src/sensors/lidar_point_cloud_filter/test/test_validation_core.py` diff --git a/tests/colcon_unit_test_packages.yaml b/tests/colcon_unit_test_packages.yaml index 5e0bd0ebb..54f35c9ce 100644 --- a/tests/colcon_unit_test_packages.yaml +++ b/tests/colcon_unit_test_packages.yaml @@ -9,5 +9,4 @@ robot: packages: - natnet_ros2 - lidar_point_cloud_filter - # Skips ament_copyright / flake8 / pep257 on Python packages; run linters separately. pytest_args: "-m not linter" diff --git a/tests/conftest.py b/tests/conftest.py index 31fd29076..7e0484e32 100644 --- a/tests/conftest.py +++ b/tests/conftest.py @@ -1,3 +1,4 @@ +import importlib.util import json import logging import os @@ -36,11 +37,17 @@ "robot_setup_bash": "/root/AirStack/robot/ros_ws/install/setup.bash", "extra_env": { "ISAAC_SIM_USE_STANDALONE": "true", - "ISAAC_SIM_SCRIPT_NAME": "example_multi_px4_pegasus_launch_script.py", + # Use the NatNet launch script so test_natnet_pose_alive always runs. + # It is a superset of the plain script: same drones + NatNet emulator. + "ISAAC_SIM_SCRIPT_NAME": "example_multi_px4_pegasus_natnet_launch_script.py", "PLAY_SIM_ON_START": "true", # Multi script gates RTX LiDAR on this flag; example_one always spawns it. # `sensors` tests expect ouster topics + lidar_point_cloud_filter path. "ENABLE_LIDAR": "true", + # Always enable the robot-side NatNet stack for Isaac Sim tests so that + # test_natnet_pose_alive runs regardless of the user's local .env setting. + "LAUNCH_NATNET": "true", + "SITL_PARAM_PROFILE": "px4-vision", }, }, } @@ -51,6 +58,62 @@ ) +def repo_path(*parts: str) -> Path: + """Resolve a path relative to the repository root. + + ``AIRSTACK_ROOT`` is exported by CI and defaults to the repo root locally, + so this is the single source of truth for cross-tree paths — no test or + proxy file should hardcode ``Path(__file__).parents[N]`` walks. + """ + return Path(AIRSTACK_ROOT).joinpath(*parts) + + +def register_unit_tests(target_globals: dict, test_dir: Path, *module_files: str) -> None: + """Register co-located unit tests with a thin proxy module under ``tests/``. + + AirStack keeps unit test **source** next to each package (colcon convention), + but CI runs pytest tests/. Each proxy file calls this helper so pytest + discovers the real tests. + + For each module_file in test_dir: + + 1. Prepend test_dir and test_dir.parent (package/extension root) to + sys.path so the loaded module can import production code and sibling + helpers (e.g. natnet_test_helpers). + 2. Load the file with importlib.util.spec_from_file_location under a + synthetic module name (_unit__) to avoid circular + imports when proxy and source share the same basename. + 3. Copy every test_* callable from the loaded module into + target_globals (typically the proxy's globals()) so pytest + collects them as if they lived under tests/. + 4. Wrap each callable with pytest.mark.unit so -m unit works even + when the source module omits pytestmark. + + Args: + target_globals: Namespace to populate (pass globals() from the proxy). + test_dir: Directory containing co-located test modules + (e.g. repo_path("robot/ros_ws/src//test")). + *module_files: One or more filenames under test_dir; multiple files + may be folded into a single proxy. + + pytest /test/ or colcon test bypasses proxies; use a local conftest.py in the package test/ dir for sys.path setup instead. + """ + for path in (test_dir, test_dir.parent): + entry = str(path) + if entry not in sys.path: + sys.path.insert(0, entry) + prefix = re.sub(r"\W", "_", test_dir.parent.name) + for module_file in module_files: + real_file = test_dir / module_file + module_name = f"_unit_{prefix}_{Path(module_file).stem}" + spec = importlib.util.spec_from_file_location(module_name, real_file) + module = importlib.util.module_from_spec(spec) + spec.loader.exec_module(module) + for name in dir(module): + if name.startswith("test_"): + target_globals[name] = pytest.mark.unit(getattr(module, name)) + + def load_colcon_unit_test_config(workspace="robot"): """Load colcon test package list and pytest args from tests/colcon_unit_test_packages.yaml.""" if not COLCON_UNIT_TEST_PACKAGES_YAML.is_file(): @@ -76,17 +139,21 @@ def colcon_test_robot_command(workspace="robot"): """Shell command for colcon test over unit-test packages (robot workspace).""" packages, pytest_args = load_colcon_unit_test_config(workspace) pkg_list = " ".join(packages) + ros_plugin_disable = ( + "PYTEST_ADDOPTS='-p no:launch_testing -p no:launch_testing_ros' " + ) cmd = ( - f"colcon test --packages-select {pkg_list} " + ros_plugin_disable + + f"colcon test --packages-select {pkg_list} " "--event-handlers console_direct+ --return-code-on-test-failure" ) if pytest_args: cmd += f' --pytest-args "{pytest_args}"' return cmd # Unit tests live co-located with their ROS 2 packages in robot/ros_ws/src/. -# Thin proxy files under tests/robot/ re-export those tests so that -# `pytest tests/` and `airstack test -m unit` discover them without any -# sys.path manipulation here. Each proxy file sets up its own paths. +# Thin proxy files under tests/robot/ (etc.) call register_unit_tests() so +# `pytest tests/` and `airstack test -m unit` discover them without sys.path +# boilerplate in this root conftest. RUN_DIR = None LOGS_DIR = None ROS_DISTRO_SETUP = "/opt/ros/jazzy/setup.bash" @@ -124,13 +191,25 @@ def pytest_addoption(parser): "sweep in test_takeoff_hover_land. Default: 0.5,1,2") +def _chmod_world_writable(path: Path) -> None: + """Best-effort: allow Docker and host users to share tests/results.""" + try: + path.chmod(0o777) + except OSError: + pass + + def pytest_configure(config): global RUN_DIR, LOGS_DIR timestamp = datetime.now().strftime("%Y-%m-%d_%H-%M-%S") results_root = Path(AIRSTACK_ROOT) / "tests" / "results" + results_root.mkdir(parents=True, exist_ok=True) + _chmod_world_writable(results_root) RUN_DIR = results_root / timestamp LOGS_DIR = RUN_DIR / "logs" LOGS_DIR.mkdir(parents=True, exist_ok=True) + _chmod_world_writable(RUN_DIR) + _chmod_world_writable(LOGS_DIR) config.option.xmlpath = str(RUN_DIR / "results.xml") @@ -196,16 +275,17 @@ def pytest_generate_tests(metafunc): # Run cheap/fast-fail tests first so real problems surface early: -# docker image builds → colcon workspace builds → liveliness (infra) → sensors -# (ROS topic streams) → autonomy flight tests. +# unit → docker image builds → colcon builds → integration (robot container) → +# liveliness (full stack infra) → sensors → autonomy flight tests. _MODULE_ORDER = [ # Unit tests first — fast, hermetic, no Docker. Any module whose dotted # name starts with "robot." or "sim." is a proxy for a package-level unit # test and sorts into this leading slot via the prefix check below. "__unit__", - # System tests follow in dependency order. "system.test_build_docker", "system.test_build_packages", + # Integration needs robot-desktop image + colcon-built packages (e.g. natnet_ros2). + "__integration__", "system.test_liveliness", "system.test_sensors", "system.test_takeoff_hover_land", @@ -228,12 +308,14 @@ def _rank(name, order): def _module_key(item): """Return the ordering key for an item. - Unit-test proxies live under ``robot/``, ``sim/``, or ``gcs/`` and are - identified by their nodeid prefix. Everything else uses the dotted module - ``__name__`` looked up against ``_MODULE_ORDER``. + Unit-test proxies live under robot/, sim/, or gcs/. + Integration tests live under integration/. + Everything else uses the dotted module __name__ against _MODULE_ORDER. """ if item.nodeid.startswith(("robot/", "sim/", "gcs/")): return _rank("__unit__", _MODULE_ORDER) + if item.nodeid.startswith("integration/"): + return _rank("__integration__", _MODULE_ORDER) return _rank(getattr(item.module, "__name__", ""), _MODULE_ORDER) @@ -309,13 +391,26 @@ def current_log(): return _nodeid_dotted(_CURRENT_ITEM.nodeid, with_path_sep=True) +# Matches any ANSI escape sequence (color codes, cursor moves, etc.) +_ANSI_RE = re.compile(r'\x1b(?:[@-Z\\-_]|\[[0-?]*[ -/]*[@-~])') + + def read_log_tail(log_name=None, lines=50): + """Return the last *lines* lines of a test log file, cleaned up for display. + + Strips ANSI escape codes and normalises carriage-return progress lines + (e.g. Docker build context updates) so the result renders as plain text + with real newline characters. + """ log_name = log_name or current_log() if not log_name: return "" log_path = LOGS_DIR / f"{log_name}.log" if log_path.exists(): - all_lines = log_path.read_text().splitlines() + text = log_path.read_text() + text = _ANSI_RE.sub('', text) # remove colour codes + text = text.replace('\r\n', '\n').replace('\r', '\n') # normalise CR + all_lines = text.splitlines() return "\n".join(all_lines[-lines:]) return "" @@ -776,8 +871,8 @@ def airstack_env(request): up_cmd_duration_s = round(time.time() - t0, 2) logger.info("airstack up returned %d in %.2fs", up_result.returncode, up_cmd_duration_s) - assert up_result.returncode == 0, \ - f"airstack up failed:\n{read_log_tail(log)}" + if up_result.returncode != 0: + pytest.fail(f"airstack up failed:\n{read_log_tail(log)}") env = { "sim": sim, @@ -802,4 +897,50 @@ def airstack_env(request): airstack_cmd("down", timeout=120, log_name=log) down_duration_s = round(time.time() - t3, 2) logger.info("Teardown finished in %.2fs", down_duration_s) - m.record(tid, "airstack_down_duration_s", down_duration_s, unit="s") \ No newline at end of file + m.record(tid, "airstack_down_duration_s", down_duration_s, unit="s") + + +# ── integration tier (tests/integration/) ───────────────────────────────── + +_INTEGRATION_ROBOT_PATTERN = "robot.*desktop" +# Robot-only bring-up: autonomy stack on, no sim profile, single robot. +_INTEGRATION_ENV = { + "AUTOLAUNCH": "true", + "NUM_ROBOTS": "1", + "COMPOSE_PROFILES": "desktop", +} + + +@pytest.fixture(scope="module") +def robot_autonomy_stack(request): + """Robot-desktop container for integration tests (no sim, no GPU). + + Yields ``{"container": , "brought_up": bool}``. Reuses an already + running container (fast local iteration, left running afterward); otherwise + runs ``airstack up robot-desktop`` and tears it down after the module. + Behaves like the ``build_packages`` fixture — always brings up Docker when + no container is found. + """ + existing = find_container(_INTEGRATION_ROBOT_PATTERN) + if existing and container_running(existing): + yield {"container": existing, "brought_up": False} + return + + log = "robot_autonomy_stack" + with logger_to(log): + missing = missing_images(env=_INTEGRATION_ENV) + if missing: + pytest.skip("robot-desktop image not built locally: " + ", ".join(missing)) + airstack_cmd("down", timeout=120, log_name=log) + result = airstack_cmd("up", "robot-desktop", + env_overrides=_INTEGRATION_ENV, timeout=180, log_name=log) + if result.returncode != 0: + pytest.fail(f"`airstack up robot-desktop` failed:\n{read_log_tail(log)}") + + container = wait_for_container(_INTEGRATION_ROBOT_PATTERN, timeout=120) + assert container, "robot-desktop container not Running after 120s" + try: + yield {"container": container, "brought_up": True} + finally: + with logger_to(log): + airstack_cmd("down", timeout=120, log_name=log) \ No newline at end of file diff --git a/tests/docker/docker-compose.yaml b/tests/docker/docker-compose.yaml index 4e392d284..841788917 100644 --- a/tests/docker/docker-compose.yaml +++ b/tests/docker/docker-compose.yaml @@ -13,5 +13,7 @@ services: environment: - AIRSTACK_ROOT=${AIRSTACK_PATH} - DISPLAY=${DISPLAY:-} + - LAUNCH_NATNET=${LAUNCH_NATNET:-false} + - NATNET_BODY_NAME=${NATNET_BODY_NAME:-Drone} working_dir: ${AIRSTACK_PATH}/tests network_mode: host diff --git a/tests/integration/README.md b/tests/integration/README.md new file mode 100644 index 000000000..e703403c1 --- /dev/null +++ b/tests/integration/README.md @@ -0,0 +1,55 @@ +# Integration tests (`tests/integration/`) + +The **integration** tier sits between **unit** and **system**: + +| Tier | Lives in | Brings up | Hardware | Mark | +|------|----------|-----------|----------|------| +| unit | `/test/` + proxies in `tests/robot/`, `tests/sim/`, `tests/gcs/` | nothing | none | `unit` | +| **integration** | **`tests/integration//`** | **robot container + a host-side component** | **Docker (no sim/GPU)** | **`integration`** | +| system | `tests/system/` | full sim + robot + GCS | Docker + GPU + sim license | `liveliness`, `sensors`, `takeoff_hover_land` | + +An integration test wires a few **real** components together — for example the +robot autonomy stack plus a host-side NatNet server — without using a full simulator or GPU. + +**Collection order** (see `pytest_collection_modifyitems` in [`../conftest.py`](../conftest.py)): + +## The harness: `robot_autonomy_stack` + +The shared bring-up fixture lives in the root [`../conftest.py`](../conftest.py) +(alongside `airstack_env`, matching the repo convention that fixtures are +defined there). Request it instead of hand-rolling `airstack up`: + +- **Reuses** an already-running `robot-desktop` container (fast local iteration; + left running afterward). +- Otherwise brings one up (`airstack up robot-desktop`, autonomy on, no sim) + and tears it down after. + +## Running + +```bash +# Reuse a container you already started: +AUTOLAUNCH=false airstack up robot-desktop +pytest tests/integration/ -m integration -v + +# One scenario by path: +pytest tests/integration/natnet/ -m integration -v + +# Or let the harness bring the container up/down itself: +pytest tests/integration/ -m integration -v + +# On CI / a PR, on-demand: +# /pytest -m integration +``` + +## Adding a scenario + +1. Create `tests/integration//test_*.py`. +2. Set `pytestmark = pytest.mark.integration`. +3. Request the `robot_autonomy_stack` fixture for the container. +4. Filter by path (`tests/integration//`) when you only want that scenario. + +## Residents + +| Scenario | What it verifies | +|----------|------------------| +| [`natnet/`](natnet/) | Host NatNet emulator → `natnet_ros2` → pose topic Hz | diff --git a/tests/integration/natnet/README.md b/tests/integration/natnet/README.md new file mode 100644 index 000000000..4e9330c32 --- /dev/null +++ b/tests/integration/natnet/README.md @@ -0,0 +1,151 @@ +# NatNet ↔ robot autonomy integration + +Host-side NatNet wire-protocol tests that drive the Python emulator against +`natnet_ros2_node` in a real robot container. First resident of the +[`integration`](../README.md) tier (no sim, no GPU). + +Mark: `integration`. Filter this scenario with `tests/integration/natnet/`. + +For the **in-sim** end-to-end check (Isaac emulator + full stack), see +[Liveliness sentinel](#liveliness-sentinel-sim-end-to-end) below and +[`tests/system/test_liveliness.py`](../../system/test_liveliness.py). + +## What it verifies + +Three variants in [`test_natnet_integration.py`](test_natnet_integration.py). +All start a host-side `NatNetUnicastServer`, launch `natnet_ros2_node` in the +robot container pointed at the Docker bridge gateway, and assert a sustained +pose stream at **≥ 5 Hz** on the configured topic(s), e.g.: + +- `/{ROBOT_NAME}/perception/optitrack/drone/pose_cov` (wait for first message) +- `/{ROBOT_NAME}/perception/optitrack/drone` (Hz sample) + +| Test | Path | +|------|------| +| **`test_natnet_ros2_receives_drone_pose_hz`** | Hand-built `sFrameOfMocapData` frames enqueued on a raw `NatNetUnicastServer` (no USD). Minimal wire + SDK check. | +| **`test_natnet_ros2_receives_isaac_wrapper_pose_hz`** | Full Isaac data path: in-memory USD stage, `NatNetInterfaceConfig`, `author_interface`, `NatNetServerManager.sample_once()` on a moving prim — same sampling logic as the in-sim physics-step callback. Skips without `usd-core` (`pxr`). Pose-value fidelity is covered hermetically by the emulator's `test_pose_streaming.py` loopback. | +| **`test_natnet_ros2_multi_body_drone_and_target`** | Two bodies (drone id 1 + target id 100) with distinct relative topics; asserts both pose streams and that the target's `pose_cov` topic is **absent** (`body_pose_cov=false`). Exercises the multi-body profile + per-body `pose`/`pose_cov` toggles. | + +These tests **do not** start the full perception bringup or `LAUNCH_NATNET`; they +exec `natnet_ros2_node` directly with the flattened per-body params +(`body_names`/`body_ids`/`body_topics`/`body_pose`/`body_pose_cov`) and no MAVROS bridge. + +## Requirements + +- Docker daemon (robot-desktop container reachable from pytest). +- **`natnet_ros2_node` built** in the robot image (OptiTrack NatNet SDK is + license-gated — run `airstack setup --natnet`, then + `bws --packages-select natnet_ros2` in the container). Tests **skip** if the + node binary is missing. +- Host-side emulator package on `PYTHONPATH` (the test adds + `simulation/isaac-sim/extensions/optitrack.natnet.emulator` — not pip-installed + on the host). +- Ephemeral UDP ports on the host gateway IP (Docker default route as seen from + inside the container). + +The robot container comes from the shared **`robot_autonomy_stack`** fixture in +[`tests/conftest.py`](../../conftest.py) (see the [integration tier README](../README.md)). + +## Running + +```bash +# 1. One-time: NatNet SDK + build natnet_ros2 in the robot image +airstack setup --natnet # or NATNET_ACCEPT_LICENSE=1 airstack setup --natnet +docker exec airstack-robot-desktop-1 bash -lc 'bws --packages-select natnet_ros2' + +# 2a. Reuse an existing robot container (fast local iteration): +AUTOLAUNCH=false airstack up robot-desktop +pytest tests/integration/natnet/ -m integration -v + +# 2b. Let the harness bring the container up/down: +pytest tests/integration/natnet/ -m integration -v +``` +On CI / PR (write access): `/pytest -m integration` + +## Architecture + +``` +┌──────────────────────────────────────────────────────────────┐ +│ Host (pytest) │ +│ NatNetUnicastServer @ docker bridge gateway IP │ +│ • raw variant: hand-built frame queue │ +│ • Isaac variant: NatNetServerManager.sample_once(USD) │ +└────────────────────────────┬─────────────────────────────────┘ + │ UDP unicast (cmd + data ports) +┌────────────────────────────▼─────────────────────────────────┐ +│ Robot container (robot-desktop) │ +│ natnet_ros2_node (libNatNet 4.4 client) │ +│ → /{ROBOT_NAME}/{body topic}[/pose_cov] per configured body │ +└──────────────────────────────────────────────────────────────┘ +``` + +**In sim (liveliness tier):** the server runs inside the Isaac Sim container +(`172.31.0.200` by default). Use a NatNet Pegasus launch script +(`example_one_px4_pegasus_natnet_launch_script.py` or +`example_multi_px4_pegasus_natnet_launch_script.py`); `natnet_ros2` in the +robot stack connects via `natnet_config.yaml` (`server_ip` → emulator IP). + +**Catalog / MODELDEF:** The server holds a MODELDEF **wire cache** only +(`set_model_def_payload()`). Scene semantics (body names, streaming IDs, target +prim paths) come from the Isaac layer (`NatNetInterfaceConfig`, USD interface +prim, or launch-script `build_drone_config`). See the +[emulator README](../../../simulation/isaac-sim/extensions/optitrack.natnet.emulator/README.md). + +## Liveliness sentinel (sim end-to-end) + +The integration tier proves **robot client + host emulator** without Isaac. +The matching **system** check is +`TestLiveliness::test_natnet_pose_alive` in +[`test_liveliness.py`](../../system/test_liveliness.py): + +- **Gated on `LAUNCH_NATNET=true`** (skipped otherwise — normal liveliness runs + are unaffected). +- Asserts `/{robot_n}/{natnet pose topic}/pose_cov` ≥ 5 Hz per robot (the drone + body's configured topic — default `perception/optitrack/drone`). +- Override the checked topic with `NATNET_POSE_TOPIC` (default + `perception/optitrack/drone`). The sim body name (`NATNET_BODY_NAME`, default + `Drone`) is decoupled from the published topic, which the robot profile sets. + +Sim auto-start: set `ISAAC_SIM_SCRIPT_NAME` to a NatNet launch script and +`LAUNCH_NATNET=true` on the robot. Convenience bundle: +`airstack up --env-file overrides/isaac-natnet-vision.env` (NatNet script + +PX4 external-vision SITL profile). + +## libNatNet 4.4 unicast — verified wire contract + +The emulator is validated against the **real `libNatNet.so`** (not just the Python +`NatNetClient`) with a minimal C probe that registers `SetFrameReceivedCallback` +and `NatNet_SetLogCallback`. All of the following must hold for the SDK to deliver +frames to the callback: + +| Requirement | Why | +|-------------|-----| +| `NAT_CONNECT` → `sSender_Server` (279 B), name `Motive` | libNatNet reads `Motive 3.1 / NatNet 4.4` | +| `NAT_ECHOREQUEST` → `NAT_ECHORESPONSE` (16 B) | Prevents libNatNet assert | +| Frame ends with a **4-byte end-of-data tag** after `params` | libNatNet's frame unpacker reads it; without it the unpacked size mismatches `nDataBytes` and **every frame is silently dropped** | +| `NAT_FRAMEOFDATA` sent from the **data port** (source port == `data_port`) | libNatNet routes unicast frames by the server's data port. Frames sent from the **command** port are treated as command traffic and dropped — no error, no callback | +| `NAT_KEEPALIVE` gets **no reply** | An echo reply makes libNatNet log `Received unrecognized message Message=10` | + +With these in place the C probe reports `Server: Motive 3.1.0.0 NatNet 4.4.0.0`, +`data descriptions: 1`, and **~74 Hz** of frame callbacks. + +> The lenient Python `NatNetClient` accepts frames *without* the end-of-data tag +> and *on the command port*, which is why it appeared to work while libNatNet did +> not. Always validate against the C SDK. + +Full handshake notes and sniffing workflow: +[optitrack-development skill](../../../.agents/skills/optitrack-development/SKILL.md). + +## After changing natnet_ros2 or the emulator + +Rebuild in the robot container: + +```bash +docker exec airstack-robot-desktop-1 bash -lc 'bws --packages-select natnet_ros2' +``` + +Unit tests (protocol, serializers, Isaac wrapper loopback): + +```bash +pytest tests/sim/optitrack_natnet_emulator/ -m unit -v +``` diff --git a/tests/integration/natnet/test_natnet_integration.py b/tests/integration/natnet/test_natnet_integration.py new file mode 100644 index 000000000..43fa4a7f3 --- /dev/null +++ b/tests/integration/natnet/test_natnet_integration.py @@ -0,0 +1,376 @@ +# Copyright (c) 2024 Carnegie Mellon University +# MIT License - see LICENSE in the repository root for full text. +"""NatNet - robot autonomy integration tests. + +Host-side variants stream frames to ``natnet_ros2_node`` in the robot container and +assert pose topics stay alive at >= 5 Hz: (1) raw ``NatNetUnicastServer`` hand-built +single-body frames; (2) ``NatNetServerManager`` sampling an in-memory USD stage +(Isaac wrapper path, no sim/GPU); (3) a multi-body profile (drone + target) that +exercises per-body topic overrides and the pose / pose_cov toggles. + +The node is parameterised with the flattened per-body arrays +(``body_names`` / ``body_ids`` / ``body_topics`` / ``body_pose`` / ``body_pose_cov``) +that natnet_ros2.launch.py derives from a robot's natnet_config.yaml profile. + +Multi-robot (NUM_ROBOTS=3, per-robot profiles) is exercised in-sim by +``tests/system/test_liveliness.py::test_natnet_pose_alive``. +""" + +from __future__ import annotations + +import subprocess +import sys +import threading +import time + +import pytest + +from conftest import ( # noqa: E402 — pytest adds tests/ to sys.path + docker_exec, + repo_path, + ros2_env, + sample_hz, + wait_for_first_message, +) + +# Emulator is not pip-installed on the host; add extension root + test helpers. +_EXT_ROOT = repo_path("simulation/isaac-sim/extensions/optitrack.natnet.emulator") +for _path in (_EXT_ROOT, _EXT_ROOT / "test"): + if str(_path) not in sys.path: + sys.path.insert(0, str(_path)) + +from optitrack.natnet.emulator import NatNetUnicastServer, TransmissionType # noqa: E402 +from optitrack.natnet.emulator.server import natnet_data_types as dt # noqa: E402 +from natnet_test_helpers import ephemeral_udp_port # noqa: E402 + +pytestmark = pytest.mark.integration + +_ROBOT_SETUP = "/root/AirStack/robot/ros_ws/install/setup.bash" +_NATNET_NODE = "/root/AirStack/robot/ros_ws/install/natnet_ros2/lib/natnet_ros2/natnet_ros2_node" +_WARMUP_S = 2.0 +_STREAM_HOLD_S = 12.0 +_MIN_HZ = 5.0 + +# Robot image has route/netstat but not `ip`; /proc/net/route is always present. +_DEFAULT_GATEWAY_CMD = ( + """awk '$2 == "00000000" { printf "%d.%d.%d.%d\\n", """ + """"0x" substr($3,7,2), "0x" substr($3,5,2), "0x" substr($3,3,2), "0x" substr($3,1,2); exit }' """ + """/proc/net/route""" +) + + +def _docker_default_gateway(container: str) -> str: + result = docker_exec(container, _DEFAULT_GATEWAY_CMD, timeout=10) + gateway = result.stdout.strip() + if not gateway: + pytest.skip(f"Could not resolve default gateway inside {container}") + return gateway + + +def _container_env(container: str, var: str, default: str) -> str: + # ROBOT_NAME / ROS_DOMAIN_ID are set in .bashrc (login shell), not container ENV. + # .bashrc may print "Sourcing ..." to stdout; take the last line as the value. + result = docker_exec(container, f"bash -lc 'echo ${var}'") + lines = [line.strip() for line in result.stdout.splitlines() if line.strip()] + value = lines[-1] if lines else "" + return value if value else default + + +def _natnet_node_available(container: str) -> bool: + result = docker_exec(container, f"test -x {_NATNET_NODE} && echo yes || echo no") + return "yes" in result.stdout + + +def _stop_stale_natnet_nodes(container: str) -> None: + docker_exec(container, "pkill -f natnet_ros2_node || true") + time.sleep(0.5) + + +# Each body: (streaming_id, rigid_body_name). The raw server frame carries ids only; +# the node maps ids → topics via its body_* params. +_DRONE_BODY = (1, "Drone") +_TARGET_BODY = (100, "Target") + + +def _make_frame(frame_num: int, body_ids) -> dt.sFrameOfMocapData: + frame = dt.sFrameOfMocapData() + frame.iFrame = frame_num + frame.nRigidBodies = len(body_ids) + for slot, body_id in enumerate(body_ids): + rb = frame.RigidBodies[slot] + rb.ID = body_id + rb.qw = 1.0 + # Bit 0 = tracking valid; natnet_ros2 skips bodies without it (natnet_logic.hpp). + rb.params = 1 + return frame + + +def _frame_publisher( + server: NatNetUnicastServer, stop_event: threading.Event, body_ids=(1,) +) -> None: + frame_num = 0 + interval = 1.0 / server.publish_rate + while not stop_event.is_set(): + server.enqueue_mocap_data(_make_frame(frame_num, body_ids)) + frame_num += 1 + time.sleep(interval) + + +def _launch_natnet_node(container, host_ip, command_port, domain_id, bodies=None): + """Start natnet_ros2_node in the container pointed at the host emulator. + + ``bodies`` is a list of (id, name, topic, pose, pose_cov); defaults to a single + Drone body on topic ``perception/optitrack/drone`` (the shipped config default). + """ + if bodies is None: + bodies = [(1, "Drone", "perception/optitrack/drone", "true", "true")] + ids = ",".join(str(b[0]) for b in bodies) + names = ",".join(b[1] for b in bodies) + topics = ",".join(b[2] for b in bodies) + pose = ",".join(b[3] for b in bodies) + pose_cov = ",".join(b[4] for b in bodies) + launch_cmd = ( + f"bash -lc '{ros2_env(_ROBOT_SETUP, domain_id)} && " + f"exec {_NATNET_NODE} --ros-args " + f"-p server_ip:={host_ip} " + f"-p command_port:={command_port} " + f"-p body_names:=[{names}] " + f"-p body_ids:=[{ids}] " + f"-p body_topics:=[{topics}] " + f"-p body_pose:=[{pose}] " + f"-p body_pose_cov:=[{pose_cov}]'" + ) + return subprocess.Popen( + ["docker", "exec", container, "bash", "-c", launch_cmd], + stdout=subprocess.PIPE, + stderr=subprocess.STDOUT, + text=True, + ) + + +def _assert_pose_stream( + container, robot_name, domain_id, topic="perception/optitrack/drone", pose_cov=True +): + """Wait for the pose topic then assert a sustained rate >= _MIN_HZ. + + A body configured with ``body_pose_cov=false`` never publishes the ``/pose_cov`` + variant, so detect the first message on whichever topic the body actually emits. + """ + pose_topic = f"/{robot_name}/{topic}" + detect_topic = f"{pose_topic}/pose_cov" if pose_cov else pose_topic + + time.sleep(_WARMUP_S) + first_msg_s = wait_for_first_message( + container, detect_topic, domain_id, _ROBOT_SETUP, timeout=int(_STREAM_HOLD_S) + ) + assert first_msg_s is not None, ( + f"No messages on {detect_topic} within {_STREAM_HOLD_S}s " + "(NatNet connect or frame stream failed)" + ) + hz = sample_hz( + container, + pose_topic, + domain_id, + _ROBOT_SETUP, + duration=min(8, int(_STREAM_HOLD_S - first_msg_s)), + window=20, + ) + assert hz is not None, f"No sustained stream on {pose_topic}" + assert hz >= _MIN_HZ, f"Expected >= {_MIN_HZ} Hz on {pose_topic}, got {hz}" + + +def _terminate(proc) -> None: + if proc is None: + return + proc.terminate() + try: + proc.wait(timeout=5) + except subprocess.TimeoutExpired: + proc.kill() + + +def test_natnet_ros2_receives_drone_pose_hz(robot_autonomy_stack): + """Raw-server path: hand-built frames on NatNetUnicastServer.""" + container = robot_autonomy_stack["container"] + + if not _natnet_node_available(container): + pytest.skip( + "natnet_ros2_node not built — run airstack setup (NatNet SDK) and " + "bws --packages-select natnet_ros2 in the robot container" + ) + + _stop_stale_natnet_nodes(container) + + host_ip = _docker_default_gateway(container) + command_port = ephemeral_udp_port(host_ip) + robot_name = _container_env(container, "ROBOT_NAME", "robot_1") + domain_id = int(_container_env(container, "ROS_DOMAIN_ID", "0")) + + server = NatNetUnicastServer( + local_interface=host_ip, + transmission_type=TransmissionType.UNICAST, + multicast_address=None, + command_port=command_port, + ) + server.publish_rate = 50 + + stop_event = threading.Event() + publisher = threading.Thread( + target=_frame_publisher, args=(server, stop_event), daemon=True + ) + + node_proc: subprocess.Popen[str] | None = None + try: + # Seed dummy frames before the client connects; keep streaming the whole window. + publisher.start() + time.sleep(0.1) + server.start() + node_proc = _launch_natnet_node(container, host_ip, command_port, domain_id) + _assert_pose_stream(container, robot_name, domain_id) + finally: + stop_event.set() + publisher.join(timeout=2.0) + _terminate(node_proc) + server.shutdown() + + +def test_natnet_ros2_receives_isaac_wrapper_pose_hz(robot_autonomy_stack): + """Isaac-wrapper path: NatNetServerManager.sample_once on a moving USD prim. + + Tests that the wrapper feeds the real robot client end-to-end. Pose-value fidelity + is covered by test_pose_streaming.py loopback. + """ + pytest.importorskip("pxr") + import math + + from pxr import Gf, Usd, UsdGeom + + from optitrack.natnet.emulator.isaac import ( + BodyBinding, + NatNetInterfaceConfig, + NatNetServerManager, + author_interface, + ) + + container = robot_autonomy_stack["container"] + if not _natnet_node_available(container): + pytest.skip("natnet_ros2_node not built — run airstack setup (NatNet SDK)") + + _stop_stale_natnet_nodes(container) + + host_ip = _docker_default_gateway(container) + command_port = ephemeral_udp_port(host_ip) + data_port = ephemeral_udp_port(host_ip) + while data_port == command_port: + data_port = ephemeral_udp_port(host_ip) + robot_name = _container_env(container, "ROBOT_NAME", "robot_1") + domain_id = int(_container_env(container, "ROS_DOMAIN_ID", "0")) + + stage = Usd.Stage.CreateInMemory() + xform = UsdGeom.Xform.Define(stage, "/World/base_link") + translate_op = xform.AddTranslateOp() + translate_op.Set(Gf.Vec3d(0.0, 0.0, 1.0)) + cfg = NatNetInterfaceConfig( + server_ip=host_ip, + command_port=command_port, + data_port=data_port, + publish_rate=50.0, + bodies=[BodyBinding("Drone", "/World/base_link", streaming_id=1)], + ) + author_interface(stage, "/World/NatNetInterface", cfg) + + manager = NatNetServerManager(server_factory=None) # real server factory + stop_event = threading.Event() + + def _sampler(): + # Stand in for the in-sim physics-step callback: move the prim and sample. + interval = 1.0 / cfg.publish_rate + t = 0.0 + while not stop_event.is_set(): + translate_op.Set(Gf.Vec3d(math.sin(t), 0.0, 1.0)) + manager.sample_once(stage) + t += interval + time.sleep(interval) + + sampler = threading.Thread(target=_sampler, daemon=True) + + node_proc: subprocess.Popen[str] | None = None + try: + assert manager.start_server(cfg) is True + sampler.start() + time.sleep(0.1) + node_proc = _launch_natnet_node(container, host_ip, command_port, domain_id) + _assert_pose_stream(container, robot_name, domain_id) + finally: + stop_event.set() + sampler.join(timeout=2.0) + _terminate(node_proc) + manager.stop_server() + + +def test_natnet_ros2_multi_body_drone_and_target(robot_autonomy_stack): + """Multi-body profile: one robot tracks a drone + a static target. + + Streams two bodies (drone id 1, target id 100) and configures the node like a + robot profile with two bodies and distinct relative topics. Asserts: the drone + pose streams >= 5 Hz on its custom topic; the target pose streams on its own + topic; and the target's pose_cov topic is absent (body_pose_cov=false). + """ + container = robot_autonomy_stack["container"] + + if not _natnet_node_available(container): + pytest.skip("natnet_ros2_node not built — run airstack setup (NatNet SDK)") + + _stop_stale_natnet_nodes(container) + + host_ip = _docker_default_gateway(container) + command_port = ephemeral_udp_port(host_ip) + robot_name = _container_env(container, "ROBOT_NAME", "robot_1") + domain_id = int(_container_env(container, "ROS_DOMAIN_ID", "0")) + + server = NatNetUnicastServer( + local_interface=host_ip, + transmission_type=TransmissionType.UNICAST, + multicast_address=None, + command_port=command_port, + ) + server.publish_rate = 50 + + bodies = [ + (_DRONE_BODY[0], _DRONE_BODY[1], "perception/optitrack/drone", "true", "true"), + (_TARGET_BODY[0], _TARGET_BODY[1], "perception/optitrack/target", "true", "false"), + ] + body_ids = (_DRONE_BODY[0], _TARGET_BODY[0]) + + stop_event = threading.Event() + publisher = threading.Thread( + target=_frame_publisher, args=(server, stop_event, body_ids), daemon=True + ) + + node_proc: subprocess.Popen[str] | None = None + try: + publisher.start() + time.sleep(0.1) + server.start() + node_proc = _launch_natnet_node(container, host_ip, command_port, domain_id, bodies) + # Drone (pose + pose_cov) and target (pose only) both stream. + _assert_pose_stream(container, robot_name, domain_id, "perception/optitrack/drone") + _assert_pose_stream( + container, robot_name, domain_id, "perception/optitrack/target", pose_cov=False + ) + + # body_pose_cov=false → the target pose_cov publisher must not exist. + target_cov = f"/{robot_name}/perception/optitrack/target/pose_cov" + topics = docker_exec( + container, + f"bash -lc '{ros2_env(_ROBOT_SETUP, domain_id)} && ros2 topic list'", + timeout=15, + ).stdout + assert target_cov not in topics.split(), ( + f"{target_cov} should not exist when body_pose_cov=false; topics:\n{topics}" + ) + finally: + stop_event.set() + publisher.join(timeout=2.0) + _terminate(node_proc) + server.shutdown() diff --git a/tests/pytest.ini b/tests/pytest.ini index a664ccbf3..6e7bd7c6b 100644 --- a/tests/pytest.ini +++ b/tests/pytest.ini @@ -3,6 +3,7 @@ markers = unit: Fast hermetic tests (no Docker stack; numpy / pure Python) build_docker: Docker image build tests build_packages: Colcon workspace build tests + integration: Cross-component integration tests (robot container + a host-side component; no sim/GPU) liveliness: Container and process health (Docker, tmux, sentinel ROS 2 nodes) sensors: Sim and robot sensor topic rates, LiDAR validation, sim RTF takeoff_hover_land: End-to-end takeoff / hover / land action tests diff --git a/tests/requirements.txt b/tests/requirements.txt index a4b43e6bb..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/robot/README.md b/tests/robot/README.md index a4409c4b1..2c7806367 100644 --- a/tests/robot/README.md +++ b/tests/robot/README.md @@ -20,13 +20,13 @@ convention): robot/ros_ws/src///test/test_.py ← source of truth ``` -**This directory** contains thin proxy files that load the real test module via -`importlib` and re-export its `test_*` functions, making them discoverable by -`pytest tests/` and `airstack test -m unit` without any changes to the CI -workflow. Each proxy is ~15 lines. +**This directory** contains thin proxy files that call ``register_unit_tests()`` +(from [`tests/conftest.py`](../conftest.py)) to register co-located ``test_*`` +functions for ``pytest tests/`` and ``airstack test -m unit``. Each proxy is +~10 lines. ``` -tests/robot///test_.py ← proxy (re-exports above) +tests/robot///test_.py ← proxy (registers above) ``` Both `airstack test -m unit` (pytest path via proxy) and diff --git a/tests/robot/perception/natnet_ros2/test_natnet_ros2.py b/tests/robot/perception/natnet_ros2/test_natnet_ros2.py index fc9a78bde..a8bcb1dfb 100644 --- a/tests/robot/perception/natnet_ros2/test_natnet_ros2.py +++ b/tests/robot/perception/natnet_ros2/test_natnet_ros2.py @@ -1,32 +1,11 @@ # Copyright (c) 2024 Carnegie Mellon University # MIT License - see LICENSE in the repository root for full text. -"""Proxy: re-exposes natnet_ros2 unit tests from the package source tree. +"""Proxy: registers natnet_ros2 Python unit tests for pytest tests/.""" -Unit test logic lives co-located with its package (ROS 2 / colcon convention): - robot/ros_ws/src/perception/natnet_ros2/test/test_natnet_ros2.py +from conftest import register_unit_tests, repo_path -This file makes those tests discoverable by ``pytest tests/`` (CI) and -``airstack test -m unit`` without any changes to the CI workflow. -Run ``colcon test --packages-select natnet_ros2`` to also execute the C++ -gtests and ament linters. -""" - -import importlib.util -import sys -from pathlib import Path - -_repo_root = Path(__file__).resolve().parents[4] -_pkg_test = _repo_root / "robot/ros_ws/src/perception/natnet_ros2/test" -_real_file = _pkg_test / "test_natnet_ros2.py" - -# Load the real module under a unique name to avoid the circular-import that -# would occur if we used `from test_natnet_ros2 import *` (this file has the -# same name and pytest adds its directory to sys.path at collection time). -_spec = importlib.util.spec_from_file_location("_natnet_ros2_unit_tests", _real_file) -_real = importlib.util.module_from_spec(_spec) -_spec.loader.exec_module(_real) - -# Re-export every test_* symbol so pytest collects them from this proxy. -for _name in dir(_real): - if _name.startswith("test_"): - globals()[_name] = getattr(_real, _name) +register_unit_tests( + globals(), + repo_path("robot/ros_ws/src/perception/natnet_ros2/test"), + "test_natnet_ros2.py", +) diff --git a/tests/robot/sensors/lidar_point_cloud_filter/test_validation_core.py b/tests/robot/sensors/lidar_point_cloud_filter/test_validation_core.py index e7babf9d4..677680092 100644 --- a/tests/robot/sensors/lidar_point_cloud_filter/test_validation_core.py +++ b/tests/robot/sensors/lidar_point_cloud_filter/test_validation_core.py @@ -1,6 +1,6 @@ # Copyright (c) 2024 Carnegie Mellon University # MIT License - see LICENSE in the repository root for full text. -"""Proxy: re-exposes validation_core unit tests from the package source tree. +"""Proxy: registers validation_core unit tests from the package source tree. Unit test logic lives co-located with its package (ROS 2 / colcon convention): robot/ros_ws/src/sensors/lidar_point_cloud_filter/test/test_validation_core.py @@ -11,28 +11,10 @@ the ament linters. """ -import importlib.util -import sys -from pathlib import Path +from conftest import register_unit_tests, repo_path -_repo_root = Path(__file__).resolve().parents[4] -_pkg_test = _repo_root / "robot/ros_ws/src/sensors/lidar_point_cloud_filter/test" -_pkg_root = _pkg_test.parent # adds lidar_point_cloud_filter/ package to sys.path -_real_file = _pkg_test / "test_validation_core.py" - -# Make the package module importable so the real test can do -# `from lidar_point_cloud_filter.validation_core import ...` -if str(_pkg_root) not in sys.path: - sys.path.insert(0, str(_pkg_root)) - -# Load the real module under a unique name to avoid the circular-import that -# would occur if we used `from test_validation_core import *` (this file has -# the same name and pytest adds its directory to sys.path at collection time). -_spec = importlib.util.spec_from_file_location("_lidar_validation_unit_tests", _real_file) -_real = importlib.util.module_from_spec(_spec) -_spec.loader.exec_module(_real) - -# Re-export every test_* symbol so pytest collects them from this proxy. -for _name in dir(_real): - if _name.startswith("test_"): - globals()[_name] = getattr(_real, _name) +register_unit_tests( + globals(), + repo_path("robot/ros_ws/src/sensors/lidar_point_cloud_filter/test"), + "test_validation_core.py", +) diff --git a/tests/sim/README.md b/tests/sim/README.md index 09f45f6a6..ceae2ebbd 100644 --- a/tests/sim/README.md +++ b/tests/sim/README.md @@ -6,9 +6,11 @@ AirSim bridge utilities). Mark fast, hermetic checks with `@pytest.mark.unit`. Tests that require a GPU, full sim, or Docker belong in [`tests/system/`](../system/) instead. +Cross-component tests that need the robot container (but not a sim) belong in +[`tests/integration/`](../integration/) (mark: `integration`). Suggested layout: | Directory | Purpose | |-----------|---------| -| `motive_emulator/` | Motive / NatNet protocol emulation / parsing | +| `optitrack_natnet_emulator/` | NatNet emulator unit tests (proxy; mark: `unit`). Includes pure protocol/serializer/config-model checks plus `pxr`-guarded USD interface-authoring tests (skip without `usd-core`). | diff --git a/tests/sim/motive_emulator/README.md b/tests/sim/motive_emulator/README.md deleted file mode 100644 index 0e682c448..000000000 --- a/tests/sim/motive_emulator/README.md +++ /dev/null @@ -1,61 +0,0 @@ -# Motive / NatNet Emulator - -This directory is the future home of **integration tests** that drive a real -NatNet wire-protocol mock server against `natnet_ros2_node`. - -## Why here, not in the package test/ dir? - -Unit tests for pure logic live in -`tests/robot/perception/natnet_ros2/test_natnet_logic.cpp` and run via `colcon -test` with no network or SDK required (uses `FakeNatNetClient`). - -The emulator tests here will require an actual UDP server that speaks the NatNet -protocol, so they belong in the `sensors` mark of the system test suite alongside -other topic-streaming tests. - -## Planned implementation - -The mock server should: - -1. Open a UDP socket on the NatNet command port (default 1510). -2. Respond to `NAT_CONNECT` (message type 0) with a `NAT_SERVERINFO` (type 1) - packet containing a canned `sServerDescription`. -3. Respond to `NAT_REQUEST_MODELDEF` (type 4) with a `NAT_MODELDEF` (type 5) - packet describing one or more rigid bodies. -4. Stream `NAT_FRAMEOFDATA` (type 7) packets to the client's data port at a - configurable rate with synthetic pose data. - -### Reference - -The NatNet wire format is documented in the NatNet SDK developer notes and the -`PacketClient` example shipped with the SDK (available inside the robot Docker -container after `airstack setup --natnet`). - -## Relationship to `FakeNatNetClient` - -``` - ┌──────────────────────────────────────┐ - │ Test boundary │ - colcon gtest │ FakeNatNetClient (in-process) │ ← unit tests (no network) - │ test_natnet_logic.cpp │ - └──────────────────────────────────────┘ - - ┌──────────────────────────────────────┐ - │ Network boundary │ - pytest sensors │ MotiveEmulator (UDP server, Python) │ ← integration tests - │ NatNetClientAdapter → NatNetClient │ - │ natnet_ros2_node (full ROS node) │ - └──────────────────────────────────────┘ -``` - -The `FakeNatNetClient` seam (already implemented) lets unit tests verify all -connection-outcome logic paths. The emulator here will verify the full -end-to-end path including the NatNet SDK's own parser. - -## When to add this - -Implement the emulator when: -- The OptiTrack emulator service is placed under `simulation/optitrack-emulator/` - or `tests/sim/motive_emulator/` -- The `sensors` test mark is extended to include `natnet_ros2` topic checks -- CI has access to the robot container with the NatNet SDK installed diff --git a/tests/sim/optitrack_natnet_emulator/test_catalog.py b/tests/sim/optitrack_natnet_emulator/test_catalog.py new file mode 100644 index 000000000..8bb6b4730 --- /dev/null +++ b/tests/sim/optitrack_natnet_emulator/test_catalog.py @@ -0,0 +1,11 @@ +# Copyright (c) 2024 Carnegie Mellon University +# MIT License - see LICENSE in the repository root for full text. +"""Proxy: registers NatNet emulator catalog-builder unit tests.""" + +from conftest import register_unit_tests, repo_path + +register_unit_tests( + globals(), + repo_path("simulation/isaac-sim/extensions/optitrack.natnet.emulator/test"), + "test_catalog.py", +) diff --git a/tests/sim/optitrack_natnet_emulator/test_defaults_and_catalog.py b/tests/sim/optitrack_natnet_emulator/test_defaults_and_catalog.py new file mode 100644 index 000000000..c598d25f0 --- /dev/null +++ b/tests/sim/optitrack_natnet_emulator/test_defaults_and_catalog.py @@ -0,0 +1,12 @@ +# Copyright (c) 2024 Carnegie Mellon University +# MIT License - see LICENSE in the repository root for full text. +"""Proxy: registers optitrack.natnet.emulator default/catalog unit tests for pytest tests/.""" + +from conftest import register_unit_tests, repo_path + +register_unit_tests( + globals(), + repo_path("simulation/isaac-sim/extensions/optitrack.natnet.emulator/test"), + "test_defaults.py", + "test_server_catalog.py", +) diff --git a/tests/sim/optitrack_natnet_emulator/test_discovery.py b/tests/sim/optitrack_natnet_emulator/test_discovery.py new file mode 100644 index 000000000..d0559b68f --- /dev/null +++ b/tests/sim/optitrack_natnet_emulator/test_discovery.py @@ -0,0 +1,11 @@ +# Copyright (c) 2024 Carnegie Mellon University +# MIT License - see LICENSE in the repository root for full text. +"""Proxy: registers NatNet emulator format_interface unit tests.""" + +from conftest import register_unit_tests, repo_path + +register_unit_tests( + globals(), + repo_path("simulation/isaac-sim/extensions/optitrack.natnet.emulator/test"), + "test_discovery.py", +) diff --git a/tests/sim/optitrack_natnet_emulator/test_frames.py b/tests/sim/optitrack_natnet_emulator/test_frames.py new file mode 100644 index 000000000..7a95f783e --- /dev/null +++ b/tests/sim/optitrack_natnet_emulator/test_frames.py @@ -0,0 +1,11 @@ +# Copyright (c) 2024 Carnegie Mellon University +# MIT License - see LICENSE in the repository root for full text. +"""Proxy: registers optitrack.natnet.emulator pose->frame builder unit tests.""" + +from conftest import register_unit_tests, repo_path + +register_unit_tests( + globals(), + repo_path("simulation/isaac-sim/extensions/optitrack.natnet.emulator/test"), + "test_frames.py", +) diff --git a/tests/sim/optitrack_natnet_emulator/test_interface_authoring.py b/tests/sim/optitrack_natnet_emulator/test_interface_authoring.py new file mode 100644 index 000000000..56a4d410c --- /dev/null +++ b/tests/sim/optitrack_natnet_emulator/test_interface_authoring.py @@ -0,0 +1,11 @@ +# Copyright (c) 2024 Carnegie Mellon University +# MIT License - see LICENSE in the repository root for full text. +"""Proxy: registers optitrack.natnet.emulator USD authoring unit tests (skips without ``pxr``).""" + +from conftest import register_unit_tests, repo_path + +register_unit_tests( + globals(), + repo_path("simulation/isaac-sim/extensions/optitrack.natnet.emulator/test"), + "test_interface_authoring.py", +) diff --git a/tests/sim/optitrack_natnet_emulator/test_interface_config.py b/tests/sim/optitrack_natnet_emulator/test_interface_config.py new file mode 100644 index 000000000..a1e894bdc --- /dev/null +++ b/tests/sim/optitrack_natnet_emulator/test_interface_config.py @@ -0,0 +1,11 @@ +# Copyright (c) 2024 Carnegie Mellon University +# MIT License - see LICENSE in the repository root for full text. +"""Proxy: registers optitrack.natnet.emulator interface config-model unit tests.""" + +from conftest import register_unit_tests, repo_path + +register_unit_tests( + globals(), + repo_path("simulation/isaac-sim/extensions/optitrack.natnet.emulator/test"), + "test_interface_config.py", +) diff --git a/tests/sim/optitrack_natnet_emulator/test_pose_sampling.py b/tests/sim/optitrack_natnet_emulator/test_pose_sampling.py new file mode 100644 index 000000000..241b58e83 --- /dev/null +++ b/tests/sim/optitrack_natnet_emulator/test_pose_sampling.py @@ -0,0 +1,11 @@ +# Copyright (c) 2024 Carnegie Mellon University +# MIT License - see LICENSE in the repository root for full text. +"""Proxy: registers optitrack.natnet.emulator pose-sampling / resync unit tests.""" + +from conftest import register_unit_tests, repo_path + +register_unit_tests( + globals(), + repo_path("simulation/isaac-sim/extensions/optitrack.natnet.emulator/test"), + "test_pose_sampling.py", +) diff --git a/tests/sim/optitrack_natnet_emulator/test_pose_streaming.py b/tests/sim/optitrack_natnet_emulator/test_pose_streaming.py new file mode 100644 index 000000000..1d5480bf7 --- /dev/null +++ b/tests/sim/optitrack_natnet_emulator/test_pose_streaming.py @@ -0,0 +1,11 @@ +# Copyright (c) 2024 Carnegie Mellon University +# MIT License - see LICENSE in the repository root for full text. +"""Proxy: registers optitrack.natnet.emulator end-to-end pose-streaming test.""" + +from conftest import register_unit_tests, repo_path + +register_unit_tests( + globals(), + repo_path("simulation/isaac-sim/extensions/optitrack.natnet.emulator/test"), + "test_pose_streaming.py", +) diff --git a/tests/sim/optitrack_natnet_emulator/test_scene_setup.py b/tests/sim/optitrack_natnet_emulator/test_scene_setup.py new file mode 100644 index 000000000..8f124ee4d --- /dev/null +++ b/tests/sim/optitrack_natnet_emulator/test_scene_setup.py @@ -0,0 +1,11 @@ +# Copyright (c) 2024 Carnegie Mellon University +# MIT License - see LICENSE in the repository root for full text. +"""Proxy: registers optitrack.natnet.emulator launch-helper (scene_setup) tests.""" + +from conftest import register_unit_tests, repo_path + +register_unit_tests( + globals(), + repo_path("simulation/isaac-sim/extensions/optitrack.natnet.emulator/test"), + "test_scene_setup.py", +) diff --git a/tests/sim/optitrack_natnet_emulator/test_serializers.py b/tests/sim/optitrack_natnet_emulator/test_serializers.py new file mode 100644 index 000000000..f64cf3b35 --- /dev/null +++ b/tests/sim/optitrack_natnet_emulator/test_serializers.py @@ -0,0 +1,11 @@ +# Copyright (c) 2024 Carnegie Mellon University +# MIT License - see LICENSE in the repository root for full text. +"""Proxy: registers optitrack.natnet.emulator serializer unit tests for pytest tests/.""" + +from conftest import register_unit_tests, repo_path + +register_unit_tests( + globals(), + repo_path("simulation/isaac-sim/extensions/optitrack.natnet.emulator/test"), + "test_serializers.py", +) diff --git a/tests/sim/optitrack_natnet_emulator/test_server_from_config.py b/tests/sim/optitrack_natnet_emulator/test_server_from_config.py new file mode 100644 index 000000000..69cf75d96 --- /dev/null +++ b/tests/sim/optitrack_natnet_emulator/test_server_from_config.py @@ -0,0 +1,11 @@ +# Copyright (c) 2024 Carnegie Mellon University +# MIT License - see LICENSE in the repository root for full text. +"""Proxy: registers NatNet emulator server-from-config unit tests.""" + +from conftest import register_unit_tests, repo_path + +register_unit_tests( + globals(), + repo_path("simulation/isaac-sim/extensions/optitrack.natnet.emulator/test"), + "test_server_from_config.py", +) diff --git a/tests/sim/optitrack_natnet_emulator/test_server_lifecycle.py b/tests/sim/optitrack_natnet_emulator/test_server_lifecycle.py new file mode 100644 index 000000000..f1eddeed9 --- /dev/null +++ b/tests/sim/optitrack_natnet_emulator/test_server_lifecycle.py @@ -0,0 +1,11 @@ +# Copyright (c) 2024 Carnegie Mellon University +# MIT License - see LICENSE in the repository root for full text. +"""Proxy: registers optitrack.natnet.emulator server lifecycle unit tests.""" + +from conftest import register_unit_tests, repo_path + +register_unit_tests( + globals(), + repo_path("simulation/isaac-sim/extensions/optitrack.natnet.emulator/test"), + "test_server_lifecycle.py", +) diff --git a/tests/sim/optitrack_natnet_emulator/test_target_resolution.py b/tests/sim/optitrack_natnet_emulator/test_target_resolution.py new file mode 100644 index 000000000..803d21939 --- /dev/null +++ b/tests/sim/optitrack_natnet_emulator/test_target_resolution.py @@ -0,0 +1,11 @@ +# Copyright (c) 2024 Carnegie Mellon University +# MIT License - see LICENSE in the repository root for full text. +"""Proxy: registers optitrack.natnet.emulator target-resolution unit tests.""" + +from conftest import register_unit_tests, repo_path + +register_unit_tests( + globals(), + repo_path("simulation/isaac-sim/extensions/optitrack.natnet.emulator/test"), + "test_target_resolution.py", +) diff --git a/tests/sim/optitrack_natnet_emulator/test_unicast_protocol.py b/tests/sim/optitrack_natnet_emulator/test_unicast_protocol.py new file mode 100644 index 000000000..b163407cc --- /dev/null +++ b/tests/sim/optitrack_natnet_emulator/test_unicast_protocol.py @@ -0,0 +1,11 @@ +# Copyright (c) 2024 Carnegie Mellon University +# MIT License - see LICENSE in the repository root for full text. +"""Proxy: registers optitrack.natnet.emulator unicast-protocol unit tests for pytest tests/.""" + +from conftest import register_unit_tests, repo_path + +register_unit_tests( + globals(), + repo_path("simulation/isaac-sim/extensions/optitrack.natnet.emulator/test"), + "test_unicast_protocol.py", +) diff --git a/tests/system/test_build_docker.py b/tests/system/test_build_docker.py index 556e885e5..fb8b4ed1c 100644 --- a/tests/system/test_build_docker.py +++ b/tests/system/test_build_docker.py @@ -8,7 +8,10 @@ class TestDockerBuilds: def _build_and_record(self, service, env=None): result = airstack_cmd("image-build", service, timeout=3600) - assert result.returncode == 0, f"{service} build failed (exit {result.returncode}):\n{read_log_tail()}" + if result.returncode != 0: + pytest.fail( + f"{service} build failed (exit {result.returncode}):\n{read_log_tail()}" + ) size = docker_image_size_mb(service, env=env) if size is not None: diff --git a/tests/system/test_build_packages.py b/tests/system/test_build_packages.py index 5c54cfee3..7a0aaa5fa 100644 --- a/tests/system/test_build_packages.py +++ b/tests/system/test_build_packages.py @@ -32,13 +32,15 @@ def test_colcon_build_robot(self): result = airstack_cmd("up", "robot-desktop", env_overrides={"AUTOLAUNCH": "false", "DISPLAY": ""}, timeout=120) - assert result.returncode == 0, f"airstack up failed:\n{read_log_tail()}" + if result.returncode != 0: + pytest.fail(f"airstack up failed:\n{read_log_tail()}") container = wait_for_container("robot.*desktop", timeout=60) assert container, "Robot container not found" build = docker_exec(container, "bash -ic bws", timeout=600) - assert build.returncode == 0, f"colcon build failed:\n{read_log_tail()}" + if build.returncode != 0: + pytest.fail(f"colcon build failed:\n{read_log_tail()}") finally: airstack_cmd("down") @@ -57,7 +59,8 @@ def test_colcon_test_robot(self): result = airstack_cmd("up", "robot-desktop", env_overrides={"AUTOLAUNCH": "false", "DISPLAY": ""}, timeout=120) - assert result.returncode == 0, f"airstack up failed:\n{read_log_tail()}" + if result.returncode != 0: + pytest.fail(f"airstack up failed:\n{read_log_tail()}") container = wait_for_container("robot.*desktop", timeout=60) assert container, "Robot container not found" @@ -67,16 +70,18 @@ def test_colcon_test_robot(self): "bash -ic \"bws --cmake-args '-DBUILD_TESTING=ON'\"", timeout=600, ) - assert build.returncode == 0, f"colcon build (with testing) failed:\n{read_log_tail()}" + if build.returncode != 0: + pytest.fail(f"colcon build (with testing) failed:\n{read_log_tail()}") test = docker_exec( container, f"bash -ic '{colcon_test_robot_command('robot')}'", timeout=300, ) - assert test.returncode == 0, ( - f"colcon test failed (packages: {', '.join(packages)}):\n{read_log_tail()}" - ) + if test.returncode != 0: + pytest.fail( + f"colcon test failed (packages: {', '.join(packages)}):\n{read_log_tail()}" + ) finally: airstack_cmd("down") @@ -86,13 +91,15 @@ def test_colcon_build_gcs(self): result = airstack_cmd("up", "gcs", env_overrides={"AUTOLAUNCH": "false", "DISPLAY": ""}, timeout=120) - assert result.returncode == 0, f"airstack up failed:\n{read_log_tail()}" + if result.returncode != 0: + pytest.fail(f"airstack up failed:\n{read_log_tail()}") container = wait_for_container("gcs", timeout=60) assert container, "GCS container not found" build = docker_exec(container, "bash -ic bws", timeout=600) - assert build.returncode == 0, f"colcon build failed:\n{read_log_tail()}" + if build.returncode != 0: + pytest.fail(f"colcon build failed:\n{read_log_tail()}") finally: airstack_cmd("down") @@ -106,7 +113,8 @@ def test_colcon_build_ms_airsim(self): "URDF_FILE": "robot_descriptions/iris/urdf/iris_stereo.ms-airsim.urdf"}, timeout=120, ) - assert result.returncode == 0, f"airstack up failed:\n{read_log_tail()}" + if result.returncode != 0: + pytest.fail(f"airstack up failed:\n{read_log_tail()}") container = wait_for_container("ms-airsim", timeout=60) assert container, "ms-airsim container not found" @@ -116,6 +124,7 @@ def test_colcon_build_ms_airsim(self): "cd /root/ros_ws && colcon build --symlink-install", timeout=600, ) - assert build.returncode == 0, f"colcon build failed:\n{read_log_tail()}" + if build.returncode != 0: + pytest.fail(f"colcon build failed:\n{read_log_tail()}") finally: airstack_cmd("down") diff --git a/tests/system/test_liveliness.py b/tests/system/test_liveliness.py index 342e5f49a..a7ccd88fa 100644 --- a/tests/system/test_liveliness.py +++ b/tests/system/test_liveliness.py @@ -7,6 +7,7 @@ Sensor topic rates, bridge stereo Hz, LiDAR echo/sanity, and sim RTF live in ``system/test_sensors.py`` (``@pytest.mark.sensors``), ordered after this module. """ +import os import time import pytest @@ -21,6 +22,7 @@ logger, ros2_exec, sample_compute_usage, + sample_hz, wait_for_first_message, ) @@ -31,6 +33,13 @@ "/robot_{N}/trajectory_controller/trajectory_control_node", ] +# Relative pose topic from the robot's natnet_config.yaml profile (drone body), namespaced +# per robot below. Each profile's drone uses this same leaf, so it holds for multi-robot too. +_NATNET_POSE_TOPIC = os.environ.get("NATNET_POSE_TOPIC", "perception/optitrack/drone") +_NATNET_MIN_HZ = 5.0 +# Cold Isaac boot: Pegasus load, Play, UDP connect — 60s was too tight vs other budgets. +_NATNET_FIRST_MSG_TIMEOUT = 120 + def _parse_panes(raw): """Return (crashed, active_count). Input lines: 'session:window|pane_pid|title|kids'. @@ -251,6 +260,49 @@ def ready(): fail_msg=lambda: f"sentinel nodes not ready after 300s: {last_msg[0]}", ) + @pytest.mark.dependency(depends=["sim_ready", "nodes"]) + def test_natnet_pose_alive(self, airstack_env): + """With LAUNCH_NATNET=true, /{robot}/{natnet_pose_topic}/pose_cov >= 5 Hz. + + In-sim counterpart to the host-side integration tests. Checks the pose_cov + variant of the drone body's configured topic (natnet_config.yaml profile). + Skips when the stack was brought up without LAUNCH_NATNET=true (e.g. msairsim). + """ + cfg = airstack_env["cfg"] + natnet_enabled = cfg.get("extra_env", {}).get("LAUNCH_NATNET", "").lower() in ( + "1", "true", "yes", "on" + ) + if not natnet_enabled: + pytest.skip("LAUNCH_NATNET not set for this sim config — NatNet sentinel skipped") + robot_containers = get_robot_containers(airstack_env["robot_pattern"]) + m = get_metrics() + tid = current_test_id() + + failures = [] + for n in range(1, airstack_env["num_robots"] + 1): + container = robot_containers[n - 1] + topic = f"/robot_{n}/{_NATNET_POSE_TOPIC}/pose_cov" + first = wait_for_first_message( + container, + topic, + domain_id=n, + setup_bash=cfg["robot_setup_bash"], + timeout=_NATNET_FIRST_MSG_TIMEOUT, + ) + if first is None: + failures.append( + f"robot_{n}: no message on {topic} within {_NATNET_FIRST_MSG_TIMEOUT}s" + ) + continue + hz = sample_hz( + container, topic, domain_id=n, setup_bash=cfg["robot_setup_bash"], duration=5, window=20 + ) + m.record(tid, f"natnet_pose_hz_robot_{n}", hz if hz is not None else "none", unit="Hz") + if hz is None or hz < _NATNET_MIN_HZ: + failures.append(f"robot_{n}: {topic} at {hz} Hz (< {_NATNET_MIN_HZ})") + + assert not failures, "NatNet pose sentinel failed: " + "; ".join(failures) + @pytest.mark.dependency(depends=["sim_ready", "nodes", "tmux"]) def test_stable(self, airstack_env, request): """Poll infra only: tmux, sentinel nodes, compute (no sensor topic Hz)."""