Johnliu/optitrack emulation by JohnYanxinLiu · Pull Request #367 · castacks/AirStack

JohnYanxinLiu · 2026-06-15T14:21:08Z

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:

Send a Takeoff command to each drone (target altitude 10 m).
Confirm in the Foxglove 3D panel that each drone rises to ~10 m and holds.
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

…sages

…handlers Refactor the emulator server around a pre-packed MODELDEF wire cache and add the NAT_REQUEST_MODELDEF and NAT_KEEPALIVE handlers so libNatNet 4.4 clients complete the unicast handshake. Mock a real Motive server (server name "Motive") and move hardcoded Drone reference constants into a new defaults.py so scene semantics stay out of the wire layer. Drop the vendored NatNet SDK README that should not live in-tree. Co-authored-by: Cursor <cursoragent@cursor.com>

…defaults Cover the MODELDEF/frame serializers, the unicast handshake protocol, and the defaults/server catalog. A package-level test/conftest.py puts the extension root on sys.path and registers the `unit` marker so `pytest test/` runs the suite directly without per-file boilerplate. Co-authored-by: Cursor <cursoragent@cursor.com>

…ture Add repo_path() and reexport_unit_tests() to tests/conftest.py so the thin proxy files re-export co-located package tests without hardcoded Path(__file__).parents[N] walks or per-file sys.path boilerplate; rewrite the robot and sim proxies to use them. Register the `integration` marker and the robot_autonomy_stack bring-up fixture (gated behind --run-integration) for the new integration tier. Co-authored-by: Cursor <cursoragent@cursor.com>

Introduce tests/integration/ as a new tier between unit and system: real components wired together (robot autonomy container + a host-side component) with no sim or GPU. Move the NatNet emulator <-> natnet_ros2 test here as the first resident (marks: integration, natnet), driven by the shared robot_autonomy_stack fixture, and remove the old tests/sim/motive_emulator location. Co-authored-by: Cursor <cursoragent@cursor.com>

Update AGENTS.md, tests/README.md, and the add-unit-tests / optitrack-development skills to describe the four test tiers (unit, integration, system), the repo_path/reexport_unit_tests proxy pattern, the --run-integration flag, and the new tests/integration/natnet location. Co-authored-by: Cursor <cursoragent@cursor.com>

NatNet emulator installation is baked into the isaac-sim image, bind-mounted for live edits, and enabled in Kit config. Co-authored-by: Cursor <cursoragent@cursor.com>

…ever)

Remap vision_pose_converter to the interface/mavros namespace, install mavros_extras in the robot image, and add layered PX4_PARAM_PROFILE env files so Isaac SITL can fuse NatNet external vision (EKF2_EV_CTRL, no GPS, indoor mag disabled). Document the vision profile and provide an overrides bundle for NatNet + vision sim bring-up. Co-authored-by: Cursor <cursoragent@cursor.com>

PX4 with EKF2_GPS_CTRL=0 has valid local vision fusion but no global position, so AUTO.LOITER preflight fails until an origin is set. Add mavros_gp_origin_node to publish a one-shot set_gp_origin after MAVROS connects, skipping when a real origin already exists. Launched alongside vision_pose_converter when publish_to_mavros is enabled. Co-authored-by: Cursor <cursoragent@cursor.com>

…vertised tolerances

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.

Co-authored-by: Copilot Autofix powered by AI <175728472+Copilot@users.noreply.github.com>

JohnYanxinLiu · 2026-07-07T02:20:53Z

+            [3.0e-6, 0.0,    0.0,
+             0.0,    3.0e-6, 0.0,
+             0.0,    0.0,    3.0e-6]
+    robot_2:


Additional robots are added if user desires to use or modify, but are instantiated unless more robots are initialized in AirStack.

…ests + mocap tier Resolves the in-progress develop merge, conforming this branch to develop's test infrastructure while preserving the branch's additions: - tests/conftest.py: adopt develop's in-memory logging (_LAST_CMD_OUTPUT + log_cli), drop the file-based LOGS_DIR path, and remove the now-orphaned _ANSI_RE. Retain register_unit_tests() and the integration (motion-capture) tier. - pytest.ini / AGENTS.md / SKILL.md: unify the mark set to the full union (unit, build_docker, build_packages, integration, liveliness, sensors, takeoff_hover_land, autonomy). - .env: VERSION 0.19.0-alpha.6 (> develop's alpha.5, clears the version gate). - Remove accidental junk files (--gui, --num-robots, --sim, --stress-iterations, --trajectory-types, -v) and stale merge-artifact pre-commit hooks. Note: committed as a single parent (the interrupted merge left no MERGE_HEAD). Co-Authored-By: Claude Opus 4.8 <noreply@anthropic.com>

…y measurement The OptiTrack NatNet emulator now writes each outgoing frame's TransmitTimestamp in the server's high-resolution clock domain, matching the clock used in the NAT_ECHORESPONSE handshake (time.time() nanoseconds) and the advertised HighResClockFrequency (1e9 ticks/s). This lets a connected client recover per-message transit latency via NatNetClient::SecondsSinceHostTimestamp(), so the drone-side latency benchmark produces real numbers end-to-end in sim/CI. Verified: emulator send-path tests pass. Co-Authored-By: Claude Opus 4.8 <noreply@anthropic.com>

… Cube Orange hop Adds a one-shot latency benchmark to the NatNet client. After the first frame the node skips a warm-up interval, then accumulates each message's transit latency over a fixed window and logs a formatted RCLCPP_INFO summary (mean, stdev, and an end-to-end estimate that adds a modeled PX4/Cube Orange hardware latency). - natnet_client_adapter: compute per-frame transit latency via the SDK's SecondsSinceHostTimestamp(TransmitTimestamp); thread it through a FrameCallbackCtx so the C trampoline can reach the client. FrameSample gains transit_latency_s / has_latency. - natnet_ros2_node: sample latency in on_frame() (warm-up 5s, window 20s by default), compute Bessel-corrected stdev, and log mean/stdev + cube_orange_latency_ms (default 5 ms; models MAVROS→MAVLink→uORB→EKF2, the piece not captured by the measured transport). New params wired through the launch flattener and natnet_config.yaml. Depends on the emulator stamping TransmitTimestamp for meaningful sim/CI numbers. Verified: colcon build + 59 gtests pass. Co-Authored-By: Claude Opus 4.8 <noreply@anthropic.com>

…ution fixes (#370) Foundational real-robot deployment fixes extracted from the OptiTrack emulation PR (#367) so they can be reviewed and merged first; #367 will be rebased on top afterward, shrinking its diff. Docker / ARM build: - Add TARGET_ARCH build arg (default x86_64) to Dockerfile.robot and use it to parametrize LD_LIBRARY_PATH, so the aarch64 (Jetson/l4t, voxl) images link against the correct arch triplet. - docker-compose.yaml passes TARGET_ARCH: aarch64 to the voxl and l4t image builds. - Install ros-${ROS_DISTRO}-mavros-extras (generic dep; also provides the vision_pose plugin used by external-pose deployments). Robot name resolution: - .bashrc now follows a pre-set ROBOT_NAME (e.g. injected by docker compose) instead of always overriding it from the container/hostname mapping. The bws() flock build lock is retained. - default_robot_name_map.yaml catch-all fallback maps to unknown_robot (valid ROS namespace token) instead of unknown-robot. Version bumped 0.19.0-alpha.5 -> 0.19.0-alpha.6 for the version-increment gate. Note: the trajectory_controller/trajectory_library robustness fixes originally listed for extraction are already present on develop (PR #365), so they are not included here. Co-authored-by: Claude Opus 4.8 <noreply@anthropic.com>

JohnYanxinLiu · 2026-07-23T16:12:02Z

Superseded by #373. Rather than rebase this branch's 44 interleaved commits onto the reworked test harness, #373 rebuilds the OptiTrack work cleanly (extract-by-path onto the test-infra base) and also folds in the real-robot PX4 external-vision fusion from the Hummingbird repo. Closing in favor of #373.

…ion baseline 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 <noreply@anthropic.com>

#374) * feat(perception): bring natnet_ros2 client up to the optitrack_emulation baseline 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 <noreply@anthropic.com> * feat(natnet): real-robot PX4 external-vision fusion (mocap → EKF2) 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 <noreply@anthropic.com> * docs(natnet): PX4 external-vision setup guide + height-datum explainer 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 <noreply@anthropic.com> * 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 <noreply@anthropic.com> * chore: bump version to 0.19.0-alpha.14 * fix(natnet): make the NatNet client actually reachable + correct EV tuning 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 <noreply@anthropic.com> * add a real-robot OptiTrack deployment override 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 <noreply@anthropic.com> * config bodies per robot profile; trim comments to the docs 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 <noreply@anthropic.com> * put the mocap floor at the shared world datum 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 <noreply@anthropic.com> * fail the build when the geoid dataset is missing 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 <noreply@anthropic.com> * abbreviated Dockerfile comment on geographic lib installation * 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 <noreply@anthropic.com> * comment trim * point the companion-link section at the PX4 docs 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 <noreply@anthropic.com> * frame section 4 around mavros_gp_origin, demote the 36 m note 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 <noreply@anthropic.com> * reject an unknown connection_type instead of defaulting to unicast 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 <noreply@anthropic.com> * px4 external vision docs trim * trim natnet node comments; note the latency figure is an estimate 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 <noreply@anthropic.com> * trim the external-vision tuning notes 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 <noreply@anthropic.com>

JohnYanxinLiu and others added 25 commits June 4, 2026 15:38

WIP implementation

70eedb4

optitrack emulator isaac-sim boilerplate

2f3ddfc

current documentation update

e9cfc79

bugfix applied to assigning hostname for server on startup

e9e5c7e

first round of unit tests for optitrack connection and basic data mes…

394b03d

…sages

model definition implementation with unit tests

11522bb

incremented version tag

4ce3712

feat(isaac-sim): install and mount optitrack.natnet.emulator extension

c427472

NatNet emulator installation is baked into the isaac-sim image, bind-mounted for live edits, and enabled in Kit config. Co-authored-by: Cursor <cursoragent@cursor.com>

NatNet server object stub with isaac-sim extension implemented

8c2cfc6

Scans and detects NatNetInterface object parameters

95eef12

Implemented server starting and stopping

a71655e

working single agent case

846ba70

added capability to set z or y axis up (we should keep z-axis up, how…

bd37eda

…ever)

references pegasus sim drone body which is what moves.

71e92f2

added realistic optitrack sensor noise

db673b6

adjusted position and orientation covariances to match optitrack's ad…

24f410f

…vertised tolerances

fixed commentsand some docs

5883321

removed EV delay from sim

8279d90

JohnYanxinLiu force-pushed the johnliu/optitrack_emulation branch from 9a6a546 to d6c6ef4 Compare June 16, 2026 23:35

mass code cleaning

83b5929

JohnYanxinLiu force-pushed the johnliu/optitrack_emulation branch from d6c6ef4 to 83b5929 Compare June 16, 2026 23:48

JohnYanxinLiu added 2 commits June 17, 2026 00:54

multiagent support, vision pose cleanup, example scripts cleanup

694720c

fixed global position initialization issue for navigation task

9ce2bfb

JohnYanxinLiu added 2 commits June 18, 2026 16:54

reduced default case to single agent with commented options

5400e88

adjust tests for vision estimate case.

a6e91d3

JohnYanxinLiu requested a review from Copilot June 19, 2026 16:07

Copilot started reviewing on behalf of JohnYanxinLiu June 19, 2026 16:07 View session

Copilot AI reviewed Jun 19, 2026

View reviewed changes

Comment thread tests/system/test_takeoff_hover_land.py Outdated

Comment thread simulation/isaac-sim/extensions/optitrack.natnet.emulator/setup.py

Comment thread ...c-sim/extensions/optitrack.natnet.emulator/optitrack/natnet/emulator/server/natnet_server.py

JohnYanxinLiu and others added 3 commits June 19, 2026 12:17

Adding requirements for natnet emulator isaac-sim extension

326cae8

Co-authored-by: Copilot Autofix powered by AI <175728472+Copilot@users.noreply.github.com>

Cleans up ros topic listening for takeoff and hover test

b46a1f9

Co-authored-by: Copilot Autofix powered by AI <175728472+Copilot@users.noreply.github.com>

Zero checker for natnet emulator publish rate parameter

9369492

Co-authored-by: Copilot Autofix powered by AI <175728472+Copilot@users.noreply.github.com>

JohnYanxinLiu requested review from fangyikuan, kay-000, krrishj18 and neelay-j June 19, 2026 16:20

JohnYanxinLiu marked this pull request as ready for review June 19, 2026 16:21

fixed takeoff_hover_land test

2a4ac0b

JohnYanxinLiu commented Jul 7, 2026

View reviewed changes

JohnYanxinLiu and others added 3 commits July 14, 2026 14:12

This was referenced Jul 19, 2026

Generic robot infra: aarch64 build args + robot-name resolution fixes #369

Closed

l4t infra fixes: aarch64 build args + robot-name resolution #370

Merged

JohnYanxinLiu mentioned this pull request Jul 22, 2026

Test infra rework: YAML-driven unit-test collection + integration tier #372

Merged

JohnYanxinLiu marked this pull request as draft July 22, 2026 15:20

JohnYanxinLiu mentioned this pull request Jul 23, 2026

OptiTrack e2e: NatNet emulator + real-robot PX4 external-vision fusion #373

Closed

JohnYanxinLiu closed this Jul 23, 2026

JohnYanxinLiu mentioned this pull request Jul 23, 2026

OptiTrack (1/3): robot-side NatNet client + PX4 external-vision fusion #374

Merged

Navigation Menu

Uh oh!

Johnliu/optitrack emulation - #367

Johnliu/optitrack emulation#367
JohnYanxinLiu wants to merge 44 commits into
developfrom
johnliu/optitrack_emulation

JohnYanxinLiu commented Jun 15, 2026 •

edited

Loading

Uh oh!

Copilot AI left a comment

Uh oh!

Uh oh!

Uh oh!

Uh oh!

JohnYanxinLiu Jul 7, 2026

Uh oh!

JohnYanxinLiu commented Jul 23, 2026

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

2 participants

Uh oh!

Conversation

JohnYanxinLiu commented Jun 15, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

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

How did you implement it?

How do you run and use it?

Bring up the full stack

Verify NatNet topics are publishing

Step 3 — Verify local odometry is updating

Step 4 — Takeoff all drones via Foxglove

Step 5 — Run the random walk navigation planner

Teardown

Testing with PyTest

Documentation

Versioning

Uh oh!

Copilot AI left a comment

Choose a reason for hiding this comment

Pull request overview

Reviewed changes

Uh oh!

Uh oh!

Uh oh!

Uh oh!

JohnYanxinLiu Jul 7, 2026

Choose a reason for hiding this comment

Uh oh!

JohnYanxinLiu commented Jul 23, 2026

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

2 participants

JohnYanxinLiu commented Jun 15, 2026 •

edited

Loading