diff --git a/.env b/.env index 00cd4c639..c9527e1f8 100644 --- a/.env +++ b/.env @@ -12,7 +12,7 @@ PROJECT_NAME="airstack" # If you've run ./airstack.sh setup, then this will auto-generate from the git commit hash every time a change is made # to a Dockerfile or docker-compose.yaml file. Otherwise this can also be set explicitly to make a release version. # auto-generated from git commit hash -VERSION="0.19.0-alpha.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 index 884fc1e0a..1579006ec 100644 --- 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 ` still resolves. Each run produces a timestamped directory under `tests/results//` 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 index a5ea32f38..43940f0b7 100644 --- 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 index 93a07dda8..6172d1e98 100644 --- 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 new file mode 100644 index 000000000..a3812dcb5 Binary files /dev/null and b/tests/assets/waypoint_flight_foxglove.png differ diff --git a/tests/assets/waypoint_flight_isaac.jpg b/tests/assets/waypoint_flight_isaac.jpg new file mode 100644 index 000000000..16602d3ce Binary files /dev/null and b/tests/assets/waypoint_flight_isaac.jpg differ diff --git a/tests/assets/waypoint_flight_msairsim_blocks.jpg b/tests/assets/waypoint_flight_msairsim_blocks.jpg new file mode 100644 index 000000000..935b0722b Binary files /dev/null and b/tests/assets/waypoint_flight_msairsim_blocks.jpg differ diff --git a/tests/conftest.py b/tests/conftest.py index 47a668e65..62644eb98 100644 --- 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 index dc8c939de..6f4547c0c 100644 --- 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 diff --git a/tests/system/test_waypoint_flight.py b/tests/system/test_waypoint_flight.py new file mode 100644 index 000000000..1ad501b4b --- /dev/null +++ b/tests/system/test_waypoint_flight.py @@ -0,0 +1,372 @@ +"""Ordered-waypoint navigation flight tests. + +Per (sim, num_robots, iter): ready → takeoff → navigate waypoint route → land. + +After takeoff the test sends the route to the local planner's NavigateTask +action (``/robot_N/tasks/navigate``) as a dense ``nav_msgs/Path`` and +captures odometry throughout. Pass/fail is judged by the standalone +``tests/waypoint_checker.py``: the odometry track must pass within +``--waypoint-tolerance`` of every waypoint **in order**, each within +``--waypoint-timeout`` seconds of the previous arrival, and additionally +end within ``--goal-tolerance`` of the final waypoint. Judging on the +odometry track (rather than the action result) keeps the success criterion +independent of any particular planner implementation — swap the global or +local planner and the same test still judges the flight. + +Tolerance calibration: 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 deeply (7-10 m observed in Isaac). Hence the loose default +intermediate tolerance (15 m) with a tight final goal tolerance +(2.5 m = NavigateTask's 1.5 m + tracking-point lag; capture continues +after action success until the drone is stationary, since the action +succeeds on the tracking point, which leads the drone by up to the +look-ahead distance). Route legs must be >= 2x the intermediate +tolerance for the corridor check to discriminate route-following from +goal-beelining. + +Waypoints are given relative to the robot's pose at dispatch (x forward +along initial heading, z up from dispatch altitude) so routes are +spawn-point and sim agnostic. + +Route design constraint: NavigateTask declares success when the tracking +point is within goal_tolerance_m of the route's FINAL pose, so a route +that closes back on its start "succeeds" instantly without flying. +Routes must end away from the start; the default is an open square. +""" + +import math +import time + +import pytest + +from conftest import ( + current_test_id, + get_metrics, + get_robot_containers, + logger, + ros2_exec, +) +from system.test_fixed_trajectory import ( + ODOM_SCHEMA, + TARGET_ALTITUDE_M, + _action_message, + _action_ok, + _finish_captures, + _landing_one_robot, + _quat_to_yaw, + _run_parallel, + _stamp, + _start_captures, + _takeoff_one_robot, + _transform_to_world, +) +from waypoint_checker import check_track, parse_waypoints + +PX4_READY_TIMEOUT_S = 300.0 +NAVIGATE_GOAL_TOLERANCE_M = 1.5 # goal_tolerance_m sent in the NavigateTask goal +# The action succeeds on the TRACKING POINT (controller reference), which leads +# the drone by up to the look-ahead distance (~10 m observed) — keep capturing +# after the action returns until the drone itself stops moving. +MAX_SETTLE_S = 30.0 +SETTLE_POLL_S = 2.0 +SETTLE_STATIONARY_M = 0.3 + +METRIC_UNITS = { + "waypoint_success": "", + "waypoints_reached": "", + "navigate_action_success": "", + "route_time_sim_s": "s", + "ready_duration_sys_s": "s", + # Everything else defaults to "m". +} + + +def _record(robot_n: int, metrics_dict: dict) -> None: + """Record per-robot scalar metrics; unit inferred from METRIC_UNITS.""" + m = get_metrics() + tid = current_test_id() + higher = {"waypoint_success", "waypoints_reached", "navigate_action_success"} + for key, value in metrics_dict.items(): + if value is None: + continue + unit = METRIC_UNITS.get(key, "m") + direction = "higher_is_better" if key in higher else "lower_is_better" + m.record(tid, f"robot_{robot_n}.{key}", value, unit=unit, direction=direction) + + +PLAN_POINT_SPACING_M = 1.0 + + +def _densify(pts: list[tuple[float, float, float]], + spacing: float = PLAN_POINT_SPACING_M) -> list[tuple[float, float, float]]: + """Linearly interpolate between consecutive points at ~spacing intervals. + + The local planner walks the global plan by distance with a look-ahead; + sparse poses (e.g. 10 m apart) get skipped over and corners are cut, so + the dispatched plan must be dense like a real global planner's output. + """ + dense = [pts[0]] + for (ax, ay, az), (bx, by, bz) in zip(pts, pts[1:]): + d = math.sqrt((bx - ax) ** 2 + (by - ay) ** 2 + (bz - az) ** 2) + steps = max(1, math.ceil(d / spacing)) + for i in range(1, steps + 1): + f = i / steps + dense.append((ax + (bx - ax) * f, + ay + (by - ay) * f, + az + (bz - az) * f)) + return dense + + +def _build_navigate_goal(world_pts: list[tuple[float, float, float]], + frame_id: str, tolerance_m: float) -> str: + """YAML goal for a NavigateTask send_goal call from world-frame waypoints. + + frame_id must be a real TF frame: the local planner TF-transforms the + plan by its header frame and dies on an empty one. + """ + poses = ", ".join( + f"{{pose: {{position: {{x: {x:.3f}, y: {y:.3f}, z: {z:.3f}}}, " + f"orientation: {{w: 1.0}}}}}}" + for x, y, z in world_pts + ) + return (f"{{global_plan: {{header: {{frame_id: '{frame_id}'}}, " + f"poses: [{poses}]}}, goal_tolerance_m: {tolerance_m}}}") + + +def _snapshot_start_pose(robot_container: str, cfg: dict, n: int): + """World-frame (x, y, z, yaw, frame_id) of robot n, from one odom sample.""" + snap = ros2_exec( + robot_container, + f"timeout 5 ros2 topic echo --once --csv " + f"/robot_{n}/interface/mavros/local_position/odom", + domain_id=n, setup_bash=cfg["robot_setup_bash"], timeout=10, + ) + for line in snap.stdout.splitlines(): + parts = line.strip().split(",") + if len(parts) >= len(ODOM_SCHEMA): + try: + row = dict(zip(ODOM_SCHEMA, parts)) + return ( + float(row["pose.pose.position.x"]), + float(row["pose.pose.position.y"]), + float(row["pose.pose.position.z"]), + _quat_to_yaw( + float(row["pose.pose.orientation.x"]), + float(row["pose.pose.orientation.y"]), + float(row["pose.pose.orientation.z"]), + float(row["pose.pose.orientation.w"]), + ), + row["header.frame_id"].strip() or "map", + ) + except (ValueError, KeyError): + pass + return 0.0, 0.0, TARGET_ALTITUDE_M, 0.0, "map" + + +def _navigate_one_robot(n: int, robot_container: str, cfg: dict, + waypoints_rel: list[tuple[float, float, float]], + tolerance_m: float, goal_tolerance_m: float, + budget_s: float) -> None: + route_timeout = budget_s * len(waypoints_rel) + 30.0 + + x0, y0, z0, yaw0, frame_id = _snapshot_start_pose(robot_container, cfg, n) + world_pts = _transform_to_world(waypoints_rel, x0, y0, z0, yaw0) + logger.info("robot_%d waypoint route (frame %s): %s", n, frame_id, + [(round(x, 1), round(y, 1), round(z, 1)) for x, y, z in world_pts]) + # Dispatch a dense plan from the current pose through the waypoints + # (mirrors real global-planner output); judge only the user waypoints. + plan_pts = _densify([(x0, y0, z0)] + world_pts) + + streams = _start_captures(robot_container, cfg["robot_setup_bash"], + n, route_timeout + MAX_SETTLE_S + 10, "waypoints") + goal = _build_navigate_goal(plan_pts, frame_id, NAVIGATE_GOAL_TOLERANCE_M) + result = ros2_exec( + robot_container, + f'ros2 action send_goal --feedback /robot_{n}/tasks/navigate ' + f'task_msgs/action/NavigateTask "{goal}"', + domain_id=n, setup_bash=cfg["robot_setup_bash"], + timeout=int(route_timeout + 15), + ) + # Record the drone catching up to the tracking point: poll its position + # until it is stationary (arrived and hovering) or MAX_SETTLE_S elapses. + settle_deadline = time.monotonic() + MAX_SETTLE_S + prev_pos = None + while time.monotonic() < settle_deadline: + px, py, pz, _, _ = _snapshot_start_pose(robot_container, cfg, n) + if prev_pos is not None and math.dist((px, py, pz), prev_pos) < SETTLE_STATIONARY_M: + break + prev_pos = (px, py, pz) + time.sleep(SETTLE_POLL_S) + odom = _finish_captures(streams) + + action_success = _action_ok(result.stdout) + _record(n, {"navigate_action_success": 1.0 if action_success else 0.0}) + if not action_success: + logger.warning("robot_%d navigate action did not succeed: %s", + n, _action_message(result.stdout)) + + if not odom: + pytest.fail(f"robot_{n} waypoint flight: no odom samples captured") + + rows = [(_stamp(r), + r["pose.pose.position.x"], + r["pose.pose.position.y"], + r["pose.pose.position.z"]) for r in odom] + verdict = check_track(rows, world_pts, tolerance_m, budget_s) + + reached = sum(1 for w in verdict["waypoints"] if w["reached"]) + goal_error_m = verdict["waypoints"][-1]["closest_approach_m"] + metrics = { + "waypoint_success": 1.0 if verdict["success"] else 0.0, + "waypoints_reached": float(reached), + "route_time_sim_s": verdict.get("total_time_s"), + "worst_closest_approach_m": max( + w["closest_approach_m"] for w in verdict["waypoints"]), + "final_goal_error_m": goal_error_m, + } + _record(n, metrics) + for w in verdict["waypoints"]: + logger.info( + "robot_%d waypoint %d: reached=%s closest=%.2fm elapsed=%ss", + n, w["index"], w["reached"], w["closest_approach_m"], + w.get("elapsed_from_prev_s", "n/a")) + + assert verdict["success"], ( + f"robot_{n} reached {reached}/{len(world_pts)} waypoints in order " + f"(tolerance {tolerance_m}m, budget {budget_s}s/waypoint); " + f"closest approaches: " + f"{[w['closest_approach_m'] for w in verdict['waypoints']]}" + ) + assert goal_error_m <= goal_tolerance_m, ( + f"robot_{n} final goal error {goal_error_m:.2f}m exceeds " + f"{goal_tolerance_m}m (NavigateTask tolerance " + f"{NAVIGATE_GOAL_TOLERANCE_M}m + tracking margin)" + ) + + +# ── test class ───────────────────────────────────────────────────────────── + +@pytest.mark.waypoint_flight +@pytest.mark.timeout(2400) +class TestWaypointFlight: + """Full takeoff → waypoint route → land chain, judged by waypoint_checker. + + Route, tolerance, and per-waypoint budget come from --waypoints, + --waypoint-tolerance, and --waypoint-timeout. + """ + + @pytest.fixture(scope="session") + def _failed_envs(self): + return set() + + @pytest.fixture(autouse=True) + def _chain_guard(self, request, airstack_env, _failed_envs): + """Skip tests whose env was poisoned by an earlier failure. + + A waypoint-flight failure does NOT poison the env — landing always + runs after a successful takeoff. Takeoff or landing failures do. + """ + env_id = (airstack_env["sim"], airstack_env["num_robots"], + airstack_env["iteration"]) + if env_id in _failed_envs: + pytest.skip(f"earlier waypoint-flight test failed in {env_id}") + yield + rep = getattr(request.node, "_rep_call", None) + if rep is not None and rep.failed: + if "test_waypoint_route" not in request.node.name: + _failed_envs.add(env_id) + + @pytest.fixture + def waypoints_rel(self, request): + return parse_waypoints(request.config.getoption("--waypoints")) + + @pytest.fixture + def tolerance_m(self, request): + return float(request.config.getoption("--waypoint-tolerance")) + + @pytest.fixture + def goal_tolerance_m(self, request): + return float(request.config.getoption("--goal-tolerance")) + + @pytest.fixture + def budget_s(self, request): + return float(request.config.getoption("--waypoint-timeout")) + + @pytest.mark.dependency(name="wpf_ready") + def test_px4_ready(self, airstack_env): + """Wait until MAVROS is connected and local_position/odom publishes.""" + cfg = airstack_env["cfg"] + robot_container = get_robot_containers(airstack_env["robot_pattern"])[0] + num_robots = airstack_env["num_robots"] + + started = time.time() + connected: set[int] = set() + pending = list(range(1, num_robots + 1)) + deadline = started + PX4_READY_TIMEOUT_S + + while pending and time.time() < deadline: + for n in list(pending): + if n not in connected: + r = ros2_exec( + robot_container, + f"timeout 5 ros2 topic echo --once --csv " + f"--field connected /robot_{n}/interface/mavros/state", + domain_id=n, setup_bash=cfg["robot_setup_bash"], timeout=10, + ) + if any(line.strip() == "True" for line in r.stdout.splitlines()): + connected.add(n) + else: + continue + r = ros2_exec( + robot_container, + f"timeout 5 ros2 topic echo --once " + f"/robot_{n}/interface/mavros/local_position/odom", + domain_id=n, setup_bash=cfg["robot_setup_bash"], timeout=10, + ) + if r.returncode == 0 and "pose:" in r.stdout: + _record(n, {"ready_duration_sys_s": + round(time.time() - started, 2)}) + pending.remove(n) + if pending: + logger.info("px4_ready: connected=%s pending=%s elapsed=%.0fs", + sorted(connected), pending, time.time() - started) + time.sleep(2.0) + + if pending: + pytest.fail( + f"robots {sorted(pending)} not ready (MAVROS connected + odom) " + f"within {PX4_READY_TIMEOUT_S:.0f}s" + ) + + @pytest.mark.dependency(name="wpf_takeoff", depends=["wpf_ready"]) + def test_takeoff(self, airstack_env): + """Take off to TARGET_ALTITUDE_M at a fixed velocity of 1 m/s.""" + cfg = airstack_env["cfg"] + robot_container = get_robot_containers(airstack_env["robot_pattern"])[0] + _run_parallel( + airstack_env["num_robots"], + lambda n: _takeoff_one_robot(n, robot_container, cfg, TARGET_ALTITUDE_M), + ) + + @pytest.mark.dependency(name="wpf_route", depends=["wpf_takeoff"]) + def test_waypoint_route(self, airstack_env, waypoints_rel, tolerance_m, + goal_tolerance_m, budget_s): + """Send NavigateTask with the route; judge odometry with waypoint_checker.""" + cfg = airstack_env["cfg"] + robot_container = get_robot_containers(airstack_env["robot_pattern"])[0] + _run_parallel( + airstack_env["num_robots"], + lambda n: _navigate_one_robot(n, robot_container, cfg, waypoints_rel, + tolerance_m, goal_tolerance_m, budget_s), + ) + + @pytest.mark.dependency(name="wpf_land", depends=["wpf_takeoff"]) + def test_landing(self, airstack_env): + """Land the drone; runs even when test_waypoint_route fails.""" + cfg = airstack_env["cfg"] + robot_container = get_robot_containers(airstack_env["robot_pattern"])[0] + _run_parallel( + airstack_env["num_robots"], + lambda n: _landing_one_robot(n, robot_container, cfg), + ) diff --git a/tests/waypoint_checker.py b/tests/waypoint_checker.py new file mode 100644 index 000000000..7a7683157 --- /dev/null +++ b/tests/waypoint_checker.py @@ -0,0 +1,164 @@ +#!/usr/bin/env python3 +"""Standalone ordered-waypoint track checker. + +Judges whether an odometry track visited an ordered list of waypoints, each +within a distance tolerance and a per-waypoint time budget. Used by +``tests/system/test_waypoint_flight.py``, but deliberately dependency-free +(stdlib only) and runnable outside the AirStack harness: success is defined +purely on the odometry track, not on any AirStack-specific interface, so the +same checker can judge waypoint flight on any ROS 2 system that can dump +odometry to CSV:: + + ros2 topic echo --csv /robot_1/interface/mavros/local_position/odom > odom.csv + python3 waypoint_checker.py --odom-csv odom.csv \ + --waypoints "10,0,10; 10,10,10; 0,10,10" --tolerance 1.5 --budget 120 + +Exit code 0 iff every waypoint was reached in order within tolerance and +budget; a JSON verdict is printed to stdout either way. + +Input CSV format: the flattened ``ros2 topic echo --csv`` output of +``nav_msgs/Odometry`` (header stamp in columns 0-1, position x/y/z in columns +4-6). Lines that do not parse (ros2 banners, partial writes) are skipped. +""" + +import argparse +import json +import math +import sys + +# Column indices in `ros2 topic echo --csv` output for nav_msgs/Odometry +# (all primitives flattened in declaration order). +_COL_STAMP_SEC = 0 +_COL_STAMP_NSEC = 1 +_COL_POS_X = 4 +_COL_POS_Y = 5 +_COL_POS_Z = 6 +# A full Odometry row has 4 header/frame fields + 7 pose + 36 cov + 6 twist +# + 36 cov = 89 columns; require at least position columns to be present. +_MIN_COLUMNS = 7 + + +def parse_odom_csv(path): + """Parse a ros2 ``--csv`` odometry dump into [(t, x, y, z), ...] rows.""" + rows = [] + with open(path) as fh: + for line in fh: + parts = line.strip().split(",") + if len(parts) < _MIN_COLUMNS: + continue + try: + t = float(parts[_COL_STAMP_SEC]) + float(parts[_COL_STAMP_NSEC]) * 1e-9 + x = float(parts[_COL_POS_X]) + y = float(parts[_COL_POS_Y]) + z = float(parts[_COL_POS_Z]) + except ValueError: + continue + rows.append((t, x, y, z)) + return rows + + +def parse_waypoints(spec): + """Parse ``"x,y,z; x,y,z; ..."`` into [(x, y, z), ...].""" + waypoints = [] + for chunk in spec.split(";"): + chunk = chunk.strip() + if not chunk: + continue + parts = [p.strip() for p in chunk.split(",")] + if len(parts) != 3: + raise ValueError(f"waypoint {chunk!r} is not 'x,y,z'") + waypoints.append(tuple(float(p) for p in parts)) + if not waypoints: + raise ValueError("no waypoints given") + return waypoints + + +def check_track(rows, waypoints, tolerance_m, budget_s_per_waypoint): + """Check that the track visits every waypoint in order. + + A waypoint counts as reached at the first sample (searching forward from + the previous waypoint's arrival) within ``tolerance_m`` of it, provided + that sample's time is within ``budget_s_per_waypoint`` of the previous + arrival (or of track start, for the first waypoint). Times are whatever + clock stamped the odometry (sim time in AirStack runs). + + Returns a verdict dict; ``verdict["success"]`` is the pass/fail judgment. + """ + verdict = { + "success": False, + "num_odom_samples": len(rows), + "tolerance_m": tolerance_m, + "budget_s_per_waypoint": budget_s_per_waypoint, + "waypoints": [], + } + if not rows: + verdict["error"] = "no odometry samples" + return verdict + + start_idx = 0 + prev_arrival_t = rows[0][0] + all_reached = True + + for wi, (wx, wy, wz) in enumerate(waypoints): + # arrival = FIRST sample within tolerance (preserves ordering + # semantics); closest_approach = true minimum over the whole + # remaining track, so the report reflects how near the drone + # actually got, not just the tolerance boundary crossing. + arrival_idx = None + closest = math.inf + for i in range(start_idx, len(rows)): + t, x, y, z = rows[i] + d = math.sqrt((x - wx) ** 2 + (y - wy) ** 2 + (z - wz) ** 2) + if d < closest: + closest = d + if arrival_idx is None and d <= tolerance_m: + arrival_idx = i + + entry = { + "index": wi, + "target": [wx, wy, wz], + "closest_approach_m": round(closest, 3), + "reached": arrival_idx is not None, + } + if arrival_idx is not None: + arrival_t = rows[arrival_idx][0] + elapsed = arrival_t - prev_arrival_t + entry["elapsed_from_prev_s"] = round(elapsed, 3) + entry["within_budget"] = elapsed <= budget_s_per_waypoint + if not entry["within_budget"]: + all_reached = False + start_idx = arrival_idx + prev_arrival_t = arrival_t + else: + all_reached = False + # Keep evaluating later waypoints from the same index so the + # verdict reports closest approaches for all of them. + verdict["waypoints"].append(entry) + + verdict["total_time_s"] = round(rows[-1][0] - rows[0][0], 3) + verdict["success"] = all_reached + return verdict + + +def main(argv=None): + ap = argparse.ArgumentParser(description=__doc__.splitlines()[0]) + ap.add_argument("--odom-csv", required=True, + help="ros2 topic echo --csv dump of nav_msgs/Odometry") + ap.add_argument("--waypoints", required=True, + help="Ordered waypoints 'x,y,z; x,y,z; ...' in the odometry frame") + ap.add_argument("--tolerance", type=float, default=1.5, + help="Pass distance to each waypoint in meters (default 1.5)") + ap.add_argument("--budget", type=float, default=120.0, + help="Time budget per waypoint in seconds of odometry " + "clock (default 120)") + args = ap.parse_args(argv) + + rows = parse_odom_csv(args.odom_csv) + waypoints = parse_waypoints(args.waypoints) + verdict = check_track(rows, waypoints, args.tolerance, args.budget) + print(json.dumps(verdict, indent=2)) + return 0 if verdict["success"] else 1 + + +if __name__ == "__main__": + sys.exit(main())