From dfb798e0cf4aec53b11b9937fe1946b2d76818fc Mon Sep 17 00:00:00 2001 From: John Liu Date: Tue, 12 May 2026 16:15:03 -0400 Subject: [PATCH 01/21] incremented version tag --- .env | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.env b/.env index a90a1143e..92403f8c0 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.18.0-alpha.10" +VERSION="0.18.0-alpha.11" # 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. From cf78c9eea1cb9649bef37148f40232625309464e Mon Sep 17 00:00:00 2001 From: John Liu Date: Tue, 12 May 2026 16:15:37 -0400 Subject: [PATCH 02/21] docker image builds on l4t with generalizability features for other ros and linux versions --- robot/docker/Dockerfile.robot | 134 +++++++++++++++++++++---------- robot/docker/docker-compose.yaml | 6 ++ 2 files changed, 97 insertions(+), 43 deletions(-) diff --git a/robot/docker/Dockerfile.robot b/robot/docker/Dockerfile.robot index 55b1ad40f..facdb341c 100644 --- a/robot/docker/Dockerfile.robot +++ b/robot/docker/Dockerfile.robot @@ -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. +RUN python3 -m pip install --no-cache-dir --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/humble/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 @@ -279,22 +314,31 @@ RUN apt update && apt install -y --no-install-recommends \ tmux \ && rm -rf /var/lib/apt/lists/* +# Freeze pip and setuptools versions. +RUN python3 -m pip install --no-cache-dir --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 +346,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 +368,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..560eb5aeb 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: @@ -115,6 +116,7 @@ services: REAL_ROBOT: true SKIP_MACVO: true SKIP_TENSORRT: true + ROS_DISTRO: jazzy tags: - *voxl_image cache_from: @@ -147,9 +149,13 @@ 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 REAL_ROBOT: true + SKIP_MACVO: true + ROS_DISTRO: humble + PYTHON_VERSION: "3.10" tags: - *l4t_image cache_from: From b2531de74e43de215f7be09b6cd2559aa192cc6b Mon Sep 17 00:00:00 2001 From: John Liu Date: Tue, 12 May 2026 16:55:02 -0400 Subject: [PATCH 03/21] documentation and claude skills for developing a new profile. --- .agents/skills/docker-build-profiles/SKILL.md | 133 ++++++++++++++++++ docs/development/docker-build-profiles.md | 50 +++++++ 2 files changed, 183 insertions(+) create mode 100644 .agents/skills/docker-build-profiles/SKILL.md create mode 100644 docs/development/docker-build-profiles.md diff --git a/.agents/skills/docker-build-profiles/SKILL.md b/.agents/skills/docker-build-profiles/SKILL.md new file mode 100644 index 000000000..d5229dc8d --- /dev/null +++ b/.agents/skills/docker-build-profiles/SKILL.md @@ -0,0 +1,133 @@ +# 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. + +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/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. From 5821f2d1c6f316491bd9e33a76f2b10590979cdc Mon Sep 17 00:00:00 2001 From: John Liu Date: Wed, 13 May 2026 17:29:10 -0400 Subject: [PATCH 04/21] initial natnet implementation --- mkdocs.yml | 1 + robot/docker/docker-compose.yaml | 2 + .../src/perception/natnet_ros2/.gitignore | 27 ++ .../src/perception/natnet_ros2/CMakeLists.txt | 37 ++ .../src/perception/natnet_ros2/README.md | 196 ++++++++ .../natnet_ros2/config/natnet_config.yaml | 41 ++ .../natnet_ros2/launch/natnet_ros2.launch.xml | 66 +++ .../launch/vision_pose_converter.launch.xml | 39 ++ .../src/perception/natnet_ros2/package.xml | 37 ++ .../natnet_ros2/src/natnet_client.py | 421 ++++++++++++++++++ .../natnet_ros2/src/natnet_ros2_node.py | 221 +++++++++ .../src/vision_pose_converter_node.py | 94 ++++ .../launch/perception.launch.xml | 10 +- 13 files changed, 1191 insertions(+), 1 deletion(-) create mode 100644 robot/ros_ws/src/perception/natnet_ros2/.gitignore create mode 100644 robot/ros_ws/src/perception/natnet_ros2/CMakeLists.txt create mode 100644 robot/ros_ws/src/perception/natnet_ros2/README.md create mode 100644 robot/ros_ws/src/perception/natnet_ros2/config/natnet_config.yaml create mode 100644 robot/ros_ws/src/perception/natnet_ros2/launch/natnet_ros2.launch.xml create mode 100644 robot/ros_ws/src/perception/natnet_ros2/launch/vision_pose_converter.launch.xml create mode 100644 robot/ros_ws/src/perception/natnet_ros2/package.xml create mode 100644 robot/ros_ws/src/perception/natnet_ros2/src/natnet_client.py create mode 100644 robot/ros_ws/src/perception/natnet_ros2/src/natnet_ros2_node.py create mode 100644 robot/ros_ws/src/perception/natnet_ros2/src/vision_pose_converter_node.py diff --git a/mkdocs.yml b/mkdocs.yml index 00a1aee14..bfbdccb06 100644 --- a/mkdocs.yml +++ b/mkdocs.yml @@ -111,6 +111,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/docker-compose.yaml b/robot/docker/docker-compose.yaml index 560eb5aeb..bd07aa4d2 100644 --- a/robot/docker/docker-compose.yaml +++ b/robot/docker/docker-compose.yaml @@ -177,6 +177,7 @@ services: - driver: nvidia count: 1 capabilities: [gpu] + runtime: nvidia environment: !override - ROBOT_NAME_SOURCE=hostname # see .bashrc - AUTOLAUNCH=${AUTOLAUNCH:-true} @@ -234,6 +235,7 @@ services: privileged: true ipc: host pid: host + runtime: nvidia environment: - NVIDIA_DRIVER_CAPABILITIES=all - DISPLAY 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..2eb707f11 --- /dev/null +++ b/robot/ros_ws/src/perception/natnet_ros2/CMakeLists.txt @@ -0,0 +1,37 @@ +cmake_minimum_required(VERSION 3.8) +project(natnet_ros2) + +# Compiler settings +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# Pure Python package - no SDK required +# Uses pure Python parsing of NatNet protocol (no C++ dependencies) + +# Find dependencies +find_package(ament_cmake REQUIRED) + +# Install launch files +install(DIRECTORY launch/ + DESTINATION share/${PROJECT_NAME}/launch) + +# Install config files +install(DIRECTORY config/ + DESTINATION share/${PROJECT_NAME}/config) + +# Install executable scripts and shared modules into the same directory +# so natnet_ros2_node.py can "from natnet_client import ..." at runtime +install(PROGRAMS + src/natnet_client.py + src/natnet_ros2_node.py + src/vision_pose_converter_node.py + DESTINATION lib/${PROJECT_NAME}) + +# Testing +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_files() +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..ce675ad1f --- /dev/null +++ b/robot/ros_ws/src/perception/natnet_ros2/README.md @@ -0,0 +1,196 @@ +# 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 + server_port: 1511 # NatNet UDP port + + # 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 (Direct OptiTrack) +```bash +ros2 launch natnet_ros2 natnet_ros2.launch.xml \ + natnet_server_ip:=192.168.1.1 \ + natnet_server_port:=1511 \ + publish_direct_optitrack:=true +``` + +### With MAVROS Bridge +```bash +ros2 launch natnet_ros2 natnet_ros2.launch.xml \ + natnet_server_ip:=192.168.1.1 \ + publish_to_mavros:=true +``` + +### From Sensors Bringup +The sensor layer orchestrator can include this module conditionally: +```bash +ros2 launch sensors_bringup sensors_bringup.launch.xml \ + enable_natnet:=true \ + natnet_server_ip:=192.168.1.1 +``` + +## 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.xml + ``` +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..ee8254e0e --- /dev/null +++ b/robot/ros_ws/src/perception/natnet_ros2/config/natnet_config.yaml @@ -0,0 +1,41 @@ +# NatNet ROS 2 Configuration + +natnet: + # Motive PC network settings + server_ip: "192.168.1.1" # IP of machine running Motive + client_ip: "192.168.1.100" # IP of this machine + server_type: "unicast" # "multicast" not currently supported + command_port: 1510 # NatNet command port (default 1510) + data_port: 1511 # NatNet data port (default 1511) + natnet_versions: # Try these versions in order + - [4, 4, 0, 0] + - [4, 3, 0, 0] + - [4, 2, 0, 0] + - [4, 1, 0, 0] + - [4, 0, 0, 0] + + # Rigid body tracking + body_name: "robot_1" # Label used in topic names for the tracked body + body_id: -1 # Motive rigid body ID to track; -1 = publish all bodies + + # Publishing behavior + publish_direct_optitrack: true # Publish /perception/optitrack/{body_name} + publish_to_mavros: false # Publish to /mavros/vision_pose/ for PX4 fusion + + # Frame IDs + frame_id: "world" # Reference frame for poses + child_frame_id: "base_link" # Child frame (when publishing odometry) + + # Covariance (default values for uncertainty) + position_covariance: + [0.1, 0.0, 0.0, + 0.0, 0.1, 0.0, + 0.0, 0.0, 0.1] # 3x3 matrix (flattened) + + orientation_covariance: + [0.01, 0.0, 0.0, + 0.0, 0.01, 0.0, + 0.0, 0.0, 0.01] # 3x3 matrix (flattened) + + # Logging + debug: false # Enable debug messages diff --git a/robot/ros_ws/src/perception/natnet_ros2/launch/natnet_ros2.launch.xml b/robot/ros_ws/src/perception/natnet_ros2/launch/natnet_ros2.launch.xml new file mode 100644 index 000000000..553499848 --- /dev/null +++ b/robot/ros_ws/src/perception/natnet_ros2/launch/natnet_ros2.launch.xml @@ -0,0 +1,66 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 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..621d99a22 --- /dev/null +++ b/robot/ros_ws/src/perception/natnet_ros2/launch/vision_pose_converter.launch.xml @@ -0,0 +1,39 @@ + + + + + + + + + + + + + + + + + + + + + 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..d91efa34e --- /dev/null +++ b/robot/ros_ws/src/perception/natnet_ros2/package.xml @@ -0,0 +1,37 @@ + + + + natnet_ros2 + 0.1.0 + + NatNet ROS 2 wrapper for OptiTrack Motive motion capture integration. + Receives NatNet UDP packets from external Motive PC and publishes + pose data into the AirStack perception layer. + + + AirLab CMU + Apache-2.0 + + + ament_cmake + + + rclpy + geometry_msgs + nav_msgs + tf_transformations + airstack_msgs + + + mavros_msgs + + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_cmake + + diff --git a/robot/ros_ws/src/perception/natnet_ros2/src/natnet_client.py b/robot/ros_ws/src/perception/natnet_ros2/src/natnet_client.py new file mode 100644 index 000000000..532623a97 --- /dev/null +++ b/robot/ros_ws/src/perception/natnet_ros2/src/natnet_client.py @@ -0,0 +1,421 @@ +#!/usr/bin/env python3 + +""" +NatNet Protocol Client (Pure Python) + +Decodes NatNet 4.4 protocol UDP packets from OptiTrack Motive software. +Supports rigid body tracking via pure Python parsing (no SDK required). + +Reference: OptiTrack NatNet 4.4 Documentation +https://v20.wiki.optitrack.com/index.php?title=NatNet +""" + +import struct +import socket +import threading +import time +from typing import Dict, List, Tuple, Optional, Callable +from dataclasses import dataclass +from enum import IntEnum + + +class MessageType(IntEnum): + """NatNet message types""" + FRAME_OF_DATA = 101 + MODELDEF = 102 + REQUEST_MODELDEF = 103 + SENDMODE_MULTICAST = 200 + SENDMODE_UNICAST = 201 + NAT_REQUEST = 0 + NAT_RESPONSE = 1 + NAT_REQUEST_REQUEST = 3 + NAT_REQUEST_MODELDEF = 4 + + +@dataclass +class RigidBodyData: + """Rigid body pose and kinematic data""" + id: int + position: Tuple[float, float, float] # (x, y, z) + rotation: Tuple[float, float, float, float] # (qx, qy, qz, qw) + markers: List[Tuple[float, float, float]] # List of marker positions + mean_marker_error: float + tracking_valid: bool + timestamp: float + + +@dataclass +class FrameData: + """Complete frame of motion capture data""" + frame_number: int + timestamp: float + rigid_bodies: Dict[int, RigidBodyData] + + +class NatNetClient: + """ + Pure Python NatNet protocol client for OptiTrack Motive. + + Receives NatNet UDP packets and decodes motion capture frames. + Supports rigid body tracking (basic motion capture data). + + Features: + - Version negotiation: Requests NatNet version from Motive + - Fallback support: Tries NatNet 4.4 → 4.3 → 4.2 → 4.1 → 4.0 + - Robust parsing: Handles optional fields and frame variations + + Usage: + client = NatNetClient("192.168.1.1", 1511) + client.set_frame_callback(my_callback) + client.start() + # ... receives frames in background thread + client.stop() + """ + + def __init__(self, server_ip: str, server_port: int): + """ + Initialize NatNet client. + + Args: + server_ip: IP address of Motive PC + server_port: Local UDP port to listen on (default 1511) + """ + self.server_ip = server_ip + self.server_port = server_port + self.sock = None + self.running = False + self.receive_thread = None + self._user_callback: Optional[Callable[[FrameData], None]] = None + self.negotiated_version = None + + # Versions to try in order (4.4 → 4.3 → ... → 4.0) + self.supported_versions = [ + (4, 4, 0, 0), + (4, 3, 0, 0), + (4, 2, 0, 0), + (4, 1, 0, 0), + (4, 0, 0, 0), + ] + + def set_frame_callback(self, callback: Callable[[FrameData], None]): + """ + Register callback to receive decoded frames. + + Args: + callback: Function(FrameData) called when frame is received + """ + self._user_callback = callback + + def _negotiate_version(self) -> bool: + """ + Negotiate NatNet version with Motive PC via command port (1510). + + Sends SetNatNetVersion request for each supported version in order. + Stores the negotiated version in self.negotiated_version. + + Returns: + True if version successfully negotiated, False if all attempts failed + """ + command_port = 1510 + command_sock = None + + try: + # Create UDP socket for command communication + command_sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + command_sock.settimeout(2.0) # 2 second timeout per attempt + + # Try each supported version in order (4.4 → 4.3 → ... → 4.0) + for version_tuple in self.supported_versions: + major, minor, build, revision = version_tuple + + try: + # Build SetNatNetVersion command packet + # Format: message_type (2B) + packet_size (2B) + version (4B each) + message_type = 0 # SetNatNetVersion request + packet_size = 20 # Header + version data + + packet = struct.pack( + '= 8: + resp_type, resp_size = struct.unpack(' bool: + """ + Start listening for NatNet UDP packets. + + First negotiates NatNet version with Motive PC, then binds to data port. + + Returns: + True if socket bound successfully, False otherwise + """ + try: + # Attempt to negotiate version with Motive before starting data listener + print("[NatNetClient] Attempting version negotiation with Motive PC...") + self._negotiate_version() + + # Bind to data port for frame reception + self.sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + self.sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) + self.sock.bind(('0.0.0.0', self.server_port)) + self.sock.setblocking(True) + + self.running = True + self.receive_thread = threading.Thread(target=self._receive_loop, daemon=True) + self.receive_thread.start() + + version_str = f"{self.negotiated_version[0]}.{self.negotiated_version[1]}" + print(f"[NatNetClient] Listening on UDP port {self.server_port} " + f"(NatNet {version_str})") + return True + + except Exception as e: + print(f"[NatNetClient] Failed to start: {e}") + return False + + def stop(self): + """Stop listening and clean up resources.""" + self.running = False + if self.receive_thread: + self.receive_thread.join(timeout=1.0) + if self.sock: + self.sock.close() + print("[NatNetClient] Stopped") + + def _receive_loop(self): + """Background thread that receives and processes UDP packets.""" + while self.running: + try: + data, addr = self.sock.recvfrom(65535) + frame = self.parse_frame(data) + if frame and self._user_callback: + self._user_callback(frame) + except socket.timeout: + continue + except Exception as e: + if self.running: + print(f"[NatNetClient] Error receiving packet: {e}") + + def parse_frame(self, data: bytes) -> Optional[FrameData]: + """ + Parse a NatNet frame from UDP packet data. + + NatNet 4.4 frame structure: + - Packet ID (2 bytes): Message type + - Packet size (2 bytes): Total packet size + - Frame number (4 bytes) + - Marker set count (4 bytes) + [marker set data] + - Rigid body count (4 bytes) + [rigid body data] + - Latency (4 bytes) + - Timestamp (8 bytes, optional) + + Args: + data: Raw UDP packet bytes from Motive + + Returns: + FrameData if successful, None if parsing fails + """ + try: + if len(data) < 4: + return None + + offset = 0 + + # Parse packet header + message_type, packet_size = struct.unpack(' 0 else latency, + rigid_bodies=rigid_bodies + ) + + except Exception as e: + print(f"[NatNetClient] Error parsing frame: {e}") + return None + + def _skip_marker_set(self, data: bytes, offset: int) -> int: + """Skip over marker set data in frame (we don't need it for rigid body tracking).""" + try: + # Marker set name (null-terminated string) + while offset < len(data) and data[offset] != 0: + offset += 1 + offset += 1 # Skip null terminator + + # Number of markers in this set + if offset + 4 > len(data): + return offset + num_markers = struct.unpack(' Tuple[Optional[RigidBodyData], int]: + """ + Parse a single rigid body from frame data. + + Returns: + (RigidBodyData, new_offset) or (None, offset) if parsing fails + """ + try: + start_offset = offset + + # Rigid body ID + if offset + 4 > len(data): + return None, offset + rb_id = struct.unpack(' len(data): + return None, start_offset + pos_x, pos_y, pos_z = struct.unpack(' len(data): + return None, start_offset + qx, qy, qz, qw = struct.unpack(' len(data): + return None, start_offset + num_markers = struct.unpack(' len(data): + # Not enough data for markers + num_markers = 0 + else: + for _ in range(num_markers): + mx, my, mz = struct.unpack(' len(data): + return None, start_offset + mean_error = struct.unpack(' len(data): + return None, start_offset + tracking_valid = bool(struct.unpack('= 0: + self.get_logger().info( + f'Tracking body ID {self.body_id} as "{self.body_name}"' + ) + else: + self.get_logger().info('Tracking all bodies (body_id = -1)') + + def _build_6x6_covariance( + self, pos_cov_3x3: list, ori_cov_3x3: list + ) -> list: + """Build a 6x6 covariance matrix (row-major) from two 3x3 blocks.""" + c = [0.0] * 36 + # Position block (rows 0-2, cols 0-2) + for r in range(3): + for col in range(3): + c[r * 6 + col] = float(pos_cov_3x3[r * 3 + col]) + # Orientation block (rows 3-5, cols 3-5) + for r in range(3): + for col in range(3): + c[(r + 3) * 6 + (col + 3)] = float(ori_cov_3x3[r * 3 + col]) + return c + + def destroy_node(self): + """Clean up resources on shutdown.""" + if self.natnet_client: + self.natnet_client.stop() + super().destroy_node() + + def _on_frame_received(self, frame: FrameData): + """Callback invoked when a complete NatNet frame is received.""" + try: + if self.debug: + self.get_logger().debug( + f'Frame {frame.frame_number}: ' + f'{len(frame.rigid_bodies)} rigid bodies' + ) + + for rb_id, rb_data in frame.rigid_bodies.items(): + if not rb_data.tracking_valid: + continue + + # Filter by configured body_id when set + if self.body_id >= 0 and rb_id != self.body_id: + continue + + body_name = self._get_body_name(rb_id) + + if self.publish_direct: + self._publish_pose(body_name, rb_data) + + # Always publish pose_cov as the primary pose output + self._publish_pose_cov(body_name, rb_data) + + except Exception as e: + self.get_logger().error(f'Error processing frame: {e}') + + def _publish_pose(self, body_name: str, rb_data) -> None: + """Publish direct optitrack pose as PoseStamped.""" + topic = f'/{self.robot_name}/perception/optitrack/{body_name}' + if topic not in self.pose_publishers: + self.pose_publishers[topic] = self.create_publisher( + PoseStamped, topic, 10 + ) + + msg = PoseStamped() + msg.header.frame_id = self.frame_id + msg.header.stamp = self.get_clock().now().to_msg() + msg.pose.position.x = float(rb_data.position[0]) + msg.pose.position.y = float(rb_data.position[1]) + msg.pose.position.z = float(rb_data.position[2]) + msg.pose.orientation.x = float(rb_data.rotation[0]) + msg.pose.orientation.y = float(rb_data.rotation[1]) + msg.pose.orientation.z = float(rb_data.rotation[2]) + msg.pose.orientation.w = float(rb_data.rotation[3]) + + self.pose_publishers[topic].publish(msg) + + def _publish_pose_cov(self, body_name: str, rb_data) -> None: + """Publish PoseWithCovarianceStamped for this body.""" + topic = f'/{self.robot_name}/perception/optitrack/{body_name}/pose_cov' + if topic not in self.pose_cov_publishers: + self.pose_cov_publishers[topic] = self.create_publisher( + PoseWithCovarianceStamped, topic, 10 + ) + + msg = PoseWithCovarianceStamped() + msg.header.frame_id = self.frame_id + msg.header.stamp = self.get_clock().now().to_msg() + msg.pose.pose.position.x = float(rb_data.position[0]) + msg.pose.pose.position.y = float(rb_data.position[1]) + msg.pose.pose.position.z = float(rb_data.position[2]) + msg.pose.pose.orientation.x = float(rb_data.rotation[0]) + msg.pose.pose.orientation.y = float(rb_data.rotation[1]) + msg.pose.pose.orientation.z = float(rb_data.rotation[2]) + msg.pose.pose.orientation.w = float(rb_data.rotation[3]) + msg.pose.covariance = self.covariance_6x6 + + self.pose_cov_publishers[topic].publish(msg) + + def _get_body_name(self, body_id: int) -> str: + """ + Map a rigid body ID to a topic-friendly name. + + When tracking a specific body (body_id param >= 0), the configured + body_name is used. For all-body mode, falls back to body_{id}. + """ + if self.body_id >= 0 and body_id == self.body_id: + return self.body_name + return f'body_{body_id}' + + +def main(args=None): + rclpy.init(args=args) + try: + node = NatNetROS2Node() + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + rclpy.shutdown() + + +if __name__ == '__main__': + main() 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 100644 index 000000000..521b52577 --- /dev/null +++ b/robot/ros_ws/src/perception/natnet_ros2/src/vision_pose_converter_node.py @@ -0,0 +1,94 @@ +#!/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') + + # Declare parameters + self.declare_parameter('frame_id', 'world') + self.declare_parameter('child_frame_id', 'base_link') + + # Get parameters + self.frame_id = self.get_parameter('frame_id').value + self.child_frame_id = self.get_parameter('child_frame_id').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("Vision pose converter node started") + + def _on_pose(self, msg: PoseWithCovarianceStamped): + """ + Callback for incoming NatNet pose. + + Converts and republishes for MAVROS consumption. + """ + try: + # Stamp the message with the configured reference frame ID + msg.header.frame_id = self.frame_id + + # Republish with covariance + self.pose_cov_pub.publish(msg) + + # Also publish PoseStamped version + 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/perception_bringup/launch/perception.launch.xml b/robot/ros_ws/src/perception/perception_bringup/launch/perception.launch.xml index bdc1d7d25..d9e476624 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,8 @@ + + @@ -68,8 +70,14 @@ + + + + + - Date: Thu, 14 May 2026 00:37:19 -0400 Subject: [PATCH 05/21] deployment to jetson with ros2 jazzy now fixed --- .agents/skills/docker-build-profiles/SKILL.md | 1 + .env | 2 +- airstack.sh | 36 +++++++++++++ robot/docker/Dockerfile.l4t-stack-base | 50 +++++++++++++++++++ robot/docker/Dockerfile.robot | 14 +++--- robot/docker/docker-compose.yaml | 30 +++++++++-- robot/docker/zed/Dockerfile.zed-l4t | 2 +- 7 files changed, 122 insertions(+), 13 deletions(-) create mode 100644 robot/docker/Dockerfile.l4t-stack-base diff --git a/.agents/skills/docker-build-profiles/SKILL.md b/.agents/skills/docker-build-profiles/SKILL.md index d5229dc8d..64b579a01 100644 --- a/.agents/skills/docker-build-profiles/SKILL.md +++ b/.agents/skills/docker-build-profiles/SKILL.md @@ -48,6 +48,7 @@ Guidance for agents when editing the repo 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." diff --git a/.env b/.env index 92403f8c0..fb08969f0 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.18.0-alpha.11" +VERSION="cc3b392d" # 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/airstack.sh b/airstack.sh index 3d1e955e3..f82224aaf 100755 --- a/airstack.sh +++ b/airstack.sh @@ -728,6 +728,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 +800,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/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 facdb341c..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 @@ -85,8 +85,8 @@ RUN apt update && apt install -y --no-install-recommends \ gdb \ && rm -rf /var/lib/apt/lists/* -# Freeze pip and setuptools versions. -RUN python3 -m pip install --no-cache-dir --upgrade \ +# 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 @@ -210,7 +210,7 @@ RUN echo "=== Python version ===" && \ dpkg -l | grep -i ament || echo "No ament packages found in dpkg" && \ echo "" && \ echo "=== ROS Python packages ===" && \ - ls -la /opt/ros/humble/lib/python*/dist-packages/ 2>/dev/null | head -20 || echo "No ROS python packages found" + 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) @@ -310,12 +310,14 @@ 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. -RUN python3 -m pip install --no-cache-dir --upgrade \ +# 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 diff --git a/robot/docker/docker-compose.yaml b/robot/docker/docker-compose.yaml index bd07aa4d2..fdd6458da 100644 --- a/robot/docker/docker-compose.yaml +++ b/robot/docker/docker-compose.yaml @@ -136,6 +136,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: @@ -151,15 +168,16 @@ services: 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 - ROS_DISTRO: humble - PYTHON_VERSION: "3.10" + 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: > @@ -178,8 +196,8 @@ services: count: 1 capabilities: [gpu] runtime: nvidia - environment: !override - - ROBOT_NAME_SOURCE=hostname # see .bashrc + environment: + - ROBOT_NAME_SOURCE=hostname - AUTOLAUNCH=${AUTOLAUNCH:-true} - LAUNCH_PACKAGE=autonomy_bringup - AUTONOMY_ROLE=full # l4t profile: Jetson runs everything onboard @@ -212,10 +230,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: > 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} && \ From 6312dbdfed27feda439dd4f029d04530435ac939 Mon Sep 17 00:00:00 2001 From: John Liu Date: Thu, 14 May 2026 00:38:51 -0400 Subject: [PATCH 06/21] unit testing dependency fix --- robot/ros_ws/src/perception/natnet_ros2/CMakeLists.txt | 2 +- robot/ros_ws/src/perception/natnet_ros2/package.xml | 1 + 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/robot/ros_ws/src/perception/natnet_ros2/CMakeLists.txt b/robot/ros_ws/src/perception/natnet_ros2/CMakeLists.txt index 2eb707f11..6ec87bcde 100644 --- a/robot/ros_ws/src/perception/natnet_ros2/CMakeLists.txt +++ b/robot/ros_ws/src/perception/natnet_ros2/CMakeLists.txt @@ -31,7 +31,7 @@ install(PROGRAMS # Testing if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) - ament_lint_auto_find_test_files() + ament_lint_auto_find_test_dependencies() endif() ament_package() diff --git a/robot/ros_ws/src/perception/natnet_ros2/package.xml b/robot/ros_ws/src/perception/natnet_ros2/package.xml index d91efa34e..3cb16cdd0 100644 --- a/robot/ros_ws/src/perception/natnet_ros2/package.xml +++ b/robot/ros_ws/src/perception/natnet_ros2/package.xml @@ -26,6 +26,7 @@ mavros_msgs + ament_lint_auto ament_copyright ament_flake8 ament_pep257 From 1c6f4235091c1704eeab28ec9150afde06e6b1f3 Mon Sep 17 00:00:00 2001 From: John Liu Date: Thu, 14 May 2026 00:40:46 -0400 Subject: [PATCH 07/21] added optitrack perception to launch --- .../perception/perception_bringup/launch/perception.launch.xml | 2 ++ 1 file changed, 2 insertions(+) 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 d9e476624..ea2161ae5 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 @@ -69,6 +69,7 @@ + + Date: Thu, 14 May 2026 09:43:55 -0400 Subject: [PATCH 08/21] put tag version back in --- .env | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.env b/.env index fb08969f0..92403f8c0 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="cc3b392d" +VERSION="0.18.0-alpha.11" # 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. From 75b7a44e4f6b28f7613c73d2a222fd2aae91fd5e Mon Sep 17 00:00:00 2001 From: John Date: Fri, 15 May 2026 16:19:32 -0400 Subject: [PATCH 09/21] added instructions for Agents to run tests --- AGENTS.md | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/AGENTS.md b/AGENTS.md index f9eed977c..e65d170c1 100644 --- a/AGENTS.md +++ b/AGENTS.md @@ -195,6 +195,11 @@ 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`). + + ```bash + docker exec airstack-robot-desktop-1 bash -c "sws && colcon test --packages-select natnet_ros2 --event-handlers console_direct+" + ``` 2. **System Level:** Full simulation tests (Isaac Sim or Microsoft AirSim legacy) - End-to-end autonomy stack testing From 74ce0f4ce4b871a395de98ead1eb7f95cd71e5fd Mon Sep 17 00:00:00 2001 From: John Liu Date: Fri, 15 May 2026 17:58:30 -0400 Subject: [PATCH 10/21] attempt at completely custom Optitrack Parser (Not working) --- .../src/perception/natnet_ros2/README.md | 36 +- .../natnet_ros2/config/natnet_config.yaml | 64 +- .../config/vision_pose_converter.yaml | 9 + .../natnet_ros2/launch/natnet_ros2.launch.py | 99 +++ .../natnet_ros2/launch/natnet_ros2.launch.xml | 66 -- .../launch/vision_pose_converter.launch.xml | 24 +- .../src/perception/natnet_ros2/package.xml | 5 + .../natnet_ros2/src/natnet_client.py | 610 +++++++++++++----- .../natnet_ros2/src/natnet_ros2_node.py | 45 +- .../src/vision_pose_converter_node.py | 8 +- .../launch/perception.launch.xml | 11 +- 11 files changed, 665 insertions(+), 312 deletions(-) create mode 100644 robot/ros_ws/src/perception/natnet_ros2/config/vision_pose_converter.yaml create mode 100644 robot/ros_ws/src/perception/natnet_ros2/launch/natnet_ros2.launch.py delete mode 100644 robot/ros_ws/src/perception/natnet_ros2/launch/natnet_ros2.launch.xml mode change 100644 => 100755 robot/ros_ws/src/perception/natnet_ros2/src/natnet_client.py mode change 100644 => 100755 robot/ros_ws/src/perception/natnet_ros2/src/natnet_ros2_node.py mode change 100644 => 100755 robot/ros_ws/src/perception/natnet_ros2/src/vision_pose_converter_node.py diff --git a/robot/ros_ws/src/perception/natnet_ros2/README.md b/robot/ros_ws/src/perception/natnet_ros2/README.md index ce675ad1f..23b5d13d3 100644 --- a/robot/ros_ws/src/perception/natnet_ros2/README.md +++ b/robot/ros_ws/src/perception/natnet_ros2/README.md @@ -62,7 +62,7 @@ Edit `config/natnet_config.yaml`: natnet: # Motive PC network settings server_ip: "192.168.1.1" # IP of Motive PC - server_port: 1511 # NatNet UDP port + data_port: 1511 # Local UDP port bound for NatNet data stream # Rigid body tracking body_names: @@ -84,29 +84,25 @@ natnet: ## Launch -### Basic Launch (Direct OptiTrack) -```bash -ros2 launch natnet_ros2 natnet_ros2.launch.xml \ - natnet_server_ip:=192.168.1.1 \ - natnet_server_port:=1511 \ - publish_direct_optitrack:=true -``` +### Basic launch -### With MAVROS Bridge -```bash -ros2 launch natnet_ros2 natnet_ros2.launch.xml \ - natnet_server_ip:=192.168.1.1 \ - publish_to_mavros:=true -``` +Parameters come from `config/natnet_config.yaml` (network, body, covariance). Optional overrides: -### From Sensors Bringup -The sensor layer orchestrator can include this module conditionally: ```bash -ros2 launch sensors_bringup sensors_bringup.launch.xml \ - enable_natnet:=true \ - natnet_server_ip:=192.168.1.1 +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 @@ -150,7 +146,7 @@ Each container instance gets its own `ROBOT_NAME` and `ROS_DOMAIN_ID`: 2. Configure server IP in `natnet_config.yaml` 3. Launch the node: ```bash - ros2 launch natnet_ros2 natnet_ros2.launch.xml + ros2 launch natnet_ros2 natnet_ros2.launch.py ``` 4. Verify topics: ```bash 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 index ee8254e0e..29fc7a2f0 100644 --- a/robot/ros_ws/src/perception/natnet_ros2/config/natnet_config.yaml +++ b/robot/ros_ws/src/perception/natnet_ros2/config/natnet_config.yaml @@ -1,41 +1,35 @@ -# NatNet ROS 2 Configuration +# 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 -natnet: - # Motive PC network settings - server_ip: "192.168.1.1" # IP of machine running Motive - client_ip: "192.168.1.100" # IP of this machine - server_type: "unicast" # "multicast" not currently supported - command_port: 1510 # NatNet command port (default 1510) - data_port: 1511 # NatNet data port (default 1511) - natnet_versions: # Try these versions in order - - [4, 4, 0, 0] - - [4, 3, 0, 0] - - [4, 2, 0, 0] - - [4, 1, 0, 0] - - [4, 0, 0, 0] +/**: + ros__parameters: + server_ip: "192.168.0.77" + # 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" - # Rigid body tracking - body_name: "robot_1" # Label used in topic names for the tracked body - body_id: -1 # Motive rigid body ID to track; -1 = publish all bodies + command_port: 1510 + data_port: 1511 + negotiation_enabled: true - # Publishing behavior - publish_direct_optitrack: true # Publish /perception/optitrack/{body_name} - publish_to_mavros: false # Publish to /mavros/vision_pose/ for PX4 fusion - - # Frame IDs - frame_id: "world" # Reference frame for poses - child_frame_id: "base_link" # Child frame (when publishing odometry) + body_name: "HummingbirdDrone" + body_id: 3 - # Covariance (default values for uncertainty) - position_covariance: - [0.1, 0.0, 0.0, - 0.0, 0.1, 0.0, - 0.0, 0.0, 0.1] # 3x3 matrix (flattened) + publish_direct_optitrack: true + publish_to_mavros: false - orientation_covariance: - [0.01, 0.0, 0.0, - 0.0, 0.01, 0.0, - 0.0, 0.0, 0.01] # 3x3 matrix (flattened) + frame_id: "world" + debug: false - # Logging - debug: false # Enable debug messages + 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..95e60566d --- /dev/null +++ b/robot/ros_ws/src/perception/natnet_ros2/config/vision_pose_converter.yaml @@ -0,0 +1,9 @@ +# 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" + use_sim_time: false 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..76f61e165 --- /dev/null +++ b/robot/ros_ws/src/perception/natnet_ros2/launch/natnet_ros2.launch.py @@ -0,0 +1,99 @@ +#!/usr/bin/env python3 +"""Bring up NatNet node; optionally MAVROS bridge per natnet_config.yaml.""" + +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')) + + actions = [ + Node( + package='natnet_ros2', + executable='natnet_ros2_node.py', + 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/natnet_ros2.launch.xml b/robot/ros_ws/src/perception/natnet_ros2/launch/natnet_ros2.launch.xml deleted file mode 100644 index 553499848..000000000 --- a/robot/ros_ws/src/perception/natnet_ros2/launch/natnet_ros2.launch.xml +++ /dev/null @@ -1,66 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 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 index 621d99a22..aad9c4474 100644 --- 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 @@ -6,6 +6,9 @@ Optional bridge that re-publishes NatNet pose_cov data on the MAVROS vision_pose topics so PX4 can fuse external pose estimates. + Static params (frame_id, child_frame_id, default use_sim_time): config/vision_pose_converter.yaml + Topic wiring still uses body_name + ROBOT_NAME below. + Input (from NatNet node): /{ROBOT_NAME}/perception/optitrack/{body_name}/pose_cov @@ -14,26 +17,29 @@ /{ROBOT_NAME}/mavros/vision_pose/pose_cov --> - + + + + + + description="Forwarded after YAML so sim stacks can force true"/> - + + - - - - - + diff --git a/robot/ros_ws/src/perception/natnet_ros2/package.xml b/robot/ros_ws/src/perception/natnet_ros2/package.xml index 3cb16cdd0..ad315e611 100644 --- a/robot/ros_ws/src/perception/natnet_ros2/package.xml +++ b/robot/ros_ws/src/perception/natnet_ros2/package.xml @@ -25,6 +25,11 @@ mavros_msgs + ament_index_python + launch + launch_ros + python3-yaml + ament_lint_auto ament_copyright diff --git a/robot/ros_ws/src/perception/natnet_ros2/src/natnet_client.py b/robot/ros_ws/src/perception/natnet_ros2/src/natnet_client.py old mode 100644 new mode 100755 index 532623a97..44c10e328 --- a/robot/ros_ws/src/perception/natnet_ros2/src/natnet_client.py +++ b/robot/ros_ws/src/perception/natnet_ros2/src/natnet_client.py @@ -20,16 +20,18 @@ class MessageType(IntEnum): - """NatNet message types""" - FRAME_OF_DATA = 101 - MODELDEF = 102 - REQUEST_MODELDEF = 103 - SENDMODE_MULTICAST = 200 - SENDMODE_UNICAST = 201 - NAT_REQUEST = 0 - NAT_RESPONSE = 1 - NAT_REQUEST_REQUEST = 3 + """NatNet command-channel / packet IDs (NatNetTypes.h).""" + + NAT_CONNECT = 0 + NAT_SERVERINFO = 1 + NAT_REQUEST = 2 + NAT_RESPONSE = 3 NAT_REQUEST_MODELDEF = 4 + NAT_MODELDEF = 5 + NAT_REQUEST_FRAMEOFDATA = 6 + FRAME_OF_DATA = 7 # NAT_FRAMEOFDATA (mocap data stream) + NAT_MESSAGESTRING = 8 + NAT_UNRECOGNIZED_REQUEST = 100 @dataclass @@ -63,25 +65,47 @@ class NatNetClient: - Version negotiation: Requests NatNet version from Motive - Fallback support: Tries NatNet 4.4 → 4.3 → 4.2 → 4.1 → 4.0 - Robust parsing: Handles optional fields and frame variations - + Usage: - client = NatNetClient("192.168.1.1", 1511) + client = NatNetClient("192.168.1.1", 1511) # Motive IP, local data UDP port client.set_frame_callback(my_callback) client.start() # ... receives frames in background thread client.stop() """ - def __init__(self, server_ip: str, server_port: int): + def __init__( + self, + server_ip: str, + data_port: int, + *, + command_port: int = 1510, + local_ip: str = '0.0.0.0', + negotiation_enabled: bool = True, + ): """ Initialize NatNet client. - + Args: - server_ip: IP address of Motive PC - server_port: Local UDP port to listen on (default 1511) + server_ip: IP address of the Motive PC (command channel + unicast source). + data_port: UDP port on **this machine** to bind for the NatNet **data** stream + (default 1511). Not the Motive hostname; use ``server_ip`` for that. + command_port: UDP port for NatNet commands on Motive (default 1510) + local_ip: Address of the local NIC to bind for command + data sockets. + Use ``0.0.0.0`` to let the OS choose (fine with a single interface). + Set to your LAN IP (e.g. Jetson on OptiTrack subnet) when multiple + interfaces exist so outbound packets to Motive use the correct + source address for unicast streaming (NatNet SDK ``localAddress``). + negotiation_enabled: If False, skip UDP command-channel handshake + (``NAT_CONNECT`` / ``SetNatNetVersion``). Use when Motive never replies + on ``command_port`` (firewall, Docker bridge, etc.) but mocap data still + arrives on ``data_port``. """ self.server_ip = server_ip - self.server_port = server_port + self.data_port = data_port + self.command_port = command_port + self.local_ip = local_ip + self.negotiation_enabled = negotiation_enabled self.sock = None self.running = False self.receive_thread = None @@ -106,84 +130,302 @@ def set_frame_callback(self, callback: Callable[[FrameData], None]): """ self._user_callback = callback + @staticmethod + def _pack_command_packet(message_id: int, payload: bytes = b'') -> bytes: + """NatNet sPacket: uint16 iMessage, uint16 nDataBytes, payload.""" + return struct.pack(' None: + """Decode and print NatNet command packet layout before send (debug).""" + if len(packet) < 4: + print( + f"[NatNetClient] negotiate TX {label}: INVALID len={len(packet)} " + f"hex={packet.hex()}" + ) + return + msg_id, n_data = struct.unpack(' str: + try: + return MessageType(msg_id).name + except ValueError: + return f'UNKNOWN({msg_id})' + + @classmethod + def _log_incoming_command_packet(cls, data: bytes, addr: Tuple[str, int]) -> None: + """Log one datagram received on the NatNet command socket during negotiation.""" + if len(data) < 4: + print( + f"[NatNetClient] negotiate RX from {addr[0]}:{addr[1]}: " + f'too_short len={len(data)} hex={data.hex()}' + ) + return + msg_id, n_payload = struct.unpack('= 4: + rc = struct.unpack(' Optional[Tuple[int, int, int, int]]: + """ + Parse NatNetVersion[4] from NAT_SERVERINFO (message id 1). + + Motive may send either: + + - **Fixed C struct** (common): ``szName[256]``, then App ``Version[4]``, then + ``NatNetVersion[4]`` at offset **260** from the start of the payload — requires + ``len(payload) >= 264`` (NatNetTypes.h ``#pragma pack(1)``). + - **Tight packing**: null-terminated ``szName``, then versions — typical when the + packet is **smaller** than the full padded struct. + """ + payload = packet[4:] + if len(payload) < 9: + return None + + if len(payload) >= 264: + nv = payload[260:264] + return (int(nv[0]), int(nv[1]), int(nv[2]), int(nv[3])) + + z = payload.find(b'\x00') + if z >= 0: + o = z + 1 + if o + 8 <= len(payload): + nv = payload[o + 4 : o + 8] + return (int(nv[0]), int(nv[1]), int(nv[2]), int(nv[3])) + + return None + + def _recv_command_until( + self, + sock: socket.socket, + *, + end_mono: float, + accept_message_ids: Tuple[int, ...], + log_all_rx: bool = False, + ) -> Optional[bytes]: + """Drain UDP replies until one matches ``accept_message_ids`` or timeout.""" + while time.monotonic() < end_mono: + sock.settimeout(max(0.001, end_mono - time.monotonic())) + try: + data, addr = sock.recvfrom(65535) + except socket.timeout: + continue + if hasattr(self, '_negotiation_recv_count'): + self._negotiation_recv_count += 1 + if log_all_rx: + try: + self._log_incoming_command_packet(data, addr) + except Exception: + pass + if len(data) < 4: + continue + msg_id = struct.unpack(' bool: """ - Negotiate NatNet version with Motive PC via command port (1510). - - Sends SetNatNetVersion request for each supported version in order. - Stores the negotiated version in self.negotiated_version. - - Returns: - True if version successfully negotiated, False if all attempts failed + Align with Motive's NatNet command protocol (UDP command port, default 1510): + + 1. Send NAT_CONNECT; read NAT_SERVERINFO to learn the server's NatNet version. + 2. For **unicast** bitstream selection (Motive 3.0+), send NAT_REQUEST with the + remote command ``SetNatNetVersion,,,,`` until + NAT_RESPONSE indicates success (SDK / PacketClient pattern). + + Reference: OptiTrack NatNetTypes.h (message IDs), PacketClient.cpp, NatNet 4.x docs. """ - command_port = 1510 command_sock = None - + try: - # Create UDP socket for command communication command_sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) - command_sock.settimeout(2.0) # 2 second timeout per attempt - - # Try each supported version in order (4.4 → 4.3 → ... → 4.0) + command_sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) + per_phase_timeout = 3.0 + self._negotiation_recv_count = 0 + if self.local_ip and self.local_ip != '0.0.0.0': + command_sock.bind((self.local_ip, 0)) + + server_from_info: Optional[Tuple[int, int, int, int]] = None + + # --- Phase 1: NAT_CONNECT → NAT_SERVERINFO --- + connect_pkt = self._pack_command_packet(MessageType.NAT_CONNECT, b'') + self._print_outgoing_command_packet( + connect_pkt, + f'NAT_CONNECT → {self.server_ip}:{self.command_port}', + ) + try: + for attempt in range(3): + command_sock.sendto( + connect_pkt, + (self.server_ip, self.command_port), + ) + if attempt < 2: + time.sleep(0.05) + t_end = time.monotonic() + per_phase_timeout + info = self._recv_command_until( + command_sock, + end_mono=t_end, + accept_message_ids=(MessageType.NAT_SERVERINFO,), + log_all_rx=True, + ) + if info: + server_from_info = self._parse_natnet_version_from_server_info( + info) + if server_from_info: + print( + "[NatNetClient] Motive NatNet version (from SERVERINFO): " + f"{server_from_info[0]}.{server_from_info[1]}." + f"{server_from_info[2]}.{server_from_info[3]}" + ) + else: + pay_len = struct.unpack('= 8: - resp_type, resp_size = struct.unpack('= 4: + rc = struct.unpack(' bool: """ @@ -194,29 +436,39 @@ def start(self) -> bool: Returns: True if socket bound successfully, False otherwise """ - try: - # Attempt to negotiate version with Motive before starting data listener + # Negotiate NatNet version with Motive before starting data listener (optional). + if self.negotiation_enabled: print("[NatNetClient] Attempting version negotiation with Motive PC...") self._negotiate_version() - - # Bind to data port for frame reception - self.sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) - self.sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) - self.sock.bind(('0.0.0.0', self.server_port)) - self.sock.setblocking(True) - - self.running = True - self.receive_thread = threading.Thread(target=self._receive_loop, daemon=True) - self.receive_thread.start() - - version_str = f"{self.negotiated_version[0]}.{self.negotiated_version[1]}" - print(f"[NatNetClient] Listening on UDP port {self.server_port} " - f"(NatNet {version_str})") - return True + else: + print( + "[NatNetClient] Skipping command-channel negotiation " + "(negotiation_enabled=false)." + ) + self.negotiated_version = self.supported_versions[0] - except Exception as e: - print(f"[NatNetClient] Failed to start: {e}") - return False + bind_ip = ( + self.local_ip + if self.local_ip and self.local_ip != '0.0.0.0' + else '0.0.0.0' + ) + self.sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + self.sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) + self.sock.bind((bind_ip, self.data_port)) + + self.sock.setblocking(True) + + self.running = True + self.receive_thread = threading.Thread(target=self._receive_loop, daemon=True) + self.receive_thread.start() + + print( + f"[NatNetClient] Listening for unicast mocap on {bind_ip}:{self.data_port} " + f"(Motive {self.server_ip}; command UDP {self.command_port}; " + f"NatNet parse {self.negotiated_version[0]}.{self.negotiated_version[1]})" + ) + return True + def stop(self): """Stop listening and clean up resources.""" @@ -225,6 +477,7 @@ def stop(self): self.receive_thread.join(timeout=1.0) if self.sock: self.sock.close() + self.sock = None print("[NatNetClient] Stopped") def _receive_loop(self): @@ -245,14 +498,13 @@ def parse_frame(self, data: bytes) -> Optional[FrameData]: """ Parse a NatNet frame from UDP packet data. - NatNet 4.4 frame structure: - - Packet ID (2 bytes): Message type - - Packet size (2 bytes): Total packet size - - Frame number (4 bytes) - - Marker set count (4 bytes) + [marker set data] - - Rigid body count (4 bytes) + [rigid body data] - - Latency (4 bytes) - - Timestamp (8 bytes, optional) + NatNet FrameOfData (message id 7) core layout used here: + + - Header (4), frame number (4), marker sets, **unlabeled/other markers** (count + 12×count), + rigid body count, rigid bodies (NatNet 3+ layout: no per-body marker blobs). + + Skeletons, labeled markers, timing, etc. follow rigid bodies and are ignored so long as + rigid-body parsing stays aligned with ``negotiated_version`` (major/minor). Args: data: Raw UDP packet bytes from Motive @@ -270,8 +522,11 @@ def parse_frame(self, data: bytes) -> Optional[FrameData]: message_type, packet_size = struct.unpack(' Optional[FrameData]: offset += 4 for _ in range(num_marker_sets): offset = self._skip_marker_set(data, offset) - + + # Unlabeled / "other" markers (vector3 each) — required skip before rigid bodies + # (matches PacketClient + python_natnet MocapFrameMessage.deserialize). + if offset + 4 > len(data): + return None + num_other_markers = struct.unpack(' len(data): + return None + offset += skip_other + + major = self.negotiated_version[0] if self.negotiated_version else 4 + minor = self.negotiated_version[1] if self.negotiated_version else 4 + # Parse rigid bodies + if offset + 4 > len(data): + return None num_rigid_bodies = struct.unpack(' 0 else latency, - rigid_bodies=rigid_bodies + timestamp=float(frame_number), + rigid_bodies=rigid_bodies, ) except Exception as e: @@ -339,63 +599,75 @@ def _skip_marker_set(self, data: bytes, offset: int) -> int: except: return offset - def _parse_rigid_body(self, data: bytes, offset: int) -> Tuple[Optional[RigidBodyData], int]: + def _parse_rigid_body( + self, + data: bytes, + offset: int, + *, + natnet_major: int, + natnet_minor: int, + ) -> Tuple[Optional[RigidBodyData], int]: """ - Parse a single rigid body from frame data. - + Parse one rigid body from a FrameOfData payload. + + NatNet 3.0+ omits per-body marker positions; use labeled markers if needed. + + NatNet 2.x (major < 3) embeds marker positions/IDs/sizes before mean error. + Tracking validity uses ``params & 0x01`` when NatNet >= 2.6 or Motive 3+. + Returns: - (RigidBodyData, new_offset) or (None, offset) if parsing fails + (RigidBodyData, new_offset) or (None, original_offset) if parsing fails """ try: start_offset = offset - - # Rigid body ID + if offset + 4 > len(data): return None, offset rb_id = struct.unpack(' len(data): + + if offset + 28 > len(data): return None, start_offset pos_x, pos_y, pos_z = struct.unpack(' len(data): - return None, start_offset qx, qy, qz, qw = struct.unpack(' len(data): - return None, start_offset - num_markers = struct.unpack(' len(data): - # Not enough data for markers - num_markers = 0 - else: + + markers: List[Tuple[float, float, float]] = [] + + if natnet_major < 3: + if offset + 4 > len(data): + return None, start_offset + num_markers = struct.unpack(' len(data): + return None, start_offset for _ in range(num_markers): mx, my, mz = struct.unpack(' len(data): - return None, start_offset - mean_error = struct.unpack(' len(data): - return None, start_offset - tracking_valid = bool(struct.unpack('= 2: + id_bytes = num_markers * 4 + sz_bytes = num_markers * 4 + if offset + id_bytes + sz_bytes > len(data): + return None, start_offset + offset += id_bytes + sz_bytes + + mean_error = 0.0 + if natnet_major >= 2: + if offset + 4 > len(data): + return None, start_offset + mean_error = struct.unpack('= 6) or natnet_major > 2: + if offset + 2 > len(data): + return None, start_offset + params = struct.unpack(' Tuple[Optional[RigidBod markers=markers, mean_marker_error=mean_error, tracking_valid=tracking_valid, - timestamp=0.0 # Will be set from frame timestamp + timestamp=0.0, ) - + return rb_data, offset - + except Exception as e: print(f"[NatNetClient] Error parsing rigid body: {e}") - return None, offset + return None, start_offset if __name__ == "__main__": diff --git a/robot/ros_ws/src/perception/natnet_ros2/src/natnet_ros2_node.py b/robot/ros_ws/src/perception/natnet_ros2/src/natnet_ros2_node.py old mode 100644 new mode 100755 index 26e3673bc..a3a2dbe16 --- a/robot/ros_ws/src/perception/natnet_ros2/src/natnet_ros2_node.py +++ b/robot/ros_ws/src/perception/natnet_ros2/src/natnet_ros2_node.py @@ -34,10 +34,16 @@ class NatNetROS2Node(Node): def __init__(self): super().__init__('natnet_ros2_node') + self.get_logger().info("########################################################") + self.get_logger().info("Began NatNet ROS 2 Node") + self.get_logger().info("########################################################") + # Declare parameters self.declare_parameter('server_ip', '192.168.1.1') self.declare_parameter('command_port', 1510) self.declare_parameter('data_port', 1511) + # Local NIC for UDP bind (NatNet SDK localAddress). 0.0.0.0 = OS chooses. + self.declare_parameter('client_ip', '0.0.0.0') self.declare_parameter('body_name', 'robot_1') # body_id: rigid body ID from Motive to track; -1 = track all bodies self.declare_parameter('body_id', -1) @@ -45,6 +51,7 @@ def __init__(self): self.declare_parameter('publish_to_mavros', False) self.declare_parameter('frame_id', 'world') self.declare_parameter('debug', False) + self.declare_parameter('negotiation_enabled', True) # Covariance: 3x3 flattened (9 values each) self.declare_parameter( 'position_covariance', @@ -61,13 +68,16 @@ def __init__(self): # Get parameters server_ip = self.get_parameter('server_ip').value + command_port = self.get_parameter('command_port').value data_port = self.get_parameter('data_port').value + client_ip = self.get_parameter('client_ip').value self.body_name = self.get_parameter('body_name').value self.body_id = self.get_parameter('body_id').value self.publish_direct = self.get_parameter('publish_direct_optitrack').value self.publish_mavros = self.get_parameter('publish_to_mavros').value self.frame_id = self.get_parameter('frame_id').value self.debug = self.get_parameter('debug').value + negotiation_enabled = self.get_parameter('negotiation_enabled').value # Build 6x6 covariance matrix from 3x3 position and orientation blocks pos_cov = self.get_parameter('position_covariance').value @@ -81,8 +91,29 @@ def __init__(self): self.pose_publishers: dict = {} self.pose_cov_publishers: dict = {} + self.get_logger().info("########################################################") + self.get_logger().info("NatNet ROS 2 Node Parameters:") + self.get_logger().info("########################################################") + self.get_logger().info(f'Server IP: {server_ip}') + self.get_logger().info(f'Client IP: {client_ip}') + self.get_logger().info(f'Command port: {command_port}') + self.get_logger().info(f'Data port: {data_port}') + self.get_logger().info(f'Body name: {self.body_name}') + self.get_logger().info(f'Body id: {self.body_id}') + self.get_logger().info(f'Publish direct: {self.publish_direct}') + self.get_logger().info(f'Publish to mavros: {self.publish_mavros}') + self.get_logger().info(f'Frame id: {self.frame_id}') + self.get_logger().info(f'Debug: {self.debug}') + self.get_logger().info(f'Negotiation enabled: {negotiation_enabled}') + # Initialize NatNet client - self.natnet_client = NatNetClient(server_ip, data_port) + self.natnet_client = NatNetClient( + server_ip, + data_port, + command_port=command_port, + local_ip=client_ip, + negotiation_enabled=negotiation_enabled, + ) self.natnet_client.set_frame_callback(self._on_frame_received) if not self.natnet_client.start(): @@ -101,6 +132,7 @@ def __init__(self): ) else: self.get_logger().info('Tracking all bodies (body_id = -1)') + self.get_logger().info("########################################################") def _build_6x6_covariance( self, pos_cov_3x3: list, ori_cov_3x3: list @@ -127,7 +159,7 @@ def _on_frame_received(self, frame: FrameData): """Callback invoked when a complete NatNet frame is received.""" try: if self.debug: - self.get_logger().debug( + self.get_logger().info( f'Frame {frame.frame_number}: ' f'{len(frame.rigid_bodies)} rigid bodies' ) @@ -208,13 +240,20 @@ def _get_body_name(self, body_id: int) -> str: def main(args=None): rclpy.init(args=args) + node = None try: node = NatNetROS2Node() rclpy.spin(node) except KeyboardInterrupt: pass finally: - rclpy.shutdown() + if node is not None: + node.destroy_node() + if rclpy.ok(): + try: + rclpy.shutdown() + except Exception: + pass if __name__ == '__main__': 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 old mode 100644 new mode 100755 index 521b52577..b9b265f38 --- 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 @@ -25,11 +25,10 @@ class VisionPoseConverterNode(Node): def __init__(self): super().__init__('vision_pose_converter') - # Declare parameters self.declare_parameter('frame_id', 'world') self.declare_parameter('child_frame_id', 'base_link') + self.declare_parameter('use_sim_time', False) - # Get parameters self.frame_id = self.get_parameter('frame_id').value self.child_frame_id = self.get_parameter('child_frame_id').value @@ -53,7 +52,10 @@ def __init__(self): 10 ) - self.get_logger().info("Vision pose converter node started") + self.get_logger().info( + f'Vision pose converter started ' + f'(frame_id={self.frame_id!r}, child_frame_id={self.child_frame_id!r})' + ) def _on_pose(self, msg: PoseWithCovarianceStamped): """ 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 ea2161ae5..c187c8bf0 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,7 +2,8 @@ - + + @@ -72,12 +73,8 @@ - - - - + + From 95287d8585dc839535ee088c54c15610cf319891 Mon Sep 17 00:00:00 2001 From: John Liu Date: Fri, 15 May 2026 19:28:47 -0400 Subject: [PATCH 11/21] fully implemented NatNetSDK natnet ros2 wrapper natively in AirStack. Hand test in mocap room successful --- airstack.sh | 25 + .../src/perception/natnet_ros2/CMakeLists.txt | 62 +- .../natnet_ros2/config/natnet_config.yaml | 17 +- .../config/vision_pose_converter.yaml | 5 +- .../natnet_ros2/launch/natnet_ros2.launch.py | 2 +- .../src/perception/natnet_ros2/package.xml | 12 +- .../scripts/download-natnet-sdk.sh | 155 ++++ .../natnet_ros2/src/natnet_client.py | 693 ------------------ .../natnet_ros2/src/natnet_ros2_node.cpp | 492 +++++++++++++ .../natnet_ros2/src/natnet_ros2_node.py | 260 ------- .../src/vision_pose_converter_node.py | 32 +- 11 files changed, 771 insertions(+), 984 deletions(-) create mode 100644 robot/ros_ws/src/perception/natnet_ros2/scripts/download-natnet-sdk.sh delete mode 100755 robot/ros_ws/src/perception/natnet_ros2/src/natnet_client.py create mode 100644 robot/ros_ws/src/perception/natnet_ros2/src/natnet_ros2_node.cpp delete mode 100755 robot/ros_ws/src/perception/natnet_ros2/src/natnet_ros2_node.py diff --git a/airstack.sh b/airstack.sh index f82224aaf..052f844f7 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" @@ -577,18 +578,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 +669,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 diff --git a/robot/ros_ws/src/perception/natnet_ros2/CMakeLists.txt b/robot/ros_ws/src/perception/natnet_ros2/CMakeLists.txt index 6ec87bcde..822e27add 100644 --- a/robot/ros_ws/src/perception/natnet_ros2/CMakeLists.txt +++ b/robot/ros_ws/src/perception/natnet_ros2/CMakeLists.txt @@ -1,34 +1,64 @@ cmake_minimum_required(VERSION 3.8) project(natnet_ros2) -# Compiler settings if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() -# Pure Python package - no SDK required -# Uses pure Python parsing of NatNet protocol (no C++ dependencies) - -# Find dependencies +# ament / ROS 2 dependencies find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(nav_msgs REQUIRED) -# Install launch files -install(DIRECTORY launch/ - DESTINATION share/${PROJECT_NAME}/launch) +# --------------------------------------------------------------------------- +# NatNet SDK — pre-built shared library bundled in this package. +# Headers: include/natnet/ Library: lib/libNatNet.so +# --------------------------------------------------------------------------- +add_library(NatNet SHARED IMPORTED) +set_target_properties(NatNet PROPERTIES + IMPORTED_LOCATION "${CMAKE_CURRENT_SOURCE_DIR}/lib/libNatNet.so" + INTERFACE_INCLUDE_DIRECTORIES "${CMAKE_CURRENT_SOURCE_DIR}/include/natnet" +) -# Install config files -install(DIRECTORY config/ - DESTINATION share/${PROJECT_NAME}/config) +# --------------------------------------------------------------------------- +# C++ NatNet ROS 2 node +# --------------------------------------------------------------------------- +add_executable(natnet_ros2_node src/natnet_ros2_node.cpp) +ament_target_dependencies(natnet_ros2_node rclcpp geometry_msgs nav_msgs) +target_link_libraries(natnet_ros2_node NatNet) + +# $ORIGIN RPATH so the node finds libNatNet.so in the same install directory +# without needing LD_LIBRARY_PATH at runtime. +set_target_properties(natnet_ros2_node PROPERTIES + INSTALL_RPATH "$ORIGIN" + BUILD_WITH_INSTALL_RPATH TRUE +) + +install(TARGETS natnet_ros2_node + DESTINATION lib/${PROJECT_NAME}) -# Install executable scripts and shared modules into the same directory -# so natnet_ros2_node.py can "from natnet_client import ..." at runtime +# Install the bundled NatNet .so alongside the executable +install(FILES lib/libNatNet.so + DESTINATION lib/${PROJECT_NAME}) + +# --------------------------------------------------------------------------- +# Python nodes (vision_pose_converter remains Python) +# --------------------------------------------------------------------------- install(PROGRAMS - src/natnet_client.py - src/natnet_ros2_node.py src/vision_pose_converter_node.py DESTINATION lib/${PROJECT_NAME}) -# Testing +# --------------------------------------------------------------------------- +# Launch and config files +# --------------------------------------------------------------------------- +install(DIRECTORY launch/ + DESTINATION share/${PROJECT_NAME}/launch) + +install(DIRECTORY config/ + DESTINATION share/${PROJECT_NAME}/config) + +# --------------------------------------------------------------------------- if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() 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 index 29fc7a2f0..250a81a62 100644 --- a/robot/ros_ws/src/perception/natnet_ros2/config/natnet_config.yaml +++ b/robot/ros_ws/src/perception/natnet_ros2/config/natnet_config.yaml @@ -13,13 +13,26 @@ command_port: 1510 data_port: 1511 - negotiation_enabled: true + + # "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" body_name: "HummingbirdDrone" body_id: 3 publish_direct_optitrack: true - publish_to_mavros: false + publish_to_mavros: true frame_id: "world" debug: false 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 index 95e60566d..a52181786 100644 --- 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 @@ -6,4 +6,7 @@ ros__parameters: frame_id: "world" child_frame_id: "$(env ROBOT_NAME robot_1)/base_link" - use_sim_time: false + # 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/launch/natnet_ros2.launch.py b/robot/ros_ws/src/perception/natnet_ros2/launch/natnet_ros2.launch.py index 76f61e165..b968e1ec7 100644 --- 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 @@ -54,7 +54,7 @@ def launch_setup(context, *_args, **_kwargs): actions = [ Node( package='natnet_ros2', - executable='natnet_ros2_node.py', + executable='natnet_ros2_node', name='natnet_ros2_node', output='screen', parameters=[ParameterFile(config_file, allow_substs=True)], diff --git a/robot/ros_ws/src/perception/natnet_ros2/package.xml b/robot/ros_ws/src/perception/natnet_ros2/package.xml index ad315e611..436d11b99 100644 --- a/robot/ros_ws/src/perception/natnet_ros2/package.xml +++ b/robot/ros_ws/src/perception/natnet_ros2/package.xml @@ -5,20 +5,22 @@ 0.1.0 NatNet ROS 2 wrapper for OptiTrack Motive motion capture integration. - Receives NatNet UDP packets from external Motive PC and publishes - pose data into the AirStack perception layer. + Receives NatNet data from external Motive PC via the official NatNet SDK + and publishes pose data into the AirStack perception layer. AirLab CMU Apache-2.0 - ament_cmake - - rclpy + + rclcpp geometry_msgs nav_msgs + + + rclpy tf_transformations airstack_msgs 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..e90c18db2 --- /dev/null +++ b/robot/ros_ws/src/perception/natnet_ros2/scripts/download-natnet-sdk.sh @@ -0,0 +1,155 @@ +#!/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/legal/software-license-agreement + +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() { + 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 + + 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 + + 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.py b/robot/ros_ws/src/perception/natnet_ros2/src/natnet_client.py deleted file mode 100755 index 44c10e328..000000000 --- a/robot/ros_ws/src/perception/natnet_ros2/src/natnet_client.py +++ /dev/null @@ -1,693 +0,0 @@ -#!/usr/bin/env python3 - -""" -NatNet Protocol Client (Pure Python) - -Decodes NatNet 4.4 protocol UDP packets from OptiTrack Motive software. -Supports rigid body tracking via pure Python parsing (no SDK required). - -Reference: OptiTrack NatNet 4.4 Documentation -https://v20.wiki.optitrack.com/index.php?title=NatNet -""" - -import struct -import socket -import threading -import time -from typing import Dict, List, Tuple, Optional, Callable -from dataclasses import dataclass -from enum import IntEnum - - -class MessageType(IntEnum): - """NatNet command-channel / packet IDs (NatNetTypes.h).""" - - NAT_CONNECT = 0 - NAT_SERVERINFO = 1 - NAT_REQUEST = 2 - NAT_RESPONSE = 3 - NAT_REQUEST_MODELDEF = 4 - NAT_MODELDEF = 5 - NAT_REQUEST_FRAMEOFDATA = 6 - FRAME_OF_DATA = 7 # NAT_FRAMEOFDATA (mocap data stream) - NAT_MESSAGESTRING = 8 - NAT_UNRECOGNIZED_REQUEST = 100 - - -@dataclass -class RigidBodyData: - """Rigid body pose and kinematic data""" - id: int - position: Tuple[float, float, float] # (x, y, z) - rotation: Tuple[float, float, float, float] # (qx, qy, qz, qw) - markers: List[Tuple[float, float, float]] # List of marker positions - mean_marker_error: float - tracking_valid: bool - timestamp: float - - -@dataclass -class FrameData: - """Complete frame of motion capture data""" - frame_number: int - timestamp: float - rigid_bodies: Dict[int, RigidBodyData] - - -class NatNetClient: - """ - Pure Python NatNet protocol client for OptiTrack Motive. - - Receives NatNet UDP packets and decodes motion capture frames. - Supports rigid body tracking (basic motion capture data). - - Features: - - Version negotiation: Requests NatNet version from Motive - - Fallback support: Tries NatNet 4.4 → 4.3 → 4.2 → 4.1 → 4.0 - - Robust parsing: Handles optional fields and frame variations - - Usage: - client = NatNetClient("192.168.1.1", 1511) # Motive IP, local data UDP port - client.set_frame_callback(my_callback) - client.start() - # ... receives frames in background thread - client.stop() - """ - - def __init__( - self, - server_ip: str, - data_port: int, - *, - command_port: int = 1510, - local_ip: str = '0.0.0.0', - negotiation_enabled: bool = True, - ): - """ - Initialize NatNet client. - - Args: - server_ip: IP address of the Motive PC (command channel + unicast source). - data_port: UDP port on **this machine** to bind for the NatNet **data** stream - (default 1511). Not the Motive hostname; use ``server_ip`` for that. - command_port: UDP port for NatNet commands on Motive (default 1510) - local_ip: Address of the local NIC to bind for command + data sockets. - Use ``0.0.0.0`` to let the OS choose (fine with a single interface). - Set to your LAN IP (e.g. Jetson on OptiTrack subnet) when multiple - interfaces exist so outbound packets to Motive use the correct - source address for unicast streaming (NatNet SDK ``localAddress``). - negotiation_enabled: If False, skip UDP command-channel handshake - (``NAT_CONNECT`` / ``SetNatNetVersion``). Use when Motive never replies - on ``command_port`` (firewall, Docker bridge, etc.) but mocap data still - arrives on ``data_port``. - """ - self.server_ip = server_ip - self.data_port = data_port - self.command_port = command_port - self.local_ip = local_ip - self.negotiation_enabled = negotiation_enabled - self.sock = None - self.running = False - self.receive_thread = None - self._user_callback: Optional[Callable[[FrameData], None]] = None - self.negotiated_version = None - - # Versions to try in order (4.4 → 4.3 → ... → 4.0) - self.supported_versions = [ - (4, 4, 0, 0), - (4, 3, 0, 0), - (4, 2, 0, 0), - (4, 1, 0, 0), - (4, 0, 0, 0), - ] - - def set_frame_callback(self, callback: Callable[[FrameData], None]): - """ - Register callback to receive decoded frames. - - Args: - callback: Function(FrameData) called when frame is received - """ - self._user_callback = callback - - @staticmethod - def _pack_command_packet(message_id: int, payload: bytes = b'') -> bytes: - """NatNet sPacket: uint16 iMessage, uint16 nDataBytes, payload.""" - return struct.pack(' None: - """Decode and print NatNet command packet layout before send (debug).""" - if len(packet) < 4: - print( - f"[NatNetClient] negotiate TX {label}: INVALID len={len(packet)} " - f"hex={packet.hex()}" - ) - return - msg_id, n_data = struct.unpack(' str: - try: - return MessageType(msg_id).name - except ValueError: - return f'UNKNOWN({msg_id})' - - @classmethod - def _log_incoming_command_packet(cls, data: bytes, addr: Tuple[str, int]) -> None: - """Log one datagram received on the NatNet command socket during negotiation.""" - if len(data) < 4: - print( - f"[NatNetClient] negotiate RX from {addr[0]}:{addr[1]}: " - f'too_short len={len(data)} hex={data.hex()}' - ) - return - msg_id, n_payload = struct.unpack('= 4: - rc = struct.unpack(' Optional[Tuple[int, int, int, int]]: - """ - Parse NatNetVersion[4] from NAT_SERVERINFO (message id 1). - - Motive may send either: - - - **Fixed C struct** (common): ``szName[256]``, then App ``Version[4]``, then - ``NatNetVersion[4]`` at offset **260** from the start of the payload — requires - ``len(payload) >= 264`` (NatNetTypes.h ``#pragma pack(1)``). - - **Tight packing**: null-terminated ``szName``, then versions — typical when the - packet is **smaller** than the full padded struct. - """ - payload = packet[4:] - if len(payload) < 9: - return None - - if len(payload) >= 264: - nv = payload[260:264] - return (int(nv[0]), int(nv[1]), int(nv[2]), int(nv[3])) - - z = payload.find(b'\x00') - if z >= 0: - o = z + 1 - if o + 8 <= len(payload): - nv = payload[o + 4 : o + 8] - return (int(nv[0]), int(nv[1]), int(nv[2]), int(nv[3])) - - return None - - def _recv_command_until( - self, - sock: socket.socket, - *, - end_mono: float, - accept_message_ids: Tuple[int, ...], - log_all_rx: bool = False, - ) -> Optional[bytes]: - """Drain UDP replies until one matches ``accept_message_ids`` or timeout.""" - while time.monotonic() < end_mono: - sock.settimeout(max(0.001, end_mono - time.monotonic())) - try: - data, addr = sock.recvfrom(65535) - except socket.timeout: - continue - if hasattr(self, '_negotiation_recv_count'): - self._negotiation_recv_count += 1 - if log_all_rx: - try: - self._log_incoming_command_packet(data, addr) - except Exception: - pass - if len(data) < 4: - continue - msg_id = struct.unpack(' bool: - """ - Align with Motive's NatNet command protocol (UDP command port, default 1510): - - 1. Send NAT_CONNECT; read NAT_SERVERINFO to learn the server's NatNet version. - 2. For **unicast** bitstream selection (Motive 3.0+), send NAT_REQUEST with the - remote command ``SetNatNetVersion,,,,`` until - NAT_RESPONSE indicates success (SDK / PacketClient pattern). - - Reference: OptiTrack NatNetTypes.h (message IDs), PacketClient.cpp, NatNet 4.x docs. - """ - command_sock = None - - try: - command_sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) - command_sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) - per_phase_timeout = 3.0 - self._negotiation_recv_count = 0 - if self.local_ip and self.local_ip != '0.0.0.0': - command_sock.bind((self.local_ip, 0)) - - server_from_info: Optional[Tuple[int, int, int, int]] = None - - # --- Phase 1: NAT_CONNECT → NAT_SERVERINFO --- - connect_pkt = self._pack_command_packet(MessageType.NAT_CONNECT, b'') - self._print_outgoing_command_packet( - connect_pkt, - f'NAT_CONNECT → {self.server_ip}:{self.command_port}', - ) - try: - for attempt in range(3): - command_sock.sendto( - connect_pkt, - (self.server_ip, self.command_port), - ) - if attempt < 2: - time.sleep(0.05) - t_end = time.monotonic() + per_phase_timeout - info = self._recv_command_until( - command_sock, - end_mono=t_end, - accept_message_ids=(MessageType.NAT_SERVERINFO,), - log_all_rx=True, - ) - if info: - server_from_info = self._parse_natnet_version_from_server_info( - info) - if server_from_info: - print( - "[NatNetClient] Motive NatNet version (from SERVERINFO): " - f"{server_from_info[0]}.{server_from_info[1]}." - f"{server_from_info[2]}.{server_from_info[3]}" - ) - else: - pay_len = struct.unpack('= 4: - rc = struct.unpack(' bool: - """ - Start listening for NatNet UDP packets. - - First negotiates NatNet version with Motive PC, then binds to data port. - - Returns: - True if socket bound successfully, False otherwise - """ - # Negotiate NatNet version with Motive before starting data listener (optional). - if self.negotiation_enabled: - print("[NatNetClient] Attempting version negotiation with Motive PC...") - self._negotiate_version() - else: - print( - "[NatNetClient] Skipping command-channel negotiation " - "(negotiation_enabled=false)." - ) - self.negotiated_version = self.supported_versions[0] - - bind_ip = ( - self.local_ip - if self.local_ip and self.local_ip != '0.0.0.0' - else '0.0.0.0' - ) - self.sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) - self.sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) - self.sock.bind((bind_ip, self.data_port)) - - self.sock.setblocking(True) - - self.running = True - self.receive_thread = threading.Thread(target=self._receive_loop, daemon=True) - self.receive_thread.start() - - print( - f"[NatNetClient] Listening for unicast mocap on {bind_ip}:{self.data_port} " - f"(Motive {self.server_ip}; command UDP {self.command_port}; " - f"NatNet parse {self.negotiated_version[0]}.{self.negotiated_version[1]})" - ) - return True - - - def stop(self): - """Stop listening and clean up resources.""" - self.running = False - if self.receive_thread: - self.receive_thread.join(timeout=1.0) - if self.sock: - self.sock.close() - self.sock = None - print("[NatNetClient] Stopped") - - def _receive_loop(self): - """Background thread that receives and processes UDP packets.""" - while self.running: - try: - data, addr = self.sock.recvfrom(65535) - frame = self.parse_frame(data) - if frame and self._user_callback: - self._user_callback(frame) - except socket.timeout: - continue - except Exception as e: - if self.running: - print(f"[NatNetClient] Error receiving packet: {e}") - - def parse_frame(self, data: bytes) -> Optional[FrameData]: - """ - Parse a NatNet frame from UDP packet data. - - NatNet FrameOfData (message id 7) core layout used here: - - - Header (4), frame number (4), marker sets, **unlabeled/other markers** (count + 12×count), - rigid body count, rigid bodies (NatNet 3+ layout: no per-body marker blobs). - - Skeletons, labeled markers, timing, etc. follow rigid bodies and are ignored so long as - rigid-body parsing stays aligned with ``negotiated_version`` (major/minor). - - Args: - data: Raw UDP packet bytes from Motive - - Returns: - FrameData if successful, None if parsing fails - """ - try: - if len(data) < 4: - return None - - offset = 0 - - # Parse packet header - message_type, packet_size = struct.unpack(' len(data): - return None - num_other_markers = struct.unpack(' len(data): - return None - offset += skip_other - - major = self.negotiated_version[0] if self.negotiated_version else 4 - minor = self.negotiated_version[1] if self.negotiated_version else 4 - - # Parse rigid bodies - if offset + 4 > len(data): - return None - num_rigid_bodies = struct.unpack(' int: - """Skip over marker set data in frame (we don't need it for rigid body tracking).""" - try: - # Marker set name (null-terminated string) - while offset < len(data) and data[offset] != 0: - offset += 1 - offset += 1 # Skip null terminator - - # Number of markers in this set - if offset + 4 > len(data): - return offset - num_markers = struct.unpack(' Tuple[Optional[RigidBodyData], int]: - """ - Parse one rigid body from a FrameOfData payload. - - NatNet 3.0+ omits per-body marker positions; use labeled markers if needed. - - NatNet 2.x (major < 3) embeds marker positions/IDs/sizes before mean error. - Tracking validity uses ``params & 0x01`` when NatNet >= 2.6 or Motive 3+. - - Returns: - (RigidBodyData, new_offset) or (None, original_offset) if parsing fails - """ - try: - start_offset = offset - - if offset + 4 > len(data): - return None, offset - rb_id = struct.unpack(' len(data): - return None, start_offset - pos_x, pos_y, pos_z = struct.unpack(' len(data): - return None, start_offset - num_markers = struct.unpack(' len(data): - return None, start_offset - for _ in range(num_markers): - mx, my, mz = struct.unpack('= 2: - id_bytes = num_markers * 4 - sz_bytes = num_markers * 4 - if offset + id_bytes + sz_bytes > len(data): - return None, start_offset - offset += id_bytes + sz_bytes - - mean_error = 0.0 - if natnet_major >= 2: - if offset + 4 > len(data): - return None, start_offset - mean_error = struct.unpack('= 6) or natnet_major > 2: - if offset + 2 > len(data): - return None, start_offset - params = struct.unpack(' +#include +#include +#include +#include +#include +#include +#include + +// NatNet SDK (bundled in include/natnet/, lib/libNatNet.so) +#include "NatNetClient.h" +#include "NatNetCAPI.h" +#include "NatNetTypes.h" + +// ROS 2 +#include "rclcpp/rclcpp.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp" + + +// Forward declaration +class NatNetROS2Node; + +// --------------------------------------------------------------------------- +// SDK log callback — registered globally, routes to ROS logger when available. +// --------------------------------------------------------------------------- +static NatNetROS2Node* g_node_ptr = nullptr; + +void NATNET_CALLCONV natnet_log_callback(Verbosity level, const char* message); +void NATNET_CALLCONV frame_callback(sFrameOfMocapData* data, void* pUserContext); + + +// --------------------------------------------------------------------------- +// 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); + // "unicast" or "multicast" + this->declare_parameter("connection_type", std::string("unicast")); + // Multicast group address used when connection_type = "multicast". + // Must match Motive's "Data Streaming" multicast address setting. + // OptiTrack default: 239.255.42.99 (NATNET_DEFAULT_MULTICAST_ADDRESS) + this->declare_parameter("multicast_address", + std::string(NATNET_DEFAULT_MULTICAST_ADDRESS)); + 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 --------------------------------------------- + server_ip_ = this->get_parameter("server_ip").as_string(); + client_ip_ = this->get_parameter("client_ip").as_string(); + command_port_ = static_cast(this->get_parameter("command_port").as_int()); + data_port_ = static_cast(this->get_parameter("data_port").as_int()); + connection_type_ = this->get_parameter("connection_type").as_string(); + multicast_address_ = this->get_parameter("multicast_address").as_string(); + 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(); + + // Validate connection_type + if (connection_type_ != "unicast" && connection_type_ != "multicast") { + RCLCPP_WARN(get_logger(), + "Unknown connection_type '%s' — falling back to 'unicast'.", + connection_type_.c_str()); + connection_type_ = "unicast"; + } + + const auto pos_cov = this->get_parameter("position_covariance").as_double_array(); + const auto ori_cov = this->get_parameter("orientation_covariance").as_double_array(); + build_covariance_6x6(pos_cov, ori_cov); + + // ROBOT_NAME: set by AirStack's robot_name_map resolver at container startup. + 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", server_ip_.c_str()); + RCLCPP_INFO(get_logger(), " client_ip: %s", client_ip_.c_str()); + RCLCPP_INFO(get_logger(), " command_port: %d", static_cast(command_port_)); + RCLCPP_INFO(get_logger(), " data_port: %d", static_cast(data_port_)); + RCLCPP_INFO(get_logger(), " connection_type: %s", connection_type_.c_str()); + if (connection_type_ == "multicast") { + RCLCPP_INFO(get_logger(), " multicast_address: %s", multicast_address_.c_str()); + } + RCLCPP_INFO(get_logger(), " body_name: %s", body_name_.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(), " frame_id: %s", frame_id_.c_str()); + RCLCPP_INFO(get_logger(), "========================================="); + + // Route SDK log messages to ROS logger (set before Connect) + NatNet_SetLogCallback(natnet_log_callback); + + // Connect and pre-create publishers + connect(); + + // 1 Hz timer: refreshes data descriptions when Motive signals a model-list change. + refresh_timer_ = this->create_wall_timer( + std::chrono::seconds(1), + std::bind(&NatNetROS2Node::refresh_descriptions_if_needed, this)); + } + + // ----------------------------------------------------------------------- + ~NatNetROS2Node() + { + g_node_ptr = nullptr; + if (client_) { + client_->Disconnect(); + } + } + + // ----------------------------------------------------------------------- + // Called from frame_callback() on the NatNet SDK receive thread. + // rclcpp::Publisher::publish() is thread-safe; Clock::now() is thread-safe. + // pub_mutex_ serialises map lookups with the refresh timer that may call + // create_publisher() on the spin thread. + // ----------------------------------------------------------------------- + void on_frame(sFrameOfMocapData* data) + { + if (!data) return; + + // Bit 1 = model list changed; schedule a re-fetch on the spin thread. + if (data->params & 0x02) { + needs_description_refresh_.store(true, std::memory_order_relaxed); + } + + if (debug_) { + RCLCPP_DEBUG(get_logger(), "Frame %d: %d rigid bodies, ts=%.4f s", + data->iFrame, data->nRigidBodies, data->fTimestamp); + } + + const rclcpp::Time stamp = this->get_clock()->now(); + + for (int i = 0; i < data->nRigidBodies; ++i) { + const sRigidBodyData& rb = data->RigidBodies[i]; + + // Bit 0 of params = tracking valid + if (!(rb.params & 0x01)) { + if (debug_) { + RCLCPP_DEBUG(get_logger(), " RB id=%d: tracking invalid, skipping", rb.ID); + } + continue; + } + + // Filter by configured body_id when not tracking all bodies + if (body_id_ >= 0 && rb.ID != body_id_) continue; + + std::lock_guard lock(pub_mutex_); + + const auto pub_it = publishers_.find(rb.ID); + if (pub_it == publishers_.end()) { + // Publisher not yet created (body appeared before first description fetch). + // The refresh timer will create it within 1 s. + needs_description_refresh_.store(true, std::memory_order_relaxed); + continue; + } + + 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 = static_cast(rb.x); + msg.pose.position.y = static_cast(rb.y); + msg.pose.position.z = static_cast(rb.z); + msg.pose.orientation.x = static_cast(rb.qx); + msg.pose.orientation.y = static_cast(rb.qy); + msg.pose.orientation.z = static_cast(rb.qz); + msg.pose.orientation.w = static_cast(rb.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 = static_cast(rb.x); + cov_msg.pose.pose.position.y = static_cast(rb.y); + cov_msg.pose.pose.position.z = static_cast(rb.z); + cov_msg.pose.pose.orientation.x = static_cast(rb.qx); + cov_msg.pose.pose.orientation.y = static_cast(rb.qy); + cov_msg.pose.pose.orientation.z = static_cast(rb.qz); + cov_msg.pose.pose.orientation.w = static_cast(rb.qw); + cov_msg.pose.covariance = covariance_6x6_; + bp.pose_cov_pub->publish(cov_msg); + } + } + } + +private: + // ----------------------------------------------------------------------- + void connect() + { + client_ = std::make_unique(); + + sNatNetClientConnectParams params; + params.serverAddress = server_ip_.c_str(); + params.localAddress = client_ip_.c_str(); + params.serverCommandPort = command_port_; + params.serverDataPort = data_port_; + + if (connection_type_ == "multicast") { + params.connectionType = ConnectionType_Multicast; + params.multicastAddress = multicast_address_.c_str(); + } else { + params.connectionType = ConnectionType_Unicast; + params.multicastAddress = nullptr; + } + + const ErrorCode err = client_->Connect(params); + if (err != ErrorCode_OK) { + RCLCPP_ERROR(get_logger(), + "NatNetClient::Connect failed (ErrorCode %d). " + "Check server_ip, ports, and firewall on the Motive PC.", + static_cast(err)); + return; + } + + // Log server details + sServerDescription desc; + memset(&desc, 0, sizeof(desc)); + if (client_->GetServerDescription(&desc) == ErrorCode_OK && desc.HostPresent) { + RCLCPP_INFO(get_logger(), + "Connected to Motive '%s' v%d.%d (NatNet %d.%d) at %s", + desc.szHostApp, + static_cast(desc.HostAppVersion[0]), + static_cast(desc.HostAppVersion[1]), + static_cast(desc.NatNetVersion[0]), + static_cast(desc.NatNetVersion[1]), + server_ip_.c_str()); + } else { + RCLCPP_WARN(get_logger(), + "Connected to %s but GetServerDescription returned no host info.", + server_ip_.c_str()); + } + + // Pre-create publishers for all known rigid bodies + fetch_descriptions_and_create_publishers(); + + // Register frame callback last — frames start arriving immediately + client_->SetFrameReceivedCallback(frame_callback, this); + RCLCPP_INFO(get_logger(), "Frame callback registered — receiving mocap data."); + } + + // ----------------------------------------------------------------------- + // Fetches sDataDescriptions from Motive and creates publishers for every + // rigid body asset. Called on the spin thread (constructor + timer). + // ----------------------------------------------------------------------- + void fetch_descriptions_and_create_publishers() + { + if (!client_) return; + + std::lock_guard lock(pub_mutex_); + + // Always ensure the statically-configured body has a publisher, even if + // GetDataDescriptionList hasn't returned yet or body_id is in a skeleton. + if (body_id_ >= 0) { + ensure_publishers_locked(body_id_, body_name_); + } + + sDataDescriptions* desc_list = nullptr; + const ErrorCode err = client_->GetDataDescriptionList(&desc_list); + if (err != ErrorCode_OK || !desc_list) { + RCLCPP_WARN(get_logger(), + "GetDataDescriptionList failed (ErrorCode %d). " + "Will retry when model list changes.", static_cast(err)); + return; + } + + int newly_created = 0; + 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_desc = *dd.Data.RigidBodyDescription; + + // Store name for all bodies (including skeleton bones) for lookup in on_frame + body_names_[rb_desc.ID] = rb_desc.szName; + + // Skip skeleton bones (they have a valid parentID) + if (rb_desc.parentID >= 0) continue; + + // When tracking a single body, skip others to avoid noisy topics + if (body_id_ >= 0 && rb_desc.ID != body_id_) continue; + + if (ensure_publishers_locked(rb_desc.ID, rb_desc.szName)) { + ++newly_created; + } + } + + NatNet_FreeDescriptions(desc_list); + + 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."); + } + } + + // ----------------------------------------------------------------------- + // Creates PoseStamped + PoseWithCovarianceStamped publishers for a body + // if they don't exist yet. Returns true if publishers were created. + // Must be called with pub_mutex_ held. + // ----------------------------------------------------------------------- + bool ensure_publishers_locked(int32_t id, const std::string& name) + { + if (publishers_.count(id)) return false; + + const std::string topic_base = + "/" + robot_name_ + "/perception/optitrack/" + name; + + BodyPublishers bp; + if (publish_direct_) { + bp.pose_pub = this->create_publisher(topic_base, 10); + } + bp.pose_cov_pub = this->create_publisher( + topic_base + "/pose_cov", 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; + } + + // ----------------------------------------------------------------------- + // Called by the 1 Hz timer on the spin thread. + // ----------------------------------------------------------------------- + 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."); + fetch_descriptions_and_create_publishers(); + } + + // ----------------------------------------------------------------------- + void build_covariance_6x6( + const std::vector& pos_cov, + const std::vector& ori_cov) + { + covariance_6x6_.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) { + if (r * 3 + c < np) + covariance_6x6_[r * 6 + c] = pos_cov[r * 3 + c]; + if (r * 3 + c < no) + covariance_6x6_[(r + 3) * 6 + (c + 3)] = ori_cov[r * 3 + c]; + } + } + } + + // ----------------------------------------------------------------------- + // Parameters + std::string server_ip_; + std::string client_ip_; + uint16_t command_port_ = NATNET_DEFAULT_PORT_COMMAND; + uint16_t data_port_ = NATNET_DEFAULT_PORT_DATA; + std::string connection_type_ = "unicast"; + std::string multicast_address_ = NATNET_DEFAULT_MULTICAST_ADDRESS; + 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_{}; + + // NatNet SDK client + std::unique_ptr client_; + + // Publisher management — protected by pub_mutex_. + // Both on_frame() (SDK thread) and fetch_descriptions_and_create_publishers() + // (spin thread) must hold this mutex before accessing these maps. + std::mutex pub_mutex_; + std::unordered_map body_names_; + std::unordered_map publishers_; + + // Set by on_frame when Motive signals model list changed (params bit 1). + // Consumed and cleared by the 1 Hz refresh timer on the spin thread. + std::atomic needs_description_refresh_{false}; + + rclcpp::TimerBase::SharedPtr refresh_timer_; +}; + + +// --------------------------------------------------------------------------- +// SDK log callback — filters verbose Info/Debug messages unless needed. +// --------------------------------------------------------------------------- +void NATNET_CALLCONV natnet_log_callback(Verbosity level, const char* message) +{ + if (!g_node_ptr) { + if (level >= Verbosity_Warning) { + fprintf(stderr, "[NatNet][%s] %s\n", + (level == Verbosity_Warning) ? "WARN" : "ERROR", message); + } + return; + } + switch (level) { + case Verbosity_Debug: + RCLCPP_DEBUG(g_node_ptr->get_logger(), "[NatNet] %s", message); break; + case Verbosity_Info: + RCLCPP_DEBUG(g_node_ptr->get_logger(), "[NatNet] %s", message); break; + case Verbosity_Warning: + RCLCPP_WARN(g_node_ptr->get_logger(), "[NatNet] %s", message); break; + case Verbosity_Error: + RCLCPP_ERROR(g_node_ptr->get_logger(), "[NatNet] %s", message); break; + default: break; + } +} + +// --------------------------------------------------------------------------- +// Frame callback — called on the NatNet SDK receive thread. +// --------------------------------------------------------------------------- +void NATNET_CALLCONV frame_callback(sFrameOfMocapData* data, void* pUserContext) +{ + if (pUserContext) { + static_cast(pUserContext)->on_frame(data); + } +} + +// --------------------------------------------------------------------------- +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + + auto node = std::make_shared(); + // Set global after construction so the log callback has a valid pointer. + g_node_ptr = node.get(); + + rclcpp::spin(node); + + g_node_ptr = nullptr; + rclcpp::shutdown(); + return 0; +} diff --git a/robot/ros_ws/src/perception/natnet_ros2/src/natnet_ros2_node.py b/robot/ros_ws/src/perception/natnet_ros2/src/natnet_ros2_node.py deleted file mode 100755 index a3a2dbe16..000000000 --- a/robot/ros_ws/src/perception/natnet_ros2/src/natnet_ros2_node.py +++ /dev/null @@ -1,260 +0,0 @@ -#!/usr/bin/env python3 - -""" -NatNet ROS 2 Node - -Receives motion capture data from OptiTrack Motive via NatNet UDP protocol -and publishes poses to the AirStack perception layer. - -Uses pure Python parsing (no SDK required). - -Data flow: -1. Listen to NatNet UDP packets from external Motive PC -2. Decode rigid body poses and positions via NatNet protocol -3. Publish PoseStamped to /{robot_name}/perception/optitrack/{body_name} -4. Publish PoseWithCovarianceStamped to /{robot_name}/perception/optitrack/pose_cov -5. Optionally bridge to MAVROS for PX4 fusion (via vision_pose_converter_node) -""" - -import os -import sys - -import rclpy -from rclpy.node import Node -from geometry_msgs.msg import PoseStamped, PoseWithCovarianceStamped - -# natnet_client.py lives in the same installed directory -sys.path.insert(0, os.path.dirname(__file__)) -from natnet_client import NatNetClient, FrameData # noqa: E402 - - -class NatNetROS2Node(Node): - """ROS 2 node for NatNet motion capture integration.""" - - def __init__(self): - super().__init__('natnet_ros2_node') - - self.get_logger().info("########################################################") - self.get_logger().info("Began NatNet ROS 2 Node") - self.get_logger().info("########################################################") - - # Declare parameters - self.declare_parameter('server_ip', '192.168.1.1') - self.declare_parameter('command_port', 1510) - self.declare_parameter('data_port', 1511) - # Local NIC for UDP bind (NatNet SDK localAddress). 0.0.0.0 = OS chooses. - self.declare_parameter('client_ip', '0.0.0.0') - self.declare_parameter('body_name', 'robot_1') - # body_id: rigid body ID from Motive to track; -1 = track all bodies - self.declare_parameter('body_id', -1) - self.declare_parameter('publish_direct_optitrack', True) - self.declare_parameter('publish_to_mavros', False) - self.declare_parameter('frame_id', 'world') - self.declare_parameter('debug', False) - self.declare_parameter('negotiation_enabled', True) - # Covariance: 3x3 flattened (9 values each) - self.declare_parameter( - 'position_covariance', - [0.1, 0.0, 0.0, - 0.0, 0.1, 0.0, - 0.0, 0.0, 0.1] - ) - self.declare_parameter( - 'orientation_covariance', - [0.01, 0.0, 0.0, - 0.0, 0.01, 0.0, - 0.0, 0.0, 0.01] - ) - - # Get parameters - server_ip = self.get_parameter('server_ip').value - command_port = self.get_parameter('command_port').value - data_port = self.get_parameter('data_port').value - client_ip = self.get_parameter('client_ip').value - self.body_name = self.get_parameter('body_name').value - self.body_id = self.get_parameter('body_id').value - self.publish_direct = self.get_parameter('publish_direct_optitrack').value - self.publish_mavros = self.get_parameter('publish_to_mavros').value - self.frame_id = self.get_parameter('frame_id').value - self.debug = self.get_parameter('debug').value - negotiation_enabled = self.get_parameter('negotiation_enabled').value - - # Build 6x6 covariance matrix from 3x3 position and orientation blocks - pos_cov = self.get_parameter('position_covariance').value - ori_cov = self.get_parameter('orientation_covariance').value - self.covariance_6x6 = self._build_6x6_covariance(pos_cov, ori_cov) - - # Get robot name from environment - self.robot_name = os.environ.get('ROBOT_NAME', 'robot_1') - - # Publisher dictionaries (created on first publish per topic) - self.pose_publishers: dict = {} - self.pose_cov_publishers: dict = {} - - self.get_logger().info("########################################################") - self.get_logger().info("NatNet ROS 2 Node Parameters:") - self.get_logger().info("########################################################") - self.get_logger().info(f'Server IP: {server_ip}') - self.get_logger().info(f'Client IP: {client_ip}') - self.get_logger().info(f'Command port: {command_port}') - self.get_logger().info(f'Data port: {data_port}') - self.get_logger().info(f'Body name: {self.body_name}') - self.get_logger().info(f'Body id: {self.body_id}') - self.get_logger().info(f'Publish direct: {self.publish_direct}') - self.get_logger().info(f'Publish to mavros: {self.publish_mavros}') - self.get_logger().info(f'Frame id: {self.frame_id}') - self.get_logger().info(f'Debug: {self.debug}') - self.get_logger().info(f'Negotiation enabled: {negotiation_enabled}') - - # Initialize NatNet client - self.natnet_client = NatNetClient( - server_ip, - data_port, - command_port=command_port, - local_ip=client_ip, - negotiation_enabled=negotiation_enabled, - ) - self.natnet_client.set_frame_callback(self._on_frame_received) - - if not self.natnet_client.start(): - self.get_logger().error( - f'Failed to bind UDP socket on port {data_port}' - ) - raise RuntimeError('Failed to initialize NatNet UDP listener') - - self.get_logger().info( - f'NatNet node started (listening on UDP port {data_port})' - ) - self.get_logger().info(f'Robot name: {self.robot_name}') - if self.body_id >= 0: - self.get_logger().info( - f'Tracking body ID {self.body_id} as "{self.body_name}"' - ) - else: - self.get_logger().info('Tracking all bodies (body_id = -1)') - self.get_logger().info("########################################################") - - def _build_6x6_covariance( - self, pos_cov_3x3: list, ori_cov_3x3: list - ) -> list: - """Build a 6x6 covariance matrix (row-major) from two 3x3 blocks.""" - c = [0.0] * 36 - # Position block (rows 0-2, cols 0-2) - for r in range(3): - for col in range(3): - c[r * 6 + col] = float(pos_cov_3x3[r * 3 + col]) - # Orientation block (rows 3-5, cols 3-5) - for r in range(3): - for col in range(3): - c[(r + 3) * 6 + (col + 3)] = float(ori_cov_3x3[r * 3 + col]) - return c - - def destroy_node(self): - """Clean up resources on shutdown.""" - if self.natnet_client: - self.natnet_client.stop() - super().destroy_node() - - def _on_frame_received(self, frame: FrameData): - """Callback invoked when a complete NatNet frame is received.""" - try: - if self.debug: - self.get_logger().info( - f'Frame {frame.frame_number}: ' - f'{len(frame.rigid_bodies)} rigid bodies' - ) - - for rb_id, rb_data in frame.rigid_bodies.items(): - if not rb_data.tracking_valid: - continue - - # Filter by configured body_id when set - if self.body_id >= 0 and rb_id != self.body_id: - continue - - body_name = self._get_body_name(rb_id) - - if self.publish_direct: - self._publish_pose(body_name, rb_data) - - # Always publish pose_cov as the primary pose output - self._publish_pose_cov(body_name, rb_data) - - except Exception as e: - self.get_logger().error(f'Error processing frame: {e}') - - def _publish_pose(self, body_name: str, rb_data) -> None: - """Publish direct optitrack pose as PoseStamped.""" - topic = f'/{self.robot_name}/perception/optitrack/{body_name}' - if topic not in self.pose_publishers: - self.pose_publishers[topic] = self.create_publisher( - PoseStamped, topic, 10 - ) - - msg = PoseStamped() - msg.header.frame_id = self.frame_id - msg.header.stamp = self.get_clock().now().to_msg() - msg.pose.position.x = float(rb_data.position[0]) - msg.pose.position.y = float(rb_data.position[1]) - msg.pose.position.z = float(rb_data.position[2]) - msg.pose.orientation.x = float(rb_data.rotation[0]) - msg.pose.orientation.y = float(rb_data.rotation[1]) - msg.pose.orientation.z = float(rb_data.rotation[2]) - msg.pose.orientation.w = float(rb_data.rotation[3]) - - self.pose_publishers[topic].publish(msg) - - def _publish_pose_cov(self, body_name: str, rb_data) -> None: - """Publish PoseWithCovarianceStamped for this body.""" - topic = f'/{self.robot_name}/perception/optitrack/{body_name}/pose_cov' - if topic not in self.pose_cov_publishers: - self.pose_cov_publishers[topic] = self.create_publisher( - PoseWithCovarianceStamped, topic, 10 - ) - - msg = PoseWithCovarianceStamped() - msg.header.frame_id = self.frame_id - msg.header.stamp = self.get_clock().now().to_msg() - msg.pose.pose.position.x = float(rb_data.position[0]) - msg.pose.pose.position.y = float(rb_data.position[1]) - msg.pose.pose.position.z = float(rb_data.position[2]) - msg.pose.pose.orientation.x = float(rb_data.rotation[0]) - msg.pose.pose.orientation.y = float(rb_data.rotation[1]) - msg.pose.pose.orientation.z = float(rb_data.rotation[2]) - msg.pose.pose.orientation.w = float(rb_data.rotation[3]) - msg.pose.covariance = self.covariance_6x6 - - self.pose_cov_publishers[topic].publish(msg) - - def _get_body_name(self, body_id: int) -> str: - """ - Map a rigid body ID to a topic-friendly name. - - When tracking a specific body (body_id param >= 0), the configured - body_name is used. For all-body mode, falls back to body_{id}. - """ - if self.body_id >= 0 and body_id == self.body_id: - return self.body_name - return f'body_{body_id}' - - -def main(args=None): - rclpy.init(args=args) - node = None - try: - node = NatNetROS2Node() - rclpy.spin(node) - except KeyboardInterrupt: - pass - finally: - if node is not None: - node.destroy_node() - if rclpy.ok(): - try: - rclpy.shutdown() - except Exception: - pass - - -if __name__ == '__main__': - main() 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 index b9b265f38..9a36f879d 100755 --- 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 @@ -27,10 +27,11 @@ def __init__(self): self.declare_parameter('frame_id', 'world') self.declare_parameter('child_frame_id', 'base_link') - self.declare_parameter('use_sim_time', False) + 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( @@ -54,23 +55,42 @@ def __init__(self): 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'(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: - # Stamp the message with the configured reference frame ID msg.header.frame_id = self.frame_id + if self.canonical_quaternion: + msg.pose.pose.orientation = self._canonical_quaternion( + msg.pose.pose.orientation + ) - # Republish with covariance self.pose_cov_pub.publish(msg) - # Also publish PoseStamped version pose_msg = PoseStamped() pose_msg.header = msg.header pose_msg.pose = msg.pose.pose From c6ed2f86b1021486df39292a424295bda4fe661a Mon Sep 17 00:00:00 2001 From: John Date: Tue, 19 May 2026 19:02:11 -0400 Subject: [PATCH 12/21] unit test restructuring --- .agents/skills/configure-multi-robot/SKILL.md | 2 +- .agents/skills/run-system-tests/SKILL.md | 39 +++++----- AGENTS.md | 21 +++--- airstack.sh | 3 +- .../development/intermediate/testing/index.md | 14 ++-- .../lidar_point_cloud_filter/README.md | 4 +- tests/README.md | 55 +++++++++----- tests/conftest.py | 19 +++-- tests/parse_metrics.py | 12 ++- tests/pytest.ini | 1 + tests/requirements.txt | 1 + tests/robot/README.md | 16 ++++ tests/robot/behavior/README.md | 3 + tests/robot/global/README.md | 3 + tests/robot/interface/README.md | 3 + tests/robot/local/README.md | 3 + tests/robot/perception/README.md | 3 + tests/robot/sensors/README.md | 4 + .../test_validation_core.py | 66 +++++++++++++++++ .../validate_lidar_filter_clouds.py | 40 +++++----- .../validation_core.py | 73 +++++++++++++++++++ tests/sensor_probes.py | 4 +- tests/sim/README.md | 14 ++++ tests/sim/motive_emulator/README.md | 4 + tests/system/__init__.py | 1 + tests/{ => system}/test_build_docker.py | 0 tests/{ => system}/test_build_packages.py | 0 tests/{ => system}/test_liveliness.py | 2 +- tests/{ => system}/test_sensors.py | 4 +- tests/{ => system}/test_takeoff_hover_land.py | 0 30 files changed, 325 insertions(+), 89 deletions(-) create mode 100644 tests/robot/README.md create mode 100644 tests/robot/behavior/README.md create mode 100644 tests/robot/global/README.md create mode 100644 tests/robot/interface/README.md create mode 100644 tests/robot/local/README.md create mode 100644 tests/robot/perception/README.md create mode 100644 tests/robot/sensors/README.md create mode 100644 tests/robot/sensors/lidar_point_cloud_filter/test_validation_core.py rename {robot/ros_ws/src/sensors/lidar_point_cloud_filter/scripts => tests/robot/sensors/lidar_point_cloud_filter}/validate_lidar_filter_clouds.py (87%) create mode 100644 tests/robot/sensors/lidar_point_cloud_filter/validation_core.py create mode 100644 tests/sim/README.md create mode 100644 tests/sim/motive_emulator/README.md create mode 100644 tests/system/__init__.py rename tests/{ => system}/test_build_docker.py (100%) rename tests/{ => system}/test_build_packages.py (100%) rename tests/{ => system}/test_liveliness.py (99%) rename tests/{ => system}/test_sensors.py (97%) rename tests/{ => system}/test_takeoff_hover_land.py (100%) 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/run-system-tests/SKILL.md b/.agents/skills/run-system-tests/SKILL.md index 868d8c695..f9a108238 100644 --- a/.agents/skills/run-system-tests/SKILL.md +++ b/.agents/skills/run-system-tests/SKILL.md @@ -22,15 +22,18 @@ 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`) that mirror `robot/ros_ws/src/` layers and sim-side code. | 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 +42,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 +137,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 +201,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 +214,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 +269,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 +277,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/AGENTS.md b/AGENTS.md index e65d170c1..8644764de 100644 --- a/AGENTS.md +++ b/AGENTS.md @@ -201,29 +201,32 @@ docker exec airstack-robot-desktop-1 bash -c "ros2 topic echo --onc docker exec airstack-robot-desktop-1 bash -c "sws && colcon test --packages-select natnet_ros2 --event-handlers console_direct+" ``` -2. **System Level:** Full simulation tests (Isaac Sim or Microsoft AirSim legacy) +2. **Repo-root unit tests (`pytest`, `unit` mark):** Fast, hermetic checks under [`tests/robot/`](tests/robot/) and [`tests/sim/`](tests/sim/), mirroring `robot/ros_ws/src/` layers and sim-side tools. Declared in [`tests/pytest.ini`](tests/pytest.ini). Example: `airstack test -m unit -v` (no full Docker stack). + +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 052f844f7..08379cf26 100755 --- a/airstack.sh +++ b/airstack.sh @@ -195,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" diff --git a/docs/development/intermediate/testing/index.md b/docs/development/intermediate/testing/index.md index 9279f96c4..52832bbae 100644 --- a/docs/development/intermediate/testing/index.md +++ b/docs/development/intermediate/testing/index.md @@ -1,21 +1,25 @@ # 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 several test layers: ROS 2 package tests (`colcon test`), **repo-root unit tests** under [`tests/robot/`](../../../../tests/robot/) and [`tests/sim/`](../../../../tests/sim/) (`pytest -m unit`), and **system tests** under [`tests/system/`](../../../../tests/system/) (pytest, full Docker stack). -## System tests (`tests/`) +## System tests (`tests/system/`) The canonical reference is **[`tests/README.md`](../../../../tests/README.md)** (also included in the MkDocs site). 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`. **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*. +## Unit tests (`tests/robot/`, `tests/sim/`) + +Hermetic tests use the `unit` mark (see `tests/pytest.ini`). Layout mirrors onboard layers (`behavior`, `global`, …) and sim-side code. Example: LiDAR filter numeric rules in `tests/robot/sensors/lidar_point_cloud_filter/test_validation_core.py`. + ## Other testing docs - [Testing frameworks](testing_frameworks.md) — `colcon test`, rostest patterns 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..26eb3db4e 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 **`tests/robot/sensors/lidar_point_cloud_filter/validation_core.py`**, covered by **`test_validation_core.py`** (`pytest -m unit`) and imported by **`tests/robot/sensors/lidar_point_cloud_filter/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 `tests/robot/sensors/lidar_point_cloud_filter/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/tests/README.md b/tests/README.md index e4362dd4c..839da0469 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,22 @@ 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)). Example: [`robot/sensors/lidar_point_cloud_filter/validation_core.py`](robot/sensors/lidar_point_cloud_filter/validation_core.py) + [`test_validation_core.py`](robot/sensors/lidar_point_cloud_filter/test_validation_core.py) (numpy-only rules also used by `validate_lidar_filter_clouds.py`, which is a sibling in the same directory). 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 +39,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 +52,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`` | ``tests/robot/sensors/lidar_point_cloud_filter/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 +100,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 +121,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 +206,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 +270,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/conftest.py b/tests/conftest.py index ef8b88ec6..c40bd7848 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 @@ -44,6 +45,12 @@ } AIRSTACK_ROOT = os.environ.get("AIRSTACK_ROOT", str(Path(__file__).parent.parent)) +# LiDAR filter ``validation_core`` lives under ``tests/`` (not in ``robot/ros_ws/src``). +_lidar_vc = Path(AIRSTACK_ROOT) / "tests/robot/sensors/lidar_point_cloud_filter" +if _lidar_vc.is_dir(): + _p = str(_lidar_vc) + if _p not in sys.path: + sys.path.insert(0, _p) RUN_DIR = None LOGS_DIR = None ROS_DISTRO_SETUP = "/opt/ros/jazzy/setup.bash" @@ -156,11 +163,11 @@ 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", + "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. @@ -186,7 +193,7 @@ def pytest_collection_modifyitems(items): # (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..7f914bd46 100644 --- a/tests/requirements.txt +++ b/tests/requirements.txt @@ -4,3 +4,4 @@ pytest-dependency tabulate psutil pandas +numpy diff --git a/tests/robot/README.md b/tests/robot/README.md new file mode 100644 index 000000000..abdf942b7 --- /dev/null +++ b/tests/robot/README.md @@ -0,0 +1,16 @@ +# Robot-side unit tests + +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/` | + +Tests here are marked `@pytest.mark.unit` unless they need Docker or a live sim (use `tests/system/` or `tests/sim/` instead). + +Keep **test-only** Python (fixtures, pure numeric checks, protocol stubs) under `tests/` — for example `tests/robot/sensors/lidar_point_cloud_filter/validation_core.py`. `tests/conftest.py` extends `sys.path` where needed so `pytest` can import those modules. 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/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..471860a37 --- /dev/null +++ b/tests/robot/sensors/lidar_point_cloud_filter/test_validation_core.py @@ -0,0 +1,66 @@ +# Copyright 2026 AirLab CMU +# SPDX-License-Identifier: Apache-2.0 +"""Unit tests for ``validation_core`` (numpy-only).""" + +import numpy as np +import pytest + +from validation_core import ( + 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/lidar_point_cloud_filter/scripts/validate_lidar_filter_clouds.py b/tests/robot/sensors/lidar_point_cloud_filter/validate_lidar_filter_clouds.py similarity index 87% rename from robot/ros_ws/src/sensors/lidar_point_cloud_filter/scripts/validate_lidar_filter_clouds.py rename to tests/robot/sensors/lidar_point_cloud_filter/validate_lidar_filter_clouds.py index 7f50d45f6..e95143e86 100644 --- a/robot/ros_ws/src/sensors/lidar_point_cloud_filter/scripts/validate_lidar_filter_clouds.py +++ b/tests/robot/sensors/lidar_point_cloud_filter/validate_lidar_filter_clouds.py @@ -5,7 +5,7 @@ Run inside the robot container with workspace sourced and ROS_DOMAIN_ID set:: - python3 /root/AirStack/robot/ros_ws/src/sensors/lidar_point_cloud_filter/scripts/validate_lidar_filter_clouds.py --robot-num 1 + python3 /root/AirStack/tests/robot/sensors/lidar_point_cloud_filter/validate_lidar_filter_clouds.py --robot-num 1 Checks (after receiving a non-empty filtered cloud): * Filtered ``.../point_cloud``: all coordinates finite; **minimum range** must be at @@ -28,6 +28,7 @@ import subprocess import sys import time +from pathlib import Path import numpy as np import rclpy @@ -36,6 +37,18 @@ from sensor_msgs.msg import PointCloud2 from sensor_msgs_py import point_cloud2 +# ``validation_core`` is a sibling of this file under ``tests/robot/sensors/lidar_point_cloud_filter/``. +_here = Path(__file__).resolve().parent +if str(_here) not in sys.path: + sys.path.insert(0, str(_here)) + +from validation_core import ( + 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 +87,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 +131,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 +150,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 +166,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/tests/robot/sensors/lidar_point_cloud_filter/validation_core.py b/tests/robot/sensors/lidar_point_cloud_filter/validation_core.py new file mode 100644 index 000000000..1a975e61b --- /dev/null +++ b/tests/robot/sensors/lidar_point_cloud_filter/validation_core.py @@ -0,0 +1,73 @@ +# Copyright 2026 AirLab CMU +# SPDX-License-Identifier: Apache-2.0 +"""Pure-numeric LiDAR filter validation helpers (no ROS imports). + +Lives under ``tests/`` only. Shared by ``scripts/validate_lidar_filter_clouds.py`` +(runtime checks in the robot container, adds this directory to ``sys.path``) +and ``test_validation_core.py`` (``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/tests/sensor_probes.py b/tests/sensor_probes.py index cbb634261..fdbebb97e 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,7 +50,7 @@ ] LIDAR_CLOUD_VALIDATE_SCRIPT = ( - "/root/AirStack/robot/ros_ws/src/sensors/lidar_point_cloud_filter/scripts/" + "/root/AirStack/tests/robot/sensors/lidar_point_cloud_filter/" "validate_lidar_filter_clouds.py" ) 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..c991c4aa5 --- /dev/null +++ b/tests/sim/motive_emulator/README.md @@ -0,0 +1,4 @@ +# Motive / NatNet emulator tests + +Add `@pytest.mark.unit` tests for the Motive emulator once it lives under +`simulation/` (or another sim path). 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 100% rename from tests/test_build_packages.py rename to tests/system/test_build_packages.py 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 From 77b32b7966e591f167eda2604a48f3d861b59839 Mon Sep 17 00:00:00 2001 From: John Date: Wed, 20 May 2026 12:01:19 -0400 Subject: [PATCH 13/21] reorganized natnet logic for unit-testability --- .../src/perception/natnet_ros2/CMakeLists.txt | 15 + .../include/natnet_ros2/natnet_logic.hpp | 214 +++++++++ .../src/perception/natnet_ros2/package.xml | 1 + .../natnet_ros2/src/natnet_ros2_node.cpp | 100 ++-- tests/conftest.py | 13 +- .../natnet_ros2/test_natnet_logic.cpp | 449 ++++++++++++++++++ .../natnet_ros2/test_natnet_ros2.py | 152 ++++++ 7 files changed, 886 insertions(+), 58 deletions(-) create mode 100644 robot/ros_ws/src/perception/natnet_ros2/include/natnet_ros2/natnet_logic.hpp create mode 100644 tests/robot/perception/natnet_ros2/test_natnet_logic.cpp create mode 100644 tests/robot/perception/natnet_ros2/test_natnet_ros2.py diff --git a/robot/ros_ws/src/perception/natnet_ros2/CMakeLists.txt b/robot/ros_ws/src/perception/natnet_ros2/CMakeLists.txt index 822e27add..6ff7166ac 100644 --- a/robot/ros_ws/src/perception/natnet_ros2/CMakeLists.txt +++ b/robot/ros_ws/src/perception/natnet_ros2/CMakeLists.txt @@ -25,6 +25,9 @@ set_target_properties(NatNet PROPERTIES # C++ NatNet ROS 2 node # --------------------------------------------------------------------------- add_executable(natnet_ros2_node src/natnet_ros2_node.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) @@ -58,10 +61,22 @@ install(DIRECTORY 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) --- + # Source lives under tests/ (repo root) to keep test files out of robot/ros_ws/src. + # From this package directory, 5 levels up reaches the repo root. + find_package(ament_cmake_gtest REQUIRED) + ament_add_gtest(test_natnet_logic + "${CMAKE_CURRENT_SOURCE_DIR}/../../../../../tests/robot/perception/natnet_ros2/test_natnet_logic.cpp") + target_include_directories(test_natnet_logic PRIVATE + $) endif() ament_package() 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..b18d4ed24 --- /dev/null +++ b/robot/ros_ws/src/perception/natnet_ros2/include/natnet_ros2/natnet_logic.hpp @@ -0,0 +1,214 @@ +// natnet_logic.hpp — pure C++ helpers for natnet_ros2 (no ROS, no NatNet SDK). +// +// Four responsibility areas: +// +// 1. Covariance assembly +// 2. Topic names +// 3. Connection-configuration helpers (SDK-independent) +// 4. Rigid-body frame helpers (SDK-independent, for unit-testing frame logic) +// +// The NatNet SDK types (sNatNetClientConnectParams, sRigidBodyData, …) are only +// used inside natnet_ros2_node.cpp. All logic operated on here uses plain C++ +// types so that test/test_natnet_logic.cpp compiles with only gtest. + +#pragma once + +#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; +} + +} // namespace natnet_ros2 diff --git a/robot/ros_ws/src/perception/natnet_ros2/package.xml b/robot/ros_ws/src/perception/natnet_ros2/package.xml index 436d11b99..9b881304c 100644 --- a/robot/ros_ws/src/perception/natnet_ros2/package.xml +++ b/robot/ros_ws/src/perception/natnet_ros2/package.xml @@ -38,6 +38,7 @@ ament_flake8 ament_pep257 python3-pytest + ament_cmake_gtest ament_cmake 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 index b63e7ca95..a0c8193a0 100644 --- 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 @@ -33,6 +33,9 @@ #include "geometry_msgs/msg/pose_stamped.hpp" #include "geometry_msgs/msg/pose_with_covariance_stamped.hpp" +// Pure logic helpers (no SDK, no ROS — testable without either) +#include "natnet_ros2/natnet_logic.hpp" + // Forward declaration class NatNetROS2Node; @@ -103,17 +106,18 @@ class NatNetROS2Node : public rclcpp::Node frame_id_ = this->get_parameter("frame_id").as_string(); debug_ = this->get_parameter("debug").as_bool(); - // Validate connection_type - if (connection_type_ != "unicast" && connection_type_ != "multicast") { + // Validate connection_type (falls back to "unicast" for unknown values) + const std::string requested_ct = connection_type_; + connection_type_ = natnet_ros2::validate_connection_type(connection_type_); + if (connection_type_ != requested_ct) { RCLCPP_WARN(get_logger(), "Unknown connection_type '%s' — falling back to 'unicast'.", - connection_type_.c_str()); - connection_type_ = "unicast"; + requested_ct.c_str()); } const auto pos_cov = this->get_parameter("position_covariance").as_double_array(); const auto ori_cov = this->get_parameter("orientation_covariance").as_double_array(); - build_covariance_6x6(pos_cov, ori_cov); + covariance_6x6_ = natnet_ros2::build_covariance_6x6(pos_cov, ori_cov); // ROBOT_NAME: set by AirStack's robot_name_map resolver at container startup. const char* rn = std::getenv("ROBOT_NAME"); @@ -169,7 +173,7 @@ class NatNetROS2Node : public rclcpp::Node if (!data) return; // Bit 1 = model list changed; schedule a re-fetch on the spin thread. - if (data->params & 0x02) { + if (natnet_ros2::model_list_changed(data->params)) { needs_description_refresh_.store(true, std::memory_order_relaxed); } @@ -184,7 +188,7 @@ class NatNetROS2Node : public rclcpp::Node const sRigidBodyData& rb = data->RigidBodies[i]; // Bit 0 of params = tracking valid - if (!(rb.params & 0x01)) { + if (!natnet_ros2::is_tracking_valid(rb.params)) { if (debug_) { RCLCPP_DEBUG(get_logger(), " RB id=%d: tracking invalid, skipping", rb.ID); } @@ -192,7 +196,7 @@ class NatNetROS2Node : public rclcpp::Node } // Filter by configured body_id when not tracking all bodies - if (body_id_ >= 0 && rb.ID != body_id_) continue; + if (!natnet_ros2::should_publish_body(body_id_, rb.ID)) continue; std::lock_guard lock(pub_mutex_); @@ -206,32 +210,36 @@ class NatNetROS2Node : public rclcpp::Node const BodyPublishers& bp = pub_it->second; + const natnet_ros2::PoseData pose = natnet_ros2::rb_to_pose( + natnet_ros2::RigidBodySample{ + rb.ID, rb.x, rb.y, rb.z, rb.qx, rb.qy, rb.qz, rb.qw, rb.params}); + if (publish_direct_ && bp.pose_pub) { PoseStamped msg; - msg.header.frame_id = frame_id_; - msg.header.stamp = stamp; - msg.pose.position.x = static_cast(rb.x); - msg.pose.position.y = static_cast(rb.y); - msg.pose.position.z = static_cast(rb.z); - msg.pose.orientation.x = static_cast(rb.qx); - msg.pose.orientation.y = static_cast(rb.qy); - msg.pose.orientation.z = static_cast(rb.qz); - msg.pose.orientation.w = static_cast(rb.qw); + 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 = static_cast(rb.x); - cov_msg.pose.pose.position.y = static_cast(rb.y); - cov_msg.pose.pose.position.z = static_cast(rb.z); - cov_msg.pose.pose.orientation.x = static_cast(rb.qx); - cov_msg.pose.pose.orientation.y = static_cast(rb.qy); - cov_msg.pose.pose.orientation.z = static_cast(rb.qz); - cov_msg.pose.pose.orientation.w = static_cast(rb.qw); - cov_msg.pose.covariance = covariance_6x6_; + 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); } } @@ -243,15 +251,19 @@ class NatNetROS2Node : public rclcpp::Node { client_ = std::make_unique(); + const natnet_ros2::ConnectConfig cfg = natnet_ros2::make_connect_config( + server_ip_, client_ip_, command_port_, data_port_, + connection_type_, multicast_address_); + sNatNetClientConnectParams params; - params.serverAddress = server_ip_.c_str(); - params.localAddress = client_ip_.c_str(); - params.serverCommandPort = command_port_; - params.serverDataPort = data_port_; + 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 (connection_type_ == "multicast") { + if (natnet_ros2::is_multicast(cfg)) { params.connectionType = ConnectionType_Multicast; - params.multicastAddress = multicast_address_.c_str(); + params.multicastAddress = cfg.multicast_address.c_str(); } else { params.connectionType = ConnectionType_Unicast; params.multicastAddress = nullptr; @@ -358,14 +370,14 @@ class NatNetROS2Node : public rclcpp::Node if (publishers_.count(id)) return false; const std::string topic_base = - "/" + robot_name_ + "/perception/optitrack/" + name; + 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( - topic_base + "/pose_cov", 10); + natnet_ros2::optitrack_pose_cov_topic(robot_name_, name), 10); publishers_.emplace(id, std::move(bp)); @@ -387,24 +399,6 @@ class NatNetROS2Node : public rclcpp::Node fetch_descriptions_and_create_publishers(); } - // ----------------------------------------------------------------------- - void build_covariance_6x6( - const std::vector& pos_cov, - const std::vector& ori_cov) - { - covariance_6x6_.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) { - if (r * 3 + c < np) - covariance_6x6_[r * 6 + c] = pos_cov[r * 3 + c]; - if (r * 3 + c < no) - covariance_6x6_[(r + 3) * 6 + (c + 3)] = ori_cov[r * 3 + c]; - } - } - } - // ----------------------------------------------------------------------- // Parameters std::string server_ip_; diff --git a/tests/conftest.py b/tests/conftest.py index c40bd7848..ae9b6a76c 100644 --- a/tests/conftest.py +++ b/tests/conftest.py @@ -45,11 +45,14 @@ } AIRSTACK_ROOT = os.environ.get("AIRSTACK_ROOT", str(Path(__file__).parent.parent)) -# LiDAR filter ``validation_core`` lives under ``tests/`` (not in ``robot/ros_ws/src``). -_lidar_vc = Path(AIRSTACK_ROOT) / "tests/robot/sensors/lidar_point_cloud_filter" -if _lidar_vc.is_dir(): - _p = str(_lidar_vc) - if _p not in sys.path: +# Unit-test helpers live under tests/ (not in robot/ros_ws/src). +# Add each directory that contains importable test helpers (e.g. validation_core.py). +_UNIT_TEST_DIRS = [ + "tests/robot/sensors/lidar_point_cloud_filter", +] +for _d in _UNIT_TEST_DIRS: + _p = str(Path(AIRSTACK_ROOT) / _d) + if Path(_p).is_dir() and _p not in sys.path: sys.path.insert(0, _p) RUN_DIR = None LOGS_DIR = None diff --git a/tests/robot/perception/natnet_ros2/test_natnet_logic.cpp b/tests/robot/perception/natnet_ros2/test_natnet_logic.cpp new file mode 100644 index 000000000..f71ea2cbf --- /dev/null +++ b/tests/robot/perception/natnet_ros2/test_natnet_logic.cpp @@ -0,0 +1,449 @@ +// Copyright 2026 AirLab CMU +// SPDX-License-Identifier: Apache-2.0 +// +// 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" + +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); +} 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..7f75fdae1 --- /dev/null +++ b/tests/robot/perception/natnet_ros2/test_natnet_ros2.py @@ -0,0 +1,152 @@ +# Copyright 2026 AirLab CMU +# SPDX-License-Identifier: Apache-2.0 +"""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. + Add C++ tests under robot/ros_ws/src/perception/natnet_ros2/test/ using ament_add_gtest. +""" + +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 actual source directory (parents[4] = repo root). +_natnet_src = Path(__file__).resolve().parents[4] / "robot/ros_ws/src/perception/natnet_ros2/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) From 138a303b4dc43ffdb500dad860ec2d48caaa0593 Mon Sep 17 00:00:00 2001 From: John Date: Wed, 20 May 2026 16:47:39 -0400 Subject: [PATCH 14/21] unit testing restructuring to have unit tests in src and proxies in test. Unit tests workflows created --- .agents/skills/add-unit-tests/SKILL.md | 306 ++++++++++++++++ .agents/skills/run-system-tests/SKILL.md | 30 +- .github/workflows/unit-tests.yml | 54 +++ AGENTS.md | 5 +- .../development/intermediate/testing/index.md | 48 ++- .../intermediate/testing/unit_testing.md | 208 +++++++++++ mkdocs.yml | 6 +- .../src/perception/natnet_ros2/CMakeLists.txt | 78 ++-- .../natnet_ros2/natnet_client_adapter.hpp | 60 +++ .../include/natnet_ros2/natnet_logic.hpp | 150 +++++++- .../natnet_ros2/launch/natnet_ros2.launch.py | 19 +- .../src/perception/natnet_ros2/package.xml | 2 +- .../scripts/download-natnet-sdk.sh | 26 +- .../natnet_ros2/src/natnet_client_adapter.cpp | 179 +++++++++ .../natnet_ros2/src/natnet_ros2_node.cpp | 343 +++++------------- .../natnet_ros2/test/fake_natnet_client.hpp | 163 +++++++++ .../natnet_ros2/test}/test_natnet_logic.cpp | 312 +++++++++++++++- .../natnet_ros2/test/test_natnet_ros2.py | 152 ++++++++ .../validation_core.py | 10 +- .../scripts}/validate_lidar_filter_clouds.py | 19 +- .../test/test_validation_core.py | 75 ++++ tests/conftest.py | 13 +- .../natnet_ros2/test_natnet_ros2.py | 170 ++------- .../test_validation_core.py | 104 ++---- tests/sensor_probes.py | 4 +- tests/sim/motive_emulator/README.md | 63 +++- tests/system/test_build_packages.py | 33 ++ 27 files changed, 2077 insertions(+), 555 deletions(-) create mode 100644 .agents/skills/add-unit-tests/SKILL.md create mode 100644 .github/workflows/unit-tests.yml create mode 100644 robot/ros_ws/src/perception/natnet_ros2/include/natnet_ros2/natnet_client_adapter.hpp create mode 100644 robot/ros_ws/src/perception/natnet_ros2/src/natnet_client_adapter.cpp create mode 100644 robot/ros_ws/src/perception/natnet_ros2/test/fake_natnet_client.hpp rename {tests/robot/perception/natnet_ros2 => robot/ros_ws/src/perception/natnet_ros2/test}/test_natnet_logic.cpp (57%) create mode 100644 robot/ros_ws/src/perception/natnet_ros2/test/test_natnet_ros2.py rename {tests/robot/sensors => robot/ros_ws/src/sensors/lidar_point_cloud_filter}/lidar_point_cloud_filter/validation_core.py (87%) rename {tests/robot/sensors/lidar_point_cloud_filter => robot/ros_ws/src/sensors/lidar_point_cloud_filter/scripts}/validate_lidar_filter_clouds.py (90%) create mode 100644 robot/ros_ws/src/sensors/lidar_point_cloud_filter/test/test_validation_core.py diff --git a/.agents/skills/add-unit-tests/SKILL.md b/.agents/skills/add-unit-tests/SKILL.md new file mode 100644 index 000000000..e32080ff4 --- /dev/null +++ b/.agents/skills/add-unit-tests/SKILL.md @@ -0,0 +1,306 @@ +--- +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, the CI workflow that gates every push, 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 `unit-tests.yml` push/PR check | Same path — fast, no Docker | +| `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.yml` (`.github/workflows/unit-tests.yml`) runs `pytest tests/ -m unit` +on every push and PR — 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. + +--- + +## 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? | `.github/workflows/unit-tests.yml` — `ubuntu-latest`, ~10 s cold start | +| When does that workflow trigger? | Every push and every PR | +| 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/unit-tests.yml` — CI gate on every push/PR +- `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/run-system-tests/SKILL.md b/.agents/skills/run-system-tests/SKILL.md index f9a108238..53f93e842 100644 --- a/.agents/skills/run-system-tests/SKILL.md +++ b/.agents/skills/run-system-tests/SKILL.md @@ -25,7 +25,28 @@ This skill is about the **test harness itself** — pytest marks, fixtures, the 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`) that mirror `robot/ros_ws/src/` layers and sim-side code. +- **`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 | `unit-tests.yml` (`ubuntu-latest`, ~10 s) | `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 | |------|------|---------------|-------------------| @@ -154,11 +175,12 @@ Total parametrize cardinality for sim tests = `len(sims) × len(num_robots) × s ## Running Tests via PR Comment -The `system-tests.yml` workflow accepts three trigger paths: +The `system-tests.yml` workflow accepts four trigger paths: 1. **PR opened** (same-repo only) — auto-runs pytest with conftest defaults. Fork PRs are skipped to keep arbitrary code off the self-hosted runner. -2. **`/pytest` issue comment** on a PR — only honored from users with `OWNER`, `MEMBER`, or `COLLABORATOR` author association. Fork PRs are explicitly rejected by the `Resolve PR head` step (the PR's head repo must equal `${context.repo.owner}/${context.repo.repo}`). -3. **`workflow_dispatch`** — manual run from the Actions tab with form inputs (`marks`, `sim`, `num_robots`, `stress_iterations`, `stable_duration`, `baseline_run_id`). +2. **PR approved by a reviewer** (same-repo only) — re-runs the full suite on the approved state of the branch, giving a final validation pass before merge. +3. **`/pytest` issue comment** on a PR — only honored from users with `OWNER`, `MEMBER`, or `COLLABORATOR` author association. Fork PRs are explicitly rejected by the `Resolve PR head` step (the PR's head repo must equal `${context.repo.owner}/${context.repo.repo}`). +4. **`workflow_dispatch`** — manual run from the Actions tab with form inputs (`marks`, `sim`, `num_robots`, `stress_iterations`, `stable_duration`, `baseline_run_id`). ### `/pytest` comment grammar diff --git a/.github/workflows/unit-tests.yml b/.github/workflows/unit-tests.yml new file mode 100644 index 000000000..2b6fd7130 --- /dev/null +++ b/.github/workflows/unit-tests.yml @@ -0,0 +1,54 @@ +name: Unit Tests + +# Fast, hermetic unit tests — pure Python, no Docker, no GPU. +# Runs on every push and every PR so feedback arrives in seconds, +# not after the GPU runner provisions. +# +# These tests mirror the robot/ros_ws/src/ layer layout: +# tests/robot/perception/… proxy → robot/ros_ws/src/perception/…/test/ +# tests/robot/sensors/… proxy → robot/ros_ws/src/sensors/…/test/ +# tests/sim/… proxy → simulation/…/test/ (future) +# tests/gcs/… proxy → gcs/…/test/ (future) + +on: + push: + branches: + - '**' + pull_request: + +jobs: + unit-tests: + name: Unit Tests (pytest -m unit) + runs-on: ubuntu-latest + + steps: + - name: Checkout + uses: actions/checkout@v4 + + - name: Set up Python + uses: actions/setup-python@v5 + with: + python-version: '3.12' + cache: pip + cache-dependency-path: tests/requirements.txt + + - name: Install dependencies + run: pip install pytest numpy + + - name: Run unit tests + env: + AIRSTACK_ROOT: ${{ github.workspace }} + run: | + pytest tests/ -m unit \ + -v \ + --log-cli-level=INFO \ + --log-cli-format='%(asctime)s [%(levelname)s] %(message)s' \ + --log-cli-date-format='%H:%M:%S' + + - name: Upload test results + if: always() + uses: actions/upload-artifact@v4 + with: + name: unit-test-results-${{ github.sha }} + path: tests/results/ + retention-days: 30 diff --git a/AGENTS.md b/AGENTS.md index 8644764de..5e9af81ae 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 | @@ -201,7 +202,7 @@ docker exec airstack-robot-desktop-1 bash -c "ros2 topic echo --onc docker exec airstack-robot-desktop-1 bash -c "sws && colcon test --packages-select natnet_ros2 --event-handlers console_direct+" ``` -2. **Repo-root unit tests (`pytest`, `unit` mark):** Fast, hermetic checks under [`tests/robot/`](tests/robot/) and [`tests/sim/`](tests/sim/), mirroring `robot/ros_ws/src/` layers and sim-side tools. Declared in [`tests/pytest.ini`](tests/pytest.ini). Example: `airstack test -m unit -v` (no full Docker stack). +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/` (the CI command) discovers them. A dedicated CI workflow (`.github/workflows/unit-tests.yml`) runs these on every push and PR on a GitHub-hosted runner — no Docker or GPU needed. 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 @@ -256,7 +257,7 @@ GitHub Actions workflows live in [`.github/workflows/`](.github/workflows/): | Workflow | Trigger | Purpose | |----------|---------|---------| -| [`system-tests.yml`](.github/workflows/system-tests.yml) | PR opened, `/pytest` PR comment (write-access only), or `workflow_dispatch` | Runs the `tests/` suite on an ephemeral GPU runner; posts metrics report (with regression diff vs base branch / `main`) as a PR comment and to the job summary | +| [`system-tests.yml`](.github/workflows/system-tests.yml) | Push to `main`, PR opened, PR approved by reviewer, `/pytest` PR comment (write-access only), or `workflow_dispatch` | Runs the `tests/` suite on an ephemeral GPU runner; posts metrics report (with regression diff vs base branch / `main`) as a PR comment and to the job summary | | [`docker-build.yml`](.github/workflows/docker-build.yml) | Push to `main`/`develop` that changes `.env` (`VERSION=`), or manual dispatch | Builds, pushes, and cosign-signs all compose images on the ephemeral runner | | [`check-version-increment.yml`](.github/workflows/check-version-increment.yml) | Pull request | Validates `.env` `VERSION=` is valid semver and strictly greater than the base branch | | `deploy_docs_from_{main,develop,release}.yaml` | Push to the matching branch (`docs/**`, `mkdocs.yml`, `*.md`) | Publishes versioned MkDocs site via `mike` | diff --git a/docs/development/intermediate/testing/index.md b/docs/development/intermediate/testing/index.md index 52832bbae..d14fc753d 100644 --- a/docs/development/intermediate/testing/index.md +++ b/docs/development/intermediate/testing/index.md @@ -1,27 +1,57 @@ # Testing -AirStack uses several test layers: ROS 2 package tests (`colcon test`), **repo-root unit tests** under [`tests/robot/`](../../../../tests/robot/) and [`tests/sim/`](../../../../tests/sim/) (`pytest -m unit`), and **system tests** under [`tests/system/`](../../../../tests/system/) (pytest, full Docker stack). +AirStack uses three complementary test layers, each with a distinct scope and +hardware requirement: + +| 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 | + +## 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 +``` + +A dedicated CI workflow (`.github/workflows/unit-tests.yml`) runs these on every +push and PR on a GitHub-hosted `ubuntu-latest` runner — fast mandatory feedback +before the heavier GPU-based system tests are 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/`) -The canonical reference is **[`tests/README.md`](../../../../tests/README.md)** (also included in the MkDocs site). In short: +Full Docker-stack integration tests. The canonical reference is +**[`tests/README.md`](../../../../tests/README.md)**. In short: | Mark | Module | Role | -|------|--------|------| +|---|---|---| | `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`. - -**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*. - -## Unit tests (`tests/robot/`, `tests/sim/`) +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`. -Hermetic tests use the `unit` mark (see `tests/pytest.ini`). Layout mirrors onboard layers (`behavior`, `global`, …) and sim-side code. Example: LiDAR filter numeric rules in `tests/robot/sensors/lidar_point_cloud_filter/test_validation_core.py`. +**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..580d6e8d8 100644 --- a/docs/development/intermediate/testing/unit_testing.md +++ b/docs/development/intermediate/testing/unit_testing.md @@ -0,0 +1,208 @@ +# 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 workflow + +`.github/workflows/unit-tests.yml` runs on **every push and every pull request** on a GitHub-hosted `ubuntu-latest` runner. Cold-start time is ~10 seconds; the test run itself is under 1 second. This provides fast mandatory feedback before the heavier GPU-based system tests are needed. + +```yaml +# Simplified — see .github/workflows/unit-tests.yml for the full file +- run: pip install pytest numpy +- run: pytest tests/ -m unit -v + env: + AIRSTACK_ROOT: ${{ github.workspace }} +``` + +Results are uploaded as artifacts (`unit-test-results-`) with 30-day retention. + +## 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 `unit-tests.yml` 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 bfbdccb06..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 diff --git a/robot/ros_ws/src/perception/natnet_ros2/CMakeLists.txt b/robot/ros_ws/src/perception/natnet_ros2/CMakeLists.txt index 6ff7166ac..aa07e8024 100644 --- a/robot/ros_ws/src/perception/natnet_ros2/CMakeLists.txt +++ b/robot/ros_ws/src/perception/natnet_ros2/CMakeLists.txt @@ -12,38 +12,56 @@ find_package(geometry_msgs REQUIRED) find_package(nav_msgs REQUIRED) # --------------------------------------------------------------------------- -# NatNet SDK — pre-built shared library bundled in this package. +# 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. # --------------------------------------------------------------------------- -add_library(NatNet SHARED IMPORTED) -set_target_properties(NatNet PROPERTIES - IMPORTED_LOCATION "${CMAKE_CURRENT_SOURCE_DIR}/lib/libNatNet.so" - INTERFACE_INCLUDE_DIRECTORIES "${CMAKE_CURRENT_SOURCE_DIR}/include/natnet" -) +set(_NATNET_LIB "${CMAKE_CURRENT_SOURCE_DIR}/lib/libNatNet.so") +set(_NATNET_INC "${CMAKE_CURRENT_SOURCE_DIR}/include/natnet") -# --------------------------------------------------------------------------- -# C++ NatNet ROS 2 node -# --------------------------------------------------------------------------- -add_executable(natnet_ros2_node src/natnet_ros2_node.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) +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}" + ) -# $ORIGIN RPATH so the node finds libNatNet.so in the same install directory -# without needing LD_LIBRARY_PATH at runtime. -set_target_properties(natnet_ros2_node PROPERTIES - INSTALL_RPATH "$ORIGIN" - BUILD_WITH_INSTALL_RPATH TRUE -) + # 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}) + # $ORIGIN RPATH so the node finds libNatNet.so in the install directory + # without needing LD_LIBRARY_PATH at runtime. + set_target_properties(natnet_ros2_node PROPERTIES + INSTALL_RPATH "$ORIGIN" + BUILD_WITH_INSTALL_RPATH TRUE + ) -# Install the bundled NatNet .so alongside the executable -install(FILES lib/libNatNet.so - DESTINATION lib/${PROJECT_NAME}) + install(TARGETS natnet_ros2_node + DESTINATION lib/${PROJECT_NAME}) + + install(FILES "${_NATNET_LIB}" + DESTINATION lib/${PROJECT_NAME}) +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) @@ -70,13 +88,11 @@ if(BUILD_TESTING) ament_lint_auto_find_test_dependencies() # --- gtest: pure-logic unit tests (no NatNet SDK, no rclcpp) --- - # Source lives under tests/ (repo root) to keep test files out of robot/ros_ws/src. - # From this package directory, 5 levels up reaches the repo root. find_package(ament_cmake_gtest REQUIRED) - ament_add_gtest(test_natnet_logic - "${CMAKE_CURRENT_SOURCE_DIR}/../../../../../tests/robot/perception/natnet_ros2/test_natnet_logic.cpp") + 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/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 index b18d4ed24..f23216565 100644 --- 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 @@ -1,20 +1,42 @@ +// 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). // -// Four responsibility areas: +// Five responsibility areas: // // 1. Covariance assembly // 2. Topic names // 3. Connection-configuration helpers (SDK-independent) -// 4. Rigid-body frame helpers (SDK-independent, for unit-testing frame logic) +// 4. Rigid-body frame helpers (SDK-independent) +// 5. Abstraction seam: INatNetClient interface + negotiation logic // -// The NatNet SDK types (sNatNetClientConnectParams, sRigidBodyData, …) are only -// used inside natnet_ros2_node.cpp. All logic operated on here uses plain C++ -// types so that test/test_natnet_logic.cpp compiles with only gtest. +// 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 @@ -211,4 +233,122 @@ 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 index b968e1ec7..aa631947a 100644 --- 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 @@ -1,5 +1,10 @@ #!/usr/bin/env python3 -"""Bring up NatNet node; optionally MAVROS bridge per natnet_config.yaml.""" +"""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), the executable will +be absent and this launch file skips it with a warning rather than crashing. +""" from __future__ import annotations @@ -51,6 +56,18 @@ def launch_setup(context, *_args, **_kwargs): publish_mavros = bool(ros_params.get('publish_to_mavros', False)) body_name = str(ros_params.get('body_name', 'robot_1')) + # natnet_ros2_node is only built when the NatNet SDK is present. + # Skip gracefully rather than crashing if the SDK was not installed. + pkg_share = get_package_share_directory('natnet_ros2') + node_path = Path(pkg_share).parent / 'natnet_ros2' / 'natnet_ros2_node' + if not node_path.exists(): + import logging + logging.getLogger('natnet_ros2.launch').warning( + 'natnet_ros2_node executable not found — NatNet SDK not installed. ' + "Run 'airstack setup' to enable OptiTrack support." + ) + return [] + actions = [ Node( package='natnet_ros2', diff --git a/robot/ros_ws/src/perception/natnet_ros2/package.xml b/robot/ros_ws/src/perception/natnet_ros2/package.xml index 9b881304c..f9632b0f7 100644 --- a/robot/ros_ws/src/perception/natnet_ros2/package.xml +++ b/robot/ros_ws/src/perception/natnet_ros2/package.xml @@ -10,7 +10,7 @@ AirLab CMU - Apache-2.0 + MIT 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 index e90c18db2..e91360197 100644 --- 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 @@ -69,7 +69,7 @@ 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/legal/software-license-agreement +https://optitrack.com/about/legal/eula This installer will download the SDK into the natnet_ros2 module tree: - lib/libNatNet.so @@ -125,6 +125,15 @@ install_files() { } 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" @@ -135,12 +144,15 @@ main() { choose_archive show_license_notice - 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 + 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")" 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..e4ae2a7b7 --- /dev/null +++ b/robot/ros_ws/src/perception/natnet_ros2/src/natnet_client_adapter.cpp @@ -0,0 +1,179 @@ +// 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. +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; + case ErrorCode_Timeout: return NatNetResult::Timeout; + 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 index a0c8193a0..65059f659 100644 --- 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 @@ -17,36 +17,19 @@ #include #include #include -#include #include #include #include #include -// NatNet SDK (bundled in include/natnet/, lib/libNatNet.so) -#include "NatNetClient.h" -#include "NatNetCAPI.h" -#include "NatNetTypes.h" - // ROS 2 #include "rclcpp/rclcpp.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "geometry_msgs/msg/pose_with_covariance_stamped.hpp" -// Pure logic helpers (no SDK, no ROS — testable without either) +// Pure logic + interface (no SDK, testable with FakeNatNetClient) #include "natnet_ros2/natnet_logic.hpp" - - -// Forward declaration -class NatNetROS2Node; - -// --------------------------------------------------------------------------- -// SDK log callback — registered globally, routes to ROS logger when available. -// --------------------------------------------------------------------------- -static NatNetROS2Node* g_node_ptr = nullptr; - -void NATNET_CALLCONV natnet_log_callback(Verbosity level, const char* message); -void NATNET_CALLCONV frame_callback(sFrameOfMocapData* data, void* pUserContext); +#include "natnet_ros2/natnet_client_adapter.hpp" // --------------------------------------------------------------------------- @@ -55,12 +38,12 @@ void NATNET_CALLCONV frame_callback(sFrameOfMocapData* data, void* pUserContext) class NatNetROS2Node : public rclcpp::Node { public: - using PoseStamped = geometry_msgs::msg::PoseStamped; + using PoseStamped = geometry_msgs::msg::PoseStamped; using PoseWithCovarianceStamped = geometry_msgs::msg::PoseWithCovarianceStamped; struct BodyPublishers { - rclcpp::Publisher::SharedPtr pose_pub; + rclcpp::Publisher::SharedPtr pose_pub; rclcpp::Publisher::SharedPtr pose_cov_pub; }; @@ -73,13 +56,8 @@ class NatNetROS2Node : public rclcpp::Node this->declare_parameter("client_ip", "0.0.0.0"); this->declare_parameter("command_port", 1510); this->declare_parameter("data_port", 1511); - // "unicast" or "multicast" this->declare_parameter("connection_type", std::string("unicast")); - // Multicast group address used when connection_type = "multicast". - // Must match Motive's "Data Streaming" multicast address setting. - // OptiTrack default: 239.255.42.99 (NATNET_DEFAULT_MULTICAST_ADDRESS) - this->declare_parameter("multicast_address", - std::string(NATNET_DEFAULT_MULTICAST_ADDRESS)); + 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); @@ -94,60 +72,53 @@ class NatNetROS2Node : public rclcpp::Node std::vector{0.01,0.,0., 0.,0.01,0., 0.,0.,0.01}); // ----- Read parameters --------------------------------------------- - server_ip_ = this->get_parameter("server_ip").as_string(); - client_ip_ = this->get_parameter("client_ip").as_string(); - command_port_ = static_cast(this->get_parameter("command_port").as_int()); - data_port_ = static_cast(this->get_parameter("data_port").as_int()); - connection_type_ = this->get_parameter("connection_type").as_string(); - multicast_address_ = this->get_parameter("multicast_address").as_string(); - 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(); - - // Validate connection_type (falls back to "unicast" for unknown values) - const std::string requested_ct = connection_type_; - connection_type_ = natnet_ros2::validate_connection_type(connection_type_); - if (connection_type_ != requested_ct) { + 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'.", - requested_ct.c_str()); + this->get_parameter("connection_type").as_string().c_str()); } - const auto pos_cov = this->get_parameter("position_covariance").as_double_array(); - const auto ori_cov = this->get_parameter("orientation_covariance").as_double_array(); - covariance_6x6_ = natnet_ros2::build_covariance_6x6(pos_cov, ori_cov); + 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(); - // ROBOT_NAME: set by AirStack's robot_name_map resolver at container startup. - const char* rn = std::getenv("ROBOT_NAME"); + 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", server_ip_.c_str()); - RCLCPP_INFO(get_logger(), " client_ip: %s", client_ip_.c_str()); - RCLCPP_INFO(get_logger(), " command_port: %d", static_cast(command_port_)); - RCLCPP_INFO(get_logger(), " data_port: %d", static_cast(data_port_)); - RCLCPP_INFO(get_logger(), " connection_type: %s", connection_type_.c_str()); - if (connection_type_ == "multicast") { - RCLCPP_INFO(get_logger(), " multicast_address: %s", multicast_address_.c_str()); + 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_name: %s", body_name_.c_str()); - RCLCPP_INFO(get_logger(), " body_id: %d (%s)", + RCLCPP_INFO(get_logger(), " body_id: %d (%s)", static_cast(body_id_), (body_id_ < 0) ? "track all" : "single body"); - RCLCPP_INFO(get_logger(), " frame_id: %s", frame_id_.c_str()); RCLCPP_INFO(get_logger(), "========================================="); - // Route SDK log messages to ROS logger (set before Connect) - NatNet_SetLogCallback(natnet_log_callback); - - // Connect and pre-create publishers - connect(); + // Production client — NatNetClientAdapter wraps the SDK + client_ = std::make_unique(); + connect_and_setup(connect_cfg); - // 1 Hz timer: refreshes data descriptions when Motive signals a model-list change. refresh_timer_ = this->create_wall_timer( std::chrono::seconds(1), std::bind(&NatNetROS2Node::refresh_descriptions_if_needed, this)); @@ -156,63 +127,45 @@ class NatNetROS2Node : public rclcpp::Node // ----------------------------------------------------------------------- ~NatNetROS2Node() { - g_node_ptr = nullptr; - if (client_) { - client_->Disconnect(); - } + if (client_) { client_->disconnect(); } } // ----------------------------------------------------------------------- - // Called from frame_callback() on the NatNet SDK receive thread. - // rclcpp::Publisher::publish() is thread-safe; Clock::now() is thread-safe. - // pub_mutex_ serialises map lookups with the refresh timer that may call - // create_publisher() on the spin thread. + // Called from the NatNetClientAdapter's frame trampoline. + // publish() and Clock::now() are thread-safe; pub_mutex_ guards map access. // ----------------------------------------------------------------------- - void on_frame(sFrameOfMocapData* data) + void on_frame(const natnet_ros2::FrameSample & frame) { - if (!data) return; - - // Bit 1 = model list changed; schedule a re-fetch on the spin thread. - if (natnet_ros2::model_list_changed(data->params)) { + 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: %d rigid bodies, ts=%.4f s", - data->iFrame, data->nRigidBodies, data->fTimestamp); + 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 (int i = 0; i < data->nRigidBodies; ++i) { - const sRigidBodyData& rb = data->RigidBodies[i]; - - // Bit 0 of params = tracking valid + 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); + RCLCPP_DEBUG(get_logger(), " RB id=%d: tracking invalid, skipping", rb.id); } continue; } - - // Filter by configured body_id when not tracking all bodies - if (!natnet_ros2::should_publish_body(body_id_, 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); + const auto pub_it = publishers_.find(rb.id); if (pub_it == publishers_.end()) { - // Publisher not yet created (body appeared before first description fetch). - // The refresh timer will create it within 1 s. needs_description_refresh_.store(true, std::memory_order_relaxed); continue; } - const BodyPublishers& bp = pub_it->second; - - const natnet_ros2::PoseData pose = natnet_ros2::rb_to_pose( - natnet_ros2::RigidBodySample{ - rb.ID, rb.x, rb.y, rb.z, rb.qx, rb.qy, rb.qz, rb.qw, rb.params}); + 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; @@ -247,111 +200,67 @@ class NatNetROS2Node : public rclcpp::Node private: // ----------------------------------------------------------------------- - void connect() + void connect_and_setup(const natnet_ros2::ConnectConfig & cfg) { - client_ = std::make_unique(); + const natnet_ros2::NegotiationResult neg = + natnet_ros2::negotiate(*client_, cfg); - const natnet_ros2::ConnectConfig cfg = natnet_ros2::make_connect_config( - server_ip_, client_ip_, command_port_, data_port_, - connection_type_, multicast_address_); - - 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 (natnet_ros2::is_multicast(cfg)) { - params.connectionType = ConnectionType_Multicast; - params.multicastAddress = cfg.multicast_address.c_str(); - } else { - params.connectionType = ConnectionType_Unicast; - params.multicastAddress = nullptr; - } - - const ErrorCode err = client_->Connect(params); - if (err != ErrorCode_OK) { - RCLCPP_ERROR(get_logger(), - "NatNetClient::Connect failed (ErrorCode %d). " - "Check server_ip, ports, and firewall on the Motive PC.", - static_cast(err)); + if (!neg.ok) { + RCLCPP_ERROR(get_logger(), "%s", neg.log_message.c_str()); return; } - // Log server details - sServerDescription desc; - memset(&desc, 0, sizeof(desc)); - if (client_->GetServerDescription(&desc) == ErrorCode_OK && desc.HostPresent) { - RCLCPP_INFO(get_logger(), - "Connected to Motive '%s' v%d.%d (NatNet %d.%d) at %s", - desc.szHostApp, - static_cast(desc.HostAppVersion[0]), - static_cast(desc.HostAppVersion[1]), - static_cast(desc.NatNetVersion[0]), - static_cast(desc.NatNetVersion[1]), - server_ip_.c_str()); + if (neg.server_info.host_present) { + RCLCPP_INFO(get_logger(), "%s", neg.log_message.c_str()); } else { - RCLCPP_WARN(get_logger(), - "Connected to %s but GetServerDescription returned no host info.", - server_ip_.c_str()); + RCLCPP_WARN(get_logger(), "%s", neg.log_message.c_str()); } - // Pre-create publishers for all known rigid bodies - fetch_descriptions_and_create_publishers(); + refresh_descriptions_locked(); - // Register frame callback last — frames start arriving immediately - client_->SetFrameReceivedCallback(frame_callback, this); + client_->set_frame_callback( + [this](const natnet_ros2::FrameSample & f) { on_frame(f); }); RCLCPP_INFO(get_logger(), "Frame callback registered — receiving mocap data."); } // ----------------------------------------------------------------------- - // Fetches sDataDescriptions from Motive and creates publishers for every - // rigid body asset. Called on the spin thread (constructor + timer). - // ----------------------------------------------------------------------- - void fetch_descriptions_and_create_publishers() + void refresh_descriptions_if_needed() { - if (!client_) return; - + 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(); + } - // Always ensure the statically-configured body has a publisher, even if - // GetDataDescriptionList hasn't returned yet or body_id is in a skeleton. - if (body_id_ >= 0) { - ensure_publishers_locked(body_id_, body_name_); - } + // ----------------------------------------------------------------------- + // Must be called with pub_mutex_ held (or from single-threaded init). + // ----------------------------------------------------------------------- + void refresh_descriptions_locked() + { + if (!client_) { return; } - sDataDescriptions* desc_list = nullptr; - const ErrorCode err = client_->GetDataDescriptionList(&desc_list); - if (err != ErrorCode_OK || !desc_list) { - RCLCPP_WARN(get_logger(), - "GetDataDescriptionList failed (ErrorCode %d). " - "Will retry when model list changes.", static_cast(err)); - 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 (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_desc = *dd.Data.RigidBodyDescription; + for (const auto & bd : bodies) { + // Store name for every body (including skeleton bones) + body_names_[bd.id] = bd.name; - // Store name for all bodies (including skeleton bones) for lookup in on_frame - body_names_[rb_desc.ID] = rb_desc.szName; + // Skip skeleton bones (parent_id >= 0) + if (bd.parent_id >= 0) { continue; } - // Skip skeleton bones (they have a valid parentID) - if (rb_desc.parentID >= 0) continue; + // When tracking a single body, skip others + if (!natnet_ros2::should_publish_body(body_id_, bd.id)) { continue; } - // When tracking a single body, skip others to avoid noisy topics - if (body_id_ >= 0 && rb_desc.ID != body_id_) continue; - - if (ensure_publishers_locked(rb_desc.ID, rb_desc.szName)) { - ++newly_created; - } + if (ensure_publisher_locked(bd.id, bd.name)) { ++newly_created; } } - NatNet_FreeDescriptions(desc_list); - if (newly_created > 0) { RCLCPP_INFO(get_logger(), "Data descriptions refreshed: %d new publisher(s) created.", newly_created); @@ -361,13 +270,9 @@ class NatNetROS2Node : public rclcpp::Node } // ----------------------------------------------------------------------- - // Creates PoseStamped + PoseWithCovarianceStamped publishers for a body - // if they don't exist yet. Returns true if publishers were created. - // Must be called with pub_mutex_ held. - // ----------------------------------------------------------------------- - bool ensure_publishers_locked(int32_t id, const std::string& name) + bool ensure_publisher_locked(int32_t id, const std::string & name) { - if (publishers_.count(id)) return false; + if (publishers_.count(id)) { return false; } const std::string topic_base = natnet_ros2::optitrack_topic_base(robot_name_, name); @@ -388,25 +293,7 @@ class NatNetROS2Node : public rclcpp::Node } // ----------------------------------------------------------------------- - // Called by the 1 Hz timer on the spin thread. - // ----------------------------------------------------------------------- - 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."); - fetch_descriptions_and_create_publishers(); - } - - // ----------------------------------------------------------------------- - // Parameters - std::string server_ip_; - std::string client_ip_; - uint16_t command_port_ = NATNET_DEFAULT_PORT_COMMAND; - uint16_t data_port_ = NATNET_DEFAULT_PORT_DATA; - std::string connection_type_ = "unicast"; - std::string multicast_address_ = NATNET_DEFAULT_MULTICAST_ADDRESS; + // Parameters / state std::string body_name_; int32_t body_id_ = -1; bool publish_direct_ = true; @@ -416,71 +303,23 @@ class NatNetROS2Node : public rclcpp::Node std::array covariance_6x6_{}; - // NatNet SDK client - std::unique_ptr client_; + std::unique_ptr client_; - // Publisher management — protected by pub_mutex_. - // Both on_frame() (SDK thread) and fetch_descriptions_and_create_publishers() - // (spin thread) must hold this mutex before accessing these maps. std::mutex pub_mutex_; std::unordered_map body_names_; std::unordered_map publishers_; - // Set by on_frame when Motive signals model list changed (params bit 1). - // Consumed and cleared by the 1 Hz refresh timer on the spin thread. std::atomic needs_description_refresh_{false}; - rclcpp::TimerBase::SharedPtr refresh_timer_; }; // --------------------------------------------------------------------------- -// SDK log callback — filters verbose Info/Debug messages unless needed. -// --------------------------------------------------------------------------- -void NATNET_CALLCONV natnet_log_callback(Verbosity level, const char* message) -{ - if (!g_node_ptr) { - if (level >= Verbosity_Warning) { - fprintf(stderr, "[NatNet][%s] %s\n", - (level == Verbosity_Warning) ? "WARN" : "ERROR", message); - } - return; - } - switch (level) { - case Verbosity_Debug: - RCLCPP_DEBUG(g_node_ptr->get_logger(), "[NatNet] %s", message); break; - case Verbosity_Info: - RCLCPP_DEBUG(g_node_ptr->get_logger(), "[NatNet] %s", message); break; - case Verbosity_Warning: - RCLCPP_WARN(g_node_ptr->get_logger(), "[NatNet] %s", message); break; - case Verbosity_Error: - RCLCPP_ERROR(g_node_ptr->get_logger(), "[NatNet] %s", message); break; - default: break; - } -} - -// --------------------------------------------------------------------------- -// Frame callback — called on the NatNet SDK receive thread. -// --------------------------------------------------------------------------- -void NATNET_CALLCONV frame_callback(sFrameOfMocapData* data, void* pUserContext) -{ - if (pUserContext) { - static_cast(pUserContext)->on_frame(data); - } -} - -// --------------------------------------------------------------------------- -int main(int argc, char** argv) +int main(int argc, char ** argv) { rclcpp::init(argc, argv); - auto node = std::make_shared(); - // Set global after construction so the log callback has a valid pointer. - g_node_ptr = node.get(); - rclcpp::spin(node); - - g_node_ptr = nullptr; rclcpp::shutdown(); return 0; } 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/tests/robot/perception/natnet_ros2/test_natnet_logic.cpp b/robot/ros_ws/src/perception/natnet_ros2/test/test_natnet_logic.cpp similarity index 57% rename from tests/robot/perception/natnet_ros2/test_natnet_logic.cpp rename to robot/ros_ws/src/perception/natnet_ros2/test/test_natnet_logic.cpp index f71ea2cbf..7a144ef9b 100644 --- a/tests/robot/perception/natnet_ros2/test_natnet_logic.cpp +++ b/robot/ros_ws/src/perception/natnet_ros2/test/test_natnet_logic.cpp @@ -1,5 +1,22 @@ -// Copyright 2026 AirLab CMU -// SPDX-License-Identifier: Apache-2.0 +// 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. // @@ -10,6 +27,7 @@ #include #include "natnet_ros2/natnet_logic.hpp" +#include "fake_natnet_client.hpp" using namespace natnet_ros2; @@ -447,3 +465,293 @@ TEST(FrameSample, CovarianceStampedIntoMessage) 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/tests/robot/sensors/lidar_point_cloud_filter/validation_core.py b/robot/ros_ws/src/sensors/lidar_point_cloud_filter/lidar_point_cloud_filter/validation_core.py similarity index 87% rename from tests/robot/sensors/lidar_point_cloud_filter/validation_core.py rename to robot/ros_ws/src/sensors/lidar_point_cloud_filter/lidar_point_cloud_filter/validation_core.py index 1a975e61b..5d3f22cfd 100644 --- a/tests/robot/sensors/lidar_point_cloud_filter/validation_core.py +++ b/robot/ros_ws/src/sensors/lidar_point_cloud_filter/lidar_point_cloud_filter/validation_core.py @@ -1,10 +1,10 @@ -# 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. """Pure-numeric LiDAR filter validation helpers (no ROS imports). -Lives under ``tests/`` only. Shared by ``scripts/validate_lidar_filter_clouds.py`` -(runtime checks in the robot container, adds this directory to ``sys.path``) -and ``test_validation_core.py`` (``pytest -m unit``). +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 diff --git a/tests/robot/sensors/lidar_point_cloud_filter/validate_lidar_filter_clouds.py b/robot/ros_ws/src/sensors/lidar_point_cloud_filter/scripts/validate_lidar_filter_clouds.py similarity index 90% rename from tests/robot/sensors/lidar_point_cloud_filter/validate_lidar_filter_clouds.py rename to robot/ros_ws/src/sensors/lidar_point_cloud_filter/scripts/validate_lidar_filter_clouds.py index e95143e86..f53460a56 100644 --- a/tests/robot/sensors/lidar_point_cloud_filter/validate_lidar_filter_clouds.py +++ b/robot/ros_ws/src/sensors/lidar_point_cloud_filter/scripts/validate_lidar_filter_clouds.py @@ -1,17 +1,17 @@ #!/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:: - python3 /root/AirStack/tests/robot/sensors/lidar_point_cloud_filter/validate_lidar_filter_clouds.py --robot-num 1 + python3 /root/AirStack/robot/ros_ws/src/sensors/lidar_point_cloud_filter/scripts/validate_lidar_filter_clouds.py --robot-num 1 Checks (after receiving a non-empty filtered cloud): * 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 @@ -37,12 +37,13 @@ from sensor_msgs.msg import PointCloud2 from sensor_msgs_py import point_cloud2 -# ``validation_core`` is a sibling of this file under ``tests/robot/sensors/lidar_point_cloud_filter/``. -_here = Path(__file__).resolve().parent -if str(_here) not in sys.path: - sys.path.insert(0, str(_here)) +# 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 validation_core import ( +from lidar_point_cloud_filter.validation_core import ( # noqa: E402 near_range_tolerance, ranges_xyz_from_points_xyz, raw_filtered_near_range_ok, 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/tests/conftest.py b/tests/conftest.py index ae9b6a76c..9b4964a1a 100644 --- a/tests/conftest.py +++ b/tests/conftest.py @@ -45,15 +45,10 @@ } AIRSTACK_ROOT = os.environ.get("AIRSTACK_ROOT", str(Path(__file__).parent.parent)) -# Unit-test helpers live under tests/ (not in robot/ros_ws/src). -# Add each directory that contains importable test helpers (e.g. validation_core.py). -_UNIT_TEST_DIRS = [ - "tests/robot/sensors/lidar_point_cloud_filter", -] -for _d in _UNIT_TEST_DIRS: - _p = str(Path(AIRSTACK_ROOT) / _d) - if Path(_p).is_dir() and _p not in sys.path: - sys.path.insert(0, _p) +# 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" diff --git a/tests/robot/perception/natnet_ros2/test_natnet_ros2.py b/tests/robot/perception/natnet_ros2/test_natnet_ros2.py index 7f75fdae1..fc9a78bde 100644 --- a/tests/robot/perception/natnet_ros2/test_natnet_ros2.py +++ b/tests/robot/perception/natnet_ros2/test_natnet_ros2.py @@ -1,152 +1,32 @@ -# Copyright 2026 AirLab CMU -# SPDX-License-Identifier: Apache-2.0 -"""Unit tests for natnet_ros2 Python source code. +# 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. -These tests import the actual production source files and stub out ROS at the -import boundary so no ROS installation is required. +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 -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. - Add C++ tests under robot/ros_ws/src/perception/natnet_ros2/test/ using ament_add_gtest. +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 -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 actual source directory (parents[4] = repo root). -_natnet_src = Path(__file__).resolve().parents[4] / "robot/ros_ws/src/perception/natnet_ros2/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) +_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/lidar_point_cloud_filter/test_validation_core.py b/tests/robot/sensors/lidar_point_cloud_filter/test_validation_core.py index 471860a37..e7babf9d4 100644 --- a/tests/robot/sensors/lidar_point_cloud_filter/test_validation_core.py +++ b/tests/robot/sensors/lidar_point_cloud_filter/test_validation_core.py @@ -1,66 +1,38 @@ -# Copyright 2026 AirLab CMU -# SPDX-License-Identifier: Apache-2.0 -"""Unit tests for ``validation_core`` (numpy-only).""" - -import numpy as np -import pytest - -from validation_core import ( - 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) +# 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 fdbebb97e..140e10c0a 100644 --- a/tests/sensor_probes.py +++ b/tests/sensor_probes.py @@ -50,8 +50,8 @@ ] LIDAR_CLOUD_VALIDATE_SCRIPT = ( - "/root/AirStack/tests/robot/sensors/lidar_point_cloud_filter/" - "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/motive_emulator/README.md b/tests/sim/motive_emulator/README.md index c991c4aa5..0e682c448 100644 --- a/tests/sim/motive_emulator/README.md +++ b/tests/sim/motive_emulator/README.md @@ -1,4 +1,61 @@ -# Motive / NatNet emulator tests +# Motive / NatNet Emulator -Add `@pytest.mark.unit` tests for the Motive emulator once it lives under -`simulation/` (or another sim path). +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/test_build_packages.py b/tests/system/test_build_packages.py index 40bcf978b..b91b78671 100644 --- a/tests/system/test_build_packages.py +++ b/tests/system/test_build_packages.py @@ -41,6 +41,39 @@ 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. + """ + 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, + "bash -ic 'colcon test --event-handlers console_direct+ " + "--return-code-on-test-failure'", + timeout=300, + ) + assert test.returncode == 0, f"colcon test failed:\n{read_log_tail()}" + finally: + airstack_cmd("down") + def test_colcon_build_gcs(self): _warn_if_prebuilt("gcs/ros_ws") try: From bd6a4e266b2009c513e467ce1b296db60992f08c Mon Sep 17 00:00:00 2001 From: John Date: Wed, 20 May 2026 16:57:35 -0400 Subject: [PATCH 15/21] reupdated documentation for current state of testing --- .agents/skills/run-system-tests/SKILL.md | 7 +++-- AGENTS.md | 2 +- .../lidar_point_cloud_filter/README.md | 4 +-- tests/README.md | 18 +++++++++++-- tests/robot/README.md | 27 ++++++++++++++++--- 5 files changed, 46 insertions(+), 12 deletions(-) diff --git a/.agents/skills/run-system-tests/SKILL.md b/.agents/skills/run-system-tests/SKILL.md index 53f93e842..4dd25d233 100644 --- a/.agents/skills/run-system-tests/SKILL.md +++ b/.agents/skills/run-system-tests/SKILL.md @@ -175,12 +175,11 @@ Total parametrize cardinality for sim tests = `len(sims) × len(num_robots) × s ## Running Tests via PR Comment -The `system-tests.yml` workflow accepts four trigger paths: +The `system-tests.yml` workflow accepts three trigger paths: 1. **PR opened** (same-repo only) — auto-runs pytest with conftest defaults. Fork PRs are skipped to keep arbitrary code off the self-hosted runner. -2. **PR approved by a reviewer** (same-repo only) — re-runs the full suite on the approved state of the branch, giving a final validation pass before merge. -3. **`/pytest` issue comment** on a PR — only honored from users with `OWNER`, `MEMBER`, or `COLLABORATOR` author association. Fork PRs are explicitly rejected by the `Resolve PR head` step (the PR's head repo must equal `${context.repo.owner}/${context.repo.repo}`). -4. **`workflow_dispatch`** — manual run from the Actions tab with form inputs (`marks`, `sim`, `num_robots`, `stress_iterations`, `stable_duration`, `baseline_run_id`). +2. **`/pytest` issue comment** on a PR — only honored from users with `OWNER`, `MEMBER`, or `COLLABORATOR` author association. Fork PRs are explicitly rejected by the `Resolve PR head` step (the PR's head repo must equal `${context.repo.owner}/${context.repo.repo}`). +3. **`workflow_dispatch`** — manual run from the Actions tab with form inputs (`marks`, `sim`, `num_robots`, `stress_iterations`, `stable_duration`, `baseline_run_id`). ### `/pytest` comment grammar diff --git a/AGENTS.md b/AGENTS.md index 5e9af81ae..d5a150d59 100644 --- a/AGENTS.md +++ b/AGENTS.md @@ -257,7 +257,7 @@ GitHub Actions workflows live in [`.github/workflows/`](.github/workflows/): | Workflow | Trigger | Purpose | |----------|---------|---------| -| [`system-tests.yml`](.github/workflows/system-tests.yml) | Push to `main`, PR opened, PR approved by reviewer, `/pytest` PR comment (write-access only), or `workflow_dispatch` | Runs the `tests/` suite on an ephemeral GPU runner; posts metrics report (with regression diff vs base branch / `main`) as a PR comment and to the job summary | +| [`system-tests.yml`](.github/workflows/system-tests.yml) | PR opened, `/pytest` PR comment (write-access only), or `workflow_dispatch` | Runs the `tests/` suite on an ephemeral GPU runner; posts metrics report (with regression diff vs base branch / `main`) as a PR comment and to the job summary | | [`docker-build.yml`](.github/workflows/docker-build.yml) | Push to `main`/`develop` that changes `.env` (`VERSION=`), or manual dispatch | Builds, pushes, and cosign-signs all compose images on the ephemeral runner | | [`check-version-increment.yml`](.github/workflows/check-version-increment.yml) | Pull request | Validates `.env` `VERSION=` is valid semver and strictly greater than the base branch | | `deploy_docs_from_{main,develop,release}.yaml` | Push to the matching branch (`docs/**`, `mkdocs.yml`, `*.md`) | Publishes versioned MkDocs site via `mike` | 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 26eb3db4e..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/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 **`tests/robot/sensors/lidar_point_cloud_filter/validation_core.py`**, covered by **`test_validation_core.py`** (`pytest -m unit`) and imported by **`tests/robot/sensors/lidar_point_cloud_filter/validate_lidar_filter_clouds.py`** at runtime. 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 `tests/robot/sensors/lidar_point_cloud_filter/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/tests/README.md b/tests/README.md index 839da0469..f10942ff7 100644 --- a/tests/README.md +++ b/tests/README.md @@ -26,7 +26,21 @@ Shared fixtures live in `tests/conftest.py`. Use `airstack test -m unit -v` for ### Unit tests (`tests/robot/`, `tests/sim/`) -Hermetic tests use `@pytest.mark.unit` (see [`pytest.ini`](pytest.ini)). Example: [`robot/sensors/lidar_point_cloud_filter/validation_core.py`](robot/sensors/lidar_point_cloud_filter/validation_core.py) + [`test_validation_core.py`](robot/sensors/lidar_point_cloud_filter/test_validation_core.py) (numpy-only rules also used by `validate_lidar_filter_clouds.py`, which is a sibling in the same directory). +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 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). @@ -66,7 +80,7 @@ and are driven by [`system/test_sensors.py`](system/test_sensors.py): | 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`` | ``tests/robot/sensors/lidar_point_cloud_filter/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 diff --git a/tests/robot/README.md b/tests/robot/README.md index abdf942b7..a4409c4b1 100644 --- a/tests/robot/README.md +++ b/tests/robot/README.md @@ -1,4 +1,4 @@ -# Robot-side unit tests +# Robot-side unit test proxies Layout mirrors [`robot/ros_ws/src/`](../../robot/ros_ws/src/) autonomy layers: @@ -11,6 +11,27 @@ Layout mirrors [`robot/ros_ws/src/`](../../robot/ros_ws/src/) autonomy layers: | `perception/` | `robot/ros_ws/src/perception/` | | `sensors/` | `robot/ros_ws/src/sensors/` | -Tests here are marked `@pytest.mark.unit` unless they need Docker or a live sim (use `tests/system/` or `tests/sim/` instead). +## Design: co-location + proxy -Keep **test-only** Python (fixtures, pure numeric checks, protocol stubs) under `tests/` — for example `tests/robot/sensors/lidar_point_cloud_filter/validation_core.py`. `tests/conftest.py` extends `sys.path` where needed so `pytest` can import those modules. +**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. From 5377171fd9f4a791fe8cd7cb3a67f4fcc79a5941 Mon Sep 17 00:00:00 2001 From: John Date: Wed, 20 May 2026 17:09:19 -0400 Subject: [PATCH 16/21] change unit tests to occur with system tests so that environment is builtgit status --- .agents/skills/add-unit-tests/SKILL.md | 14 ++--- .agents/skills/run-system-tests/SKILL.md | 2 +- .github/workflows/unit-tests.yml | 54 ------------------- AGENTS.md | 2 +- .../development/intermediate/testing/index.md | 5 +- .../intermediate/testing/unit_testing.md | 20 ++++--- tests/conftest.py | 19 ++++++- 7 files changed, 38 insertions(+), 78 deletions(-) delete mode 100644 .github/workflows/unit-tests.yml diff --git a/.agents/skills/add-unit-tests/SKILL.md b/.agents/skills/add-unit-tests/SKILL.md index e32080ff4..30f58fd2e 100644 --- a/.agents/skills/add-unit-tests/SKILL.md +++ b/.agents/skills/add-unit-tests/SKILL.md @@ -1,6 +1,6 @@ --- 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, the CI workflow that gates every push, and how to extend the pattern to sim and GCS modules. +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 @@ -45,7 +45,7 @@ and re-exports every `test_*` function. This means: |---|---| | `pytest tests/ -m unit` | Proxy in `tests/robot/` → loads real test from package | | `airstack test -m unit` | Same path | -| CI `unit-tests.yml` push/PR check | Same path — fast, no Docker | +| 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 @@ -187,8 +187,8 @@ robot///test_.py::test_my_function_basic ### 6. CI picks it up automatically -`unit-tests.yml` (`.github/workflows/unit-tests.yml`) runs `pytest tests/ -m unit` -on every push and PR — no changes to CI needed. +Unit tests are discovered by `pytest tests/` and run as part of `system-tests.yml` +(triggered on PR open) — no changes to CI needed. --- @@ -279,8 +279,8 @@ pytest.ini or CI changes needed. | 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? | `.github/workflows/unit-tests.yml` — `ubuntu-latest`, ~10 s cold start | -| When does that workflow trigger? | Every push and every PR | +| 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 | @@ -298,7 +298,7 @@ Corresponding proxies: `tests/robot/perception/natnet_ros2/test_natnet_ros2.py`, ## Files to Know -- `.github/workflows/unit-tests.yml` — CI gate on every push/PR +- `.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) diff --git a/.agents/skills/run-system-tests/SKILL.md b/.agents/skills/run-system-tests/SKILL.md index 4dd25d233..453bbc953 100644 --- a/.agents/skills/run-system-tests/SKILL.md +++ b/.agents/skills/run-system-tests/SKILL.md @@ -32,7 +32,7 @@ The suite lives at `tests/` (repo root) and is fully pytest-based. Configuration | Concern | Unit (`-m unit`) | System (`-m liveliness` etc.) | |---|---|---| | Hardware required | None — pure Python | Docker daemon, NVIDIA GPU, sim license | -| CI workflow | `unit-tests.yml` (`ubuntu-latest`, ~10 s) | `system-tests.yml` (GPU OpenStack VM) | +| 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 | diff --git a/.github/workflows/unit-tests.yml b/.github/workflows/unit-tests.yml deleted file mode 100644 index 2b6fd7130..000000000 --- a/.github/workflows/unit-tests.yml +++ /dev/null @@ -1,54 +0,0 @@ -name: Unit Tests - -# Fast, hermetic unit tests — pure Python, no Docker, no GPU. -# Runs on every push and every PR so feedback arrives in seconds, -# not after the GPU runner provisions. -# -# These tests mirror the robot/ros_ws/src/ layer layout: -# tests/robot/perception/… proxy → robot/ros_ws/src/perception/…/test/ -# tests/robot/sensors/… proxy → robot/ros_ws/src/sensors/…/test/ -# tests/sim/… proxy → simulation/…/test/ (future) -# tests/gcs/… proxy → gcs/…/test/ (future) - -on: - push: - branches: - - '**' - pull_request: - -jobs: - unit-tests: - name: Unit Tests (pytest -m unit) - runs-on: ubuntu-latest - - steps: - - name: Checkout - uses: actions/checkout@v4 - - - name: Set up Python - uses: actions/setup-python@v5 - with: - python-version: '3.12' - cache: pip - cache-dependency-path: tests/requirements.txt - - - name: Install dependencies - run: pip install pytest numpy - - - name: Run unit tests - env: - AIRSTACK_ROOT: ${{ github.workspace }} - run: | - pytest tests/ -m unit \ - -v \ - --log-cli-level=INFO \ - --log-cli-format='%(asctime)s [%(levelname)s] %(message)s' \ - --log-cli-date-format='%H:%M:%S' - - - name: Upload test results - if: always() - uses: actions/upload-artifact@v4 - with: - name: unit-test-results-${{ github.sha }} - path: tests/results/ - retention-days: 30 diff --git a/AGENTS.md b/AGENTS.md index d5a150d59..b53069e75 100644 --- a/AGENTS.md +++ b/AGENTS.md @@ -202,7 +202,7 @@ docker exec airstack-robot-desktop-1 bash -c "ros2 topic echo --onc 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/` (the CI command) discovers them. A dedicated CI workflow (`.github/workflows/unit-tests.yml`) runs these on every push and PR on a GitHub-hosted runner — no Docker or GPU needed. Example: `airstack test -m unit -v`. See `add-unit-tests` skill. +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 diff --git a/docs/development/intermediate/testing/index.md b/docs/development/intermediate/testing/index.md index d14fc753d..0769e2b09 100644 --- a/docs/development/intermediate/testing/index.md +++ b/docs/development/intermediate/testing/index.md @@ -21,9 +21,8 @@ airstack test -m unit -v pytest tests/ -m unit -v ``` -A dedicated CI workflow (`.github/workflows/unit-tests.yml`) runs these on every -push and PR on a GitHub-hosted `ubuntu-latest` runner — fast mandatory feedback -before the heavier GPU-based system tests are needed. +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). diff --git a/docs/development/intermediate/testing/unit_testing.md b/docs/development/intermediate/testing/unit_testing.md index 580d6e8d8..f69056008 100644 --- a/docs/development/intermediate/testing/unit_testing.md +++ b/docs/development/intermediate/testing/unit_testing.md @@ -49,20 +49,18 @@ pytest tests/ -m unit -v Unit tests complete in under one second for the current suite. -## CI workflow +## CI -`.github/workflows/unit-tests.yml` runs on **every push and every pull request** on a GitHub-hosted `ubuntu-latest` runner. Cold-start time is ~10 seconds; the test run itself is under 1 second. This provides fast mandatory feedback before the heavier GPU-based system tests are needed. +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: -```yaml -# Simplified — see .github/workflows/unit-tests.yml for the full file -- run: pip install pytest numpy -- run: pytest tests/ -m unit -v - env: - AIRSTACK_ROOT: ${{ github.workspace }} +```bash +airstack test -m unit -v +# or directly (requires tests/requirements.txt installed): +AIRSTACK_ROOT=$(pwd) pytest tests/ -m unit -v ``` -Results are uploaded as artifacts (`unit-test-results-`) with 30-day retention. - ## Current test coverage | Package | Test file | What is covered | @@ -198,7 +196,7 @@ tests/gcs//test_.py ← proxy ``` `pytest tests/ -m unit` discovers them automatically — no changes to `pytest.ini` -or `unit-tests.yml` needed. +or CI changes needed. ## See also diff --git a/tests/conftest.py b/tests/conftest.py index 9b4964a1a..c3376893a 100644 --- a/tests/conftest.py +++ b/tests/conftest.py @@ -161,6 +161,11 @@ def pytest_generate_tests(metafunc): # docker image builds → colcon workspace builds → liveliness (infra) → sensors # (ROS topic streams) → autonomy flight tests. _MODULE_ORDER = [ + # 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", @@ -182,10 +187,22 @@ 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 From ef95968bfd365f2f50bedc3020f78d6b5ae76985 Mon Sep 17 00:00:00 2001 From: John Date: Wed, 20 May 2026 17:17:04 -0400 Subject: [PATCH 17/21] generalizes natnet parameters and disables natnet automatically for launch --- .../src/perception/natnet_ros2/config/natnet_config.yaml | 9 ++++++--- .../perception_bringup/launch/perception.launch.xml | 2 +- 2 files changed, 7 insertions(+), 4 deletions(-) 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 index 250a81a62..69fc11d1c 100644 --- a/robot/ros_ws/src/perception/natnet_ros2/config/natnet_config.yaml +++ b/robot/ros_ws/src/perception/natnet_ros2/config/natnet_config.yaml @@ -6,7 +6,9 @@ /**: ros__parameters: - server_ip: "192.168.0.77" + # 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" @@ -28,8 +30,9 @@ # OptiTrack default is 239.255.42.99. multicast_address: "239.255.42.99" - body_name: "HummingbirdDrone" - body_id: 3 + # 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 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 c187c8bf0..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 @@ -3,7 +3,7 @@ - + From 26119bc1fb9be7b7170c2964580bfd9a517f8f11 Mon Sep 17 00:00:00 2001 From: John Date: Wed, 20 May 2026 21:34:44 -0400 Subject: [PATCH 18/21] natnet client adaptor now references correct error code from NatNet SDK 4.4.0.0 --- .../src/perception/natnet_ros2/src/natnet_client_adapter.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) 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 index e4ae2a7b7..1cd30286b 100644 --- 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 @@ -71,13 +71,14 @@ void NATNET_CALLCONV sdk_frame_callback(sFrameOfMocapData * data, void * /*ctx*/ } /// 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; - case ErrorCode_Timeout: return NatNetResult::Timeout; default: return NatNetResult::InternalError; } } From f27911e4a9a9d8c24519f1d0be44a5acc2a6a953 Mon Sep 17 00:00:00 2001 From: John Date: Wed, 20 May 2026 21:50:53 -0400 Subject: [PATCH 19/21] increment version tag --- .env | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.env b/.env index 079796374..c76897158 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.18.0" +VERSION="0.18.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. From 08040b72e7bed45c137dc410582b554e2efeca5b Mon Sep 17 00:00:00 2001 From: John Date: Wed, 20 May 2026 23:02:24 -0400 Subject: [PATCH 20/21] bug fixes to natnet launching from env file --- robot/docker/docker-compose.yaml | 3 +++ .../src/perception/natnet_ros2/CMakeLists.txt | 17 ++++++++------- .../env-hooks/natnet_library_path.dsv.in | 1 + .../natnet_ros2/launch/natnet_ros2.launch.py | 21 ++++++++++--------- 4 files changed, 24 insertions(+), 18 deletions(-) create mode 100644 robot/ros_ws/src/perception/natnet_ros2/env-hooks/natnet_library_path.dsv.in diff --git a/robot/docker/docker-compose.yaml b/robot/docker/docker-compose.yaml index fdd6458da..e0f13b996 100644 --- a/robot/docker/docker-compose.yaml +++ b/robot/docker/docker-compose.yaml @@ -27,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: > @@ -126,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; @@ -201,6 +203,7 @@ services: - 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 diff --git a/robot/ros_ws/src/perception/natnet_ros2/CMakeLists.txt b/robot/ros_ws/src/perception/natnet_ros2/CMakeLists.txt index aa07e8024..ff47a00da 100644 --- a/robot/ros_ws/src/perception/natnet_ros2/CMakeLists.txt +++ b/robot/ros_ws/src/perception/natnet_ros2/CMakeLists.txt @@ -43,18 +43,19 @@ if(EXISTS "${_NATNET_LIB}" AND EXISTS "${_NATNET_INC}") ament_target_dependencies(natnet_ros2_node rclcpp geometry_msgs nav_msgs) target_link_libraries(natnet_ros2_node NatNet) - # $ORIGIN RPATH so the node finds libNatNet.so in the install directory - # without needing LD_LIBRARY_PATH at runtime. - set_target_properties(natnet_ros2_node PROPERTIES - INSTALL_RPATH "$ORIGIN" - BUILD_WITH_INSTALL_RPATH TRUE - ) - install(TARGETS natnet_ros2_node DESTINATION lib/${PROJECT_NAME}) - install(FILES "${_NATNET_LIB}" + # 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" 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/launch/natnet_ros2.launch.py b/robot/ros_ws/src/perception/natnet_ros2/launch/natnet_ros2.launch.py index aa631947a..cb7c178d8 100644 --- 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 @@ -2,8 +2,9 @@ """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), the executable will -be absent and this launch file skips it with a warning rather than crashing. +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 @@ -56,17 +57,17 @@ def launch_setup(context, *_args, **_kwargs): publish_mavros = bool(ros_params.get('publish_to_mavros', False)) body_name = str(ros_params.get('body_name', 'robot_1')) - # natnet_ros2_node is only built when the NatNet SDK is present. - # Skip gracefully rather than crashing if the SDK was not installed. + # 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 / 'natnet_ros2' / 'natnet_ros2_node' + node_path = Path(pkg_share).parent.parent / 'lib' / 'natnet_ros2' / 'natnet_ros2_node' if not node_path.exists(): - import logging - logging.getLogger('natnet_ros2.launch').warning( - 'natnet_ros2_node executable not found — NatNet SDK not installed. ' - "Run 'airstack setup' to enable OptiTrack support." + 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.' ) - return [] actions = [ Node( From 6debb778d3af62455d805166158090e339b526fd Mon Sep 17 00:00:00 2001 From: John Date: Thu, 21 May 2026 02:35:27 -0400 Subject: [PATCH 21/21] fixed failing systems test due to depends issue and specifying unit tests via yaml --- .../assets/package_template/setup.py | 4 +- .agents/skills/add-unit-tests/SKILL.md | 5 ++- .../msgs/airstack_msgs/package.xml | 3 +- .../ros_packages/msgs/task_msgs/package.xml | 3 +- .../controls/pid_controller_msgs/package.xml | 3 +- .../lidar_point_cloud_filter/setup.cfg | 8 ++++ .../sensors/lidar_point_cloud_filter/setup.py | 4 +- .../src/sensors/sensor_interfaces/package.xml | 5 ++- tests/colcon_unit_test_packages.yaml | 13 +++++++ tests/conftest.py | 38 +++++++++++++++++++ tests/requirements.txt | 1 + tests/system/test_build_packages.py | 14 +++++-- 12 files changed, 89 insertions(+), 12 deletions(-) create mode 100644 tests/colcon_unit_test_packages.yaml 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 index 30f58fd2e..7d1d3b582 100644 --- a/.agents/skills/add-unit-tests/SKILL.md +++ b/.agents/skills/add-unit-tests/SKILL.md @@ -246,7 +246,10 @@ docker exec airstack-robot-desktop-1 bash -c \ ``` 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. +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/`. --- 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/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/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/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/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 c3376893a..31fd29076 100644 --- a/tests/conftest.py +++ b/tests/conftest.py @@ -13,6 +13,7 @@ from pathlib import Path import pytest +import yaml SIM_CONFIG = { "msairsim": { @@ -45,6 +46,43 @@ } 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 diff --git a/tests/requirements.txt b/tests/requirements.txt index 7f914bd46..a4b43e6bb 100644 --- a/tests/requirements.txt +++ b/tests/requirements.txt @@ -1,6 +1,7 @@ pytest pytest-timeout pytest-dependency +pyyaml tabulate psutil pandas diff --git a/tests/system/test_build_packages.py b/tests/system/test_build_packages.py index b91b78671..5c54cfee3 100644 --- a/tests/system/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) @@ -47,7 +48,11 @@ def test_colcon_test_robot(self): 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": ""}, @@ -66,11 +71,12 @@ def test_colcon_test_robot(self): test = docker_exec( container, - "bash -ic 'colcon test --event-handlers console_direct+ " - "--return-code-on-test-failure'", + f"bash -ic '{colcon_test_robot_command('robot')}'", timeout=300, ) - assert test.returncode == 0, f"colcon test failed:\n{read_log_tail()}" + assert test.returncode == 0, ( + f"colcon test failed (packages: {', '.join(packages)}):\n{read_log_tail()}" + ) finally: airstack_cmd("down")