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 --run-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