OptiTrack (2/3): NatNet server emulator + host integration tests - #375
Draft
JohnYanxinLiu wants to merge 11 commits into
Draft
OptiTrack (2/3): NatNet server emulator + host integration tests#375JohnYanxinLiu wants to merge 11 commits into
JohnYanxinLiu wants to merge 11 commits into
Conversation
…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>
JohnYanxinLiu
force-pushed
the
johnliu/optitrack-autonomy
branch
from
August 13, 2026 15:29
eaf4b13 to
a1287ca
Compare
JohnYanxinLiu
force-pushed
the
johnliu/optitrack-natnet-server
branch
from
August 13, 2026 15:31
4e19fba to
dfb75fb
Compare
JohnYanxinLiu
changed the base branch from
johnliu/optitrack-autonomy
to
develop
August 13, 2026 15:33
This was referenced Aug 13, 2026
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>
… 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
force-pushed
the
johnliu/optitrack-natnet-server
branch
from
August 13, 2026 16:04
dfb75fb to
2952d14
Compare
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
1 participant
Add this suggestion to a batch that can be applied as a single commit.This suggestion is invalid because no changes were made to the code.Suggestions cannot be applied while the pull request is closed.Suggestions cannot be applied while viewing a subset of changes.Only one suggestion per line can be applied in a batch.Add this suggestion to a batch that can be applied as a single commit.Applying suggestions on deleted lines is not supported.You must change the existing code in this line in order to create a valid suggestion.Outdated suggestions cannot be applied.This suggestion has been applied or marked resolved.Suggestions cannot be applied from pending reviews.Suggestions cannot be applied on multi-line comments.Suggestions cannot be applied while the pull request is queued to merge.Suggestion cannot be applied right now. Please check back later.
What features did you add and/or bugs did you 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.
A pure-Python NatNet server emulator (
optitrack.natnet.emulator) that speaks enough of theOptiTrack Motive wire protocol for
natnet_ros2to 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 (
integrationmark) that stand the emulator up in-process,point the real
natnet_ros2client 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.
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:
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.
To drive it by hand, see
simulation/isaac-sim/extensions/optitrack.natnet.emulator/README.md. Then point a client at itby setting
NATNET_SERVER_IPto the host running the emulator and bringing up the robot stackwith
LAUNCH_NATNET=true.The streamed body must match the robot's profile in
natnet_config.yaml— the client filtersframes by numeric id, and a mismatch is silent on both sides.
Testing with PyTest
Two tiers:
unit— the emulator's own co-locatedtest/(~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 thereal
natnet_ros2client against it, asserts the ROS topic publishes at rate. Needs the robotcontainer; no sim, no GPU.
Exact command
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
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. Theuser-facing sim documentation lands with the Isaac integration in #376, where there is
something to click.
Yes for a protocol library: what is emulated, what is deliberately not, and how to drive it.
N/A.
Versioning
Yes —
0.19.0-alpha.14.🤖 Generated with Claude Code