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

[JohnYanxinLiu]

Stacked PR — 1 of 3. All three target develop so each can be reviewed and tested
independently. Merge in order: #374 → #375 → #376. Because they are stacked, #375 and
#376 currently show this PR's commits too; those diffs shrink automatically as each merges.

PR	Adds	Own diff
#374 (this)	robot-side OptiTrack client + PX4 external-vision fusion	28 files
#375	NatNet server emulator + host integration tests	25 files
#376	Isaac wrapper, sim EV fusion, Circle e2e	39 files

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

• Which GitHub issue does this address?

None — this is the robot-side half of running AirStack on OptiTrack motion capture, brought
up against a real Jetson + Cube Orange in a mocap room.

• Additional description if not fully described in the GitHub issue

Lets PX4 fly on mocap instead of GPS: natnet_ros2 connects to a Motive server,
publishes the tracked rigid body, and a bridge feeds it to PX4 EKF2 as external vision.

Three pieces:

1. natnet_ros2 client brought up to the optitrack_emulation baseline — a natnet:
   config schema describing multiple robots and multiple rigid bodies per robot, plus
   per-message latency reporting.
2. PX4 external-vision fusion — mavros_gp_origin injects a synthetic global position
   (so modes needing one will arm without GNSS) with a geoid-corrected origin altitude,
   vision_pose_converter bridges the mocap pose to MAVROS, and px4_param_setter
   checks the FCU's EKF2 parameters (auto_set: false by default; warn/halt).
3. Deployment override — overrides/l4t-optitrack-realrobot.env, the mocap counterpart
   to l4t-px4-realrobot.env.

Bugs fixed, all found while deploying:

• NATNET_SERVER_IP could never resolve. natnet_config.yaml reads it via
  $(env ...), but no compose service declared it, so the client always fell back to a
  hardcoded default and could reach neither the in-sim emulator nor a real Motive host.

• The tracked rigid body could never match. robot_1's profile pinned a site-specific
  body (id 1146) that no emulator streams. The client filters frames by numeric id, so
  a mismatch produces a connected client that silently never publishes. It now defaults to
  the emulator's body (Drone, id 1).

• An unknown connection_type silently became unicast. A typo like mutlicast
  produced a client on the wrong transport that never received a frame, with only a warning.
  validate_connection_type now throws and the node fails at startup naming the bad value.

• The image could ship without the GeographicLib egm96-5 geoid, and MAVROS will not
  start without it — it builds the geoid in its UAS core before any plugin loads and
  throws if the file is absent, so mavros_node terminates outright. Confirmed on the
  Jetson: the l4t image needs that download for MAVROS to run in offboard. mavros' own
  install_geographiclib_datasets.sh swallows a failed download and still exits 0, and
  geographiclib-tools was only a transitive dependency — Dockerfile.robot now pins the
  tool and asserts the file landed, failing the build instead.

• EV tuning corrected from flight bags — 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 valid mocap
  updates and blocked arming. 0.05 is a 25 cm gate, still far tighter than PX4's 0.1 default.

• Please add videos and images

N/A — no GUI component. Setup guide: docs/robot/px4_external_vision.md.

How did you implement it?

Per-robot bodies live in natnet_config.yaml, not the environment. Each
robots.<robot_name> profile names the bodies that robot tracks, and the launch file
selects the profile by ROBOT_NAME. A single global env var cannot express per-robot
values, so it would block the multi-robot case the profiles already handle.
NATNET_SERVER_IP stays in the environment because one Motive host serves every robot.

px4_param_setter is a checker, not a writer. Nothing here writes parameters to a real
FCU; they are set once in QGroundControl and the node warns on mismatch, so a missed
parameter shows up in the logs rather than in the air.

The synthetic GPS origin is geoid-corrected. geographic_msgs/GeoPoint.altitude is a
height above the WGS-84 ellipsoid and MAVROS converts it to AMSL with egm96-5. Publishing
the shared datum's literal 90.0 anchors PX4 at AMSL = 90 − N ≈ 36 m while mocap says the
floor is z = 0 — the drone reads ~36 m sitting on the floor. The node publishes
N + desired_floor_amsl instead, computing N at runtime so the conversion cancels
exactly. desired_floor_amsl: 36.0 puts the floor at the shared datum, so the robot's
global altitude agrees with sim and the GCS. Not yet confirmed on hardware.

Rationale lives in docs/robot/px4_external_vision.md rather than in comments, and its
parameter table — which was stale, still showing EV_DELAY 15.0 / EVP_NOISE 0.01 — was
corrected to match the config.

How do you run and use it?

Real robot (Jetson):

# 1. Set the Motive host
$EDITOR overrides/l4t-optitrack-realrobot.env       # NATNET_SERVER_IP

# 2. Set the tracked rigid body to match Motive, in this robot's profile
$EDITOR robot/ros_ws/src/perception/natnet_ros2/config/natnet_config.yaml
#        robots.robot_1.bodies[0].rigid_body_name / .id

# 3. Set the EKF2 parameters once in QGroundControl — docs/robot/px4_external_vision.md

# 4. Bring up the Jetson stack
airstack image-build --profile l4t robot-l4t
airstack up --env-file overrides/l4t-optitrack-realrobot.env robot-l4t

Verify the chain in order — each stage failing points at a different cause:

SETUP='source /root/AirStack/robot/ros_ws/install/setup.bash'

# mocap arrives (silent => wrong server IP, or body id doesn't match Motive)
docker exec -e ROS_DOMAIN_ID=1 <robot-container> bash -lc \
  "$SETUP && ros2 topic hz /robot_1/perception/optitrack/drone/pose_cov"

# bridged to MAVROS
docker exec -e ROS_DOMAIN_ID=1 <robot-container> bash -lc \
  "$SETUP && ros2 topic hz /robot_1/interface/mavros/vision_pose/pose_cov"

# PX4 fused it
docker exec -e ROS_DOMAIN_ID=1 <robot-container> bash -lc \
  "$SETUP && ros2 topic echo --once /robot_1/interface/mavros/local_position/pose"

Watch the robot container's startup logs for px4_param_setter mismatch warnings — that is
the check that the FCU is actually configured for external vision.

Sim reviewers: this PR is the robot half only and has no emulator to talk to. To exercise
it without hardware, review #376, which adds the in-sim NatNet emulator and a
one-command override.

Testing with PyTest

• What pytests did you add?

natnet_ros2 unit tests (unit mark, co-located test/) covering the config-flattening
helpers: $(env ...) expansion for the server block, per-robot profile selection, body-list
flattening, covariance defaults, topic namespacing, and the quaternion canonicalization in
vision_pose_converter.

There is also a C++ gtest suite (test_natnet_logic.cpp) over the SDK-free logic —
covariance assembly, topic naming, connection config, frame helpers, and the negotiation
seam — which runs under colcon test in the container rather than pytest.

• Exact command

airstack test -m unit

• Expected results

All pass; natnet_ros2 contributes 14 pytest cases. They are hermetic — no Docker, sim
or GPU. The gtest suite is 60 cases.

The hardware path (a real Motive server and FCU) is not covered by automated tests. #375
adds host integration tests that drive this client from an emulated server, and #376
adds a sim e2e flight.

Documentation

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

y — adds docs/robot/px4_external_vision.md to the nav.

• Sufficient scope?

Yes. The guide covers the three things that must all be right (EKF2 parameters, the
companion MAVLink link, the vision pose pipeline), a parameter table with the reasoning per
entry, the height-datum explanation, and a symptom → cause → where-to-look troubleshooting
table. Companion-link setup links out to the PX4 docs rather than restating them.

• Sufficient visual media?

Data-path diagrams are in the guide; there is no GUI surface.

Versioning

• Version bumped?

Yes — 0.19.0-alpha.13.

---

Known gaps

• The desired_floor_amsl: 36.0 choice is not yet confirmed on hardware — verify the
  reported global altitude on the next mocap flight.
• EKF2_EV_DELAY is only partly measured: ~0.7 ms of it is a real transport measurement,
  the ~5 ms FCU hop is an estimate (diagnostic only, never fused). Documented in the guide;
  7.0 flies and this has not been chased down.

🤖 Generated with Claude Code

[JohnYanxinLiu]

Code manually reviewed and verified on real hardware.