OptiTrack (3/3): Isaac wrapper, mocap EV fusion in sim, and a Circle-trajectory e2e

[JohnYanxinLiu]

Stacked PR — 3 of 3. All three target develop. Merge in order:
#374 → #375 → #376. This PR's diff currently includes #374's and #375's commits; it
shrinks to the 39 files below once those merge.

Review just this PR's own change:

git diff johnliu/optitrack-natnet-server...johnliu/optitrack-isaac-wrapper

What features did you add and/or bugs did you address?

• Which GitHub issue does this address?

None. #375 added a NatNet server emulator with no pose source. This drives it from a live Isaac
Sim scene, so the whole OptiTrack stack runs in simulation with no mocap hardware.

• Additional description if not fully described in the GitHub issue

• Isaac wrapper — a USD scene interface (NatNetInterface prim + schema) binding stage
  prims to streamed rigid bodies, so the emulator streams a live flying drone's pose. Includes
  natnet variants of the single- and multi-drone Pegasus launch scripts.

• overrides/isaac-natnet-vision.env — one command to bring up Isaac + emulator + PX4 on
  mocap external-vision fusion.

• Sim e2e (optitrack mark) — flies a Circle trajectory on mocap EKF2 fusion.

• PR CI now enforces exactly that one flight instead of pytest's full defaults.

Four bugs fixed along the way, all found by actually running it:

1. PX4 never fused the mocap. EKF2_EV_CTRL defaults to 0 (verified in the firmware
   shipped in our image) and the isaac compose set no PX4 parameters at all, so the emulator
   could stream perfectly while PX4 flew on sim GPS. The compose now passes the EKF2
   external-vision set as PX4_PARAM_*, which PX4 SITL's rcS applies at boot.
2. The e2e test proved nothing. test_px4_fuses_vision asserted only that
   local_position/pose publishes — which it does off GPS regardless. It passed with external
   vision disabled. Replaced with the Circle flight.
3. The launch scripts could not import the emulator — ModuleNotFoundError: No module named 'optitrack'. Being on a Kit --ext-folder search path only makes Kit aware of an
   extension; it does not put the package on sys.path.
4. Emulator/client body mismatch was possible and silent. The two ends are now pinned
   together explicitly (see below).

• Please add videos and images

Startup, from the Isaac tmux pane:

[ext: optitrack.natnet.emulator-0.1.0] startup
[natnet] Server started on 172.31.0.200 (cmd 1510 / data 1511) with 2 body(ies).
[natnet] Emulator started: 'Drone' (-> /World/base_link/body), 'Target' (-> /World/target)

Measured on the robot side with the stack up:

Stage	Rate
perception/optitrack/drone/pose_cov	101 Hz
interface/mavros/vision_pose/pose_cov	33 Hz
interface/mavros/local_position/pose	32 Hz

How did you implement it?

EV parameters are opt-in and default to no-op. Each PX4_PARAM_* entry defaults to PX4's
own default, read out of the firmware in this image (EKF2_EV_CTRL=0, EKF2_HGT_REF=1,
EKF2_GPS_CTRL=7, …). Leaving them unset is therefore an explicit no-op and every non-mocap sim
is unaffected; the mocap path opts in. They cannot be defined-but-empty — the rcS loop has no
empty-value guard and would run param set X "". Values mirror px4_params.yaml from #374, so
sim and the real robot run the same EV configuration.

The streamed body is a constant in the launch script, not an env var.
NATNET_BODY_NAME / NATNET_BODY_ID are set in
example_one_px4_pegasus_natnet_launch_script.py and must match a body entry in the robot's
profile in natnet_config.yaml. The client reads its bodies from that per-robot profile, which
is what lets each robot in a multi-robot scene track a different body — a single global env var
could not express that. The pairing is documented inline in both scripts, in the emulator sim
doc, and in the optitrack-development skill, including the failure mode: the client filters
frames by numeric id, so a mismatch gives a client that connects and then never publishes,
with no error on either side.

Extension install mirrors pegasus.simulator: Dockerfile.isaac-ros bakes a copy into the
Kit shared exts dir and pip install -es it; compose bind-mounts the repo copy over it so edits
stay live; the launch scripts enable_extension() before importing.

The e2e flies a Circle rather than hovering. With GPS, baro and range aiding all disabled,
mocap is the only position source — so tracking a circle within a cross-track tolerance
exercises the entire chain under sustained lateral motion, which is where a wrong EV delay or a
too-tight innovation gate actually shows up. It reuses test_fixed_trajectory.py's phase chain
and cross-track scoring rather than reimplementing them.

CI scope: the pull_request branch previously passed no args, so opening any PR ran every
mark, both sims, all four trajectory types. It now runs this one flight. Everything else remains
available on demand — /pytest <args> comments, workflow_dispatch inputs, local
airstack test.

How do you run and use it?

# Required once — the extension install is a Dockerfile change
airstack image-build --profile isaac-sim

# Isaac + emulator + PX4 on mocap EV fusion, one command
airstack up --env-file overrides/isaac-natnet-vision.env

Isaac logs to its tmux pane, not docker logs:

docker exec isaac-sim tmux capture-pane -p -t isaac -S -200 | grep -i natnet
# expect: [natnet] Server started on 172.31.0.200 ... with 2 body(ies).

Then confirm the chain on the robot:

SETUP='source /root/AirStack/robot/ros_ws/install/setup.bash'
docker exec -e ROS_DOMAIN_ID=1 airstack-robot-desktop-1 bash -lc \
  "$SETUP && ros2 topic hz /robot_1/perception/optitrack/drone/pose_cov"     # ~100 Hz
docker exec -e ROS_DOMAIN_ID=1 airstack-robot-desktop-1 bash -lc \
  "$SETUP && ros2 topic hz /robot_1/interface/mavros/local_position/pose"    # ~30 Hz

Negative control, worth running once — it is what proves the test isn't vacuous. Set
PX4_EV_CTRL="0" in the override and re-run: with GPS also off there is no position source, so
the flight must fail. If it still passes, the test is not proving fusion.

Use airstack up --env-file ..., not raw docker compose --env-file ... — compose replaces
.env rather than layering it, so a direct invocation blanks VERSION and the registry.
airstack.sh passes both files.

Testing with PyTest

• What pytests did you add?

The optitrack sim e2e: emulator → natnet_ros2 at rate (dependency gate), then takeoff →
Circle trajectory → land, flown with mocap as the only position source, scored on
cross-track error.

• Exact command

airstack test -m optitrack

• Expected results

The drone completes the circle within tolerance. On develop this test passes while PX4 flies
on GPS; on this branch it only passes if EKF2 is genuinely fusing the mocap stream.

Honest status: the emulator chain is verified live (rates above, measured in a running
container) and the EV parameters are verified to resolve and reach PX4 SITL. The full
airstack test -m optitrack flight has not yet been run end to end — it needs a GPU Isaac
bring-up. Reviewers with a GPU runner, that is the thing to exercise.

Documentation

• Was mkdocs.yml updated? (y/n)

y — adds the emulator sim documentation to the nav.

• Sufficient scope?

Yes: how the emulator binds to a USD scene, how to bring the stack up with one override, a
"Changing which body is streamed" section naming exactly which two files must agree and warning
that a mismatch fails silently, the sim-vs-real differences, and an optitrack-development
skill for agents working in this area.

• Sufficient visual media?

The startup log and the measured rate table above are the observable surface; the sim itself is
the demo.

Versioning

• Version bumped?

Yes — 0.19.0-alpha.15.

🤖 Generated with Claude Code