Johnliu/optitrack autonomy

[JohnYanxinLiu]

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

• GitHub issue:

• Adds native OptiTrack motion capture integration to AirStack via the NatNet SDK.
  The natnet_ros2 package receives rigid body pose data from an external Motive PC
  over NatNet UDP and publishes into the AirStack perception layer as standard ROS 2
  PoseStamped / PoseWithCovarianceStamped topics, with an optional MAVROS bridge
  for PX4 external pose feedback.

  Also included: unit testing infrastructure improvements — pytest registration fixes
  for lidar_point_cloud_filter, a YAML-driven colcon test manifest
  (tests/colcon_unit_test_packages.yaml), and ~55 C++ gtests + 7 Python unit tests
  for natnet logic.

How did you implement it?

• C++ NatNet node (natnet_ros2_node.cpp): connects to Motive via the NatNet SDK,
  registers a frame callback that decodes rigid body poses, and publishes them namespaced
  under /{ROBOT_NAME}/perception/optitrack/{body_name}.

• Python vision pose converter (vision_pose_converter_node.py): subscribes to the
  raw optitrack pose and re-publishes as /{ROBOT_NAME}/mavros/vision_pose/pose with
  canonical quaternion normalization for PX4.

• Opt-in by default: LAUNCH_NATNET defaults to false in docker-compose.yaml
  (LAUNCH_NATNET=${LAUNCH_NATNET:-false}). Set LAUNCH_NATNET=true in .env to
  activate. If true but the SDK was not installed, the launch file raises a
  RuntimeError (fail-hard rather than silently skip).

• Conditional build: CMakeLists.txt detects the SDK at configure time. If absent,
  the natnet_ros2_node executable is skipped with a warning — the package still builds
  cleanly for CI without the SDK.

• SDK licensing: The OptiTrack NatNet SDK is proprietary and not redistributed.
  Users install it locally via airstack setup (explicit license acceptance). AirStack
  remains fully open-source.

• LD_LIBRARY_PATH: An ament env-hook (natnet_library_path.dsv.in) adds the SDK
  .so to LD_LIBRARY_PATH on sws so no manual path configuration is needed.

• Unit test isolation: C++ logic (frame decoding, covariance matrix construction,
  topic name building) is separated into a thin adapter class so it can be tested
  without the real SDK or a live ROS node. Tests live in
  robot/ros_ws/src/perception/natnet_ros2/test/test_natnet_logic.cpp.

• YAML test manifest: tests/colcon_unit_test_packages.yaml lists which packages
  are gated under colcon test in CI. This replaces a hardcoded list in
  test_build_packages.py and makes it easy to add new packages without touching the
  test harness.

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?
  To install the NatNet SDK, run
  airstack setup

You can launch airstack with NatNet enabled:
AUTOLAUNCH=true; LAUNCH_NATNET=true; airstack up

For now, this will only launch the natnet application without connecting to any server. Soon, an Isaac-Sim emulator will be implemented to give this more confidence. For now, NatNet is not launched by defualt.

• Write a detailed procedure with EXACT BASH COMMANDS so that another maintainer can replicate and understand the benefits of your feature, and reproduce the videos and images you added above.
  (Listed above)

Testing with PyTest

• What pytests did you add to ensure the feature is reliable and robust? What metrics are used?
  Unit tests for lidar_filter_cloud are reorganized, and unit tests regarding the helper functions and coordinate framing for optitrack are added.

• What's the exact command to run the pytests that test your feature? i.e. airstack test -m ...
  To test python-based unit tests, run airstack test -m unit -v
  To run C++ unit tests and building, run airstack test -m build_packages -k test_colcon_test_robot -v

• What are the expected results of the tests? What should a maintainer look at to understand whether the test succeeded?
  All tests should pass.

Documentation

• Was mkdocs.yml updated? (y/n)
  y

• Do the docs have sufficient scope such that a newcomer can easily reproduce and use your feature?
  yes, the structure of the unit tests are decided and documented in the development procedure. SKILLS directory is updated with templates for new agents to contribute.

• Is there sufficient visual media?
  N/A

Versioning

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

[Copilot]

Pull request overview

Adds optional OptiTrack/NatNet motion-capture support to the robot perception stack and expands the testing infrastructure by separating fast unit tests from Docker-based system tests, plus adding CI/build tooling and Docker profile improvements.

Changes:

• Introduces natnet_ros2 (C++ NatNet node + Python vision pose converter), launch/config files, and an SDK download/install helper with licensing gate.
• Restructures pytest into tests/system/ + tests/robot/ proxy-based unit tests (@pytest.mark.unit), adds a YAML-driven colcon test manifest, and adds/updates system tests.
• Updates robot Docker build/profile plumbing (ROS_DISTRO args, Jetson L4T stack-base image, compose wiring) and documentation/nav to reflect the new workflows.

Reviewed changes

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

Show a summary per file

File	Description
tests/system/test_takeoff_hover_land.py	New PX4 takeoff/hover/land system test chain with per-velocity ordering and metrics capture
tests/system/test_sensors.py	Fixes import path for liveliness helpers after moving into tests/system/
tests/system/test_liveliness.py	Updates docstring reference to new sensors module path
tests/system/test_build_packages.py	Adds colcon test step gated by YAML package list
tests/system/test_build_docker.py	New system test for building images and recording image sizes
tests/system/init.py	Makes tests/system importable as a package (dotted module names)
tests/sim/README.md	Adds structure guidance for sim-side unit tests
tests/sim/motive_emulator/README.md	Plans for a future NatNet wire-protocol emulator integration test
tests/sensor_probes.py	Updates docs and fixes LiDAR validation script path
tests/robot/sensors/README.md	Documents unit test proxy layout for sensors layer
tests/robot/sensors/lidar_point_cloud_filter/test_validation_core.py	Pytest proxy that re-exports package-local LiDAR unit tests
tests/robot/README.md	Documents the co-location + proxy testing pattern
tests/robot/perception/README.md	Documents unit test proxy layout for perception layer
tests/robot/perception/natnet_ros2/test_natnet_ros2.py	Pytest proxy that re-exports natnet_ros2 Python unit tests
tests/robot/local/README.md	Documents unit test proxy layout for local layer
tests/robot/interface/README.md	Documents unit test proxy layout for interface layer
tests/robot/global/README.md	Documents unit test proxy layout for global layer
tests/robot/behavior/README.md	Documents unit test proxy layout for behavior layer
tests/requirements.txt	Adds PyYAML and NumPy for new test/config parsing
tests/README.md	Major update: clarifies system vs unit tests, paths, marks, and examples
tests/pytest.ini	Registers unit mark and keeps test discovery rooted at tests/
tests/parse_metrics.py	Updates node-id parsing to support system.* module prefix
tests/conftest.py	Adds YAML-driven colcon test config + module ordering (unit before system)
tests/colcon_unit_test_packages.yaml	New manifest of ROS packages gated by colcon test in CI
robot/ros_ws/src/sensors/sensor_interfaces/package.xml	Reorders/adjusts dependency entries (adds sensor_msgs)
robot/ros_ws/src/sensors/lidar_point_cloud_filter/test/test_validation_core.py	Adds NumPy-only unit tests for LiDAR validation rules
robot/ros_ws/src/sensors/lidar_point_cloud_filter/setup.py	Switches from tests_require to extras_require['test']
robot/ros_ws/src/sensors/lidar_point_cloud_filter/setup.cfg	Adds pytest configuration for package-local test discovery
robot/ros_ws/src/sensors/lidar_point_cloud_filter/scripts/validate_lidar_filter_clouds.py	Refactors validation into reusable pure-numeric helper module
robot/ros_ws/src/sensors/lidar_point_cloud_filter/README.md	Updates docs for new test layout and validation_core usage
robot/ros_ws/src/sensors/lidar_point_cloud_filter/lidar_point_cloud_filter/validation_core.py	New pure-numeric validation helpers (no ROS imports)
robot/ros_ws/src/perception/perception_bringup/launch/perception.launch.xml	Adds launch_natnet arg and conditional include for NatNet launch
robot/ros_ws/src/perception/natnet_ros2/test/test_natnet_ros2.py	Adds ROS-stubbed Python unit tests for vision pose converter logic
robot/ros_ws/src/perception/natnet_ros2/test/test_natnet_logic.cpp	Adds extensive gtests for SDK-independent NatNet logic and seams
robot/ros_ws/src/perception/natnet_ros2/test/fake_natnet_client.hpp	Adds in-process NatNet client test double for unit tests
robot/ros_ws/src/perception/natnet_ros2/src/vision_pose_converter_node.py	Adds Python node bridging pose_cov to MAVROS vision_pose topics
robot/ros_ws/src/perception/natnet_ros2/src/natnet_ros2_node.cpp	Adds C++ NatNet SDK ROS2 node publishing Pose/PoseWithCovariance
robot/ros_ws/src/perception/natnet_ros2/src/natnet_client_adapter.cpp	Adds SDK adapter implementation (only TU including NatNet headers)
robot/ros_ws/src/perception/natnet_ros2/scripts/download-natnet-sdk.sh	Adds SDK downloader/installer with explicit license acceptance flow
robot/ros_ws/src/perception/natnet_ros2/README.md	Adds module docs (usage, topics, config, troubleshooting)
robot/ros_ws/src/perception/natnet_ros2/package.xml	New ROS 2 package manifest for natnet_ros2
robot/ros_ws/src/perception/natnet_ros2/launch/vision_pose_converter.launch.xml	Launches converter node with remaps to NatNet + MAVROS topics
robot/ros_ws/src/perception/natnet_ros2/launch/natnet_ros2.launch.py	Launches NatNet node; conditionally includes MAVROS bridge
robot/ros_ws/src/perception/natnet_ros2/include/natnet_ros2/natnet_logic.hpp	New pure-logic header (covariance, topic names, negotiation seam)
robot/ros_ws/src/perception/natnet_ros2/include/natnet_ros2/natnet_client_adapter.hpp	Declares NatNet SDK adapter without including SDK headers
robot/ros_ws/src/perception/natnet_ros2/env-hooks/natnet_library_path.dsv.in	Adds LD_LIBRARY_PATH hook for NatNet shared library
robot/ros_ws/src/perception/natnet_ros2/config/vision_pose_converter.yaml	Params for converter (frame IDs, canonical quaternion)
robot/ros_ws/src/perception/natnet_ros2/config/natnet_config.yaml	ROS 2 parameter file for NatNet connection/body/publish settings
robot/ros_ws/src/perception/natnet_ros2/CMakeLists.txt	Conditional build/install based on SDK presence; installs hooks/tests
robot/ros_ws/src/perception/natnet_ros2/.gitignore	Ignores proprietary SDK artifacts in-tree
robot/ros_ws/src/local/controls/pid_controller_msgs/package.xml	Reorders interface package metadata entry
robot/docker/zed/Dockerfile.zed-l4t	Fixes GeoGraphicLib dev package name for L4T build
robot/docker/Dockerfile.robot	Adds ROS_DISTRO/PYTHON_VERSION args and refactors ROS package installs
robot/docker/Dockerfile.l4t-stack-base	New Jetson intermediary base image to normalize ROS/Python environment
robot/docker/docker-compose.yaml	Wires LAUNCH_NATNET env; adds L4T stack-base service and build args
mkdocs.yml	Adds testing docs sections and NatNet module doc link
docs/development/intermediate/testing/unit_testing.md	New guide for unit-test proxy pattern and CI workflow
docs/development/intermediate/testing/index.md	Updates testing overview (unit vs package vs system)
docs/development/docker-build-profiles.md	New doc on docker build profiles/build-args
common/ros_packages/msgs/task_msgs/package.xml	Reorders interface package metadata entry
common/ros_packages/msgs/airstack_msgs/package.xml	Reorders interface package metadata entry
airstack.sh	Adds --no-natnet setup option; ensures robot-l4t stack-base prebuild
AGENTS.md	Updates test documentation and adds new agent skill references
.env	Bumps version to 0.19.0-alpha.1
.agents/skills/run-system-tests/SKILL.md	Updates skill docs for new tests/system/ structure and unit tests
.agents/skills/docker-build-profiles/SKILL.md	Adds build-profile guidance and validation snippets
.agents/skills/configure-multi-robot/SKILL.md	Updates references to new system test path
.agents/skills/add-unit-tests/SKILL.md	New skill documenting co-location + proxy unit test workflow
.agents/skills/add-ros2-package/assets/package_template/setup.py	Updates template to use extras_require['test']

Comments suppressed due to low confidence (1)

robot/ros_ws/src/perception/natnet_ros2/src/natnet_client_adapter.cpp:169

• set_frame_callback() stores the callback in tl_frame_cb and registers sdk_frame_callback with a null context. If multiple adapters are constructed or the callback fires on a different thread, this can dispatch to the wrong callback or none at all. Prefer registering sdk_frame_callback with this as the context and dispatching to a member-stored callback via that context pointer.

---

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

[krrishj18]

Tested locally, everything seems to work.

[krrishj18]

When I view the docs i can't find a link to this file unless i search for it. Either link it from an existing page under development of have it as an option in the right side menu

[JohnYanxinLiu]

Moved, added to yaml, adjusted location of the doc.

[krrishj18]

Do we want natnet to be installed by default?

[JohnYanxinLiu]

Thanks! We want NatNet to not be installed by default. Should be opt-in for users. Fixed.

[krrishj18]

Inline with my other comment, we should decide if it's enabled by default and accordingly edit this sentence

[JohnYanxinLiu]

Resolved with previous comment setting skipping SDK installation to true by default.

[krrishj18]

I think it'll be helpful if we have a link to slite on setting up/calibrating the optitrack system at the RIC.

I don't think it make sense for that to be part of airstack docs cause it's not relevant to non-CMU people but it'll be helpful to link it here for lab members so they have quick access. You can probably link it to slite and have your PPT in Slite or some other documentation/links.

[JohnYanxinLiu]

I don't think the link to the slite or slides should be added to AirStack docs in that case. I think AirStack should be strictly kept general for everyone. We can keep CMU/RIC-specific things internally.

I compromised at linking the official Optitrack documentation on calibrating the room.

[krrishj18]

Not something you did but i just looked and this state_estimation.md file doesn't exist. Can you just delete this line as part of docs clean up? Thanks