diff --git a/.env b/.env index 04de90a2d..70736e9ac 100644 --- a/.env +++ b/.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.14" # 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. diff --git a/CHANGELOG.md b/CHANGELOG.md index 68f37e9b0..2453e74e6 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -9,6 +9,7 @@ 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 @@ -16,6 +17,8 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0 - `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` +- NatNet server emulator (`optitrack.natnet.emulator`, protocol core) — pure-Python OptiTrack Motive server emulation so `natnet_ros2` can be driven without hardware; host integration tests (`tests/integration/natnet/`) wire it to the robot client ### Changed @@ -33,6 +36,10 @@ 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 ## [1.0.0] - 2024-12-19 diff --git a/docs/robot/px4_external_vision.md b/docs/robot/px4_external_vision.md new file mode 100644 index 000000000..d69fcc336 --- /dev/null +++ b/docs/robot/px4_external_vision.md @@ -0,0 +1,292 @@ +# 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` | `7.0` | Measured OptiTrack→EKF2 latency (ms): ~0.7 ms LAN + ~5 ms Cube hop. `natnet_ros2_node` logs the measured figure. **Do not raise this to chase apparent lag — see below.** | +| `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. + +### Two tuning results worth not rediscovering + +Both came out of back-to-back flight bags with everything else held constant. + +**Raising `EKF2_EV_DELAY` makes tracking worse, not better.** 50.0 was trialled against +7.0: median |odom − mocap| while moving went 0.037 → 0.060 m, and X-axis RMS 0.023 → +0.049 m. The tell is the *negative* best-fit time shift (−20 ms → −60 ms, pinned at the +sweep edge): over-declaring the delay makes EKF2 attribute the measurement to a state +that is too old, so the estimate runs **ahead** of truth during motion rather than +behind. Raising this value can never compensate for apparent lag in RViz — it does the +opposite. + +**Large drift-and-snap excursions are a Motive problem, not a gate problem.** They were +first blamed on `EKF2_EVP_NOISE` being too tight. That was wrong. The cause was a 90° +body-yaw offset in the Motive rigid-body definition: EKF2 fuses vision yaw and snaps its +heading to it, so its nav frame was 90° off and IMU-predicted motion fought the (correct) +vision position. Fixing the rigid body in Motive cut moving error 0.25 → 0.04 m and the +odom/mocap path-length ratio 2.33× → 1.12×. + +If you see drift-and-snap, **check the Motive rigid-body definition first**. 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 once Motive is +fixed. + +The wider `EKF2_EVP_NOISE` (0.05) is still the right value on its own merits: the previous +0.01 gave only a 5 cm gate, tight enough to reject legitimate updates and refuse to arm. + +**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`:** publish the origin altitude as +`N + desired_floor_amsl` instead of the literal 90.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. + +`geographic_msgs/GeoPoint.altitude` is defined as a height above the **WGS‑84 +ellipsoid**, and MAVROS applies the geoid model itself, so `N + desired_floor_amsl` is +not a workaround — it is the correctly-expressed ellipsoidal altitude. Do **not** send +AMSL here; that would double-convert. + +### Choosing `desired_floor_amsl` + +This sets what AMSL the mocap floor (vision `z = 0`) reports. `local_position.z` equals +the OptiTrack height for **any** value — this only affects the *global* altitude the +vehicle reports. + +We use **36.0**, the shared world datum expressed in AMSL (90 m ellipsoidal − N ≈ 54 m), +so the robot's global position agrees with where sim and the GCS place the same world +origin. The published ellipsoidal origin then works out to ≈ 90 m — the datum itself. + +Setting `0.0` instead puts the mocap floor at sea level. Local flight is identical, but +the robot's reported global altitude then disagrees with sim/GCS by ~36 m. + +> Not yet confirmed on hardware — verify the reported global altitude on the next +> mocap flight. + +**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` | diff --git a/mkdocs.yml b/mkdocs.yml index a36d4c75f..ba91013fa 100644 --- a/mkdocs.yml +++ b/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: diff --git a/overrides/l4t-optitrack-realrobot.env b/overrides/l4t-optitrack-realrobot.env new file mode 100644 index 000000000..c88253419 --- /dev/null +++ b/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" diff --git a/robot/docker/robot-base-docker-compose.yaml b/robot/docker/robot-base-docker-compose.yaml index 8cb714e18..793cf6cab 100644 --- a/robot/docker/robot-base-docker-compose.yaml +++ b/robot/docker/robot-base-docker-compose.yaml @@ -21,6 +21,8 @@ services: - ONBOARD_BASE_PORT=${ONBOARD_BASE_PORT} - ROBOT_NAME_MAP_CONFIG_FILE=${ROBOT_NAME_MAP_CONFIG_FILE:-default_robot_name_map.yaml} - DEBUG_RVIZ=${DEBUG_RVIZ:-false} + # OptiTrack / NatNet + - NATNET_SERVER_IP=${NATNET_SERVER_IP:-172.31.0.200} volumes: # display stuff - $HOME/.Xauthority:/.Xauthority diff --git a/robot/ros_ws/src/perception/natnet_ros2/CMakeLists.txt b/robot/ros_ws/src/perception/natnet_ros2/CMakeLists.txt index ff47a00da..7d762f688 100644 --- a/robot/ros_ws/src/perception/natnet_ros2/CMakeLists.txt +++ b/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}) # --------------------------------------------------------------------------- diff --git a/robot/ros_ws/src/perception/natnet_ros2/README.md b/robot/ros_ws/src/perception/natnet_ros2/README.md index eb44763b4..8a3970b4f 100644 --- a/robot/ros_ws/src/perception/natnet_ros2/README.md +++ b/robot/ros_ws/src/perception/natnet_ros2/README.md @@ -15,8 +15,9 @@ This module provides a bridge between OptiTrack Motive motion capture systems an - Receives **NatNet UDP packets** from an external Motive PC (configurable IP/port) - **Decodes motion capture frames** containing rigid body positions and orientations - **Publishes pose data** to the AirStack perception layer in standard ROS 2 formats -- **Supports multi-robot** via ROBOT_NAME namespacing -- **Optionally bridges** to MAVROS for PX4 external pose feedback +- **Tracks multiple rigid bodies per robot** (e.g. a drone for state estimation plus a separate target), each mapped to its own topic +- **Supports multi-robot** via per-robot profiles selected by `ROBOT_NAME` +- **Optionally bridges** to MAVROS for PX4 external pose feedback (per-robot) - **Respects OptiTrack licensing** by keeping the NatNet SDK external (host-side download with explicit consent) ## Architecture @@ -25,13 +26,18 @@ This module provides a bridge between OptiTrack Motive motion capture systems an Motive (External PC) ↓ NatNet UDP (port 1511) ↓ -NatNet ROS 2 Node - ├→ /robot_1/perception/optitrack/{body_name} (PoseStamped, optional) - ├→ /robot_1/perception/optitrack/{body_name}/pose_cov (PoseWithCovarianceStamped, always) - └→ (Optional, publish_to_mavros: true) - vision_pose_converter_node - ├→ /robot_1/mavros/vision_pose/pose - └→ /robot_1/mavros/vision_pose/pose_cov +NatNet ROS 2 Node (loads the ROBOT_NAME profile from natnet_config.yaml) + │ per configured body (one or more): + ├→ /{ROBOT_NAME}/{topic} (PoseStamped, when pose: true) + ├→ /{ROBOT_NAME}/{topic}/pose_cov (PoseWithCovarianceStamped, when pose_cov: true) + └→ (Optional, vision_pose.enabled: true) + mavros_gp_origin_node + └→ /{ROBOT_NAME}/interface/mavros/global_position/set_gp_origin + px4_param_setter_node + └→ /{ROBOT_NAME}/interface/mavros/param/set (external-vision PX4 params) + vision_pose_converter_node (reads input/output topics from the profile) + ├→ /{ROBOT_NAME}/interface/mavros/vision_pose/pose + └→ /{ROBOT_NAME}/interface/mavros/vision_pose/pose_cov ``` ## Interfaces @@ -39,63 +45,122 @@ NatNet ROS 2 Node ### Inputs - **Network**: NatNet UDP stream from Motive PC (external network) -- **Configuration**: `natnet_config.yaml` with server IP, ports, `body_name`, and covariance +- **Configuration**: `natnet_config.yaml` — generic `server` settings plus a `robots` map of per-robot profiles (body list + optional MAVROS `vision_pose` block). The launch file selects the profile matching `ROBOT_NAME`. ### Outputs -For each tracked rigid body `{body_name}` from Motive: +For each rigid body in the robot's profile, `topic` is a **relative** leaf namespaced +under `/{ROBOT_NAME}/` (it defaults to `perception/optitrack/{rigid_body_name}` when +omitted): -#### Direct OptiTrack pose (optional) +#### Direct OptiTrack pose -- **Topic**: `/{ROBOT_NAME}/perception/optitrack/{body_name}` +- **Topic**: `/{ROBOT_NAME}/{topic}` - **Type**: `geometry_msgs/PoseStamped` - **Description**: Position and orientation only (no covariance) -- **Enabled by**: `publish_direct_optitrack: true` in config (default: `true`) +- **Enabled by**: `pose: true` on that body (per body) -#### Pose with covariance (always) +#### Pose with covariance -- **Topic**: `/{ROBOT_NAME}/perception/optitrack/{body_name}/pose_cov` +- **Topic**: `/{ROBOT_NAME}/{topic}/pose_cov` - **Type**: `geometry_msgs/PoseWithCovarianceStamped` -- **Description**: Same pose as above plus a 6×6 covariance matrix (`position_covariance` and `orientation_covariance` from config). Published whenever the rigid body is tracked — independent of `publish_direct_optitrack` and `publish_to_mavros`. - -#### MAVROS vision pose bridge (optional) - -When `publish_to_mavros: true`, `vision_pose_converter_node` subscribes to `pose_cov` and republishes for PX4: - -- **Topic**: `/{ROBOT_NAME}/mavros/vision_pose/pose` — `geometry_msgs/PoseStamped` (pose extracted from the covariance message) -- **Topic**: `/{ROBOT_NAME}/mavros/vision_pose/pose_cov` — `geometry_msgs/PoseWithCovarianceStamped` (full message, quaternion optionally canonicalized) -- **Enabled by**: `publish_to_mavros: true` in config +- **Description**: Same pose plus a 6×6 covariance matrix from that body's `position_covariance` / `orientation_covariance`. +- **Enabled by**: `pose_cov: true` on that body (per body) + +#### MAVROS vision pose bridge (optional, per robot) + +When the robot's `vision_pose.enabled: true`, `vision_pose_converter_node` subscribes to the configured `input_topic` (a body's `pose_cov`) and republishes for PX4 on the configured outputs: + +- **Topic** (`output_pose_topic`): `/{ROBOT_NAME}/interface/mavros/vision_pose/pose` — `geometry_msgs/PoseStamped` (pose extracted from the covariance message) +- **Topic** (`output_pose_cov_topic`): `/{ROBOT_NAME}/interface/mavros/vision_pose/pose_cov` — `geometry_msgs/PoseWithCovarianceStamped` (full message, quaternion optionally canonicalized) +- **Enabled by**: `vision_pose.enabled: true` in the robot's profile +- **Retargetable**: change `input_topic` / `output_pose_topic` / `output_pose_cov_topic` (relative, namespaced) to bridge to other middleware +- **PX4 side**: set `SITL_PARAM_PROFILE=px4-vision` in `.env` so Isaac SITL loads EKF2 external-vision params from `simulation/isaac-sim/docker/sitl-files/px4-vision.env` + +##### Synthetic GPS origin (mocap / no-GNSS arming) + +With GNSS disabled (`EKF2_GPS_CTRL=0`), PX4 fused EKF has **no global position**. This fails preflight checks and refuse to arm. When `vision_pose.enabled: true`, +`mavros_gp_origin_node` publishes a synthetic origin once at startup: + +- **Topic**: `/{ROBOT_NAME}/interface/mavros/global_position/set_gp_origin` — `geographic_msgs/GeoPointStamped` +- **Guarded**: waits for `mavros/state.connected`, then publishes only if no + origin already exists (it watches `…/global_position/gp_origin`), so a + GNSS-equipped vehicle is left untouched. +- **Params** (`config/mavros_gp_origin.yaml`): `enabled` (default `true`), + `latitude/longitude/altitude` (default Lisbon — the AirStack shared world + datum; **must match** the GCS origin in `gcs_visualizer/gcs_utils.py` and the + sim's `gps_utils.py` so Foxglove waypoints transform 1:1), `settle_sec`. + Set `enabled: false` to rely on real GNSS. + +##### PX4 parameter enforcement (external-vision EKF2 setup) + +When `vision_pose.enabled: true`, `px4_param_setter_node` pushes the PX4 +parameter set for OptiTrack-only flight through the MAVROS param plugin at +startup, so the FCU doesn't need manual QGroundControl configuration: + +- **Services used**: `/{ROBOT_NAME}/interface/mavros/param/get_parameters` + (read current), `…/param/set` (`mavros_msgs/ParamSetV2`, set + verify readback) +- **Idempotent**: waits for `mavros/state.connected` + `settle_sec` (initial + param-table pull), reads each param first, and skips ones already correct — + PX4 persists parameters, so subsequent boots are a verify-only pass. +- **Reboot warning**: if any parameter actually changed, it logs a warning to + reboot the FCU before flight so EKF2 restarts with a clean fusion config. +- **Params** (`config/px4_params.yaml`): `enabled`, `settle_sec`, + `retry_period_sec`, `max_attempts`, and the `params.*` map of desired FCU + values — external-vision fusion (`EKF2_EV_CTRL: 11`, `EKF2_HGT_REF: 3`), + GPS/mag/baro disabled (`EKF2_GPS_CTRL: 0`, `EKF2_MAG_TYPE: 5`, + `EKF2_BARO_CTRL: 0`), measured vision delay (`EKF2_EV_DELAY: 6.0` ms), and + EV noise floors (`EKF2_EV_NOISE_MD: 1`, `EKF2_EVP_NOISE`, `EKF2_EVA_NOISE`). + YAML type selects the MAVLink param type: write floats with a decimal point + (`6.0`), integers bare. Values assume PX4 ≥ 1.14; for older firmware use + `EKF2_AID_MASK: 24` / `EKF2_HGT_MODE: 3` instead. ## Configuration -Edit `config/natnet_config.yaml`: +`config/natnet_config.yaml` uses a custom `natnet:` schema (not a flat ROS 2 param +file): generic `server` settings shared by every agent, then a `robots` map of +per-robot profiles. The launch file parses it, selects the profile matching the +container's `ROBOT_NAME`, flattens the body list into node parameters, and brings up +the MAVROS bridge only when that robot's `vision_pose.enabled` is true. ```yaml -/**: - ros__parameters: - server_ip: "192.168.1.100" # IP of the Motive PC +natnet: + server: # generic across all agents + server_ip: "$(env NATNET_SERVER_IP 172.31.0.200)" client_ip: "0.0.0.0" command_port: 1510 data_port: 1511 - connection_type: "unicast" # or "multicast" - - body_name: "Drone" # rigid body name in Motive (case-sensitive) - body_id: -1 # -1 = publish all bodies in the frame - - publish_direct_optitrack: true # PoseStamped on …/optitrack/{body_name} - publish_to_mavros: false # include vision_pose_converter → MAVROS - + connection_type: "unicast" # or "multicast" + multicast_address: "239.255.42.99" frame_id: "world" - - position_covariance: [0.1, 0.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.1] - orientation_covariance: [0.01, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.01] + debug: false + robots: + robot_1: + vision_pose: # per-robot MAVROS bridge (omit/false to skip) + enabled: true + input_topic: "perception/optitrack/drone/pose_cov" + output_pose_topic: "interface/mavros/vision_pose/pose" + output_pose_cov_topic: "interface/mavros/vision_pose/pose_cov" + bodies: # one or more tracked rigid bodies + - rigid_body_name: "Drone" # Motive name (case-sensitive) + id: 1 # Motive streaming id + topic: "perception/optitrack/drone" # relative → /{ROBOT_NAME}/ + pose: true # publish PoseStamped + pose_cov: true # publish PoseWithCovarianceStamped + position_covariance: [1.0e-6, 0, 0, 0, 1.0e-6, 0, 0, 0, 1.0e-6] + orientation_covariance: [3.0e-6, 0, 0, 0, 3.0e-6, 0, 0, 0, 3.0e-6] ``` +To track an additional body (e.g. a target) for a robot, add another entry under that +robot's `bodies`. To add a robot, add a new key under `robots`. The shipped file +includes commented scaffolding for a 3-drone fleet where `robot_1` and `robot_2` also +track a shared `Target` body and `robot_3` tracks only its drone. + ## Launch ### Basic launch -Parameters come from `config/natnet_config.yaml` (network, body, covariance). Optional overrides: +Parameters come from `config/natnet_config.yaml` (server + the `ROBOT_NAME` profile). Optional overrides: ```bash ros2 launch natnet_ros2 natnet_ros2.launch.py \ @@ -106,7 +171,7 @@ ros2 launch natnet_ros2 natnet_ros2.launch.py \ ### MAVROS bridge -Set `publish_to_mavros: true` in `natnet_config.yaml`. The launch file reads `publish_to_mavros` and `body_name` from that YAML to decide whether to include `vision_pose_converter.launch.xml`. +Set `vision_pose.enabled: true` in the robot's profile. The launch file includes `vision_pose_converter.launch.xml` (plus `mavros_gp_origin.launch.xml` and `px4_param_setter.launch.xml`) and forwards the profile's `input_topic` / `output_pose_topic` / `output_pose_cov_topic`. ### From perception bringup @@ -140,13 +205,18 @@ The SDK will be installed into `robot/ros_ws/src/perception/natnet_ros2/lib/` an ### Multi-Robot Support Each container instance gets its own `ROBOT_NAME` and `ROS_DOMAIN_ID`: -- Topics: `/{ROBOT_NAME}/perception/optitrack/{body_name}` and `/{ROBOT_NAME}/perception/optitrack/{body_name}/pose_cov` -- Supported via launch file argument forwarding +- The node loads the `robots[$ROBOT_NAME]` profile, so each robot tracks only the bodies (and runs the MAVROS bridge) configured for it. +- Topics are namespaced under `/{ROBOT_NAME}/` from each body's relative `topic`. +- Set `NUM_ROBOTS=N`; each replica resolves its own `ROBOT_NAME` (via `resolve_robot_name.py`) and auto-selects its profile — no per-robot env overrides. ### Error Handling - Invalid/malformed packets are skipped with debug logging - Lost connectivity logs warnings; gracefully recovers when stream resumes - Covariance in config allows tuning uncertainty per deployment +- **Connect retry:** the initial handshake is retried every 2 s until it + succeeds, so the node tolerates the NatNet server starting *after* the robot + (e.g. a Motive PC powered on later, or the Isaac Sim NatNet emulator which only + binds ~100 s into sim boot). The retry timer cancels itself on first success. ## Testing @@ -157,9 +227,9 @@ Each container instance gets its own `ROBOT_NAME` and `ROS_DOMAIN_ID`: ```bash ros2 launch natnet_ros2 natnet_ros2.launch.py ``` -4. Verify topics: +4. Verify topics (default profile maps the `Drone` body to `perception/optitrack/drone`): ```bash - ros2 topic echo /robot_1/perception/optitrack/Drone/pose_cov + ros2 topic echo /robot_1/perception/optitrack/drone/pose_cov ``` ### Without Real Hardware (Mock) @@ -167,7 +237,7 @@ TODO: Implement Motive simulator in Isaac Sim to generate fake NatNet packets ## Known Limitations -- When `body_id: -1`, all rigid bodies in the Motive frame get publishers; filter by subscribing to the `{body_name}` you care about +- The node publishes only bodies listed in the robot's profile (matched by `id`); bodies streamed by Motive but absent from the profile are ignored. - MAVROS bridge applies frame_id override and quaternion canonicalization; full PX4 frame alignment may still need tuning per airframe - No support for skeleton tracking or labeled markers yet (future enhancement) diff --git a/robot/ros_ws/src/perception/natnet_ros2/config/mavros_gp_origin.yaml b/robot/ros_ws/src/perception/natnet_ros2/config/mavros_gp_origin.yaml new file mode 100644 index 000000000..0035f017b --- /dev/null +++ b/robot/ros_ws/src/perception/natnet_ros2/config/mavros_gp_origin.yaml @@ -0,0 +1,22 @@ +# Synthetic GPS origin for mocap / no-GNSS flight via MAVROS. +# Loaded by mavros_gp_origin.launch.xml when publish_to_mavros is enabled. +# See docs/robot/px4_external_vision.md for the height-datum explanation. + +/**: + ros__parameters: + # Skipped if an origin already exists (e.g. a GNSS-equipped vehicle). + enabled: true + # MUST match the GCS world origin (gcs_visualizer/gcs_utils.py) and the sim + # datum (launch_scripts/gps_utils.py), or the relay computes a huge ENU offset. + latitude: 38.736832 + longitude: -9.137977 + # Shared world datum; used directly when use_geoid_altitude is false or in sim. + altitude: 90.0 + # Real hardware: derive origin altitude from the geoid so local_position z + # equals OptiTrack height. Auto-skipped in sim. + use_geoid_altitude: true + # AMSL of the mocap floor. 36.0 = the shared world datum (90 m ellipsoidal) in AMSL, + # so the robot's global altitude agrees with sim and the GCS. + desired_floor_amsl: 36.0 + geoid_model: "egm96-5" + settle_sec: 5.0 diff --git a/robot/ros_ws/src/perception/natnet_ros2/config/natnet_config.yaml b/robot/ros_ws/src/perception/natnet_ros2/config/natnet_config.yaml index 69fc11d1c..6ad16e074 100644 --- a/robot/ros_ws/src/perception/natnet_ros2/config/natnet_config.yaml +++ b/robot/ros_ws/src/perception/natnet_ros2/config/natnet_config.yaml @@ -1,51 +1,101 @@ -# NatNet ROS 2 parameters — loaded by natnet_ros2.launch.py (NatNet node + MAVROS gate). -# publish_to_mavros / body_name are read by the launch file to decide vision_pose_converter include. -# -# Use /** so parameters apply regardless of namespace (e.g. /robot_1/perception/natnet_ros2_node). -# See: https://docs.ros.org/en/humble/Tutorials/Beginner-CLI-Tools/Understanding-ROS2-Parameters.html - -/**: - ros__parameters: - # IP address of the PC running Motive (OptiTrack server). - # Change this to match your local network before launching NatNet. - server_ip: "192.168.1.100" - # Motive learns unicast destination from outbound UDP source IP — bind explicitly when you have - # multiple NICs (e.g. Docker 172.17.* vs LAN). +# NatNet ROS 2 configuration — parsed by natnet_ros2.launch.py, which selects the +# profile matching the container's ROBOT_NAME. +# See docs/robot/px4_external_vision.md for the schema and setup guide. + +natnet: + + # --- Connection settings (generic across all agents) ----------------------- + server: + # Motive host; defaults to the in-sim emulator. Set NATNET_SERVER_IP per deployment. + server_ip: "$(env NATNET_SERVER_IP 172.31.0.200)" + # Bind explicitly when the client has multiple NICs. client_ip: "0.0.0.0" command_port: 1510 data_port: 1511 - # "unicast" — point-to-point; Motive streams directly to this machine's IP. - # Requires Motive unicast streaming enabled and client_ip set - # to the correct NIC when multiple interfaces are present. - # "multicast" — Motive broadcasts to a multicast group; any machine on the - # subnet that joins the group receives all body data. - # Use for multi-robot setups where every robot receives the - # full frame and filters by body_id. + # "unicast" (default) or "multicast"; multicast_address applies to the latter. connection_type: "unicast" - - # Only used when connection_type = "multicast". - # Must match Motive > Edit > Preferences > Data Streaming > Multicast Interface. - # OptiTrack default is 239.255.42.99. multicast_address: "239.255.42.99" - # Name of the rigid body as defined in Motive. Must match exactly (case-sensitive). - body_name: "Drone" - body_id: -1 - - publish_direct_optitrack: true - publish_to_mavros: true - frame_id: "world" debug: false - position_covariance: - [0.1, 0.0, 0.0, - 0.0, 0.1, 0.0, - 0.0, 0.0, 0.1] + # Per-message latency reporting. + latency_sampling_warmup_s: 5.0 + latency_sampling_window_s: 20.0 + cube_orange_latency_ms: 5.0 + + # --- Per-robot profiles (selected by ROBOT_NAME) --------------------------- + robots: + + robot_1: + # MAVROS vision_pose bridge; enabled=false skips the converter. + vision_pose: + enabled: true + input_topic: "perception/optitrack/drone/pose_cov" + output_pose_topic: "interface/mavros/vision_pose/pose" + output_pose_cov_topic: "interface/mavros/vision_pose/pose_cov" - orientation_covariance: - [0.01, 0.0, 0.0, - 0.0, 0.01, 0.0, - 0.0, 0.0, 0.01] + # Rigid bodies this robot tracks. name and id must match Motive exactly; the + # client filters frames by numeric id. Defaults match the in-sim emulator. + bodies: + - rigid_body_name: "Drone" + id: 1 + topic: "perception/optitrack/drone" + pose: true + pose_cov: true + position_covariance: + [1.0e-6, 0.0, 0.0, + 0.0, 1.0e-6, 0.0, + 0.0, 0.0, 1.0e-6] + orientation_covariance: + [3.0e-6, 0.0, 0.0, + 0.0, 3.0e-6, 0.0, + 0.0, 0.0, 3.0e-6] + robot_2: + vision_pose: + enabled: true + input_topic: "perception/optitrack/drone/pose_cov" + output_pose_topic: "interface/mavros/vision_pose/pose" + output_pose_cov_topic: "interface/mavros/vision_pose/pose_cov" + bodies: + - rigid_body_name: "Drone2" + id: 2 + topic: "perception/optitrack/drone" + pose: true + pose_cov: true + - rigid_body_name: "Target" # shared target — also tracked by robot_1 + id: 100 + topic: "perception/optitrack/target" + pose: true + pose_cov: false + position_covariance: + [1.0e-6, 0.0, 0.0, + 0.0, 1.0e-6, 0.0, + 0.0, 0.0, 1.0e-6] + orientation_covariance: + [3.0e-6, 0.0, 0.0, + 0.0, 3.0e-6, 0.0, + 0.0, 0.0, 3.0e-6] + + robot_3: + vision_pose: + enabled: true + input_topic: "perception/optitrack/drone/pose_cov" + output_pose_topic: "interface/mavros/vision_pose/pose" + output_pose_cov_topic: "interface/mavros/vision_pose/pose_cov" + bodies: + - rigid_body_name: "Drone3" + id: 3 + topic: "perception/optitrack/drone" + pose: true + pose_cov: true + position_covariance: + [1.0e-6, 0.0, 0.0, + 0.0, 1.0e-6, 0.0, + 0.0, 0.0, 1.0e-6] + orientation_covariance: + [3.0e-6, 0.0, 0.0, + 0.0, 3.0e-6, 0.0, + 0.0, 0.0, 3.0e-6] diff --git a/robot/ros_ws/src/perception/natnet_ros2/config/px4_params.yaml b/robot/ros_ws/src/perception/natnet_ros2/config/px4_params.yaml new file mode 100644 index 000000000..c2bd3b7be --- /dev/null +++ b/robot/ros_ws/src/perception/natnet_ros2/config/px4_params.yaml @@ -0,0 +1,43 @@ +# PX4 parameters for OptiTrack-only (external vision) flight, checked at startup by +# px4_param_setter. See docs/robot/px4_external_vision.md for what each one does, the +# tuning rationale, and how to set them in QGroundControl. +# +# TYPE MATTERS: integers bare (11), floats with a decimal point (7.0), so the MAVLink +# param type matches the FCU's declaration. +# +# Values assume PX4 >= 1.14. For older firmware use EKF2_AID_MASK: 24 and +# EKF2_HGT_MODE: 3 instead. + +/**: + ros__parameters: + enabled: true + # Check-only by default: read the FCU's params and flag differences, never write. + auto_set: false + # On mismatch with auto_set:false — 'warn' (log diffs) or 'halt' (exit non-zero). + on_mismatch: "warn" + # Initial full param pull over serial (115200) is slow; give it time. + settle_sec: 10.0 + retry_period_sec: 2.0 + max_attempts: 30 + + params: + # Fuse vision horizontal position (1) + vertical position (2) + yaw (8). + EKF2_EV_CTRL: 11 + # Vision is the height reference. + EKF2_HGT_REF: 3 + EKF2_GPS_CTRL: 0 + # Magnetometer off; yaw comes from vision. + EKF2_MAG_TYPE: 5 + EKF2_BARO_CTRL: 0 + # Remove the baro at system level, not just from fusion, so the height datum is + # deterministic on every boot. WARNING: no baro backup — indoor mocap only. + SYS_HAS_BARO: 0 + EKF2_RNG_CTRL: 0 + # Do NOT raise to chase apparent lag; higher is measurably worse (see docs). + EKF2_EV_DELAY: 7.0 + # Use the NOISE floors below rather than the message covariance. + EKF2_EV_NOISE_MD: 1 + # Also sets the innovation gate (EKF2_EVP_GATE sigma wide): 0.05 -> ~25 cm. + EKF2_EVP_NOISE: 0.05 + EKF2_EVA_NOISE: 0.05 + COM_ARM_WO_GPS: 1 diff --git a/robot/ros_ws/src/perception/natnet_ros2/config/vision_pose_converter.yaml b/robot/ros_ws/src/perception/natnet_ros2/config/vision_pose_converter.yaml index a52181786..f14b4d2d7 100644 --- a/robot/ros_ws/src/perception/natnet_ros2/config/vision_pose_converter.yaml +++ b/robot/ros_ws/src/perception/natnet_ros2/config/vision_pose_converter.yaml @@ -1,12 +1,13 @@ # Vision pose converter → MAVROS bridge parameters. # Loaded by vision_pose_converter.launch.xml via . -# $(env ROBOT_NAME ...) is expanded by launch substitution. /**: ros__parameters: frame_id: "world" child_frame_id: "$(env ROBOT_NAME robot_1)/base_link" # Normalise quaternion to canonical form (qw >= 0) before publishing. - # Recommended for ArduPilot EKF3 and any consumer sensitive to sign flips. - # PX4 EKF2 handles either sign internally, so this is optional for PX4. canonical_quaternion: true + # Cap the rate forwarded to MAVROS (0 = passthrough); EKF2 needs only 30-50 Hz. + max_rate_hz: 50.0 + # Which vision_pose topic(s) to forward: 'pose', 'pose_cov', or 'both'. + publish_mode: "pose_cov" diff --git a/robot/ros_ws/src/perception/natnet_ros2/include/natnet_ros2/natnet_client_adapter.hpp b/robot/ros_ws/src/perception/natnet_ros2/include/natnet_ros2/natnet_client_adapter.hpp index 04b3638ef..b64b254df 100644 --- a/robot/ros_ws/src/perception/natnet_ros2/include/natnet_ros2/natnet_client_adapter.hpp +++ b/robot/ros_ws/src/perception/natnet_ros2/include/natnet_ros2/natnet_client_adapter.hpp @@ -52,9 +52,20 @@ class NatNetClientAdapter : public INatNetClient void set_frame_callback(std::function cb) override; void disconnect() override; + // Context handed to the SDK's C frame callback. Bundles the client (needed to + // convert TransmitTimestamp → latency via SecondsSinceHostTimestamp) with the + // user callback, since the SDK passes only a single void* through. Public so the + // file-scope trampoline in the .cpp can reinterpret the void* ctx. + struct FrameCallbackCtx + { + NatNetClient * client = nullptr; + std::function * cb = nullptr; + }; + private: std::unique_ptr client_; std::function user_cb_; + FrameCallbackCtx cb_ctx_{}; }; } // namespace natnet_ros2 diff --git a/robot/ros_ws/src/perception/natnet_ros2/include/natnet_ros2/natnet_logic.hpp b/robot/ros_ws/src/perception/natnet_ros2/include/natnet_ros2/natnet_logic.hpp index f23216565..115a2fc98 100644 --- a/robot/ros_ws/src/perception/natnet_ros2/include/natnet_ros2/natnet_logic.hpp +++ b/robot/ros_ws/src/perception/natnet_ros2/include/natnet_ros2/natnet_logic.hpp @@ -18,22 +18,13 @@ // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE // SOFTWARE. -// natnet_logic.hpp — pure C++ helpers for natnet_ros2 (no ROS, no NatNet SDK). -// -// Five responsibility areas: -// -// 1. Covariance assembly -// 2. Topic names -// 3. Connection-configuration helpers (SDK-independent) -// 4. Rigid-body frame helpers (SDK-independent) -// 5. Abstraction seam: INatNetClient interface + negotiation logic -// -// NatNet SDK types (sNatNetClientConnectParams, sRigidBodyData, …) are only -// used inside natnet_ros2_node.cpp and natnet_client_adapter.cpp. -// All logic here uses plain C++ so test_natnet_logic.cpp compiles with only gtest. +// natnet_logic.hpp — pure C++ helpers for natnet_ros2 (no ROS, no NatNet SDK), so +// test_natnet_logic.cpp compiles with only gtest. SDK types stay in +// natnet_ros2_node.cpp / natnet_client_adapter.cpp. #pragma once +#include #include #include #include @@ -91,6 +82,34 @@ inline std::string optitrack_pose_cov_topic( return optitrack_topic_base(robot_name, body_name) + "/pose_cov"; } +/// Namespace a relative topic leaf under /{robot_name}/. +/// +/// Leading slashes in \p relative are stripped so the result always has exactly +/// one. Used for the per-body ``topic`` overrides in natnet_config.yaml, which are +/// relative and namespaced by the node at runtime. +inline std::string namespaced_topic( + const std::string & robot_name, + const std::string & relative) +{ + const std::size_t start = relative.find_first_not_of('/'); + const std::string leaf = + (start == std::string::npos) ? std::string{} : relative.substr(start); + return "/" + robot_name + "/" + leaf; +} + +/// Topic base for one configured body: the per-body relative override when set, +/// otherwise the default /{robot_name}/perception/optitrack/{body_name}. +inline std::string body_topic_base( + const std::string & robot_name, + const std::string & body_name, + const std::string & relative_override) +{ + if (relative_override.empty()) { + return optitrack_topic_base(robot_name, body_name); + } + return namespaced_topic(robot_name, relative_override); +} + // =========================================================================== // 3. Connection-configuration helpers @@ -177,6 +196,13 @@ struct FrameSample int32_t frame_num = 0; float timestamp = 0.f; int16_t params = 0; ///< NatNet frame.params bitmask + /// Seconds elapsed since the server transmitted this frame, as reported by + /// NatNetClient::SecondsSinceHostTimestamp(TransmitTimestamp). This is the + /// transit + client-processing latency the drone observes per message. + double transit_latency_s = 0.0; + /// True when transit_latency_s is meaningful (server supplied a non-zero + /// TransmitTimestamp). Older servers / streams without timing info leave it false. + bool has_latency = false; std::vector bodies; }; @@ -199,6 +225,16 @@ inline bool should_publish_body(int32_t filter_id, int32_t rb_id) return filter_id < 0 || rb_id == filter_id; } +/// Returns true when rb_id is one of the configured body ids. +/// +/// Multi-body variant of should_publish_body(): the node tracks a fixed set of +/// ids from natnet_config.yaml and publishes only those (empty set → nothing). +inline bool body_is_configured(const std::vector & configured_ids, int32_t rb_id) +{ + return std::find(configured_ids.begin(), configured_ids.end(), rb_id) + != configured_ids.end(); +} + /// Double-precision pose extracted from a RigidBodySample. struct PoseData { diff --git a/robot/ros_ws/src/perception/natnet_ros2/launch/mavros_gp_origin.launch.xml b/robot/ros_ws/src/perception/natnet_ros2/launch/mavros_gp_origin.launch.xml new file mode 100644 index 000000000..4cb1f785b --- /dev/null +++ b/robot/ros_ws/src/perception/natnet_ros2/launch/mavros_gp_origin.launch.xml @@ -0,0 +1,34 @@ + + + + + + + + + + + + + + + + + + + diff --git a/robot/ros_ws/src/perception/natnet_ros2/launch/natnet_ros2.launch.py b/robot/ros_ws/src/perception/natnet_ros2/launch/natnet_ros2.launch.py index cb7c178d8..440eaf8d6 100644 --- a/robot/ros_ws/src/perception/natnet_ros2/launch/natnet_ros2.launch.py +++ b/robot/ros_ws/src/perception/natnet_ros2/launch/natnet_ros2.launch.py @@ -1,5 +1,10 @@ #!/usr/bin/env python3 -"""Bring up NatNet node; optionally MAVROS bridge per natnet_config.yaml. +"""Bring up the NatNet node from natnet_config.yaml; optionally the MAVROS bridge. + +The config uses a custom ``natnet:`` schema (server settings + per-robot profiles), +so this launch file parses it, selects the profile matching ``ROBOT_NAME``, flattens +the body list into node parameters, and — when the robot's ``vision_pose`` block is +enabled — includes the MAVROS GP-origin + vision_pose_converter bridges. natnet_ros2_node is a C++ executable that requires the OptiTrack NatNet SDK. If the SDK was not installed (``airstack setup`` not run) and the workspace @@ -10,8 +15,9 @@ from __future__ import annotations import os +import re from pathlib import Path -from typing import cast +from typing import Any, cast import yaml from ament_index_python.packages import get_package_share_directory @@ -20,11 +26,28 @@ from launch.launch_description_sources import FrontendLaunchDescriptionSource from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node -from launch_ros.parameter_descriptions import ParameterFile +# Per-body covariance fallback when a body omits its own (sub-0.1 mm / sub-0.1 deg). +_DEFAULT_POSITION_COVARIANCE = [1.0e-6, 0.0, 0.0, 0.0, 1.0e-6, 0.0, 0.0, 0.0, 1.0e-6] +_DEFAULT_ORIENTATION_COVARIANCE = [3.0e-6, 0.0, 0.0, 0.0, 3.0e-6, 0.0, 0.0, 0.0, 3.0e-6] + +_ENV_SUBST = re.compile(r"\$\(env\s+(\w+)(?:\s+([^)]*))?\)") + + +def _expand_env(value: Any) -> Any: + """Expand ``$(env VAR default)`` tokens in a string using os.environ.""" + if not isinstance(value, str): + return value + + def _replace(match: re.Match) -> str: + var, default = match.group(1), match.group(2) + return os.environ.get(var, default if default is not None else "") + + return _ENV_SUBST.sub(_replace, value) -def _ros_params_from_file(config_path: str) -> dict: - """Parse /** / ros__parameters block from a ROS 2 parameter YAML.""" + +def _load_natnet_config(config_path: str) -> dict: + """Parse the ``natnet:`` block from the config YAML.""" path = Path(config_path) if not path.is_file(): return {} @@ -32,34 +55,116 @@ def _ros_params_from_file(config_path: str) -> dict: data = yaml.safe_load(f) if not isinstance(data, dict): return {} - block = data.get('/**') - if not isinstance(block, dict): - return {} - params = block.get('ros__parameters', {}) - return cast(dict, params) if isinstance(params, dict) else {} + natnet = data.get('natnet', {}) + return cast(dict, natnet) if isinstance(natnet, dict) else {} + + +def _flatten_covariance(values: Any, fallback: list[float]) -> list[float]: + """Coerce a 9-element covariance block to floats, falling back when absent.""" + if not isinstance(values, (list, tuple)) or len(values) == 0: + return list(fallback) + return [float(v) for v in values] + + +def _build_node_params(server: dict, profile: dict) -> dict: + """Flatten the server block + a robot's body list into node parameters.""" + bodies = profile.get('bodies', []) or [] + + params: dict[str, Any] = { + 'server_ip': str(_expand_env(server.get('server_ip', '172.31.0.200'))), + 'client_ip': str(_expand_env(server.get('client_ip', '0.0.0.0'))), + 'command_port': int(server.get('command_port', 1510)), + 'data_port': int(server.get('data_port', 1511)), + 'connection_type': str(server.get('connection_type', 'unicast')), + 'multicast_address': str(server.get('multicast_address', '239.255.42.99')), + 'frame_id': str(server.get('frame_id', 'world')), + 'debug': bool(server.get('debug', False)), + 'latency_sampling_warmup_s': float(server.get('latency_sampling_warmup_s', 5.0)), + 'latency_sampling_window_s': float(server.get('latency_sampling_window_s', 20.0)), + 'cube_orange_latency_ms': float(server.get('cube_orange_latency_ms', 5.0)), + } + + body_names: list[str] = [] + body_ids: list[int] = [] + body_topics: list[str] = [] + body_pose: list[bool] = [] + body_pose_cov: list[bool] = [] + body_position_covariance: list[float] = [] + body_orientation_covariance: list[float] = [] + + for body in bodies: + body_names.append(str(body.get('rigid_body_name', ''))) + body_ids.append(int(body.get('id', -1))) + body_topics.append(str(body.get('topic', ''))) + body_pose.append(bool(body.get('pose', True))) + body_pose_cov.append(bool(body.get('pose_cov', True))) + body_position_covariance.extend( + _flatten_covariance(body.get('position_covariance'), _DEFAULT_POSITION_COVARIANCE) + ) + body_orientation_covariance.extend( + _flatten_covariance(body.get('orientation_covariance'), _DEFAULT_ORIENTATION_COVARIANCE) + ) + + params.update( + { + 'body_names': body_names, + 'body_ids': body_ids, + 'body_topics': body_topics, + 'body_pose': body_pose, + 'body_pose_cov': body_pose_cov, + 'body_position_covariance': body_position_covariance, + 'body_orientation_covariance': body_orientation_covariance, + } + ) + return params + + +def _namespaced(robot_name: str, relative: str) -> str: + """Namespace a relative topic under /{robot_name}/.""" + return '/' + robot_name + '/' + relative.lstrip('/') def generate_launch_description() -> LaunchDescription: pkg_share = get_package_share_directory('natnet_ros2') default_natnet_yaml = os.path.join(pkg_share, 'config', 'natnet_config.yaml') default_vp_yaml = os.path.join(pkg_share, 'config', 'vision_pose_converter.yaml') + default_gp_origin_yaml = os.path.join(pkg_share, 'config', 'mavros_gp_origin.yaml') + default_px4_params_yaml = os.path.join(pkg_share, 'config', 'px4_params.yaml') config_file = LaunchConfiguration('config_file') vision_pose_config_file = LaunchConfiguration('vision_pose_config_file') + gp_origin_config_file = LaunchConfiguration('gp_origin_config_file') + px4_params_config_file = LaunchConfiguration('px4_params_config_file') use_sim_time = LaunchConfiguration('use_sim_time') def launch_setup(context, *_args, **_kwargs): cfg_path = config_file.perform(context) vp_path = vision_pose_config_file.perform(context) + gp_path = gp_origin_config_file.perform(context) + px4_path = px4_params_config_file.perform(context) ust = use_sim_time.perform(context) - ros_params = _ros_params_from_file(cfg_path) - publish_mavros = bool(ros_params.get('publish_to_mavros', False)) - body_name = str(ros_params.get('body_name', 'robot_1')) + robot_name = os.environ.get('ROBOT_NAME', 'robot_1') + natnet = _load_natnet_config(cfg_path) + server = natnet.get('server', {}) if isinstance(natnet, dict) else {} + robots = natnet.get('robots', {}) if isinstance(natnet, dict) else {} + profile = robots.get(robot_name, {}) if isinstance(robots, dict) else {} + + if not profile: + print( + f"[natnet_ros2.launch] WARNING: no profile for ROBOT_NAME='{robot_name}' " + f"in {cfg_path}; node will start with no tracked bodies." + ) + + node_params = _build_node_params(server, profile) + # launch_ros / rclpy cannot infer the type of an empty-list parameter, so drop + # any empty arrays; the node declares matching empty defaults and tracks nothing. + node_params = { + k: v for k, v in node_params.items() if not (isinstance(v, list) and len(v) == 0) + } # pkg_share = /share/natnet_ros2 → go up two levels to reach , # then down into lib/natnet_ros2/ where colcon installs executables. - pkg_share = get_package_share_directory('natnet_ros2') node_path = Path(pkg_share).parent.parent / 'lib' / 'natnet_ros2' / 'natnet_ros2_node' if not node_path.exists(): raise RuntimeError( @@ -75,11 +180,49 @@ def launch_setup(context, *_args, **_kwargs): executable='natnet_ros2_node', name='natnet_ros2_node', output='screen', - parameters=[ParameterFile(config_file, allow_substs=True)], + parameters=[node_params], + # The closed-source NatNet SDK can assert (SIGABRT) on connect + # in odd network states; restart rather than losing mocap. + respawn=True, + respawn_delay=2.0, ), ] - if publish_mavros: + vision_pose = profile.get('vision_pose', {}) if isinstance(profile, dict) else {} + if vision_pose.get('enabled', False): + input_topic = _namespaced( + robot_name, str(vision_pose.get('input_topic', 'perception/optitrack/drone/pose_cov')) + ) + output_pose_topic = _namespaced( + robot_name, str(vision_pose.get('output_pose_topic', 'interface/mavros/vision_pose/pose')) + ) + output_pose_cov_topic = _namespaced( + robot_name, + str(vision_pose.get('output_pose_cov_topic', 'interface/mavros/vision_pose/pose_cov')), + ) + + actions.append( + IncludeLaunchDescription( + FrontendLaunchDescriptionSource( + os.path.join(pkg_share, 'launch', 'mavros_gp_origin.launch.xml'), + ), + launch_arguments=[ + ('config_file', gp_path), + ('use_sim_time', ust), + ], + ), + ) + actions.append( + IncludeLaunchDescription( + FrontendLaunchDescriptionSource( + os.path.join(pkg_share, 'launch', 'px4_param_setter.launch.xml'), + ), + launch_arguments=[ + ('config_file', px4_path), + ('use_sim_time', ust), + ], + ), + ) actions.append( IncludeLaunchDescription( FrontendLaunchDescriptionSource( @@ -87,7 +230,9 @@ def launch_setup(context, *_args, **_kwargs): ), launch_arguments=[ ('config_file', vp_path), - ('body_name', body_name), + ('input_topic', input_topic), + ('output_pose_topic', output_pose_topic), + ('output_pose_cov_topic', output_pose_cov_topic), ('use_sim_time', ust), ], ), @@ -99,18 +244,28 @@ def launch_setup(context, *_args, **_kwargs): DeclareLaunchArgument( 'config_file', default_value=default_natnet_yaml, - description='NatNet parameter YAML (/** ros__parameters). ' - 'publish_to_mavros and body_name control MAVROS include.', + description='NatNet config YAML (natnet: server + per-robot profiles). ' + 'The robot profile selected by ROBOT_NAME drives bodies + MAVROS include.', ), DeclareLaunchArgument( 'vision_pose_config_file', default_value=default_vp_yaml, - description='vision_pose_converter parameter YAML.', + description='vision_pose_converter parameter YAML (frame_id, canonical_quaternion).', + ), + DeclareLaunchArgument( + 'gp_origin_config_file', + default_value=default_gp_origin_yaml, + description='mavros_gp_origin parameter YAML.', + ), + DeclareLaunchArgument( + 'px4_params_config_file', + default_value=default_px4_params_yaml, + description='px4_param_setter parameter YAML (params.* = desired FCU parameters).', ), DeclareLaunchArgument( 'use_sim_time', default_value='false', - description='Forwarded to vision_pose_converter.launch.xml.', + description='Forwarded to MAVROS bridge launch files.', ), OpaqueFunction(function=launch_setup), ], diff --git a/robot/ros_ws/src/perception/natnet_ros2/launch/px4_param_setter.launch.xml b/robot/ros_ws/src/perception/natnet_ros2/launch/px4_param_setter.launch.xml new file mode 100644 index 000000000..a3852b0c8 --- /dev/null +++ b/robot/ros_ws/src/perception/natnet_ros2/launch/px4_param_setter.launch.xml @@ -0,0 +1,36 @@ + + + + + + + + + + + + + + + + + + + diff --git a/robot/ros_ws/src/perception/natnet_ros2/launch/vision_pose_converter.launch.xml b/robot/ros_ws/src/perception/natnet_ros2/launch/vision_pose_converter.launch.xml index aad9c4474..803cbdad7 100644 --- a/robot/ros_ws/src/perception/natnet_ros2/launch/vision_pose_converter.launch.xml +++ b/robot/ros_ws/src/perception/natnet_ros2/launch/vision_pose_converter.launch.xml @@ -3,26 +3,31 @@ - + + + - - - + + + diff --git a/robot/ros_ws/src/perception/natnet_ros2/package.xml b/robot/ros_ws/src/perception/natnet_ros2/package.xml index f9632b0f7..1f9e04f4d 100644 --- a/robot/ros_ws/src/perception/natnet_ros2/package.xml +++ b/robot/ros_ws/src/perception/natnet_ros2/package.xml @@ -26,6 +26,8 @@ mavros_msgs + geographic_msgs + rcl_interfaces ament_index_python launch diff --git a/robot/ros_ws/src/perception/natnet_ros2/src/mavros_gp_origin_node.py b/robot/ros_ws/src/perception/natnet_ros2/src/mavros_gp_origin_node.py new file mode 100755 index 000000000..b4151d835 --- /dev/null +++ b/robot/ros_ws/src/perception/natnet_ros2/src/mavros_gp_origin_node.py @@ -0,0 +1,198 @@ +#!/usr/bin/env python3 + +""" +MAVROS GPS Origin Node + +Publishes a synthetic GPS origin to MAVROS once at startup for mocap / no-GNSS +flight. With GNSS disabled, PX4 fuses vision into a valid local position but +has no global position, so modes that require one (e.g. AUTO.LOITER) refuse to +arm. Setting an origin lets PX4 derive global position from the fused estimate. + +The publish is guarded: it waits for MAVROS to connect, watches for an existing +origin, and only publishes if none is present — GNSS-equipped vehicles are left +untouched. +""" + +import shutil +import subprocess + +import rclpy +from rclpy.node import Node +from geographic_msgs.msg import GeoPointStamped +from mavros_msgs.msg import State + + +class MavrosGpOriginNode(Node): + """One-shot synthetic GPS origin publisher for MAVROS / PX4.""" + + def __init__(self): + super().__init__('mavros_gp_origin') + + self.declare_parameter('enabled', True) + # Defaults match the AirStack shared world datum (Lisbon) used by the GCS + # (gcs_utils.py) and sim (gps_utils.py). Normally overridden by + # config/mavros_gp_origin.yaml; kept in sync to avoid a stale fallback. + self.declare_parameter('latitude', 38.736832) + self.declare_parameter('longitude', -9.137977) + self.declare_parameter('altitude', 90.0) + # Real-hardware geoid handling. mavros/PX4 treat the origin altitude as a + # WGS-84 ELLIPSOIDAL height and internally apply the egm96-5 geoid model + # (mavros_uas::egm96_5) to convert to/from AMSL. With no GNSS/baro the + # vehicle height comes purely from vision (mocap floor ~ 0 AMSL), so to + # make local_position z equal the OptiTrack height the origin's ellipsoidal + # altitude must be: N(lat,lon) + desired_floor_amsl, where N is the geoid + # undulation. Because the SAME egm96-5 model computes N here and inside + # mavros, the undulation cancels EXACTLY (accuracy is independent of the + # model's absolute error). Skipped when use_sim_time=true: sim's synthetic + # GPS carries no geoid separation and uses the literal altitude. + self.declare_parameter('use_geoid_altitude', False) + # AMSL (m) assigned to the mocap floor / vision z = 0. Local z equals OptiTrack z + # for any value; this only sets what global altitude the floor reports. + self.declare_parameter('desired_floor_amsl', 36.0) + # Geoid model — MUST match mavros (egm96-5) for exact cancellation. + self.declare_parameter('geoid_model', 'egm96-5') + # Seconds to wait after MAVROS connects (listening for an existing + # origin) before publishing our synthetic one. + self.declare_parameter('settle_sec', 5.0) + + self._enabled = self.get_parameter('enabled').value + if not self._enabled: + self.get_logger().info('Synthetic GPS origin disabled (enabled=false).') + return + + self._lat = self.get_parameter('latitude').value + self._lon = self.get_parameter('longitude').value + self._settle_sec = self.get_parameter('settle_sec').value + self._alt = self._resolve_altitude() + + self._done = False + self._origin_exists = False + self._connected_since = None + self._publish_count = 0 + + self._set_origin_pub = self.create_publisher( + GeoPointStamped, 'set_gps_origin', 10 + ) + self._origin_sub = self.create_subscription( + GeoPointStamped, 'current_gps_origin', self._on_existing_origin, 10 + ) + self._state_sub = self.create_subscription( + State, 'mavros_state', self._on_mavros_state, 10 + ) + self._timer = self.create_timer(1.0, self._tick) + + self.get_logger().info( + f'MAVROS GPS origin node started ' + f'(lat={self._lat}, lon={self._lon}, alt={self._alt}, ' + f'settle_sec={self._settle_sec})' + ) + + def _geoid_undulation(self, lat, lon, model): + """ + Geoid undulation N (metres, height of the geoid above the WGS-84 + ellipsoid) at (lat, lon) via GeographicLib's GeoidEval — the same + egm96-5 dataset mavros loads (mavros_uas::egm96_5), so N cancels exactly + against mavros' internal ellipsoid<->AMSL conversion. Raises on failure. + """ + exe = shutil.which('GeoidEval') + if exe is None: + raise RuntimeError('GeoidEval not found on PATH (install GeographicLib tools)') + proc = subprocess.run( + [exe, '-n', model], + input=f'{lat:.9f} {lon:.9f}\n', + capture_output=True, text=True, timeout=10.0, + ) + if proc.returncode != 0: + raise RuntimeError( + f'GeoidEval rc={proc.returncode}: {proc.stderr.strip() or proc.stdout.strip()}' + ) + return float(proc.stdout.strip().split()[0]) + + def _resolve_altitude(self): + """ + Origin altitude to publish: the literal `altitude` param, unless + use_geoid_altitude is set on real hardware, in which case it is the + egm96-5 geoid undulation at (lat, lon) plus desired_floor_amsl. + """ + if not self.get_parameter('use_geoid_altitude').value: + return self.get_parameter('altitude').value + if self.get_parameter('use_sim_time').value: + self.get_logger().info( + 'use_sim_time=true: using literal altitude (sim datum), not geoid.' + ) + return self.get_parameter('altitude').value + floor = self.get_parameter('desired_floor_amsl').value + model = self.get_parameter('geoid_model').value + try: + n = self._geoid_undulation(self._lat, self._lon, model) + except Exception as e: + literal = self.get_parameter('altitude').value + self.get_logger().error( + f'use_geoid_altitude=true but geoid lookup failed ({e}); falling ' + f'back to literal altitude {literal} m. LOCAL Z WILL BE OFFSET BY ' + f'THE GEOID (tens of m) — fix GeographicLib/GeoidEval before flight.' + ) + return literal + alt = n + floor + self.get_logger().info( + f'Geoid origin altitude: N({model})={n:.4f} + floor_amsl={floor:.4f} ' + f'=> {alt:.4f} m ellipsoidal (local z will equal OptiTrack z).' + ) + return alt + + def _on_existing_origin(self, _msg: GeoPointStamped): + """An origin already exists (e.g. from GNSS) — never override it.""" + if not self._origin_exists and not self._done: + self.get_logger().info( + 'Existing GPS origin detected; skipping synthetic origin.' + ) + self._origin_exists = True + + def _on_mavros_state(self, msg: State): + if msg.connected and self._connected_since is None: + self._connected_since = self.get_clock().now() + + def _tick(self): + if self._done: + return + if self._origin_exists: + self._done = True + self._timer.cancel() + return + if self._connected_since is None: + return + elapsed = (self.get_clock().now() - self._connected_since).nanoseconds * 1e-9 + if elapsed < self._settle_sec: + return + + msg = GeoPointStamped() + msg.header.stamp = self.get_clock().now().to_msg() + msg.position.latitude = self._lat + msg.position.longitude = self._lon + msg.position.altitude = self._alt + self._set_origin_pub.publish(msg) + self._publish_count += 1 + self.get_logger().info( + f'Published synthetic GPS origin ' + f'(lat={self._lat}, lon={self._lon}, alt={self._alt}) ' + f'[{self._publish_count}/3]' + ) + # Publish a few times in case MAVROS subscribed late, then stop. + if self._publish_count >= 3: + self._done = True + self._timer.cancel() + + +def main(args=None): + rclpy.init(args=args) + try: + node = MavrosGpOriginNode() + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/robot/ros_ws/src/perception/natnet_ros2/src/natnet_client_adapter.cpp b/robot/ros_ws/src/perception/natnet_ros2/src/natnet_client_adapter.cpp index 186f5572c..78caa940b 100644 --- a/robot/ros_ws/src/perception/natnet_ros2/src/natnet_client_adapter.cpp +++ b/robot/ros_ws/src/perception/natnet_ros2/src/natnet_client_adapter.cpp @@ -44,14 +44,22 @@ namespace void NATNET_CALLCONV sdk_frame_callback(sFrameOfMocapData * data, void * ctx) { - auto * frame_cb = static_cast *>(ctx); - if (!data || !frame_cb || !*frame_cb) { return; } + auto * cb_ctx = static_cast(ctx); + if (!data || !cb_ctx || !cb_ctx->cb || !*cb_ctx->cb) { return; } FrameSample fs; fs.frame_num = data->iFrame; fs.timestamp = data->fTimestamp; fs.params = static_cast(data->params); + // TransmitTimestamp is 0 on servers/streams that don't populate frame timing; + // only compute latency when it's present so downstream sampling can skip it. + if (data->TransmitTimestamp != 0 && cb_ctx->client) { + fs.transit_latency_s = + cb_ctx->client->SecondsSinceHostTimestamp(data->TransmitTimestamp); + fs.has_latency = true; + } + fs.bodies.reserve(static_cast(data->nRigidBodies)); for (int i = 0; i < data->nRigidBodies; ++i) { const sRigidBodyData & rb = data->RigidBodies[i]; @@ -63,7 +71,7 @@ void NATNET_CALLCONV sdk_frame_callback(sFrameOfMocapData * data, void * ctx) fs.bodies.push_back(s); } - (*frame_cb)(fs); + (*cb_ctx->cb)(fs); } /// Map NatNet SDK ErrorCode to our NatNetResult. @@ -159,8 +167,10 @@ std::vector NatNetClientAdapter::get_body_descriptors() void NatNetClientAdapter::set_frame_callback( std::function cb) { - user_cb_ = std::move(cb); - client_->SetFrameReceivedCallback(sdk_frame_callback, &user_cb_); + user_cb_ = std::move(cb); + cb_ctx_.client = client_.get(); + cb_ctx_.cb = &user_cb_; + client_->SetFrameReceivedCallback(sdk_frame_callback, &cb_ctx_); } // --------------------------------------------------------------------------- @@ -170,7 +180,9 @@ void NatNetClientAdapter::disconnect() client_->SetFrameReceivedCallback(sdk_frame_callback, nullptr); client_->Disconnect(); } - user_cb_ = nullptr; + user_cb_ = nullptr; + cb_ctx_.client = nullptr; + cb_ctx_.cb = nullptr; } } // namespace natnet_ros2 diff --git a/robot/ros_ws/src/perception/natnet_ros2/src/natnet_ros2_node.cpp b/robot/ros_ws/src/perception/natnet_ros2/src/natnet_ros2_node.cpp index 65059f659..2245055d4 100644 --- a/robot/ros_ws/src/perception/natnet_ros2/src/natnet_ros2_node.cpp +++ b/robot/ros_ws/src/perception/natnet_ros2/src/natnet_ros2_node.cpp @@ -1,26 +1,21 @@ -// natnet_ros2_node.cpp -// -// ROS 2 NatNet SDK node for OptiTrack Motive integration. -// -// Published topics (per tracked rigid body): -// /{robot_name}/perception/optitrack/{body_name} → PoseStamped -// /{robot_name}/perception/optitrack/{body_name}/pose_cov → PoseWithCovarianceStamped -// -// Parameters (see config/natnet_config.yaml): -// server_ip, client_ip, command_port, data_port, -// body_name, body_id (-1 = all), publish_direct_optitrack, -// frame_id, debug, position_covariance, orientation_covariance -// -// ROBOT_NAME is read from the environment variable set by AirStack's -// robot_name_map resolver at container startup. +// natnet_ros2_node.cpp — ROS 2 NatNet SDK node for OptiTrack Motive. +// Parameters are flattened from config/natnet_config.yaml by natnet_ros2.launch.py. +// See docs/robot/px4_external_vision.md. +#include #include -#include +#include +#include #include #include -#include #include #include +#include + +// POSIX sockets for the pre-connect reachability probe +#include +#include +#include // ROS 2 #include "rclcpp/rclcpp.hpp" @@ -32,6 +27,51 @@ #include "natnet_ros2/natnet_client_adapter.hpp" +// Ping the Motive command port before handing the server to the SDK: Connect() can +// assert deep in ClientCore::ValidateHostConnection (SIGABRT) rather than returning +// NetworkError when the host is unreachable. Do not remove this pre-check. +static bool natnet_server_reachable( + const std::string & server_ip, int command_port, int timeout_ms) +{ + const int fd = ::socket(AF_INET, SOCK_DGRAM, 0); + if (fd < 0) { return false; } + + timeval tv{}; + tv.tv_sec = timeout_ms / 1000; + tv.tv_usec = (timeout_ms % 1000) * 1000; + ::setsockopt(fd, SOL_SOCKET, SO_RCVTIMEO, &tv, sizeof(tv)); + + sockaddr_in addr{}; + addr.sin_family = AF_INET; + addr.sin_port = htons(static_cast(command_port)); + if (::inet_pton(AF_INET, server_ip.c_str(), &addr.sin_addr) != 1) { + ::close(fd); + return false; + } + + // Official connect packet: header (msg_id=NAT_CONNECT(0), size=271), + // 270-byte payload starting with "Ping" + NatNet version at offset 265, + // then a trailing NUL (matches the SDK PythonClient's send_request). + std::array pkt{}; + pkt[2] = 271 & 0xFF; + pkt[3] = 271 >> 8; + pkt[4] = 'P'; pkt[5] = 'i'; pkt[6] = 'n'; pkt[7] = 'g'; + pkt[4 + 265] = 4; // requested NatNet version 4.2.0.0 + pkt[4 + 266] = 2; + + bool reachable = false; + if (::sendto(fd, pkt.data(), pkt.size(), 0, + reinterpret_cast(&addr), sizeof(addr)) == + static_cast(pkt.size())) + { + uint8_t reply[512]; + reachable = ::recv(fd, reply, sizeof(reply), 0) > 0; + } + ::close(fd); + return reachable; +} + + // --------------------------------------------------------------------------- // NatNetROS2Node // --------------------------------------------------------------------------- @@ -41,8 +81,14 @@ class NatNetROS2Node : public rclcpp::Node using PoseStamped = geometry_msgs::msg::PoseStamped; using PoseWithCovarianceStamped = geometry_msgs::msg::PoseWithCovarianceStamped; - struct BodyPublishers + struct BodyConfig { + int32_t id = -1; + std::string rigid_body_name; + std::string topic_base; + bool publish_pose = true; + bool publish_pose_cov = true; + std::array covariance{}; rclcpp::Publisher::SharedPtr pose_pub; rclcpp::Publisher::SharedPtr pose_cov_pub; }; @@ -52,24 +98,33 @@ class NatNetROS2Node : public rclcpp::Node : Node("natnet_ros2_node") { // ----- Parameters -------------------------------------------------- - this->declare_parameter("server_ip", "192.168.1.1"); - this->declare_parameter("client_ip", "0.0.0.0"); - this->declare_parameter("command_port", 1510); - this->declare_parameter("data_port", 1511); - this->declare_parameter("connection_type", std::string("unicast")); - this->declare_parameter("multicast_address", std::string("239.255.42.99")); - this->declare_parameter("body_name", "robot_1"); - this->declare_parameter("body_id", -1); - this->declare_parameter("publish_direct_optitrack", true); - this->declare_parameter("publish_to_mavros", false); - this->declare_parameter("frame_id", "world"); - this->declare_parameter("debug", false); - this->declare_parameter( - "position_covariance", - std::vector{0.1,0.,0., 0.,0.1,0., 0.,0.,0.1}); - this->declare_parameter( - "orientation_covariance", - std::vector{0.01,0.,0., 0.,0.01,0., 0.,0.,0.01}); + this->declare_parameter("server_ip", "192.168.1.1"); + this->declare_parameter("client_ip", "0.0.0.0"); + this->declare_parameter("command_port", 1510); + this->declare_parameter("data_port", 1511); + this->declare_parameter("connection_type", std::string("unicast")); + this->declare_parameter("multicast_address", std::string("239.255.42.99")); + this->declare_parameter("frame_id", "world"); + this->declare_parameter("debug", false); + + // Latency sampling: skip a warm-up interval after the first frame (lets the + // SDK clock-sync settle and the stream reach steady state), then accumulate + // per-message transit latency over a fixed window and log a one-shot summary. + this->declare_parameter("latency_sampling_warmup_s", 5.0); + this->declare_parameter("latency_sampling_window_s", 20.0); + // Modeled latency for a pose to traverse the flight-controller hardware + // (MAVROS → MAVLink over USB/serial → PX4 uORB → EKF2). Added on top of the + // measured OptiTrack→ROS transport latency for the reported end-to-end figure. + this->declare_parameter("cube_orange_latency_ms", 5.0); + + // Parallel per-body arrays (flattened from natnet_config.yaml by the launch file). + this->declare_parameter("body_names", std::vector{}); + this->declare_parameter("body_ids", std::vector{}); + this->declare_parameter("body_topics", std::vector{}); + this->declare_parameter("body_pose", std::vector{}); + this->declare_parameter("body_pose_cov", std::vector{}); + this->declare_parameter("body_position_covariance", std::vector{}); + this->declare_parameter("body_orientation_covariance", std::vector{}); // ----- Read parameters --------------------------------------------- const auto connect_cfg = natnet_ros2::make_connect_config( @@ -88,19 +143,18 @@ class NatNetROS2Node : public rclcpp::Node this->get_parameter("connection_type").as_string().c_str()); } - body_name_ = this->get_parameter("body_name").as_string(); - body_id_ = static_cast(this->get_parameter("body_id").as_int()); - publish_direct_ = this->get_parameter("publish_direct_optitrack").as_bool(); - frame_id_ = this->get_parameter("frame_id").as_string(); - debug_ = this->get_parameter("debug").as_bool(); + frame_id_ = this->get_parameter("frame_id").as_string(); + debug_ = this->get_parameter("debug").as_bool(); - covariance_6x6_ = natnet_ros2::build_covariance_6x6( - this->get_parameter("position_covariance").as_double_array(), - this->get_parameter("orientation_covariance").as_double_array()); + latency_warmup_s_ = this->get_parameter("latency_sampling_warmup_s").as_double(); + latency_window_s_ = this->get_parameter("latency_sampling_window_s").as_double(); + cube_orange_latency_ms_ = this->get_parameter("cube_orange_latency_ms").as_double(); const char * rn = std::getenv("ROBOT_NAME"); robot_name_ = rn ? rn : "robot_1"; + build_body_configs(); + RCLCPP_INFO(get_logger(), "========================================="); RCLCPP_INFO(get_logger(), "NatNet ROS 2 Node"); RCLCPP_INFO(get_logger(), " robot_name: %s", robot_name_.c_str()); @@ -110,18 +164,19 @@ class NatNetROS2Node : public rclcpp::Node if (natnet_ros2::is_multicast(connect_cfg)) { RCLCPP_INFO(get_logger(), " multicast_addr: %s", connect_cfg.multicast_address.c_str()); } - RCLCPP_INFO(get_logger(), " body_id: %d (%s)", - static_cast(body_id_), - (body_id_ < 0) ? "track all" : "single body"); + RCLCPP_INFO(get_logger(), " tracked bodies: %zu", bodies_.size()); RCLCPP_INFO(get_logger(), "========================================="); // Production client — NatNetClientAdapter wraps the SDK client_ = std::make_unique(); - connect_and_setup(connect_cfg); + connect_cfg_ = connect_cfg; - refresh_timer_ = this->create_wall_timer( - std::chrono::seconds(1), - std::bind(&NatNetROS2Node::refresh_descriptions_if_needed, this)); + // Try to connect now; keep retrying. + if (!connect_and_setup(connect_cfg_)) { + connect_timer_ = this->create_wall_timer( + std::chrono::seconds(2), + std::bind(&NatNetROS2Node::retry_connect, this)); + } } // ----------------------------------------------------------------------- @@ -132,14 +187,10 @@ class NatNetROS2Node : public rclcpp::Node // ----------------------------------------------------------------------- // Called from the NatNetClientAdapter's frame trampoline. - // publish() and Clock::now() are thread-safe; pub_mutex_ guards map access. + // publish() and Clock::now() are thread-safe; bodies_ is immutable after init. // ----------------------------------------------------------------------- void on_frame(const natnet_ros2::FrameSample & frame) { - if (natnet_ros2::model_list_changed(frame.params)) { - needs_description_refresh_.store(true, std::memory_order_relaxed); - } - if (debug_) { RCLCPP_DEBUG(get_logger(), "Frame %d: %zu rigid bodies, ts=%.4f s", frame.frame_num, frame.bodies.size(), static_cast(frame.timestamp)); @@ -147,6 +198,8 @@ class NatNetROS2Node : public rclcpp::Node const rclcpp::Time stamp = this->get_clock()->now(); + maybe_sample_latency(frame, stamp); + for (const auto & rb : frame.bodies) { if (!natnet_ros2::is_tracking_valid(rb.params)) { if (debug_) { @@ -154,20 +207,14 @@ class NatNetROS2Node : public rclcpp::Node } continue; } - if (!natnet_ros2::should_publish_body(body_id_, rb.id)) { continue; } - - std::lock_guard lock(pub_mutex_); - const auto pub_it = publishers_.find(rb.id); - if (pub_it == publishers_.end()) { - needs_description_refresh_.store(true, std::memory_order_relaxed); - continue; - } + const auto it = bodies_.find(rb.id); + if (it == bodies_.end()) { continue; } // not configured for this robot const natnet_ros2::PoseData pose = natnet_ros2::rb_to_pose(rb); - const BodyPublishers & bp = pub_it->second; + const BodyConfig & body = it->second; - if (publish_direct_ && bp.pose_pub) { + if (body.publish_pose && body.pose_pub) { PoseStamped msg; msg.header.frame_id = frame_id_; msg.header.stamp = stamp; @@ -178,10 +225,10 @@ class NatNetROS2Node : public rclcpp::Node msg.pose.orientation.y = pose.qy; msg.pose.orientation.z = pose.qz; msg.pose.orientation.w = pose.qw; - bp.pose_pub->publish(msg); + body.pose_pub->publish(msg); } - if (bp.pose_cov_pub) { + if (body.publish_pose_cov && body.pose_cov_pub) { PoseWithCovarianceStamped cov_msg; cov_msg.header.frame_id = frame_id_; cov_msg.header.stamp = stamp; @@ -192,22 +239,32 @@ class NatNetROS2Node : public rclcpp::Node cov_msg.pose.pose.orientation.y = pose.qy; cov_msg.pose.pose.orientation.z = pose.qz; cov_msg.pose.pose.orientation.w = pose.qw; - cov_msg.pose.covariance = covariance_6x6_; - bp.pose_cov_pub->publish(cov_msg); + cov_msg.pose.covariance = body.covariance; + body.pose_cov_pub->publish(cov_msg); } } } private: // ----------------------------------------------------------------------- - void connect_and_setup(const natnet_ros2::ConnectConfig & cfg) + // Returns true once the handshake succeeds. + bool connect_and_setup(const natnet_ros2::ConnectConfig & cfg) { + // Wire-level probe first — see natnet_server_reachable() for why the + // SDK must never attempt Connect against a non-answering host. + if (!natnet_server_reachable(cfg.server_ip, cfg.command_port, 500)) { + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 10000, + "Motive at %s:%d not answering NatNet ping — waiting to connect.", + cfg.server_ip.c_str(), cfg.command_port); + return false; + } + const natnet_ros2::NegotiationResult neg = natnet_ros2::negotiate(*client_, cfg); if (!neg.ok) { - RCLCPP_ERROR(get_logger(), "%s", neg.log_message.c_str()); - return; + RCLCPP_WARN(get_logger(), "%s", neg.log_message.c_str()); + return false; } if (neg.server_info.host_present) { @@ -216,103 +273,201 @@ class NatNetROS2Node : public rclcpp::Node RCLCPP_WARN(get_logger(), "%s", neg.log_message.c_str()); } - refresh_descriptions_locked(); - client_->set_frame_callback( [this](const natnet_ros2::FrameSample & f) { on_frame(f); }); RCLCPP_INFO(get_logger(), "Frame callback registered — receiving mocap data."); + connected_ = true; + return true; } // ----------------------------------------------------------------------- - void refresh_descriptions_if_needed() + // Timer-driven reconnect. + void retry_connect() { - if (!needs_description_refresh_.exchange(false, std::memory_order_relaxed)) { + if (connected_) { + if (connect_timer_) { connect_timer_->cancel(); } return; } - RCLCPP_INFO(get_logger(), "Model list change detected — refreshing data descriptions."); - std::lock_guard lock(pub_mutex_); - refresh_descriptions_locked(); + RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 10000, + "NatNet not connected — retrying handshake to %s ...", + connect_cfg_.server_ip.c_str()); + if (connect_and_setup(connect_cfg_) && connect_timer_) { + connect_timer_->cancel(); + } } // ----------------------------------------------------------------------- - // Must be called with pub_mutex_ held (or from single-threaded init). + // Build the per-body config map + publishers from the parallel param arrays. + // Publishers are created up front (config-driven), so streaming begins as soon + // as frames arrive — no dependency on Motive's data-description handshake. // ----------------------------------------------------------------------- - void refresh_descriptions_locked() + void build_body_configs() { - if (!client_) { return; } - - // Always ensure the statically-configured body has a publisher - if (body_id_ >= 0) { - ensure_publisher_locked(body_id_, body_name_); + const auto names = this->get_parameter("body_names").as_string_array(); + const auto ids = this->get_parameter("body_ids").as_integer_array(); + const auto topics = this->get_parameter("body_topics").as_string_array(); + const auto pose = this->get_parameter("body_pose").as_bool_array(); + const auto pose_cov = this->get_parameter("body_pose_cov").as_bool_array(); + const auto pos_cov = this->get_parameter("body_position_covariance").as_double_array(); + const auto ori_cov = this->get_parameter("body_orientation_covariance").as_double_array(); + + const std::size_t n = std::min(names.size(), ids.size()); + if (names.size() != ids.size()) { + RCLCPP_WARN(get_logger(), + "body_names (%zu) and body_ids (%zu) length mismatch — using %zu.", + names.size(), ids.size(), n); } - const auto bodies = client_->get_body_descriptors(); - int newly_created = 0; - for (const auto & bd : bodies) { - // Store name for every body (including skeleton bones) - body_names_[bd.id] = bd.name; + for (std::size_t i = 0; i < n; ++i) { + BodyConfig body; + body.id = static_cast(ids[i]); + body.rigid_body_name = names[i]; + body.publish_pose = (i < pose.size()) ? pose[i] : true; + body.publish_pose_cov = (i < pose_cov.size()) ? pose_cov[i] : true; - // Skip skeleton bones (parent_id >= 0) - if (bd.parent_id >= 0) { continue; } + const std::string relative = (i < topics.size()) ? topics[i] : std::string{}; + body.topic_base = + natnet_ros2::body_topic_base(robot_name_, body.rigid_body_name, relative); - // When tracking a single body, skip others - if (!natnet_ros2::should_publish_body(body_id_, bd.id)) { continue; } + body.covariance = natnet_ros2::build_covariance_6x6( + cov_slice(pos_cov, i, _DEFAULT_POSITION_COVARIANCE), + cov_slice(ori_cov, i, _DEFAULT_ORIENTATION_COVARIANCE)); - if (ensure_publisher_locked(bd.id, bd.name)) { ++newly_created; } + if (body.publish_pose) { + body.pose_pub = this->create_publisher(body.topic_base, 10); + } + if (body.publish_pose_cov) { + body.pose_cov_pub = this->create_publisher( + body.topic_base + "/pose_cov", 10); + } + + RCLCPP_INFO(get_logger(), + "Tracking body id=%d name='%s' → %s (pose=%d pose_cov=%d)", + static_cast(body.id), body.rigid_body_name.c_str(), + body.topic_base.c_str(), + static_cast(body.publish_pose), + static_cast(body.publish_pose_cov)); + + bodies_.emplace(body.id, std::move(body)); } + } + + // ----------------------------------------------------------------------- + // Return the i-th 9-element covariance block from a flattened array, or the + // built-in default when the slice is missing. + static std::vector cov_slice( + const std::vector & flat, std::size_t i, const std::vector & fallback) + { + const std::size_t start = i * 9; + if (flat.size() < start + 9) { return fallback; } + return std::vector(flat.begin() + start, flat.begin() + start + 9); + } + + // ----------------------------------------------------------------------- + // Accumulate per-message transit latency over a fixed window and log a + // one-shot mean/stdev summary. Called once per frame from on_frame() (the SDK + // receive thread); all sampling state is touched only here, so no locking. + void maybe_sample_latency(const natnet_ros2::FrameSample & frame, + const rclcpp::Time & now) + { + if (latency_reported_ || !frame.has_latency) { return; } - if (newly_created > 0) { + if (!latency_first_seen_) { + latency_first_seen_ = true; + latency_first_time_ = now; RCLCPP_INFO(get_logger(), - "Data descriptions refreshed: %d new publisher(s) created.", newly_created); - } else { - RCLCPP_DEBUG(get_logger(), "Data descriptions refreshed: no new publishers."); + "Latency sampling armed: %.1fs warm-up, then %.1fs sampling window.", + latency_warmup_s_, latency_window_s_); + return; + } + + const double elapsed = (now - latency_first_time_).seconds(); + if (elapsed < latency_warmup_s_) { return; } // still warming up + if (elapsed > latency_warmup_s_ + latency_window_s_) { // window closed + report_latency(); + return; } + + const double lat = frame.transit_latency_s; + latency_count_ += 1; + latency_sum_s_ += lat; + latency_sum_sq_s_ += lat * lat; } // ----------------------------------------------------------------------- - bool ensure_publisher_locked(int32_t id, const std::string & name) + // Compute and log the latency summary once, then latch so it never repeats. + void report_latency() { - if (publishers_.count(id)) { return false; } + latency_reported_ = true; - const std::string topic_base = - natnet_ros2::optitrack_topic_base(robot_name_, name); + if (latency_count_ == 0) { + RCLCPP_WARN(get_logger(), + "Latency window elapsed but no timestamped frames were sampled " + "(server may not populate TransmitTimestamp)."); + return; + } - BodyPublishers bp; - if (publish_direct_) { - bp.pose_pub = this->create_publisher(topic_base, 10); + const double n = static_cast(latency_count_); + const double mean_s = latency_sum_s_ / n; + double var_s2 = 0.0; + if (latency_count_ > 1) { + // Sample variance (Bessel-corrected); clamp tiny negatives from round-off. + var_s2 = (latency_sum_sq_s_ - n * mean_s * mean_s) / (n - 1.0); + if (var_s2 < 0.0) { var_s2 = 0.0; } } - bp.pose_cov_pub = this->create_publisher( - natnet_ros2::optitrack_pose_cov_topic(robot_name_, name), 10); - publishers_.emplace(id, std::move(bp)); + const double mean_ms = mean_s * 1.0e3; + const double stdev_ms = std::sqrt(var_s2) * 1.0e3; + const double total_ms = mean_ms + cube_orange_latency_ms_; RCLCPP_INFO(get_logger(), - "Publisher registered: id=%d name='%s' → %s[/pose_cov]", - static_cast(id), name.c_str(), topic_base.c_str()); - return true; + "\n" + "========= OptiTrack -> drone message latency =========\n" + " sampling window : %.1f s (%llu frames)\n" + " transport mean : %.3f ms\n" + " transport std dev : %.3f ms\n" + " Cube Orange (model) : %.3f ms\n" + " estimated total : %.3f ms (to PX4 / EKF2 fusion)\n" + "======================================================", + latency_window_s_, + static_cast(latency_count_), + mean_ms, stdev_ms, cube_orange_latency_ms_, total_ms); } // ----------------------------------------------------------------------- // Parameters / state - std::string body_name_; - int32_t body_id_ = -1; - bool publish_direct_ = true; std::string frame_id_; - bool debug_ = false; + bool debug_ = false; std::string robot_name_; - std::array covariance_6x6_{}; + // Latency sampling parameters + running accumulators. + double latency_warmup_s_ = 5.0; + double latency_window_s_ = 20.0; + double cube_orange_latency_ms_ = 5.0; + bool latency_first_seen_ = false; + bool latency_reported_ = false; + rclcpp::Time latency_first_time_{0, 0, RCL_ROS_TIME}; + uint64_t latency_count_ = 0; + double latency_sum_s_ = 0.0; + double latency_sum_sq_s_ = 0.0; std::unique_ptr client_; + natnet_ros2::ConnectConfig connect_cfg_; + bool connected_ = false; + + std::unordered_map bodies_; - std::mutex pub_mutex_; - std::unordered_map body_names_; - std::unordered_map publishers_; + rclcpp::TimerBase::SharedPtr connect_timer_; - std::atomic needs_description_refresh_{false}; - rclcpp::TimerBase::SharedPtr refresh_timer_; + static const std::vector _DEFAULT_POSITION_COVARIANCE; + static const std::vector _DEFAULT_ORIENTATION_COVARIANCE; }; +const std::vector NatNetROS2Node::_DEFAULT_POSITION_COVARIANCE = + {1.0e-6, 0., 0., 0., 1.0e-6, 0., 0., 0., 1.0e-6}; +const std::vector NatNetROS2Node::_DEFAULT_ORIENTATION_COVARIANCE = + {3.0e-6, 0., 0., 0., 3.0e-6, 0., 0., 0., 3.0e-6}; + // --------------------------------------------------------------------------- int main(int argc, char ** argv) diff --git a/robot/ros_ws/src/perception/natnet_ros2/src/px4_param_setter_node.py b/robot/ros_ws/src/perception/natnet_ros2/src/px4_param_setter_node.py new file mode 100755 index 000000000..1a08bef30 --- /dev/null +++ b/robot/ros_ws/src/perception/natnet_ros2/src/px4_param_setter_node.py @@ -0,0 +1,296 @@ +#!/usr/bin/env python3 + +""" +PX4 Parameter Checker Node + +Compares the FCU's PX4 parameters against a configured set (see +config/px4_params.yaml) for mocap-only flight (OptiTrack external vision, no GNSS, +no magnetometer). **By default it only checks and flags** — it does not write to the +FCU. The desired values are meant to be set once by a human in QGroundControl (see +docs/robot/…/px4_external_vision.md); this node is a safety net that catches a +mis-configured FCU before flight. + +Two safety flags control behaviour: + +- ``auto_set`` (default ``false``): when ``true``, the node also *writes* any + mismatched param via ``param/set`` (ParamSetV2) and verifies the readback — the + legacy enforce behaviour. When ``false`` (default) the node never writes. +- ``on_mismatch`` (``warn`` | ``halt``, default ``warn``): with ``auto_set=false``, + what to do when a param disagrees. ``warn`` logs the diffs and lets the stack come + up; ``halt`` logs fatal and exits non-zero so a ``required`` launch node tears the + stack down. + +For each entry under the ``params.`` prefix the node waits for an FCU connection + +settle, reads the current value via ``get_parameters``, and compares. Type mapping +follows the YAML literal: integers → MAVLink int params, floats → float params — +write ``6.0`` (not ``6``) for float params like EKF2_EV_DELAY so the type matches. +""" + +import math +import sys + +import rclpy +from rclpy.node import Node +from rclpy.parameter import Parameter +from rcl_interfaces.msg import ParameterValue, ParameterType +from rcl_interfaces.srv import GetParameters +from mavros_msgs.msg import State +from mavros_msgs.srv import ParamSetV2 + + +class Px4ParamSetterNode(Node): + """PX4 parameter checker (optionally setter) via the MAVROS param plugin.""" + + def __init__(self): + super().__init__( + 'px4_param_setter', + automatically_declare_parameters_from_overrides=True, + ) + + self._enabled = self._param_or('enabled', True) + if not self._enabled: + self.get_logger().info('PX4 param checker disabled (enabled=false).') + return + + # Safety flags: check-only by default; opt in to writing with auto_set. + self._auto_set = bool(self._param_or('auto_set', False)) + self._on_mismatch = str(self._param_or('on_mismatch', 'warn')).lower() + if self._on_mismatch not in ('warn', 'halt'): + self.get_logger().warn( + f"Invalid on_mismatch {self._on_mismatch!r}; falling back to 'warn'." + ) + self._on_mismatch = 'warn' + + # Seconds after MAVROS connects before the first attempt (initial + # param-table pull over serial takes a while at 115200 baud). + self._settle_sec = float(self._param_or('settle_sec', 10.0)) + self._retry_period_sec = float(self._param_or('retry_period_sec', 2.0)) + self._max_attempts = int(self._param_or('max_attempts', 30)) + + # Desired FCU params from the params.* prefix; YAML int → PX4 int32, + # YAML float → PX4 float. + self._desired = { + name: p.value + for name, p in self.get_parameters_by_prefix('params').items() + } + self._pending = dict(self._desired) + self._changed: list[str] = [] + self._skipped: list[str] = [] + # (param_id, current, desired) for params that disagree and were NOT set + # (auto_set=false). Drives the on_mismatch policy in _finish(). + self._mismatched: list[tuple] = [] + self._attempts = 0 + self._connected_since = None + self._done = False + self._inflight = False + + if not self._pending: + self.get_logger().warn('No params.* entries configured; nothing to do.') + self._done = True + return + + self._get_cli = self.create_client(GetParameters, 'param_get_parameters') + self._set_cli = self.create_client(ParamSetV2, 'param_set') + self._state_sub = self.create_subscription( + State, 'mavros_state', self._on_mavros_state, 10 + ) + self._timer = self.create_timer(self._retry_period_sec, self._tick) + + mode = 'auto-set' if self._auto_set else f'check-only (on_mismatch={self._on_mismatch})' + self.get_logger().info( + f'PX4 param checker started [{mode}]: {len(self._pending)} params ' + f'({", ".join(sorted(self._pending))}), settle_sec={self._settle_sec}' + ) + + def _param_or(self, name, default): + """Return a declared-from-overrides parameter value, or the default.""" + if self.has_parameter(name): + value = self.get_parameter(name).value + if value is not None: + return value + return default + + # --- MAVROS state ------------------------------------------------------ + + def _on_mavros_state(self, msg: State): + if msg.connected and self._connected_since is None: + self._connected_since = self.get_clock().now() + self.get_logger().info('FCU connected; waiting for param table to settle.') + + # --- Main retry loop --------------------------------------------------- + + def _tick(self): + if self._done or self._inflight: + return + if self._connected_since is None: + return + elapsed = (self.get_clock().now() - self._connected_since).nanoseconds * 1e-9 + if elapsed < self._settle_sec: + return + if not self._pending: + self._finish() + return + if self._attempts >= self._max_attempts: + self.get_logger().error( + f'Giving up after {self._attempts} attempts; ' + f'unset params: {", ".join(sorted(self._pending))}' + ) + self._finish() + return + + self._attempts += 1 + param_id = sorted(self._pending)[0] + if not self._get_cli.service_is_ready() or not self._set_cli.service_is_ready(): + self.get_logger().info('MAVROS param services not ready yet; retrying.') + return + + self._inflight = True + req = GetParameters.Request(names=[param_id]) + future = self._get_cli.call_async(req) + future.add_done_callback( + lambda f, pid=param_id: self._on_get_done(pid, f) + ) + + # --- Get → compare → set → verify chain -------------------------------- + + def _on_get_done(self, param_id: str, future): + try: + resp = future.result() + except Exception as e: # noqa: BLE001 — retry on any transport error + self.get_logger().warn(f'{param_id}: get_parameters failed ({e}); will retry.') + self._inflight = False + return + + current = resp.values[0] if resp.values else None + if current is not None and self._matches(current, self._desired[param_id]): + self.get_logger().info(f'{param_id}: already {self._desired[param_id]} — skipping.') + self._skipped.append(param_id) + del self._pending[param_id] + self._inflight = False + return + if current is None or current.type == ParameterType.PARAMETER_NOT_SET: + # Param table likely not pulled yet — retry rather than flag/force-set. + self.get_logger().info(f'{param_id}: not in MAVROS param table yet; will retry.') + self._inflight = False + return + + # Mismatch. Check-only mode (default): record and flag, never write. + if not self._auto_set: + self._mismatched.append( + (param_id, self._value_of(current), self._desired[param_id]) + ) + del self._pending[param_id] + self._inflight = False + return + + req = ParamSetV2.Request() + req.force_set = False + req.param_id = param_id + req.value = self._to_parameter_value(self._desired[param_id]) + set_future = self._set_cli.call_async(req) + set_future.add_done_callback( + lambda f, pid=param_id, old=self._value_of(current): self._on_set_done(pid, old, f) + ) + + def _on_set_done(self, param_id: str, old_value, future): + self._inflight = False + try: + resp = future.result() + except Exception as e: # noqa: BLE001 — retry on any transport error + self.get_logger().warn(f'{param_id}: set failed ({e}); will retry.') + return + + desired = self._desired[param_id] + if not resp.success or not self._matches(resp.value, desired): + self.get_logger().warn( + f'{param_id}: set rejected or readback mismatch ' + f'(wanted {desired}, got {self._value_of(resp.value)}); will retry.' + ) + return + + self.get_logger().info(f'{param_id}: {old_value} -> {desired}') + self._changed.append(param_id) + del self._pending[param_id] + + def _finish(self): + self._done = True + self._timer.cancel() + self.get_logger().info( + f'PX4 param check finished: {len(self._skipped)} already correct, ' + f'{len(self._changed)} set, {len(self._mismatched)} mismatched, ' + f'{len(self._pending)} unread.' + ) + if self._changed: + self.get_logger().warn( + f'FCU parameters changed ({", ".join(sorted(self._changed))}). ' + 'Reboot the flight controller before flying so EKF2 starts clean.' + ) + + # Check-only mismatches: report each, then apply the on_mismatch policy. + if self._mismatched: + for pid, current, desired in sorted(self._mismatched): + self.get_logger().warn( + f'{pid}: FCU has {current}, expected {desired} ' + '(not set — auto_set=false). Fix in QGroundControl.' + ) + names = ", ".join(sorted(p for p, _, _ in self._mismatched)) + if self._on_mismatch == 'halt': + self.get_logger().fatal( + f'{len(self._mismatched)} PX4 param(s) wrong for external-vision ' + f'flight ({names}); halting (on_mismatch=halt). Set them in ' + 'QGroundControl or enable auto_set.' + ) + # SystemExit propagates out of spin(); main()'s finally shuts down + # rclpy. Non-zero code lets a `required` launch node tear the stack down. + sys.exit(1) + self.get_logger().warn( + f'{len(self._mismatched)} PX4 param(s) differ from the external-vision ' + f'set ({names}); continuing (on_mismatch=warn).' + ) + + # --- Value helpers ------------------------------------------------------ + + @staticmethod + def _to_parameter_value(value) -> ParameterValue: + pv = ParameterValue() + if isinstance(value, bool) or isinstance(value, int): + pv.type = ParameterType.PARAMETER_INTEGER + pv.integer_value = int(value) + elif isinstance(value, float): + pv.type = ParameterType.PARAMETER_DOUBLE + pv.double_value = value + else: + raise TypeError(f'Unsupported PX4 param value type: {type(value)}') + return pv + + @staticmethod + def _value_of(pv: ParameterValue): + if pv.type == ParameterType.PARAMETER_INTEGER: + return pv.integer_value + if pv.type == ParameterType.PARAMETER_DOUBLE: + return pv.double_value + return None + + @classmethod + def _matches(cls, pv: ParameterValue, desired) -> bool: + current = cls._value_of(pv) + if current is None: + return False + # FCU floats are float32 — compare with a tolerance that absorbs the + # float64 → float32 round trip. + return math.isclose(float(current), float(desired), rel_tol=1e-5, abs_tol=1e-6) + + +def main(args=None): + rclpy.init(args=args) + try: + node = Px4ParamSetterNode() + rclpy.spin(node) + except (KeyboardInterrupt, rclpy.executors.ExternalShutdownException): + pass + finally: + rclpy.try_shutdown() + + +if __name__ == '__main__': + main() diff --git a/robot/ros_ws/src/perception/natnet_ros2/src/vision_pose_converter_node.py b/robot/ros_ws/src/perception/natnet_ros2/src/vision_pose_converter_node.py index 9a36f879d..88caede52 100755 --- a/robot/ros_ws/src/perception/natnet_ros2/src/vision_pose_converter_node.py +++ b/robot/ros_ws/src/perception/natnet_ros2/src/vision_pose_converter_node.py @@ -7,6 +7,12 @@ for PX4 external pose estimation and state fusion. Converts from NatNet coordinate frame to a frame suitable for MAVROS. + +Topics are configurable so the bridge can be retargeted to other middleware. +``input_topic`` / ``output_pose_topic`` / ``output_pose_cov_topic`` default to the +relative names ``input_pose`` / ``output_pose`` / ``output_pose_cov`` (remappable), +but natnet_ros2.launch.py overrides them with the absolute, ROBOT_NAME-namespaced +topics from the robot's ``vision_pose`` block in natnet_config.yaml. """ import rclpy @@ -28,15 +34,46 @@ def __init__(self): self.declare_parameter('frame_id', 'world') self.declare_parameter('child_frame_id', 'base_link') self.declare_parameter('canonical_quaternion', True) + # Max output rate to MAVROS (0 = passthrough). Each pose becomes a + # ~116-byte VISION_POSITION_ESTIMATE on the FCU serial link; at + # 115200 baud (~11.5 kB/s) a full-rate 100+ Hz mocap stream alone + # overflows the MAVROS TX queue. EKF2 only needs 30-50 Hz. + self.declare_parameter('max_rate_hz', 30.0) + # Which MAVROS vision_pose topic(s) to publish: 'pose', 'pose_cov', or + # 'both'. MAVROS turns EACH of vision_pose/pose and vision_pose/pose_cov + # into its own VISION_POSITION_ESTIMATE on the FCU link, so 'both' sends + # msg 102 at 2x the rate. Use a single topic to halve serial TX load. + self.declare_parameter('publish_mode', 'both') + # Topic names — overridden by the launch file from the per-robot + # vision_pose block; defaults are the historical remappable relative names. + self.declare_parameter('input_topic', 'input_pose') + self.declare_parameter('output_pose_topic', 'output_pose') + self.declare_parameter('output_pose_cov_topic', 'output_pose_cov') self.frame_id = self.get_parameter('frame_id').value self.child_frame_id = self.get_parameter('child_frame_id').value self.canonical_quaternion = self.get_parameter('canonical_quaternion').value + max_rate_hz = self.get_parameter('max_rate_hz').value + publish_mode = str(self.get_parameter('publish_mode').value).lower() + if publish_mode not in ('pose', 'pose_cov', 'both'): + self.get_logger().warn( + f"Invalid publish_mode {publish_mode!r}; falling back to 'both'" + ) + publish_mode = 'both' + self._publish_pose = publish_mode in ('pose', 'both') + self._publish_pose_cov = publish_mode in ('pose_cov', 'both') + # 0.95 factor so an input stream at exactly max_rate_hz doesn't beat + # against the period check and alias down to half rate. + self._min_period_ns = 0 if max_rate_hz <= 0.0 else int(0.95e9 / max_rate_hz) + self._last_pub_ns = 0 + input_topic = self.get_parameter('input_topic').value + output_pose_topic = self.get_parameter('output_pose_topic').value + output_pose_cov_topic = self.get_parameter('output_pose_cov_topic').value # Subscribers self.pose_sub = self.create_subscription( PoseWithCovarianceStamped, - 'input_pose', + input_topic, self._on_pose, 10 ) @@ -44,19 +81,22 @@ def __init__(self): # Publishers self.pose_pub = self.create_publisher( PoseStamped, - 'output_pose', + output_pose_topic, 10 ) self.pose_cov_pub = self.create_publisher( PoseWithCovarianceStamped, - 'output_pose_cov', + output_pose_cov_topic, 10 ) self.get_logger().info( f'Vision pose converter started ' f'(frame_id={self.frame_id!r}, child_frame_id={self.child_frame_id!r}, ' - f'canonical_quaternion={self.canonical_quaternion})' + f'canonical_quaternion={self.canonical_quaternion}, ' + f'max_rate_hz={max_rate_hz}, publish_mode={publish_mode!r}, ' + f'input_topic={input_topic!r}, output_pose_topic={output_pose_topic!r}, ' + f'output_pose_cov_topic={output_pose_cov_topic!r})' ) @staticmethod @@ -83,18 +123,26 @@ def _on_pose(self, msg: PoseWithCovarianceStamped): so that EKF consumers never see a sign-flip discontinuity. """ try: + if self._min_period_ns: + now_ns = self.get_clock().now().nanoseconds + if now_ns - self._last_pub_ns < self._min_period_ns: + return + self._last_pub_ns = now_ns + msg.header.frame_id = self.frame_id if self.canonical_quaternion: msg.pose.pose.orientation = self._canonical_quaternion( msg.pose.pose.orientation ) - self.pose_cov_pub.publish(msg) + if self._publish_pose_cov: + self.pose_cov_pub.publish(msg) - pose_msg = PoseStamped() - pose_msg.header = msg.header - pose_msg.pose = msg.pose.pose - self.pose_pub.publish(pose_msg) + if self._publish_pose: + pose_msg = PoseStamped() + pose_msg.header = msg.header + pose_msg.pose = msg.pose.pose + self.pose_pub.publish(pose_msg) except Exception as e: self.get_logger().error(f"Error converting pose: {e}") diff --git a/robot/ros_ws/src/perception/natnet_ros2/test/test_natnet_logic.cpp b/robot/ros_ws/src/perception/natnet_ros2/test/test_natnet_logic.cpp index 7a144ef9b..5796c121b 100644 --- a/robot/ros_ws/src/perception/natnet_ros2/test/test_natnet_logic.cpp +++ b/robot/ros_ws/src/perception/natnet_ros2/test/test_natnet_logic.cpp @@ -136,6 +136,46 @@ TEST(TopicNames, LeadingSlashPresent) EXPECT_EQ(optitrack_topic_base("robot_1", "Body")[0], '/'); } +TEST(TopicNames, NamespacedTopicStripsLeadingSlashes) +{ + EXPECT_EQ(namespaced_topic("robot_1", "perception/optitrack/drone"), + "/robot_1/perception/optitrack/drone"); + EXPECT_EQ(namespaced_topic("robot_1", "/perception/optitrack/drone"), + "/robot_1/perception/optitrack/drone"); + EXPECT_EQ(namespaced_topic("robot_2", "///a/b"), "/robot_2/a/b"); +} + +TEST(TopicNames, BodyTopicBaseUsesOverrideWhenSet) +{ + // Empty override → default perception/optitrack/{name} + EXPECT_EQ(body_topic_base("robot_1", "Drone", ""), + "/robot_1/perception/optitrack/Drone"); + // Non-empty override → namespaced relative leaf (decoupled from body name) + EXPECT_EQ(body_topic_base("robot_1", "Drone", "perception/optitrack/drone"), + "/robot_1/perception/optitrack/drone"); + EXPECT_EQ(body_topic_base("robot_3", "Target", "perception/optitrack/target"), + "/robot_3/perception/optitrack/target"); +} + +// =========================================================================== +// Multi-body filtering — body_is_configured +// =========================================================================== + +TEST(BodyIsConfigured, MatchesConfiguredIds) +{ + const std::vector ids = {1, 100}; + EXPECT_TRUE(body_is_configured(ids, 1)); + EXPECT_TRUE(body_is_configured(ids, 100)); + EXPECT_FALSE(body_is_configured(ids, 2)); +} + +TEST(BodyIsConfigured, EmptySetMatchesNothing) +{ + const std::vector ids = {}; + EXPECT_FALSE(body_is_configured(ids, 0)); + EXPECT_FALSE(body_is_configured(ids, 1)); +} + // =========================================================================== // Server negotiation — validate_connection_type diff --git a/robot/ros_ws/src/perception/natnet_ros2/test/test_natnet_ros2.py b/robot/ros_ws/src/perception/natnet_ros2/test/test_natnet_ros2.py index cba5a3444..37526cd18 100644 --- a/robot/ros_ws/src/perception/natnet_ros2/test/test_natnet_ros2.py +++ b/robot/ros_ws/src/perception/natnet_ros2/test/test_natnet_ros2.py @@ -1,20 +1,15 @@ # Copyright (c) 2024 Carnegie Mellon University # MIT License - see LICENSE in the repository root for full text. -"""Unit tests for natnet_ros2 Python source code. +"""Unit tests for natnet_ros2 Python helpers (no ROS install required). -These tests import the actual production source files and stub out ROS at the -import boundary so no ROS installation is required. +Stubs rclpy/launch at import time. Covers ``VisionPoseConverterNode`` quaternion +canonicalisation, configurable-topic wiring, and ``natnet_ros2.launch.py`` +profile-flattening helpers (server + per-body arrays, env expansion, namespacing). -Coverage here: - vision_pose_converter_node.py → VisionPoseConverterNode._canonical_quaternion() - → VisionPoseConverterNode._on_pose() frame_id assignment - -NOT covered here (C++ — requires colcon build + gtest): - natnet_ros2_node.cpp → build_covariance_6x6(), topic name construction, - connection_type validation, SDK frame callback logic. - These live in test_natnet_logic.cpp in the same test/ directory. +C++ logic (``natnet_logic.hpp``) is tested in ``test_natnet_logic.cpp`` via colcon. """ +import importlib.util import sys from pathlib import Path from types import SimpleNamespace @@ -28,22 +23,31 @@ # metaclass machinery returns a Mock for attribute access instead of running # __init_subclass__ / defining methods). We supply a real dummy base class # so the actual class body — including _canonical_quaternion — is defined. +# +# The fake also records declared params and created sub/pub topics so the +# configurable-topic wiring can be asserted without a ROS install. # --------------------------------------------------------------------------- class _FakeNode: + # Per-test parameter overrides keyed by name; consulted by declare_parameter so + # values survive the node's super().__init__ (which resets per-instance state). + _overrides: dict = {} + def __init__(self, name: str): - pass + self._params: dict = {} + self.created_subscriptions: list = [] + self.created_publishers: list = [] def get_logger(self): return MagicMock() - def declare_parameter(self, *args, **kwargs): - pass + def declare_parameter(self, name, default=None): + self._params[name] = self._overrides.get(name, default) def get_parameter(self, name): - m = MagicMock() - m.value = MagicMock() - return m - def create_subscription(self, *args, **kwargs): + return SimpleNamespace(value=self._params.get(name)) + def create_subscription(self, msg_type, topic, callback, qos): + self.created_subscriptions.append(topic) return MagicMock() - def create_publisher(self, *args, **kwargs): + def create_publisher(self, msg_type, topic, qos): + self.created_publishers.append(topic) return MagicMock() @@ -62,6 +66,36 @@ def create_publisher(self, *args, **kwargs): from vision_pose_converter_node import VisionPoseConverterNode # noqa: E402 +# --------------------------------------------------------------------------- +# Load natnet_ros2.launch.py with its heavy launch/ROS deps stubbed, so the +# pure flattening helpers can be unit-tested without a ROS install. +# --------------------------------------------------------------------------- + +for _mod in ( + "ament_index_python", + "ament_index_python.packages", + "launch", + "launch.actions", + "launch.launch_description_sources", + "launch.substitutions", + "launch_ros", + "launch_ros.actions", +): + sys.modules.setdefault(_mod, MagicMock()) + +# yaml is only needed by _load_natnet_config (not the flattening helpers); stub it +# if PyYAML is absent so the launch module still imports in a minimal unit env. +try: + import yaml # noqa: F401 +except ImportError: + sys.modules.setdefault("yaml", MagicMock()) + +_launch_path = Path(__file__).resolve().parent.parent / "launch" / "natnet_ros2.launch.py" +_spec = importlib.util.spec_from_file_location("natnet_ros2_launch_under_test", _launch_path) +natnet_launch = importlib.util.module_from_spec(_spec) +_spec.loader.exec_module(natnet_launch) + + # --------------------------------------------------------------------------- # Helpers # --------------------------------------------------------------------------- @@ -150,3 +184,104 @@ def test_canonical_quaternion_dual_sign_produces_same_result(): assert out_pos.x == pytest.approx(out_neg.x) assert out_pos.y == pytest.approx(out_neg.y) assert out_pos.z == pytest.approx(out_neg.z) + + +# --------------------------------------------------------------------------- +# VisionPoseConverterNode — configurable input/output topics +# --------------------------------------------------------------------------- + +@pytest.mark.unit +def test_vision_pose_converter_default_topics(): + """Defaults reproduce the historical relative (remappable) topic names.""" + node = VisionPoseConverterNode() + assert node.created_subscriptions == ["input_pose"] + assert node.created_publishers == ["output_pose", "output_pose_cov"] + + +@pytest.mark.unit +def test_vision_pose_converter_topic_overrides_applied(): + """When the topic params are set, sub/pub use those exact names.""" + _FakeNode._overrides = { + "input_topic": "/robot_2/perception/optitrack/drone/pose_cov", + "output_pose_topic": "/robot_2/custom/vision/pose", + "output_pose_cov_topic": "/robot_2/custom/vision/pose_cov", + } + try: + node = VisionPoseConverterNode() + finally: + _FakeNode._overrides = {} + assert node.created_subscriptions == ["/robot_2/perception/optitrack/drone/pose_cov"] + assert node.created_publishers == [ + "/robot_2/custom/vision/pose", + "/robot_2/custom/vision/pose_cov", + ] + + +# --------------------------------------------------------------------------- +# natnet_ros2.launch.py — pure config-flattening helpers +# --------------------------------------------------------------------------- + +@pytest.mark.unit +def test_expand_env_uses_default_when_unset(monkeypatch): + monkeypatch.delenv("NATNET_SERVER_IP", raising=False) + assert natnet_launch._expand_env("$(env NATNET_SERVER_IP 172.31.0.200)") == "172.31.0.200" + + +@pytest.mark.unit +def test_expand_env_uses_environment_value(monkeypatch): + monkeypatch.setenv("NATNET_SERVER_IP", "10.0.0.5") + assert natnet_launch._expand_env("$(env NATNET_SERVER_IP 172.31.0.200)") == "10.0.0.5" + + +@pytest.mark.unit +def test_namespaced_strips_and_prefixes(): + assert natnet_launch._namespaced("robot_1", "perception/optitrack/drone") == \ + "/robot_1/perception/optitrack/drone" + assert natnet_launch._namespaced("robot_2", "/already/abs") == "/robot_2/already/abs" + + +@pytest.mark.unit +def test_build_node_params_flattens_bodies(): + server = {"server_ip": "1.2.3.4", "command_port": 1510, "connection_type": "unicast"} + profile = { + "bodies": [ + { + "rigid_body_name": "Drone", + "id": 1, + "topic": "perception/optitrack/drone", + "pose": True, + "pose_cov": True, + "position_covariance": [9.0] * 9, + "orientation_covariance": [8.0] * 9, + }, + { + "rigid_body_name": "Target", + "id": 100, + "topic": "perception/optitrack/target", + "pose": True, + "pose_cov": False, + }, + ] + } + params = natnet_launch._build_node_params(server, profile) + + assert params["server_ip"] == "1.2.3.4" + assert params["body_names"] == ["Drone", "Target"] + assert params["body_ids"] == [1, 100] + assert params["body_topics"] == ["perception/optitrack/drone", "perception/optitrack/target"] + assert params["body_pose"] == [True, True] + assert params["body_pose_cov"] == [True, False] + # 9 floats per body, flattened in body order. + assert len(params["body_position_covariance"]) == 18 + assert params["body_position_covariance"][:9] == [9.0] * 9 + # Target omitted its covariance → built-in default fills its slice. + assert params["body_position_covariance"][9:] == natnet_launch._DEFAULT_POSITION_COVARIANCE + + +@pytest.mark.unit +def test_build_node_params_empty_profile(): + """A robot with no profile yields empty body arrays (node tracks nothing).""" + params = natnet_launch._build_node_params({}, {}) + assert params["body_names"] == [] + assert params["body_ids"] == [] + assert params["body_position_covariance"] == [] diff --git a/robot/ros_ws/src/perception/perception_bringup/launch/perception.launch.xml b/robot/ros_ws/src/perception/perception_bringup/launch/perception.launch.xml index a79ff85a1..1e9f8662e 100644 --- a/robot/ros_ws/src/perception/perception_bringup/launch/perception.launch.xml +++ b/robot/ros_ws/src/perception/perception_bringup/launch/perception.launch.xml @@ -75,7 +75,8 @@ - + diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/.gitignore b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/.gitignore new file mode 100644 index 000000000..0db35ef6e --- /dev/null +++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/.gitignore @@ -0,0 +1,9 @@ +# OptiTrack SDK archives and build artifacts (reference tree may exist locally) +**/*.obj +**/*.pdb +**/*.exe +**/*.iobj +**/*.ipdb +**/*.tlog/ +**/__pycache__/ +**/*.pyc diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/README.md b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/README.md new file mode 100644 index 000000000..3bb05f0b6 --- /dev/null +++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/README.md @@ -0,0 +1,160 @@ +# OptiTrack NatNet Emulator (Isaac Sim Extension) + +Python NatNet **server** emulator for AirStack simulation and integration testing with [`natnet_ros2`](../../../../robot/ros_ws/src/perception/natnet_ros2/). + +The extension has two layers: + +1. **Transport + protocol** (`optitrack.natnet.emulator.server`) — UDP NatNet server, ctypes wire types, MODELDEF cache, frame streaming. Importable outside Isaac Sim (unit tests, host-side integration). +2. **Isaac integration** (`optitrack.natnet.emulator.isaac`) — stage-driven `/World/NatNetInterface` config prim, pose sampling on physics steps, Kit UI editor, and Pegasus launch-script helpers. + +## Layout + +``` +optitrack.natnet.emulator/ +├── config/extension.toml # Kit manifest (server module + UI entry point) +├── schema/schema.usda # Typed NatNet interface attribute definitions +├── setup.py +├── docs/ # (legacy design notes — see docs/simulation/isaac_sim/natnet_emulator.md) +├── test/ # Co-located unit tests (proxied by tests/sim/) +└── optitrack/natnet/emulator/ + ├── defaults.py # Reference Drone → prim bindings for tests + ├── server/ # NatNet UDP server (transport + protocol) + │ ├── natnet_server.py # Base server, queue, MODELDEF cache + │ ├── natnet_unicast_server.py + │ ├── natnet_data_types.py + │ ├── natnet_model_types.py + │ └── natnet_server_types.py + └── isaac/ # Isaac Sim wrapper (Kit + USD) + ├── config.py # Pure-Python NatNetInterfaceConfig model + ├── usd_bindings.py # Author/read interface prims on a stage + ├── catalog.py # Config → sDataDescriptions (MODELDEF) + ├── frames.py # Prim poses → sFrameOfMocapData + ├── manager.py # NatNetServerManager (lifecycle + sampling) + ├── scene_setup.py # Pegasus launch helpers (start_drone_natnet_server) + └── ui_extension.py # Docked editor panel (NatNetEmulatorExtension) +``` + +## Responsibilities + +| Layer | Role | +|-------|------| +| **Server** | UDP transport; `NAT_CONNECT` / `NAT_SERVERINFO`; `NAT_REQUEST_MODELDEF`; `NAT_KEEPALIVE`; `NAT_ECHOREQUEST` / `NAT_ECHORESPONSE`; `NAT_FRAMEOFDATA` on the **data port** (1511). MODELDEF stored as packed bytes via `set_model_def_payload()`. Frames enqueued with `enqueue_mocap_data()`. | +| **Isaac wrapper** | Authors and reads the NatNet interface config prim; builds MODELDEF from scene config; samples tracked prim world poses each physics step; calls `flush_mocap_data()` synchronously (background timer disabled — see below). | +| **`defaults.py`** | Hardcoded `Drone` → `/World/base_link` binding for legacy tests; production paths use the stage prim via `scene_setup.build_drone_config()`. | + +The server does **not** own prim-path bindings. The Isaac layer calls `set_model_def_payload(catalog.pack())` after building `sDataDescriptions` from the interface config. + +## Stage-driven config prim + +Configuration lives on a USD prim (conventionally `/World/NatNetInterface`) with `natnet:*` attributes: + +- Server: IP, unicast/multicast mode, command/data ports, publish rate, NatNet version, up-axis, optional pose noise. +- Bodies: multi-apply `natnet:body::*` fields mapping rigid-body name / streaming ID → target prim path. + +`NatNetServerManager` scans the stage, resyncs the catalog when the prim changes, and streams one rigid body per configured target. Missing prims emit **lost** bodies (NaN position, tracking-invalid bit clear) until the target appears — important for Pegasus drones spawned on first Play. + +**Up axis:** default `Z` passes Isaac/USD world poses through unchanged (matches `natnet_ros2`). Set `Y` to emulate a Y-up Motive room. + +## Streaming model (Isaac) + +Inside Kit, the server's background `_data_update_loop` is **disabled** (`auto_stream = False`) because the GIL-starved daemon thread does not reliably transmit frames. Instead, each physics step: + +1. `NatNetServerManager.sample_once()` reads prim poses and `enqueue_mocap_data(frame)`. +2. `NatNetUnicastServer.flush_mocap_data()` sends immediately on the physics-step thread. + +Outside Isaac (host unit tests), `auto_stream=True` uses the timer-driven loop. + +Default Docker sim IP: **`172.31.0.200`** (Isaac container on the AirStack bridge network). + +## Enabling in AirStack + +**Robot:** `LAUNCH_NATNET=true` in `.env` → `natnet_ros2` in perception bringup. Configure Motive/emulator IP in [`natnet_config.yaml`](../../../../robot/ros_ws/src/perception/natnet_ros2/config/natnet_config.yaml). + +**Isaac Sim:** set `ISAAC_SIM_SCRIPT_NAME` to a NatNet launch script (NatNet always starts — no `LAUNCH_NATNET` gate in the script): + +| Script | Use | +|--------|-----| +| `example_one_px4_pegasus_natnet_launch_script.py` | Single drone + static `Target` | +| `example_multi_px4_pegasus_natnet_launch_script.py` | `NUM_ROBOTS` drones + shared `Target` (system tests with NatNet use this even for `NUM_ROBOTS=1`) | + +Baseline Pegasus scripts (`example_one_px4_pegasus_launch_script.py`, `example_multi_px4_pegasus_launch_script.py`) have **no** NatNet integration. + +Convenience bundle for NatNet + external-vision PX4 SITL: + +```bash +airstack up --env-file overrides/isaac-natnet-vision.env +``` + +See [optitrack-development skill](../../../../.agents/skills/optitrack-development/SKILL.md) for wire-protocol details, libNatNet 4.4 unicast quirks, and debugging. + +## Usage + +### Server only (no Kit) + +```python +from optitrack.natnet.emulator import NatNetUnicastServer, make_default_drone_catalog +from optitrack.natnet.emulator.isaac.frames import BodySample, build_frame + +server = NatNetUnicastServer(local_interface="172.31.0.200") +server.set_model_def_payload(make_default_drone_catalog().pack()) +server.start() + +frame = build_frame(0, [BodySample(1, (0, 0, 1), (0, 0, 0, 1))]) +server.enqueue_mocap_data(frame) +server.flush_mocap_data() +``` + +### Isaac launch script + +```python +from optitrack.natnet.emulator.isaac import start_drone_natnet_server + +# Keep a reference to the manager for the sim lifetime. +manager = start_drone_natnet_server( + stage, + drones=[("Drone", 1, "/World/drone1/base_link")], + server_ip="172.31.0.200", +) +``` + +### Kit UI + +The extension registers **Window → NatNet Emulator** — a docked panel to create/edit the interface prim, start/stop the server, and view live body readouts. The same `NatNetServerManager` backs both the UI and launch-script paths. + +## Protocol notes (unicast, libNatNet 4.4) + +| Port | Traffic | +|------|---------| +| **1510** | Command: `NAT_CONNECT`, `NAT_REQUEST_MODELDEF`, keepalives, echo | +| **1511** | Data: `NAT_FRAMEOFDATA` — **must** be sent from a socket bound to the data port | + +Frames sent from the command socket are silently dropped by libNatNet. Every frame payload must include the 4-byte end-of-data tag expected by the C SDK unpacker. + +Full handshake layouts and sniffing workflow: [optitrack-development skill](../../../../.agents/skills/optitrack-development/SKILL.md). + +## Tests + +| Tier | Mark | What | +|------|------|------| +| Unit | `unit` | Serializers, protocol, config, USD authoring, catalog, pose sampling, server lifecycle, scene setup | +| Integration | `integration` | Host emulator → robot `natnet_ros2` pose Hz | + +Co-located tests live in `test/`. Pytest discovers them via thin proxies in [`tests/sim/optitrack_natnet_emulator/`](../../../../tests/sim/optitrack_natnet_emulator/). + +```bash +# Unit (no Docker / no SDK) +pytest tests/sim/optitrack_natnet_emulator/ -m unit -v + +# Integration (robot container + NatNet SDK) +pytest tests/integration/natnet/ -m integration -v +``` + +Representative unit modules: `test_unicast_protocol.py`, `test_pose_streaming.py`, `test_interface_authoring.py`, `test_server_lifecycle.py`, `test_scene_setup.py`. + +## Reference material + +- User guide: [`docs/simulation/isaac_sim/natnet_emulator.md`](../../../../docs/simulation/isaac_sim/natnet_emulator.md) +- Robot client: [`natnet_ros2/README.md`](../../../../robot/ros_ws/src/perception/natnet_ros2/README.md) +- Integration tier: [`tests/integration/natnet/README.md`](../../../../tests/integration/natnet/README.md) + +OptiTrack SDK sample headers may exist locally under `NatNetClientSDK/` for wire-format reference; they are **not** redistributed by AirStack (proprietary license). diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/__init__.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/__init__.py new file mode 100644 index 000000000..39ed38144 --- /dev/null +++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/__init__.py @@ -0,0 +1 @@ +"""OptiTrack NatNet packages for AirStack Isaac Sim integration.""" diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/__init__.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/__init__.py new file mode 100644 index 000000000..b19da2cbc --- /dev/null +++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/__init__.py @@ -0,0 +1 @@ +"""NatNet simulation components.""" diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/__init__.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/__init__.py new file mode 100644 index 000000000..e819d8b1b --- /dev/null +++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/__init__.py @@ -0,0 +1,20 @@ +"""OptiTrack Motive NatNet emulator for Isaac Sim.""" + +from .defaults import ( + DEFAULT_DRONE_BINDING, + DEFAULT_TRACKED_BODY_BINDINGS, + TrackedBodyBinding, +) +from .server import Client, NatNetServer, NatNetUnicastServer, TransmissionType +from .server.natnet_model_types import make_default_drone_catalog + +__all__ = [ + "Client", + "DEFAULT_DRONE_BINDING", + "DEFAULT_TRACKED_BODY_BINDINGS", + "NatNetServer", + "NatNetUnicastServer", + "TrackedBodyBinding", + "TransmissionType", + "make_default_drone_catalog", +] diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/defaults.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/defaults.py new file mode 100644 index 000000000..2c1915264 --- /dev/null +++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/defaults.py @@ -0,0 +1,25 @@ +"""Reference tracked-body defaults for tests and the future Isaac Sim wrapper.""" + +from __future__ import annotations + +from dataclasses import dataclass + + +@dataclass(frozen=True) +class TrackedBodyBinding: + """Maps a NatNet rigid body to a USD prim path (not sent on the NatNet wire).""" + + name: str + id: int + prim_path: str + parent_id: int = -1 + + +# Single-drone NatNet Pegasus scenes (example_one_px4_pegasus_natnet_launch_script.py). +DEFAULT_DRONE_BINDING = TrackedBodyBinding( + name="Drone", + id=1, + prim_path="/World/base_link", +) + +DEFAULT_TRACKED_BODY_BINDINGS: tuple[TrackedBodyBinding, ...] = (DEFAULT_DRONE_BINDING,) diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/__init__.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/__init__.py new file mode 100644 index 000000000..c84c1fe4b --- /dev/null +++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/__init__.py @@ -0,0 +1,11 @@ +"""NatNet UDP server implementation (unicast; multicast planned).""" + +from .natnet_server import Client, NatNetServer, TransmissionType +from .natnet_unicast_server import NatNetUnicastServer + +__all__ = [ + "Client", + "NatNetServer", + "NatNetUnicastServer", + "TransmissionType", +] diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_common.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_common.py new file mode 100644 index 000000000..1eb32177c --- /dev/null +++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_common.py @@ -0,0 +1,27 @@ +from enum import IntEnum +import ctypes + +class ModelLimits(IntEnum): + MAX_MODELS = 2000 # maximum number of total models (data descriptions) + MAX_MARKERSETS = 1000 # maximum number of MarkerSets + MAX_RIGIDBODIES = 1000 # maximum number of RigidBodies + MAX_ASSETS = 1000 # Maximum number of Assets + MAX_NAMELENGTH = 256 # maximum length for strings + MAX_MARKERS = 200 # maximum number of markers per MarkerSet + MAX_RBMARKERS = 20 # maximum number of markers per RigidBody + MAX_SKELETONS = 100 # maximum number of skeletons + MAX_SKELRIGIDBODIES = 200 # maximum number of RididBodies per Skeleton + MAX_LABELED_MARKERS = 1000 # maximum number of labeled markers per frame + MAX_UNLABELED_MARKERS = 1000 # maximum number of unlabeled (other) markers per frame + + MAX_FORCEPLATES = 100 # maximum number of force plate 'bundles' + MAX_DEVICES = 100 # maximum number of peripheral device 'bundles' + MAX_ANALOG_CHANNELS = 32 # maximum number of data channels (signals) per analog/force plate device + MAX_ANALOG_SUBFRAMES = 30 # maximum number of analog/force plate frames per mocap frame + + MAX_PACKETSIZE = 65503 # max size of packet in bytes (actual packet size is dynamic) + # (65535 byte IP limit - 20 byte IP header - 8 byte UDP header - 4 byte sPacket header = 65503 bytes) + + + +MarkerData = ctypes.c_float * 3 diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_data_types.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_data_types.py new file mode 100644 index 000000000..91e23a412 --- /dev/null +++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_data_types.py @@ -0,0 +1,261 @@ +import ctypes +import struct +from .natnet_common import ModelLimits, MarkerData + +class sMarker(ctypes.Structure): + _pack_ = 1 + _fields_ = [ + ("ID", ctypes.c_int32), + ("x", ctypes.c_float), + ("y", ctypes.c_float), + ("z", ctypes.c_float), + ("size", ctypes.c_float), + ("params", ctypes.c_int16), + ("residual", ctypes.c_float) + ] + + def pack(self) -> bytes: + return struct.pack(' bytes: + # szName is null-terminated on the wire. + name_bytes = self.szName.rstrip(b'\x00') + b'\x00' + payload = bytearray(name_bytes) + payload += struct.pack(' bytes: + return struct.pack(' bytes: + payload = bytearray(struct.pack(' bytes: + payload = bytearray(struct.pack(' bytes: + payload = bytearray(struct.pack(' bytes: + payload = bytearray(struct.pack(' bytes: + payload = bytearray(struct.pack(' bytes: + """NatNet 4.1+ prefixes each collection with a 4-byte byte count.""" + payload = bytearray(struct.pack(' 0) or natnet_major > 4: + payload += struct.pack(' bytes: + payload = bytearray() + + payload += struct.pack(' bytes: + # szName is null-terminated on the wire, not fixed MAX_NAMELENGTH. + name_bytes = self.szName.rstrip(b"\x00") + b"\x00" + payload = bytearray(name_bytes) + payload += struct.pack( + " bytes: + if self.type == int(DataDescriptors.Descriptor_RigidBody): + body = self.RigidBodyDescription.pack() + else: + raise ValueError(f"Unsupported data description type: {self.type}") + payload = bytearray(struct.pack(" bytes: + payload = bytearray(struct.pack(" sDataDescriptions: + """Build the default single-body catalog (Drone id=1) for natnet_ros2.""" + descriptions = sDataDescriptions() + descriptions.nDataDescriptions = 1 + desc = descriptions.arrDataDescriptions[0] + desc.type = int(DataDescriptors.Descriptor_RigidBody) + rb = desc.RigidBodyDescription + rb.szName = b"Drone" + rb.ID = 1 + rb.parentID = -1 + rb.offsetqw = 1.0 + rb.nMarkers = 0 + return descriptions diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_server.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_server.py new file mode 100644 index 000000000..5ef19009e --- /dev/null +++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_server.py @@ -0,0 +1,353 @@ +from . import natnet_data_types as DataMessages +from . import natnet_server_types as ServerMessages +from . import natnet_model_types as ModelTypes +from enum import Enum +import socket +import threading +import queue +import signal +import ctypes +import time +import typing + + +class TransmissionType(str, Enum): + UNICAST = "unicast" + MULTICAST = "multicast" + +class Client: + def __init__(self, ip: str, port: int, version: typing.Tuple[int, int, int, int] = (4, 4, 0, 0)): + self.ip = ip + self.port = port + self.version = version + self.subscribed_assets = set() + self.socket_lock = threading.Lock() + + def __hash__(self): + # Uniquely identify a client session by their IP and their unique command port. + return hash((self.ip, self.port)) + + def __eq__(self, other): + return (isinstance(other, Client) and + self.ip == other.ip and + self.port == other.port) + + +class NatNetServer: + def __init__(self, + local_interface : str = "172.31.0.200", + transmission_type: TransmissionType = TransmissionType.MULTICAST, + multicast_address : str = "239.255.42.99", + command_port: int = 1510, + data_port : int = 1511, + motive_app_version : typing.Tuple[int, int, int, int]=(3, 1, 0, 0), + natnet_version : typing.Tuple[int, int, int, int]=(4, 4, 0, 0), + high_res_clock_freq : int = 1_000_000_000, + publish_rate : int = 100 # Hz (default 100Hz) + ): + + self.local_interface = local_interface + self.transmission_type = transmission_type + self.multicast_address = multicast_address + self.command_port = command_port + self.data_port = data_port + self.motive_app_version = motive_app_version + self.natnet_version = natnet_version + self.high_res_clock_freq = high_res_clock_freq + self.publish_rate = publish_rate + + self._validate_init_params() + + self.server_description = self._build_server_description() + # Initialize synchronously safe data structures for server state and mocap data + + # Thread-safe queue for Mocp frames + self.mocap_data_queue = queue.Queue(maxsize=100) + self._last_mocap_frame: DataMessages.sFrameOfMocapData | None = None + self._last_mocap_lock = threading.Lock() + + # Thread list and shutdown event + self.threads = [] + self.shutdown_event = threading.Event() + + # Connected clients for unicast mode + self.connected_clients : typing.Set[Client] = set() + self.clients_lock : threading.Lock = threading.Lock() + + # MODELDEF wire cache (Isaac wrapper updates via set_model_def_payload) + self._model_def_lock = threading.Lock() + self._model_def_payload: bytes = ModelTypes.make_default_drone_catalog().pack() + + # Sockets + self.command_socket : socket.socket | None = None + self.data_socket : socket.socket | None = None + + self.running = False + + # When True (default), the background data loop streams frames on its own timer. + # Set False when an external driver (the Isaac wrapper's physics-step callback) + # sends frames synchronously via ``flush_mocap_data``. + self.auto_stream = True + + # start() launches two daemon threads: a command listener (handshake / MODELDEF / keepalive) + # and a data loop that streams mocap frames. The transmission-specific behavior lives in the unicast/multicast subclass. + + def _signal_handler(self, signum, frame): + print(f"\n[NatNetServer] Received interrupt signal {signum}. Initiating shutdown...") + self.shutdown() + + def enqueue_mocap_data(self, new_data: DataMessages.sFrameOfMocapData): + # Thread-safe method to push new physics frames (called by Isaac-Sim extension) + if self.mocap_data_queue.full(): + try: + # Drop oldest frame if falling behind + self.mocap_data_queue.get_nowait() + except queue.Empty: + pass + self.mocap_data_queue.put(new_data) + with self._last_mocap_lock: + self._last_mocap_frame = new_data + + def _get_last_known_mocap_frame(self) -> DataMessages.sFrameOfMocapData | None: + with self._last_mocap_lock: + return self._last_mocap_frame + + def set_model_def_payload(self, payload: bytes) -> None: + """Replace MODELDEF body served on NAT_REQUEST_MODELDEF (Isaac wrapper calls this).""" + with self._model_def_lock: + self._model_def_payload = payload + + def set_model_def_from_descriptions( + self, descriptions: ModelTypes.sDataDescriptions + ) -> None: + """Pack descriptions once and store as the MODELDEF wire cache.""" + self.set_model_def_payload(descriptions.pack()) + + def _get_model_def_payload(self) -> bytes: + """Return cached MODELDEF bytes (command thread only).""" + with self._model_def_lock: + return self._model_def_payload + + def start(self): + # Bind sockets and launch worker threads automatically on init + + # Register signal handlers for graceful shutdown (Catches Ctrl+C and kill) + try: + signal.signal(signal.SIGINT, self._signal_handler) + signal.signal(signal.SIGTERM, self._signal_handler) + except ValueError: + pass # Safe fallback if not called from the main thread + + # 1. Setup Command Socket (Receives connection/discovery requests) + self.command_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM, socket.IPPROTO_UDP) + self.command_socket.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) + self.command_socket.bind(('', self.command_port)) + + # 2. Setup Data Socket (Sends outward Mocap frames). + # Bind to the data port so frames leave with source port == data_port. + # libNatNet routes unicast NAT_FRAMEOFDATA by the server's data port + self.data_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM, socket.IPPROTO_UDP) + self.data_socket.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) + self.data_socket.bind(('', self.data_port)) + if self.transmission_type == TransmissionType.MULTICAST: + self.data_socket.setsockopt(socket.IPPROTO_IP, socket.IP_MULTICAST_IF, socket.inet_aton(self.local_interface)) + + # 3. Launch Threads + cmd_thread = threading.Thread(target=self._command_listener_loop, daemon=True) + data_thread = threading.Thread(target=self._data_update_loop, daemon=True) + + self.threads.extend([cmd_thread, data_thread]) + + for t in self.threads: + t.start() + + self.running = True + + def shutdown(self): + # Cleanly shutdown threads and close sockets + self.running = False + self.shutdown_event.set() + + if self.command_socket: + self.command_socket.close() + + if self.data_socket: + self.data_socket.close() + + for t in self.threads: + if t.is_alive(): + t.join(timeout=1.0) + + def _validate_init_params(self): + + # Validate the local_interface is a valid IP address + if not self.local_interface or not isinstance(self.local_interface, str) or self.local_interface.count('.') != 3: + raise ValueError(f"Invalid local interface IP address: {self.local_interface}") + + # Validate between transmission types and address requirements + if self.transmission_type not in TransmissionType: + raise ValueError(f"Invalid transmission type: {self.transmission_type}. Must be 'unicast' or 'multicast'.") + + if self.transmission_type == TransmissionType.MULTICAST and not self.multicast_address: + raise ValueError("Multicast address must be provided for multicast transmission type.") + + if self.transmission_type == TransmissionType.UNICAST and self.multicast_address: + raise ValueError("Multicast address should not be provided for unicast transmission type.") + + if not (0 < self.command_port < 65536): + raise ValueError(f"Invalid command port: {self.command_port}. Must be between 1 and 65535.") + + if not (0 < self.data_port < 65536): + raise ValueError(f"Invalid data port: {self.data_port}. Must be between 1 and 65535.") + + if self.command_port == self.data_port: + raise ValueError("Command port and data port must be different.") + + if self.motive_app_version and (not isinstance(self.motive_app_version, tuple) or len(self.motive_app_version) != 4): + raise ValueError(f"Invalid Motive app version: {self.motive_app_version}. Must be a tuple of 4 integers (major, minor, build, revision).") + + if self.natnet_version and (not isinstance(self.natnet_version, tuple) or len(self.natnet_version) != 4): + raise ValueError(f"Invalid NatNet version: {self.natnet_version}. Must be a tuple of 4 integers (major, minor, build, revision).") + + if self.motive_app_version and not self.motive_app_version[0] == 3: + raise ValueError(f"Unsupported Motive app version: {self.motive_app_version}. Minimum supported version is 3.0.0.0. Recommended to use 3.1.0.0") + + if not self.natnet_version[0] == 4: + raise ValueError(f"Unsupported NatNet version: {self.natnet_version}. Minimum supported version is 4.0.0.0. Recommended to use 4.4.0.0") + + if self.high_res_clock_freq <= 0: + raise ValueError( + f"Invalid high resolution clock frequency: {self.high_res_clock_freq}. Must be a positive integer representing the frequency in Hz." + ) + + if self.publish_rate <= 0: + raise ValueError( + f"Invalid publish rate: {self.publish_rate}. Must be a positive number representing Hz." + ) + def _get_latest_mocap_packet(self) -> DataMessages.sFrameOfMocapData | None: + # Thread-safe method to retrieve the latest mocap data to be sent + try: + return self.mocap_data_queue.get_nowait() + except queue.Empty: + return None + + @staticmethod + def _pad_fixed_string(value: bytes) -> bytes: + """Null-pad a byte string to MAX_NAMELENGTH for fixed-size NatNet name fields.""" + truncated = value[: ServerMessages.MAX_NAMELENGTH - 1] + return truncated + b"\x00" * (ServerMessages.MAX_NAMELENGTH - len(truncated)) + + @staticmethod + def _assign_version_bytes(field: ctypes.Array, version: typing.Tuple[int, int, int, int]) -> None: + for index, component in enumerate(version): + field[index] = component + + @staticmethod + def _assign_ipv4_bytes(field: ctypes.Array, address: str | bytes) -> None: + octets = socket.inet_aton(address) if isinstance(address, str) else address + for index, octet in enumerate(octets): + field[index] = octet + + def _build_server_description(self) -> ServerMessages.sServerDescription: + # Helper to build the server description struct with current server info (e.g. on startup or in response to command request) + description = ServerMessages.sServerDescription() + description.HostPresent = True + description.szHostComputerName = self._pad_fixed_string( + socket.gethostname().encode("utf-8") + ) + self._assign_ipv4_bytes(description.HostComputerAddress, self.local_interface) + description.szHostApp = self._pad_fixed_string(b"Motive") + self._assign_version_bytes(description.HostAppVersion, self.motive_app_version) + self._assign_version_bytes(description.NatNetVersion, self.natnet_version) + description.HighResClockFrequency = self.high_res_clock_freq + description.bConnectionInfoValid = True + description.ConnectionDataPort = self.data_port + description.ConnectionMulticast = self.transmission_type == TransmissionType.MULTICAST + + if self.transmission_type == TransmissionType.MULTICAST: + self._assign_ipv4_bytes(description.ConnectionMulticastAddress, self.multicast_address) + else: + self._assign_ipv4_bytes(description.ConnectionMulticastAddress, b"\x00\x00\x00\x00") + + return description + + def _build_connect_response_payload(self) -> bytes: + """NAT_CONNECT reply: libNatNet parses NAT_SERVERINFO payload as sSender_Server.""" + sender = ServerMessages.sSender_Server() + sender.Common.szName = self._pad_fixed_string(b"Motive") + self._assign_version_bytes(sender.Common.Version, self.motive_app_version) + self._assign_version_bytes(sender.Common.NatNetVersion, self.natnet_version) + sender.HighResClockFrequency = self.high_res_clock_freq + sender.DataPort = self.data_port + sender.IsMulticast = self.transmission_type == TransmissionType.MULTICAST + if self.transmission_type == TransmissionType.MULTICAST: + self._assign_ipv4_bytes(sender.MulticastGroupAddress, self.multicast_address) + else: + self._assign_ipv4_bytes(sender.MulticastGroupAddress, b"\x00\x00\x00\x00") + return sender.pack() + + def _send_packet_to_client( + self, + client: Client, + message_id: ServerMessages.MessageId | int, + payload: bytes, + sock: socket.socket | None = None, + ) -> None: + """Send a NatNet packet to a unicast client (libNatNet 4.4). + + Command replies go out the command socket; mocap frames go out the data socket. + """ + if self.shutdown_event.is_set(): + return + sock = sock or self.command_socket + if not sock: + raise ValueError("[NatNetServer] Socket not initialized. Cannot send packet.") + + header = ServerMessages.sPacketHeader( + iMessage=int(message_id), + nDataBytes=len(payload), + ) + packet = header.pack() + payload + try: + with client.socket_lock: + sock.sendto(packet, (client.ip, client.port)) + except OSError as e: + raise ValueError( + f"[NatNetServer] Error sending message {int(message_id)} to " + f"client {client.ip}:{client.port}: {e}" + ) from e + + def _data_update_loop(self): # Stub: Different betweeen multicast and unicast server implementations, as they will need to handle client connections differently (multicast will just send to the multicast group address) + # Loop to update mocap data and send packets at regular intervals. + pass + + def _send_data_packet(self, client: Client, data_message: DataMessages.sFrameOfMocapData): + # Serialize frame payload and send via the data socket. + # + # Stamp the transmit time in the server's high-resolution clock domain so + # the client can recover per-message transit latency via + # NatNetClient::SecondsSinceHostTimestamp(TransmitTimestamp). This must match + # the clock used in the NAT_ECHORESPONSE handshake (time.time() nanoseconds) + # and the advertised HighResClockFrequency (defaults to 1e9 ticks/s), so the + # SDK's server-clock estimate and this timestamp share one timeline. + data_message.TransmitTimestamp = int(time.time() * 1_000_000_000) + try: + packet_bytes = data_message.pack() + except Exception as e: + raise ValueError(f"[NatNetServer] Error serializing data message: {e}") from e + + self._send_packet_to_client( + client, + ServerMessages.MessageId.NAT_FRAMEOFDATA, + packet_bytes, + sock=self.data_socket, + ) + + def _command_listener_loop(self): # Stub: Different betweeen multicast and unicast server implementations, as they will need to handle client connections differently (multicast will just send to the multicast group address) + # Loop to listen for and handle incoming command requests (e.g. from client apps) + pass + + def _handle_command_request(self, request_data: bytes): # Stub: Different betweeen multicast and unicast server implementations, as they will need to handle client connections differently (multicast will just send to the multicast group address) + # Parse incoming command request, perform requested action, and send response if needed + pass + diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_server_types.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_server_types.py new file mode 100644 index 000000000..3083f1a6e --- /dev/null +++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_server_types.py @@ -0,0 +1,156 @@ +import ctypes +import struct +from enum import IntEnum + +MAX_NAMELENGTH = 256 +MAX_PACKETSIZE = 65503 + +# NatNet SDK sServerDescription uses default struct alignment (#pragma pack(pop)), not pack(1). +SERVER_DESCRIPTION_WIRE_SIZE = 552 +# NAT_CONNECT / NAT_SERVERINFO reply uses packed sSender_Server (#pragma pack(1) in NatNetTypes.h). +SENDER_SERVER_WIRE_SIZE = 256 + 4 + 4 + 8 + 2 + 1 + 4 # 279 + +# Client/server message ids +class MessageId(IntEnum): + NAT_CONNECT = 0 + NAT_SERVERINFO = 1 + NAT_REQUEST = 2 + NAT_RESPONSE = 3 + NAT_REQUEST_MODELDEF = 4 + NAT_MODELDEF = 5 + NAT_REQUEST_FRAMEOFDATA = 6 + NAT_FRAMEOFDATA = 7 + NAT_MESSAGESTRING = 8 + NAT_DISCONNECT = 9 + NAT_KEEPALIVE = 10 + NAT_DISCONNECTBYTIMEOUT = 11 + NAT_ECHOREQUEST = 12 + NAT_ECHORESPONSE = 13 + NAT_DISCOVERY = 14 + NAT_UNRECOGNIZED_REQUEST = 100 + +# Server/Sender configuration and info +def _fixed_name(field: ctypes.Array) -> bytes: + raw = bytes(field).split(b"\x00", 1)[0] + b"\x00" + if len(raw) > MAX_NAMELENGTH: + raw = raw[: MAX_NAMELENGTH - 1] + b"\x00" + return raw + b"\x00" * (MAX_NAMELENGTH - len(raw)) + + +class sSender(ctypes.Structure): + _pack_ = 1 + _fields_ = [ + ("szName", ctypes.c_char * MAX_NAMELENGTH), # host app's name + ("Version", ctypes.c_uint8 * 4), # host app's version [major.minor.build.revision] + ("NatNetVersion", ctypes.c_uint8 * 4) # host app's NatNet version + ] + + def pack(self) -> bytes: + payload = bytearray() + payload += _fixed_name(self.szName) + payload += bytes(self.Version) + payload += bytes(self.NatNetVersion) + return bytes(payload) + +class sSender_Server(ctypes.Structure): + _pack_ = 1 + _fields_ = [ + ("Common", sSender), + ("HighResClockFrequency", ctypes.c_uint64), + ("DataPort", ctypes.c_uint16), + ("IsMulticast", ctypes.c_bool), + ("MulticastGroupAddress", ctypes.c_uint8 * 4) + ] + + def pack(self) -> bytes: + payload = bytearray(self.Common.pack()) + payload += struct.pack(" bytes: + # Wire layout matches NatNet SDK on x86-64 (3 pad bytes before HighResClockFrequency). + payload = bytearray() + payload.append(1 if self.HostPresent else 0) + payload += _fixed_name(self.szHostComputerName) + payload += bytes(self.HostComputerAddress) + payload += _fixed_name(self.szHostApp) + payload += bytes(self.HostAppVersion) + payload += bytes(self.NatNetVersion) + while len(payload) % 8: + payload.append(0) + payload += struct.pack(" bytes: + return bytes(self) + +# Connection types enum matching NatNet SDK rules +class ConnectionType(IntEnum): + ConnectionType_Multicast = 0 + ConnectionType_Unicast = 1 + +class sNatNetClientConnectParams(ctypes.Structure): + """ + Python ctypes translation of the C++ sNatNetClientConnectParams struct. + Enforces a packed structure byte alignment matching the NatNet binary network protocol. + """ + _pack_ = 1 + _fields_ = [ + ("connectionType", ctypes.c_int32), # 4 bytes (mapping to standard ConnectionType enum) + ("serverCommandPort", ctypes.c_uint16), # 2 bytes + ("serverDataPort", ctypes.c_uint16), # 2 bytes + + # NOTE: Represented as void pointers (c_void_p) to safely match the host system's native bit size (e.g., 8 bytes on 64-bit) without string data unpacking overhead. + ("serverAddress", ctypes.c_void_p), + ("localAddress", ctypes.c_void_p), + ("multicastAddress", ctypes.c_void_p), + + ("subscribedDataOnly", ctypes.c_bool), # 1 byte + ("BitstreamVersion", ctypes.c_uint8 * 4) # 4 bytes: [Major, Minor, Build, Revision] + ] + + def pack(self) -> bytes: + return bytes(self) \ No newline at end of file diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_unicast_server.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_unicast_server.py new file mode 100644 index 000000000..d9c0a1e22 --- /dev/null +++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_unicast_server.py @@ -0,0 +1,172 @@ +import ctypes +import time + +from . import natnet_server_types as ServerTypes +from .natnet_server import TransmissionType, Client, NatNetServer + + +class NatNetUnicastServer(NatNetServer): + def __init__(self, + local_interface="172.31.0.200", + transmission_type: TransmissionType = TransmissionType.UNICAST, + multicast_address=None, + command_port=1510, + data_port=1511 + ): + + if not transmission_type == TransmissionType.UNICAST: + raise ValueError("Transmission type 'MULTICAST' is not supported in NatNetUnicastServer. Please use NatNetMulticastServer instead.") + + super().__init__(local_interface, transmission_type, multicast_address, command_port, data_port) + + def _data_update_loop(self): + # Loop to update mocap data and send packets at regular intervals. + # When auto_stream is False the frames are pumped externally (Isaac physics step), + # so this thread only idles — but stays alive for clean shutdown. + while not self.shutdown_event.is_set(): + time.sleep(1 / self.publish_rate) + if not self.auto_stream: + continue + self.flush_mocap_data() + + def flush_mocap_data(self): + """Send the latest (or last) mocap frame to every connected client, once.""" + with self.clients_lock: + clients = list(self.connected_clients) + if not clients: + return + + data_messages = self._get_latest_mocap_packet() + + if data_messages is None: # If the server stops producing frames, use the last known frame. + data_messages = self._get_last_known_mocap_frame() + if data_messages is None: + return + + for client in clients: + try: + self._send_data_packet(client, data_messages) + except ValueError as e: + print(str(e)) + continue + + def _command_listener_loop(self): + # Listens on UDP command socket for incoming command requests from clients. + # Handles incoming client handshakes and teardown. + + print(f"[Command Listener] Command listener thread started. Listening for incoming client command requests on UDP address:port {self.local_interface}:{self.command_port}...") + + while not self.shutdown_event.is_set(): + try: + data, addr = self.command_socket.recvfrom(1024) # Buffer size of 1024 bytes should be sufficient for command requests + if not data: + continue + self._handle_command_request(data, addr) + except Exception as e: + if self.shutdown_event.is_set(): + break + print(f"[Command Listener] Error receiving command request: {e}") + time.sleep(0.1) # Sleep briefly to avoid tight loop on errors + + def _handle_command_request(self, request_data: bytes, client_address: tuple): + """ + Processes standard binary headers and registers unicast endpoints. + """ + header_size = ctypes.sizeof(ServerTypes.sPacketHeader) + if len(request_data) < header_size: + return + + # Parse the header via ctypes + header = ServerTypes.sPacketHeader.from_buffer_copy(request_data[:header_size]) + + # Handle Connection Handshake + if header.iMessage == int(ServerTypes.MessageId.NAT_CONNECT): + client_requested_version = self.natnet_version # Fallback to server's version. Version handshaking not supported in this extension. + + client_ip, client_port = client_address + + # Create and store a new client object + new_client = Client(client_ip, client_port, version=client_requested_version) + try: + with self.clients_lock: + self.connected_clients.discard(new_client) # Remove any existing client with the same IP and port + self.connected_clients.add(new_client) # Add the new client to the connected clients list + print(f"[Command Handler] Added client {new_client.ip}:{new_client.port} to connected clients list.") + except Exception as e: + print(f"[Command Handler] Error adding client {new_client.ip}:{new_client.port} to connected clients list: {e}") + return + + try: + self._send_packet_to_client( + new_client, + ServerTypes.MessageId.NAT_SERVERINFO, + self._build_connect_response_payload(), + ) + except ValueError as e: + raise ValueError( + f"[Command Handler] Error sending server description to client {client_address}: {e}" + ) from e + print( + f"[Command Handler] Sent server description to client address " + f"through its port {client_address}." + ) + return + + # Non-handshake commands require a prior NAT_CONNECT from this endpoint. + client_ip, client_port = client_address + client = self._find_client(client_ip, client_port) + if client is None: + print( + f"[Command Handler] Ignoring message {header.iMessage} from " + f"unregistered client {client_address}." + ) + return + + if header.iMessage == int(ServerTypes.MessageId.NAT_REQUEST_MODELDEF): + try: + self._send_packet_to_client( + client, + ServerTypes.MessageId.NAT_MODELDEF, + self._get_model_def_payload(), + ) + except ValueError as e: + print( + f"[Command Handler] Error sending MODELDEF to client " + f"{client_address}: {e}" + ) + return + + if header.iMessage == int(ServerTypes.MessageId.NAT_KEEPALIVE): + # Receiving a keepalive refreshes the client's liveness; nothing to send back. + return + + if header.iMessage == int(ServerTypes.MessageId.NAT_ECHOREQUEST): + echo_payload = request_data[header_size : header_size + header.nDataBytes] + # libNatNet expects clientRequestTimestamp + hostReceivedTimestamp (8 + 8 bytes). + host_ts = int(time.time() * 1_000_000_000).to_bytes(8, "little", signed=False) + response_payload = echo_payload[:8].ljust(8, b"\x00") + host_ts + try: + self._send_packet_to_client( + client, + ServerTypes.MessageId.NAT_ECHORESPONSE, + response_payload, + ) + except ValueError as e: + print( + f"[Command Handler] Error sending ECHORESPONSE to client " + f"{client_address}: {e}" + ) + return + + print( + f"[Command Handler] Unhandled message id {header.iMessage} from " + f"registered client {client_address}." + ) + + def _find_client(self, ip: str, port: int) -> Client | None: + target = Client(ip, port) + with self.clients_lock: + for client in self.connected_clients: + if client == target: + return client + return None diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/setup.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/setup.py new file mode 100644 index 000000000..1a1c153fc --- /dev/null +++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/setup.py @@ -0,0 +1,23 @@ +"""Isaac Sim extension install metadata for the OptiTrack NatNet emulator.""" + +import os + +from setuptools import find_packages, setup + +EXTENSION_PATH = os.path.dirname(os.path.realpath(__file__)) + +setup( + name="optitrack-natnet-emulator", + version="0.1.0", + description="NatNet UDP server emulator for Isaac Sim and natnet_ros2 integration", + license="MIT", + include_package_data=True, + python_requires=">=3.10", + install_requires=[ + "numpy", + "scipy", + ], + packages=find_packages(where="."), + package_dir={"": "."}, + zip_safe=False, +) diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/natnet_test_helpers.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/natnet_test_helpers.py new file mode 100644 index 000000000..07d6c8a5e --- /dev/null +++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/natnet_test_helpers.py @@ -0,0 +1,105 @@ +"""Shared helpers for optitrack.natnet.emulator unit tests.""" + +from __future__ import annotations + +import socket +import struct +import time +from contextlib import contextmanager + +from optitrack.natnet.emulator import NatNetUnicastServer, TransmissionType +from optitrack.natnet.emulator.server import natnet_server_types as st + + +def ephemeral_udp_port(host: str = "127.0.0.1") -> int: + """Return a free UDP port on *host* by binding and releasing a probe socket.""" + with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as probe: + probe.bind((host, 0)) + return probe.getsockname()[1] + + +class NatNetTestClient: + """Minimal UDP client for NatNet command-port protocol tests.""" + + def __init__(self, host: str = "127.0.0.1", timeout: float = 2.0) -> None: + self._host = host + self._sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + self._sock.bind((host, 0)) + self._sock.settimeout(timeout) + + @property + def local_port(self) -> int: + return self._sock.getsockname()[1] + + def send_message( + self, + server_port: int, + message_id: st.MessageId | int, + payload: bytes = b"", + server_host: str | None = None, + ) -> None: + header = st.sPacketHeader( + iMessage=int(message_id), + nDataBytes=len(payload), + ) + self.send_raw(header.pack() + payload, server_port, server_host) + + def send_raw( + self, + data: bytes, + server_port: int, + server_host: str | None = None, + ) -> None: + """Send a raw UDP datagram (for malformed / malicious packet tests).""" + self._sock.sendto(data, (server_host or self._host, server_port)) + + def send_header_only( + self, + server_port: int, + message_id: st.MessageId | int, + declared_payload_len: int, + server_host: str | None = None, + ) -> None: + """Send a header whose nDataBytes does not match any trailing payload.""" + header = struct.pack(" tuple[int, bytes, tuple[str, int]]: + data, addr = self._sock.recvfrom(65535) + message_id, payload_len = struct.unpack(" None: + self._sock.close() + + +@contextmanager +def running_unicast_server( + command_port: int | None = None, + local_interface: str = "127.0.0.1", + publish_rate: int = 100, +): + """Start NatNetUnicastServer on ephemeral (or fixed) command + data ports. + + Both ports are ephemeral by default so concurrent/sequential tests never + collide on the well-known 1510/1511 pair. + """ + port = command_port if command_port is not None else ephemeral_udp_port(local_interface) + data_port = ephemeral_udp_port(local_interface) + while data_port == port: + data_port = ephemeral_udp_port(local_interface) + server = NatNetUnicastServer( + local_interface=local_interface, + transmission_type=TransmissionType.UNICAST, + multicast_address=None, + command_port=port, + data_port=data_port, + ) + server.publish_rate = publish_rate + server.start() + time.sleep(0.05) + try: + yield server, port + finally: + server.shutdown() diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_defaults.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_defaults.py new file mode 100644 index 000000000..7cc72b036 --- /dev/null +++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_defaults.py @@ -0,0 +1,26 @@ +# Copyright (c) 2024 Carnegie Mellon University +# MIT License - see LICENSE in the repository root for full text. +"""Unit tests for hardcoded tracked-body defaults.""" + +from __future__ import annotations + +import pytest + +from optitrack.natnet.emulator.defaults import ( + DEFAULT_DRONE_BINDING, + DEFAULT_TRACKED_BODY_BINDINGS, +) + + +pytestmark = pytest.mark.unit + + +def test_default_drone_binding_matches_natnet_ros2_config(): + assert DEFAULT_DRONE_BINDING.name == "Drone" + assert DEFAULT_DRONE_BINDING.id == 1 + assert DEFAULT_DRONE_BINDING.parent_id == -1 + assert DEFAULT_DRONE_BINDING.prim_path == "/World/base_link" + + +def test_default_tracked_body_bindings_contains_drone_only(): + assert DEFAULT_TRACKED_BODY_BINDINGS == (DEFAULT_DRONE_BINDING,) diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_serializers.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_serializers.py new file mode 100644 index 000000000..0db5a34df --- /dev/null +++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_serializers.py @@ -0,0 +1,341 @@ +# Copyright (c) 2024 Carnegie Mellon University +# MIT License - see LICENSE in the repository root for full text. +"""Unit tests for NatNet wire serializers (no network).""" + +from __future__ import annotations + +import ctypes +import struct + +import pytest + +from optitrack.natnet.emulator import NatNetUnicastServer, TransmissionType +from optitrack.natnet.emulator.server import natnet_data_types as dt +from optitrack.natnet.emulator.server import natnet_model_types as mt +from optitrack.natnet.emulator.server import natnet_server_types as st +from optitrack.natnet.emulator.server.natnet_common import ModelLimits + + +pytestmark = pytest.mark.unit + + +# ============================================================================= +# natnet_server_types — transport / handshake +# ============================================================================= + + +def test_packet_header_pack_size_and_endianness(): + header = st.sPacketHeader( + iMessage=int(st.MessageId.NAT_FRAMEOFDATA), + nDataBytes=42, + ) + packed = header.pack() + + assert len(packed) == ctypes.sizeof(st.sPacketHeader) == 4 + message_id, payload_len = struct.unpack(" server only; real Motive sends no reply. An echo + # reply makes libNatNet log "Received unrecognized message Message=10". + with running_unicast_server() as (server, command_port): + client = NatNetTestClient(timeout=0.5) + try: + client.send_message(command_port, st.MessageId.NAT_CONNECT) + client.recv_message() + + client.send_message(command_port, st.MessageId.NAT_KEEPALIVE) + with pytest.raises(socket.timeout): + client.recv_message() + + # Client stays registered and keeps receiving frames. + assert len(server.connected_clients) == 1 + finally: + client.close() + + +# ============================================================================= +# Malformed datagrams — registered client & recovery +# ============================================================================= + + +def test_unknown_message_from_registered_client_gets_no_reply(): + with running_unicast_server() as (server, command_port): + client = NatNetTestClient(timeout=0.5) + try: + client.send_message(command_port, st.MessageId.NAT_CONNECT) + client.recv_message() + + client.send_message(command_port, 999) + with pytest.raises(socket.timeout): + client.recv_message() + + assert len(server.connected_clients) == 1 + finally: + client.close() + + +def test_server_survives_malformed_burst_then_valid_connect(): + with running_unicast_server() as (server, command_port): + client = NatNetTestClient(timeout=2.0) + try: + client.send_raw(b"", command_port) + client.send_raw(b"\xff", command_port) + client.send_header_only(command_port, 999, declared_payload_len=50000) + client.send_message(command_port, st.MessageId.NAT_REQUEST_MODELDEF) + + client.send_message(command_port, st.MessageId.NAT_CONNECT) + message_id, _payload, _addr = client.recv_message() + finally: + client.close() + + assert message_id == int(st.MessageId.NAT_SERVERINFO) + assert len(server.connected_clients) == 1 diff --git a/tests/colcon_unit_test_packages.yaml b/tests/colcon_unit_test_packages.yaml index 65cdd38cc..e7a204e40 100644 --- a/tests/colcon_unit_test_packages.yaml +++ b/tests/colcon_unit_test_packages.yaml @@ -13,3 +13,9 @@ robot: - lidar_point_cloud_filter # Skips ament_copyright / flake8 / pep257 on Python packages; run linters separately. pytest_args: "-m not linter" + +# Simulation-side extensions (globbed under simulation/**//test). Collected by +# `pytest tests/` on the host runner; not part of the robot colcon workspace. +sim: + packages: + - optitrack.natnet.emulator diff --git a/tests/conftest.py b/tests/conftest.py index 62644eb98..e3078d146 100644 --- a/tests/conftest.py +++ b/tests/conftest.py @@ -73,6 +73,16 @@ def pytest_configure(config): run_dir = session.init_run_dir(AIRSTACK_ROOT) config.option.xmlpath = str(run_dir / "results.xml") + # Co-located unit tests import their own package (e.g. `optitrack.natnet.emulator`, + # `lidar_point_cloud_filter.validation_core`). Put each package/extension import + # root (the parent of its test/ dir) on sys.path so they resolve without a + # per-package conftest.py — a second conftest.py collides with this root one as + # module `conftest` under --import-mode=importlib and breaks `from conftest import`. + for d in unit_test_dirs(): + root = str(d.parent) + if root not in sys.path: + sys.path.insert(0, root) + # Collect co-located unit tests: their files live outside tests/, so add the # explicit non-linter test files to the collection args. Skip when an explicit # path was given on the CLI (args_source == ARGS) so `pytest tests/system/foo.py` diff --git a/tests/integration/natnet/README.md b/tests/integration/natnet/README.md new file mode 100644 index 000000000..4e9330c32 --- /dev/null +++ b/tests/integration/natnet/README.md @@ -0,0 +1,151 @@ +# NatNet ↔ robot autonomy integration + +Host-side NatNet wire-protocol tests that drive the Python emulator against +`natnet_ros2_node` in a real robot container. First resident of the +[`integration`](../README.md) tier (no sim, no GPU). + +Mark: `integration`. Filter this scenario with `tests/integration/natnet/`. + +For the **in-sim** end-to-end check (Isaac emulator + full stack), see +[Liveliness sentinel](#liveliness-sentinel-sim-end-to-end) below and +[`tests/system/test_liveliness.py`](../../system/test_liveliness.py). + +## What it verifies + +Three variants in [`test_natnet_integration.py`](test_natnet_integration.py). +All start a host-side `NatNetUnicastServer`, launch `natnet_ros2_node` in the +robot container pointed at the Docker bridge gateway, and assert a sustained +pose stream at **≥ 5 Hz** on the configured topic(s), e.g.: + +- `/{ROBOT_NAME}/perception/optitrack/drone/pose_cov` (wait for first message) +- `/{ROBOT_NAME}/perception/optitrack/drone` (Hz sample) + +| Test | Path | +|------|------| +| **`test_natnet_ros2_receives_drone_pose_hz`** | Hand-built `sFrameOfMocapData` frames enqueued on a raw `NatNetUnicastServer` (no USD). Minimal wire + SDK check. | +| **`test_natnet_ros2_receives_isaac_wrapper_pose_hz`** | Full Isaac data path: in-memory USD stage, `NatNetInterfaceConfig`, `author_interface`, `NatNetServerManager.sample_once()` on a moving prim — same sampling logic as the in-sim physics-step callback. Skips without `usd-core` (`pxr`). Pose-value fidelity is covered hermetically by the emulator's `test_pose_streaming.py` loopback. | +| **`test_natnet_ros2_multi_body_drone_and_target`** | Two bodies (drone id 1 + target id 100) with distinct relative topics; asserts both pose streams and that the target's `pose_cov` topic is **absent** (`body_pose_cov=false`). Exercises the multi-body profile + per-body `pose`/`pose_cov` toggles. | + +These tests **do not** start the full perception bringup or `LAUNCH_NATNET`; they +exec `natnet_ros2_node` directly with the flattened per-body params +(`body_names`/`body_ids`/`body_topics`/`body_pose`/`body_pose_cov`) and no MAVROS bridge. + +## Requirements + +- Docker daemon (robot-desktop container reachable from pytest). +- **`natnet_ros2_node` built** in the robot image (OptiTrack NatNet SDK is + license-gated — run `airstack setup --natnet`, then + `bws --packages-select natnet_ros2` in the container). Tests **skip** if the + node binary is missing. +- Host-side emulator package on `PYTHONPATH` (the test adds + `simulation/isaac-sim/extensions/optitrack.natnet.emulator` — not pip-installed + on the host). +- Ephemeral UDP ports on the host gateway IP (Docker default route as seen from + inside the container). + +The robot container comes from the shared **`robot_autonomy_stack`** fixture in +[`tests/conftest.py`](../../conftest.py) (see the [integration tier README](../README.md)). + +## Running + +```bash +# 1. One-time: NatNet SDK + build natnet_ros2 in the robot image +airstack setup --natnet # or NATNET_ACCEPT_LICENSE=1 airstack setup --natnet +docker exec airstack-robot-desktop-1 bash -lc 'bws --packages-select natnet_ros2' + +# 2a. Reuse an existing robot container (fast local iteration): +AUTOLAUNCH=false airstack up robot-desktop +pytest tests/integration/natnet/ -m integration -v + +# 2b. Let the harness bring the container up/down: +pytest tests/integration/natnet/ -m integration -v +``` +On CI / PR (write access): `/pytest -m integration` + +## Architecture + +``` +┌──────────────────────────────────────────────────────────────┐ +│ Host (pytest) │ +│ NatNetUnicastServer @ docker bridge gateway IP │ +│ • raw variant: hand-built frame queue │ +│ • Isaac variant: NatNetServerManager.sample_once(USD) │ +└────────────────────────────┬─────────────────────────────────┘ + │ UDP unicast (cmd + data ports) +┌────────────────────────────▼─────────────────────────────────┐ +│ Robot container (robot-desktop) │ +│ natnet_ros2_node (libNatNet 4.4 client) │ +│ → /{ROBOT_NAME}/{body topic}[/pose_cov] per configured body │ +└──────────────────────────────────────────────────────────────┘ +``` + +**In sim (liveliness tier):** the server runs inside the Isaac Sim container +(`172.31.0.200` by default). Use a NatNet Pegasus launch script +(`example_one_px4_pegasus_natnet_launch_script.py` or +`example_multi_px4_pegasus_natnet_launch_script.py`); `natnet_ros2` in the +robot stack connects via `natnet_config.yaml` (`server_ip` → emulator IP). + +**Catalog / MODELDEF:** The server holds a MODELDEF **wire cache** only +(`set_model_def_payload()`). Scene semantics (body names, streaming IDs, target +prim paths) come from the Isaac layer (`NatNetInterfaceConfig`, USD interface +prim, or launch-script `build_drone_config`). See the +[emulator README](../../../simulation/isaac-sim/extensions/optitrack.natnet.emulator/README.md). + +## Liveliness sentinel (sim end-to-end) + +The integration tier proves **robot client + host emulator** without Isaac. +The matching **system** check is +`TestLiveliness::test_natnet_pose_alive` in +[`test_liveliness.py`](../../system/test_liveliness.py): + +- **Gated on `LAUNCH_NATNET=true`** (skipped otherwise — normal liveliness runs + are unaffected). +- Asserts `/{robot_n}/{natnet pose topic}/pose_cov` ≥ 5 Hz per robot (the drone + body's configured topic — default `perception/optitrack/drone`). +- Override the checked topic with `NATNET_POSE_TOPIC` (default + `perception/optitrack/drone`). The sim body name (`NATNET_BODY_NAME`, default + `Drone`) is decoupled from the published topic, which the robot profile sets. + +Sim auto-start: set `ISAAC_SIM_SCRIPT_NAME` to a NatNet launch script and +`LAUNCH_NATNET=true` on the robot. Convenience bundle: +`airstack up --env-file overrides/isaac-natnet-vision.env` (NatNet script + +PX4 external-vision SITL profile). + +## libNatNet 4.4 unicast — verified wire contract + +The emulator is validated against the **real `libNatNet.so`** (not just the Python +`NatNetClient`) with a minimal C probe that registers `SetFrameReceivedCallback` +and `NatNet_SetLogCallback`. All of the following must hold for the SDK to deliver +frames to the callback: + +| Requirement | Why | +|-------------|-----| +| `NAT_CONNECT` → `sSender_Server` (279 B), name `Motive` | libNatNet reads `Motive 3.1 / NatNet 4.4` | +| `NAT_ECHOREQUEST` → `NAT_ECHORESPONSE` (16 B) | Prevents libNatNet assert | +| Frame ends with a **4-byte end-of-data tag** after `params` | libNatNet's frame unpacker reads it; without it the unpacked size mismatches `nDataBytes` and **every frame is silently dropped** | +| `NAT_FRAMEOFDATA` sent from the **data port** (source port == `data_port`) | libNatNet routes unicast frames by the server's data port. Frames sent from the **command** port are treated as command traffic and dropped — no error, no callback | +| `NAT_KEEPALIVE` gets **no reply** | An echo reply makes libNatNet log `Received unrecognized message Message=10` | + +With these in place the C probe reports `Server: Motive 3.1.0.0 NatNet 4.4.0.0`, +`data descriptions: 1`, and **~74 Hz** of frame callbacks. + +> The lenient Python `NatNetClient` accepts frames *without* the end-of-data tag +> and *on the command port*, which is why it appeared to work while libNatNet did +> not. Always validate against the C SDK. + +Full handshake notes and sniffing workflow: +[optitrack-development skill](../../../.agents/skills/optitrack-development/SKILL.md). + +## After changing natnet_ros2 or the emulator + +Rebuild in the robot container: + +```bash +docker exec airstack-robot-desktop-1 bash -lc 'bws --packages-select natnet_ros2' +``` + +Unit tests (protocol, serializers, Isaac wrapper loopback): + +```bash +pytest tests/sim/optitrack_natnet_emulator/ -m unit -v +``` diff --git a/tests/integration/natnet/test_natnet_integration.py b/tests/integration/natnet/test_natnet_integration.py new file mode 100644 index 000000000..e386fd1ee --- /dev/null +++ b/tests/integration/natnet/test_natnet_integration.py @@ -0,0 +1,302 @@ +# Copyright (c) 2024 Carnegie Mellon University +# MIT License - see LICENSE in the repository root for full text. +"""NatNet - robot autonomy integration tests. + +Host-side variants stream frames to ``natnet_ros2_node`` in the robot container and +assert pose topics stay alive at >= 5 Hz: (1) raw ``NatNetUnicastServer`` hand-built +single-body frames; (2) ``NatNetServerManager`` sampling an in-memory USD stage +(Isaac wrapper path, no sim/GPU); (3) a multi-body profile (drone + target) that +exercises per-body topic overrides and the pose / pose_cov toggles. + +The node is parameterised with the flattened per-body arrays +(``body_names`` / ``body_ids`` / ``body_topics`` / ``body_pose`` / ``body_pose_cov``) +that natnet_ros2.launch.py derives from a robot's natnet_config.yaml profile. + +Multi-robot (NUM_ROBOTS=3, per-robot profiles) is exercised in-sim by +``tests/system/test_liveliness.py::test_natnet_pose_alive``. +""" + +from __future__ import annotations + +import subprocess +import sys +import threading +import time + +import pytest + +from conftest import ( # noqa: E402 — pytest adds tests/ to sys.path + docker_exec, + repo_path, + ros2_env, + sample_hz, + wait_for_first_message, +) + +# Emulator is not pip-installed on the host; add extension root + test helpers. +_EXT_ROOT = repo_path("simulation/isaac-sim/extensions/optitrack.natnet.emulator") +for _path in (_EXT_ROOT, _EXT_ROOT / "test"): + if str(_path) not in sys.path: + sys.path.insert(0, str(_path)) + +from optitrack.natnet.emulator import NatNetUnicastServer, TransmissionType # noqa: E402 +from optitrack.natnet.emulator.server import natnet_data_types as dt # noqa: E402 +from natnet_test_helpers import ephemeral_udp_port # noqa: E402 + +pytestmark = pytest.mark.integration + +_ROBOT_SETUP = "/root/AirStack/robot/ros_ws/install/setup.bash" +_NATNET_NODE = "/root/AirStack/robot/ros_ws/install/natnet_ros2/lib/natnet_ros2/natnet_ros2_node" +_WARMUP_S = 2.0 +_STREAM_HOLD_S = 12.0 +_MIN_HZ = 5.0 + +# Robot image has route/netstat but not `ip`; /proc/net/route is always present. +_DEFAULT_GATEWAY_CMD = ( + """awk '$2 == "00000000" { printf "%d.%d.%d.%d\\n", """ + """"0x" substr($3,7,2), "0x" substr($3,5,2), "0x" substr($3,3,2), "0x" substr($3,1,2); exit }' """ + """/proc/net/route""" +) + + +def _docker_default_gateway(container: str) -> str: + result = docker_exec(container, _DEFAULT_GATEWAY_CMD, timeout=10) + gateway = result.stdout.strip() + if not gateway: + pytest.skip(f"Could not resolve default gateway inside {container}") + return gateway + + +def _container_env(container: str, var: str, default: str) -> str: + # ROBOT_NAME / ROS_DOMAIN_ID are set in .bashrc (login shell), not container ENV. + # .bashrc may print "Sourcing ..." to stdout; take the last line as the value. + result = docker_exec(container, f"bash -lc 'echo ${var}'") + lines = [line.strip() for line in result.stdout.splitlines() if line.strip()] + value = lines[-1] if lines else "" + return value if value else default + + +def _natnet_node_available(container: str) -> bool: + result = docker_exec(container, f"test -x {_NATNET_NODE} && echo yes || echo no") + return "yes" in result.stdout + + +def _stop_stale_natnet_nodes(container: str) -> None: + docker_exec(container, "pkill -f natnet_ros2_node || true") + time.sleep(0.5) + + +# Each body: (streaming_id, rigid_body_name). The raw server frame carries ids only; +# the node maps ids → topics via its body_* params. +_DRONE_BODY = (1, "Drone") +_TARGET_BODY = (100, "Target") + + +def _make_frame(frame_num: int, body_ids) -> dt.sFrameOfMocapData: + frame = dt.sFrameOfMocapData() + frame.iFrame = frame_num + frame.nRigidBodies = len(body_ids) + for slot, body_id in enumerate(body_ids): + rb = frame.RigidBodies[slot] + rb.ID = body_id + rb.qw = 1.0 + # Bit 0 = tracking valid; natnet_ros2 skips bodies without it (natnet_logic.hpp). + rb.params = 1 + return frame + + +def _frame_publisher( + server: NatNetUnicastServer, stop_event: threading.Event, body_ids=(1,) +) -> None: + frame_num = 0 + interval = 1.0 / server.publish_rate + while not stop_event.is_set(): + server.enqueue_mocap_data(_make_frame(frame_num, body_ids)) + frame_num += 1 + time.sleep(interval) + + +def _launch_natnet_node(container, host_ip, command_port, domain_id, bodies=None): + """Start natnet_ros2_node in the container pointed at the host emulator. + + ``bodies`` is a list of (id, name, topic, pose, pose_cov); defaults to a single + Drone body on topic ``perception/optitrack/drone`` (the shipped config default). + """ + if bodies is None: + bodies = [(1, "Drone", "perception/optitrack/drone", "true", "true")] + ids = ",".join(str(b[0]) for b in bodies) + names = ",".join(b[1] for b in bodies) + topics = ",".join(b[2] for b in bodies) + pose = ",".join(b[3] for b in bodies) + pose_cov = ",".join(b[4] for b in bodies) + launch_cmd = ( + f"bash -lc '{ros2_env(_ROBOT_SETUP, domain_id)} && " + f"exec {_NATNET_NODE} --ros-args " + f"-p server_ip:={host_ip} " + f"-p command_port:={command_port} " + f"-p body_names:=[{names}] " + f"-p body_ids:=[{ids}] " + f"-p body_topics:=[{topics}] " + f"-p body_pose:=[{pose}] " + f"-p body_pose_cov:=[{pose_cov}]'" + ) + return subprocess.Popen( + ["docker", "exec", container, "bash", "-c", launch_cmd], + stdout=subprocess.PIPE, + stderr=subprocess.STDOUT, + text=True, + ) + + +def _assert_pose_stream( + container, robot_name, domain_id, topic="perception/optitrack/drone", pose_cov=True +): + """Wait for the pose topic then assert a sustained rate >= _MIN_HZ. + + A body configured with ``body_pose_cov=false`` never publishes the ``/pose_cov`` + variant, so detect the first message on whichever topic the body actually emits. + """ + pose_topic = f"/{robot_name}/{topic}" + detect_topic = f"{pose_topic}/pose_cov" if pose_cov else pose_topic + + time.sleep(_WARMUP_S) + first_msg_s = wait_for_first_message( + container, detect_topic, domain_id, _ROBOT_SETUP, timeout=int(_STREAM_HOLD_S) + ) + assert first_msg_s is not None, ( + f"No messages on {detect_topic} within {_STREAM_HOLD_S}s " + "(NatNet connect or frame stream failed)" + ) + hz = sample_hz( + container, + pose_topic, + domain_id, + _ROBOT_SETUP, + duration=min(8, int(_STREAM_HOLD_S - first_msg_s)), + window=20, + ) + assert hz is not None, f"No sustained stream on {pose_topic}" + assert hz >= _MIN_HZ, f"Expected >= {_MIN_HZ} Hz on {pose_topic}, got {hz}" + + +def _terminate(proc) -> None: + if proc is None: + return + proc.terminate() + try: + proc.wait(timeout=5) + except subprocess.TimeoutExpired: + proc.kill() + + +def test_natnet_ros2_receives_drone_pose_hz(robot_autonomy_stack): + """Raw-server path: hand-built frames on NatNetUnicastServer.""" + container = robot_autonomy_stack["container"] + + if not _natnet_node_available(container): + pytest.skip( + "natnet_ros2_node not built — run airstack setup (NatNet SDK) and " + "bws --packages-select natnet_ros2 in the robot container" + ) + + _stop_stale_natnet_nodes(container) + + host_ip = _docker_default_gateway(container) + command_port = ephemeral_udp_port(host_ip) + robot_name = _container_env(container, "ROBOT_NAME", "robot_1") + domain_id = int(_container_env(container, "ROS_DOMAIN_ID", "0")) + + server = NatNetUnicastServer( + local_interface=host_ip, + transmission_type=TransmissionType.UNICAST, + multicast_address=None, + command_port=command_port, + ) + server.publish_rate = 50 + + stop_event = threading.Event() + publisher = threading.Thread( + target=_frame_publisher, args=(server, stop_event), daemon=True + ) + + node_proc: subprocess.Popen[str] | None = None + try: + # Seed dummy frames before the client connects; keep streaming the whole window. + publisher.start() + time.sleep(0.1) + server.start() + node_proc = _launch_natnet_node(container, host_ip, command_port, domain_id) + _assert_pose_stream(container, robot_name, domain_id) + finally: + stop_event.set() + publisher.join(timeout=2.0) + _terminate(node_proc) + server.shutdown() + + +def test_natnet_ros2_multi_body_drone_and_target(robot_autonomy_stack): + """Multi-body profile: one robot tracks a drone + a static target. + + Streams two bodies (drone id 1, target id 100) and configures the node like a + robot profile with two bodies and distinct relative topics. Asserts: the drone + pose streams >= 5 Hz on its custom topic; the target pose streams on its own + topic; and the target's pose_cov topic is absent (body_pose_cov=false). + """ + container = robot_autonomy_stack["container"] + + if not _natnet_node_available(container): + pytest.skip("natnet_ros2_node not built — run airstack setup (NatNet SDK)") + + _stop_stale_natnet_nodes(container) + + host_ip = _docker_default_gateway(container) + command_port = ephemeral_udp_port(host_ip) + robot_name = _container_env(container, "ROBOT_NAME", "robot_1") + domain_id = int(_container_env(container, "ROS_DOMAIN_ID", "0")) + + server = NatNetUnicastServer( + local_interface=host_ip, + transmission_type=TransmissionType.UNICAST, + multicast_address=None, + command_port=command_port, + ) + server.publish_rate = 50 + + bodies = [ + (_DRONE_BODY[0], _DRONE_BODY[1], "perception/optitrack/drone", "true", "true"), + (_TARGET_BODY[0], _TARGET_BODY[1], "perception/optitrack/target", "true", "false"), + ] + body_ids = (_DRONE_BODY[0], _TARGET_BODY[0]) + + stop_event = threading.Event() + publisher = threading.Thread( + target=_frame_publisher, args=(server, stop_event, body_ids), daemon=True + ) + + node_proc: subprocess.Popen[str] | None = None + try: + publisher.start() + time.sleep(0.1) + server.start() + node_proc = _launch_natnet_node(container, host_ip, command_port, domain_id, bodies) + # Drone (pose + pose_cov) and target (pose only) both stream. + _assert_pose_stream(container, robot_name, domain_id, "perception/optitrack/drone") + _assert_pose_stream( + container, robot_name, domain_id, "perception/optitrack/target", pose_cov=False + ) + + # body_pose_cov=false → the target pose_cov publisher must not exist. + target_cov = f"/{robot_name}/perception/optitrack/target/pose_cov" + topics = docker_exec( + container, + f"bash -lc '{ros2_env(_ROBOT_SETUP, domain_id)} && ros2 topic list'", + timeout=15, + ).stdout + assert target_cov not in topics.split(), ( + f"{target_cov} should not exist when body_pose_cov=false; topics:\n{topics}" + ) + finally: + stop_event.set() + publisher.join(timeout=2.0) + _terminate(node_proc) + server.shutdown()