From 81f1e80c73f2cbd3f5d89246a669de7dd63c04b7 Mon Sep 17 00:00:00 2001 From: John Date: Thu, 23 Jul 2026 00:13:46 -0400 Subject: [PATCH 01/19] feat(perception): bring natnet_ros2 client up to the optitrack_emulation baseline MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Take the natnet_ros2 package from #367 onto the reworked base: the C++ NatNet client (natnet_ros2_node + client adapter + natnet_logic seam), the base mavros_gp_origin and vision_pose_converter nodes, per-robot natnet_config profiles, launch files, and the co-located C++/Python unit tests. natnet_ros2 is already listed in tests/colcon_unit_test_packages.yaml, so the base's YAML-driven collection picks up the updated unit tests directly — no proxy files. Real-robot PX4 external-vision fusion (px4_param_setter, geoid-corrected origin, EV-pose bounds) is layered on next. Co-Authored-By: Claude Opus 4.8 --- .../src/perception/natnet_ros2/CMakeLists.txt | 6 +- .../src/perception/natnet_ros2/README.md | 133 ++++--- .../natnet_ros2/config/mavros_gp_origin.yaml | 22 ++ .../natnet_ros2/config/natnet_config.yaml | 154 ++++++-- .../natnet_ros2/natnet_client_adapter.hpp | 11 + .../include/natnet_ros2/natnet_logic.hpp | 46 +++ .../launch/mavros_gp_origin.launch.xml | 34 ++ .../natnet_ros2/launch/natnet_ros2.launch.py | 174 +++++++-- .../launch/vision_pose_converter.launch.xml | 34 +- .../src/perception/natnet_ros2/package.xml | 1 + .../natnet_ros2/src/mavros_gp_origin_node.py | 126 +++++++ .../natnet_ros2/src/natnet_client_adapter.cpp | 24 +- .../natnet_ros2/src/natnet_ros2_node.cpp | 342 ++++++++++++------ .../src/vision_pose_converter_node.py | 24 +- .../natnet_ros2/test/test_natnet_logic.cpp | 40 ++ .../natnet_ros2/test/test_natnet_ros2.py | 173 ++++++++- 16 files changed, 1077 insertions(+), 267 deletions(-) create mode 100644 robot/ros_ws/src/perception/natnet_ros2/config/mavros_gp_origin.yaml create mode 100644 robot/ros_ws/src/perception/natnet_ros2/launch/mavros_gp_origin.launch.xml create mode 100755 robot/ros_ws/src/perception/natnet_ros2/src/mavros_gp_origin_node.py diff --git a/robot/ros_ws/src/perception/natnet_ros2/CMakeLists.txt b/robot/ros_ws/src/perception/natnet_ros2/CMakeLists.txt index ff47a00da..86342cb7c 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,11 @@ 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 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..773947d72 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,16 @@ 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 + 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 +43,99 @@ 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`. +- **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) +#### MAVROS vision pose bridge (optional, per robot) -When `publish_to_mavros: true`, `vision_pose_converter_node` subscribes to `pose_cov` and republishes for PX4: +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**: `/{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 +- **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. ## 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 +146,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` (and `mavros_gp_origin.launch.xml`) and forwards the profile's `input_topic` / `output_pose_topic` / `output_pose_cov_topic`. ### From perception bringup @@ -140,13 +180,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 +202,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 +212,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..bf2abab68 --- /dev/null +++ b/robot/ros_ws/src/perception/natnet_ros2/config/mavros_gp_origin.yaml @@ -0,0 +1,22 @@ +# Synthetic GPS origin for mocap / no-GNSS flight via MAVROS. +# Loaded by mavros_gp_origin.launch.xml when publish_to_mavros is enabled. + +/**: + 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 + altitude: 90.0 + # 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..ad17810d1 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 172.31.0.200)" + # 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: "Drone1" + id: 1 + 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/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..b706cc48c 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 "") -def _ros_params_from_file(config_path: str) -> dict: - """Parse /** / ros__parameters block from a ROS 2 parameter YAML.""" + return _ENV_SUBST.sub(_replace, value) + + +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,113 @@ 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') config_file = LaunchConfiguration('config_file') vision_pose_config_file = LaunchConfiguration('vision_pose_config_file') + gp_origin_config_file = LaunchConfiguration('gp_origin_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) 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 +177,34 @@ 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], ), ] - 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( @@ -87,7 +212,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 +226,23 @@ 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( '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/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..621469145 100644 --- a/robot/ros_ws/src/perception/natnet_ros2/package.xml +++ b/robot/ros_ws/src/perception/natnet_ros2/package.xml @@ -26,6 +26,7 @@ mavros_msgs + geographic_msgs 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..1f21464bc --- /dev/null +++ b/robot/ros_ws/src/perception/natnet_ros2/src/mavros_gp_origin_node.py @@ -0,0 +1,126 @@ +#!/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 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) + # 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._alt = self.get_parameter('altitude').value + self._settle_sec = self.get_parameter('settle_sec').value + + 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 _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..a0889dcc9 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,29 @@ // // 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 // ROS 2 #include "rclcpp/rclcpp.hpp" @@ -41,8 +45,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 +62,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 +107,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 +128,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 +151,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 +162,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 +171,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 +189,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 +203,23 @@ 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) { 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 +228,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(get_logger(), + "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)); } + } + + // ----------------------------------------------------------------------- + // 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); + } - if (newly_created > 0) { + // ----------------------------------------------------------------------- + // 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::mutex pub_mutex_; - std::unordered_map body_names_; - std::unordered_map publishers_; + std::unordered_map bodies_; - std::atomic needs_description_refresh_{false}; - rclcpp::TimerBase::SharedPtr refresh_timer_; + rclcpp::TimerBase::SharedPtr connect_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/vision_pose_converter_node.py b/robot/ros_ws/src/perception/natnet_ros2/src/vision_pose_converter_node.py index 9a36f879d..81d2df3dd 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,23 @@ def __init__(self): self.declare_parameter('frame_id', 'world') self.declare_parameter('child_frame_id', 'base_link') self.declare_parameter('canonical_quaternion', True) + # 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 + 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 +58,21 @@ 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'input_topic={input_topic!r}, output_pose_topic={output_pose_topic!r}, ' + f'output_pose_cov_topic={output_pose_cov_topic!r})' ) @staticmethod 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"] == [] From 6f3d94e9c7d8bbe36d6fa52626ae8a6ca00bf758 Mon Sep 17 00:00:00 2001 From: John Date: Thu, 23 Jul 2026 00:17:57 -0400 Subject: [PATCH 02/19] =?UTF-8?q?feat(natnet):=20real-robot=20PX4=20extern?= =?UTF-8?q?al-vision=20fusion=20(mocap=20=E2=86=92=20EKF2)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Layer the Hummingbird real-robot fusion pipeline onto natnet_ros2 so an OptiTrack-only drone (no GNSS/mag/baro) fuses mocap pose into PX4 EKF2: - mavros_gp_origin_node: publishes a guarded synthetic GPS origin. On real HW, use_geoid_altitude feeds the egm96-5 geoid undulation (N ≈ 54 m at Lisbon) so mavros's ellipsoidal→AMSL conversion cancels and local z == OptiTrack z (fixes the ~36 m = 90 − 54 boot offset; see docs). Auto-skipped in sim. - vision_pose_converter_node: rate-limited mocap → MAVROS vision_pose bridge. - px4_params.yaml: the external-vision EKF2 param set. - natnet_ros2.launch.py wires the bridges when a robot's vision_pose block is on. px4_param_setter reworked into a **checker** (R3): auto_set=false by default — it reads and *flags* FCU params that differ from the desired set instead of writing them; on_mismatch=warn|halt (default warn). Set the params in QGroundControl; the node is the pre-flight safety net. auto_set=true restores the legacy enforce path. Excludes the duplicate vendored NatNet SDK (sensors/natnet_ros2) and deployment override .envs. Co-Authored-By: Claude Opus 4.8 --- .../src/perception/natnet_ros2/CMakeLists.txt | 1 + .../src/perception/natnet_ros2/README.md | 27 +- .../natnet_ros2/config/mavros_gp_origin.yaml | 11 + .../natnet_ros2/config/natnet_config.yaml | 6 +- .../natnet_ros2/config/px4_params.yaml | 64 ++++ .../config/vision_pose_converter.yaml | 13 + .../natnet_ros2/launch/natnet_ros2.launch.py | 23 ++ .../launch/px4_param_setter.launch.xml | 36 +++ .../src/perception/natnet_ros2/package.xml | 1 + .../natnet_ros2/src/mavros_gp_origin_node.py | 73 ++++- .../natnet_ros2/src/natnet_ros2_node.cpp | 68 +++- .../natnet_ros2/src/px4_param_setter_node.py | 296 ++++++++++++++++++ .../src/vision_pose_converter_node.py | 42 ++- 13 files changed, 650 insertions(+), 11 deletions(-) create mode 100644 robot/ros_ws/src/perception/natnet_ros2/config/px4_params.yaml create mode 100644 robot/ros_ws/src/perception/natnet_ros2/launch/px4_param_setter.launch.xml create mode 100755 robot/ros_ws/src/perception/natnet_ros2/src/px4_param_setter_node.py diff --git a/robot/ros_ws/src/perception/natnet_ros2/CMakeLists.txt b/robot/ros_ws/src/perception/natnet_ros2/CMakeLists.txt index 86342cb7c..7d762f688 100644 --- a/robot/ros_ws/src/perception/natnet_ros2/CMakeLists.txt +++ b/robot/ros_ws/src/perception/natnet_ros2/CMakeLists.txt @@ -69,6 +69,7 @@ endif() 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 773947d72..8a3970b4f 100644 --- a/robot/ros_ws/src/perception/natnet_ros2/README.md +++ b/robot/ros_ws/src/perception/natnet_ros2/README.md @@ -33,6 +33,8 @@ NatNet ROS 2 Node (loads the ROBOT_NAME profile from natnet_config.yaml) └→ (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 @@ -90,6 +92,29 @@ With GNSS disabled (`EKF2_GPS_CTRL=0`), PX4 fused EKF has **no global position** 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 `config/natnet_config.yaml` uses a custom `natnet:` schema (not a flat ROS 2 param @@ -146,7 +171,7 @@ ros2 launch natnet_ros2 natnet_ros2.launch.py \ ### MAVROS bridge -Set `vision_pose.enabled: true` in the robot's profile. The launch file includes `vision_pose_converter.launch.xml` (and `mavros_gp_origin.launch.xml`) and forwards the profile's `input_topic` / `output_pose_topic` / `output_pose_cov_topic`. +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 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 index bf2abab68..e3e6cb501 100644 --- 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 @@ -16,7 +16,18 @@ # 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 ad17810d1..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 @@ -19,7 +19,7 @@ natnet: 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 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" @@ -67,8 +67,8 @@ natnet: # (case-sensitive). topic is a relative leaf namespaced under /{ROBOT_NAME}/; # pose / pose_cov toggle the PoseStamped and PoseWithCovarianceStamped variants. bodies: - - rigid_body_name: "Drone1" - id: 1 + - rigid_body_name: "Hummingbird" + id: 1146 topic: "perception/optitrack/drone" pose: true pose_cov: true 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/launch/natnet_ros2.launch.py b/robot/ros_ws/src/perception/natnet_ros2/launch/natnet_ros2.launch.py index b706cc48c..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 @@ -129,16 +129,19 @@ def generate_launch_description() -> LaunchDescription: 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) robot_name = os.environ.get('ROBOT_NAME', 'robot_1') @@ -178,6 +181,10 @@ def launch_setup(context, *_args, **_kwargs): name='natnet_ros2_node', output='screen', 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, ), ] @@ -205,6 +212,17 @@ def launch_setup(context, *_args, **_kwargs): ], ), ) + 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( @@ -239,6 +257,11 @@ def launch_setup(context, *_args, **_kwargs): 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', 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/package.xml b/robot/ros_ws/src/perception/natnet_ros2/package.xml index 621469145..1f9e04f4d 100644 --- a/robot/ros_ws/src/perception/natnet_ros2/package.xml +++ b/robot/ros_ws/src/perception/natnet_ros2/package.xml @@ -27,6 +27,7 @@ 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 index 1f21464bc..7f94b399a 100755 --- 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 @@ -13,6 +13,9 @@ untouched. """ +import shutil +import subprocess + import rclpy from rclpy.node import Node from geographic_msgs.msg import GeoPointStamped @@ -32,6 +35,21 @@ def __init__(self): 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) @@ -43,8 +61,8 @@ def __init__(self): self._lat = self.get_parameter('latitude').value self._lon = self.get_parameter('longitude').value - self._alt = self.get_parameter('altitude').value self._settle_sec = self.get_parameter('settle_sec').value + self._alt = self._resolve_altitude() self._done = False self._origin_exists = False @@ -68,6 +86,59 @@ def __init__(self): 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: 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 a0889dcc9..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 @@ -26,6 +26,11 @@ #include #include +// POSIX sockets for the pre-connect reachability probe +#include +#include +#include + // ROS 2 #include "rclcpp/rclcpp.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" @@ -36,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 // --------------------------------------------------------------------------- @@ -214,6 +271,15 @@ class NatNetROS2Node : public rclcpp::Node // 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); @@ -243,7 +309,7 @@ class NatNetROS2Node : public rclcpp::Node if (connect_timer_) { connect_timer_->cancel(); } return; } - RCLCPP_INFO(get_logger(), + 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_) { 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 81d2df3dd..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 @@ -34,6 +34,16 @@ 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') @@ -43,6 +53,19 @@ def __init__(self): 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 @@ -71,6 +94,7 @@ def __init__(self): 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'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})' ) @@ -99,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}") From 632b715bcb30eb7b5751eb83bcd442508732cd4d Mon Sep 17 00:00:00 2001 From: John Date: Thu, 23 Jul 2026 00:34:45 -0400 Subject: [PATCH 03/19] docs(natnet): PX4 external-vision setup guide + height-datum explainer MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Move the PX4 external-vision setup guide into docs/ (was a repo-root markdown) and wire it into the mkdocs nav under Perception. Adapt it to the reworked param checker (auto_set default off; check-and-flag, not enforce), and add a "height datum" section explaining the ~36 m local_z offset: AirStack's 90.0 ellipsoidal world datum minus the egm96-5 geoid undulation (N ≈ 54 m at Lisbon) = 36 m; fixed by publishing the geoid-corrected origin altitude so mavros's conversion cancels. Documents why it's invisible in sim and why the shared 90.0 datum must not be changed globally. Co-Authored-By: Claude Opus 4.8 --- docs/robot/px4_external_vision.md | 246 ++++++++++++++++++++++++++++++ mkdocs.yml | 1 + 2 files changed, 247 insertions(+) create mode 100644 docs/robot/px4_external_vision.md 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 4b77be708..f45ef0963 100644 --- a/mkdocs.yml +++ b/mkdocs.yml @@ -118,6 +118,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: From 38102087e3ef0cbb3639f693bfb1d49f701af24b Mon Sep 17 00:00:00 2001 From: John Date: Thu, 23 Jul 2026 14:19:33 -0400 Subject: [PATCH 04/19] feat(perception): point natnet launch include at the natnet_config schema Refine the perception bringup comment on the LAUNCH_NATNET include so it points at the per-robot natnet_config.yaml schema parsed by natnet_ros2.launch.py. Co-Authored-By: Claude Opus 4.8 --- .../perception/perception_bringup/launch/perception.launch.xml | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) 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 @@ - + From b5f0efe8f11782cdc49b6bc99fdf0d41564253cc Mon Sep 17 00:00:00 2001 From: John Date: Thu, 23 Jul 2026 14:19:33 -0400 Subject: [PATCH 05/19] chore: bump version to 0.19.0-alpha.14 --- .env | 2 +- CHANGELOG.md | 1 + 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/.env b/.env index 4aa2502f1..70736e9ac 100644 --- a/.env +++ b/.env @@ -12,7 +12,7 @@ PROJECT_NAME="airstack" # If you've run ./airstack.sh setup, then this will auto-generate from the git commit hash every time a change is made # to a Dockerfile or docker-compose.yaml file. Otherwise this can also be set explicitly to make a release version. # auto-generated from git commit hash -VERSION="0.19.0-alpha.13" +VERSION="0.19.0-alpha.14" # Choose "dev" or "prebuilt". "dev" is for mounted code that must be built live. "prebuilt" is for built ros_ws baked into the image DOCKER_IMAGE_BUILD_MODE="dev" # Where to push and pull images from. Can replace with your docker hub username if using docker hub. diff --git a/CHANGELOG.md b/CHANGELOG.md index 8c99c905e..8945a9c66 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -16,6 +16,7 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0 - `overrides/l4t-px4-realrobot.env` — site-agnostic deployment override for a single real PX4 robot on a Jetson (aarch64/l4t) - `integration` test tier (`tests/integration/`, `integration` mark) with a shared `robot_autonomy_stack` fixture (robot container, no sim/GPU) - `waypoint_flight` system test (`tests/system/test_waypoint_flight.py`): takeoff → ordered waypoint route via `NavigateTask` (dispatched as a dense plan) → land, judged on the odometry track by the standalone stdlib-only `tests/waypoint_checker.py` (in-order corridor arrival within `--waypoint-tolerance`, final goal within `--goal-tolerance`, per-waypoint `--waypoint-timeout`); validated end-to-end in Isaac Sim; serves as the standard acceptance check after integrating or swapping a planner module +- Real-robot PX4 external-vision fusion in `natnet_ros2` (OptiTrack mocap → EKF2): `mavros_gp_origin` (geoid-corrected synthetic GPS origin so `local_position.z` == OptiTrack z, fixing the ~36 m boot offset), `vision_pose_converter`, and a PX4 param **checker** (`px4_param_setter`, `auto_set` off by default; `on_mismatch` warn/halt) — setup guide at `docs/robot/px4_external_vision.md` ### Changed From 56814b3424aa67a31668baa02971d4d0eed99c2f Mon Sep 17 00:00:00 2001 From: John Date: Fri, 31 Jul 2026 18:37:14 -0400 Subject: [PATCH 06/19] fix(natnet): make the NatNet client actually reachable + correct EV tuning MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Three defects that together meant the OptiTrack client could never connect to anything, in sim or on a real robot. 1. NATNET_SERVER_IP was unreachable config. natnet_config.yaml resolves it via $(env ...), but docker compose only injects variables named in a service's `environment:` block and no service declared it — not the compose files, not .env, not tests/system/test_optitrack_e2e.py. The client therefore always fell back to its hardcoded default (192.168.123.199), which is neither the in-sim emulator (172.31.0.200) nor any Motive host. Forwarded in robot-base-docker-compose.yaml, defaulting to the emulator so the sim path works unconfigured. 2. The tracked rigid body could never match. robot_1 pinned "Hummingbird" id 1146 while the emulator streams "Drone" id 1, and the NatNet client filters incoming frames by NUMERIC id — a mismatch yields a connected client that silently never publishes. Body name/id now accept $(env ...) (expanded in _build_node_params, with the id still coerced to int) and default to the emulator's body; sites override via NATNET_BODY_NAME / NATNET_BODY_ID. 3. EV tuning was not the deployment-validated set. EKF2_EV_DELAY 8.0 -> 7.0 and EKF2_EVP_NOISE 0.01 -> 0.05. EKF2_EVP_NOISE is not marker precision: it also sets the innovation gate at EKF2_EVP_GATE (default 5) sigma, so 0.01 gave a 5 cm gate that rejected legitimate mocap updates and refused to arm. 0.05 is a 25 cm gate, still far tighter than PX4's 0.1 default. px4_params.yaml keeps the evidence inline, including two results that are expensive to rediscover: raising EKF2_EV_DELAY to 50.0 measurably degrades tracking (the negative best-fit time shift shows the estimate running ahead of truth), and the drift-and-snap excursions were a 90 deg body-yaw offset in the Motive rigid-body definition, not a gate problem — so the fix belongs in Motive, never as yaw compensation in code. Adds two unit tests covering body-field env expansion and the emulator-matching defaults (natnet_ros2: 14 -> 16 passing). Co-Authored-By: Claude Opus 5 --- CHANGELOG.md | 3 ++ robot/docker/robot-base-docker-compose.yaml | 10 +++++ .../natnet_ros2/config/natnet_config.yaml | 14 +++++-- .../natnet_ros2/config/px4_params.yaml | 38 ++++++++++++++++--- .../natnet_ros2/launch/natnet_ros2.launch.py | 6 ++- .../natnet_ros2/test/test_natnet_ros2.py | 33 ++++++++++++++++ 6 files changed, 93 insertions(+), 11 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 8945a9c66..eb0c7d10f 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -40,6 +40,9 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0 - Dropped `ROBOT_NAME` / `ROS_DOMAIN_ID` from `overrides/l4t-px4-realrobot.env`: no compose service declares either, so an env file could never set them and the lines were inert - `bag_record/bag_recording_status` was bridged GCS -> robot in `domain_bridge.yaml`, the same direction as the command it answers, so recorder status never reached the GCS and every recording indicator stayed blank - `bag_record_node` passed `--exclude` to `ros2 bag record`, which Jazzy renamed to `--exclude-regex`. It is now an ambiguous prefix of four options, so argparse rejected the command and any section using `exclude:` (including `log.yaml`'s `airstack` section, i.e. everything but the cameras) recorded nothing — surfacing only as a usage dump in the node's stdout. Multiple `exclude:` entries are now alternated into one regex instead of repeating a single-valued flag, which had silently kept only the last +- `natnet_config.yaml`'s `$(env NATNET_SERVER_IP ...)` could never resolve: no compose service declared the variable, so the NatNet client always fell back to its hardcoded default and could reach neither the in-sim emulator nor a real Motive host. `NATNET_SERVER_IP` / `NATNET_BODY_NAME` / `NATNET_BODY_ID` are now forwarded in `robot-base-docker-compose.yaml`, defaulting to the in-sim emulator +- NatNet rigid-body name/id are now `$(env ...)`-substitutable and default to the emulator's body (`Drone`, id 1). The tracked config previously pinned a site-specific body (id 1146) that no emulator streams — and since the client filters frames by numeric id, that produced a connected client that never published +- OptiTrack external-vision tuning corrected from real-flight bags: `EKF2_EV_DELAY` 8.0 → 7.0 and `EKF2_EVP_NOISE` 0.01 → 0.05. The old 0.01 gave a 5 cm innovation gate (`EKF2_EVP_GATE` × 5σ) that rejected valid mocap updates and blocked arming; `px4_params.yaml` now records the supporting measurements and the drift-and-snap misdiagnosis so neither is repeated ## [1.0.0] - 2024-12-19 diff --git a/robot/docker/robot-base-docker-compose.yaml b/robot/docker/robot-base-docker-compose.yaml index 8cb714e18..406f362d6 100644 --- a/robot/docker/robot-base-docker-compose.yaml +++ b/robot/docker/robot-base-docker-compose.yaml @@ -21,6 +21,16 @@ services: - ONBOARD_BASE_PORT=${ONBOARD_BASE_PORT} - ROBOT_NAME_MAP_CONFIG_FILE=${ROBOT_NAME_MAP_CONFIG_FILE:-default_robot_name_map.yaml} - DEBUG_RVIZ=${DEBUG_RVIZ:-false} + # OptiTrack / NatNet. natnet_config.yaml reads these via $(env ...), but a variable + # only reaches the container if it is named here — without this block the config's + # env substitution can never resolve and the client always falls back to its default. + # Defaults target the in-sim NatNet emulator (isaac-sim's static IP on + # airstack_network, streaming body "Drone" id 1), so the sim path works with no + # override; real deployments set these in their overrides/*.env to the Motive host + # and the rigid-body name/id configured in Motive. + - NATNET_SERVER_IP=${NATNET_SERVER_IP:-172.31.0.200} + - NATNET_BODY_NAME=${NATNET_BODY_NAME:-Drone} + - NATNET_BODY_ID=${NATNET_BODY_ID:-1} volumes: # display stuff - $HOME/.Xauthority:/.Xauthority 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 0049788d4..685373558 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 @@ -18,8 +18,10 @@ 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)" + # container on the airstack_network (172.31.0.200). NATNET_SERVER_IP is forwarded + # into the container by robot/docker/robot-base-docker-compose.yaml; set it in an + # overrides/*.env to point at a real Motive host. + server_ip: "$(env NATNET_SERVER_IP 172.31.0.200)" # 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" @@ -66,9 +68,13 @@ natnet: # 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. + # The NatNet client filters incoming frames by NUMERIC id, so this must equal the + # streaming id of the rigid body in Motive — a mismatch yields a connected client + # that never publishes. Defaults match the in-sim emulator ("Drone", id 1) so the + # sim path works unconfigured; override per site via NATNET_BODY_NAME/ID. bodies: - - rigid_body_name: "Hummingbird" - id: 1146 + - rigid_body_name: "$(env NATNET_BODY_NAME Drone)" + id: "$(env NATNET_BODY_ID 1)" topic: "perception/optitrack/drone" pose: true pose_cov: true 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 index ace4048e8..69b38f6b2 100644 --- a/robot/ros_ws/src/perception/natnet_ros2/config/px4_params.yaml +++ b/robot/ros_ws/src/perception/natnet_ros2/config/px4_params.yaml @@ -51,14 +51,42 @@ # 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 + # OptiTrack->EKF2 delay. natnet_ros2_node MEASURES ~5.7 ms end to end (0.7 ms + # LAN transport + the 5 ms modelled flight-controller hop), and + # vision_pose_converter preserves the mocap header stamp exactly (0.0008 m RMS + # mocap->vision_pose, measured from a flight bag), so EKF2 already knows the true + # measurement time and this only has to cover the residual hop. + # + # Do NOT raise this to compensate for apparent lag in RViz — it does the + # opposite. 50.0 was trialled and is measurably WORSE; back-to-back bags with + # everything else identical (50.0 vs 7.0): + # median |odom-mocap| while moving 0.060 m -> 0.037 m + # X-axis RMS 0.049 m -> 0.023 m + # best-fit time shift -60 ms (pinned at sweep edge) -> -20 ms + # The NEGATIVE best-fit shift is the tell: over-declaring the delay makes EKF2 + # attribute the measurement to a state ~44 ms too old, so the estimate runs AHEAD + # of truth during motion rather than behind. + EKF2_EV_DELAY: 7.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 + # NOT the mocap system's marker precision — this is the filter's total assumed + # position uncertainty, and it also sets the innovation gate, which is + # EKF2_EVP_GATE (PX4 default 5) sigma wide. 0.05 gives a 25 cm gate; the previous + # 0.01 gave only 5 cm, tight enough to reject legitimate updates and refuse to + # arm. Still far tighter than PX4's 0.1 default. + # + # HISTORY: this was first raised on the theory that a too-tight gate caused the + # large drift-and-snap excursions seen in early mocap flights. That theory was + # WRONG — the cause was a 90 deg body-yaw offset in the Motive rigid-body + # definition. EKF2 fuses vision YAW and snaps its heading to it, so its nav frame + # was 90 deg off and IMU-predicted motion fought the (correct) vision position. + # Fixing the rigid body in Motive cut moving error 0.25 m -> 0.04 m and the + # odom/mocap path-length ratio 2.33x -> 1.12x. If you see drift-and-snap, check + # the Motive rigid-body definition FIRST; do not add yaw compensation in code + # (natnet_ros2 and vision_pose_converter are deliberate identity pass-throughs). + # The wider gate is retained as reasonable in its own right, not as a fix for that. + EKF2_EVP_NOISE: 0.05 EKF2_EVA_NOISE: 0.05 # Allow arming without GPS. COM_ARM_WO_GPS: 1 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 440eaf8d6..bf0d3c94d 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 @@ -93,8 +93,10 @@ def _build_node_params(server: dict, profile: dict) -> dict: 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))) + # Name and id accept $(env ...) like the server block, so a site can point the + # client at its Motive rigid body without editing the tracked config. + body_names.append(str(_expand_env(body.get('rigid_body_name', '')))) + body_ids.append(int(_expand_env(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))) 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 37526cd18..32e76a9aa 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 @@ -278,6 +278,39 @@ def test_build_node_params_flattens_bodies(): assert params["body_position_covariance"][9:] == natnet_launch._DEFAULT_POSITION_COVARIANCE +@pytest.mark.unit +def test_build_node_params_expands_env_in_body_name_and_id(monkeypatch): + """Body name/id accept $(env ...) so a site can retarget the tracked rigid body + without editing the config; the id must still come out as an int.""" + monkeypatch.setenv("NATNET_BODY_NAME", "Hawk") + monkeypatch.setenv("NATNET_BODY_ID", "9") + profile = {"bodies": [{ + "rigid_body_name": "$(env NATNET_BODY_NAME Drone)", + "id": "$(env NATNET_BODY_ID 1)", + "topic": "perception/optitrack/drone", + }]} + params = natnet_launch._build_node_params({}, profile) + assert params["body_names"] == ["Hawk"] + assert params["body_ids"] == [9] + + +@pytest.mark.unit +def test_build_node_params_body_env_defaults_match_emulator(monkeypatch): + """Unset env → the in-sim NatNet emulator's body ("Drone", id 1), so the sim path + works with no override. A mismatch here means a connected client that never + publishes, since the NatNet client filters frames by numeric id.""" + monkeypatch.delenv("NATNET_BODY_NAME", raising=False) + monkeypatch.delenv("NATNET_BODY_ID", raising=False) + profile = {"bodies": [{ + "rigid_body_name": "$(env NATNET_BODY_NAME Drone)", + "id": "$(env NATNET_BODY_ID 1)", + "topic": "perception/optitrack/drone", + }]} + params = natnet_launch._build_node_params({}, profile) + assert params["body_names"] == ["Drone"] + assert params["body_ids"] == [1] + + @pytest.mark.unit def test_build_node_params_empty_profile(): """A robot with no profile yields empty body arrays (node tracks nothing).""" From 926f5bb3d5309cfc45ceea14373f38a99118699a Mon Sep 17 00:00:00 2001 From: John Date: Thu, 13 Aug 2026 09:25:19 -0400 Subject: [PATCH 07/19] add a real-robot OptiTrack deployment override MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Mocap counterpart to l4t-px4-realrobot.env: same Jetson stack, plus the NatNet server/body settings and LAUNCH_NATNET. Carries the two things that are easy to get wrong and produce no error. The body id must match Motive's streaming id, since the client filters frames numerically and a mismatch just never publishes. And nothing writes the EKF2 external-vision parameters to a real FCU — px4_param_setter only reads them back and warns — so they have to be set once in QGroundControl. Co-Authored-By: Claude Opus 5 --- CHANGELOG.md | 1 + overrides/l4t-optitrack-realrobot.env | 78 +++++++++++++++++++++++++++ 2 files changed, 79 insertions(+) create mode 100644 overrides/l4t-optitrack-realrobot.env diff --git a/CHANGELOG.md b/CHANGELOG.md index eb0c7d10f..85a643ab8 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -9,6 +9,7 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0 ### Added +- `overrides/l4t-optitrack-realrobot.env` — deployment override for a real Jetson robot flying on OptiTrack mocap (PX4 EKF2 external vision instead of GPS): the NatNet server/body settings, plus the multi-NIC and FCU-parameter notes that path needs - Feature notebook workflow (`use-feature-notebook` skill): every agent-implemented feature gets a local, gitignored `notebook/NNN-feature-slug/` entry with a status-tracked `design_spec.md` (written before coding) and `results/` artifacts + self-contained `results_summary.md` that populate the feature's PR description - Battery and telemetry display in GCS RQT control panel (voltage and percentage per robot when MAVROS battery topic is bridged) - `TARGET_ARCH` build arg (default `x86_64`) in `Dockerfile.robot` to arch-parametrize `LD_LIBRARY_PATH`; `docker-compose.yaml` passes `TARGET_ARCH: aarch64` to the `voxl` and `l4t` real-robot image builds diff --git a/overrides/l4t-optitrack-realrobot.env b/overrides/l4t-optitrack-realrobot.env new file mode 100644 index 000000000..ebc950282 --- /dev/null +++ b/overrides/l4t-optitrack-realrobot.env @@ -0,0 +1,78 @@ +# Real-robot deployment on an NVIDIA Jetson (aarch64 / l4t) flying on OptiTrack mocap: +# PX4 EKF2 fuses the mocap pose as external vision instead of GPS. +# +# Same as overrides/l4t-px4-realrobot.env plus the OptiTrack path. Use that file instead +# if the vehicle flies on GPS. +# +# Build (first time / after image changes): +# airstack image-build --profile l4t robot-l4t +# Run: +# airstack up --env-file overrides/l4t-optitrack-realrobot.env robot-l4t +# +# Data path: +# Motive -> natnet_ros2 -> vision_pose_converter +# -> /{robot}/interface/mavros/vision_pose/pose_cov -> PX4 EKF2 +# +# Setup guide, including the PX4 parameters this expects on the FCU: +# docs/robot/px4_external_vision.md + +# Only bring up the Jetson stack (robot-l4t + zed-l4t). +COMPOSE_PROFILES="l4t" + +# Launch the autonomy stack automatically on container start. +AUTOLAUNCH="true" +NUM_ROBOTS="1" + +# Launches entire robot autonomy stack +AUTONOMY_ROLE="full" + +# --- Robot identity ----------------------------------------------------------- +# Resolved from this device's hostname: name the Jetson robot-1 on the HOST +# Run the following: ``hostnamectl set-hostname robot-1`` +# resulting in robot_1 on domain 1. +# A hostname that matches no rule in default_robot_name_map.yaml falls through to +# unknown_robot on domain 0; the container prints a warning at shell start when it does. + +# --- OptiTrack / NatNet ------------------------------------------------------- +LAUNCH_NATNET="true" + +# IP of the PC running Motive. THE ONE SETTING THAT ALWAYS DIFFERS PER SITE — there is no +# sensible default, so set it before the first flight. +NATNET_SERVER_IP="192.168.1.100" + +# Rigid body to track. The NatNet client filters incoming frames by NUMERIC id, so the id +# must match the rigid body's streaming id in Motive exactly — a mismatch gives you a +# client that connects and then never publishes, with no error. The name is matched +# case-sensitively and is only used for logging/lookup. +NATNET_BODY_NAME="Drone" +NATNET_BODY_ID="1" + +# Multi-NIC note: natnet_config.yaml binds client_ip 0.0.0.0 by default. Motive learns +# where to send unicast data from the inbound packet's source IP, so if this Jetson has +# both wifi and a wired link to the OptiTrack LAN, confirm the mocap traffic actually +# leaves the wired interface. +# +# Also make sure the OptiTrack LAN link does not hand out DNS or a default route, or its +# router will hijack name resolution and take down internet access over wifi: +# nmcli con mod "" ipv4.ignore-auto-dns yes ipv4.never-default yes + +# --- Flight controller (MAVROS) ---------------------------------------------- +# Jetson UART. Note some airframes wire the FCU through an external USB-serial adapter +# instead, in which case this is e.g. /dev/ttyUSB0:921600 — check yours. +FCU_URL="/dev/ttyTHS4:115200" + +# PX4 EKF2 must be configured for external vision (EKF2_EV_CTRL, EKF2_HGT_REF, GPS off, +# ...). Unlike the sim — where the same parameters are pushed at SITL boot — nothing here +# writes them to a real FCU. Set them once in QGroundControl per the setup guide above. +# On startup px4_param_setter READS them back and warns on any mismatch (it is a checker +# by default, auto_set: false in natnet_ros2/config/px4_params.yaml), so a missed +# parameter shows up in the logs rather than in the air. + +# --- Robot description (PX4 iris w/ sensors; override for your airframe) ------ +URDF_FILE="robot_descriptions/iris/urdf/iris_with_sensors.pegasus.robot.urdf" + +# --- Flight-data recording ---------------------------------------------------- +# Where rosbags land on the host (bind-mounted to /bags in-container). RECORD_BAGS brings +# the recorder up; start/stop it from the GCS control panel. +BAG_STORAGE_PATH="/media/airlab/Storage/airstack_collection" +RECORD_BAGS="false" From 7dcdd17f7e8e994c29490e764fb303b43e576a5f Mon Sep 17 00:00:00 2001 From: John Date: Thu, 13 Aug 2026 11:53:41 -0400 Subject: [PATCH 08/19] config bodies per robot profile; trim comments to the docs MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The rigid body a robot tracks is now set only in its natnet_config.yaml profile, keyed by ROBOT_NAME. NATNET_BODY_NAME / NATNET_BODY_ID are gone: a single global env var cannot express per-robot values, so it blocked the multi-robot case the profiles already handle. NATNET_SERVER_IP stays in the environment — one Motive host serves every robot. Comments across the package are cut back to what is not evident from the code. The EKF2 tuning results that were buried in px4_params.yaml move into docs/robot/px4_external_vision.md, which also had stale values (EV_DELAY 15.0, EVP_NOISE 0.01) contradicting the config: that raising EV_DELAY measurably hurts tracking, and that drift-and-snap was a Motive rigid-body yaw offset rather than a gate problem. Kept: the license header, and the note on why the SDK needs a reachability pre-check before Connect(). Co-Authored-By: Claude Opus 5 --- CHANGELOG.md | 4 +- docs/robot/px4_external_vision.md | 33 +++++++- overrides/l4t-optitrack-realrobot.env | 63 +++------------ robot/docker/robot-base-docker-compose.yaml | 10 +-- .../natnet_ros2/config/mavros_gp_origin.yaml | 27 ++----- .../natnet_ros2/config/natnet_config.yaml | 58 +++---------- .../natnet_ros2/config/px4_params.yaml | 81 ++++--------------- .../config/vision_pose_converter.yaml | 16 +--- .../include/natnet_ros2/natnet_logic.hpp | 16 +--- .../natnet_ros2/launch/natnet_ros2.launch.py | 6 +- .../natnet_ros2/src/natnet_ros2_node.cpp | 33 ++------ .../natnet_ros2/test/test_natnet_ros2.py | 33 -------- 12 files changed, 91 insertions(+), 289 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 85a643ab8..b35aee895 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -41,8 +41,8 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0 - Dropped `ROBOT_NAME` / `ROS_DOMAIN_ID` from `overrides/l4t-px4-realrobot.env`: no compose service declares either, so an env file could never set them and the lines were inert - `bag_record/bag_recording_status` was bridged GCS -> robot in `domain_bridge.yaml`, the same direction as the command it answers, so recorder status never reached the GCS and every recording indicator stayed blank - `bag_record_node` passed `--exclude` to `ros2 bag record`, which Jazzy renamed to `--exclude-regex`. It is now an ambiguous prefix of four options, so argparse rejected the command and any section using `exclude:` (including `log.yaml`'s `airstack` section, i.e. everything but the cameras) recorded nothing — surfacing only as a usage dump in the node's stdout. Multiple `exclude:` entries are now alternated into one regex instead of repeating a single-valued flag, which had silently kept only the last -- `natnet_config.yaml`'s `$(env NATNET_SERVER_IP ...)` could never resolve: no compose service declared the variable, so the NatNet client always fell back to its hardcoded default and could reach neither the in-sim emulator nor a real Motive host. `NATNET_SERVER_IP` / `NATNET_BODY_NAME` / `NATNET_BODY_ID` are now forwarded in `robot-base-docker-compose.yaml`, defaulting to the in-sim emulator -- NatNet rigid-body name/id are now `$(env ...)`-substitutable and default to the emulator's body (`Drone`, id 1). The tracked config previously pinned a site-specific body (id 1146) that no emulator streams — and since the client filters frames by numeric id, that produced a connected client that never published +- `natnet_config.yaml`'s `$(env NATNET_SERVER_IP ...)` could never resolve: no compose service declared the variable, so the NatNet client always fell back to its hardcoded default and could reach neither the in-sim emulator nor a real Motive host. It is now forwarded in `robot-base-docker-compose.yaml`, defaulting to the in-sim emulator +- The NatNet rigid body tracked by `robot_1` defaulted to a site-specific body (id 1146) that no emulator streams; since the client filters frames by numeric id, that produced a connected client that never published. It now defaults to the emulator's body (`Drone`, id 1). Per-robot bodies are configured in each robot's profile in `natnet_config.yaml`, selected by `ROBOT_NAME` - OptiTrack external-vision tuning corrected from real-flight bags: `EKF2_EV_DELAY` 8.0 → 7.0 and `EKF2_EVP_NOISE` 0.01 → 0.05. The old 0.01 gave a 5 cm innovation gate (`EKF2_EVP_GATE` × 5σ) that rejected valid mocap updates and blocked arming; `px4_params.yaml` now records the supporting measurements and the drift-and-snap misdiagnosis so neither is repeated ## [1.0.0] - 2024-12-19 diff --git a/docs/robot/px4_external_vision.md b/docs/robot/px4_external_vision.md index 15e98606d..1d9c57ca1 100644 --- a/docs/robot/px4_external_vision.md +++ b/docs/robot/px4_external_vision.md @@ -31,16 +31,43 @@ either way it's a one-time thing per airframe. | `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_DELAY` | `7.0` | Measured OptiTrack→EKF2 latency (ms): ~0.7 ms LAN + ~5 ms Cube hop. `natnet_ros2_node` logs the measured figure. **Do not raise this to chase apparent lag — see below.** | | `EKF2_EV_NOISE_MD` | `1` | Use the `EKF2_EV*_NOISE` floors below instead of the message covariance (which is `1e-6` — too optimistic to fuse safely). | -| `EKF2_EVP_NOISE` | `0.01` | Vision **position** noise floor (m). | +| `EKF2_EVP_NOISE` | `0.05` | Vision **position** noise floor (m). Not marker precision — it also sets the innovation gate, `EKF2_EVP_GATE` (default 5) sigma wide, so this is a 25 cm gate. | | `EKF2_EVA_NOISE` | `0.05` | Vision **angle** noise floor (rad). | | `COM_ARM_WO_GPS` | `1` | Allow arming without GPS. | **Type matters.** Integers are written bare (`11`); floats need a decimal point -(`15.0`) so the MAVLink param type matches the FCU's declaration. Getting this +(`7.0`) so the MAVLink param type matches the FCU's declaration. Getting this wrong makes the set silently reject. +### Two tuning results worth not rediscovering + +Both came out of back-to-back flight bags with everything else held constant. + +**Raising `EKF2_EV_DELAY` makes tracking worse, not better.** 50.0 was trialled against +7.0: median |odom − mocap| while moving went 0.037 → 0.060 m, and X-axis RMS 0.023 → +0.049 m. The tell is the *negative* best-fit time shift (−20 ms → −60 ms, pinned at the +sweep edge): over-declaring the delay makes EKF2 attribute the measurement to a state +that is too old, so the estimate runs **ahead** of truth during motion rather than +behind. Raising this value can never compensate for apparent lag in RViz — it does the +opposite. + +**Large drift-and-snap excursions are a Motive problem, not a gate problem.** They were +first blamed on `EKF2_EVP_NOISE` being too tight. That was wrong. The cause was a 90° +body-yaw offset in the Motive rigid-body definition: EKF2 fuses vision yaw and snaps its +heading to it, so its nav frame was 90° off and IMU-predicted motion fought the (correct) +vision position. Fixing the rigid body in Motive cut moving error 0.25 → 0.04 m and the +odom/mocap path-length ratio 2.33× → 1.12×. + +If you see drift-and-snap, **check the Motive rigid-body definition first**. Do not add +yaw compensation in code — `natnet_ros2` and `vision_pose_converter` are deliberate +identity pass-throughs, and a code-side correction would double-compensate once Motive is +fixed. + +The wider `EKF2_EVP_NOISE` (0.05) is still the right value on its own merits: the previous +0.01 gave only a 5 cm gate, tight enough to reject legitimate updates and refuse to arm. + **Reboot after any change.** Fusion-source (`EKF2_*`) params are safest applied from a clean estimator start — reboot the flight controller before flying. The param setter prints a warning whenever it actually changes something. diff --git a/overrides/l4t-optitrack-realrobot.env b/overrides/l4t-optitrack-realrobot.env index ebc950282..c88253419 100644 --- a/overrides/l4t-optitrack-realrobot.env +++ b/overrides/l4t-optitrack-realrobot.env @@ -1,78 +1,35 @@ # Real-robot deployment on an NVIDIA Jetson (aarch64 / l4t) flying on OptiTrack mocap: -# PX4 EKF2 fuses the mocap pose as external vision instead of GPS. +# PX4 EKF2 fuses the mocap pose as external vision instead of GPS. Use +# overrides/l4t-px4-realrobot.env instead if the vehicle flies on GPS. # -# Same as overrides/l4t-px4-realrobot.env plus the OptiTrack path. Use that file instead -# if the vehicle flies on GPS. +# Build: airstack image-build --profile l4t robot-l4t +# Run: airstack up --env-file overrides/l4t-optitrack-realrobot.env robot-l4t # -# Build (first time / after image changes): -# airstack image-build --profile l4t robot-l4t -# Run: -# airstack up --env-file overrides/l4t-optitrack-realrobot.env robot-l4t -# -# Data path: -# Motive -> natnet_ros2 -> vision_pose_converter -# -> /{robot}/interface/mavros/vision_pose/pose_cov -> PX4 EKF2 -# -# Setup guide, including the PX4 parameters this expects on the FCU: +# Setup guide (PX4 parameters, frames, troubleshooting): # docs/robot/px4_external_vision.md -# Only bring up the Jetson stack (robot-l4t + zed-l4t). COMPOSE_PROFILES="l4t" - -# Launch the autonomy stack automatically on container start. AUTOLAUNCH="true" NUM_ROBOTS="1" - -# Launches entire robot autonomy stack AUTONOMY_ROLE="full" # --- Robot identity ----------------------------------------------------------- # Resolved from this device's hostname: name the Jetson robot-1 on the HOST -# Run the following: ``hostnamectl set-hostname robot-1`` -# resulting in robot_1 on domain 1. -# A hostname that matches no rule in default_robot_name_map.yaml falls through to -# unknown_robot on domain 0; the container prints a warning at shell start when it does. +# hostnamectl set-hostname robot-1 -> robot_1 on domain 1 # --- OptiTrack / NatNet ------------------------------------------------------- LAUNCH_NATNET="true" - -# IP of the PC running Motive. THE ONE SETTING THAT ALWAYS DIFFERS PER SITE — there is no -# sensible default, so set it before the first flight. +# Motive host. No sensible default — set this before the first flight. NATNET_SERVER_IP="192.168.1.100" -# Rigid body to track. The NatNet client filters incoming frames by NUMERIC id, so the id -# must match the rigid body's streaming id in Motive exactly — a mismatch gives you a -# client that connects and then never publishes, with no error. The name is matched -# case-sensitively and is only used for logging/lookup. -NATNET_BODY_NAME="Drone" -NATNET_BODY_ID="1" - -# Multi-NIC note: natnet_config.yaml binds client_ip 0.0.0.0 by default. Motive learns -# where to send unicast data from the inbound packet's source IP, so if this Jetson has -# both wifi and a wired link to the OptiTrack LAN, confirm the mocap traffic actually -# leaves the wired interface. -# -# Also make sure the OptiTrack LAN link does not hand out DNS or a default route, or its -# router will hijack name resolution and take down internet access over wifi: -# nmcli con mod "" ipv4.ignore-auto-dns yes ipv4.never-default yes - # --- Flight controller (MAVROS) ---------------------------------------------- -# Jetson UART. Note some airframes wire the FCU through an external USB-serial adapter -# instead, in which case this is e.g. /dev/ttyUSB0:921600 — check yours. +# Jetson UART; some airframes wire the FCU through USB-serial instead +# (e.g. /dev/ttyUSB0:921600). FCU_URL="/dev/ttyTHS4:115200" -# PX4 EKF2 must be configured for external vision (EKF2_EV_CTRL, EKF2_HGT_REF, GPS off, -# ...). Unlike the sim — where the same parameters are pushed at SITL boot — nothing here -# writes them to a real FCU. Set them once in QGroundControl per the setup guide above. -# On startup px4_param_setter READS them back and warns on any mismatch (it is a checker -# by default, auto_set: false in natnet_ros2/config/px4_params.yaml), so a missed -# parameter shows up in the logs rather than in the air. - -# --- Robot description (PX4 iris w/ sensors; override for your airframe) ------ +# --- Robot description -------------------------------------------------------- URDF_FILE="robot_descriptions/iris/urdf/iris_with_sensors.pegasus.robot.urdf" # --- Flight-data recording ---------------------------------------------------- -# Where rosbags land on the host (bind-mounted to /bags in-container). RECORD_BAGS brings -# the recorder up; start/stop it from the GCS control panel. BAG_STORAGE_PATH="/media/airlab/Storage/airstack_collection" RECORD_BAGS="false" diff --git a/robot/docker/robot-base-docker-compose.yaml b/robot/docker/robot-base-docker-compose.yaml index 406f362d6..793cf6cab 100644 --- a/robot/docker/robot-base-docker-compose.yaml +++ b/robot/docker/robot-base-docker-compose.yaml @@ -21,16 +21,8 @@ services: - ONBOARD_BASE_PORT=${ONBOARD_BASE_PORT} - ROBOT_NAME_MAP_CONFIG_FILE=${ROBOT_NAME_MAP_CONFIG_FILE:-default_robot_name_map.yaml} - DEBUG_RVIZ=${DEBUG_RVIZ:-false} - # OptiTrack / NatNet. natnet_config.yaml reads these via $(env ...), but a variable - # only reaches the container if it is named here — without this block the config's - # env substitution can never resolve and the client always falls back to its default. - # Defaults target the in-sim NatNet emulator (isaac-sim's static IP on - # airstack_network, streaming body "Drone" id 1), so the sim path works with no - # override; real deployments set these in their overrides/*.env to the Motive host - # and the rigid-body name/id configured in Motive. + # OptiTrack / NatNet - NATNET_SERVER_IP=${NATNET_SERVER_IP:-172.31.0.200} - - NATNET_BODY_NAME=${NATNET_BODY_NAME:-Drone} - - NATNET_BODY_ID=${NATNET_BODY_ID:-1} volumes: # display stuff - $HOME/.Xauthority:/.Xauthority 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 index e3e6cb501..a32087e78 100644 --- 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 @@ -1,33 +1,20 @@ # Synthetic GPS origin for mocap / no-GNSS flight via MAVROS. # Loaded by mavros_gp_origin.launch.xml when publish_to_mavros is enabled. +# See docs/robot/px4_external_vision.md for the height-datum explanation. /**: ros__parameters: - # 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). + # Skipped if an origin already exists (e.g. 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). + # MUST match the GCS world origin (gcs_visualizer/gcs_utils.py) and the sim + # datum (launch_scripts/gps_utils.py), or the relay computes a huge ENU offset. latitude: 38.736832 longitude: -9.137977 - # 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. + # Shared world datum; used directly when use_geoid_altitude is false or in sim. 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. + # Real hardware: derive origin altitude from the geoid so local_position z + # equals OptiTrack height. Auto-skipped in sim. 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 685373558..6ad16e074 100644 --- a/robot/ros_ws/src/perception/natnet_ros2/config/natnet_config.yaml +++ b/robot/ros_ws/src/perception/natnet_ros2/config/natnet_config.yaml @@ -1,51 +1,27 @@ -# NatNet ROS 2 configuration — parsed by natnet_ros2.launch.py. -# -# 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 ROS 2 configuration — parsed by natnet_ros2.launch.py, which selects the +# profile matching the container's ROBOT_NAME. +# See docs/robot/px4_external_vision.md for the schema and setup guide. natnet: # --- Connection settings (generic across all agents) ----------------------- server: - # IP of the PC running Motive (OptiTrack server). Defaults to the Isaac Sim - # container on the airstack_network (172.31.0.200). NATNET_SERVER_IP is forwarded - # into the container by robot/docker/robot-base-docker-compose.yaml; set it in an - # overrides/*.env to point at a real Motive host. + # Motive host; defaults to the in-sim emulator. Set NATNET_SERVER_IP per deployment. server_ip: "$(env NATNET_SERVER_IP 172.31.0.200)" - # 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). + # Bind explicitly when the client has multiple NICs. client_ip: "0.0.0.0" command_port: 1510 data_port: 1511 - # "unicast" — point-to-point; Motive streams directly to this machine (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. + # "unicast" (default) or "multicast"; multicast_address applies to the latter. connection_type: "unicast" - # Only used when connection_type = "multicast" (OptiTrack default 239.255.42.99). multicast_address: "239.255.42.99" - # Frame applied to every published pose. debug enables per-frame logging. frame_id: "world" debug: false - # 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. + # Per-message latency reporting. latency_sampling_warmup_s: 5.0 latency_sampling_window_s: 20.0 cube_orange_latency_ms: 5.0 @@ -54,31 +30,21 @@ natnet: 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. + # MAVROS vision_pose bridge; enabled=false skips the converter. vision_pose: enabled: true input_topic: "perception/optitrack/drone/pose_cov" output_pose_topic: "interface/mavros/vision_pose/pose" output_pose_cov_topic: "interface/mavros/vision_pose/pose_cov" - # 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. - # The NatNet client filters incoming frames by NUMERIC id, so this must equal the - # streaming id of the rigid body in Motive — a mismatch yields a connected client - # that never publishes. Defaults match the in-sim emulator ("Drone", id 1) so the - # sim path works unconfigured; override per site via NATNET_BODY_NAME/ID. + # Rigid bodies this robot tracks. name and id must match Motive exactly; the + # client filters frames by numeric id. Defaults match the in-sim emulator. bodies: - - rigid_body_name: "$(env NATNET_BODY_NAME Drone)" - id: "$(env NATNET_BODY_ID 1)" + - rigid_body_name: "Drone" + id: 1 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, 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 index 69b38f6b2..c2bd3b7be 100644 --- a/robot/ros_ws/src/perception/natnet_ros2/config/px4_params.yaml +++ b/robot/ros_ws/src/perception/natnet_ros2/config/px4_params.yaml @@ -1,27 +1,19 @@ -# PX4 parameters enforced at startup for OptiTrack-only (external vision) flight. -# Loaded by px4_param_setter.launch.xml via . +# PX4 parameters for OptiTrack-only (external vision) flight, checked at startup by +# px4_param_setter. See docs/robot/px4_external_vision.md for what each one does, the +# tuning rationale, and how to set them in QGroundControl. # -# 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: integers bare (11), floats with a decimal point (7.0), so the MAVLink +# param type matches the FCU's declaration. # -# 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. +# Values assume PX4 >= 1.14. 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. + # Check-only by default: read the FCU's params and flag differences, never write. 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 with auto_set:false — 'warn' (log diffs) or 'halt' (exit non-zero). on_mismatch: "warn" # Initial full param pull over serial (115200) is slow; give it time. settle_sec: 10.0 @@ -30,63 +22,22 @@ 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). + # Vision is the height reference. EKF2_HGT_REF: 3 - # No GPS fusion. EKF2_GPS_CTRL: 0 - # Magnetometer disabled — yaw comes from vision. + # Magnetometer off; 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. + # Remove the baro at system level, not just from fusion, so the height datum is + # deterministic on every boot. WARNING: no baro backup — indoor mocap only. SYS_HAS_BARO: 0 - # No range-finder height aiding (no rangefinder present; avoids a stray - # competing height source). EKF2_RNG_CTRL: 0 - # OptiTrack->EKF2 delay. natnet_ros2_node MEASURES ~5.7 ms end to end (0.7 ms - # LAN transport + the 5 ms modelled flight-controller hop), and - # vision_pose_converter preserves the mocap header stamp exactly (0.0008 m RMS - # mocap->vision_pose, measured from a flight bag), so EKF2 already knows the true - # measurement time and this only has to cover the residual hop. - # - # Do NOT raise this to compensate for apparent lag in RViz — it does the - # opposite. 50.0 was trialled and is measurably WORSE; back-to-back bags with - # everything else identical (50.0 vs 7.0): - # median |odom-mocap| while moving 0.060 m -> 0.037 m - # X-axis RMS 0.049 m -> 0.023 m - # best-fit time shift -60 ms (pinned at sweep edge) -> -20 ms - # The NEGATIVE best-fit shift is the tell: over-declaring the delay makes EKF2 - # attribute the measurement to a state ~44 ms too old, so the estimate runs AHEAD - # of truth during motion rather than behind. + # Do NOT raise to chase apparent lag; higher is measurably worse (see docs). EKF2_EV_DELAY: 7.0 - # Use the EKF2_EV*_NOISE floors below instead of the (1e-6) message - # covariance, which is too optimistic to fuse safely. + # Use the NOISE floors below rather than the message covariance. EKF2_EV_NOISE_MD: 1 - # NOT the mocap system's marker precision — this is the filter's total assumed - # position uncertainty, and it also sets the innovation gate, which is - # EKF2_EVP_GATE (PX4 default 5) sigma wide. 0.05 gives a 25 cm gate; the previous - # 0.01 gave only 5 cm, tight enough to reject legitimate updates and refuse to - # arm. Still far tighter than PX4's 0.1 default. - # - # HISTORY: this was first raised on the theory that a too-tight gate caused the - # large drift-and-snap excursions seen in early mocap flights. That theory was - # WRONG — the cause was a 90 deg body-yaw offset in the Motive rigid-body - # definition. EKF2 fuses vision YAW and snaps its heading to it, so its nav frame - # was 90 deg off and IMU-predicted motion fought the (correct) vision position. - # Fixing the rigid body in Motive cut moving error 0.25 m -> 0.04 m and the - # odom/mocap path-length ratio 2.33x -> 1.12x. If you see drift-and-snap, check - # the Motive rigid-body definition FIRST; do not add yaw compensation in code - # (natnet_ros2 and vision_pose_converter are deliberate identity pass-throughs). - # The wider gate is retained as reasonable in its own right, not as a fix for that. + # Also sets the innovation gate (EKF2_EVP_GATE sigma wide): 0.05 -> ~25 cm. EKF2_EVP_NOISE: 0.05 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 100ea8c52..f14b4d2d7 100644 --- a/robot/ros_ws/src/perception/natnet_ros2/config/vision_pose_converter.yaml +++ b/robot/ros_ws/src/perception/natnet_ros2/config/vision_pose_converter.yaml @@ -1,25 +1,13 @@ # Vision pose converter → MAVROS bridge parameters. # Loaded by vision_pose_converter.launch.xml via . -# $(env ROBOT_NAME ...) is expanded by launch substitution. /**: ros__parameters: frame_id: "world" child_frame_id: "$(env ROBOT_NAME robot_1)/base_link" # Normalise quaternion to canonical form (qw >= 0) before publishing. - # Recommended for ArduPilot EKF3 and any consumer sensitive to sign flips. - # PX4 EKF2 handles either sign internally, so this is optional for PX4. canonical_quaternion: true - # Cap the rate forwarded to MAVROS (0 = passthrough). Full-rate mocap - # (100+ Hz) overflows the FCU serial TX queue at 115200 baud; EKF2 only - # needs 30-50 Hz of external vision. + # Cap the rate forwarded to MAVROS (0 = passthrough); EKF2 needs only 30-50 Hz. 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. + # Which vision_pose topic(s) to forward: 'pose', 'pose_cov', or 'both'. publish_mode: "pose_cov" diff --git a/robot/ros_ws/src/perception/natnet_ros2/include/natnet_ros2/natnet_logic.hpp b/robot/ros_ws/src/perception/natnet_ros2/include/natnet_ros2/natnet_logic.hpp index 1aeccc0cb..115a2fc98 100644 --- a/robot/ros_ws/src/perception/natnet_ros2/include/natnet_ros2/natnet_logic.hpp +++ b/robot/ros_ws/src/perception/natnet_ros2/include/natnet_ros2/natnet_logic.hpp @@ -18,19 +18,9 @@ // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE // SOFTWARE. -// natnet_logic.hpp — pure C++ helpers for natnet_ros2 (no ROS, no NatNet SDK). -// -// Five responsibility areas: -// -// 1. Covariance assembly -// 2. Topic names -// 3. Connection-configuration helpers (SDK-independent) -// 4. Rigid-body frame helpers (SDK-independent) -// 5. Abstraction seam: INatNetClient interface + negotiation logic -// -// NatNet SDK types (sNatNetClientConnectParams, sRigidBodyData, …) are only -// used inside natnet_ros2_node.cpp and natnet_client_adapter.cpp. -// All logic here uses plain C++ so test_natnet_logic.cpp compiles with only gtest. +// natnet_logic.hpp — pure C++ helpers for natnet_ros2 (no ROS, no NatNet SDK), so +// test_natnet_logic.cpp compiles with only gtest. SDK types stay in +// natnet_ros2_node.cpp / natnet_client_adapter.cpp. #pragma once 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 bf0d3c94d..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 @@ -93,10 +93,8 @@ def _build_node_params(server: dict, profile: dict) -> dict: body_orientation_covariance: list[float] = [] for body in bodies: - # Name and id accept $(env ...) like the server block, so a site can point the - # client at its Motive rigid body without editing the tracked config. - body_names.append(str(_expand_env(body.get('rigid_body_name', '')))) - body_ids.append(int(_expand_env(body.get('id', -1)))) + 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))) 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 ff4b7f88d..2245055d4 100644 --- a/robot/ros_ws/src/perception/natnet_ros2/src/natnet_ros2_node.cpp +++ b/robot/ros_ws/src/perception/natnet_ros2/src/natnet_ros2_node.cpp @@ -1,20 +1,6 @@ -// natnet_ros2_node.cpp -// -// ROS 2 NatNet SDK node for OptiTrack Motive integration. -// -// 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 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. +// natnet_ros2_node.cpp — ROS 2 NatNet SDK node for OptiTrack Motive. +// Parameters are flattened from config/natnet_config.yaml by natnet_ros2.launch.py. +// See docs/robot/px4_external_vision.md. #include #include @@ -41,16 +27,9 @@ #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. -// --------------------------------------------------------------------------- +// Ping the Motive command port before handing the server to the SDK: Connect() can +// assert deep in ClientCore::ValidateHostConnection (SIGABRT) rather than returning +// NetworkError when the host is unreachable. Do not remove this pre-check. static bool natnet_server_reachable( const std::string & server_ip, int command_port, int timeout_ms) { 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 32e76a9aa..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 @@ -278,39 +278,6 @@ def test_build_node_params_flattens_bodies(): assert params["body_position_covariance"][9:] == natnet_launch._DEFAULT_POSITION_COVARIANCE -@pytest.mark.unit -def test_build_node_params_expands_env_in_body_name_and_id(monkeypatch): - """Body name/id accept $(env ...) so a site can retarget the tracked rigid body - without editing the config; the id must still come out as an int.""" - monkeypatch.setenv("NATNET_BODY_NAME", "Hawk") - monkeypatch.setenv("NATNET_BODY_ID", "9") - profile = {"bodies": [{ - "rigid_body_name": "$(env NATNET_BODY_NAME Drone)", - "id": "$(env NATNET_BODY_ID 1)", - "topic": "perception/optitrack/drone", - }]} - params = natnet_launch._build_node_params({}, profile) - assert params["body_names"] == ["Hawk"] - assert params["body_ids"] == [9] - - -@pytest.mark.unit -def test_build_node_params_body_env_defaults_match_emulator(monkeypatch): - """Unset env → the in-sim NatNet emulator's body ("Drone", id 1), so the sim path - works with no override. A mismatch here means a connected client that never - publishes, since the NatNet client filters frames by numeric id.""" - monkeypatch.delenv("NATNET_BODY_NAME", raising=False) - monkeypatch.delenv("NATNET_BODY_ID", raising=False) - profile = {"bodies": [{ - "rigid_body_name": "$(env NATNET_BODY_NAME Drone)", - "id": "$(env NATNET_BODY_ID 1)", - "topic": "perception/optitrack/drone", - }]} - params = natnet_launch._build_node_params({}, profile) - assert params["body_names"] == ["Drone"] - assert params["body_ids"] == [1] - - @pytest.mark.unit def test_build_node_params_empty_profile(): """A robot with no profile yields empty body arrays (node tracks nothing).""" From 7b5bacd7e0d51f0f387566fb17cda73b19d6f142 Mon Sep 17 00:00:00 2001 From: John Date: Thu, 13 Aug 2026 13:11:09 -0400 Subject: [PATCH 09/19] put the mocap floor at the shared world datum MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit desired_floor_amsl 0.0 -> 36.0, the world datum (90 m ellipsoidal) expressed in AMSL, so a mocap robot's reported global altitude agrees with sim and the GCS instead of sitting at sea level. The published ellipsoidal origin works out to ~90 m, the datum itself. local_position.z equals the OptiTrack height for any value of this parameter — it only moves the global altitude. Reasoning lives in the external-vision doc, which also now records that GeoPoint.altitude is ellipsoidal by contract, so AMSL must not be sent here. Not yet confirmed on hardware. Co-Authored-By: Claude Opus 5 --- CHANGELOG.md | 1 + docs/robot/px4_external_vision.md | 31 +++++++++++++++---- .../natnet_ros2/config/mavros_gp_origin.yaml | 4 ++- .../natnet_ros2/src/mavros_gp_origin_node.py | 5 +-- 4 files changed, 32 insertions(+), 9 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index b35aee895..3af86c60d 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -44,6 +44,7 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0 - `natnet_config.yaml`'s `$(env NATNET_SERVER_IP ...)` could never resolve: no compose service declared the variable, so the NatNet client always fell back to its hardcoded default and could reach neither the in-sim emulator nor a real Motive host. It is now forwarded in `robot-base-docker-compose.yaml`, defaulting to the in-sim emulator - The NatNet rigid body tracked by `robot_1` defaulted to a site-specific body (id 1146) that no emulator streams; since the client filters frames by numeric id, that produced a connected client that never published. It now defaults to the emulator's body (`Drone`, id 1). Per-robot bodies are configured in each robot's profile in `natnet_config.yaml`, selected by `ROBOT_NAME` - OptiTrack external-vision tuning corrected from real-flight bags: `EKF2_EV_DELAY` 8.0 → 7.0 and `EKF2_EVP_NOISE` 0.01 → 0.05. The old 0.01 gave a 5 cm innovation gate (`EKF2_EVP_GATE` × 5σ) that rejected valid mocap updates and blocked arming; `px4_params.yaml` now records the supporting measurements and the drift-and-snap misdiagnosis so neither is repeated +- The synthetic GPS origin now places the mocap floor at the shared world datum (`desired_floor_amsl: 36.0`, i.e. 90 m ellipsoidal in AMSL) rather than at sea level, so a mocap robot's reported global altitude agrees with sim and the GCS. `local_position.z` still equals the OptiTrack height either way ## [1.0.0] - 2024-12-19 diff --git a/docs/robot/px4_external_vision.md b/docs/robot/px4_external_vision.md index 1d9c57ca1..d69fcc336 100644 --- a/docs/robot/px4_external_vision.md +++ b/docs/robot/px4_external_vision.md @@ -214,12 +214,31 @@ floor. The cause is a **datum interaction**, not a bug in any single component: **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. +with `use_geoid_altitude: true`:** publish the origin altitude as +`N + desired_floor_amsl` instead of the literal 90.0. `N` is computed at runtime with +`GeoidEval` — no hardcoded magic number — using the same egm96‑5 model MAVROS uses, so +the undulation cancels regardless of its absolute value. + +`geographic_msgs/GeoPoint.altitude` is defined as a height above the **WGS‑84 +ellipsoid**, and MAVROS applies the geoid model itself, so `N + desired_floor_amsl` is +not a workaround — it is the correctly-expressed ellipsoidal altitude. Do **not** send +AMSL here; that would double-convert. + +### Choosing `desired_floor_amsl` + +This sets what AMSL the mocap floor (vision `z = 0`) reports. `local_position.z` equals +the OptiTrack height for **any** value — this only affects the *global* altitude the +vehicle reports. + +We use **36.0**, the shared world datum expressed in AMSL (90 m ellipsoidal − N ≈ 54 m), +so the robot's global position agrees with where sim and the GCS place the same world +origin. The published ellipsoidal origin then works out to ≈ 90 m — the datum itself. + +Setting `0.0` instead puts the mocap floor at sea level. Local flight is identical, but +the robot's reported global altitude then disagrees with sim/GCS by ~36 m. + +> Not yet confirmed on hardware — verify the reported global altitude on the next +> mocap flight. **Why this never showed in sim:** the geoid path is auto-skipped when `use_sim_time: true` (sim uses the literal 90.0 on both ends), and sim's synthetic 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 index a32087e78..0035f017b 100644 --- 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 @@ -15,6 +15,8 @@ # Real hardware: derive origin altitude from the geoid so local_position z # equals OptiTrack height. Auto-skipped in sim. use_geoid_altitude: true - desired_floor_amsl: 0.0 + # AMSL of the mocap floor. 36.0 = the shared world datum (90 m ellipsoidal) in AMSL, + # so the robot's global altitude agrees with sim and the GCS. + desired_floor_amsl: 36.0 geoid_model: "egm96-5" settle_sec: 5.0 diff --git a/robot/ros_ws/src/perception/natnet_ros2/src/mavros_gp_origin_node.py b/robot/ros_ws/src/perception/natnet_ros2/src/mavros_gp_origin_node.py index 7f94b399a..b4151d835 100755 --- 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 @@ -46,8 +46,9 @@ def __init__(self): # 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) + # AMSL (m) assigned to the mocap floor / vision z = 0. Local z equals OptiTrack z + # for any value; this only sets what global altitude the floor reports. + self.declare_parameter('desired_floor_amsl', 36.0) # Geoid model — MUST match mavros (egm96-5) for exact cancellation. self.declare_parameter('geoid_model', 'egm96-5') # Seconds to wait after MAVROS connects (listening for an existing From 2a5d89bdb1b4d9a5683492aabd13e270bc9e38fc Mon Sep 17 00:00:00 2001 From: John Date: Thu, 13 Aug 2026 16:32:52 -0400 Subject: [PATCH 10/19] fail the build when the geoid dataset is missing MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit MAVROS constructs the egm96-5 geoid in its UAS core, before any plugin loads, and throws std::invalid_argument if the dataset is absent — mavros_node terminates at startup, so there is no MAVROS at all, GPS or mocap. The image could ship without it. mavros' install_geographiclib_datasets.sh sends the downloader's output to /dev/null and, on failure, prints "Error while installing" and returns without a non-zero exit, so the RUN layer succeeded regardless. The tool it calls, geographiclib-get-geoids, was also only a transitive dependency of ros-mavros rather than something we pinned. Now pins geographiclib-tools and asserts the file landed, so a failed download fails the build. Verified against the shipped image: with the downloader broken the script still exits 0, and the new test -f returns non-zero. This is the dependency the OptiTrack external-vision path needs — mavros_gp_origin resolves the geoid undulation with the same egm96-5 model — hence landing it here. Co-Authored-By: Claude Opus 5 --- CHANGELOG.md | 1 + robot/docker/Dockerfile.robot | 10 +++++++++- 2 files changed, 10 insertions(+), 1 deletion(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 3af86c60d..ad8793b17 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -45,6 +45,7 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0 - The NatNet rigid body tracked by `robot_1` defaulted to a site-specific body (id 1146) that no emulator streams; since the client filters frames by numeric id, that produced a connected client that never published. It now defaults to the emulator's body (`Drone`, id 1). Per-robot bodies are configured in each robot's profile in `natnet_config.yaml`, selected by `ROBOT_NAME` - OptiTrack external-vision tuning corrected from real-flight bags: `EKF2_EV_DELAY` 8.0 → 7.0 and `EKF2_EVP_NOISE` 0.01 → 0.05. The old 0.01 gave a 5 cm innovation gate (`EKF2_EVP_GATE` × 5σ) that rejected valid mocap updates and blocked arming; `px4_params.yaml` now records the supporting measurements and the drift-and-snap misdiagnosis so neither is repeated - The synthetic GPS origin now places the mocap floor at the shared world datum (`desired_floor_amsl: 36.0`, i.e. 90 m ellipsoidal in AMSL) rather than at sea level, so a mocap robot's reported global altitude agrees with sim and the GCS. `local_position.z` still equals the OptiTrack height either way +- The robot image could ship without the GeographicLib `egm96-5` geoid: mavros' `install_geographiclib_datasets.sh` swallows a failed download and still exits 0, so the `RUN` layer succeeded either way, and `geographiclib-tools` was only ever a transitive dependency. MAVROS builds that geoid in its UAS core before any plugin loads and throws if it is missing, so `mavros_node` died at startup on affected images. `Dockerfile.robot` now pins the tool and asserts the file exists, failing the build instead ## [1.0.0] - 2024-12-19 diff --git a/robot/docker/Dockerfile.robot b/robot/docker/Dockerfile.robot index ad73100eb..51bc44fcc 100644 --- a/robot/docker/Dockerfile.robot +++ b/robot/docker/Dockerfile.robot @@ -361,7 +361,15 @@ RUN apt-get update && apt-get install -y --no-install-recommends \ && fc-cache -f -v \ && rm -rf /var/lib/apt/lists/* -RUN /opt/ros/${ROS_DISTRO}/lib/mavros/install_geographiclib_datasets.sh +# MAVROS constructs the egm96-5 geoid in its UAS core, before any plugin loads, and +# throws if the dataset is absent — mavros_node then dies at startup. mavros' own install +# script swallows a failed download and still exits 0, so pin the tool it needs and assert +# the file landed; otherwise the build silently produces an image whose MAVROS won't run. +RUN apt-get update \ + && apt-get install -y --no-install-recommends geographiclib-tools \ + && /opt/ros/${ROS_DISTRO}/lib/mavros/install_geographiclib_datasets.sh \ + && test -f /usr/share/GeographicLib/geoids/egm96-5.pgm \ + && rm -rf /var/lib/apt/lists/* # Install DDS Router runtime library dependencies + OpenVDB RUN apt update && apt install -y --no-install-recommends \ From 76d3f8e1b3e4d30c43b64b94e8fd09290d5c008d Mon Sep 17 00:00:00 2001 From: John Date: Fri, 14 Aug 2026 12:35:46 -0400 Subject: [PATCH 11/19] abbreviated Dockerfile comment on geographic lib installation --- robot/docker/Dockerfile.robot | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/robot/docker/Dockerfile.robot b/robot/docker/Dockerfile.robot index 51bc44fcc..13ab03102 100644 --- a/robot/docker/Dockerfile.robot +++ b/robot/docker/Dockerfile.robot @@ -361,10 +361,7 @@ RUN apt-get update && apt-get install -y --no-install-recommends \ && fc-cache -f -v \ && rm -rf /var/lib/apt/lists/* -# MAVROS constructs the egm96-5 geoid in its UAS core, before any plugin loads, and -# throws if the dataset is absent — mavros_node then dies at startup. mavros' own install -# script swallows a failed download and still exits 0, so pin the tool it needs and assert -# the file landed; otherwise the build silently produces an image whose MAVROS won't run. +# MAVROS requires geographiclib-tools to be installed for any offboard control to work. RUN apt-get update \ && apt-get install -y --no-install-recommends geographiclib-tools \ && /opt/ros/${ROS_DISTRO}/lib/mavros/install_geographiclib_datasets.sh \ From 164e37b53baab94948e41f735d7d1002682403c6 Mon Sep 17 00:00:00 2001 From: John Date: Fri, 14 Aug 2026 12:58:22 -0400 Subject: [PATCH 12/19] fix repo-root doc links in the external-vision guide They resolved relative to docs/robot/, so mkdocs looked for docs/robot/robot/ros_ws/... and warned on every one. Prefixed with ../../; the file now builds warning-free. Co-Authored-By: Claude Opus 5 --- docs/robot/px4_external_vision.md | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/docs/robot/px4_external_vision.md b/docs/robot/px4_external_vision.md index d69fcc336..7d97137e7 100644 --- a/docs/robot/px4_external_vision.md +++ b/docs/robot/px4_external_vision.md @@ -20,7 +20,7 @@ It covers three things that must all be right: 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). +[`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. @@ -81,10 +81,10 @@ before flight, the stack runs a one-shot node at startup that **checks** the liv 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) +- **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), +- **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`: @@ -111,7 +111,7 @@ Disable it entirely with `enabled: false`. 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)). +([`overrides/l4t-px4-realrobot.env`](../../overrides/l4t-px4-realrobot.env)). ### Option A — USB (`/dev/ttyACM0`) — current, but has a known stall @@ -184,14 +184,14 @@ Motive (OptiTrack, 100 Hz) **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) +[`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)) +[`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. @@ -204,7 +204,7 @@ 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 + ([`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 @@ -213,7 +213,7 @@ floor. The cause is a **datum interaction**, not a bug in any single component: `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) +**Fix — [`mavros_gp_origin_node.py`](../../robot/ros_ws/src/perception/natnet_ros2/src/mavros_gp_origin_node.py) with `use_geoid_altitude: true`:** publish the origin altitude as `N + desired_floor_amsl` instead of the literal 90.0. `N` is computed at runtime with `GeoidEval` — no hardcoded magic number — using the same egm96‑5 model MAVROS uses, so From 536bf21775ab48cf96c3d1b1a0440e3ae62224c2 Mon Sep 17 00:00:00 2001 From: John Date: Fri, 14 Aug 2026 13:02:54 -0400 Subject: [PATCH 13/19] comment trim --- .../natnet_ros2/include/natnet_ros2/natnet_logic.hpp | 7 +------ 1 file changed, 1 insertion(+), 6 deletions(-) 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 115a2fc98..c26461b9c 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 @@ -196,12 +196,8 @@ 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; }; @@ -227,8 +223,7 @@ inline bool should_publish_body(int32_t filter_id, int32_t rb_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). +/// The node publishes only a fixed set of ids based on natnet_config.yaml. 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) From 337bcb71cac987e371c9511a64b80982535ef085 Mon Sep 17 00:00:00 2001 From: John Date: Fri, 14 Aug 2026 16:50:49 -0400 Subject: [PATCH 14/19] point the companion-link section at the PX4 docs MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Section 3 documented MAVLink serial setup at length — MAV_n_CONFIG / SER_TEL2_BAUD tables, wiring, USB-vs-TELEM2 comparison — all of which is standard PX4 setup that PX4 documents better and keeps current. Replaced with links to the companion computer, MAVLink peripherals, and serial configuration pages. Kept the part PX4 does not cover: the Cube Orange USB CDC-ACM stall, which starves EKF2 of vision updates and is why the companion link belongs on TELEM2. Four other sections and the troubleshooting table point here for that symptom. 65 lines -> 19. Co-Authored-By: Claude Opus 5 --- docs/robot/px4_external_vision.md | 72 ++++++------------------------- 1 file changed, 13 insertions(+), 59 deletions(-) diff --git a/docs/robot/px4_external_vision.md b/docs/robot/px4_external_vision.md index 7d97137e7..9b90ed041 100644 --- a/docs/robot/px4_external_vision.md +++ b/docs/robot/px4_external_vision.md @@ -7,7 +7,7 @@ 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). +2. **Companion MAVLink link** — how the Jetson talks to the Cube (see the PX4 docs). 3. **Vision pose pipeline** — how a mocap pose becomes a `VISION_POSITION_ESTIMATE`. > Scope: PX4 ≥ 1.14 (the `EKF2_EV_CTRL` / `EKF2_GPS_CTRL` era). For older @@ -109,68 +109,22 @@ Disable it entirely with `enabled: false`. ## 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)). +`mavros` reaches the FCU over the serial link named by `FCU_URL` in the deployment env. +Configuring that link is standard PX4 setup, not AirStack-specific — see the PX4 docs: -### Option A — USB (`/dev/ttyACM0`) — current, but has a known stall +- [Companion computer setup](https://docs.px4.io/main/en/companion_computer/) +- [MAVLink peripherals (`MAV_n_CONFIG`, `MAV_n_MODE`)](https://docs.px4.io/main/en/peripherals/mavlink_peripherals.html) +- [Serial port configuration](https://docs.px4.io/main/en/peripherals/serial_configuration.html) -``` -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. +Use the **TELEM2 UART** for the companion link rather than USB. On Cube Orange the USB +CDC-ACM path intermittently stalls outbound transfers for 10–30 s at a time — visible as +`DROPPED Message-Id 102 … TX queue overflow` — which starves EKF2 of vision updates and +makes it dead-reckon between bursts. It is not a bandwidth problem and rate-limiting the +vision stream does not help. -**PX4 params (set once in QGC, then reboot):** +> In compose list-syntax `environment:`, values are literal — write `FCU_URL=/dev/ttyTHS1:115200` +> bare. Quoting it passes the quotes through and breaks MAVROS URL parsing. -| 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) From 2e365fed5c9eea36c5c1782227cde13f2c15680d Mon Sep 17 00:00:00 2001 From: John Date: Fri, 14 Aug 2026 17:21:10 -0400 Subject: [PATCH 15/19] frame section 4 around mavros_gp_origin, demote the 36 m note MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Section 4 now leads with what mavros_gp_origin does — inject a synthetic global position so PX4 will arm in modes that need one without GNSS — rather than presenting the height datum as a peer topic. The ~36 m offset becomes a note under it, scoped to real deployments and ending with why sim never sees it (the geoid path is skipped under use_sim_time, and sim's synthetic GPS is self-consistent with the spawn). Section 4b is gone; it had no inbound references. Dropped the "don't change the 90.0 globally" warning. Co-Authored-By: Claude Opus 5 --- docs/robot/px4_external_vision.md | 77 ++++++++++++------------------- 1 file changed, 30 insertions(+), 47 deletions(-) diff --git a/docs/robot/px4_external_vision.md b/docs/robot/px4_external_vision.md index 9b90ed041..55c4c48cd 100644 --- a/docs/robot/px4_external_vision.md +++ b/docs/robot/px4_external_vision.md @@ -8,7 +8,8 @@ It covers three things that must all be right: 1. **EKF2 parameters** — tell PX4 to fuse external vision instead of GPS/baro/mag. 2. **Companion MAVLink link** — how the Jetson talks to the Cube (see the PX4 docs). -3. **Vision pose pipeline** — how a mocap pose becomes a `VISION_POSITION_ESTIMATE`. +3. **Vision pose pipeline** — how a mocap pose becomes a `VISION_POSITION_ESTIMATE`, + and how PX4 gets a global position without GNSS. > Scope: PX4 ≥ 1.14 (the `EKF2_EV_CTRL` / `EKF2_GPS_CTRL` era). For older > firmware use `EKF2_AID_MASK: 24` and `EKF2_HGT_MODE: 3` instead of the bitmask @@ -150,60 +151,42 @@ 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`. +### Injecting a global position — `mavros_gp_origin` -**Fix — [`mavros_gp_origin_node.py`](../../robot/ros_ws/src/perception/natnet_ros2/src/mavros_gp_origin_node.py) -with `use_geoid_altitude: true`:** publish the origin altitude as -`N + desired_floor_amsl` instead of the literal 90.0. `N` is computed at runtime with -`GeoidEval` — no hardcoded magic number — using the same egm96‑5 model MAVROS uses, so -the undulation cancels regardless of its absolute value. +Vision gives PX4 a valid *local* position, but with GNSS disabled it has no *global* +one, and modes that require a global position (e.g. `AUTO.LOITER`) refuse to arm. -`geographic_msgs/GeoPoint.altitude` is defined as a height above the **WGS‑84 -ellipsoid**, and MAVROS applies the geoid model itself, so `N + desired_floor_amsl` is -not a workaround — it is the correctly-expressed ellipsoidal altitude. Do **not** send -AMSL here; that would double-convert. +[`mavros_gp_origin_node.py`](../../robot/ros_ws/src/perception/natnet_ros2/src/mavros_gp_origin_node.py) +publishes a **synthetic GPS origin** once at startup, which lets PX4 derive a global +position from the fused vision estimate. It waits for MAVROS to connect, listens for an +existing origin, and only publishes if none is present — so a GNSS-equipped vehicle is +left untouched. Location and behaviour come from +[`mavros_gp_origin.yaml`](../../robot/ros_ws/src/perception/natnet_ros2/config/mavros_gp_origin.yaml); +the defaults match the AirStack shared world datum so sim, the GCS, and the robot agree +on where world origin sits on Earth. -### Choosing `desired_floor_amsl` +!!! note "Real deployments: the origin altitude needs a geoid correction" -This sets what AMSL the mocap floor (vision `z = 0`) reports. `local_position.z` equals -the OptiTrack height for **any** value — this only affects the *global* altitude the -vehicle reports. + `geographic_msgs/GeoPoint.altitude` is a height above the **WGS-84 ellipsoid**, and + MAVROS converts it to AMSL with the **egm96-5 geoid** before handing it to PX4. Send + the shared datum's literal `90.0` and PX4 anchors its vertical frame at + `AMSL = 90 − N ≈ 36 m`, while OptiTrack says the floor is `z = 0` — the drone reads + ~36 m of altitude sitting on the floor. The gap is exactly the geoid undulation `N`. -We use **36.0**, the shared world datum expressed in AMSL (90 m ellipsoidal − N ≈ 54 m), -so the robot's global position agrees with where sim and the GCS place the same world -origin. The published ellipsoidal origin then works out to ≈ 90 m — the datum itself. + With `use_geoid_altitude: true` the node publishes `N + desired_floor_amsl` instead, + computing `N` at runtime via `GeoidEval` with the same egm96-5 model MAVROS uses, so + the conversion cancels exactly. This is not a workaround — it is the correctly + expressed *ellipsoidal* altitude. Do **not** send AMSL here; it would double-convert. -Setting `0.0` instead puts the mocap floor at sea level. Local flight is identical, but -the robot's reported global altitude then disagrees with sim/GCS by ~36 m. + `desired_floor_amsl` chooses what AMSL the mocap floor reports; `local_position.z` + equals the OptiTrack height either way. We use **36.0**, the shared datum in AMSL, so + the robot's global altitude agrees with sim and the GCS. *Not yet confirmed on + hardware — verify the reported global altitude on the next mocap flight.* -> Not yet confirmed on hardware — verify the reported global altitude on the next -> mocap flight. + **Not needed in sim.** The geoid path is skipped when `use_sim_time: true`: sim's + synthetic GPS is self-consistent with the spawn and uses the literal datum altitude + on both ends, so there is no ellipsoidal-vs-AMSL mismatch to correct. -**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 From de383c78703dc8e9e2f48db3002392ff8ddbfd6c Mon Sep 17 00:00:00 2001 From: John Date: Fri, 14 Aug 2026 17:29:06 -0400 Subject: [PATCH 16/19] reject an unknown connection_type instead of defaulting to unicast MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit validate_connection_type returned "unicast" for anything it did not recognise, so "mutlicast" or "Unicast" produced a client that connected on the wrong transport and then never received a frame — with only a warning to show for it. It now throws std::invalid_argument naming the offending value, and the node turns that into a fatal startup error rather than a warning it flies past. Case-sensitivity is deliberate: accepting "Unicast" would mean the config silently disagrees with itself. Tests updated from fallback to throw, plus one asserting the message names the bad value. 60 gtests pass. Co-Authored-By: Claude Opus 5 --- CHANGELOG.md | 1 + .../include/natnet_ros2/natnet_logic.hpp | 13 ++++--- .../natnet_ros2/src/natnet_ros2_node.cpp | 29 ++++++++-------- .../natnet_ros2/test/test_natnet_logic.cpp | 34 ++++++++++++------- 4 files changed, 47 insertions(+), 30 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index ad8793b17..0d12c4108 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -46,6 +46,7 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0 - OptiTrack external-vision tuning corrected from real-flight bags: `EKF2_EV_DELAY` 8.0 → 7.0 and `EKF2_EVP_NOISE` 0.01 → 0.05. The old 0.01 gave a 5 cm innovation gate (`EKF2_EVP_GATE` × 5σ) that rejected valid mocap updates and blocked arming; `px4_params.yaml` now records the supporting measurements and the drift-and-snap misdiagnosis so neither is repeated - The synthetic GPS origin now places the mocap floor at the shared world datum (`desired_floor_amsl: 36.0`, i.e. 90 m ellipsoidal in AMSL) rather than at sea level, so a mocap robot's reported global altitude agrees with sim and the GCS. `local_position.z` still equals the OptiTrack height either way - The robot image could ship without the GeographicLib `egm96-5` geoid: mavros' `install_geographiclib_datasets.sh` swallows a failed download and still exits 0, so the `RUN` layer succeeded either way, and `geographiclib-tools` was only ever a transitive dependency. MAVROS builds that geoid in its UAS core before any plugin loads and throws if it is missing, so `mavros_node` died at startup on affected images. `Dockerfile.robot` now pins the tool and asserts the file exists, failing the build instead +- An unrecognised `connection_type` in `natnet_config.yaml` silently fell back to `unicast`, so a typo produced a client that connected on the wrong transport and never received frames. `validate_connection_type` now throws and `natnet_ros2_node` fails at startup naming the offending value ## [1.0.0] - 2024-12-19 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 c26461b9c..f2c93b838 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 @@ -28,6 +28,7 @@ #include #include #include +#include #include #include @@ -115,11 +116,15 @@ inline std::string body_topic_base( // 3. Connection-configuration helpers // =========================================================================== -/// Return ct if it is "unicast" or "multicast"; otherwise return "unicast". +/// Return ct if it is "unicast" or "multicast"; otherwise throw. +/// +/// Deliberately strict: silently falling back to "unicast" turns a typo into a +/// vehicle that connects to the wrong transport and never receives frames. inline std::string validate_connection_type(const std::string & ct) { if (ct == "unicast" || ct == "multicast") { return ct; } - return "unicast"; + throw std::invalid_argument( + "connection_type must be \"unicast\" or \"multicast\", got \"" + ct + "\""); } /// SDK-independent connection configuration aggregate. @@ -132,12 +137,12 @@ struct ConnectConfig std::string client_ip = "0.0.0.0"; uint16_t command_port = 1510u; uint16_t data_port = 1511u; - std::string connection_type = "unicast"; ///< validated + std::string connection_type = "unicast"; ///< "unicast" or "multicast" std::string multicast_address = "239.255.42.99"; }; /// Build a validated ConnectConfig from raw user-supplied strings. -/// connection_type is normalised via validate_connection_type(). +/// Throws std::invalid_argument when connection_type is not "unicast"/"multicast". inline ConnectConfig make_connect_config( const std::string & server_ip, const std::string & client_ip, 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 2245055d4..d52baaf59 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 @@ -8,6 +8,7 @@ #include #include #include +#include #include #include #include @@ -127,20 +128,20 @@ class NatNetROS2Node : public rclcpp::Node this->declare_parameter("body_orientation_covariance", std::vector{}); // ----- Read parameters --------------------------------------------- - const auto connect_cfg = natnet_ros2::make_connect_config( - this->get_parameter("server_ip").as_string(), - this->get_parameter("client_ip").as_string(), - static_cast(this->get_parameter("command_port").as_int()), - static_cast(this->get_parameter("data_port").as_int()), - this->get_parameter("connection_type").as_string(), - this->get_parameter("multicast_address").as_string()); - - if (connect_cfg.connection_type != - this->get_parameter("connection_type").as_string()) - { - RCLCPP_WARN(get_logger(), - "Unknown connection_type '%s' — falling back to 'unicast'.", - this->get_parameter("connection_type").as_string().c_str()); + // A bad connection_type is fatal rather than defaulted: silently using + // unicast would look healthy while never receiving a frame. + natnet_ros2::ConnectConfig connect_cfg; + try { + connect_cfg = natnet_ros2::make_connect_config( + this->get_parameter("server_ip").as_string(), + this->get_parameter("client_ip").as_string(), + static_cast(this->get_parameter("command_port").as_int()), + static_cast(this->get_parameter("data_port").as_int()), + this->get_parameter("connection_type").as_string(), + this->get_parameter("multicast_address").as_string()); + } catch (const std::invalid_argument & e) { + RCLCPP_FATAL(get_logger(), "Invalid natnet configuration: %s", e.what()); + throw; } frame_id_ = this->get_parameter("frame_id").as_string(); 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 5796c121b..f469c2a29 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 @@ -191,17 +191,28 @@ TEST(ValidateConnectionType, MulticastPassesThrough) EXPECT_EQ(validate_connection_type("multicast"), "multicast"); } -TEST(ValidateConnectionType, UnknownFallsBackToUnicast) +TEST(ValidateConnectionType, UnknownThrows) { - EXPECT_EQ(validate_connection_type("broadcast"), "unicast"); - EXPECT_EQ(validate_connection_type(""), "unicast"); - EXPECT_EQ(validate_connection_type("UDP"), "unicast"); + EXPECT_THROW(validate_connection_type("broadcast"), std::invalid_argument); + EXPECT_THROW(validate_connection_type(""), std::invalid_argument); + EXPECT_THROW(validate_connection_type("UDP"), std::invalid_argument); } -TEST(ValidateConnectionType, CaseSensitiveFallsBack) +TEST(ValidateConnectionType, CaseSensitiveThrows) { - EXPECT_EQ(validate_connection_type("Unicast"), "unicast"); - EXPECT_EQ(validate_connection_type("MULTICAST"), "unicast"); + // Accepting "Unicast" would mean the config silently disagrees with itself. + EXPECT_THROW(validate_connection_type("Unicast"), std::invalid_argument); + EXPECT_THROW(validate_connection_type("MULTICAST"), std::invalid_argument); +} + +TEST(ValidateConnectionType, MessageNamesTheOffendingValue) +{ + try { + validate_connection_type("broadcst"); + FAIL() << "expected std::invalid_argument"; + } catch (const std::invalid_argument & e) { + EXPECT_NE(std::string(e.what()).find("broadcst"), std::string::npos); + } } @@ -233,12 +244,11 @@ TEST(ConnectConfig, MulticastConfigIsMulticast) EXPECT_EQ(cfg.multicast_address, "239.255.42.99"); } -TEST(ConnectConfig, InvalidConnectionTypeFallsBackToUnicast) +TEST(ConnectConfig, InvalidConnectionTypeThrows) { - const auto cfg = make_connect_config( - "10.0.0.1", "0.0.0.0", 1510, 1511, "broadcast"); - EXPECT_EQ(cfg.connection_type, "unicast"); - EXPECT_FALSE(is_multicast(cfg)); + EXPECT_THROW( + make_connect_config("10.0.0.1", "0.0.0.0", 1510, 1511, "broadcast"), + std::invalid_argument); } TEST(ConnectConfig, PortsArePreserved) From 50eb6a5e71132dfb626c8d8dbe4cfa852df71c0f Mon Sep 17 00:00:00 2001 From: John Date: Fri, 14 Aug 2026 17:25:47 -0400 Subject: [PATCH 17/19] px4 external vision docs trim --- docs/robot/px4_external_vision.md | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/docs/robot/px4_external_vision.md b/docs/robot/px4_external_vision.md index 55c4c48cd..dab88c5d0 100644 --- a/docs/robot/px4_external_vision.md +++ b/docs/robot/px4_external_vision.md @@ -175,13 +175,11 @@ on where world origin sits on Earth. With `use_geoid_altitude: true` the node publishes `N + desired_floor_amsl` instead, computing `N` at runtime via `GeoidEval` with the same egm96-5 model MAVROS uses, so - the conversion cancels exactly. This is not a workaround — it is the correctly - expressed *ellipsoidal* altitude. Do **not** send AMSL here; it would double-convert. + the conversion cancels out. `desired_floor_amsl` chooses what AMSL the mocap floor reports; `local_position.z` equals the OptiTrack height either way. We use **36.0**, the shared datum in AMSL, so - the robot's global altitude agrees with sim and the GCS. *Not yet confirmed on - hardware — verify the reported global altitude on the next mocap flight.* + the robot's global altitude agrees with sim and the GCS. **Not needed in sim.** The geoid path is skipped when `use_sim_time: true`: sim's synthetic GPS is self-consistent with the spawn and uses the literal datum altitude From b3a03a05f3b47d22f6ad279de736dcf39b3188c5 Mon Sep 17 00:00:00 2001 From: John Date: Fri, 14 Aug 2026 17:44:49 -0400 Subject: [PATCH 18/19] trim natnet node comments; note the latency figure is an estimate MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Comment trims in natnet_ros2_node.cpp (no code change). Records what cube_orange_latency_ms actually is: an estimate of the FCU hop, added to a logged total and never fused. Only the transport half of EKF2_EV_DELAY is measured, and that measurement starts at the NatNet server transmit, so Motive's own capture pipeline is not in it either. Also notes, for whoever retunes next, that the node stamps poses with its receive time — so delay after that stamp does not belong in EKF2_EV_DELAY, which points lower than 7.0 and matches the negative best-fit shift already recorded. Not chased down; 7.0 flies. CameraMidExposureTimestamp would replace the estimate with a measurement if it ever matters. Co-Authored-By: Claude Opus 5 --- docs/robot/px4_external_vision.md | 26 ++++++++++++++++++- .../natnet_ros2/src/natnet_ros2_node.cpp | 17 +++++------- 2 files changed, 32 insertions(+), 11 deletions(-) diff --git a/docs/robot/px4_external_vision.md b/docs/robot/px4_external_vision.md index dab88c5d0..f0c44f471 100644 --- a/docs/robot/px4_external_vision.md +++ b/docs/robot/px4_external_vision.md @@ -32,7 +32,7 @@ either way it's a one-time thing per airframe. | `EKF2_GPS_CTRL` | `0` | No GPS fusion. | | `EKF2_MAG_TYPE` | `5` | Magnetometer disabled — yaw comes from vision. | | `EKF2_BARO_CTRL` | `0` | No baro fusion; height is pure vision. Set to `1` to keep baro as a backup height source. | -| `EKF2_EV_DELAY` | `7.0` | Measured OptiTrack→EKF2 latency (ms): ~0.7 ms LAN + ~5 ms Cube hop. `natnet_ros2_node` logs the measured figure. **Do not raise this to chase apparent lag — see below.** | +| `EKF2_EV_DELAY` | `7.0` | OptiTrack→EKF2 latency (ms): ~0.7 ms measured LAN transport + a ~5 ms *estimated* FCU hop. **Do not raise this to chase apparent lag — see below.** | | `EKF2_EV_NOISE_MD` | `1` | Use the `EKF2_EV*_NOISE` floors below instead of the message covariance (which is `1e-6` — too optimistic to fuse safely). | | `EKF2_EVP_NOISE` | `0.05` | Vision **position** noise floor (m). Not marker precision — it also sets the innovation gate, `EKF2_EVP_GATE` (default 5) sigma wide, so this is a 25 cm gate. | | `EKF2_EVA_NOISE` | `0.05` | Vision **angle** noise floor (rad). | @@ -66,6 +66,30 @@ yaw compensation in code — `natnet_ros2` and `vision_pose_converter` are delib identity pass-throughs, and a code-side correction would double-compensate once Motive is fixed. +### The latency figure is only partly measured + +`EKF2_EV_DELAY` is currently `7.0` ms: roughly `0.7` measured plus a `5.0` estimate +(`cube_orange_latency_ms` in `natnet_config.yaml`). Only the first part is real. + +- **Measured:** `natnet_ros2_node` derives transport latency from the NatNet + `TransmitTimestamp` — i.e. from *server transmit* to client receipt. It does not + include Motive's own capture→transmit pipeline (exposure, centroiding, solving), + which is typically several ms and happens before that clock starts. +- **Estimated:** `cube_orange_latency_ms` models the MAVROS → MAVLink → uORB → EKF2 hop. + It is **diagnostic only** — it is added to a logged "estimated total" and is never + fused. It is also baud-dependent: ~5 ms is about the serialisation time of one + `VISION_POSITION_ESTIMATE` at 115200, and far less at 921600. + +Worth knowing before anyone retunes: `natnet_ros2_node` stamps poses with its own +receive time, not the mocap capture time, so strictly only the delay incurred *before* +that stamp belongs in `EKF2_EV_DELAY` — the FCU hop happens after it and EKF2 already +absorbs late arrival through its measurement buffer. That suggests the true value is +lower than 7.0, which is consistent with the still-negative best-fit shift above. + +**This has not been chased down and 7.0 flies.** If it ever matters, the clean fix is to +measure rather than model: NatNet 3.0+ frames carry `CameraMidExposureTimestamp`, which +would give true capture→arrival latency directly and remove the estimate entirely. + The wider `EKF2_EVP_NOISE` (0.05) is still the right value on its own merits: the previous 0.01 gave only a 5 cm gate, tight enough to reject legitimate updates and refuse to arm. 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 d52baaf59..58b7c2fda 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 @@ -108,14 +108,13 @@ class NatNetROS2Node : public rclcpp::Node 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. + // Latency sampling: warmup + window for mean/stdev, then log a summary of measured latency. 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. + // Suggested latency for the Cube Orange (PX4) from the OptiTrack Motive model. + // This is added to the measured transport latency to estimate total latency to PX4. + // NOTE: an estimate, not a measurement — see docs/robot/px4_external_vision.md. + // Diagnostic only; it is logged, never fused. this->declare_parameter("cube_orange_latency_ms", 5.0); // Parallel per-body arrays (flattened from natnet_config.yaml by the launch file). @@ -128,8 +127,7 @@ class NatNetROS2Node : public rclcpp::Node this->declare_parameter("body_orientation_covariance", std::vector{}); // ----- Read parameters --------------------------------------------- - // A bad connection_type is fatal rather than defaulted: silently using - // unicast would look healthy while never receiving a frame. + // Fatally fail if the config is invalid (e.g. unknown connection_type). natnet_ros2::ConnectConfig connect_cfg; try { connect_cfg = natnet_ros2::make_connect_config( @@ -251,8 +249,7 @@ class NatNetROS2Node : public rclcpp::Node // 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. + // Wire-level probe first 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.", From 8fbb1c04342940f4b113738d7e9371eea2628a2d Mon Sep 17 00:00:00 2001 From: John Date: Fri, 14 Aug 2026 17:53:45 -0400 Subject: [PATCH 19/19] trim the external-vision tuning notes MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Replaces the two long tuning write-ups with a short troubleshooting tip (check the Motive rigid-body definition first — x forward, z up) and cuts the latency section back to what is measured versus estimated. Fixed a dangling "see below" in the EKF2_EV_DELAY table row, which pointed at the removed tuning result; the warning it carried is now stated inline. Co-Authored-By: Claude Opus 5 --- docs/robot/px4_external_vision.md | 45 ++++--------------------------- 1 file changed, 5 insertions(+), 40 deletions(-) diff --git a/docs/robot/px4_external_vision.md b/docs/robot/px4_external_vision.md index f0c44f471..4c7be444b 100644 --- a/docs/robot/px4_external_vision.md +++ b/docs/robot/px4_external_vision.md @@ -32,7 +32,7 @@ either way it's a one-time thing per airframe. | `EKF2_GPS_CTRL` | `0` | No GPS fusion. | | `EKF2_MAG_TYPE` | `5` | Magnetometer disabled — yaw comes from vision. | | `EKF2_BARO_CTRL` | `0` | No baro fusion; height is pure vision. Set to `1` to keep baro as a backup height source. | -| `EKF2_EV_DELAY` | `7.0` | OptiTrack→EKF2 latency (ms): ~0.7 ms measured LAN transport + a ~5 ms *estimated* FCU hop. **Do not raise this to chase apparent lag — see below.** | +| `EKF2_EV_DELAY` | `7.0` | OptiTrack→EKF2 latency (ms): ~0.7 ms measured LAN transport + a ~5 ms *estimated* FCU hop. **Raising this does not compensate for apparent lag — it makes the estimate run ahead of truth.** | | `EKF2_EV_NOISE_MD` | `1` | Use the `EKF2_EV*_NOISE` floors below instead of the message covariance (which is `1e-6` — too optimistic to fuse safely). | | `EKF2_EVP_NOISE` | `0.05` | Vision **position** noise floor (m). Not marker precision — it also sets the innovation gate, `EKF2_EVP_GATE` (default 5) sigma wide, so this is a 25 cm gate. | | `EKF2_EVA_NOISE` | `0.05` | Vision **angle** noise floor (rad). | @@ -42,56 +42,21 @@ either way it's a one-time thing per airframe. (`7.0`) so the MAVLink param type matches the FCU's declaration. Getting this wrong makes the set silently reject. -### Two tuning results worth not rediscovering +### Troubleshooting tips -Both came out of back-to-back flight bags with everything else held constant. - -**Raising `EKF2_EV_DELAY` makes tracking worse, not better.** 50.0 was trialled against -7.0: median |odom − mocap| while moving went 0.037 → 0.060 m, and X-axis RMS 0.023 → -0.049 m. The tell is the *negative* best-fit time shift (−20 ms → −60 ms, pinned at the -sweep edge): over-declaring the delay makes EKF2 attribute the measurement to a state -that is too old, so the estimate runs **ahead** of truth during motion rather than -behind. Raising this value can never compensate for apparent lag in RViz — it does the -opposite. - -**Large drift-and-snap excursions are a Motive problem, not a gate problem.** They were -first blamed on `EKF2_EVP_NOISE` being too tight. That was wrong. The cause was a 90° -body-yaw offset in the Motive rigid-body definition: EKF2 fuses vision yaw and snaps its -heading to it, so its nav frame was 90° off and IMU-predicted motion fought the (correct) -vision position. Fixing the rigid body in Motive cut moving error 0.25 → 0.04 m and the -odom/mocap path-length ratio 2.33× → 1.12×. - -If you see drift-and-snap, **check the Motive rigid-body definition first**. Do not add -yaw compensation in code — `natnet_ros2` and `vision_pose_converter` are deliberate -identity pass-throughs, and a code-side correction would double-compensate once Motive is -fixed. +If you see drift-and-snap, **check the Motive PC rigid-body definition first** and ensure the x axis points forward. Then, make sure that Motive is streaming the position with z-axis up. ### The latency figure is only partly measured `EKF2_EV_DELAY` is currently `7.0` ms: roughly `0.7` measured plus a `5.0` estimate -(`cube_orange_latency_ms` in `natnet_config.yaml`). Only the first part is real. +(`cube_orange_latency_ms` in `natnet_config.yaml`). Only the first part is empirically measured currently. - **Measured:** `natnet_ros2_node` derives transport latency from the NatNet `TransmitTimestamp` — i.e. from *server transmit* to client receipt. It does not include Motive's own capture→transmit pipeline (exposure, centroiding, solving), which is typically several ms and happens before that clock starts. - **Estimated:** `cube_orange_latency_ms` models the MAVROS → MAVLink → uORB → EKF2 hop. - It is **diagnostic only** — it is added to a logged "estimated total" and is never - fused. It is also baud-dependent: ~5 ms is about the serialisation time of one - `VISION_POSITION_ESTIMATE` at 115200, and far less at 921600. - -Worth knowing before anyone retunes: `natnet_ros2_node` stamps poses with its own -receive time, not the mocap capture time, so strictly only the delay incurred *before* -that stamp belongs in `EKF2_EV_DELAY` — the FCU hop happens after it and EKF2 already -absorbs late arrival through its measurement buffer. That suggests the true value is -lower than 7.0, which is consistent with the still-negative best-fit shift above. - -**This has not been chased down and 7.0 flies.** If it ever matters, the clean fix is to -measure rather than model: NatNet 3.0+ frames carry `CameraMidExposureTimestamp`, which -would give true capture→arrival latency directly and remove the estimate entirely. - -The wider `EKF2_EVP_NOISE` (0.05) is still the right value on its own merits: the previous -0.01 gave only a 5 cm gate, tight enough to reject legitimate updates and refuse to arm. + It is **estimated only**. **Reboot after any change.** Fusion-source (`EKF2_*`) params are safest applied from a clean estimator start — reboot the flight controller before flying. The