OptiTrack (2/3): NatNet server emulator + host integration tests by JohnYanxinLiu · Pull Request #375 · castacks/AirStack

JohnYanxinLiu · 2026-07-23T18:21:11Z

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

Review just this PR's own change:
git diff johnliu/optitrack-autonomy...johnliu/optitrack-natnet-server

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

Which GitHub issue does this address?

None. #374 added a NatNet client that could only be exercised with a real OptiTrack Motive
server. This adds an emulated server, so the client can be developed and tested without
mocap hardware.

Additional description if not fully described in the GitHub issue

A pure-Python NatNet server emulator (optitrack.natnet.emulator) that speaks enough of the
OptiTrack Motive wire protocol for natnet_ros2 to connect to it and receive rigid-body poses:
the command/data socket pair, connect and model-definition request handling, frame
serialization, and rigid-body tracking-valid flags.

It is the protocol core only — no Isaac or USD dependency, so it runs anywhere Python does. The
Isaac integration that drives it from a live sim scene is #376.

Also adds host integration tests (integration mark) that stand the emulator up in-process,
point the real natnet_ros2 client at it, and assert poses arrive on the ROS side at rate.
These need a robot container but no sim and no GPU, which puts real coverage on the #374
client in a tier that can run cheaply.

Please add videos and images

N/A — a protocol library and its tests; no user-facing surface.

How did you implement it?

Split so the protocol is testable in isolation:

serializers / frames — pure functions from rigid-body samples to NatNet wire bytes.
server lifecycle — sockets, connect/model-definition request handling, the publish loop.
catalog — the set of streamed bodies (name, streaming id, pose source), so a caller adds
bodies without touching protocol code.

That boundary is what lets #376 attach Isaac USD prims as pose sources without the protocol
layer knowing Isaac exists.

Emulating the server rather than replaying a capture was deliberate: it exercises the client's
real connect/discovery path, which is where most of the failure modes live. A wrong body id or
server IP produces a connected client that silently never publishes — the exact class of bug
#374 fixes — and only a live handshake reproduces it.

How do you run and use it?

The emulator is a library; the integration tests are the entry point.

# emulator server -> natnet_ros2 client, on the host (robot container, no GPU)
airstack test -m integration

To drive it by hand, see
simulation/isaac-sim/extensions/optitrack.natnet.emulator/README.md. Then point a client at it
by setting NATNET_SERVER_IP to the host running the emulator and bringing up the robot stack
with LAUNCH_NATNET=true.

The streamed body must match the robot's profile in natnet_config.yaml — the client filters
frames by numeric id, and a mismatch is silent on both sides.

For the one-command version of this — emulator running inside Isaac Sim, streaming a live
flying drone — see #376.

Testing with PyTest

What pytests did you add?

Two tiers:

unit — the emulator's own co-located test/ (~15 files): serializers, frame assembly,
discovery/connect handling, server lifecycle, catalog behavior, target resolution, pose
sampling, defaults. Pure Python, no sockets to a real server, no Docker.
integration (tests/integration/natnet/) — stands the emulator up on the host, runs the
real natnet_ros2 client against it, asserts the ROS topic publishes at rate. Needs the robot
container; no sim, no GPU.
Exact command

airstack test -m unit          # emulator protocol tests
airstack test -m integration   # emulator -> natnet_ros2 end to end on the host

Expected results

Both green. The integration tier is the meaningful one: it proves a real client connects to this
emulator and receives poses, which is the whole reason the emulator exists.

Documentation

Was mkdocs.yml updated? (y/n)

n — the emulator documents itself in
simulation/isaac-sim/extensions/optitrack.natnet.emulator/README.md (protocol scope, layout,
how to run the tests), and the integration tier in tests/integration/natnet/README.md. The
user-facing sim documentation lands with the Isaac integration in #376, where there is
something to click.

Sufficient scope?

Yes for a protocol library: what is emulated, what is deliberately not, and how to drive it.

Sufficient visual media?

N/A.

Versioning

Version bumped?

Yes — 0.19.0-alpha.14.

🤖 Generated with Claude Code

…ion baseline Take the natnet_ros2 package from #367 onto the reworked base: the C++ NatNet client (natnet_ros2_node + client adapter + natnet_logic seam), the base mavros_gp_origin and vision_pose_converter nodes, per-robot natnet_config profiles, launch files, and the co-located C++/Python unit tests. natnet_ros2 is already listed in tests/colcon_unit_test_packages.yaml, so the base's YAML-driven collection picks up the updated unit tests directly — no proxy files. Real-robot PX4 external-vision fusion (px4_param_setter, geoid-corrected origin, EV-pose bounds) is layered on next. Co-Authored-By: Claude Opus 4.8 <noreply@anthropic.com>

Layer the Hummingbird real-robot fusion pipeline onto natnet_ros2 so an OptiTrack-only drone (no GNSS/mag/baro) fuses mocap pose into PX4 EKF2: - mavros_gp_origin_node: publishes a guarded synthetic GPS origin. On real HW, use_geoid_altitude feeds the egm96-5 geoid undulation (N ≈ 54 m at Lisbon) so mavros's ellipsoidal→AMSL conversion cancels and local z == OptiTrack z (fixes the ~36 m = 90 − 54 boot offset; see docs). Auto-skipped in sim. - vision_pose_converter_node: rate-limited mocap → MAVROS vision_pose bridge. - px4_params.yaml: the external-vision EKF2 param set. - natnet_ros2.launch.py wires the bridges when a robot's vision_pose block is on. px4_param_setter reworked into a **checker** (R3): auto_set=false by default — it reads and *flags* FCU params that differ from the desired set instead of writing them; on_mismatch=warn|halt (default warn). Set the params in QGroundControl; the node is the pre-flight safety net. auto_set=true restores the legacy enforce path. Excludes the duplicate vendored NatNet SDK (sensors/natnet_ros2) and deployment override .envs. Co-Authored-By: Claude Opus 4.8 <noreply@anthropic.com>

Move the PX4 external-vision setup guide into docs/ (was a repo-root markdown) and wire it into the mkdocs nav under Perception. Adapt it to the reworked param checker (auto_set default off; check-and-flag, not enforce), and add a "height datum" section explaining the ~36 m local_z offset: AirStack's 90.0 ellipsoidal world datum minus the egm96-5 geoid undulation (N ≈ 54 m at Lisbon) = 36 m; fixed by publishing the geoid-corrected origin altitude so mavros's conversion cancels. Documents why it's invisible in sim and why the shared 90.0 datum must not be changed globally. Co-Authored-By: Claude Opus 4.8 <noreply@anthropic.com>

…hema Refine the perception bringup comment on the LAUNCH_NATNET include so it points at the per-robot natnet_config.yaml schema parsed by natnet_ros2.launch.py. Co-Authored-By: Claude Opus 4.8 <noreply@anthropic.com>

…uning Three defects that together meant the OptiTrack client could never connect to anything, in sim or on a real robot. 1. NATNET_SERVER_IP was unreachable config. natnet_config.yaml resolves it via $(env ...), but docker compose only injects variables named in a service's `environment:` block and no service declared it — not the compose files, not .env, not tests/system/test_optitrack_e2e.py. The client therefore always fell back to its hardcoded default (192.168.123.199), which is neither the in-sim emulator (172.31.0.200) nor any Motive host. Forwarded in robot-base-docker-compose.yaml, defaulting to the emulator so the sim path works unconfigured. 2. The tracked rigid body could never match. robot_1 pinned "Hummingbird" id 1146 while the emulator streams "Drone" id 1, and the NatNet client filters incoming frames by NUMERIC id — a mismatch yields a connected client that silently never publishes. Body name/id now accept $(env ...) (expanded in _build_node_params, with the id still coerced to int) and default to the emulator's body; sites override via NATNET_BODY_NAME / NATNET_BODY_ID. 3. EV tuning was not the deployment-validated set. EKF2_EV_DELAY 8.0 -> 7.0 and EKF2_EVP_NOISE 0.01 -> 0.05. EKF2_EVP_NOISE is not marker precision: it also sets the innovation gate at EKF2_EVP_GATE (default 5) sigma, so 0.01 gave a 5 cm gate that rejected legitimate mocap updates and refused to arm. 0.05 is a 25 cm gate, still far tighter than PX4's 0.1 default. px4_params.yaml keeps the evidence inline, including two results that are expensive to rediscover: raising EKF2_EV_DELAY to 50.0 measurably degrades tracking (the negative best-fit time shift shows the estimate running ahead of truth), and the drift-and-snap excursions were a 90 deg body-yaw offset in the Motive rigid-body definition, not a gate problem — so the fix belongs in Motive, never as yaw compensation in code. Adds two unit tests covering body-field env expansion and the emulator-matching defaults (natnet_ros2: 14 -> 16 passing). Co-Authored-By: Claude Opus 5 <noreply@anthropic.com>

Mocap counterpart to l4t-px4-realrobot.env: same Jetson stack, plus the NatNet server/body settings and LAUNCH_NATNET. Carries the two things that are easy to get wrong and produce no error. The body id must match Motive's streaming id, since the client filters frames numerically and a mismatch just never publishes. And nothing writes the EKF2 external-vision parameters to a real FCU — px4_param_setter only reads them back and warns — so they have to be set once in QGroundControl. Co-Authored-By: Claude Opus 5 <noreply@anthropic.com>

The rigid body a robot tracks is now set only in its natnet_config.yaml profile, keyed by ROBOT_NAME. NATNET_BODY_NAME / NATNET_BODY_ID are gone: a single global env var cannot express per-robot values, so it blocked the multi-robot case the profiles already handle. NATNET_SERVER_IP stays in the environment — one Motive host serves every robot. Comments across the package are cut back to what is not evident from the code. The EKF2 tuning results that were buried in px4_params.yaml move into docs/robot/px4_external_vision.md, which also had stale values (EV_DELAY 15.0, EVP_NOISE 0.01) contradicting the config: that raising EV_DELAY measurably hurts tracking, and that drift-and-snap was a Motive rigid-body yaw offset rather than a gate problem. Kept: the license header, and the note on why the SDK needs a reachability pre-check before Connect(). Co-Authored-By: Claude Opus 5 <noreply@anthropic.com>

desired_floor_amsl 0.0 -> 36.0, the world datum (90 m ellipsoidal) expressed in AMSL, so a mocap robot's reported global altitude agrees with sim and the GCS instead of sitting at sea level. The published ellipsoidal origin works out to ~90 m, the datum itself. local_position.z equals the OptiTrack height for any value of this parameter — it only moves the global altitude. Reasoning lives in the external-vision doc, which also now records that GeoPoint.altitude is ellipsoidal by contract, so AMSL must not be sent here. Not yet confirmed on hardware. Co-Authored-By: Claude Opus 5 <noreply@anthropic.com>

MAVROS constructs the egm96-5 geoid in its UAS core, before any plugin loads, and throws std::invalid_argument if the dataset is absent — mavros_node terminates at startup, so there is no MAVROS at all, GPS or mocap. The image could ship without it. mavros' install_geographiclib_datasets.sh sends the downloader's output to /dev/null and, on failure, prints "Error while installing" and returns without a non-zero exit, so the RUN layer succeeded regardless. The tool it calls, geographiclib-get-geoids, was also only a transitive dependency of ros-mavros rather than something we pinned. Now pins geographiclib-tools and asserts the file landed, so a failed download fails the build. Verified against the shipped image: with the downloader broken the script still exits 0, and the new test -f returns non-zero. This is the dependency the OptiTrack external-vision path needs — mavros_gp_origin resolves the geoid undulation with the same egm96-5 model — hence landing it here. Co-Authored-By: Claude Opus 5 <noreply@anthropic.com>

They resolved relative to docs/robot/, so mkdocs looked for docs/robot/robot/ros_ws/... and warned on every one. Prefixed with ../../; the file now builds warning-free. Co-Authored-By: Claude Opus 5 <noreply@anthropic.com>

Section 3 documented MAVLink serial setup at length — MAV_n_CONFIG / SER_TEL2_BAUD tables, wiring, USB-vs-TELEM2 comparison — all of which is standard PX4 setup that PX4 documents better and keeps current. Replaced with links to the companion computer, MAVLink peripherals, and serial configuration pages. Kept the part PX4 does not cover: the Cube Orange USB CDC-ACM stall, which starves EKF2 of vision updates and is why the companion link belongs on TELEM2. Four other sections and the troubleshooting table point here for that symptom. 65 lines -> 19. Co-Authored-By: Claude Opus 5 <noreply@anthropic.com>

Section 4 now leads with what mavros_gp_origin does — inject a synthetic global position so PX4 will arm in modes that need one without GNSS — rather than presenting the height datum as a peer topic. The ~36 m offset becomes a note under it, scoped to real deployments and ending with why sim never sees it (the geoid path is skipped under use_sim_time, and sim's synthetic GPS is self-consistent with the spawn). Section 4b is gone; it had no inbound references. Dropped the "don't change the 90.0 globally" warning. Co-Authored-By: Claude Opus 5 <noreply@anthropic.com>

validate_connection_type returned "unicast" for anything it did not recognise, so "mutlicast" or "Unicast" produced a client that connected on the wrong transport and then never received a frame — with only a warning to show for it. It now throws std::invalid_argument naming the offending value, and the node turns that into a fatal startup error rather than a warning it flies past. Case-sensitivity is deliberate: accepting "Unicast" would mean the config silently disagrees with itself. Tests updated from fallback to throw, plus one asserting the message names the bad value. 60 gtests pass. Co-Authored-By: Claude Opus 5 <noreply@anthropic.com>

Comment trims in natnet_ros2_node.cpp (no code change). Records what cube_orange_latency_ms actually is: an estimate of the FCU hop, added to a logged total and never fused. Only the transport half of EKF2_EV_DELAY is measured, and that measurement starts at the NatNet server transmit, so Motive's own capture pipeline is not in it either. Also notes, for whoever retunes next, that the node stamps poses with its receive time — so delay after that stamp does not belong in EKF2_EV_DELAY, which points lower than 7.0 and matches the negative best-fit shift already recorded. Not chased down; 7.0 flies. CameraMidExposureTimestamp would replace the estimate with a measurement if it ever matters. Co-Authored-By: Claude Opus 5 <noreply@anthropic.com>

Replaces the two long tuning write-ups with a short troubleshooting tip (check the Motive rigid-body definition first — x forward, z up) and cuts the latency section back to what is measured versus estimated. Fixed a dangling "see below" in the EKF2_EV_DELAY table row, which pointed at the removed tuning result; the warning it carried is now stated inline. Co-Authored-By: Claude Opus 5 <noreply@anthropic.com>

… tests The pure-Python NatNet server that emulates an OptiTrack Motive server so natnet_ros2 can be driven without hardware. USD/Isaac-free — this is the protocol + server core (unicast server, data/model/server types, serializers, default catalogs). The Isaac wrapper that maps a USD scene onto this server lands next. Registers the emulator package's co-located unit tests via a `sim:` entry in tests/colcon_unit_test_packages.yaml (base's simulation/**/<pkg>/test glob). The root conftest now puts each unit-test package's import root on sys.path so co-located tests import their package without a per-package conftest.py. Co-Authored-By: Claude Opus 4.8 <noreply@anthropic.com>

Drive the real natnet_ros2 client from the host NatNet server emulator and check the drone pose reaches ROS at rate (single-body and multi-body profiles). No sim, no GPU — uses the base's `robot_autonomy_stack` fixture + `integration` mark. The Isaac-wrapper variant lands with the Isaac wrapper PR. Co-Authored-By: Claude Opus 4.8 <noreply@anthropic.com>

JohnYanxinLiu mentioned this pull request Jul 23, 2026

OptiTrack e2e: NatNet emulator + real-robot PX4 external-vision fusion #373

Closed

JohnYanxinLiu mentioned this pull request Jul 31, 2026

Robot deployment fixes: bag recording + adding warning for robot-identity failure #377

Merged

JohnYanxinLiu and others added 7 commits August 5, 2026 15:54

chore: bump version to 0.19.0-alpha.13

6b0eb12

JohnYanxinLiu force-pushed the johnliu/optitrack-autonomy branch from eaf4b13 to a1287ca Compare August 13, 2026 15:29

JohnYanxinLiu force-pushed the johnliu/optitrack-natnet-server branch from 4e19fba to dfb75fb Compare August 13, 2026 15:31

JohnYanxinLiu changed the base branch from johnliu/optitrack-autonomy to develop August 13, 2026 15:33

This was referenced Aug 13, 2026

OptiTrack (1/3): robot-side NatNet client + PX4 external-vision fusion #374

Open

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

Draft

JohnYanxinLiu force-pushed the johnliu/optitrack-natnet-server branch from dfb75fb to 2952d14 Compare August 13, 2026 16:04

JohnYanxinLiu force-pushed the johnliu/optitrack-natnet-server branch from 2952d14 to d3fca87 Compare August 13, 2026 17:12

JohnYanxinLiu force-pushed the johnliu/optitrack-natnet-server branch from d3fca87 to 8c13642 Compare August 13, 2026 20:33

JohnYanxinLiu and others added 2 commits August 14, 2026 12:35

abbreviated Dockerfile comment on geographic lib installation

691fee0

fix repo-root doc links in the external-vision guide

5fc20f3

They resolved relative to docs/robot/, so mkdocs looked for docs/robot/robot/ros_ws/... and warned on every one. Prefixed with ../../; the file now builds warning-free. Co-Authored-By: Claude Opus 5 <noreply@anthropic.com>

JohnYanxinLiu force-pushed the johnliu/optitrack-natnet-server branch from 8c13642 to 460c5e3 Compare August 14, 2026 17:00

JohnYanxinLiu and others added 2 commits August 14, 2026 13:02

comment trim

a8db780

JohnYanxinLiu force-pushed the johnliu/optitrack-natnet-server branch from 460c5e3 to f4d6e23 Compare August 14, 2026 20:51

JohnYanxinLiu force-pushed the johnliu/optitrack-natnet-server branch from f4d6e23 to 59225a9 Compare August 14, 2026 21:22

JohnYanxinLiu force-pushed the johnliu/optitrack-natnet-server branch from 59225a9 to 9ee5db3 Compare August 14, 2026 21:30

px4 external vision docs trim

0264cb2

JohnYanxinLiu force-pushed the johnliu/optitrack-natnet-server branch 2 times, most recently from bdfeca3 to 6cbe1b0 Compare August 14, 2026 21:37

JohnYanxinLiu force-pushed the johnliu/optitrack-natnet-server branch from 6cbe1b0 to 3756ffc Compare August 14, 2026 21:45

JohnYanxinLiu and others added 4 commits August 14, 2026 17:53

chore: bump version to 0.19.0-alpha.14

461204c

JohnYanxinLiu force-pushed the johnliu/optitrack-natnet-server branch from 3756ffc to 461204c Compare August 14, 2026 21:54

Navigation Menu

Uh oh!

OptiTrack (2/3): NatNet server emulator + host integration tests - #375

OptiTrack (2/3): NatNet server emulator + host integration tests#375
JohnYanxinLiu wants to merge 22 commits into
developfrom
johnliu/optitrack-natnet-server

JohnYanxinLiu commented Jul 23, 2026 •

edited

Loading

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

1 participant

Uh oh!

Conversation

JohnYanxinLiu commented Jul 23, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

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

How did you implement it?

How do you run and use it?

Testing with PyTest

Documentation

Versioning

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

1 participant

JohnYanxinLiu commented Jul 23, 2026 •

edited

Loading