Johnliu/optitrack emulation

[JohnYanxinLiu]

What features did you add and/or bugs did you address?

• Which GitHub issue does this address?
  N/A

• Additional description if not fully described in the GitHub issue
  Optitrack (MoCAP) NatNet protocol implementation:

• robot client (natnet_ros2)

• Isaac Sim Motive emulator

• protocol handshake

• and libNatNet 4.4 unicast mocked server behavior.

• Please add videos and images to demonstrate the feature. Please upload videos to somewhere persistent (e.g. YouTube or Vimeo) for archival purposes.
  N/A

How did you implement it?

• Algorithm details, design decisions, engineering notes, and any other relevant information about the implementation should be included
• client code is implemented in the robot container from a previous PR. With the following changes:
  • Multi-agent profile selection — natnet_config.yaml defines a robots: map keyed by ROBOT_NAME; the launch file reads the container's ROBOT_NAME env var at startup and flattens only that robot's body list into node parameters, so a single config file serves the whole fleet.
  • GPS origin initialisation — mavros_gp_origin_node publishes a one-shot synthetic GPS origin to MAVROS after a 5-second settle period, guarded against overwriting an existing origin (e.g. from real GNSS); defaults were corrected from Zürich to Lisbon to match the GCS datum and fix ~1.8 km wrong-way navigation offsets.
  • OptiTrack → MAVROS conversion — vision_pose_converter_node subscribes to a body's PoseWithCovarianceStamped and republishes to vision_pose/pose and vision_pose/pose_cov with quaternion canonicalisation (qw ≥ 0) to prevent EKF sign-flip discontinuities; topic names are injected directly by the launch file from the per-robot vision_pose: config block.
  • Dockerfile ARM fix — TARGET_ARCH: aarch64 was moved from environment (runtime) to build.args (build time) for robot-l4t and robot-voxl-onboard, so Dockerfile.robot's ARG TARGET_ARCH=x86_64 correctly overrides LD_LIBRARY_PATH to aarch64-linux-gnu during the image build.
• NatNet emulator: The NatNet Server is a pure Python NatNet 4.1 UDP server with ctypes wire structs rereated from the NatNet SDK (Sample files) (https://www.optitrack.com/support/downloads) . The Isaac Sim wrapper layer samples tracked prim poses each physics step and enqueues them into to the server; configuration lives on a /World/NatNetInterface USD prim with a Kit docked UI editor.

How do you run and use it?

• What commands and button presses do you use to manually launch the stack to use your new feature?
  The recommended way to exercise the full feature set — multi-drone NatNet vision-pose — also covers the single-drone case.

Bring up the full stack

airstack up --env-file overrides/isaac-natnet-vision.env

This starts 3 robot containers and Isaac Sim running example_multi_px4_pegasus_natnet_launch_script.py with LAUNCH_NATNET=true and SITL_PARAM_PROFILE=px4-vision. Wait until Isaac Sim reaches the Pegasus "Play" state and the robot containers report ROS 2 nodes live.

Verify NatNet topics are publishing

For each drone, confirm the OptiTrack rigid-body pose is being received and published:

# Drone body pose for robot_1 (replace robot_1/robot_2/robot_3 as needed)
docker exec airstack-robot-desktop-1 bash -lc "ros2 topic hz /robot_1/perception/optitrack/Drone1"

# Target body pose (robot_1 and robot_2 profiles include a Target body)
docker exec airstack-robot-desktop-1 bash -lc "ros2 topic hz /robot_1/perception/optitrack/Target"

# Vision pose being fed to MAVROS/PX4
docker exec airstack-robot-desktop-1 bash -lc "ros2 topic hz /robot_1/interface/mavros/vision_pose/pose"

Expected: all topics publishing at ~120 Hz (NatNet stream rate).

Step 3 — Verify local odometry is updating

docker exec airstack-robot-desktop-1 bash -lc "ros2 topic echo /robot_1/odometry --once"
docker exec airstack-robot-desktop-2 bash -lc "ros2 topic echo /robot_2/odometry --once"
docker exec airstack-robot-desktop-3 bash -lc "ros2 topic echo /robot_3/odometry --once"

Check that the x, y, z values in the pose field match the spawn positions of the drones in Isaac Sim (drones spawn spaced along the X axis at the same interval). Confirm values are updating as the simulation runs.

Step 4 — Takeoff all drones via Foxglove

Open Foxglove (GCS) and connect to ws://localhost:8765. For each robot:

1. Send a Takeoff command to each drone (target altitude 10 m).
2. Confirm in the Foxglove 3D panel that each drone rises to ~10 m and holds.
3. Confirm ros2 topic echo /robot_N/odometry --once reports z ≈ 10.0.

Step 5 — Run the random walk navigation planner

With all drones hovering, trigger the navigation task using the random walk (exploration) planner from the Foxglove GCS panel. Verify that:

• Each drone begins navigating independently.
• The status: navigating feedback appears in the GCS for each robot.
• Drones move to different waypoints without flying off to unexpected far-away locations (which would indicate a GPS datum mismatch — the bug fixed in this PR).
• The position readout in Foxglove tracks the odometry topic reasonably.

Teardown

airstack stop

Testing with PyTest

• What pytests were added?

  • robot/ros_ws/src/perception/natnet_ros2/test/test_natnet_logic.cpp — C++ unit tests for the natnet_logic.hpp state machine (body fan-out, tracking-valid gating, covariance injection, multi-body profiles).
  • robot/ros_ws/src/perception/natnet_ros2/test/test_natnet_ros2.py — Python unit tests for VisionPoseConverterNode (quaternion canonicalisation, configurable-topic wiring) and natnet_ros2.launch.py profile-flattening helpers (env expansion, body array construction, namespacing).
  • simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/ — 17 unit tests covering the emulator serializers, unicast handshake protocol, MODELDEF catalog, pose sampling, scene setup, and server lifecycle.
  • tests/integration/natnet/test_natnet_integration.py — 3 integration tests: (1) raw NatNetUnicastServer hand-built single-body frames → natnet_ros2_node in the robot container; (2) NatNetServerManager sampling an in-memory USD stage; (3) multi-body profile (drone + target) exercising per-body topic overrides and pose/pose_cov toggles. Metric: all body topics must publish at ≥ 5 Hz for 12 s.

• Exact commands:

# Unit tests (no Docker)
airstack test -m unit -v

# Integration tests (requires running robot container)
airstack test -m integration -v

CI system tests (GPU runner) — trigger via PR comment:

/pytest -m liveliness --sim isaacsim --num-robots 1 --stress-iterations 1
/pytest -m sensors --sim isaacsim --num-robots 1 --stress-iterations 1
/pytest -m liveliness --sim isaacsim --num-robots 3 --stress-iterations 1
/pytest -m takeoff_hover_land --sim isaacsim --num-robots 1

• Expected results:
  • Unit tests: all PASSED; C++ tests reported by colcon test, Python tests by pytest.
  • Integration tests: test_natnet_pose_hz variants each PASSED; check the per-test Hz measurement logged to stdout (should be ≥ 5 Hz).
  • liveliness: all containers healthy, /clock active, sentinel ROS 2 nodes present for all NUM_ROBOTS.
  • sensors: camera, LiDAR, and (when LAUNCH_NATNET=true) vision_pose topics all stream at expected Hz; check metrics.json Hz values and the CI PR comment diff vs baseline.
  • takeoff_hover_land: drone completes all 4 phases (PX4-ready → takeoff → hover → land) without coordinate errors; pass/fail visible in the CI PR comment.

Documentation

• mkdocs.yml updated: Yes — added natnet_ros2 README, docker-build-profiles.md, unit_testing.md, OSMO tutorial, and tests/integration/ docs to navigation.
• Scope: Newcomers can follow robot/ros_ws/src/perception/natnet_ros2/README.md for NatNet setup (sim and real hardware), docs/tutorials/airstack_on_osmo.md for cloud-GPU dev, and docs/development/intermediate/testing/unit_testing.md for the test proxy pattern; agent skills (optitrack-development, add-unit-tests) are updated.

Versioning

• Did you make sure to bump the version number in the .env file according to semantic versioning?
  Yes

[Copilot]

Pull request overview

Adds an OptiTrack/NatNet motion-capture “loop” for AirStack—covering a Python NatNet server emulator (incl. Isaac Sim wrapper), updated natnet_ros2 client bringup/config for multi-robot profiles + MAVROS bridging, and expanded pytest coverage (unit, new integration tier, and system-test sentinels) to validate end-to-end external-vision operation.

Changes:

• Introduce an Isaac Sim extension implementing a NatNet 4.x unicast server emulator + USD-driven configuration and pose sampling.
• Update natnet_ros2 to support per-robot multi-body profiles, configurable vision-pose topic wiring, and a one-shot MAVROS GPS origin publisher for no-GNSS arming/modes.
• Extend the pytest harness with an integration tier, NatNet liveliness sentinel checks, and improved system-test robustness for vision/NatNet profiles.

Reviewed changes

Copilot reviewed 117 out of 117 changed files in this pull request and generated 3 comments.

Show a summary per file

File	Description
tests/system/test_takeoff_hover_land.py	Adds vision-profile settle time, arm-retry behavior, and vision-aware timeouts for takeoff/land system tests.
tests/system/test_liveliness.py	Adds NatNet pose topic sentinel (gated on LAUNCH_NATNET=true) to catch broken mocap streaming in-system.
tests/system/test_build_packages.py	Switches to pytest.fail(...) for clearer failure output in build/package system tests.
tests/system/test_build_docker.py	Switches to pytest.fail(...) for clearer failure output in docker build system tests.
tests/sim/README.md	Documents new NatNet emulator unit-test proxy location and usd-core-guarded USD tests.
tests/sim/optitrack_natnet_emulator/test_unicast_protocol.py	Proxy registering emulator unicast-protocol unit tests for centralized pytest discovery.
tests/sim/optitrack_natnet_emulator/test_target_resolution.py	Proxy registering emulator target-resolution unit tests for centralized pytest discovery.
tests/sim/optitrack_natnet_emulator/test_server_lifecycle.py	Proxy registering emulator server-lifecycle unit tests for centralized pytest discovery.
tests/sim/optitrack_natnet_emulator/test_server_from_config.py	Proxy registering emulator server-from-config unit tests for centralized pytest discovery.
tests/sim/optitrack_natnet_emulator/test_serializers.py	Proxy registering emulator serializer unit tests for centralized pytest discovery.
tests/sim/optitrack_natnet_emulator/test_scene_setup.py	Proxy registering emulator scene-setup helper unit tests for centralized pytest discovery.
tests/sim/optitrack_natnet_emulator/test_pose_streaming.py	Proxy registering emulator pose-streaming unit tests for centralized pytest discovery.
tests/sim/optitrack_natnet_emulator/test_pose_sampling.py	Proxy registering emulator pose-sampling unit tests for centralized pytest discovery.
tests/sim/optitrack_natnet_emulator/test_interface_config.py	Proxy registering emulator config-model unit tests for centralized pytest discovery.
tests/sim/optitrack_natnet_emulator/test_interface_authoring.py	Proxy registering emulator USD authoring unit tests for centralized pytest discovery.
tests/sim/optitrack_natnet_emulator/test_frames.py	Proxy registering emulator pose→frame builder unit tests for centralized pytest discovery.
tests/sim/optitrack_natnet_emulator/test_discovery.py	Proxy registering emulator interface discovery/formatting unit tests for centralized pytest discovery.
tests/sim/optitrack_natnet_emulator/test_defaults_and_catalog.py	Proxy registering emulator defaults + server-catalog unit tests for centralized pytest discovery.
tests/sim/optitrack_natnet_emulator/test_catalog.py	Proxy registering emulator catalog-builder unit tests for centralized pytest discovery.
tests/sim/motive_emulator/README.md	Removes obsolete placeholder README in favor of the new emulator + integration tier.
tests/sensor_probes.py	Uses is_isaac_sim() helper for sim checks (includes isaacsim_natnet variant).
tests/robot/sensors/lidar_point_cloud_filter/test_validation_core.py	Migrates proxy to register_unit_tests() helper.
tests/robot/README.md	Updates docs to describe register_unit_tests() proxy pattern (vs importlib re-export).
tests/robot/perception/natnet_ros2/test_natnet_ros2.py	Migrates natnet_ros2 proxy to register_unit_tests() helper.
tests/requirements.txt	Adds SciPy and USD (usd-core) to support emulator math + optional USD-based tests.
tests/README.md	Documents the new integration tier and adds isaacsim_natnet to default sim targets.
tests/pytest.ini	Registers new integration pytest marker.
tests/integration/README.md	Introduces integration-tier structure, purpose, fixture usage, and execution guidance.
tests/integration/natnet/README.md	Documents host-emulator ↔ robot-container NatNet integration scenario and expectations.
tests/docker/docker-compose.yaml	Plumbs NatNet-related env vars into the test runner container.
tests/colcon_unit_test_packages.yaml	Minor cleanup of comments for colcon unit-test package list.
simulation/isaac-sim/launch_scripts/example_one_px4_pegasus_launch_script.py	Ensures GPS home/origin is written before PX4 SITL starts to align with GCS datum.
simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_target_resolution.py	Adds USD-stage target resolution unit tests (guarded by pxr).
simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_server_lifecycle.py	Adds server manager lifecycle tests using a mocked server factory.
simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_server_from_config.py	Adds tests validating MODELDEF handshake and restart behavior using real sockets.
simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_server_catalog.py	Adds tests for MODELDEF payload cache behavior and catalog packing.
simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_scene_setup.py	Adds hermetic tests for launch-script helper config construction and validation.
simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_pose_streaming.py	Adds loopback tests verifying sampled USD pose makes it into NAT_FRAMEOFDATA.
simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_interface_config.py	Adds unit tests for the pure-Python interface config model and validation.
simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_interface_authoring.py	Adds USD author/read round-trip tests for interface prim attributes.
simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_frames.py	Adds unit tests for pose→frame conversion, flags, and packing expectations.
simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_discovery.py	Adds unit tests for human-readable interface formatting/inspection helper.
simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_defaults.py	Adds tests ensuring default tracked-body bindings match expected natnet_ros2 config.
simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/test_catalog.py	Adds catalog packing limits/ordering tests and duplicate-target detection tests.
simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/natnet_test_helpers.py	Adds shared UDP test helpers (ephemeral ports, minimal client, server context manager).
simulation/isaac-sim/extensions/optitrack.natnet.emulator/test/conftest.py	Adds local test-path setup + marker registration for direct extension test runs.
simulation/isaac-sim/extensions/optitrack.natnet.emulator/setup.py	Adds setuptools metadata to allow pip editable install inside Isaac Sim image.
simulation/isaac-sim/extensions/optitrack.natnet.emulator/schema/schema.usda	Adds codeless USD schema describing interface prim attributes for Kit UI friendliness.
simulation/isaac-sim/extensions/optitrack.natnet.emulator/README.md	Documents emulator architecture, configuration, protocol notes, and test tiers.
simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_unicast_server.py	Implements NatNet unicast server command handling + data flush behavior.
simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_server_types.py	Adds ctypes wire structs and packing logic for serverinfo/packet headers.
simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_model_types.py	Adds MODELDEF-related ctypes structs and packing routines.
simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_common.py	Defines protocol/model size limits used by the emulator.
simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/init.py	Exposes server classes/types as a small import surface.
simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/usd_bindings.py	Adds USD author/read/find helpers for the NatNet interface prim.
simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/scene_setup.py	Adds launch-script helpers to author interface prims and start server managers.
simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/frames.py	Implements pose→NatNet frame conversion, axis emulation, and optional pose noise.
simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/catalog.py	Builds MODELDEF catalogs from config and detects duplicate target prims.
simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/isaac/init.py	Exposes Isaac integration API surface for scripts/UI.
simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/defaults.py	Defines default tracked-body bindings used by tests/examples.
simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/init.py	Exposes top-level emulator API (server + defaults + catalog helper).
simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/init.py	Adds namespace package init for NatNet modules.
simulation/isaac-sim/extensions/optitrack.natnet.emulator/optitrack/init.py	Adds namespace package init for OptiTrack-related modules.
simulation/isaac-sim/extensions/optitrack.natnet.emulator/config/extension.toml	Registers Kit extension modules and dependencies.
simulation/isaac-sim/extensions/optitrack.natnet.emulator/.gitignore	Ignores local SDK/build artifacts and Python cache files.
simulation/isaac-sim/docker/user_TEMPLATE.config.json	Enables the NatNet emulator extension by default in Isaac Sim user config template.
simulation/isaac-sim/docker/sitl-files/px4-vision.env	Adds PX4 SITL parameter bundle for external-vision (vision_pose → EKF2) fusion.
simulation/isaac-sim/docker/sitl-files/default.env	Adds default (no override) SITL bundle placeholder for compose selection.
simulation/isaac-sim/docker/Dockerfile.isaac-ros	Installs PX4 deps directly and adds editable install of NatNet emulator extension.
simulation/isaac-sim/docker/docker-compose.yaml	Loads SITL param bundles and mounts/enables the NatNet emulator extension.
robot/ros_ws/src/perception/perception_bringup/launch/perception.launch.xml	Updates NatNet-related bringup comment to reflect new multi-robot config schema.
robot/ros_ws/src/perception/natnet_ros2/test/test_natnet_ros2.py	Expands unit tests for topic wiring + launch helper flattening (ROS deps stubbed).
robot/ros_ws/src/perception/natnet_ros2/test/test_natnet_logic.cpp	Adds tests for namespaced topic handling and configured-body filtering.
robot/ros_ws/src/perception/natnet_ros2/src/vision_pose_converter_node.py	Adds topic-parameterized wiring for flexible vision_pose bridging.
robot/ros_ws/src/perception/natnet_ros2/src/mavros_gp_origin_node.py	Adds one-shot synthetic GPS origin publisher guarded against overriding existing origin.
robot/ros_ws/src/perception/natnet_ros2/package.xml	Adds geographic_msgs dependency for GPS origin publication message type.
robot/ros_ws/src/perception/natnet_ros2/launch/vision_pose_converter.launch.xml	Switches from remaps to explicit topic params supplied by natnet profile config.
robot/ros_ws/src/perception/natnet_ros2/launch/mavros_gp_origin.launch.xml	Adds launch file wiring the GPS origin node into MAVROS topic namespace.
robot/ros_ws/src/perception/natnet_ros2/include/natnet_ros2/natnet_logic.hpp	Adds helpers for namespacing per-body topic overrides and multi-body id filtering.
robot/ros_ws/src/perception/natnet_ros2/config/natnet_config.yaml	Replaces single-body params with multi-robot, multi-body profile schema + vision_pose block.
robot/ros_ws/src/perception/natnet_ros2/config/mavros_gp_origin.yaml	Adds Lisbon-defaulted GPS origin config matching GCS/sim datum.
robot/ros_ws/src/perception/natnet_ros2/CMakeLists.txt	Installs new Python node and refines comments around install behavior.
robot/docker/Dockerfile.robot	Makes ROS LD_LIBRARY_PATH arch suffix configurable via build arg; adds mavros extras.
robot/docker/docker-compose.yaml	Passes TARGET_ARCH=aarch64 at build time for VOXL and L4T profiles.
overrides/isaac-natnet-vision.env	Adds convenience env-file override for NatNet + PX4 external-vision profile runs.
mkdocs.yml	Adds navigation entry for NatNet emulator documentation.
docs/simulation/isaac_sim/spawning_drones.md	Documents new NatNet-enabled Pegasus launch scripts.
docs/simulation/isaac_sim/pegasus_scene_setup.md	Documents SITL_PARAM_PROFILE bundles and external-vision setup.
docs/simulation/isaac_sim/docker.md	Documents SITL env bundles and related compose wiring.
docs/robot/docker/index.md	Documents TARGET_ARCH build-arg semantics for ROS library path suffixing.
docs/development/intermediate/testing/unit_testing.md	Updates unit-test proxy explanation to register_unit_tests() and references emulator.
docs/development/intermediate/testing/index.md	Updates unit-test docs to reflect register_unit_tests() proxy mechanism.
docs/development/intermediate/docker-build-profiles.md	Documents TARGET_ARCH build arg and provides examples.
airstack.sh	Updates default --sim help text to include isaacsim_natnet.
AGENTS.md	Adds optitrack-development skill reference and updates testing tier descriptions.
.github/workflows/system-tests.yml	Adds isaacsim_natnet to workflow defaults and ensures isaac profile selection matches both isaac targets.
.env	Bumps version to 0.19.0-alpha.4.
.agents/skills/run-system-tests/SKILL.md	Updates docs for isaacsim_natnet default sim targets and proxy pattern.
.agents/skills/docker-build-profiles/SKILL.md	Updates docs to emphasize TARGET_ARCH as build arg and its effect on LD_LIBRARY_PATH.
.agents/skills/add-unit-tests/SKILL.md	Updates proxy guidance to use register_unit_tests() and documents its contract.

---

💡 Add Copilot custom instructions for smarter, more guided reviews. Learn how to get started.