diff --git a/.agents/skills/add-ros2-package/assets/package_template/setup.py b/.agents/skills/add-ros2-package/assets/package_template/setup.py index 4056e5d5f..3982cc356 100644 --- a/.agents/skills/add-ros2-package/assets/package_template/setup.py +++ b/.agents/skills/add-ros2-package/assets/package_template/setup.py @@ -26,7 +26,9 @@ maintainer_email='your.email@example.com', # TODO: Update description='Brief description of your module', # TODO: Update license='Apache-2.0', - tests_require=['pytest'], + extras_require={ + 'test': ['pytest'], + }, entry_points={ 'console_scripts': [ # TODO: Add your node executables here diff --git a/.agents/skills/add-unit-tests/SKILL.md b/.agents/skills/add-unit-tests/SKILL.md new file mode 100644 index 000000000..7d1d3b582 --- /dev/null +++ b/.agents/skills/add-unit-tests/SKILL.md @@ -0,0 +1,309 @@ +--- +name: add-unit-tests +description: Add Python or C++ unit tests to an AirStack ROS 2 package. Covers the co-location pattern (test source in package/test/), the thin proxy that makes tests discoverable by pytest tests/ and airstack test -m unit, and how to extend the pattern to sim and GCS modules. +license: MIT +metadata: + author: AirLab CMU + repository: AirStack +--- + +# Skill: Add Unit Tests to an AirStack Module + +## When to Use + +Use this skill when: + +- Adding Python unit tests for a ROS 2 package (perception, sensors, local, global, behavior, interface) +- Adding C++ unit tests (`gtest`) to a package already using `ament_cmake` +- Extending unit tests to sim-side Python (`tests/sim/`) or GCS modules (`tests/gcs/`) +- Verifying that `airstack test -m unit` and `pytest tests/` (CI) pick up your new tests + +For system tests (full Docker stack, sim, sensors, takeoff/hover/land) see the +`run-system-tests` skill instead. + +## Architecture Overview + +Unit tests follow a **co-location + proxy** pattern: + +``` +robot/ros_ws/src/// +├── src/ # production source (Python or C++) +├── test/ +│ ├── test_.py # ← unit test SOURCE (canonical location) +│ ├── test_.cpp # ← C++ gtest SOURCE (optional) +│ └── fake_.hpp # ← C++ test doubles (optional) +└── CMakeLists.txt # wires ament_add_gtest under BUILD_TESTING + +tests/robot/// +└── test_.py # ← thin PROXY (re-exports tests from above) +``` + +The **proxy** is a one-file shim that loads the real test module with `importlib` +and re-exports every `test_*` function. This means: + +| Invocation | What runs | +|---|---| +| `pytest tests/ -m unit` | Proxy in `tests/robot/` → loads real test from package | +| `airstack test -m unit` | Same path | +| CI `system-tests.yml` (PR open / approved) | Same path via `pytest tests/` | +| `colcon test --packages-select ` | Real test in `package/test/` directly | + +## Step-by-Step: Adding a Python Unit Test + +### 1. Identify pure-Python logic to test + +Good candidates are functions/classes with **no ROS or hardware dependencies**: +- Pure math / geometry helpers +- Protocol parsers +- Data-structure converters +- Any function that takes plain Python types and returns plain Python types + +If the code imports ROS types, stub them out at the import boundary +(see `test_natnet_ros2.py` for the `sys.modules` stub pattern). + +### 2. Write the test source in the package + +Create `robot/ros_ws/src///test/test_.py`: + +```python +# Copyright (c) 2024 Carnegie Mellon University +# MIT License - see LICENSE in the repository root for full text. +"""Unit tests for — .""" + +import sys +from pathlib import Path +import pytest + +# Add the package src/ dir so the production module is importable +# without colcon installing the package first. +_src = Path(__file__).resolve().parent.parent / "src" +if str(_src) not in sys.path: + sys.path.insert(0, str(_src)) + +from my_module import my_function # noqa: E402 + + +@pytest.mark.unit +def test_my_function_basic(): + assert my_function(1, 2) == 3 +``` + +**Key points:** +- Always decorate with `@pytest.mark.unit` — this is the filter for fast runs. +- Compute paths relative to `__file__` (`parent.parent / "src"`) — never hardcode + absolute paths. +- For packages with a Python module directory (`//`), add the package + root (`parent.parent`) to `sys.path` and import as + `from . import ...`. +- If the code uses ROS types, stub `sys.modules` before importing: + +```python +import sys +from unittest.mock import MagicMock + +sys.modules.setdefault("rclpy", MagicMock()) +sys.modules.setdefault("rclpy.node", MagicMock()) +sys.modules.setdefault("geometry_msgs", MagicMock()) +sys.modules.setdefault("geometry_msgs.msg", MagicMock()) +# ... then import your module +``` + +For `rclpy.node.Node` subclasses use a real dummy base class instead of a +`MagicMock()` to ensure `__init_subclass__` fires and method bodies are defined +(see `test_natnet_ros2.py` for the full pattern). + +### 3. Write the thin proxy in tests/robot/ + +Create `tests/robot///test_.py`: + +```python +# Copyright (c) 2024 Carnegie Mellon University +# MIT License - see LICENSE in the repository root for full text. +"""Proxy: re-exposes unit tests from the package source tree. + +Unit test logic lives co-located with the package source (ROS 2 / colcon convention): + robot/ros_ws/src///test/test_.py + +This file makes those tests discoverable by ``pytest tests/`` (CI) and +``airstack test -m unit`` without any changes to the CI workflow. +""" +import importlib.util +import sys +from pathlib import Path + +_repo_root = Path(__file__).resolve().parents[N] # adjust N so this resolves to repo root +_pkg_test = _repo_root / "robot/ros_ws/src///test" +_real_file = _pkg_test / "test_.py" + +# If the test imports from a package module, ensure the package root is on sys.path. +# Example: _pkg_root = _pkg_test.parent; sys.path.insert(0, str(_pkg_root)) + +# Load the real module under a unique name to avoid the circular import that +# would occur if we used `from test_ import *` (this file has the same +# name, and pytest adds its directory to sys.path at collection time). +_spec = importlib.util.spec_from_file_location("__unit_tests", _real_file) +_real = importlib.util.module_from_spec(_spec) +_spec.loader.exec_module(_real) + +# Re-export every test_* symbol so pytest collects them from this proxy. +for _name in dir(_real): + if _name.startswith("test_"): + globals()[_name] = getattr(_real, _name) +``` + +**Counting `parents[N]` to reach the repo root:** + +| Proxy location | `parents[N]` for repo root | +|---|---| +| `tests/robot///` | `parents[4]` | +| `tests/sim//` | `parents[3]` | +| `tests/gcs//` | `parents[3]` | + +### 4. Ensure the tests/ directory structure exists + +```bash +mkdir -p tests/robot// +touch tests/robot///__init__.py # only if needed for conftest path discovery +``` + +The READMEs in `tests/robot/behavior/`, `tests/robot/global/`, etc. describe the +purpose of each layer mirror. Update the layer README when you add a new package. + +### 5. Run locally to verify + +```bash +# From repo root — no container needed +cd tests +pytest -m unit -v +# or +airstack test -m unit -v +``` + +All 14+ existing tests plus your new ones should pass. The proxy output shows: +``` +robot///test_.py::test_my_function_basic + <- ../robot/ros_ws/src///test/test_.py PASSED +``` + +### 6. CI picks it up automatically + +Unit tests are discovered by `pytest tests/` and run as part of `system-tests.yml` +(triggered on PR open) — no changes to CI needed. + +--- + +## Step-by-Step: Adding a C++ gtest + +C++ tests don't use the proxy pattern — they live entirely within the package and +run exclusively via `colcon test`. + +### 1. Write the test in `package/test/` + +```cpp +// Copyright (c) 2024 Carnegie Mellon University +// MIT License - see LICENSE in the repository root for full text. +#include +#include "my_package/my_header.hpp" + +TEST(MyGroup, BasicCase) { + EXPECT_EQ(my_function(1, 2), 3); +} +``` + +### 2. Wire `ament_add_gtest` in `CMakeLists.txt` + +```cmake +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() + + find_package(ament_cmake_gtest REQUIRED) + ament_add_gtest(test_my_name test/test_my_name.cpp) + target_include_directories(test_my_name PRIVATE + $ + $) + # Link any production library targets here if needed: + # target_link_libraries(test_my_name my_lib) +endif() +``` + +### 3. Add test depend in `package.xml` + +```xml +ament_cmake_gtest +``` + +### 4. Build and run + +```bash +# Inside the robot container: +docker exec airstack-robot-desktop-1 bash -c \ + "bws --cmake-args '-DBUILD_TESTING=ON' --packages-select " +docker exec airstack-robot-desktop-1 bash -c \ + "colcon test --packages-select --event-handlers console_direct+" +docker exec airstack-robot-desktop-1 bash -c \ + "colcon test-result --all" +``` + +The `build_packages` system test in CI (`tests/system/test_build_packages.py`) also +runs `colcon test` with `BUILD_TESTING=ON` for the robot container. Packages gated +there are listed in [`tests/colcon_unit_test_packages.yaml`](../../../tests/colcon_unit_test_packages.yaml) +— add your package under `robot.packages` when it has gtests or pytest tests in +`package/test/`. + +--- + +## Extending to sim and GCS + +The same proxy pattern applies verbatim: + +**Sim-side Python** (e.g. motive emulator protocol logic): +``` +simulation/...//test/test_.py ← source +tests/sim//test_.py ← proxy (parents[3] = repo root) +``` + +**GCS modules**: +``` +gcs/...//test/test_.py ← source +tests/gcs//test_.py ← proxy (parents[3] = repo root) +``` + +`pytest tests/ -m unit` discovers them through the proxy without any +pytest.ini or CI changes needed. + +--- + +## Pattern Summary + +| Concern | Answer | +|---|---| +| Where does test source live? | `/…//test/` (co-located with the package) | +| Where does pytest discover tests? | `tests/robot/` (or `tests/sim/`, `tests/gcs/`) via thin proxy | +| How does the proxy avoid circular import? | `importlib.util.spec_from_file_location` with a unique module name | +| What mark do all unit tests use? | `@pytest.mark.unit` | +| What CI workflow runs them? | `system-tests.yml` — runs `pytest tests/` which includes unit tests | +| When does that workflow trigger? | PR opened, `/pytest` comment, `workflow_dispatch` | +| Do system tests (`liveliness`, etc.) run too? | No — `-m unit` filters to hermetic tests only | +| Does `colcon test` also run these? | Yes — Python tests in `package/test/` are discovered by colcon's pytest runner | +| Can I add pure C++ gtests? | Yes — `ament_add_gtest` in CMakeLists.txt, no proxy needed | + +## Reference Implementations + +| Package | Python test | What it covers | +|---|---|---| +| `natnet_ros2` | `robot/ros_ws/src/perception/natnet_ros2/test/test_natnet_ros2.py` | `VisionPoseConverterNode._canonical_quaternion` (ROS-stubbed) | +| `natnet_ros2` (C++) | `robot/ros_ws/src/perception/natnet_ros2/test/test_natnet_logic.cpp` | `build_covariance_6x6`, `negotiate()`, `INatNetClient` seam | +| `lidar_point_cloud_filter` | `robot/ros_ws/src/sensors/lidar_point_cloud_filter/test/test_validation_core.py` | Pure-numpy range validation rules | + +Corresponding proxies: `tests/robot/perception/natnet_ros2/test_natnet_ros2.py`, +`tests/robot/sensors/lidar_point_cloud_filter/test_validation_core.py`. + +## Files to Know + +- `.github/workflows/system-tests.yml` — CI workflow (runs `pytest tests/` including unit tests) +- `tests/pytest.ini` — mark registration (`unit`, `build_docker`, etc.) +- `tests/robot/` — proxy layer mirroring `robot/ros_ws/src/` +- `tests/sim/` — proxy layer for sim-side code (future) +- `tests/gcs/` — proxy layer for GCS code (future) +- `tests/README.md` — full test harness reference diff --git a/.agents/skills/configure-multi-robot/SKILL.md b/.agents/skills/configure-multi-robot/SKILL.md index 4fc977472..d147fdcf2 100644 --- a/.agents/skills/configure-multi-robot/SKILL.md +++ b/.agents/skills/configure-multi-robot/SKILL.md @@ -242,7 +242,7 @@ env_overrides = { } ``` -Tests that act on robots iterate `n=1..num_robots` and address them as `/robot_{n}/...` directly (see `_takeoff_one_robot` in `tests/test_takeoff_hover_land.py`). The test sets `ROS_DOMAIN_ID=n` for each per-robot subprocess (`domain_id=n` in `ros2_exec(...)`), matching what the resolver assigned inside the container. **If you write a new test that talks to a robot, follow this same `domain_id=n` + `/robot_{n}/...` pattern.** +Tests that act on robots iterate `n=1..num_robots` and address them as `/robot_{n}/...` directly (see `_takeoff_one_robot` in `tests/system/test_takeoff_hover_land.py`). The test sets `ROS_DOMAIN_ID=n` for each per-robot subprocess (`domain_id=n` in `ros2_exec(...)`), matching what the resolver assigned inside the container. **If you write a new test that talks to a robot, follow this same `domain_id=n` + `/robot_{n}/...` pattern.** CLI passthrough: diff --git a/.agents/skills/docker-build-profiles/SKILL.md b/.agents/skills/docker-build-profiles/SKILL.md new file mode 100644 index 000000000..64b579a01 --- /dev/null +++ b/.agents/skills/docker-build-profiles/SKILL.md @@ -0,0 +1,134 @@ +# docker-build-profiles SKILL + +Summary +- Purpose: Provide actionable build-time validation snippets and YAML guidance for AirStack Docker builds. Designed for Claude/GPT-style agents that automate repo changes, CI checks, or PR review suggestions. +- Location: .agents/skills/docker-build-profiles/SKILL.md + +When to use +- When adding or updating a `docker-compose` profile that passes `PYTHON_VERSION`, `ROS_DISTRO`, or other numeric-like build args. +- When an automated agent needs to verify a new profile will produce a correct `PYTHONPATH` and avoid YAML float-parsing bugs. + +Actions the agent can perform +1. Validate `docker-compose.yaml` args are quoted when numeric-like (e.g. `PYTHON_VERSION: "3.10"`). +2. Insert a build-time validation `RUN` into `robot/docker/Dockerfile.robot` to fail early when the ROS Python path does not exist. +3. Add or update a short test in documentation showing how to build the `builder` stage and check `ament_package` import. +4. Suggest `network: host` under `build:` for L4T/Jetson profiles only when necessary (kernel iptables workarounds). + +Snippets (copyable) + +- YAML-check rule (agent pseudocode): + + - If a `build.args` key named `PYTHON_VERSION` exists and the value matches `/^\d+\.\d+$/`, ensure it's a quoted string in YAML; otherwise update to `""`. + +- Dockerfile validation snippet (recommended, place before using `PYTHON_VERSION` to compose `PYTHONPATH`): + +```dockerfile +RUN test -d /opt/ros/${ROS_DISTRO}/lib/python${PYTHON_VERSION} \ + || (echo "Invalid PYTHON_VERSION=${PYTHON_VERSION} or missing ROS python path" && exit 1) +``` + +- Quick builder-stage test commands (agent can run or instruct user to run): + +```bash +DOCKER_BUILDKIT=1 docker build --target builder \ + -f robot/docker/Dockerfile.robot \ + --build-arg BASE_IMAGE= \ + --build-arg ROS_DISTRO= \ + --build-arg PYTHON_VERSION="" \ + -t airstack-builder-test:local robot/docker + +docker run --rm -it airstack-builder-test:local bash -c "python3 -c 'import ament_package; print(ament_package.__file__)'" +``` + +Guidance for agents when editing the repo +- Prefer making minimal, reversible changes: add the `RUN test -d ...` check early in the Dockerfile and gate it with informative message text. +- When updating `docker-compose.yaml`, only quote the numeric-like values; do not change unrelated fields. +- If creating PRs, include a short note in the PR description instructing maintainers to run the builder-stage sanity build on both an amd64 desktop profile and an arm64 L4T profile. + +Troubleshooting notes +- YAML quirk: unquoted `3.10` may be parsed as float `3.1` — this changes path strings and breaks imports (e.g., `python3.1` instead of `python3.10`). +- Jetson/L4T builds may require `network: host` during the build to avoid kernel iptables/raw table missing-module errors. +- Jetson **`robot-l4t`** builds from **`robot-l4t-stack-base`** (`robot/docker/Dockerfile.l4t-stack-base`), not raw dustynv, so **`Dockerfile.robot` stays Ubuntu-shaped.** `airstack image-build --profile l4t robot-l4t` triggers **`robot-l4t-stack-base`** first (`airstack.sh`); bare `compose build robot-l4t` can still parallelize badly, so list stack-base explicitly if not using AirStack CLI. + +Examples of agent prompts +- "Check `robot/docker/docker-compose.yaml` for `PYTHON_VERSION` entries and quote any unquoted numeric values; open a PR with the fixes and include a test log from a builder-stage build." +- "Insert a build-time validation `RUN` in `robot/docker/Dockerfile.robot` that ensures `/opt/ros/${ROS_DISTRO}/lib/python${PYTHON_VERSION}` exists; push as a separate small commit." + +Notes +- This SKILL is intended for agent workflows (automated PRs, repo fixes, review suggestions). Keep changes explicit and reversible. +- For human-facing docs, maintain a high-level page in `docs/` that links to this SKILL for actionable snippets and agent tasks. + +SKILL vs human docs + +- Keep SKILLs low-level and exact: this file contains raw `docker` commands and copyable build-time snippets intended for agents and automation. +- Keep human-facing docs (`docs/`) showing the `airstack` CLI equivalents and higher-level workflows. This reduces cognitive load for maintainers while preserving exact commands in SKILLs for automation and debugging. +- For the robot profile, human docs should prefer `airstack image-build --target builder --progress=plain ` when showing how to inspect build output. + +Creating a new profile (step-by-step) + +This section shows the minimal, recommended steps an agent or maintainer should perform to add a new `docker-compose` profile that builds from `Dockerfile.robot`. + +1. Pick a sensible service name and base image + + - Choose a service name that clearly indicates the platform, e.g. `robot-desktop`, `robot-l4t`, or `robot-myboard`. + - Select an appropriate `BASE_IMAGE` (amd64 desktop base or `nvcr.io/nvidia/l4t-jetpack:...` for Jetson). + +2. Add the profile with quoted numeric args + + - Add a service block in `robot/docker/docker-compose.yaml` (or an override file) and set `build.args` for the profile. + - Always quote `PYTHON_VERSION` values (e.g. `"3.10"`) so YAML does not convert them to floats. + + Example snippet to add: + + ```yaml + robot-myboard: + build: + context: ./robot/docker + dockerfile: ./Dockerfile.robot + args: + BASE_IMAGE: nvcr.io/nvidia/l4t-jetpack:r36.4.0 + ROS_DISTRO: humble + PYTHON_VERSION: "3.10" + REAL_ROBOT: true + SKIP_MACVO: true + # for L4T builds only when necessary + # network: host + ``` + +3. Add an optional validate-early check (recommended) + + - Insert the `RUN test -d /opt/ros/${ROS_DISTRO}/lib/python${PYTHON_VERSION}` check near the top of `Dockerfile.robot` (before `ENV PYTHONPATH` or any Python-dependent operations). This ensures the build fails fast with a clear message. + +4. Run the builder-stage sanity build + + - Run the builder-target build locally (or in CI) to verify the image picks up the correct Python/ROS paths and that `ament_package` imports: + + ```bash + DOCKER_BUILDKIT=1 docker build --target builder \ + -f robot/docker/Dockerfile.robot \ + --build-arg BASE_IMAGE=nvcr.io/nvidia/l4t-jetpack:r36.4.0 \ + --build-arg ROS_DISTRO=humble \ + --build-arg PYTHON_VERSION="3.10" \ + -t airstack-builder-test:local robot/docker + + docker run --rm airstack-builder-test:local python3 -c "import ament_package; print('ok', ament_package.__file__)" + ``` + +5. Smoke-run the full compose build (optional but recommended) + + - Use `docker compose -f robot/docker/docker-compose.yaml build robot-myboard` to ensure compose passes the args correctly. + +6. Prepare the PR with clear validation notes + + - Make the code change small and focused (one commit to `docker-compose.yaml`, one optional commit for the `Dockerfile` validation line). + - In the PR description include the builder-stage test command output and request a reviewer to run the builder-stage test on both an amd64 and arm64 profile if possible. + +7. Merge and monitor + + - After merge, ensure CI (if configured) runs the sanity build or that maintainers run the checks on the target hardware. + +Agent implementation tips + +- When automating the change, produce a single commit that updates only the new service block and, if needed, a second commit that adds the `RUN` check to `Dockerfile.robot`. +- If the target is Jetson/L4T, add `network: host` under `build:` only when prior builds show iptables/kernel errors; do not enable it by default. +- If you detect a pre-existing unquoted `PYTHON_VERSION` in the repo, prefer to update that entry in-place and include an explanatory commit message about YAML float parsing. diff --git a/.agents/skills/run-system-tests/SKILL.md b/.agents/skills/run-system-tests/SKILL.md index 868d8c695..453bbc953 100644 --- a/.agents/skills/run-system-tests/SKILL.md +++ b/.agents/skills/run-system-tests/SKILL.md @@ -22,15 +22,39 @@ This skill is about the **test harness itself** — pytest marks, fixtures, the ## Test Suite Overview -The suite lives at `tests/` (repo root) and is fully pytest-based. Configuration is in `tests/pytest.ini` and shared infrastructure in `tests/conftest.py`. Marks include `build_docker`, `build_packages`, `liveliness`, `sensors`, and `takeoff_hover_land`: +The suite lives at `tests/` (repo root) and is fully pytest-based. Configuration is in `tests/pytest.ini` and shared infrastructure in `tests/conftest.py`. + +- **`tests/system/`** — Docker stack integration tests. Marks: `build_docker`, `build_packages`, `liveliness`, `sensors`, `takeoff_hover_land`. +- **`tests/robot/`** and **`tests/sim/`** — Hermetic **unit** tests (`@pytest.mark.unit`). These are **thin proxy files** that re-export tests from each ROS 2 package's own `test/` directory (co-located with the source, the ROS 2 / colcon convention). The proxy pattern keeps test source next to the code it tests while making tests discoverable by `pytest tests/`. + +### Unit tests vs system tests + +| Concern | Unit (`-m unit`) | System (`-m liveliness` etc.) | +|---|---|---| +| Hardware required | None — pure Python | Docker daemon, NVIDIA GPU, sim license | +| CI workflow | `system-tests.yml` (included in `pytest tests/`) | `system-tests.yml` (GPU OpenStack VM) | +| Trigger | Every push + PR (automatic) | PR opened, `/pytest` comment, `workflow_dispatch` | +| Source location | `/test/test_*.py` (proxied via `tests/robot/`) | `tests/system/` | +| How to add | See `add-unit-tests` skill | See *Adding a New System Test* below | + +Run unit tests without any Docker stack: + +```bash +airstack test -m unit -v +# or +pytest tests/ -m unit -v # AIRSTACK_ROOT=$(pwd) for direct pytest +``` + +For details on the proxy pattern and adding new unit tests, see the +`add-unit-tests` skill. | File | Mark | What it tests | Hardware required | |------|------|---------------|-------------------| -| `tests/test_build_docker.py` | `build_docker` | `airstack image-build` for `robot-desktop`, `gcs`, `isaac-sim`, `ms-airsim`; records image size to `metrics.json` | Docker daemon | -| `tests/test_build_packages.py` | `build_packages` | `colcon build` (`bws`) inside the robot, GCS, and ms-airsim ROS workspaces — brought up with `AUTOLAUNCH=false` | Docker daemon | -| `tests/test_liveliness.py` | `liveliness` | Stack bring-up: containers Running, `/clock` readiness, tmux panes, sentinel ROS 2 nodes, compute, infra-only `test_stable` | Docker daemon, NVIDIA GPU + `nvidia-container-toolkit`, sim license / Omniverse creds | -| `tests/test_sensors.py` | `sensors` | Topic Hz (Isaac: batched on sim + robot; LiDAR `echo-once` + cloud sanity), RTF, `test_sensor_streams_stable` | Docker daemon, NVIDIA GPU + `nvidia-container-toolkit`, sim license / Omniverse creds | -| `tests/test_takeoff_hover_land.py` | `takeoff_hover_land` | 4-phase flight chain per `(sim, num_robots, iteration, velocity)`: `test_px4_ready` → `test_takeoff` → `test_hover` → `test_landing`. Records altitude error, overshoot, hover stability, landing accuracy, odometry drift | Docker daemon, NVIDIA GPU, sim license | +| `tests/system/test_build_docker.py` | `build_docker` | `airstack image-build` for `robot-desktop`, `gcs`, `isaac-sim`, `ms-airsim`; records image size to `metrics.json` | Docker daemon | +| `tests/system/test_build_packages.py` | `build_packages` | `colcon build` (`bws`) inside the robot, GCS, and ms-airsim ROS workspaces — brought up with `AUTOLAUNCH=false` | Docker daemon | +| `tests/system/test_liveliness.py` | `liveliness` | Stack bring-up: containers Running, `/clock` readiness, tmux panes, sentinel ROS 2 nodes, compute, infra-only `test_stable` | Docker daemon, NVIDIA GPU + `nvidia-container-toolkit`, sim license / Omniverse creds | +| `tests/system/test_sensors.py` | `sensors` | Topic Hz (Isaac: batched on sim + robot; LiDAR `echo-once` + cloud sanity), RTF, `test_sensor_streams_stable` | Docker daemon, NVIDIA GPU + `nvidia-container-toolkit`, sim license / Omniverse creds | +| `tests/system/test_takeoff_hover_land.py` | `takeoff_hover_land` | 4-phase flight chain per `(sim, num_robots, iteration, velocity)`: `test_px4_ready` → `test_takeoff` → `test_hover` → `test_landing`. Records altitude error, overshoot, hover stability, landing accuracy, odometry drift | Docker daemon, NVIDIA GPU, sim license | The marks are declared in `tests/pytest.ini`. **Do not invent new marks ad-hoc** — register any new mark there or pytest will warn about unknown marks. @@ -39,10 +63,10 @@ The marks are declared in `tests/pytest.ini`. **Do not invent new marks ad-hoc** `conftest.py` enforces a deterministic global order so cheap-and-fast-failing tests surface first: ``` -test_build_docker → test_build_packages → test_liveliness → test_sensors → test_takeoff_hover_land +system.test_build_docker → system.test_build_packages → system.test_liveliness → system.test_sensors → system.test_takeoff_hover_land ``` -Within `test_takeoff_hover_land`, items are re-sorted to `(airstack_env, velocity, phase)` so each `(sim, robots, iter)` env brings the stack up once and the drone goes ground → air → ground per velocity before pytest moves to the next velocity. +Within `system.test_takeoff_hover_land`, items are re-sorted to `(airstack_env, velocity, phase)` so each `(sim, robots, iter)` env brings the stack up once and the drone goes ground → air → ground per velocity before pytest moves to the next velocity. ### Isaac Sim (`sensors`): why Hz is batched and LiDAR uses `echo --once` @@ -134,10 +158,10 @@ The `airstack_env` fixture is parametrized over `(sim, num_robots, iteration)` t | `--sim` | `msairsim,isaacsim` | `airstack_env` | One env-tuple per sim | | `--num-robots` | `1,3` | `airstack_env` | Cross-product with sim | | `--stress-iterations` | `1` | `airstack_env` | Up/down cycles per `(sim, num_robots)` | -| `--stable-duration` | `120` | `test_liveliness::test_stable` and `test_sensors::test_sensor_streams_stable` | Total seconds polled | -| `--stable-interval` | `10` | `test_liveliness::test_stable` and `test_sensors::test_sensor_streams_stable` | Seconds between polls | +| `--stable-duration` | `120` | `system.test_liveliness::test_stable` and `system.test_sensors::test_sensor_streams_stable` | Total seconds polled | +| `--stable-interval` | `10` | `system.test_liveliness::test_stable` and `system.test_sensors::test_sensor_streams_stable` | Seconds between polls | | `--gui` | off (headless) | `airstack_env` | Sets `QT_QPA_PLATFORM=offscreen` when off | -| `--takeoff-velocities` | `0.5` (current default) | `test_takeoff_hover_land` | One full 4-phase chain per velocity | +| `--takeoff-velocities` | `0.5` (current default) | `system.test_takeoff_hover_land` | One full 4-phase chain per velocity | Total parametrize cardinality for sim tests = `len(sims) × len(num_robots) × stress_iterations × len(velocities for takeoff)`. Keep this small locally — a 2×2×3×3 sweep on a workstation is several hours. @@ -198,10 +222,10 @@ tests/results/2025-04-21_14-30-00/ ├── results.xml # JUnit XML — durations + pass/fail per test ├── metrics.json # Custom metrics keyed by test_node_id → metric_key └── logs/ - ├── test_build_docker.TestDockerBuilds.test_build_robot_desktop.log - ├── test_sensors.TestSensors.test_sensor_streams_stable[msairsim-rob#1-iter0].log - ├── test_liveliness.TestLiveliness.test_stable[msairsim-rob#1-iter0].log - ├── airstack_env.test_liveliness.TestLiveliness.test_robot_containers_running[...].log + ├── system.test_build_docker.TestDockerBuilds.test_build_robot_desktop.log + ├── system.test_sensors.TestSensors.test_sensor_streams_stable[msairsim-rob#1-iter0].log + ├── system.test_liveliness.TestLiveliness.test_stable[msairsim-rob#1-iter0].log + ├── airstack_env.system.test_liveliness.TestLiveliness.test_robot_containers_running[...].log └── ... ``` @@ -211,7 +235,7 @@ tests/results/2025-04-21_14-30-00/ ```json { - "test_liveliness.TestLiveliness.test_stable[msairsim-rob#1-iter0]": { + "system.test_liveliness.TestLiveliness.test_stable[msairsim-rob#1-iter0]": { "airstack_up_duration_s": {"value": 42.7, "unit": "s", "direction": "lower_is_better"}, "robot.sensors.front_stereo.left.image_rect.hz_samples": { "samples": [{"t": 10, "value": 19.27}, {"t": 20, "value": 19.31}, ...] @@ -266,7 +290,7 @@ If your test... ### 2. File location and naming -- File: `tests/test_.py` — matches pytest's default test discovery (`test_*.py`) +- File: `tests/system/test_.py` — matches pytest's default test discovery (`test_*.py`) under the system suite - Class: `Test` with the mark applied at the class level: `@pytest.mark.` - Add a class-level `@pytest.mark.timeout()` — long-running sim tests need it - Imports: pull helpers from `conftest` directly (`from conftest import ...`); `tests/` is on `sys.path` because `testpaths = .` in pytest.ini @@ -274,7 +298,7 @@ If your test... ### 3. Decide if you need `airstack_env` - **Need full stack up (sim + robot + GCS)?** Take `airstack_env` as a fixture argument. You'll automatically be parametrized over `(sim, num_robots, iteration)` from CLI flags — `pytest_generate_tests` in conftest activates this only for tests that name the fixture. -- **Just need one container or no containers?** Don't take `airstack_env` — bring up only what you need with `airstack_cmd("up", "", env_overrides={"AUTOLAUNCH": "false"})` and tear down in a `try/finally`, the way `test_build_packages.py` does. +- **Just need one container or no containers?** Don't take `airstack_env` — bring up only what you need with `airstack_cmd("up", "", env_overrides={"AUTOLAUNCH": "false"})` and tear down in a `try/finally`, the way `tests/system/test_build_packages.py` does. - **Need extra parametrization** (e.g. velocity for `takeoff_hover_land`)? Add a module-level `pytest_generate_tests(metafunc)` in your test file. Don't put it in `conftest.py` unless it applies broadly. ### 4. Use the existing helpers diff --git a/.env b/.env index d16b8c649..5451be746 100644 --- a/.env +++ b/.env @@ -12,7 +12,7 @@ PROJECT_NAME="airstack" # If you've run ./airstack.sh setup, then this will auto-generate from the git commit hash every time a change is made # to a Dockerfile or docker-compose.yaml file. Otherwise this can also be set explicitly to make a release version. # auto-generated from git commit hash -VERSION="0.19.0-alpha.0" +VERSION="0.19.0-alpha.1" # Choose "dev" or "prebuilt". "dev" is for mounted code that must be built live. "prebuilt" is for built ros_ws baked into the image DOCKER_IMAGE_BUILD_MODE="dev" # Where to push and pull images from. Can replace with your docker hub username if using docker hub. diff --git a/AGENTS.md b/AGENTS.md index f9eed977c..b53069e75 100644 --- a/AGENTS.md +++ b/AGENTS.md @@ -93,6 +93,7 @@ For detailed step-by-step instructions, refer to the **`.agents/skills/`** direc | [debug-module](.agents/skills/debug-module) | Autonomous debugging of ROS 2 modules | | [update-documentation](.agents/skills/update-documentation) | Documenting new modules and updating mkdocs | | [test-in-simulation](.agents/skills/test-in-simulation) | End-to-end simulation testing of a module | +| [add-unit-tests](.agents/skills/add-unit-tests) | Adding Python or C++ unit tests to a ROS 2 package (co-location + proxy pattern, CI workflow, extending to sim/GCS) | | [run-system-tests](.agents/skills/run-system-tests) | Running the pytest system test harness (marks, MetricsRecorder, /pytest PR trigger) | | [add-behavior-tree-node](.agents/skills/add-behavior-tree-node) | Creating behavior tree nodes | | [use-airstack-cli](.agents/skills/use-airstack-cli) | Using the `airstack` CLI and the non-interactive `docker exec` pattern | @@ -195,30 +196,38 @@ docker exec airstack-robot-desktop-1 bash -c "ros2 topic echo --onc - Verify module behavior in isolation - Test with synthetic data - Located in module's `test/` directory + - **Run in the robot container** with `colcon test` (after `bws`), not via `airstack test -m unit`. The root [`tests/`](tests/) suite does **not** register a `unit` pytest mark; `airstack test -m ` only selects marks declared in [`tests/pytest.ini`](tests/pytest.ini) (`build_docker`, `build_packages`, `liveliness`, `sensors`, `takeoff_hover_land`). -2. **System Level:** Full simulation tests (Isaac Sim or Microsoft AirSim legacy) + ```bash + docker exec airstack-robot-desktop-1 bash -c "sws && colcon test --packages-select natnet_ros2 --event-handlers console_direct+" + ``` + +2. **Unit tests (`pytest`, `unit` mark):** Fast, hermetic checks. Test **source** lives co-located with each ROS 2 package in `/test/` (standard colcon convention). Thin **proxy** files in [`tests/robot/`](tests/robot/) and [`tests/sim/`](tests/sim/) re-export those tests so `pytest tests/` discovers them. Unit tests run as part of the `system-tests.yml` suite. Example: `airstack test -m unit -v`. See `add-unit-tests` skill. + +3. **System Level (`tests/system/`):** Full simulation tests (Isaac Sim or Microsoft AirSim legacy) - End-to-end autonomy stack testing - Real sensor simulation - Multi-robot scenarios - - Implemented in [`tests/`](tests/) — see below + - Pytest modules in [`tests/system/`](tests/system/) — see below -### System Test Suite (`tests/`) +### System Test Suite (`tests/system/`) -Pytest-based system tests live at the repo root in [`tests/`](tests/). They bring up the full Docker stack (sim + robot + GCS) and verify container health, ROS 2 node presence, compute usage, sensor topic streams (``sensors`` mark), and end-to-end flight behavior. +Pytest-based system tests live under [`tests/system/`](tests/system/). They bring up the full Docker stack (sim + robot + GCS) and verify container health, ROS 2 node presence, compute usage, sensor topic streams (``sensors`` mark), and end-to-end flight behavior. | File | Mark | What it tests | Hardware | |------|------|---------------|----------| -| [`tests/test_build_docker.py`](tests/test_build_docker.py) | `build_docker` | Docker image builds (robot-desktop, gcs, isaac-sim, ms-airsim) | Docker | -| [`tests/test_build_packages.py`](tests/test_build_packages.py) | `build_packages` | `colcon build` inside each container | Docker | -| [`tests/test_liveliness.py`](tests/test_liveliness.py) | `liveliness` | Stack bring-up: containers, ``/clock`` readiness, tmux, sentinel ROS 2 nodes, compute, infra-only stability poll | Docker, GPU, sim license | -| [`tests/test_sensors.py`](tests/test_sensors.py) | `sensors` | Topic Hz (Isaac: batched sim + robot ``ros2 topic hz``; filtered LiDAR ``echo-once`` + validation script), RTF, sensor stability time-series | Docker, GPU, sim license | -| [`tests/test_takeoff_hover_land.py`](tests/test_takeoff_hover_land.py) | `takeoff_hover_land` | 4-phase flight chain (PX4 ready → takeoff → hover → land) per (sim, num_robots, iter, velocity) | Docker, GPU, sim license | +| [`tests/system/test_build_docker.py`](tests/system/test_build_docker.py) | `build_docker` | Docker image builds (robot-desktop, gcs, isaac-sim, ms-airsim) | Docker | +| [`tests/system/test_build_packages.py`](tests/system/test_build_packages.py) | `build_packages` | `colcon build` inside each container | Docker | +| [`tests/system/test_liveliness.py`](tests/system/test_liveliness.py) | `liveliness` | Stack bring-up: containers, ``/clock`` readiness, tmux, sentinel ROS 2 nodes, compute, infra-only stability poll | Docker, GPU, sim license | +| [`tests/system/test_sensors.py`](tests/system/test_sensors.py) | `sensors` | Topic Hz (Isaac: batched sim + robot ``ros2 topic hz``; filtered LiDAR ``echo-once`` + validation script), RTF, sensor stability time-series | Docker, GPU, sim license | +| [`tests/system/test_takeoff_hover_land.py`](tests/system/test_takeoff_hover_land.py) | `takeoff_hover_land` | 4-phase flight chain (PX4 ready → takeoff → hover → land) per (sim, num_robots, iter, velocity) | Docker, GPU, sim license | Shared fixtures, the `airstack_env` parametrized fixture, and `MetricsRecorder` live in [`tests/conftest.py`](tests/conftest.py). Each run produces a timestamped directory under `tests/results//` with `results.xml`, `metrics.json`, and per-test logs. [`tests/parse_metrics.py`](tests/parse_metrics.py) generates a markdown report (single-run or diff-vs-baseline; exits 1 on regression). **Run via the CLI** (containerized runner — no local Python needed): ```bash +airstack test -m unit -v airstack test -m "build_docker or build_packages" -v airstack test -m liveliness --sim msairsim --num-robots 1 --stress-iterations 1 -v airstack test -m sensors --sim isaacsim --num-robots 1 --stress-iterations 1 -v diff --git a/airstack.sh b/airstack.sh index 3d1e955e3..08379cf26 100755 --- a/airstack.sh +++ b/airstack.sh @@ -108,6 +108,7 @@ function print_command_help { echo "Options:" echo " --no-shell Don't modify shell configuration" echo " --no-config Skip configuration tasks (Isaac Sim, Nucleus, Git hooks)" + echo " --no-natnet Skip NatNet SDK installation for OptiTrack motion capture support" echo "" echo "This command adds an 'airstack' function to your shell profile that will" echo "automatically find and use the airstack.sh script from the current directory" @@ -194,10 +195,11 @@ function print_command_help { echo "Results are written to tests/results//." echo "" echo "Test marks (-m):" + echo " unit Fast hermetic tests (robot/sim mirrored layout; no Docker stack)" echo " build_docker Docker image build tests (no GPU needed)" echo " build_packages colcon workspace build tests (no GPU needed)" echo " liveliness Full stack up: nodes, topics, compute, stability" - echo " autonomy Takeoff / hover / land flight chain" + echo " takeoff_hover_land Takeoff / hover / land flight chain" echo "" echo "AirStack-specific options:" echo " --sim=TARGETS Comma-separated sim targets" @@ -577,18 +579,33 @@ function cmd_install { log_info "Installation complete!" } +function cmd_setup_natnet_sdk { + local sdk_script="$PROJECT_ROOT/robot/ros_ws/src/perception/natnet_ros2/scripts/download-natnet-sdk.sh" + + if [ ! -f "$sdk_script" ]; then + log_warn "NatNet SDK installer not found at $sdk_script" + return 0 + fi + + log_info "Checking NatNet SDK installation..." + bash "$sdk_script" +} + function cmd_setup { log_info "Setting up AirStack environment..." # Check for --no-shell flag local modify_shell=true local skip_config=false + local skip_natnet=false for arg in "$@"; do if [ "$arg" == "--no-shell" ]; then modify_shell=false elif [ "$arg" == "--no-config" ]; then skip_config=true + elif [ "$arg" == "--no-natnet" ]; then + skip_natnet=true fi done @@ -653,6 +670,15 @@ function cmd_setup { fi fi + # Install NatNet SDK unless explicitly skipped. + if [ "$skip_natnet" = false ]; then + if declare -f "cmd_setup_natnet_sdk" > /dev/null; then + cmd_setup_natnet_sdk + else + log_warn "NatNet SDK setup helper not loaded. Skipping NatNet SDK installation." + fi + fi + # Run configuration tasks if not skipped if [ "$skip_config" = false ]; then # Check if the config module is available @@ -728,6 +754,35 @@ function classify_compose_args { done } +# robot-l4t uses Dockerfile.robot with BASE_IMAGE=robot-l4t-stack-base. Compose v2+ may schedule +# service builds in parallel, so BUILD FROM that tag can race before stack-base finishes. Ensure +# the intermediary image exists first (still requires --profile l4t like the robot-l4t build). +function ensure_robot_l4t_stack_base() { + local -n _ga="$1" + local -n _sc="$2" + local wants_l4t=false + local has_stack=false + for arg in "${_sc[@]}"; do + if [[ "$arg" == robot-l4t ]]; then + wants_l4t=true + fi + if [[ "$arg" == robot-l4t-stack-base ]]; then + has_stack=true + fi + done + if [[ "$wants_l4t" != true ]] || [[ "$has_stack" == true ]]; then + return 0 + fi + log_info "Building robot-l4t-stack-base before robot-l4t..." + local build_opts=() + for arg in "${_sc[@]}"; do + if [[ "$arg" == -* ]]; then + build_opts+=("$arg") + fi + done + run_docker_compose -f "$PROJECT_ROOT/docker-compose.yaml" "${_ga[@]}" build "${build_opts[@]}" robot-l4t-stack-base +} + function cmd_up { check_docker @@ -771,6 +826,13 @@ function cmd_image_build { local global_args=() local subcmd_args=() classify_compose_args global_args subcmd_args "$@" + # Jetson only: building robot-l4t needs robot-l4t-stack-base first + for arg in "${subcmd_args[@]}"; do + if [[ "$arg" == robot-l4t ]]; then + ensure_robot_l4t_stack_base global_args subcmd_args + break + fi + done # Registry-cache mode (CI / opt-in): pre-pull existing images to seed the # local cache, build with BUILDKIT_INLINE_CACHE=1 so the resulting image diff --git a/common/ros_packages/msgs/airstack_msgs/package.xml b/common/ros_packages/msgs/airstack_msgs/package.xml index c040458b1..06c93abe6 100644 --- a/common/ros_packages/msgs/airstack_msgs/package.xml +++ b/common/ros_packages/msgs/airstack_msgs/package.xml @@ -16,11 +16,12 @@ rosidl_default_generators rosidl_default_runtime - rosidl_interface_packages ament_lint_auto ament_lint_common + rosidl_interface_packages + ament_cmake diff --git a/common/ros_packages/msgs/task_msgs/package.xml b/common/ros_packages/msgs/task_msgs/package.xml index 8440930c2..8e97d3397 100644 --- a/common/ros_packages/msgs/task_msgs/package.xml +++ b/common/ros_packages/msgs/task_msgs/package.xml @@ -17,11 +17,12 @@ airstack_msgs rosidl_default_runtime - rosidl_interface_packages ament_lint_auto ament_lint_common + rosidl_interface_packages + ament_cmake diff --git a/docs/development/docker-build-profiles.md b/docs/development/docker-build-profiles.md new file mode 100644 index 000000000..194f871fd --- /dev/null +++ b/docs/development/docker-build-profiles.md @@ -0,0 +1,50 @@ +# Docker build profiles and build-args + +This page gives a high-level overview of the build-time knobs used to create AirStack robot images. The low-level, copyable snippets and validation checks live in [.agents/skills/docker-build-profiles/SKILL.md](../../.agents/skills/docker-build-profiles/SKILL.md). + +## What this covers + +- AirStack uses one Dockerfile, [robot/docker/Dockerfile.robot](../../robot/docker/Dockerfile.robot), to build multiple image variants. +- The active variant is selected by `build.args` in [robot/docker/docker-compose.yaml](../../robot/docker/docker-compose.yaml). +- The key inputs are `BASE_IMAGE`, `ROS_DISTRO`, and `PYTHON_VERSION`. + +## Common build args + +- `BASE_IMAGE` selects the OS/vendor base image. +- `ROS_DISTRO` selects the ROS 2 distro to install. +- `PYTHON_VERSION` must match the ROS Python path for the distro. Quote it in YAML, for example `"3.10"`. +- `REAL_ROBOT`, `SKIP_MACVO`, and `SKIP_TENSORRT` toggle image content for platform-specific builds. + +## Network mode + +- `network: host` is used as a workaround for Jetson/L4T build issues with Docker networking and host kernel modules. +- It is generally not needed for the desktop profile. + +## Where to look for examples + +- The current service profiles in [robot/docker/docker-compose.yaml](../../robot/docker/docker-compose.yaml) show the desktop and L4T build args. +- The agent SKILL contains the detailed profile-creation steps, validation snippet, and YAML quoting guidance. + +## Human-friendly `airstack` commands + +For readers, prefer the `airstack` workflow instead of raw Docker commands. + +```bash +# Build a compose service image +airstack image-build robot-desktop + +# Build and start a service +airstack image-build robot-desktop +airstack up robot-desktop + +# Inspect the robot image build with full output +airstack image-build --target builder --progress=plain robot-desktop + +# Open a shell in a running container +airstack connect robot-desktop --command=bash + +# View container logs +airstack logs robot-desktop +``` + +For agent-facing automation and low-level debugging, use the SKILL instead of this page. diff --git a/docs/development/intermediate/testing/index.md b/docs/development/intermediate/testing/index.md index 9279f96c4..0769e2b09 100644 --- a/docs/development/intermediate/testing/index.md +++ b/docs/development/intermediate/testing/index.md @@ -1,23 +1,56 @@ # Testing -AirStack uses several test layers: ROS 2 package tests (`colcon test`), and **system tests** under [`tests/`](../../../../tests/) at the repo root (pytest, full Docker stack). +AirStack uses three complementary test layers, each with a distinct scope and +hardware requirement: -## System tests (`tests/`) +| Layer | Where | Mark / Tool | Hardware | +|---|---|---|---| +| **Unit tests** | `tests/robot/`, `tests/sim/` | `pytest -m unit` | None — pure Python | +| **Package tests** | `/test/` | `colcon test` | Robot container | +| **System tests** | `tests/system/` | `pytest -m liveliness` etc. | Docker, GPU, sim license | -The canonical reference is **[`tests/README.md`](../../../../tests/README.md)** (also included in the MkDocs site). In short: +## Unit tests (`pytest -m unit`) + +Fast, hermetic Python tests that run in seconds with no Docker or GPU. Test source +lives **co-located with its ROS 2 package** (`/test/`) and is re-exported +through thin proxy files in `tests/robot/` for centralized discovery. + +```bash +airstack test -m unit -v +# or directly: +pytest tests/ -m unit -v +``` + +Unit tests run as part of `system-tests.yml` via `pytest tests/` and can also be +run locally with no Docker or GPU needed. + +→ **[Unit Testing Guide](unit_testing.md)** — patterns, proxy layout, CI workflow, + how to add tests for new packages (Python and C++ gtest). + +## System tests (`tests/system/`) + +Full Docker-stack integration tests. The canonical reference is +**[`tests/README.md`](../../../../tests/README.md)**. In short: | Mark | Module | Role | -|------|--------|------| -| `liveliness` | `test_liveliness.py` | Containers, `/clock` readiness, tmux, sentinel ROS 2 nodes, compute, infra-only stability poll | -| `sensors` | `test_sensors.py` | Sim + robot stereo/depth Hz, filtered LiDAR (`echo --once` + validation script on Isaac), sim RTF, sensor stability time-series | -| `takeoff_hover_land` | `test_takeoff_hover_land.py` | Four-phase flight chain per configuration | +|---|---|---| +| `liveliness` | `system/test_liveliness.py` | Containers, `/clock` readiness, tmux, sentinel ROS 2 nodes, compute, infra-only stability poll | +| `sensors` | `system/test_sensors.py` | Sim + robot stereo/depth Hz, filtered LiDAR (`echo --once` + validation script on Isaac), sim RTF, sensor stability time-series | +| `takeoff_hover_land` | `system/test_takeoff_hover_land.py` | Four-phase flight chain per configuration | -Collection order is defined in `tests/conftest.py` (`liveliness` before `sensors` before `takeoff_hover_land`). Each mark’s test **class** uses **class-scoped** `airstack_env`, so combining marks with **`and`** runs multiple full stack bring-ups per `(sim, num_robots, iteration)` — see *Bring-up scope* in `tests/README.md`. +Collection order is defined in `tests/conftest.py` (`liveliness` before `sensors` +before `takeoff_hover_land`). Each mark's test class uses **class-scoped** +`airstack_env`, so combining marks with `and` runs multiple full stack bring-ups +per `(sim, num_robots, iteration)` — see *Bring-up scope* in `tests/README.md`. -**Isaac Sim:** the `sensors` implementation batches `ros2 topic hz` on sim and robot paths and avoids `hz` on filtered `PointCloud2`; pytest enables `ENABLE_LIDAR` for the multi-drone Pegasus script. Details: **`tests/README.md`** → *Isaac Sim and the sensors mark*. +**Isaac Sim:** the `sensors` implementation batches `ros2 topic hz` on sim and +robot paths and avoids `hz` on filtered `PointCloud2`; pytest enables `ENABLE_LIDAR` +for the multi-drone Pegasus script. Details: **`tests/README.md`** → *Isaac Sim and +the sensors mark*. ## Other testing docs +- [Unit Testing](unit_testing.md) — `@pytest.mark.unit`, proxy pattern, CI workflow - [Testing frameworks](testing_frameworks.md) — `colcon test`, rostest patterns - [Integration testing](integration_testing.md) - [CI/CD](ci_cd.md) — pipeline overview diff --git a/docs/development/intermediate/testing/unit_testing.md b/docs/development/intermediate/testing/unit_testing.md index e69de29bb..f69056008 100644 --- a/docs/development/intermediate/testing/unit_testing.md +++ b/docs/development/intermediate/testing/unit_testing.md @@ -0,0 +1,206 @@ +# Unit Testing + +AirStack unit tests are **fast, hermetic, and purely Python** — no Docker stack, no GPU, no running containers. They run locally in seconds and gate every pull request via a dedicated GitHub Actions workflow on a standard `ubuntu-latest` runner. + +## Design principles + +- **Co-located with source.** Test files live in `/test/` alongside the code they test. This is the standard ROS 2 / colcon convention and ensures tests are discovered by both `colcon test` and `pytest`. +- **Proxy for centralized discovery.** A thin shim in `tests/robot///` re-exports the test functions so `pytest tests/` (the CI command) and `airstack test -m unit` discover them without any changes to the CI workflow. +- **`@pytest.mark.unit` on every test.** The `unit` mark is the filter that keeps unit tests isolated from system tests that need Docker, GPUs, and sim licenses. + +## Repository layout + +``` +robot/ros_ws/src/ +└── // + ├── src/ # production source + └── test/ + ├── test_.py # unit test source ← canonical location + ├── test_.cpp # C++ gtest source (optional) + └── fake_.hpp # C++ test doubles (optional) + +tests/ +├── robot/ +│ └── // +│ └── test_.py # thin proxy → package test/ +├── sim/ # future: sim-side unit tests +└── gcs/ # future: GCS unit tests +``` + +When pytest collects `tests/robot/…/test_.py`, the `<-` annotation in the +output shows the actual source location: + +``` +robot/perception/natnet_ros2/test_natnet_ros2.py::test_canonical_quaternion_identity + <- ../robot/ros_ws/src/perception/natnet_ros2/test/test_natnet_ros2.py PASSED +``` + +## Running unit tests + +```bash +# Locally — no container or Docker stack required +airstack test -m unit -v + +# Or directly with pytest (AIRSTACK_ROOT must point to the repo root) +export AIRSTACK_ROOT=$(pwd) +pip install pytest numpy +pytest tests/ -m unit -v +``` + +Unit tests complete in under one second for the current suite. + +## CI + +Unit tests are collected and run as part of `system-tests.yml` via `pytest tests/` +(no marks specified on PR open = all tests including `unit`). Run them locally at +any time with no infrastructure required: + +```bash +airstack test -m unit -v +# or directly (requires tests/requirements.txt installed): +AIRSTACK_ROOT=$(pwd) pytest tests/ -m unit -v +``` + +## Current test coverage + +| Package | Test file | What is covered | +|---|---|---| +| `natnet_ros2` | `perception/natnet_ros2/test/test_natnet_ros2.py` | `VisionPoseConverterNode._canonical_quaternion` (ROS stubbed with `sys.modules`) | +| `natnet_ros2` (C++) | `perception/natnet_ros2/test/test_natnet_logic.cpp` | `build_covariance_6x6`, topic name helpers, `ConnectConfig`, `negotiate()` via `FakeNatNetClient` | +| `lidar_point_cloud_filter` | `sensors/lidar_point_cloud_filter/test/test_validation_core.py` | Pure-numpy LiDAR range validation rules | + +## Adding a new unit test + +### Python + +**1. Write the test source in the package:** + +```python +# robot/ros_ws/src///test/test_my_module.py +import sys +from pathlib import Path +import pytest + +# Make the package importable without a colcon install +_src = Path(__file__).resolve().parent.parent / "src" +if str(_src) not in sys.path: + sys.path.insert(0, str(_src)) + +from my_module import my_function # noqa: E402 + + +@pytest.mark.unit +def test_basic(): + assert my_function(1, 2) == 3 +``` + +If the production code inherits from `rclpy.node.Node`, stub ROS at the import +boundary: + +```python +import sys +from unittest.mock import MagicMock + +class _FakeNode: + def __init__(self, name): pass + def get_logger(self): return MagicMock() + def declare_parameter(self, *a, **kw): pass + def get_parameter(self, name): + m = MagicMock(); m.value = MagicMock(); return m + def create_subscription(self, *a, **kw): return MagicMock() + def create_publisher(self, *a, **kw): return MagicMock() + +_rclpy_node_mod = MagicMock() +_rclpy_node_mod.Node = _FakeNode +sys.modules.setdefault("rclpy", MagicMock()) +sys.modules["rclpy.node"] = _rclpy_node_mod +# ... then import your module +``` + +**2. Write the thin proxy in `tests/robot/`:** + +```python +# tests/robot///test_my_module.py +"""Proxy: re-exposes unit tests from the package source tree.""" +import importlib.util +from pathlib import Path + +_repo_root = Path(__file__).resolve().parents[4] # adjust depth if needed +_real_file = _repo_root / "robot/ros_ws/src///test/test_my_module.py" + +_spec = importlib.util.spec_from_file_location("__unit_tests", _real_file) +_real = importlib.util.module_from_spec(_spec) +_spec.loader.exec_module(_real) + +for _name in dir(_real): + if _name.startswith("test_"): + globals()[_name] = getattr(_real, _name) +``` + +The unique module name (e.g. `"__unit_tests"`) prevents a circular import: +pytest adds the proxy's directory to `sys.path` at collection time, which would +cause `from test_my_module import *` to import the proxy itself. + +**3. Verify:** + +```bash +pytest tests/ -m unit -v +``` + +### C++ (gtest) + +C++ tests live entirely in the package and run via `colcon test` — no proxy needed. + +**`CMakeLists.txt`:** + +```cmake +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + ament_add_gtest(test_my_name test/test_my_name.cpp) + target_include_directories(test_my_name PRIVATE + $ + $) +endif() +``` + +**`package.xml`:** + +```xml +ament_cmake_gtest +``` + +**Run inside the robot container:** + +```bash +docker exec airstack-robot-desktop-1 bash -c \ + "bws --cmake-args '-DBUILD_TESTING=ON' --packages-select " +docker exec airstack-robot-desktop-1 bash -c \ + "colcon test --packages-select --event-handlers console_direct+" +``` + +The `build_packages` CI job (`tests/system/test_build_packages.py`) also runs +`colcon test` with `BUILD_TESTING=ON` so C++ gtests are gated in CI as well. + +## Extending to sim and GCS + +The proxy pattern extends to other components. As sim-side Python logic (e.g. the +[Motive emulator](../../../../tests/sim/motive_emulator/README.md)) and GCS modules +acquire unit-testable code, follow the same layout: + +``` +simulation/...//test/test_.py ← source +tests/sim//test_.py ← proxy (parents[3] to reach repo root) + +gcs/...//test/test_.py ← source +tests/gcs//test_.py ← proxy +``` + +`pytest tests/ -m unit` discovers them automatically — no changes to `pytest.ini` +or CI changes needed. + +## See also + +- [`.agents/skills/add-unit-tests`](../../../../.agents/skills/add-unit-tests/SKILL.md) — step-by-step agent workflow +- [System tests](../../../../tests/README.md) — full Docker-stack integration tests +- [CI/CD](ci_cd.md) — pipeline overview and ephemeral runner architecture +- [Testing frameworks](testing_frameworks.md) — `colcon test`, ament linters diff --git a/mkdocs.yml b/mkdocs.yml index 00a1aee14..b1a6c8a46 100644 --- a/mkdocs.yml +++ b/mkdocs.yml @@ -65,8 +65,10 @@ nav: - docs/development/beginner/fork_your_own_project.md - Intermediate Tutorials: - Testing: - - tests/README.md - - tests/ci-cd-orchestrator.md + - Overview: docs/development/intermediate/testing/index.md + - Unit Testing: docs/development/intermediate/testing/unit_testing.md + - System Tests: tests/README.md + - CI/CD Orchestrator: tests/ci-cd-orchestrator.md - Frame Conventions: docs/development/intermediate/frame_conventions.md - Contributing: - docs/development/intermediate/contributing.md @@ -111,6 +113,7 @@ nav: - Perception: - docs/robot/autonomy/perception/index.md - State Estimation: docs/robot/autonomy/perception/state_estimation.md + - NatNet (OptiTrack): robot/ros_ws/src/perception/natnet_ros2/README.md - Local: - docs/robot/autonomy/local/index.md - World Model: diff --git a/robot/docker/Dockerfile.l4t-stack-base b/robot/docker/Dockerfile.l4t-stack-base new file mode 100644 index 000000000..0ebb8cd14 --- /dev/null +++ b/robot/docker/Dockerfile.l4t-stack-base @@ -0,0 +1,50 @@ +# Thin intermediary for Jetson: dustynv Jazzy (Noble) + quirks so Dockerfile.robot matches Ubuntu/desktop flow. +# +# Builds: PROJECT_DOCKER_REGISTRY/...:_robot-l4t-stack-base_${DOCKER_IMAGE_BUILD_MODE} +# Build before robot-l4t: compose build robot-l4t-stack-base robot-l4t +# +# - Refreshes ROS apt keyring (dusty snapshots can carry expired signatures). +# - Sets pip defaults to PyPI and clears dusty PIP_CONSTRAINT / PIP_EXTRA_INDEX_URL (mirror hash/index surprises during build). +# - Reconciles OpenCV CUDA overlay vs Ubuntu libopencv*-dev pulls from ROS stacks. +# - Rewrites prebuilt /opt/ros to use /usr/bin/python3 instead of dusty /opt/venv, then removes the venv +# so colcon/rosidl use the same system Python path as Dockerfile.robot Ubuntu images. + +ARG DUSTYNV_IMAGE=dustynv/ros:jazzy-ros-base-r36.4.0-cu128-24.04 +ARG ROS_DISTRO=jazzy +FROM ${DUSTYNV_IMAGE} + +ARG ROS_DISTRO=jazzy +ENV ROS_DISTRO=${ROS_DISTRO} +ENV DEBIAN_FRONTEND=noninteractive + +RUN curl -fsSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key \ + -o /usr/share/keyrings/ros-archive-keyring.gpg \ + && chmod 644 /usr/share/keyrings/ros-archive-keyring.gpg + +ENV PIP_INDEX_URL=https://pypi.org/simple \ + PIP_CONSTRAINT= \ + PIP_EXTRA_INDEX_URL= + +RUN apt-get update \ + && apt-get install -y -o Dpkg::Options::=--force-overwrite --no-install-recommends \ + libopencv-dev \ + python3-empy \ + python3-yaml \ + && rm -rf /var/lib/apt/lists/* + +# Point all dusty /opt/venv/python references at system Python (matches Ubuntu rosidl behavior). +RUN ros="/opt/ros/${ROS_DISTRO}"; \ + if [ -d "$ros" ]; then \ + ( grep -rlIZ '/opt/venv' "$ros" 2>/dev/null || true ) \ + | xargs -0 -r sed -i \ + -e 's#/opt/venv/bin/python3#/usr/bin/python3#g' \ + -e 's#/opt/venv/bin:##g' \ + -e 's#:/opt/venv/bin##g'; \ + fi; \ + rm -rf /opt/venv + +ENV AMENT_PYTHON_EXECUTABLE=/usr/bin/python3 +ENV PYTHON_EXECUTABLE=/usr/bin/python3 + +# Prefer system/cuda tooling over any removed venv prefix (CUDA symlink is usually /usr/local/cuda). +ENV PATH="/usr/local/cuda/bin:/usr/local/sbin:/usr/local/bin:/usr/sbin:/usr/bin:/sbin:/bin" diff --git a/robot/docker/Dockerfile.robot b/robot/docker/Dockerfile.robot index 55b1ad40f..6415fb87b 100644 --- a/robot/docker/Dockerfile.robot +++ b/robot/docker/Dockerfile.robot @@ -1,4 +1,4 @@ -# either ubuntu:24.04 or l4t. ubuntu:24.04 is default +# either ubuntu:24.04 / nvidia CUDA, or Jetson intermediary from Dockerfile.l4t-stack-base (see robot-l4t in compose). ARG BASE_IMAGE # ============================================================ # Stage 1 — builder: compile/download everything @@ -13,6 +13,12 @@ ARG INSTALL_FLAGS="-o APT::Get::AllowUnauthenticated=true" ARG SKIP_MACVO=false ARG SKIP_TENSORRT=false +ARG PIP_VERSION=24.0 +ARG PYTHON_VERSION=3.12 + +ARG ROS_DISTRO=jazzy +ENV ROS_DISTRO=${ROS_DISTRO} + # from https://github.com/athackst/dockerfiles/blob/main/ros2/jazzy.Dockerfile ENV DEBIAN_FRONTEND=noninteractive @@ -53,16 +59,15 @@ RUN sudo add-apt-repository universe \ && curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg \ && echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu $(. /etc/os-release && echo $UBUNTU_CODENAME) main" | sudo tee /etc/apt/sources.list.d/ros2.list > /dev/null \ && apt-get ${UPDATE_FLAGS} update -y && apt-get ${INSTALL_FLAGS} install -y --no-install-recommends \ - ros-jazzy-desktop \ + ros-${ROS_DISTRO}-desktop \ python3-argcomplete \ && rm -rf /var/lib/apt/lists/* -ENV ROS_DISTRO=jazzy -ENV AMENT_PREFIX_PATH=/opt/ros/jazzy -ENV COLCON_PREFIX_PATH=/opt/ros/jazzy -ENV LD_LIBRARY_PATH=/opt/ros/jazzy/lib/x86_64-linux-gnu:/opt/ros/jazzy/lib -ENV PATH=/opt/ros/jazzy/bin:$PATH -ENV PYTHONPATH=/opt/ros/jazzy/local/lib/python3.12/dist-packages:/opt/ros/jazzy/lib/python3.12/site-packages +ENV AMENT_PREFIX_PATH=/opt/ros/${ROS_DISTRO} +ENV COLCON_PREFIX_PATH=/opt/ros/${ROS_DISTRO} +ENV LD_LIBRARY_PATH=/opt/ros/${ROS_DISTRO}/lib/x86_64-linux-gnu:/opt/ros/${ROS_DISTRO}/lib +ENV PATH=/opt/ros/${ROS_DISTRO}/bin:$PATH +ENV PYTHONPATH=/opt/ros/${ROS_DISTRO}/local/lib/python${PYTHON_VERSION}/dist-packages:/opt/ros/${ROS_DISTRO}/lib/python${PYTHON_VERSION}/site-packages ENV ROS_PYTHON_VERSION=3 ENV ROS_VERSION=2 ENV ROS_AUTOMATIC_DISCOVERY_RANGE=SUBNET @@ -80,24 +85,31 @@ RUN apt update && apt install -y --no-install-recommends \ gdb \ && rm -rf /var/lib/apt/lists/* +# Freeze pip and setuptools versions (ignore-installed: apt-shipped wheel/setuptools have no pip RECORD). +RUN python3 -m pip install --no-cache-dir --break-system-packages --ignore-installed --upgrade \ + "pip==${PIP_VERSION}" \ + "setuptools==79.0.1" \ + wheel + # Install any additional ROS2 packages RUN apt update -y && apt install -y --no-install-recommends \ ros-dev-tools \ - ros-jazzy-mavros \ - ros-jazzy-tf2* \ - ros-jazzy-stereo-image-proc \ - ros-jazzy-image-view \ - ros-jazzy-topic-tools \ - ros-jazzy-grid-map \ - ros-jazzy-domain-bridge \ - ros-jazzy-rosbag2-storage-mcap \ - ros-jazzy-xacro \ - ros-jazzy-foxglove-bridge \ + ros-${ROS_DISTRO}-mavros \ + ros-${ROS_DISTRO}-tf2* \ + ros-${ROS_DISTRO}-stereo-image-proc \ + ros-${ROS_DISTRO}-image-view \ + ros-${ROS_DISTRO}-topic-tools \ + ros-${ROS_DISTRO}-grid-map \ + ros-${ROS_DISTRO}-domain-bridge \ + ros-${ROS_DISTRO}-rosbag2-storage-mcap \ + ros-${ROS_DISTRO}-xacro \ + ros-${ROS_DISTRO}-ament-package \ + ros-${ROS_DISTRO}-foxglove-bridge \ libcgal-dev \ python3-colcon-common-extensions \ && rm -rf /var/lib/apt/lists/* -RUN /opt/ros/jazzy/lib/mavros/install_geographiclib_datasets.sh +RUN /opt/ros/${ROS_DISTRO}/lib/mavros/install_geographiclib_datasets.sh # Install TensorRT (NVIDIA/L4T images only, unless SKIP_TENSORRT=true) # Note: TensorRT 8 packages may not be available for Ubuntu 24.04, so this is optional @@ -181,6 +193,25 @@ RUN if [ "${SKIP_MACVO}" != "true" ]; then \ # TMux config RUN git clone --depth 1 https://github.com/tmux-plugins/tpm /root/.tmux/plugins/tpm +# Diagnostic: Check Python environment before DDS Router build +RUN echo "=== Python version ===" && \ + python3 --version && \ + echo "" && \ + echo "=== PYTHONPATH ===" && \ + echo "$PYTHONPATH" && \ + echo "" && \ + echo "=== sys.path ===" && \ + python3 -c "import sys; print('\n'.join(sys.path))" && \ + echo "" && \ + echo "=== Checking ament_package ===" && \ + python3 -c "import ament_package; print('✓ ament_package found at:', ament_package.__file__)" || echo "✗ ament_package NOT found" && \ + echo "" && \ + echo "=== Checking dpkg for ament packages ===" && \ + dpkg -l | grep -i ament || echo "No ament packages found in dpkg" && \ + echo "" && \ + echo "=== ROS Python packages ===" && \ + ls -la /opt/ros/${ROS_DISTRO}/lib/python*/dist-packages/ 2>/dev/null | head -20 || echo "No ROS python packages found" + # Install eProsima DDS Router # System library dependencies (Asio, TinyXML2, OpenSSL, yaml-cpp) RUN apt update && apt install -y --no-install-recommends \ @@ -214,6 +245,12 @@ ARG INSTALL_FLAGS="-o APT::Get::AllowUnauthenticated=true" ARG SKIP_MACVO=false ARG SKIP_TENSORRT=false +ARG PIP_VERSION=24.0 +ARG PYTHON_VERSION=3.12 + +ARG ROS_DISTRO +ENV ROS_DISTRO=${ROS_DISTRO} + ENV DEBIAN_FRONTEND=noninteractive SHELL ["/bin/bash", "-c"] @@ -253,17 +290,15 @@ RUN sudo add-apt-repository universe \ && curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg \ && echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu $(. /etc/os-release && echo $UBUNTU_CODENAME) main" | sudo tee /etc/apt/sources.list.d/ros2.list > /dev/null \ && apt-get ${UPDATE_FLAGS} update -y && apt-get ${INSTALL_FLAGS} install -y --no-install-recommends \ - ros-jazzy-desktop \ + ros-${ROS_DISTRO}-desktop \ python3-argcomplete \ && rm -rf /var/lib/apt/lists/* -# Carry over all ROS2 ENV vars from the builder stage -ENV ROS_DISTRO=jazzy -ENV AMENT_PREFIX_PATH=/opt/ros/jazzy -ENV COLCON_PREFIX_PATH=/opt/ros/jazzy -ENV LD_LIBRARY_PATH=/opt/ros/jazzy/lib/x86_64-linux-gnu:/opt/ros/jazzy/lib -ENV PATH=/opt/ros/jazzy/bin:$PATH -ENV PYTHONPATH=/opt/ros/jazzy/local/lib/python3.12/dist-packages:/opt/ros/jazzy/lib/python3.12/site-packages +ENV AMENT_PREFIX_PATH=/opt/ros/${ROS_DISTRO} +ENV COLCON_PREFIX_PATH=/opt/ros/${ROS_DISTRO} +ENV LD_LIBRARY_PATH=/opt/ros/${ROS_DISTRO}/lib/x86_64-linux-gnu:/opt/ros/${ROS_DISTRO}/lib +ENV PATH=/opt/ros/${ROS_DISTRO}/bin:$PATH +ENV PYTHONPATH=/opt/ros/${ROS_DISTRO}/local/lib/python${PYTHON_VERSION}/dist-packages:/opt/ros/${ROS_DISTRO}/lib/python${PYTHON_VERSION}/site-packages ENV ROS_PYTHON_VERSION=3 ENV ROS_VERSION=2 ENV ROS_AUTOMATIC_DISCOVERY_RANGE=SUBNET @@ -275,26 +310,37 @@ RUN apt update && apt install -y --no-install-recommends \ vim nano tree \ less htop jq \ python3-pip \ + python3-yaml \ + python3-empy \ python3-rosdep \ tmux \ && rm -rf /var/lib/apt/lists/* +# Freeze pip and setuptools versions (ignore-installed: apt-shipped wheel/setuptools have no pip RECORD). +RUN python3 -m pip install --no-cache-dir --break-system-packages --ignore-installed --upgrade \ + "pip==${PIP_VERSION}" \ + "setuptools==79.0.1" \ + wheel + # Install runtime ROS2 packages (no libcgal-dev) RUN apt update -y && apt install -y --no-install-recommends \ ros-dev-tools \ - ros-jazzy-mavros \ - ros-jazzy-tf2* \ - ros-jazzy-stereo-image-proc \ - ros-jazzy-image-view \ - ros-jazzy-topic-tools \ - ros-jazzy-grid-map \ - ros-jazzy-domain-bridge \ - ros-jazzy-rosbag2-storage-mcap \ - ros-jazzy-xacro \ - ros-jazzy-foxglove-bridge \ + ros-${ROS_DISTRO}-mavros \ + ros-${ROS_DISTRO}-tf2* \ + ros-${ROS_DISTRO}-stereo-image-proc \ + ros-${ROS_DISTRO}-image-view \ + ros-${ROS_DISTRO}-topic-tools \ + ros-${ROS_DISTRO}-grid-map \ + ros-${ROS_DISTRO}-domain-bridge \ + ros-${ROS_DISTRO}-rosbag2-storage-mcap \ + ros-${ROS_DISTRO}-xacro \ + ros-${ROS_DISTRO}-ament-package \ + ros-${ROS_DISTRO}-foxglove-bridge \ python3-colcon-common-extensions \ && rm -rf /var/lib/apt/lists/* +# TODO: consider splitting this into a separate "desktop-plus" image, since foxglove-bridge is a large install and not strictly necessary for most robot use cases + # Install emoji font support and refresh font cache RUN apt-get update && apt-get install -y --no-install-recommends \ fonts-noto-color-emoji \ @@ -302,7 +348,7 @@ RUN apt-get update && apt-get install -y --no-install-recommends \ && fc-cache -f -v \ && rm -rf /var/lib/apt/lists/* -RUN /opt/ros/jazzy/lib/mavros/install_geographiclib_datasets.sh +RUN /opt/ros/${ROS_DISTRO}/lib/mavros/install_geographiclib_datasets.sh # Install DDS Router runtime library dependencies + OpenVDB RUN apt update && apt install -y --no-install-recommends \ @@ -324,12 +370,16 @@ RUN if echo "$BASE_IMAGE" | grep -qE "(nvidia|l4t)" && [ "${SKIP_TENSORRT}" != " && rm -rf /var/lib/apt/lists/*; \ fi -# Install Foxglove Studio desktop app -RUN wget -q https://get.foxglove.dev/desktop/latest/foxglove-studio-latest-linux-amd64.deb -O /tmp/foxglove-studio.deb \ - && apt-get ${UPDATE_FLAGS} update \ - && apt-get ${INSTALL_FLAGS} install -y --no-install-recommends /tmp/foxglove-studio.deb \ - && rm /tmp/foxglove-studio.deb \ - && rm -rf /var/lib/apt/lists/* +# Install Foxglove Studio desktop app only for non-real-robot images +RUN if [ "${REAL_ROBOT}" != "true" ] && [ "$(dpkg --print-architecture)" = "amd64" ]; then \ + wget -q https://get.foxglove.dev/desktop/latest/foxglove-studio-latest-linux-amd64.deb -O /tmp/foxglove-studio.deb && \ + apt-get ${UPDATE_FLAGS} update && \ + apt-get ${INSTALL_FLAGS} install -y --no-install-recommends /tmp/foxglove-studio.deb && \ + rm /tmp/foxglove-studio.deb; \ + else \ + echo "Skipping Foxglove Studio install (REAL_ROBOT=${REAL_ROBOT}, arch=$(dpkg --print-architecture))"; \ + fi && \ + rm -rf /var/lib/apt/lists/* # Add ability to SSH (libglfw3-dev and libglm-dev kept per spec) RUN apt-get ${UPDATE_FLAGS} update && apt-get ${INSTALL_FLAGS} install -y --no-install-recommends \ diff --git a/robot/docker/docker-compose.yaml b/robot/docker/docker-compose.yaml index dfbe71fd3..e0f13b996 100644 --- a/robot/docker/docker-compose.yaml +++ b/robot/docker/docker-compose.yaml @@ -14,6 +14,7 @@ services: dockerfile: ./Dockerfile.robot args: BASE_IMAGE: nvidia/cuda:13.0.2-base-ubuntu24.04 + ROS_DISTRO: jazzy tags: - *desktop_image cache_from: @@ -26,6 +27,7 @@ services: - LAUNCH_PACKAGE=desktop_bringup # desktop_bringup adds RViz; real robots use autonomy_bringup - AUTONOMY_ROLE=full - SIM_IP=${SIM_IP:-172.31.0.200} + - LAUNCH_NATNET=${LAUNCH_NATNET:-false} # FCU_URL and TGT_SYSTEM not set, dynamically calculated in interface.launch.py # 'command' uses variables so that it can be shared across robot-desktop and robot-l4t, with different launch packages and roles. command: > @@ -115,6 +117,7 @@ services: REAL_ROBOT: true SKIP_MACVO: true SKIP_TENSORRT: true + ROS_DISTRO: jazzy tags: - *voxl_image cache_from: @@ -124,6 +127,7 @@ services: - AUTOLAUNCH=${AUTOLAUNCH:-true} - LAUNCH_PACKAGE=autonomy_bringup - AUTONOMY_ROLE=onboard # VOXL is always lite-only; never runs global planning + - LAUNCH_NATNET=${LAUNCH_NATNET:-false} command: > bash -c " tmux new -d -s bringup; @@ -134,6 +138,23 @@ services: network_mode: host deploy: !reset {} # remove nvidia driver for voxl + # =================================================================================================================== + # Intermediate Jetson stack: dusty Jazzy + ROS keyring / pip / OpenCV shim for Dockerfile.robot (same recipe as Ubuntu). + # `airstack image-build --profile l4t robot-l4t` builds this first automatically; raw `compose build robot-l4t` may parallelize incorrectly. + robot-l4t-stack-base: + profiles: + - l4t + image: &l4t_stack_base_image ${PROJECT_DOCKER_REGISTRY}/${PROJECT_NAME}:v${VERSION}_robot-l4t-stack-base_${DOCKER_IMAGE_BUILD_MODE} + build: + dockerfile: ./Dockerfile.l4t-stack-base + network: host + args: + DUSTYNV_IMAGE: dustynv/ros:jazzy-ros-base-r36.4.0-cu128-24.04 + tags: + - *l4t_stack_base_image + cache_from: + - *l4t_stack_base_image + # =================================================================================================================== # for running on an NVIDIA jetson (linux for tegra) device robot-l4t: @@ -147,13 +168,18 @@ services: image: &l4t_image ${PROJECT_DOCKER_REGISTRY}/${PROJECT_NAME}:v${VERSION}_robot-l4t_${DOCKER_IMAGE_BUILD_MODE} build: dockerfile: ./Dockerfile.robot + network: host args: - BASE_IMAGE: nvcr.io/nvidia/l4t-jetpack:r36.4.0 + BASE_IMAGE: *l4t_stack_base_image REAL_ROBOT: true + SKIP_MACVO: true + SKIP_TENSORRT: true + ROS_DISTRO: jazzy tags: - *l4t_image cache_from: - *l4t_image + - *l4t_stack_base_image # we use tmux send-keys so that the session stays alive ipc: host command: > @@ -171,11 +197,13 @@ services: - driver: nvidia count: 1 capabilities: [gpu] - environment: !override - - ROBOT_NAME_SOURCE=hostname # see .bashrc + runtime: nvidia + environment: + - ROBOT_NAME_SOURCE=hostname - AUTOLAUNCH=${AUTOLAUNCH:-true} - LAUNCH_PACKAGE=autonomy_bringup - AUTONOMY_ROLE=full # l4t profile: Jetson runs everything onboard + - LAUNCH_NATNET=${LAUNCH_NATNET:-false} # mavros mavlink settings - FCU_URL="/dev/ttyTHS4:115200" - TGT_SYSTEM=1 @@ -205,10 +233,12 @@ services: build: context: ./ dockerfile: zed/Dockerfile.zed-l4t + network: host args: L4T_MAJOR: 36 L4T_MINOR: 4 L4T_PATCH: 0 + IMAGE_NAME: dustynv/ros:jazzy-desktop-r36.4.0-cu128-24.04 cache_from: - *zed_l4t_image command: > @@ -228,6 +258,7 @@ services: privileged: true ipc: host pid: host + runtime: nvidia environment: - NVIDIA_DRIVER_CAPABILITIES=all - DISPLAY diff --git a/robot/docker/zed/Dockerfile.zed-l4t b/robot/docker/zed/Dockerfile.zed-l4t index 5f25ceaf3..b39ec2bc7 100644 --- a/robot/docker/zed/Dockerfile.zed-l4t +++ b/robot/docker/zed/Dockerfile.zed-l4t @@ -35,7 +35,7 @@ RUN echo "# R${L4T_MAJOR} (release), REVISION: ${L4T_MINOR}" > /etc/nv_tegra_rel apt-get -o Acquire::AllowInsecureRepositories=true -o Acquire::AllowDowngradeToInsecureRepositories=true update -y || true && \ apt-get -o APT::Get::AllowUnauthenticated=true install -y --no-install-recommends zstd wget less cmake curl gnupg2 \ build-essential python3 python3-pip python3-dev python3-setuptools libusb-1.0-0-dev \ - libgeographic-dev libdraco-dev zlib1g-dev -y + libgeographiclib-dev libdraco-dev zlib1g-dev -y RUN pip install protobuf --index-url https://pypi.jetson-ai-lab.io/jp6/cu126 RUN wget -q --no-check-certificate -O ZED_SDK_Linux_JP.run \ ${ZED_SDK_URL} && \ diff --git a/robot/ros_ws/src/local/controls/pid_controller_msgs/package.xml b/robot/ros_ws/src/local/controls/pid_controller_msgs/package.xml index 8dd8872f5..ba8f3126e 100644 --- a/robot/ros_ws/src/local/controls/pid_controller_msgs/package.xml +++ b/robot/ros_ws/src/local/controls/pid_controller_msgs/package.xml @@ -14,11 +14,12 @@ rosidl_default_generators rosidl_default_runtime - rosidl_interface_packages ament_lint_auto ament_lint_common + rosidl_interface_packages + ament_cmake diff --git a/robot/ros_ws/src/perception/natnet_ros2/.gitignore b/robot/ros_ws/src/perception/natnet_ros2/.gitignore new file mode 100644 index 000000000..f4a20d2b9 --- /dev/null +++ b/robot/ros_ws/src/perception/natnet_ros2/.gitignore @@ -0,0 +1,27 @@ +# Ignore proprietary NatNetSDK (downloaded at setup time) +lib/libNatNet.so +include/natnet/ + +# Python runtime and cache +__pycache__/ +*.py[cod] +*$py.class +*.so +.Python +env/ +venv/ +ENV/ +*.egg-info/ +dist/ +build/ + +# IDE +.vscode/ +.idea/ +*.swp +*.swo +*~ + +# ROS build artifacts +devel/ +install/ diff --git a/robot/ros_ws/src/perception/natnet_ros2/CMakeLists.txt b/robot/ros_ws/src/perception/natnet_ros2/CMakeLists.txt new file mode 100644 index 000000000..ff47a00da --- /dev/null +++ b/robot/ros_ws/src/perception/natnet_ros2/CMakeLists.txt @@ -0,0 +1,99 @@ +cmake_minimum_required(VERSION 3.8) +project(natnet_ros2) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# ament / ROS 2 dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(nav_msgs REQUIRED) + +# --------------------------------------------------------------------------- +# NatNet SDK — pre-built shared library downloaded via `airstack setup`. +# Headers: include/natnet/ Library: lib/libNatNet.so +# +# The SDK is proprietary and is NOT committed to the repository. +# Run `airstack setup` (or `airstack setup --natnet`) to download and place +# the SDK files before building this package. +# +# If the SDK is absent the C++ node is skipped with a warning; the Python +# vision_pose_converter_node.py and all launch/config files are still installed +# so the rest of the autonomy stack builds cleanly. +# --------------------------------------------------------------------------- +set(_NATNET_LIB "${CMAKE_CURRENT_SOURCE_DIR}/lib/libNatNet.so") +set(_NATNET_INC "${CMAKE_CURRENT_SOURCE_DIR}/include/natnet") + +if(EXISTS "${_NATNET_LIB}" AND EXISTS "${_NATNET_INC}") + add_library(NatNet SHARED IMPORTED) + set_target_properties(NatNet PROPERTIES + IMPORTED_LOCATION "${_NATNET_LIB}" + INTERFACE_INCLUDE_DIRECTORIES "${_NATNET_INC}" + ) + + # C++ NatNet ROS 2 node + add_executable(natnet_ros2_node + src/natnet_ros2_node.cpp + src/natnet_client_adapter.cpp) + target_include_directories(natnet_ros2_node PUBLIC + $ + $) + ament_target_dependencies(natnet_ros2_node rclcpp geometry_msgs nav_msgs) + target_link_libraries(natnet_ros2_node NatNet) + + install(TARGETS natnet_ros2_node + DESTINATION lib/${PROJECT_NAME}) + + # Install libNatNet.so alongside the node and register an environment hook so + # that sourcing the workspace adds lib/natnet_ros2/ to LD_LIBRARY_PATH. + # Use PROGRAMS (not FILES) to preserve the execute bit — shared libraries + # must be executable for the dynamic linker to map them. + install(PROGRAMS "${_NATNET_LIB}" + DESTINATION lib/${PROJECT_NAME}) + + ament_environment_hooks( + "${CMAKE_CURRENT_SOURCE_DIR}/env-hooks/natnet_library_path.dsv.in" + ) +else() + message(WARNING + "[natnet_ros2] NatNet SDK not found — skipping natnet_ros2_node build.\n" + " Expected: ${_NATNET_LIB}\n" + " ${_NATNET_INC}/\n" + " Run 'airstack setup' to download the OptiTrack NatNet SDK.") +endif() + +# --------------------------------------------------------------------------- +# Python nodes (vision_pose_converter remains Python) +# --------------------------------------------------------------------------- +install(PROGRAMS + src/vision_pose_converter_node.py + DESTINATION lib/${PROJECT_NAME}) + +# --------------------------------------------------------------------------- +# Launch and config files +# --------------------------------------------------------------------------- +install(DIRECTORY launch/ + DESTINATION share/${PROJECT_NAME}/launch) + +install(DIRECTORY config/ + DESTINATION share/${PROJECT_NAME}/config) + +install(DIRECTORY include/ + DESTINATION include) + +# --------------------------------------------------------------------------- +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() + + # --- gtest: pure-logic unit tests (no NatNet SDK, no rclcpp) --- + find_package(ament_cmake_gtest REQUIRED) + ament_add_gtest(test_natnet_logic test/test_natnet_logic.cpp) + target_include_directories(test_natnet_logic PRIVATE + $ + $) +endif() + +ament_package() diff --git a/robot/ros_ws/src/perception/natnet_ros2/README.md b/robot/ros_ws/src/perception/natnet_ros2/README.md new file mode 100644 index 000000000..23b5d13d3 --- /dev/null +++ b/robot/ros_ws/src/perception/natnet_ros2/README.md @@ -0,0 +1,192 @@ +# NatNet ROS 2 Wrapper + +OptiTrack NatNet ROS 2 wrapper for motion capture integration in AirStack (optional). Receives rigid body pose data from an external Motive PC via NatNet UDP protocol and publishes into the AirStack perception layer. + +**Note:** This module is only required if you intend to use OptiTrack Motive motion capture systems. If you do not plan to use OptiTrack, you can skip the NatNet SDK setup with `airstack setup --no-natnet`. + +## Overview + +This module provides a bridge between OptiTrack Motive motion capture systems and the AirStack autonomy stack. It: + +- Receives **NatNet UDP packets** from an external Motive PC (configurable IP/port) +- **Decodes motion capture frames** containing rigid body positions and orientations +- **Publishes pose data** to the AirStack perception layer in standard ROS 2 formats +- **Supports multi-robot** via ROBOT_NAME namespacing +- **Optionally bridges** to MAVROS for PX4 external pose feedback +- **Respects OptiTrack licensing** by keeping the NatNet SDK external (host-side download with explicit consent) + +## Architecture + +``` +Motive (External PC) + ↓ NatNet UDP (port 1511) + ↓ +NatNet ROS 2 Node + ├→ /robot_1/perception/optitrack/{body_name} (PoseStamped) + ├→ /robot_1/perception/optitrack/pose_cov (PoseWithCovarianceStamped) + └→ (Optional) /robot_1/mavros/vision_pose/pose (for PX4) +``` + +## Interfaces + +### Inputs + +- **Network**: NatNet UDP stream from Motive PC (external network) +- **Configuration**: `natnet_config.yaml` with server IP, port, body names, covariance + +### Outputs + +#### Direct OptiTrack Poses +- **Topic**: `/{ROBOT_NAME}/perception/optitrack/{body_name}` (PoseStamped) +- **Type**: `geometry_msgs/PoseStamped` +- **Description**: Individual rigid body poses +- **Enabled by**: `publish_direct_optitrack: true` in config + +#### Aggregated Pose with Covariance +- **Topic**: `/{ROBOT_NAME}/perception/optitrack/pose_cov` (PoseWithCovarianceStamped) +- **Type**: `geometry_msgs/PoseWithCovarianceStamped` +- **Description**: Primary pose with uncertainty +- **Enabled by**: `publish_to_mavros: true` or always available + +#### MAVROS Vision Pose Bridge (Optional) +- **Topic**: `/{ROBOT_NAME}/mavros/vision_pose/pose` (PoseStamped) +- **Type**: `geometry_msgs/PoseStamped` (converted for MAVROS/PX4) +- **Description**: External pose feedback for PX4 EKF fusion +- **Enabled by**: `publish_to_mavros: true` in config + +## Configuration + +Edit `config/natnet_config.yaml`: + +```yaml +natnet: + # Motive PC network settings + server_ip: "192.168.1.1" # IP of Motive PC + data_port: 1511 # Local UDP port bound for NatNet data stream + + # Rigid body tracking + body_names: + - "quad_1" + - "marker_cloud_1" + + # Publishing behavior + publish_direct_optitrack: true # Publish individual body topics + publish_to_mavros: false # Bridge to MAVROS for PX4 + + # Frame IDs + frame_id: "world" + child_frame_id: "base_link" + + # Covariance (default uncertainty) + position_covariance: [0.1, 0.0, 0.0, ...] + orientation_covariance: [0.01, 0.0, 0.0, ...] +``` + +## Launch + +### Basic launch + +Parameters come from `config/natnet_config.yaml` (network, body, covariance). Optional overrides: + +```bash +ros2 launch natnet_ros2 natnet_ros2.launch.py \ + config_file:=/path/to/custom_natnet.yaml \ + vision_pose_config_file:=/path/to/custom_vision_pose.yaml \ + use_sim_time:=true +``` + +### MAVROS bridge + +Set `publish_to_mavros: true` in `natnet_config.yaml`. The launch file reads `publish_to_mavros` and `body_name` from that YAML to decide whether to include `vision_pose_converter.launch.xml`. + +### From perception bringup + +With `LAUNCH_NATNET=true` in `.env`, `perception.launch.xml` includes `natnet_ros2.launch.py`. + +## Dependencies + +### Runtime +- `rclpy` — ROS 2 Python client +- `geometry_msgs` — Standard pose message types +- `tf_transformations` — Quaternion and rotation utilities +- `mavros_msgs` — Optional, for MAVROS bridge + +### Required +- **OptiTrack NatNet SDK** (Linux SDK) — **REQUIRED**, downloaded via `airstack setup` + +### Installation +To install the NatNet SDK and accept the license: +```bash +airstack setup +``` +The SDK will be installed into `robot/ros_ws/src/perception/natnet_ros2/lib/` and `robot/ros_ws/src/perception/natnet_ros2/include/natnet/` after accepting the OptiTrack License Agreement. + +## Implementation Details + +### Protocol Support +- **NatNet Version**: 4.4+ (SDK handles protocol negotiation) +- **Packet Type**: Frame of Data with rigid bodies and markers +- **Transport**: UDP (configurable port, default 1511) +- **SDK**: OptiTrack NatNet SDK handles all protocol parsing + +### Multi-Robot Support +Each container instance gets its own `ROBOT_NAME` and `ROS_DOMAIN_ID`: +- Topic published as: `/{ROBOT_NAME}/perception/optitrack/{body_name}` +- Supported via launch file argument forwarding + +### Error Handling +- Invalid/malformed packets are skipped with debug logging +- Lost connectivity logs warnings; gracefully recovers when stream resumes +- Covariance in config allows tuning uncertainty per deployment + +## Testing + +### With Real Motive +1. Ensure Motive PC and robot are on same network +2. Configure server IP in `natnet_config.yaml` +3. Launch the node: + ```bash + ros2 launch natnet_ros2 natnet_ros2.launch.py + ``` +4. Verify topics: + ```bash + ros2 topic echo /robot_1/perception/optitrack/pose_cov + ``` + +### Without Real Hardware (Mock) +TODO: Implement Motive simulator in Isaac Sim to generate fake NatNet packets + +## Known Limitations + +- Rigid body body name mapping currently uses generic `body_N` naming; TODO: load from config +- MAVROS bridge converter is a stub; needs coordinate frame transformation for PX4 +- No support for skeleton tracking or labeled markers yet (future enhancement) + +## References + +- [OptiTrack NatNet Protocol Documentation](https://v20.wiki.optitrack.com/index.php?title=NatNet) +- [NatNet SDK Download](https://optitrack.com/software/natnet-sdk/) +- [MAVROS Vision Pose Plugin](https://github.com/mavlink/mavros/tree/master/mavros_plugins#vision_pose_estimation) + +## Troubleshooting + +### No data being received +- Check Motive PC IP address in config +- Verify UDP port is not blocked by firewall +- Use `ros2 topic hz` to check if data is arriving + +### Topics not published +- Check `ros2 node list` — should see `natnet_ros2_node` +- Check `ros2 topic list | grep optitrack` — should see published topics +- Look at logs: `ros2 node info natnet_ros2_node` + +### Low frame rate or dropped frames +- Reduce other network traffic +- Check NatNet streaming rate in Motive (default 120 Hz) +- Monitor CPU usage: `docker stats` + +## License + +Apache 2.0 — See [LICENSE](../../../LICENSE) + +**Note on NatNet SDK Licensing**: The OptiTrack NatNet SDK is proprietary software governed by the OptiTrack Software License Agreement. Users download and install the SDK locally under their own license compliance. AirStack does not redistribute the SDK and remains fully open-source. diff --git a/robot/ros_ws/src/perception/natnet_ros2/config/natnet_config.yaml b/robot/ros_ws/src/perception/natnet_ros2/config/natnet_config.yaml new file mode 100644 index 000000000..69fc11d1c --- /dev/null +++ b/robot/ros_ws/src/perception/natnet_ros2/config/natnet_config.yaml @@ -0,0 +1,51 @@ +# NatNet ROS 2 parameters — loaded by natnet_ros2.launch.py (NatNet node + MAVROS gate). +# publish_to_mavros / body_name are read by the launch file to decide vision_pose_converter include. +# +# Use /** so parameters apply regardless of namespace (e.g. /robot_1/perception/natnet_ros2_node). +# See: https://docs.ros.org/en/humble/Tutorials/Beginner-CLI-Tools/Understanding-ROS2-Parameters.html + +/**: + ros__parameters: + # IP address of the PC running Motive (OptiTrack server). + # Change this to match your local network before launching NatNet. + server_ip: "192.168.1.100" + # Motive learns unicast destination from outbound UDP source IP — bind explicitly when you have + # multiple NICs (e.g. Docker 172.17.* vs LAN). + client_ip: "0.0.0.0" + + command_port: 1510 + data_port: 1511 + + # "unicast" — point-to-point; Motive streams directly to this machine's IP. + # Requires Motive unicast streaming enabled and client_ip set + # to the correct NIC when multiple interfaces are present. + # "multicast" — Motive broadcasts to a multicast group; any machine on the + # subnet that joins the group receives all body data. + # Use for multi-robot setups where every robot receives the + # full frame and filters by body_id. + connection_type: "unicast" + + # Only used when connection_type = "multicast". + # Must match Motive > Edit > Preferences > Data Streaming > Multicast Interface. + # OptiTrack default is 239.255.42.99. + multicast_address: "239.255.42.99" + + # Name of the rigid body as defined in Motive. Must match exactly (case-sensitive). + body_name: "Drone" + body_id: -1 + + publish_direct_optitrack: true + publish_to_mavros: true + + frame_id: "world" + debug: false + + position_covariance: + [0.1, 0.0, 0.0, + 0.0, 0.1, 0.0, + 0.0, 0.0, 0.1] + + orientation_covariance: + [0.01, 0.0, 0.0, + 0.0, 0.01, 0.0, + 0.0, 0.0, 0.01] diff --git a/robot/ros_ws/src/perception/natnet_ros2/config/vision_pose_converter.yaml b/robot/ros_ws/src/perception/natnet_ros2/config/vision_pose_converter.yaml new file mode 100644 index 000000000..a52181786 --- /dev/null +++ b/robot/ros_ws/src/perception/natnet_ros2/config/vision_pose_converter.yaml @@ -0,0 +1,12 @@ +# Vision pose converter → MAVROS bridge parameters. +# Loaded by vision_pose_converter.launch.xml via . +# $(env ROBOT_NAME ...) is expanded by launch substitution. + +/**: + ros__parameters: + frame_id: "world" + child_frame_id: "$(env ROBOT_NAME robot_1)/base_link" + # Normalise quaternion to canonical form (qw >= 0) before publishing. + # Recommended for ArduPilot EKF3 and any consumer sensitive to sign flips. + # PX4 EKF2 handles either sign internally, so this is optional for PX4. + canonical_quaternion: true diff --git a/robot/ros_ws/src/perception/natnet_ros2/env-hooks/natnet_library_path.dsv.in b/robot/ros_ws/src/perception/natnet_ros2/env-hooks/natnet_library_path.dsv.in new file mode 100644 index 000000000..8046498e2 --- /dev/null +++ b/robot/ros_ws/src/perception/natnet_ros2/env-hooks/natnet_library_path.dsv.in @@ -0,0 +1 @@ +prepend-non-duplicate;LD_LIBRARY_PATH;lib/natnet_ros2 diff --git a/robot/ros_ws/src/perception/natnet_ros2/include/natnet_ros2/natnet_client_adapter.hpp b/robot/ros_ws/src/perception/natnet_ros2/include/natnet_ros2/natnet_client_adapter.hpp new file mode 100644 index 000000000..04b3638ef --- /dev/null +++ b/robot/ros_ws/src/perception/natnet_ros2/include/natnet_ros2/natnet_client_adapter.hpp @@ -0,0 +1,60 @@ +// Copyright (c) 2024 Carnegie Mellon University +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// +// natnet_client_adapter.hpp — declaration of NatNetClientAdapter. +// +// NatNetClientAdapter wraps the NatNet SDK's NatNetClient and implements +// INatNetClient. It is the only place in the codebase that includes NatNet +// SDK headers; unit tests use FakeNatNetClient instead. +// +// Implementation: src/natnet_client_adapter.cpp + +#pragma once + +#include "natnet_ros2/natnet_logic.hpp" + +#include +#include +#include + +// Forward-declare the SDK type so this header stays SDK-header-free. +class NatNetClient; + +namespace natnet_ros2 +{ + +class NatNetClientAdapter : public INatNetClient +{ +public: + NatNetClientAdapter(); + ~NatNetClientAdapter() override; + + NatNetResult connect(const ConnectConfig & cfg) override; + bool get_server_info(ServerInfo & out) override; + std::vector get_body_descriptors() override; + void set_frame_callback(std::function cb) override; + void disconnect() override; + +private: + std::unique_ptr client_; + std::function user_cb_; +}; + +} // namespace natnet_ros2 diff --git a/robot/ros_ws/src/perception/natnet_ros2/include/natnet_ros2/natnet_logic.hpp b/robot/ros_ws/src/perception/natnet_ros2/include/natnet_ros2/natnet_logic.hpp new file mode 100644 index 000000000..f23216565 --- /dev/null +++ b/robot/ros_ws/src/perception/natnet_ros2/include/natnet_ros2/natnet_logic.hpp @@ -0,0 +1,354 @@ +// Copyright (c) 2024 Carnegie Mellon University +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +// natnet_logic.hpp — pure C++ helpers for natnet_ros2 (no ROS, no NatNet SDK). +// +// Five responsibility areas: +// +// 1. Covariance assembly +// 2. Topic names +// 3. Connection-configuration helpers (SDK-independent) +// 4. Rigid-body frame helpers (SDK-independent) +// 5. Abstraction seam: INatNetClient interface + negotiation logic +// +// NatNet SDK types (sNatNetClientConnectParams, sRigidBodyData, …) are only +// used inside natnet_ros2_node.cpp and natnet_client_adapter.cpp. +// All logic here uses plain C++ so test_natnet_logic.cpp compiles with only gtest. + +#pragma once + +#include +#include +#include +#include +#include + +namespace natnet_ros2 +{ + +// =========================================================================== +// 1. Covariance +// =========================================================================== + +/// Build a row-major 36-element 6×6 covariance from two flat 3×3 blocks. +/// +/// pos_cov (up to 9 elements) fills the top-left 3×3 block (rows/cols 0-2). +/// ori_cov (up to 9 elements) fills the bottom-right 3×3 block (rows/cols 3-5). +/// All other entries are zero. +inline std::array build_covariance_6x6( + const std::vector & pos_cov, + const std::vector & ori_cov) +{ + std::array cov{}; + cov.fill(0.0); + const int np = static_cast(pos_cov.size()); + const int no = static_cast(ori_cov.size()); + for (int r = 0; r < 3; ++r) { + for (int c = 0; c < 3; ++c) { + const int idx = r * 3 + c; + if (idx < np) { cov[r * 6 + c] = pos_cov[idx]; } + if (idx < no) { cov[(r + 3) * 6 + (c + 3)] = ori_cov[idx]; } + } + } + return cov; +} + + +// =========================================================================== +// 2. Topic names +// =========================================================================== + +/// Base topic for a rigid body: /{robot_name}/perception/optitrack/{body_name} +inline std::string optitrack_topic_base( + const std::string & robot_name, + const std::string & body_name) +{ + return "/" + robot_name + "/perception/optitrack/" + body_name; +} + +/// PoseWithCovarianceStamped topic: …/{body_name}/pose_cov +inline std::string optitrack_pose_cov_topic( + const std::string & robot_name, + const std::string & body_name) +{ + return optitrack_topic_base(robot_name, body_name) + "/pose_cov"; +} + + +// =========================================================================== +// 3. Connection-configuration helpers +// =========================================================================== + +/// Return ct if it is "unicast" or "multicast"; otherwise return "unicast". +inline std::string validate_connection_type(const std::string & ct) +{ + if (ct == "unicast" || ct == "multicast") { return ct; } + return "unicast"; +} + +/// SDK-independent connection configuration aggregate. +/// +/// natnet_ros2_node.cpp converts this into sNatNetClientConnectParams; tests +/// exercise the pure logic without linking the NatNet SDK. +struct ConnectConfig +{ + std::string server_ip = "192.168.1.1"; + std::string client_ip = "0.0.0.0"; + uint16_t command_port = 1510u; + uint16_t data_port = 1511u; + std::string connection_type = "unicast"; ///< validated + std::string multicast_address = "239.255.42.99"; +}; + +/// Build a validated ConnectConfig from raw user-supplied strings. +/// connection_type is normalised via validate_connection_type(). +inline ConnectConfig make_connect_config( + const std::string & server_ip, + const std::string & client_ip, + uint16_t command_port, + uint16_t data_port, + const std::string & connection_type, + const std::string & multicast_address = "239.255.42.99") +{ + return ConnectConfig{ + server_ip, + client_ip, + command_port, + data_port, + validate_connection_type(connection_type), + multicast_address + }; +} + +/// Returns true when the config requests a multicast connection. +inline bool is_multicast(const ConnectConfig & cfg) +{ + return cfg.connection_type == "multicast"; +} + +/// Returns true when the multicast address should be used. +/// When false, the multicast_address field is irrelevant and should be nullptr +/// when passed to sNatNetClientConnectParams. +inline bool needs_multicast_address(const ConnectConfig & cfg) +{ + return is_multicast(cfg); +} + + +// =========================================================================== +// 4. Rigid-body frame helpers (SDK-independent) +// =========================================================================== + +/// Lightweight, SDK-free representation of a single rigid-body sample. +/// natnet_ros2_node.cpp converts sRigidBodyData → RigidBodySample. +struct RigidBodySample +{ + int32_t id = 0; + float x = 0.f; + float y = 0.f; + float z = 0.f; + float qx = 0.f; + float qy = 0.f; + float qz = 0.f; + float qw = 1.f; + int16_t params = 0; ///< NatNet rb.params bitmask +}; + +/// Lightweight, SDK-free representation of one frame of mocap data. +struct FrameSample +{ + int32_t frame_num = 0; + float timestamp = 0.f; + int16_t params = 0; ///< NatNet frame.params bitmask + std::vector bodies; +}; + +/// Returns true when bit 0 of rb.params is set (NatNet: tracking valid). +inline bool is_tracking_valid(int16_t rb_params) +{ + return (rb_params & 0x01) != 0; +} + +/// Returns true when bit 1 of frame.params is set (NatNet: model list changed). +inline bool model_list_changed(int16_t frame_params) +{ + return (frame_params & 0x02) != 0; +} + +/// Returns true when the rigid body should be published. +/// filter_id < 0 means "publish all bodies"; otherwise only the matching ID. +inline bool should_publish_body(int32_t filter_id, int32_t rb_id) +{ + return filter_id < 0 || rb_id == filter_id; +} + +/// Double-precision pose extracted from a RigidBodySample. +struct PoseData +{ + double x = 0.0; + double y = 0.0; + double z = 0.0; + double qx = 0.0; + double qy = 0.0; + double qz = 0.0; + double qw = 1.0; +}; + +/// Convert a RigidBodySample to a double-precision PoseData. +inline PoseData rb_to_pose(const RigidBodySample & rb) +{ + return PoseData{ + static_cast(rb.x), + static_cast(rb.y), + static_cast(rb.z), + static_cast(rb.qx), + static_cast(rb.qy), + static_cast(rb.qz), + static_cast(rb.qw) + }; +} + +/// Fill a 36-element covariance array into a pre-allocated ROS-style covariance +/// field from a pre-built std::array. +/// Returns a copy of the array (ROS msg.covariance = cov6x6_to_array(...)). +inline std::array cov6x6_to_array(const std::array & src) +{ + return src; +} + +// =========================================================================== +// 5. Abstraction seam: INatNetClient + negotiation logic +// =========================================================================== + +/// SDK-independent result codes for connection attempts. +enum class NatNetResult +{ + OK, + NetworkError, + InvalidAddress, + Timeout, + InternalError, +}; + +inline const char * natnet_result_str(NatNetResult r) +{ + switch (r) { + case NatNetResult::OK: return "OK"; + case NatNetResult::NetworkError: return "NetworkError"; + case NatNetResult::InvalidAddress: return "InvalidAddress"; + case NatNetResult::Timeout: return "Timeout"; + case NatNetResult::InternalError: return "InternalError"; + } + return "Unknown"; +} + +/// Server identity returned after a successful connection. +struct ServerInfo +{ + bool host_present = false; + std::string host_app_name; + int host_app_version[4] = {}; ///< major.minor.build.revision + int natnet_version[4] = {}; ///< major.minor.build.revision +}; + +/// SDK-independent description of one rigid-body asset. +struct BodyDescriptor +{ + int32_t id = 0; + std::string name; + int32_t parent_id = -1; ///< >= 0 → skeleton bone; skip for top-level publishing +}; + +/// Result of the connect + GetServerDescription handshake. +struct NegotiationResult +{ + bool ok = false; + ServerInfo server_info; + std::string log_message; ///< human-readable outcome for the ROS logger +}; + +/// Pure-virtual client interface — implemented by NatNetClientAdapter (production) +/// and FakeNatNetClient (unit tests). +/// +/// Depends only on natnet_logic.hpp types; never includes NatNet SDK headers. +class INatNetClient +{ +public: + virtual ~INatNetClient() = default; + + /// Attempt to connect to a Motive server. + virtual NatNetResult connect(const ConnectConfig & cfg) = 0; + + /// Populate \p out with server identity. Returns false when host info is + /// unavailable (HostPresent == false in the SDK's sServerDescription). + virtual bool get_server_info(ServerInfo & out) = 0; + + /// Return descriptions of all rigid-body assets currently known to Motive. + /// Returns empty on failure; callers should retry on model-list-changed. + virtual std::vector get_body_descriptors() = 0; + + /// Register a callback invoked on every incoming frame. + /// The callback is called from the SDK receive thread. + virtual void set_frame_callback(std::function cb) = 0; + + /// Disconnect from the server and release SDK resources. + virtual void disconnect() = 0; +}; + +/// Execute the connect + GetServerDescription handshake and return a structured +/// result. Pure logic: no ROS calls, no SDK types — fully testable with a fake. +inline NegotiationResult negotiate(INatNetClient & client, const ConnectConfig & cfg) +{ + NegotiationResult result; + + const NatNetResult err = client.connect(cfg); + if (err != NatNetResult::OK) { + result.ok = false; + result.log_message = std::string("NatNetClient::Connect failed (") + + natnet_result_str(err) + + ") — server=" + cfg.server_ip + + " port=" + std::to_string(cfg.command_port) + + " type=" + cfg.connection_type; + return result; + } + + result.ok = true; + + const bool host_ok = client.get_server_info(result.server_info); + if (!host_ok || !result.server_info.host_present) { + result.log_message = "Connected to " + cfg.server_ip + + " but GetServerDescription returned no host info."; + } else { + result.log_message = "Connected to Motive '" + + result.server_info.host_app_name + + "' v" + + std::to_string(result.server_info.host_app_version[0]) + + "." + + std::to_string(result.server_info.host_app_version[1]) + + " (NatNet " + + std::to_string(result.server_info.natnet_version[0]) + + "." + + std::to_string(result.server_info.natnet_version[1]) + + ") at " + cfg.server_ip; + } + return result; +} + +} // namespace natnet_ros2 diff --git a/robot/ros_ws/src/perception/natnet_ros2/launch/natnet_ros2.launch.py b/robot/ros_ws/src/perception/natnet_ros2/launch/natnet_ros2.launch.py new file mode 100644 index 000000000..cb7c178d8 --- /dev/null +++ b/robot/ros_ws/src/perception/natnet_ros2/launch/natnet_ros2.launch.py @@ -0,0 +1,117 @@ +#!/usr/bin/env python3 +"""Bring up NatNet node; optionally MAVROS bridge per natnet_config.yaml. + +natnet_ros2_node is a C++ executable that requires the OptiTrack NatNet SDK. +If the SDK was not installed (``airstack setup`` not run) and the workspace +has not been rebuilt, launching this file will raise a RuntimeError with +instructions. Set LAUNCH_NATNET=false in .env to disable OptiTrack entirely. +""" + +from __future__ import annotations + +import os +from pathlib import Path +from typing import cast + +import yaml +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, OpaqueFunction +from launch.launch_description_sources import FrontendLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node +from launch_ros.parameter_descriptions import ParameterFile + + +def _ros_params_from_file(config_path: str) -> dict: + """Parse /** / ros__parameters block from a ROS 2 parameter YAML.""" + path = Path(config_path) + if not path.is_file(): + return {} + with path.open(encoding='utf-8') as f: + data = yaml.safe_load(f) + if not isinstance(data, dict): + return {} + block = data.get('/**') + if not isinstance(block, dict): + return {} + params = block.get('ros__parameters', {}) + return cast(dict, params) if isinstance(params, dict) else {} + + +def generate_launch_description() -> LaunchDescription: + pkg_share = get_package_share_directory('natnet_ros2') + default_natnet_yaml = os.path.join(pkg_share, 'config', 'natnet_config.yaml') + default_vp_yaml = os.path.join(pkg_share, 'config', 'vision_pose_converter.yaml') + + config_file = LaunchConfiguration('config_file') + vision_pose_config_file = LaunchConfiguration('vision_pose_config_file') + use_sim_time = LaunchConfiguration('use_sim_time') + + def launch_setup(context, *_args, **_kwargs): + cfg_path = config_file.perform(context) + vp_path = vision_pose_config_file.perform(context) + ust = use_sim_time.perform(context) + + ros_params = _ros_params_from_file(cfg_path) + publish_mavros = bool(ros_params.get('publish_to_mavros', False)) + body_name = str(ros_params.get('body_name', 'robot_1')) + + # pkg_share = /share/natnet_ros2 → go up two levels to reach , + # then down into lib/natnet_ros2/ where colcon installs executables. + pkg_share = get_package_share_directory('natnet_ros2') + node_path = Path(pkg_share).parent.parent / 'lib' / 'natnet_ros2' / 'natnet_ros2_node' + if not node_path.exists(): + raise RuntimeError( + 'natnet_ros2_node executable not found — NatNet SDK is not installed.\n' + "Run 'airstack setup' to download and install the OptiTrack NatNet SDK,\n" + 'then rebuild the workspace: bws --packages-select natnet_ros2\n' + 'Or set LAUNCH_NATNET=false in .env to disable OptiTrack.' + ) + + actions = [ + Node( + package='natnet_ros2', + executable='natnet_ros2_node', + name='natnet_ros2_node', + output='screen', + parameters=[ParameterFile(config_file, allow_substs=True)], + ), + ] + + if publish_mavros: + actions.append( + IncludeLaunchDescription( + FrontendLaunchDescriptionSource( + os.path.join(pkg_share, 'launch', 'vision_pose_converter.launch.xml'), + ), + launch_arguments=[ + ('config_file', vp_path), + ('body_name', body_name), + ('use_sim_time', ust), + ], + ), + ) + return actions + + return LaunchDescription( + [ + DeclareLaunchArgument( + 'config_file', + default_value=default_natnet_yaml, + description='NatNet parameter YAML (/** ros__parameters). ' + 'publish_to_mavros and body_name control MAVROS include.', + ), + DeclareLaunchArgument( + 'vision_pose_config_file', + default_value=default_vp_yaml, + description='vision_pose_converter parameter YAML.', + ), + DeclareLaunchArgument( + 'use_sim_time', + default_value='false', + description='Forwarded to vision_pose_converter.launch.xml.', + ), + OpaqueFunction(function=launch_setup), + ], + ) diff --git a/robot/ros_ws/src/perception/natnet_ros2/launch/vision_pose_converter.launch.xml b/robot/ros_ws/src/perception/natnet_ros2/launch/vision_pose_converter.launch.xml new file mode 100644 index 000000000..aad9c4474 --- /dev/null +++ b/robot/ros_ws/src/perception/natnet_ros2/launch/vision_pose_converter.launch.xml @@ -0,0 +1,45 @@ + + + + + + + + + + + + + + + + + + + + + + diff --git a/robot/ros_ws/src/perception/natnet_ros2/package.xml b/robot/ros_ws/src/perception/natnet_ros2/package.xml new file mode 100644 index 000000000..f9632b0f7 --- /dev/null +++ b/robot/ros_ws/src/perception/natnet_ros2/package.xml @@ -0,0 +1,46 @@ + + + + natnet_ros2 + 0.1.0 + + NatNet ROS 2 wrapper for OptiTrack Motive motion capture integration. + Receives NatNet data from external Motive PC via the official NatNet SDK + and publishes pose data into the AirStack perception layer. + + + AirLab CMU + MIT + + ament_cmake + + + rclcpp + geometry_msgs + nav_msgs + + + rclpy + tf_transformations + airstack_msgs + + + mavros_msgs + + ament_index_python + launch + launch_ros + python3-yaml + + + ament_lint_auto + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + ament_cmake_gtest + + + ament_cmake + + diff --git a/robot/ros_ws/src/perception/natnet_ros2/scripts/download-natnet-sdk.sh b/robot/ros_ws/src/perception/natnet_ros2/scripts/download-natnet-sdk.sh new file mode 100644 index 000000000..e91360197 --- /dev/null +++ b/robot/ros_ws/src/perception/natnet_ros2/scripts/download-natnet-sdk.sh @@ -0,0 +1,167 @@ +#!/bin/bash + +set -euo pipefail + +module_root="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)" +sdk_lib_dir="${module_root}/lib" +sdk_include_dir="${module_root}/include/natnet" + +sdk_archive_name="" +sdk_download_url="" +temp_dir="" + +info() { + printf '[INFO] %s\n' "$1" +} + +warn() { + printf '[WARN] %s\n' "$1" >&2 +} + +fail() { + printf '[ERROR] %s\n' "$1" >&2 + exit 1 +} + +have() { + command -v "$1" >/dev/null 2>&1 +} + +cleanup() { + if [ -n "$temp_dir" ] && [ -d "$temp_dir" ]; then + rm -rf "$temp_dir" + fi +} + +trap cleanup EXIT + +sdk_is_installed() { + [ -f "${sdk_lib_dir}/libNatNet.so" ] || return 1 + [ -n "$(find "${sdk_include_dir}" -maxdepth 1 -type f -name 'NatNet*' -print -quit 2>/dev/null)" ] +} + +choose_archive() { + case "$(uname -m)" in + x86_64) + sdk_archive_name="NatNet_SDK_4.4_ubuntu.tar" + sdk_download_url="https://d2mzlempwep3hb.cloudfront.net/NatNetSDKLinux/ubuntu/${sdk_archive_name}" + info "Using the x86_64 NatNet SDK package" + ;; + aarch64|arm*) + sdk_archive_name="NatNet_SDK_4.4_ubuntu_ARM.tar" + sdk_download_url="https://d2mzlempwep3hb.cloudfront.net/NatNetSDKLinux/ubuntu_arm/${sdk_archive_name}" + info "Using the ARM NatNet SDK package" + ;; + *) + fail "Unsupported architecture: $(uname -m)" + ;; + esac +} + +show_license_notice() { + cat <<'EOF' +=============================================================================== +OptiTrack NatNet SDK License +=============================================================================== + +The NatNet SDK is used for OptiTrack Motive motion capture integration. +It is only required if you intend to use OptiTrack with AirStack. + +The SDK is proprietary and AirStack does not redistribute it. +Please review the OptiTrack terms before continuing: +https://optitrack.com/about/legal/eula + +This installer will download the SDK into the natnet_ros2 module tree: + - lib/libNatNet.so + - include/natnet/NatNet* + +If you do not plan to use OptiTrack, you can skip this step by pressing 'N' or by running: + airstack setup --no-natnet + +=============================================================================== +EOF +} + +download_archive() { + info "Downloading ${sdk_archive_name}" + + if have wget; then + wget -O "${temp_dir}/${sdk_archive_name}" "${sdk_download_url}" + return 0 + fi + + if have curl; then + curl -fsSL -o "${temp_dir}/${sdk_archive_name}" "${sdk_download_url}" + return 0 + fi + + fail "Neither wget nor curl is available. Install one of them and try again." +} + +extract_archive() { + info "Extracting archive" + mkdir -p "${temp_dir}/extract" + tar -xf "${temp_dir}/${sdk_archive_name}" -C "${temp_dir}/extract" +} + +install_files() { + local source_lib + local source_include + + source_lib="$(find "${temp_dir}/extract" -type f -name 'libNatNet.so' -print -quit)" + source_include="$(find "${temp_dir}/extract" -type d -name include -print -quit)" + + [ -n "${source_lib}" ] || fail "libNatNet.so was not found inside the SDK archive" + [ -n "${source_include}" ] || fail "include/ was not found inside the SDK archive" + + mkdir -p "${sdk_lib_dir}" "${sdk_include_dir}" + + info "Installing library and headers into the module tree" + cp "${source_lib}" "${sdk_lib_dir}/" + find "${source_include}" -maxdepth 1 -type f -name 'NatNet*' -exec cp -f {} "${sdk_include_dir}/" \; + + [ -f "${sdk_lib_dir}/libNatNet.so" ] || fail "NatNet library copy did not complete" + [ -n "$(find "${sdk_include_dir}" -maxdepth 1 -type f -name 'NatNet*' -print -quit)" ] || fail "NatNet headers copy did not complete" +} + +main() { + # Non-interactive / CI mode: set NATNET_ACCEPT_LICENSE=1 or pass --accept-license. + # By using this flag you confirm that you have read and accept the OptiTrack + # Software License Agreement (https://optitrack.com/about/legal/eula). + local auto_accept=false + for arg in "$@"; do + [[ "$arg" == "--accept-license" ]] && auto_accept=true + done + [[ "${NATNET_ACCEPT_LICENSE:-0}" == "1" ]] && auto_accept=true + + if sdk_is_installed; then + info "NatNet SDK already installed" + info "Library: ${sdk_lib_dir}/libNatNet.so" + info "Headers: ${sdk_include_dir}" + exit 0 + fi + + choose_archive + show_license_notice + + if [[ "$auto_accept" == "true" ]]; then + info "NATNET_ACCEPT_LICENSE=1 / --accept-license set — accepting license non-interactively" + else + read -r -p "Accept the OptiTrack NatNet SDK license and download the SDK now? [Y/n] " reply + reply="${reply:-y}" + if ! [[ "${reply}" =~ ^[Yy]$ ]]; then + warn "NatNet SDK installation skipped" + exit 1 + fi + fi + + temp_dir="$(mktemp -d "/tmp/natnet-sdk.XXXXXX")" + download_archive + extract_archive + install_files + + info "NatNet SDK installation completed" + info "Rebuild with: colcon build --packages-select natnet_ros2" +} + +main "$@" diff --git a/robot/ros_ws/src/perception/natnet_ros2/src/natnet_client_adapter.cpp b/robot/ros_ws/src/perception/natnet_ros2/src/natnet_client_adapter.cpp new file mode 100644 index 000000000..1cd30286b --- /dev/null +++ b/robot/ros_ws/src/perception/natnet_ros2/src/natnet_client_adapter.cpp @@ -0,0 +1,180 @@ +// Copyright (c) 2024 Carnegie Mellon University +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// +// natnet_client_adapter.cpp — NatNetClientAdapter implementation. +// +// This is the ONLY translation unit that includes NatNet SDK headers. +// All other code (including tests) depends only on INatNetClient. + +#include "natnet_ros2/natnet_client_adapter.hpp" + +// NatNet SDK (bundled: include/natnet/, lib/libNatNet.so) +#include "NatNetClient.h" +#include "NatNetCAPI.h" +#include "NatNetTypes.h" + +#include + +namespace natnet_ros2 +{ + +// --------------------------------------------------------------------------- +// SDK frame callback trampoline — file-scope so it has C linkage compatible +// with the NATNET_CALLCONV calling convention. +// --------------------------------------------------------------------------- +namespace +{ + +// We store the user callback per-adapter instance in a thread-local to avoid +// a global. Limitation: only one adapter instance per thread (sufficient for +// the single-node use case). +thread_local std::function * tl_frame_cb = nullptr; + +void NATNET_CALLCONV sdk_frame_callback(sFrameOfMocapData * data, void * /*ctx*/) +{ + if (!data || !tl_frame_cb || !*tl_frame_cb) { return; } + + FrameSample fs; + fs.frame_num = data->iFrame; + fs.timestamp = data->fTimestamp; + fs.params = static_cast(data->params); + + fs.bodies.reserve(static_cast(data->nRigidBodies)); + for (int i = 0; i < data->nRigidBodies; ++i) { + const sRigidBodyData & rb = data->RigidBodies[i]; + RigidBodySample s; + s.id = rb.ID; + s.x = rb.x; s.y = rb.y; s.z = rb.z; + s.qx = rb.qx; s.qy = rb.qy; s.qz = rb.qz; s.qw = rb.qw; + s.params = static_cast(rb.params); + fs.bodies.push_back(s); + } + + (*tl_frame_cb)(fs); +} + +/// Map NatNet SDK ErrorCode to our NatNetResult. +/// Note: ErrorCode_Timeout was added in NatNet SDK >= 4.5 and is absent in 4.4. +/// If upgrading the SDK, add: case ErrorCode_Timeout: return NatNetResult::Timeout; +NatNetResult from_sdk_error(ErrorCode ec) +{ + switch (ec) { + case ErrorCode_OK: return NatNetResult::OK; + case ErrorCode_Network: return NatNetResult::NetworkError; + case ErrorCode_InvalidArgument: return NatNetResult::InvalidAddress; + default: return NatNetResult::InternalError; + } +} + +} // anonymous namespace + + +// --------------------------------------------------------------------------- +NatNetClientAdapter::NatNetClientAdapter() +: client_(std::make_unique()) +{} + +NatNetClientAdapter::~NatNetClientAdapter() +{ + disconnect(); +} + +// --------------------------------------------------------------------------- +NatNetResult NatNetClientAdapter::connect(const ConnectConfig & cfg) +{ + sNatNetClientConnectParams params; + params.serverAddress = cfg.server_ip.c_str(); + params.localAddress = cfg.client_ip.c_str(); + params.serverCommandPort = cfg.command_port; + params.serverDataPort = cfg.data_port; + + if (is_multicast(cfg)) { + params.connectionType = ConnectionType_Multicast; + params.multicastAddress = cfg.multicast_address.c_str(); + } else { + params.connectionType = ConnectionType_Unicast; + params.multicastAddress = nullptr; + } + + return from_sdk_error(client_->Connect(params)); +} + +// --------------------------------------------------------------------------- +bool NatNetClientAdapter::get_server_info(ServerInfo & out) +{ + sServerDescription desc; + std::memset(&desc, 0, sizeof(desc)); + const ErrorCode ec = client_->GetServerDescription(&desc); + if (ec != ErrorCode_OK) { return false; } + + out.host_present = desc.HostPresent; + out.host_app_name = desc.szHostApp; + for (int i = 0; i < 4; ++i) { + out.host_app_version[i] = static_cast(desc.HostAppVersion[i]); + out.natnet_version[i] = static_cast(desc.NatNetVersion[i]); + } + return true; +} + +// --------------------------------------------------------------------------- +std::vector NatNetClientAdapter::get_body_descriptors() +{ + std::vector result; + + sDataDescriptions * desc_list = nullptr; + if (client_->GetDataDescriptionList(&desc_list) != ErrorCode_OK || !desc_list) { + return result; + } + + for (int i = 0; i < desc_list->nDataDescriptions; ++i) { + const sDataDescription & dd = desc_list->arrDataDescriptions[i]; + if (dd.type != Descriptor_RigidBody || !dd.Data.RigidBodyDescription) { continue; } + const sRigidBodyDescription & rb = *dd.Data.RigidBodyDescription; + + BodyDescriptor bd; + bd.id = rb.ID; + bd.name = rb.szName; + bd.parent_id = rb.parentID; + result.push_back(bd); + } + + NatNet_FreeDescriptions(desc_list); + return result; +} + +// --------------------------------------------------------------------------- +void NatNetClientAdapter::set_frame_callback( + std::function cb) +{ + user_cb_ = std::move(cb); + tl_frame_cb = &user_cb_; + client_->SetFrameReceivedCallback(sdk_frame_callback, nullptr); +} + +// --------------------------------------------------------------------------- +void NatNetClientAdapter::disconnect() +{ + if (client_) { + client_->Disconnect(); + } + tl_frame_cb = nullptr; +} + +} // namespace natnet_ros2 diff --git a/robot/ros_ws/src/perception/natnet_ros2/src/natnet_ros2_node.cpp b/robot/ros_ws/src/perception/natnet_ros2/src/natnet_ros2_node.cpp new file mode 100644 index 000000000..65059f659 --- /dev/null +++ b/robot/ros_ws/src/perception/natnet_ros2/src/natnet_ros2_node.cpp @@ -0,0 +1,325 @@ +// natnet_ros2_node.cpp +// +// ROS 2 NatNet SDK node for OptiTrack Motive integration. +// +// Published topics (per tracked rigid body): +// /{robot_name}/perception/optitrack/{body_name} → PoseStamped +// /{robot_name}/perception/optitrack/{body_name}/pose_cov → PoseWithCovarianceStamped +// +// Parameters (see config/natnet_config.yaml): +// server_ip, client_ip, command_port, data_port, +// body_name, body_id (-1 = all), publish_direct_optitrack, +// frame_id, debug, position_covariance, orientation_covariance +// +// ROBOT_NAME is read from the environment variable set by AirStack's +// robot_name_map resolver at container startup. + +#include +#include +#include +#include +#include +#include +#include + +// ROS 2 +#include "rclcpp/rclcpp.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp" + +// Pure logic + interface (no SDK, testable with FakeNatNetClient) +#include "natnet_ros2/natnet_logic.hpp" +#include "natnet_ros2/natnet_client_adapter.hpp" + + +// --------------------------------------------------------------------------- +// NatNetROS2Node +// --------------------------------------------------------------------------- +class NatNetROS2Node : public rclcpp::Node +{ +public: + using PoseStamped = geometry_msgs::msg::PoseStamped; + using PoseWithCovarianceStamped = geometry_msgs::msg::PoseWithCovarianceStamped; + + struct BodyPublishers + { + rclcpp::Publisher::SharedPtr pose_pub; + rclcpp::Publisher::SharedPtr pose_cov_pub; + }; + + // ----------------------------------------------------------------------- + explicit NatNetROS2Node() + : Node("natnet_ros2_node") + { + // ----- Parameters -------------------------------------------------- + this->declare_parameter("server_ip", "192.168.1.1"); + this->declare_parameter("client_ip", "0.0.0.0"); + this->declare_parameter("command_port", 1510); + this->declare_parameter("data_port", 1511); + this->declare_parameter("connection_type", std::string("unicast")); + this->declare_parameter("multicast_address", std::string("239.255.42.99")); + this->declare_parameter("body_name", "robot_1"); + this->declare_parameter("body_id", -1); + this->declare_parameter("publish_direct_optitrack", true); + this->declare_parameter("publish_to_mavros", false); + this->declare_parameter("frame_id", "world"); + this->declare_parameter("debug", false); + this->declare_parameter( + "position_covariance", + std::vector{0.1,0.,0., 0.,0.1,0., 0.,0.,0.1}); + this->declare_parameter( + "orientation_covariance", + std::vector{0.01,0.,0., 0.,0.01,0., 0.,0.,0.01}); + + // ----- Read parameters --------------------------------------------- + const auto connect_cfg = natnet_ros2::make_connect_config( + this->get_parameter("server_ip").as_string(), + this->get_parameter("client_ip").as_string(), + static_cast(this->get_parameter("command_port").as_int()), + static_cast(this->get_parameter("data_port").as_int()), + this->get_parameter("connection_type").as_string(), + this->get_parameter("multicast_address").as_string()); + + if (connect_cfg.connection_type != + this->get_parameter("connection_type").as_string()) + { + RCLCPP_WARN(get_logger(), + "Unknown connection_type '%s' — falling back to 'unicast'.", + this->get_parameter("connection_type").as_string().c_str()); + } + + body_name_ = this->get_parameter("body_name").as_string(); + body_id_ = static_cast(this->get_parameter("body_id").as_int()); + publish_direct_ = this->get_parameter("publish_direct_optitrack").as_bool(); + frame_id_ = this->get_parameter("frame_id").as_string(); + debug_ = this->get_parameter("debug").as_bool(); + + covariance_6x6_ = natnet_ros2::build_covariance_6x6( + this->get_parameter("position_covariance").as_double_array(), + this->get_parameter("orientation_covariance").as_double_array()); + + const char * rn = std::getenv("ROBOT_NAME"); + robot_name_ = rn ? rn : "robot_1"; + + RCLCPP_INFO(get_logger(), "========================================="); + RCLCPP_INFO(get_logger(), "NatNet ROS 2 Node"); + RCLCPP_INFO(get_logger(), " robot_name: %s", robot_name_.c_str()); + RCLCPP_INFO(get_logger(), " server_ip: %s", connect_cfg.server_ip.c_str()); + RCLCPP_INFO(get_logger(), " command_port: %d", static_cast(connect_cfg.command_port)); + RCLCPP_INFO(get_logger(), " connection_type: %s", connect_cfg.connection_type.c_str()); + if (natnet_ros2::is_multicast(connect_cfg)) { + RCLCPP_INFO(get_logger(), " multicast_addr: %s", connect_cfg.multicast_address.c_str()); + } + RCLCPP_INFO(get_logger(), " body_id: %d (%s)", + static_cast(body_id_), + (body_id_ < 0) ? "track all" : "single body"); + RCLCPP_INFO(get_logger(), "========================================="); + + // Production client — NatNetClientAdapter wraps the SDK + client_ = std::make_unique(); + connect_and_setup(connect_cfg); + + refresh_timer_ = this->create_wall_timer( + std::chrono::seconds(1), + std::bind(&NatNetROS2Node::refresh_descriptions_if_needed, this)); + } + + // ----------------------------------------------------------------------- + ~NatNetROS2Node() + { + if (client_) { client_->disconnect(); } + } + + // ----------------------------------------------------------------------- + // Called from the NatNetClientAdapter's frame trampoline. + // publish() and Clock::now() are thread-safe; pub_mutex_ guards map access. + // ----------------------------------------------------------------------- + void on_frame(const natnet_ros2::FrameSample & frame) + { + if (natnet_ros2::model_list_changed(frame.params)) { + needs_description_refresh_.store(true, std::memory_order_relaxed); + } + + if (debug_) { + RCLCPP_DEBUG(get_logger(), "Frame %d: %zu rigid bodies, ts=%.4f s", + frame.frame_num, frame.bodies.size(), static_cast(frame.timestamp)); + } + + const rclcpp::Time stamp = this->get_clock()->now(); + + for (const auto & rb : frame.bodies) { + if (!natnet_ros2::is_tracking_valid(rb.params)) { + if (debug_) { + RCLCPP_DEBUG(get_logger(), " RB id=%d: tracking invalid, skipping", rb.id); + } + continue; + } + if (!natnet_ros2::should_publish_body(body_id_, rb.id)) { continue; } + + std::lock_guard lock(pub_mutex_); + + const auto pub_it = publishers_.find(rb.id); + if (pub_it == publishers_.end()) { + needs_description_refresh_.store(true, std::memory_order_relaxed); + continue; + } + + const natnet_ros2::PoseData pose = natnet_ros2::rb_to_pose(rb); + const BodyPublishers & bp = pub_it->second; + + if (publish_direct_ && bp.pose_pub) { + PoseStamped msg; + msg.header.frame_id = frame_id_; + msg.header.stamp = stamp; + msg.pose.position.x = pose.x; + msg.pose.position.y = pose.y; + msg.pose.position.z = pose.z; + msg.pose.orientation.x = pose.qx; + msg.pose.orientation.y = pose.qy; + msg.pose.orientation.z = pose.qz; + msg.pose.orientation.w = pose.qw; + bp.pose_pub->publish(msg); + } + + if (bp.pose_cov_pub) { + PoseWithCovarianceStamped cov_msg; + cov_msg.header.frame_id = frame_id_; + cov_msg.header.stamp = stamp; + cov_msg.pose.pose.position.x = pose.x; + cov_msg.pose.pose.position.y = pose.y; + cov_msg.pose.pose.position.z = pose.z; + cov_msg.pose.pose.orientation.x = pose.qx; + cov_msg.pose.pose.orientation.y = pose.qy; + cov_msg.pose.pose.orientation.z = pose.qz; + cov_msg.pose.pose.orientation.w = pose.qw; + cov_msg.pose.covariance = covariance_6x6_; + bp.pose_cov_pub->publish(cov_msg); + } + } + } + +private: + // ----------------------------------------------------------------------- + void connect_and_setup(const natnet_ros2::ConnectConfig & cfg) + { + const natnet_ros2::NegotiationResult neg = + natnet_ros2::negotiate(*client_, cfg); + + if (!neg.ok) { + RCLCPP_ERROR(get_logger(), "%s", neg.log_message.c_str()); + return; + } + + if (neg.server_info.host_present) { + RCLCPP_INFO(get_logger(), "%s", neg.log_message.c_str()); + } else { + RCLCPP_WARN(get_logger(), "%s", neg.log_message.c_str()); + } + + refresh_descriptions_locked(); + + client_->set_frame_callback( + [this](const natnet_ros2::FrameSample & f) { on_frame(f); }); + RCLCPP_INFO(get_logger(), "Frame callback registered — receiving mocap data."); + } + + // ----------------------------------------------------------------------- + void refresh_descriptions_if_needed() + { + if (!needs_description_refresh_.exchange(false, std::memory_order_relaxed)) { + return; + } + RCLCPP_INFO(get_logger(), "Model list change detected — refreshing data descriptions."); + std::lock_guard lock(pub_mutex_); + refresh_descriptions_locked(); + } + + // ----------------------------------------------------------------------- + // Must be called with pub_mutex_ held (or from single-threaded init). + // ----------------------------------------------------------------------- + void refresh_descriptions_locked() + { + if (!client_) { return; } + + // Always ensure the statically-configured body has a publisher + if (body_id_ >= 0) { + ensure_publisher_locked(body_id_, body_name_); + } + + const auto bodies = client_->get_body_descriptors(); + int newly_created = 0; + for (const auto & bd : bodies) { + // Store name for every body (including skeleton bones) + body_names_[bd.id] = bd.name; + + // Skip skeleton bones (parent_id >= 0) + if (bd.parent_id >= 0) { continue; } + + // When tracking a single body, skip others + if (!natnet_ros2::should_publish_body(body_id_, bd.id)) { continue; } + + if (ensure_publisher_locked(bd.id, bd.name)) { ++newly_created; } + } + + if (newly_created > 0) { + RCLCPP_INFO(get_logger(), + "Data descriptions refreshed: %d new publisher(s) created.", newly_created); + } else { + RCLCPP_DEBUG(get_logger(), "Data descriptions refreshed: no new publishers."); + } + } + + // ----------------------------------------------------------------------- + bool ensure_publisher_locked(int32_t id, const std::string & name) + { + if (publishers_.count(id)) { return false; } + + const std::string topic_base = + natnet_ros2::optitrack_topic_base(robot_name_, name); + + BodyPublishers bp; + if (publish_direct_) { + bp.pose_pub = this->create_publisher(topic_base, 10); + } + bp.pose_cov_pub = this->create_publisher( + natnet_ros2::optitrack_pose_cov_topic(robot_name_, name), 10); + + publishers_.emplace(id, std::move(bp)); + + RCLCPP_INFO(get_logger(), + "Publisher registered: id=%d name='%s' → %s[/pose_cov]", + static_cast(id), name.c_str(), topic_base.c_str()); + return true; + } + + // ----------------------------------------------------------------------- + // Parameters / state + std::string body_name_; + int32_t body_id_ = -1; + bool publish_direct_ = true; + std::string frame_id_; + bool debug_ = false; + std::string robot_name_; + + std::array covariance_6x6_{}; + + std::unique_ptr client_; + + std::mutex pub_mutex_; + std::unordered_map body_names_; + std::unordered_map publishers_; + + std::atomic needs_description_refresh_{false}; + rclcpp::TimerBase::SharedPtr refresh_timer_; +}; + + +// --------------------------------------------------------------------------- +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} diff --git a/robot/ros_ws/src/perception/natnet_ros2/src/vision_pose_converter_node.py b/robot/ros_ws/src/perception/natnet_ros2/src/vision_pose_converter_node.py new file mode 100755 index 000000000..9a36f879d --- /dev/null +++ b/robot/ros_ws/src/perception/natnet_ros2/src/vision_pose_converter_node.py @@ -0,0 +1,116 @@ +#!/usr/bin/env python3 + +""" +Vision Pose Converter Node + +Optional converter that bridges NatNet pose data to MAVROS vision_pose format +for PX4 external pose estimation and state fusion. + +Converts from NatNet coordinate frame to a frame suitable for MAVROS. +""" + +import rclpy +from rclpy.node import Node +from geometry_msgs.msg import PoseStamped, PoseWithCovarianceStamped + + +class VisionPoseConverterNode(Node): + """ + Converts NatNet pose to MAVROS vision_pose format. + + Listens to NatNet pose data and publishes to MAVROS for external + pose feedback to PX4 autopilot. + """ + + def __init__(self): + super().__init__('vision_pose_converter') + + self.declare_parameter('frame_id', 'world') + self.declare_parameter('child_frame_id', 'base_link') + self.declare_parameter('canonical_quaternion', True) + + self.frame_id = self.get_parameter('frame_id').value + self.child_frame_id = self.get_parameter('child_frame_id').value + self.canonical_quaternion = self.get_parameter('canonical_quaternion').value + + # Subscribers + self.pose_sub = self.create_subscription( + PoseWithCovarianceStamped, + 'input_pose', + self._on_pose, + 10 + ) + + # Publishers + self.pose_pub = self.create_publisher( + PoseStamped, + 'output_pose', + 10 + ) + self.pose_cov_pub = self.create_publisher( + PoseWithCovarianceStamped, + 'output_pose_cov', + 10 + ) + + self.get_logger().info( + f'Vision pose converter started ' + f'(frame_id={self.frame_id!r}, child_frame_id={self.child_frame_id!r}, ' + f'canonical_quaternion={self.canonical_quaternion})' + ) + + @staticmethod + def _canonical_quaternion(o): + """ + Return the quaternion in canonical form (qw >= 0) by negating all + four components when qw < 0. + + q and -q represent the same 3-D rotation, but some EKF implementations + (ArduPilot EKF3 in particular) are sensitive to sign flips between + consecutive frames. Keeping qw >= 0 guarantees a consistent + representation across the full orientation space. + """ + if o.w < 0.0: + o.x, o.y, o.z, o.w = -o.x, -o.y, -o.z, -o.w + return o + + def _on_pose(self, msg: PoseWithCovarianceStamped): + """ + Callback for incoming NatNet pose. + + Converts and republishes for MAVROS consumption. + Normalises the quaternion to canonical form (qw >= 0) before publishing + so that EKF consumers never see a sign-flip discontinuity. + """ + try: + msg.header.frame_id = self.frame_id + if self.canonical_quaternion: + msg.pose.pose.orientation = self._canonical_quaternion( + msg.pose.pose.orientation + ) + + self.pose_cov_pub.publish(msg) + + pose_msg = PoseStamped() + pose_msg.header = msg.header + pose_msg.pose = msg.pose.pose + self.pose_pub.publish(pose_msg) + + except Exception as e: + self.get_logger().error(f"Error converting pose: {e}") + + +def main(args=None): + """Main entry point""" + rclpy.init(args=args) + try: + node = VisionPoseConverterNode() + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/robot/ros_ws/src/perception/natnet_ros2/test/fake_natnet_client.hpp b/robot/ros_ws/src/perception/natnet_ros2/test/fake_natnet_client.hpp new file mode 100644 index 000000000..3ee0dd8b2 --- /dev/null +++ b/robot/ros_ws/src/perception/natnet_ros2/test/fake_natnet_client.hpp @@ -0,0 +1,163 @@ +// Copyright (c) 2024 Carnegie Mellon University +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// +// fake_natnet_client.hpp — in-process test double for INatNetClient. +// +// Used exclusively in unit tests (test_natnet_logic.cpp). +// Never included in production binaries. +// +// Usage: +// FakeNatNetClient fake; +// fake.connect_result = NatNetResult::OK; +// fake.server_info = { .host_present = true, .host_app_name = "Motive" }; +// fake.body_descriptors = {{ .id=1, .name="Drone", .parent_id=-1 }}; +// +// auto result = natnet_ros2::negotiate(fake, cfg); +// EXPECT_TRUE(result.ok); + +#pragma once + +#include "natnet_ros2/natnet_logic.hpp" + +#include +#include +#include + +namespace natnet_ros2 +{ + +class FakeNatNetClient : public INatNetClient +{ +public: + // ----------------------------------------------------------------------- + // Configurable behaviour — set before calling negotiate() / testing + // ----------------------------------------------------------------------- + + /// What connect() should return. + NatNetResult connect_result = NatNetResult::OK; + + /// What get_server_info() should populate and return. + /// Set host_present = true to simulate a fully-identified server. + ServerInfo server_info; + + /// get_server_info() return value (independent of server_info.host_present, + /// so tests can simulate "SDK call failed" vs. "host not present"). + bool server_info_call_succeeds = true; + + /// What get_body_descriptors() should return. + std::vector body_descriptors; + + // ----------------------------------------------------------------------- + // Call-record state — inspect after exercising the fake + // ----------------------------------------------------------------------- + + bool connect_was_called = false; + ConnectConfig last_connect_config; + + bool server_info_was_called = false; + bool descriptors_was_called = false; + bool set_callback_was_called = false; + bool disconnect_was_called = false; + + /// Frames that were injected via inject_frame(). + int frames_injected = 0; + + // ----------------------------------------------------------------------- + // INatNetClient overrides + // ----------------------------------------------------------------------- + + NatNetResult connect(const ConnectConfig & cfg) override + { + connect_was_called = true; + last_connect_config = cfg; + return connect_result; + } + + bool get_server_info(ServerInfo & out) override + { + server_info_was_called = true; + out = server_info; + return server_info_call_succeeds; + } + + std::vector get_body_descriptors() override + { + descriptors_was_called = true; + return body_descriptors; + } + + void set_frame_callback(std::function cb) override + { + set_callback_was_called = true; + frame_cb_ = cb; + } + + void disconnect() override + { + disconnect_was_called = true; + } + + // ----------------------------------------------------------------------- + // Test helper: push a synthetic frame into the registered callback. + // ----------------------------------------------------------------------- + void inject_frame(const FrameSample & frame) + { + if (frame_cb_) { + ++frames_injected; + frame_cb_(frame); + } + } + + /// Convenience: build and inject a single-body tracking frame. + void inject_body(int32_t id, float x, float y, float z, + float qx = 0.f, float qy = 0.f, + float qz = 0.f, float qw = 1.f, + int16_t rb_params = 0x01 /* tracking valid */, + int16_t frame_params = 0x00) + { + FrameSample f; + f.params = frame_params; + RigidBodySample rb; + rb.id = id; rb.x = x; rb.y = y; rb.z = z; + rb.qx = qx; rb.qy = qy; rb.qz = qz; rb.qw = qw; + rb.params = rb_params; + f.bodies.push_back(rb); + inject_frame(f); + } + + // ----------------------------------------------------------------------- + // Reset all recorded state (keep configuration). + // ----------------------------------------------------------------------- + void reset_records() + { + connect_was_called = false; + server_info_was_called = false; + descriptors_was_called = false; + set_callback_was_called = false; + disconnect_was_called = false; + frames_injected = 0; + last_connect_config = {}; + } + +private: + std::function frame_cb_; +}; + +} // namespace natnet_ros2 diff --git a/robot/ros_ws/src/perception/natnet_ros2/test/test_natnet_logic.cpp b/robot/ros_ws/src/perception/natnet_ros2/test/test_natnet_logic.cpp new file mode 100644 index 000000000..7a144ef9b --- /dev/null +++ b/robot/ros_ws/src/perception/natnet_ros2/test/test_natnet_logic.cpp @@ -0,0 +1,757 @@ +// Copyright (c) 2024 Carnegie Mellon University +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// +// Unit tests for natnet_ros2/natnet_logic.hpp. +// +// NO dependency on the NatNet SDK or rclcpp — compiles with gtest only. +// Run via: +// colcon test --packages-select natnet_ros2 --event-handlers console_direct+ +// colcon test-result --test-result-base build/natnet_ros2 --verbose + +#include +#include "natnet_ros2/natnet_logic.hpp" +#include "fake_natnet_client.hpp" + +using namespace natnet_ros2; + + +// =========================================================================== +// Covariance +// =========================================================================== + +TEST(BuildCovariance6x6, DiagonalBlocksLandInCorrectSlots) +{ + const std::vector pos = {0.1, 0.0, 0.0, + 0.0, 0.1, 0.0, + 0.0, 0.0, 0.1}; + const std::vector ori = {0.01, 0.0, 0.0, + 0.0, 0.01, 0.0, + 0.0, 0.0, 0.01}; + + auto cov = build_covariance_6x6(pos, ori); + + ASSERT_EQ(cov.size(), 36u); + EXPECT_DOUBLE_EQ(cov[0 * 6 + 0], 0.1); + EXPECT_DOUBLE_EQ(cov[1 * 6 + 1], 0.1); + EXPECT_DOUBLE_EQ(cov[2 * 6 + 2], 0.1); + EXPECT_DOUBLE_EQ(cov[3 * 6 + 3], 0.01); + EXPECT_DOUBLE_EQ(cov[4 * 6 + 4], 0.01); + EXPECT_DOUBLE_EQ(cov[5 * 6 + 5], 0.01); +} + +TEST(BuildCovariance6x6, CrossBlockEntriesAreZero) +{ + const std::vector ones(9, 1.0); + auto cov = build_covariance_6x6(ones, ones); + + for (int r = 0; r < 6; ++r) { + for (int c = 0; c < 6; ++c) { + const bool in_pos = (r < 3 && c < 3); + const bool in_ori = (r >= 3 && c >= 3); + if (!in_pos && !in_ori) { + EXPECT_DOUBLE_EQ(cov[r * 6 + c], 0.0) + << "Expected 0 at [" << r << "][" << c << "]"; + } + } + } +} + +TEST(BuildCovariance6x6, OffDiagonalEntriesPreserved) +{ + const std::vector pos = {1, 2, 3, 4, 5, 6, 7, 8, 9}; + const std::vector ori = {0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9}; + + auto cov = build_covariance_6x6(pos, ori); + + EXPECT_DOUBLE_EQ(cov[1 * 6 + 2], 6.0); // pos[1][2] + EXPECT_DOUBLE_EQ(cov[2 * 6 + 0], 7.0); // pos[2][0] + EXPECT_DOUBLE_EQ(cov[3 * 6 + 4], 0.2); // ori[0][1] + EXPECT_DOUBLE_EQ(cov[5 * 6 + 4], 0.8); // ori[2][1] +} + +TEST(BuildCovariance6x6, ShortInputFillsRemainingWithZero) +{ + auto cov = build_covariance_6x6({0.5}, {0.05}); + + EXPECT_DOUBLE_EQ(cov[0 * 6 + 0], 0.5); + EXPECT_DOUBLE_EQ(cov[3 * 6 + 3], 0.05); + EXPECT_DOUBLE_EQ(cov[0 * 6 + 1], 0.0); + EXPECT_DOUBLE_EQ(cov[4 * 6 + 4], 0.0); +} + +TEST(BuildCovariance6x6, EmptyInputsProduceAllZeros) +{ + auto cov = build_covariance_6x6({}, {}); + for (double v : cov) { EXPECT_DOUBLE_EQ(v, 0.0); } +} + +TEST(BuildCovariance6x6, OutputIsExactly36Elements) +{ + EXPECT_EQ(static_cast(build_covariance_6x6({}, {}).size()), 36); +} + + +// =========================================================================== +// Topic names +// =========================================================================== + +TEST(TopicNames, BaseTopicFormat) +{ + EXPECT_EQ(optitrack_topic_base("robot_1", "Drone"), + "/robot_1/perception/optitrack/Drone"); +} + +TEST(TopicNames, PoseCovTopicAppendsSuffix) +{ + const std::string base = optitrack_topic_base("robot_1", "Drone"); + const std::string cov = optitrack_pose_cov_topic("robot_1", "Drone"); + EXPECT_EQ(cov, base + "/pose_cov"); +} + +TEST(TopicNames, DifferentRobotsGetDifferentNamespaces) +{ + EXPECT_NE(optitrack_topic_base("robot_1", "Body"), + optitrack_topic_base("robot_2", "Body")); +} + +TEST(TopicNames, LeadingSlashPresent) +{ + EXPECT_EQ(optitrack_topic_base("robot_1", "Body")[0], '/'); +} + + +// =========================================================================== +// Server negotiation — validate_connection_type +// =========================================================================== + +TEST(ValidateConnectionType, UnicastPassesThrough) +{ + EXPECT_EQ(validate_connection_type("unicast"), "unicast"); +} + +TEST(ValidateConnectionType, MulticastPassesThrough) +{ + EXPECT_EQ(validate_connection_type("multicast"), "multicast"); +} + +TEST(ValidateConnectionType, UnknownFallsBackToUnicast) +{ + EXPECT_EQ(validate_connection_type("broadcast"), "unicast"); + EXPECT_EQ(validate_connection_type(""), "unicast"); + EXPECT_EQ(validate_connection_type("UDP"), "unicast"); +} + +TEST(ValidateConnectionType, CaseSensitiveFallsBack) +{ + EXPECT_EQ(validate_connection_type("Unicast"), "unicast"); + EXPECT_EQ(validate_connection_type("MULTICAST"), "unicast"); +} + + +// =========================================================================== +// Server negotiation — ConnectConfig + make_connect_config +// =========================================================================== + +TEST(ConnectConfig, DefaultsAreUnicast) +{ + const ConnectConfig cfg{}; + EXPECT_EQ(cfg.connection_type, "unicast"); + EXPECT_FALSE(is_multicast(cfg)); +} + +TEST(ConnectConfig, UnicastConfigNotMulticast) +{ + const auto cfg = make_connect_config( + "10.0.0.1", "0.0.0.0", 1510, 1511, "unicast"); + EXPECT_FALSE(is_multicast(cfg)); + EXPECT_FALSE(needs_multicast_address(cfg)); +} + +TEST(ConnectConfig, MulticastConfigIsMulticast) +{ + const auto cfg = make_connect_config( + "10.0.0.1", "0.0.0.0", 1510, 1511, "multicast", "239.255.42.99"); + EXPECT_TRUE(is_multicast(cfg)); + EXPECT_TRUE(needs_multicast_address(cfg)); + EXPECT_EQ(cfg.multicast_address, "239.255.42.99"); +} + +TEST(ConnectConfig, InvalidConnectionTypeFallsBackToUnicast) +{ + const auto cfg = make_connect_config( + "10.0.0.1", "0.0.0.0", 1510, 1511, "broadcast"); + EXPECT_EQ(cfg.connection_type, "unicast"); + EXPECT_FALSE(is_multicast(cfg)); +} + +TEST(ConnectConfig, PortsArePreserved) +{ + const auto cfg = make_connect_config( + "192.168.0.100", "192.168.0.200", 9000u, 9001u, "unicast"); + EXPECT_EQ(cfg.server_ip, "192.168.0.100"); + EXPECT_EQ(cfg.client_ip, "192.168.0.200"); + EXPECT_EQ(cfg.command_port, 9000u); + EXPECT_EQ(cfg.data_port, 9001u); +} + +TEST(ConnectConfig, CustomMulticastAddress) +{ + const auto cfg = make_connect_config( + "10.0.0.1", "0.0.0.0", 1510, 1511, "multicast", "239.0.0.1"); + EXPECT_EQ(cfg.multicast_address, "239.0.0.1"); +} + +TEST(ConnectConfig, UnicastAddressFieldIgnored) +{ + // multicast_address is still stored but should not be passed to the SDK + const auto cfg = make_connect_config( + "10.0.0.1", "0.0.0.0", 1510, 1511, "unicast", "239.255.42.99"); + EXPECT_FALSE(needs_multicast_address(cfg)); +} + + +// =========================================================================== +// Data streaming — is_tracking_valid +// =========================================================================== + +TEST(IsTrackingValid, Bit0SetMeansValid) +{ + EXPECT_TRUE(is_tracking_valid(0x01)); + EXPECT_TRUE(is_tracking_valid(0x03)); // bits 0 and 1 + EXPECT_TRUE(is_tracking_valid(0xFF)); +} + +TEST(IsTrackingValid, Bit0ClearMeansInvalid) +{ + EXPECT_FALSE(is_tracking_valid(0x00)); + EXPECT_FALSE(is_tracking_valid(0x02)); // only bit 1 set + EXPECT_FALSE(is_tracking_valid(0xFE)); // all bits except 0 +} + + +// =========================================================================== +// Data streaming — model_list_changed +// =========================================================================== + +TEST(ModelListChanged, Bit1SetMeansChanged) +{ + EXPECT_TRUE(model_list_changed(0x02)); + EXPECT_TRUE(model_list_changed(0x03)); + EXPECT_TRUE(model_list_changed(0xFF)); +} + +TEST(ModelListChanged, Bit1ClearMeansNotChanged) +{ + EXPECT_FALSE(model_list_changed(0x00)); + EXPECT_FALSE(model_list_changed(0x01)); // only bit 0 + EXPECT_FALSE(model_list_changed(0xFD)); // all bits except 1 +} + + +// =========================================================================== +// Data streaming — should_publish_body +// =========================================================================== + +TEST(ShouldPublishBody, NegativeFilterMeansPublishAll) +{ + EXPECT_TRUE(should_publish_body(-1, 0)); + EXPECT_TRUE(should_publish_body(-1, 1)); + EXPECT_TRUE(should_publish_body(-1, 999)); +} + +TEST(ShouldPublishBody, ZeroFilterAllowsOnlyId0) +{ + EXPECT_TRUE(should_publish_body(0, 0)); + EXPECT_FALSE(should_publish_body(0, 1)); + EXPECT_FALSE(should_publish_body(0, 999)); +} + +TEST(ShouldPublishBody, PositiveFilterMatchesExact) +{ + EXPECT_TRUE(should_publish_body(5, 5)); + EXPECT_FALSE(should_publish_body(5, 4)); + EXPECT_FALSE(should_publish_body(5, 6)); +} + + +// =========================================================================== +// Data streaming — rb_to_pose (sample data conversion) +// =========================================================================== + +TEST(RbToPose, PositionComponentsConvertedToDouble) +{ + RigidBodySample rb; + rb.x = 1.5f; rb.y = -2.25f; rb.z = 0.5f; + rb.qx = 0.f; rb.qy = 0.f; rb.qz = 0.f; rb.qw = 1.f; + + const PoseData p = rb_to_pose(rb); + + EXPECT_DOUBLE_EQ(p.x, static_cast(1.5f)); + EXPECT_DOUBLE_EQ(p.y, static_cast(-2.25f)); + EXPECT_DOUBLE_EQ(p.z, static_cast(0.5f)); +} + +TEST(RbToPose, OrientationComponentsConvertedToDouble) +{ + RigidBodySample rb; + rb.x = 0.f; rb.y = 0.f; rb.z = 0.f; + // 90-degree rotation about Z: qw = cos(45°), qz = sin(45°) + rb.qx = 0.f; + rb.qy = 0.f; + rb.qz = 0.7071068f; + rb.qw = 0.7071068f; + + const PoseData p = rb_to_pose(rb); + + EXPECT_NEAR(p.qz, 0.7071068, 1e-6); + EXPECT_NEAR(p.qw, 0.7071068, 1e-6); + EXPECT_DOUBLE_EQ(p.qx, 0.0); + EXPECT_DOUBLE_EQ(p.qy, 0.0); +} + +TEST(RbToPose, IdentityOrientationPreserved) +{ + RigidBodySample rb; // default: x=y=z=0, qw=1 + const PoseData p = rb_to_pose(rb); + + EXPECT_DOUBLE_EQ(p.x, 0.0); + EXPECT_DOUBLE_EQ(p.y, 0.0); + EXPECT_DOUBLE_EQ(p.z, 0.0); + EXPECT_DOUBLE_EQ(p.qx, 0.0); + EXPECT_DOUBLE_EQ(p.qy, 0.0); + EXPECT_DOUBLE_EQ(p.qz, 0.0); + EXPECT_DOUBLE_EQ(p.qw, 1.0); +} + +TEST(RbToPose, NegativeCoordinates) +{ + RigidBodySample rb; + rb.x = -10.f; rb.y = -20.f; rb.z = -30.f; + rb.qw = 1.f; + + const PoseData p = rb_to_pose(rb); + + EXPECT_DOUBLE_EQ(p.x, static_cast(-10.f)); + EXPECT_DOUBLE_EQ(p.y, static_cast(-20.f)); + EXPECT_DOUBLE_EQ(p.z, static_cast(-30.f)); +} + + +// =========================================================================== +// Data streaming — FrameSample helpers (integration-style scenarios) +// =========================================================================== + +// Simulate a frame where one body is tracking and one is not. +TEST(FrameSample, TrackingFilterApplied) +{ + FrameSample frame; + frame.frame_num = 42; + frame.timestamp = 1.234f; + frame.params = 0x00; + + RigidBodySample tracking, lost; + tracking.id = 1; + tracking.params = 0x01; // valid + lost.id = 2; + lost.params = 0x00; // invalid + + frame.bodies = {tracking, lost}; + + int published = 0; + for (const auto & rb : frame.bodies) { + if (is_tracking_valid(rb.params)) { + ++published; + } + } + EXPECT_EQ(published, 1); +} + +// Simulate a frame that signals model-list changed while also carrying data. +TEST(FrameSample, ModelListChangedFlagDetected) +{ + FrameSample frame; + frame.params = 0x02; // bit 1 set + + EXPECT_TRUE(model_list_changed(frame.params)); +} + +// Simulate single-body tracking filter: only body id=3 should be published. +TEST(FrameSample, SingleBodyFilterSelectsCorrectBody) +{ + FrameSample frame; + frame.params = 0x00; + + for (int id : {1, 2, 3, 4, 5}) { + RigidBodySample rb; + rb.id = id; + rb.params = 0x01; // all tracking valid + frame.bodies.push_back(rb); + } + + constexpr int32_t filter = 3; + std::vector published; + + for (const auto & rb : frame.bodies) { + if (is_tracking_valid(rb.params) && should_publish_body(filter, rb.id)) { + published.push_back(rb.id); + } + } + + ASSERT_EQ(published.size(), 1u); + EXPECT_EQ(published[0], 3); +} + +// Simulate all-body mode: every valid body gets a PoseData. +TEST(FrameSample, AllBodyModePublishesAllTrackedBodies) +{ + FrameSample frame; + for (int id = 1; id <= 4; ++id) { + RigidBodySample rb; + rb.id = id; + rb.x = static_cast(id); + rb.params = (id % 2 == 0) ? int16_t(0x01) : int16_t(0x00); // even = valid + frame.bodies.push_back(rb); + } + + std::vector out; + for (const auto & rb : frame.bodies) { + if (is_tracking_valid(rb.params) && should_publish_body(-1, rb.id)) { + out.push_back(rb_to_pose(rb)); + } + } + + // Bodies 2 and 4 are valid + ASSERT_EQ(out.size(), 2u); + EXPECT_DOUBLE_EQ(out[0].x, static_cast(2.f)); + EXPECT_DOUBLE_EQ(out[1].x, static_cast(4.f)); +} + +// Verify covariance is stamped into the output as expected. +TEST(FrameSample, CovarianceStampedIntoMessage) +{ + const std::vector pos_cov(9, 0.1); + const std::vector ori_cov(9, 0.01); + const auto cov = build_covariance_6x6(pos_cov, ori_cov); + + // Simulate what natnet_ros2_node.cpp does when building PoseWithCovarianceStamped + std::array msg_covariance = cov6x6_to_array(cov); + + // Position diagonal + EXPECT_DOUBLE_EQ(msg_covariance[0 * 6 + 0], 0.1); + EXPECT_DOUBLE_EQ(msg_covariance[1 * 6 + 1], 0.1); + EXPECT_DOUBLE_EQ(msg_covariance[2 * 6 + 2], 0.1); + // Orientation diagonal + EXPECT_DOUBLE_EQ(msg_covariance[3 * 6 + 3], 0.01); + EXPECT_DOUBLE_EQ(msg_covariance[4 * 6 + 4], 0.01); + EXPECT_DOUBLE_EQ(msg_covariance[5 * 6 + 5], 0.01); + // Cross-block zeros + EXPECT_DOUBLE_EQ(msg_covariance[0 * 6 + 3], 0.0); + EXPECT_DOUBLE_EQ(msg_covariance[3 * 6 + 0], 0.0); +} + + +// =========================================================================== +// Server negotiation — negotiate() + FakeNatNetClient +// =========================================================================== + +// --------------- helpers --------------------------------------------------- + +static ConnectConfig make_test_cfg(const std::string & ct = "unicast") +{ + return make_connect_config("192.168.1.100", "0.0.0.0", 1510u, 1511u, ct); +} + +static ServerInfo make_server_info(bool present = true, + const std::string & app = "Motive", + int vmaj = 3, int vmin = 1, + int nnmaj = 4, int nnmin = 1) +{ + ServerInfo si; + si.host_present = present; + si.host_app_name = app; + si.host_app_version[0] = vmaj; + si.host_app_version[1] = vmin; + si.natnet_version[0] = nnmaj; + si.natnet_version[1] = nnmin; + return si; +} + +// ----------- negotiate() success paths ------------------------------------ + +TEST(Negotiate, SuccessWithHostPresent) +{ + FakeNatNetClient fake; + fake.connect_result = NatNetResult::OK; + fake.server_info = make_server_info(true, "Motive", 3, 1, 4, 1); + + const auto result = negotiate(fake, make_test_cfg()); + + EXPECT_TRUE(result.ok); + EXPECT_TRUE(result.server_info.host_present); + EXPECT_EQ(result.server_info.host_app_name, "Motive"); + EXPECT_EQ(result.server_info.host_app_version[0], 3); + EXPECT_EQ(result.server_info.natnet_version[0], 4); + + EXPECT_TRUE(fake.connect_was_called); + EXPECT_TRUE(fake.server_info_was_called); + // log message should mention the server IP + EXPECT_NE(result.log_message.find("192.168.1.100"), std::string::npos); +} + +TEST(Negotiate, SuccessButHostNotPresent) +{ + FakeNatNetClient fake; + fake.connect_result = NatNetResult::OK; + fake.server_info = make_server_info(false); + + const auto result = negotiate(fake, make_test_cfg()); + + EXPECT_TRUE(result.ok); // connection itself succeeded + EXPECT_FALSE(result.server_info.host_present); + // log message should flag the missing host info + EXPECT_NE(result.log_message.find("no host info"), std::string::npos); +} + +TEST(Negotiate, SuccessServerInfoCallFails) +{ + // SDK's GetServerDescription returns an error (simulated via call_succeeds=false) + FakeNatNetClient fake; + fake.connect_result = NatNetResult::OK; + fake.server_info_call_succeeds = false; + + const auto result = negotiate(fake, make_test_cfg()); + + EXPECT_TRUE(result.ok); + EXPECT_FALSE(result.server_info.host_present); + EXPECT_NE(result.log_message.find("no host info"), std::string::npos); +} + +// ----------- negotiate() failure paths ------------------------------------ + +TEST(Negotiate, NetworkErrorReturnsFalse) +{ + FakeNatNetClient fake; + fake.connect_result = NatNetResult::NetworkError; + + const auto result = negotiate(fake, make_test_cfg()); + + EXPECT_FALSE(result.ok); + EXPECT_FALSE(fake.server_info_was_called); // should not reach GetServerDescription + EXPECT_NE(result.log_message.find("NetworkError"), std::string::npos); + EXPECT_NE(result.log_message.find("192.168.1.100"), std::string::npos); +} + +TEST(Negotiate, InvalidAddressReturnsFalse) +{ + FakeNatNetClient fake; + fake.connect_result = NatNetResult::InvalidAddress; + + const auto result = negotiate(fake, make_test_cfg()); + + EXPECT_FALSE(result.ok); + EXPECT_NE(result.log_message.find("InvalidAddress"), std::string::npos); +} + +TEST(Negotiate, TimeoutReturnsFalse) +{ + FakeNatNetClient fake; + fake.connect_result = NatNetResult::Timeout; + + const auto result = negotiate(fake, make_test_cfg()); + + EXPECT_FALSE(result.ok); + EXPECT_NE(result.log_message.find("Timeout"), std::string::npos); +} + +// ----------- negotiate() passes ConnectConfig correctly ------------------- + +TEST(Negotiate, UnicastConfigPassedToClient) +{ + FakeNatNetClient fake; + const auto cfg = make_test_cfg("unicast"); + negotiate(fake, cfg); + + EXPECT_EQ(fake.last_connect_config.connection_type, "unicast"); + EXPECT_EQ(fake.last_connect_config.server_ip, "192.168.1.100"); + EXPECT_EQ(fake.last_connect_config.command_port, 1510u); +} + +TEST(Negotiate, MulticastConfigPassedToClient) +{ + FakeNatNetClient fake; + const auto cfg = make_connect_config( + "10.0.0.1", "0.0.0.0", 1510u, 1511u, "multicast", "239.0.0.1"); + negotiate(fake, cfg); + + EXPECT_EQ(fake.last_connect_config.connection_type, "multicast"); + EXPECT_EQ(fake.last_connect_config.multicast_address, "239.0.0.1"); +} + +// ----------- log message content ------------------------------------------ + +TEST(Negotiate, SuccessLogContainsAppAndVersion) +{ + FakeNatNetClient fake; + fake.connect_result = NatNetResult::OK; + fake.server_info = make_server_info(true, "MotiveBody", 2, 5, 4, 0); + + const auto result = negotiate(fake, make_test_cfg()); + + EXPECT_NE(result.log_message.find("MotiveBody"), std::string::npos); + EXPECT_NE(result.log_message.find("2.5"), std::string::npos); // v2.5 + EXPECT_NE(result.log_message.find("4.0"), std::string::npos); // NatNet 4.0 +} + +TEST(Negotiate, FailureLogContainsPortAndType) +{ + FakeNatNetClient fake; + fake.connect_result = NatNetResult::Timeout; + const auto cfg = make_connect_config( + "10.1.2.3", "0.0.0.0", 9000u, 9001u, "multicast"); + + const auto result = negotiate(fake, cfg); + + EXPECT_NE(result.log_message.find("9000"), std::string::npos); + EXPECT_NE(result.log_message.find("multicast"), std::string::npos); +} + + +// =========================================================================== +// FakeNatNetClient — frame injection +// =========================================================================== + +TEST(FakeNatNetClient, CallbackNotCalledBeforeRegistration) +{ + FakeNatNetClient fake; + // No set_frame_callback called — inject_frame should be a no-op + fake.inject_body(1, 1.f, 2.f, 3.f); + EXPECT_EQ(fake.frames_injected, 0); +} + +TEST(FakeNatNetClient, CallbackInvokedAfterRegistration) +{ + FakeNatNetClient fake; + + std::vector received; + fake.set_frame_callback([&](const FrameSample & f) { received.push_back(f); }); + + fake.inject_body(1, 1.f, 2.f, 3.f); + fake.inject_body(2, 4.f, 5.f, 6.f); + + EXPECT_EQ(fake.frames_injected, 2); + ASSERT_EQ(received.size(), 2u); +} + +TEST(FakeNatNetClient, InjectedBodyDataIsPreserved) +{ + FakeNatNetClient fake; + + FrameSample captured; + fake.set_frame_callback([&](const FrameSample & f) { captured = f; }); + + fake.inject_body(42, 1.5f, -2.5f, 0.75f, + 0.f, 0.f, 0.7071068f, 0.7071068f, + 0x01 /* tracking valid */); + + ASSERT_EQ(captured.bodies.size(), 1u); + const auto & rb = captured.bodies[0]; + EXPECT_EQ(rb.id, 42); + EXPECT_FLOAT_EQ(rb.x, 1.5f); + EXPECT_FLOAT_EQ(rb.y, -2.5f); + EXPECT_FLOAT_EQ(rb.z, 0.75f); + EXPECT_NEAR(rb.qz, 0.7071068f, 1e-6f); + EXPECT_TRUE(is_tracking_valid(rb.params)); +} + +TEST(FakeNatNetClient, ModelListChangedFlagDeliveredInFrame) +{ + FakeNatNetClient fake; + + bool model_changed = false; + fake.set_frame_callback([&](const FrameSample & f) { + model_changed = model_list_changed(f.params); + }); + + FrameSample f; + f.params = 0x02; // bit 1 = model list changed + fake.inject_frame(f); + + EXPECT_TRUE(model_changed); +} + +TEST(FakeNatNetClient, ResetRecordsClearsState) +{ + FakeNatNetClient fake; + fake.set_frame_callback([](const FrameSample &) {}); + fake.inject_body(1, 0.f, 0.f, 0.f); + + fake.reset_records(); + + EXPECT_FALSE(fake.connect_was_called); + EXPECT_FALSE(fake.set_callback_was_called); + EXPECT_EQ(fake.frames_injected, 0); +} + + +// =========================================================================== +// Body descriptors + filtering +// =========================================================================== + +TEST(BodyDescriptor, SkeletonBoneHasPositiveParentId) +{ + BodyDescriptor bone; + bone.id = 10; + bone.name = "Hip"; + bone.parent_id = 5; // part of skeleton with id=5 + + EXPECT_GE(bone.parent_id, 0); // should be skipped in publisher creation +} + +TEST(BodyDescriptor, TopLevelBodyHasNegativeParentId) +{ + BodyDescriptor body; + body.id = 1; + body.name = "Drone"; + body.parent_id = -1; + + EXPECT_LT(body.parent_id, 0); // should be published +} + +TEST(FakeNatNetClient, GetBodyDescriptorsReturnsConfigured) +{ + FakeNatNetClient fake; + fake.body_descriptors = { + {1, "Drone1", -1}, + {2, "Drone2", -1}, + {3, "Hip", 2}, // skeleton bone + }; + + const auto descs = fake.get_body_descriptors(); + + ASSERT_EQ(descs.size(), 3u); + EXPECT_TRUE(fake.descriptors_was_called); + + // Only top-level bodies should be published (parent_id < 0) + int top_level = 0; + for (const auto & d : descs) { + if (d.parent_id < 0) { ++top_level; } + } + EXPECT_EQ(top_level, 2); +} diff --git a/robot/ros_ws/src/perception/natnet_ros2/test/test_natnet_ros2.py b/robot/ros_ws/src/perception/natnet_ros2/test/test_natnet_ros2.py new file mode 100644 index 000000000..cba5a3444 --- /dev/null +++ b/robot/ros_ws/src/perception/natnet_ros2/test/test_natnet_ros2.py @@ -0,0 +1,152 @@ +# Copyright (c) 2024 Carnegie Mellon University +# MIT License - see LICENSE in the repository root for full text. +"""Unit tests for natnet_ros2 Python source code. + +These tests import the actual production source files and stub out ROS at the +import boundary so no ROS installation is required. + +Coverage here: + vision_pose_converter_node.py → VisionPoseConverterNode._canonical_quaternion() + → VisionPoseConverterNode._on_pose() frame_id assignment + +NOT covered here (C++ — requires colcon build + gtest): + natnet_ros2_node.cpp → build_covariance_6x6(), topic name construction, + connection_type validation, SDK frame callback logic. + These live in test_natnet_logic.cpp in the same test/ directory. +""" + +import sys +from pathlib import Path +from types import SimpleNamespace +from unittest.mock import MagicMock + +# --------------------------------------------------------------------------- +# Stub ROS before importing the source. +# +# The key subtlety: VisionPoseConverterNode inherits from rclpy.node.Node. +# If Node is a plain MagicMock() the class body is never executed (Python's +# metaclass machinery returns a Mock for attribute access instead of running +# __init_subclass__ / defining methods). We supply a real dummy base class +# so the actual class body — including _canonical_quaternion — is defined. +# --------------------------------------------------------------------------- + +class _FakeNode: + def __init__(self, name: str): + pass + def get_logger(self): + return MagicMock() + def declare_parameter(self, *args, **kwargs): + pass + def get_parameter(self, name): + m = MagicMock() + m.value = MagicMock() + return m + def create_subscription(self, *args, **kwargs): + return MagicMock() + def create_publisher(self, *args, **kwargs): + return MagicMock() + + +_rclpy_node_mod = MagicMock() +_rclpy_node_mod.Node = _FakeNode +sys.modules.setdefault("rclpy", MagicMock()) +sys.modules["rclpy.node"] = _rclpy_node_mod +sys.modules.setdefault("geometry_msgs", MagicMock()) +sys.modules.setdefault("geometry_msgs.msg", MagicMock()) + +# Add the package's src/ directory (co-located: test/ → package root → src/). +_natnet_src = Path(__file__).resolve().parent.parent / "src" +if str(_natnet_src) not in sys.path: + sys.path.insert(0, str(_natnet_src)) + +from vision_pose_converter_node import VisionPoseConverterNode # noqa: E402 + + +# --------------------------------------------------------------------------- +# Helpers +# --------------------------------------------------------------------------- + +def _quat(x: float, y: float, z: float, w: float) -> SimpleNamespace: + """Minimal quaternion-like object matching the expected interface.""" + return SimpleNamespace(x=x, y=y, z=z, w=w) + + +# --------------------------------------------------------------------------- +# VisionPoseConverterNode._canonical_quaternion +# --------------------------------------------------------------------------- + +import pytest + + +@pytest.mark.unit +def test_canonical_quaternion_positive_w_unchanged(): + """Quaternion with w > 0 must not be altered.""" + q = _quat(0.1, 0.2, 0.3, 0.9) + out = VisionPoseConverterNode._canonical_quaternion(q) + assert out.w == pytest.approx(0.9) + assert out.x == pytest.approx(0.1) + assert out.y == pytest.approx(0.2) + assert out.z == pytest.approx(0.3) + + +@pytest.mark.unit +def test_canonical_quaternion_negative_w_flipped(): + """Quaternion with w < 0 must have all four components negated.""" + q = _quat(0.1, 0.2, 0.3, -0.9) + out = VisionPoseConverterNode._canonical_quaternion(q) + assert out.w == pytest.approx(0.9) + assert out.x == pytest.approx(-0.1) + assert out.y == pytest.approx(-0.2) + assert out.z == pytest.approx(-0.3) + + +@pytest.mark.unit +def test_canonical_quaternion_zero_w_unchanged(): + """w == 0 satisfies w >= 0 so no flip should occur.""" + q = _quat(1.0, 0.0, 0.0, 0.0) + out = VisionPoseConverterNode._canonical_quaternion(q) + assert out.w == pytest.approx(0.0) + assert out.x == pytest.approx(1.0) + + +@pytest.mark.unit +def test_canonical_quaternion_identity(): + q = _quat(0.0, 0.0, 0.0, 1.0) + out = VisionPoseConverterNode._canonical_quaternion(q) + assert out.w == pytest.approx(1.0) + assert out.x == pytest.approx(0.0) + + +@pytest.mark.unit +def test_canonical_quaternion_returns_same_object(): + """The method mutates and returns the same object (not a copy).""" + q = _quat(0.0, 0.0, 0.0, 1.0) + out = VisionPoseConverterNode._canonical_quaternion(q) + assert out is q + + +@pytest.mark.unit +def test_canonical_quaternion_w_stays_non_negative(): + """After canonicalisation w must always be >= 0.""" + cases = [ + _quat(0.0, 0.0, 0.7071, 0.7071), + _quat(0.0, 0.0, -0.7071, -0.7071), + _quat(0.5, -0.5, 0.5, -0.5), + _quat(0.0, 0.0, 1.0, 0.0), + ] + for q in cases: + out = VisionPoseConverterNode._canonical_quaternion(q) + assert out.w >= 0.0, f"w={out.w} after canonicalisation of {q}" + + +@pytest.mark.unit +def test_canonical_quaternion_dual_sign_produces_same_result(): + """q and -q must both canonicalise to the same output.""" + q_pos = _quat(0.1, 0.2, 0.3, 0.9) + q_neg = _quat(-0.1, -0.2, -0.3, -0.9) + out_pos = VisionPoseConverterNode._canonical_quaternion(q_pos) + out_neg = VisionPoseConverterNode._canonical_quaternion(q_neg) + assert out_pos.w == pytest.approx(out_neg.w) + assert out_pos.x == pytest.approx(out_neg.x) + assert out_pos.y == pytest.approx(out_neg.y) + assert out_pos.z == pytest.approx(out_neg.z) diff --git a/robot/ros_ws/src/perception/perception_bringup/launch/perception.launch.xml b/robot/ros_ws/src/perception/perception_bringup/launch/perception.launch.xml index bdc1d7d25..e433e94d4 100644 --- a/robot/ros_ws/src/perception/perception_bringup/launch/perception.launch.xml +++ b/robot/ros_ws/src/perception/perception_bringup/launch/perception.launch.xml @@ -2,6 +2,9 @@ + + + @@ -67,7 +70,11 @@ + + + + diff --git a/robot/ros_ws/src/sensors/lidar_point_cloud_filter/README.md b/robot/ros_ws/src/sensors/lidar_point_cloud_filter/README.md index 2822c059f..44449f50f 100644 --- a/robot/ros_ws/src/sensors/lidar_point_cloud_filter/README.md +++ b/robot/ros_ws/src/sensors/lidar_point_cloud_filter/README.md @@ -37,10 +37,10 @@ Included from `sensors_bringup` under the robot and `sensors` namespaces. Defaul ## System tests (`sensors` mark) -Sensor checks (sim + robot topic rates, LiDAR validation) live in repo-root **`tests/test_sensors.py`** (`pytest -m sensors`), which runs **after** **`tests/test_liveliness.py`** in the default collection order. For **Isaac Sim** (`--sim isaacsim`), that suite: +Sensor checks (sim + robot topic rates, LiDAR validation) live in repo-root **`tests/system/test_sensors.py`** (`pytest -m sensors`), which runs **after** **`tests/system/test_liveliness.py`** in the default collection order. **Numpy-only** LiDAR filter rules live in **`lidar_point_cloud_filter/validation_core.py`** (this package's module directory), covered by **`test/test_validation_core.py`** (`pytest -m unit` via proxy in `tests/robot/`) and imported by **`scripts/validate_lidar_filter_clouds.py`** at runtime. For **Isaac Sim** (`--sim isaacsim`), that suite: - Proves the **filtered** topic is alive (`ros2 topic echo --once` on `.../point_cloud` — large clouds are not probed with `ros2 topic hz`). -- Runs `scripts/validate_lidar_filter_clouds.py` inside each robot container: checks the **filtered** cloud against `near_range_m`, optionally compares behavior when **`point_cloud_raw`** has near-field returns. +- Runs `robot/ros_ws/src/sensors/lidar_point_cloud_filter/scripts/validate_lidar_filter_clouds.py` inside each robot container: checks the **filtered** cloud against `near_range_m`, optionally compares behavior when **`point_cloud_raw`** has near-field returns. **Microsoft AirSim** does not guarantee `sensors/ouster` topics on that profile; those steps are skipped there. diff --git a/robot/ros_ws/src/sensors/lidar_point_cloud_filter/lidar_point_cloud_filter/validation_core.py b/robot/ros_ws/src/sensors/lidar_point_cloud_filter/lidar_point_cloud_filter/validation_core.py new file mode 100644 index 000000000..5d3f22cfd --- /dev/null +++ b/robot/ros_ws/src/sensors/lidar_point_cloud_filter/lidar_point_cloud_filter/validation_core.py @@ -0,0 +1,73 @@ +# Copyright (c) 2024 Carnegie Mellon University +# MIT License - see LICENSE in the repository root for full text. +"""Pure-numeric LiDAR filter validation helpers (no ROS imports). + +Shared by: + scripts/validate_lidar_filter_clouds.py — runtime echo-based cloud check (robot container) + test/test_validation_core.py — pytest unit tests (``pytest -m unit``) +""" + +from __future__ import annotations + +import numpy as np + + +def near_range_tolerance(near_range_m: float) -> float: + """Slack around ``near_range_m`` (same rule as the validate script).""" + return max(0.05, float(near_range_m) * 0.05) + + +def ranges_xyz_from_points_xyz(points: np.ndarray) -> np.ndarray | None: + """Euclidean range per row for ``(N, 3)`` xyz. + + Returns ``None`` if shape is wrong or any coordinate is non-finite. + """ + arr = np.asarray(points, dtype=np.float64) + if arr.ndim != 2 or arr.shape[1] != 3: + return None + if arr.size == 0: + return np.array([], dtype=np.float64) + if not np.isfinite(arr).all(): + return None + return np.linalg.norm(arr, axis=1) + + +def validate_filtered_ranges( + ranges: np.ndarray, + near_range_m: float, + *, + long_range_min_m: float = 2.0, +) -> tuple[bool, str]: + """Check filtered cloud range statistics against ``near_range_m``. + + Returns ``(True, "")`` on success, else ``(False, reason)``. + """ + fr = np.asarray(ranges, dtype=np.float64) + if fr.size == 0: + return False, 'filtered cloud is empty' + mn_f = float(fr.min()) + tol = near_range_tolerance(near_range_m) + if mn_f < float(near_range_m) - tol: + return ( + False, + f'filtered min range {mn_f:.4f}m < near_range_m ({near_range_m}) - tol {tol:.4f}', + ) + if float(fr.max()) < long_range_min_m: + return ( + False, + f'expected long-range returns; filtered max range {float(fr.max()):.4f}m', + ) + return True, '' + + +def raw_filtered_near_range_ok( + mn_raw: float, + mn_filtered: float, + near_range_m: float, + tol: float | None = None, +) -> bool: + """If raw has near-field clutter, filtered minimum must still clear ``near_range_m``.""" + t = near_range_tolerance(near_range_m) if tol is None else tol + if mn_raw < float(near_range_m) - t and mn_filtered < float(near_range_m) - t: + return False + return True diff --git a/robot/ros_ws/src/sensors/lidar_point_cloud_filter/scripts/validate_lidar_filter_clouds.py b/robot/ros_ws/src/sensors/lidar_point_cloud_filter/scripts/validate_lidar_filter_clouds.py index 7f50d45f6..f53460a56 100644 --- a/robot/ros_ws/src/sensors/lidar_point_cloud_filter/scripts/validate_lidar_filter_clouds.py +++ b/robot/ros_ws/src/sensors/lidar_point_cloud_filter/scripts/validate_lidar_filter_clouds.py @@ -1,6 +1,6 @@ #!/usr/bin/env python3 -# Copyright 2026 AirLab CMU -# SPDX-License-Identifier: Apache-2.0 +# Copyright (c) 2024 Carnegie Mellon University +# MIT License - see LICENSE in the repository root for full text. """One-shot ROS 2 check for liveliness: filtered LiDAR cloud vs raw (Isaac / Pegasus). Run inside the robot container with workspace sourced and ROS_DOMAIN_ID set:: @@ -11,7 +11,7 @@ * Filtered ``.../point_cloud``: all coordinates finite; **minimum range** must be at least ``near_range_m`` from the running filter node (minus tolerance). The tolerance is ``max(0.05 m, 5% of near_range_m)`` — the ``0.05`` is **slack - around the configured near range**, not a standalone “no points within 5 cm” + around the configured near range**, not a standalone "no points within 5 cm" rule. At least one return beyond 2 m so long-range points are not stripped. * Raw ``.../point_cloud_raw`` (optional): if present and contains near-field returns below ``near_range_m``, the filtered cloud must still respect the @@ -28,6 +28,7 @@ import subprocess import sys import time +from pathlib import Path import numpy as np import rclpy @@ -36,6 +37,19 @@ from sensor_msgs.msg import PointCloud2 from sensor_msgs_py import point_cloud2 +# validation_core lives in the package module (installed by colcon, or importable +# by adding the package root to sys.path for development use). +_pkg_root = Path(__file__).resolve().parent.parent +if str(_pkg_root) not in sys.path: + sys.path.insert(0, str(_pkg_root)) + +from lidar_point_cloud_filter.validation_core import ( # noqa: E402 + near_range_tolerance, + ranges_xyz_from_points_xyz, + raw_filtered_near_range_ok, + validate_filtered_ranges, +) + def _read_near_range_m(robot_num: int) -> float: """Query ``near_range_m`` from the running filter node; default 0.75.""" @@ -74,9 +88,7 @@ def _ranges_xyz(msg: PointCloud2) -> np.ndarray | None: if not pts: return np.array([], dtype=np.float64) arr = np.array([(float(p[0]), float(p[1]), float(p[2])) for p in pts], dtype=np.float64) - if not np.isfinite(arr).all(): - return None - return np.linalg.norm(arr, axis=1) + return ranges_xyz_from_points_xyz(arr) def _wait_for_cloud( @@ -120,7 +132,7 @@ def main() -> int: reliable = not args.qos_best_effort near_range_m = _read_near_range_m(n) - tol = max(0.05, near_range_m * 0.05) + tol = near_range_tolerance(near_range_m) node = None try: @@ -139,20 +151,11 @@ def main() -> int: print('ERROR: filtered cloud is empty', file=sys.stderr) return 1 - mn_f = float(fr.min()) - if mn_f < near_range_m - tol: - print( - f'ERROR: filtered min range {mn_f:.4f}m < near_range_m ({near_range_m}) - tol {tol:.4f}', - file=sys.stderr, - ) - return 1 - - if float(fr.max()) < 2.0: - print( - f'ERROR: expected long-range returns; filtered max range {float(fr.max()):.4f}m', - file=sys.stderr, - ) + ok_f, err_f = validate_filtered_ranges(fr, near_range_m) + if not ok_f: + print(f'ERROR: {err_f}', file=sys.stderr) return 1 + mn_f = float(fr.min()) rmsg = _wait_for_cloud(node, raw_topic, min(30.0, args.timeout), reliable) if rmsg is not None: @@ -164,7 +167,7 @@ def main() -> int: f'INFO: raw min range {mn_r:.4f}m (< near_range_m); ' f'filtered min {mn_f:.4f}m (must stay >= near_range_m)', ) - if mn_r < near_range_m - tol and mn_f < near_range_m - tol: + if not raw_filtered_near_range_ok(mn_r, mn_f, near_range_m, tol): print( 'ERROR: raw has near-field clutter but filtered min still below near_range_m', file=sys.stderr, diff --git a/robot/ros_ws/src/sensors/lidar_point_cloud_filter/setup.cfg b/robot/ros_ws/src/sensors/lidar_point_cloud_filter/setup.cfg index ba67e395d..55f87f13c 100644 --- a/robot/ros_ws/src/sensors/lidar_point_cloud_filter/setup.cfg +++ b/robot/ros_ws/src/sensors/lidar_point_cloud_filter/setup.cfg @@ -2,3 +2,11 @@ script_dir=$base/lib/lidar_point_cloud_filter [install] install_scripts=$base/lib/lidar_point_cloud_filter + +[tool:pytest] +testpaths = test +python_files = test_*.py +python_classes = Test* +python_functions = test_* +markers = + unit: Hermetic unit tests (no ROS stack required) diff --git a/robot/ros_ws/src/sensors/lidar_point_cloud_filter/setup.py b/robot/ros_ws/src/sensors/lidar_point_cloud_filter/setup.py index 3935a7fe3..3c09016db 100644 --- a/robot/ros_ws/src/sensors/lidar_point_cloud_filter/setup.py +++ b/robot/ros_ws/src/sensors/lidar_point_cloud_filter/setup.py @@ -18,7 +18,9 @@ maintainer_email='ajong@andrew.cmu.edu', description='Near-range sphere filter for lidar PointCloud2', license='Apache-2.0', - tests_require=['pytest'], + extras_require={ + 'test': ['pytest'], + }, entry_points={ 'console_scripts': [ 'lidar_point_cloud_filter_node = lidar_point_cloud_filter.lidar_point_cloud_filter_node:main', diff --git a/robot/ros_ws/src/sensors/lidar_point_cloud_filter/test/test_validation_core.py b/robot/ros_ws/src/sensors/lidar_point_cloud_filter/test/test_validation_core.py new file mode 100644 index 000000000..04526c478 --- /dev/null +++ b/robot/ros_ws/src/sensors/lidar_point_cloud_filter/test/test_validation_core.py @@ -0,0 +1,75 @@ +# Copyright (c) 2024 Carnegie Mellon University +# MIT License - see LICENSE in the repository root for full text. +"""Unit tests for ``validation_core`` (numpy-only).""" + +import sys +from pathlib import Path + +import numpy as np +import pytest + +# validation_core lives in the package module directory (importable as a +# package member both here and in the validate_lidar_filter_clouds.py script). +_pkg_root = Path(__file__).resolve().parent.parent +if str(_pkg_root) not in sys.path: + sys.path.insert(0, str(_pkg_root)) + +from lidar_point_cloud_filter.validation_core import ( # noqa: E402 + near_range_tolerance, + ranges_xyz_from_points_xyz, + raw_filtered_near_range_ok, + validate_filtered_ranges, +) + + +@pytest.mark.unit +def test_near_range_tolerance(): + assert near_range_tolerance(0.5) == pytest.approx(0.05) + assert near_range_tolerance(2.0) == pytest.approx(0.1) + + +@pytest.mark.unit +def test_ranges_xyz_from_points_xyz(): + pts = np.array([[3.0, 4.0, 0.0], [0.0, 0.0, 1.0]], dtype=np.float64) + r = ranges_xyz_from_points_xyz(pts) + assert r is not None + assert r[0] == pytest.approx(5.0) + assert r[1] == pytest.approx(1.0) + + +@pytest.mark.unit +def test_ranges_xyz_rejects_nonfinite(): + pts = np.array([[1.0, np.nan, 0.0]], dtype=np.float64) + assert ranges_xyz_from_points_xyz(pts) is None + + +@pytest.mark.unit +def test_validate_filtered_ranges_ok(): + # All points between 1 m and 5 m from origin — clears default long_range_min_m=2 + fr = np.array([1.0, 2.0, 5.0], dtype=np.float64) + ok, msg = validate_filtered_ranges(fr, near_range_m=0.75) + assert ok + assert msg == '' + + +@pytest.mark.unit +def test_validate_filtered_ranges_too_close(): + fr = np.array([0.2, 5.0], dtype=np.float64) + ok, msg = validate_filtered_ranges(fr, near_range_m=0.75) + assert not ok + assert 'min range' in msg + + +@pytest.mark.unit +def test_validate_filtered_ranges_no_long_range(): + fr = np.array([1.0, 1.5], dtype=np.float64) + ok, msg = validate_filtered_ranges(fr, near_range_m=0.75) + assert not ok + assert 'long-range' in msg + + +@pytest.mark.unit +def test_raw_filtered_near_range_ok(): + tol = near_range_tolerance(0.75) + assert raw_filtered_near_range_ok(0.1, 0.8, 0.75, tol) + assert not raw_filtered_near_range_ok(0.1, 0.2, 0.75, tol) diff --git a/robot/ros_ws/src/sensors/sensor_interfaces/package.xml b/robot/ros_ws/src/sensors/sensor_interfaces/package.xml index c3df3349b..0fd76afe0 100644 --- a/robot/ros_ws/src/sensors/sensor_interfaces/package.xml +++ b/robot/ros_ws/src/sensors/sensor_interfaces/package.xml @@ -12,11 +12,12 @@ rosidl_default_runtime - rosidl_interface_packages + sensor_msgs + ament_lint_auto ament_lint_common - sensor_msgs + rosidl_interface_packages ament_cmake diff --git a/tests/README.md b/tests/README.md index e4362dd4c..f10942ff7 100644 --- a/tests/README.md +++ b/tests/README.md @@ -1,6 +1,12 @@ -# System Testing +# Testing (`tests/`) -AirStack's system tests bring up the full Docker-based stack — simulator, robot containers, and GCS — and verify end-to-end behavior: container health, ROS 2 node presence, sensor publishing rates (in the `sensors` mark), and compute resource usage. Tests are written in Python with pytest and live under `tests/` at the repo root. +AirStack's **pytest** tree under `tests/` has three roles: + +1. **`tests/system/`** — Docker stack tests (sim + robot + GCS): liveliness, sensor Hz, takeoff/hover/land, image/workspace builds. +2. **`tests/robot/`** — Fast **unit** tests that mirror `robot/ros_ws/src/` (`behavior`, `global`, `interface`, `local`, `perception`, `sensors`). Mark: `unit`. +3. **`tests/sim/`** — Unit tests for simulation-side helpers (e.g. Motive / NatNet emulator). Mark: `unit`. + +Shared fixtures live in `tests/conftest.py`. Use `airstack test -m unit -v` for hermetic tests only, or the marks below for the full stack. @@ -8,16 +14,36 @@ AirStack's system tests bring up the full Docker-based stack — simulator, robo ## Test Suite Structure +### System tests (`tests/system/`) + | Module | Mark | What it tests | Hardware required | |--------|------|---------------|-------------------| -| [`test_build_docker.py`](../../../../tests/test_build_docker.py) | `build_docker` | Docker image builds (robot-desktop, gcs, isaac-sim, ms-airsim); records image sizes | Docker daemon | -| [`test_build_packages.py`](../../../../tests/test_build_packages.py) | `build_packages` | `colcon build` inside each container (robot, GCS, ms-airsim ROS workspace) | Docker daemon | -| [`test_liveliness.py`](../../../../tests/test_liveliness.py) | `liveliness` | Stack bring-up: container Running state, ``/clock`` readiness, tmux panes, sentinel ROS 2 nodes, compute snapshot, infra-only ``test_stable`` (tmux + nodes + compute) | Docker daemon, GPU, sim license | -| [`test_sensors.py`](../../../../tests/test_sensors.py) | `sensors` | After liveliness in collection order: sim + robot stereo/depth Hz (**Isaac:** batched ``ros2 topic hz`` to avoid bridge overload; **ms-airsim:** single batch), filtered LiDAR via ``echo --once`` + cloud sanity (isaacsim), sim RTF, ``test_sensor_streams_stable`` | Docker daemon, GPU, sim license | -| [`test_takeoff_hover_land.py`](../../../../tests/test_takeoff_hover_land.py) | `takeoff_hover_land` | End-to-end flight: PX4 readiness gate, takeoff to 10 m, hover stability, land — one chain per (sim, num_robots, iteration, velocity) | Docker daemon, GPU, sim license | +| [`system/test_build_docker.py`](system/test_build_docker.py) | `build_docker` | Docker image builds (robot-desktop, gcs, isaac-sim, ms-airsim); records image sizes | Docker daemon | +| [`system/test_build_packages.py`](system/test_build_packages.py) | `build_packages` | `colcon build` inside each container (robot, GCS, ms-airsim ROS workspace) | Docker daemon | +| [`system/test_liveliness.py`](system/test_liveliness.py) | `liveliness` | Stack bring-up: container Running state, ``/clock`` readiness, tmux panes, sentinel ROS 2 nodes, compute snapshot, infra-only ``test_stable`` (tmux + nodes + compute) | Docker daemon, GPU, sim license | +| [`system/test_sensors.py`](system/test_sensors.py) | `sensors` | After liveliness in collection order: sim + robot stereo/depth Hz (**Isaac:** batched ``ros2 topic hz`` to avoid bridge overload; **ms-airsim:** single batch), filtered LiDAR via ``echo --once`` + cloud sanity (isaacsim), sim RTF, ``test_sensor_streams_stable`` | Docker daemon, GPU, sim license | +| [`system/test_takeoff_hover_land.py`](system/test_takeoff_hover_land.py) | `takeoff_hover_land` | End-to-end flight: PX4 readiness gate, takeoff to 10 m, hover stability, land — one chain per (sim, num_robots, iteration, velocity) | Docker daemon, GPU, sim license | + +### Unit tests (`tests/robot/`, `tests/sim/`) + +Hermetic tests use `@pytest.mark.unit` (see [`pytest.ini`](pytest.ini)). + +**Co-location + proxy pattern:** test source lives alongside its ROS 2 package at +`robot/ros_ws/src///test/test_*.py` (the ROS 2 / colcon convention). +Files in `tests/robot/` are thin proxies that re-export those tests so that +`pytest tests/` discovers them. Both `airstack test -m unit` and +`colcon test --packages-select ` run the same test source. + +Example: `robot/ros_ws/src/sensors/lidar_point_cloud_filter/test/test_validation_core.py` +tests the numpy-only range validation rules in +`robot/ros_ws/src/sensors/lidar_point_cloud_filter/lidar_point_cloud_filter/validation_core.py` +(also used by `scripts/validate_lidar_filter_clouds.py` inside the robot container). + +See [Unit Testing Guide](../docs/development/intermediate/testing/unit_testing.md) +and the `add-unit-tests` agent skill for full details. Marks can be combined with pytest logic: -`-m "build_docker or build_packages"`, `-m liveliness`, `-m sensors`, `-m takeoff_hover_land`, or e.g. `-m "liveliness or sensors"` (see **Bring-up scope** below). +`-m unit`, `-m "build_docker or build_packages"`, `-m liveliness`, `-m sensors`, `-m takeoff_hover_land`, or e.g. `-m "liveliness or sensors"` (see **Bring-up scope** below). ### Bring-up scope (`airstack_env`) @@ -27,7 +53,7 @@ Marks can be combined with pytest logic: ## Test Infrastructure -All shared fixtures, helpers, and configuration live in [`tests/conftest.py`](../../../../tests/conftest.py). +All shared fixtures, helpers, and configuration live in [`conftest.py`](conftest.py). ### `airstack_env` fixture @@ -40,21 +66,21 @@ Parametrized over `(sim, num_robots, iteration)` tuples derived from CLI flags. ### Isaac Sim and the `sensors` mark -**LiDAR in pytest:** [`tests/conftest.py`](../../../../tests/conftest.py) sets +**LiDAR in pytest:** [`conftest.py`](conftest.py) sets `ENABLE_LIDAR=true` in `SIM_CONFIG["isaacsim"]["extra_env"]` so the multi-drone Pegasus script (`example_multi_px4_pegasus_launch_script.py`) attaches RTX LiDAR the same way the single-drone script always does. Without that flag the multi script would not spawn LiDAR OmniGraphs. -**Topic checks** live in [`tests/sensor_probes.py`](../../../../tests/sensor_probes.py) -and are driven by [`tests/test_sensors.py`](../../../../tests/test_sensors.py): +**Topic checks** live in [`sensor_probes.py`](sensor_probes.py) +and are driven by [`system/test_sensors.py`](system/test_sensors.py): | Path | What we measure | How | |------|-----------------|-----| | Sim → `/clock`, stereo images, stereo depth | Publish rate | ``ros2 topic hz`` on the sim container: ``/clock`` alone, then **chunks of two** ``image_rect`` topics, then **chunks of two** depth topics (``ISAACSIM_HZ_CHUNK_SIZE`` in ``sensor_probes.py``). | | Robot → same topic names (bridge) | Publish rate | Same **two-at-a-time** chunking on the robot container for Isaac. ms-airsim: one batch of four topics. | | Robot → filtered ``.../ouster/point_cloud`` | Stream alive | ``ros2 topic echo --once`` per robot (not Hz — large ``PointCloud2``). | -| LiDAR geometry | Near-range vs ``near_range_m`` | ``lidar_point_cloud_filter/scripts/validate_lidar_filter_clouds.py`` (raw vs filtered). | +| LiDAR geometry | Near-range vs ``near_range_m`` | ``robot/ros_ws/src/sensors/lidar_point_cloud_filter/scripts/validate_lidar_filter_clouds.py`` (raw vs filtered). | Sim **RTF** (real-time factor from ``/clock``) is also in the `sensors` suite. **`test_sensor_streams_stable`** repeats sim + robot stereo + LiDAR probes every @@ -88,11 +114,11 @@ tests/results/ ├── results.xml # JUnit XML — test durations and pass/fail status ├── metrics.json # Custom metrics (image sizes, Hz, compute, timing) └── logs/ - ├── test_build_docker.TestDockerBuilds.test_build_robot_desktop.log - ├── airstack_env.test_liveliness.TestLiveliness.test_robot_containers_running[msairsim-rob#1-iter0].log - ├── test_liveliness.TestLiveliness.test_robot_containers_running[msairsim-rob#1-iter0].log - ├── test_liveliness.TestLiveliness.test_stable[msairsim-rob#1-iter0].log - ├── test_sensors.TestSensors.test_sensor_streams_stable[msairsim-rob#1-iter0].log + ├── system.test_build_docker.TestDockerBuilds.test_build_robot_desktop.log + ├── airstack_env.system.test_liveliness.TestLiveliness.test_robot_containers_running[msairsim-rob#1-iter0].log + ├── system.test_liveliness.TestLiveliness.test_robot_containers_running[msairsim-rob#1-iter0].log + ├── system.test_liveliness.TestLiveliness.test_stable[msairsim-rob#1-iter0].log + ├── system.test_sensors.TestSensors.test_sensor_streams_stable[msairsim-rob#1-iter0].log └── ... # More per-test logs; another airstack_env.* per class using the fixture ``` @@ -109,6 +135,9 @@ arguments directly to pytest. No local Python environment needed. ```bash # From the repo root (AirStack must be set up: airstack setup): +# Unit tests only — no GPU, no full Docker stack (numpy-only + pure Python) +airstack test -m unit -v + # Build tests only — fast, no GPU needed airstack test -m "build_docker or build_packages" -v @@ -191,7 +220,7 @@ pytest tests/ -m sensors \ --- -## Autonomy Tests (`test_takeoff_hover_land.py`) +## Autonomy Tests (`system/test_takeoff_hover_land.py`) `TestTakeoffHoverLand` runs a **4-phase flight chain** for every combination of `(sim, num_robots, iteration, velocity)`. The drone returns to the ground after @@ -255,7 +284,7 @@ airstack test -m takeoff_hover_land \ ## Metrics Reporting (`parse_metrics.py`) -[`tests/parse_metrics.py`](../../../../tests/parse_metrics.py) reads `results.xml` and `metrics.json` from a run directory and produces a markdown report. It has two modes: +[`parse_metrics.py`](parse_metrics.py) reads `results.xml` and `metrics.json` from a run directory and produces a markdown report. It has two modes: ### Single-run report diff --git a/tests/colcon_unit_test_packages.yaml b/tests/colcon_unit_test_packages.yaml new file mode 100644 index 000000000..5e0bd0ebb --- /dev/null +++ b/tests/colcon_unit_test_packages.yaml @@ -0,0 +1,13 @@ +# Packages run via `colcon test` in system.test_build_packages.test_colcon_test_robot. +# +# Add a package here when it has gtests (ament_add_gtest) and/or pytest tests under +# /test/. Keep in sync with tests/robot/ proxies for Python unit tests. +# +# See: docs/development/intermediate/testing/unit_testing.md + +robot: + packages: + - natnet_ros2 + - lidar_point_cloud_filter + # Skips ament_copyright / flake8 / pep257 on Python packages; run linters separately. + pytest_args: "-m not linter" diff --git a/tests/conftest.py b/tests/conftest.py index ef8b88ec6..31fd29076 100644 --- a/tests/conftest.py +++ b/tests/conftest.py @@ -4,6 +4,7 @@ import re import shlex import subprocess +import sys import threading import time from concurrent.futures import ThreadPoolExecutor, as_completed @@ -12,6 +13,7 @@ from pathlib import Path import pytest +import yaml SIM_CONFIG = { "msairsim": { @@ -44,6 +46,47 @@ } AIRSTACK_ROOT = os.environ.get("AIRSTACK_ROOT", str(Path(__file__).parent.parent)) +COLCON_UNIT_TEST_PACKAGES_YAML = ( + Path(AIRSTACK_ROOT) / "tests" / "colcon_unit_test_packages.yaml" +) + + +def load_colcon_unit_test_config(workspace="robot"): + """Load colcon test package list and pytest args from tests/colcon_unit_test_packages.yaml.""" + if not COLCON_UNIT_TEST_PACKAGES_YAML.is_file(): + raise FileNotFoundError( + f"Missing {COLCON_UNIT_TEST_PACKAGES_YAML} — add packages to gate in colcon test." + ) + with COLCON_UNIT_TEST_PACKAGES_YAML.open(encoding="utf-8") as f: + data = yaml.safe_load(f) or {} + if workspace not in data: + raise KeyError( + f"No '{workspace}' entry in {COLCON_UNIT_TEST_PACKAGES_YAML.name}" + ) + cfg = data[workspace] or {} + packages = cfg.get("packages") or [] + if not packages: + raise ValueError( + f"'{workspace}.packages' is empty in {COLCON_UNIT_TEST_PACKAGES_YAML.name}" + ) + return packages, cfg.get("pytest_args", "") + + +def colcon_test_robot_command(workspace="robot"): + """Shell command for colcon test over unit-test packages (robot workspace).""" + packages, pytest_args = load_colcon_unit_test_config(workspace) + pkg_list = " ".join(packages) + cmd = ( + f"colcon test --packages-select {pkg_list} " + "--event-handlers console_direct+ --return-code-on-test-failure" + ) + if pytest_args: + cmd += f' --pytest-args "{pytest_args}"' + return cmd +# Unit tests live co-located with their ROS 2 packages in robot/ros_ws/src/. +# Thin proxy files under tests/robot/ re-export those tests so that +# `pytest tests/` and `airstack test -m unit` discover them without any +# sys.path manipulation here. Each proxy file sets up its own paths. RUN_DIR = None LOGS_DIR = None ROS_DISTRO_SETUP = "/opt/ros/jazzy/setup.bash" @@ -156,11 +199,16 @@ def pytest_generate_tests(metafunc): # docker image builds → colcon workspace builds → liveliness (infra) → sensors # (ROS topic streams) → autonomy flight tests. _MODULE_ORDER = [ - "test_build_docker", - "test_build_packages", - "test_liveliness", - "test_sensors", - "test_takeoff_hover_land", + # Unit tests first — fast, hermetic, no Docker. Any module whose dotted + # name starts with "robot." or "sim." is a proxy for a package-level unit + # test and sorts into this leading slot via the prefix check below. + "__unit__", + # System tests follow in dependency order. + "system.test_build_docker", + "system.test_build_packages", + "system.test_liveliness", + "system.test_sensors", + "system.test_takeoff_hover_land", ] # Within test_takeoff_hover_land, each (env, velocity) runs phases in this chain order. @@ -177,16 +225,28 @@ def _rank(name, order): return order.index(name) if name in order else len(order) +def _module_key(item): + """Return the ordering key for an item. + + Unit-test proxies live under ``robot/``, ``sim/``, or ``gcs/`` and are + identified by their nodeid prefix. Everything else uses the dotted module + ``__name__`` looked up against ``_MODULE_ORDER``. + """ + if item.nodeid.startswith(("robot/", "sim/", "gcs/")): + return _rank("__unit__", _MODULE_ORDER) + return _rank(getattr(item.module, "__name__", ""), _MODULE_ORDER) + + def pytest_collection_modifyitems(items): # 1. Cross-module: enforce `_MODULE_ORDER`. Stable sort keeps within-module # order intact, so pytest's default file/class order survives. - items.sort(key=lambda it: _rank(getattr(it.module, "__name__", ""), _MODULE_ORDER)) + items.sort(key=_module_key) # 2. Within test_takeoff_hover_land: sort by (airstack_env, velocity, phase) so each # (sim, robots, iter) env brings up the stack once and the drone goes # ground→air→ground per velocity. def phase(item): - if getattr(item.module, "__name__", "") != "test_takeoff_hover_land": + if getattr(item.module, "__name__", "") != "system.test_takeoff_hover_land": return None name = item.originalname or item.name.split("[", 1)[0] return _rank(name, _AUTONOMY_PHASE_ORDER) diff --git a/tests/parse_metrics.py b/tests/parse_metrics.py index 6900e7305..f2267e627 100644 --- a/tests/parse_metrics.py +++ b/tests/parse_metrics.py @@ -51,9 +51,15 @@ def _split_test_name(name): - """`test_liveliness.TestLiveliness.test_foo[id]` → - (module="test_liveliness", display="test_foo[id]"). Drops the Class segment - for display since there's one class per module (same for ``test_sensors``).""" + """Dotted pytest node id → (module_prefix, display_without_class). + + Supports legacy ``test_liveliness.TestLiveliness.test_foo[id]`` and nested + ``system.test_liveliness.TestLiveliness.test_foo[id]``. + """ + m = re.match(r"^(.*)\.(Test\w+)\.(test\w+)(\[.*\])?$", name) + if m: + mod, _cls, test, bracket = m.groups() + return mod, f"{test}{bracket or ''}" parts = name.split(".", 2) if len(parts) == 3: return parts[0], parts[2] diff --git a/tests/pytest.ini b/tests/pytest.ini index 78a916a0c..a664ccbf3 100644 --- a/tests/pytest.ini +++ b/tests/pytest.ini @@ -1,5 +1,6 @@ [pytest] markers = + unit: Fast hermetic tests (no Docker stack; numpy / pure Python) build_docker: Docker image build tests build_packages: Colcon workspace build tests liveliness: Container and process health (Docker, tmux, sentinel ROS 2 nodes) diff --git a/tests/requirements.txt b/tests/requirements.txt index bc2a16d4d..a4b43e6bb 100644 --- a/tests/requirements.txt +++ b/tests/requirements.txt @@ -1,6 +1,8 @@ pytest pytest-timeout pytest-dependency +pyyaml tabulate psutil pandas +numpy diff --git a/tests/robot/README.md b/tests/robot/README.md new file mode 100644 index 000000000..a4409c4b1 --- /dev/null +++ b/tests/robot/README.md @@ -0,0 +1,37 @@ +# Robot-side unit test proxies + +Layout mirrors [`robot/ros_ws/src/`](../../robot/ros_ws/src/) autonomy layers: + +| Directory | Maps to ROS workspace | +|-----------|----------------------| +| `behavior/` | `robot/ros_ws/src/behavior/` | +| `global/` | `robot/ros_ws/src/global/` | +| `interface/` | `robot/ros_ws/src/interface/` | +| `local/` | `robot/ros_ws/src/local/` | +| `perception/` | `robot/ros_ws/src/perception/` | +| `sensors/` | `robot/ros_ws/src/sensors/` | + +## Design: co-location + proxy + +**Test source** lives co-located with each ROS 2 package (the standard colcon +convention): + +``` +robot/ros_ws/src///test/test_.py ← source of truth +``` + +**This directory** contains thin proxy files that load the real test module via +`importlib` and re-export its `test_*` functions, making them discoverable by +`pytest tests/` and `airstack test -m unit` without any changes to the CI +workflow. Each proxy is ~15 lines. + +``` +tests/robot///test_.py ← proxy (re-exports above) +``` + +Both `airstack test -m unit` (pytest path via proxy) and +`colcon test --packages-select ` (direct path to source) run the same +test functions from the same file. + +All test functions must carry `@pytest.mark.unit`. For adding new tests see the +`add-unit-tests` agent skill. diff --git a/tests/robot/behavior/README.md b/tests/robot/behavior/README.md new file mode 100644 index 000000000..713fd31f2 --- /dev/null +++ b/tests/robot/behavior/README.md @@ -0,0 +1,3 @@ +# Unit tests — behavior layer + +Add `@pytest.mark.unit` tests for `robot/ros_ws/src/behavior/` packages here. diff --git a/tests/robot/global/README.md b/tests/robot/global/README.md new file mode 100644 index 000000000..280c41dec --- /dev/null +++ b/tests/robot/global/README.md @@ -0,0 +1,3 @@ +# Unit tests — global layer + +Add `@pytest.mark.unit` tests for `robot/ros_ws/src/global/` packages here. diff --git a/tests/robot/interface/README.md b/tests/robot/interface/README.md new file mode 100644 index 000000000..ea4ee8b5c --- /dev/null +++ b/tests/robot/interface/README.md @@ -0,0 +1,3 @@ +# Unit tests — interface layer + +Add `@pytest.mark.unit` tests for `robot/ros_ws/src/interface/` packages here. diff --git a/tests/robot/local/README.md b/tests/robot/local/README.md new file mode 100644 index 000000000..118cc2071 --- /dev/null +++ b/tests/robot/local/README.md @@ -0,0 +1,3 @@ +# Unit tests — local layer + +Add `@pytest.mark.unit` tests for `robot/ros_ws/src/local/` packages here. diff --git a/tests/robot/perception/README.md b/tests/robot/perception/README.md new file mode 100644 index 000000000..350ef9fb0 --- /dev/null +++ b/tests/robot/perception/README.md @@ -0,0 +1,3 @@ +# Unit tests — perception layer + +Add `@pytest.mark.unit` tests for `robot/ros_ws/src/perception/` packages here. diff --git a/tests/robot/perception/natnet_ros2/test_natnet_ros2.py b/tests/robot/perception/natnet_ros2/test_natnet_ros2.py new file mode 100644 index 000000000..fc9a78bde --- /dev/null +++ b/tests/robot/perception/natnet_ros2/test_natnet_ros2.py @@ -0,0 +1,32 @@ +# Copyright (c) 2024 Carnegie Mellon University +# MIT License - see LICENSE in the repository root for full text. +"""Proxy: re-exposes natnet_ros2 unit tests from the package source tree. + +Unit test logic lives co-located with its package (ROS 2 / colcon convention): + robot/ros_ws/src/perception/natnet_ros2/test/test_natnet_ros2.py + +This file makes those tests discoverable by ``pytest tests/`` (CI) and +``airstack test -m unit`` without any changes to the CI workflow. +Run ``colcon test --packages-select natnet_ros2`` to also execute the C++ +gtests and ament linters. +""" + +import importlib.util +import sys +from pathlib import Path + +_repo_root = Path(__file__).resolve().parents[4] +_pkg_test = _repo_root / "robot/ros_ws/src/perception/natnet_ros2/test" +_real_file = _pkg_test / "test_natnet_ros2.py" + +# Load the real module under a unique name to avoid the circular-import that +# would occur if we used `from test_natnet_ros2 import *` (this file has the +# same name and pytest adds its directory to sys.path at collection time). +_spec = importlib.util.spec_from_file_location("_natnet_ros2_unit_tests", _real_file) +_real = importlib.util.module_from_spec(_spec) +_spec.loader.exec_module(_real) + +# Re-export every test_* symbol so pytest collects them from this proxy. +for _name in dir(_real): + if _name.startswith("test_"): + globals()[_name] = getattr(_real, _name) diff --git a/tests/robot/sensors/README.md b/tests/robot/sensors/README.md new file mode 100644 index 000000000..8a44129eb --- /dev/null +++ b/tests/robot/sensors/README.md @@ -0,0 +1,4 @@ +# Unit tests — sensors layer + +Package-specific folders (for example `lidar_point_cloud_filter/`) mirror +`robot/ros_ws/src/sensors//`. diff --git a/tests/robot/sensors/lidar_point_cloud_filter/test_validation_core.py b/tests/robot/sensors/lidar_point_cloud_filter/test_validation_core.py new file mode 100644 index 000000000..e7babf9d4 --- /dev/null +++ b/tests/robot/sensors/lidar_point_cloud_filter/test_validation_core.py @@ -0,0 +1,38 @@ +# Copyright (c) 2024 Carnegie Mellon University +# MIT License - see LICENSE in the repository root for full text. +"""Proxy: re-exposes validation_core unit tests from the package source tree. + +Unit test logic lives co-located with its package (ROS 2 / colcon convention): + robot/ros_ws/src/sensors/lidar_point_cloud_filter/test/test_validation_core.py + +This file makes those tests discoverable by ``pytest tests/`` (CI) and +``airstack test -m unit`` without any changes to the CI workflow. +Run ``colcon test --packages-select lidar_point_cloud_filter`` to also execute +the ament linters. +""" + +import importlib.util +import sys +from pathlib import Path + +_repo_root = Path(__file__).resolve().parents[4] +_pkg_test = _repo_root / "robot/ros_ws/src/sensors/lidar_point_cloud_filter/test" +_pkg_root = _pkg_test.parent # adds lidar_point_cloud_filter/ package to sys.path +_real_file = _pkg_test / "test_validation_core.py" + +# Make the package module importable so the real test can do +# `from lidar_point_cloud_filter.validation_core import ...` +if str(_pkg_root) not in sys.path: + sys.path.insert(0, str(_pkg_root)) + +# Load the real module under a unique name to avoid the circular-import that +# would occur if we used `from test_validation_core import *` (this file has +# the same name and pytest adds its directory to sys.path at collection time). +_spec = importlib.util.spec_from_file_location("_lidar_validation_unit_tests", _real_file) +_real = importlib.util.module_from_spec(_spec) +_spec.loader.exec_module(_real) + +# Re-export every test_* symbol so pytest collects them from this proxy. +for _name in dir(_real): + if _name.startswith("test_"): + globals()[_name] = getattr(_real, _name) diff --git a/tests/sensor_probes.py b/tests/sensor_probes.py index cbb634261..140e10c0a 100644 --- a/tests/sensor_probes.py +++ b/tests/sensor_probes.py @@ -2,7 +2,7 @@ # SPDX-License-Identifier: Apache-2.0 """ROS 2 sensor stream checks (sim + robot) for system tests. -Used by ``test_sensors.py``. Liveliness (``test_liveliness.py``) stays limited to +Used by ``system/test_sensors.py``. Liveliness (``system/test_liveliness.py``) stays limited to containers, tmux, and sentinel nodes; sensor Hz / LiDAR validation lives here. **Isaac Sim (`env["sim"] == "isaacsim"`)** — Pegasus / OmniGraph ROS bridges are @@ -50,8 +50,8 @@ ] LIDAR_CLOUD_VALIDATE_SCRIPT = ( - "/root/AirStack/robot/ros_ws/src/sensors/lidar_point_cloud_filter/scripts/" - "validate_lidar_filter_clouds.py" + "/root/AirStack/robot/ros_ws/src/sensors/lidar_point_cloud_filter/" + "scripts/validate_lidar_filter_clouds.py" ) # Shorter Hz sample during sensor stability polling. diff --git a/tests/sim/README.md b/tests/sim/README.md new file mode 100644 index 000000000..09f45f6a6 --- /dev/null +++ b/tests/sim/README.md @@ -0,0 +1,14 @@ +# Simulation-side unit tests + +Tests for **simulation components** that are not part of the onboard ROS workspace +(for example an OptiTrack Motive / NatNet emulator, Isaac launch helpers, or +AirSim bridge utilities). + +Mark fast, hermetic checks with `@pytest.mark.unit`. Tests that require a GPU, +full sim, or Docker belong in [`tests/system/`](../system/) instead. + +Suggested layout: + +| Directory | Purpose | +|-----------|---------| +| `motive_emulator/` | Motive / NatNet protocol emulation / parsing | diff --git a/tests/sim/motive_emulator/README.md b/tests/sim/motive_emulator/README.md new file mode 100644 index 000000000..0e682c448 --- /dev/null +++ b/tests/sim/motive_emulator/README.md @@ -0,0 +1,61 @@ +# Motive / NatNet Emulator + +This directory is the future home of **integration tests** that drive a real +NatNet wire-protocol mock server against `natnet_ros2_node`. + +## Why here, not in the package test/ dir? + +Unit tests for pure logic live in +`tests/robot/perception/natnet_ros2/test_natnet_logic.cpp` and run via `colcon +test` with no network or SDK required (uses `FakeNatNetClient`). + +The emulator tests here will require an actual UDP server that speaks the NatNet +protocol, so they belong in the `sensors` mark of the system test suite alongside +other topic-streaming tests. + +## Planned implementation + +The mock server should: + +1. Open a UDP socket on the NatNet command port (default 1510). +2. Respond to `NAT_CONNECT` (message type 0) with a `NAT_SERVERINFO` (type 1) + packet containing a canned `sServerDescription`. +3. Respond to `NAT_REQUEST_MODELDEF` (type 4) with a `NAT_MODELDEF` (type 5) + packet describing one or more rigid bodies. +4. Stream `NAT_FRAMEOFDATA` (type 7) packets to the client's data port at a + configurable rate with synthetic pose data. + +### Reference + +The NatNet wire format is documented in the NatNet SDK developer notes and the +`PacketClient` example shipped with the SDK (available inside the robot Docker +container after `airstack setup --natnet`). + +## Relationship to `FakeNatNetClient` + +``` + ┌──────────────────────────────────────┐ + │ Test boundary │ + colcon gtest │ FakeNatNetClient (in-process) │ ← unit tests (no network) + │ test_natnet_logic.cpp │ + └──────────────────────────────────────┘ + + ┌──────────────────────────────────────┐ + │ Network boundary │ + pytest sensors │ MotiveEmulator (UDP server, Python) │ ← integration tests + │ NatNetClientAdapter → NatNetClient │ + │ natnet_ros2_node (full ROS node) │ + └──────────────────────────────────────┘ +``` + +The `FakeNatNetClient` seam (already implemented) lets unit tests verify all +connection-outcome logic paths. The emulator here will verify the full +end-to-end path including the NatNet SDK's own parser. + +## When to add this + +Implement the emulator when: +- The OptiTrack emulator service is placed under `simulation/optitrack-emulator/` + or `tests/sim/motive_emulator/` +- The `sensors` test mark is extended to include `natnet_ros2` topic checks +- CI has access to the robot container with the NatNet SDK installed diff --git a/tests/system/__init__.py b/tests/system/__init__.py new file mode 100644 index 000000000..0169df793 --- /dev/null +++ b/tests/system/__init__.py @@ -0,0 +1 @@ +"""Docker-based system and integration tests (stack bring-up, sensors, flight).""" diff --git a/tests/test_build_docker.py b/tests/system/test_build_docker.py similarity index 100% rename from tests/test_build_docker.py rename to tests/system/test_build_docker.py diff --git a/tests/test_build_packages.py b/tests/system/test_build_packages.py similarity index 64% rename from tests/test_build_packages.py rename to tests/system/test_build_packages.py index 40bcf978b..5c54cfee3 100644 --- a/tests/test_build_packages.py +++ b/tests/system/test_build_packages.py @@ -2,7 +2,8 @@ import pytest -from conftest import (AIRSTACK_ROOT, airstack_cmd, docker_exec, logger, +from conftest import (AIRSTACK_ROOT, airstack_cmd, colcon_test_robot_command, + docker_exec, load_colcon_unit_test_config, logger, read_log_tail, wait_for_container) @@ -41,6 +42,44 @@ def test_colcon_build_robot(self): finally: airstack_cmd("down") + def test_colcon_test_robot(self): + """Build with BUILD_TESTING=ON then run colcon test inside the robot container. + + Kept separate from test_colcon_build_robot so that a test failure does + not block the build-only health check, and so the test step can be + re-run independently without a full rebuild. + + Package list and pytest args come from tests/colcon_unit_test_packages.yaml. + Workspace-wide ament linter tests are not gated here. + """ + packages, _ = load_colcon_unit_test_config("robot") + try: + result = airstack_cmd("up", "robot-desktop", + env_overrides={"AUTOLAUNCH": "false", "DISPLAY": ""}, + timeout=120) + assert result.returncode == 0, f"airstack up failed:\n{read_log_tail()}" + + container = wait_for_container("robot.*desktop", timeout=60) + assert container, "Robot container not found" + + build = docker_exec( + container, + "bash -ic \"bws --cmake-args '-DBUILD_TESTING=ON'\"", + timeout=600, + ) + assert build.returncode == 0, f"colcon build (with testing) failed:\n{read_log_tail()}" + + test = docker_exec( + container, + f"bash -ic '{colcon_test_robot_command('robot')}'", + timeout=300, + ) + assert test.returncode == 0, ( + f"colcon test failed (packages: {', '.join(packages)}):\n{read_log_tail()}" + ) + finally: + airstack_cmd("down") + def test_colcon_build_gcs(self): _warn_if_prebuilt("gcs/ros_ws") try: diff --git a/tests/test_liveliness.py b/tests/system/test_liveliness.py similarity index 99% rename from tests/test_liveliness.py rename to tests/system/test_liveliness.py index cd8b027e0..342e5f49a 100644 --- a/tests/test_liveliness.py +++ b/tests/system/test_liveliness.py @@ -5,7 +5,7 @@ snapshots, and a short stability window (infra only — no camera/LiDAR Hz here). Sensor topic rates, bridge stereo Hz, LiDAR echo/sanity, and sim RTF live in -``test_sensors.py`` (``@pytest.mark.sensors``), ordered after this module. +``system/test_sensors.py`` (``@pytest.mark.sensors``), ordered after this module. """ import time diff --git a/tests/test_sensors.py b/tests/system/test_sensors.py similarity index 97% rename from tests/test_sensors.py rename to tests/system/test_sensors.py index f00facb33..8170286d6 100644 --- a/tests/test_sensors.py +++ b/tests/system/test_sensors.py @@ -1,4 +1,4 @@ -"""Sensor stream and LiDAR validation — runs after ``test_liveliness`` (see ``_MODULE_ORDER``). +"""Sensor stream and LiDAR validation — runs after ``system.test_liveliness`` (see ``_MODULE_ORDER``). Uses the same ``airstack_env`` parametrization as liveliness. With ``class``-scoped fixtures this module performs its **own** stack bring-up when selected; combined @@ -20,7 +20,7 @@ check_robot_stereo_hz, check_sim_publishing, ) -from test_liveliness import _check_sentinel_nodes, _poll_until +from system.test_liveliness import _check_sentinel_nodes, _poll_until @pytest.mark.sensors diff --git a/tests/test_takeoff_hover_land.py b/tests/system/test_takeoff_hover_land.py similarity index 100% rename from tests/test_takeoff_hover_land.py rename to tests/system/test_takeoff_hover_land.py