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

.env

@@ -12,7 +12,7 @@ PROJECT_NAME="airstack"
 # If you've run ./airstack.sh setup, then this will auto-generate from the git commit hash every time a change is made 
 # to a Dockerfile or docker-compose.yaml file. Otherwise this can also be set explicitly to make a release version.
 # auto-generated from git commit hash
-VERSION="0.19.0-alpha.12"
+VERSION="0.19.0-alpha.13"
 # Choose "dev" or "prebuilt". "dev" is for mounted code that must be built live. "prebuilt" is for built ros_ws baked into the image
 DOCKER_IMAGE_BUILD_MODE="dev"  
 # Where to push and pull images from. Can replace with your docker hub username if using docker hub.

CHANGELOG.md

@@ -9,13 +9,15 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0
 
 ### Added
 
+- `overrides/l4t-optitrack-realrobot.env` — deployment override for a real Jetson robot flying on OptiTrack mocap (PX4 EKF2 external vision instead of GPS): the NatNet server/body settings, plus the multi-NIC and FCU-parameter notes that path needs
 - Feature notebook workflow (`use-feature-notebook` skill): every agent-implemented feature gets a local, gitignored `notebook/NNN-feature-slug/` entry with a status-tracked `design_spec.md` (written before coding) and `results/` artifacts + self-contained `results_summary.md` that populate the feature's PR description
 - Battery and telemetry display in GCS RQT control panel (voltage and percentage per robot when MAVROS battery topic is bridged)
 - `TARGET_ARCH` build arg (default `x86_64`) in `Dockerfile.robot` to arch-parametrize `LD_LIBRARY_PATH`; `docker-compose.yaml` passes `TARGET_ARCH: aarch64` to the `voxl` and `l4t` real-robot image builds
 - `ros-${ROS_DISTRO}-mavros-extras` in the robot image (provides the vision_pose plugin used for external-pose deployments)
 - `overrides/l4t-px4-realrobot.env` — site-agnostic deployment override for a single real PX4 robot on a Jetson (aarch64/l4t)
 - `integration` test tier (`tests/integration/`, `integration` mark) with a shared `robot_autonomy_stack` fixture (robot container, no sim/GPU)
 - `waypoint_flight` system test (`tests/system/test_waypoint_flight.py`): takeoff → ordered waypoint route via `NavigateTask` (dispatched as a dense plan) → land, judged on the odometry track by the standalone stdlib-only `tests/waypoint_checker.py` (in-order corridor arrival within `--waypoint-tolerance`, final goal within `--goal-tolerance`, per-waypoint `--waypoint-timeout`); validated end-to-end in Isaac Sim; serves as the standard acceptance check after integrating or swapping a planner module
+- Real-robot PX4 external-vision fusion in `natnet_ros2` (OptiTrack mocap → EKF2): `mavros_gp_origin` (geoid-corrected synthetic GPS origin so `local_position.z` == OptiTrack z, fixing the ~36 m boot offset), `vision_pose_converter`, and a PX4 param **checker** (`px4_param_setter`, `auto_set` off by default; `on_mismatch` warn/halt) — setup guide at `docs/robot/px4_external_vision.md`
 
 ### Changed
 
@@ -33,6 +35,12 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0
 - Dropped `ROBOT_NAME` / `ROS_DOMAIN_ID` from `overrides/l4t-px4-realrobot.env`: no compose service declares either, so an env file could never set them and the lines were inert
 - `bag_record/bag_recording_status` was bridged GCS -> robot in `domain_bridge.yaml`, the same direction as the command it answers, so recorder status never reached the GCS and every recording indicator stayed blank
 - `bag_record_node` passed `--exclude` to `ros2 bag record`, which Jazzy renamed to `--exclude-regex`. It is now an ambiguous prefix of four options, so argparse rejected the command and any section using `exclude:` (including `log.yaml`'s `airstack` section, i.e. everything but the cameras) recorded nothing — surfacing only as a usage dump in the node's stdout. Multiple `exclude:` entries are now alternated into one regex instead of repeating a single-valued flag, which had silently kept only the last
+- `natnet_config.yaml`'s `$(env NATNET_SERVER_IP ...)` could never resolve: no compose service declared the variable, so the NatNet client always fell back to its hardcoded default and could reach neither the in-sim emulator nor a real Motive host. It is now forwarded in `robot-base-docker-compose.yaml`, defaulting to the in-sim emulator
+- The NatNet rigid body tracked by `robot_1` defaulted to a site-specific body (id 1146) that no emulator streams; since the client filters frames by numeric id, that produced a connected client that never published. It now defaults to the emulator's body (`Drone`, id 1). Per-robot bodies are configured in each robot's profile in `natnet_config.yaml`, selected by `ROBOT_NAME`
+- OptiTrack external-vision tuning corrected from real-flight bags: `EKF2_EV_DELAY` 8.0 → 7.0 and `EKF2_EVP_NOISE` 0.01 → 0.05. The old 0.01 gave a 5 cm innovation gate (`EKF2_EVP_GATE` × 5σ) that rejected valid mocap updates and blocked arming; `px4_params.yaml` now records the supporting measurements and the drift-and-snap misdiagnosis so neither is repeated
+- The synthetic GPS origin now places the mocap floor at the shared world datum (`desired_floor_amsl: 36.0`, i.e. 90 m ellipsoidal in AMSL) rather than at sea level, so a mocap robot's reported global altitude agrees with sim and the GCS. `local_position.z` still equals the OptiTrack height either way
+- The robot image could ship without the GeographicLib `egm96-5` geoid: mavros' `install_geographiclib_datasets.sh` swallows a failed download and still exits 0, so the `RUN` layer succeeded either way, and `geographiclib-tools` was only ever a transitive dependency. MAVROS builds that geoid in its UAS core before any plugin loads and throws if it is missing, so `mavros_node` died at startup on affected images. `Dockerfile.robot` now pins the tool and asserts the file exists, failing the build instead
+- An unrecognised `connection_type` in `natnet_config.yaml` silently fell back to `unicast`, so a typo produced a client that connected on the wrong transport and never received frames. `validate_connection_type` now throws and `natnet_ros2_node` fails at startup naming the offending value
 
 ## [1.0.0] - 2024-12-19
 

docs/robot/px4_external_vision.md

@@ -0,0 +1,216 @@
+# PX4 External-Vision (OptiTrack) Setup
+
+Runbook for flying a PX4 vehicle (Cube Orange) on **OptiTrack mocap as the sole
+position source** — no GNSS, no magnetometer — with an onboard companion
+computer (Jetson) running the AirStack robot stack.
+
+It covers three things that must all be right:
+
+1. **EKF2 parameters** — tell PX4 to fuse external vision instead of GPS/baro/mag.
+2. **Companion MAVLink link** — how the Jetson talks to the Cube (see the PX4 docs).
+3. **Vision pose pipeline** — how a mocap pose becomes a `VISION_POSITION_ESTIMATE`,
+   and how PX4 gets a global position without GNSS.
+
+> Scope: PX4 ≥ 1.14 (the `EKF2_EV_CTRL` / `EKF2_GPS_CTRL` era). For older
+> firmware use `EKF2_AID_MASK: 24` and `EKF2_HGT_MODE: 3` instead of the bitmask
+> params below.
+
+---
+
+## 1. EKF2 parameters (external vision)
+
+These are enforced automatically at startup by the `px4_param_setter` node (see
+below), sourced from
+[`robot/ros_ws/src/perception/natnet_ros2/config/px4_params.yaml`](../../robot/ros_ws/src/perception/natnet_ros2/config/px4_params.yaml).
+You can also set them by hand in QGroundControl — PX4 persists parameters, so
+either way it's a one-time thing per airframe.
+
+| Parameter | Value | Meaning |
+|---|---|---|
+| `EKF2_EV_CTRL` | `11` | Fuse vision **horizontal pos (1) + vertical pos (2) + yaw (8)**. Add bit **4** (velocity) only if a vision *speed* source is also streamed. |
+| `EKF2_HGT_REF` | `3` | Vision is the primary height reference (not baro / GPS). |
+| `EKF2_GPS_CTRL` | `0` | No GPS fusion. |
+| `EKF2_MAG_TYPE` | `5` | Magnetometer disabled — yaw comes from vision. |
+| `EKF2_BARO_CTRL` | `0` | No baro fusion; height is pure vision. Set to `1` to keep baro as a backup height source. |
+| `EKF2_EV_DELAY` | `7.0` | OptiTrack→EKF2 latency (ms): ~0.7 ms measured LAN transport + a ~5 ms *estimated* FCU hop. **Raising this does not compensate for apparent lag — it makes the estimate run ahead of truth.** |
+| `EKF2_EV_NOISE_MD` | `1` | Use the `EKF2_EV*_NOISE` floors below instead of the message covariance (which is `1e-6` — too optimistic to fuse safely). |
+| `EKF2_EVP_NOISE` | `0.05` | Vision **position** noise floor (m). Not marker precision — it also sets the innovation gate, `EKF2_EVP_GATE` (default 5) sigma wide, so this is a 25 cm gate. |
+| `EKF2_EVA_NOISE` | `0.05` | Vision **angle** noise floor (rad). |
+| `COM_ARM_WO_GPS` | `1` | Allow arming without GPS. |
+
+**Type matters.** Integers are written bare (`11`); floats need a decimal point
+(`7.0`) so the MAVLink param type matches the FCU's declaration. Getting this
+wrong makes the set silently reject.
+
+### Troubleshooting tips
+
+If you see drift-and-snap, **check the Motive PC rigid-body definition first** and ensure the x axis points forward. Then, make sure that Motive is streaming the position with z-axis up.
+
+### The latency figure is only partly measured
+
+`EKF2_EV_DELAY` is currently `7.0` ms: roughly `0.7` measured plus a `5.0` estimate
+(`cube_orange_latency_ms` in `natnet_config.yaml`). Only the first part is empirically measured currently.
+
+- **Measured:** `natnet_ros2_node` derives transport latency from the NatNet
+  `TransmitTimestamp` — i.e. from *server transmit* to client receipt. It does not
+  include Motive's own capture→transmit pipeline (exposure, centroiding, solving),
+  which is typically several ms and happens before that clock starts.
+- **Estimated:** `cube_orange_latency_ms` models the MAVROS → MAVLink → uORB → EKF2 hop.
+  It is **estimated only**.
+
+**Reboot after any change.** Fusion-source (`EKF2_*`) params are safest applied
+from a clean estimator start — reboot the flight controller before flying. The
+param setter prints a warning whenever it actually changes something.
+
+---
+
+## 2. The param checker (`px4_param_setter`)
+
+Set the table above **once in QGroundControl**. To catch a mis-configured FCU
+before flight, the stack runs a one-shot node at startup that **checks** the live
+params against the desired set. **By default it only checks and flags — it does not
+write to the FCU.**
+
+- **Node:** [`px4_param_setter_node.py`](../../robot/ros_ws/src/perception/natnet_ros2/src/px4_param_setter_node.py)
+- **Config:** [`config/px4_params.yaml`](../../robot/ros_ws/src/perception/natnet_ros2/config/px4_params.yaml)
+  (everything under `params.` is a desired FCU parameter)
+- **Launch:** [`launch/px4_param_setter.launch.xml`](../../robot/ros_ws/src/perception/natnet_ros2/launch/px4_param_setter.launch.xml),
+  included from `natnet_ros2.launch.py` when the robot's `vision_pose` block is enabled.
+
+Two safety flags in `px4_params.yaml`:
+
+| Flag | Default | Behaviour |
+|------|---------|-----------|
+| `auto_set` | `false` | `false`: read + compare only, never write. `true`: also push mismatched params via `param/set` and verify (the legacy enforce path). |
+| `on_mismatch` | `warn` | With `auto_set: false`, on a wrong param — `warn`: log the diffs, keep the stack up. `halt`: log fatal + exit non-zero so a `required` launch node tears the stack down before flight. |
+
+Per parameter it waits for an FCU connection + `settle_sec` (default 10 s), reads
+the current value, and compares (float32 tolerance). A clean run logs
+`10 already correct, 0 mismatched`. A mismatch under the default (`auto_set: false`,
+`on_mismatch: warn`) logs, e.g., `EKF2_HGT_REF: FCU has 1, expected 3 (not set —
+auto_set=false). Fix in QGroundControl.`
+
+Disable it entirely with `enabled: false`.
+
+> **The checker does NOT configure the companion link** (`MAV_*` / `SER_*`
+> params in section 3) — those are set once in QGC.
+
+---
+
+## 3. Companion MAVLink link (Jetson ↔ Cube)
+
+`mavros` reaches the FCU over the serial link named by `FCU_URL` in the deployment env.
+Configuring that link is standard PX4 setup, not AirStack-specific — see the PX4 docs:
+
+- [Companion computer setup](https://docs.px4.io/main/en/companion_computer/)
+- [MAVLink peripherals (`MAV_n_CONFIG`, `MAV_n_MODE`)](https://docs.px4.io/main/en/peripherals/mavlink_peripherals.html)
+- [Serial port configuration](https://docs.px4.io/main/en/peripherals/serial_configuration.html)
+
+Use the **TELEM2 UART** for the companion link rather than USB. On Cube Orange the USB
+CDC-ACM path intermittently stalls outbound transfers for 10–30 s at a time — visible as
+`DROPPED Message-Id 102 … TX queue overflow` — which starves EKF2 of vision updates and
+makes it dead-reckon between bursts. It is not a bandwidth problem and rate-limiting the
+vision stream does not help.
+
+> In compose list-syntax `environment:`, values are literal — write `FCU_URL=/dev/ttyTHS1:115200`
+> bare. Quoting it passes the quotes through and breaks MAVROS URL parsing.
+
+
+## 4. Vision pose pipeline (mocap → PX4)
+
+```
+Motive (OptiTrack, 100 Hz)
+  → natnet_ros2_node        publishes the rigid body as a ROS pose (ENU)
+  → vision_pose_converter   rate-limit + quaternion canonicalize (passthrough)
+  → mavros vision_pose      converts ENU→NED, sends VISION_POSITION_ESTIMATE (msg 102)
+  → PX4 EKF2                fuses per the params in section 1
+```
+
+**Frame convention — the thing to get right.** MAVROS's `vision_pose` plugin
+expects **ROS ENU** and converts to PX4 NED internally. The
+[`vision_pose_converter_node.py`](../../robot/ros_ws/src/perception/natnet_ros2/src/vision_pose_converter_node.py)
+does **no coordinate transform** — it only rewrites `frame_id`, optionally
+canonicalizes the quaternion sign (`qw ≥ 0`), and rate-limits. **So
+`natnet_ros2_node` must already publish ENU.** If position/yaw come out rotated
+or axis-swapped, fix it there, not in the converter.
+
+**Rate limiting.** `max_rate_hz` (default 50 in
+[`vision_pose_converter.yaml`](../../robot/ros_ws/src/perception/natnet_ros2/config/vision_pose_converter.yaml))
+caps the stream to MAVROS. EKF2 only needs 30–50 Hz. Note this is about not
+saturating a healthy serial link — it does **not** fix the USB CDC stall in
+section 3.
+
+### Injecting a global position — `mavros_gp_origin`
+
+Vision gives PX4 a valid *local* position, but with GNSS disabled it has no *global*
+one, and modes that require a global position (e.g. `AUTO.LOITER`) refuse to arm.
+
+[`mavros_gp_origin_node.py`](../../robot/ros_ws/src/perception/natnet_ros2/src/mavros_gp_origin_node.py)
+publishes a **synthetic GPS origin** once at startup, which lets PX4 derive a global
+position from the fused vision estimate. It waits for MAVROS to connect, listens for an
+existing origin, and only publishes if none is present — so a GNSS-equipped vehicle is
+left untouched. Location and behaviour come from
+[`mavros_gp_origin.yaml`](../../robot/ros_ws/src/perception/natnet_ros2/config/mavros_gp_origin.yaml);
+the defaults match the AirStack shared world datum so sim, the GCS, and the robot agree
+on where world origin sits on Earth.
+
+!!! note "Real deployments: the origin altitude needs a geoid correction"
+
+    `geographic_msgs/GeoPoint.altitude` is a height above the **WGS-84 ellipsoid**, and
+    MAVROS converts it to AMSL with the **egm96-5 geoid** before handing it to PX4. Send
+    the shared datum's literal `90.0` and PX4 anchors its vertical frame at
+    `AMSL = 90 − N ≈ 36 m`, while OptiTrack says the floor is `z = 0` — the drone reads
+    ~36 m of altitude sitting on the floor. The gap is exactly the geoid undulation `N`.
+
+    With `use_geoid_altitude: true` the node publishes `N + desired_floor_amsl` instead,
+    computing `N` at runtime via `GeoidEval` with the same egm96-5 model MAVROS uses, so
+    the conversion cancels out.
+
+    `desired_floor_amsl` chooses what AMSL the mocap floor reports; `local_position.z`
+    equals the OptiTrack height either way. We use **36.0**, the shared datum in AMSL, so
+    the robot's global altitude agrees with sim and the GCS.
+
+    **Not needed in sim.** The geoid path is skipped when `use_sim_time: true`: sim's
+    synthetic GPS is self-consistent with the spawn and uses the literal datum altitude
+    on both ends, so there is no ellipsoidal-vs-AMSL mismatch to correct.
+
+
+## 5. Verify it's actually fusing
+
+**Live, in the QGC MAVLink _Console_** (not the Inspector — it can't see
+companion→FCU messages):
+
+```
+listener vehicle_visual_odometry     # should be steady ~50 Hz, not gappy
+listener estimator_status
+```
+
+On the ROS side: `/{ROBOT_NAME}/interface/mavros/local_position/pose` should
+publish and track the mocap. Hand-lift test: raise the vehicle, Z should go up
+(Motive Z-up correct); translate it and check the sign/axis match.
+
+**Definitive, from the SD-card ulog** ([Flight Review](https://logs.px4.io) or
+PlotJuggler):
+
+- `estimator_innovations` → **`ev_hpos` / `ev_vpos` / `ev_yaw`** and their
+  **test ratios**. Ratio > 1 ⇒ EKF2 is *rejecting* the measurement
+  (frame / timing / covariance). Near-zero with occasional gaps ⇒ fusing fine
+  but starved by dropped messages (section 3).
+- `estimator_status_flags` → **`cs_ev_pos` / `cs_ev_yaw`** — confirms EV fusion
+  is actually active. If unset, EKF2 isn't fusing vision regardless of params.
+- `vehicle_visual_odometry` rate in the log quantifies how many `102`s actually
+  arrived.
+
+---
+
+## Troubleshooting quick reference
+
+| Symptom | Likely cause | Where to look |
+|---|---|---|
+| `DROPPED Message-Id 102 … TX queue overflow` | Cube USB CDC OUT stall | Section 3 → move to TELEM2 |
+| mavros local pos drifts away from mocap over time | Dropped `102`s starving EKF2 | Fix link first, then recheck |
+| Constant rotation between mocap and EKF2 pose | Yaw/frame misalignment | `natnet_ros2_node` frame (must be ENU); `EKF2_EV_CTRL` yaw bit |
+| Axes swapped / uncorrelated | Wrong frame convention | `natnet_ros2_node`, not the converter |
+| Param set "rejected or readback mismatch" | Wrong literal type (int vs float) | Section 1 — floats need a decimal point |
+| Won't arm | GPS still required | `COM_ARM_WO_GPS: 1`, reboot |
+| EV innovation test ratio > 1 | EKF2 rejecting vision | Retune `EKF2_EV_DELAY`, `EKF2_EVP_NOISE` / `EKF2_EVA_NOISE` |

mkdocs.yml

@@ -117,6 +117,7 @@ nav:
           - Perception:
               - docs/robot/autonomy/perception/index.md
               - NatNet (OptiTrack): robot/ros_ws/src/perception/natnet_ros2/README.md
+              - PX4 External Vision (mocap): docs/robot/px4_external_vision.md
           - Local:
               - docs/robot/autonomy/local/index.md
               - World Model:

overrides/l4t-optitrack-realrobot.env

@@ -0,0 +1,35 @@
+# Real-robot deployment on an NVIDIA Jetson (aarch64 / l4t) flying on OptiTrack mocap:
+# PX4 EKF2 fuses the mocap pose as external vision instead of GPS. Use
+# overrides/l4t-px4-realrobot.env instead if the vehicle flies on GPS.
+#
+# Build:  airstack image-build --profile l4t robot-l4t
+# Run:    airstack up --env-file overrides/l4t-optitrack-realrobot.env robot-l4t
+#
+# Setup guide (PX4 parameters, frames, troubleshooting):
+#   docs/robot/px4_external_vision.md
+
+COMPOSE_PROFILES="l4t"
+AUTOLAUNCH="true"
+NUM_ROBOTS="1"
+AUTONOMY_ROLE="full"
+
+# --- Robot identity -----------------------------------------------------------
+# Resolved from this device's hostname: name the Jetson robot-1 on the HOST
+#   hostnamectl set-hostname robot-1     -> robot_1 on domain 1
+
+# --- OptiTrack / NatNet -------------------------------------------------------
+LAUNCH_NATNET="true"
+# Motive host. No sensible default — set this before the first flight.
+NATNET_SERVER_IP="192.168.1.100"
+
+# --- Flight controller (MAVROS) ----------------------------------------------
+# Jetson UART; some airframes wire the FCU through USB-serial instead
+# (e.g. /dev/ttyUSB0:921600).
+FCU_URL="/dev/ttyTHS4:115200"
+
+# --- Robot description --------------------------------------------------------
+URDF_FILE="robot_descriptions/iris/urdf/iris_with_sensors.pegasus.robot.urdf"
+
+# --- Flight-data recording ----------------------------------------------------
+BAG_STORAGE_PATH="/media/airlab/Storage/airstack_collection"
+RECORD_BAGS="false"