castacks · andrewjong · Aug 4, 2026 · Aug 4, 2026 · Aug 4, 2026 · Aug 4, 2026
diff --git a/.env b/.env
@@ -12,7 +12,7 @@ PROJECT_NAME="airstack"
 # If you've run ./airstack.sh setup, then this will auto-generate from the git commit hash every time a change is made 
 # to a Dockerfile or docker-compose.yaml file. Otherwise this can also be set explicitly to make a release version.
 # auto-generated from git commit hash
-VERSION="0.19.0-alpha.8"
+VERSION="0.19.0-alpha.9"
 # Choose "dev" or "prebuilt". "dev" is for mounted code that must be built live. "prebuilt" is for built ros_ws baked into the image
 DOCKER_IMAGE_BUILD_MODE="dev"  
 # Where to push and pull images from. Can replace with your docker hub username if using docker hub.

diff --git a/AGENTS.md b/AGENTS.md
@@ -222,6 +222,7 @@ Pytest-based system tests live under [`tests/system/`](tests/system/). They brin
 | [`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 |
 | [`tests/system/test_fixed_trajectory.py`](tests/system/test_fixed_trajectory.py) | `autonomy` | 4-phase flight chain (PX4 ready → takeoff → execute Circle/Figure8/Racetrack/Line trajectory → land) per (sim, num_robots, iter, trajectory_type); records cross-track error and path RMSE | Docker, GPU, sim license |
+| [`tests/system/test_waypoint_flight.py`](tests/system/test_waypoint_flight.py) | `waypoint_flight` | 4-phase flight chain (PX4 ready → takeoff → NavigateTask waypoint route → land) per (sim, num_robots, iter); pass/fail judged on the odometry track by the standalone [`tests/waypoint_checker.py`](tests/waypoint_checker.py) (in-order corridor arrival within `--waypoint-tolerance`, final goal within `--goal-tolerance`, per-waypoint `--waypoint-timeout`) | Docker, GPU, sim license |
 
 The pytest hooks and the `airstack_env` / `robot_autonomy_stack` fixtures live in [`tests/conftest.py`](tests/conftest.py); the shared helpers are split by concern into the [`tests/harness/`](tests/harness/) package (`session`, `discovery`, `commands`, `containers`, `metrics` (with `MetricsRecorder`), `sim`, `collection`) and re-exported through `conftest`, so `from conftest import <name>` still resolves. Each run produces a timestamped directory under `tests/results/<timestamp>/` with `summary.txt`, `results.xml`, and `metrics.json` (no per-test log files — live output streams to the terminal via `log_cli`). [`tests/parse_metrics.py`](tests/parse_metrics.py) generates a markdown report (single-run or diff-vs-baseline; exits 1 on regression).
 

diff --git a/CHANGELOG.md b/CHANGELOG.md
@@ -14,6 +14,7 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0
 - `ros-${ROS_DISTRO}-mavros-extras` in the robot image (provides the vision_pose plugin used for external-pose deployments)
 - `overrides/l4t-px4-realrobot.env` — site-agnostic deployment override for a single real PX4 robot on a Jetson (aarch64/l4t)
 - `integration` test tier (`tests/integration/`, `integration` mark) with a shared `robot_autonomy_stack` fixture (robot container, no sim/GPU)
+- `waypoint_flight` system test (`tests/system/test_waypoint_flight.py`): takeoff → ordered waypoint route via `NavigateTask` (dispatched as a dense plan) → land, judged on the odometry track by the standalone stdlib-only `tests/waypoint_checker.py` (in-order corridor arrival within `--waypoint-tolerance`, final goal within `--goal-tolerance`, per-waypoint `--waypoint-timeout`); validated end-to-end in Isaac Sim; serves as the standard acceptance check after integrating or swapping a planner module
 
 ### Changed
 

diff --git a/tests/README.md b/tests/README.md
@@ -24,6 +24,7 @@ Pytest hooks and the shared fixtures live in `tests/conftest.py`; reusable helpe
 | [`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 |
 | [`system/test_fixed_trajectory.py`](system/test_fixed_trajectory.py) | `autonomy` | Fixed-pattern trajectory evaluation: takeoff, execute a trajectory (Circle, Figure8, Racetrack, Line), record path deviation metrics, land — one chain per (sim, num_robots, iteration, trajectory_type) | Docker daemon, GPU, sim license |
+| [`system/test_waypoint_flight.py`](system/test_waypoint_flight.py) | `waypoint_flight` | Ordered-waypoint navigation: takeoff, send a waypoint route to `NavigateTask`, judge the odometry track with the standalone [`waypoint_checker.py`](waypoint_checker.py), land — one chain per (sim, num_robots, iteration) | Docker daemon, GPU, sim license |
 
 ### Unit tests (co-located)
 
@@ -55,7 +56,7 @@ container or brings one up automatically (like `build_packages`), then tears it
 Collection order runs integration after `build_packages` and before the sim tiers.
 
 Marks can be combined with pytest logic:
-`-m unit`, `-m "build_docker or build_packages"`, `-m integration`, `-m liveliness`, `-m sensors`, `-m takeoff_hover_land`, `-m autonomy`, or e.g. `-m "liveliness or sensors"` (see **Bring-up scope** below).
+`-m unit`, `-m "build_docker or build_packages"`, `-m integration`, `-m liveliness`, `-m sensors`, `-m takeoff_hover_land`, `-m autonomy`, `-m waypoint_flight`, or e.g. `-m "liveliness or sensors"` (see **Bring-up scope** below).
 
 ### Bring-up scope (`airstack_env`)
 
@@ -373,6 +374,124 @@ airstack test -m autonomy \
 
 ---
 
+## Waypoint Flight Tests (`system/test_waypoint_flight.py`)
+
+`TestWaypointFlight` runs a **4-phase flight chain** per `(sim, num_robots,
+iteration)`: after takeoff it sends an ordered waypoint route to the local
+planner's `NavigateTask` action (`/robot_N/tasks/navigate`) as a **dense**
+`nav_msgs/Path` (interpolated at 1 m from the current pose through the
+waypoints, mirroring real global-planner output), captures odometry
+throughout, then lands.
+
+| Isaac Sim | ms-airsim (Blocks) |
+| --------- | ------------------ |
+| ![Isaac Sim waypoint flight](assets/waypoint_flight_isaac.jpg) | ![ms-airsim Blocks waypoint flight](assets/waypoint_flight_msairsim_blocks.jpg) |
+
+![Foxglove during a waypoint flight](assets/waypoint_flight_foxglove.png)
+*Foxglove (GCS dashboard) during the route: planned path and expanded
+obstacle voxels in the 3D panel, Robot Tasks panel, live stereo feed.*
+
+Pass/fail is judged by the standalone
+[`waypoint_checker.py`](waypoint_checker.py): the odometry track must pass
+within `--waypoint-tolerance` of **every waypoint in order**, each within
+`--waypoint-timeout` seconds (odometry clock) of the previous arrival, and
+additionally end within `--goal-tolerance` of the final waypoint. The
+criterion is defined purely on the odometry track — not the action result —
+so swapping the global or local planner leaves the judgment unchanged. This
+makes the test the standard acceptance check after integrating or swapping a
+planner module.
+
+Waypoints are specified **relative to the robot pose at dispatch** (x forward
+along the initial heading, z up from dispatch altitude), so routes are
+spawn-point and simulator agnostic. The default route is an open 30 m square
+flown 10 m above takeoff altitude (~20 m AGL) so it clears scene clutter in
+both default scenes (Isaac open plane, AirSim Blocks) — this test judges
+route-following, not obstacle avoidance.
+
+**Tolerance calibration** (validated against stock Isaac Sim flight): the
+stack's navigation contract is *reach the goal precisely, follow the route
+corridor loosely*. Stock `droan_gl` scores candidate trajectories with
+`cost = deviation - path_distance`, which cuts corners (~4–7 m observed), so
+the intermediate tolerance is loose (15 m) while the final goal is tight
+(2.5 m = NavigateTask's 1.5 m goal tolerance + tracking lag). `NavigateTask`
+succeeds on the **tracking point**, which leads the drone by up to the
+look-ahead distance, so the test keeps capturing after the action returns
+until the drone is stationary (max 30 s). Two route-design rules follow:
+routes must **end away from the start** (the action succeeds instantly on a
+closed loop), and legs should be **≥ 2× the intermediate tolerance** so the
+corridor check can discriminate route-following from goal-beelining.
+
+### Phase order
+
+| Phase | Test | What happens |
+| ----- | ---- | ------------ |
+| 1 | `test_px4_ready` | Waits for MAVROS connected + odometry publishing; per env |
+| 2 | `test_takeoff` | Takeoff to 10 m at 1 m/s; asserts altitude within 10 % |
+| 3 | `test_waypoint_route` | Sends `NavigateTask`; captures odom; asserts checker verdict |
+| 4 | `test_landing` | Sends `LandTask`; asserts final altitude < 0.5 m |
+
+A `test_waypoint_route` failure does **not** poison the chain — `test_landing`
+always runs so the drone returns to the ground.
+
+### Recorded metrics
+
+| Metric key | Unit | Description |
+| ---------- | ---- | ----------- |
+| `ready_duration_sys_s` | s | Wall-clock time from test start until PX4 ready |
+| `waypoint_success` | — | 1.0 if the checker passed the whole route |
+| `waypoints_reached` | — | Waypoints reached in order (`higher_is_better`) |
+| `navigate_action_success` | — | 1.0 if the action returned `success: true` |
+| `route_time_sim_s` | s | Odometry-clock time over the captured route |
+| `worst_closest_approach_m` | m | Largest closest-approach distance over all waypoints |
+| `final_goal_error_m` | m | Closest approach to the final waypoint (asserted ≤ `--goal-tolerance`) |
+
+### The standalone checker
+
+[`waypoint_checker.py`](waypoint_checker.py) is stdlib-only and judges any
+odometry CSV against a route, independent of the AirStack harness — useful
+for judging waypoint flight on other ROS 2 systems or in agent-evaluation
+settings:
+
+```bash
+ros2 topic echo --csv /robot_1/interface/mavros/local_position/odom > odom.csv
+python3 tests/waypoint_checker.py --odom-csv odom.csv \
+  --waypoints "10,0,10; 10,10,10; 0,10,10" --tolerance 1.5 --budget 120
+```
+
+It prints a JSON verdict (per-waypoint reached/closest-approach/elapsed) and
+exits 0 on pass, 1 on fail. Note the CLI takes waypoints in the **odometry
+frame** (the pytest wrapper does the relative-to-world transform).
+
+### Running waypoint flight tests
+
+```bash
+# Default 10 m square route; ms-airsim; 1 robot
+airstack test -m waypoint_flight \
+  --sim msairsim \
+  --num-robots 1 \
+  --stress-iterations 1 \
+  -v
+
+# Custom route with an altitude change, Isaac Sim
+airstack test -m waypoint_flight \
+  --sim isaacsim \
+  --num-robots 1 \
+  --waypoints "30,0,0; 30,30,5; 0,30,5" \
+  --goal-tolerance 2.0 \
+  -v
+```
+
+### CLI option reference (waypoint-specific)
+
+| Option | Default | Description |
+| ------ | ------- | ----------- |
+| `--waypoints` | `30,0,10; 30,30,10; 0,30,10` | Ordered route `x,y,z; ...` relative to dispatch pose; must end away from start |
+| `--waypoint-tolerance` | `15` | Pass distance (m) to each intermediate waypoint (corridor check) |
+| `--goal-tolerance` | `2.5` | Pass distance (m) to the final waypoint |
+| `--waypoint-timeout` | `120` | Per-waypoint time budget (s, odometry clock) |
+
+---
+
 ## Metrics Reporting (`parse_metrics.py`)
 
 [`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:

diff --git a/tests/assets/waypoint_flight_foxglove.png b/tests/assets/waypoint_flight_foxglove.png
diff --git a/tests/assets/waypoint_flight_isaac.jpg b/tests/assets/waypoint_flight_isaac.jpg
diff --git a/tests/assets/waypoint_flight_msairsim_blocks.jpg b/tests/assets/waypoint_flight_msairsim_blocks.jpg
diff --git a/tests/conftest.py b/tests/conftest.py
@@ -40,6 +40,33 @@ def pytest_addoption(parser):
     parser.addoption("--trajectory-types", default="Circle,Figure8,Racetrack,Line",
                      help="Comma-separated fixed trajectory types to sweep in "
                           "test_fixed_trajectory. Default: Circle,Figure8,Racetrack,Line")
+    parser.addoption("--waypoints", default="30,0,10; 30,30,10; 0,30,10",
+                     help="Ordered waypoint route for test_waypoint_flight as "
+                          "'x,y,z; x,y,z; ...', relative to the robot pose at "
+                          "dispatch (x forward along heading, z up). The route "
+                          "must END AWAY from the start: NavigateTask succeeds "
+                          "when the robot is within tolerance of the FINAL "
+                          "pose, so a closed loop succeeds instantly without "
+                          "flying. Legs should be >= 2x --waypoint-tolerance "
+                          "or the corridor check cannot discriminate "
+                          "route-following from goal-beelining. Default: open "
+                          "30 m square (3 corners) climbing 10 m above "
+                          "takeoff altitude, so the route clears scene "
+                          "clutter (e.g. AirSim Blocks) — this test judges "
+                          "route-following, not obstacle avoidance.")
+    parser.addoption("--waypoint-tolerance", default="15",
+                     help="Pass distance (m) to each intermediate waypoint in "
+                          "test_waypoint_flight. Calibrated to stock droan_gl "
+                          "plan-following, which trades deviation for path "
+                          "progress 1:1 and cuts corners deeply (7-10 m "
+                          "observed in Isaac). Default: 15")
+    parser.addoption("--goal-tolerance", default="2.5",
+                     help="Pass distance (m) to the FINAL waypoint in "
+                          "test_waypoint_flight: NavigateTask goal tolerance "
+                          "(1.5 m) plus tracking-point lag margin. Default: 2.5")
+    parser.addoption("--waypoint-timeout", default="120",
+                     help="Per-waypoint time budget (s, odometry clock) in "
+                          "test_waypoint_flight. Default: 120")
 
 
 def pytest_configure(config):

diff --git a/tests/pytest.ini b/tests/pytest.ini
@@ -8,6 +8,7 @@ markers =
     sensors: Sim and robot sensor topic rates, LiDAR validation, sim RTF
     takeoff_hover_land: End-to-end takeoff / hover / land action tests
     autonomy: Fixed-pattern trajectory path-tracker benchmark (test_fixed_trajectory.py)
+    waypoint_flight: Ordered-waypoint navigation judged on the odometry track (test_waypoint_flight.py)
 testpaths = .
 addopts = -v --durations=0 --import-mode=importlib
 cache_dir = /tmp/.pytest_cache