diff --git a/.env b/.env index 00cd4c639..c9527e1f8 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.8" +VERSION="0.19.0-alpha.9" # Choose "dev" or "prebuilt". "dev" is for mounted code that must be built live. "prebuilt" is for built ros_ws baked into the image DOCKER_IMAGE_BUILD_MODE="dev" # Where to push and pull images from. Can replace with your docker hub username if using docker hub. diff --git a/CHANGELOG.md b/CHANGELOG.md index a5ea32f38..34554edd1 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -14,6 +14,7 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0 - `ros-${ROS_DISTRO}-mavros-extras` in the robot image (provides the vision_pose plugin used for external-pose deployments) - `overrides/l4t-px4-realrobot.env` — site-agnostic deployment override for a single real PX4 robot on a Jetson (aarch64/l4t) - `integration` test tier (`tests/integration/`, `integration` mark) with a shared `robot_autonomy_stack` fixture (robot container, no sim/GPU) +- Real-robot PX4 external-vision fusion in `natnet_ros2` (OptiTrack mocap → EKF2): `mavros_gp_origin` (geoid-corrected synthetic GPS origin so `local_position.z` == OptiTrack z, fixing the ~36 m boot offset), `vision_pose_converter`, and a PX4 param **checker** (`px4_param_setter`, `auto_set` off by default; `on_mismatch` warn/halt) — setup guide at `docs/robot/px4_external_vision.md` ### Changed diff --git a/docs/robot/px4_external_vision.md b/docs/robot/px4_external_vision.md new file mode 100644 index 000000000..15e98606d --- /dev/null +++ b/docs/robot/px4_external_vision.md @@ -0,0 +1,246 @@ +# PX4 External-Vision (OptiTrack) Setup + +Runbook for flying a PX4 vehicle (Cube Orange) on **OptiTrack mocap as the sole +position source** — no GNSS, no magnetometer — with an onboard companion +computer (Jetson) running the AirStack robot stack. + +It covers three things that must all be right: + +1. **EKF2 parameters** — tell PX4 to fuse external vision instead of GPS/baro/mag. +2. **Companion MAVLink link** — how the Jetson talks to the Cube (USB vs TELEM2 UART). +3. **Vision pose pipeline** — how a mocap pose becomes a `VISION_POSITION_ESTIMATE`. + +> Scope: PX4 ≥ 1.14 (the `EKF2_EV_CTRL` / `EKF2_GPS_CTRL` era). For older +> firmware use `EKF2_AID_MASK: 24` and `EKF2_HGT_MODE: 3` instead of the bitmask +> params below. + +--- + +## 1. EKF2 parameters (external vision) + +These are enforced automatically at startup by the `px4_param_setter` node (see +below), sourced from +[`robot/ros_ws/src/perception/natnet_ros2/config/px4_params.yaml`](robot/ros_ws/src/perception/natnet_ros2/config/px4_params.yaml). +You can also set them by hand in QGroundControl — PX4 persists parameters, so +either way it's a one-time thing per airframe. + +| Parameter | Value | Meaning | +|---|---|---| +| `EKF2_EV_CTRL` | `11` | Fuse vision **horizontal pos (1) + vertical pos (2) + yaw (8)**. Add bit **4** (velocity) only if a vision *speed* source is also streamed. | +| `EKF2_HGT_REF` | `3` | Vision is the primary height reference (not baro / GPS). | +| `EKF2_GPS_CTRL` | `0` | No GPS fusion. | +| `EKF2_MAG_TYPE` | `5` | Magnetometer disabled — yaw comes from vision. | +| `EKF2_BARO_CTRL` | `0` | No baro fusion; height is pure vision. Set to `1` to keep baro as a backup height source. | +| `EKF2_EV_DELAY` | `15.0` | Measured OptiTrack→EKF2 latency (ms): ~1 ms LAN + ~5 ms Cube hop, rounded up. `natnet_ros2_node` logs the measured figure; retune from Flight Review EV innovations. | +| `EKF2_EV_NOISE_MD` | `1` | Use the `EKF2_EV*_NOISE` floors below instead of the message covariance (which is `1e-6` — too optimistic to fuse safely). | +| `EKF2_EVP_NOISE` | `0.01` | Vision **position** noise floor (m). | +| `EKF2_EVA_NOISE` | `0.05` | Vision **angle** noise floor (rad). | +| `COM_ARM_WO_GPS` | `1` | Allow arming without GPS. | + +**Type matters.** Integers are written bare (`11`); floats need a decimal point +(`15.0`) so the MAVLink param type matches the FCU's declaration. Getting this +wrong makes the set silently reject. + +**Reboot after any change.** Fusion-source (`EKF2_*`) params are safest applied +from a clean estimator start — reboot the flight controller before flying. The +param setter prints a warning whenever it actually changes something. + +--- + +## 2. The param checker (`px4_param_setter`) + +Set the table above **once in QGroundControl**. To catch a mis-configured FCU +before flight, the stack runs a one-shot node at startup that **checks** the live +params against the desired set. **By default it only checks and flags — it does not +write to the FCU.** + +- **Node:** [`px4_param_setter_node.py`](robot/ros_ws/src/perception/natnet_ros2/src/px4_param_setter_node.py) +- **Config:** [`config/px4_params.yaml`](robot/ros_ws/src/perception/natnet_ros2/config/px4_params.yaml) + (everything under `params.` is a desired FCU parameter) +- **Launch:** [`launch/px4_param_setter.launch.xml`](robot/ros_ws/src/perception/natnet_ros2/launch/px4_param_setter.launch.xml), + included from `natnet_ros2.launch.py` when the robot's `vision_pose` block is enabled. + +Two safety flags in `px4_params.yaml`: + +| Flag | Default | Behaviour | +|------|---------|-----------| +| `auto_set` | `false` | `false`: read + compare only, never write. `true`: also push mismatched params via `param/set` and verify (the legacy enforce path). | +| `on_mismatch` | `warn` | With `auto_set: false`, on a wrong param — `warn`: log the diffs, keep the stack up. `halt`: log fatal + exit non-zero so a `required` launch node tears the stack down before flight. | + +Per parameter it waits for an FCU connection + `settle_sec` (default 10 s), reads +the current value, and compares (float32 tolerance). A clean run logs +`10 already correct, 0 mismatched`. A mismatch under the default (`auto_set: false`, +`on_mismatch: warn`) logs, e.g., `EKF2_HGT_REF: FCU has 1, expected 3 (not set — +auto_set=false). Fix in QGroundControl.` + +Disable it entirely with `enabled: false`. + +> **The checker does NOT configure the companion link** (`MAV_*` / `SER_*` +> params in section 3) — those are set once in QGC. + +--- + +## 3. Companion MAVLink link (Jetson ↔ Cube) + +The Jetson's `mavros` connects to the Cube over a serial link chosen by +`FCU_URL` in the deployment env +([`overrides/l4t-px4-realrobot.env`](overrides/l4t-px4-realrobot.env)). + +### Option A — USB (`/dev/ttyACM0`) — current, but has a known stall + +``` +FCU_URL=/dev/ttyACM0:115200 +``` + +The Cube auto-starts a MAVLink instance on USB (`SYS_USB_AUTO=2`, +`cdcacm_autostart`). This works but has a **documented failure mode on this +hardware**: the Cube Orange (`2dae:1016`) intermittently NAKs USB **OUT** +transfers for 10–30 s windows, even at low data rates. The symptom is bursts of: + +``` +mavconn: 0: DROPPED Message-Id 102 [...] MAVConnSerial::send_message: TX queue overflow + at line 165 in ./src/interface.cpp +``` + +Every *outbound* message type is affected (102 VISION_POSITION_ESTIMATE, 82, 111, +0 HEARTBEAT, …), the **inbound** direction stays perfect, and it is **not a +bandwidth problem** — it reproduces at ~200 B/s. Rate-limiting the vision stream +does **not** help. Closing and reopening the port clears it. Root cause is the +Cube's USB CDC-ACM stack under sustained OUT load, not the AirStack side. + +**Consequence for external vision:** dropped `102`s mean EKF2 receives vision +only in bursts, dead-reckons on the IMU between them, and its local estimate +drifts away from the mocap pose. Fix the link before trusting fusion. + +### Option B — TELEM2 UART — recommended for a companion computer + +Move the companion link off USB onto the TELEM2 UART (the PX4-recommended +companion connection), which sidesteps the USB CDC path entirely. + +**Wiring:** Cube **TELEM2** ↔ a Jetson UART (e.g. `/dev/ttyTHS1`), TX↔RX +crossed, common ground. Do **not** connect the FCU 5 V rail to the Jetson. + +**PX4 params (set once in QGC, then reboot):** + +| Parameter | Value | Meaning | +|---|---|---| +| `MAV_1_CONFIG` | `102` | Start a second MAVLink instance on **TELEM2**. (`0` = disabled; `101` = TELEM1, used by the SiK radio.) | +| `MAV_1_MODE` | `2` | **Onboard** mode — the message set for a companion computer. | +| `MAV_1_RATE` | `0` | Unlimited (or a byte/s cap if you want to bound the link). | +| `MAV_1_FORWARD` | `0` | Don't forward other links onto this one. | +| `SER_TEL2_BAUD` | `115200` | TELEM2 baud (raise to `921600` if the UART and cabling are clean). | + +**Deployment env:** + +``` +FCU_URL=/dev/ttyTHS1:115200 +``` + +> No-quotes gotcha: in the compose list-syntax `environment:`, values are +> literal — `FCU_URL="/dev/ttyTHS1:115200"` passes the quotes through and breaks +> MAVROS URL parsing. Write it bare. + +You can leave the USB MAVLink instance enabled as a spare / QGC-over-USB port; +it won't conflict with TELEM2. + +--- + +## 4. Vision pose pipeline (mocap → PX4) + +``` +Motive (OptiTrack, 100 Hz) + → natnet_ros2_node publishes the rigid body as a ROS pose (ENU) + → vision_pose_converter rate-limit + quaternion canonicalize (passthrough) + → mavros vision_pose converts ENU→NED, sends VISION_POSITION_ESTIMATE (msg 102) + → PX4 EKF2 fuses per the params in section 1 +``` + +**Frame convention — the thing to get right.** MAVROS's `vision_pose` plugin +expects **ROS ENU** and converts to PX4 NED internally. The +[`vision_pose_converter_node.py`](robot/ros_ws/src/perception/natnet_ros2/src/vision_pose_converter_node.py) +does **no coordinate transform** — it only rewrites `frame_id`, optionally +canonicalizes the quaternion sign (`qw ≥ 0`), and rate-limits. **So +`natnet_ros2_node` must already publish ENU.** If position/yaw come out rotated +or axis-swapped, fix it there, not in the converter. + +**Rate limiting.** `max_rate_hz` (default 50 in +[`vision_pose_converter.yaml`](robot/ros_ws/src/perception/natnet_ros2/config/vision_pose_converter.yaml)) +caps the stream to MAVROS. EKF2 only needs 30–50 Hz. Note this is about not +saturating a healthy serial link — it does **not** fix the USB CDC stall in +section 3. + +--- + +## 4b. The height datum: why `local_position.z` was ~36 m off + +On real hardware the drone reported ~36 m of altitude while sitting on the mocap +floor. The cause is a **datum interaction**, not a bug in any single component: + +- AirStack anchors the world at a shared origin altitude of **90.0 m**, used by sim + ([`gps_utils.py`](simulation/isaac-sim/launch_scripts/gps_utils.py)), the GCS + (`gcs_utils.py ORIGIN_ALT`), and the synthetic GPS origin. That 90.0 is a **WGS‑84 + ellipsoidal** height. +- MAVROS/PX4 convert a GPS-origin altitude from ellipsoidal to **AMSL** using the + **egm96‑5 geoid**. At the Lisbon datum the geoid undulation is **N ≈ 54 m**. +- Publishing the literal 90.0 makes PX4 anchor its vertical frame at + `AMSL = 90 − 54 = 36 m`, while OptiTrack says the floor is `z = 0`. The + **36 m gap** is exactly `90 − N`. + +**Fix — [`mavros_gp_origin_node.py`](robot/ros_ws/src/perception/natnet_ros2/src/mavros_gp_origin_node.py) +with `use_geoid_altitude: true`:** instead of the literal 90.0, publish the origin +altitude as `N + desired_floor_amsl` (≈ `54 + 0`). MAVROS's egm96‑5 conversion then +cancels (`54 − 54 = 0`), so `local_position.z` equals the OptiTrack height (floor = 0). +`N` is computed at runtime with `GeoidEval` — no hardcoded magic number — using the +same egm96‑5 model MAVROS uses, so the undulation cancels regardless of its absolute +value. + +**Why this never showed in sim:** the geoid path is auto-skipped when +`use_sim_time: true` (sim uses the literal 90.0 on both ends), and sim's synthetic +GPS is self-consistent with the spawn — there's no ellipsoidal-vs-AMSL mismatch. The +bug is structurally real-hardware-only. + +> **Don't "fix" it by changing the 90.0 globally** — it's a shared sim/GCS/origin +> datum; changing it breaks sim↔GCS consistency. The `gcs_utils.py` altitudes are +> display-only (visualization), not flight inputs. + +--- + +## 5. Verify it's actually fusing + +**Live, in the QGC MAVLink _Console_** (not the Inspector — it can't see +companion→FCU messages): + +``` +listener vehicle_visual_odometry # should be steady ~50 Hz, not gappy +listener estimator_status +``` + +On the ROS side: `/{ROBOT_NAME}/interface/mavros/local_position/pose` should +publish and track the mocap. Hand-lift test: raise the vehicle, Z should go up +(Motive Z-up correct); translate it and check the sign/axis match. + +**Definitive, from the SD-card ulog** ([Flight Review](https://logs.px4.io) or +PlotJuggler): + +- `estimator_innovations` → **`ev_hpos` / `ev_vpos` / `ev_yaw`** and their + **test ratios**. Ratio > 1 ⇒ EKF2 is *rejecting* the measurement + (frame / timing / covariance). Near-zero with occasional gaps ⇒ fusing fine + but starved by dropped messages (section 3). +- `estimator_status_flags` → **`cs_ev_pos` / `cs_ev_yaw`** — confirms EV fusion + is actually active. If unset, EKF2 isn't fusing vision regardless of params. +- `vehicle_visual_odometry` rate in the log quantifies how many `102`s actually + arrived. + +--- + +## Troubleshooting quick reference + +| Symptom | Likely cause | Where to look | +|---|---|---| +| `DROPPED Message-Id 102 … TX queue overflow` | Cube USB CDC OUT stall | Section 3 → move to TELEM2 | +| mavros local pos drifts away from mocap over time | Dropped `102`s starving EKF2 | Fix link first, then recheck | +| Constant rotation between mocap and EKF2 pose | Yaw/frame misalignment | `natnet_ros2_node` frame (must be ENU); `EKF2_EV_CTRL` yaw bit | +| Axes swapped / uncorrelated | Wrong frame convention | `natnet_ros2_node`, not the converter | +| Param set "rejected or readback mismatch" | Wrong literal type (int vs float) | Section 1 — floats need a decimal point | +| Won't arm | GPS still required | `COM_ARM_WO_GPS: 1`, reboot | +| EV innovation test ratio > 1 | EKF2 rejecting vision | Retune `EKF2_EV_DELAY`, `EKF2_EVP_NOISE` / `EKF2_EVA_NOISE` | diff --git a/mkdocs.yml b/mkdocs.yml index e75d85bc7..527bb899d 100644 --- a/mkdocs.yml +++ b/mkdocs.yml @@ -116,6 +116,7 @@ nav: - Perception: - docs/robot/autonomy/perception/index.md - NatNet (OptiTrack): robot/ros_ws/src/perception/natnet_ros2/README.md + - PX4 External Vision (mocap): docs/robot/px4_external_vision.md - Local: - docs/robot/autonomy/local/index.md - World Model: 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..e3e6cb501 --- /dev/null +++ b/robot/ros_ws/src/perception/natnet_ros2/config/mavros_gp_origin.yaml @@ -0,0 +1,33 @@ +# Synthetic GPS origin for mocap / no-GNSS flight via MAVROS. +# Loaded by mavros_gp_origin.launch.xml when publish_to_mavros is enabled. + +/**: + ros__parameters: + # With GNSS disabled, PX4 has no global position, so modes that require one + # (e.g. AUTO.LOITER) refuse to arm. Setting an origin lets PX4 derive a + # global position from the fused vision estimate. Guarded: skipped if an + # origin already exists (e.g. on a GNSS-equipped vehicle). + enabled: true + # MUST match the GCS world origin so Foxglove waypoints transform 1:1: + # - gcs_visualizer/gcs_utils.py ORIGIN_LAT / ORIGIN_LON + # - sim launch_scripts/gps_utils.py DEFAULT_WORLD_ORIGIN + # If this disagrees with the GCS (e.g. the old Zurich SITL default), the + # relay computes a boot-ENU offset of ~1.8e6 m and the drone flies the + # wrong way. Default is Lisbon (the AirStack shared world datum). + latitude: 38.736832 + longitude: -9.137977 + # Literal fallback / sim datum (ellipsoidal). Used directly only when + # use_geoid_altitude is false OR use_sim_time is true. Keep = the shared + # world datum (90.0) so sim and the GCS agree. + altitude: 90.0 + # Real hardware: derive the origin altitude from the egm96-5 geoid undulation + # at (latitude, longitude) so local_position z equals the OptiTrack height. + # Same model as mavros (mavros_uas::egm96_5) => the undulation cancels exactly. + # Auto-skipped in sim (use_sim_time=true). desired_floor_amsl = AMSL of the + # mocap floor (vision z=0); 0.0 makes local z == OptiTrack z. + use_geoid_altitude: true + desired_floor_amsl: 0.0 + geoid_model: "egm96-5" + # Wait this long after MAVROS connects (listening for an existing origin) + # before publishing the synthetic one. + 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..0049788d4 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,129 @@ -# 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. +# NatNet ROS 2 configuration — parsed by natnet_ros2.launch.py. # -# 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). +# Unlike a plain ROS 2 parameter file, this uses a custom `natnet:` schema so a +# single file can describe multiple robots, each tracking multiple rigid bodies. +# The launch file selects the profile matching the container's ROBOT_NAME, flattens +# it into node parameters, and (optionally) brings up the MAVROS vision_pose bridge. +# +# Schema: +# natnet: +# server: generic connection settings shared by every agent +# robots: one profile per ROBOT_NAME +# : +# vision_pose: whether/how to forward a body to MAVROS (optional) +# bodies: rigid bodies this robot subscribes to (one or more) + +natnet: + + # --- Connection settings (generic across all agents) ----------------------- + server: + # IP of the PC running Motive (OptiTrack server). Defaults to the Isaac Sim + # container on the airstack_network (172.31.0.200). + server_ip: "$(env NATNET_SERVER_IP 192.168.123.199)" + # Motive learns the unicast destination from the outbound UDP source IP — bind + # explicitly when the client has multiple NICs (e.g. Docker 172.17.* vs LAN). 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" — point-to-point; Motive streams directly to this machine (default). + # "multicast" — Motive broadcasts to a group; every robot receives the full frame + # and filters by the body ids in its profile. Use for multi-robot. 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. + # Only used when connection_type = "multicast" (OptiTrack default 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 applied to every published pose. debug enables per-frame logging. 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. The node skips latency_sampling_warmup_s after + # the first frame, then accumulates transit latency (from the NatNet + # TransmitTimestamp) over latency_sampling_window_s and logs a one-shot mean/stdev. + # cube_orange_latency_ms models the extra hop through the PX4 flight controller + # hardware (MAVROS -> MAVLink over USB/serial -> uORB -> EKF2) and is added to the + # measured transport latency for the reported end-to-end figure. + 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 entirely. + # input_topic is one of this robot's body pose_cov topics; the two outputs map + # to MAVROS's vision_pose/pose (PoseStamped) and vision_pose/pose_cov + # (PoseWithCovarianceStamped) subscribers. Topics are relative and namespaced + # under /{ROBOT_NAME}/ — redirect them to interface with other middleware. + 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. rigid_body_name must match Motive exactly + # (case-sensitive). topic is a relative leaf namespaced under /{ROBOT_NAME}/; + # pose / pose_cov toggle the PoseStamped and PoseWithCovarianceStamped variants. + bodies: + - rigid_body_name: "Hummingbird" + id: 1146 + topic: "perception/optitrack/drone" + pose: true + pose_cov: true + # Covariances are per-body. Sub-0.1 mm / sub-0.1 deg for OptiTrack precision. + 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..ace4048e8 --- /dev/null +++ b/robot/ros_ws/src/perception/natnet_ros2/config/px4_params.yaml @@ -0,0 +1,64 @@ +# PX4 parameters enforced at startup for OptiTrack-only (external vision) flight. +# Loaded by px4_param_setter.launch.xml via . +# +# Every entry under params. is read via MAVROS get_parameters, skipped when the +# FCU already holds the value, and otherwise pushed via param/set + verified. +# PX4 persists parameters, so after the first boot this is a verify-only pass. +# +# TYPE MATTERS: write integers bare (11) and floats with a decimal point (6.0) +# so the MAVLink param type matches the FCU's declaration. +# +# Values assume PX4 >= 1.14 (EKF2_EV_CTRL / EKF2_GPS_CTRL era). For older +# firmware use EKF2_AID_MASK: 24 and EKF2_HGT_MODE: 3 instead. + +/**: + ros__parameters: + enabled: true + # SAFETY: check-only by default — the node reads the FCU params and flags any + # that differ from the set below, but never writes. Set the params once in + # QGroundControl (see docs). Flip auto_set:true only if you want the node to + # push them to the FCU itself. + auto_set: false + # With auto_set:false, what to do on a mismatch: 'warn' (log diffs, keep the + # stack up) or 'halt' (log fatal + exit non-zero so a required launch node + # tears the stack down before flight). + 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). + # Add bit 4 (velocity) only if a vision_speed source is streamed too. + EKF2_EV_CTRL: 11 + # Vision is the height reference (not baro / GPS). + EKF2_HGT_REF: 3 + # No GPS fusion. + EKF2_GPS_CTRL: 0 + # Magnetometer disabled — yaw comes from vision. + EKF2_MAG_TYPE: 5 + # No baro fusion; height is pure vision. Comment out to keep baro as backup. + EKF2_BARO_CTRL: 0 + # Remove the barometer at the SYSTEM level, not just from EKF2 fusion. + # EKF2_BARO_CTRL=0 only disables baro *fusion*, but EKF2 still seeds the + # initial height datum from baro at startup — which pins local z ~36 m off + # when the FCU boots before the vision stream is up. SYS_HAS_BARO=0 makes + # vision the SOLE height source, so the height datum is deterministic on + # every boot (no ekf2 reset needed). WARNING: no baro backup — if vision + # drops mid-flight the altitude estimate diverges. Indoor mocap only. + SYS_HAS_BARO: 0 + # No range-finder height aiding (no rangefinder present; avoids a stray + # competing height source). + EKF2_RNG_CTRL: 0 + # Measured OptiTrack->EKF2 delay: ~1 ms LAN transport + 5 ms Cube Orange + # hop (natnet_ros2_node logs the measured figure; retune from Flight + # Review EV innovations if needed). + EKF2_EV_DELAY: 8.0 + # Use the EKF2_EV*_NOISE floors below instead of the (1e-6) message + # covariance, which is too optimistic to fuse safely. + EKF2_EV_NOISE_MD: 1 + EKF2_EVP_NOISE: 0.01 + EKF2_EVA_NOISE: 0.05 + # Allow arming without GPS. + 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..100ea8c52 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 @@ -10,3 +10,16 @@ # 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). Full-rate mocap + # (100+ Hz) overflows the FCU serial TX queue at 115200 baud; EKF2 only + # needs 30-50 Hz of external vision. + max_rate_hz: 50.0 + # Which MAVROS vision_pose topic(s) to forward: 'pose', 'pose_cov', or 'both'. + # MAVROS emits one VISION_POSITION_ESTIMATE per topic, so 'both' doubles the + # msg-102 rate on the FCU serial link. A single topic halves the TX load; the + # msg-102 wire size is identical for 'pose' vs 'pose_cov', so 'pose_cov' carries + # the covariance (natnet default ~1mm pos / ~1.7mrad ori) at no extra cost. + # NOTE: PX4 EKF2 only uses the message covariance when EKF2_EV_NOISE_MD=1; + # with EKF2_EV_NOISE_MD=0 it uses EKF2_EVP_NOISE/EKF2_EVA_NOISE and 'pose_cov' + # is equivalent to 'pose' at the estimator. + 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..1aeccc0cb 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 @@ -34,6 +34,7 @@ #pragma once +#include #include #include #include @@ -91,6 +92,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 +206,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 +235,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..7f94b399a --- /dev/null +++ b/robot/ros_ws/src/perception/natnet_ros2/src/mavros_gp_origin_node.py @@ -0,0 +1,197 @@ +#!/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. 0.0 => local z == OptiTrack z. + self.declare_parameter('desired_floor_amsl', 0.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..ff4b7f88d 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 @@ -2,25 +2,34 @@ // // 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 +// Published topics (per configured rigid body): +// /{robot_name}/{topic} → PoseStamped (when pose=true) +// /{robot_name}/{topic}/pose_cov → PoseWithCovarianceStamped (when pose_cov=true) +// where {topic} defaults to perception/optitrack/{rigid_body_name} when unset. // -// 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 +// Parameters are flattened from config/natnet_config.yaml by natnet_ros2.launch.py: +// server_ip, client_ip, command_port, data_port, connection_type, +// multicast_address, frame_id, debug, and parallel per-body arrays: +// body_names[], body_ids[], body_topics[], body_pose[], body_pose_cov[], +// body_position_covariance[] / body_orientation_covariance[] (9·N, sliced per body). // // ROBOT_NAME is read from the environment variable set by AirStack's // robot_name_map resolver at container startup. +#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 +41,58 @@ #include "natnet_ros2/natnet_client_adapter.hpp" +// --------------------------------------------------------------------------- +// Send a NAT_CONNECT ping to the Motive command port on a throwaway UDP socket +// and wait for any reply (Motive and the emulator answer with NAT_SERVERINFO). +// +// The SDK's Connect() can fire an assert() deep in ClientCore:: +// ValidateHostConnection (→ SIGABRT) instead of returning NetworkError when +// the host is unreachable in certain states (observed after a host network +// switch), so never hand the SDK a server that doesn't answer the wire +// handshake first. +// --------------------------------------------------------------------------- +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 +102,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 +119,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 +164,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 +185,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 +208,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 +219,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 +228,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 +246,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 +260,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 +294,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 (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)); - if (ensure_publisher_locked(bd.id, bd.name)) { ++newly_created; } + bodies_.emplace(body.id, std::move(body)); } + } - if (newly_created > 0) { + // ----------------------------------------------------------------------- + // 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 (!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 @@ - +