Skip to content

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

Closed
JohnYanxinLiu wants to merge 8 commits into
johnliu/test-infra-reworkfrom
johnliu/optitrack-e2e
Closed

OptiTrack e2e: NatNet emulator + real-robot PX4 external-vision fusion#373
JohnYanxinLiu wants to merge 8 commits into
johnliu/test-infra-reworkfrom
johnliu/optitrack-e2e

Conversation

@JohnYanxinLiu

@JohnYanxinLiu JohnYanxinLiu commented Jul 23, 2026

Copy link
Copy Markdown
Collaborator

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

  • Which GitHub issue does this address?

    Final phase of the OptiTrack work. Supersedes Johnliu/optitrack emulation #367 — rebuilt cleanly on the reworked test harness instead of rebasing Johnliu/optitrack emulation #367's 44 interleaved commits, and folds in the real-robot PX4 external-vision fixes that lived only on the Hummingbird repo.

    Stacked on Test infra rework: YAML-driven unit-test collection + integration tier #372 (johnliu/test-infra-rework) — base this PR on johnliu/test-infra-rework so the diff shows only the 8 OptiTrack commits. Retarget to develop after l4t deployment fixes: make the Jetson profile build + boot on real hardware #371 + Test infra rework: YAML-driven unit-test collection + integration tier #372 merge.

  • Additional description if not fully described in the GitHub issue

    Two capabilities, plus the test wiring:

    1. Sim NatNet emulator (end-to-end without hardware) — simulation/isaac-sim/extensions/optitrack.natnet.emulator/** streams rigid-body poses as a real NatNet server, so natnet_ros2 can be driven in Isaac Sim. Plus the natnet Pegasus launch scripts.

    2. Real-robot PX4 external-vision fusion — extends natnet_ros2:

    • mavros_gp_origin publishes a guarded synthetic GPS origin. On real hardware use_geoid_altitude feeds the egm96-5 geoid undulation so mavros's ellipsoidal→AMSL conversion cancels and local_position.z == OptiTrack z — fixing the ~36 m boot offset (90 m ellipsoidal world datum − ~54 m geoid = 36 m; invisible in sim because the geoid path is skipped under use_sim_time).
    • vision_pose_converter bridges mocap pose → MAVROS vision_pose.
    • px4_param_setter is a checker, not an enforcer: auto_set off by default (read + flag, never write), on_mismatch = warn (default) | halt. Set the EKF2 params in QGC; the node is the pre-flight safety net.

How did you implement it?

  • Extract-by-path + harness reconcile, not a git-rebase of Johnliu/optitrack emulation #367 (both Johnliu/optitrack emulation #367 and the base rewrote the test harness). The emulator/natnet_ros2 content is taken by path; the test bits are reconciled onto the base's YAML-driven collection.
  • Emulator unit tests register via one sim: entry in tests/colcon_unit_test_packages.yaml (the base's simulation/**/<pkg>/test glob) — replacing Johnliu/optitrack emulation #367's 14 proxy files + register_unit_tests.
  • No isaacsim_natnet in the default sim matrix. That would re-run the whole liveliness/sensors/flight suite under NatNet for two assertions. Instead: the cheap host-side tests/integration/natnet/ (emulator → natnet_ros2 Hz, no GPU) + one dedicated tests/system/test_optitrack_e2e.py (optitrack mark, single NatNet Isaac bring-up).
  • Fixed a real bug found along the way: the emulator's test/conftest.py collided with the root conftest under --import-mode=importlib; the root conftest now puts each unit-test package's import root on sys.path generically, so the per-package conftest is removed.
  • Excludes the duplicate vendored NatNet SDK (sensors/natnet_ros2, ~7k lines of proprietary headers) and deployment-specific override .envs.

How do you run and use it?

# Host-side (no GPU): emulator → natnet_ros2 pose Hz
airstack test -m integration -v

# Full sim e2e (needs GPU + Isaac Sim): emulator → natnet_ros2 → PX4 EV fusion
airstack test -m "build_docker or optitrack" --sim isaacsim -v

# Emulator + natnet_ros2 unit tests (hermetic)
airstack test -m unit -v

Testing with PyTest

  • Unit (unit): emulator server/protocol + natnet_ros2 co-located tests — 153 pass locally, 0 failures. Loosened one emulator pose assertion to pytest.approx(abs=1e-2) (float32/USD-decompose noise).
  • Integration (integration): tests/integration/natnet/ — host emulator → natnet_ros2 pose Hz, no GPU.
  • OptiTrack e2e (optitrack, new mark): tests/system/test_optitrack_e2e.py — one Isaac bring-up asserting emulator → natnet_ros2 pose_cov ≥ 5 Hz then PX4 local_position alive. Skips when isaac-sim isn't built.
  • Expected: -m unit clean; the sim e2e passes on a GPU runner (single-robot; multi-robot flight variants are hardware-bound).

Documentation

  • Was mkdocs.yml updated? (y/n) y — added Perception → PX4 External Vision (mocap) → docs/robot/px4_external_vision.md (the setup guide + the height-datum explainer + the param-checker flags). natnet_ros2/README.md covers the client.

Versioning

🤖 Generated with Claude Code

JohnYanxinLiu and others added 8 commits July 23, 2026 12:05
@JohnYanxinLiu @claude
feat(sim): add OptiTrack NatNet emulator isaac-sim extension + natnet…
2a68a3f 
… launch scripts

Extracted from the optitrack_emulation branch (#367) onto the reworked test-infra
base. Adds the sim-side NatNet server emulator so the natnet_ros2 client can be
exercised end-to-end in Isaac Sim without OptiTrack hardware:
- simulation/isaac-sim/extensions/optitrack.natnet.emulator/** — NatNet server
  emulation, USD bindings, UI extension, and its co-located unit tests.
- launch_scripts: example_one/multi_px4_pegasus_natnet_launch_script.py (spawn the
  emulator alongside PX4), and example_one_px4_pegasus_launch_script.py gains the
  shared GPS world-origin setup (gps_utils) so sim, GCS, and the robot agree on the
  world datum.
- docs/simulation/isaac_sim/natnet_emulator.md + optitrack-development skill.

Co-Authored-By: Claude Opus 4.8 <noreply@anthropic.com>
@JohnYanxinLiu @claude
feat(perception): bring natnet_ros2 client up to the optitrack_emulat…
93791c1 
…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>
@JohnYanxinLiu @claude
feat(natnet): real-robot PX4 external-vision fusion (mocap → EKF2)
46d663e 
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>
@JohnYanxinLiu @claude
test(natnet): register emulator unit tests via the package YAML + deps
38ca197 
Reconcile #367's optitrack test bits onto the base's YAML-driven collection instead
of its old proxy/register_unit_tests pattern:
- colcon_unit_test_packages.yaml: add a `sim:` workspace listing
  optitrack.natnet.emulator. The base's sim glob (simulation/**/<pkg>/test) collects
  its 15 co-located test files directly — replaces #367's 14 proxy files under
  tests/sim/optitrack_natnet_emulator/.
- tests/requirements.txt: scipy + usd-core (emulator server math + USD bindings).
- emulator test/conftest.py: drop the stale "CI uses proxies" note.
- perception.launch.xml: natnet include comment points at the natnet_config schema.

No isaacsim_natnet added to the default sim matrix (that would re-run the whole
system suite under natnet — the dedicated e2e test lands next instead).

Co-Authored-By: Claude Opus 4.8 <noreply@anthropic.com>
@JohnYanxinLiu @claude
test(natnet): host integration test + dedicated sim e2e (optitrack mark)
b4255e2 
Two-tier OptiTrack testing instead of re-running the whole system suite under a
third `isaacsim_natnet` sim:
- tests/integration/natnet/ (from #367): host emulator → natnet_ros2 pose Hz, no
  GPU. Uses the base's integration tier (robot_autonomy_stack fixture, `integration`
  mark) as-is.
- tests/system/test_optitrack_e2e.py (new, `optitrack` mark): one dedicated Isaac
  bring-up (example_one_px4_pegasus_natnet_launch_script + LAUNCH_NATNET=true, 1
  robot) asserting only the NatNet chain — emulator → natnet_ros2 pose_cov >= 5 Hz,
  then PX4 local_position alive (EKF2 fusing the vision). Skips when isaac-sim
  isn't built.

Fix a conftest collision: the emulator's test/conftest.py was imported as bare
`conftest` under --import-mode=importlib, shadowing tests/conftest.py and breaking
`from conftest import` in the system tests. Root conftest now puts each unit-test
package's import root on sys.path generically, so the per-package conftest is
removed. Register the `optitrack` mark and slot the e2e last in _MODULE_ORDER.

Co-Authored-By: Claude Opus 4.8 <noreply@anthropic.com>
@JohnYanxinLiu @claude
docs(natnet): PX4 external-vision setup guide + height-datum explainer
5c43f64 
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>
@JohnYanxinLiu @claude
test(natnet): loosen emulator pose tolerance for float32/USD noise
826e952 
test_streamed_frame_carries_sampled_usd_pose compared the sampled pose to 3-decimal
exactness (`round(x,3) == 7.0`), which trips on float32 + USD xform-decompose noise
(7.0 sampled back as 7.001) depending on the usd-core version. Use pytest.approx with
abs=1e-2 — mm precision is more than enough for the emulator's pose round-trip.

Co-Authored-By: Claude Opus 4.8 <noreply@anthropic.com>
@JohnYanxinLiu @claude
chore: bump version to 0.19.0-alpha.9
601802e 
Version-increment gate: bump above the test-infra base (alpha.8). Record the
OptiTrack emulator + real-robot external-vision fusion + tests in the changelog.

Co-Authored-By: Claude Opus 4.8 <noreply@anthropic.com>
@JohnYanxinLiu JohnYanxinLiu mentioned this pull request Jul 23, 2026
Closed
@JohnYanxinLiu

JohnYanxinLiu commented Jul 23, 2026

Copy link
Copy Markdown
Collaborator Author

Split into three smaller, stacked PRs for reviewability (same content, re-partitioned by feature/test-tier):

Closing in favor of #374 → #375 → #376.

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Reviewers

No reviews

Assignees

No one assigned

Labels

None yet

Projects

None yet

Milestone

No milestone

Development

Successfully merging this pull request may close these issues.

1 participant

@JohnYanxinLiu