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	27 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 publishes a geoid-corrected synthetic
   GPS origin so local_position.z equals OptiTrack z (fixing a ~36 m boot offset),
   vision_pose_converter bridges the mocap pose to MAVROS, and px4_param_setter checks
   the FCU's EKF2 parameters (auto_set: false by default; on_mismatch warn/halt).
3. Deployment override — overrides/l4t-optitrack-realrobot.env, the mocap counterpart to
   l4t-px4-realrobot.env.

Also fixes three defects 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. Now forwarded in
  robot-base-docker-compose.yaml.

• 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).

• 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.

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.

Tuning rationale lives in the docs, not in comments. docs/robot/px4_external_vision.md
gained a "Two tuning results worth not rediscovering" section, and its parameter table — which
was stale, still showing EKF2_EV_DELAY 15.0 / EKF2_EVP_NOISE 0.01 — was corrected to
match the config:

• Raising EKF2_EV_DELAY is measurably worse, not better. 50.0 vs 7.0: median |odom −
  mocap| while moving 0.037 → 0.060 m. The negative best-fit time shift is the tell — over-
  declaring delay makes EKF2 attribute the measurement to too old a state, so the estimate runs
  ahead of truth. It can never compensate for apparent lag.
• The drift-and-snap excursions were not a gate problem — they were a 90° body-yaw offset in
  the Motive rigid-body definition. Fixing the rigid body cut moving error 0.25 → 0.04 m.
  Do not add yaw compensation in code; natnet_ros2 and vision_pose_converter are
  deliberate identity pass-throughs and a code-side correction would double-compensate.

Comments across the package were cut back to what is not evident from the code. Two kept
deliberately: the license header, and the note on why the SDK needs a reachability pre-check
before Connect() (it can SIGABRT instead of returning an error, so the check looks redundant
and is not).

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.

• Exact command

airstack test -m unit

• Expected results

All pass; natnet_ros2 contributes 14. They are hermetic — no Docker, sim or GPU.

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, frame
conventions, height datum), a parameter table with the reasoning per entry, the two tuning
results above, and a symptom → cause → where-to-look troubleshooting table.

• Sufficient visual media?

Diagrams of the data path and the height-datum explainer are in the guide.

Versioning

• Version bumped?

Yes — 0.19.0-alpha.13.

🤖 Generated with Claude Code