From df38ac6702c47bbbc6ceafb4ec81094005b82a37 Mon Sep 17 00:00:00 2001 From: John Date: Thu, 23 Jul 2026 00:13:46 -0400 Subject: [PATCH 01/33] 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 05e44a235f3cc52e60713803c61a0e337641e175 Mon Sep 17 00:00:00 2001 From: John Date: Thu, 23 Jul 2026 00:17:57 -0400 Subject: [PATCH 02/33] =?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 170f714269039e5957e5551d645d52d86063969e Mon Sep 17 00:00:00 2001 From: John Date: Thu, 23 Jul 2026 00:34:45 -0400 Subject: [PATCH 03/33] 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 a36d4c75f..ba91013fa 100644 --- a/mkdocs.yml +++ b/mkdocs.yml @@ -117,6 +117,7 @@ nav: - Perception: - docs/robot/autonomy/perception/index.md - NatNet (OptiTrack): robot/ros_ws/src/perception/natnet_ros2/README.md + - PX4 External Vision (mocap): docs/robot/px4_external_vision.md - Local: - docs/robot/autonomy/local/index.md - World Model: From 44ecdc6fc82d8a4b65ea7e1e1c5ede55b817cef7 Mon Sep 17 00:00:00 2001 From: John Date: Thu, 23 Jul 2026 14:19:33 -0400 Subject: [PATCH 04/33] 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 6b0eb12f3bc7e36316847abc82f223273be0b964 Mon Sep 17 00:00:00 2001 From: John Date: Thu, 23 Jul 2026 14:19:33 -0400 Subject: [PATCH 05/33] chore: bump version to 0.19.0-alpha.13 --- .env | 2 +- CHANGELOG.md | 1 + 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/.env b/.env index 04de90a2d..4aa2502f1 100644 --- a/.env +++ b/.env @@ -12,7 +12,7 @@ PROJECT_NAME="airstack" # If you've run ./airstack.sh setup, then this will auto-generate from the git commit hash every time a change is made # to a Dockerfile or docker-compose.yaml file. Otherwise this can also be set explicitly to make a release version. # auto-generated from git commit hash -VERSION="0.19.0-alpha.12" +VERSION="0.19.0-alpha.13" # Choose "dev" or "prebuilt". "dev" is for mounted code that must be built live. "prebuilt" is for built ros_ws baked into the image DOCKER_IMAGE_BUILD_MODE="dev" # Where to push and pull images from. Can replace with your docker hub username if using docker hub. diff --git a/CHANGELOG.md b/CHANGELOG.md index 68f37e9b0..809081cc6 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 2071bdbad7d0d4bab82ae19b92a4ed5de1845984 Mon Sep 17 00:00:00 2001 From: John Date: Fri, 31 Jul 2026 18:37:14 -0400 Subject: [PATCH 06/33] 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 809081cc6..b909800e5 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -34,6 +34,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 a1287caa92f639b2d512f887a312fab4f679bc40 Mon Sep 17 00:00:00 2001 From: John Date: Thu, 13 Aug 2026 09:25:19 -0400 Subject: [PATCH 07/33] 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 b909800e5..5735a85c9 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 2c467efd05006d400c6e7be9c05ca5a60a4b9804 Mon Sep 17 00:00:00 2001 From: John Date: Thu, 13 Aug 2026 11:53:41 -0400 Subject: [PATCH 08/33] 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 5735a85c9..ff8475051 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -35,8 +35,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 2047a1f1d6afbb5b9583a4f0d3b90ad6e0baa490 Mon Sep 17 00:00:00 2001 From: John Date: Thu, 13 Aug 2026 13:11:09 -0400 Subject: [PATCH 09/33] 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 ff8475051..3a41b0f4c 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -38,6 +38,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 8156ea03a8cf1ab4bcf61e34ebd9ab0cd7d670e6 Mon Sep 17 00:00:00 2001 From: John Date: Thu, 13 Aug 2026 16:32:52 -0400 Subject: [PATCH 10/33] 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 3a41b0f4c..dc21663f0 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -39,6 +39,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 3ff968750..d4af44beb 100644 --- a/robot/docker/Dockerfile.robot +++ b/robot/docker/Dockerfile.robot @@ -355,7 +355,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 691fee0c2492998f79d35169fbeb054b7681c0cd Mon Sep 17 00:00:00 2001 From: John Date: Fri, 14 Aug 2026 12:35:46 -0400 Subject: [PATCH 11/33] 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 d4af44beb..bfa06c1b8 100644 --- a/robot/docker/Dockerfile.robot +++ b/robot/docker/Dockerfile.robot @@ -355,10 +355,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 5fc20f3deb441874494ec104f1229ec9c9444121 Mon Sep 17 00:00:00 2001 From: John Date: Fri, 14 Aug 2026 12:58:22 -0400 Subject: [PATCH 12/33] 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 a8db780b5750a3d21eefc74ffc9c758bc176c644 Mon Sep 17 00:00:00 2001 From: John Date: Fri, 14 Aug 2026 13:02:54 -0400 Subject: [PATCH 13/33] 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 b1f9372543fc2786c9edbc5ef7f3d18a0420bb45 Mon Sep 17 00:00:00 2001 From: John Date: Fri, 14 Aug 2026 16:50:49 -0400 Subject: [PATCH 14/33] 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 5ba5899c103b088585efa13206c79dc1210f18ca Mon Sep 17 00:00:00 2001 From: John Date: Fri, 14 Aug 2026 17:21:10 -0400 Subject: [PATCH 15/33] 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 5dbd68c51b3580364ada6ee642de71a323f5ed23 Mon Sep 17 00:00:00 2001 From: John Date: Fri, 14 Aug 2026 17:29:06 -0400 Subject: [PATCH 16/33] 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 dc21663f0..0e373509d 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -40,6 +40,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 0264cb2e3920c81c275c34d43d5f011bbd278889 Mon Sep 17 00:00:00 2001 From: John Date: Fri, 14 Aug 2026 17:25:47 -0400 Subject: [PATCH 17/33] 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 6528934b3f37bc4d6c9a7f8db1c5ba7638c96c21 Mon Sep 17 00:00:00 2001 From: John Date: Fri, 14 Aug 2026 17:44:49 -0400 Subject: [PATCH 18/33] 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 0fcd299236dc2fb0b5257c0f82599f9019c4661f Mon Sep 17 00:00:00 2001 From: John Date: Fri, 14 Aug 2026 17:53:45 -0400 Subject: [PATCH 19/33] 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 From 8610b86d027f1572dedfe30789f801ec7e810782 Mon Sep 17 00:00:00 2001 From: John Date: Thu, 23 Jul 2026 14:15:14 -0400 Subject: [PATCH 20/33] feat(sim): add NatNet server emulator (protocol core) + register unit tests MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The pure-Python NatNet server that emulates an OptiTrack Motive server so natnet_ros2 can be driven without hardware. USD/Isaac-free — this is the protocol + server core (unicast server, data/model/server types, serializers, default catalogs). The Isaac wrapper that maps a USD scene onto this server lands next. Registers the emulator package's co-located unit tests via a `sim:` entry in tests/colcon_unit_test_packages.yaml (base's simulation/**//test glob). The root conftest now puts each unit-test package's import root on sys.path so co-located tests import their package without a per-package conftest.py. Co-Authored-By: Claude Opus 4.8 --- .../optitrack.natnet.emulator/.gitignore | 9 + .../optitrack.natnet.emulator/README.md | 160 ++++++++ .../optitrack/__init__.py | 1 + .../optitrack/natnet/__init__.py | 1 + .../optitrack/natnet/emulator/__init__.py | 20 + .../optitrack/natnet/emulator/defaults.py | 25 ++ .../natnet/emulator/server/__init__.py | 11 + .../natnet/emulator/server/natnet_common.py | 27 ++ .../emulator/server/natnet_data_types.py | 261 ++++++++++++ .../emulator/server/natnet_model_types.py | 134 +++++++ .../natnet/emulator/server/natnet_server.py | 353 ++++++++++++++++ .../emulator/server/natnet_server_types.py | 156 ++++++++ .../emulator/server/natnet_unicast_server.py | 172 ++++++++ .../optitrack.natnet.emulator/setup.py | 23 ++ .../test/natnet_test_helpers.py | 105 +++++ .../test/test_defaults.py | 26 ++ .../test/test_serializers.py | 341 ++++++++++++++++ .../test/test_server_catalog.py | 78 ++++ .../test/test_unicast_protocol.py | 284 +++++++++++++ tests/colcon_unit_test_packages.yaml | 6 + tests/conftest.py | 10 + tests/integration/natnet/README.md | 151 +++++++ .../natnet/test_natnet_integration.py | 376 ++++++++++++++++++ 23 files changed, 2730 insertions(+) create mode 100644 simulation/isaac-sim/extensions/optitrack.natnet.emulator/.gitignore create mode 100644 simulation/isaac-sim/extensions/optitrack.natnet.emulator/README.md create mode 100644 simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/__init__.py create mode 100644 simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/__init__.py create mode 100644 simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/__init__.py create mode 100644 simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/defaults.py create mode 100644 simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/__init__.py create mode 100644 simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_common.py create mode 100644 simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_data_types.py create mode 100644 simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_model_types.py create mode 100644 simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_server.py create mode 100644 simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_server_types.py create mode 100644 simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_unicast_server.py create mode 100644 simulation/isaac-sim/extensions/optitrack.natnet.emulator/setup.py create mode 100644 simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/natnet_test_helpers.py create mode 100644 simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_defaults.py create mode 100644 simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_serializers.py create mode 100644 simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_server_catalog.py create mode 100644 simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_unicast_protocol.py create mode 100644 tests/integration/natnet/README.md create mode 100644 tests/integration/natnet/test_natnet_integration.py diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/.gitignore b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/.gitignore new file mode 100644 index 000000000..0db35ef6e --- /dev/null +++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/.gitignore @@ -0,0 +1,9 @@ +# OptiTrack SDK archives and build artifacts (reference tree may exist locally) +**/*.obj +**/*.pdb +**/*.exe +**/*.iobj +**/*.ipdb +**/*.tlog/ +**/__pycache__/ +**/*.pyc diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/README.md b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/README.md new file mode 100644 index 000000000..3bb05f0b6 --- /dev/null +++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/README.md @@ -0,0 +1,160 @@ +# OptiTrack NatNet Emulator (Isaac Sim Extension) + +Python NatNet **server** emulator for AirStack simulation and integration testing with [`natnet_ros2`](../../../../robot/ros_ws/src/perception/natnet_ros2/). + +The extension has two layers: + +1. **Transport + protocol** (`optitrack.natnet.emulator.server`) — UDP NatNet server, ctypes wire types, MODELDEF cache, frame streaming. Importable outside Isaac Sim (unit tests, host-side integration). +2. **Isaac integration** (`optitrack.natnet.emulator.isaac`) — stage-driven `/World/NatNetInterface` config prim, pose sampling on physics steps, Kit UI editor, and Pegasus launch-script helpers. + +## Layout + +``` +optitrack.natnet.emulator/ +├── config/extension.toml # Kit manifest (server module + UI entry point) +├── schema/schema.usda # Typed NatNet interface attribute definitions +├── setup.py +├── docs/ # (legacy design notes — see docs/simulation/isaac_sim/natnet_emulator.md) +├── test/ # Co-located unit tests (proxied by tests/sim/) +└── optitrack/natnet/emulator/ + ├── defaults.py # Reference Drone → prim bindings for tests + ├── server/ # NatNet UDP server (transport + protocol) + │ ├── natnet_server.py # Base server, queue, MODELDEF cache + │ ├── natnet_unicast_server.py + │ ├── natnet_data_types.py + │ ├── natnet_model_types.py + │ └── natnet_server_types.py + └── isaac/ # Isaac Sim wrapper (Kit + USD) + ├── config.py # Pure-Python NatNetInterfaceConfig model + ├── usd_bindings.py # Author/read interface prims on a stage + ├── catalog.py # Config → sDataDescriptions (MODELDEF) + ├── frames.py # Prim poses → sFrameOfMocapData + ├── manager.py # NatNetServerManager (lifecycle + sampling) + ├── scene_setup.py # Pegasus launch helpers (start_drone_natnet_server) + └── ui_extension.py # Docked editor panel (NatNetEmulatorExtension) +``` + +## Responsibilities + +| Layer | Role | +|-------|------| +| **Server** | UDP transport; `NAT_CONNECT` / `NAT_SERVERINFO`; `NAT_REQUEST_MODELDEF`; `NAT_KEEPALIVE`; `NAT_ECHOREQUEST` / `NAT_ECHORESPONSE`; `NAT_FRAMEOFDATA` on the **data port** (1511). MODELDEF stored as packed bytes via `set_model_def_payload()`. Frames enqueued with `enqueue_mocap_data()`. | +| **Isaac wrapper** | Authors and reads the NatNet interface config prim; builds MODELDEF from scene config; samples tracked prim world poses each physics step; calls `flush_mocap_data()` synchronously (background timer disabled — see below). | +| **`defaults.py`** | Hardcoded `Drone` → `/World/base_link` binding for legacy tests; production paths use the stage prim via `scene_setup.build_drone_config()`. | + +The server does **not** own prim-path bindings. The Isaac layer calls `set_model_def_payload(catalog.pack())` after building `sDataDescriptions` from the interface config. + +## Stage-driven config prim + +Configuration lives on a USD prim (conventionally `/World/NatNetInterface`) with `natnet:*` attributes: + +- Server: IP, unicast/multicast mode, command/data ports, publish rate, NatNet version, up-axis, optional pose noise. +- Bodies: multi-apply `natnet:body::*` fields mapping rigid-body name / streaming ID → target prim path. + +`NatNetServerManager` scans the stage, resyncs the catalog when the prim changes, and streams one rigid body per configured target. Missing prims emit **lost** bodies (NaN position, tracking-invalid bit clear) until the target appears — important for Pegasus drones spawned on first Play. + +**Up axis:** default `Z` passes Isaac/USD world poses through unchanged (matches `natnet_ros2`). Set `Y` to emulate a Y-up Motive room. + +## Streaming model (Isaac) + +Inside Kit, the server's background `_data_update_loop` is **disabled** (`auto_stream = False`) because the GIL-starved daemon thread does not reliably transmit frames. Instead, each physics step: + +1. `NatNetServerManager.sample_once()` reads prim poses and `enqueue_mocap_data(frame)`. +2. `NatNetUnicastServer.flush_mocap_data()` sends immediately on the physics-step thread. + +Outside Isaac (host unit tests), `auto_stream=True` uses the timer-driven loop. + +Default Docker sim IP: **`172.31.0.200`** (Isaac container on the AirStack bridge network). + +## Enabling in AirStack + +**Robot:** `LAUNCH_NATNET=true` in `.env` → `natnet_ros2` in perception bringup. Configure Motive/emulator IP in [`natnet_config.yaml`](../../../../robot/ros_ws/src/perception/natnet_ros2/config/natnet_config.yaml). + +**Isaac Sim:** set `ISAAC_SIM_SCRIPT_NAME` to a NatNet launch script (NatNet always starts — no `LAUNCH_NATNET` gate in the script): + +| Script | Use | +|--------|-----| +| `example_one_px4_pegasus_natnet_launch_script.py` | Single drone + static `Target` | +| `example_multi_px4_pegasus_natnet_launch_script.py` | `NUM_ROBOTS` drones + shared `Target` (system tests with NatNet use this even for `NUM_ROBOTS=1`) | + +Baseline Pegasus scripts (`example_one_px4_pegasus_launch_script.py`, `example_multi_px4_pegasus_launch_script.py`) have **no** NatNet integration. + +Convenience bundle for NatNet + external-vision PX4 SITL: + +```bash +airstack up --env-file overrides/isaac-natnet-vision.env +``` + +See [optitrack-development skill](../../../../.agents/skills/optitrack-development/SKILL.md) for wire-protocol details, libNatNet 4.4 unicast quirks, and debugging. + +## Usage + +### Server only (no Kit) + +```python +from optitrack.natnet.emulator import NatNetUnicastServer, make_default_drone_catalog +from optitrack.natnet.emulator.isaac.frames import BodySample, build_frame + +server = NatNetUnicastServer(local_interface="172.31.0.200") +server.set_model_def_payload(make_default_drone_catalog().pack()) +server.start() + +frame = build_frame(0, [BodySample(1, (0, 0, 1), (0, 0, 0, 1))]) +server.enqueue_mocap_data(frame) +server.flush_mocap_data() +``` + +### Isaac launch script + +```python +from optitrack.natnet.emulator.isaac import start_drone_natnet_server + +# Keep a reference to the manager for the sim lifetime. +manager = start_drone_natnet_server( + stage, + drones=[("Drone", 1, "/World/drone1/base_link")], + server_ip="172.31.0.200", +) +``` + +### Kit UI + +The extension registers **Window → NatNet Emulator** — a docked panel to create/edit the interface prim, start/stop the server, and view live body readouts. The same `NatNetServerManager` backs both the UI and launch-script paths. + +## Protocol notes (unicast, libNatNet 4.4) + +| Port | Traffic | +|------|---------| +| **1510** | Command: `NAT_CONNECT`, `NAT_REQUEST_MODELDEF`, keepalives, echo | +| **1511** | Data: `NAT_FRAMEOFDATA` — **must** be sent from a socket bound to the data port | + +Frames sent from the command socket are silently dropped by libNatNet. Every frame payload must include the 4-byte end-of-data tag expected by the C SDK unpacker. + +Full handshake layouts and sniffing workflow: [optitrack-development skill](../../../../.agents/skills/optitrack-development/SKILL.md). + +## Tests + +| Tier | Mark | What | +|------|------|------| +| Unit | `unit` | Serializers, protocol, config, USD authoring, catalog, pose sampling, server lifecycle, scene setup | +| Integration | `integration` | Host emulator → robot `natnet_ros2` pose Hz | + +Co-located tests live in `test/`. Pytest discovers them via thin proxies in [`tests/sim/optitrack_natnet_emulator/`](../../../../tests/sim/optitrack_natnet_emulator/). + +```bash +# Unit (no Docker / no SDK) +pytest tests/sim/optitrack_natnet_emulator/ -m unit -v + +# Integration (robot container + NatNet SDK) +pytest tests/integration/natnet/ -m integration -v +``` + +Representative unit modules: `test_unicast_protocol.py`, `test_pose_streaming.py`, `test_interface_authoring.py`, `test_server_lifecycle.py`, `test_scene_setup.py`. + +## Reference material + +- User guide: [`docs/simulation/isaac_sim/natnet_emulator.md`](../../../../docs/simulation/isaac_sim/natnet_emulator.md) +- Robot client: [`natnet_ros2/README.md`](../../../../robot/ros_ws/src/perception/natnet_ros2/README.md) +- Integration tier: [`tests/integration/natnet/README.md`](../../../../tests/integration/natnet/README.md) + +OptiTrack SDK sample headers may exist locally under `NatNetClientSDK/` for wire-format reference; they are **not** redistributed by AirStack (proprietary license). diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/__init__.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/__init__.py new file mode 100644 index 000000000..39ed38144 --- /dev/null +++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/__init__.py @@ -0,0 +1 @@ +"""OptiTrack NatNet packages for AirStack Isaac Sim integration.""" diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/__init__.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/__init__.py new file mode 100644 index 000000000..b19da2cbc --- /dev/null +++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/__init__.py @@ -0,0 +1 @@ +"""NatNet simulation components.""" diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/__init__.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/__init__.py new file mode 100644 index 000000000..e819d8b1b --- /dev/null +++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/__init__.py @@ -0,0 +1,20 @@ +"""OptiTrack Motive NatNet emulator for Isaac Sim.""" + +from .defaults import ( + DEFAULT_DRONE_BINDING, + DEFAULT_TRACKED_BODY_BINDINGS, + TrackedBodyBinding, +) +from .server import Client, NatNetServer, NatNetUnicastServer, TransmissionType +from .server.natnet_model_types import make_default_drone_catalog + +__all__ = [ + "Client", + "DEFAULT_DRONE_BINDING", + "DEFAULT_TRACKED_BODY_BINDINGS", + "NatNetServer", + "NatNetUnicastServer", + "TrackedBodyBinding", + "TransmissionType", + "make_default_drone_catalog", +] diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/defaults.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/defaults.py new file mode 100644 index 000000000..2c1915264 --- /dev/null +++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/defaults.py @@ -0,0 +1,25 @@ +"""Reference tracked-body defaults for tests and the future Isaac Sim wrapper.""" + +from __future__ import annotations + +from dataclasses import dataclass + + +@dataclass(frozen=True) +class TrackedBodyBinding: + """Maps a NatNet rigid body to a USD prim path (not sent on the NatNet wire).""" + + name: str + id: int + prim_path: str + parent_id: int = -1 + + +# Single-drone NatNet Pegasus scenes (example_one_px4_pegasus_natnet_launch_script.py). +DEFAULT_DRONE_BINDING = TrackedBodyBinding( + name="Drone", + id=1, + prim_path="/World/base_link", +) + +DEFAULT_TRACKED_BODY_BINDINGS: tuple[TrackedBodyBinding, ...] = (DEFAULT_DRONE_BINDING,) diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/__init__.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/__init__.py new file mode 100644 index 000000000..c84c1fe4b --- /dev/null +++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/__init__.py @@ -0,0 +1,11 @@ +"""NatNet UDP server implementation (unicast; multicast planned).""" + +from .natnet_server import Client, NatNetServer, TransmissionType +from .natnet_unicast_server import NatNetUnicastServer + +__all__ = [ + "Client", + "NatNetServer", + "NatNetUnicastServer", + "TransmissionType", +] diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_common.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_common.py new file mode 100644 index 000000000..1eb32177c --- /dev/null +++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_common.py @@ -0,0 +1,27 @@ +from enum import IntEnum +import ctypes + +class ModelLimits(IntEnum): + MAX_MODELS = 2000 # maximum number of total models (data descriptions) + MAX_MARKERSETS = 1000 # maximum number of MarkerSets + MAX_RIGIDBODIES = 1000 # maximum number of RigidBodies + MAX_ASSETS = 1000 # Maximum number of Assets + MAX_NAMELENGTH = 256 # maximum length for strings + MAX_MARKERS = 200 # maximum number of markers per MarkerSet + MAX_RBMARKERS = 20 # maximum number of markers per RigidBody + MAX_SKELETONS = 100 # maximum number of skeletons + MAX_SKELRIGIDBODIES = 200 # maximum number of RididBodies per Skeleton + MAX_LABELED_MARKERS = 1000 # maximum number of labeled markers per frame + MAX_UNLABELED_MARKERS = 1000 # maximum number of unlabeled (other) markers per frame + + MAX_FORCEPLATES = 100 # maximum number of force plate 'bundles' + MAX_DEVICES = 100 # maximum number of peripheral device 'bundles' + MAX_ANALOG_CHANNELS = 32 # maximum number of data channels (signals) per analog/force plate device + MAX_ANALOG_SUBFRAMES = 30 # maximum number of analog/force plate frames per mocap frame + + MAX_PACKETSIZE = 65503 # max size of packet in bytes (actual packet size is dynamic) + # (65535 byte IP limit - 20 byte IP header - 8 byte UDP header - 4 byte sPacket header = 65503 bytes) + + + +MarkerData = ctypes.c_float * 3 diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_data_types.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_data_types.py new file mode 100644 index 000000000..91e23a412 --- /dev/null +++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_data_types.py @@ -0,0 +1,261 @@ +import ctypes +import struct +from .natnet_common import ModelLimits, MarkerData + +class sMarker(ctypes.Structure): + _pack_ = 1 + _fields_ = [ + ("ID", ctypes.c_int32), + ("x", ctypes.c_float), + ("y", ctypes.c_float), + ("z", ctypes.c_float), + ("size", ctypes.c_float), + ("params", ctypes.c_int16), + ("residual", ctypes.c_float) + ] + + def pack(self) -> bytes: + return struct.pack(' bytes: + # szName is null-terminated on the wire. + name_bytes = self.szName.rstrip(b'\x00') + b'\x00' + payload = bytearray(name_bytes) + payload += struct.pack(' bytes: + return struct.pack(' bytes: + payload = bytearray(struct.pack(' bytes: + payload = bytearray(struct.pack(' bytes: + payload = bytearray(struct.pack(' bytes: + payload = bytearray(struct.pack(' bytes: + payload = bytearray(struct.pack(' bytes: + """NatNet 4.1+ prefixes each collection with a 4-byte byte count.""" + payload = bytearray(struct.pack(' 0) or natnet_major > 4: + payload += struct.pack(' bytes: + payload = bytearray() + + payload += struct.pack(' bytes: + # szName is null-terminated on the wire, not fixed MAX_NAMELENGTH. + name_bytes = self.szName.rstrip(b"\x00") + b"\x00" + payload = bytearray(name_bytes) + payload += struct.pack( + " bytes: + if self.type == int(DataDescriptors.Descriptor_RigidBody): + body = self.RigidBodyDescription.pack() + else: + raise ValueError(f"Unsupported data description type: {self.type}") + payload = bytearray(struct.pack(" bytes: + payload = bytearray(struct.pack(" sDataDescriptions: + """Build the default single-body catalog (Drone id=1) for natnet_ros2.""" + descriptions = sDataDescriptions() + descriptions.nDataDescriptions = 1 + desc = descriptions.arrDataDescriptions[0] + desc.type = int(DataDescriptors.Descriptor_RigidBody) + rb = desc.RigidBodyDescription + rb.szName = b"Drone" + rb.ID = 1 + rb.parentID = -1 + rb.offsetqw = 1.0 + rb.nMarkers = 0 + return descriptions diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_server.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_server.py new file mode 100644 index 000000000..5ef19009e --- /dev/null +++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_server.py @@ -0,0 +1,353 @@ +from . import natnet_data_types as DataMessages +from . import natnet_server_types as ServerMessages +from . import natnet_model_types as ModelTypes +from enum import Enum +import socket +import threading +import queue +import signal +import ctypes +import time +import typing + + +class TransmissionType(str, Enum): + UNICAST = "unicast" + MULTICAST = "multicast" + +class Client: + def __init__(self, ip: str, port: int, version: typing.Tuple[int, int, int, int] = (4, 4, 0, 0)): + self.ip = ip + self.port = port + self.version = version + self.subscribed_assets = set() + self.socket_lock = threading.Lock() + + def __hash__(self): + # Uniquely identify a client session by their IP and their unique command port. + return hash((self.ip, self.port)) + + def __eq__(self, other): + return (isinstance(other, Client) and + self.ip == other.ip and + self.port == other.port) + + +class NatNetServer: + def __init__(self, + local_interface : str = "172.31.0.200", + transmission_type: TransmissionType = TransmissionType.MULTICAST, + multicast_address : str = "239.255.42.99", + command_port: int = 1510, + data_port : int = 1511, + motive_app_version : typing.Tuple[int, int, int, int]=(3, 1, 0, 0), + natnet_version : typing.Tuple[int, int, int, int]=(4, 4, 0, 0), + high_res_clock_freq : int = 1_000_000_000, + publish_rate : int = 100 # Hz (default 100Hz) + ): + + self.local_interface = local_interface + self.transmission_type = transmission_type + self.multicast_address = multicast_address + self.command_port = command_port + self.data_port = data_port + self.motive_app_version = motive_app_version + self.natnet_version = natnet_version + self.high_res_clock_freq = high_res_clock_freq + self.publish_rate = publish_rate + + self._validate_init_params() + + self.server_description = self._build_server_description() + # Initialize synchronously safe data structures for server state and mocap data + + # Thread-safe queue for Mocp frames + self.mocap_data_queue = queue.Queue(maxsize=100) + self._last_mocap_frame: DataMessages.sFrameOfMocapData | None = None + self._last_mocap_lock = threading.Lock() + + # Thread list and shutdown event + self.threads = [] + self.shutdown_event = threading.Event() + + # Connected clients for unicast mode + self.connected_clients : typing.Set[Client] = set() + self.clients_lock : threading.Lock = threading.Lock() + + # MODELDEF wire cache (Isaac wrapper updates via set_model_def_payload) + self._model_def_lock = threading.Lock() + self._model_def_payload: bytes = ModelTypes.make_default_drone_catalog().pack() + + # Sockets + self.command_socket : socket.socket | None = None + self.data_socket : socket.socket | None = None + + self.running = False + + # When True (default), the background data loop streams frames on its own timer. + # Set False when an external driver (the Isaac wrapper's physics-step callback) + # sends frames synchronously via ``flush_mocap_data``. + self.auto_stream = True + + # start() launches two daemon threads: a command listener (handshake / MODELDEF / keepalive) + # and a data loop that streams mocap frames. The transmission-specific behavior lives in the unicast/multicast subclass. + + def _signal_handler(self, signum, frame): + print(f"\n[NatNetServer] Received interrupt signal {signum}. Initiating shutdown...") + self.shutdown() + + def enqueue_mocap_data(self, new_data: DataMessages.sFrameOfMocapData): + # Thread-safe method to push new physics frames (called by Isaac-Sim extension) + if self.mocap_data_queue.full(): + try: + # Drop oldest frame if falling behind + self.mocap_data_queue.get_nowait() + except queue.Empty: + pass + self.mocap_data_queue.put(new_data) + with self._last_mocap_lock: + self._last_mocap_frame = new_data + + def _get_last_known_mocap_frame(self) -> DataMessages.sFrameOfMocapData | None: + with self._last_mocap_lock: + return self._last_mocap_frame + + def set_model_def_payload(self, payload: bytes) -> None: + """Replace MODELDEF body served on NAT_REQUEST_MODELDEF (Isaac wrapper calls this).""" + with self._model_def_lock: + self._model_def_payload = payload + + def set_model_def_from_descriptions( + self, descriptions: ModelTypes.sDataDescriptions + ) -> None: + """Pack descriptions once and store as the MODELDEF wire cache.""" + self.set_model_def_payload(descriptions.pack()) + + def _get_model_def_payload(self) -> bytes: + """Return cached MODELDEF bytes (command thread only).""" + with self._model_def_lock: + return self._model_def_payload + + def start(self): + # Bind sockets and launch worker threads automatically on init + + # Register signal handlers for graceful shutdown (Catches Ctrl+C and kill) + try: + signal.signal(signal.SIGINT, self._signal_handler) + signal.signal(signal.SIGTERM, self._signal_handler) + except ValueError: + pass # Safe fallback if not called from the main thread + + # 1. Setup Command Socket (Receives connection/discovery requests) + self.command_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM, socket.IPPROTO_UDP) + self.command_socket.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) + self.command_socket.bind(('', self.command_port)) + + # 2. Setup Data Socket (Sends outward Mocap frames). + # Bind to the data port so frames leave with source port == data_port. + # libNatNet routes unicast NAT_FRAMEOFDATA by the server's data port + self.data_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM, socket.IPPROTO_UDP) + self.data_socket.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) + self.data_socket.bind(('', self.data_port)) + if self.transmission_type == TransmissionType.MULTICAST: + self.data_socket.setsockopt(socket.IPPROTO_IP, socket.IP_MULTICAST_IF, socket.inet_aton(self.local_interface)) + + # 3. Launch Threads + cmd_thread = threading.Thread(target=self._command_listener_loop, daemon=True) + data_thread = threading.Thread(target=self._data_update_loop, daemon=True) + + self.threads.extend([cmd_thread, data_thread]) + + for t in self.threads: + t.start() + + self.running = True + + def shutdown(self): + # Cleanly shutdown threads and close sockets + self.running = False + self.shutdown_event.set() + + if self.command_socket: + self.command_socket.close() + + if self.data_socket: + self.data_socket.close() + + for t in self.threads: + if t.is_alive(): + t.join(timeout=1.0) + + def _validate_init_params(self): + + # Validate the local_interface is a valid IP address + if not self.local_interface or not isinstance(self.local_interface, str) or self.local_interface.count('.') != 3: + raise ValueError(f"Invalid local interface IP address: {self.local_interface}") + + # Validate between transmission types and address requirements + if self.transmission_type not in TransmissionType: + raise ValueError(f"Invalid transmission type: {self.transmission_type}. Must be 'unicast' or 'multicast'.") + + if self.transmission_type == TransmissionType.MULTICAST and not self.multicast_address: + raise ValueError("Multicast address must be provided for multicast transmission type.") + + if self.transmission_type == TransmissionType.UNICAST and self.multicast_address: + raise ValueError("Multicast address should not be provided for unicast transmission type.") + + if not (0 < self.command_port < 65536): + raise ValueError(f"Invalid command port: {self.command_port}. Must be between 1 and 65535.") + + if not (0 < self.data_port < 65536): + raise ValueError(f"Invalid data port: {self.data_port}. Must be between 1 and 65535.") + + if self.command_port == self.data_port: + raise ValueError("Command port and data port must be different.") + + if self.motive_app_version and (not isinstance(self.motive_app_version, tuple) or len(self.motive_app_version) != 4): + raise ValueError(f"Invalid Motive app version: {self.motive_app_version}. Must be a tuple of 4 integers (major, minor, build, revision).") + + if self.natnet_version and (not isinstance(self.natnet_version, tuple) or len(self.natnet_version) != 4): + raise ValueError(f"Invalid NatNet version: {self.natnet_version}. Must be a tuple of 4 integers (major, minor, build, revision).") + + if self.motive_app_version and not self.motive_app_version[0] == 3: + raise ValueError(f"Unsupported Motive app version: {self.motive_app_version}. Minimum supported version is 3.0.0.0. Recommended to use 3.1.0.0") + + if not self.natnet_version[0] == 4: + raise ValueError(f"Unsupported NatNet version: {self.natnet_version}. Minimum supported version is 4.0.0.0. Recommended to use 4.4.0.0") + + if self.high_res_clock_freq <= 0: + raise ValueError( + f"Invalid high resolution clock frequency: {self.high_res_clock_freq}. Must be a positive integer representing the frequency in Hz." + ) + + if self.publish_rate <= 0: + raise ValueError( + f"Invalid publish rate: {self.publish_rate}. Must be a positive number representing Hz." + ) + def _get_latest_mocap_packet(self) -> DataMessages.sFrameOfMocapData | None: + # Thread-safe method to retrieve the latest mocap data to be sent + try: + return self.mocap_data_queue.get_nowait() + except queue.Empty: + return None + + @staticmethod + def _pad_fixed_string(value: bytes) -> bytes: + """Null-pad a byte string to MAX_NAMELENGTH for fixed-size NatNet name fields.""" + truncated = value[: ServerMessages.MAX_NAMELENGTH - 1] + return truncated + b"\x00" * (ServerMessages.MAX_NAMELENGTH - len(truncated)) + + @staticmethod + def _assign_version_bytes(field: ctypes.Array, version: typing.Tuple[int, int, int, int]) -> None: + for index, component in enumerate(version): + field[index] = component + + @staticmethod + def _assign_ipv4_bytes(field: ctypes.Array, address: str | bytes) -> None: + octets = socket.inet_aton(address) if isinstance(address, str) else address + for index, octet in enumerate(octets): + field[index] = octet + + def _build_server_description(self) -> ServerMessages.sServerDescription: + # Helper to build the server description struct with current server info (e.g. on startup or in response to command request) + description = ServerMessages.sServerDescription() + description.HostPresent = True + description.szHostComputerName = self._pad_fixed_string( + socket.gethostname().encode("utf-8") + ) + self._assign_ipv4_bytes(description.HostComputerAddress, self.local_interface) + description.szHostApp = self._pad_fixed_string(b"Motive") + self._assign_version_bytes(description.HostAppVersion, self.motive_app_version) + self._assign_version_bytes(description.NatNetVersion, self.natnet_version) + description.HighResClockFrequency = self.high_res_clock_freq + description.bConnectionInfoValid = True + description.ConnectionDataPort = self.data_port + description.ConnectionMulticast = self.transmission_type == TransmissionType.MULTICAST + + if self.transmission_type == TransmissionType.MULTICAST: + self._assign_ipv4_bytes(description.ConnectionMulticastAddress, self.multicast_address) + else: + self._assign_ipv4_bytes(description.ConnectionMulticastAddress, b"\x00\x00\x00\x00") + + return description + + def _build_connect_response_payload(self) -> bytes: + """NAT_CONNECT reply: libNatNet parses NAT_SERVERINFO payload as sSender_Server.""" + sender = ServerMessages.sSender_Server() + sender.Common.szName = self._pad_fixed_string(b"Motive") + self._assign_version_bytes(sender.Common.Version, self.motive_app_version) + self._assign_version_bytes(sender.Common.NatNetVersion, self.natnet_version) + sender.HighResClockFrequency = self.high_res_clock_freq + sender.DataPort = self.data_port + sender.IsMulticast = self.transmission_type == TransmissionType.MULTICAST + if self.transmission_type == TransmissionType.MULTICAST: + self._assign_ipv4_bytes(sender.MulticastGroupAddress, self.multicast_address) + else: + self._assign_ipv4_bytes(sender.MulticastGroupAddress, b"\x00\x00\x00\x00") + return sender.pack() + + def _send_packet_to_client( + self, + client: Client, + message_id: ServerMessages.MessageId | int, + payload: bytes, + sock: socket.socket | None = None, + ) -> None: + """Send a NatNet packet to a unicast client (libNatNet 4.4). + + Command replies go out the command socket; mocap frames go out the data socket. + """ + if self.shutdown_event.is_set(): + return + sock = sock or self.command_socket + if not sock: + raise ValueError("[NatNetServer] Socket not initialized. Cannot send packet.") + + header = ServerMessages.sPacketHeader( + iMessage=int(message_id), + nDataBytes=len(payload), + ) + packet = header.pack() + payload + try: + with client.socket_lock: + sock.sendto(packet, (client.ip, client.port)) + except OSError as e: + raise ValueError( + f"[NatNetServer] Error sending message {int(message_id)} to " + f"client {client.ip}:{client.port}: {e}" + ) from e + + def _data_update_loop(self): # Stub: Different betweeen multicast and unicast server implementations, as they will need to handle client connections differently (multicast will just send to the multicast group address) + # Loop to update mocap data and send packets at regular intervals. + pass + + def _send_data_packet(self, client: Client, data_message: DataMessages.sFrameOfMocapData): + # Serialize frame payload and send via the data socket. + # + # Stamp the transmit time in the server's high-resolution clock domain so + # the client can recover per-message transit latency via + # NatNetClient::SecondsSinceHostTimestamp(TransmitTimestamp). This must match + # the clock used in the NAT_ECHORESPONSE handshake (time.time() nanoseconds) + # and the advertised HighResClockFrequency (defaults to 1e9 ticks/s), so the + # SDK's server-clock estimate and this timestamp share one timeline. + data_message.TransmitTimestamp = int(time.time() * 1_000_000_000) + try: + packet_bytes = data_message.pack() + except Exception as e: + raise ValueError(f"[NatNetServer] Error serializing data message: {e}") from e + + self._send_packet_to_client( + client, + ServerMessages.MessageId.NAT_FRAMEOFDATA, + packet_bytes, + sock=self.data_socket, + ) + + def _command_listener_loop(self): # Stub: Different betweeen multicast and unicast server implementations, as they will need to handle client connections differently (multicast will just send to the multicast group address) + # Loop to listen for and handle incoming command requests (e.g. from client apps) + pass + + def _handle_command_request(self, request_data: bytes): # Stub: Different betweeen multicast and unicast server implementations, as they will need to handle client connections differently (multicast will just send to the multicast group address) + # Parse incoming command request, perform requested action, and send response if needed + pass + diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_server_types.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_server_types.py new file mode 100644 index 000000000..3083f1a6e --- /dev/null +++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_server_types.py @@ -0,0 +1,156 @@ +import ctypes +import struct +from enum import IntEnum + +MAX_NAMELENGTH = 256 +MAX_PACKETSIZE = 65503 + +# NatNet SDK sServerDescription uses default struct alignment (#pragma pack(pop)), not pack(1). +SERVER_DESCRIPTION_WIRE_SIZE = 552 +# NAT_CONNECT / NAT_SERVERINFO reply uses packed sSender_Server (#pragma pack(1) in NatNetTypes.h). +SENDER_SERVER_WIRE_SIZE = 256 + 4 + 4 + 8 + 2 + 1 + 4 # 279 + +# Client/server message ids +class MessageId(IntEnum): + NAT_CONNECT = 0 + NAT_SERVERINFO = 1 + NAT_REQUEST = 2 + NAT_RESPONSE = 3 + NAT_REQUEST_MODELDEF = 4 + NAT_MODELDEF = 5 + NAT_REQUEST_FRAMEOFDATA = 6 + NAT_FRAMEOFDATA = 7 + NAT_MESSAGESTRING = 8 + NAT_DISCONNECT = 9 + NAT_KEEPALIVE = 10 + NAT_DISCONNECTBYTIMEOUT = 11 + NAT_ECHOREQUEST = 12 + NAT_ECHORESPONSE = 13 + NAT_DISCOVERY = 14 + NAT_UNRECOGNIZED_REQUEST = 100 + +# Server/Sender configuration and info +def _fixed_name(field: ctypes.Array) -> bytes: + raw = bytes(field).split(b"\x00", 1)[0] + b"\x00" + if len(raw) > MAX_NAMELENGTH: + raw = raw[: MAX_NAMELENGTH - 1] + b"\x00" + return raw + b"\x00" * (MAX_NAMELENGTH - len(raw)) + + +class sSender(ctypes.Structure): + _pack_ = 1 + _fields_ = [ + ("szName", ctypes.c_char * MAX_NAMELENGTH), # host app's name + ("Version", ctypes.c_uint8 * 4), # host app's version [major.minor.build.revision] + ("NatNetVersion", ctypes.c_uint8 * 4) # host app's NatNet version + ] + + def pack(self) -> bytes: + payload = bytearray() + payload += _fixed_name(self.szName) + payload += bytes(self.Version) + payload += bytes(self.NatNetVersion) + return bytes(payload) + +class sSender_Server(ctypes.Structure): + _pack_ = 1 + _fields_ = [ + ("Common", sSender), + ("HighResClockFrequency", ctypes.c_uint64), + ("DataPort", ctypes.c_uint16), + ("IsMulticast", ctypes.c_bool), + ("MulticastGroupAddress", ctypes.c_uint8 * 4) + ] + + def pack(self) -> bytes: + payload = bytearray(self.Common.pack()) + payload += struct.pack(" bytes: + # Wire layout matches NatNet SDK on x86-64 (3 pad bytes before HighResClockFrequency). + payload = bytearray() + payload.append(1 if self.HostPresent else 0) + payload += _fixed_name(self.szHostComputerName) + payload += bytes(self.HostComputerAddress) + payload += _fixed_name(self.szHostApp) + payload += bytes(self.HostAppVersion) + payload += bytes(self.NatNetVersion) + while len(payload) % 8: + payload.append(0) + payload += struct.pack(" bytes: + return bytes(self) + +# Connection types enum matching NatNet SDK rules +class ConnectionType(IntEnum): + ConnectionType_Multicast = 0 + ConnectionType_Unicast = 1 + +class sNatNetClientConnectParams(ctypes.Structure): + """ + Python ctypes translation of the C++ sNatNetClientConnectParams struct. + Enforces a packed structure byte alignment matching the NatNet binary network protocol. + """ + _pack_ = 1 + _fields_ = [ + ("connectionType", ctypes.c_int32), # 4 bytes (mapping to standard ConnectionType enum) + ("serverCommandPort", ctypes.c_uint16), # 2 bytes + ("serverDataPort", ctypes.c_uint16), # 2 bytes + + # NOTE: Represented as void pointers (c_void_p) to safely match the host system's native bit size (e.g., 8 bytes on 64-bit) without string data unpacking overhead. + ("serverAddress", ctypes.c_void_p), + ("localAddress", ctypes.c_void_p), + ("multicastAddress", ctypes.c_void_p), + + ("subscribedDataOnly", ctypes.c_bool), # 1 byte + ("BitstreamVersion", ctypes.c_uint8 * 4) # 4 bytes: [Major, Minor, Build, Revision] + ] + + def pack(self) -> bytes: + return bytes(self) \ No newline at end of file diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_unicast_server.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_unicast_server.py new file mode 100644 index 000000000..d9c0a1e22 --- /dev/null +++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_unicast_server.py @@ -0,0 +1,172 @@ +import ctypes +import time + +from . import natnet_server_types as ServerTypes +from .natnet_server import TransmissionType, Client, NatNetServer + + +class NatNetUnicastServer(NatNetServer): + def __init__(self, + local_interface="172.31.0.200", + transmission_type: TransmissionType = TransmissionType.UNICAST, + multicast_address=None, + command_port=1510, + data_port=1511 + ): + + if not transmission_type == TransmissionType.UNICAST: + raise ValueError("Transmission type 'MULTICAST' is not supported in NatNetUnicastServer. Please use NatNetMulticastServer instead.") + + super().__init__(local_interface, transmission_type, multicast_address, command_port, data_port) + + def _data_update_loop(self): + # Loop to update mocap data and send packets at regular intervals. + # When auto_stream is False the frames are pumped externally (Isaac physics step), + # so this thread only idles — but stays alive for clean shutdown. + while not self.shutdown_event.is_set(): + time.sleep(1 / self.publish_rate) + if not self.auto_stream: + continue + self.flush_mocap_data() + + def flush_mocap_data(self): + """Send the latest (or last) mocap frame to every connected client, once.""" + with self.clients_lock: + clients = list(self.connected_clients) + if not clients: + return + + data_messages = self._get_latest_mocap_packet() + + if data_messages is None: # If the server stops producing frames, use the last known frame. + data_messages = self._get_last_known_mocap_frame() + if data_messages is None: + return + + for client in clients: + try: + self._send_data_packet(client, data_messages) + except ValueError as e: + print(str(e)) + continue + + def _command_listener_loop(self): + # Listens on UDP command socket for incoming command requests from clients. + # Handles incoming client handshakes and teardown. + + print(f"[Command Listener] Command listener thread started. Listening for incoming client command requests on UDP address:port {self.local_interface}:{self.command_port}...") + + while not self.shutdown_event.is_set(): + try: + data, addr = self.command_socket.recvfrom(1024) # Buffer size of 1024 bytes should be sufficient for command requests + if not data: + continue + self._handle_command_request(data, addr) + except Exception as e: + if self.shutdown_event.is_set(): + break + print(f"[Command Listener] Error receiving command request: {e}") + time.sleep(0.1) # Sleep briefly to avoid tight loop on errors + + def _handle_command_request(self, request_data: bytes, client_address: tuple): + """ + Processes standard binary headers and registers unicast endpoints. + """ + header_size = ctypes.sizeof(ServerTypes.sPacketHeader) + if len(request_data) < header_size: + return + + # Parse the header via ctypes + header = ServerTypes.sPacketHeader.from_buffer_copy(request_data[:header_size]) + + # Handle Connection Handshake + if header.iMessage == int(ServerTypes.MessageId.NAT_CONNECT): + client_requested_version = self.natnet_version # Fallback to server's version. Version handshaking not supported in this extension. + + client_ip, client_port = client_address + + # Create and store a new client object + new_client = Client(client_ip, client_port, version=client_requested_version) + try: + with self.clients_lock: + self.connected_clients.discard(new_client) # Remove any existing client with the same IP and port + self.connected_clients.add(new_client) # Add the new client to the connected clients list + print(f"[Command Handler] Added client {new_client.ip}:{new_client.port} to connected clients list.") + except Exception as e: + print(f"[Command Handler] Error adding client {new_client.ip}:{new_client.port} to connected clients list: {e}") + return + + try: + self._send_packet_to_client( + new_client, + ServerTypes.MessageId.NAT_SERVERINFO, + self._build_connect_response_payload(), + ) + except ValueError as e: + raise ValueError( + f"[Command Handler] Error sending server description to client {client_address}: {e}" + ) from e + print( + f"[Command Handler] Sent server description to client address " + f"through its port {client_address}." + ) + return + + # Non-handshake commands require a prior NAT_CONNECT from this endpoint. + client_ip, client_port = client_address + client = self._find_client(client_ip, client_port) + if client is None: + print( + f"[Command Handler] Ignoring message {header.iMessage} from " + f"unregistered client {client_address}." + ) + return + + if header.iMessage == int(ServerTypes.MessageId.NAT_REQUEST_MODELDEF): + try: + self._send_packet_to_client( + client, + ServerTypes.MessageId.NAT_MODELDEF, + self._get_model_def_payload(), + ) + except ValueError as e: + print( + f"[Command Handler] Error sending MODELDEF to client " + f"{client_address}: {e}" + ) + return + + if header.iMessage == int(ServerTypes.MessageId.NAT_KEEPALIVE): + # Receiving a keepalive refreshes the client's liveness; nothing to send back. + return + + if header.iMessage == int(ServerTypes.MessageId.NAT_ECHOREQUEST): + echo_payload = request_data[header_size : header_size + header.nDataBytes] + # libNatNet expects clientRequestTimestamp + hostReceivedTimestamp (8 + 8 bytes). + host_ts = int(time.time() * 1_000_000_000).to_bytes(8, "little", signed=False) + response_payload = echo_payload[:8].ljust(8, b"\x00") + host_ts + try: + self._send_packet_to_client( + client, + ServerTypes.MessageId.NAT_ECHORESPONSE, + response_payload, + ) + except ValueError as e: + print( + f"[Command Handler] Error sending ECHORESPONSE to client " + f"{client_address}: {e}" + ) + return + + print( + f"[Command Handler] Unhandled message id {header.iMessage} from " + f"registered client {client_address}." + ) + + def _find_client(self, ip: str, port: int) -> Client | None: + target = Client(ip, port) + with self.clients_lock: + for client in self.connected_clients: + if client == target: + return client + return None diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/setup.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/setup.py new file mode 100644 index 000000000..1a1c153fc --- /dev/null +++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/setup.py @@ -0,0 +1,23 @@ +"""Isaac Sim extension install metadata for the OptiTrack NatNet emulator.""" + +import os + +from setuptools import find_packages, setup + +EXTENSION_PATH = os.path.dirname(os.path.realpath(__file__)) + +setup( + name="optitrack-natnet-emulator", + version="0.1.0", + description="NatNet UDP server emulator for Isaac Sim and natnet_ros2 integration", + license="MIT", + include_package_data=True, + python_requires=">=3.10", + install_requires=[ + "numpy", + "scipy", + ], + packages=find_packages(where="."), + package_dir={"": "."}, + zip_safe=False, +) diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/natnet_test_helpers.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/natnet_test_helpers.py new file mode 100644 index 000000000..07d6c8a5e --- /dev/null +++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/natnet_test_helpers.py @@ -0,0 +1,105 @@ +"""Shared helpers for optitrack.natnet.emulator unit tests.""" + +from __future__ import annotations + +import socket +import struct +import time +from contextlib import contextmanager + +from optitrack.natnet.emulator import NatNetUnicastServer, TransmissionType +from optitrack.natnet.emulator.server import natnet_server_types as st + + +def ephemeral_udp_port(host: str = "127.0.0.1") -> int: + """Return a free UDP port on *host* by binding and releasing a probe socket.""" + with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as probe: + probe.bind((host, 0)) + return probe.getsockname()[1] + + +class NatNetTestClient: + """Minimal UDP client for NatNet command-port protocol tests.""" + + def __init__(self, host: str = "127.0.0.1", timeout: float = 2.0) -> None: + self._host = host + self._sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + self._sock.bind((host, 0)) + self._sock.settimeout(timeout) + + @property + def local_port(self) -> int: + return self._sock.getsockname()[1] + + def send_message( + self, + server_port: int, + message_id: st.MessageId | int, + payload: bytes = b"", + server_host: str | None = None, + ) -> None: + header = st.sPacketHeader( + iMessage=int(message_id), + nDataBytes=len(payload), + ) + self.send_raw(header.pack() + payload, server_port, server_host) + + def send_raw( + self, + data: bytes, + server_port: int, + server_host: str | None = None, + ) -> None: + """Send a raw UDP datagram (for malformed / malicious packet tests).""" + self._sock.sendto(data, (server_host or self._host, server_port)) + + def send_header_only( + self, + server_port: int, + message_id: st.MessageId | int, + declared_payload_len: int, + server_host: str | None = None, + ) -> None: + """Send a header whose nDataBytes does not match any trailing payload.""" + header = struct.pack(" tuple[int, bytes, tuple[str, int]]: + data, addr = self._sock.recvfrom(65535) + message_id, payload_len = struct.unpack(" None: + self._sock.close() + + +@contextmanager +def running_unicast_server( + command_port: int | None = None, + local_interface: str = "127.0.0.1", + publish_rate: int = 100, +): + """Start NatNetUnicastServer on ephemeral (or fixed) command + data ports. + + Both ports are ephemeral by default so concurrent/sequential tests never + collide on the well-known 1510/1511 pair. + """ + port = command_port if command_port is not None else ephemeral_udp_port(local_interface) + data_port = ephemeral_udp_port(local_interface) + while data_port == port: + data_port = ephemeral_udp_port(local_interface) + server = NatNetUnicastServer( + local_interface=local_interface, + transmission_type=TransmissionType.UNICAST, + multicast_address=None, + command_port=port, + data_port=data_port, + ) + server.publish_rate = publish_rate + server.start() + time.sleep(0.05) + try: + yield server, port + finally: + server.shutdown() diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_defaults.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_defaults.py new file mode 100644 index 000000000..7cc72b036 --- /dev/null +++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_defaults.py @@ -0,0 +1,26 @@ +# Copyright (c) 2024 Carnegie Mellon University +# MIT License - see LICENSE in the repository root for full text. +"""Unit tests for hardcoded tracked-body defaults.""" + +from __future__ import annotations + +import pytest + +from optitrack.natnet.emulator.defaults import ( + DEFAULT_DRONE_BINDING, + DEFAULT_TRACKED_BODY_BINDINGS, +) + + +pytestmark = pytest.mark.unit + + +def test_default_drone_binding_matches_natnet_ros2_config(): + assert DEFAULT_DRONE_BINDING.name == "Drone" + assert DEFAULT_DRONE_BINDING.id == 1 + assert DEFAULT_DRONE_BINDING.parent_id == -1 + assert DEFAULT_DRONE_BINDING.prim_path == "/World/base_link" + + +def test_default_tracked_body_bindings_contains_drone_only(): + assert DEFAULT_TRACKED_BODY_BINDINGS == (DEFAULT_DRONE_BINDING,) diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_serializers.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_serializers.py new file mode 100644 index 000000000..0db5a34df --- /dev/null +++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_serializers.py @@ -0,0 +1,341 @@ +# Copyright (c) 2024 Carnegie Mellon University +# MIT License - see LICENSE in the repository root for full text. +"""Unit tests for NatNet wire serializers (no network).""" + +from __future__ import annotations + +import ctypes +import struct + +import pytest + +from optitrack.natnet.emulator import NatNetUnicastServer, TransmissionType +from optitrack.natnet.emulator.server import natnet_data_types as dt +from optitrack.natnet.emulator.server import natnet_model_types as mt +from optitrack.natnet.emulator.server import natnet_server_types as st +from optitrack.natnet.emulator.server.natnet_common import ModelLimits + + +pytestmark = pytest.mark.unit + + +# ============================================================================= +# natnet_server_types — transport / handshake +# ============================================================================= + + +def test_packet_header_pack_size_and_endianness(): + header = st.sPacketHeader( + iMessage=int(st.MessageId.NAT_FRAMEOFDATA), + nDataBytes=42, + ) + packed = header.pack() + + assert len(packed) == ctypes.sizeof(st.sPacketHeader) == 4 + message_id, payload_len = struct.unpack(" server only; real Motive sends no reply. An echo + # reply makes libNatNet log "Received unrecognized message Message=10". + with running_unicast_server() as (server, command_port): + client = NatNetTestClient(timeout=0.5) + try: + client.send_message(command_port, st.MessageId.NAT_CONNECT) + client.recv_message() + + client.send_message(command_port, st.MessageId.NAT_KEEPALIVE) + with pytest.raises(socket.timeout): + client.recv_message() + + # Client stays registered and keeps receiving frames. + assert len(server.connected_clients) == 1 + finally: + client.close() + + +# ============================================================================= +# Malformed datagrams — registered client & recovery +# ============================================================================= + + +def test_unknown_message_from_registered_client_gets_no_reply(): + with running_unicast_server() as (server, command_port): + client = NatNetTestClient(timeout=0.5) + try: + client.send_message(command_port, st.MessageId.NAT_CONNECT) + client.recv_message() + + client.send_message(command_port, 999) + with pytest.raises(socket.timeout): + client.recv_message() + + assert len(server.connected_clients) == 1 + finally: + client.close() + + +def test_server_survives_malformed_burst_then_valid_connect(): + with running_unicast_server() as (server, command_port): + client = NatNetTestClient(timeout=2.0) + try: + client.send_raw(b"", command_port) + client.send_raw(b"\xff", command_port) + client.send_header_only(command_port, 999, declared_payload_len=50000) + client.send_message(command_port, st.MessageId.NAT_REQUEST_MODELDEF) + + client.send_message(command_port, st.MessageId.NAT_CONNECT) + message_id, _payload, _addr = client.recv_message() + finally: + client.close() + + assert message_id == int(st.MessageId.NAT_SERVERINFO) + assert len(server.connected_clients) == 1 diff --git a/tests/colcon_unit_test_packages.yaml b/tests/colcon_unit_test_packages.yaml index 65cdd38cc..e7a204e40 100644 --- a/tests/colcon_unit_test_packages.yaml +++ b/tests/colcon_unit_test_packages.yaml @@ -13,3 +13,9 @@ robot: - lidar_point_cloud_filter # Skips ament_copyright / flake8 / pep257 on Python packages; run linters separately. pytest_args: "-m not linter" + +# Simulation-side extensions (globbed under simulation/**//test). Collected by +# `pytest tests/` on the host runner; not part of the robot colcon workspace. +sim: + packages: + - optitrack.natnet.emulator diff --git a/tests/conftest.py b/tests/conftest.py index 62644eb98..e3078d146 100644 --- a/tests/conftest.py +++ b/tests/conftest.py @@ -73,6 +73,16 @@ def pytest_configure(config): run_dir = session.init_run_dir(AIRSTACK_ROOT) config.option.xmlpath = str(run_dir / "results.xml") + # Co-located unit tests import their own package (e.g. `optitrack.natnet.emulator`, + # `lidar_point_cloud_filter.validation_core`). Put each package/extension import + # root (the parent of its test/ dir) on sys.path so they resolve without a + # per-package conftest.py — a second conftest.py collides with this root one as + # module `conftest` under --import-mode=importlib and breaks `from conftest import`. + for d in unit_test_dirs(): + root = str(d.parent) + if root not in sys.path: + sys.path.insert(0, root) + # Collect co-located unit tests: their files live outside tests/, so add the # explicit non-linter test files to the collection args. Skip when an explicit # path was given on the CLI (args_source == ARGS) so `pytest tests/system/foo.py` diff --git a/tests/integration/natnet/README.md b/tests/integration/natnet/README.md new file mode 100644 index 000000000..4e9330c32 --- /dev/null +++ b/tests/integration/natnet/README.md @@ -0,0 +1,151 @@ +# NatNet ↔ robot autonomy integration + +Host-side NatNet wire-protocol tests that drive the Python emulator against +`natnet_ros2_node` in a real robot container. First resident of the +[`integration`](../README.md) tier (no sim, no GPU). + +Mark: `integration`. Filter this scenario with `tests/integration/natnet/`. + +For the **in-sim** end-to-end check (Isaac emulator + full stack), see +[Liveliness sentinel](#liveliness-sentinel-sim-end-to-end) below and +[`tests/system/test_liveliness.py`](../../system/test_liveliness.py). + +## What it verifies + +Three variants in [`test_natnet_integration.py`](test_natnet_integration.py). +All start a host-side `NatNetUnicastServer`, launch `natnet_ros2_node` in the +robot container pointed at the Docker bridge gateway, and assert a sustained +pose stream at **≥ 5 Hz** on the configured topic(s), e.g.: + +- `/{ROBOT_NAME}/perception/optitrack/drone/pose_cov` (wait for first message) +- `/{ROBOT_NAME}/perception/optitrack/drone` (Hz sample) + +| Test | Path | +|------|------| +| **`test_natnet_ros2_receives_drone_pose_hz`** | Hand-built `sFrameOfMocapData` frames enqueued on a raw `NatNetUnicastServer` (no USD). Minimal wire + SDK check. | +| **`test_natnet_ros2_receives_isaac_wrapper_pose_hz`** | Full Isaac data path: in-memory USD stage, `NatNetInterfaceConfig`, `author_interface`, `NatNetServerManager.sample_once()` on a moving prim — same sampling logic as the in-sim physics-step callback. Skips without `usd-core` (`pxr`). Pose-value fidelity is covered hermetically by the emulator's `test_pose_streaming.py` loopback. | +| **`test_natnet_ros2_multi_body_drone_and_target`** | Two bodies (drone id 1 + target id 100) with distinct relative topics; asserts both pose streams and that the target's `pose_cov` topic is **absent** (`body_pose_cov=false`). Exercises the multi-body profile + per-body `pose`/`pose_cov` toggles. | + +These tests **do not** start the full perception bringup or `LAUNCH_NATNET`; they +exec `natnet_ros2_node` directly with the flattened per-body params +(`body_names`/`body_ids`/`body_topics`/`body_pose`/`body_pose_cov`) and no MAVROS bridge. + +## Requirements + +- Docker daemon (robot-desktop container reachable from pytest). +- **`natnet_ros2_node` built** in the robot image (OptiTrack NatNet SDK is + license-gated — run `airstack setup --natnet`, then + `bws --packages-select natnet_ros2` in the container). Tests **skip** if the + node binary is missing. +- Host-side emulator package on `PYTHONPATH` (the test adds + `simulation/isaac-sim/extensions/optitrack.natnet.emulator` — not pip-installed + on the host). +- Ephemeral UDP ports on the host gateway IP (Docker default route as seen from + inside the container). + +The robot container comes from the shared **`robot_autonomy_stack`** fixture in +[`tests/conftest.py`](../../conftest.py) (see the [integration tier README](../README.md)). + +## Running + +```bash +# 1. One-time: NatNet SDK + build natnet_ros2 in the robot image +airstack setup --natnet # or NATNET_ACCEPT_LICENSE=1 airstack setup --natnet +docker exec airstack-robot-desktop-1 bash -lc 'bws --packages-select natnet_ros2' + +# 2a. Reuse an existing robot container (fast local iteration): +AUTOLAUNCH=false airstack up robot-desktop +pytest tests/integration/natnet/ -m integration -v + +# 2b. Let the harness bring the container up/down: +pytest tests/integration/natnet/ -m integration -v +``` +On CI / PR (write access): `/pytest -m integration` + +## Architecture + +``` +┌──────────────────────────────────────────────────────────────┐ +│ Host (pytest) │ +│ NatNetUnicastServer @ docker bridge gateway IP │ +│ • raw variant: hand-built frame queue │ +│ • Isaac variant: NatNetServerManager.sample_once(USD) │ +└────────────────────────────┬─────────────────────────────────┘ + │ UDP unicast (cmd + data ports) +┌────────────────────────────▼─────────────────────────────────┐ +│ Robot container (robot-desktop) │ +│ natnet_ros2_node (libNatNet 4.4 client) │ +│ → /{ROBOT_NAME}/{body topic}[/pose_cov] per configured body │ +└──────────────────────────────────────────────────────────────┘ +``` + +**In sim (liveliness tier):** the server runs inside the Isaac Sim container +(`172.31.0.200` by default). Use a NatNet Pegasus launch script +(`example_one_px4_pegasus_natnet_launch_script.py` or +`example_multi_px4_pegasus_natnet_launch_script.py`); `natnet_ros2` in the +robot stack connects via `natnet_config.yaml` (`server_ip` → emulator IP). + +**Catalog / MODELDEF:** The server holds a MODELDEF **wire cache** only +(`set_model_def_payload()`). Scene semantics (body names, streaming IDs, target +prim paths) come from the Isaac layer (`NatNetInterfaceConfig`, USD interface +prim, or launch-script `build_drone_config`). See the +[emulator README](../../../simulation/isaac-sim/extensions/optitrack.natnet.emulator/README.md). + +## Liveliness sentinel (sim end-to-end) + +The integration tier proves **robot client + host emulator** without Isaac. +The matching **system** check is +`TestLiveliness::test_natnet_pose_alive` in +[`test_liveliness.py`](../../system/test_liveliness.py): + +- **Gated on `LAUNCH_NATNET=true`** (skipped otherwise — normal liveliness runs + are unaffected). +- Asserts `/{robot_n}/{natnet pose topic}/pose_cov` ≥ 5 Hz per robot (the drone + body's configured topic — default `perception/optitrack/drone`). +- Override the checked topic with `NATNET_POSE_TOPIC` (default + `perception/optitrack/drone`). The sim body name (`NATNET_BODY_NAME`, default + `Drone`) is decoupled from the published topic, which the robot profile sets. + +Sim auto-start: set `ISAAC_SIM_SCRIPT_NAME` to a NatNet launch script and +`LAUNCH_NATNET=true` on the robot. Convenience bundle: +`airstack up --env-file overrides/isaac-natnet-vision.env` (NatNet script + +PX4 external-vision SITL profile). + +## libNatNet 4.4 unicast — verified wire contract + +The emulator is validated against the **real `libNatNet.so`** (not just the Python +`NatNetClient`) with a minimal C probe that registers `SetFrameReceivedCallback` +and `NatNet_SetLogCallback`. All of the following must hold for the SDK to deliver +frames to the callback: + +| Requirement | Why | +|-------------|-----| +| `NAT_CONNECT` → `sSender_Server` (279 B), name `Motive` | libNatNet reads `Motive 3.1 / NatNet 4.4` | +| `NAT_ECHOREQUEST` → `NAT_ECHORESPONSE` (16 B) | Prevents libNatNet assert | +| Frame ends with a **4-byte end-of-data tag** after `params` | libNatNet's frame unpacker reads it; without it the unpacked size mismatches `nDataBytes` and **every frame is silently dropped** | +| `NAT_FRAMEOFDATA` sent from the **data port** (source port == `data_port`) | libNatNet routes unicast frames by the server's data port. Frames sent from the **command** port are treated as command traffic and dropped — no error, no callback | +| `NAT_KEEPALIVE` gets **no reply** | An echo reply makes libNatNet log `Received unrecognized message Message=10` | + +With these in place the C probe reports `Server: Motive 3.1.0.0 NatNet 4.4.0.0`, +`data descriptions: 1`, and **~74 Hz** of frame callbacks. + +> The lenient Python `NatNetClient` accepts frames *without* the end-of-data tag +> and *on the command port*, which is why it appeared to work while libNatNet did +> not. Always validate against the C SDK. + +Full handshake notes and sniffing workflow: +[optitrack-development skill](../../../.agents/skills/optitrack-development/SKILL.md). + +## After changing natnet_ros2 or the emulator + +Rebuild in the robot container: + +```bash +docker exec airstack-robot-desktop-1 bash -lc 'bws --packages-select natnet_ros2' +``` + +Unit tests (protocol, serializers, Isaac wrapper loopback): + +```bash +pytest tests/sim/optitrack_natnet_emulator/ -m unit -v +``` diff --git a/tests/integration/natnet/test_natnet_integration.py b/tests/integration/natnet/test_natnet_integration.py new file mode 100644 index 000000000..43fa4a7f3 --- /dev/null +++ b/tests/integration/natnet/test_natnet_integration.py @@ -0,0 +1,376 @@ +# Copyright (c) 2024 Carnegie Mellon University +# MIT License - see LICENSE in the repository root for full text. +"""NatNet - robot autonomy integration tests. + +Host-side variants stream frames to ``natnet_ros2_node`` in the robot container and +assert pose topics stay alive at >= 5 Hz: (1) raw ``NatNetUnicastServer`` hand-built +single-body frames; (2) ``NatNetServerManager`` sampling an in-memory USD stage +(Isaac wrapper path, no sim/GPU); (3) a multi-body profile (drone + target) that +exercises per-body topic overrides and the pose / pose_cov toggles. + +The node is parameterised with the flattened per-body arrays +(``body_names`` / ``body_ids`` / ``body_topics`` / ``body_pose`` / ``body_pose_cov``) +that natnet_ros2.launch.py derives from a robot's natnet_config.yaml profile. + +Multi-robot (NUM_ROBOTS=3, per-robot profiles) is exercised in-sim by +``tests/system/test_liveliness.py::test_natnet_pose_alive``. +""" + +from __future__ import annotations + +import subprocess +import sys +import threading +import time + +import pytest + +from conftest import ( # noqa: E402 — pytest adds tests/ to sys.path + docker_exec, + repo_path, + ros2_env, + sample_hz, + wait_for_first_message, +) + +# Emulator is not pip-installed on the host; add extension root + test helpers. +_EXT_ROOT = repo_path("simulation/isaac-sim/extensions/optitrack.natnet.emulator") +for _path in (_EXT_ROOT, _EXT_ROOT / "test"): + if str(_path) not in sys.path: + sys.path.insert(0, str(_path)) + +from optitrack.natnet.emulator import NatNetUnicastServer, TransmissionType # noqa: E402 +from optitrack.natnet.emulator.server import natnet_data_types as dt # noqa: E402 +from natnet_test_helpers import ephemeral_udp_port # noqa: E402 + +pytestmark = pytest.mark.integration + +_ROBOT_SETUP = "/root/AirStack/robot/ros_ws/install/setup.bash" +_NATNET_NODE = "/root/AirStack/robot/ros_ws/install/natnet_ros2/lib/natnet_ros2/natnet_ros2_node" +_WARMUP_S = 2.0 +_STREAM_HOLD_S = 12.0 +_MIN_HZ = 5.0 + +# Robot image has route/netstat but not `ip`; /proc/net/route is always present. +_DEFAULT_GATEWAY_CMD = ( + """awk '$2 == "00000000" { printf "%d.%d.%d.%d\\n", """ + """"0x" substr($3,7,2), "0x" substr($3,5,2), "0x" substr($3,3,2), "0x" substr($3,1,2); exit }' """ + """/proc/net/route""" +) + + +def _docker_default_gateway(container: str) -> str: + result = docker_exec(container, _DEFAULT_GATEWAY_CMD, timeout=10) + gateway = result.stdout.strip() + if not gateway: + pytest.skip(f"Could not resolve default gateway inside {container}") + return gateway + + +def _container_env(container: str, var: str, default: str) -> str: + # ROBOT_NAME / ROS_DOMAIN_ID are set in .bashrc (login shell), not container ENV. + # .bashrc may print "Sourcing ..." to stdout; take the last line as the value. + result = docker_exec(container, f"bash -lc 'echo ${var}'") + lines = [line.strip() for line in result.stdout.splitlines() if line.strip()] + value = lines[-1] if lines else "" + return value if value else default + + +def _natnet_node_available(container: str) -> bool: + result = docker_exec(container, f"test -x {_NATNET_NODE} && echo yes || echo no") + return "yes" in result.stdout + + +def _stop_stale_natnet_nodes(container: str) -> None: + docker_exec(container, "pkill -f natnet_ros2_node || true") + time.sleep(0.5) + + +# Each body: (streaming_id, rigid_body_name). The raw server frame carries ids only; +# the node maps ids → topics via its body_* params. +_DRONE_BODY = (1, "Drone") +_TARGET_BODY = (100, "Target") + + +def _make_frame(frame_num: int, body_ids) -> dt.sFrameOfMocapData: + frame = dt.sFrameOfMocapData() + frame.iFrame = frame_num + frame.nRigidBodies = len(body_ids) + for slot, body_id in enumerate(body_ids): + rb = frame.RigidBodies[slot] + rb.ID = body_id + rb.qw = 1.0 + # Bit 0 = tracking valid; natnet_ros2 skips bodies without it (natnet_logic.hpp). + rb.params = 1 + return frame + + +def _frame_publisher( + server: NatNetUnicastServer, stop_event: threading.Event, body_ids=(1,) +) -> None: + frame_num = 0 + interval = 1.0 / server.publish_rate + while not stop_event.is_set(): + server.enqueue_mocap_data(_make_frame(frame_num, body_ids)) + frame_num += 1 + time.sleep(interval) + + +def _launch_natnet_node(container, host_ip, command_port, domain_id, bodies=None): + """Start natnet_ros2_node in the container pointed at the host emulator. + + ``bodies`` is a list of (id, name, topic, pose, pose_cov); defaults to a single + Drone body on topic ``perception/optitrack/drone`` (the shipped config default). + """ + if bodies is None: + bodies = [(1, "Drone", "perception/optitrack/drone", "true", "true")] + ids = ",".join(str(b[0]) for b in bodies) + names = ",".join(b[1] for b in bodies) + topics = ",".join(b[2] for b in bodies) + pose = ",".join(b[3] for b in bodies) + pose_cov = ",".join(b[4] for b in bodies) + launch_cmd = ( + f"bash -lc '{ros2_env(_ROBOT_SETUP, domain_id)} && " + f"exec {_NATNET_NODE} --ros-args " + f"-p server_ip:={host_ip} " + f"-p command_port:={command_port} " + f"-p body_names:=[{names}] " + f"-p body_ids:=[{ids}] " + f"-p body_topics:=[{topics}] " + f"-p body_pose:=[{pose}] " + f"-p body_pose_cov:=[{pose_cov}]'" + ) + return subprocess.Popen( + ["docker", "exec", container, "bash", "-c", launch_cmd], + stdout=subprocess.PIPE, + stderr=subprocess.STDOUT, + text=True, + ) + + +def _assert_pose_stream( + container, robot_name, domain_id, topic="perception/optitrack/drone", pose_cov=True +): + """Wait for the pose topic then assert a sustained rate >= _MIN_HZ. + + A body configured with ``body_pose_cov=false`` never publishes the ``/pose_cov`` + variant, so detect the first message on whichever topic the body actually emits. + """ + pose_topic = f"/{robot_name}/{topic}" + detect_topic = f"{pose_topic}/pose_cov" if pose_cov else pose_topic + + time.sleep(_WARMUP_S) + first_msg_s = wait_for_first_message( + container, detect_topic, domain_id, _ROBOT_SETUP, timeout=int(_STREAM_HOLD_S) + ) + assert first_msg_s is not None, ( + f"No messages on {detect_topic} within {_STREAM_HOLD_S}s " + "(NatNet connect or frame stream failed)" + ) + hz = sample_hz( + container, + pose_topic, + domain_id, + _ROBOT_SETUP, + duration=min(8, int(_STREAM_HOLD_S - first_msg_s)), + window=20, + ) + assert hz is not None, f"No sustained stream on {pose_topic}" + assert hz >= _MIN_HZ, f"Expected >= {_MIN_HZ} Hz on {pose_topic}, got {hz}" + + +def _terminate(proc) -> None: + if proc is None: + return + proc.terminate() + try: + proc.wait(timeout=5) + except subprocess.TimeoutExpired: + proc.kill() + + +def test_natnet_ros2_receives_drone_pose_hz(robot_autonomy_stack): + """Raw-server path: hand-built frames on NatNetUnicastServer.""" + container = robot_autonomy_stack["container"] + + if not _natnet_node_available(container): + pytest.skip( + "natnet_ros2_node not built — run airstack setup (NatNet SDK) and " + "bws --packages-select natnet_ros2 in the robot container" + ) + + _stop_stale_natnet_nodes(container) + + host_ip = _docker_default_gateway(container) + command_port = ephemeral_udp_port(host_ip) + robot_name = _container_env(container, "ROBOT_NAME", "robot_1") + domain_id = int(_container_env(container, "ROS_DOMAIN_ID", "0")) + + server = NatNetUnicastServer( + local_interface=host_ip, + transmission_type=TransmissionType.UNICAST, + multicast_address=None, + command_port=command_port, + ) + server.publish_rate = 50 + + stop_event = threading.Event() + publisher = threading.Thread( + target=_frame_publisher, args=(server, stop_event), daemon=True + ) + + node_proc: subprocess.Popen[str] | None = None + try: + # Seed dummy frames before the client connects; keep streaming the whole window. + publisher.start() + time.sleep(0.1) + server.start() + node_proc = _launch_natnet_node(container, host_ip, command_port, domain_id) + _assert_pose_stream(container, robot_name, domain_id) + finally: + stop_event.set() + publisher.join(timeout=2.0) + _terminate(node_proc) + server.shutdown() + + +def test_natnet_ros2_receives_isaac_wrapper_pose_hz(robot_autonomy_stack): + """Isaac-wrapper path: NatNetServerManager.sample_once on a moving USD prim. + + Tests that the wrapper feeds the real robot client end-to-end. Pose-value fidelity + is covered by test_pose_streaming.py loopback. + """ + pytest.importorskip("pxr") + import math + + from pxr import Gf, Usd, UsdGeom + + from optitrack.natnet.emulator.isaac import ( + BodyBinding, + NatNetInterfaceConfig, + NatNetServerManager, + author_interface, + ) + + container = robot_autonomy_stack["container"] + if not _natnet_node_available(container): + pytest.skip("natnet_ros2_node not built — run airstack setup (NatNet SDK)") + + _stop_stale_natnet_nodes(container) + + host_ip = _docker_default_gateway(container) + command_port = ephemeral_udp_port(host_ip) + data_port = ephemeral_udp_port(host_ip) + while data_port == command_port: + data_port = ephemeral_udp_port(host_ip) + robot_name = _container_env(container, "ROBOT_NAME", "robot_1") + domain_id = int(_container_env(container, "ROS_DOMAIN_ID", "0")) + + stage = Usd.Stage.CreateInMemory() + xform = UsdGeom.Xform.Define(stage, "/World/base_link") + translate_op = xform.AddTranslateOp() + translate_op.Set(Gf.Vec3d(0.0, 0.0, 1.0)) + cfg = NatNetInterfaceConfig( + server_ip=host_ip, + command_port=command_port, + data_port=data_port, + publish_rate=50.0, + bodies=[BodyBinding("Drone", "/World/base_link", streaming_id=1)], + ) + author_interface(stage, "/World/NatNetInterface", cfg) + + manager = NatNetServerManager(server_factory=None) # real server factory + stop_event = threading.Event() + + def _sampler(): + # Stand in for the in-sim physics-step callback: move the prim and sample. + interval = 1.0 / cfg.publish_rate + t = 0.0 + while not stop_event.is_set(): + translate_op.Set(Gf.Vec3d(math.sin(t), 0.0, 1.0)) + manager.sample_once(stage) + t += interval + time.sleep(interval) + + sampler = threading.Thread(target=_sampler, daemon=True) + + node_proc: subprocess.Popen[str] | None = None + try: + assert manager.start_server(cfg) is True + sampler.start() + time.sleep(0.1) + node_proc = _launch_natnet_node(container, host_ip, command_port, domain_id) + _assert_pose_stream(container, robot_name, domain_id) + finally: + stop_event.set() + sampler.join(timeout=2.0) + _terminate(node_proc) + manager.stop_server() + + +def test_natnet_ros2_multi_body_drone_and_target(robot_autonomy_stack): + """Multi-body profile: one robot tracks a drone + a static target. + + Streams two bodies (drone id 1, target id 100) and configures the node like a + robot profile with two bodies and distinct relative topics. Asserts: the drone + pose streams >= 5 Hz on its custom topic; the target pose streams on its own + topic; and the target's pose_cov topic is absent (body_pose_cov=false). + """ + container = robot_autonomy_stack["container"] + + if not _natnet_node_available(container): + pytest.skip("natnet_ros2_node not built — run airstack setup (NatNet SDK)") + + _stop_stale_natnet_nodes(container) + + host_ip = _docker_default_gateway(container) + command_port = ephemeral_udp_port(host_ip) + robot_name = _container_env(container, "ROBOT_NAME", "robot_1") + domain_id = int(_container_env(container, "ROS_DOMAIN_ID", "0")) + + server = NatNetUnicastServer( + local_interface=host_ip, + transmission_type=TransmissionType.UNICAST, + multicast_address=None, + command_port=command_port, + ) + server.publish_rate = 50 + + bodies = [ + (_DRONE_BODY[0], _DRONE_BODY[1], "perception/optitrack/drone", "true", "true"), + (_TARGET_BODY[0], _TARGET_BODY[1], "perception/optitrack/target", "true", "false"), + ] + body_ids = (_DRONE_BODY[0], _TARGET_BODY[0]) + + stop_event = threading.Event() + publisher = threading.Thread( + target=_frame_publisher, args=(server, stop_event, body_ids), daemon=True + ) + + node_proc: subprocess.Popen[str] | None = None + try: + publisher.start() + time.sleep(0.1) + server.start() + node_proc = _launch_natnet_node(container, host_ip, command_port, domain_id, bodies) + # Drone (pose + pose_cov) and target (pose only) both stream. + _assert_pose_stream(container, robot_name, domain_id, "perception/optitrack/drone") + _assert_pose_stream( + container, robot_name, domain_id, "perception/optitrack/target", pose_cov=False + ) + + # body_pose_cov=false → the target pose_cov publisher must not exist. + target_cov = f"/{robot_name}/perception/optitrack/target/pose_cov" + topics = docker_exec( + container, + f"bash -lc '{ros2_env(_ROBOT_SETUP, domain_id)} && ros2 topic list'", + timeout=15, + ).stdout + assert target_cov not in topics.split(), ( + f"{target_cov} should not exist when body_pose_cov=false; topics:\n{topics}" + ) + finally: + stop_event.set() + publisher.join(timeout=2.0) + _terminate(node_proc) + server.shutdown() From 7663b5734ab56ae3e71ae3670d2761445ae075e8 Mon Sep 17 00:00:00 2001 From: John Date: Thu, 23 Jul 2026 14:15:14 -0400 Subject: [PATCH 21/33] =?UTF-8?q?test(natnet):=20host=20integration=20test?= =?UTF-8?q?s=20=E2=80=94=20emulator=20server=20=E2=86=92=20natnet=5Fros2?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Drive the real natnet_ros2 client from the host NatNet server emulator and check the drone pose reaches ROS at rate (single-body and multi-body profiles). No sim, no GPU — uses the base's `robot_autonomy_stack` fixture + `integration` mark. The Isaac-wrapper variant lands with the Isaac wrapper PR. Co-Authored-By: Claude Opus 4.8 --- .../natnet/test_natnet_integration.py | 74 ------------------- 1 file changed, 74 deletions(-) diff --git a/tests/integration/natnet/test_natnet_integration.py b/tests/integration/natnet/test_natnet_integration.py index 43fa4a7f3..e386fd1ee 100644 --- a/tests/integration/natnet/test_natnet_integration.py +++ b/tests/integration/natnet/test_natnet_integration.py @@ -234,80 +234,6 @@ def test_natnet_ros2_receives_drone_pose_hz(robot_autonomy_stack): server.shutdown() -def test_natnet_ros2_receives_isaac_wrapper_pose_hz(robot_autonomy_stack): - """Isaac-wrapper path: NatNetServerManager.sample_once on a moving USD prim. - - Tests that the wrapper feeds the real robot client end-to-end. Pose-value fidelity - is covered by test_pose_streaming.py loopback. - """ - pytest.importorskip("pxr") - import math - - from pxr import Gf, Usd, UsdGeom - - from optitrack.natnet.emulator.isaac import ( - BodyBinding, - NatNetInterfaceConfig, - NatNetServerManager, - author_interface, - ) - - container = robot_autonomy_stack["container"] - if not _natnet_node_available(container): - pytest.skip("natnet_ros2_node not built — run airstack setup (NatNet SDK)") - - _stop_stale_natnet_nodes(container) - - host_ip = _docker_default_gateway(container) - command_port = ephemeral_udp_port(host_ip) - data_port = ephemeral_udp_port(host_ip) - while data_port == command_port: - data_port = ephemeral_udp_port(host_ip) - robot_name = _container_env(container, "ROBOT_NAME", "robot_1") - domain_id = int(_container_env(container, "ROS_DOMAIN_ID", "0")) - - stage = Usd.Stage.CreateInMemory() - xform = UsdGeom.Xform.Define(stage, "/World/base_link") - translate_op = xform.AddTranslateOp() - translate_op.Set(Gf.Vec3d(0.0, 0.0, 1.0)) - cfg = NatNetInterfaceConfig( - server_ip=host_ip, - command_port=command_port, - data_port=data_port, - publish_rate=50.0, - bodies=[BodyBinding("Drone", "/World/base_link", streaming_id=1)], - ) - author_interface(stage, "/World/NatNetInterface", cfg) - - manager = NatNetServerManager(server_factory=None) # real server factory - stop_event = threading.Event() - - def _sampler(): - # Stand in for the in-sim physics-step callback: move the prim and sample. - interval = 1.0 / cfg.publish_rate - t = 0.0 - while not stop_event.is_set(): - translate_op.Set(Gf.Vec3d(math.sin(t), 0.0, 1.0)) - manager.sample_once(stage) - t += interval - time.sleep(interval) - - sampler = threading.Thread(target=_sampler, daemon=True) - - node_proc: subprocess.Popen[str] | None = None - try: - assert manager.start_server(cfg) is True - sampler.start() - time.sleep(0.1) - node_proc = _launch_natnet_node(container, host_ip, command_port, domain_id) - _assert_pose_stream(container, robot_name, domain_id) - finally: - stop_event.set() - sampler.join(timeout=2.0) - _terminate(node_proc) - manager.stop_server() - - def test_natnet_ros2_multi_body_drone_and_target(robot_autonomy_stack): """Multi-body profile: one robot tracks a drone + a static target. From 461204cd74293573e985c9396d9f432f367d9a64 Mon Sep 17 00:00:00 2001 From: John Date: Thu, 23 Jul 2026 14:15:42 -0400 Subject: [PATCH 22/33] 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 0e373509d..b3a9298b9 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -18,6 +18,7 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0 - `integration` test tier (`tests/integration/`, `integration` mark) with a shared `robot_autonomy_stack` fixture (robot container, no sim/GPU) - `waypoint_flight` system test (`tests/system/test_waypoint_flight.py`): takeoff → ordered waypoint route via `NavigateTask` (dispatched as a dense plan) → land, judged on the odometry track by the standalone stdlib-only `tests/waypoint_checker.py` (in-order corridor arrival within `--waypoint-tolerance`, final goal within `--goal-tolerance`, per-waypoint `--waypoint-timeout`); validated end-to-end in Isaac Sim; serves as the standard acceptance check after integrating or swapping a planner module - Real-robot PX4 external-vision fusion in `natnet_ros2` (OptiTrack mocap → EKF2): `mavros_gp_origin` (geoid-corrected synthetic GPS origin so `local_position.z` == OptiTrack z, fixing the ~36 m boot offset), `vision_pose_converter`, and a PX4 param **checker** (`px4_param_setter`, `auto_set` off by default; `on_mismatch` warn/halt) — setup guide at `docs/robot/px4_external_vision.md` +- NatNet server emulator (`optitrack.natnet.emulator`, protocol core) — pure-Python OptiTrack Motive server emulation so `natnet_ros2` can be driven without hardware; host integration tests (`tests/integration/natnet/`) wire it to the robot client ### Changed From bf8cd01f9a2ed652282fae8104b16d2484f2c722 Mon Sep 17 00:00:00 2001 From: John Date: Thu, 23 Jul 2026 14:17:52 -0400 Subject: [PATCH 23/33] =?UTF-8?q?feat(sim):=20Isaac=20wrapper=20for=20the?= =?UTF-8?q?=20NatNet=20emulator=20(USD=20scene=20=E2=86=92=20server)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The Isaac integration layer that maps a live USD scene onto the NatNet server: catalog/config/frames/manager/scene_setup/ui_extension/usd_bindings, the extension manifest (config/), and the USD schema. Adds the natnet Pegasus launch scripts that spawn the emulator alongside PX4 in Isaac Sim, the isaac unit tests (incl. a float-tolerance loosen on the pose round-trip for float32/USD noise), and the Isaac-wrapper host integration test. scipy + usd-core added for the emulator's USD/pose-sampling tests. Co-Authored-By: Claude Opus 4.8 --- .../config/extension.toml | 23 + .../natnet/emulator/isaac/__init__.py | 62 +++ .../natnet/emulator/isaac/catalog.py | 53 ++ .../optitrack/natnet/emulator/isaac/config.py | 234 +++++++++ .../optitrack/natnet/emulator/isaac/frames.py | 129 +++++ .../natnet/emulator/isaac/manager.py | 403 ++++++++++++++++ .../natnet/emulator/isaac/scene_setup.py | 142 ++++++ .../natnet/emulator/isaac/ui_extension.py | 453 ++++++++++++++++++ .../natnet/emulator/isaac/usd_bindings.py | 216 +++++++++ .../schema/schema.usda | 100 ++++ .../test/test_catalog.py | 111 +++++ .../test/test_discovery.py | 29 ++ .../test/test_frames.py | 129 +++++ .../test/test_interface_authoring.py | 126 +++++ .../test/test_interface_config.py | 186 +++++++ .../test/test_pose_sampling.py | 238 +++++++++ .../test/test_pose_streaming.py | 86 ++++ .../test/test_scene_setup.py | 100 ++++ .../test/test_server_from_config.py | 85 ++++ .../test/test_server_lifecycle.py | 155 ++++++ .../test/test_target_resolution.py | 84 ++++ ..._multi_px4_pegasus_natnet_launch_script.py | 256 ++++++++++ .../example_one_px4_pegasus_launch_script.py | 22 + ...le_one_px4_pegasus_natnet_launch_script.py | 273 +++++++++++ .../natnet/test_natnet_integration.py | 74 +++ tests/requirements.txt | 2 + 26 files changed, 3771 insertions(+) create mode 100644 simulation/isaac-sim/extensions/optitrack.natnet.emulator/config/extension.toml create mode 100644 simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/__init__.py create mode 100644 simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/catalog.py create mode 100644 simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/config.py create mode 100644 simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/frames.py create mode 100644 simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/manager.py create mode 100644 simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/scene_setup.py create mode 100644 simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/ui_extension.py create mode 100644 simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/usd_bindings.py create mode 100644 simulation/isaac-sim/extensions/optitrack.natnet.emulator/schema/schema.usda create mode 100644 simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_catalog.py create mode 100644 simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_discovery.py create mode 100644 simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_frames.py create mode 100644 simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_interface_authoring.py create mode 100644 simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_interface_config.py create mode 100644 simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_pose_sampling.py create mode 100644 simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_pose_streaming.py create mode 100644 simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_scene_setup.py create mode 100644 simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_server_from_config.py create mode 100644 simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_server_lifecycle.py create mode 100644 simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_target_resolution.py create mode 100644 simulation/isaac-sim/launch_scripts/example_multi_px4_pegasus_natnet_launch_script.py create mode 100644 simulation/isaac-sim/launch_scripts/example_one_px4_pegasus_natnet_launch_script.py diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/config/extension.toml b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/config/extension.toml new file mode 100644 index 000000000..a5df394da --- /dev/null +++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/config/extension.toml @@ -0,0 +1,23 @@ +[package] +version = "0.1.0" +title = "OptiTrack NatNet Emulator" +description = "NatNet UDP server emulator for Isaac Sim integration with natnet_ros2" +category = "Simulation" +keywords = ["optitrack", "natnet", "mocap", "simulation"] + +[dependencies] +"omni.isaac.core" = {} +"omni.usd" = {} +"omni.ui" = {} +"omni.kit.menu.utils" = {} + +# Pure transport/types package (no Kit UI; safe to import anywhere). +[[python.module]] +name = "optitrack.natnet.emulator" + +# Kit UI entry point: NatNetEmulatorExtension (menu + config-prim authoring window). +[[python.module]] +name = "optitrack.natnet.emulator.isaac.ui_extension" + +[python.build-system] +requires = ["setuptools"] diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/__init__.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/__init__.py new file mode 100644 index 000000000..ffd1b9e93 --- /dev/null +++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/__init__.py @@ -0,0 +1,62 @@ +# Copyright (c) 2024 Carnegie Mellon University +# MIT License - see LICENSE in the repository root for full text. +"""Isaac Sim integration for the NatNet emulator (stage-driven config prim). + +``config`` is pure Python. ``usd_bindings`` imports ``pxr`` lazily, so +importing this package is safe in non-Isaac environments. +""" + +from .config import ( + BodyBinding, + NatNetInterfaceConfig, + body_attr_name, + make_instance_key, +) +from .catalog import build_catalog, find_duplicate_targets +from .frames import BodySample, build_frame, make_rigid_body_data +from .manager import NatNetServerManager, default_server_factory, format_interface +from .scene_setup import ( + DEFAULT_INTERFACE_PATH, + DEFAULT_TARGET_PATH, + DEFAULT_TARGET_POSITION, + DEFAULT_TARGET_STREAMING_ID, + author_static_target, + build_drone_config, + start_drone_natnet_server, +) +from .usd_bindings import ( + author_interface, + find_interfaces, + is_interface, + read_interface, + read_world_pose, + resolve_targets, +) + +__all__ = [ + "DEFAULT_INTERFACE_PATH", + "DEFAULT_TARGET_PATH", + "DEFAULT_TARGET_POSITION", + "DEFAULT_TARGET_STREAMING_ID", + "BodyBinding", + "BodySample", + "NatNetInterfaceConfig", + "NatNetServerManager", + "author_interface", + "author_static_target", + "body_attr_name", + "build_catalog", + "build_drone_config", + "build_frame", + "default_server_factory", + "find_duplicate_targets", + "find_interfaces", + "format_interface", + "is_interface", + "make_instance_key", + "make_rigid_body_data", + "read_interface", + "read_world_pose", + "resolve_targets", + "start_drone_natnet_server", +] diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/catalog.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/catalog.py new file mode 100644 index 000000000..9c3bfaa35 --- /dev/null +++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/catalog.py @@ -0,0 +1,53 @@ +# Copyright (c) 2024 Carnegie Mellon University +# MIT License - see LICENSE in the repository root for full text. +""" +Turn a :class:`NatNetInterfaceConfig` into the server's MODELDEF catalog +(``sDataDescriptions`` of rigid bodies). Pure Python + ctypes (the ``server`` +package is stdlib-only), so this is hermetically unit-testable — no USD, no Kit. +""" + +from __future__ import annotations + +from ..server.natnet_common import ModelLimits +from ..server.natnet_model_types import DataDescriptors, sDataDescriptions +from .config import NatNetInterfaceConfig + +# szName is null-terminated on the wire; reserve one byte for the terminator. +_MAX_NAME_BYTES = int(ModelLimits.MAX_NAMELENGTH) - 1 +_MAX_MODELS = int(ModelLimits.MAX_MODELS) + + +def build_catalog(config: NatNetInterfaceConfig) -> sDataDescriptions: + """Build an ``sDataDescriptions`` rigid-body catalog from the config bodies. + + No bodies -> an empty catalog (``nDataDescriptions == 0``). Names longer than + the NatNet name field are truncated. Raises ``ValueError`` if there are more + bodies than the protocol allows. + """ + bodies = config.bodies + if len(bodies) > _MAX_MODELS: + raise ValueError( + f"Too many bodies for one catalog: {len(bodies)} > {_MAX_MODELS} (MAX_MODELS)" + ) + + descriptions = sDataDescriptions() + descriptions.nDataDescriptions = len(bodies) + for i, body in enumerate(bodies): + desc = descriptions.arrDataDescriptions[i] + desc.type = int(DataDescriptors.Descriptor_RigidBody) + rb = desc.RigidBodyDescription + rb.szName = body.rigid_body_name.encode("utf-8")[:_MAX_NAME_BYTES] + rb.ID = int(body.streaming_id) + rb.parentID = int(body.parent_id) + rb.offsetqw = 1.0 # identity quaternion offset + rb.nMarkers = 0 + return descriptions + + +def find_duplicate_targets(config: NatNetInterfaceConfig) -> list[str]: + """Return target prim paths referenced by more than one body (empties ignored).""" + counts: dict[str, int] = {} + for body in config.bodies: + if body.target_prim: + counts[body.target_prim] = counts.get(body.target_prim, 0) + 1 + return [path for path, count in counts.items() if count > 1] diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/config.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/config.py new file mode 100644 index 000000000..56c866ab1 --- /dev/null +++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/config.py @@ -0,0 +1,234 @@ +# Copyright (c) 2024 Carnegie Mellon University +# MIT License - see LICENSE in the repository root for full text. +"""Pure-Python config model for the stage-driven NatNet interface. + +The USD binding layer (author/read against a ``Usd.Stage``) +lives in ``usd_bindings.py`` and depends on this model. + +Attribute names follow the multi-apply schema convention +(``natnet:body::``). +The custom-attribute backing is for a future typed applied schema. +""" + +from __future__ import annotations + +from dataclasses import dataclass, field +from typing import Any, Iterable, Mapping + +# --- attribute name constants (USD property names) ----------------------------- + +ATTR_NAMESPACE = "natnet" +MARKER_ATTR = "natnet:isInterface" + +ATTR_SERVER_ENABLED = "natnet:serverEnabled" +ATTR_SERVER_IP = "natnet:serverIp" +ATTR_MODE = "natnet:mode" +ATTR_MULTICAST_ADDR = "natnet:multicastAddr" +ATTR_COMMAND_PORT = "natnet:commandPort" +ATTR_DATA_PORT = "natnet:dataPort" +ATTR_PUBLISH_RATE = "natnet:publishRate" +ATTR_NATNET_VERSION = "natnet:natnetVersion" +ATTR_UP_AXIS = "natnet:upAxis" + +ATTR_POSE_NOISE_ENABLED = "natnet:poseNoiseEnabled" +ATTR_POSE_NOISE_STD_METERS = "natnet:poseNoiseStdMeters" +ATTR_POSE_NOISE_ROTATION_DEG = "natnet:poseNoiseRotationDeg" + +BODY_PREFIX = "natnet:body:" +BODY_FIELD_RIGID_BODY_NAME = "rigidBodyName" +BODY_FIELD_STREAMING_ID = "streamingId" +BODY_FIELD_PARENT_ID = "parentId" +BODY_FIELD_TARGET = "target" + +VALID_MODES = ("unicast", "multicast") + +# Streamed up-axis. Motive exposes an "Up Axis" setting; the reference natnet_ros2 +# driver requires it set to Z and passes coordinates through untouched. +# Isaac Sim / USD is natively Z-up, so "Z" is a pass-through that matches the rest of the +# AirStack stack (default). "Y" emulates a default (Y-up) Motive by rotating the +# streamed pose -90deg about X. +VALID_UP_AXES = ("Y", "Z") + +# defaults shared with NatNetUnicastServer +DEFAULT_SERVER_IP = "172.31.0.200" +DEFAULT_MULTICAST_ADDR = "239.255.42.99" +DEFAULT_COMMAND_PORT = 1510 +DEFAULT_DATA_PORT = 1511 +DEFAULT_PUBLISH_RATE = 100.0 +DEFAULT_NATNET_VERSION = "4.4.0.0" +DEFAULT_UP_AXIS = "Z" +DEFAULT_POSE_NOISE_ENABLED = True +DEFAULT_POSE_NOISE_STD_METERS = 0.0005 +DEFAULT_POSE_NOISE_ROTATION_DEG = 0.05 + + +def body_attr_name(key: str, field_name: str) -> str: + """USD property name for a body-binding field on the given instance key.""" + return f"{BODY_PREFIX}{key}:{field_name}" + + +def make_instance_key(name: str, used: set[str]) -> str: + """Derive a valid, unique multi-apply instance token from a rigid body name. + + USD property/instance tokens must be identifier-like; sanitize non-alnum chars + to underscores and disambiguate collisions with a numeric suffix. + """ + sanitized = "".join(c if c.isalnum() else "_" for c in name).strip("_") + if not sanitized: + sanitized = "body" + if sanitized[0].isdigit(): + sanitized = f"b_{sanitized}" + key = sanitized + i = 1 + while key in used: + key = f"{sanitized}_{i}" + i += 1 + used.add(key) + return key + + +@dataclass +class BodyBinding: + """One tracked rigid body: a Motive name/ID mapped to a USD prim path.""" + + rigid_body_name: str + target_prim: str + streaming_id: int = 1 + parent_id: int = -1 + + @classmethod + def from_dict(cls, data: Mapping[str, Any], *, target_prim: str | None = None) -> "BodyBinding": + d = dict(data) + resolved_target = target_prim if target_prim is not None else d.get("target_prim") + if not resolved_target: + raise ValueError("BodyBinding requires a target_prim (USD path of the tracked prim)") + if "rigid_body_name" not in d: + raise ValueError("BodyBinding requires a rigid_body_name") + return cls( + rigid_body_name=str(d["rigid_body_name"]), + target_prim=str(resolved_target), + streaming_id=int(d.get("streaming_id", 1)), + parent_id=int(d.get("parent_id", -1)), + ) + + def to_dict(self) -> dict[str, Any]: + return { + "rigid_body_name": self.rigid_body_name, + "target_prim": self.target_prim, + "streaming_id": self.streaming_id, + "parent_id": self.parent_id, + } + + +def _normalize_bodies(bodies: Any) -> list[BodyBinding]: + """Accept a list of dicts/BodyBindings, or a ``{prim_path: {...}}`` mapping.""" + if bodies is None: + return [] + out: list[BodyBinding] = [] + if isinstance(bodies, Mapping): + for prim_path, body in bodies.items(): + out.append(BodyBinding.from_dict(body, target_prim=prim_path)) + return out + if isinstance(bodies, Iterable): + for body in bodies: + if isinstance(body, BodyBinding): + out.append(body) + else: + out.append(BodyBinding.from_dict(body)) + return out + raise ValueError(f"`bodies` must be a list or a mapping, got {type(bodies).__name__}") + + +@dataclass +class NatNetInterfaceConfig: + """Server-level config plus the body catalog for one NatNet interface prim.""" + + server_enabled: bool = True + server_ip: str = DEFAULT_SERVER_IP + mode: str = "unicast" + multicast_addr: str = DEFAULT_MULTICAST_ADDR + command_port: int = DEFAULT_COMMAND_PORT + data_port: int = DEFAULT_DATA_PORT + publish_rate: float = DEFAULT_PUBLISH_RATE + natnet_version: str = DEFAULT_NATNET_VERSION + up_axis: str = DEFAULT_UP_AXIS + pose_noise_enabled: bool = DEFAULT_POSE_NOISE_ENABLED + pose_noise_std_meters: float = DEFAULT_POSE_NOISE_STD_METERS + pose_noise_rotation_deg: float = DEFAULT_POSE_NOISE_ROTATION_DEG + bodies: list[BodyBinding] = field(default_factory=list) + + @classmethod + def from_dict(cls, data: Mapping[str, Any]) -> "NatNetInterfaceConfig": + d = dict(data) + return cls( + server_enabled=bool(d.get("server_enabled", True)), + server_ip=str(d.get("server_ip", DEFAULT_SERVER_IP)), + mode=str(d.get("mode", "unicast")), + multicast_addr=str(d.get("multicast_addr", DEFAULT_MULTICAST_ADDR)), + command_port=int(d.get("command_port", DEFAULT_COMMAND_PORT)), + data_port=int(d.get("data_port", DEFAULT_DATA_PORT)), + publish_rate=float(d.get("publish_rate", DEFAULT_PUBLISH_RATE)), + natnet_version=str(d.get("natnet_version", DEFAULT_NATNET_VERSION)), + up_axis=str(d.get("up_axis", DEFAULT_UP_AXIS)).upper(), + pose_noise_enabled=bool(d.get("pose_noise_enabled", DEFAULT_POSE_NOISE_ENABLED)), + pose_noise_std_meters=float(d.get("pose_noise_std_meters", DEFAULT_POSE_NOISE_STD_METERS)), + pose_noise_rotation_deg=float(d.get("pose_noise_rotation_deg", DEFAULT_POSE_NOISE_ROTATION_DEG)), + bodies=_normalize_bodies(d.get("bodies")), + ) + + def to_dict(self) -> dict[str, Any]: + return { + "server_enabled": self.server_enabled, + "server_ip": self.server_ip, + "mode": self.mode, + "multicast_addr": self.multicast_addr, + "command_port": self.command_port, + "data_port": self.data_port, + "publish_rate": self.publish_rate, + "natnet_version": self.natnet_version, + "up_axis": self.up_axis, + "pose_noise_enabled": self.pose_noise_enabled, + "pose_noise_std_meters": self.pose_noise_std_meters, + "pose_noise_rotation_deg": self.pose_noise_rotation_deg, + "bodies": [b.to_dict() for b in self.bodies], + } + + def validate(self) -> "NatNetInterfaceConfig": + """Raise ``ValueError`` (aggregating all problems) if the config is invalid.""" + errors: list[str] = [] + if self.mode not in VALID_MODES: + errors.append(f"mode must be one of {VALID_MODES}, got {self.mode!r}") + if str(self.up_axis).upper() not in VALID_UP_AXES: + errors.append(f"up_axis must be one of {VALID_UP_AXES}, got {self.up_axis!r}") + for port_name, port in (("command_port", self.command_port), ("data_port", self.data_port)): + if not (0 < port < 65536): + errors.append(f"{port_name} must be in 1..65535, got {port}") + if self.command_port == self.data_port: + errors.append("command_port and data_port must differ") + if self.publish_rate <= 0: + errors.append(f"publish_rate must be > 0, got {self.publish_rate}") + if self.pose_noise_std_meters < 0: + errors.append( + f"pose_noise_std_meters must be >= 0, got {self.pose_noise_std_meters}" + ) + if self.pose_noise_rotation_deg < 0: + errors.append( + f"pose_noise_rotation_deg must be >= 0, got {self.pose_noise_rotation_deg}" + ) + for i, body in enumerate(self.bodies): + if not body.rigid_body_name: + errors.append(f"body[{i}] rigid_body_name must be non-empty") + names = [b.rigid_body_name for b in self.bodies] + if len(set(names)) != len(names): + errors.append("rigid_body_name values must be unique across bodies") + ids = [b.streaming_id for b in self.bodies] + if len(set(ids)) != len(ids): + errors.append("streaming_id values must be unique across bodies") + if errors: + raise ValueError("Invalid NatNetInterfaceConfig: " + "; ".join(errors)) + return self + + def assign_instance_keys(self) -> list[tuple[str, BodyBinding]]: + """Pair each body with a deterministic, unique multi-apply instance key.""" + used: set[str] = set() + return [(make_instance_key(b.rigid_body_name, used), b) for b in self.bodies] diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/frames.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/frames.py new file mode 100644 index 000000000..576969b80 --- /dev/null +++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/frames.py @@ -0,0 +1,129 @@ +# Copyright (c) 2024 Carnegie Mellon University +# MIT License - see LICENSE in the repository root for full text. +"""Pose -> NatNet frame conversion (the data-enqueue path). + +Pure Python + ctypes. Sampled prim world poses become an +``sFrameOfMocapData`` of rigid bodies that the server +streams to the client. + +**Frame convention:** Motive exposes an "Up Axis" setting. AirStack's ``natnet_ros2`` +requires it set to **Z** and copy the rigid-body pose straight through +(``rb_to_pose`` is an identity copy). Isaac Sim is Z-up, so the default ``up_axis="Z"`` +emits the prim's USD world pose **as-is**. ``up_axis="Y"`` emulates a default (Y-up) +Motive by rotating the pose -90 deg about X. + +**params bits** (must match the client's ``is_tracking_valid`` / ``model_list_changed``): +- ``0x01`` on a rigid body marks tracking valid — the client *skips* bodies without it. +- ``0x02`` on the frame signals the model list changed so the client re-requests MODELDEF(set the frame after the catalog changes, e.g. a body added live). +""" + +from __future__ import annotations + +import math +import numpy as np +from dataclasses import dataclass +from scipy.spatial.transform import Rotation +from ..server.natnet_data_types import sFrameOfMocapData, sRigidBodyData + +TRACKING_VALID = 0x01 +MODEL_LIST_CHANGED = 0x02 + + +@dataclass +class BodySample: + """One sampled rigid body: streaming ID + world pose, or an invalid (lost) body.""" + + streaming_id: int + position: tuple[float, float, float] = (0.0, 0.0, 0.0) + orientation: tuple[float, float, float, float] = (0.0, 0.0, 0.0, 1.0) # qx,qy,qz,qw + valid: bool = True + + @classmethod + def lost(cls, streaming_id: int) -> "BodySample": + """An untracked body (missing prim): NaN position, tracking-invalid bit clear.""" + nan = float("nan") + return cls(streaming_id, (nan, nan, nan), (0.0, 0.0, 0.0, 1.0), valid=False) + + +def to_motive_pose(position: tuple[float, float, float], orientation: tuple[float, float, float, float], up_axis: str = "Z"): + """Re-express an Isaac (Z-up) world pose in Motive's streamed up-axis frame. + + Returns ``(position, orientation)`` re-axed for the given ``up_axis``: + + - ``"Z"`` (default) — identity pass-through. Isaac/USD is Z-up and the + reference Motive setup streams Z-up, so the pose flows through unchanged and + matches ``natnet_ros2`` (which does no axis conversion). + - ``"Y"`` — emulate a default Y-up Motive by rotating the pose -90 deg about X + (Isaac ``+Z`` -> Motive ``+Y``): ``(x, y, z) -> (x, z, -y)``. This is a + proper right-handed -> right-handed change of basis (det = +1), so the + quaternion's vector part takes the same swap and the scalar part is + unchanged: ``(qx, qy, qz, qw) -> (qx, qz, -qy, qw)``. + + Non-finite components (a lost body's NaN position) pass through unchanged. + """ + if str(up_axis).upper() != "Y": + return position, orientation + x, y, z = position + qx, qy, qz, qw = orientation + return (x, z, -y), (qx, qz, -qy, qw) + + +def make_rigid_body_data(sample: BodySample) -> sRigidBodyData: + """Build one ``sRigidBodyData`` from a sample (sets the tracking-valid bit).""" + rb = sRigidBodyData() + rb.ID = int(sample.streaming_id) + x, y, z = sample.position + qx, qy, qz, qw = sample.orientation + rb.x, rb.y, rb.z = float(x), float(y), float(z) + rb.qx, rb.qy, rb.qz, rb.qw = float(qx), float(qy), float(qz), float(qw) + rb.MeanError = 0.0 + rb.params = TRACKING_VALID if sample.valid else 0 + return rb + + +def build_frame( + frame_number: int, + samples, + *, + timestamp: float = 0.0, + model_list_changed: bool = False, +) -> sFrameOfMocapData: + """Assemble an ``sFrameOfMocapData`` of rigid bodies from samples.""" + frame = sFrameOfMocapData() + frame.iFrame = int(frame_number) + samples = list(samples) + frame.nRigidBodies = len(samples) + for i, sample in enumerate(samples): + frame.RigidBodies[i] = make_rigid_body_data(sample) + frame.fTimestamp = float(timestamp) + frame.params = MODEL_LIST_CHANGED if model_list_changed else 0 + return frame + + +def is_finite_pose(sample: BodySample) -> bool: + """True if all position/orientation components are finite (no NaN/inf).""" + return all(math.isfinite(v) for v in (*sample.position, *sample.orientation)) + + +def apply_pose_noise( + position: tuple[float, float, float], + orientation: tuple[float, float, float, float], + pose_noise_std_meters: float, + pose_noise_rotation_deg: float, +) -> tuple[tuple[float, float, float], tuple[float, float, float, float]]: + """Add independent Gaussian noise to position (m) and orientation (deg, XYZ euler).""" + + x, y, z = position + if pose_noise_std_meters > 0.0: + x += np.random.normal(0, pose_noise_std_meters) + y += np.random.normal(0, pose_noise_std_meters) + z += np.random.normal(0, pose_noise_std_meters) + + roll, pitch, yaw = Rotation.from_quat(orientation).as_euler("xyz", degrees=True) + if pose_noise_rotation_deg > 0.0: + roll += np.random.normal(0, pose_noise_rotation_deg) + pitch += np.random.normal(0, pose_noise_rotation_deg) + yaw += np.random.normal(0, pose_noise_rotation_deg) + + qx, qy, qz, qw = Rotation.from_euler("xyz", (roll, pitch, yaw), degrees=True).as_quat() + return (x, y, z), (float(qx), float(qy), float(qz), float(qw)) diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/manager.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/manager.py new file mode 100644 index 000000000..398d761eb --- /dev/null +++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/manager.py @@ -0,0 +1,403 @@ +# Copyright (c) 2024 Carnegie Mellon University +# MIT License - see LICENSE in the repository root for full text. +""" +``NatNetServerManager`` detects interface prims, samples poses from the stage, +and owns a **single** server instance it can start and stop. +On each enable it builds a MODELDEF catalog from the config and constructs a +fresh server via an injectable factory. +""" + +from __future__ import annotations + +from .catalog import build_catalog, find_duplicate_targets +from .config import DEFAULT_UP_AXIS, NatNetInterfaceConfig +from .frames import BodySample, apply_pose_noise, build_frame, to_motive_pose +from .usd_bindings import find_interfaces, read_interface, read_world_pose, resolve_targets + + +def _catalog_signature(config: NatNetInterfaceConfig): + """Identity of the catalog (body id/name set) — changes trigger a MODELDEF refresh.""" + return tuple((b.streaming_id, b.rigid_body_name) for b in config.bodies) + + +def _parse_version(version_str: str) -> tuple[int, int, int, int]: + try: + parts = tuple(int(x) for x in str(version_str).split(".")) + except ValueError: + parts = () + return (parts + (0, 0, 0, 0))[:4] + + +def default_server_factory(config: NatNetInterfaceConfig): + """Construct (but do not start) a ``NatNetUnicastServer`` from a config.""" + from ..server import NatNetUnicastServer, TransmissionType + + if config.mode != "unicast": + raise NotImplementedError( + f"mode {config.mode!r} is not supported yet (unicast only)" + ) + server = NatNetUnicastServer( + local_interface=config.server_ip, + transmission_type=TransmissionType.UNICAST, + multicast_address=None, + command_port=config.command_port, + data_port=config.data_port, + ) + server.publish_rate = config.publish_rate + server.natnet_version = _parse_version(config.natnet_version) + return server + + +def format_interface(prim_path: str, cfg: NatNetInterfaceConfig) -> str: + """Render a human-readable multi-line summary of one interface config.""" + lines = [f"[natnet] Interface @ {prim_path}"] + lines.append(f" serverEnabled : {cfg.server_enabled}") + lines.append(f" serverIp : {cfg.server_ip}") + lines.append(f" mode : {cfg.mode}") + if cfg.mode == "multicast": + lines.append(f" multicastAddr : {cfg.multicast_addr}") + lines.append(f" commandPort : {cfg.command_port}") + lines.append(f" dataPort : {cfg.data_port}") + lines.append(f" publishRate : {cfg.publish_rate}") + lines.append(f" natnetVersion : {cfg.natnet_version}") + lines.append(f" upAxis : {cfg.up_axis}") + lines.append(f" poseNoise : enabled={cfg.pose_noise_enabled}") + lines.append( + f" std={cfg.pose_noise_std_meters} m, rot={cfg.pose_noise_rotation_deg} deg" + ) + if cfg.bodies: + lines.append(f" bodies ({len(cfg.bodies)}):") + for b in cfg.bodies: + target = b.target_prim or "" + lines.append( + f" - {b.rigid_body_name} (id={b.streaming_id}, parent={b.parent_id}) -> {target}" + ) + else: + lines.append(" bodies : (none)") + return "\n".join(lines) + + +class NatNetServerManager: + """Detects interface prims, prints config, and owns one server instance.""" + + def __init__(self, server_factory=None): + self._stage_event_sub = None + self._usd_listener = None + self._scan_tick_sub = None + self._scan_pending = False + self._server = None + self._server_factory = server_factory or default_server_factory + # Sampling state. ``_needs_resync`` is the "latest config has been read" + # flag inverted: a NatNet prim edit sets it True (stale); the next physics + # sample re-reads the catalog/targets and clears it. ``_sample_cache`` holds + # the resolved (streaming_id, name, prim) tuples sampled every step. + self._needs_resync = False + self._sample_cache: list = [] + self._frame_counter = 0 + self._catalog_signature = None + self._physx_sub = None + # Streamed up-axis (from the interface config; re-read on every resync). + # "Z" (default) streams the Isaac/USD world pose as-is; "Y" re-axes it to + # emulate a default Y-up Motive. See frames.to_motive_pose. + self._up_axis = DEFAULT_UP_AXIS + # Pose noise. + self._pose_noise_enabled = False + self._pose_noise_std_meters = 0.0 + self._pose_noise_rotation_deg = 0.0 + + # --- lifecycle ------------------------------------------------------------- + + def on_startup(self): + import omni.usd + + usd_context = omni.usd.get_context() + self._stage_event_sub = usd_context.get_stage_event_stream().create_subscription_to_pop( + self._on_stage_event, name="natnet_manager_stage_events" + ) + self._register_usd_listener() + self._subscribe_physics() + print("[natnet] NatNetServerManager initialized") + self.scan_and_print() + + def on_shutdown(self): + self.stop_server() + self._physx_sub = None + self._stage_event_sub = None + self._scan_tick_sub = None + self._scan_pending = False + self._revoke_usd_listener() + + def _subscribe_physics(self): + # Sample + enqueue poses on every physics step (only fires while playing). + try: + import omni.physx + + self._physx_sub = omni.physx.get_physx_interface().subscribe_physics_step_events( + self._on_physics_step + ) + except Exception as exc: # Kit/physx only + print(f"[natnet] Physics step subscription unavailable: {exc}") + self._physx_sub = None + + def _on_physics_step(self, _dt): + if self._server is not None: + self.sample_once() + + # --- scanning -------------------------------------------------------------- + + def scan_and_print(self, *_): + """Find every interface prim and print its parsed config.""" + stage = self._get_stage() + if stage is None: + return + interfaces = find_interfaces(stage) + if not interfaces: + print("[natnet] Scan: no NatNetInterface prims on stage.") + return + print(f"[natnet] Scan: {len(interfaces)} interface(s) detected.") + for prim in interfaces: + cfg = read_interface(prim) + print(format_interface(prim.GetPath().pathString, cfg)) + + # --- server lifecycle (single instance; USD-free, factory-injectable) ------ + + @property + def is_running(self) -> bool: + return self._server is not None + + @property + def server(self): + return self._server + + def start_server(self, config: NatNetInterfaceConfig) -> bool: + """Build the catalog, construct a fresh server, and start it — once. + + Idempotent: if a server is already running this is a no-op returning False. + Returns True when a new server was created and started. + """ + if self._server is not None: + print("[natnet] start_server ignored: a server is already running.") + return False + catalog = build_catalog(config) + server = self._server_factory(config) + server.set_model_def_payload(catalog.pack()) + # Pump frames from our sample_once (physics-step) thread rather than the + # server's background timer: inside the Isaac Sim process that daemon thread + # is starved by the render/physics main loop, so frames never get sent. + if hasattr(server, "auto_stream"): + server.auto_stream = False + server.start() + self._server = server + # Force a resync on the first sampled frame so the prim->pose cache is built + # from the live stage (and the catalog signature is seeded). + self._needs_resync = True + self._frame_counter = 0 + # None so the first resync reports "changed" and the first streamed frame + # flags model_list_changed (nudging the client to (re)read MODELDEF). + self._catalog_signature = None + print( + f"[natnet] Server started on {config.server_ip} " + f"(cmd {config.command_port} / data {config.data_port}) " + f"with {len(config.bodies)} body(ies)." + ) + return True + + def stop_server(self) -> bool: + """Shut down the running server (fresh instance is built on next start). + + Idempotent: returns False if nothing was running. + """ + if self._server is None: + return False + try: + self._server.shutdown() + finally: + self._server = None + self._sample_cache = [] + self._needs_resync = False + print("[natnet] Server stopped.") + return True + + def toggle_server(self, config: NatNetInterfaceConfig) -> bool: + """Start if stopped, stop if running. Returns the resulting running state.""" + if self.is_running: + self.stop_server() + else: + self.start_server(config) + return self.is_running + + def apply_enabled(self, config: NatNetInterfaceConfig) -> None: + """Reconcile running state to ``config.server_enabled`` (start/stop).""" + if config.server_enabled and not self.is_running: + self.start_server(config) + elif not config.server_enabled and self.is_running: + self.stop_server() + + def log_target_diagnostics(self, config: NatNetInterfaceConfig) -> None: + """Warn about missing target prims and duplicate targets (best-effort).""" + stage = self._get_stage() + if stage is not None: + _existing, missing = resolve_targets(stage, config) + for body in missing: + print( + f"[natnet] WARNING: body '{body.rigid_body_name}' target prim " + f"missing or empty: {body.target_prim or ''}" + ) + for path in find_duplicate_targets(config): + print(f"[natnet] WARNING: multiple bodies target the same prim: {path}") + + # --- scripting entry point ------------------------------------------------- + + def start_from_stage(self) -> bool: + """Find the interface prim on the current stage, read it, and start. + + Convenience for scripts/Pegasus launchers: author the prim (see + ``author_interface``) then call this. Returns False if nothing to start. + """ + stage = self._get_stage() + if stage is None: + print("[natnet] start_from_stage: no active stage.") + return False + interfaces = find_interfaces(stage) + if not interfaces: + print("[natnet] start_from_stage: no NatNetInterface prim found.") + return False + config = read_interface(interfaces[0]) + self.log_target_diagnostics(config) + return self.start_server(config) + + # --- pose sampling + dynamic catalog (the data-enqueue path) --------------- + + def mark_dirty(self) -> None: + """Flag that the on-stage config changed; next sample re-reads the catalog.""" + self._needs_resync = True + + def _resync(self, stage) -> bool: + """Re-read the interface config, rebuild the catalog, and re-resolve targets. + + Returns True if the catalog (body id/name set) actually changed, so the next + frame can flag ``model_list_changed`` and the client re-requests MODELDEF. + """ + interfaces = find_interfaces(stage) + if not interfaces: + self._sample_cache = [] + return False + config = read_interface(interfaces[0]) + self._up_axis = config.up_axis + self._pose_noise_enabled = config.pose_noise_enabled + self._pose_noise_std_meters = config.pose_noise_std_meters + self._pose_noise_rotation_deg = config.pose_noise_rotation_deg + if self._server is not None: + self._server.set_model_def_payload(build_catalog(config).pack()) + # Cache target *paths* (not prim handles): the prim is re-resolved every + # sample so bodies whose target is created *after* the server starts — e.g. + # a Pegasus drone base_link spawned on the first Play tick — start streaming + # a valid pose as soon as the prim appears (instead of being stuck "lost"). + self._sample_cache = [ + (body.streaming_id, body.rigid_body_name, body.target_prim) + for body in config.bodies + ] + signature = _catalog_signature(config) + changed = signature != self._catalog_signature + self._catalog_signature = signature + return changed + + def sample_once(self, stage=None): + """Sample every body's USD world pose and enqueue one frame to the server. + + Resyncs the catalog first if the config is dirty (so bodies added/removed + live are picked up). Returns the enqueued frame (or None if nothing to do). + """ + if self._server is None: + return None + if stage is None: + stage = self._get_stage() + if stage is None: + return None + + model_changed = False + if self._needs_resync: + model_changed = self._resync(stage) + self._needs_resync = False + + samples = [] + for streaming_id, _name, target_path in self._sample_cache: + prim = stage.GetPrimAtPath(target_path) if target_path else None + pose = read_world_pose(prim) if prim is not None else None + if pose is None: + samples.append(BodySample.lost(streaming_id)) + else: + position, orientation = to_motive_pose(*pose, up_axis=self._up_axis) + if self._pose_noise_enabled: + position, orientation = apply_pose_noise(position, orientation, self._pose_noise_std_meters, self._pose_noise_rotation_deg) + samples.append(BodySample(streaming_id, position, orientation, valid=True)) + + frame = build_frame( + self._frame_counter, samples, model_list_changed=model_changed + ) + self._frame_counter += 1 + self._server.enqueue_mocap_data(frame) + # Send synchronously from this (physics-step) thread. + flush_mocap_data = getattr(self._server, "flush_mocap_data", None) + if callable(flush_mocap_data): + flush_mocap_data() + return frame + + # --- stage / USD notifications -------------------------------------------- + + def _get_stage(self): + import omni.usd + + return omni.usd.get_context().get_stage() + + def _on_stage_event(self, event): + import omni.usd + + if event.type == int(omni.usd.StageEventType.OPENED): + self._register_usd_listener() + self.scan_and_print() + + def _register_usd_listener(self): + from pxr import Tf, Usd + + stage = self._get_stage() + if stage is None: + return + self._revoke_usd_listener() + self._usd_listener = Tf.Notice.Register( + Usd.Notice.ObjectsChanged, self._on_objects_changed, stage + ) + + def _revoke_usd_listener(self): + if self._usd_listener is not None: + self._usd_listener.Revoke() + self._usd_listener = None + + def _on_objects_changed(self, notice, sender): + # Only re-scan when something NatNet-related changed + try: + paths = list(notice.GetResyncedPaths()) + list(notice.GetChangedInfoOnlyPaths()) + except Exception: + paths = [] + if any(("NatNetInterface" in str(p)) or ("natnet:" in str(p)) for p in paths): + # A NatNet prim changed: mark the sampler dirty so the next physics step re-reads the catalog. + self._needs_resync = True + # Debounce author_interface() calls into one scan on the next update tick. + self._request_scan() + + def _request_scan(self): + if self._scan_pending: + return + self._scan_pending = True + import omni.kit.app + + self._scan_tick_sub = ( + omni.kit.app.get_app() + .get_update_event_stream() + .create_subscription_to_pop(self._on_scan_tick, name="natnet_manager_scan_tick") + ) + + def _on_scan_tick(self, _event): + self._scan_pending = False + self._scan_tick_sub = None + self.scan_and_print() diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/scene_setup.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/scene_setup.py new file mode 100644 index 000000000..d7e804c61 --- /dev/null +++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/scene_setup.py @@ -0,0 +1,142 @@ +# Copyright (c) 2024 Carnegie Mellon University +# MIT License - see LICENSE in the repository root for full text. +"""Standalone-launch helpers: stand up a drone NatNet interface on scene load. + +Used by the Pegasus example launch scripts so a Motive-compatible NatNet server +comes up automatically with one rigid body per drone ``base_link`` — no UI clicks. + +Two layers, mirroring the rest of the package: + +- ``build_drone_config`` is **pure** (no USD / Kit), so it unit-tests hermetically. +- ``start_drone_natnet_server`` authors the interface prim and owns a + :class:`~optitrack.natnet.emulator.isaac.manager.NatNetServerManager` that samples + poses on each physics step. It imports ``pxr``/``omni`` lazily (only when called). +""" + +from __future__ import annotations + +from typing import Iterable, Sequence, Tuple + +from .config import ( + DEFAULT_COMMAND_PORT, + DEFAULT_DATA_PORT, + DEFAULT_POSE_NOISE_ENABLED, + DEFAULT_POSE_NOISE_ROTATION_DEG, + DEFAULT_POSE_NOISE_STD_METERS, + DEFAULT_PUBLISH_RATE, + DEFAULT_SERVER_IP, + DEFAULT_UP_AXIS, + BodyBinding, + NatNetInterfaceConfig, +) + +# Where the example scripts author the single interface prim. +DEFAULT_INTERFACE_PATH = "/World/NatNetInterface" + +# Default world prim + position for the demo "target" body (a static placeholder +# the example scripts stream alongside the drones so a tracked target is available). +DEFAULT_TARGET_PATH = "/World/target" +DEFAULT_TARGET_POSITION = (2.0, 0.0, 1.0) +DEFAULT_TARGET_STREAMING_ID = 100 + +# (rigid_body_name, streaming_id, target_prim_path) +DroneSpec = Tuple[str, int, str] + + +def author_static_target( + stage, + prim_path: str = DEFAULT_TARGET_PATH, + position: Sequence[float] = DEFAULT_TARGET_POSITION, +): + """Author a static ``Xform`` prim to act as a NatNet-tracked target. + + Creates ``prim_path`` (a plain transform with a single translate op) at + ``position`` so the emulator can sample it like any other tracked body. The + prim is static — no physics, no animation — representing a fixed point of + interest that drones can be commanded toward. Imports ``pxr`` lazily so this + module stays importable outside Isaac. Returns ``prim_path``. + """ + from pxr import Gf, UsdGeom + + xform = UsdGeom.Xform.Define(stage, prim_path) + xform.AddTranslateOp().Set(Gf.Vec3d(float(position[0]), float(position[1]), float(position[2]))) + return prim_path + + +def build_drone_config( + drones: Iterable[DroneSpec], + *, + server_ip: str = DEFAULT_SERVER_IP, + mode: str = "unicast", + command_port: int = DEFAULT_COMMAND_PORT, + data_port: int = DEFAULT_DATA_PORT, + publish_rate: float = DEFAULT_PUBLISH_RATE, + server_enabled: bool = True, + up_axis: str = DEFAULT_UP_AXIS, + pose_noise_enabled: bool = DEFAULT_POSE_NOISE_ENABLED, + pose_noise_std_meters: float = DEFAULT_POSE_NOISE_STD_METERS, + pose_noise_rotation_deg: float = DEFAULT_POSE_NOISE_ROTATION_DEG, +) -> NatNetInterfaceConfig: + """Build a validated config with one rigid body per drone. + + ``drones`` is an iterable of ``(rigid_body_name, streaming_id, target_prim)`` + tuples — typically one per spawned drone, with ``target_prim`` pointing at the + drone's ``base_link``. Raises ``ValueError`` (via ``validate``) on duplicate + names/ids or bad ports. + """ + bodies = [ + BodyBinding( + rigid_body_name=str(name), + target_prim=str(target), + streaming_id=int(streaming_id), + ) + for name, streaming_id, target in drones + ] + cfg = NatNetInterfaceConfig( + server_enabled=server_enabled, + server_ip=server_ip, + mode=mode, + command_port=command_port, + data_port=data_port, + publish_rate=publish_rate, + up_axis=up_axis, + pose_noise_enabled=pose_noise_enabled, + pose_noise_std_meters=pose_noise_std_meters, + pose_noise_rotation_deg=pose_noise_rotation_deg, + bodies=bodies, + ) + cfg.validate() + return cfg + + +def start_drone_natnet_server( + stage, + drones: Sequence[DroneSpec], + *, + prim_path: str = DEFAULT_INTERFACE_PATH, + start: bool = True, + **config_kwargs, +): + """Author a NatNet interface prim from ``drones`` and return a running manager. + + Authors ``prim_path`` (overwriting any existing interface) with one rigid body + per drone, then creates a :class:`NatNetServerManager` that subscribes to physics + steps and starts the server. Play the sim to stream poses. Keep a reference to + the returned manager so it isn't garbage-collected (which would tear + down the physics subscription and stop the server). + + Returns the ``NatNetServerManager``. If ``start`` is + False (or the config is authored disabled), + the manager is created but the server is left stopped. + """ + from .manager import NatNetServerManager + from .usd_bindings import author_interface + + cfg = build_drone_config(drones, **config_kwargs) + author_interface(stage, prim_path, cfg) + + manager = NatNetServerManager() + manager.on_startup() + if start and cfg.server_enabled: + manager.start_from_stage() + return manager diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/ui_extension.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/ui_extension.py new file mode 100644 index 000000000..50a581551 --- /dev/null +++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/ui_extension.py @@ -0,0 +1,453 @@ +# Copyright (c) 2024 Carnegie Mellon University +# MIT License - see LICENSE in the repository root for full text. +"""Kit extension entry: docked editor for the NatNet interface config prim. + +Create/manage the ``/World/NatNetInterface`` prim. The window docks to the +bottom-right (alongside the Property panel, like Pegasus) so it's easy to find. + +Sync model is explicit and user-driven via the top button row. +""" + +from __future__ import annotations + +import omni.ext + +from .config import VALID_MODES, VALID_UP_AXES, BodyBinding, NatNetInterfaceConfig +from .manager import NatNetServerManager +from .usd_bindings import author_interface, find_interfaces, read_interface, read_world_pose + +_DEFAULT_PRIM_PATH = "/World/NatNetInterface" +_LABEL_WIDTH = 140 +_POS_REFRESH_PERIOD = 1.0 / 6.0 # seconds between live USD position reads + +_COLOR_LIVE = 0xFF33CC33 # green: prim resolves and server is streaming +_COLOR_IDLE = 0xFFAAAAAA # grey: prim resolves but server not running +_COLOR_LOST = 0xFF3333FF # red: no prim / NaN + + +class NatNetEmulatorExtension(omni.ext.IExt): + """Registers the Window menu entry + the docked editor panel.""" + + def on_startup(self, ext_id): # noqa: D401 - Kit lifecycle hook + self._window = None + self._bodies_frame = None + self._cfg = NatNetInterfaceConfig() + self._row_readouts = {} + self._pos_refresh_sub = None + self._last_pos_refresh = 0.0 + self._manager = NatNetServerManager() + self._manager.on_startup() + self._add_menu() + self._subscribe_position_refresh() + + def on_shutdown(self): + self._remove_menu() + self._pos_refresh_sub = None + self._row_readouts = {} + if self._manager is not None: + self._manager.on_shutdown() + self._manager = None + if self._window is not None: + self._window.destroy() + self._window = None + + # --- live position readout ------------------------------------------------- + + def _subscribe_position_refresh(self): + try: + import omni.kit.app + except Exception: # pragma: no cover - Kit only + return + self._pos_refresh_sub = ( + omni.kit.app.get_app() + .get_update_event_stream() + .create_subscription_to_pop(self._on_pos_refresh, name="natnet_ui_pos_refresh") + ) + + def _on_pos_refresh(self, _event): + import time + + if self._window is None or not self._window.visible or not self._row_readouts: + return + now = time.monotonic() + if now - self._last_pos_refresh < _POS_REFRESH_PERIOD: + return + self._last_pos_refresh = now + stage = self._get_stage() + running = self._manager is not None and self._manager.is_running + for idx, (status_label, pos_label) in list(self._row_readouts.items()): + if not (0 <= idx < len(self._cfg.bodies)): + continue + target = self._cfg.bodies[idx].target_prim + symbol, color, text = self._row_readout(stage, target, running) + status_label.text = symbol + status_label.style = {"color": color} + pos_label.text = text + pos_label.style = {"color": color} + + def _row_readout(self, stage, target, running): + if not target: + return "\u25cb", _COLOR_IDLE, "no target prim" + prim = stage.GetPrimAtPath(target) if stage is not None else None + pose = read_world_pose(prim) if prim is not None else None + if pose is None: + return "\u2717", _COLOR_LOST, "NaN (prim missing)" + (x, y, z), _quat = pose + text = f"{x:+.3f}, {y:+.3f}, {z:+.3f}" + if running: + return "\u25cf", _COLOR_LIVE, text + return "\u25cf", _COLOR_IDLE, text + + # --- menu ------------------------------------------------------------------ + + def _add_menu(self): + try: + import omni.kit.menu.utils as menu_utils + from omni.kit.menu.utils import MenuItemDescription + except Exception: # pragma: no cover - Kit only + return + self._menu_entries = [ + MenuItemDescription(name="NatNet Interface", onclick_fn=self._toggle_window) + ] + menu_utils.add_menu_items(self._menu_entries, "Window") + + def _remove_menu(self): + try: + import omni.kit.menu.utils as menu_utils + except Exception: # pragma: no cover - Kit only + return + if getattr(self, "_menu_entries", None): + menu_utils.remove_menu_items(self._menu_entries, "Window") + self._menu_entries = None + + # --- window ---------------------------------------------------------------- + + def _toggle_window(self, *_): + import omni.ui as ui + + if self._window is None: + self._window = ui.Window("NatNet Interface", width=400, height=600) + self._window.frame.set_build_fn(self._build_window) + # Dock bottom-right next to the Property panel, like Pegasus. + self._window.deferred_dock_in("Property", ui.DockPolicy.CURRENT_WINDOW_IS_ACTIVE) + self._window.visible = True + return + self._window.visible = not self._window.visible + + def _refresh(self, *_): + if self._window is not None: + self._window.frame.rebuild() + + def _build_window(self): + import omni.ui as ui + + with ui.ScrollingFrame(): + with ui.VStack(spacing=6, height=0): + ui.Label("NatNet interface", height=0, style={"font_size": 16}) + + with ui.HStack(height=28, spacing=6): + ui.Button("Create Interface", clicked_fn=self._create_server) + ui.Button("Save", clicked_fn=self._save) + ui.Button("Load from Stage", clicked_fn=self._load_from_stage) + ui.Button("Print config", clicked_fn=self._print_config) + + running = self._manager is not None and self._manager.is_running + with ui.HStack(height=28, spacing=6): + ui.Button( + "Stop Server" if running else "Start Server", + clicked_fn=self._toggle_server, + ) + ui.Label( + f"Server: {'RUNNING' if running else 'stopped'}", + width=0, + style={"color": 0xFF33CC33 if running else 0xFF888888}, + ) + + ui.Label( + "\u26a0 Remember to save after each edit", + height=0, + word_wrap=True, + style={"color": 0xFF33CCFF, "font_size": 14}, + ) + + ui.Label(self._status_text(), height=0, word_wrap=True) + + ui.Separator(height=6) + self._bool_row(ui, "Server enabled", "server_enabled", self._cfg.server_enabled) + self._bool_row(ui, "Pose noise enabled", "pose_noise_enabled", self._cfg.pose_noise_enabled) + self._float_row(ui, "Pose noise std meters", "pose_noise_std_meters", self._cfg.pose_noise_std_meters) + self._float_row(ui, "Pose noise rotation deg", "pose_noise_rotation_deg", self._cfg.pose_noise_rotation_deg) + self._str_row(ui, "Server IP", "server_ip", self._cfg.server_ip) + self._combo_row(ui, "Mode", "mode", self._cfg.mode, VALID_MODES) + self._int_row(ui, "Command port", "command_port", self._cfg.command_port) + self._int_row(ui, "Data port", "data_port", self._cfg.data_port) + self._float_row(ui, "Publish rate (Hz)", "publish_rate", self._cfg.publish_rate) + self._combo_row(ui, "Up axis", "up_axis", self._cfg.up_axis, VALID_UP_AXES) + + ui.Separator(height=6) + ui.Label("Tracked bodies", height=0, style={"font_size": 14}) + self._bodies_frame = ui.Frame(height=0) + self._bodies_frame.set_build_fn(self._build_bodies) + with ui.HStack(height=0, spacing=6): + ui.Button("Add body (from selection)", clicked_fn=self._add_body) + + def _status_text(self): + prim = self._find_interface() + if prim is None: + return "No prim on stage yet — Save or Create Server to author one." + return f"Prim on stage: {prim.GetPath().pathString} (Save to push edits, Load to pull)" + + # --- server field rows (edit the working copy only) ------------------------ + + def _bool_row(self, ui, label, key, value): + with ui.HStack(height=0): + ui.Label(label, width=_LABEL_WIDTH) + cb = ui.CheckBox() + cb.model.set_value(bool(value)) + cb.model.add_value_changed_fn( + lambda m, k=key: self._set_cfg_field(k, m.get_value_as_bool()) + ) + + def _str_row(self, ui, label, key, value): + with ui.HStack(height=0): + ui.Label(label, width=_LABEL_WIDTH) + model = ui.StringField().model + model.set_value(str(value)) + model.add_value_changed_fn( + lambda m, k=key: self._set_cfg_field(k, m.get_value_as_string()) + ) + + def _int_row(self, ui, label, key, value): + with ui.HStack(height=0): + ui.Label(label, width=_LABEL_WIDTH) + model = ui.IntField().model + model.set_value(int(value)) + model.add_value_changed_fn( + lambda m, k=key: self._set_cfg_field(k, m.get_value_as_int()) + ) + + def _float_row(self, ui, label, key, value): + with ui.HStack(height=0): + ui.Label(label, width=_LABEL_WIDTH) + model = ui.FloatField().model + model.set_value(float(value)) + model.add_value_changed_fn( + lambda m, k=key: self._set_cfg_field(k, m.get_value_as_float()) + ) + + def _combo_row(self, ui, label, key, value, choices): + with ui.HStack(height=0): + ui.Label(label, width=_LABEL_WIDTH) + index = choices.index(value) if value in choices else 0 + combo = ui.ComboBox(index, *choices) + combo.model.get_item_value_model().add_value_changed_fn( + lambda m, k=key, c=choices: self._set_cfg_field(k, c[m.get_value_as_int()]) + ) + + def _set_cfg_field(self, attr, value): + setattr(self._cfg, attr, value) + + # --- bodies ---------------------------------------------------------------- + + def _rebuild_bodies(self, *_): + if self._bodies_frame is not None: + self._bodies_frame.rebuild() + + def _build_bodies(self): + import omni.ui as ui + + self._row_readouts = {} + with ui.VStack(spacing=6, height=0): + if not self._cfg.bodies: + ui.Label(" (no bodies — select a prim and click Add body)", height=0) + return + with ui.HStack(height=0, spacing=4): + ui.Label("Rigid body name", width=ui.Fraction(1)) + ui.Label("ID", width=40) + ui.Label("Parent", width=50) + ui.Label("Target prim", width=ui.Fraction(2)) + ui.Spacer(width=98) + for idx, body in enumerate(self._cfg.bodies): + self._build_body_row(ui, idx, body) + + def _build_body_row(self, ui, idx, body): + with ui.VStack(height=0, spacing=2): + with ui.HStack(height=0, spacing=4): + name = ui.StringField(width=ui.Fraction(1)).model + name.set_value(body.rigid_body_name) + name.add_value_changed_fn( + lambda m, i=idx: self._set_body_field(i, "rigid_body_name", m.get_value_as_string()) + ) + + sid = ui.IntField(width=40).model + sid.set_value(body.streaming_id) + sid.add_value_changed_fn( + lambda m, i=idx: self._set_body_field(i, "streaming_id", m.get_value_as_int()) + ) + + parent = ui.IntField(width=50).model + parent.set_value(body.parent_id) + parent.add_value_changed_fn( + lambda m, i=idx: self._set_body_field(i, "parent_id", m.get_value_as_int()) + ) + + target = ui.StringField(width=ui.Fraction(2), tooltip="USD path of the tracked prim").model + target.set_value(body.target_prim) + target.add_value_changed_fn( + lambda m, i=idx: self._set_body_field(i, "target_prim", m.get_value_as_string()) + ) + + ui.Button("set target", width=70, clicked_fn=lambda i=idx: self._retarget_body(i)) + ui.Button("x", width=24, clicked_fn=lambda i=idx: self._remove_body_at(i)) + + # Live readout: status dot + world position pulled from the USD stage. + stage = self._get_stage() + running = self._manager is not None and self._manager.is_running + symbol, color, text = self._row_readout(stage, body.target_prim, running) + with ui.HStack(height=0, spacing=6): + ui.Spacer(width=4) + status_label = ui.Label(symbol, width=14, style={"color": color}) + ui.Label("pos:", width=30, style={"color": _COLOR_IDLE}) + pos_label = ui.Label(text, width=ui.Fraction(1), style={"color": color}) + self._row_readouts[idx] = (status_label, pos_label) + + def _set_body_field(self, index, attr, value): + if 0 <= index < len(self._cfg.bodies): + setattr(self._cfg.bodies[index], attr, value) + + def _add_body(self): + next_id = max((b.streaming_id for b in self._cfg.bodies), default=0) + 1 + target = self._selected_target_path(self._find_interface()) + name = target.rsplit("/", 1)[-1] if target else f"Body{next_id}" + existing = {b.rigid_body_name for b in self._cfg.bodies} + while name in existing: + name = f"{name}_{next_id}" + self._cfg.bodies.append(BodyBinding(rigid_body_name=name, target_prim=target, streaming_id=next_id)) + self._rebuild_bodies() + + def _remove_body_at(self, index): + if 0 <= index < len(self._cfg.bodies): + self._cfg.bodies.pop(index) + self._rebuild_bodies() + + def _retarget_body(self, index): + import carb + + path = self._selected_target_path(self._find_interface()) + if not path: + carb.log_warn("[natnet] Select a prim in the viewport to retarget this body.") + return + if 0 <= index < len(self._cfg.bodies): + self._cfg.bodies[index].target_prim = path + self._rebuild_bodies() + + # --- stage helpers --------------------------------------------------------- + + def _get_stage(self): + import omni.usd + + return omni.usd.get_context().get_stage() + + def _find_interface(self): + stage = self._get_stage() + if stage is None: + return None + interfaces = find_interfaces(stage) + return interfaces[0] if interfaces else None + + def _interface_path(self): + prim = self._find_interface() + return prim.GetPath().pathString if prim is not None else _DEFAULT_PRIM_PATH + + def _select(self, prim_path): + import omni.usd + + omni.usd.get_context().get_selection().set_selected_prim_paths([prim_path], True) + + def _selected_target_path(self, interface_prim): + import omni.usd + + sel = omni.usd.get_context().get_selection().get_selected_prim_paths() + iface_path = interface_prim.GetPath().pathString if interface_prim else None + for path in sel: + if path != iface_path: + return path + return "" + + # --- explicit sync actions ------------------------------------------------- + + def _save(self): + import carb + + stage = self._get_stage() + if stage is None: + carb.log_error("[natnet] No active stage.") + return + try: + self._cfg.validate() + except ValueError as exc: + carb.log_error(f"[natnet] Not saved: {exc}") + return + path = self._interface_path() + author_interface(stage, path, self._cfg) + carb.log_info(f"[natnet] Saved interface to {path} ({len(self._cfg.bodies)} bodies).") + self._refresh() + + def _load_from_stage(self): + import carb + + prim = self._find_interface() + if prim is None: + self._cfg = NatNetInterfaceConfig() + carb.log_warn("[natnet] No interface on stage — reset to defaults.") + else: + self._cfg = read_interface(prim) + carb.log_info(f"[natnet] Loaded interface from {prim.GetPath().pathString}.") + self._refresh() + + def _print_config(self): + # Print whatever is authored on the stage (the source of truth). + if self._manager is not None: + self._manager.scan_and_print() + + def _toggle_server(self): + # Start/stop the live server at the click of this button, regardless of the + # serverEnabled attribute. Builds from the prim that's actually on the stage. + import carb + + if self._manager is None: + return + if not self._manager.is_running: + if self._find_interface() is None: + carb.log_warn("[natnet] No interface on stage — Create/Save one first.") + return + try: + self._manager.start_from_stage() + except Exception as exc: # noqa: BLE001 - surface to the user + carb.log_error(f"[natnet] Could not start server: {exc}") + else: + self._manager.stop_server() + self._refresh() + + def _create_server(self): + import carb + + stage = self._get_stage() + if stage is None: + carb.log_error("[natnet] No active stage.") + return + prim = self._find_interface() + if prim is None: + try: + self._cfg.validate() + except ValueError as exc: + carb.log_error(f"[natnet] Cannot create: {exc}") + return + author_interface(stage, _DEFAULT_PRIM_PATH, self._cfg) + path = _DEFAULT_PRIM_PATH + carb.log_info(f"[natnet] Created interface prim at {path}. (Server start: later commit.)") + else: + path = prim.GetPath().pathString + carb.log_info(f"[natnet] Interface already exists at {path}. (Server start: later commit.)") + self._select(path) + self._refresh() diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/usd_bindings.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/usd_bindings.py new file mode 100644 index 000000000..262216ee2 --- /dev/null +++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/usd_bindings.py @@ -0,0 +1,216 @@ +# Copyright (c) 2024 Carnegie Mellon University +# MIT License - see LICENSE in the repository root for full text. +"""USD binding layer: author / read / find NatNet interface prims on a stage. + +``pxr`` is imported lazily inside each function so importing this module doesn't +require USD. + +Backing today is plain namespaced custom attributes + relationships. Property names +follow the multi-apply schema convention (``natnet:body::``). +""" + +from __future__ import annotations + +from typing import Any + +from .config import ( + ATTR_COMMAND_PORT, + ATTR_DATA_PORT, + ATTR_MODE, + ATTR_MULTICAST_ADDR, + ATTR_NATNET_VERSION, + ATTR_POSE_NOISE_ENABLED, + ATTR_POSE_NOISE_ROTATION_DEG, + ATTR_POSE_NOISE_STD_METERS, + ATTR_PUBLISH_RATE, + ATTR_SERVER_ENABLED, + ATTR_SERVER_IP, + ATTR_UP_AXIS, + BODY_FIELD_PARENT_ID, + BODY_FIELD_RIGID_BODY_NAME, + BODY_FIELD_STREAMING_ID, + BODY_FIELD_TARGET, + BODY_PREFIX, + DEFAULT_COMMAND_PORT, + DEFAULT_DATA_PORT, + DEFAULT_MULTICAST_ADDR, + DEFAULT_NATNET_VERSION, + DEFAULT_POSE_NOISE_ENABLED, + DEFAULT_POSE_NOISE_ROTATION_DEG, + DEFAULT_POSE_NOISE_STD_METERS, + DEFAULT_PUBLISH_RATE, + DEFAULT_SERVER_IP, + DEFAULT_UP_AXIS, + MARKER_ATTR, + BodyBinding, + NatNetInterfaceConfig, + body_attr_name, +) + + +def author_interface(stage, prim_path: str, config: Any) -> Any: + """Create/overwrite a NatNet interface prim at ``prim_path`` from ``config``. + + ``config`` may be a :class:`NatNetInterfaceConfig` or a plain ``dict`` (passed + through ``from_dict``). Returns the ``Usd.Prim``. + """ + from pxr import Sdf + + cfg = config if isinstance(config, NatNetInterfaceConfig) else NatNetInterfaceConfig.from_dict(config) + cfg.validate() + + prim = stage.DefinePrim(prim_path, "Scope") + + # Overwrite semantics: drop any previously-authored body properties so removed + # bodies don't linger across re-authoring. + _clear_body_properties(prim) + + _set(prim, MARKER_ATTR, Sdf.ValueTypeNames.Bool, True) + _set(prim, ATTR_SERVER_ENABLED, Sdf.ValueTypeNames.Bool, cfg.server_enabled) + _set(prim, ATTR_SERVER_IP, Sdf.ValueTypeNames.String, cfg.server_ip) + _set(prim, ATTR_MODE, Sdf.ValueTypeNames.Token, cfg.mode) + _set(prim, ATTR_MULTICAST_ADDR, Sdf.ValueTypeNames.String, cfg.multicast_addr) + _set(prim, ATTR_COMMAND_PORT, Sdf.ValueTypeNames.Int, cfg.command_port) + _set(prim, ATTR_DATA_PORT, Sdf.ValueTypeNames.Int, cfg.data_port) + _set(prim, ATTR_PUBLISH_RATE, Sdf.ValueTypeNames.Float, cfg.publish_rate) + _set(prim, ATTR_NATNET_VERSION, Sdf.ValueTypeNames.String, cfg.natnet_version) + _set(prim, ATTR_UP_AXIS, Sdf.ValueTypeNames.Token, cfg.up_axis) + _set(prim, ATTR_POSE_NOISE_ENABLED, Sdf.ValueTypeNames.Bool, cfg.pose_noise_enabled) + _set(prim, ATTR_POSE_NOISE_STD_METERS, Sdf.ValueTypeNames.Float, cfg.pose_noise_std_meters) + _set(prim, ATTR_POSE_NOISE_ROTATION_DEG, Sdf.ValueTypeNames.Float, cfg.pose_noise_rotation_deg) + + for key, body in cfg.assign_instance_keys(): + _set(prim, body_attr_name(key, BODY_FIELD_RIGID_BODY_NAME), Sdf.ValueTypeNames.String, body.rigid_body_name) + _set(prim, body_attr_name(key, BODY_FIELD_STREAMING_ID), Sdf.ValueTypeNames.Int, body.streaming_id) + _set(prim, body_attr_name(key, BODY_FIELD_PARENT_ID), Sdf.ValueTypeNames.Int, body.parent_id) + rel = prim.CreateRelationship(body_attr_name(key, BODY_FIELD_TARGET), False) + rel.SetTargets([Sdf.Path(body.target_prim)] if body.target_prim else []) + + return prim + + +def read_interface(prim) -> NatNetInterfaceConfig: + """Reconstruct a :class:`NatNetInterfaceConfig` from an authored interface prim.""" + return NatNetInterfaceConfig( + server_enabled=bool(_get(prim, ATTR_SERVER_ENABLED, True)), + server_ip=str(_get(prim, ATTR_SERVER_IP, DEFAULT_SERVER_IP)), + mode=str(_get(prim, ATTR_MODE, "unicast")), + multicast_addr=str(_get(prim, ATTR_MULTICAST_ADDR, DEFAULT_MULTICAST_ADDR)), + command_port=int(_get(prim, ATTR_COMMAND_PORT, DEFAULT_COMMAND_PORT)), + data_port=int(_get(prim, ATTR_DATA_PORT, DEFAULT_DATA_PORT)), + publish_rate=float(_get(prim, ATTR_PUBLISH_RATE, DEFAULT_PUBLISH_RATE)), + natnet_version=str(_get(prim, ATTR_NATNET_VERSION, DEFAULT_NATNET_VERSION)), + up_axis=str(_get(prim, ATTR_UP_AXIS, DEFAULT_UP_AXIS)), + pose_noise_enabled=bool(_get(prim, ATTR_POSE_NOISE_ENABLED, DEFAULT_POSE_NOISE_ENABLED)), + pose_noise_std_meters=float(_get(prim, ATTR_POSE_NOISE_STD_METERS, DEFAULT_POSE_NOISE_STD_METERS)), + pose_noise_rotation_deg=float(_get(prim, ATTR_POSE_NOISE_ROTATION_DEG, DEFAULT_POSE_NOISE_ROTATION_DEG)), + bodies=_read_bodies(prim), + ) + + +def find_interfaces(stage) -> list: + """Return every prim on the stage marked as a NatNet interface.""" + interfaces = [] + for prim in stage.Traverse(): + attr = prim.GetAttribute(MARKER_ATTR) + if attr and attr.HasAuthoredValue() and bool(attr.Get()): + interfaces.append(prim) + return interfaces + + +def is_interface(prim) -> bool: + attr = prim.GetAttribute(MARKER_ATTR) + return bool(attr and attr.HasAuthoredValue() and bool(attr.Get())) + + +def read_world_pose(prim): + """Return ``((x, y, z), (qx, qy, qz, qw))`` from a prim's USD world transform. + + Reads the position/orientation **stored in the USD stage** (the local-to-world + transform), which is what the physics step writes back each frame. Returns + ``None`` for an invalid/non-xformable prim so callers can mark the body lost. + """ + from pxr import Usd, UsdGeom + + if prim is None or not prim.IsValid(): + return None + xformable = UsdGeom.Xformable(prim) + if not xformable: + return None + matrix = xformable.ComputeLocalToWorldTransform(Usd.TimeCode.Default()) + translation = matrix.ExtractTranslation() + quat = matrix.ExtractRotationQuat() # Gf.Quatd, normalized + imaginary = quat.GetImaginary() + position = (float(translation[0]), float(translation[1]), float(translation[2])) + orientation = ( + float(imaginary[0]), + float(imaginary[1]), + float(imaginary[2]), + float(quat.GetReal()), + ) + return position, orientation + + +def resolve_targets(stage, config): + """Split a config's bodies into (existing, missing) by target prim presence. + + A body whose ``target_prim`` is empty or points at a non-existent prim lands in + ``missing``. Returns two lists of :class:`BodyBinding`. + """ + existing = [] + missing = [] + for body in config.bodies: + prim = stage.GetPrimAtPath(body.target_prim) if body.target_prim else None + if prim is not None and prim.IsValid(): + existing.append(body) + else: + missing.append(body) + return existing, missing + + +# --- internal helpers ---------------------------------------------------------- + + +def _set(prim, name, type_name, value): + attr = prim.CreateAttribute(name, type_name) + attr.Set(value) + return attr + + +def _get(prim, name, default): + attr = prim.GetAttribute(name) + if attr and attr.HasAuthoredValue(): + return attr.Get() + return default + + +def _clear_body_properties(prim) -> None: + for name in list(prim.GetPropertyNames()): + if name.startswith(BODY_PREFIX): + prim.RemoveProperty(name) + + +def _read_bodies(prim) -> list[BodyBinding]: + suffix = f":{BODY_FIELD_RIGID_BODY_NAME}" + keys = [ + name[len(BODY_PREFIX): -len(suffix)] + for name in prim.GetPropertyNames() + if name.startswith(BODY_PREFIX) and name.endswith(suffix) + ] + + bodies: list[BodyBinding] = [] + for key in keys: + rel = prim.GetRelationship(body_attr_name(key, BODY_FIELD_TARGET)) + targets = rel.GetTargets() if rel else [] + bodies.append( + BodyBinding( + rigid_body_name=str(_get(prim, body_attr_name(key, BODY_FIELD_RIGID_BODY_NAME), "")), + target_prim=str(targets[0]) if targets else "", + streaming_id=int(_get(prim, body_attr_name(key, BODY_FIELD_STREAMING_ID), 1)), + parent_id=int(_get(prim, body_attr_name(key, BODY_FIELD_PARENT_ID), -1)), + ) + ) + + # Stable, deterministic order (independent of USD property iteration order). + bodies.sort(key=lambda b: (b.streaming_id, b.rigid_body_name)) + return bodies diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/schema/schema.usda b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/schema/schema.usda new file mode 100644 index 000000000..55defc731 --- /dev/null +++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/schema/schema.usda @@ -0,0 +1,100 @@ +#usda 1.0 +( + """ + NatNet emulator applied API schemas (CODELESS). + + These schemas give the + interface prim typed, Property-panel-friendly attributes WITHOUT compiled + classes. They are codeless — `skipCodeGeneration = true` below — but still need + USD's plugin system to discover the generated registry. + + To produce the registry files (run once, in an env with USD tooling): + + usdGenSchema schema/schema.usda schema/ + + That emits `schema/generatedSchema.usda` and `schema/plugInfo.json`. The Kit + extension then registers the plugin dir on startup (Plug.Registry().RegisterPlugins). + + Until that registration is verified inside Kit, `optitrack.natnet.emulator.isaac` + authors the SAME attribute names as plain namespaced custom attributes (the + registration-free fallback), so nothing here is required for the facade to work. + """ + subLayers = [ + @usd/schema.usda@, + @usdGeom/schema.usda@ + ] +) +{ +} + +over "GLOBAL" ( + customData = { + bool skipCodeGeneration = true + string libraryName = "optitrackNatNet" + string libraryPath = "." + string libraryPrefix = "OptiTrackNatNet" + } +) +{ +} + +class "NatNetInterfaceAPI" ( + inherits = + customData = { + token apiSchemaType = "singleApply" + } + doc = "Marks a prim as a NatNet emulator interface and holds server-level config." +) +{ + bool natnet:isInterface = true ( + doc = "Discovery marker — find_interfaces() scans for prims with this set true." + ) + bool natnet:serverEnabled = true ( + doc = "When true the manager keeps a server running; toggling restarts it." + ) + string natnet:serverIp = "172.31.0.200" ( + doc = "Server interface IP (NatNetUnicastServer.local_interface)." + ) + token natnet:mode = "unicast" ( + allowedTokens = ["unicast", "multicast"] + doc = "Transmission mode." + ) + string natnet:multicastAddr = "239.255.42.99" ( + doc = "Multicast group (only used when mode = multicast)." + ) + int natnet:commandPort = 1510 ( + doc = "NatNet command port." + ) + int natnet:dataPort = 1511 ( + doc = "NatNet data port (frames stream from this source port)." + ) + float natnet:publishRate = 100 ( + doc = "Frame publish rate in Hz." + ) + string natnet:natnetVersion = "4.4.0.0" ( + doc = "Advertised NatNet protocol version." + ) +} + +class "NatNetBodyBindingAPI" ( + inherits = + customData = { + token apiSchemaType = "multipleApply" + token propertyNamespacePrefix = "natnet:body" + } + doc = "One tracked rigid body entry on a NatNet interface prim (apply once per body)." +) +{ + string natnet:body:__INSTANCE_NAME__:rigidBodyName = "" ( + doc = "Motive rigid body name (sRigidBodyDescription.szName)." + ) + int natnet:body:__INSTANCE_NAME__:streamingId = 1 ( + doc = "Streaming ID (sRigidBodyDescription.ID)." + ) + int natnet:body:__INSTANCE_NAME__:parentId = -1 ( + doc = "Parent rigid body ID (-1 if none)." + ) + rel natnet:body:__INSTANCE_NAME__:target ( + doc = "Tracked prim whose world pose is streamed for this body." + ) +} diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_catalog.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_catalog.py new file mode 100644 index 000000000..a3cc8cd34 --- /dev/null +++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_catalog.py @@ -0,0 +1,111 @@ +# Copyright (c) 2024 Carnegie Mellon University +# MIT License - see LICENSE in the repository root for full text. +"""Catalog builder: body counts, wire fidelity, truncation, MAX_MODELS, duplicate targets.""" + +from __future__ import annotations + +import struct + +import pytest + +from optitrack.natnet.emulator.isaac.catalog import build_catalog, find_duplicate_targets +from optitrack.natnet.emulator.isaac.config import BodyBinding, NatNetInterfaceConfig +from optitrack.natnet.emulator.server import natnet_model_types as mt +from optitrack.natnet.emulator.server.natnet_common import ModelLimits + +pytestmark = pytest.mark.unit + + +def _unpack_bodies(payload: bytes): + """Decode a packed sDataDescriptions into [(name, id, parentID), ...].""" + (n,) = struct.unpack_from("frame builder tests (no USD, no Kit).""" + +from __future__ import annotations + +import math +import struct + +import pytest + +from optitrack.natnet.emulator.isaac.frames import ( + MODEL_LIST_CHANGED, + TRACKING_VALID, + BodySample, + apply_pose_noise, + build_frame, + make_rigid_body_data, + to_motive_pose, +) + +pytestmark = pytest.mark.unit + + +def test_to_motive_pose_z_is_identity(): + pos = (1.0, 2.0, 3.0) + quat = (0.1, 0.2, 0.3, 0.9) + assert to_motive_pose(pos, quat, up_axis="Z") == (pos, quat) + # Case-insensitive and default to Z. + assert to_motive_pose(pos, quat, up_axis="z") == (pos, quat) + assert to_motive_pose(pos, quat) == (pos, quat) + + +def test_to_motive_pose_y_swaps_axes_and_quat(): + # (x, y, z) -> (x, z, -y); quat vector part takes the same swap, scalar kept. + pos, quat = to_motive_pose((1.0, 2.0, 3.0), (0.1, 0.2, 0.3, 0.9), up_axis="Y") + assert pos == (1.0, 3.0, -2.0) + assert quat == (0.1, 0.3, -0.2, 0.9) + + +def test_to_motive_pose_y_maps_isaac_up_to_motive_up(): + # Isaac +Z (up) must become Motive +Y (up) under the Y-up emulation. + pos, _ = to_motive_pose((0.0, 0.0, 1.0), (0.0, 0.0, 0.0, 1.0), up_axis="y") + assert pos == (0.0, 1.0, 0.0) + + +def test_make_rigid_body_data_copies_pose_and_sets_valid_bit(): + rb = make_rigid_body_data( + BodySample(7, (1.0, 2.0, 3.0), (0.0, 0.0, 0.7071068, 0.7071068), valid=True) + ) + assert rb.ID == 7 + assert (rb.x, rb.y, rb.z) == (1.0, 2.0, 3.0) + assert rb.qw == pytest.approx(0.7071068) + assert rb.params & TRACKING_VALID # client requires this bit or it skips the body + + +def test_lost_sample_clears_valid_bit_and_is_nan(): + rb = make_rigid_body_data(BodySample.lost(3)) + assert rb.ID == 3 + assert rb.params & TRACKING_VALID == 0 + assert math.isnan(rb.x) and math.isnan(rb.y) and math.isnan(rb.z) + + +def test_build_frame_no_bodies(): + frame = build_frame(0, []) + assert frame.iFrame == 0 + assert frame.nRigidBodies == 0 + assert frame.params == 0 + + +def test_apply_pose_noise_zero_std_is_identity(): + position = (1.0, 2.0, 3.0) + orientation = (0.0, 0.0, 0.0, 1.0) + pos_out, quat_out = apply_pose_noise(position, orientation, 0.0, 0.0) + assert pos_out == position + assert quat_out == pytest.approx(orientation) + + +def test_apply_pose_noise_preserves_y_position(): + np = pytest.importorskip("numpy") + np.random.seed(0) + position = (0.0, 1.5, 0.0) + orientation = (0.0, 0.0, 0.0, 1.0) + pos_out, _ = apply_pose_noise(position, orientation, 0.001, 0.0) + # Before the euler-yaw shadowing bug, y collapsed to ~0 instead of staying near 1.5. + assert pos_out[1] == pytest.approx(1.5, abs=0.01) + + +def test_apply_pose_noise_adds_position_jitter(): + np = pytest.importorskip("numpy") + np.random.seed(1) + position = (0.0, 0.0, 0.0) + orientation = (0.0, 0.0, 0.0, 1.0) + pos_out, _ = apply_pose_noise(position, orientation, 0.001, 0.0) + assert pos_out != position + + +def test_build_frame_multiple_bodies_preserve_order(): + samples = [ + BodySample(1, (1.0, 0.0, 0.0)), + BodySample(2, (0.0, 2.0, 0.0)), + BodySample(5, (0.0, 0.0, 3.0)), + ] + frame = build_frame(42, samples) + assert frame.iFrame == 42 + assert frame.nRigidBodies == 3 + assert frame.RigidBodies[0].ID == 1 and frame.RigidBodies[0].x == 1.0 + assert frame.RigidBodies[1].ID == 2 and frame.RigidBodies[1].y == 2.0 + assert frame.RigidBodies[2].ID == 5 and frame.RigidBodies[2].z == 3.0 + + +def test_model_list_changed_sets_frame_param_bit(): + assert build_frame(0, [], model_list_changed=True).params & MODEL_LIST_CHANGED + assert build_frame(0, [], model_list_changed=False).params & MODEL_LIST_CHANGED == 0 + + +def test_frame_packs_and_rigid_body_section_decodes(): + frame = build_frame(9, [BodySample(4, (1.5, -2.5, 3.5), (0.0, 0.0, 0.0, 1.0))]) + payload = frame.pack(natnet_major=4, natnet_minor=4) + + # iFrame, then 4.4 counted sections (count+size each) for markersets & other markers. + (iframe,) = struct.unpack_from(" author -> read is stable + author_interface(stage, "/World/NatNetInterface", cfg) + assert read_interface(find_interfaces(stage)[0]) == cfg + + +def test_reauthoring_removes_stale_bodies(): + stage = _new_stage() + author_interface(stage, "/World/NatNetInterface", _CONFIG) + + single = NatNetInterfaceConfig.from_dict( + {"bodies": [{"rigid_body_name": "Drone", "target_prim": "/World/base_link", "streaming_id": 1}]} + ) + author_interface(stage, "/World/NatNetInterface", single) + + cfg = read_interface(find_interfaces(stage)[0]) + assert [b.rigid_body_name for b in cfg.bodies] == ["Drone"] + + +def test_up_axis_authors_and_reads_back(): + stage = _new_stage() + # Default (absent) -> Z. + author_interface(stage, "/World/NatNetInterface", _CONFIG) + assert read_interface(find_interfaces(stage)[0]).up_axis == "Z" + + # Explicit Y survives the USD round trip. + cfg = NatNetInterfaceConfig.from_dict({**_CONFIG, "up_axis": "Y"}) + author_interface(stage, "/World/NatNetInterface", cfg) + assert read_interface(find_interfaces(stage)[0]).up_axis == "Y" + + +def test_pose_noise_authors_and_reads_back(): + stage = _new_stage() + cfg = NatNetInterfaceConfig.from_dict( + { + **_CONFIG, + "pose_noise_enabled": False, + "pose_noise_std_meters": 0.001, + "pose_noise_rotation_deg": 0.1, + } + ) + author_interface(stage, "/World/NatNetInterface", cfg) + read = read_interface(find_interfaces(stage)[0]) + assert read.pose_noise_enabled is False + assert read.pose_noise_std_meters == pytest.approx(0.001) + assert read.pose_noise_rotation_deg == pytest.approx(0.1) + + +def test_empty_target_round_trips(): + # The UI's "Add body" can create a body with no target yet (set later in the + # Property panel); it must author and read back cleanly with an empty target. + stage = _new_stage() + cfg = NatNetInterfaceConfig(bodies=[BodyBinding("Drone", "", 1)]) + author_interface(stage, "/World/NatNetInterface", cfg) + read = read_interface(find_interfaces(stage)[0]) + assert read.bodies[0].rigid_body_name == "Drone" + assert read.bodies[0].target_prim == "" + + +def test_invalid_config_raises_before_authoring(): + stage = _new_stage() + with pytest.raises(ValueError): + author_interface(stage, "/World/NatNetInterface", {"mode": "bogus"}) + assert find_interfaces(stage) == [] diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_interface_config.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_interface_config.py new file mode 100644 index 000000000..9247c5f12 --- /dev/null +++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_interface_config.py @@ -0,0 +1,186 @@ +# Copyright (c) 2024 Carnegie Mellon University +# MIT License - see LICENSE in the repository root for full text. +"""Hermetic unit tests for the pure-Python NatNet interface config model. + +No USD / Isaac imports — exercises dataclasses, dict normalization, the attribute +name builder, instance-key generation, and validation. +""" + +from __future__ import annotations + +import pytest + +from optitrack.natnet.emulator.isaac.config import ( + DEFAULT_POSE_NOISE_ENABLED, + DEFAULT_POSE_NOISE_ROTATION_DEG, + DEFAULT_POSE_NOISE_STD_METERS, + BodyBinding, + NatNetInterfaceConfig, + body_attr_name, + make_instance_key, +) + +pytestmark = pytest.mark.unit + + +def test_defaults_match_server_expectations(): + cfg = NatNetInterfaceConfig() + assert cfg.server_enabled is True + assert cfg.server_ip == "172.31.0.200" + assert cfg.mode == "unicast" + assert cfg.command_port == 1510 + assert cfg.data_port == 1511 + assert cfg.up_axis == "Z" # Isaac/USD native; matches the reference Motive setup + assert cfg.pose_noise_enabled is DEFAULT_POSE_NOISE_ENABLED + assert cfg.pose_noise_std_meters == DEFAULT_POSE_NOISE_STD_METERS + assert cfg.pose_noise_rotation_deg == DEFAULT_POSE_NOISE_ROTATION_DEG + assert cfg.bodies == [] + + +def test_up_axis_from_dict_normalizes_case(): + assert NatNetInterfaceConfig.from_dict({"up_axis": "y"}).up_axis == "Y" + assert NatNetInterfaceConfig.from_dict({"up_axis": "z"}).up_axis == "Z" + # Absent -> default Z. + assert NatNetInterfaceConfig.from_dict({}).up_axis == "Z" + + +def test_up_axis_survives_round_trip(): + cfg = NatNetInterfaceConfig.from_dict({"up_axis": "Y"}) + assert NatNetInterfaceConfig.from_dict(cfg.to_dict()).up_axis == "Y" + + +def test_pose_noise_survives_round_trip(): + cfg = NatNetInterfaceConfig.from_dict( + { + "pose_noise_enabled": False, + "pose_noise_std_meters": 0.001, + "pose_noise_rotation_deg": 0.1, + } + ) + restored = NatNetInterfaceConfig.from_dict(cfg.to_dict()) + assert restored.pose_noise_enabled is False + assert restored.pose_noise_std_meters == 0.001 + assert restored.pose_noise_rotation_deg == 0.1 + + +def test_from_dict_with_bodies_as_list(): + cfg = NatNetInterfaceConfig.from_dict( + { + "server_ip": "10.0.0.5", + "bodies": [ + {"rigid_body_name": "Drone", "target_prim": "/World/base_link", "streaming_id": 1}, + ], + } + ) + assert cfg.server_ip == "10.0.0.5" + assert len(cfg.bodies) == 1 + assert cfg.bodies[0] == BodyBinding("Drone", "/World/base_link", 1, -1) + + +def test_from_dict_with_bodies_as_prim_mapping(): + # The "dictionary of prims -> rigid body names and stuff" form. + cfg = NatNetInterfaceConfig.from_dict( + { + "bodies": { + "/World/base_link": {"rigid_body_name": "Drone", "streaming_id": 1}, + "/World/target": {"rigid_body_name": "Target", "streaming_id": 2}, + } + } + ) + by_name = {b.rigid_body_name: b for b in cfg.bodies} + assert by_name["Drone"].target_prim == "/World/base_link" + assert by_name["Target"].target_prim == "/World/target" + assert by_name["Target"].streaming_id == 2 + + +def test_to_dict_round_trip(): + cfg = NatNetInterfaceConfig.from_dict( + { + "mode": "multicast", + "publish_rate": 120, + "bodies": [{"rigid_body_name": "Drone", "target_prim": "/World/base_link"}], + } + ) + restored = NatNetInterfaceConfig.from_dict(cfg.to_dict()) + assert restored == cfg + + +def test_body_from_dict_requires_target_and_name(): + with pytest.raises(ValueError): + BodyBinding.from_dict({"rigid_body_name": "Drone"}) # no target_prim + with pytest.raises(ValueError): + BodyBinding.from_dict({"target_prim": "/World/base_link"}) # no name + + +def test_body_attr_name_builder(): + assert body_attr_name("Drone", "streamingId") == "natnet:body:Drone:streamingId" + + +def test_make_instance_key_sanitizes_and_dedupes(): + used: set[str] = set() + assert make_instance_key("Drone 1", used) == "Drone_1" + # collision after sanitization -> numeric suffix + assert make_instance_key("Drone-1", used) == "Drone_1_1" + # leading digit gets a safe prefix + assert make_instance_key("3PO", used).startswith("b_") + + +def test_assign_instance_keys_are_unique(): + cfg = NatNetInterfaceConfig( + bodies=[ + BodyBinding("Drone", "/World/a", 1), + BodyBinding("Drone", "/World/b", 2), # duplicate display name + ] + ) + keys = [k for k, _ in cfg.assign_instance_keys()] + assert len(set(keys)) == 2 + + +@pytest.mark.parametrize( + "overrides", + [ + {"mode": "bogus"}, + {"command_port": 0}, + {"data_port": 70000}, + {"command_port": 1510, "data_port": 1510}, + {"publish_rate": 0}, + {"up_axis": "X"}, + {"up_axis": "bogus"}, + {"pose_noise_std_meters": -0.001}, + {"pose_noise_rotation_deg": -0.1}, + ], +) +def test_validate_rejects_bad_server_config(overrides): + cfg = NatNetInterfaceConfig(**overrides) + with pytest.raises(ValueError): + cfg.validate() + + +def test_validate_rejects_duplicate_streaming_ids(): + cfg = NatNetInterfaceConfig( + bodies=[ + BodyBinding("A", "/World/a", 1), + BodyBinding("B", "/World/b", 1), + ] + ) + with pytest.raises(ValueError): + cfg.validate() + + +def test_validate_rejects_blank_rigid_body_name(): + cfg = NatNetInterfaceConfig(bodies=[BodyBinding("", "/World/a", 1)]) + with pytest.raises(ValueError): + cfg.validate() + + +def test_validate_allows_empty_target(): + # An empty target is valid: a freshly added body to be pointed in the UI/Property panel. + cfg = NatNetInterfaceConfig(bodies=[BodyBinding("Drone", "", 1)]) + assert cfg.validate() is cfg + + +def test_validate_accepts_good_config(): + cfg = NatNetInterfaceConfig( + bodies=[BodyBinding("Drone", "/World/base_link", 1)] + ) + assert cfg.validate() is cfg diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_pose_sampling.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_pose_sampling.py new file mode 100644 index 000000000..25db5ad96 --- /dev/null +++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_pose_sampling.py @@ -0,0 +1,238 @@ +# Copyright (c) 2024 Carnegie Mellon University +# MIT License - see LICENSE in the repository root for full text. +"""Pose sampling and catalog resync against an in-memory USD stage (fake server, no sockets).""" + +from __future__ import annotations + +import math + +import pytest + +pytest.importorskip("pxr") + +from pxr import Gf, Usd, UsdGeom # noqa: E402 + +from optitrack.natnet.emulator.isaac.config import BodyBinding, NatNetInterfaceConfig # noqa: E402 +from optitrack.natnet.emulator.isaac.frames import MODEL_LIST_CHANGED, TRACKING_VALID # noqa: E402 +from optitrack.natnet.emulator.isaac.manager import NatNetServerManager # noqa: E402 +from optitrack.natnet.emulator.isaac.usd_bindings import author_interface, read_world_pose # noqa: E402 + +pytestmark = pytest.mark.unit + + +class FakeServer: + def __init__(self): + self.frames = [] + self.payloads = [] + + def set_model_def_payload(self, payload): + self.payloads.append(payload) + + def start(self): + pass + + def shutdown(self): + pass + + def enqueue_mocap_data(self, frame): + self.frames.append(frame) + + +def _xform(stage, path, translate=(0.0, 0.0, 0.0)): + xform = UsdGeom.Xform.Define(stage, path) + xform.AddTranslateOp().Set(Gf.Vec3d(*translate)) + return xform + + +def _manager_with_fake(): + fake = FakeServer() + mgr = NatNetServerManager(server_factory=lambda cfg: fake) + return mgr, fake + + +# --- read_world_pose ------------------------------------------------------------- + + +def test_read_world_pose_returns_translation(): + stage = Usd.Stage.CreateInMemory() + _xform(stage, "/World/base_link", translate=(1.0, 2.0, 3.0)) + pose = read_world_pose(stage.GetPrimAtPath("/World/base_link")) + assert pose is not None + (x, y, z), (qx, qy, qz, qw) = pose + assert (round(x, 3), round(y, 3), round(z, 3)) == (1.0, 2.0, 3.0) + assert qw == pytest.approx(1.0) + + +def test_read_world_pose_invalid_prim_is_none(): + stage = Usd.Stage.CreateInMemory() + assert read_world_pose(stage.GetPrimAtPath("/World/nope")) is None + + +# --- sample_once ----------------------------------------------------------------- + + +def test_sample_once_no_bodies(): + stage = Usd.Stage.CreateInMemory() + author_interface(stage, "/World/NatNetInterface", NatNetInterfaceConfig()) + mgr, fake = _manager_with_fake() + mgr.start_server(NatNetInterfaceConfig(server_ip="127.0.0.1")) + frame = mgr.sample_once(stage) + assert frame is not None and frame.nRigidBodies == 0 + + +def test_sample_once_streams_world_pose(): + stage = Usd.Stage.CreateInMemory() + _xform(stage, "/World/base_link", translate=(4.0, 5.0, 6.0)) + cfg = NatNetInterfaceConfig( + server_ip="127.0.0.1", + pose_noise_enabled=False, + bodies=[BodyBinding("Drone", "/World/base_link", 1)], + ) + author_interface(stage, "/World/NatNetInterface", cfg) + mgr, fake = _manager_with_fake() + mgr.start_server(cfg) + + frame = mgr.sample_once(stage) + assert frame.nRigidBodies == 1 + rb = frame.RigidBodies[0] + assert rb.ID == 1 + assert (round(rb.x, 3), round(rb.y, 3), round(rb.z, 3)) == (4.0, 5.0, 6.0) + assert rb.params & TRACKING_VALID + # First frame after start resyncs -> client should be told the model list changed. + assert frame.params & MODEL_LIST_CHANGED + + +def test_sample_once_missing_prim_is_lost(): + stage = Usd.Stage.CreateInMemory() + cfg = NatNetInterfaceConfig( + server_ip="127.0.0.1", bodies=[BodyBinding("Ghost", "/World/missing", 9)] + ) + author_interface(stage, "/World/NatNetInterface", cfg) + mgr, fake = _manager_with_fake() + mgr.start_server(cfg) + + frame = mgr.sample_once(stage) + rb = frame.RigidBodies[0] + assert rb.ID == 9 + assert rb.params & TRACKING_VALID == 0 + assert math.isnan(rb.x) + + +def test_moving_prim_updates_streamed_position(): + stage = Usd.Stage.CreateInMemory() + xform = _xform(stage, "/World/base_link", translate=(0.0, 0.0, 0.0)) + cfg = NatNetInterfaceConfig( + server_ip="127.0.0.1", + pose_noise_enabled=False, + bodies=[BodyBinding("Drone", "/World/base_link", 1)], + ) + author_interface(stage, "/World/NatNetInterface", cfg) + mgr, fake = _manager_with_fake() + mgr.start_server(cfg) + + mgr.sample_once(stage) + xform.GetOrderedXformOps()[0].Set(Gf.Vec3d(10.0, 0.0, 0.0)) + frame = mgr.sample_once(stage) + assert round(frame.RigidBodies[0].x, 3) == 10.0 + + +def test_up_axis_z_streams_isaac_pose_as_is(): + stage = Usd.Stage.CreateInMemory() + _xform(stage, "/World/base_link", translate=(1.0, 2.0, 3.0)) + cfg = NatNetInterfaceConfig( + server_ip="127.0.0.1", + up_axis="Z", + pose_noise_enabled=False, + bodies=[BodyBinding("Drone", "/World/base_link", 1)], + ) + author_interface(stage, "/World/NatNetInterface", cfg) + mgr, _fake = _manager_with_fake() + mgr.start_server(cfg) + + rb = mgr.sample_once(stage).RigidBodies[0] + assert (round(rb.x, 3), round(rb.y, 3), round(rb.z, 3)) == (1.0, 2.0, 3.0) + + +def test_up_axis_y_reaxes_streamed_pose(): + # Y-up Motive emulation: Isaac (x, y, z) streams as (x, z, -y). + stage = Usd.Stage.CreateInMemory() + _xform(stage, "/World/base_link", translate=(1.0, 2.0, 3.0)) + cfg = NatNetInterfaceConfig( + server_ip="127.0.0.1", + up_axis="Y", + pose_noise_enabled=False, + bodies=[BodyBinding("Drone", "/World/base_link", 1)], + ) + author_interface(stage, "/World/NatNetInterface", cfg) + mgr, _fake = _manager_with_fake() + mgr.start_server(cfg) + + rb = mgr.sample_once(stage).RigidBodies[0] + assert (round(rb.x, 3), round(rb.y, 3), round(rb.z, 3)) == (1.0, 3.0, -2.0) + + +def test_body_added_while_live_is_picked_up_on_resync(): + stage = Usd.Stage.CreateInMemory() + _xform(stage, "/World/a", translate=(1.0, 0.0, 0.0)) + _xform(stage, "/World/b", translate=(0.0, 2.0, 0.0)) + cfg1 = NatNetInterfaceConfig( + server_ip="127.0.0.1", bodies=[BodyBinding("A", "/World/a", 1)] + ) + author_interface(stage, "/World/NatNetInterface", cfg1) + mgr, fake = _manager_with_fake() + mgr.start_server(cfg1) + + first = mgr.sample_once(stage) + assert first.nRigidBodies == 1 + + # Add a second body live: re-author the prim, then mark dirty (the UI/USD-notice + # path calls mark_dirty for us in Kit). + cfg2 = NatNetInterfaceConfig( + server_ip="127.0.0.1", + bodies=[BodyBinding("A", "/World/a", 1), BodyBinding("B", "/World/b", 2)], + ) + author_interface(stage, "/World/NatNetInterface", cfg2) + mgr.mark_dirty() + + second = mgr.sample_once(stage) + assert second.nRigidBodies == 2 + assert second.params & MODEL_LIST_CHANGED # catalog grew -> tell the client + ids = {second.RigidBodies[i].ID for i in range(second.nRigidBodies)} + assert ids == {1, 2} + # MODELDEF payload was refreshed on the server for the new catalog. + assert len(fake.payloads) >= 2 + + +def test_target_prim_created_after_start_becomes_valid(): + """A body whose target prim is spawned *after* the server starts (e.g. a Pegasus + drone base_link created on the first Play tick) must start streaming a valid pose + as soon as the prim appears — no mark_dirty/resync required, because the target + path is re-resolved every sample.""" + stage = Usd.Stage.CreateInMemory() + cfg = NatNetInterfaceConfig( + server_ip="127.0.0.1", + pose_noise_enabled=False, + bodies=[BodyBinding("Drone", "/World/drone1/base_link", 1)], + ) + author_interface(stage, "/World/NatNetInterface", cfg) + mgr, _fake = _manager_with_fake() + mgr.start_server(cfg) + + # Prim does not exist yet -> lost. + first = mgr.sample_once(stage) + assert first.RigidBodies[0].params & TRACKING_VALID == 0 + + # Spawn the target prim later (simulating the Play-tick drone creation). + _xform(stage, "/World/drone1/base_link", translate=(7.0, 8.0, 9.0)) + + # Next sample re-resolves the path -> valid pose, with no mark_dirty(). + second = mgr.sample_once(stage) + rb = second.RigidBodies[0] + assert rb.params & TRACKING_VALID + assert (round(rb.x, 3), round(rb.y, 3), round(rb.z, 3)) == (7.0, 8.0, 9.0) + + +def test_sample_once_noop_without_server(): + stage = Usd.Stage.CreateInMemory() + mgr, _fake = _manager_with_fake() + assert mgr.sample_once(stage) is None diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_pose_streaming.py b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_pose_streaming.py new file mode 100644 index 000000000..bc526c5b5 --- /dev/null +++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_pose_streaming.py @@ -0,0 +1,86 @@ +# Copyright (c) 2024 Carnegie Mellon University +# MIT License - see LICENSE in the repository root for full text. +"""Loopback: sample_once on a USD prim → NAT_FRAMEOFDATA with sampled position. + +Real server + UDP sockets + in-memory stage. Requires pxr. +""" + +from __future__ import annotations + +import socket +import struct +import time + +import pytest + +pytest.importorskip("pxr") + +from pxr import Gf, Usd, UsdGeom # noqa: E402 + +from natnet_test_helpers import NatNetTestClient, ephemeral_udp_port # noqa: E402 + +from optitrack.natnet.emulator.isaac.config import BodyBinding, NatNetInterfaceConfig # noqa: E402 +from optitrack.natnet.emulator.isaac.manager import NatNetServerManager # noqa: E402 +from optitrack.natnet.emulator.isaac.usd_bindings import author_interface # noqa: E402 +from optitrack.natnet.emulator.server import natnet_server_types as st # noqa: E402 + +pytestmark = pytest.mark.unit + + +def _decode_first_rigid_body(payload: bytes): + # iFrame(4) + markersets(count4+size4) + othermarkers(count4+size4) = 20, then + # rigid bodies: count(4) + size(4) at 20, first body at 28: id + xyz. + rb_count, _rb_size = struct.unpack_from("1``: ``Drone1``, ``Drone2``, … (ids 1..N) + +Intended multi-robot profile pairing (see ``natnet_config.yaml`` commented scaffolding): + - ``robot_1``: tracks its drone + the shared ``Target`` + - ``robot_2``: tracks its drone + the shared ``Target`` + - ``robot_3``: tracks its drone only (no Target in profile) + +Set ``NUM_ROBOTS=3`` on both sim and robot stacks; each container picks its profile +via ``ROBOT_NAME``. + +Env: + - ``NUM_ROBOTS`` (default 1) + - ``ENABLE_LIDAR`` (default false) + - ``PLAY_SIM_ON_START`` (default true) + - ``NATNET_BODY_NAME`` / ``NATNET_TARGET_NAME`` (optional name overrides) +""" + +import asyncio +import os +import sys +import time + +import carb +from isaacsim import SimulationApp + +_headless = os.environ.get("ISAAC_SIM_HEADLESS", "false").lower() == "true" +simulation_app = SimulationApp({"headless": _headless}) + +import omni.kit.app +import omni.timeline +import omni.usd + +from omni.isaac.core.world import World + +from pegasus.simulator.params import SIMULATION_ENVIRONMENTS +from pegasus.simulator.logic.interface.pegasus_interface import PegasusInterface +from pegasus.simulator.ogn.api.spawn_multirotor import spawn_px4_multirotor_node +from pegasus.simulator.ogn.api.spawn_zed_camera import add_zed_stereo_camera_subgraph +from pegasus.simulator.ogn.api.spawn_rtx_lidar import add_rtx_lidar_subgraph + +sys.path.insert(0, os.path.normpath(os.path.join(os.path.dirname(os.path.abspath(__file__)), "..", "utils"))) +from scene_prep import scale_stage_prim, add_colliders, add_dome_light, save_scene_as_contained_usd + +from optitrack.natnet.emulator.isaac import ( + DEFAULT_TARGET_PATH, + DEFAULT_TARGET_POSITION, + DEFAULT_TARGET_STREAMING_ID, + author_static_target, + start_drone_natnet_server, +) + +# --------------------- CONFIGURATION --------------------- +ENV_URL = SIMULATION_ENVIRONMENTS["Default Environment"] +STAGE_SCALE = 1.0 +SAVE_SCENE_TO = None +DRONE_USD = "~/.local/share/ov/data/documents/Kit/shared/exts/pegasus.simulator/pegasus/simulator/assets/Robots/Iris/iris.usd" + +NUM_ROBOTS = int(os.environ.get("NUM_ROBOTS", "1")) +ENABLE_LIDAR = os.environ.get("ENABLE_LIDAR", "false").lower() == "true" +NATNET_BODY_NAME = os.environ.get("NATNET_BODY_NAME", "Drone") +NATNET_TARGET_NAME = os.environ.get("NATNET_TARGET_NAME", "Target") + +_NATNET_SERVER_KWARGS = { + "pose_noise_enabled": True, + "pose_noise_std_meters": 0.0005, + "pose_noise_rotation_deg": 0.05, +} +# --------------------------------------------------------- + + +ext_manager = omni.kit.app.get_app().get_extension_manager() +for ext in [ + "omni.graph.core", + "omni.graph.action", + "omni.graph.action_nodes", + "isaacsim.core.nodes", + "omni.graph.ui", + "omni.graph.visualization.nodes", + "omni.graph.scriptnode", + "omni.graph.window.action", + "omni.graph.window.generic", + "omni.graph.ui_nodes", + "pegasus.simulator", +]: + if not ext_manager.is_extension_enabled(ext): + ext_manager.set_extension_enabled_immediate(ext, True) + + +def wait_for_stage(stage, timeout_s: float = 10.0): + for _ in range(int(timeout_s / 0.1)): + omni.kit.app.get_app().update() + world_prim = stage.GetPrimAtPath("/World") + if world_prim.IsValid(): + non_physics = [c for c in world_prim.GetChildren() if c.GetName() != "PhysicsScene"] + if non_physics: + return True + time.sleep(0.1) + return False + + +def _drone_body_name(index: int) -> str: + """Single agent uses bare ``Drone``; multi uses ``Drone1``, ``Drone2``, …""" + return NATNET_BODY_NAME if NUM_ROBOTS == 1 else f"{NATNET_BODY_NAME}{index}" + + +def spawn_drone(index: int): + robot_name = f"robot_{index}" + drone_prim = f"/World/drone{index}/base_link" + init_x = 2.0 * (index - 1) - 2.0 * (NUM_ROBOTS - 1) / 2.0 + + graph_handle = spawn_px4_multirotor_node( + pegasus_node_name=f"PX4Multirotor_{index}", + drone_prim=drone_prim, + robot_name=robot_name, + vehicle_id=index, + domain_id=index, + usd_file=DRONE_USD, + init_pos=[init_x, 0.0, 0.07], + init_orient=[0.0, 0.0, 0.0, 1.0], + ) + + add_zed_stereo_camera_subgraph( + parent_graph_handle=graph_handle, + drone_prim=drone_prim, + robot_name=robot_name, + camera_name="ZEDCamera", + camera_offset=[0.2, 0.0, -0.05], + camera_rotation_offset=[0.0, 0.0, 0.0], + ) + + if ENABLE_LIDAR: + add_rtx_lidar_subgraph( + parent_graph_handle=graph_handle, + drone_prim=drone_prim, + robot_name=robot_name, + lidar_config="ouster_os1", + lidar_topic_name="point_cloud_raw", + lidar_offset=[0.0, 0.0, 0.025], + lidar_rotation_offset=[0.0, 0.0, 0.0], + min_range=0.75, + ) + + +class PegasusApp: + + def __init__(self): + self.timeline = omni.timeline.get_timeline_interface() + self.natnet_manager = None + + self.pg = PegasusInterface() + self.pg._world = World(**self.pg._world_settings) + self.world = self.pg.world + self.timeline.stop() + + self.pg.load_environment(ENV_URL) + + stage = omni.usd.get_context().get_stage() + if stage is None: + raise RuntimeError("Stage failed to load") + + if not wait_for_stage(stage): + carb.log_warn("Stage load timed out — continuing anyway.") + + stage_prim = stage.GetPrimAtPath("/World/stage") + if stage_prim.IsValid(): + scale_stage_prim(stage, "/World/stage", STAGE_SCALE) + add_colliders(stage_prim) + for _ in range(10): + omni.kit.app.get_app().update() + else: + carb.log_warn("/World/stage not found — skipping scale and collision.") + + add_dome_light(stage) + + if SAVE_SCENE_TO: + import tempfile + tmp_usd = os.path.join(tempfile.gettempdir(), "prepared_scene.usd") + success, error = asyncio.get_event_loop().run_until_complete( + omni.usd.get_context().export_as_stage_async(tmp_usd) + ) + if success: + os.makedirs(SAVE_SCENE_TO, exist_ok=True) + save_scene_as_contained_usd(tmp_usd, SAVE_SCENE_TO) + os.remove(tmp_usd) + else: + carb.log_error(f"Scene export failed: {error}") + + print(f"[example_multi_natnet] Spawning {NUM_ROBOTS} drone(s), lidar={'on' if ENABLE_LIDAR else 'off'}") + for i in range(1, NUM_ROBOTS + 1): + spawn_drone(i) + + self._setup_natnet(stage) + self.play_on_start = os.environ.get("PLAY_SIM_ON_START", "true").lower() == "true" + + def _setup_natnet(self, stage): + """Author NatNet bodies: one per drone plus one shared static target.""" + try: + author_static_target(stage, DEFAULT_TARGET_PATH, DEFAULT_TARGET_POSITION) + bodies = [ + (_drone_body_name(i), i, f"/World/drone{i}/base_link/body") + for i in range(1, NUM_ROBOTS + 1) + ] + bodies.append((NATNET_TARGET_NAME, DEFAULT_TARGET_STREAMING_ID, DEFAULT_TARGET_PATH)) + + self.natnet_manager = start_drone_natnet_server( + stage, bodies, **_NATNET_SERVER_KWARGS + ) + carb.log_warn( + f"[natnet] Emulator started with {NUM_ROBOTS} drone body(ies) " + f"and shared target '{NATNET_TARGET_NAME}' (robot_1/robot_2 subscribe via " + f"natnet_config; robot_3 omits Target)." + ) + except Exception as exc: # noqa: BLE001 - never let NatNet kill the sim + carb.log_error(f"[natnet] Failed to start emulator: {exc}") + self.natnet_manager = None + + def run(self): + if self.play_on_start: + self.timeline.play() + else: + self.timeline.stop() + + app = omni.kit.app.get_app() + while simulation_app.is_running(): + world = World.instance() + if world is not None and hasattr(world, '_scene'): + world.step(render=True) + if world is not self.world: + self.world = world + self.pg._world = world + else: + app.update() + + carb.log_warn("Closing simulation.") + if self.natnet_manager is not None: + self.natnet_manager.on_shutdown() + self.timeline.stop() + simulation_app.close() + + +def main(): + PegasusApp().run() + + +if __name__ == "__main__": + main() diff --git a/simulation/isaac-sim/launch_scripts/example_one_px4_pegasus_launch_script.py b/simulation/isaac-sim/launch_scripts/example_one_px4_pegasus_launch_script.py index 11819fc2f..53c975e09 100755 --- a/simulation/isaac-sim/launch_scripts/example_one_px4_pegasus_launch_script.py +++ b/simulation/isaac-sim/launch_scripts/example_one_px4_pegasus_launch_script.py @@ -97,6 +97,12 @@ sys.path.insert(0, os.path.normpath(os.path.join(os.path.dirname(os.path.abspath(__file__)), "..", "utils"))) from scene_prep import scale_stage_prim, add_colliders, add_dome_light, save_scene_as_contained_usd +# gps_utils lives in this launch_scripts directory. +_LAUNCH_SCRIPTS_DIR = os.path.dirname(os.path.abspath(__file__)) +if _LAUNCH_SCRIPTS_DIR not in sys.path: + sys.path.insert(0, _LAUNCH_SCRIPTS_DIR) +from gps_utils import set_gps_origins, DEFAULT_WORLD_ORIGIN + # --------------------- CONFIGURATION --------------------- # Environment to load. Swap this URL/key for any other scene. @@ -111,6 +117,18 @@ SAVE_SCENE_TO = None # e.g. os.path.expanduser("~/AirStack/my_scene/") DRONE_USD = "~/.local/share/ov/data/documents/Kit/shared/exts/pegasus.simulator/pegasus/simulator/assets/Robots/Iris/iris.usd" + +# GPS world anchor: what world (0, 0, 0) maps to in real GPS coordinates. Must +# match the GCS origin (gcs_visualizer/gcs_utils.py) and the robot's +# natnet_ros2 mavros_gp_origin.yaml, otherwise PX4 SITL defaults to Zurich and +# Foxglove waypoints pick up a ~1.8e6 m boot-ENU offset (wrong-way navigation). +WORLD_GPS_ORIGIN = DEFAULT_WORLD_ORIGIN + +# Single drone spawned at the world origin. domain_id / spawn must match the +# spawn_px4_multirotor_node call below. +DRONE_CONFIGS = [ + {"domain_id": 1, "x_m": 0.0, "y_m": 0.0, "z_m": 0.07}, +] # --------------------------------------------------------- @@ -149,6 +167,10 @@ def wait_for_stage(stage, timeout_s: float = 10.0): class PegasusApp: def __init__(self): + # Write GPS home before spawning so the drone's global position shares + # the GCS datum. Must run before the PX4 SITL subprocess starts. + set_gps_origins(DRONE_CONFIGS, world_origin=WORLD_GPS_ORIGIN) + self.timeline = omni.timeline.get_timeline_interface() # Start Pegasus interface + world diff --git a/simulation/isaac-sim/launch_scripts/example_one_px4_pegasus_natnet_launch_script.py b/simulation/isaac-sim/launch_scripts/example_one_px4_pegasus_natnet_launch_script.py new file mode 100644 index 000000000..12fa2213f --- /dev/null +++ b/simulation/isaac-sim/launch_scripts/example_one_px4_pegasus_natnet_launch_script.py @@ -0,0 +1,273 @@ +#!/usr/bin/env python +""" +Single-drone PX4 Pegasus launcher with OptiTrack NatNet mocap streaming. + +Same scene prep and sensor stack as ``example_one_px4_pegasus_launch_script.py``, plus +a Motive-compatible NatNet server that always streams: + + - ``Drone`` (id 1) from the Pegasus ``body`` prim under ``/World/base_link`` + - ``Target`` (id 100) from a static ``/World/target`` prim + +Pair with robot-side ``LAUNCH_NATNET=true`` and a matching ``natnet_config.yaml`` +profile. To consume the target on the robot, add a Target body to the profile +(see the commented scaffolding in ``natnet_config.yaml``). + +Override rigid-body names with ``NATNET_BODY_NAME`` / ``NATNET_TARGET_NAME``. +""" + +import os +import sys +import time +import asyncio + +import carb +from isaacsim import SimulationApp + +_LIVESTREAM = os.environ.get("ISAAC_SIM_LIVESTREAM", "").lower() == "true" + +if _LIVESTREAM: + _SIM_APP_CONFIG = { + "width": 1280, + "height": 720, + "window_width": 1920, + "window_height": 1080, + "headless": True, + "hide_ui": False, + "renderer": "RaytracedLighting", + "display_options": 3286, + } +else: + _SIM_APP_CONFIG = {"headless": False} + +simulation_app = SimulationApp(launch_config=_SIM_APP_CONFIG) + +if _LIVESTREAM: + from isaacsim.core.utils.extensions import enable_extension + simulation_app.set_setting("/app/window/drawMouse", True) + simulation_app.set_setting("/app/livestream/enabled", True) + LIVESTREAM_UDP_PORT = int(os.environ.get("ISAAC_SIM_LIVESTREAM_UDP_PORT", "49099")) + simulation_app.set_setting("/app/livestream/fixedHostPort", LIVESTREAM_UDP_PORT) + simulation_app.set_setting("/app/livestream/minHostPort", LIVESTREAM_UDP_PORT) + simulation_app.set_setting("/app/livestream/maxHostPort", LIVESTREAM_UDP_PORT) + enable_extension("omni.kit.livestream.webrtc") + +import omni.kit.app +import omni.timeline +import omni.usd + +from omni.isaac.core.world import World + +from pegasus.simulator.params import SIMULATION_ENVIRONMENTS +from pegasus.simulator.logic.interface.pegasus_interface import PegasusInterface +from pegasus.simulator.ogn.api.spawn_multirotor import spawn_px4_multirotor_node +from pegasus.simulator.ogn.api.spawn_zed_camera import add_zed_stereo_camera_subgraph +from pegasus.simulator.ogn.api.spawn_rtx_lidar import add_rtx_lidar_subgraph + +sys.path.insert(0, os.path.normpath(os.path.join(os.path.dirname(os.path.abspath(__file__)), "..", "utils"))) +from scene_prep import scale_stage_prim, add_colliders, add_dome_light, save_scene_as_contained_usd + +# gps_utils lives in this launch_scripts directory. +_LAUNCH_SCRIPTS_DIR = os.path.dirname(os.path.abspath(__file__)) +if _LAUNCH_SCRIPTS_DIR not in sys.path: + sys.path.insert(0, _LAUNCH_SCRIPTS_DIR) +from gps_utils import set_gps_origins, DEFAULT_WORLD_ORIGIN + +from optitrack.natnet.emulator.isaac import ( + DEFAULT_TARGET_PATH, + DEFAULT_TARGET_POSITION, + DEFAULT_TARGET_STREAMING_ID, + author_static_target, + start_drone_natnet_server, +) + +# --------------------- CONFIGURATION --------------------- +ENV_URL = SIMULATION_ENVIRONMENTS["Default Environment"] +STAGE_SCALE = 1.0 +SAVE_SCENE_TO = None +DRONE_USD = "~/.local/share/ov/data/documents/Kit/shared/exts/pegasus.simulator/pegasus/simulator/assets/Robots/Iris/iris.usd" + +# GPS world anchor: what world (0, 0, 0) maps to in real GPS coordinates. Must +# match the GCS origin (gcs_visualizer/gcs_utils.py) and the robot's +# natnet_ros2 mavros_gp_origin.yaml. In vision/mocap mode the robot-side +# mavros_gp_origin node is the authoritative datum; this keeps PX4's SITL home +# consistent with it so the two never disagree. +WORLD_GPS_ORIGIN = DEFAULT_WORLD_ORIGIN + +# Single drone spawned at the world origin. domain_id / spawn must match the +# spawn_px4_multirotor_node call below. +DRONE_CONFIGS = [ + {"domain_id": 1, "x_m": 0.0, "y_m": 0.0, "z_m": 0.07}, +] + +NATNET_BODY_NAME = os.environ.get("NATNET_BODY_NAME", "Drone") +NATNET_TARGET_NAME = os.environ.get("NATNET_TARGET_NAME", "Target") + +_NATNET_SERVER_KWARGS = { + "pose_noise_enabled": True, + "pose_noise_std_meters": 0.0005, + "pose_noise_rotation_deg": 0.05, +} +# --------------------------------------------------------- + + +ext_manager = omni.kit.app.get_app().get_extension_manager() +for ext in [ + "omni.graph.core", + "omni.graph.action", + "omni.graph.action_nodes", + "isaacsim.core.nodes", + "omni.graph.ui", + "omni.graph.visualization.nodes", + "omni.graph.scriptnode", + "omni.graph.window.action", + "omni.graph.window.generic", + "omni.graph.ui_nodes", + "pegasus.simulator", +]: + if not ext_manager.is_extension_enabled(ext): + ext_manager.set_extension_enabled_immediate(ext, True) + + +def wait_for_stage(stage, timeout_s: float = 10.0): + for _ in range(int(timeout_s / 0.1)): + omni.kit.app.get_app().update() + world_prim = stage.GetPrimAtPath("/World") + if world_prim.IsValid(): + non_physics = [c for c in world_prim.GetChildren() if c.GetName() != "PhysicsScene"] + if non_physics: + return True + time.sleep(0.1) + return False + + +class PegasusApp: + + def __init__(self): + # Write GPS home before spawning so the drone's global position shares + # the GCS datum. Must run before the PX4 SITL subprocess starts. + set_gps_origins(DRONE_CONFIGS, world_origin=WORLD_GPS_ORIGIN) + + self.timeline = omni.timeline.get_timeline_interface() + self.natnet_manager = None + + self.pg = PegasusInterface() + self.pg._world = World(**self.pg._world_settings) + self.world = self.pg.world + self.timeline.stop() + + self.pg.load_environment(ENV_URL) + + stage = omni.usd.get_context().get_stage() + if stage is None: + raise RuntimeError("Stage failed to load") + + if not wait_for_stage(stage): + carb.log_warn("Stage load timed out — continuing anyway.") + + stage_prim = stage.GetPrimAtPath("/World/stage") + if stage_prim.IsValid(): + scale_stage_prim(stage, "/World/stage", STAGE_SCALE) + add_colliders(stage_prim) + for _ in range(10): + omni.kit.app.get_app().update() + else: + carb.log_warn("/World/stage not found — skipping scale and collision.") + + add_dome_light(stage) + + if SAVE_SCENE_TO: + import tempfile + tmp_usd = os.path.join(tempfile.gettempdir(), "prepared_scene.usd") + success, error = asyncio.get_event_loop().run_until_complete( + omni.usd.get_context().export_as_stage_async(tmp_usd) + ) + if success: + os.makedirs(SAVE_SCENE_TO, exist_ok=True) + save_scene_as_contained_usd(tmp_usd, SAVE_SCENE_TO) + os.remove(tmp_usd) + else: + carb.log_error(f"Scene export failed: {error}") + + graph_handle = spawn_px4_multirotor_node( + pegasus_node_name="PX4Multirotor", + drone_prim="/World/base_link", + robot_name="robot_1", + vehicle_id=1, + domain_id=1, + usd_file=DRONE_USD, + init_pos=[0.0, 0.0, 0.07], + init_orient=[0.0, 0.0, 0.0, 1.0], + ) + + add_zed_stereo_camera_subgraph( + parent_graph_handle=graph_handle, + drone_prim="/World/base_link", + robot_name="robot_1", + camera_name="ZEDCamera", + camera_offset=[0.2, 0.0, -0.05], + camera_rotation_offset=[0.0, 0.0, 0.0], + ) + + add_rtx_lidar_subgraph( + parent_graph_handle=graph_handle, + drone_prim="/World/base_link", + robot_name="robot_1", + lidar_config="ouster_os1", + lidar_topic_name="point_cloud_raw", + lidar_offset=[0.0, 0.0, 0.025], + lidar_rotation_offset=[0.0, 0.0, 0.0], + min_range=0.75, + ) + + self._setup_natnet(stage) + self.play_on_start = os.environ.get("PLAY_SIM_ON_START", "true").lower() == "true" + + def _setup_natnet(self, stage): + """Author the NatNet interface prim (drone + static target) and start the server.""" + try: + author_static_target(stage, DEFAULT_TARGET_PATH, DEFAULT_TARGET_POSITION) + bodies = [ + (NATNET_BODY_NAME, 1, "/World/base_link/body"), + (NATNET_TARGET_NAME, DEFAULT_TARGET_STREAMING_ID, DEFAULT_TARGET_PATH), + ] + self.natnet_manager = start_drone_natnet_server( + stage, bodies, **_NATNET_SERVER_KWARGS + ) + carb.log_warn( + f"[natnet] Emulator started: '{NATNET_BODY_NAME}' (-> /World/base_link/body), " + f"'{NATNET_TARGET_NAME}' (-> {DEFAULT_TARGET_PATH})." + ) + except Exception as exc: # noqa: BLE001 - never let NatNet kill the sim + carb.log_error(f"[natnet] Failed to start emulator: {exc}") + self.natnet_manager = None + + def run(self): + if self.play_on_start: + self.timeline.play() + else: + self.timeline.stop() + + app = omni.kit.app.get_app() + while simulation_app.is_running(): + world = World.instance() + if world is not None and hasattr(world, '_scene'): + world.step(render=True) + if world is not self.world: + self.world = world + self.pg._world = world + else: + app.update() + + carb.log_warn("Closing simulation.") + if self.natnet_manager is not None: + self.natnet_manager.on_shutdown() + self.timeline.stop() + simulation_app.close() + + +def main(): + PegasusApp().run() + + +if __name__ == "__main__": + main() diff --git a/tests/integration/natnet/test_natnet_integration.py b/tests/integration/natnet/test_natnet_integration.py index e386fd1ee..43fa4a7f3 100644 --- a/tests/integration/natnet/test_natnet_integration.py +++ b/tests/integration/natnet/test_natnet_integration.py @@ -234,6 +234,80 @@ def test_natnet_ros2_receives_drone_pose_hz(robot_autonomy_stack): server.shutdown() +def test_natnet_ros2_receives_isaac_wrapper_pose_hz(robot_autonomy_stack): + """Isaac-wrapper path: NatNetServerManager.sample_once on a moving USD prim. + + Tests that the wrapper feeds the real robot client end-to-end. Pose-value fidelity + is covered by test_pose_streaming.py loopback. + """ + pytest.importorskip("pxr") + import math + + from pxr import Gf, Usd, UsdGeom + + from optitrack.natnet.emulator.isaac import ( + BodyBinding, + NatNetInterfaceConfig, + NatNetServerManager, + author_interface, + ) + + container = robot_autonomy_stack["container"] + if not _natnet_node_available(container): + pytest.skip("natnet_ros2_node not built — run airstack setup (NatNet SDK)") + + _stop_stale_natnet_nodes(container) + + host_ip = _docker_default_gateway(container) + command_port = ephemeral_udp_port(host_ip) + data_port = ephemeral_udp_port(host_ip) + while data_port == command_port: + data_port = ephemeral_udp_port(host_ip) + robot_name = _container_env(container, "ROBOT_NAME", "robot_1") + domain_id = int(_container_env(container, "ROS_DOMAIN_ID", "0")) + + stage = Usd.Stage.CreateInMemory() + xform = UsdGeom.Xform.Define(stage, "/World/base_link") + translate_op = xform.AddTranslateOp() + translate_op.Set(Gf.Vec3d(0.0, 0.0, 1.0)) + cfg = NatNetInterfaceConfig( + server_ip=host_ip, + command_port=command_port, + data_port=data_port, + publish_rate=50.0, + bodies=[BodyBinding("Drone", "/World/base_link", streaming_id=1)], + ) + author_interface(stage, "/World/NatNetInterface", cfg) + + manager = NatNetServerManager(server_factory=None) # real server factory + stop_event = threading.Event() + + def _sampler(): + # Stand in for the in-sim physics-step callback: move the prim and sample. + interval = 1.0 / cfg.publish_rate + t = 0.0 + while not stop_event.is_set(): + translate_op.Set(Gf.Vec3d(math.sin(t), 0.0, 1.0)) + manager.sample_once(stage) + t += interval + time.sleep(interval) + + sampler = threading.Thread(target=_sampler, daemon=True) + + node_proc: subprocess.Popen[str] | None = None + try: + assert manager.start_server(cfg) is True + sampler.start() + time.sleep(0.1) + node_proc = _launch_natnet_node(container, host_ip, command_port, domain_id) + _assert_pose_stream(container, robot_name, domain_id) + finally: + stop_event.set() + sampler.join(timeout=2.0) + _terminate(node_proc) + manager.stop_server() + + def test_natnet_ros2_multi_body_drone_and_target(robot_autonomy_stack): """Multi-body profile: one robot tracks a drone + a static target. diff --git a/tests/requirements.txt b/tests/requirements.txt index a4b43e6bb..757d1ebdb 100644 --- a/tests/requirements.txt +++ b/tests/requirements.txt @@ -6,3 +6,5 @@ tabulate psutil pandas numpy +scipy +usd-core From 50222c6032db2a22e104a45f752b4232ee12bc29 Mon Sep 17 00:00:00 2001 From: John Date: Thu, 23 Jul 2026 14:17:52 -0400 Subject: [PATCH 24/33] test(natnet): dedicated OptiTrack sim e2e (optitrack mark) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit One dedicated Isaac bring-up (example_one_px4_pegasus_natnet_launch_script + LAUNCH_NATNET=true) that asserts the full NatNet chain: emulator → natnet_ros2 pose_cov >= 5 Hz, then PX4 local_position alive (EKF2 fusing the vision). Its own `optitrack` mark + _MODULE_ORDER slot — deliberately NOT a third parametrized sim, so the generic liveliness/sensors/flight suites aren't re-run under NatNet. Co-Authored-By: Claude Opus 4.8 --- tests/harness/collection.py | 2 + tests/pytest.ini | 1 + tests/system/test_optitrack_e2e.py | 135 +++++++++++++++++++++++++++++ 3 files changed, 138 insertions(+) create mode 100644 tests/system/test_optitrack_e2e.py diff --git a/tests/harness/collection.py b/tests/harness/collection.py index aa20c9f1e..cca487d07 100644 --- a/tests/harness/collection.py +++ b/tests/harness/collection.py @@ -24,6 +24,8 @@ "system.test_sensors", "system.test_takeoff_hover_land", "system.test_fixed_trajectory", + # OptiTrack e2e: its own single NatNet bring-up, runs after the flight suites. + "system.test_optitrack_e2e", ] # Within test_takeoff_hover_land, each (env, velocity) runs phases in this chain order. diff --git a/tests/pytest.ini b/tests/pytest.ini index 6f4547c0c..69538af34 100644 --- a/tests/pytest.ini +++ b/tests/pytest.ini @@ -9,6 +9,7 @@ markers = takeoff_hover_land: End-to-end takeoff / hover / land action tests autonomy: Fixed-pattern trajectory path-tracker benchmark (test_fixed_trajectory.py) waypoint_flight: Ordered-waypoint navigation judged on the odometry track (test_waypoint_flight.py) + optitrack: OptiTrack NatNet end-to-end (sim emulator → natnet_ros2 → PX4 EV fusion) testpaths = . addopts = -v --durations=0 --import-mode=importlib cache_dir = /tmp/.pytest_cache diff --git a/tests/system/test_optitrack_e2e.py b/tests/system/test_optitrack_e2e.py new file mode 100644 index 000000000..87cafebfd --- /dev/null +++ b/tests/system/test_optitrack_e2e.py @@ -0,0 +1,135 @@ +"""OptiTrack NatNet end-to-end (sim). + +A single dedicated bring-up that exercises the whole OptiTrack path in Isaac Sim: +the in-sim NatNet **emulator** streams rigid-body poses → ``natnet_ros2`` publishes +the drone pose → the ``vision_pose`` bridge feeds MAVROS → PX4 EKF2 fuses it. + +This intentionally does NOT add ``isaacsim_natnet`` as a third parametrized sim in +the ``airstack_env`` matrix (that would re-run the entire liveliness/sensors/flight +suite under NatNet for two assertions). Instead we bring the NatNet stack up **once** +here and assert only the NatNet-specific chain. The cheap, GPU-free half of this +(host emulator → ``natnet_ros2`` Hz) lives in ``tests/integration/natnet/``. + +Mark: ``optitrack``. Needs Docker + GPU + Isaac Sim license; skips cleanly when the +isaac-sim image isn't built locally. +""" +import os +import time + +import pytest + +from conftest import ( # noqa: E402 — pytest adds tests/ to sys.path + airstack_cmd, + container_running, + find_container, + get_metrics, + get_robot_containers, + logger, + missing_images, + read_log_tail, + sample_hz, + wait_for_container, + wait_for_first_message, +) + +pytestmark = pytest.mark.optitrack + +# Single-drone NatNet Isaac stack: the natnet Pegasus script spawns the emulator +# alongside PX4, and LAUNCH_NATNET=true brings up natnet_ros2 + the vision_pose / +# gp_origin / param bridges on the robot. +_E2E_ENV = { + "NUM_ROBOTS": "1", + "COMPOSE_PROFILES": "desktop,isaac-sim", + "AUTOLAUNCH": "true", + "ISAAC_SIM_USE_STANDALONE": "true", + "ISAAC_SIM_SCRIPT_NAME": "example_one_px4_pegasus_natnet_launch_script.py", + "PLAY_SIM_ON_START": "true", + "LAUNCH_NATNET": "true", + # Headless: no X on the CI runner. + "QT_QPA_PLATFORM": "offscreen", +} + +_ROBOT_PATTERN = "robot.*desktop" +_ROBOT_SETUP_BASH = "/root/AirStack/robot/ros_ws/install/setup.bash" +_ROBOT_DOMAIN = 1 +# Drone body's relative pose topic from natnet_config.yaml, namespaced per robot. +_NATNET_POSE_TOPIC = os.environ.get("NATNET_POSE_TOPIC", "perception/optitrack/drone") +_NATNET_MIN_HZ = 5.0 +# PX4 fused local position (proves EKF2 accepted the external vision). +_PX4_LOCAL_POSE_TOPIC = "interface/mavros/local_position/pose" +# Cold Isaac boot: Pegasus load + Play + emulator UDP connect. +_FIRST_MSG_TIMEOUT = 180 + + +@pytest.fixture(scope="module") +def optitrack_sim_stack(request): + """Bring the NatNet Isaac stack up once for the module; tear it down after. + + Reuses an already-running robot-desktop container (fast local iteration); + otherwise brings the stack up. Skips when the isaac-sim image isn't built. + """ + existing = find_container(_ROBOT_PATTERN) + if existing and container_running(existing): + yield {"container": existing, "brought_up": False} + return + + missing = missing_images(env=_E2E_ENV) + if missing: + pytest.skip("isaac-sim / robot image not built locally: " + ", ".join(missing)) + + airstack_cmd("down", timeout=120, log_name="optitrack_e2e") + result = airstack_cmd("up", env_overrides=_E2E_ENV, timeout=300, log_name="optitrack_e2e") + if result.returncode != 0: + pytest.fail(f"`airstack up` (natnet isaac) failed:\n{read_log_tail('optitrack_e2e')}") + + container = wait_for_container(_ROBOT_PATTERN, timeout=180) + assert container, "robot-desktop container not Running after 180s" + try: + yield {"container": container, "brought_up": True} + finally: + airstack_cmd("down", timeout=120, log_name="optitrack_e2e") + + +def _robot_container(stack): + # robot_1 lives on the first (index-1) replica. + return get_robot_containers(_ROBOT_PATTERN)[0] if not stack["brought_up"] \ + else wait_for_container(_ROBOT_PATTERN, timeout=60) + + +class TestOptitrackE2E: + + @pytest.mark.dependency(name="natnet_pose") + def test_natnet_pose_alive(self, optitrack_sim_stack): + """Emulator → natnet_ros2 → vision_pose: the drone pose_cov streams >= 5 Hz.""" + container = _robot_container(optitrack_sim_stack) + topic = f"/robot_{_ROBOT_DOMAIN}/{_NATNET_POSE_TOPIC}/pose_cov" + + first = wait_for_first_message( + container, topic, domain_id=_ROBOT_DOMAIN, + setup_bash=_ROBOT_SETUP_BASH, timeout=_FIRST_MSG_TIMEOUT, + ) + assert first is not None, ( + f"no NatNet pose on {topic} within {_FIRST_MSG_TIMEOUT}s " + "(emulator → natnet_ros2 path down)" + ) + hz = sample_hz(container, topic, domain_id=_ROBOT_DOMAIN, + setup_bash=_ROBOT_SETUP_BASH, duration=5, window=20) + get_metrics().record("test_optitrack_e2e.natnet_pose_hz", + "natnet_pose_hz", hz if hz is not None else "none", unit="Hz") + assert hz is not None and hz >= _NATNET_MIN_HZ, \ + f"{topic} at {hz} Hz (< {_NATNET_MIN_HZ})" + + @pytest.mark.dependency(depends=["natnet_pose"]) + def test_px4_fuses_vision(self, optitrack_sim_stack): + """PX4 EKF2 fuses the external vision: local_position/pose streams.""" + container = _robot_container(optitrack_sim_stack) + topic = f"/robot_{_ROBOT_DOMAIN}/{_PX4_LOCAL_POSE_TOPIC}" + + first = wait_for_first_message( + container, topic, domain_id=_ROBOT_DOMAIN, + setup_bash=_ROBOT_SETUP_BASH, timeout=_FIRST_MSG_TIMEOUT, + ) + assert first is not None, ( + f"no PX4 local_position on {topic} within {_FIRST_MSG_TIMEOUT}s " + "(EKF2 not fusing external vision)" + ) From d60d7193f792ff8e9d0a37dffcc0570fe4f64456 Mon Sep 17 00:00:00 2001 From: John Date: Thu, 23 Jul 2026 14:20:12 -0400 Subject: [PATCH 25/33] docs(natnet): emulator sim doc + optitrack-development skill Add the NatNet emulator Isaac Sim documentation (docs/simulation/isaac_sim/ natnet_emulator.md) and the optitrack-development agent skill covering the emulator, natnet_ros2, and the NatNet wire-protocol handshake. Co-Authored-By: Claude Opus 4.8 --- .agents/skills/optitrack-development/SKILL.md | 220 ++++++++++++++ docs/simulation/isaac_sim/natnet_emulator.md | 277 ++++++++++++++++++ 2 files changed, 497 insertions(+) create mode 100644 .agents/skills/optitrack-development/SKILL.md create mode 100644 docs/simulation/isaac_sim/natnet_emulator.md diff --git a/.agents/skills/optitrack-development/SKILL.md b/.agents/skills/optitrack-development/SKILL.md new file mode 100644 index 000000000..fcfe66e87 --- /dev/null +++ b/.agents/skills/optitrack-development/SKILL.md @@ -0,0 +1,220 @@ +--- +name: optitrack-development +description: Develop and integrate OptiTrack NatNet in AirStack — robot client (natnet_ros2), Isaac Sim Motive emulator, wire-protocol handshake, and libNatNet 4.4 unicast behavior. Use when working on natnet_ros2, optitrack.natnet.emulator, LAUNCH_NATNET, or NatNet UDP protocol compatibility. +license: Apache-2.0 +metadata: + author: AirLab CMU + repository: AirStack +--- + +# Skill: OptiTrack / NatNet Development + +## When to Use + +- Implementing or debugging the **Motive emulator** in Isaac Sim + (`simulation/isaac-sim/extensions/optitrack.natnet.emulator/`) +- Integrating or testing **`natnet_ros2`** on the robot stack +- Understanding **NatNet wire protocol** (connect, model def, frame streaming) +- Capturing what **`libNatNet.so`** actually sends on the network +- Enabling OptiTrack in sim: `LAUNCH_NATNET=true`, `natnet_config.yaml`, Docker IPs + +## Architecture in AirStack + +```mermaid +flowchart LR + subgraph sim ["Isaac Sim (172.31.0.200)"] + Emulator["optitrack.natnet.emulator\n(NatNet UDP server)"] + end + subgraph robot ["Robot container"] + Node["natnet_ros2_node"] + SDK["libNatNet.so client"] + Node --> SDK + end + SDK -->|"UDP 1510 (unicast: cmd + frames)"| Emulator + Node --> Topics["/{ROBOT_NAME}/perception/optitrack/..."] +``` + +| Component | Path | Role | +|-----------|------|------| +| Robot client | [`robot/ros_ws/src/perception/natnet_ros2/`](../../../robot/ros_ws/src/perception/natnet_ros2/) | ROS 2 node; uses **official NatNet SDK** (`NatNetClient::Connect`) | +| SDK install | `natnet_ros2/lib/libNatNet.so`, `include/natnet/` | Download via `airstack setup --natnet` (proprietary, not in git) | +| Emulator (WIP) | [`simulation/isaac-sim/extensions/optitrack.natnet.emulator/`](../../../simulation/isaac-sim/extensions/optitrack.natnet.emulator/) | Python NatNet **server** for sim / integration tests | +| Integration tests | [`tests/integration/natnet/README.md`](../../../tests/integration/natnet/README.md) | End-to-end UDP tests against real SDK parser (mark: `integration`) | + +**Enable on robot:** `LAUNCH_NATNET=true` in `.env` → [`perception.launch.xml`](../../../robot/ros_ws/src/perception/perception_bringup/launch/perception.launch.xml) includes `natnet_ros2.launch.py`. + +**Enable in sim:** set ``ISAAC_SIM_SCRIPT_NAME`` to a NatNet Pegasus launch script (no env gate in the script — NatNet always starts): + +| Script | Use | +|--------|-----| +| [`example_one_px4_pegasus_natnet_launch_script.py`](../../../simulation/isaac-sim/launch_scripts/example_one_px4_pegasus_natnet_launch_script.py) | Single drone + static ``Target`` | +| [`example_multi_px4_pegasus_natnet_launch_script.py`](../../../simulation/isaac-sim/launch_scripts/example_multi_px4_pegasus_natnet_launch_script.py) | ``NUM_ROBOTS`` drones + shared ``Target`` (pair with 3-profile ``natnet_config.yaml``) | + +Helpers: [`isaac/scene_setup.py`](../../../simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/scene_setup.py) (`start_drone_natnet_server`, `author_static_target`). Drone body: single = ``Drone``; multi = ``Drone`` (id ``i``); target = ``Target`` (id 100). Override names with ``NATNET_BODY_NAME`` / ``NATNET_TARGET_NAME``. Baseline Pegasus scripts (no NatNet) remain ``example_one_px4_pegasus_launch_script.py`` / ``example_multi_px4_pegasus_launch_script.py``. + +**Default client config:** unicast, `server_ip` → Motive/emulator (use `172.31.0.200` for Isaac container), ports 1510/1511. The config is per-robot: each `robots[$ROBOT_NAME]` profile lists the bodies it tracks (each a `rigid_body_name` + `id` mapped to a relative `topic`, with `pose`/`pose_cov` toggles and per-body covariance) and an optional `vision_pose` block that drives the MAVROS bridge. See [`natnet_config.yaml`](../../../robot/ros_ws/src/perception/natnet_ros2/config/natnet_config.yaml). + +## NatNet: Two UDP Channels + +| Port (server default) | Channel | Direction | +|----------------------|---------|-----------| +| **1510** | Command | Client → server: `NAT_CONNECT`, `NAT_REQUEST_MODELDEF`, keepalives. Server → client: `NAT_SERVERINFO`, `NAT_MODELDEF`, `NAT_RESPONSE` | +| **1511** | Data | Server → client: `NAT_FRAMEOFDATA` (mocap frames). Multicast group `239.255.42.99` when using multicast. **The server must send frames from a socket bound to the data port** (source port == `data_port`); see below. | + +**Critical rules (verified against the real `libNatNet.so` 4.4 unicast + `NatNet_SetLogCallback`):** + +- Command **responses** go to the client's endpoint from `recvfrom` on the server command listener (`1510`), sent via the **command** socket. +- **Frames must be sent from the server's DATA socket** (bound to `data_port`, e.g. `1511`) so the datagram **source port == `data_port`**. libNatNet routes inbound unicast datagrams by source port: frames from the **command** port are treated as command traffic and **silently dropped** (no error, no callback). This was the single biggest gotcha. +- **libNatNet 4.4 unicast uses one client UDP socket** (one ephemeral local port for command send/recv and frame recv). The client receives frames there regardless of the server's source port — but libNatNet only **dispatches** them to the frame callback when they came from the server's data port. Do **not** assume `data_port = cmd_port + 1`. +- **Every `NAT_FRAMEOFDATA` must end with a 4-byte end-of-data tag** (after the frame `params`). libNatNet's unpacker reads it; without it the unpacked length mismatches `nDataBytes` and the SDK drops the whole frame. (The lenient Python `NatNetClient` does not require it — always validate against the C SDK.) +- The **269-byte `NAT_CONNECT` payload does not include** the client port; the port is learned from the datagram **source address** on `NAT_CONNECT`. +- Do **not** trust `/proc`/`ss` alone for the client port — extra bound sockets may appear that do not match wire traffic. **`NAT_CONNECT` source `(ip, port)` is ground truth.** +- Do **not** parse connect payloads with in-memory `sNatNetClientConnectParams` (contains pointers). Use on-wire layouts below. + +## libNatNet 4.4 `NAT_CONNECT` (verified 2025-06) + +Observed against `127.0.0.1:1510` with the same unicast params as [`natnet_client_adapter.cpp`](../../../robot/ros_ws/src/perception/natnet_ros2/src/natnet_client_adapter.cpp). + +### What the client sends + +| Field | Observed value | +|-------|----------------| +| Message | `NAT_CONNECT` (0), `nDataBytes = 269`, total datagram 273 bytes | +| Payload layout | `sSender` (264 B) + `sConnectionOptions` (5 B) | +| `sSender.szName` | `"NatNetLib"` | +| `sSender.Version` | `[4, 4, 0, 0]` | +| `sSender.NatNetVersion` | `[4, 4, 0, 0]` | +| `subscribedDataOnly` | `0` | +| `BitstreamVersion` | `[0, 0, 0, 0]` → client defers to server version | +| Trailing port bytes | **None** (exactly 269 bytes; not PacketClient's optional +4) | +| UDP source port | Ephemeral (e.g. `41449`) — **client command + data port (same socket)** | + +Example hex (payload only, after 4-byte header): + +``` +NatNetLib\0 ... (256-byte name field) +04 04 00 00 (Version) +04 04 00 00 (NatNetVersion) +00 (subscribedDataOnly) +00 00 00 00 (BitstreamVersion) +``` + +## libNatNet 4.4 unicast: single client socket (verified 2025-06) + +Confirmed with wire capture on server `:1510`/`:1511`, `strace` on a minimal `NatNetClient::Connect()` binary, and `/proc//net/udp` cross-checks against the same `libNatNet.so` used by `natnet_ros2`. + +### What we observed + +| Signal | Result | +|--------|--------| +| Wire capture on server `:1510` | All client packets (`NAT_CONNECT`, `NAT_KEEPALIVE`, `NAT_REQUEST_MODELDEF`) from **one** source port | +| Wire capture on server `:1511` | **No** inbound packets from the client | +| strace on minimal client | **One** `bind()`, **one** fd for all `sendto` → server `:1510` and `recvfrom` ← server `:1510` | +| `NAT_CONNECT` payload | **No** trailing client port bytes (269 B total) | + +### Emulator rule (unicast + `natnet_ros2`) + +For libNatNet 4.4 unicast, treat the client as **single-endpoint**: + +```text +On NAT_CONNECT → store client_endpoint = (ip, port) from recvfrom +NAT_SERVERINFO → sendto(command_socket, client_endpoint) # source port = command_port +NAT_MODELDEF → sendto(command_socket, client_endpoint) # source port = command_port +NAT_FRAMEOFDATA → sendto(data_socket, client_endpoint) # source port = data_port (REQUIRED) +NAT_KEEPALIVE → no reply (client -> server only) +``` + +The client always learns its endpoint from the **`NAT_CONNECT` source address** (the +client uses a single socket), so the **destination** of frames is that endpoint. The +**source** of frames, however, must be the server's data port — bind a dedicated +`data_socket` to `('', data_port)` and `sendto` frames from it. + +`ConnectionDataPort = 1511` in `NAT_SERVERINFO` is required (the SDK uses it to +recognize the data channel — i.e. which source port valid frames arrive from). + +### When two client ports may still apply + +- **Multicast** clients (separate multicast data listener on `239.255.42.99:1511`) +- **PacketClient-style** samples that open explicit command + data sockets (optional +4 port bytes in connect) +- Other NatNet client implementations — always verify with protocol capture before assuming a two-socket model + +Do **not** assume `data_port = cmd_port + 1` for any client without capture. + +### What the server must reply (for `Connect()` + `GetServerDescription()`) + +1. **`NAT_SERVERINFO` (1)** on the **command port** to the connect datagram source. +2. Payload: packed **`sSender_Server`** (279 B), **not** `sServerDescription`. libNatNet + parses the `NAT_SERVERINFO` payload as `sSender_Server`; sending the larger + `sServerDescription` makes it misread the version/host. Fields: + - `Common.szName = "Motive"` (256-byte field) + - `Common.Version = {3, 1, 0, 0}` (Motive app), `Common.NatNetVersion = {4, 4, 0, 0}` + - `HighResClockFrequency`, `DataPort = 1511`, `IsMulticast = 0` (unicast) + +Pre-built in emulator: [`NatNetServer._build_connect_response_payload()`](../../../simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_server.py). + +### After connect (required for `natnet_ros2` topics) + +| SDK call | Server must handle | +|----------|-------------------| +| `GetDataDescriptionList()` | `NAT_REQUEST_MODELDEF` → `NAT_MODELDEF` with rigid body name/ID (e.g. `"Drone"`) | +| Frame callback | Stream `NAT_FRAMEOFDATA` to **`NAT_CONNECT` source `(ip, port)`** from the server **data socket** (source port = `data_port`); end each frame with the 4-byte EOD tag; set `rb.params & 0x01` (tracking valid) | +| Unicast keepalive | Accept `NAT_KEEPALIVE` on command port; **send no reply** | + +Verified end-to-end against the real `libNatNet.so` with a C probe that registers +`SetFrameReceivedCallback` + `NatNet_SetLogCallback`: with the data-port source, +EOD tag, `sSender_Server` reply, and no keepalive reply, the probe reports +`Server: Motive 3.1.0.0 NatNet 4.4.0.0`, `data descriptions: 1`, and ~74 Hz callbacks. + +## Wire format reference (do not confuse) + +| Client type | Connect payload | +|-------------|-----------------| +| **`libNatNet` / `natnet_ros2`** | `sSender` + `sConnectionOptions` (269 B observed) | +| **PacketClient sample** | Same + optional 4 trailing bytes (often zero in sample) | +| **Python NatNetClient sample** | Legacy 270-byte `"Ping"` blob — **not** used by `natnet_ros2` | + +API struct `sNatNetClientConnectParams` ([`NatNetTypes.h`](../../../simulation/isaac-sim/extensions/optitrack.natnet.emulator/NatNetClientSDK/NatNetSDK/include/NatNetTypes.h)) is for `Connect()` in process memory only — **not** the on-wire layout. + +## Protocol capture (optional, for debugging) + +Not part of the repo. If you need to re-verify wire behavior or debug a new client/server pairing, build a **minimal out-of-band harness**: + +1. **Minimal C++ client** — tiny binary linking `libNatNet.so` from `natnet_ros2`; call `NatNetClient::Connect()` with the same params as [`natnet_client_adapter.cpp`](../../../robot/ros_ws/src/perception/natnet_ros2/src/natnet_client_adapter.cpp). Optional: `GetDataDescriptionList()`, frame callback, `--hold-seconds` sleep. +2. **Python UDP stub server** — bind `:1510` (and optionally `:1511`); reply to `NAT_CONNECT` with canned `NAT_SERVERINFO`, to `NAT_REQUEST_MODELDEF` with `NAT_MODELDEF`, to `NAT_KEEPALIVE` with ack; log every `(ip, port)` and message id. +3. **Connect capture** — run the client against the stub; hex-dump the first datagram; confirm 269-byte `sSender` + `sConnectionOptions` payload and ephemeral source port. +4. **Endpoint discovery** — during a full connect + model-def fetch: + - `tcpdump -i any udp and host ` or the stub's packet log + - `strace -e trace=bind,sendto,recvfrom` on the client binary + - `/proc//net/udp` or `ss -uapn` (treat **`NAT_CONNECT` source port** as ground truth if they disagree) +5. **Frame delivery check** — confirm the client's frame callback fires. Register both `SetFrameReceivedCallback` **and** `NatNet_SetLogCallback` (the log callback surfaces silent drops). Frames must be sent from the server **data socket** (source port = `data_port`) and end with the 4-byte EOD tag, or the SDK drops them with no callback. + +Use the SDK's `NatNetTypes.h` and `PacketClient.cpp` for on-wire layouts — not in-memory `sNatNetClientConnectParams`. + +## Emulator implementation checklist + +1. **Command listener** on `0.0.0.0:1510` +2. **`NAT_CONNECT`** → register `client_endpoint` from `recvfrom`; reply `NAT_SERVERINFO` +3. **`NAT_REQUEST_MODELDEF`** → reply `NAT_MODELDEF` (match `body_name` in config) +4. **Frame loop** → `NAT_FRAMEOFDATA` to `client_endpoint` **from the data socket** (source port = `data_port`); end each frame with the 4-byte EOD tag +5. **Isaac integration** → sample drone pose → `sFrameOfMocapData` → `enqueue_mocap_data()` +6. **Docker** → emulator on `172.31.0.200`; robot `server_ip` points there + +## Testing levels + +| Level | Approach | Validates | +|-------|----------|-----------| +| Unit (no network) | `test_natnet_logic.cpp`, `FakeNatNetClient` | Negotiation logic, topic names | +| Protocol capture | Minimal client + UDP stub (see above) | Wire-format `NAT_CONNECT`, client endpoint model | +| Integration | `tests/integration/natnet/` | Full SDK parser + `natnet_ros2_node` (mark: `integration`) | +| System (future) | `airstack test -m sensors` | Topic Hz on `/perception/optitrack/...` | + +```bash +# Unit tests (robot container) +docker exec airstack-robot-desktop-1 bash -c "sws && colcon test --packages-select natnet_ros2 --event-handlers console_direct+" +``` + +## References + +- OptiTrack NatNet docs: https://docs.optitrack.com/developer-tools/natnet-sdk/natnet-4.0 +- SDK samples (wire format): `NatNet_SDK_*/Samples/PacketClient/`, `PythonClient/` (legacy connect in Python only) +- Integration test: [`tests/integration/natnet/README.md`](../../../tests/integration/natnet/README.md) diff --git a/docs/simulation/isaac_sim/natnet_emulator.md b/docs/simulation/isaac_sim/natnet_emulator.md new file mode 100644 index 000000000..2bda072ee --- /dev/null +++ b/docs/simulation/isaac_sim/natnet_emulator.md @@ -0,0 +1,277 @@ +# NatNet Emulator (OptiTrack Simulation) + +The `optitrack.natnet.emulator` Isaac Sim extension lets you test the full +[`natnet_ros2`](../../../robot/ros_ws/src/perception/natnet_ros2/README.md) +perception stack in simulation without a physical OptiTrack system. It runs a +Motive-compatible NatNet UDP server inside Isaac Sim, streams rigid-body poses +sampled from USD prim world transforms, and presents the same wire protocol +that real Motive software uses — so `natnet_ros2` cannot tell the difference. + +## How it works + +``` +Isaac Sim (physics step) + ↓ sample prim world pose +NatNetServerManager (/World/NatNetInterface USD prim) + ↓ encode sFrameOfMocapData (NatNet 4.1 wire format) +NatNetUnicastServer ──UDP 1510/1511──► natnet_ros2_node (robot container) + ↓ + /robot_N/perception/optitrack/{body} + /robot_N/interface/mavros/vision_pose/pose +``` + +Configuration lives on a `/World/NatNetInterface` USD prim with `natnet:*` +attributes. Because it is USD, the config **persists when you save the stage** — +re-opening a `.usd` file restores the catalog and server settings without +re-running any script. + +Each physics step the extension: + +1. Reads the world transform of each tracked prim. +2. Packs a `sFrameOfMocapData` frame (one `sRigidBodyData` entry per body). +3. Flushes the frame immediately on the physics-step thread (no background timer). + +Bodies whose target prim is missing emit a **lost** frame (NaN position, +tracking-invalid bit clear) until the prim appears — this handles Pegasus drones +that are spawned on the first Play tick. + +Optional **sensor noise** (`pose_noise_std_meters`, `pose_noise_rotation_deg`) +adds Gaussian position and orientation perturbation to simulate real OptiTrack +measurement uncertainty. + +--- + +## Using the pre-built launch scripts + +The easiest way to start is with the provided Pegasus launch scripts. Set +`ISAAC_SIM_SCRIPT_NAME` in your environment or use the convenience override: + +```bash +# Multi-drone NatNet + PX4 external-vision (3 robots, vision-pose mode) +airstack up --env-file overrides/isaac-natnet-vision.env +``` + +`overrides/isaac-natnet-vision.env` sets: + +| Variable | Value | +|---|---| +| `NUM_ROBOTS` | `3` | +| `LAUNCH_NATNET` | `true` | +| `SITL_PARAM_PROFILE` | `px4-vision` | +| `ISAAC_SIM_SCRIPT_NAME` | `example_multi_px4_pegasus_natnet_launch_script.py` | + +### Available NatNet launch scripts + +| Script | Use case | +|---|---| +| `example_one_px4_pegasus_natnet_launch_script.py` | Single drone + static `Target` body | +| `example_multi_px4_pegasus_natnet_launch_script.py` | `NUM_ROBOTS` drones + shared `Target` body | + +Both scripts set up GPS origins (via `gps_utils.py`) so the GCS datum matches +PX4, start the NatNet emulator, and play the simulation automatically. + +!!! note "Baseline scripts have no NatNet" + `example_one_px4_pegasus_launch_script.py` and + `example_multi_px4_pegasus_launch_script.py` do **not** include NatNet. Use + the `*_natnet_*` variants above when you need mocap simulation. + +### Body naming + +| `NUM_ROBOTS` | Body names streamed | +|---|---| +| 1 | `Drone`, `Target` | +| N > 1 | `Drone1`, `Drone2`, …, `DroneN`, `Target` | + +Override the base name with `NATNET_BODY_NAME` (default `Drone`) and +`NATNET_TARGET_NAME` (default `Target`). These must match the +`rigid_body_name` entries in your robot's +[`natnet_config.yaml`](../../../robot/ros_ws/src/perception/natnet_ros2/config/natnet_config.yaml) +profile. + +### What the robot container needs + +Set `LAUNCH_NATNET=true` (already included in `isaac-natnet-vision.env`). +`natnet_ros2` connects to the emulator at `172.31.0.200` (the Isaac Sim +container's address on the AirStack bridge network) — the default in +`natnet_config.yaml`. + +After bringup, verify the stream is flowing: + +```bash +# Drone pose arriving from emulator +docker exec airstack-robot-desktop-1 bash -lc \ + "ros2 topic hz /robot_1/perception/optitrack/drone" + +# Vision pose forwarded to MAVROS / PX4 +docker exec airstack-robot-desktop-1 bash -lc \ + "ros2 topic hz /robot_1/interface/mavros/vision_pose/pose" +``` + +--- + +## Adding NatNet to your own launch script + +Call `start_drone_natnet_server` after your Pegasus drones are spawned: + +```python +import sys, os +sys.path.insert(0, os.path.join(os.path.dirname(__file__), "..", "utils")) +from optitrack.natnet.emulator.isaac import ( + start_drone_natnet_server, + author_static_target, + DEFAULT_TARGET_PATH, + DEFAULT_TARGET_STREAMING_ID, +) + +stage = omni.usd.get_context().get_stage() + +# Optional: add a static target body the robot can navigate toward. +author_static_target(stage, DEFAULT_TARGET_PATH, position=(2.0, 0.0, 1.0)) + +# One entry per drone: (rigid_body_name, streaming_id, target_prim_path) +drones = [ + ("Drone", 1, "/World/drone1/base_link/body"), +] + +# Keep a reference for the sim lifetime — dropping it stops the server. +self.natnet_manager = start_drone_natnet_server( + stage, + drones=drones, + server_ip="172.31.0.200", # Isaac container IP on AirStack bridge network + pose_noise_enabled=True, + pose_noise_std_meters=0.0005, + pose_noise_rotation_deg=0.05, +) +``` + +For multiple drones, add one tuple per drone: + +```python +drones = [ + ("Drone1", 1, "/World/drone1/base_link/body"), + ("Drone2", 2, "/World/drone2/base_link/body"), + ("Drone3", 3, "/World/drone3/base_link/body"), +] +``` + +`start_drone_natnet_server` also accepts the static target as a body — include +it explicitly if you want it: + +```python +from optitrack.natnet.emulator.isaac import DEFAULT_TARGET_STREAMING_ID, DEFAULT_TARGET_PATH + +drones = [ + ("Drone", 1, "/World/drone1/base_link/body"), + ("Target", DEFAULT_TARGET_STREAMING_ID, DEFAULT_TARGET_PATH), +] +``` + +--- + +## Using the Kit UI panel + +The extension registers a docked panel under **Window → NatNet Interface** in +the Isaac Sim menu bar (appears alongside the Pegasus panel). + +### Opening the panel + +Open Isaac Sim, load your scene, then go to **Window → NatNet Interface**. The +panel docks next to the Property panel in the bottom-right. + +### Panel controls + +| Button | Action | +|---|---| +| **Create Interface** | Author a fresh `/World/NatNetInterface` prim with current settings | +| **Save** | Push the form fields into the USD prim on the stage | +| **Load from Stage** | Pull the existing prim's values back into the form | +| **Print config** | Log the current config to the console | +| **Start Server / Stop Server** | Toggle the NatNet UDP server | + +!!! warning "Always Save before Start" + The server reads its configuration from the USD prim, not the form. Press + **Save** after every edit, then **Start Server**. + +### Server settings + +| Field | Default | Description | +|---|---|---| +| Server enabled | `true` | Uncheck to prevent auto-start on stage open | +| Server IP | `172.31.0.200` | IP the UDP socket binds to (Isaac container address) | +| Mode | `unicast` | `unicast` for direct; `multicast` for broadcast | +| Command port | `1510` | NatNet command channel | +| Data port | `1511` | NatNet data channel (frame stream) | +| Publish rate (Hz) | `120` | Target frame rate | +| Up axis | `Z` | `Z` passes poses through unchanged; `Y` re-axes for Y-up Motive | +| Pose noise enabled | `true` | Add Gaussian noise to simulate real sensor uncertainty | +| Pose noise std (m) | `0.0005` | Position noise std dev (0.5 mm, matching OptiTrack spec) | +| Pose noise rotation (deg) | `0.05` | Orientation noise std dev | + +### Adding tracked bodies + +1. In the Stage tree, **select the prim** you want to track (e.g. `/World/drone1/base_link/body`). +2. Click **Add body (from selection)** in the panel. +3. Fill in the **rigid body name** (must match the `rigid_body_name` in `natnet_config.yaml`) and **streaming ID**. +4. Click **Save**, then **Start Server**. + +Each body row shows a live readout of the prim's current world position with a +colour-coded status indicator: + +- 🟢 Green dot — server running, prim found, pose valid +- ⚫ Grey dot — prim found but server not running +- ✗ Red — prim missing or NaN position + +### Persistence + +After configuring the panel, save your USD stage (**File → Save**). The +`natnet:*` attributes are written into the `.usd` file. Re-opening the stage +restores the full catalog automatically — no script or panel interaction needed +unless you want to change the config. + +--- + +## Configuration reference + +`start_drone_natnet_server` and `build_drone_config` accept these keyword +arguments (all optional): + +| Parameter | Default | Description | +|---|---|---| +| `server_ip` | `"172.31.0.200"` | IP to bind the UDP server to | +| `mode` | `"unicast"` | `"unicast"` or `"multicast"` | +| `command_port` | `1510` | NatNet command port | +| `data_port` | `1511` | NatNet data port | +| `publish_rate` | `120.0` | Frame streaming rate (Hz) | +| `up_axis` | `"Z"` | Axis convention (`"Z"` or `"Y"`) | +| `pose_noise_enabled` | `True` | Enable sensor noise | +| `pose_noise_std_meters` | `0.0005` | Position noise std dev (m) | +| `pose_noise_rotation_deg` | `0.05` | Orientation noise std dev (degrees) | + +--- + +## Troubleshooting + +**`natnet_ros2` connects but no pose topics appear** + +- Check that `rigid_body_name` in `natnet_config.yaml` matches exactly what the + emulator is streaming (case-sensitive). Run `ros2 topic list` inside the robot + container and look for `/robot_N/perception/optitrack/...`. + +**Server starts but no data arrives in `natnet_ros2`** + +- Confirm the server IP matches the Isaac container's address (`172.31.0.200` + on the AirStack bridge). Check with `docker network inspect airstack_network`. +- The NatNet data port (1511) must be bound to the *data* socket — frames sent + from the command socket are silently dropped by libNatNet 4.4. + +**Emulator streams but `vision_pose` is empty** + +- `vision_pose` forwarding requires `vision_pose.enabled: true` in the robot's + `natnet_config.yaml` profile and `SITL_PARAM_PROFILE=px4-vision` so PX4 + accepts external vision instead of GPS. + +**Body shows ✗ red / NaN in the UI panel** + +- The target prim doesn't exist yet. This is normal before pressing Play (Pegasus + spawns the drone `base_link` prim on the first physics tick). After Play the + indicator should turn green within one frame. From 3aeb19a00fa38cea1839860317fc832913921e68 Mon Sep 17 00:00:00 2001 From: John Date: Thu, 23 Jul 2026 14:20:12 -0400 Subject: [PATCH 26/33] chore: bump version to 0.19.0-alpha.15 --- .env | 2 +- CHANGELOG.md | 1 + 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/.env b/.env index 70736e9ac..8cf4e408c 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.14" +VERSION="0.19.0-alpha.15" # 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 b3a9298b9..cd64b1358 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -19,6 +19,7 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0 - `waypoint_flight` system test (`tests/system/test_waypoint_flight.py`): takeoff → ordered waypoint route via `NavigateTask` (dispatched as a dense plan) → land, judged on the odometry track by the standalone stdlib-only `tests/waypoint_checker.py` (in-order corridor arrival within `--waypoint-tolerance`, final goal within `--goal-tolerance`, per-waypoint `--waypoint-timeout`); validated end-to-end in Isaac Sim; serves as the standard acceptance check after integrating or swapping a planner module - Real-robot PX4 external-vision fusion in `natnet_ros2` (OptiTrack mocap → EKF2): `mavros_gp_origin` (geoid-corrected synthetic GPS origin so `local_position.z` == OptiTrack z, fixing the ~36 m boot offset), `vision_pose_converter`, and a PX4 param **checker** (`px4_param_setter`, `auto_set` off by default; `on_mismatch` warn/halt) — setup guide at `docs/robot/px4_external_vision.md` - NatNet server emulator (`optitrack.natnet.emulator`, protocol core) — pure-Python OptiTrack Motive server emulation so `natnet_ros2` can be driven without hardware; host integration tests (`tests/integration/natnet/`) wire it to the robot client +- Isaac wrapper for the NatNet emulator (USD scene → server) + natnet Pegasus launch scripts, and a dedicated OptiTrack sim e2e test (`optitrack` mark, `tests/system/test_optitrack_e2e.py`) that validates emulator → `natnet_ros2` → PX4 EV fusion in one Isaac bring-up ### Changed From a800869f55bc90abe7e0f47ffd4e95ae2c9c85df Mon Sep 17 00:00:00 2001 From: John Date: Fri, 31 Jul 2026 18:38:25 -0400 Subject: [PATCH 27/33] fix(sim): register the NatNet emulator via the Kit ext-folder The Isaac launch scripts import `optitrack.natnet.emulator`, but Kit was only pointed at the shared exts dir (`~/.local/share/ov/data/documents/Kit/shared/exts`), where Dockerfile.isaac-ros installs pegasus.simulator at image build. The emulator lives in the repo at simulation/isaac-sim/extensions/ and is never copied there, so it was not a registered extension and the import depended on ambient sys.path. Kit accepts repeated --ext-folder, so both standalone commands now pass the repo's extensions dir as a second search root. Chosen over copying the extension into the shared dir at build time because the repo tree is bind-mounted: emulator edits take effect on relaunch instead of requiring an image rebuild. Co-Authored-By: Claude Opus 5 --- CHANGELOG.md | 2 ++ simulation/isaac-sim/docker/docker-compose.yaml | 8 ++++++-- 2 files changed, 8 insertions(+), 2 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index cd64b1358..e27500f42 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -43,6 +43,8 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0 - 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 +- `natnet_ros2_node` on `robot_1` now compares the NatNet server's MODELDEF drone-body count (`Drone` / `Drone1`…`DroneN`, excluding `Target` and skeleton bones) against `NUM_ROBOTS` after the handshake and logs an error on mismatch, so a sim launch script and `natnet_config.yaml` that disagree about how many drones exist is caught at startup rather than as a robot that silently never receives frames. `NUM_ROBOTS` is forwarded into the robot container for it +- Isaac Sim now searches the repo's own `simulation/isaac-sim/extensions` via a second `--ext-folder`, so `optitrack.natnet.emulator` resolves as a registered Kit extension (previously only the Kit shared exts dir was searched, where the emulator is never installed) ## [1.0.0] - 2024-12-19 diff --git a/simulation/isaac-sim/docker/docker-compose.yaml b/simulation/isaac-sim/docker/docker-compose.yaml index dfd699aa4..ce45d9773 100644 --- a/simulation/isaac-sim/docker/docker-compose.yaml +++ b/simulation/isaac-sim/docker/docker-compose.yaml @@ -12,12 +12,16 @@ services: - *image_tag container_name: isaac-sim entrypoint: "" + # Two --ext-folder entries: the Kit shared exts dir (where Dockerfile.isaac-ros + # installs pegasus.simulator at image build), plus the repo's own extensions dir so + # optitrack.natnet.emulator resolves. The repo copy is bind-mounted, so edits to the + # emulator take effect on relaunch without an image rebuild. command: > bash -c " tmux new -d -s isaac; if [ $$AUTOLAUNCH = 'true' ]; then if [ \"${ISAAC_SIM_USE_STANDALONE}\" = 'true' ]; then - tmux send-keys -t isaac 'PYTHONPATH="$$ISAAC_SIM_PYTHONPATH" /isaac-sim/python.sh /isaac-sim/AirStack/simulation/isaac-sim/launch_scripts/${ISAAC_SIM_SCRIPT_NAME} --ext-folder ~/.local/share/ov/data/documents/Kit/shared/exts' ENTER + tmux send-keys -t isaac 'PYTHONPATH="$$ISAAC_SIM_PYTHONPATH" /isaac-sim/python.sh /isaac-sim/AirStack/simulation/isaac-sim/launch_scripts/${ISAAC_SIM_SCRIPT_NAME} --ext-folder ~/.local/share/ov/data/documents/Kit/shared/exts --ext-folder /isaac-sim/AirStack/simulation/isaac-sim/extensions' ENTER else tmux send-keys -t isaac 'ros2 launch isaacsim run_isaacsim.launch.py install_path:=/isaac-sim gui:=\"${ISAAC_SIM_GUI}\" play_sim_on_start:=\"${PLAY_SIM_ON_START}\" ' ENTER fi @@ -120,7 +124,7 @@ services: command: > bash -c " tmux new -d -s isaac; - tmux send-keys -t isaac 'PYTHONPATH=\"$$ISAAC_SIM_PYTHONPATH\" /isaac-sim/python.sh /isaac-sim/AirStack/simulation/isaac-sim/launch_scripts/${ISAAC_SIM_SCRIPT_NAME} --ext-folder ~/.local/share/ov/data/documents/Kit/shared/exts --/app/livestream/enabled=true' ENTER; + tmux send-keys -t isaac 'PYTHONPATH=\"$$ISAAC_SIM_PYTHONPATH\" /isaac-sim/python.sh /isaac-sim/AirStack/simulation/isaac-sim/launch_scripts/${ISAAC_SIM_SCRIPT_NAME} --ext-folder ~/.local/share/ov/data/documents/Kit/shared/exts --ext-folder /isaac-sim/AirStack/simulation/isaac-sim/extensions --/app/livestream/enabled=true' ENTER; sleep infinity" environment: # Inherit everything from isaac-sim and append: From 3677e03a980d0a30a6540db1257ea031fed24653 Mon Sep 17 00:00:00 2001 From: John Date: Wed, 12 Aug 2026 07:01:58 -0400 Subject: [PATCH 28/33] make the sim actually fuse the mocap stream MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit EKF2_EV_CTRL defaults to 0, and the isaac compose set no PX4 params at all, so PX4 discarded the vision entirely and flew on sim GPS. The emulator could stream perfectly and change nothing. PX4 SITL's rcS applies any PX4_PARAM_ env var at boot and Pegasus passes the container env through, so no new mechanism is needed. Each entry defaults to PX4's own default, read out of the firmware in this image — unset is an explicit no-op and non-mocap sims are unaffected. They cannot be defined-but-empty: the rcS loop has no empty-value guard. Also hooks NATNET_BODY_ID in the single-drone launch script. The emulator hardcoded streaming id 1 while the client reads the env var, so a real Motive id would desync the two into a connected client that never publishes. Co-Authored-By: Claude Opus 5 --- CHANGELOG.md | 2 + .../isaac-sim/docker/docker-compose.yaml | 42 +++++++++++++++++++ ...le_one_px4_pegasus_natnet_launch_script.py | 7 +++- 3 files changed, 50 insertions(+), 1 deletion(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index e27500f42..c65e392f0 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -44,6 +44,8 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0 - 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 - `natnet_ros2_node` on `robot_1` now compares the NatNet server's MODELDEF drone-body count (`Drone` / `Drone1`…`DroneN`, excluding `Target` and skeleton bones) against `NUM_ROBOTS` after the handshake and logs an error on mismatch, so a sim launch script and `natnet_config.yaml` that disagree about how many drones exist is caught at startup rather than as a robot that silently never receives frames. `NUM_ROBOTS` is forwarded into the robot container for it +- Isaac Sim PX4 never fused the mocap stream: `EKF2_EV_CTRL` defaults to 0 and the isaac compose set no PX4 parameters, so the emulator could stream perfectly while PX4 flew on sim GPS. The compose now passes the EKF2 external-vision set as `PX4_PARAM_*` (applied by PX4 SITL's `rcS` at boot), each defaulting to PX4's own default so non-mocap sims are unaffected; the mocap path opts in +- The NatNet emulator hardcoded the drone's streaming id to 1 while the client reads `NATNET_BODY_ID`, so a real Motive id desynced the two into a connected client that never published (`example_one_px4_pegasus_natnet_launch_script.py`) - Isaac Sim now searches the repo's own `simulation/isaac-sim/extensions` via a second `--ext-folder`, so `optitrack.natnet.emulator` resolves as a registered Kit extension (previously only the Kit shared exts dir was searched, where the emulator is never installed) ## [1.0.0] - 2024-12-19 diff --git a/simulation/isaac-sim/docker/docker-compose.yaml b/simulation/isaac-sim/docker/docker-compose.yaml index ce45d9773..b977f92e0 100644 --- a/simulation/isaac-sim/docker/docker-compose.yaml +++ b/simulation/isaac-sim/docker/docker-compose.yaml @@ -51,6 +51,48 @@ services: - ARDUPILOT_PHYSICS_HZ=${ARDUPILOT_PHYSICS_HZ:-800} - PX4_RENDERING_HZ=${PX4_RENDERING_HZ:-30} - ARDUPILOT_RENDERING_HZ=${ARDUPILOT_RENDERING_HZ:-120} + # --- PX4 SITL EKF2 external-vision (mocap) parameters ----------------------------- + # PX4 SITL's rcS applies any PX4_PARAM_ env var via `param set` at boot, and + # Pegasus passes the container env straight through to the PX4 process + # (px4_launch_tool.py, os.environ.copy()). Without these EKF2_EV_CTRL is 0, so PX4 + # DISCARDS the vision stream entirely and flies on sim GPS — the NatNet emulator can + # be streaming perfectly and change nothing. + # + # Each default below is PX4's own default, so leaving these unset is an explicit + # no-op and every non-mocap sim behaves exactly as before. The mocap path opts in by + # setting them (see tests/system/test_optitrack_e2e.py). They must not be defined- + # but-empty: the rcS loop has no empty-value guard and would run `param set X ""`. + # + # Values mirror the deployment-validated set in + # robot/ros_ws/src/perception/natnet_ros2/config/px4_params.yaml — that file is the + # single source of truth and carries the full rationale for each one. + # Defaults below are PX4's OWN defaults, read out of the firmware in this image + # (src/modules/ekf2/params_*.yaml, module.yaml, commander_params.c). Keep them in + # sync if PX4 is upgraded — a stale default here silently changes every sim. + # name PX4 default mocap (EV) value + # EKF2_EV_CTRL 0 11 pos xy + z + yaw + # EKF2_HGT_REF 1 (GPS) 3 vision is the height reference + # EKF2_GPS_CTRL 7 0 no GPS fusion + # EKF2_MAG_TYPE 0 5 mag off; yaw from vision + # EKF2_BARO_CTRL 1 0 no baro fusion + # SYS_HAS_BARO 1 0 remove baro at system level + # EKF2_RNG_CTRL 1 0 no range-finder aiding + # EKF2_EV_DELAY 0 7.0 ms; deployment-measured + # EKF2_EV_NOISE_MD 0 1 use the NOISE floors, not msg cov + # EKF2_EVP_NOISE 0.1 0.05 also sets the innovation gate + # EKF2_EVA_NOISE 0.1 0.05 + # COM_ARM_WO_GPS is deliberately absent: its PX4 default is already 1. + - PX4_PARAM_EKF2_EV_CTRL=${PX4_EV_CTRL:-0} + - PX4_PARAM_EKF2_HGT_REF=${PX4_EV_HGT_REF:-1} + - PX4_PARAM_EKF2_GPS_CTRL=${PX4_EV_GPS_CTRL:-7} + - PX4_PARAM_EKF2_MAG_TYPE=${PX4_EV_MAG_TYPE:-0} + - PX4_PARAM_EKF2_BARO_CTRL=${PX4_EV_BARO_CTRL:-1} + - PX4_PARAM_SYS_HAS_BARO=${PX4_EV_SYS_HAS_BARO:-1} + - PX4_PARAM_EKF2_RNG_CTRL=${PX4_EV_RNG_CTRL:-1} + - PX4_PARAM_EKF2_EV_DELAY=${PX4_EV_DELAY:-0.0} + - PX4_PARAM_EKF2_EV_NOISE_MD=${PX4_EV_NOISE_MD:-0} + - PX4_PARAM_EKF2_EVP_NOISE=${PX4_EV_EVP_NOISE:-0.1} + - PX4_PARAM_EKF2_EVA_NOISE=${PX4_EV_EVA_NOISE:-0.1} deploy: # let it use the GPU resources: diff --git a/simulation/isaac-sim/launch_scripts/example_one_px4_pegasus_natnet_launch_script.py b/simulation/isaac-sim/launch_scripts/example_one_px4_pegasus_natnet_launch_script.py index 12fa2213f..522f8de14 100644 --- a/simulation/isaac-sim/launch_scripts/example_one_px4_pegasus_natnet_launch_script.py +++ b/simulation/isaac-sim/launch_scripts/example_one_px4_pegasus_natnet_launch_script.py @@ -100,6 +100,11 @@ ] NATNET_BODY_NAME = os.environ.get("NATNET_BODY_NAME", "Drone") +# Streaming id the emulator advertises for the drone. The NatNet client filters incoming +# frames by NUMERIC id, so this must match the body id in natnet_config.yaml, which reads +# the same env var — a mismatch yields a connected client that never publishes. Both +# default to 1, so the sim path works unconfigured. +NATNET_BODY_ID = int(os.environ.get("NATNET_BODY_ID", "1")) NATNET_TARGET_NAME = os.environ.get("NATNET_TARGET_NAME", "Target") _NATNET_SERVER_KWARGS = { @@ -227,7 +232,7 @@ def _setup_natnet(self, stage): try: author_static_target(stage, DEFAULT_TARGET_PATH, DEFAULT_TARGET_POSITION) bodies = [ - (NATNET_BODY_NAME, 1, "/World/base_link/body"), + (NATNET_BODY_NAME, NATNET_BODY_ID, "/World/base_link/body"), (NATNET_TARGET_NAME, DEFAULT_TARGET_STREAMING_ID, DEFAULT_TARGET_PATH), ] self.natnet_manager = start_drone_natnet_server( From 50b1371eb856ae86325359ef0d4699c277af230a Mon Sep 17 00:00:00 2001 From: John Date: Wed, 12 Aug 2026 07:02:11 -0400 Subject: [PATCH 29/33] fly a circle on mocap fusion instead of asserting a topic exists MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit test_px4_fuses_vision claimed to prove EKF2 fused the external vision but only waited for local_position/pose, which publishes off GPS regardless — it passed with vision disabled. The stack now comes up with GPS, baro and range aiding off, so mocap is the vehicle's only position source, and the module flies the Circle trajectory. Sustained lateral motion is where a wrong EV delay or a too-tight innovation gate shows up; a hover would not reveal either. Cross-track error is scored by the same helpers the autonomy benchmark uses, imported rather than reimplemented. test_px4_fuses_vision is kept as the pre-flight gate — it now establishes only that an estimate exists, and says so. Co-Authored-By: Claude Opus 5 --- CHANGELOG.md | 3 +- tests/system/test_optitrack_e2e.py | 81 ++++++++++++++++++++++++++++-- 2 files changed, 79 insertions(+), 5 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index c65e392f0..5780f242c 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -19,7 +19,7 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0 - `waypoint_flight` system test (`tests/system/test_waypoint_flight.py`): takeoff → ordered waypoint route via `NavigateTask` (dispatched as a dense plan) → land, judged on the odometry track by the standalone stdlib-only `tests/waypoint_checker.py` (in-order corridor arrival within `--waypoint-tolerance`, final goal within `--goal-tolerance`, per-waypoint `--waypoint-timeout`); validated end-to-end in Isaac Sim; serves as the standard acceptance check after integrating or swapping a planner module - Real-robot PX4 external-vision fusion in `natnet_ros2` (OptiTrack mocap → EKF2): `mavros_gp_origin` (geoid-corrected synthetic GPS origin so `local_position.z` == OptiTrack z, fixing the ~36 m boot offset), `vision_pose_converter`, and a PX4 param **checker** (`px4_param_setter`, `auto_set` off by default; `on_mismatch` warn/halt) — setup guide at `docs/robot/px4_external_vision.md` - NatNet server emulator (`optitrack.natnet.emulator`, protocol core) — pure-Python OptiTrack Motive server emulation so `natnet_ros2` can be driven without hardware; host integration tests (`tests/integration/natnet/`) wire it to the robot client -- Isaac wrapper for the NatNet emulator (USD scene → server) + natnet Pegasus launch scripts, and a dedicated OptiTrack sim e2e test (`optitrack` mark, `tests/system/test_optitrack_e2e.py`) that validates emulator → `natnet_ros2` → PX4 EV fusion in one Isaac bring-up +- Isaac wrapper for the NatNet emulator (USD scene → server) + natnet Pegasus launch scripts, and a dedicated OptiTrack sim e2e test (`optitrack` mark, `tests/system/test_optitrack_e2e.py`) that flies a **Circle trajectory on mocap EKF2 fusion** — GPS, baro and range aiding are disabled for the run, so the OptiTrack stream is the vehicle's only position source and cross-track error scores the whole chain ### Changed @@ -46,6 +46,7 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0 - `natnet_ros2_node` on `robot_1` now compares the NatNet server's MODELDEF drone-body count (`Drone` / `Drone1`…`DroneN`, excluding `Target` and skeleton bones) against `NUM_ROBOTS` after the handshake and logs an error on mismatch, so a sim launch script and `natnet_config.yaml` that disagree about how many drones exist is caught at startup rather than as a robot that silently never receives frames. `NUM_ROBOTS` is forwarded into the robot container for it - Isaac Sim PX4 never fused the mocap stream: `EKF2_EV_CTRL` defaults to 0 and the isaac compose set no PX4 parameters, so the emulator could stream perfectly while PX4 flew on sim GPS. The compose now passes the EKF2 external-vision set as `PX4_PARAM_*` (applied by PX4 SITL's `rcS` at boot), each defaulting to PX4's own default so non-mocap sims are unaffected; the mocap path opts in - The NatNet emulator hardcoded the drone's streaming id to 1 while the client reads `NATNET_BODY_ID`, so a real Motive id desynced the two into a connected client that never published (`example_one_px4_pegasus_natnet_launch_script.py`) +- `test_optitrack_e2e.py::test_px4_fuses_vision` asserted only that `local_position/pose` publishes, which it does off GPS — the check passed with external vision disabled. It is now the pre-flight gate (an estimate exists) and the Circle flight is the actual proof of fusion - Isaac Sim now searches the repo's own `simulation/isaac-sim/extensions` via a second `--ext-folder`, so `optitrack.natnet.emulator` resolves as a registered Kit extension (previously only the Kit shared exts dir was searched, where the emulator is never installed) ## [1.0.0] - 2024-12-19 diff --git a/tests/system/test_optitrack_e2e.py b/tests/system/test_optitrack_e2e.py index 87cafebfd..4d51472bd 100644 --- a/tests/system/test_optitrack_e2e.py +++ b/tests/system/test_optitrack_e2e.py @@ -31,12 +31,28 @@ wait_for_container, wait_for_first_message, ) +from system.test_fixed_trajectory import ( + TARGET_ALTITUDE_M, + _landing_one_robot, + _run_parallel, + _takeoff_one_robot, + _trajectory_one_robot, +) pytestmark = pytest.mark.optitrack # Single-drone NatNet Isaac stack: the natnet Pegasus script spawns the emulator # alongside PX4, and LAUNCH_NATNET=true brings up natnet_ros2 + the vision_pose / # gp_origin / param bridges on the robot. +# +# The PX4_EV_* entries switch PX4 SITL's EKF2 to mocap external vision and turn GPS, +# baro and range aiding OFF, so the OptiTrack stream is the vehicle's ONLY position +# source. They map to PX4_PARAM_* env vars in simulation/isaac-sim/docker/docker-compose.yaml +# (PX4's rcS applies those at boot) and mirror the deployment-validated set in +# robot/ros_ws/src/perception/natnet_ros2/config/px4_params.yaml. +# +# Without them EKF2_EV_CTRL is 0, PX4 silently discards the vision and flies on sim GPS — +# which is what made the previous version of this module's fusion check vacuous. _E2E_ENV = { "NUM_ROBOTS": "1", "COMPOSE_PROFILES": "desktop,isaac-sim", @@ -45,10 +61,27 @@ "ISAAC_SIM_SCRIPT_NAME": "example_one_px4_pegasus_natnet_launch_script.py", "PLAY_SIM_ON_START": "true", "LAUNCH_NATNET": "true", + # EKF2 external-vision (mocap) configuration — see comment above. + "PX4_EV_CTRL": "11", # fuse vision horizontal pos + vertical pos + yaw + "PX4_EV_HGT_REF": "3", # vision is the height reference + "PX4_EV_GPS_CTRL": "0", # no GPS fusion + "PX4_EV_MAG_TYPE": "5", # mag off; yaw comes from vision + "PX4_EV_BARO_CTRL": "0", # no baro fusion + "PX4_EV_SYS_HAS_BARO": "0", # remove baro at the system level (height datum) + "PX4_EV_RNG_CTRL": "0", # no range-finder aiding + "PX4_EV_DELAY": "7.0", + "PX4_EV_NOISE_MD": "1", + "PX4_EV_EVP_NOISE": "0.05", + "PX4_EV_EVA_NOISE": "0.05", # Headless: no X on the CI runner. "QT_QPA_PLATFORM": "offscreen", } +# The trajectory flown to prove fusion. Circle is the enforced PR gate: sustained lateral +# motion is where a wrong EV delay or a too-tight innovation gate actually shows up, which +# a stationary hover would never reveal. +_E2E_TRAJECTORY = "Circle" + _ROBOT_PATTERN = "robot.*desktop" _ROBOT_SETUP_BASH = "/root/AirStack/robot/ros_ws/install/setup.bash" _ROBOT_DOMAIN = 1 @@ -60,6 +93,10 @@ # Cold Isaac boot: Pegasus load + Play + emulator UDP connect. _FIRST_MSG_TIMEOUT = 180 +# The flight helpers imported from test_fixed_trajectory take an `airstack_env`-style cfg; +# `robot_setup_bash` is the only key any of them reads. +_TRAJ_CFG = {"robot_setup_bash": _ROBOT_SETUP_BASH} + @pytest.fixture(scope="module") def optitrack_sim_stack(request): @@ -119,9 +156,15 @@ def test_natnet_pose_alive(self, optitrack_sim_stack): assert hz is not None and hz >= _NATNET_MIN_HZ, \ f"{topic} at {hz} Hz (< {_NATNET_MIN_HZ})" - @pytest.mark.dependency(depends=["natnet_pose"]) + @pytest.mark.dependency(name="ev_ready", depends=["natnet_pose"]) def test_px4_fuses_vision(self, optitrack_sim_stack): - """PX4 EKF2 fuses the external vision: local_position/pose streams.""" + """PX4 publishes a fused local position, so it is ready to be commanded. + + This only establishes that an estimate EXISTS — it is deliberately not the proof + that vision is being fused, because local_position/pose publishes off any aiding + source. The flight below is the proof: with GPS, baro and range aiding disabled in + _E2E_ENV, mocap is the only thing that can produce this estimate at all. + """ container = _robot_container(optitrack_sim_stack) topic = f"/robot_{_ROBOT_DOMAIN}/{_PX4_LOCAL_POSE_TOPIC}" @@ -130,6 +173,36 @@ def test_px4_fuses_vision(self, optitrack_sim_stack): setup_bash=_ROBOT_SETUP_BASH, timeout=_FIRST_MSG_TIMEOUT, ) assert first is not None, ( - f"no PX4 local_position on {topic} within {_FIRST_MSG_TIMEOUT}s " - "(EKF2 not fusing external vision)" + f"no PX4 local_position on {topic} within {_FIRST_MSG_TIMEOUT}s — " + "EKF2 has no valid position estimate. With GPS/baro/range aiding off, that " + "means the external-vision path never reached it." ) + + @pytest.mark.dependency(name="ev_takeoff", depends=["ev_ready"]) + @pytest.mark.timeout(2400) + def test_takeoff(self, optitrack_sim_stack): + """Take off to TARGET_ALTITUDE_M flying on the mocap-fused estimate.""" + container = _robot_container(optitrack_sim_stack) + _run_parallel(1, lambda n: _takeoff_one_robot( + n, container, _TRAJ_CFG, TARGET_ALTITUDE_M)) + + @pytest.mark.dependency(name="ev_circle", depends=["ev_takeoff"]) + @pytest.mark.timeout(2400) + def test_circle_trajectory(self, optitrack_sim_stack): + """Fly a Circle with mocap as the only position source. + + This is the end-to-end proof: emulator → natnet_ros2 → vision_pose → MAVROS → + EKF2 → controller → airframe. Cross-track error is scored by the same code the + autonomy benchmark uses, so a mocap regression shows up as path deviation rather + than as a topic that merely exists. + """ + container = _robot_container(optitrack_sim_stack) + _run_parallel(1, lambda n: _trajectory_one_robot( + n, container, _TRAJ_CFG, _E2E_TRAJECTORY)) + + @pytest.mark.dependency(name="ev_land", depends=["ev_takeoff"]) + @pytest.mark.timeout(2400) + def test_landing(self, optitrack_sim_stack): + """Land the drone; runs even when the trajectory phase fails.""" + container = _robot_container(optitrack_sim_stack) + _run_parallel(1, lambda n: _landing_one_robot(n, container, _TRAJ_CFG)) From 89c92e318f88ff2e82748cf817622e8c2df6d1f8 Mon Sep 17 00:00:00 2001 From: John Date: Wed, 12 Aug 2026 07:02:20 -0400 Subject: [PATCH 30/33] enforce only the mocap circle flight on PR open MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The pull_request branch passed no args, so opening a PR ran pytest's defaults: every mark, both sims, all four trajectory types. Now it runs the one end-to-end flight that covers the whole chain. Every other suite is unchanged and still reachable on demand — /pytest comments, workflow_dispatch inputs, and local airstack test. Co-Authored-By: Claude Opus 5 --- .github/workflows/system-tests.yml | 20 +++++++++++++++++--- CHANGELOG.md | 1 + 2 files changed, 18 insertions(+), 3 deletions(-) diff --git a/.github/workflows/system-tests.yml b/.github/workflows/system-tests.yml index 3ddebda58..6cb9c0ee2 100644 --- a/.github/workflows/system-tests.yml +++ b/.github/workflows/system-tests.yml @@ -134,9 +134,23 @@ jobs: if (st := os.environ.get('INPUT_STABLE', '').strip()): args.extend(['--stable-duration', st]) elif event == 'pull_request': - # PR-opened auto-run uses pytest's conftest defaults — same as - # /pytest with no args. - args = [] + # PR-opened auto-run enforces ONE end-to-end flight: the Circle + # trajectory flown on OptiTrack/mocap EKF2 fusion. It exercises the + # whole chain (emulator -> natnet_ros2 -> vision_pose -> MAVROS -> + # EKF2 -> controller -> airframe) and scores cross-track error, so a + # regression anywhere in it fails the PR. + # + # Everything else stays reproducible ON DEMAND and is unchanged: + # - comment `/pytest -m 'autonomy' --trajectory-types Circle,Figure8` + # - workflow_dispatch inputs (marks / sim / num_robots / iterations) + # - locally, `airstack test -m ` + # Previously this was `args = []`, i.e. pytest's conftest defaults — + # every mark, both sims, all four trajectory types on every PR open. + args = [ + '-m', 'build_packages or optitrack', + '--sim', 'isaacsim', + '--num-robots', '1', + ] else: body = os.environ.get('COMMENT_BODY', '') # Only the first line is parsed — everything below it is diff --git a/CHANGELOG.md b/CHANGELOG.md index 5780f242c..7ec17d0a8 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -23,6 +23,7 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0 ### Changed +- Opening a PR now runs one enforced end-to-end flight — the Circle trajectory on OptiTrack/mocap EKF2 fusion — instead of pytest's full defaults (every mark, both sims, all four trajectory types). Every other suite is unchanged and still available on demand via a `/pytest ` comment, `workflow_dispatch` inputs, or local `airstack test -m ` - `robot-l4t` compose service knobs are now env-overridable (`AUTONOMY_ROLE`, `FCU_URL`, and the rosbag path via `BAG_STORAGE_PATH`); `FCU_URL` unquoted so the literal serial path reaches MAVROS - `zed-l4t` image: ZED SDK 4.2 → 5.2 with the coupled ROS deps (`zed_msgs` 5.2.1, `point_cloud_transport(_plugins)` 4.x, add `backward_ros`) - Unit tests are defined by `tests/colcon_unit_test_packages.yaml`: `conftest.py` collects each listed package's co-located `test/` dir under `--import-mode=importlib` and marks it `unit` (ament lint files are skipped and run under `colcon test`) From 26bbc9c90fce4f53278326c95788ed7ecb9dc310 Mon Sep 17 00:00:00 2001 From: John Date: Thu, 13 Aug 2026 09:26:27 -0400 Subject: [PATCH 31/33] add an isaac natnet mocap override MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Brings up the emulator plus PX4 on external-vision fusion in one command — the same configuration test_optitrack_e2e.py uses, so the test environment is reproducible by hand. Sets PLAY_SIM_ON_START explicitly because the root .env ships it false: the scene then loads paused, /clock never ticks, and every use_sim_time node sits frozen while the stack looks healthy. Co-Authored-By: Claude Opus 5 --- CHANGELOG.md | 1 + overrides/isaac-natnet-vision.env | 66 +++++++++++++++++++++++++++++++ 2 files changed, 67 insertions(+) create mode 100644 overrides/isaac-natnet-vision.env diff --git a/CHANGELOG.md b/CHANGELOG.md index 7ec17d0a8..376b4a901 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/isaac-natnet-vision.env` — brings up Isaac Sim with the NatNet emulator and PX4 flying on mocap EKF2 external vision (GPS/baro/range aiding off), i.e. the configuration `tests/system/test_optitrack_e2e.py` runs, reproducible by hand - `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) diff --git a/overrides/isaac-natnet-vision.env b/overrides/isaac-natnet-vision.env new file mode 100644 index 000000000..fae542244 --- /dev/null +++ b/overrides/isaac-natnet-vision.env @@ -0,0 +1,66 @@ +# Isaac Sim + OptiTrack NatNet mocap, with PX4 flying on external-vision (EV) fusion. +# +# Usage: +# airstack up --env-file overrides/isaac-natnet-vision.env +# +# Data path: +# in-sim NatNet emulator -> natnet_ros2 -> vision_pose_converter +# -> /{robot}/interface/mavros/vision_pose/pose_cov -> PX4 EKF2 +# +# This is the same configuration tests/system/test_optitrack_e2e.py brings up, so it is +# the way to reproduce that test's environment by hand. Real-robot counterpart: +# overrides/l4t-optitrack-realrobot.env + +COMPOSE_PROFILES="desktop,isaac-sim" +AUTOLAUNCH="true" +NUM_ROBOTS="1" + +# --- Isaac scene -------------------------------------------------------------- +ISAAC_SIM_USE_STANDALONE="true" +ISAAC_SIM_SCRIPT_NAME="example_one_px4_pegasus_natnet_launch_script.py" +# Multi-agent variant — spawns NUM_ROBOTS drones and streams one rigid body per drone, +# using the drone index as the streaming id: +# ISAAC_SIM_SCRIPT_NAME="example_multi_px4_pegasus_natnet_launch_script.py" + +# REQUIRED: the root .env ships this "false", which loads the scene PAUSED. Pegasus +# publishes /clock off the playback tick, so paused means /clock never ticks, every +# use_sim_time node sits frozen, and the stack looks healthy while nothing moves. +PLAY_SIM_ON_START="true" + +# --- OptiTrack / NatNet ------------------------------------------------------- +LAUNCH_NATNET="true" + +# The emulator runs inside the isaac-sim container, which holds this static IP on +# airstack_network. Stated explicitly because this is the one setting that differs +# between sim and a real deployment, where it is the Motive host's address. +NATNET_SERVER_IP="172.31.0.200" + +# Must agree between the emulator (launch script) and the client (natnet_config.yaml), +# which both read these. The client filters incoming frames by NUMERIC id, so a mismatch +# yields a connected client that never publishes. +NATNET_BODY_NAME="Drone" +NATNET_BODY_ID="1" + +# --- PX4 EKF2 external-vision fusion ----------------------------------------- +# Without these PX4 ignores the vision stream entirely — EKF2_EV_CTRL defaults to 0 — and +# flies on sim GPS, so the emulator can be streaming perfectly and change nothing. +# +# These become PX4_PARAM_* in simulation/isaac-sim/docker/docker-compose.yaml, which PX4 +# SITL's rcS applies at boot. Values mirror the deployment-validated set in +# robot/ros_ws/src/perception/natnet_ros2/config/px4_params.yaml — see that file for the +# rationale behind the delay and noise figures. +# +# GPS, baro and range aiding are all switched OFF, so mocap is the vehicle's only +# position source. That is deliberate: it means a successful flight is proof that EV +# fusion works, and that killing the emulator leaves PX4 with no estimate at all. +PX4_EV_CTRL="11" # fuse vision horizontal position + vertical position + yaw +PX4_EV_HGT_REF="3" # vision is the height reference +PX4_EV_GPS_CTRL="0" # no GPS fusion +PX4_EV_MAG_TYPE="5" # magnetometer off; yaw comes from vision +PX4_EV_BARO_CTRL="0" # no barometer fusion +PX4_EV_SYS_HAS_BARO="0" # remove the baro at system level, so the height datum is vision +PX4_EV_RNG_CTRL="0" # no range-finder aiding +PX4_EV_DELAY="7.0" # ms, measured end to end on the real deployment +PX4_EV_NOISE_MD="1" # use the NOISE floors below, not the message covariance +PX4_EV_EVP_NOISE="0.05" # m; also sets the innovation gate (EKF2_EVP_GATE sigma wide) +PX4_EV_EVA_NOISE="0.05" # rad From 71c7fff8d20e5eb1fd0690eabc221a43c0aef09b Mon Sep 17 00:00:00 2001 From: John Date: Thu, 13 Aug 2026 10:07:20 -0400 Subject: [PATCH 32/33] install the natnet emulator as a real Kit extension MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The natnet launch scripts died with ModuleNotFoundError: No module named 'optitrack'. Pointing Kit's --ext-folder at the repo extensions dir was not enough — that only makes Kit aware of an extension, it does not put the package on sys.path. Handle it the same way pegasus.simulator already is: bake a copy into the Kit shared exts dir and pip-install it editable, then bind-mount the repo copy over it so edits stay live. The scripts now enable_extension() before importing, which registers the extension and its omni.isaac.core / omni.usd dependencies. The repo-extensions --ext-folder flag is dropped; the extension now lives in the dir the image already searches. Verified in a running container: extension starts, emulator serves on 172.31.0.200 :1510/:1511, and the robot sees /robot_1/perception/optitrack/drone/pose_cov at ~101 Hz feeding vision_pose and PX4 local_position at ~32 Hz. Co-Authored-By: Claude Opus 5 --- CHANGELOG.md | 2 +- simulation/isaac-sim/docker/Dockerfile.isaac-ros | 12 ++++++++++++ simulation/isaac-sim/docker/docker-compose.yaml | 10 ++++++++-- .../extensions/optitrack.natnet.emulator/.gitignore | 5 +++++ ...example_multi_px4_pegasus_natnet_launch_script.py | 8 +++++++- .../example_one_px4_pegasus_natnet_launch_script.py | 11 ++++++++++- 6 files changed, 43 insertions(+), 5 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 376b4a901..f05a6abe6 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -49,7 +49,7 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0 - Isaac Sim PX4 never fused the mocap stream: `EKF2_EV_CTRL` defaults to 0 and the isaac compose set no PX4 parameters, so the emulator could stream perfectly while PX4 flew on sim GPS. The compose now passes the EKF2 external-vision set as `PX4_PARAM_*` (applied by PX4 SITL's `rcS` at boot), each defaulting to PX4's own default so non-mocap sims are unaffected; the mocap path opts in - The NatNet emulator hardcoded the drone's streaming id to 1 while the client reads `NATNET_BODY_ID`, so a real Motive id desynced the two into a connected client that never published (`example_one_px4_pegasus_natnet_launch_script.py`) - `test_optitrack_e2e.py::test_px4_fuses_vision` asserted only that `local_position/pose` publishes, which it does off GPS — the check passed with external vision disabled. It is now the pre-flight gate (an estimate exists) and the Circle flight is the actual proof of fusion -- Isaac Sim now searches the repo's own `simulation/isaac-sim/extensions` via a second `--ext-folder`, so `optitrack.natnet.emulator` resolves as a registered Kit extension (previously only the Kit shared exts dir was searched, where the emulator is never installed) +- The NatNet emulator is now installed as a Kit extension: `Dockerfile.isaac-ros` pip-installs it editable into the Isaac python and bind-mounts the repo copy over it (the same pattern as `pegasus.simulator`), and the natnet launch scripts `enable_extension` it before importing. Being on a Kit `--ext-folder` search path only makes Kit *aware* of an extension — it does not put the package on `sys.path` — so the scripts previously died with `ModuleNotFoundError: No module named 'optitrack'` ## [1.0.0] - 2024-12-19 diff --git a/simulation/isaac-sim/docker/Dockerfile.isaac-ros b/simulation/isaac-sim/docker/Dockerfile.isaac-ros index 92ac63fdf..cbee53b9f 100644 --- a/simulation/isaac-sim/docker/Dockerfile.isaac-ros +++ b/simulation/isaac-sim/docker/Dockerfile.isaac-ros @@ -145,6 +145,18 @@ ENV ACCEPT_EULA="Y" # ENV ISAACSIM_PYTHON=/isaac-sim/python.sh RUN /isaac-sim/python.sh -m pip install --no-cache-dir -e /isaac-sim/.local/share/ov/data/documents/Kit/shared/exts/pegasus.simulator +# "Install" the OptiTrack NatNet emulator extension, same editable pattern as +# pegasus.simulator above: the copy baked in here is replaced at runtime by a bind mount +# of the repo directory (see docker-compose.yaml), and the editable install keeps +# resolving to that path — so `import optitrack...` works and edits reflect live. +# +# Being on the Kit --ext-folder search path only makes Kit AWARE of an extension; it does +# not put the package on sys.path. Without this pip install the natnet launch scripts die +# with ModuleNotFoundError: No module named 'optitrack'. +COPY extensions/optitrack.natnet.emulator \ + /isaac-sim/.local/share/ov/data/documents/Kit/shared/exts/optitrack.natnet.emulator +RUN /isaac-sim/python.sh -m pip install --no-cache-dir -e /isaac-sim/.local/share/ov/data/documents/Kit/shared/exts/optitrack.natnet.emulator + # Install PX4 things RUN git clone --branch ${PX4_VERSION} --recursive https://github.com/PX4/PX4-Autopilot.git diff --git a/simulation/isaac-sim/docker/docker-compose.yaml b/simulation/isaac-sim/docker/docker-compose.yaml index b977f92e0..cf5d7610e 100644 --- a/simulation/isaac-sim/docker/docker-compose.yaml +++ b/simulation/isaac-sim/docker/docker-compose.yaml @@ -21,7 +21,7 @@ services: tmux new -d -s isaac; if [ $$AUTOLAUNCH = 'true' ]; then if [ \"${ISAAC_SIM_USE_STANDALONE}\" = 'true' ]; then - tmux send-keys -t isaac 'PYTHONPATH="$$ISAAC_SIM_PYTHONPATH" /isaac-sim/python.sh /isaac-sim/AirStack/simulation/isaac-sim/launch_scripts/${ISAAC_SIM_SCRIPT_NAME} --ext-folder ~/.local/share/ov/data/documents/Kit/shared/exts --ext-folder /isaac-sim/AirStack/simulation/isaac-sim/extensions' ENTER + tmux send-keys -t isaac 'PYTHONPATH="$$ISAAC_SIM_PYTHONPATH" /isaac-sim/python.sh /isaac-sim/AirStack/simulation/isaac-sim/launch_scripts/${ISAAC_SIM_SCRIPT_NAME} --ext-folder ~/.local/share/ov/data/documents/Kit/shared/exts' ENTER else tmux send-keys -t isaac 'ros2 launch isaacsim run_isaacsim.launch.py install_path:=/isaac-sim gui:=\"${ISAAC_SIM_GUI}\" play_sim_on_start:=\"${PLAY_SIM_ON_START}\" ' ENTER fi @@ -114,6 +114,9 @@ services: - $HOME/docker/isaac-sim/pkg:/isaac-sim/.local/share/ov/pkg:rw \ # pegasus integration - ../extensions/PegasusSimulator/extensions/pegasus.simulator:/isaac-sim/.local/share/ov/data/documents/Kit/shared/exts/pegasus.simulator/:rw + # NatNet emulator extension, same editable pattern as pegasus above: the image + # baked in a copy and pip-installed it editable, this mount makes repo edits live. + - ../extensions/optitrack.natnet.emulator:/isaac-sim/.local/share/ov/data/documents/Kit/shared/exts/optitrack.natnet.emulator/:rw # omniverse - ./omniverse.toml:/isaac-sim/.nvidia-omniverse/config/omniverse.toml:rw - ./user.config.json:/isaac-sim/.local/share/ov/data/Kit/Isaac-Sim Full/5.1/user.config.json:rw # enables pegasus extension; IMPORTANT: set the version number without the trailing .0 @@ -166,7 +169,7 @@ services: command: > bash -c " tmux new -d -s isaac; - tmux send-keys -t isaac 'PYTHONPATH=\"$$ISAAC_SIM_PYTHONPATH\" /isaac-sim/python.sh /isaac-sim/AirStack/simulation/isaac-sim/launch_scripts/${ISAAC_SIM_SCRIPT_NAME} --ext-folder ~/.local/share/ov/data/documents/Kit/shared/exts --ext-folder /isaac-sim/AirStack/simulation/isaac-sim/extensions --/app/livestream/enabled=true' ENTER; + tmux send-keys -t isaac 'PYTHONPATH=\"$$ISAAC_SIM_PYTHONPATH\" /isaac-sim/python.sh /isaac-sim/AirStack/simulation/isaac-sim/launch_scripts/${ISAAC_SIM_SCRIPT_NAME} --ext-folder ~/.local/share/ov/data/documents/Kit/shared/exts --/app/livestream/enabled=true' ENTER; sleep infinity" environment: # Inherit everything from isaac-sim and append: @@ -189,6 +192,9 @@ services: - $HOME/docker/isaac-sim/data:/isaac-sim/.local/share/ov/data:rw - $HOME/docker/isaac-sim/pkg:/isaac-sim/.local/share/ov/pkg:rw - ../extensions/PegasusSimulator/extensions/pegasus.simulator:/isaac-sim/.local/share/ov/data/documents/Kit/shared/exts/pegasus.simulator/:rw + # NatNet emulator extension, same editable pattern as pegasus above: the image + # baked in a copy and pip-installed it editable, this mount makes repo edits live. + - ../extensions/optitrack.natnet.emulator:/isaac-sim/.local/share/ov/data/documents/Kit/shared/exts/optitrack.natnet.emulator/:rw - ./omniverse.toml:/isaac-sim/.nvidia-omniverse/config/omniverse.toml:rw - ./user.config.json:/isaac-sim/.local/share/ov/data/Kit/Isaac-Sim Full/5.1/user.config.json:rw - .dev:/isaac-sim/.dev:rw diff --git a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/.gitignore b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/.gitignore index 0db35ef6e..eebdc0623 100644 --- a/simulation/isaac-sim/extensions/optitrack.natnet.emulator/.gitignore +++ b/simulation/isaac-sim/extensions/optitrack.natnet.emulator/.gitignore @@ -7,3 +7,8 @@ **/*.tlog/ **/__pycache__/ **/*.pyc + +# Editable-install metadata: Dockerfile.isaac-ros pip-installs this extension in +# editable mode and the repo dir is bind-mounted into the container, so the build +# artifact lands here on every container start. +**/*.egg-info/ diff --git a/simulation/isaac-sim/launch_scripts/example_multi_px4_pegasus_natnet_launch_script.py b/simulation/isaac-sim/launch_scripts/example_multi_px4_pegasus_natnet_launch_script.py index 12b4799d1..68e44b530 100644 --- a/simulation/isaac-sim/launch_scripts/example_multi_px4_pegasus_natnet_launch_script.py +++ b/simulation/isaac-sim/launch_scripts/example_multi_px4_pegasus_natnet_launch_script.py @@ -51,7 +51,13 @@ sys.path.insert(0, os.path.normpath(os.path.join(os.path.dirname(os.path.abspath(__file__)), "..", "utils"))) from scene_prep import scale_stage_prim, add_colliders, add_dome_light, save_scene_as_contained_usd -from optitrack.natnet.emulator.isaac import ( +# As in the single-drone natnet script: the emulator is a Kit extension, enabled here +# before anything imports from it. +from isaacsim.core.utils.extensions import enable_extension # noqa: E402 + +enable_extension("optitrack.natnet.emulator") + +from optitrack.natnet.emulator.isaac import ( # noqa: E402 DEFAULT_TARGET_PATH, DEFAULT_TARGET_POSITION, DEFAULT_TARGET_STREAMING_ID, diff --git a/simulation/isaac-sim/launch_scripts/example_one_px4_pegasus_natnet_launch_script.py b/simulation/isaac-sim/launch_scripts/example_one_px4_pegasus_natnet_launch_script.py index 522f8de14..bcc389227 100644 --- a/simulation/isaac-sim/launch_scripts/example_one_px4_pegasus_natnet_launch_script.py +++ b/simulation/isaac-sim/launch_scripts/example_one_px4_pegasus_natnet_launch_script.py @@ -72,7 +72,16 @@ sys.path.insert(0, _LAUNCH_SCRIPTS_DIR) from gps_utils import set_gps_origins, DEFAULT_WORLD_ORIGIN -from optitrack.natnet.emulator.isaac import ( +# The NatNet emulator ships as a Kit extension (simulation/isaac-sim/extensions/ +# optitrack.natnet.emulator). Enable it before importing from it: that registers the +# extension with Kit along with its omni.isaac.core / omni.usd dependencies and its UI +# entry point. The plain import below resolves because Dockerfile.isaac-ros pip-installs +# the package into the Isaac python, the same way pegasus.simulator is handled. +from isaacsim.core.utils.extensions import enable_extension # noqa: E402 + +enable_extension("optitrack.natnet.emulator") + +from optitrack.natnet.emulator.isaac import ( # noqa: E402 DEFAULT_TARGET_PATH, DEFAULT_TARGET_POSITION, DEFAULT_TARGET_STREAMING_ID, From 650a438cd9709b5e497a6184b7916ee3e1e0916b Mon Sep 17 00:00:00 2001 From: John Date: Thu, 13 Aug 2026 11:59:44 -0400 Subject: [PATCH 33/33] set the streamed body in the script, not the environment MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The emulator read NATNET_BODY_NAME / NATNET_BODY_ID from the environment to stay in sync with the client. The client now takes its bodies from its per-robot profile in natnet_config.yaml, so the env hook was asymmetric and, being global, could not describe a multi-robot scene anyway. Both are now constants in the launch scripts, with the pairing spelled out inline, in the emulator sim doc, and in the optitrack-development skill — including that a mismatched id fails silently: the client connects and never publishes. Co-Authored-By: Claude Opus 5 --- .agents/skills/optitrack-development/SKILL.md | 3 +- docs/simulation/isaac_sim/natnet_emulator.md | 28 +++++++++++++++---- overrides/isaac-natnet-vision.env | 6 ---- ..._multi_px4_pegasus_natnet_launch_script.py | 9 ++++-- ...le_one_px4_pegasus_natnet_launch_script.py | 18 +++++++----- tests/integration/natnet/README.md | 4 +-- 6 files changed, 44 insertions(+), 24 deletions(-) diff --git a/.agents/skills/optitrack-development/SKILL.md b/.agents/skills/optitrack-development/SKILL.md index fcfe66e87..0828a9a7b 100644 --- a/.agents/skills/optitrack-development/SKILL.md +++ b/.agents/skills/optitrack-development/SKILL.md @@ -17,6 +17,7 @@ metadata: - Understanding **NatNet wire protocol** (connect, model def, frame streaming) - Capturing what **`libNatNet.so`** actually sends on the network - Enabling OptiTrack in sim: `LAUNCH_NATNET=true`, `natnet_config.yaml`, Docker IPs +- Sim testing with mocap: bring the stack up with `overrides/isaac-natnet-vision.env`, which starts Isaac + the emulator and switches PX4 EKF2 to external-vision fusion (GPS/baro/range aiding off, so mocap is the only position source) ## Architecture in AirStack @@ -50,7 +51,7 @@ flowchart LR | [`example_one_px4_pegasus_natnet_launch_script.py`](../../../simulation/isaac-sim/launch_scripts/example_one_px4_pegasus_natnet_launch_script.py) | Single drone + static ``Target`` | | [`example_multi_px4_pegasus_natnet_launch_script.py`](../../../simulation/isaac-sim/launch_scripts/example_multi_px4_pegasus_natnet_launch_script.py) | ``NUM_ROBOTS`` drones + shared ``Target`` (pair with 3-profile ``natnet_config.yaml``) | -Helpers: [`isaac/scene_setup.py`](../../../simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/scene_setup.py) (`start_drone_natnet_server`, `author_static_target`). Drone body: single = ``Drone``; multi = ``Drone`` (id ``i``); target = ``Target`` (id 100). Override names with ``NATNET_BODY_NAME`` / ``NATNET_TARGET_NAME``. Baseline Pegasus scripts (no NatNet) remain ``example_one_px4_pegasus_launch_script.py`` / ``example_multi_px4_pegasus_launch_script.py``. +Helpers: [`isaac/scene_setup.py`](../../../simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/scene_setup.py) (`start_drone_natnet_server`, `author_static_target`). Drone body: single = ``Drone`` (id 1); multi = ``Drone`` (id ``i``); target = ``Target`` (id 100). These are **constants in the launch script**, not env vars — change them there AND in the matching ``natnet_config.yaml`` profile together. The client filters frames by numeric id, so a mismatch is silent: it connects and never publishes. Baseline Pegasus scripts (no NatNet) remain ``example_one_px4_pegasus_launch_script.py`` / ``example_multi_px4_pegasus_launch_script.py``. **Default client config:** unicast, `server_ip` → Motive/emulator (use `172.31.0.200` for Isaac container), ports 1510/1511. The config is per-robot: each `robots[$ROBOT_NAME]` profile lists the bodies it tracks (each a `rigid_body_name` + `id` mapped to a relative `topic`, with `pose`/`pose_cov` toggles and per-body covariance) and an optional `vision_pose` block that drives the MAVROS bridge. See [`natnet_config.yaml`](../../../robot/ros_ws/src/perception/natnet_ros2/config/natnet_config.yaml). diff --git a/docs/simulation/isaac_sim/natnet_emulator.md b/docs/simulation/isaac_sim/natnet_emulator.md index 2bda072ee..3262665ea 100644 --- a/docs/simulation/isaac_sim/natnet_emulator.md +++ b/docs/simulation/isaac_sim/natnet_emulator.md @@ -82,11 +82,29 @@ PX4, start the NatNet emulator, and play the simulation automatically. | 1 | `Drone`, `Target` | | N > 1 | `Drone1`, `Drone2`, …, `DroneN`, `Target` | -Override the base name with `NATNET_BODY_NAME` (default `Drone`) and -`NATNET_TARGET_NAME` (default `Target`). These must match the -`rigid_body_name` entries in your robot's -[`natnet_config.yaml`](../../../robot/ros_ws/src/perception/natnet_ros2/config/natnet_config.yaml) -profile. +### Changing which body is streamed + +The streamed body name and streaming id are **constants in the launch script** +(`NATNET_BODY_NAME` / `NATNET_BODY_ID` / `NATNET_TARGET_NAME`), not environment +variables. They must match a body entry in the robot's profile in +[`natnet_config.yaml`](../../../robot/ros_ws/src/perception/natnet_ros2/config/natnet_config.yaml), +which is the only place the client reads its bodies from — that is what lets each robot +in a multi-robot scene track a different body. + +To retarget, edit **both** together: + +| Where | What | +|---|---| +| `example_one_px4_pegasus_natnet_launch_script.py` | `NATNET_BODY_NAME`, `NATNET_BODY_ID` | +| `natnet_config.yaml` → `robots..bodies[]` | `rigid_body_name`, `id` | + +!!! warning "A mismatch fails silently" + The NatNet client filters incoming frames by **numeric id**. If the ids disagree, the + client connects, the emulator streams, and the pose topic never publishes — with no + error on either side. When debugging a silent stream, check the id first. + + Deliberately not settable from an env file: one global variable cannot express + per-robot bodies, so it would break multi-robot. ### What the robot container needs diff --git a/overrides/isaac-natnet-vision.env b/overrides/isaac-natnet-vision.env index fae542244..fb033e88b 100644 --- a/overrides/isaac-natnet-vision.env +++ b/overrides/isaac-natnet-vision.env @@ -35,12 +35,6 @@ LAUNCH_NATNET="true" # between sim and a real deployment, where it is the Motive host's address. NATNET_SERVER_IP="172.31.0.200" -# Must agree between the emulator (launch script) and the client (natnet_config.yaml), -# which both read these. The client filters incoming frames by NUMERIC id, so a mismatch -# yields a connected client that never publishes. -NATNET_BODY_NAME="Drone" -NATNET_BODY_ID="1" - # --- PX4 EKF2 external-vision fusion ----------------------------------------- # Without these PX4 ignores the vision stream entirely — EKF2_EV_CTRL defaults to 0 — and # flies on sim GPS, so the emulator can be streaming perfectly and change nothing. diff --git a/simulation/isaac-sim/launch_scripts/example_multi_px4_pegasus_natnet_launch_script.py b/simulation/isaac-sim/launch_scripts/example_multi_px4_pegasus_natnet_launch_script.py index 68e44b530..ae7cbfe63 100644 --- a/simulation/isaac-sim/launch_scripts/example_multi_px4_pegasus_natnet_launch_script.py +++ b/simulation/isaac-sim/launch_scripts/example_multi_px4_pegasus_natnet_launch_script.py @@ -22,7 +22,6 @@ - ``NUM_ROBOTS`` (default 1) - ``ENABLE_LIDAR`` (default false) - ``PLAY_SIM_ON_START`` (default true) - - ``NATNET_BODY_NAME`` / ``NATNET_TARGET_NAME`` (optional name overrides) """ import asyncio @@ -73,8 +72,12 @@ NUM_ROBOTS = int(os.environ.get("NUM_ROBOTS", "1")) ENABLE_LIDAR = os.environ.get("ENABLE_LIDAR", "false").lower() == "true" -NATNET_BODY_NAME = os.environ.get("NATNET_BODY_NAME", "Drone") -NATNET_TARGET_NAME = os.environ.get("NATNET_TARGET_NAME", "Target") +# Base name for the streamed drone bodies; drone i is streamed with id i (see +# _drone_body_name below). These must match the body entries in the per-robot profiles +# in natnet_config.yaml — the client filters frames by numeric id. Set them here, not in +# an env file. +NATNET_BODY_NAME = "Drone" +NATNET_TARGET_NAME = "Target" _NATNET_SERVER_KWARGS = { "pose_noise_enabled": True, diff --git a/simulation/isaac-sim/launch_scripts/example_one_px4_pegasus_natnet_launch_script.py b/simulation/isaac-sim/launch_scripts/example_one_px4_pegasus_natnet_launch_script.py index bcc389227..826797aa7 100644 --- a/simulation/isaac-sim/launch_scripts/example_one_px4_pegasus_natnet_launch_script.py +++ b/simulation/isaac-sim/launch_scripts/example_one_px4_pegasus_natnet_launch_script.py @@ -108,13 +108,17 @@ {"domain_id": 1, "x_m": 0.0, "y_m": 0.0, "z_m": 0.07}, ] -NATNET_BODY_NAME = os.environ.get("NATNET_BODY_NAME", "Drone") -# Streaming id the emulator advertises for the drone. The NatNet client filters incoming -# frames by NUMERIC id, so this must match the body id in natnet_config.yaml, which reads -# the same env var — a mismatch yields a connected client that never publishes. Both -# default to 1, so the sim path works unconfigured. -NATNET_BODY_ID = int(os.environ.get("NATNET_BODY_ID", "1")) -NATNET_TARGET_NAME = os.environ.get("NATNET_TARGET_NAME", "Target") +# Rigid body this scene streams, and the streaming id it advertises. +# +# These MUST match a body entry in the robot's profile in +# robot/ros_ws/src/perception/natnet_ros2/config/natnet_config.yaml — the NatNet client +# filters incoming frames by NUMERIC id, so a mismatch gives a client that connects and +# then never publishes, with no error on either side. Change them here and in that file +# together, never in an env file: the client reads the body from its per-robot profile +# so that multiple robots can each track their own body. +NATNET_BODY_NAME = "Drone" +NATNET_BODY_ID = 1 +NATNET_TARGET_NAME = "Target" _NATNET_SERVER_KWARGS = { "pose_noise_enabled": True, diff --git a/tests/integration/natnet/README.md b/tests/integration/natnet/README.md index 4e9330c32..7a5f44d83 100644 --- a/tests/integration/natnet/README.md +++ b/tests/integration/natnet/README.md @@ -103,8 +103,8 @@ The matching **system** check is - Asserts `/{robot_n}/{natnet pose topic}/pose_cov` ≥ 5 Hz per robot (the drone body's configured topic — default `perception/optitrack/drone`). - Override the checked topic with `NATNET_POSE_TOPIC` (default - `perception/optitrack/drone`). The sim body name (`NATNET_BODY_NAME`, default - `Drone`) is decoupled from the published topic, which the robot profile sets. + `perception/optitrack/drone`). The body name and the published topic are decoupled; + both are set in the robot's `natnet_config.yaml` profile. Sim auto-start: set `ISAAC_SIM_SCRIPT_NAME` to a NatNet launch script and `LAUNCH_NATNET=true` on the robot. Convenience bundle: