OptiTrack (1/3): autonomy fixes — natnet_ros2 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.8"
+VERSION="0.19.0-alpha.9"
 # 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

@@ -14,6 +14,7 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0
 - `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)
+- 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
 

docs/robot/px4_external_vision.md

@@ -0,0 +1,246 @@
+# 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 (USB vs TELEM2 UART).
+3. **Vision pose pipeline** — how a mocap pose becomes a `VISION_POSITION_ESTIMATE`.
+
+> 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` | `15.0` | Measured OptiTrack→EKF2 latency (ms): ~1 ms LAN + ~5 ms Cube hop, rounded up. `natnet_ros2_node` logs the measured figure; retune from Flight Review EV innovations. |
+| `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.01` | Vision **position** noise floor (m). |
+| `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
+(`15.0`) so the MAVLink param type matches the FCU's declaration. Getting this
+wrong makes the set silently reject.
+
+**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)
+
+The Jetson's `mavros` connects to the Cube over a serial link chosen by
+`FCU_URL` in the deployment env
+([`overrides/l4t-px4-realrobot.env`](overrides/l4t-px4-realrobot.env)).
+
+### Option A — USB (`/dev/ttyACM0`) — current, but has a known stall
+
+```
+FCU_URL=/dev/ttyACM0:115200
+```
+
+The Cube auto-starts a MAVLink instance on USB (`SYS_USB_AUTO=2`,
+`cdcacm_autostart`). This works but has a **documented failure mode on this
+hardware**: the Cube Orange (`2dae:1016`) intermittently NAKs USB **OUT**
+transfers for 10–30 s windows, even at low data rates. The symptom is bursts of:
+
+```
+mavconn: 0: DROPPED Message-Id 102 [...] MAVConnSerial::send_message: TX queue overflow
+        at line 165 in ./src/interface.cpp
+```
+
+Every *outbound* message type is affected (102 VISION_POSITION_ESTIMATE, 82, 111,
+0 HEARTBEAT, …), the **inbound** direction stays perfect, and it is **not a
+bandwidth problem** — it reproduces at ~200 B/s. Rate-limiting the vision stream
+does **not** help. Closing and reopening the port clears it. Root cause is the
+Cube's USB CDC-ACM stack under sustained OUT load, not the AirStack side.
+
+**Consequence for external vision:** dropped `102`s mean EKF2 receives vision
+only in bursts, dead-reckons on the IMU between them, and its local estimate
+drifts away from the mocap pose. Fix the link before trusting fusion.
+
+### Option B — TELEM2 UART — recommended for a companion computer
+
+Move the companion link off USB onto the TELEM2 UART (the PX4-recommended
+companion connection), which sidesteps the USB CDC path entirely.
+
+**Wiring:** Cube **TELEM2** ↔ a Jetson UART (e.g. `/dev/ttyTHS1`), TX↔RX
+crossed, common ground. Do **not** connect the FCU 5 V rail to the Jetson.
+
+**PX4 params (set once in QGC, then reboot):**
+
+| Parameter | Value | Meaning |
+|---|---|---|
+| `MAV_1_CONFIG` | `102` | Start a second MAVLink instance on **TELEM2**. (`0` = disabled; `101` = TELEM1, used by the SiK radio.) |
+| `MAV_1_MODE` | `2` | **Onboard** mode — the message set for a companion computer. |
+| `MAV_1_RATE` | `0` | Unlimited (or a byte/s cap if you want to bound the link). |
+| `MAV_1_FORWARD` | `0` | Don't forward other links onto this one. |
+| `SER_TEL2_BAUD` | `115200` | TELEM2 baud (raise to `921600` if the UART and cabling are clean). |
+
+**Deployment env:**
+
+```
+FCU_URL=/dev/ttyTHS1:115200
+```
+
+> No-quotes gotcha: in the compose list-syntax `environment:`, values are
+> literal — `FCU_URL="/dev/ttyTHS1:115200"` passes the quotes through and breaks
+> MAVROS URL parsing. Write it bare.
+
+You can leave the USB MAVLink instance enabled as a spare / QGC-over-USB port;
+it won't conflict with TELEM2.
+
+---
+
+## 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.
+
+---
+
+## 4b. The height datum: why `local_position.z` was ~36 m off
+
+On real hardware the drone reported ~36 m of altitude while sitting on the mocap
+floor. The cause is a **datum interaction**, not a bug in any single component:
+
+- AirStack anchors the world at a shared origin altitude of **90.0 m**, used by sim
+  ([`gps_utils.py`](simulation/isaac-sim/launch_scripts/gps_utils.py)), the GCS
+  (`gcs_utils.py ORIGIN_ALT`), and the synthetic GPS origin. That 90.0 is a **WGS‑84
+  ellipsoidal** height.
+- MAVROS/PX4 convert a GPS-origin altitude from ellipsoidal to **AMSL** using the
+  **egm96‑5 geoid**. At the Lisbon datum the geoid undulation is **N ≈ 54 m**.
+- Publishing the literal 90.0 makes PX4 anchor its vertical frame at
+  `AMSL = 90 − 54 = 36 m`, while OptiTrack says the floor is `z = 0`. The
+  **36 m gap** is exactly `90 − N`.
+
+**Fix — [`mavros_gp_origin_node.py`](robot/ros_ws/src/perception/natnet_ros2/src/mavros_gp_origin_node.py)
+with `use_geoid_altitude: true`:** instead of the literal 90.0, publish the origin
+altitude as `N + desired_floor_amsl` (≈ `54 + 0`). MAVROS's egm96‑5 conversion then
+cancels (`54 − 54 = 0`), so `local_position.z` equals the OptiTrack height (floor = 0).
+`N` is computed at runtime with `GeoidEval` — no hardcoded magic number — using the
+same egm96‑5 model MAVROS uses, so the undulation cancels regardless of its absolute
+value.
+
+**Why this never showed in sim:** the geoid path is auto-skipped when
+`use_sim_time: true` (sim uses the literal 90.0 on both ends), and sim's synthetic
+GPS is self-consistent with the spawn — there's no ellipsoidal-vs-AMSL mismatch. The
+bug is structurally real-hardware-only.
+
+> **Don't "fix" it by changing the 90.0 globally** — it's a shared sim/GCS/origin
+> datum; changing it breaks sim↔GCS consistency. The `gcs_utils.py` altitudes are
+> display-only (visualization), not flight inputs.
+
+---
+
+## 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

@@ -116,6 +116,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:

robot/ros_ws/src/perception/natnet_ros2/CMakeLists.txt

@@ -48,8 +48,7 @@ if(EXISTS "${_NATNET_LIB}" AND EXISTS "${_NATNET_INC}")
 
   # Install libNatNet.so alongside the node and register an environment hook so
   # that sourcing the workspace adds lib/natnet_ros2/ to LD_LIBRARY_PATH.
-  # Use PROGRAMS (not FILES) to preserve the execute bit — shared libraries
-  # must be executable for the dynamic linker to map them.
+  # Use PROGRAMS (not FILES) to preserve the execute bit
   install(PROGRAMS "${_NATNET_LIB}"
     DESTINATION lib/${PROJECT_NAME})
 
@@ -65,10 +64,12 @@ else()
 endif()
 
 # ---------------------------------------------------------------------------
-# Python nodes (vision_pose_converter remains Python)
+# Python nodes
 # ---------------------------------------------------------------------------
 install(PROGRAMS
   src/vision_pose_converter_node.py
+  src/mavros_gp_origin_node.py
+  src/px4_param_setter_node.py
   DESTINATION lib/${PROJECT_NAME})
 
 # ---------------------------------------------------------------------------