From 06a7cd389a3cf554d801b2d972f21243993611ee Mon Sep 17 00:00:00 2001 From: isaac0804 Date: Wed, 21 Oct 2026 11:45:10 +0100 Subject: [PATCH 01/56] docs: refresh the roadmap against the code, and the stale notes it points at The roadmap still described the pre-evaluate.py evaluation plan, kept a mirror_swap xfail that no longer exists, and listed several items that had already shipped. Rewritten along its own convention (a resolved item becomes a one-line pointer under "Done"), with the remaining items numbered in sequence: - the strategy-evaluation items -> Done: evaluate.py and pool_v1 replaced the situation bench and ladder.py. The two genuinely open parts moved instead of disappearing: the CI stall gate into the stalls entry, the tactic contracts into the ball-holding entry. - directional tube and Replay overlays -> Done; fpp mirror_swap -> Done: the 0.52 m was the defense-area keep-out clamp, not a local planner minimum, and the xfail went in 5f97199b. - stalls -> the 2026-10-17 full round-robin figures; sim realism -> Phases 0-2 built, with what remains named. - bang-bang, trajsample, testing gaps and convergence revised for what has landed since; the unrecorded Sumatra findings (#4-7) and the shared-Sticky item removed (Sticky exists and is used). Also fixes the stale notes that pointed into the roadmap: obstacles.py's class docstring still described a growing circle, FORK_NOTES referenced two deleted roadmap sections, sim_realism's status line read as if nothing had been built, and testing_gaps #6 now records the one sim case for ball_placement_interference. --- docs/roadmap.md | 282 ++++++------------ docs/sim_realism.md | 9 +- docs/testing_gaps.md | 12 +- .../src/trajsampling/obstacles.py | 10 +- vendor/rSim/FORK_NOTES.md | 17 +- 5 files changed, 124 insertions(+), 206 deletions(-) diff --git a/docs/roadmap.md b/docs/roadmap.md index 051bdeb28..807186274 100644 --- a/docs/roadmap.md +++ b/docs/roadmap.md @@ -2,7 +2,9 @@ Larger open workstreams. Each entry: what, why, status, pointer. Investigation history lives in git log (`git log --grep=`) and commit messages, not here — when an item is -resolved, replace it with a one-line pointer under "Done". +resolved, replace it with a one-line pointer under "Done". Open items are numbered in order: +a resolved one leaves the list and the rest move up, so a number is not a stable name — cite the +entry by its title, not its number. ## Done (one-line pointers) @@ -30,179 +32,82 @@ resolved, replace it with a one-line pointer under "Done". - **Tournament gate tooling** — `--strict`, `--stop-at-first-stall`, `--fuzz-restarts SEED` (`405693c`). - **`metric_correlation.py` reads `.npz`**. - **Repo root cleanup** — first decided not to move root scripts (~45 inbound citations; `1f542fd`), then overturned 2026-10-02 because the root had grown to a dozen scripts and `tools/` already held the other tooling: the tournament drivers and `match` are now `tools/evaluation/`, and `debug_match` and `repro_from_replay` are in `tools/` (`elo` and `plot_elo` were removed 2026-10-10). `main.py`, `conftest.py` and `dashboard_server.py` stay at the root (pinned by pixi tasks, pytest and the README); `start_test_env.sh` was removed 2026-10-09 (unused; its three commands are in `docs/setup_external.md`). +- **Strategy evaluation** — `tools/evaluation/evaluate.py` judges a candidate against the champion of a fixed, versioned pool (`tools/evaluation/pools/pool_v1.json`: champion, six opponents, three held-out opponents, seeded sim noise). Candidate and champion meet the same opponents under the same noise seed, decided on paired **goals** — P(candidate stronger) >= 0.99 promotes, <= 0.2 drops, undecided after 6 rounds; `analysis/ratings.py` fits strengths over the ledger (`replays/eval/ledger.jsonl`), which never replays a fixture it already has, and `--explain` draws where the robots stand. The situation bench and `ladder.py` were built, used and then retired — the bench because its estimate is biased per strategy, so it could not rank (Spearman -0.09, `docs/sim_realism.md` "Evaluation infrastructure"). How to use it: `docs/STRATEGY_DEVELOPMENT.md`, "Evaluating a strategy". `f48d4bad`, `1849d123`, `49f29ebf`. +- **Directional-tube enemy obstacle** — trajsample's moving-obstacle obstacle is a capsule expanded along the obstacle's velocity instead of an inflated circle (`utama_core/motion_planning/src/trajsampling/obstacles.py:132-149`) — `a1b8bdd6`. +- **FastPathPlanner `mirror_swap` stall** — the 0.52 m shortfall was the defense-area keep-out clamp, not a planner local minimum: back-row targets moved to |x| = 2.9 and the xfail went (`5f97199b`, in #141); #147 separately cleared the detour-side-flip corner case (`fastpathplanner_corner_test.py`) — `0ce4e2a2`. trajsample passes all 29 scenarios of the current table; fpp passed all 19 of the one before it. +- **Geometric intention overlays in the Replay tab** — `MatchLog.trace_if_changed` overlays drawn by `dashboard/static/field_canvas.js` with its `_drawArrow` primitive: `shadow_and_mark.marks`, `give_and_go.shot_lane`/`pass_target`, `defense.shadow_post`, `block_shape.screen_line`, `press_and_contain.press`, `clear_ball.clear_target`, `dribble.segment_target`, `decoy_and_overload.lure`, `committed_robot_ids` — `c47f7549`, `b6e69b44`. +- **Shared hysteresis helper** — `Sticky` in `shared/tolerance.py` (`fd87c7f6`), now used by `pickers.friendly_closer_to_ball` and `PressAndContainTactic` instead of per-site margins — `0ce4e2a2`. ## Open -1. **Current stall count (2026-10-01, `fpp`, `e83a7466`): 0/231 matches** in the full strict - 65s round-robin (`replays/tournament_20261001_094103/`), none flagged by the possession - backstop; 93% of restarts reach NORMAL_START. `bank_v7` (deleted 2026-10-12, replaced by `bank_v8`) was - harvested from it. Earlier stall families (overload-slot COMMITTED_FROZEN, DIRECT_FREE - restart stalls, low_block's PassAndShoot holding the ball) are fixed; see `git log`. - Confirm any stall fix against the full round-robin: small-subset re-runs have repeatedly - overstated fixes. - -2. **Outer-loop strategy evaluation.** Goal: agents iterate strategies against evals without - humans watching replays. Win rate/Elo is the objective but too sparse and too expensive to - be the only signal (most 65s matches are draws; rsim is deterministic, so a repeat adds - nothing unless restarts are fuzzed). Three tiers: - - *Inner loop (tests, binary, blocks merge):* tactic contracts, kernel invariants, zero - `StallEvent`s across N seeded fuzzed matches. **Built:** stall watchdog + `--strict`, - commitment deadline, restart fuzzer, pure `Game` builder. **Not built:** CI job running - the strict seeded stall gate. - - *Outer loop, fast half (scenario bench, numbers vs a committed baseline, non-blocking; **retired 2026-10-17**, code and banks deleted, history kept):* - seeded start state + 15-30s horizon, scored as paired differentials by calibrated proxy - metrics. **Built (v1):** `start.py`, 4 hand-authored anchors, - `scenario_harvester.py` (restart transitions, trust gate `stall_events == []`), - `dynamic_screen.py`, `scenario_scorer.py`, `tools/scenario_bench.py`. 2026-09-24: TURNOVER - counts real losses only (raw turnovers are mostly nearest-robot flicker), stalls are a - flag; `--repeats K` jittered starts give the seed noise rsim's determinism hid (the - screen's default pool was the champion itself, so every live scenario came out - DETERMINED); `--against-results` compares against an earlier run's JSON, for A/B - across commits. Banks in `utama_core/scenario_bench/banks/` were committed (v3 onward; v1/v2 predate - the rulebook referee fixes and stay local). **Bank v4** (173 scenarios) = v3 (from - RR `tournament_20260928_132051`) + RR `tournament_20260928_125725`, each harvested, - screened (informative + noisy kept), then merged dropping near-duplicate starts (same - family, perspective and command; ball within 0.10 m, every robot within 0.15 m). v3 alone - held 25 such duplicates of 142, mostly kickoff formations; a second run adds ~50 new - scenarios, mostly free kicks, since kickoffs repeat and harvested open-play counters are - mostly dead. Replays of a round-robin are only needed until it is harvested. v4 at - `4b40f3c2` (press_and_pass vs low_block, 3 repeats): self vs self 173/173 identical; passes - aimed 10° off: mean -0.143, se 0.045, t = -3.19 (57 worse, 26 better), all from the free - kicks (v3: t = -2.23). **Bank v5 and no dynamic screen (2026-09-28):** the v1-v4 numbers above predate - `c27bfad7` (hysteresis state leaked between teams and matches). The dynamic screen was - removed: it varied the opponent with the candidate fixed, while an A/B varies the - candidate. Of the starts it dropped as determined, the candidate's strategy changed the - outcome in 24/40, and passes aimed 10° off moved them as much as the kept ones (dropped - t = -6.9 on 489, kept t = -3.9 on 339; press_and_pass vs low_block, 1 run each). It also - counted starts that failed to set up (robot past the field line) as neutral. v5 = every - start of `tournament_20260928_132051` with `--open-play 2`, near-duplicates and - out-of-field robots dropped: 846 starts, built in 3 min, one pass 11 min at 15 workers. - press_and_pass vs low_block, 1 run each: passes aimed 10° off, t = -7.56 over 845. With a 10 s - horizon instead of 20 s the pass takes 7-8 min but the same weakening is not seen (t = +0.76): - most of its effect comes after 10 s. - **Bank v1** - (31 scenarios; rebuild with `--harvest-from` + `--dynamic-screen` + `--save-bank`): harvested - from `tournament_20260924_124033` (692 restart scenarios from 222 stall-free matches), a - stratified 72 screened (3 opponents x 3 jittered starts, 20s): 14 informative, 17 noisy - (kept), 22 determined, 19 dead (dropped; 16 of the 20 FORCE_START restarts were dead). - Seed noise and policy spread are about the same size (~0.1-0.25 ordinal units), so a - real difference needs many scenarios; read the stderr. **Not built:** - harvesting from a real post-fix calibration run (only synthetic fixtures so far), the - lost-play weakness subset, event-triggered open-play harvesting, bank versioning/lifecycle. - - *Outer loop, slow half (ladder, acceptance gate):* candidate vs a frozen reference pool - of 4-5 strategies, both colours, K fuzz seeds, Elo anchored to the pool; full 600s - matches only vs the top of the pool. **Ladder built (2026-10-09) and retired 2026-10-17 in - favour of `tools/evaluation/evaluate.py`** (fixed pool `pool_v1`, paired sequential test). - - *Metrics — derive, don't invent:* `MatchStats` has both-sided turnovers, completed - passes, attacking-third entries, possession-under-pressure, restart-to-entry. The first - correlation study (`tools/metric_correlation.py`, `git show 24ef2c3f:benchmark_results/ - metric_correlation_20260903.md`) found turnovers/passes/entries valid and reliable; - possession and robot motion carry no signal; `ball_travel_m` is not a quality signal. - Caveat: raw `turnovers` is mostly nearest-robot flicker on contested balls (1990 raw vs - 1090 real losses over 231 matches, `git show 24ef2c3f:benchmark_results/turnover_breakdown_20260923_211717.md`); - prefer `summary.json["ball_losses"]["real_losses"]`. - Goodhart guard: if a screen improves and evaluate.py doesn't, retire the proxy. - - *Compute discipline:* paired comparison on common seeds, sequential stopping, short - sampled horizons over long matches. - - **Partly done (2026-10-17):** one entry point, `tools/evaluation/evaluate.py `, - without `--budget` (it stops by itself). Not built: contracts, and writing - `docs/strategies.md`. - - Build order: calibration tournament → bench on harvested states (retired 2026-10-17) → ladder (since retired; see evaluate.py). - -2a. **Strategy evaluation v2 (direction agreed 2026-10-09; ladder built, then retired 2026-10-17 for `evaluate.py`).** Builds the ladder - above and settles how matches vary. - - *Ladder:* a candidate plays a fixed reference pool (the current top 4-5) in several - sampled worlds, each world played twice with the teams swapped so luck cancels, both - kickoffs covered, stopping early once the result is clear; Elo anchored to the pool. The - full round-robin stays as the occasional full refresh after shared-code changes. Pure - Elo matchmaking over the whole league is not the plan: strategies counter each other, and - the round-robin's results table is what shows that. - **Built (2026-10-09), retired 2026-10-17:** `tools/evaluation/ladder.py` (replaced by - `tools/evaluation/evaluate.py`). Until worlds vary, a pairing has 8 - distinct matches (4 side/kickoff settings, each mirrored); it stops at 4 or more once wins - minus losses reaches 3 either way or can no longer change sign. Not built: Elo anchoring - (points per match against the pool is the read for now) and spot-checks of reused records. - - *Sampled worlds instead of restart fuzzing:* variation comes from realistic imperfection, - seeded so a world is reproducible: vision noise and dropped detections (`rsim_noise`, - `rsim_vanishing` already exist), then kick speed and direction spread and command delay - or lost packets (Python, in the sim's controller and `standard_ssl.py`), then per-robot - profiles once 10c has measurements, then dribbler loss (likely `vendor/rSim`). Both teams - face the same world with mirrored profiles. Ranges stay modest and written down until - measured. The distribution is fixed by the evaluator and out of reach of strategy - branches, so it is not something to tune toward. `RestartFuzzingReferee` stays as a - stall-testing tool, not an evaluation input. - - *Names:* one match runner with the schedule (round-robin, ladder) and match settings - (sides, kickoffs, world seed) as options, replacing round-robin / full-match tournament / - Elo as separate tools. `full_match_tournament.py`, `tools/elo.py` and `plot_elo.py` are - removed; the ladder, then evaluate.py, cover them. - - *Match cache:* a match's key (`replay/fingerprint.py` `match_key`) hashes the exact bytes - and paths of everything `tools/evaluation/match.py` imports, data files next to - them (not `.md`), both strategies' modules, and the match settings. New tools that call - `run_match`, and new settings added to the key only when switched on (as `fuzz_seed` is), - keep the cache. Renaming, moving or editing `match.py` or anything it imports - (even a comment), or adding sim noise, reruns every match. So: build the ladder and world - settings as additions first, and do the renames in the same batch as the next change that - forces a full rerun anyway (the sim noise). - - *Order:* write the ladder (additions only, done) → switch on vision noise and dropouts as world - settings → kick spread and command delay, together with the naming unification, then one - full round-robin → per-robot profiles after calibration. - -3. **`BangBang1D` defects** — required-overshoot and `v0 > v_max` cases produce - discontinuous trajectories (xfail-pinned in `utama_core/tests/motion_planning/ - implementation/bang_bang_edge_cases_test.py`). A correct fix (`b26a550`) was reverted (`90d068c`) because - physically correct trajectories let mutually blocked robots stall at kickoff (231/231). - Re-apply only together with an explicit yield/escape result for the all-candidates-collide - state. - -4. **trajsample architecture follow-ups** (from a TIGERs 2024 comparison + Sumatra source - audit; validate each with `tools/motion_planning_benchmark.py`, see - `docs/motion_planning_comparison.md`), roughly in order: - - Directional-tube enemy obstacle (`obstacles.py`) — attempted and reverted (regressed - `mirror_swap`, doubled `static_slalom`). Next attempt must stay at least as conservative - as the circle at small horizons. +1. **Stalls — latest full strict round-robin (2026-10-17, `replays/tournament_20261017_151052/`):** + 4 of 66 matches stalled (three committed robots frozen about 10 s, and one kick-off that + waited the referee's 15 s cap; both fixed after the run in `1731e904`), and 97% of 3232 + restarts reached NORMAL_START. Confirm any stall fix against the full round-robin: + small-subset re-runs have repeatedly overstated fixes. **Not built:** a CI job running the + strict seeded stall gate — `.github/workflows/ci.yml` runs the test suite only. + +2. **`BangBang1D` defects** — required-overshoot and `v0 > v_max` cases produce discontinuous + trajectories, pinned as 78 strict xfails on the enumerated defect seeds + (`utama_core/tests/motion_planning/implementation/bang_bang_edge_cases_test.py`; + `trajsampling_correctness_test.py` pins the same defect). A correct fix (`b26a550`) was + reverted (`90d068c`) because physically correct trajectories let mutually blocked robots + stall at kickoff (231/231). trajsample has since gained Sumatra's all-candidates-collide + fallback (`a1b8bdd6`), so re-applying the fix means validating it against that; there is + still no first-class yield/escape result for the blocked state. + +3. **trajsample architecture follow-ups** (from a TIGERs 2024 comparison; validate each with + `tools/motion_planning_benchmark.py`, see `docs/motion_planning_comparison.md`): - `Trajectory2D` drops transverse velocity at trajectory start (`bang_bang.py`); every trajectory ends at zero velocity. Want a concrete replay failure before starting. - - Fixed robot-id priority can make the wrong robot yield; no first-class "yield" result. - - Collision sampling is adaptive-timestep, not swept; intermediate targets are purely - random (hurts reproducibility in corridors); committed-trajectory reuse could validate - more. - - Sumatra audit findings #4-7: untriaged. + - Priority: a ball carrier now outranks off-ball robots (`a1b8bdd6`), but the remaining + tie-break is fixed robot id, which can make the wrong robot yield; still no first-class + "yield" result. + - Collision sampling is adaptive-timestep, not swept. (The other two parts of this entry are + done: intermediate targets are a deterministic symmetric fan with a <=90 deg hemisphere + clamp and last-direction stiction, and a committed trajectory is re-validated over a 0.5 s + window.) - Last: a jerk-limited generator (e.g. Ruckig). -5. **FastPathPlanner convergence stall (`test_mirror_swap`, xfail)** — two wing robots stall - ~0.53m short in a mirrored 6v6. Genuine local minimum. DWA solves this scenario but costs - 2-3x compute and fails others; no scheme wins every benchmark scenario, so the default - (`fpp`) is unchanged. - -6. **Referee testing gaps** — see `docs/testing_gaps.md`. Open: mypy/pyright adoption (on - hold); `ball_placement_interference` has never fired live (not treated as a bug). - -7. **Cross-tactic ball convergence** — nothing checks whether another assigned robot is - already converging on the same ball; fixed twice locally (`a59a8e5`, `0e510a0`/`ae6a6f3`). - Generalize only if a third instance appears. - -8. **grsim as a CI/tournament environment.** Unverified whether grsim installs on a GitHub - runner or can run faster than real time headless. CI hardcodes `--ignore-glob "**/*grsim*"`. - -9. **Geometric intention overlays for the Replay tab** — e.g. `ShadowAndMarkTactic._assign_marks()` - already computes `{marker: opponent}` each tick and discards it. Tactic side is small via - `MatchLog.trace_if_changed()`; `field_canvas.js` has no line/arrow primitive yet. - -9b. **Match timeline in the dashboard.** `tools/match_timeline.py` draws one window as swimlanes - (referee band, each robot's tactic, possession, distance to the ball, events); as a dashboard - panel under the Replay tab's field view it would follow the replay cursor, and clicking a lane - would seek to that tick. Data is all in the replay and its `intentions.jsonl` already - (`load_window` computes it without drawing). Not started (user idea, 2026-10-17). - -10. **More tactics from real football vocabulary** (formations, set plays, pressing schemes) - rather than variations on existing ones. - -10b. **Ball-holding contract for every `Tactic`.** Most fouls fixed on 2026-09-24 +4. **Referee testing gaps** — see `docs/testing_gaps.md`. Open there: #4 mypy/pyright adoption + (on hold). #6 stands as written for real matches, but our sim does reach the rule: a corner + placement that trapped a defender inside the keep-out circle is pinned as a passing moment + (`utama_core/tests/moments/placement_in_corner_traps_defender.json`, from + `tournament_20261017_002337`). + +5. **Cross-tactic ball convergence** — nothing checks whether another assigned robot is already + converging on the same ball; the only guard is `DecoyOverloadTactic`'s + `_teammate_already_has_ball()` (`utama_core/tactics/decoy_and_overload.py:99`). Found twice + (`a59a8e5`, `0e510a0`/`ae6a6f3`); a third case (two tactics' robots converging on one ball) + was diagnosed in `25888cce` and closed as a planner-level matter rather than by generalizing. + Revisit only if it stalls a match again. + +6. **grsim as a CI/tournament environment.** Unverified whether grsim installs on a GitHub + runner or can run faster than real time headless. CI hardcodes `--ignore-glob "**/*grsim*"` + (`ci.yml:41,43`). `vendor/rSim/FORK_NOTES.md:16` still points at a roadmap section that no + longer exists (it meant this entry). + +7. **Match timeline in the dashboard.** `tools/match_timeline.py` draws one window as swimlanes + (referee band, each robot's tactic, possession, distance to the ball, events). The Replay tab + has only a per-tick marker strip (goals, fouls, stalls, ball losses); the swimlane panel that + follows the replay cursor, with clicking a lane to seek, is what is missing. The data is all + in the replay and its `intentions.jsonl` already (`load_window` computes it without drawing). + Not started (user idea, 2026-10-17). + +8. **More tactics from real football vocabulary** (formations, set plays, pressing schemes) + rather than variations on existing ones. No new `Tactic` class has been added since this + entry: the recent work refactors existing ones (`ShadowAndMarkTactic` now uses the `man_mark` + skill, `ClearBallTactic`/`PressAndContainTactic`/`DecoyOverloadTactic` gained thresholds and + exits). + +9. **Ball-holding contract for every `Tactic`.** Most fouls fixed on 2026-09-24 (`ShadowAndMark`, `PressAndContain`, `DecoyAndOverload` lure, keeper) were one pattern: a tactic written for its main job with no branch for "my robot now has the ball", so it held or carried until `ExcessiveDribbling`/`KeeperHeldBall` fired or both teams froze on the ball. Idea: one parametrized test over all tactics that puts the ball on each assigned - robot in a few standard spots and checks the tactic releases it. Decisions still open: + robot in a few standard spots and checks the tactic releases it. `test_all_tactics.py` now + covers the generic properties (declared tag, `is_committed()` bool, a command for every + assigned robot) but not the ball-holding contract itself. Decisions still open: - *Depth:* single-tick check (first command is kick/pass, turn-to-kick, or a tracked carry) vs a short kinematic rollout that proves release before the carry limit and the keeper hold time. Leaning single-tick first; rollout only if something slips past. @@ -213,11 +118,12 @@ resolved, replace it with a one-line pointer under "Done". kernel concept, so it needs a concrete case the per-tactic approach can't handle. - *Setup for role-based tactics* (passer/receiver, committed slots): which robot gets the ball, and in which phase. - - *Rule constants:* tactics currently copy limits (`CARRY_LIMIT_M = 0.8`) instead of - importing them from the referee rules; decide whether to share one source. + - *Rule constants:* tactics still copy limits (`CARRY_LIMIT_M` in + `shared/pass_and_score_geometry.py`) instead of importing them from the referee rules; + decide whether to share one source. Expect each failing tactic to be a real fix with its own regression test. -10c. **Real-robot profiles and calibration.** Each physical robot differs (top speed, +10. **Real-robot profiles and calibration.** Each physical robot differs (top speed, dribbler grip, kicker strength) in ways hardware can't fix soon; software should correct what it can and use the rest. No measurements yet (2026-10-09), so nothing per robot is built: this entry is the plan. @@ -248,8 +154,8 @@ resolved, replace it with a one-line pointer under "Done". reach of strategy branches, so they are not something a search tunes. - *Open, not yet:* one profile per match day vs updated while playing; kick-to-kick spread in the sim (more realistic, but noisier round-robins). - - *Calibration routine (to write before measuring day):* per robot, at full battery on - the competition carpet: commanded vs measured speed at a few speeds, acceleration and + - *Calibration routine (to write before measuring day):* per robot, at full battery on the + competition carpet: commanded vs measured speed at a few speeds, acceleration and braking from vision, straight-line drift over 3 m, turn rate; ten kicks (speed from vision, direction error, roll distance) and ten chips; recharge time between kicks; a dribble course at increasing speed until the ball is lost; ten passes received. @@ -259,38 +165,42 @@ resolved, replace it with a one-line pointer under "Done". rolls ~17 m), so measured values change one file. No per-robot profile type until measurements show robots differ enough to matter. -10d. **Vision filter for real cameras.** Found 2026-10-09 reviewing `data_processing/`; rsim +11. **Vision filter for real cameras.** Found 2026-10-09 reviewing `data_processing/`; rsim (noise off) never exercises any of it. Needs logged SSL-Vision data from the real field to tune, so it waits on the hardware team like 10c. - *Velocity:* `KalmanFilter` tracks position only; `VelocityRefiner` differentiates consecutive filtered positions and that velocity feeds the next prediction. With the current noise settings (process noise twice the measurement noise) the steady-state gain is about 0.73, so positions are barely smoothed. An estimate: 1 cm vision noise - gives roughly ±0.5 m/s velocity jitter at 60 Hz. The usual SSL design is one filter with - position and velocity in its state (constant velocity for robots; for the ball, rolling - friction, plus a chip/flight model later). The `VelocityRefiner` note that smoothing - velocity "broke control loops" was measured in noiseless rsim. + gives roughly +/-0.5 m/s velocity jitter at 60 Hz. The usual SSL design is one filter + with position and velocity in its state (constant velocity for robots; for the ball, + rolling friction, plus a chip/flight model later). The `VelocityRefiner` note that + smoothing velocity "broke control loops" was measured in noiseless rsim. #144 only + added a fast path to that differentiator (`a1b8bdd6`); the substance is unchanged. - *Lost objects:* a vanished ball or robot is predicted at its last velocity forever, with no friction and no give-up time. The ball vanishes most often under a dribbling robot, where it should stay at the dribbler. Vanished robots relate to substitutions, issue #107. - *Detections:* `CameraCombiner` ignores SSL-Vision confidence, so a low-confidence - false detection with a robot's ID is averaged into the real one. - - *Noise settings:* 1 cm and 5° match rsim's noise generator, not the real cameras. + false detection with a robot's ID is averaged into the real one (`_avg_robots` sums + confidence into a variable it never uses). + - *Noise settings:* 1 cm and 5 deg match rsim's noise generator, not the real cameras. - *First step once logs exist:* replay a recorded vision log through `PositionRefiner` and compare velocity jitter and lag against a constant-velocity filter, before changing anything. -11. **Sim realism (plan, 2026-10-14):** [`sim_realism.md`](sim_realism.md). Real 2025–2026 - Division B logs against the baseline round-robin: goals per live minute match, but sim - keepers save about twice as many good chances and sim shots almost never go wide. Plan: a - realism scorecard, parameter ranges from the official logs, seeded per-match physics draws - (latency, kick noise, ball deceleration, per-robot limits, vision noise), a sensitivity - sweep, then adoption in the round-robin. P0 ahead of a real-data value model (P1) and - structured strategy search scored by it (P2). - -12. **Deferred, revisit only when forced** (minimalism): - - Shared `Sticky`/hysteresis helper beyond `shared/tolerance.py` — existing instances - differ in shape. +12. **Sim realism (plan, 2026-10-14):** [`sim_realism.md`](sim_realism.md). Phase 0 (the + scorecard and the analysis in-repo) and Phase 1 (league ranges from the 2025-26 official + logs) are done; Phase 2's knobs are built — `SimNoise` seeds latency, kick speed and + direction spread, two-phase ball friction, per-robot acceleration limits and vision + noise/dropouts per match, and is wired through `run_match`, `evaluate.py` and `pool_v1` + (`v3_seed2`). A sensitivity sweep was run (results in `sim_realism.md`). Still open: the + scorecard's remaining gaps (sim keepers save about twice as many good chances, and sim + shots almost never go wide), the 2022-24 held-out check, choosing which knobs become the + evaluation default, and using that noise in a round-robin by default (`round_robin.py` + still needs an explicit `--sim-noise`). P1 (a real-data value model) and P2 (structured + strategy search) are not started. + +13. **Deferred, revisit only when forced** (minimalism): - `@pytest.mark.engine` marker for kernel tests — wait for real CI bottleneck data. - `TickContext` responsibilities beyond `motion_controller`/`match_log`. diff --git a/docs/sim_realism.md b/docs/sim_realism.md index e351e7150..bfea15afe 100644 --- a/docs/sim_realism.md +++ b/docs/sim_realism.md @@ -1,7 +1,7 @@ # Sim realism: closing the gap between rsim and real matches -Status: plan, 2026-10-14 (priorities added 2026-10-16). Nothing below is built yet unless it -says so. +Status: plan, 2026-10-14 (priorities added 2026-10-16; Phases 0-2 are built, 2026-10-16/17). +Nothing below is built yet unless it says so. ## Where this leads @@ -338,6 +338,11 @@ direction SD 3° (per robot 1.5°), ball slide 2.5–4.0 then roll 0.27–0.33 m ### Phase 2: Knobs in the sim (1–2 weeks) +All of these are built (2026-10-17): `SimNoise` draws each knob per match from its range, seeded +and recorded in the replay, and reaches `run_match`, the round-robin option, `evaluate.py` and +`pool_v1` (`v3_seed2`). A knob at zero reproduces the old behaviour, so results played without +`--sim-noise` are unchanged. + Each knob is off by default, so current results are unchanged, and each comes with a test that pins its effect: diff --git a/docs/testing_gaps.md b/docs/testing_gaps.md index 4394637bc..16b047ed7 100644 --- a/docs/testing_gaps.md +++ b/docs/testing_gaps.md @@ -14,9 +14,13 @@ symptom. Adopting mypy/pyright is undecided given the codebase isn't typed to th **6. New rules never systematically fuzzed against real match dynamics** — partly open. `pushing` (closed, `a3f3795`) and `keeper_held_ball` (fired 10 times in 6 full matches) are -confirmed live. `ball_placement_interference` has never fired live: across ~200 placements the -longest non-placer dwell in the 0.5m stadium was 1.0s, under the rulebook's 2s grace. Not -treated as a bug; recheck if a match produces a ≥2s linger, or build an adversarial scenario. +confirmed live. `ball_placement_interference` has never fired in real play: across ~200 +placements the longest non-placer dwell in the 0.5m stadium was 1.0s, under the rulebook's 2s +grace. Not treated as a bug; recheck if a real match produces a ≥2s linger, or build an +adversarial scenario. Our own sim does reach it, so it is no longer untested: +`tests/moments/placement_in_corner_traps_defender.json` (round-robin +`tournament_20261017_002337`) pins a corner placement that trapped a defender inside the +keep-out circle, and must keep passing. ## Closed (one line each) @@ -56,4 +60,4 @@ treated as a bug; recheck if a match produces a ≥2s linger, or build an advers finish timeout (`ae6a6f3`), `ball_is_loose` ignoring a legally barred enemy near our defense area (`10fc89c`); and a same-team two-robot ball scrum (`_teammate_already_has_ball()`, `a59a8e5`). Cross-tactic convergence in general is - roadmap item 7. + roadmap item 5. diff --git a/utama_core/motion_planning/src/trajsampling/obstacles.py b/utama_core/motion_planning/src/trajsampling/obstacles.py index 6c39ed5ac..144071cce 100644 --- a/utama_core/motion_planning/src/trajsampling/obstacles.py +++ b/utama_core/motion_planning/src/trajsampling/obstacles.py @@ -102,11 +102,11 @@ class EnemyRobotObstacle: champion paper section 2.5 (equations 5-8): the opponent could be anywhere within a 1D bang-bang envelope of its current speed along its current direction of travel, for a bounded lookahead `t_max`. Modeled - here as a growing circle (the paper's "tube" refinement -- shrinking the - circle to a directional tube plus a fixed-radius circle for a stationary - robot -- is a further tightening skipped in this first pass; see - `EnemyRobotObstacle.distance_at`'s docstring for why a circle alone is - still safe, just more conservative for a fast-moving opponent). + here as the paper's directional tube: the capsule from the envelope's + rearward reach to its forward reach (`f_minus`..`f_plus`) along the + direction of travel, inflated by the robot radius, so a fast-moving + opponent is not over-inflated sideways as a growing circle would be. A + stationary opponent is a fixed-radius circle. """ p0: Tuple[float, float] diff --git a/vendor/rSim/FORK_NOTES.md b/vendor/rSim/FORK_NOTES.md index 54b3dde0e..d4700af91 100644 --- a/vendor/rSim/FORK_NOTES.md +++ b/vendor/rSim/FORK_NOTES.md @@ -13,17 +13,17 @@ This fork is the physics step of the rsim stack. The diagram of the whole tick, Since 2026-10-16 `pixi.toml` builds the `robosim` environment's `rc-robosim` from this directory, not from PyPI (see "Installed from source by pixi" below). This copy first existed because we found and fixed real bugs in rSim's native ball/dribbler/kicker -physics (see `docs/roadmap.md`'s "TODO — add grsim as a CI/tournament -environment" section, item 1, for the dribbler-release writeup, and the -"scoreless-draw pattern" section for the kick-direction writeup) and there -was nowhere else to keep a rebuildable, git-trackable copy of the fixes. +physics (the dribbler-release and kick-direction writeups are the patch diffs +below, plus `git log -- vendor/rSim/FORK_NOTES.md`; the roadmap sections that +once held them were folded into its "Done" list) and there was nowhere else to +keep a rebuildable, git-trackable copy of the fixes. The patches themselves are also saved as plain diffs at `patches/rSim-dribbler-release.diff` and `patches/rSim-kick-direction.diff` — those are the reviewable artifacts. This directory is the actual buildable source they apply to, so -whoever picks up the open regression (see roadmap TODO) doesn't have to -re-clone upstream and re-apply the diffs by hand first. +whoever picks up an open regression here doesn't have to re-clone upstream and +re-apply the diffs by hand first. ## What's patched here (already applied, not a diff you need to apply) @@ -37,9 +37,8 @@ re-clone upstream and re-apply the diffs by hand first. substep). Now also repositions the ball just clear of the kicker's collision envelope and gives it a small outward velocity. 3. `src/robosim/sslrobot.cpp` — `Kicker::kick()` had two compounding - direction bugs (see `patches/rSim-kick-direction.diff` and - `docs/roadmap.md`'s "New blocker found, native-simulator-level" writeup - for full detail): + direction bugs (see `patches/rSim-kick-direction.diff`, and the commit + message that added it, for full detail): - It gated on `isTouchingBall()`, a razor-thin box check (~3cm forward, 4cm lateral) meant for deciding whether to grab the ball into the dribbler hold — far stricter than any Python-side "do I have the From 9f2fe595467684aec81f3a61c458a902564e389a Mon Sep 17 00:00:00 2001 From: isaac0804 Date: Wed, 21 Oct 2026 18:03:09 +0100 Subject: [PATCH 02/56] docs: fix the roadmap refresh's errors; cite roadmap entries by title Corrections to the previous commit, checked against the code: - stalls: 1731e904 fixed only the kick-off that waited the 15 s cap; the three COMMITTED_FROZEN stalls of the 2026-10-17 round-robin are not yet diagnosed (docs/strategies.md says the same). - sim realism: P1's value model is fitted (passes 1-3) but not yet a better ranker than goals; only P2 is not started. - Shared hysteresis helper: Sticky's users are PassAndShoot, ClearBall and PressAndContain; pickers.friendly_closer_to_ball keeps its own hysteresis. - grsim entry: drop the note that FORK_NOTES points at a missing section (the previous commit fixed it); "like 10c" names its entry; pool_v1 holds the v3 ranges, v3_seed2 is the round-robin's seeded run of them. The roadmap now asks to cite entries by title, since open items renumber. Every comment, docstring and doc that cited a roadmap number used numbers long gone ("roadmap item 15/16", "item 5", "item 1"); each now names its entry. fastpathplanner_corner_test's docstring said seeds were pinned as stuck; the list has been empty since #147. Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_01TJGHdb8Dht67hn4Lohndf5 --- docs/roadmap.md | 24 ++++++++++--------- docs/sim_realism.md | 4 ++-- docs/testing_gaps.md | 2 +- tools/metric_correlation.py | 2 +- utama_core/custom_referee/actions.py | 4 ++-- utama_core/custom_referee/geometry.py | 2 +- .../src/controllers/trajsampling.py | 2 +- .../src/trajsampling/planner.py | 8 +++---- utama_core/shared/tolerance.py | 4 ++-- utama_core/tactics/defense.py | 2 +- .../custom_referee/test_custom_referee.py | 2 +- .../test_dribble_placement_stopspeed.py | 2 +- .../custom_referee/test_pushing_crashing.py | 2 +- utama_core/tests/engine/test_all_tactics.py | 4 ++-- .../fastpathplanner_corner_test.py | 16 ++++++------- .../trajsampling_controller_test.py | 2 +- .../trajsampling_correctness_test.py | 14 +++++------ utama_core/tests/referee/test_referee_unit.py | 4 ++-- 18 files changed, 51 insertions(+), 49 deletions(-) diff --git a/docs/roadmap.md b/docs/roadmap.md index 807186274..59130d486 100644 --- a/docs/roadmap.md +++ b/docs/roadmap.md @@ -36,15 +36,15 @@ entry by its title, not its number. - **Directional-tube enemy obstacle** — trajsample's moving-obstacle obstacle is a capsule expanded along the obstacle's velocity instead of an inflated circle (`utama_core/motion_planning/src/trajsampling/obstacles.py:132-149`) — `a1b8bdd6`. - **FastPathPlanner `mirror_swap` stall** — the 0.52 m shortfall was the defense-area keep-out clamp, not a planner local minimum: back-row targets moved to |x| = 2.9 and the xfail went (`5f97199b`, in #141); #147 separately cleared the detour-side-flip corner case (`fastpathplanner_corner_test.py`) — `0ce4e2a2`. trajsample passes all 29 scenarios of the current table; fpp passed all 19 of the one before it. - **Geometric intention overlays in the Replay tab** — `MatchLog.trace_if_changed` overlays drawn by `dashboard/static/field_canvas.js` with its `_drawArrow` primitive: `shadow_and_mark.marks`, `give_and_go.shot_lane`/`pass_target`, `defense.shadow_post`, `block_shape.screen_line`, `press_and_contain.press`, `clear_ball.clear_target`, `dribble.segment_target`, `decoy_and_overload.lure`, `committed_robot_ids` — `c47f7549`, `b6e69b44`. -- **Shared hysteresis helper** — `Sticky` in `shared/tolerance.py` (`fd87c7f6`), now used by `pickers.friendly_closer_to_ball` and `PressAndContainTactic` instead of per-site margins — `0ce4e2a2`. +- **Shared hysteresis helper** — `Sticky` in `shared/tolerance.py` (`fd87c7f6`), now used by `PassAndShootTactic`'s passer choice, `ClearBallTactic`'s target choice and `PressAndContainTactic`'s presser choice instead of per-site margins — `0ce4e2a2`. (`pickers.friendly_closer_to_ball` keeps its own tick-count hysteresis.) ## Open 1. **Stalls — latest full strict round-robin (2026-10-17, `replays/tournament_20261017_151052/`):** - 4 of 66 matches stalled (three committed robots frozen about 10 s, and one kick-off that - waited the referee's 15 s cap; both fixed after the run in `1731e904`), and 97% of 3232 - restarts reached NORMAL_START. Confirm any stall fix against the full round-robin: - small-subset re-runs have repeatedly overstated fixes. **Not built:** a CI job running the + 4 of 66 matches stalled, and 97% of 3232 restarts reached NORMAL_START. The kick-off that + waited the referee's 15 s cap is fixed (`1731e904`); the three committed robots frozen about + 10 s (`COMMITTED_FROZEN` in `stall_incidents`) are not yet diagnosed. Confirm any stall fix + against the full round-robin: small-subset re-runs have repeatedly overstated fixes. **Not built:** a CI job running the strict seeded stall gate — `.github/workflows/ci.yml` runs the test suite only. 2. **`BangBang1D` defects** — required-overshoot and `v0 > v_max` cases produce discontinuous @@ -84,8 +84,7 @@ entry by its title, not its number. 6. **grsim as a CI/tournament environment.** Unverified whether grsim installs on a GitHub runner or can run faster than real time headless. CI hardcodes `--ignore-glob "**/*grsim*"` - (`ci.yml:41,43`). `vendor/rSim/FORK_NOTES.md:16` still points at a roadmap section that no - longer exists (it meant this entry). + (`ci.yml:41,43`). 7. **Match timeline in the dashboard.** `tools/match_timeline.py` draws one window as swimlanes (referee band, each robot's tactic, possession, distance to the ball, events). The Replay tab @@ -167,7 +166,7 @@ entry by its title, not its number. 11. **Vision filter for real cameras.** Found 2026-10-09 reviewing `data_processing/`; rsim (noise off) never exercises any of it. Needs logged SSL-Vision data from the real field - to tune, so it waits on the hardware team like 10c. + to tune, so it waits on the hardware team like "Real-robot profiles and calibration". - *Velocity:* `KalmanFilter` tracks position only; `VelocityRefiner` differentiates consecutive filtered positions and that velocity feeds the next prediction. With the current noise settings (process noise twice the measurement noise) the steady-state @@ -194,12 +193,15 @@ entry by its title, not its number. logs) are done; Phase 2's knobs are built — `SimNoise` seeds latency, kick speed and direction spread, two-phase ball friction, per-robot acceleration limits and vision noise/dropouts per match, and is wired through `run_match`, `evaluate.py` and `pool_v1` - (`v3_seed2`). A sensitivity sweep was run (results in `sim_realism.md`). Still open: the + (the v3 ranges; the 2026-10-17 round-robin played them as `v3_seed2`). A sensitivity sweep + was run (results in `sim_realism.md`). Still open: the scorecard's remaining gaps (sim keepers save about twice as many good chances, and sim shots almost never go wide), the 2022-24 held-out check, choosing which knobs become the evaluation default, and using that noise in a round-robin by default (`round_robin.py` - still needs an explicit `--sim-noise`). P1 (a real-data value model) and P2 (structured - strategy search) are not started. + still needs an explicit `--sim-noise`). P1, the real-data value model, is fitted (passes 1-3, + 2026-10-16/17, in `~/dev/ssl/xg-analysis/value/`; AUC about 0.8 on held-out years) but does + not yet rank strategies better than goals, so it explains results rather than saving matches + (`sim_realism.md`, "P1"). P2 (structured strategy search scored by it) is not started. 13. **Deferred, revisit only when forced** (minimalism): - `@pytest.mark.engine` marker for kernel tests — wait for real CI bottleneck data. diff --git a/docs/sim_realism.md b/docs/sim_realism.md index bfea15afe..7c05244a0 100644 --- a/docs/sim_realism.md +++ b/docs/sim_realism.md @@ -340,8 +340,8 @@ direction SD 3° (per robot 1.5°), ball slide 2.5–4.0 then roll 0.27–0.33 m All of these are built (2026-10-17): `SimNoise` draws each knob per match from its range, seeded and recorded in the replay, and reaches `run_match`, the round-robin option, `evaluate.py` and -`pool_v1` (`v3_seed2`). A knob at zero reproduces the old behaviour, so results played without -`--sim-noise` are unchanged. +`pool_v1` (the v3 ranges, played as `v3_seed2` in the 2026-10-17 round-robin). A knob at zero +reproduces the old behaviour, so results played without `--sim-noise` are unchanged. Each knob is off by default, so current results are unchanged, and each comes with a test that pins its effect: diff --git a/docs/testing_gaps.md b/docs/testing_gaps.md index 16b047ed7..849d223b1 100644 --- a/docs/testing_gaps.md +++ b/docs/testing_gaps.md @@ -60,4 +60,4 @@ keep-out circle, and must keep passing. finish timeout (`ae6a6f3`), `ball_is_loose` ignoring a legally barred enemy near our defense area (`10fc89c`); and a same-team two-robot ball scrum (`_teammate_already_has_ball()`, `a59a8e5`). Cross-tactic convergence in general is - roadmap item 5. + the roadmap's "Cross-tactic ball convergence". diff --git a/tools/metric_correlation.py b/tools/metric_correlation.py index 2cf0c00e5..80debc3b7 100644 --- a/tools/metric_correlation.py +++ b/tools/metric_correlation.py @@ -157,7 +157,7 @@ _RESTART_LIVE_COMMANDS = {"NORMAL_START", "FORCE_START"} # --- Metrics 8-12 (2026-09-04 debugging-signal expansion): each mined from a -# real, already-traced bug mechanism in docs/roadmap.md item 15, rather than +# real, already-traced bug mechanism in docs/roadmap.md's "trajsample liveness fixes", rather than # invented speculatively -- the point is a cheap counter that would have # flagged that exact mechanism automatically instead of needing a human to # live-trace a match. See each metric's own comment below for which bug it diff --git a/utama_core/custom_referee/actions.py b/utama_core/custom_referee/actions.py index f2b82f6a6..1bd241560 100644 --- a/utama_core/custom_referee/actions.py +++ b/utama_core/custom_referee/actions.py @@ -526,7 +526,7 @@ def update(self) -> None: # Pick the placer: robot closest to the ball, sticky across ticks # (see _PLACER_REASSIGN_MARGIN_M) -- same bug shape as - # DirectFreeOursStep's kicker-identity thrashing (roadmap item 15): + # DirectFreeOursStep's kicker-identity thrashing (roadmap "trajsample liveness fixes"): # a bare min(..., key=distance) recomputed fresh every tick flips # identity on ordinary sim noise whenever two-plus robots are # near-tied, resetting whoever newly "wins" to a standing start. @@ -909,7 +909,7 @@ class DirectFreeOursStep: # `empty_command()` immediately), so the role never holds long enough # for any robot to actually close the distance — this was the real # mechanism behind the DIRECT_FREE stall traced in - # clear_danger_vs_clear_press_plus (roadmap item 15), not the + # clear_danger_vs_clear_press_plus (roadmap "trajsample liveness fixes"), not the # multi-robot planner congestion first suspected from replay data alone. _KICKER_REASSIGN_MARGIN_M = 0.3 diff --git a/utama_core/custom_referee/geometry.py b/utama_core/custom_referee/geometry.py index da4a766e9..b3d8462be 100644 --- a/utama_core/custom_referee/geometry.py +++ b/utama_core/custom_referee/geometry.py @@ -157,7 +157,7 @@ def legal_restart_position(self, x: float, y: float, keep_dist: float) -> tuple[ scoreboard -- it lets the very defender/attacker that caused the violation instantly re-trigger it, deadlocking the match in a rapid STOP/FORCE_START churn. Confirmed live for `DefenseAreaRule` - 2026-09-04 (roadmap item 15/16): 6 churn cycles in under 2 seconds + 2026-09-04 (roadmap "`custom_referee` missing-`designated_position` audit"): 6 churn cycles in under 2 seconds from exactly this gap. Only x needs clamping for either defense area (both are full-width rectangles spanning the goal line to `2*half_defense_depth` in) -- y is already legal by construction diff --git a/utama_core/motion_planning/src/controllers/trajsampling.py b/utama_core/motion_planning/src/controllers/trajsampling.py index b11601be2..bb7385aac 100644 --- a/utama_core/motion_planning/src/controllers/trajsampling.py +++ b/utama_core/motion_planning/src/controllers/trajsampling.py @@ -119,7 +119,7 @@ def calculate( # steered off-plan by its own brake drifts far enough to keep # re-triggering that same invalidation -- confirmed live on # `clear_danger_vs_clear_press_plus`'s DIRECT_FREE stall - # (roadmap item 15): braking fired ~35% of ticks near a + # (roadmap "trajsample liveness fixes"): braking fired ~35% of ticks near a # crowded obstacle, and every one of dozens of subsequent # replans was triggered by a ~0.08-0.09m position deviation, # right at the tolerance threshold -- the robot visibly diff --git a/utama_core/motion_planning/src/trajsampling/planner.py b/utama_core/motion_planning/src/trajsampling/planner.py index debb2fac6..19af4e70c 100644 --- a/utama_core/motion_planning/src/trajsampling/planner.py +++ b/utama_core/motion_planning/src/trajsampling/planner.py @@ -295,7 +295,7 @@ def _has_priority(robot_id: int, other_id: int, game: Optional[Game] = None) -> # on formation -- none of them settle for more than a fraction of a second, # so within priority-blocking's collision margin there is almost always some # higher-priority teammate's (itself about to be replaced) trajectory -# crossing somewhere. Confirmed live (roadmap item 15/16): a +# crossing somewhere. Confirmed live (roadmap "trajsample liveness fixes"): a # PREPARE_KICKOFF_YELLOW restart's designated kicker -- always the lowest # non-keeper robot ID, so by `_has_priority`'s fixed ordering also the LOWEST # priority outfield robot -- had every direct-line candidate to the ball @@ -367,8 +367,8 @@ def _priority_blocking_enabled(game: Game) -> bool: # invalidate an already-committed, currently-collision-clear trajectory just # because a higher-priority teammate's OWN committed trajectory crosses its # margin somewhere in that window. Originally widened to the full -# `MAX_LOOKAHEAD_TIME` (1.5s, see roadmap item 15's Sumatra-fidelity audit, -# finding #3) so a robot would notice a priority conflict as soon as it +# `MAX_LOOKAHEAD_TIME` (1.5s, see the roadmap's "Sumatra-fidelity audit, +# top 3 findings", finding #3) so a robot would notice a priority conflict as soon as it # appeared anywhere in its remaining plan, not only once `elapsed` caught up # to it. That's correct when the crossing trajectory is settled, but at a # kickoff/formation restart every non-keeper teammate is SIMULTANEOUSLY @@ -1295,7 +1295,7 @@ def _collision_leniency_accepts( instability in the same restart-congestion family as `_intermediate_targets`'s stale-direction/side-flip bugs: live-traced on `counter_press_vs_overload_flow`'s PREPARE_KICKOFF_BLUE stall - (roadmap item 15/16), where the blocking teammate was itself still + (roadmap "trajsample liveness fixes"), where the blocking teammate was itself still actively moving to ITS OWN formation spot (not one near-stationary robot, the earlier-fixed case) -- the collision was ~0.9m from the kicker's destination, so leniency never applied at all regardless of diff --git a/utama_core/shared/tolerance.py b/utama_core/shared/tolerance.py index 0f4dba649..fe7a96aef 100644 --- a/utama_core/shared/tolerance.py +++ b/utama_core/shared/tolerance.py @@ -1,6 +1,6 @@ """Two small, pure primitives factoring out a hand-rolled pattern that had reached six independent instances across the tactic/skill layer (see -`docs/roadmap.md`'s "Open" item 1 for the inventory that triggered this). +`docs/roadmap.md`'s "Shared hysteresis helper"). Both bugs below are boundary-condition bugs on otherwise-correct logic, not missing features — every instance already *knew* it needed a tolerance or a @@ -28,7 +28,7 @@ margin) and are what `Sticky.update()`'s signature is designed to fit, not a hypothetical generalization of them. -Deliberately NOT covered here (see roadmap item 1's classification): +Deliberately NOT covered here (see the roadmap's "Shared hysteresis helper"): - Tick-count timeouts (`_PHASE_TIMEOUT_TICKS` and friends) — a different shape (a counter vs. a threshold), and a kernel-level commitment deadline is arriving separately; per-tactic timeouts are fine to leave hand-rolled. diff --git a/utama_core/tactics/defense.py b/utama_core/tactics/defense.py index ba061a998..ffb53c03e 100644 --- a/utama_core/tactics/defense.py +++ b/utama_core/tactics/defense.py @@ -25,7 +25,7 @@ now `has_ball`), so `retriever_id` resets to `None` and the ball-carrying robot falls straight back into `defend_parameter` — pure shot-shadow positioning with no ball awareness at all — and just drags the ball along -with it for the rest of the match. Found live (roadmap item 15, +with it for the rest of the match. Found live (roadmap "trajsample liveness fixes", 2026-09-04): a `low_block` defender sliding along its own back line with the ball glued to its dribbler, a permanent `COMMITTED_FROZEN` liveness stall. Fixed by tracking `carrier_id` (the last tick's retriever, if it diff --git a/utama_core/tests/custom_referee/test_custom_referee.py b/utama_core/tests/custom_referee/test_custom_referee.py index 4abaa48ec..9b8c66ba0 100644 --- a/utama_core/tests/custom_referee/test_custom_referee.py +++ b/utama_core/tests/custom_referee/test_custom_referee.py @@ -396,7 +396,7 @@ def test_double_touch_charges_offending_teams_for_foul_counter(self): def test_designated_position_is_projected_clear_of_the_defense_area(self): """Regression, same class of bug as DefenseAreaRule/KeeperHeldBallRule/ - ExcessiveDribblingRule/PushingRule/BallSpeedRule (roadmap item 15/16): a + ExcessiveDribblingRule/PushingRule/BallSpeedRule (roadmap "`custom_referee` missing-`designated_position` audit"): a double touch resolved right at a defense-area edge must not hand back a `designated_position` inside that box.""" rule = DoubleTouchRule() diff --git a/utama_core/tests/custom_referee/test_dribble_placement_stopspeed.py b/utama_core/tests/custom_referee/test_dribble_placement_stopspeed.py index 1b78910e5..0be2a8526 100644 --- a/utama_core/tests/custom_referee/test_dribble_placement_stopspeed.py +++ b/utama_core/tests/custom_referee/test_dribble_placement_stopspeed.py @@ -78,7 +78,7 @@ def test_designated_position_is_projected_clear_of_the_defense_area(self): straight to `designated_position` and force-starts play the same tick, so an in-box position here would let the same violation instantly re-trigger (same class of bug fixed for `DefenseAreaRule`, - roadmap item 15/16).""" + roadmap "`custom_referee` missing-`designated_position` audit").""" rule = KeeperHeldBallRule(max_hold_seconds=10.0) v = None for ts in [0.0, 5.0, 10.5]: diff --git a/utama_core/tests/custom_referee/test_pushing_crashing.py b/utama_core/tests/custom_referee/test_pushing_crashing.py index eae301be3..1bf38fdd5 100644 --- a/utama_core/tests/custom_referee/test_pushing_crashing.py +++ b/utama_core/tests/custom_referee/test_pushing_crashing.py @@ -120,7 +120,7 @@ def test_symmetric_push_no_fault(self): def test_designated_position_is_projected_clear_of_the_defense_area(self): """Regression, same class of bug as KeeperHeldBallRule/ - ExcessiveDribblingRule/DefenseAreaRule (roadmap item 15/16): a + ExcessiveDribblingRule/DefenseAreaRule (roadmap "`custom_referee` missing-`designated_position` audit"): a shoving match plausibly happens right at a defense-area edge (defenders contesting a ball near their own box), so the ball position recorded at push-start must be projected clear before use diff --git a/utama_core/tests/engine/test_all_tactics.py b/utama_core/tests/engine/test_all_tactics.py index 8f72cd86f..839b7ffc6 100644 --- a/utama_core/tests/engine/test_all_tactics.py +++ b/utama_core/tests/engine/test_all_tactics.py @@ -138,8 +138,8 @@ def test_commands_every_assigned_robot_on_two_ticks(tactic_cls, robot_ids, exp_f def test_defense_carrier_stays_assigned_after_retriever_gets_the_ball(runner_factory): - """Regression test for the COMMITTED_FROZEN liveness stall (roadmap item - 15, 2026-09-04): once a retriever actually acquires the ball, + """Regression test for the COMMITTED_FROZEN liveness stall (roadmap "trajsample liveness fixes", + 2026-09-04): once a retriever actually acquires the ball, `ball_is_loose` goes False next tick and `retriever_id` resets to `None` — before the fix, that dropped the ball-carrying robot straight back into `defend_parameter`'s pure shot-shadow positioning, which has no diff --git a/utama_core/tests/motion_planning/fastpathplanner_corner_test.py b/utama_core/tests/motion_planning/fastpathplanner_corner_test.py index 4e6c2bcb6..bb32a2f85 100644 --- a/utama_core/tests/motion_planning/fastpathplanner_corner_test.py +++ b/utama_core/tests/motion_planning/fastpathplanner_corner_test.py @@ -9,13 +9,13 @@ - safety, every scenario: the carrot is always finite and the robot never enters the real enemy defense area; -- arrival: the robot gets within 0.1 m of the target within 10 s. Scenarios that - currently fail are pinned as strict xfails. They are FPP's known local minimum - (roadmap item 5): the detour side flips every tick between two positions when - the gap between a blocker and the inflated box is narrower than the clearance, - because the side memory is keyed on the obstacle hit point, which changes - between the two. A planner fix turns them into XPASS, which fails the run so - this list gets updated. +- arrival: the robot gets within 0.1 m of the target within 10 s. A scenario that + fails is pinned as a strict xfail in `_KNOWN_STUCK`, empty since #147 fixed FPP's + detour-side flip: the detour side flipped every tick between two positions when + the gap between a blocker and the inflated box was narrower than the clearance, + because the side memory was keyed on the obstacle hit point, which changes + between the two. A planner fix turns a pinned seed into XPASS, which fails the + run so this list gets updated. Every blocker keeps the route around the outside open, so all targets are reachable. """ @@ -107,7 +107,7 @@ def test_corner_route_is_safe(seed): "seed", [ ( - pytest.param(s, marks=pytest.mark.xfail(strict=True, reason="FPP detour-side flip (roadmap item 5)")) + pytest.param(s, marks=pytest.mark.xfail(strict=True, reason="FPP detour-side flip")) if s in _KNOWN_STUCK else ( pytest.param(s, marks=pytest.mark.xfail(strict=False, reason="on the edge of the flip; differs by CPU")) diff --git a/utama_core/tests/motion_planning/implementation/trajsampling_controller_test.py b/utama_core/tests/motion_planning/implementation/trajsampling_controller_test.py index 74867fcb6..d09d47a2c 100644 --- a/utama_core/tests/motion_planning/implementation/trajsampling_controller_test.py +++ b/utama_core/tests/motion_planning/implementation/trajsampling_controller_test.py @@ -43,7 +43,7 @@ def _game_with_robot(p_xy, v_xy) -> Game: def test_brake_scales_planned_velocity_not_robot_raw_velocity(): """Regression for the DIRECT_FREE congestion "retreat" bug traced live on - `clear_danger_vs_clear_press_plus` (roadmap item 15): the emergency-brake + `clear_danger_vs_clear_press_plus` (roadmap "trajsample liveness fixes"): the emergency-brake layer used to return `robot.v * brake_scale` -- the robot's own raw, already-current velocity, scaled down -- instead of the PLANNED trajectory's velocity, scaled down. Whenever the robot's actual momentum diff --git a/utama_core/tests/motion_planning/implementation/trajsampling_correctness_test.py b/utama_core/tests/motion_planning/implementation/trajsampling_correctness_test.py index 08723b6e5..0eba11b62 100644 --- a/utama_core/tests/motion_planning/implementation/trajsampling_correctness_test.py +++ b/utama_core/tests/motion_planning/implementation/trajsampling_correctness_test.py @@ -509,7 +509,7 @@ def test_try_reuse_notices_a_priority_obstacle_only_encroaching_later_in_the_tra def test_try_reuse_does_not_invalidate_on_a_priority_conflict_beyond_the_recheck_window(): """Regression for the 231/231 full-catalog stall this session traced to - `_try_reuse`'s priority re-check (see roadmap item 15/16): when it scanned + `_try_reuse`'s priority re-check (see roadmap "trajsample liveness fixes"): when it scanned the WHOLE remaining trajectory (up to the old `MAX_LOOKAHEAD_TIME`, 1.5s) instead of the current, much shorter `_PRIORITY_RECHECK_LOOKAHEAD_TIME` (0.5s), every `PREPARE_KICKOFF_YELLOW` restart stalled forever: all 5 @@ -561,7 +561,7 @@ def test_try_reuse_does_not_invalidate_on_a_priority_conflict_beyond_the_recheck def test_try_reuse_ignores_priority_conflict_when_priority_blocking_disabled(): - """Pins the restart-formation fix (roadmap item 15/16, session following + """Pins the restart-formation fix (roadmap "trajsample liveness fixes", session following the recheck-window fix above): even after shrinking the recheck window to 0.5s, a real kickoff still stalled 100% of the time in a live `debug_match.py` run, traced to a DIFFERENT mechanism than the recheck @@ -703,7 +703,7 @@ def _own_obstacle_after(planner, robot_xy, ts): def test_own_robot_obstacle_falls_back_to_real_state_once_robot_leaves_its_stale_plan(): - """Pins the DIRECT_FREE ghost-obstacle deadlock (roadmap item 15). + """Pins the DIRECT_FREE ghost-obstacle deadlock (roadmap "trajsample liveness fixes"). Robot 4 committed a plan ending next to the ball, then stopped being planned (`empty_command()` during a restart) and was later found 2m @@ -745,7 +745,7 @@ def test_own_robot_obstacle_keeps_priority_for_robot_resting_on_its_completed_pl def test_intermediate_targets_drops_a_stale_backward_pointing_last_target(): """Regression for the DIRECT_FREE_BLUE congestion stall live-traced on - `clear_danger_vs_shadow_switch` (roadmap item 15). `_intermediate_targets` + `clear_danger_vs_shadow_switch` (roadmap "trajsample liveness fixes"). `_intermediate_targets` always retried the previous tick's winning detour target FIRST, and `plan()` commits to the first collision-free candidate it finds without ever comparing it against the freshly-sorted, toward-target candidates @@ -834,7 +834,7 @@ def test_intermediate_targets_keeps_a_last_target_that_is_still_a_reasonable_det def test_intermediate_targets_inserts_same_side_jitter_around_a_kept_last_target(): """Regression for the PREPARE_KICKOFF_YELLOW side-flip stall live-traced - on `clear_danger_vs_overload_flow` (roadmap item 15/16): when `last` + on `clear_danger_vs_overload_flow` (roadmap "trajsample liveness fixes"): when `last` itself is re-tried and turns out to still be blocked (a narrow, near- static obstacle collision that doesn't clear from one tick to the next), falling straight through to `config.N_INTERMEDIATE_TARGETS` fully @@ -885,7 +885,7 @@ def test_intermediate_targets_inserts_same_side_jitter_around_a_kept_last_target def test_collision_leniency_accepts_a_far_collision_when_priority_blocking_disabled(): """Regression for the PREPARE_KICKOFF_BLUE stall live-traced on - `counter_press_vs_overload_flow` (roadmap item 15/16): during a restart + `counter_press_vs_overload_flow` (roadmap "trajsample liveness fixes"): during a restart (`priority_enabled=False`, mass-simultaneous replans, see `_priority_blocking_enabled`), a slow non-priority graze against a teammate who is itself still moving to its own formation spot was @@ -1020,7 +1020,7 @@ def test_plan_accepts_direct_trajectory_through_a_slow_obstacle_parked_on_target def test_with_current_clearance_excludes_an_obstacle_the_robot_is_still_escaping(): """Regression for the escaping-grace / clearance-report mismatch - (roadmap item 15): `_first_collision`'s numba scan grants a one-time + (roadmap "trajsample liveness fixes"): `_first_collision`'s numba scan grants a one-time "still escaping" grace to any obstacle already penetrated at t == the query start (see its own docstring -- built for GiveAndGoTactic's abandoned-receiver robot, replanning a fresh route the instant the ball diff --git a/utama_core/tests/referee/test_referee_unit.py b/utama_core/tests/referee/test_referee_unit.py index f23ca06fc..358657d18 100644 --- a/utama_core/tests/referee/test_referee_unit.py +++ b/utama_core/tests/referee/test_referee_unit.py @@ -462,7 +462,7 @@ def fake_move(game, motion_controller, robot_id, target_coords, target_oren, dri def test_placer_choice_is_sticky_across_near_tied_distances(self, monkeypatch): """Regression for the same bug shape as `DirectFreeOursStep`'s kicker - thrashing (roadmap item 15): a bare `min(..., key=distance)` + thrashing (roadmap "trajsample liveness fixes"): a bare `min(..., key=distance)` recomputed fresh every tick flips identity on ordinary sim noise whenever two-plus robots are near-tied, resetting whoever newly "wins" to a standing start. `BallPlacementOursStep` must hold its @@ -2081,7 +2081,7 @@ def test_writes_command_for_every_robot(self, monkeypatch): def test_kicker_choice_is_sticky_across_near_tied_distances(self, monkeypatch): """Regression for the DIRECT_FREE congestion stall root-caused via - `clear_danger_vs_clear_press_plus` (roadmap item 15): three robots + `clear_danger_vs_clear_press_plus` (roadmap "trajsample liveness fixes"): three robots within a couple cm of each other's distance to the ball flipped which one `min(..., key=distance)` called "closest" on ordinary rsim position noise, ~9 times/second, so no robot ever held the From c84556ea735b982f6b1d596af7f65bbd17de488a Mon Sep 17 00:00:00 2001 From: isaac0804 Date: Wed, 21 Oct 2026 18:45:55 +0100 Subject: [PATCH 03/56] fix(engine): skip the pinned goalkeeper when its robot is not on the field A team with no robot of goalkeeper_id (default 0) crashed on its first tick with KeyError in the goalkeeper tactic. Real tests often run without robot 0. Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_01TJGHdb8Dht67hn4Lohndf5 --- utama_core/engine/strategy.py | 8 ++++++ utama_core/tests/engine/test_strategy.py | 36 ++++++++++++++++++++++++ 2 files changed, 44 insertions(+) diff --git a/utama_core/engine/strategy.py b/utama_core/engine/strategy.py index 81e61468e..c3222fcd7 100644 --- a/utama_core/engine/strategy.py +++ b/utama_core/engine/strategy.py @@ -496,7 +496,15 @@ def _tick_pinned(self, game: Game) -> dict[RobotId, RobotCommand]: Never touches `_slots`/`_prev_partition`/`_pending_barrier_reset` — the pinned slot has no partition membership to reconcile, it simply runs every tick it's called. + + Skipped while the pinned robot is not on the field: a team need not + field robot `goalkeeper_id` (real robots keep their vision IDs, so a + two-robot test rarely includes ID 0), and the goalkeeper skills index + `game.friendly_robots` by that ID. """ + robots = getattr(game, "friendly_robots", None) + if robots is not None and self._pinned_robot_id not in robots: + return {} slot = self._pinned_slot commands, slot.mem = slot.tactic.tick(game, self._ctx, (self._pinned_robot_id,), slot.mem) if self.match_log is not None: diff --git a/utama_core/tests/engine/test_strategy.py b/utama_core/tests/engine/test_strategy.py index d9fc98cc9..fab1b63f6 100644 --- a/utama_core/tests/engine/test_strategy.py +++ b/utama_core/tests/engine/test_strategy.py @@ -915,3 +915,39 @@ def test_barrier_reset_clears_closer_to_ball_hysteresis_state(): strategy.tick(game2) assert True not in _CLOSER_TO_BALL_STATE + + +# --- pinned goalkeeper slot --- + + +class _GameWithRobots(_FakeGame): + def __init__(self, friendly_ids): + super().__init__() + self.friendly_robots = {rid: object() for rid in friendly_ids} + + +def _strategy_with_pinned_keeper(keeper: RecordingTactic) -> Strategy: + strategy = Strategy( + tactics={"a": RecordingTactic()}, + partitioner=Strategy.single_tactic_picker(lambda game, active: "a"), + outfield_robot_ids=(1,), + ctx=_ctx(), + ) + strategy.set_goalkeeper(0, tactic=keeper) + return strategy + + +def test_pinned_goalkeeper_is_skipped_when_its_robot_is_not_on_the_field(): + # A real two-robot test fields vision IDs 1 and 3, say, with no robot 0: the + # goalkeeper skills index friendly_robots[0] and raised KeyError. + keeper = RecordingTactic() + strategy = _strategy_with_pinned_keeper(keeper) + assert strategy.tick(_GameWithRobots({1})) == {1: "cmd-1"} + assert keeper.last_robot_ids == () + + +def test_pinned_goalkeeper_ticks_when_its_robot_is_on_the_field(): + keeper = RecordingTactic() + strategy = _strategy_with_pinned_keeper(keeper) + assert strategy.tick(_GameWithRobots({0, 1})) == {1: "cmd-1", 0: "cmd-0"} + assert keeper.last_robot_ids == (0,) From b1516df26032ca901c0791a3eb73a05219fe5e27 Mon Sep 17 00:00:00 2001 From: isaac0804 Date: Wed, 21 Oct 2026 18:45:55 +0100 Subject: [PATCH 04/56] fix(tactics): hysteresis on the shot aim in LeadAndSupport and GiveAndGo Against a centred keeper the two corners are near-equal, and the aim flipped between them every tick; a lone carrier turned back and forth ~11 s before shooting. Uses find_best_shot's switch margin, as _pass_and_score does. Changes strategy code: cached matches rerun. Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_01TJGHdb8Dht67hn4Lohndf5 --- utama_core/tactics/give_and_go.py | 17 +++++++- utama_core/tactics/lead_and_support.py | 18 +++++++- .../tests/engine/test_give_and_go_tactic.py | 40 +++++++++++++++++ .../engine/test_lead_and_support_tactic.py | 43 +++++++++++++++++++ 4 files changed, 116 insertions(+), 2 deletions(-) diff --git a/utama_core/tactics/give_and_go.py b/utama_core/tactics/give_and_go.py index 3eb5414de..5488a6d9f 100644 --- a/utama_core/tactics/give_and_go.py +++ b/utama_core/tactics/give_and_go.py @@ -148,6 +148,7 @@ def _best_receiver(game: Game, carrier_id: int, candidate_ids: tuple[int, ...]) _MIN_SAFE_PASS_DISTANCE = 0.7 # metres — matches score_pass_setup's own min_pass_distance +_SHOT_SWITCH_MARGIN = 0.15 # metres a new shot gap must beat the aimed-at one by; same as _pass_and_score's def _nearest_safe_receiver(game: Game, carrier_id: int, candidate_ids: tuple[int, ...]) -> Optional[int]: @@ -274,6 +275,7 @@ class GiveAndGoMem: lane_blocked_ticks: int = 0 # consecutive ticks _pass_exec reported the lane blocked; feeds early hop abandon carry_origin: Optional[Vector2D] = None # see shared carry_origin; caps the no-lane strafe first_touch_hold_spent: bool = False # the no-receiver hold ran its _MAX_HOP_TICKS once; never hold again + prev_best_shot_y: Optional[float] = None # last tick's aim, for find_best_shot's switch margin (see below) class GiveAndGoTactic(BaseTactic[GiveAndGoMem]): @@ -381,6 +383,7 @@ def tick( and ball_line_receive_point(game, mem.receiver_id) is not None ) if not carrier_has_ball and not pass_rolling: + mem.prev_best_shot_y = None if ball_in_own_defense_area(game): # The ball is inside our own box — an outfield robot may not # enter it (DefenseAreaRule: the keeper owns the area). Hold @@ -550,7 +553,19 @@ def tick( # cap and are forcing one regardless of lane quality. goal_x, goal_y1, goal_y2 = enemy_goal_line(game) carrier_pos = game.friendly_robots[carrier_id].p - best_shot_y, _gap = find_best_shot(carrier_pos, list(game.enemy_robots.values()), goal_x, goal_y1, goal_y2) + # With hysteresis: against a centred keeper the two corners are near-equal, + # and the aim flipped between them every tick, so a lone carrier turned back + # and forth ~11 s before shooting (1v1 on a small field, 2026-10-21). + best_shot_y, _gap = find_best_shot( + carrier_pos, + list(game.enemy_robots.values()), + goal_x, + goal_y1, + goal_y2, + prev_best_shot_y=mem.prev_best_shot_y, + switch_margin=_SHOT_SWITCH_MARGIN, + ) + mem.prev_best_shot_y = best_shot_y if ctx.match_log is not None: ctx.match_log.trace_if_changed( diff --git a/utama_core/tactics/lead_and_support.py b/utama_core/tactics/lead_and_support.py index a3b94f34c..8c02b543f 100644 --- a/utama_core/tactics/lead_and_support.py +++ b/utama_core/tactics/lead_and_support.py @@ -50,6 +50,7 @@ _MIN_SUPPORT_SEPARATION = 0.9 # metres — supports must not crowd each other or the leader _SUPPORT_FORWARD_BIAS = 0.35 # weight favouring support points closer to the enemy goal +_SHOT_SWITCH_MARGIN = 0.15 # metres a new shot gap must beat the aimed-at one by; same as _pass_and_score's def _candidate_support_points(game: Game, count: int) -> list[Vector2D]: @@ -143,6 +144,11 @@ def choose_leader(game: Game, robot_ids: tuple[int, ...]) -> int: @dataclass class LeadAndSupportMem: leader_id: Optional[int] = None + # The shot y aimed at last tick, for find_best_shot's switch-margin hysteresis: + # against a centred keeper the two corners are near-equal, and without it the + # aim flipped between them every tick, so the leader turned back and forth for + # ~11 s before drift broke the tie (1v1 on a small field, 2026-10-21). + prev_best_shot_y: Optional[float] = None class LeadAndSupportTactic(BaseTactic[LeadAndSupportMem]): @@ -184,6 +190,7 @@ def tick( leader_has_ball = has_ball(game, leader_id) if not leader_has_ball: + mem.prev_best_shot_y = None # go_to_ball itself holds outside the enemy's box instead of # chasing an unreachable ball when the keeper is holding it # there during active play — see that function's docstring. @@ -192,7 +199,16 @@ def tick( ) else: goal_x, goal_y1, goal_y2 = enemy_goal_line(game) - best_shot_y, gap = find_best_shot(leader_pos, list(game.enemy_robots.values()), goal_x, goal_y1, goal_y2) + best_shot_y, gap = find_best_shot( + leader_pos, + list(game.enemy_robots.values()), + goal_x, + goal_y1, + goal_y2, + prev_best_shot_y=mem.prev_best_shot_y, + switch_margin=_SHOT_SWITCH_MARGIN, + ) + mem.prev_best_shot_y = best_shot_y if best_shot_y is None: # See `no_shot_reposition_target`'s docstring: freezing here # never resolves against a stationary blocker. diff --git a/utama_core/tests/engine/test_give_and_go_tactic.py b/utama_core/tests/engine/test_give_and_go_tactic.py index 54f764406..16891be35 100644 --- a/utama_core/tests/engine/test_give_and_go_tactic.py +++ b/utama_core/tests/engine/test_give_and_go_tactic.py @@ -628,3 +628,43 @@ def test_a_receiver_that_left_the_slot_is_dropped(): assert mem.receiver_id != 5 assert mem.carrier_id == 1 and mem.hop_count == 1 # the possession is still the same one + + +class _OrenRecordingMotionController(MotionController): + def __init__(self): + super().__init__(mode="rsim") + self.last_target_oren: Optional[float] = None + + def calculate(self, game, robot_id, target_pos, target_oren): + self.last_target_oren = target_oren + return Vector2D(0.0, 0.0), 0.0 + + +def _aimed_side(keeper_ys) -> list[bool]: + """A lone carrier 2 m from the enemy goal (x = -4.5), facing away from it, ticked + against a keeper on the goal line at each y in turn; per tick, whether the heading + it turns to aims below the goal's centre (y < 0: the aim is near ±pi, so its sign gives the side).""" + mc = _OrenRecordingMotionController() + ctx = TickContext(motion_controller=mc, match_log=None) + tactic = GiveAndGoTactic() + mem = tactic.initial_mem() + sides = [] + for keeper_y in keeper_ys: + game = _make_game(Vector2D(-2.5, 0.0), Vector2D(0.0, 2.0), Vector2D(-4.4, keeper_y)) + _, mem = tactic.tick(game, ctx, (1,), mem) + sides.append(mc.last_target_oren < 0) + return sides + + +def test_lone_carrier_keeps_its_aim_while_a_centred_keeper_jitters(): + """Against a centred keeper both corners are near-equal; ±1 cm of keeper jitter + flipped the aim between them every tick, so a lone carrier turned back and forth + about 11 s instead of shooting (found in a 1v1 on a small field).""" + assert len(set(_aimed_side([0.01, -0.01, 0.01, -0.01]))) == 1 + + +def test_lone_carrier_switches_aim_when_the_keeper_clearly_opens_the_other_side(): + aims_low = _aimed_side([0.01])[0] + # The keeper steps 0.3 m toward the aimed-at corner: the other side is now far wider. + first, after_step = _aimed_side([0.01, -0.3 if aims_low else 0.3]) + assert first != after_step diff --git a/utama_core/tests/engine/test_lead_and_support_tactic.py b/utama_core/tests/engine/test_lead_and_support_tactic.py index 5273a1868..7cebe1735 100644 --- a/utama_core/tests/engine/test_lead_and_support_tactic.py +++ b/utama_core/tests/engine/test_lead_and_support_tactic.py @@ -104,3 +104,46 @@ def test_leader_chases_ball_directly_when_ball_is_not_in_enemy_defense_area(): # go_to_ball targets at (or an overshoot past) the ball itself, not a # defense-area hold point far from it. assert target.distance_to(Vector2D(1.0, 0.0)) < 0.5 + + +def _leader_with_ball_facing_keeper(keeper_y: float) -> Game: + """my_team_is_right=True: the enemy goal is at x = -4.5. Leader 2 m out with the + ball, facing away from the goal, the keeper on the goal line near its centre.""" + leader = Robot( + id=1, + is_friendly=True, + has_ball=True, + p=Vector2D(-2.5, 0.0), + v=Vector2D(0, 0), + a=Vector2D(0, 0), + orientation=0.0, + ) + keeper = _robot(0, -4.4, keeper_y, False) + return _game({1: leader}, {0: keeper}, (-2.41, 0.0)) + + +def _aimed_side(keeper_ys) -> list[bool]: + """Tick the leader against each keeper y in turn; per tick, whether the heading it + turns to aims below the goal's centre (y < 0: the aim is near ±pi, so its sign gives the side).""" + tactic = LeadAndSupportTactic() + mem = tactic.initial_mem() + ctx = _ctx() + sides = [] + for keeper_y in keeper_ys: + _, mem = tactic.tick(_leader_with_ball_facing_keeper(keeper_y), ctx, robot_ids=(1,), mem=mem) + sides.append(ctx.motion_controller.calculate.call_args.kwargs["target_oren"] < 0) + return sides + + +def test_leader_keeps_its_aim_while_a_centred_keeper_jitters(): + """Against a centred keeper both corners are near-equal; ±1 cm of keeper jitter + flipped the aim between them every tick, so the leader turned back and forth + instead of shooting (found in a 1v1 on a small field).""" + assert len(set(_aimed_side([0.01, -0.01, 0.01, -0.01]))) == 1 + + +def test_leader_switches_aim_when_the_keeper_clearly_opens_the_other_side(): + aims_low = _aimed_side([0.01])[0] + # The keeper steps 0.3 m toward the aimed-at corner: the other side is now far wider. + first, after_step = _aimed_side([0.01, -0.3 if aims_low else 0.3]) + assert first != after_step From 09cbcd6e60a8a82f845ed817c6859e6056db6e0f Mon Sep 17 00:00:00 2001 From: isaac0804 Date: Wed, 21 Oct 2026 18:45:56 +0100 Subject: [PATCH 05/56] feat(examples): one attacker vs one defender on real robots or in sim Field, camera half, vision/firmware IDs, IR trust and tactics in one config block; operator-only referee driven from the dashboard; starts in HALT. Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_01TJGHdb8Dht67hn4Lohndf5 --- examples/real_attacker_defender.py | 212 +++++++++++++++++++++++++++++ 1 file changed, 212 insertions(+) create mode 100644 examples/real_attacker_defender.py diff --git a/examples/real_attacker_defender.py b/examples/real_attacker_defender.py new file mode 100644 index 000000000..05e63e73e --- /dev/null +++ b/examples/real_attacker_defender.py @@ -0,0 +1,212 @@ +"""examples/real_attacker_defender.py — one attacker against one defender on a small field. + +Run: + pixi run python examples/real_attacker_defender.py # real robots (MODE below) + pixi run python examples/real_attacker_defender.py --mode rsim + pixi run python examples/real_attacker_defender.py --mode grsim # grSim must be running + # the dashboard is at http://localhost:8080: press Force Start to play, Halt to freeze + +What it does: + - Two robots, one per team, both ours on one radio: the attacker attacks the goal in + the camera's half, the defender defends it. + - Everything to edit for a test day is in the "Test-day configuration" block: the + field, which part of it the camera sees, each robot's vision and firmware IDs, and + what each robot plays. + - The referee runs with every rule off and nothing advancing on its own, so play + only starts and stops from the dashboard. Real mode starts in HALT; Ctrl+C sends + stop commands to the robots before exiting. + +Before running on real robots: + - ssl-vision's field geometry (length and width) must equal FIELD below, or the + runner stops at the first geometry packet with "Field geometry mismatch". + - The radio dongle is `PORT` in `utama_core/config/settings.py` (/dev/ttyACM0). + - Real robots are capped at REAL_PARAMS.MAX_VEL (1 m/s, `config/robot_params.py`). +""" + +import argparse +import dataclasses + +from utama_core.config.field_params import FieldBounds, FieldDimensions +from utama_core.custom_referee import CustomReferee +from utama_core.custom_referee.profiles.profile_loader import ( + AutoAdvanceConfig, + GameConfig, + RefereeProfile, + RulesConfig, +) +from utama_core.dashboard import attach_dashboard +from utama_core.dashboard.views import referee as referee_view +from utama_core.engine.abstract_strategy import AbstractStrategy +from utama_core.engine.context import TickContext +from utama_core.engine.strategy import Strategy as KernelSchedulerStrategy +from utama_core.entities.referee.referee_command import RefereeCommand +from utama_core.run import StrategyRunner +from utama_core.tactics.defense import DefenseTactic +from utama_core.tactics.give_and_go import GiveAndGoTactic +from utama_core.tactics.lead_and_support import LeadAndSupportTactic +from utama_core.tactics.press_and_contain import PressAndContainTactic + +# --------------------------------------------------------------------------- +# Test-day configuration +# --------------------------------------------------------------------------- + +MODE = "real" # "real", "rsim" or "grsim"; --mode overrides it + +# PLACEHOLDER: measure the real field. Every value is a half-size in metres +# (half length, half width, ...), as in `STANDARD_FIELD_DIMS`. +FIELD = FieldDimensions( + full_field_half_length=2.0, + full_field_half_width=1.5, + half_defense_area_depth=0.3, + half_defense_area_width=0.6, + half_goal_width=0.4, + center_circle_radius=0.3, +) + +# The part of the field the camera sees: "left" or "right" half (x < 0 or x > 0 in +# ssl-vision's coordinates), or "full". The goal in that half is the one in play. +CAMERA_HALF = "right" + +ATTACKER_COLOUR = "yellow" # the defender is the other colour + +# Each robot's vision ID (its pattern on top) -> its firmware/radio ID. Swapping a +# robot is a change here only. The two firmware IDs must differ: one radio sends both. +ATTACKER_VISION_ID, ATTACKER_FIRMWARE_ID = 0, 0 +DEFENDER_VISION_ID, DEFENDER_FIRMWARE_ID = 1, 1 + +# Robots whose ball sensor (IR) is trusted for `has_ball`, by vision ID. Put every +# robot with a working IR here. An untrusted one counts as having the ball once its +# centre is within 13 cm of the ball, whatever way it faces: the attacker can stop +# ~2 cm short and kick at nothing (seen in grSim), so watch for it if IR is off. +TRUSTED_IR_VISION_IDS: frozenset[int] = frozenset() + +ATTACKER_TACTIC = "lead" # see ATTACKER_TACTICS +DEFENDER_TACTIC = "goalkeeper" # see DEFENDER_TACTICS + +DASHBOARD_PORT = 8080 + +# --------------------------------------------------------------------------- + +ATTACKER_TACTICS = { + "lead": LeadAndSupportTactic, # alone: takes the ball, turns to the open corner and shoots + "give_and_go": GiveAndGoTactic, # built to pass first; alone it plays like "lead" +} +DEFENDER_TACTICS = { + "goalkeeper": None, # the pinned GoalkeeperTactic: stays on the goal line + "defense": DefenseTactic, # shadows the shot from in front of the defense area + "press": PressAndContainTactic, # goes at the ball carrier +} + +_NO_ROBOT = 15 # a robot ID no team uses here, for the goalkeeper slot nobody fills + + +def _field_bounds(field: FieldDimensions, half: str) -> FieldBounds: + L, W = field.full_field_half_length, field.full_field_half_width + if half == "right": + return FieldBounds(top_left=(0.0, W), bottom_right=(L, -W)) + if half == "left": + return FieldBounds(top_left=(-L, W), bottom_right=(0.0, -W)) + if half == "full": + return field.full_field_bounds + raise ValueError(f"CAMERA_HALF must be 'left', 'right' or 'full', got {half!r}") + + +def _one_robot_strategy(robot_id: int, tactic_cls) -> AbstractStrategy: + """`robot_id` plays `tactic_cls`, or keeps goal when `tactic_cls` is None.""" + outfield = () if tactic_cls is None else (robot_id,) + + def _build(motion_controller) -> KernelSchedulerStrategy: + # A Strategy needs one registered tactic even with no outfield robot to run it. + return KernelSchedulerStrategy( + tactics={"play": (tactic_cls or DefenseTactic)()}, + partitioner=KernelSchedulerStrategy.single_tactic_picker(lambda game, active: "play"), + outfield_robot_ids=outfield, + ctx=TickContext(motion_controller=motion_controller), + ) + + return AbstractStrategy( + build_kernel_strategy=_build, + goalkeeper_id=robot_id if tactic_cls is None else _NO_ROBOT, + ) + + +def _operator_profile() -> RefereeProfile: + """Every rule off and every transition left to the operator, except goals.""" + defaults = RulesConfig() + rules = RulesConfig( + **{ + f.name: dataclasses.replace(getattr(defaults, f.name), enabled=(f.name == "goal_detection")) + for f in dataclasses.fields(RulesConfig) + } + ) + return RefereeProfile( + profile_name="attacker_defender_test", + rules=rules, + game=GameConfig( + half_duration_seconds=3600.0, + kickoff_team=ATTACKER_COLOUR, + force_start_after_goal=False, + auto_advance=AutoAdvanceConfig( + stop_to_next_command=False, + prepare_kickoff_to_normal=False, + prepare_penalty_to_normal=False, + direct_free_to_normal=False, + ball_placement_to_next=False, + normal_start_to_force=False, + ), + ), + ) + + +def build_runner(mode: str) -> StrategyRunner: + real = mode == "real" + # In the simulators each team's robots are numbered from 0. + attacker_id = ATTACKER_VISION_ID if real else 0 + defender_id = DEFENDER_VISION_ID if real else 0 + attacker_is_yellow = ATTACKER_COLOUR == "yellow" + if ATTACKER_COLOUR not in ("yellow", "blue"): + raise ValueError(f"ATTACKER_COLOUR must be 'yellow' or 'blue', got {ATTACKER_COLOUR!r}") + + # The defender's goal is the one in the camera's half, so the attacker's own goal + # is on the other side. With the full field, the attacker attacks the right goal. + attacker_is_right = CAMERA_HALF == "left" + + attacker_map = {ATTACKER_VISION_ID: ATTACKER_FIRMWARE_ID} + defender_map = {DEFENDER_VISION_ID: DEFENDER_FIRMWARE_ID} + attacker_ir = TRUSTED_IR_VISION_IDS & {ATTACKER_VISION_ID} + defender_ir = TRUSTED_IR_VISION_IDS & {DEFENDER_VISION_ID} + + profile = _operator_profile() + referee = CustomReferee(profile, n_robots_yellow=1, n_robots_blue=1) + server = attach_dashboard(port=DASHBOARD_PORT) + referee_view.attach(server, referee, profile) + + return StrategyRunner( + strategy=_one_robot_strategy(attacker_id, ATTACKER_TACTICS[ATTACKER_TACTIC]), + opp_strategy=_one_robot_strategy(defender_id, DEFENDER_TACTICS[DEFENDER_TACTIC]), + my_team_is_yellow=attacker_is_yellow, + my_team_is_right=attacker_is_right, + mode=mode, + exp_friendly=1, + exp_enemy=1, + full_field_dims=FIELD, + field_bounds=_field_bounds(FIELD, CAMERA_HALF), + referee=referee, + referee_initial_command=RefereeCommand.HALT, + show_live_status=not real, + yellow_vision_to_cmd_mapping=(attacker_map if attacker_is_yellow else defender_map) if real else None, + blue_vision_to_cmd_mapping=(defender_map if attacker_is_yellow else attacker_map) if real else None, + yellow_trusted_ir_robots=(attacker_ir if attacker_is_yellow else defender_ir) if real else None, + blue_trusted_ir_robots=(defender_ir if attacker_is_yellow else attacker_ir) if real else None, + ) + + +def main() -> None: + parser = argparse.ArgumentParser(description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter) + parser.add_argument("--mode", choices=("real", "rsim", "grsim"), default=MODE) + args = parser.parse_args() + build_runner(args.mode).run() + + +if __name__ == "__main__": + main() From d6eb4f478264dbe5ecd0abffb1f714012f9fe475 Mon Sep 17 00:00:00 2001 From: isaac0804 Date: Wed, 21 Oct 2026 19:08:12 +0100 Subject: [PATCH 06/56] fix(engine): no goalkeeper row in debug_status when its robot is absent A team without the goalkeeper's robot showed a phantom goalkeeper (e.g. ID 15) in the dashboard's tactics list. Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_01TJGHdb8Dht67hn4Lohndf5 --- utama_core/engine/abstract_strategy.py | 7 +++++-- .../test_kernel_strategy_debug_status.py | 18 ++++++++++++++++++ 2 files changed, 23 insertions(+), 2 deletions(-) diff --git a/utama_core/engine/abstract_strategy.py b/utama_core/engine/abstract_strategy.py index 0da430e9d..b43961c36 100644 --- a/utama_core/engine/abstract_strategy.py +++ b/utama_core/engine/abstract_strategy.py @@ -248,10 +248,13 @@ def debug_status(self) -> dict[int, list[str]]: signal in this model), and the tactic's `TacticTag` value, which the dashboard colours robots by — used by `StrategyRunner._push_debug_status_to_referee` for the debug GUI panel. The goalkeeper is just `["goalkeeper"]`: it is - pinned outside the scheduler, so a scheduling role tag would say nothing. + pinned outside the scheduler, so a scheduling role tag would say nothing, + and is left out when its robot is not on the field (as it is not ticked then). """ game = self.game - status: dict[int, list[str]] = {self._goalkeeper_id: ["goalkeeper"]} + status: dict[int, list[str]] = {} + if self._goalkeeper_id in game.friendly_robots: + status[self._goalkeeper_id] = ["goalkeeper"] for tactic_id, info in self._kernel_strategy.slot_status(game).items(): label = tactic_id if not info["committed"] else f"{tactic_id} (committed)" for robot_id in info["robots"]: diff --git a/utama_core/tests/engine/test_kernel_strategy_debug_status.py b/utama_core/tests/engine/test_kernel_strategy_debug_status.py index a43ced1c3..430ad026e 100644 --- a/utama_core/tests/engine/test_kernel_strategy_debug_status.py +++ b/utama_core/tests/engine/test_kernel_strategy_debug_status.py @@ -76,3 +76,21 @@ def test_debug_status_tag_is_the_assigned_tactics_own_tag(split_shape_runner): assert isinstance(info["tag"], TacticTag) for robot_id in info["robots"]: assert status[robot_id][1] == info["tag"].value + + +def test_debug_status_leaves_out_a_goalkeeper_whose_robot_is_not_on_the_field(): + """A test team without the goalkeeper's robot (e.g. one attacker alone) showed a + phantom "goalkeeper" row for a robot ID that is not there.""" + from utama_core.run.strategy_runner import StrategyRunner + + strategy = AbstractStrategy(build_kernel_strategy=build_split_shape_kernel_strategy((0, 1)), goalkeeper_id=15) + runner = StrategyRunner( + strategy=strategy, my_team_is_yellow=True, my_team_is_right=True, mode="rsim", exp_friendly=2, exp_enemy=0 + ) + try: + runner.step_once() + status = runner.my.strategy.debug_status() + finally: + runner.close() + assert 15 not in status + assert set(status) == {0, 1} From 30d85742606be4c83a5a75a1aac33efd8a478939 Mon Sep 17 00:00:00 2001 From: isaac0804 Date: Wed, 21 Oct 2026 19:08:12 +0100 Subject: [PATCH 07/56] feat(run): field check on camera runs, auto-HALT on real robots FieldCheck judges raw vision, the geometry packet and radio feedback: each driven robot seen, under its ID, inside field_bounds; camera sending; field size; radio still answering; unconfigured robots in play. The Live tab shows the rows, dims everything outside field_bounds and draws the field as ssl-vision sees it. On real robots any failing row forces HALT. The dashboard opens on Live when a match is attached; Space only halts. Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_01TJGHdb8Dht67hn4Lohndf5 --- docs/custom_referee_gui.md | 20 ++ docs/tools.md | 1 + examples/real_attacker_defender.py | 4 + utama_core/custom_referee/custom_referee.py | 14 +- utama_core/dashboard/static/app.css | 6 + utama_core/dashboard/static/field_canvas.js | 51 ++++++ utama_core/dashboard/static/index.html | 4 +- utama_core/dashboard/static/live.js | 44 ++++- utama_core/dashboard/views/referee.py | 6 +- utama_core/run/field_check.py | 171 ++++++++++++++++++ utama_core/run/strategy_runner.py | 50 +++++ .../tests/strategy_runner/test_field_check.py | 159 ++++++++++++++++ 12 files changed, 520 insertions(+), 10 deletions(-) create mode 100644 utama_core/run/field_check.py create mode 100644 utama_core/tests/strategy_runner/test_field_check.py diff --git a/docs/custom_referee_gui.md b/docs/custom_referee_gui.md index 2247e8af3..bd1c4d765 100644 --- a/docs/custom_referee_gui.md +++ b/docs/custom_referee_gui.md @@ -19,6 +19,26 @@ pixi run python examples/demo_referee_gui_rsim.py # then open http://localhost The page shows the score, current/next command, stage and time left, `designated_position`, the field, the active profile and an event log; a button issues each command. +## Field check (camera runs) + +With vision from a camera (real robots or grSim) the Live tab also shows the field check +(`utama_core/run/field_check.py`), one row per question, red / amber / green: + +| Row | Fails (red) when | Warns (amber) when | +|---|---|---| +| Camera | no vision packet for 0.5 s | | +| Field size | ssl-vision's geometry differs from `full_field_dims` | no geometry packet yet | +| ` ` (each robot we drive) | never seen, lost by vision 0.5 s, or more than 0.3 m outside `field_bounds` | | +| ` radio` (real only) | it answered on the radio, then stopped | it has not answered yet (the firmware may send no feedback) | +| ` ` (any other) | | seen inside `field_bounds` but not configured: our planners cannot see it, often a wrong ID | +| Ball | | not seen, or outside `field_bounds` | + +On real robots any red row forces `HALT`, whatever the command and whether or not the page is +open; the red banner says why. Pressing Force Start while a row is red halts again at once, so +the check is also the pre-start gate. The field is drawn as ssl-vision sees it (+x to the right), +with everything outside `field_bounds` dimmed, so a half-field test shows which half the runner +expects. Space halts; nothing on the keyboard starts the robots. + ## Commands and what robots do | Button | Command | Use | Robots | diff --git a/docs/tools.md b/docs/tools.md index 04c1761d2..f8bad56a7 100644 --- a/docs/tools.md +++ b/docs/tools.md @@ -112,5 +112,6 @@ described in [STRATEGY_DEVELOPMENT.md](STRATEGY_DEVELOPMENT.md). | `examples/demo_referee_gui_rsim.py` | `CustomReferee` with the dashboard's referee tab, over rsim | | `examples/demo_referee_feedback_gui.py` | The referee UI's controller-feedback panel with fake feedback, no hardware | | `examples/demo_exhibition_road.py` | The Exhibition Road festival demo on the 4 m x 3 m field, over rsim | +| `examples/real_attacker_defender.py` | One attacker against one defender on real robots (or `--mode rsim`/`grsim`): field, camera half and IDs in one config block; the Live tab's field check gates the start | | `examples/demo_dribbler_test.py` | One robot dribbling round a rectangle on the Exhibition Road field | | `examples/demo_split_shape_match.py` | Visible grSim 6v6 of `split_shape` against itself | diff --git a/examples/real_attacker_defender.py b/examples/real_attacker_defender.py index 05e63e73e..f1b57ae4a 100644 --- a/examples/real_attacker_defender.py +++ b/examples/real_attacker_defender.py @@ -16,6 +16,10 @@ only starts and stops from the dashboard. Real mode starts in HALT; Ctrl+C sends stop commands to the robots before exiting. +On the dashboard's Live tab, the field check must be all green or amber before Force Start: +a red row (robot not seen, outside the camera half, radio lost, wrong field size) halts the +robots at once. See docs/custom_referee_gui.md, "Field check". + Before running on real robots: - ssl-vision's field geometry (length and width) must equal FIELD below, or the runner stops at the first geometry packet with "Field geometry mismatch". diff --git a/utama_core/custom_referee/custom_referee.py b/utama_core/custom_referee/custom_referee.py index b2054ef82..bee8d7446 100644 --- a/utama_core/custom_referee/custom_referee.py +++ b/utama_core/custom_referee/custom_referee.py @@ -205,6 +205,7 @@ def __init__( self._dashboard_notifier: Optional[callable] = None self._debug_status_per_robot: dict[int, list[str]] = {} self._robot_feedback_data: list[dict] = [] + self._field_check: Optional[dict] = None self._match_log = None self._match_log_tick = 0 self._last_logged_ref_data: Optional[RefereeData] = None @@ -233,7 +234,7 @@ def from_profile_name( return cls(profile, n_robots_yellow=n_robots_yellow, n_robots_blue=n_robots_blue) def attach_dashboard_notifier(self, notifier: callable) -> None: - """Set the callback invoked with (ref_data, game_frame, debug_status, robot_feedback) + """Set the callback invoked with (ref_data, game_frame, debug_status, robot_feedback, field_check) on every `step()`. Set by `dashboard.views.referee.attach()`; not meant to be called directly by strategy code. """ @@ -292,7 +293,9 @@ def step(self, game_frame: GameFrame, current_time: float) -> RefereeData: for rule in self._rules: rule.reset() if self._dashboard_notifier is not None: - self._dashboard_notifier(result, game_frame, self._debug_status_per_robot, self._robot_feedback_data) + self._dashboard_notifier( + result, game_frame, self._debug_status_per_robot, self._robot_feedback_data, self._field_check + ) self._match_log_tick += 1 if self._match_log is not None and result != self._last_logged_ref_data: @@ -339,6 +342,13 @@ def set_robot_feedback_data(self, robot_feedback_data: list[dict]) -> None: """ self._robot_feedback_data = [dict(row) for row in robot_feedback_data] + def set_field_check(self, field_check: dict) -> None: + """Set the field-test checks for GUI display (`FieldCheck.payload`). + + Called by StrategyRunner every tick when vision comes from a camera. + """ + self._field_check = field_check + def seed_clock(self, timestamp: float, initial_command: RefereeCommand = RefereeCommand.HALT) -> None: """Align all internal state-machine timers to *timestamp* and apply *initial_command*. diff --git a/utama_core/dashboard/static/app.css b/utama_core/dashboard/static/app.css index 8e712b6fd..9cd11b661 100644 --- a/utama_core/dashboard/static/app.css +++ b/utama_core/dashboard/static/app.css @@ -180,6 +180,12 @@ td.num { text-align: right; font-variant-numeric: tabular-nums; } } .conn-dot.live { background: var(--accent); } +/* Field check (live view): one row per check, coloured by status; the auto-HALT banner. */ +.check-ok { color: #5fb87a; } +.check-warn { color: #e0a14a; } +.check-fail { color: #e06a60; font-weight: 600; } +.auto-halt { background: var(--halt); color: #fff; padding: 6px 8px; border-radius: var(--radius); font-size: .75rem; } + /* Replay timeline markers: one tick per goal / foul / stall / ball loss. */ .marker-strip { position: relative; height: 10px; margin: 0 7px; } .marker { position: absolute; top: 0; width: 3px; height: 10px; margin-left: -1px; border-radius: 1px; cursor: pointer; } diff --git a/utama_core/dashboard/static/field_canvas.js b/utama_core/dashboard/static/field_canvas.js index ce0aaf90d..026ea0cf7 100644 --- a/utama_core/dashboard/static/field_canvas.js +++ b/utama_core/dashboard/static/field_canvas.js @@ -177,6 +177,8 @@ class FieldCanvas { strokeWorldRect(-g.half_length - goalDepth, -g.half_goal_width, -g.half_length, g.half_goal_width); strokeWorldRect(g.half_length, -g.half_goal_width, g.half_length + goalDepth, g.half_goal_width); + if (state.field_check) this._drawAreaInPlay(ctx, state.field_check, toX, toY, rectBounds, CW, CH); + if (state.designated) { const dx = toX(state.designated[0]), dy = toY(state.designated[1]); ctx.strokeStyle = FIELD_COLORS.marker; @@ -235,6 +237,21 @@ class FieldCanvas { this._drawTagLegend(ctx, tacticTags); } + if (state.field_check && robots) { + // A robot past the area in play by more than the margin has forced HALT on real robots. + const [x1, y1, x2, y2] = state.field_check.bounds; + const m = state.field_check.margin; + ctx.strokeStyle = "#e06a60"; + ctx.lineWidth = 2; + for (const bot of [...(robots.friendly || []), ...(robots.enemy || [])]) { + if (bot.x < x1 - m || bot.x > x2 + m || bot.y < y1 - m || bot.y > y2 + m) { + ctx.beginPath(); + ctx.arc(toX(bot.x), toY(bot.y), Math.max(2, ROBOT_RADIUS_M * scale) + 4, 0, 2 * Math.PI); + ctx.stroke(); + } + } + } + if (state.ball) { const bx = toX(state.ball.x), by = toY(state.ball.y); ctx.fillStyle = FIELD_COLORS.ball; @@ -246,6 +263,40 @@ class FieldCanvas { this._drawOverlays(ctx, state, robots, toX, toY); } + // The area in play (StrategyRunner's field_bounds): everything outside it is dimmed, so a + // half-field test shows at a glance which half the runner thinks the camera covers. + // The +x arrow ties the picture to ssl-vision's coordinates, since this view turns the + // field so our team's goal is on the right. + _drawAreaInPlay(ctx, fieldCheck, toX, toY, rectBounds, CW, CH) { + const [x1, y1, x2, y2] = fieldCheck.bounds; + const r = rectBounds(x1, y1, x2, y2); + ctx.save(); + ctx.fillStyle = "rgba(0, 0, 0, 0.45)"; + ctx.beginPath(); + ctx.rect(0, 0, CW, CH); + ctx.rect(r.x + r.w, r.y, -r.w, r.h); // opposite winding cuts the area in play out + ctx.fill(); + ctx.strokeStyle = FIELD_COLORS.laneOpen; + ctx.setLineDash([4, 3]); + ctx.lineWidth = 1.5; + ctx.strokeRect(r.x, r.y, r.w, r.h); + ctx.setLineDash([]); + const ax = toX(0), ay = CH - 6, dir = toX(1) > ax ? 1 : -1; + ctx.strokeStyle = ctx.fillStyle = FIELD_COLORS.marker; + ctx.lineWidth = 1; + ctx.beginPath(); + ctx.moveTo(ax, ay); + ctx.lineTo(ax + dir * 24, ay); + ctx.lineTo(ax + dir * 20, ay - 3); + ctx.moveTo(ax + dir * 24, ay); + ctx.lineTo(ax + dir * 20, ay + 3); + ctx.stroke(); + ctx.font = "7px ui-monospace, monospace"; + ctx.textAlign = dir > 0 ? "left" : "right"; + ctx.fillText("+x (vision)", ax + dir * 27, ay + 2); + ctx.restore(); + } + // Small key for the tactic-tag rings, only when some robot has a tag. _drawTagLegend(ctx, tacticTags) { const present = new Set(Object.values(tacticTags)); diff --git a/utama_core/dashboard/static/index.html b/utama_core/dashboard/static/index.html index 6367d6310..75e8a2fd9 100644 --- a/utama_core/dashboard/static/index.html +++ b/utama_core/dashboard/static/index.html @@ -24,6 +24,8 @@
Yellow —
Blue —
+ +
Command —
Stage —
@@ -45,7 +47,7 @@
- space = halt / resume + space = halt
diff --git a/utama_core/dashboard/static/live.js b/utama_core/dashboard/static/live.js index 4de767049..213f63463 100644 --- a/utama_core/dashboard/static/live.js +++ b/utama_core/dashboard/static/live.js @@ -32,22 +32,54 @@ currentCmd = d.command; let layoutChanged = false; - if (d.my_team_is_right !== undefined && d.my_team_is_right !== window._refMyTeamIsRight) { - window._refMyTeamIsRight = d.my_team_is_right; + // With a camera (field_check present), draw the field as ssl-vision sees it, +x to the + // right, so the picture matches the pitch in front of the operator; otherwise our goal + // on the right, as in replays. + const drawRight = d.field_check ? true : d.my_team_is_right; + if (drawRight !== undefined && drawRight !== window._refMyTeamIsRight) { + window._refMyTeamIsRight = drawRight; layoutChanged = true; } if (d.my_team_is_yellow !== undefined && d.my_team_is_yellow !== window._refMyTeamIsYellow) { window._refMyTeamIsYellow = d.my_team_is_yellow; layoutChanged = true; } - if (fieldView && layoutChanged) fieldView.setTeamLayout(d.my_team_is_right, d.my_team_is_yellow); + if (fieldView && layoutChanged) fieldView.setTeamLayout(drawRight, d.my_team_is_yellow); - renderRobotStatusInto("ref-status-entries", d); + renderFieldCheck(d.field_check); + renderRobotStatusInto("ref-status-entries", d, { hideFeedback: !!d.field_check }); // the field check has the radio rows renderTacticStatusInto("ref-tactic-entries", d.tactic_status); d.tactic_tags = tacticTagsFromStatus(d.tactic_status); if (fieldView) fieldView.draw(d); } + // The runner's field checks (utama_core/run/field_check.py): the failures first, so a + // red row is never scrolled away; on real robots any failure has already forced HALT. + function renderFieldCheck(fc) { + const box = document.getElementById("ref-field-check"); + const banner = document.getElementById("ref-auto-halt"); + if (!fc) { + box.innerHTML = ""; + banner.style.display = "none"; + return; + } + banner.style.display = fc.auto_halt ? "" : "none"; + banner.textContent = fc.auto_halt ? "Auto-HALT " + fc.auto_halt : ""; + const order = { fail: 0, warn: 1, ok: 2 }; + const rows = fc.rows.slice().sort((a, b) => order[a.status] - order[b.status]); + const fails = rows.filter((r) => r.status === "fail").length; + let html = + '
Field check — ' + + (fails ? '' + fails + " failing" : 'ready') + + "
"; + for (const r of rows) { + html += + '
' + (r.status === "ok" ? "●" : r.status === "warn" ? "▲" : "✖") + + " " + r.name + '' + r.detail + "
"; + } + box.innerHTML = html; + } + function mount() { fetch("/referee/config") .then((r) => (r.ok ? r.json() : null)) @@ -61,6 +93,7 @@ const canvas = document.getElementById("ref-field-canvas"); fieldView = new FieldCanvas(canvas, c.geometry, {}); document.getElementById("ref-profile-name").textContent = c.profile_name; + Dashboard.showView("live"); // a running match is what this page is open for }); const es = new EventSource("/events/referee"); @@ -74,9 +107,10 @@ document.addEventListener("keydown", (e) => { if (!document.getElementById("view-live").classList.contains("active")) return; + // Halt only: a stray key press must never start the robots. if (e.key === " ") { e.preventDefault(); - send(currentCmd === "HALT" ? "FORCE_START" : "HALT"); + send("HALT"); } }); } diff --git a/utama_core/dashboard/views/referee.py b/utama_core/dashboard/views/referee.py index 1fb401406..064d00726 100644 --- a/utama_core/dashboard/views/referee.py +++ b/utama_core/dashboard/views/referee.py @@ -62,8 +62,9 @@ def notify( game_frame=None, tactic_status=None, robot_feedback_data=None, + field_check=None, ) -> None: - server.notify(CHANNEL, _serialise_state(ref_data, game_frame, tactic_status, robot_feedback_data)) + server.notify(CHANNEL, _serialise_state(ref_data, game_frame, tactic_status, robot_feedback_data, field_check)) def _serialise_robots(game_frame) -> dict: @@ -96,7 +97,7 @@ def _serialise_ball(game_frame): return {"x": game_frame.ball.p.x, "y": game_frame.ball.p.y, "vx": game_frame.ball.v.x, "vy": game_frame.ball.v.y} -def _serialise_state(ref_data, game_frame=None, tactic_status=None, robot_feedback_data=None) -> dict: +def _serialise_state(ref_data, game_frame=None, tactic_status=None, robot_feedback_data=None, field_check=None) -> dict: designated = None if ref_data.designated_position is not None: try: @@ -119,6 +120,7 @@ def _serialise_state(ref_data, game_frame=None, tactic_status=None, robot_feedba "ball": _serialise_ball(game_frame), "tactic_status": tactic_status or {}, "robot_feedback": robot_feedback_data or [], + "field_check": field_check, } diff --git a/utama_core/run/field_check.py b/utama_core/run/field_check.py new file mode 100644 index 000000000..071116882 --- /dev/null +++ b/utama_core/run/field_check.py @@ -0,0 +1,171 @@ +"""Checks that a field test is set up as configured, and the faults that must stop it. + +Fed raw vision (every detection, before the roster and bounds filters), the vision +geometry packet and the radio's feedback rows, it answers the questions an operator +otherwise answers by walking the field: is every configured robot seen, under the ID +we expect, inside the part of the field in play? Is the camera sending? Is each robot +still answering on the radio? Any robot seen in play that we did not configure? + +Each answer is a row with status "ok", "warn" or "fail". `StrategyRunner` shows the +rows on the dashboard's Live tab and, on real robots, forces HALT while any row +fails (see `halt_reason`), so a lost robot or a robot leaving the area stops play +whether or not anyone is watching the page. +""" + +from __future__ import annotations + +from typing import Iterable, Optional + +from utama_core.config.field_params import FieldBounds, FieldDimensions +from utama_core.entities.data.raw_vision import RawVisionData + +# A configured robot missing from vision this long fails: a robot we cannot see is +# driven on a stale position. Several frames at ssl-vision's 60-75 Hz. +VISION_LOST_S = 0.5 +# No vision packet at all for this long fails (camera or network down). +CAMERA_SILENT_S = 0.5 +# A robot this far outside the field bounds fails: the camera half is wrong, or the +# robot is leaving the area the planner keeps it in. Within it, a robot reaching +# over the line (a defender at the half-way line) is fine. +OUT_OF_BOUNDS_MARGIN_M = 0.3 +# Vision geometry within this of the configured field matches (as the runner's own check). +GEOMETRY_TOLERANCE_M = 0.05 + +OK, WARN, FAIL = "ok", "warn", "fail" + + +def _row(name: str, status: str, detail: str) -> dict: + return {"name": name, "status": status, "detail": detail} + + +def _outside_by(bounds: FieldBounds, x: float, y: float) -> float: + """How far (x, y) lies outside `bounds`, 0 inside.""" + (x1, y1), (x2, y2) = bounds.top_left, bounds.bottom_right + dx = max(min(x1, x2) - x, 0.0, x - max(x1, x2)) + dy = max(min(y1, y2) - y, 0.0, y - max(y1, y2)) + return max(dx, dy) + + +class FieldCheck: + """Tracks what vision and the radio last reported; `rows()` judges it. + + `expected` is every robot we drive, as (colour, vision ID) with colour "yellow" + or "blue". Times are the runner's clock (`time.perf_counter()`), not vision's. + """ + + def __init__(self, expected: Iterable[tuple[str, int]], bounds: FieldBounds, field: FieldDimensions): + self.expected = sorted(expected) + self.bounds = bounds + self.field = field + self._robot_seen: dict[tuple[str, int], tuple[float, float, float]] = {} # -> (time, x, y) + self._ball_seen: Optional[tuple[float, float, float]] = None + self._packet_times: list[float] = [] + self._geometry: Optional[tuple[float, float]] = None # vision's (length, width), metres + self._radio_answered: set[tuple[str, int]] = set() + + def observe_geometry(self, field_length_mm: float, field_width_mm: float) -> None: + self._geometry = (field_length_mm / 1000.0, field_width_mm / 1000.0) + + def observe_vision(self, frames: Iterable[Optional[RawVisionData]], now: float) -> None: + """Record this tick's new vision packets (None where a camera sent nothing).""" + for frame in frames: + if frame is None: + continue + self._packet_times.append(now) + for colour, robots in (("yellow", frame.yellow_robots), ("blue", frame.blue_robots)): + for r in robots: + self._robot_seen[(colour, r.id)] = (now, r.x, r.y) + if frame.balls: + b = max(frame.balls, key=lambda b: b.confidence) + self._ball_seen = (now, b.x, b.y) + # Keep one second of packet times, for the rate. + while self._packet_times and now - self._packet_times[0] > 1.0: + self._packet_times.pop(0) + + def rows(self, now: float, radio: Optional[list[dict]] = None) -> list[dict]: + """One row per check. `radio` is the runner's robot feedback snapshot (real mode).""" + rows = [self._camera_row(now), self._geometry_row()] + radio_by_robot = {(r["team_color"], r["vision_id"]): r for r in radio or [] if "vision_id" in r} + for colour, vision_id in self.expected: + rows.append(self._robot_row(now, colour, vision_id)) + if radio is not None: + rows.append(self._radio_row(colour, vision_id, radio_by_robot.get((colour, vision_id)))) + rows.extend(self._unexpected_rows(now)) + rows.append(self._ball_row(now)) + return rows + + def halt_reason(self, rows: list[dict]) -> Optional[str]: + """The first failing row as one line, or None when nothing fails.""" + for row in rows: + if row["status"] == FAIL: + return f"{row['name']}: {row['detail']}" + return None + + def _camera_row(self, now: float) -> dict: + if not self._packet_times or now - self._packet_times[-1] > CAMERA_SILENT_S: + return _row("Camera", FAIL, "no vision packets") + return _row("Camera", OK, f"{len(self._packet_times)} packets/s") + + def _geometry_row(self) -> dict: + want = (2 * self.field.full_field_half_length, 2 * self.field.full_field_half_width) + if self._geometry is None: + return _row("Field size", WARN, f"no geometry packet yet; configured {want[0]:.2f} x {want[1]:.2f} m") + got = self._geometry + if max(abs(got[0] - want[0]), abs(got[1] - want[1])) > GEOMETRY_TOLERANCE_M: + return _row( + "Field size", FAIL, f"vision {got[0]:.2f} x {got[1]:.2f} m, configured {want[0]:.2f} x {want[1]:.2f} m" + ) + return _row("Field size", OK, f"{got[0]:.2f} x {got[1]:.2f} m") + + def _robot_row(self, now: float, colour: str, vision_id: int) -> dict: + name = f"{colour} {vision_id}" + seen = self._robot_seen.get((colour, vision_id)) + if seen is None: + return _row(name, FAIL, "never seen by vision: check its pattern and the configured ID") + t, x, y = seen + if now - t > VISION_LOST_S: + return _row(name, FAIL, f"lost by vision {now - t:.1f} s ago at ({x:.2f}, {y:.2f})") + out = _outside_by(self.bounds, x, y) + if out > OUT_OF_BOUNDS_MARGIN_M: + return _row(name, FAIL, f"at ({x:.2f}, {y:.2f}), {out:.2f} m outside the area in play") + return _row(name, OK, f"at ({x:.2f}, {y:.2f})") + + def _radio_row(self, colour: str, vision_id: int, feedback: Optional[dict]) -> dict: + name = f"{colour} {vision_id} radio" + key = (colour, vision_id) + if feedback is not None and feedback["connected"]: + self._radio_answered.add(key) + return _row(name, OK, f"firmware {feedback['port_id']} answering") + if key not in self._radio_answered: + # Not a fault: the firmware may not send feedback at all. + return _row(name, WARN, "no feedback yet") + age = feedback["age_seconds"] if feedback is not None else None + return _row(name, FAIL, "stopped answering" + (f" {age:.1f} s ago" if age is not None else "")) + + def _unexpected_rows(self, now: float) -> list[dict]: + """Robots in play we do not drive: our planners cannot see them. Often a wrong ID.""" + expected = set(self.expected) + rows = [] + for (colour, rid), (t, x, y) in sorted(self._robot_seen.items()): + if (colour, rid) in expected or now - t > VISION_LOST_S or _outside_by(self.bounds, x, y) > 0: + continue + rows.append(_row(f"{colour} {rid}", WARN, f"in play at ({x:.2f}, {y:.2f}) but not configured")) + return rows + + def _ball_row(self, now: float) -> dict: + if self._ball_seen is None or now - self._ball_seen[0] > VISION_LOST_S: + return _row("Ball", WARN, "not seen") + _, x, y = self._ball_seen + if _outside_by(self.bounds, x, y) > 0: + return _row("Ball", WARN, f"at ({x:.2f}, {y:.2f}), outside the area in play") + return _row("Ball", OK, f"at ({x:.2f}, {y:.2f})") + + def payload(self, rows: list[dict], auto_halt: Optional[str]) -> dict: + """What the dashboard draws: the rows, the area in play, and the last auto-HALT.""" + (x1, y1), (x2, y2) = self.bounds.top_left, self.bounds.bottom_right + return { + "rows": rows, + "bounds": [min(x1, x2), min(y1, y2), max(x1, x2), max(y1, y2)], + "margin": OUT_OF_BOUNDS_MARGIN_M, + "auto_halt": auto_halt, + } diff --git a/utama_core/run/strategy_runner.py b/utama_core/run/strategy_runner.py index 16e465399..38d8952f5 100644 --- a/utama_core/run/strategy_runner.py +++ b/utama_core/run/strategy_runner.py @@ -54,6 +54,7 @@ from utama_core.rsoccer_simulator.src.Utils.gaussian_noise import RsimGaussianNoise from utama_core.rsoccer_simulator.src.Utils.sim_noise import SimNoise from utama_core.run import GameGater +from utama_core.run.field_check import FieldCheck from utama_core.run.live_status import LiveStatusMixin from utama_core.run.referee_source import OfficialReferee, RefereeSource from utama_core.run.side_runtime import SideRuntime, _mirrored, _turned @@ -384,6 +385,9 @@ def __init__( ) self.control_scheme = self.my.control_scheme self.opp_control_scheme = self.opp.control_scheme if self.opp is not None else None + self.field_check = self._make_field_check() + self._field_check_rows: list[dict] = [] + self._auto_halt: Optional[str] = None # last auto-HALT's reason, shown on the dashboard ### functions below rely on self.my and self.opp ### @@ -887,6 +891,47 @@ def _setup_vision_and_referee(self) -> Tuple[deque, deque]: self.start_threads(vision_receiver) return vision_buffers, ref_buffer + def _make_field_check(self) -> Optional[FieldCheck]: + """A `FieldCheck` of the robots we drive, for vision from a camera (grSim or real).""" + if self.mode == Mode.RSIM: + return None + if self.mode == Mode.REAL: + yellow, blue = self.yellow_vision_to_cmd_mapping, self.blue_vision_to_cmd_mapping + else: + # The simulators number each team's robots from 0. + yellow, blue = ( + range(n) + for n in map_friendly_enemy_to_colors(self.my_team_is_yellow, self.exp_friendly, self.exp_enemy) + ) + driven = {"yellow": yellow, "blue": blue} + if self.opp is None: + driven["blue" if self.my_team_is_yellow else "yellow"] = () + expected = [(colour, rid) for colour, ids in driven.items() for rid in ids] + return FieldCheck(expected, self.field_bounds, self.full_field_dims) + + def _tick_field_check(self, vision_frames: List[Optional[RawVisionData]], now: float) -> None: + """Judge the field against the setup; on real robots, HALT while any check fails. + + Real mode only for the HALT: the simulators resume a HALT on their own. + """ + if self.field_check is None: + return + self.field_check.observe_vision(vision_frames, now) + rows = self.field_check.rows(now, self._robot_feedback_snapshot if self.mode == Mode.REAL else None) + self._field_check_rows = rows + reason = self.field_check.halt_reason(rows) + if ( + reason is not None + and self.mode == Mode.REAL + and isinstance(self.referee, CustomReferee) + and self._prev_custom_ref_command != RefereeCommand.HALT + ): + self.referee.force_command(RefereeCommand.HALT, self.my.current_game_frame.ts) + self._auto_halt = f"{time.strftime('%H:%M:%S')} {reason}" + self.logger.warning("Auto-HALT: %s", reason) + if isinstance(self.referee, CustomReferee): + self.referee.set_field_check(self.field_check.payload(rows, self._auto_halt)) + def _make_geometry_validation_callback(self): """Return a one-shot callback that validates vision geometry matches full_field_dims. @@ -895,6 +940,10 @@ def _make_geometry_validation_callback(self): """ def _on_geometry(field_size) -> None: + # Runs on the receiver thread, possibly before __init__ has made the check. + field_check = getattr(self, "field_check", None) + if field_check is not None: + field_check.observe_geometry(field_size.field_length, field_size.field_width) vision_half_length = field_size.field_length / 2000.0 vision_half_width = field_size.field_width / 2000.0 d = self.full_field_dims @@ -1597,6 +1646,7 @@ def _run_step(self): referee_data = self.ref_buffer.popleft() if self.ref_buffer else None else: vision_frames = [buffer.popleft() if buffer else None for buffer in self.vision_buffers] + self._tick_field_check(vision_frames, frame_start) if self.ref_buffer: self._last_referee_data = self.ref_buffer.popleft() referee_data = self._last_referee_data diff --git a/utama_core/tests/strategy_runner/test_field_check.py b/utama_core/tests/strategy_runner/test_field_check.py new file mode 100644 index 000000000..1746da934 --- /dev/null +++ b/utama_core/tests/strategy_runner/test_field_check.py @@ -0,0 +1,159 @@ +"""`FieldCheck`'s verdicts at their boundaries, and the runner's auto-HALT on real robots.""" + +from __future__ import annotations + +import logging +from types import SimpleNamespace +from unittest.mock import MagicMock + +import pytest + +from utama_core.config.enums import Mode +from utama_core.config.field_params import FieldBounds, FieldDimensions +from utama_core.custom_referee import CustomReferee +from utama_core.entities.data.raw_vision import RawBallData, RawRobotData, RawVisionData +from utama_core.entities.referee.referee_command import RefereeCommand +from utama_core.run.field_check import ( + CAMERA_SILENT_S, + OUT_OF_BOUNDS_MARGIN_M, + VISION_LOST_S, + FieldCheck, +) +from utama_core.run.strategy_runner import StrategyRunner + +FIELD = FieldDimensions( + full_field_half_length=2.0, + full_field_half_width=1.5, + half_defense_area_depth=0.3, + half_defense_area_width=0.6, + half_goal_width=0.4, + center_circle_radius=0.3, +) +RIGHT_HALF = FieldBounds(top_left=(0.0, 1.5), bottom_right=(2.0, -1.5)) + + +def _frame(yellow=(), blue=(), ball=None) -> RawVisionData: + """`yellow`/`blue`: (id, x, y) per robot; `ball`: (x, y).""" + return RawVisionData( + ts=0.0, + yellow_robots=[RawRobotData(i, x, y, 0.0, 1.0) for i, x, y in yellow], + blue_robots=[RawRobotData(i, x, y, 0.0, 1.0) for i, x, y in blue], + balls=[RawBallData(ball[0], ball[1], 0.0, 1.0)] if ball else [], + camera_id=0, + ) + + +def _check(expected=(("yellow", 1),)) -> FieldCheck: + return FieldCheck(expected, RIGHT_HALF, FIELD) + + +def _row(rows, name): + return next(r for r in rows if r["name"] == name) + + +def test_a_configured_robot_never_seen_fails(): + fc = _check() + fc.observe_vision([_frame(yellow=[(2, 1.0, 0.0)])], now=0.0) + assert _row(fc.rows(0.0), "yellow 1")["status"] == "fail" + + +def test_a_robot_lost_by_vision_fails_only_after_the_grace_period(): + fc = _check() + fc.observe_vision([_frame(yellow=[(1, 1.0, 0.0)])], now=0.0) + fc.observe_vision([_frame()], now=VISION_LOST_S - 0.01) + assert _row(fc.rows(VISION_LOST_S - 0.01), "yellow 1")["status"] == "ok" + fc.observe_vision([_frame()], now=VISION_LOST_S + 0.01) + assert _row(fc.rows(VISION_LOST_S + 0.01), "yellow 1")["status"] == "fail" + + +@pytest.mark.parametrize( + "x, status", + [ + (-OUT_OF_BOUNDS_MARGIN_M + 0.01, "ok"), # reaching over the half-way line + (-OUT_OF_BOUNDS_MARGIN_M - 0.01, "fail"), # in the other half: wrong CAMERA_HALF, or leaving + ], +) +def test_a_robot_outside_the_area_in_play_fails_past_the_margin(x, status): + fc = _check() + fc.observe_vision([_frame(yellow=[(1, x, 0.0)])], now=0.0) + assert _row(fc.rows(0.0), "yellow 1")["status"] == status + + +def test_no_vision_packets_fails_the_camera(): + fc = _check() + fc.observe_vision([_frame(yellow=[(1, 1.0, 0.0)])], now=0.0) + assert _row(fc.rows(CAMERA_SILENT_S - 0.01), "Camera")["status"] == "ok" + fc.observe_vision([None, None], now=CAMERA_SILENT_S + 0.01) + assert _row(fc.rows(CAMERA_SILENT_S + 0.01), "Camera")["status"] == "fail" + + +def test_field_size_is_warned_until_vision_sends_it_and_fails_on_a_mismatch(): + fc = _check() + assert _row(fc.rows(0.0), "Field size")["status"] == "warn" + fc.observe_geometry(4000.0, 3000.0) + assert _row(fc.rows(0.0), "Field size")["status"] == "ok" + fc.observe_geometry(9000.0, 6000.0) + assert _row(fc.rows(0.0), "Field size")["status"] == "fail" + + +def test_radio_silence_only_fails_once_the_robot_has_answered(): + """Firmware that never sends feedback must not halt every test; one that stops must.""" + fc = _check() + fc.observe_vision([_frame(yellow=[(1, 1.0, 0.0)])], now=0.0) + row = {"port_id": 4, "team_color": "yellow", "vision_id": 1, "has_ball": False} + silent = dict(row, connected=False, age_seconds=None) + assert _row(fc.rows(0.0, [silent]), "yellow 1 radio")["status"] == "warn" + assert _row(fc.rows(0.0, [dict(row, connected=True, age_seconds=0.0)]), "yellow 1 radio")["status"] == "ok" + lost = dict(row, connected=False, age_seconds=0.8) + assert _row(fc.rows(0.0, [lost]), "yellow 1 radio")["status"] == "fail" + + +def test_an_unconfigured_robot_in_play_is_warned_and_one_outside_is_ignored(): + fc = _check() + fc.observe_vision([_frame(yellow=[(1, 1.0, 0.0), (3, 0.5, 0.5)], blue=[(0, -1.0, 0.0)])], now=0.0) + rows = fc.rows(0.0) + assert _row(rows, "yellow 3")["status"] == "warn" + assert not [r for r in rows if r["name"] == "blue 0"] + + +def test_halt_reason_names_the_first_failure(): + fc = _check() + fc.observe_vision([_frame(yellow=[(1, 1.0, 0.0)], ball=(1.0, 0.0))], now=0.0) + fc.observe_geometry(4000.0, 3000.0) + assert fc.halt_reason(fc.rows(0.0)) is None + fc.observe_vision([_frame(yellow=[(1, -1.0, 0.0)])], now=0.1) + assert fc.halt_reason(fc.rows(0.1)).startswith("yellow 1:") + + +def _runner(mode: Mode, command: RefereeCommand) -> SimpleNamespace: + referee = MagicMock(spec=CustomReferee) + return SimpleNamespace( + field_check=_check(), + mode=mode, + referee=referee, + _robot_feedback_snapshot=[], + _prev_custom_ref_command=command, + _auto_halt=None, + _field_check_rows=[], + my=SimpleNamespace(current_game_frame=SimpleNamespace(ts=5.0)), + logger=logging.getLogger("test"), + ) + + +@pytest.mark.parametrize( + "mode, command, halts", + [ + (Mode.REAL, RefereeCommand.FORCE_START, True), + (Mode.REAL, RefereeCommand.STOP, True), # robots move in STOP too + (Mode.REAL, RefereeCommand.HALT, False), # already halted: nothing to force + (Mode.GRSIM, RefereeCommand.FORCE_START, False), # sim resumes HALT by itself; shown only + ], +) +def test_runner_forces_halt_on_a_failing_check_on_real_robots_only(mode, command, halts): + runner = _runner(mode, command) + StrategyRunner._tick_field_check(runner, [_frame()], now=0.0) # yellow 1 never seen + assert runner.referee.force_command.called == halts + if halts: + assert runner.referee.force_command.call_args.args[0] == RefereeCommand.HALT + assert "yellow 1" in runner._auto_halt + assert runner.referee.set_field_check.call_args.args[0]["rows"] From 88e34a0a47a080df922749d685198bdae95437ba Mon Sep 17 00:00:00 2001 From: isaac0804 Date: Wed, 21 Oct 2026 19:22:59 +0100 Subject: [PATCH 08/56] feat(run): robot profiles: push without a kicker, gentle carry without a dribbler RobotProfile(kicker, dribbler, ir) per robot, passed to StrategyRunner as {yellow,blue}_robot_profiles and applied in every mode, so a test with a broken robot can be rehearsed in a simulator. ProfiledRobotController wraps the team's robot controller: a kick becomes a 0.3 s forward push at 1 m/s (a stop command ends it); a dribble request without a dribbler never backs up and is scaled to 0.5 m/s and 2 rad/s together. A robot without a ball sensor is left out of the trusted-IR set. A profile for a robot that is not driven, or ball sensors given both ways, stops the run at startup. Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_01TJGHdb8Dht67hn4Lohndf5 --- docs/roadmap.md | 6 +- utama_core/run/strategy_runner.py | 67 ++++++++- .../src/controllers/common/robot_profile.py | 114 +++++++++++++++ .../tests/controller/test_robot_profile.py | 130 ++++++++++++++++++ 4 files changed, 312 insertions(+), 5 deletions(-) create mode 100644 utama_core/team_controller/src/controllers/common/robot_profile.py create mode 100644 utama_core/tests/controller/test_robot_profile.py diff --git a/docs/roadmap.md b/docs/roadmap.md index 59130d486..00578dee6 100644 --- a/docs/roadmap.md +++ b/docs/roadmap.md @@ -136,8 +136,10 @@ entry by its title, not its number. firmware command ID, real mode only) is the robot roster. It is validated (one entry per expected robot, integers, no command ID used by both teams on a shared transmitter, every observed vision ID covered: `game_gater.py`) and filters vision to those IDs. - `*_trusted_ir_robots` (robots whose ball sensor is trusted, the rest infer possession - from vision) is the only per-robot capability today. There is no checked-in roster + `{yellow,blue}_robot_profiles` (`RobotProfile`: kicker, dribbler, ball sensor working + or not, 2026-10-21) say what each robot can do; a robot without a kicker pushes the + ball, one without a dribbler carries it slowly, one without a ball sensor infers + possession from vision (`robot_profile.py`). These are on/off, not measured. There is no checked-in roster file: whoever writes the run script passes the mapping. The command ID is fixed in the robot's firmware, so it is the key a profile would use. - *What can differ per robot:* driving (top speed, acceleration and braking, turning, diff --git a/utama_core/run/strategy_runner.py b/utama_core/run/strategy_runner.py index 38d8952f5..330d44e95 100644 --- a/utama_core/run/strategy_runner.py +++ b/utama_core/run/strategy_runner.py @@ -79,6 +79,10 @@ RSimPVPManager, RSimRobotController, ) +from utama_core.team_controller.src.controllers.common.robot_profile import ( + ProfiledRobotController, + RobotProfile, +) from utama_core.tests.common.abstract_test_manager import ( AbstractTestManager, TestingStatus, @@ -250,6 +254,13 @@ class StrategyRunner(SimRecoveryMixin, VisionStreamStatusMixin, LiveStatusMixin) is confirmed working. Robots NOT in this set fall back to vision-proximity inference (~0.13 m). Pass ``None`` (default) to trust all IR sensors — remove this argument once sensors are stable. blue_trusted_ir_robots (FrozenSet[int], optional): Same as ``yellow_trusted_ir_robots`` for the blue team. + yellow_robot_profiles (dict[int, RobotProfile], optional): What each yellow robot has working (kicker, + dribbler, ball sensor), keyed by the ID the strategy commands (the vision ID on real robots). A robot + without a kicker pushes the ball instead of kicking it, see `ProfiledRobotController`; one without a + ball sensor has ``has_ball`` judged from vision (so not with ``yellow_trusted_ir_robots`` too). + Applies in every mode, so a test with broken hardware can be rehearsed in a simulator. + ``None``: everything works. + blue_robot_profiles (dict[int, RobotProfile], optional): Same as ``yellow_robot_profiles`` for the blue team. """ def __init__( @@ -285,6 +296,8 @@ def __init__( blue_vision_to_cmd_mapping: Optional[dict[int, int]] = None, yellow_trusted_ir_robots: Optional[FrozenSet[int]] = None, blue_trusted_ir_robots: Optional[FrozenSet[int]] = None, + yellow_robot_profiles: Optional[dict[int, RobotProfile]] = None, + blue_robot_profiles: Optional[dict[int, RobotProfile]] = None, ): self.logger = logging.getLogger(__name__) @@ -341,9 +354,6 @@ def __init__( ) self.referee_refiner = RefereeRefiner() - my_trusted_ir = yellow_trusted_ir_robots if my_team_is_yellow else blue_trusted_ir_robots - opp_trusted_ir = blue_trusted_ir_robots if my_team_is_yellow else yellow_trusted_ir_robots - # Set self.opp to a sentinel before mapping validation so _validate_vision_to_cmd_mapping # can check whether an opponent strategy is present (full SideRuntime is set later). self.opp = opp_strategy # temporary sentinel; overwritten by _setup_sides_data below @@ -359,6 +369,15 @@ def __init__( self.yellow_cmd_to_vision_mapping = {v: k for k, v in self.yellow_vision_to_cmd_mapping.items()} self.blue_cmd_to_vision_mapping = {v: k for k, v in self.blue_vision_to_cmd_mapping.items()} + self.yellow_robot_profiles = yellow_robot_profiles or {} + self.blue_robot_profiles = blue_robot_profiles or {} + for is_yellow in (True, False): + self._check_profiles_are_driven(is_yellow) + yellow_trusted_ir_robots = self._trusted_ir(True, yellow_trusted_ir_robots) + blue_trusted_ir_robots = self._trusted_ir(False, blue_trusted_ir_robots) + my_trusted_ir = yellow_trusted_ir_robots if my_team_is_yellow else blue_trusted_ir_robots + opp_trusted_ir = blue_trusted_ir_robots if my_team_is_yellow else yellow_trusted_ir_robots + if self.opp and self.mode == Mode.REAL: self._check_no_cmd_duplicate_if_transmission_sharing( self.yellow_vision_to_cmd_mapping, self.blue_vision_to_cmd_mapping @@ -1094,6 +1113,10 @@ def _load_robot_controllers(self): else: raise ValueError("mode is invalid. Must be 'rsim', 'grsim' or 'real'") + my_profiles, opp_profiles = map_colors_to_friendly_enemy( + self.my_team_is_yellow, self.yellow_robot_profiles, self.blue_robot_profiles + ) + my_robot_controller = self._with_profiles(my_robot_controller, my_profiles) self.my.strategy.load_robot_controller(my_robot_controller) self.my.strategy.load_motion_controller(self.my.motion_controller(self.mode, self.rsim_env)) if self.match_log is not None: @@ -1107,9 +1130,47 @@ def _load_robot_controllers(self): if kernel_strategy is not None: kernel_strategy.match_log = self.match_log if self.opp: + opp_robot_controller = self._with_profiles(opp_robot_controller, opp_profiles) self.opp.strategy.load_robot_controller(opp_robot_controller) self.opp.strategy.load_motion_controller(self.opp.motion_controller(self.mode, self.rsim_env)) + def _driven_ids(self, is_yellow: bool) -> set[int]: + """The IDs the strategy commands for one colour: the mapped vision IDs on real robots.""" + if self.mode == Mode.REAL: + return set(self.yellow_vision_to_cmd_mapping if is_yellow else self.blue_vision_to_cmd_mapping) + n_yellow, n_blue = map_friendly_enemy_to_colors(self.my_team_is_yellow, self.exp_friendly, self.exp_enemy) + return set(range(n_yellow if is_yellow else n_blue)) + + def _check_profiles_are_driven(self, is_yellow: bool) -> None: + """A typo in a profile's ID must stop the run, not leave a broken robot unhandled.""" + profiles = self.yellow_robot_profiles if is_yellow else self.blue_robot_profiles + driven = self._driven_ids(is_yellow) + unknown = set(profiles) - driven + if unknown: + colour = "yellow" if is_yellow else "blue" + raise ValueError( + f"Robot profiles for {colour} robots that are not driven: {sorted(unknown)} (driven: {sorted(driven)})" + ) + + def _trusted_ir(self, is_yellow: bool, trusted_ir_robots: Optional[FrozenSet[int]]) -> Optional[FrozenSet[int]]: + """The robots whose ball sensor is trusted: from the profiles, or as passed (not both).""" + profiles = self.yellow_robot_profiles if is_yellow else self.blue_robot_profiles + broken = {rid for rid, p in profiles.items() if not p.ir} + if not broken: + return trusted_ir_robots + if trusted_ir_robots is not None: + colour = "yellow" if is_yellow else "blue" + raise ValueError( + f"Give the {colour} ball sensors in robot profiles or {colour}_trusted_ir_robots, not both" + ) + return frozenset(self._driven_ids(is_yellow) - broken) + + def _with_profiles(self, controller, profiles: dict[int, RobotProfile]): + """`controller`, wrapped to fit commands to `profiles` when a kicker or dribbler is missing.""" + if all(p.kicker and p.dribbler for p in profiles.values()): + return controller + return ProfiledRobotController(controller, profiles) + def _init_refiners( self, field_dims: FieldDimensions, diff --git a/utama_core/team_controller/src/controllers/common/robot_profile.py b/utama_core/team_controller/src/controllers/common/robot_profile.py new file mode 100644 index 000000000..7aea760ba --- /dev/null +++ b/utama_core/team_controller/src/controllers/common/robot_profile.py @@ -0,0 +1,114 @@ +"""What each physical robot can do (kicker, dribbler, ball sensor), and the stand-ins for what it cannot. + +Robots go to a test with a broken kicker or dribbler and are tested anyway. Rather than +teach every tactic about broken hardware, `ProfiledRobotController` wraps a team's robot +controller and rewrites each command for what that robot has: + +- **No kicker:** a kick (or chip) becomes a push: drive straight forward at `PUSH_SPEED` + for `PUSH_SECONDS`, so the robot's front strikes the ball the way it was aiming. The + tactic aimed by turning before it kicked, so the push goes where the kick would have, + only slower. A stop command (all zero, as HALT and `close()` send) ends a push at once. +- **No dribbler:** while the tactic asks to dribble, the dribbler stays off and the robot + moves as one can without it and keep the ball in front: never backwards (the ball + stays behind) and no faster than `CARRY_SPEED` / `CARRY_TURN` (scaled together, so a + pivot around the ball keeps its shape). It pushes the ball along instead of holding it. + +It sits under the strategy in every mode, so a test with a broken robot can be rehearsed +in rsim or grSim first. It knows nothing of the game, only each robot's commands. +""" + +from __future__ import annotations + +import math +from dataclasses import dataclass +from typing import Dict, Optional, Union + +from utama_core.config.settings import CONTROL_FREQUENCY +from utama_core.entities.data.command import RobotCommand +from utama_core.team_controller.src.controllers.common.robot_controller_abstract import ( + AbstractRobotController, +) + +PUSH_SPEED = 1.0 # m/s forward for a kick's push; REAL_PARAMS.MAX_VEL, the real robots' cap +PUSH_SECONDS = 0.3 # about 0.3 m of travel: the ball is struck, the robot does not chase it +CARRY_SPEED = 0.5 # m/s, the fastest a dribbler-less robot carries the ball +CARRY_TURN = 2.0 # rad/s, the fastest it turns with the ball + +_PUSH_TICKS = round(PUSH_SECONDS * CONTROL_FREQUENCY) + + +@dataclass(frozen=True) +class RobotProfile: + """The hardware one robot has working. + + `ir`: its ball sensor. Without it `has_ball` is judged from vision (within ~13 cm of + the ball, whatever way it faces; `RobotInfoRefiner`) instead of the sensor. In a + simulator this ignores the sim's contact sensor the same way. + """ + + kicker: bool = True + dribbler: bool = True + ir: bool = True + + +def _is_stop(c: RobotCommand) -> bool: + return not (c.local_forward_vel or c.local_left_vel or c.angular_vel or c.kick or c.chip or c.dribble) + + +def _carry(c: RobotCommand) -> RobotCommand: + """`c` as a dribbler-less robot keeping the ball in front can follow it.""" + forward = max(c.local_forward_vel, 0.0) + speed = math.hypot(forward, c.local_left_vel) + scale = min(1.0, CARRY_SPEED / speed if speed else 1.0, CARRY_TURN / abs(c.angular_vel) if c.angular_vel else 1.0) + return c._replace( + local_forward_vel=forward * scale, + local_left_vel=c.local_left_vel * scale, + angular_vel=c.angular_vel * scale, + dribble=False, + ) + + +class ProfiledRobotController: + """A team's robot controller with each robot's commands fitted to its `RobotProfile`. + + `profiles` is keyed by the robot ID the strategy commands (the vision ID on real + robots); a robot without a profile is sent its commands unchanged. Everything other + than adding commands goes straight to the wrapped controller. + """ + + def __init__(self, controller: AbstractRobotController, profiles: Dict[int, RobotProfile]): + self._controller = controller + self._profiles = profiles + self._push_ticks_left: Dict[int, int] = {} + + def __getattr__(self, name): + return getattr(self._controller, name) + + def add_robot_commands( + self, + robot_commands: Union[RobotCommand, Dict[int, RobotCommand]], + robot_id: Optional[int] = None, + ) -> None: + if isinstance(robot_commands, RobotCommand): + if robot_id is None: + raise ValueError("robot_id cannot be None type!") + robot_commands = {robot_id: robot_commands} + self._controller.add_robot_commands({rid: self.fit(rid, c) for rid, c in robot_commands.items()}) + + def fit(self, robot_id: int, command: RobotCommand) -> RobotCommand: + """`command` as robot `robot_id` can carry it out; advances its push by one tick.""" + profile = self._profiles.get(robot_id) + if profile is None: + return command + if _is_stop(command): + self._push_ticks_left.pop(robot_id, None) + return command + if not profile.kicker: + if self._push_ticks_left.get(robot_id, 0) == 0 and (command.kick or command.chip): + self._push_ticks_left[robot_id] = _PUSH_TICKS + if self._push_ticks_left.get(robot_id, 0) > 0: + self._push_ticks_left[robot_id] -= 1 + return RobotCommand(PUSH_SPEED, 0.0, 0.0, kick=False, chip=False, dribble=False) + if not profile.dribbler and command.dribble: + command = _carry(command) + return command diff --git a/utama_core/tests/controller/test_robot_profile.py b/utama_core/tests/controller/test_robot_profile.py new file mode 100644 index 000000000..8d6f74f95 --- /dev/null +++ b/utama_core/tests/controller/test_robot_profile.py @@ -0,0 +1,130 @@ +"""Robot profiles: a kickerless robot pushes, a dribblerless one carries gently, a sensorless +one is judged from vision, and only robots the runner drives can have a profile.""" + +from __future__ import annotations + +from types import SimpleNamespace + +import pytest + +from utama_core.config.enums import Mode +from utama_core.entities.data.command import RobotCommand +from utama_core.run.strategy_runner import StrategyRunner +from utama_core.team_controller.src.controllers.common.robot_profile import ( + _PUSH_TICKS, + CARRY_SPEED, + CARRY_TURN, + PUSH_SPEED, + ProfiledRobotController, + RobotProfile, +) + +STOP = RobotCommand(0.0, 0.0, 0.0, kick=False, chip=False, dribble=False) +KICK = RobotCommand(0.0, 0.0, 0.0, kick=True, chip=False, dribble=False) +TURN = RobotCommand(0.0, 0.0, 1.0, kick=False, chip=False, dribble=True) + + +class _Recorder: + def __init__(self): + self.sent: list[dict] = [] + + def add_robot_commands(self, commands, robot_id=None): + self.sent.append(commands) + + +def _controller(**profile) -> tuple[ProfiledRobotController, _Recorder]: + inner = _Recorder() + return ProfiledRobotController(inner, {1: RobotProfile(**profile)}), inner + + +def test_a_kickerless_robot_pushes_forward_for_the_whole_push_then_obeys_again(): + controller, inner = _controller(kicker=False) + controller.add_robot_commands(KICK, 1) + for _ in range(_PUSH_TICKS - 1): + controller.add_robot_commands(TURN, 1) # the tactic's next commands wait for the push + controller.add_robot_commands(TURN, 1) + pushes = [c[1] for c in inner.sent[:_PUSH_TICKS]] + assert all(c.local_forward_vel == PUSH_SPEED and not c.kick and c.angular_vel == 0 for c in pushes) + assert inner.sent[_PUSH_TICKS][1].angular_vel == 1.0 + + +def test_a_stop_ends_a_push_at_once(): + """HALT and close() send all-zero commands: they must reach the robot mid-push.""" + controller, inner = _controller(kicker=False) + controller.add_robot_commands(KICK, 1) + controller.add_robot_commands(STOP, 1) + controller.add_robot_commands(TURN, 1) + assert inner.sent[1][1] == STOP + assert inner.sent[2][1].angular_vel == 1.0 + + +def test_a_dribblerless_robot_never_backs_up_and_carries_at_the_capped_speed(): + controller, inner = _controller(dribbler=False) + controller.add_robot_commands(RobotCommand(-1.0, 0.0, 0.0, kick=False, chip=False, dribble=True), 1) + controller.add_robot_commands(RobotCommand(2.0, 0.0, 0.0, kick=False, chip=False, dribble=True), 1) + backing, fast = inner.sent[0][1], inner.sent[1][1] + assert backing.local_forward_vel == 0.0 and not backing.dribble + assert fast.local_forward_vel == pytest.approx(CARRY_SPEED) + + +def test_a_dribblerless_pivot_keeps_its_shape_when_slowed(): + controller, inner = _controller(dribbler=False) + controller.add_robot_commands(RobotCommand(0.0, 0.4, 4.0, kick=False, chip=False, dribble=True), 1) + c = inner.sent[0][1] + assert c.angular_vel == pytest.approx(CARRY_TURN) + assert c.local_left_vel / c.angular_vel == pytest.approx(0.4 / 4.0) + + +def test_a_dribblerless_robot_not_dribbling_is_untouched(): + controller, inner = _controller(dribbler=False) + fast = RobotCommand(2.0, -1.0, 3.0, kick=False, chip=False, dribble=False) + controller.add_robot_commands({1: fast, 2: KICK}) + assert inner.sent[0] == {1: fast, 2: KICK} # robot 2 has no profile + + +def _runner(mode: Mode, yellow_mapping: dict, **yellow_profiles) -> SimpleNamespace: + runner = SimpleNamespace( + mode=mode, + my_team_is_yellow=True, + exp_friendly=2, + exp_enemy=1, + yellow_vision_to_cmd_mapping=yellow_mapping, + blue_vision_to_cmd_mapping={}, + yellow_robot_profiles={int(k[1:]): v for k, v in yellow_profiles.items()}, # "r3" -> robot 3 + blue_robot_profiles={}, + ) + runner._driven_ids = lambda is_yellow: StrategyRunner._driven_ids(runner, is_yellow) + return runner + + +def test_a_profile_for_a_robot_that_is_not_driven_is_refused(): + """A typo in a test-day config must stop the run, not silently leave a robot kicking air.""" + runner = _runner(Mode.REAL, {4: 0}, r3=RobotProfile(kicker=False)) + with pytest.raises(ValueError, match=r"\[3\]"): + StrategyRunner._check_profiles_are_driven(runner, is_yellow=True) + + +def test_the_controller_is_only_wrapped_when_a_kicker_or_dribbler_is_missing(): + runner = _runner(Mode.RSIM, {}) + inner = object() + assert StrategyRunner._with_profiles(runner, inner, {1: RobotProfile(ir=False)}) is inner + assert isinstance( + StrategyRunner._with_profiles(runner, inner, {1: RobotProfile(dribbler=False)}), ProfiledRobotController + ) + + +def test_a_robot_without_a_ball_sensor_is_left_out_of_the_trusted_ones(): + runner = _runner(Mode.REAL, {4: 0, 6: 1}, r6=RobotProfile(ir=False)) + assert StrategyRunner._trusted_ir(runner, True, None) == frozenset({4}) + + +def test_with_every_ball_sensor_working_the_trusted_set_passes_through(): + runner = _runner(Mode.RSIM, {}, r0=RobotProfile()) + assert StrategyRunner._trusted_ir(runner, True, None) is None # None: trust all + assert StrategyRunner._trusted_ir(runner, True, frozenset({1})) == frozenset({1}) + + +def test_ball_sensors_given_both_ways_are_refused(): + runner = _runner(Mode.RSIM, {}, r0=RobotProfile(ir=False)) + with pytest.raises(ValueError, match="not both"): + StrategyRunner._trusted_ir(runner, True, frozenset({1})) From f0b25e4bbebca189872092922a2a69cef9a851b0 Mon Sep 17 00:00:00 2001 From: isaac0804 Date: Wed, 21 Oct 2026 19:23:00 +0100 Subject: [PATCH 09/56] feat(examples,dashboard): per-robot hardware and RULES switch; tidier Live page The attacker/defender example takes one TestRobot per robot (vision ID, firmware ID, RobotProfile) and RULES = "off" | "on" ("on": the human referee profile, nothing auto-advances). The Live page lists only failing and warning field-check rows, folds the passing ones into an "N ok" line, hides the restart buttons when no rule sets up a restart, and drops the robot list the field check already covers. Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_01TJGHdb8Dht67hn4Lohndf5 --- docs/custom_referee_gui.md | 6 ++- examples/real_attacker_defender.py | 68 ++++++++++++++++---------- utama_core/dashboard/static/index.html | 4 +- utama_core/dashboard/static/live.js | 38 +++++++++----- utama_core/dashboard/views/referee.py | 4 ++ 5 files changed, 79 insertions(+), 41 deletions(-) diff --git a/docs/custom_referee_gui.md b/docs/custom_referee_gui.md index bd1c4d765..ce944e734 100644 --- a/docs/custom_referee_gui.md +++ b/docs/custom_referee_gui.md @@ -33,11 +33,13 @@ With vision from a camera (real robots or grSim) the Live tab also shows the fie | ` ` (any other) | | seen inside `field_bounds` but not configured: our planners cannot see it, often a wrong ID | | Ball | | not seen, or outside `field_bounds` | -On real robots any red row forces `HALT`, whatever the command and whether or not the page is +Only the red and amber rows are listed; the green ones fold into an "N ok" line that opens on +click. On real robots any red row forces `HALT`, whatever the command and whether or not the page is open; the red banner says why. Pressing Force Start while a row is red halts again at once, so the check is also the pre-start gate. The field is drawn as ssl-vision sees it (+x to the right), with everything outside `field_bounds` dimmed, so a half-field test shows which half the runner -expects. Space halts; nothing on the keyboard starts the robots. +expects. Space halts; nothing on the keyboard starts the robots. The Kickoff and Free Kick +buttons are hidden when the profile has no rule that sets up a restart (goal detection only). ## Commands and what robots do diff --git a/examples/real_attacker_defender.py b/examples/real_attacker_defender.py index f1b57ae4a..4ac9cf247 100644 --- a/examples/real_attacker_defender.py +++ b/examples/real_attacker_defender.py @@ -10,11 +10,10 @@ - Two robots, one per team, both ours on one radio: the attacker attacks the goal in the camera's half, the defender defends it. - Everything to edit for a test day is in the "Test-day configuration" block: the - field, which part of it the camera sees, each robot's vision and firmware IDs, and - what each robot plays. - - The referee runs with every rule off and nothing advancing on its own, so play - only starts and stops from the dashboard. Real mode starts in HALT; Ctrl+C sends - stop commands to the robots before exiting. + field, which part of it the camera sees, each robot's IDs and working hardware, what + each robot plays, and whether the referee's rules are on. + - Nothing advances on its own: play only starts and stops from the dashboard. Real + mode starts in HALT; Ctrl+C sends stop commands to the robots before exiting. On the dashboard's Live tab, the field check must be all green or amber before Force Start: a red row (robot not seen, outside the camera half, radio lost, wrong field size) halts the @@ -29,6 +28,7 @@ import argparse import dataclasses +from typing import NamedTuple from utama_core.config.field_params import FieldBounds, FieldDimensions from utama_core.custom_referee import CustomReferee @@ -37,6 +37,7 @@ GameConfig, RefereeProfile, RulesConfig, + load_profile, ) from utama_core.dashboard import attach_dashboard from utama_core.dashboard.views import referee as referee_view @@ -49,6 +50,7 @@ from utama_core.tactics.give_and_go import GiveAndGoTactic from utama_core.tactics.lead_and_support import LeadAndSupportTactic from utama_core.tactics.press_and_contain import PressAndContainTactic +from utama_core.team_controller.src.controllers.common.robot_profile import RobotProfile # --------------------------------------------------------------------------- # Test-day configuration @@ -73,20 +75,29 @@ ATTACKER_COLOUR = "yellow" # the defender is the other colour -# Each robot's vision ID (its pattern on top) -> its firmware/radio ID. Swapping a -# robot is a change here only. The two firmware IDs must differ: one radio sends both. -ATTACKER_VISION_ID, ATTACKER_FIRMWARE_ID = 0, 0 -DEFENDER_VISION_ID, DEFENDER_FIRMWARE_ID = 1, 1 -# Robots whose ball sensor (IR) is trusted for `has_ball`, by vision ID. Put every -# robot with a working IR here. An untrusted one counts as having the ball once its -# centre is within 13 cm of the ball, whatever way it faces: the attacker can stop -# ~2 cm short and kick at nothing (seen in grSim), so watch for it if IR is off. -TRUSTED_IR_VISION_IDS: frozenset[int] = frozenset() +class TestRobot(NamedTuple): + vision_id: int # its pattern on top + firmware_id: int # its radio ID; the two robots' must differ, one radio sends both + # What works on it. A robot without a kicker pushes the ball instead of kicking; one + # without a dribbler carries the ball slowly and never backs up; one without a ball + # sensor (ir) has the ball once its centre is within 13 cm of it, whatever way it faces, + # so it can stop ~2 cm short and kick at nothing (seen in grSim). + profile: RobotProfile = RobotProfile(kicker=True, dribbler=True, ir=True) + + +# Swapping a robot is a change here only. +ATTACKER = TestRobot(vision_id=0, firmware_id=0) +DEFENDER = TestRobot(vision_id=1, firmware_id=1) ATTACKER_TACTIC = "lead" # see ATTACKER_TACTICS DEFENDER_TACTIC = "goalkeeper" # see DEFENDER_TACTICS +# "off": only goals are detected. "on": the `human` referee profile's rules (fouls, out of +# bounds, defense area ...) stop play and set up restarts; the operator still starts each +# restart, nothing advances on its own. +RULES = "off" + DASHBOARD_PORT = 8080 # --------------------------------------------------------------------------- @@ -134,8 +145,14 @@ def _build(motion_controller) -> KernelSchedulerStrategy: ) -def _operator_profile() -> RefereeProfile: - """Every rule off and every transition left to the operator, except goals.""" +def _referee_profile() -> RefereeProfile: + """`RULES`' profile; every transition is left to the operator either way.""" + if RULES == "on": + human = load_profile("human") + game = dataclasses.replace(human.game, half_duration_seconds=3600.0, kickoff_team=ATTACKER_COLOUR) + return dataclasses.replace(human, profile_name="attacker_defender_test (rules on)", game=game) + if RULES != "off": + raise ValueError(f"RULES must be 'on' or 'off', got {RULES!r}") defaults = RulesConfig() rules = RulesConfig( **{ @@ -165,8 +182,8 @@ def _operator_profile() -> RefereeProfile: def build_runner(mode: str) -> StrategyRunner: real = mode == "real" # In the simulators each team's robots are numbered from 0. - attacker_id = ATTACKER_VISION_ID if real else 0 - defender_id = DEFENDER_VISION_ID if real else 0 + attacker_id = ATTACKER.vision_id if real else 0 + defender_id = DEFENDER.vision_id if real else 0 attacker_is_yellow = ATTACKER_COLOUR == "yellow" if ATTACKER_COLOUR not in ("yellow", "blue"): raise ValueError(f"ATTACKER_COLOUR must be 'yellow' or 'blue', got {ATTACKER_COLOUR!r}") @@ -175,12 +192,13 @@ def build_runner(mode: str) -> StrategyRunner: # is on the other side. With the full field, the attacker attacks the right goal. attacker_is_right = CAMERA_HALF == "left" - attacker_map = {ATTACKER_VISION_ID: ATTACKER_FIRMWARE_ID} - defender_map = {DEFENDER_VISION_ID: DEFENDER_FIRMWARE_ID} - attacker_ir = TRUSTED_IR_VISION_IDS & {ATTACKER_VISION_ID} - defender_ir = TRUSTED_IR_VISION_IDS & {DEFENDER_VISION_ID} + attacker_map = {ATTACKER.vision_id: ATTACKER.firmware_id} + defender_map = {DEFENDER.vision_id: DEFENDER.firmware_id} + # The profiles apply in the simulators too, to rehearse a test with a broken robot. + attacker_profiles = {attacker_id: ATTACKER.profile} + defender_profiles = {defender_id: DEFENDER.profile} - profile = _operator_profile() + profile = _referee_profile() referee = CustomReferee(profile, n_robots_yellow=1, n_robots_blue=1) server = attach_dashboard(port=DASHBOARD_PORT) referee_view.attach(server, referee, profile) @@ -200,8 +218,8 @@ def build_runner(mode: str) -> StrategyRunner: show_live_status=not real, yellow_vision_to_cmd_mapping=(attacker_map if attacker_is_yellow else defender_map) if real else None, blue_vision_to_cmd_mapping=(defender_map if attacker_is_yellow else attacker_map) if real else None, - yellow_trusted_ir_robots=(attacker_ir if attacker_is_yellow else defender_ir) if real else None, - blue_trusted_ir_robots=(defender_ir if attacker_is_yellow else attacker_ir) if real else None, + yellow_robot_profiles=attacker_profiles if attacker_is_yellow else defender_profiles, + blue_robot_profiles=defender_profiles if attacker_is_yellow else attacker_profiles, ) diff --git a/utama_core/dashboard/static/index.html b/utama_core/dashboard/static/index.html index 75e8a2fd9..152847e62 100644 --- a/utama_core/dashboard/static/index.html +++ b/utama_core/dashboard/static/index.html @@ -34,11 +34,11 @@ -
+
-
+
diff --git a/utama_core/dashboard/static/live.js b/utama_core/dashboard/static/live.js index 213f63463..52d990694 100644 --- a/utama_core/dashboard/static/live.js +++ b/utama_core/dashboard/static/live.js @@ -47,14 +47,25 @@ if (fieldView && layoutChanged) fieldView.setTeamLayout(drawRight, d.my_team_is_yellow); renderFieldCheck(d.field_check); - renderRobotStatusInto("ref-status-entries", d, { hideFeedback: !!d.field_check }); // the field check has the radio rows + // With a field check, its rows already give each robot's position and radio. + if (d.field_check) document.getElementById("ref-status-entries").innerHTML = ""; + else renderRobotStatusInto("ref-status-entries", d); renderTacticStatusInto("ref-tactic-entries", d.tactic_status); d.tactic_tags = tacticTagsFromStatus(d.tactic_status); if (fieldView) fieldView.draw(d); } - // The runner's field checks (utama_core/run/field_check.py): the failures first, so a - // red row is never scrolled away; on real robots any failure has already forced HALT. + // The runner's field checks (utama_core/run/field_check.py): only the problems are listed, + // failures first; the passing rows fold into one "N ok" line that opens on click (kept open + // across updates). On real robots any failure has already forced HALT. + let okOpen = false; + function rowHtml(r) { + const mark = r.status === "ok" ? "●" : r.status === "warn" ? "▲" : "✖"; + return ( + '
' + mark + " " + r.name + + '' + r.detail + "
" + ); + } function renderFieldCheck(fc) { const box = document.getElementById("ref-field-check"); const banner = document.getElementById("ref-auto-halt"); @@ -65,19 +76,19 @@ } banner.style.display = fc.auto_halt ? "" : "none"; banner.textContent = fc.auto_halt ? "Auto-HALT " + fc.auto_halt : ""; - const order = { fail: 0, warn: 1, ok: 2 }; - const rows = fc.rows.slice().sort((a, b) => order[a.status] - order[b.status]); - const fails = rows.filter((r) => r.status === "fail").length; + const fails = fc.rows.filter((r) => r.status === "fail"); + const warns = fc.rows.filter((r) => r.status === "warn"); + const oks = fc.rows.filter((r) => r.status === "ok"); let html = '
Field check — ' + - (fails ? '' + fails + " failing" : 'ready') + + (fails.length ? '' + fails.length + " failing" : 'ready') + "
"; - for (const r of rows) { - html += - '
' + (r.status === "ok" ? "●" : r.status === "warn" ? "▲" : "✖") + - " " + r.name + '' + r.detail + "
"; - } + for (const r of [...fails, ...warns]) html += rowHtml(r); + html += + '
' + oks.length + + " ok" + oks.map(rowHtml).join("") + "
"; box.innerHTML = html; + box.querySelector("#ref-check-ok").addEventListener("toggle", (e) => (okOpen = e.target.open)); } function mount() { @@ -90,6 +101,9 @@ return; } cfg = c; + for (const row of document.querySelectorAll("#view-live .restart-row")) { + row.style.display = c.restarts ? "" : "none"; + } const canvas = document.getElementById("ref-field-canvas"); fieldView = new FieldCanvas(canvas, c.geometry, {}); document.getElementById("ref-profile-name").textContent = c.profile_name; diff --git a/utama_core/dashboard/views/referee.py b/utama_core/dashboard/views/referee.py index 064d00726..fec6d03cf 100644 --- a/utama_core/dashboard/views/referee.py +++ b/utama_core/dashboard/views/referee.py @@ -9,6 +9,7 @@ from __future__ import annotations +import dataclasses import json import time from typing import TYPE_CHECKING, Optional @@ -129,6 +130,9 @@ def _build_static_config(profile: "RefereeProfile") -> dict: gm = profile.game return { "profile_name": profile.profile_name, + # Whether any rule can stop play and set up a restart; the page hides the restart + # buttons when none can (goal detection alone only stops play). + "restarts": any(getattr(r, f.name).enabled for f in dataclasses.fields(r) if f.name != "goal_detection"), "rules": { "goal_detection": { "enabled": r.goal_detection.enabled, From ecc87ae6c77757d7f913ccc31639cebdca29f28c Mon Sep 17 00:00:00 2001 From: isaac0804 Date: Wed, 21 Oct 2026 19:48:02 +0100 Subject: [PATCH 10/56] feat(run,analysis): record field tests as SSL game logs; measure robots from them StrategyRunner(game_log_path=...) records a camera run in the league's game-log format: every vision packet as received, the referee's state, the radio's bytes both ways and a run-info record (ID mappings, profiles), all on this machine's clock. tools/convert_ssl_log.py reads it unchanged. The attacker/defender example records every camera run to replays/field/, and --calibrate plays fixed moves (ScriptedMovesTactic: rest, steps at 0.3/0.6/1 m/s, sideways, turns; a pause restarts the interrupted move). tools/robot_calibration.py measures each robot from such a log: loop delay, speed gain and acceleration per axis (the sim's move model fitted to the camera's positions), vision noise at rest, missed detections and kick speed. On a synthetic robot it recovers delay within 10 ms, acceleration within 15 % and gain within 1 %. Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_01TJGHdb8Dht67hn4Lohndf5 --- docs/roadmap.md | 10 +- docs/sim_realism.md | 2 +- docs/tools.md | 3 +- examples/real_attacker_defender.py | 39 +- tools/robot_calibration.py | 18 + utama_core/analysis/robot_calibration.py | 418 ++++++++++++++++++ .../receivers/vision_receiver.py | 4 + utama_core/replay/game_log_writer.py | 132 ++++++ utama_core/run/strategy_runner.py | 46 +- utama_core/tactics/scripted_moves.py | 100 +++++ .../controllers/real/real_robot_controller.py | 9 +- .../tests/analysis/test_robot_calibration.py | 188 ++++++++ .../engine/test_scripted_moves_tactic.py | 42 ++ utama_core/tests/replay/test_fingerprint.py | 4 +- .../strategy_runner/test_game_log_sides.py | 41 ++ .../strategy_runner/test_runner_misconfig.py | 6 +- 16 files changed, 1048 insertions(+), 14 deletions(-) create mode 100644 tools/robot_calibration.py create mode 100644 utama_core/analysis/robot_calibration.py create mode 100644 utama_core/replay/game_log_writer.py create mode 100644 utama_core/tactics/scripted_moves.py create mode 100644 utama_core/tests/analysis/test_robot_calibration.py create mode 100644 utama_core/tests/engine/test_scripted_moves_tactic.py create mode 100644 utama_core/tests/strategy_runner/test_game_log_sides.py diff --git a/docs/roadmap.md b/docs/roadmap.md index 00578dee6..8624b873f 100644 --- a/docs/roadmap.md +++ b/docs/roadmap.md @@ -155,7 +155,15 @@ entry by its title, not its number. reach of strategy branches, so they are not something a search tunes. - *Open, not yet:* one profile per match day vs updated while playing; kick-to-kick spread in the sim (more realistic, but noisier round-robins). - - *Calibration routine (to write before measuring day):* per robot, at full battery on the + - *Measuring, built 2026-10-21:* every camera run records an SSL game log (raw vision, + referee, radio bytes both ways: `GameLogWriter`); `examples/real_attacker_defender.py + --calibrate` plays fixed moves (`tactics/scripted_moves.py`), and `tools/robot_calibration.py` + measures loop delay, speed gain, acceleration, vision noise and kick speed from any such log. + Checked on a synthetic robot (delay within 10 ms, acceleration within 15 %; turn + acceleration only roughly, its ramp lasts a few frames) and on grSim; not yet on a real robot. + Not yet done: the numbers feed nothing (the sim's latency, the controller), and drift, + dribbling, passes and chips are not measured. + - *Calibration routine (the full plan):* per robot, at full battery on the competition carpet: commanded vs measured speed at a few speeds, acceleration and braking from vision, straight-line drift over 3 m, turn rate; ten kicks (speed from vision, direction error, roll distance) and ten chips; recharge time between kicks; diff --git a/docs/sim_realism.md b/docs/sim_realism.md index 7c05244a0..67c6fde8e 100644 --- a/docs/sim_realism.md +++ b/docs/sim_realism.md @@ -89,7 +89,7 @@ friction). **Within** means kick-to-kick noise for the same robot. | Parameter | Why it matters | From official logs? | rsim today | |---|---|---|---| -| Sensing-to-action latency (vision, decision, radio, motor response) | Likeliest cause of the keeper gap | Only as other teams' total reaction time: from a kick to their keeper accelerating | None: commands act on the next tick | +| Sensing-to-action latency (vision, decision, radio, motor response) | Likeliest cause of the keeper gap | Only as other teams' total reaction time: from a kick to their keeper accelerating; for our robots, `tools/robot_calibration.py` on a recorded field test (command sent to motion seen) | None: commands act on the next tick | | Ball rolling deceleration (and the switch from sliding to rolling) | Pass and shot timing; keeper reaction time | Yes: fit free-rolling segments, per match (venue carpet) | Fixed in the C++ physics (`vendor/rSim`) | | Kick speed: between and within | How much time the keeper has; pass weight | Yes: peak ball speed after each kick, per team | Fixed | | Kick direction error | Wide rate (6% vs 22%) | Partly: shot miss distance, pass error relative to the receiver | None: exact | diff --git a/docs/tools.md b/docs/tools.md index f8bad56a7..a5424a407 100644 --- a/docs/tools.md +++ b/docs/tools.md @@ -112,6 +112,7 @@ described in [STRATEGY_DEVELOPMENT.md](STRATEGY_DEVELOPMENT.md). | `examples/demo_referee_gui_rsim.py` | `CustomReferee` with the dashboard's referee tab, over rsim | | `examples/demo_referee_feedback_gui.py` | The referee UI's controller-feedback panel with fake feedback, no hardware | | `examples/demo_exhibition_road.py` | The Exhibition Road festival demo on the 4 m x 3 m field, over rsim | -| `examples/real_attacker_defender.py` | One attacker against one defender on real robots (or `--mode rsim`/`grsim`): field, camera half and IDs in one config block; the Live tab's field check gates the start | +| `examples/real_attacker_defender.py` | One attacker against one defender on real robots (or `--mode rsim`/`grsim`): field, camera half, IDs and each robot's working hardware in one config block; the Live tab's field check gates the start. Records every camera run as an SSL game log in `replays/field/`; `--calibrate attacker\|defender` plays fixed moves for measuring that robot | +| `tools/robot_calibration.py` | A robot's numbers from a recorded field test (`replays/field/*.log`): loop delay, speed gain and acceleration per axis (the sim's move model fitted to the camera's positions), vision noise at rest and missed detections, kick speed. Writes `.calibration.json`. The same log converts to a replay with `tools/convert_ssl_log.py` | | `examples/demo_dribbler_test.py` | One robot dribbling round a rectangle on the Exhibition Road field | | `examples/demo_split_shape_match.py` | Visible grSim 6v6 of `split_shape` against itself | diff --git a/examples/real_attacker_defender.py b/examples/real_attacker_defender.py index 4ac9cf247..eb88c7c82 100644 --- a/examples/real_attacker_defender.py +++ b/examples/real_attacker_defender.py @@ -5,6 +5,7 @@ pixi run python examples/real_attacker_defender.py --mode rsim pixi run python examples/real_attacker_defender.py --mode grsim # grSim must be running # the dashboard is at http://localhost:8080: press Force Start to play, Halt to freeze + pixi run python examples/real_attacker_defender.py --calibrate attacker # or defender What it does: - Two robots, one per team, both ours on one radio: the attacker attacks the goal in @@ -19,6 +20,13 @@ a red row (robot not seen, outside the camera half, radio lost, wrong field size) halts the robots at once. See docs/custom_referee_gui.md, "Field check". +Measuring a robot (`--calibrate attacker|defender`): that robot plays fixed moves instead +(`tactics/scripted_moves.py`, about 70 s: 10 s still, steps forward and back at 0.3, 0.6 and +1 m/s, sideways, turns) while the other stands still. Put it 1 m inside the area, facing +along the field's long side, and a ball at rest in view; Force Start. Halting (to put it +back) restarts the interrupted move. Then measure it from the recorded log: + pixi run python tools/robot_calibration.py replays/field/.log + Before running on real robots: - ssl-vision's field geometry (length and width) must equal FIELD below, or the runner stops at the first geometry packet with "Field geometry mismatch". @@ -28,7 +36,8 @@ import argparse import dataclasses -from typing import NamedTuple +import time +from typing import NamedTuple, Optional from utama_core.config.field_params import FieldBounds, FieldDimensions from utama_core.custom_referee import CustomReferee @@ -50,6 +59,7 @@ from utama_core.tactics.give_and_go import GiveAndGoTactic from utama_core.tactics.lead_and_support import LeadAndSupportTactic from utama_core.tactics.press_and_contain import PressAndContainTactic +from utama_core.tactics.scripted_moves import ScriptedMovesTactic from utama_core.team_controller.src.controllers.common.robot_profile import RobotProfile # --------------------------------------------------------------------------- @@ -98,6 +108,10 @@ class TestRobot(NamedTuple): # restart, nothing advances on its own. RULES = "off" +# With a camera (real, grsim), record every run as an SSL game log in replays/field/: raw +# vision, referee and radio bytes, for measuring the robots later (`GameLogWriter`). +RECORD = True + DASHBOARD_PORT = 8080 # --------------------------------------------------------------------------- @@ -179,7 +193,7 @@ def _referee_profile() -> RefereeProfile: ) -def build_runner(mode: str) -> StrategyRunner: +def build_runner(mode: str, calibrate: Optional[str] = None) -> StrategyRunner: real = mode == "real" # In the simulators each team's robots are numbered from 0. attacker_id = ATTACKER.vision_id if real else 0 @@ -203,9 +217,21 @@ def build_runner(mode: str) -> StrategyRunner: server = attach_dashboard(port=DASHBOARD_PORT) referee_view.attach(server, referee, profile) + attacker_tactic, defender_tactic = ATTACKER_TACTICS[ATTACKER_TACTIC], DEFENDER_TACTICS[DEFENDER_TACTIC] + if calibrate is not None: + moves, still = ScriptedMovesTactic, lambda: ScriptedMovesTactic(script=()) + attacker_tactic, defender_tactic = (moves, still) if calibrate == "attacker" else (still, moves) + game_log = ( + f"replays/field/{time.strftime('%Y%m%d_%H%M%S')}_{mode}{'_calibrate_' + calibrate if calibrate else ''}.log" + if RECORD and mode != "rsim" + else None + ) + if game_log: + print(f"Recording to {game_log}") return StrategyRunner( - strategy=_one_robot_strategy(attacker_id, ATTACKER_TACTICS[ATTACKER_TACTIC]), - opp_strategy=_one_robot_strategy(defender_id, DEFENDER_TACTICS[DEFENDER_TACTIC]), + game_log_path=game_log, + strategy=_one_robot_strategy(attacker_id, attacker_tactic), + opp_strategy=_one_robot_strategy(defender_id, defender_tactic), my_team_is_yellow=attacker_is_yellow, my_team_is_right=attacker_is_right, mode=mode, @@ -226,8 +252,11 @@ def build_runner(mode: str) -> StrategyRunner: def main() -> None: parser = argparse.ArgumentParser(description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter) parser.add_argument("--mode", choices=("real", "rsim", "grsim"), default=MODE) + parser.add_argument("--calibrate", choices=("attacker", "defender"), help="measure this robot: fixed moves") args = parser.parse_args() - build_runner(args.mode).run() + if args.calibrate and not RECORD: + parser.error("--calibrate needs RECORD = True: the measurement is read from the log") + build_runner(args.mode, args.calibrate).run() if __name__ == "__main__": diff --git a/tools/robot_calibration.py b/tools/robot_calibration.py new file mode 100644 index 000000000..6e7b733ea --- /dev/null +++ b/tools/robot_calibration.py @@ -0,0 +1,18 @@ +# ruff: noqa: E402 +"""A robot's numbers (loop delay, speed and turn tracking, acceleration, vision noise, kick speed) +from a recorded field test. See `utama_core/analysis/robot_calibration.py`. + + pixi run python tools/robot_calibration.py replays/field/.log [--json OUT] +""" + +import sys +from pathlib import Path + +REPO_ROOT = Path(__file__).resolve().parents[1] +if str(REPO_ROOT) not in sys.path: + sys.path.insert(0, str(REPO_ROOT)) + +from utama_core.analysis.robot_calibration import main + +if __name__ == "__main__": + main() diff --git a/utama_core/analysis/robot_calibration.py b/utama_core/analysis/robot_calibration.py new file mode 100644 index 000000000..77cf3fec9 --- /dev/null +++ b/utama_core/analysis/robot_calibration.py @@ -0,0 +1,418 @@ +"""A robot's numbers from a recorded field test: what the sim and the controller need to know. + +Reads an SSL game log written by `GameLogWriter` (`StrategyRunner(game_log_path=...)`) on real +robots: the radio packets as sent and the camera's raw detections, on one clock. Best from a +`--calibrate` run of `examples/real_attacker_defender.py` (fixed moves, `ScriptedMovesTactic`); +any run works, with fewer clean steps. + +Per robot that was sent commands: +- **Steps**: each constant command held at least `MIN_STEP_S`, with the delay, acceleration + and speed reached of the sim's model of a move (`ramp`: still, then constant acceleration up + to a speed) fitted to the robot's positions, and the speed's ratio to the command (`gain`). +- **Loop delay**: the median step delay. Command sent -> robot moves -> camera sees it -> + packet received: the whole sensing-to-action loop, the sim's latency. Measured at the + onset, not by lining the command up with the response, which would add half the time + spent accelerating (the sim models that separately). +- **Vision noise** while the robot is at rest (`STILL_AFTER_S` after its last move): the + standard deviation of a single detection, from second differences / sqrt(6) (as + `physics_ranges` measures the league's), and the share of the camera's frames missing it. + The same for a ball at rest. +- **Kicks**: per kick sent, the ball's top speed within 0.6 s, when the ball was at the robot. + + pixi run python tools/robot_calibration.py replays/field/.log [--json OUT] +""" + +from __future__ import annotations + +import json +import math +from collections import Counter, defaultdict +from pathlib import Path +from typing import Optional + +import numpy as np + +from utama_core.replay.game_log_writer import RADIO_OUT, RUN_INFO +from utama_core.replay.ssl_log import VISION_2014, read_records +from utama_core.team_controller.src.generated_code.ssl_vision_wrapper_pb2 import ( + SSL_WrapperPacket, +) + +FRAME_BYTES = 12 # 0xAA, firmware id, fwd/left/turn float16 LE, dribbler 2 bytes, kicker, 0x55 +MAX_LAG_S = 0.5 +LAG_STEP_S = 0.005 +MIN_STEP_S = 0.4 +STILL_AFTER_S = 1.0 +DIFF = 3 # velocity: central difference over +-3 detections (0.1 s at 60 Hz) +KICK_WINDOW_S = 0.6 +KICK_REACH_M = 0.3 # the ball within this of the robot when the kick is sent +AXES = ("forward", "left", "turn") + + +def decode_radio(packet: bytes) -> list[tuple[int, float, float, float, int]]: + """(firmware id, forward, left, turn, kicker byte) per robot frame in one radio packet.""" + out = [] + for i in range(0, len(packet) - FRAME_BYTES + 1, FRAME_BYTES): + fr = packet[i : i + FRAME_BYTES] + if fr[0] != 0xAA or fr[-1] != 0x55 or fr[1] == 0xFF: + continue + fwd, left, turn = np.frombuffer(bytes(fr[2:8]), dtype=" [(t, fwd, left, turn, kicker)] + robots = defaultdict(list) # (colour, vision id) -> [(t, camera, x, y, th)] + balls = [] # (t, camera, x, y) + self.camera_frames: dict[int, list[float]] = defaultdict(list) + t0 = None + packet = SSL_WrapperPacket() + for ts_ns, msg_type, payload in read_records(path): + t0 = ts_ns if t0 is None else t0 + t = (ts_ns - t0) / 1e9 + if msg_type == RUN_INFO: + self.info = json.loads(payload) + elif msg_type == RADIO_OUT: + for fw, fwd, left, turn, kicker in decode_radio(payload): + commands[fw].append((t, fwd, left, turn, kicker)) + elif msg_type == VISION_2014: + packet.Clear() + packet.ParseFromString(payload) + if not packet.HasField("detection"): + continue + d = packet.detection + self.camera_frames[d.camera_id].append(t) + for colour, detected in (("yellow", d.robots_yellow), ("blue", d.robots_blue)): + for r in detected: + robots[(colour, r.robot_id)].append((t, d.camera_id, r.x / 1000, r.y / 1000, r.orientation)) + if d.balls: + b = max(d.balls, key=lambda b: b.confidence) + balls.append((t, d.camera_id, b.x / 1000, b.y / 1000)) + self.commands = {fw: np.array(rows) for fw, rows in commands.items()} + self.robots = {key: np.array(rows) for key, rows in robots.items()} + self.balls = np.array(balls).reshape(-1, 4) + + def robot_of(self, firmware_id: int) -> Optional[tuple[str, int]]: + """(colour, vision id) driven through `firmware_id`, from the run info.""" + for colour, mapping in self.info.get("vision_to_cmd", {}).items(): + for vision_id, fw in mapping.items(): + if fw == firmware_id: + return colour, int(vision_id) + return None + + +def _main_camera(rows: np.ndarray) -> np.ndarray: + """`rows` from the camera that saw the most of them, in time order, one per time.""" + cam = Counter(rows[:, 1].astype(int).tolist()).most_common(1)[0][0] + rows = rows[rows[:, 1] == cam] + rows = rows[np.argsort(rows[:, 0], kind="stable")] + return rows[np.r_[True, np.diff(rows[:, 0]) > 0]] + + +def local_velocity(track: np.ndarray) -> tuple[np.ndarray, np.ndarray]: + """Times and (forward, left, turn) velocity in the robot's frame from (t, cam, x, y, th) rows.""" + t, x, y, th = track[:, 0], track[:, 2], track[:, 3], np.unwrap(track[:, 4]) + k = DIFF + if len(t) <= 2 * k: + return np.empty(0), np.empty((0, 3)) + dt = t[2 * k :] - t[: -2 * k] + vx, vy, w = ((a[2 * k :] - a[: -2 * k]) / dt for a in (x, y, th)) + c, s = np.cos(th[k:-k]), np.sin(th[k:-k]) + return t[k:-k], np.column_stack([vx * c + vy * s, -vx * s + vy * c, w]) + + +def commanded_at(commands: np.ndarray, t: np.ndarray) -> np.ndarray: + """The (forward, left, turn) last sent before each of `t` (zero before the first).""" + i = np.searchsorted(commands[:, 0], t, side="right") - 1 + out = commands[np.clip(i, 0, None), 1:4].copy() + out[i < 0] = 0.0 + return out + + +def loop_delay(commands: np.ndarray, t: np.ndarray, measured: np.ndarray) -> Optional[float]: + """The lag in [0, MAX_LAG_S] that best correlates the command with the measured velocity: + delay plus about half the time spent accelerating, so only a rough alignment.""" + scale = commands[:, 1:4].std(axis=0) + if not np.any(scale > 0): + return None + scale[scale == 0] = 1.0 + best, best_r = None, -np.inf + for lag in np.arange(0.0, MAX_LAG_S + 1e-9, LAG_STEP_S): + pred = commanded_at(commands, t - lag) / scale + meas = measured / scale + r = np.corrcoef(pred.ravel(), meas.ravel())[0, 1] + if np.isfinite(r) and r > best_r: + best, best_r = float(lag), r + return best + + +_DELAYS = np.arange(0.0, MAX_LAG_S + 1e-9, 0.0025) +_ACCELS = np.geomspace(0.5, 400.0, 80) # m/s² or rad/s² +_SPEED_RANGE = np.linspace(0.96, 1.04, 17) +_FIT_S = 1.5 # at most this much of a step is fitted + + +def ramp(t, delay, accel, speed): + """Displacement of the sim's robot model: still for `delay`, then `accel` up to `speed`. + Arrays broadcast, so one call evaluates a whole grid of models.""" + tau = np.clip(t - delay, 0.0, None) + t_ramp = np.abs(speed) / accel + sign = np.sign(speed) + return np.where(tau < t_ramp, 0.5 * sign * accel * tau**2, sign * accel * t_ramp**2 / 2 + speed * (tau - t_ramp)) + + +def fit_step(t: np.ndarray, displacement: np.ndarray, speed: float) -> tuple[float, float, float]: + """(delay, accel, speed) of `ramp` that best fit `displacement` (least squares, free offset). + + Speed is fitted too, within `_SPEED_RANGE` of `speed`: a speed off by 1 % drifts the model + a few millimetres over a second, as much as the corner that tells delay from acceleration.""" + best, best_err = (0.0, float(_ACCELS[-1]), speed), np.inf + delay, accel = _DELAYS[:, None, None], _ACCELS[None, :, None] + for v in speed * _SPEED_RANGE: + resid = displacement - ramp(t[None, None, :], delay, accel, v) + resid -= resid.mean(axis=2, keepdims=True) + err = (resid**2).sum(axis=2) + i, j = np.unravel_index(int(np.argmin(err)), err.shape) + if err[i, j] < best_err: + best, best_err = (float(_DELAYS[i]), float(_ACCELS[j]), float(v)), err[i, j] + return best + + +def _displacement(track: np.ndarray, axis: int, t_on: float) -> np.ndarray: + """Motion since `t_on` along `axis` of the robot as it stood then (metres, or radians turned).""" + th = np.unwrap(track[:, 4]) + before = (track[:, 0] <= t_on) & (track[:, 0] > t_on - 0.2) + if not np.any(before): + before = np.arange(len(track)) == 0 + x0, y0, th0 = np.median(track[before, 2]), np.median(track[before, 3]), float(np.median(th[before])) + if axis == 2: + return th - th0 + dx, dy = track[:, 2] - x0, track[:, 3] - y0 + c, s = math.cos(th0), math.sin(th0) + return dx * c + dy * s if axis == 0 else -dx * s + dy * c + + +def steps(commands: np.ndarray, track: np.ndarray, t: np.ndarray, measured: np.ndarray, lag: float) -> list[dict]: + """Each constant non-zero command held `MIN_STEP_S` or more, and how the robot followed it. + + The speed reached is read from the velocity (`t`, `measured`) late in the step; `lag` (a + rough loop delay) only places that window. Delay and acceleration come from fitting `ramp` + to the robot's positions in `track`: finite-difference velocity smears the start of a move + by its window, which hides tens of milliseconds of delay.""" + out = [] + values = commands[:, 1:4] + change = np.flatnonzero(np.any(np.diff(values, axis=0) != 0, axis=1)) + 1 + starts = np.r_[0, change] + ends = np.r_[change, len(commands)] + for a, b in zip(starts, ends): + cmd = values[a] + t_on = commands[a, 0] + t_off = commands[b, 0] if b < len(commands) else commands[-1, 0] + if not np.any(cmd) or t_off - t_on < MIN_STEP_S: + continue + axis = int(np.argmax(np.abs(cmd))) + c = float(cmd[axis]) + settled = (t >= t_on + lag + 0.6 * (t_off - t_on)) & (t < t_off + lag) + if not np.any(settled): + continue + reached = float(np.median(measured[settled, axis])) + row = { + "axis": AXES[axis], + "commanded": round(c, 3), + "reached": round(reached, 3), + "gain": round(reached / c, 3), + "seconds": round(float(t_off - t_on), 2), + } + span = (track[:, 0] >= t_on - 0.1) & (track[:, 0] < t_on + min(t_off - t_on, _FIT_S)) + if reached * c > 0 and span.sum() >= 10: + delay, accel, speed = fit_step(track[span, 0] - t_on, _displacement(track, axis, t_on)[span], reached) + row["delay_s"], row["accel"] = round(delay, 4), round(accel, 2) + row["reached"], row["gain"] = round(speed, 3), round(speed / c, 3) + out.append(row) + return out + + +def _second_difference_noise(values: np.ndarray, runs: list[np.ndarray]) -> Optional[float]: + d2 = [np.diff(values[r], 2) for r in runs if len(r) >= 10] + d2 = np.concatenate(d2) if d2 else np.empty(0) + return float(d2.std() / math.sqrt(6)) if len(d2) >= 30 else None + + +def _runs(mask: np.ndarray) -> list[np.ndarray]: + """Index arrays of the consecutive True stretches of `mask`.""" + idx = np.flatnonzero(mask) + if len(idx) == 0: + return [] + return np.split(idx, np.flatnonzero(np.diff(idx) > 1) + 1) + + +def still_noise(track: np.ndarray, commands: np.ndarray) -> dict: + """Detection noise while the robot has been sent no motion for `STILL_AFTER_S`.""" + moving = np.any(commands[:, 1:4] != 0, axis=1) + t_moves = commands[moving, 0] + t = track[:, 0] + i_last = np.searchsorted(t_moves, t, side="right") - 1 + since = np.where(i_last >= 0, t - t_moves[np.clip(i_last, 0, None)], np.inf) + runs = _runs(since >= STILL_AFTER_S) + xy = _second_difference_noise(track[:, 2], runs), _second_difference_noise(track[:, 3], runs) + th = _second_difference_noise(np.unwrap(track[:, 4]), runs) + return { + "samples": int(sum(len(r) for r in runs)), + "position_mm": None if None in xy else round(1000 * max(xy), 2), + "heading_deg": None if th is None else round(math.degrees(th), 3), + } + + +def missing_share(track: np.ndarray, camera_frames: list[float]) -> float: + """Share of the robot's camera's frames, between its first and last detection, without it.""" + frames = np.array(camera_frames) + frames = frames[(frames >= track[0, 0]) & (frames <= track[-1, 0])] + return round(1 - len(track) / len(frames), 4) if len(frames) else 0.0 + + +def ball_noise(balls: np.ndarray) -> dict: + """Detection noise of a ball lying still (moved < 3 mm per frame for a stretch).""" + if len(balls) < 30: + return {"samples": 0, "position_mm": None} + b = _main_camera(np.column_stack([balls[:, :2], balls[:, 2:], np.zeros(len(balls))])) + step = np.hypot(np.diff(b[:, 2]), np.diff(b[:, 3])) + runs = [r for r in _runs(np.r_[step < 0.003, False]) if len(r) >= 30] + xy = _second_difference_noise(b[:, 2], runs), _second_difference_noise(b[:, 3], runs) + return { + "samples": int(sum(len(r) for r in runs)), + "position_mm": None if None in xy else round(1000 * max(xy), 2), + } + + +def kicks(commands: np.ndarray, track: np.ndarray, balls: np.ndarray) -> list[dict]: + """Ball top speed after each kick or chip sent with the ball at the robot.""" + kicking = commands[:, 4] != 0 + edges = np.flatnonzero(kicking & ~np.r_[False, kicking[:-1]]) + if len(balls) < 10: + return [] + b = _main_camera(np.column_stack([balls[:, :2], balls[:, 2:], np.zeros(len(balls))])) + k = 2 + bt = b[k:-k, 0] + speed = np.hypot(b[2 * k :, 2] - b[: -2 * k, 2], b[2 * k :, 3] - b[: -2 * k, 3]) / (b[2 * k :, 0] - b[: -2 * k, 0]) + out = [] + for e in edges: + t_kick = commands[e, 0] + i_r = np.searchsorted(track[:, 0], t_kick) + i_b = np.searchsorted(b[:, 0], t_kick) + if i_r >= len(track) or i_b >= len(b): + continue + gap = math.hypot(track[i_r, 2] - b[i_b, 2], track[i_r, 3] - b[i_b, 3]) + after = (bt >= t_kick) & (bt <= t_kick + KICK_WINDOW_S) & (speed < 15.0) + if gap > KICK_REACH_M or not np.any(after): + continue + out.append( + { + "t": round(float(t_kick), 2), + "chip": bool(commands[e, 4] == 0x0F), + "ball_speed": round(float(speed[after].max()), 2), + } + ) + return out + + +def calibrate(log: FieldLog) -> list[dict]: + """One result per robot sent any motion, as described in the module docstring.""" + results = [] + for fw, commands in sorted(log.commands.items()): + if not np.any(commands[:, 1:4] != 0): + continue + robot = log.robot_of(fw) + if robot is None or robot not in log.robots: + results.append({"firmware_id": fw, "error": "not seen by vision (no run info or never detected)"}) + continue + track = _main_camera(log.robots[robot]) + t, measured = local_velocity(track) + rough = loop_delay(commands, t, measured) + step_rows = steps(commands, track, t, measured, rough or 0.0) + delays = [r["delay_s"] for r in step_rows if "delay_s" in r] + lag = round(float(np.median(delays)), 3) if delays else None + summary = {} + for axis in AXES: + rows = [r for r in step_rows if r["axis"] == axis] + accel = [r["accel"] for r in rows if "accel" in r] + if rows: + summary[axis] = { + "gain": round(float(np.median([r["gain"] for r in rows])), 3), + "accel": round(float(np.median(accel)), 2) if accel else None, + "steps": len(rows), + } + robot_kicks = kicks(commands, track, log.balls) + results.append( + { + "robot": f"{robot[0]} {robot[1]}", + "firmware_id": fw, + "loop_delay_s": lag, + "summary": summary, + "vision": { + **still_noise(track, commands), + "missing_share": missing_share(track, log.camera_frames[int(track[0, 1])]), + }, + "steps": step_rows, + "kicks": robot_kicks, + "kick_speed_median": ( + round(float(np.median([k["ball_speed"] for k in robot_kicks if not k["chip"]])), 2) + if any(not k["chip"] for k in robot_kicks) + else None + ), + } + ) + return results + + +def report(results: list[dict], ball: dict) -> str: + lines = [] + for r in results: + if "error" in r: + lines.append(f"firmware {r['firmware_id']}: {r['error']}") + continue + lines.append(f"{r['robot']} (firmware {r['firmware_id']})") + lag = r["loop_delay_s"] + lines.append(f" loop delay {'-' if lag is None else f'{lag * 1000:.0f} ms'}") + for axis, s in r["summary"].items(): + unit = "rad/s²" if axis == "turn" else "m/s²" + accel = "-" if s["accel"] is None else f"{s['accel']} {unit}" + lines.append(f" {axis:<8} gain {s['gain']:<6} accel {accel:<14} ({s['steps']} steps)") + v = r["vision"] + lines.append( + f" vision at rest {v['position_mm']} mm, {v['heading_deg']}°, " + f"{v['missing_share']:.1%} frames missing ({v['samples']} samples)" + ) + if r["kicks"]: + lines.append(f" kicks {len(r['kicks'])}, median ball speed {r['kick_speed_median']} m/s") + for s in r["steps"]: + lines.append( + f" {s['axis']:<8} {s['commanded']:>6} -> {s['reached']:>6} " + f"(gain {s['gain']}, accel {s.get('accel', '-')}, delay {s.get('delay_s', '-')} s, {s['seconds']} s)" + ) + lines.append(f"ball at rest {ball['position_mm']} mm ({ball['samples']} samples)") + return "\n".join(lines) + + +def main(argv: Optional[list[str]] = None) -> None: + import argparse + + parser = argparse.ArgumentParser(description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter) + parser.add_argument("log", type=Path) + parser.add_argument("--json", type=Path, help="default: .calibration.json beside the log") + args = parser.parse_args(argv) + log = FieldLog(args.log) + results = calibrate(log) + ball = ball_noise(log.balls) + print(report(results, ball)) + out = args.json or args.log.with_suffix(".calibration.json") + out.write_text(json.dumps({"log": str(args.log), "run_info": log.info, "robots": results, "ball": ball}, indent=1)) + print(f"written {out}") + + +if __name__ == "__main__": + main() diff --git a/utama_core/data_processing/receivers/vision_receiver.py b/utama_core/data_processing/receivers/vision_receiver.py index ff97490e2..5d533171a 100644 --- a/utama_core/data_processing/receivers/vision_receiver.py +++ b/utama_core/data_processing/receivers/vision_receiver.py @@ -21,12 +21,14 @@ def __init__( vision_buffers: List[Deque[RawVisionData]], on_geometry: Optional[Callable] = None, stop_event: Optional[threading.Event] = None, + on_packet: Optional[Callable[[bytes], None]] = None, ): self.net = network_manager.NetworkManager(address=(MULTICAST_GROUP, VISION_PORT), bind_socket=True) self.vision_buffers = vision_buffers self._on_geometry = on_geometry self._geometry_fired = False self._stop_event = stop_event + self._on_packet = on_packet # every packet as received, before parsing (`GameLogWriter`) # Parked exception from the geometry callback — re-raised in the main # thread by StrategyRunner._run_step() so the process exits cleanly. self.thread_exception: Optional[BaseException] = None @@ -55,6 +57,8 @@ def pull_game_data(self) -> None: def _handle_packet(self, vision_packet: SSL_WrapperPacket, data: bytes) -> bool: """Parse one wrapper packet into `vision_packet` and buffer its detection. Returns False when the geometry callback failed and the receiver should stop.""" + if self._on_packet: + self._on_packet(data) vision_packet.Clear() vision_packet.ParseFromString(data) if self._on_geometry and not self._geometry_fired and vision_packet.HasField("geometry"): diff --git a/utama_core/replay/game_log_writer.py b/utama_core/replay/game_log_writer.py new file mode 100644 index 000000000..c4fd6cb40 --- /dev/null +++ b/utama_core/replay/game_log_writer.py @@ -0,0 +1,132 @@ +"""Record a run with a camera as an SSL game log: what the network and the radio carried. + +Our replays hold the filtered world; measuring the real robots needs what went in and out +before any filter: every vision packet as received (raw detections with ssl-vision's +`t_capture`), the referee's state, and the radio's bytes both ways. `GameLogWriter` writes +them in the league's game-log format (`ssl_log.py`), so the tools for official logs read a +field test unchanged: `tools/convert_ssl_log.py` turns it into a replay, and its raw-vision +sidecar gives our camera's noise. Records: + +- `VISION_2014`: each `SSL_WrapperPacket`, byte for byte. +- `REFEREE`: a `Referee` message whenever the referee's state changes. The in-process + referee sends no packets, so the message is built from its `RefereeData`. +- `RADIO_OUT` / `RADIO_IN` (ours; other readers skip unknown types): each packet written to + the radio (12 bytes per robot, `RealRobotController._generate_command_buffer`) and the + feedback bytes read back. Commanded against seen motion gives the control delay. +- `RUN_INFO` (ours): one JSON object at the start: mode, colours, sides, each team's vision + ID -> firmware ID mapping and robot profiles, so the radio's IDs can be matched to vision's. + +Every record is stamped with the receive (or send) time on this machine's clock, +`time.time_ns()`, so vision and radio share one clock: their difference is the whole loop, +camera to robot to camera. The file is written uncompressed and flushed about once a second, +so a crash loses at most that second (`read_records` drops a cut-off last record). +""" + +from __future__ import annotations + +import json +import struct +import threading +import time +from pathlib import Path +from typing import Optional + +from utama_core.entities.data.referee import RefereeData +from utama_core.team_controller.src.generated_code.ssl_gc_referee_message_pb2 import ( + Referee, +) + +# The league's format, as `ssl_log` reads it (not imported from there: the runner would pull +# in the whole reader). +LOG_HEADER = b"SSL_LOG_FILE" +REFEREE, VISION_2014 = 3, 4 +RADIO_OUT, RADIO_IN, RUN_INFO = 100, 101, 102 +_FLUSH_S = 1.0 + + +def referee_message(data: RefereeData, command_counter: int) -> Referee: + """`data` as the game controller would have sent it (times in us, positions in mm).""" + msg = Referee( + source_identifier=data.source_identifier or "utama", + packet_timestamp=int(data.time_sent * 1e6), + stage=data.stage.value, + stage_time_left=int(data.stage_time_left * 1e6), + command=data.referee_command.value, + command_counter=command_counter, + command_timestamp=int(data.referee_command_timestamp * 1e6), + ) + for team, info in ((msg.yellow, data.yellow_team), (msg.blue, data.blue_team)): + team.name = info.name + team.score = info.score + team.red_cards = info.red_cards + team.yellow_card_times.extend(info.yellow_card_times) + team.yellow_cards = info.yellow_cards + team.timeouts = info.timeouts + team.timeout_time = info.timeout_time + team.goalkeeper = info.goalkeeper + if data.designated_position is not None: + msg.designated_position.x = data.designated_position[0] * 1000 + msg.designated_position.y = data.designated_position[1] * 1000 + if data.blue_team_on_positive_half is not None: + msg.blue_team_on_positive_half = data.blue_team_on_positive_half + if data.next_command is not None: + msg.next_command = data.next_command.value + return msg + + +class GameLogWriter: + """Appends records to `path` (a `.log`); safe to call from the vision thread and the main loop.""" + + def __init__(self, path: str | Path): + self.path = Path(path) + self.path.parent.mkdir(parents=True, exist_ok=True) + self._file = open(self.path, "wb") + self._file.write(LOG_HEADER + struct.pack(">i", 1)) + self._lock = threading.Lock() + self._last_flush = time.monotonic() + self._referee_key: Optional[tuple] = None + self._command_counter = 0 + + def write(self, msg_type: int, payload: bytes, ts_ns: Optional[int] = None) -> None: + ts_ns = time.time_ns() if ts_ns is None else ts_ns + with self._lock: + if self._file.closed: + return + self._file.write(struct.pack(">qii", ts_ns, msg_type, len(payload)) + payload) + if time.monotonic() - self._last_flush > _FLUSH_S: + self._file.flush() + self._last_flush = time.monotonic() + + def run_info(self, info: dict) -> None: + self.write(RUN_INFO, json.dumps(info).encode()) + + def vision(self, packet: bytes) -> None: + self.write(VISION_2014, packet) + + def radio_out(self, packet: bytes) -> None: + self.write(RADIO_OUT, packet) + + def radio_in(self, data: bytes) -> None: + self.write(RADIO_IN, data) + + def referee(self, data: RefereeData) -> None: + """Records `data` when it differs from the last one recorded, clocks aside.""" + key = ( + data.referee_command, + data.stage, + data.yellow_team.score, + data.blue_team.score, + data.designated_position, + data.next_command, + data.blue_team_on_positive_half, + ) + if key == self._referee_key: + return + if self._referee_key is None or key[0] != self._referee_key[0]: + self._command_counter += 1 + self._referee_key = key + self.write(REFEREE, referee_message(data, self._command_counter).SerializeToString()) + + def close(self) -> None: + with self._lock: + self._file.close() diff --git a/utama_core/run/strategy_runner.py b/utama_core/run/strategy_runner.py index 330d44e95..b522955dd 100644 --- a/utama_core/run/strategy_runner.py +++ b/utama_core/run/strategy_runner.py @@ -49,6 +49,7 @@ ColumnarReplayWriter, ColumnarReplayWriterConfig, ) +from utama_core.replay.game_log_writer import GameLogWriter from utama_core.replay.replay_writer import ReplayWriter, ReplayWriterConfig from utama_core.rsoccer_simulator.src.ssl.envs import SSLStandardEnv from utama_core.rsoccer_simulator.src.Utils.gaussian_noise import RsimGaussianNoise @@ -261,6 +262,9 @@ class StrategyRunner(SimRecoveryMixin, VisionStreamStatusMixin, LiveStatusMixin) Applies in every mode, so a test with broken hardware can be rehearsed in a simulator. ``None``: everything works. blue_robot_profiles (dict[int, RobotProfile], optional): Same as ``yellow_robot_profiles`` for the blue team. + game_log_path (str, optional): Record the run as an SSL game log (`.log`): every vision packet, + the referee's state and, on real robots, the radio's bytes both ways; see `GameLogWriter`. + grSim and real mode only (rsim has no packets). """ def __init__( @@ -298,6 +302,7 @@ def __init__( blue_trusted_ir_robots: Optional[FrozenSet[int]] = None, yellow_robot_profiles: Optional[dict[int, RobotProfile]] = None, blue_robot_profiles: Optional[dict[int, RobotProfile]] = None, + game_log_path: Optional[str] = None, ): self.logger = logging.getLogger(__name__) @@ -345,6 +350,9 @@ def __init__( self.referee.override_geometry(RefereeGeometry.from_field_dims(self.full_field_dims)) + if game_log_path is not None and self.mode == Mode.RSIM: + raise ValueError("game_log_path needs vision packets: grsim or real mode, not rsim") + self.game_log = GameLogWriter(game_log_path) if game_log_path is not None else None self.vision_buffers, self.ref_buffer = self._setup_vision_and_referee() assert_valid_bounding_box( @@ -371,6 +379,22 @@ def __init__( self.yellow_robot_profiles = yellow_robot_profiles or {} self.blue_robot_profiles = blue_robot_profiles or {} + if self.game_log is not None: + self.game_log.run_info( + { + "mode": self.mode.value, + "my_team_is_yellow": my_team_is_yellow, + "my_team_is_right": my_team_is_right, + "vision_to_cmd": { + "yellow": self.yellow_vision_to_cmd_mapping, + "blue": self.blue_vision_to_cmd_mapping, + }, + "profiles": { + "yellow": {k: dataclasses.asdict(p) for k, p in self.yellow_robot_profiles.items()}, + "blue": {k: dataclasses.asdict(p) for k, p in self.blue_robot_profiles.items()}, + }, + } + ) for is_yellow in (True, False): self._check_profiles_are_driven(is_yellow) yellow_trusted_ir_robots = self._trusted_ir(True, yellow_trusted_ir_robots) @@ -903,7 +927,11 @@ def _setup_vision_and_referee(self) -> Tuple[deque, deque]: ref_buffer = deque(maxlen=1) if self.mode != Mode.RSIM: on_geometry = self._make_geometry_validation_callback() - vision_receiver = VisionReceiver(vision_buffers, on_geometry=on_geometry) + vision_receiver = VisionReceiver( + vision_buffers, + on_geometry=on_geometry, + on_packet=self.game_log.vision if getattr(self, "game_log", None) is not None else None, + ) if isinstance(self.referee, OfficialReferee): self.start_threads(vision_receiver, RefereeMessageReceiver(ref_buffer)) else: @@ -1113,6 +1141,11 @@ def _load_robot_controllers(self): else: raise ValueError("mode is invalid. Must be 'rsim', 'grsim' or 'real'") + if self.mode == Mode.REAL: + my_robot_controller.game_log = getattr(self, "game_log", None) + if self.opp: + opp_robot_controller.game_log = my_robot_controller.game_log + my_profiles, opp_profiles = map_colors_to_friendly_enemy( self.my_team_is_yellow, self.yellow_robot_profiles, self.blue_robot_profiles ) @@ -1260,6 +1293,13 @@ def _load_game(self): if self.opp: self.opp.strategy.load_game(self.opp.game) + def _with_sides(self, referee_data: "RefereeData") -> "RefereeData": + """`referee_data` with the sides filled in from ours when the referee leaves them out (ours does).""" + if referee_data.blue_team_on_positive_half is not None: + return referee_data + blue_on_positive = self.my_team_is_right != self.my_team_is_yellow + return dataclasses.replace(referee_data, blue_team_on_positive_half=blue_on_positive) + def _follow_referee_sides(self, referee_data: Optional["RefereeData"]) -> None: """Change ends when the referee says the teams have. @@ -1369,6 +1409,8 @@ def close(self, stop_command_repeat: int = 20): self.match_stats.finalize().to_json(self.stats_path) if self.replay_writer: self.replay_writer.close() + if getattr(self, "game_log", None) is not None: + self.game_log.close() # after the stop commands, so they are in the log if self.vision_stream: self.vision_stream.stop() if self.rsim_env: @@ -1711,6 +1753,8 @@ def _run_step(self): if self.ref_buffer: self._last_referee_data = self.ref_buffer.popleft() referee_data = self._last_referee_data + if getattr(self, "game_log", None) is not None and referee_data is not None: + self.game_log.referee(self._with_sides(referee_data)) self._follow_referee_sides(referee_data) diff --git a/utama_core/tactics/scripted_moves.py b/utama_core/tactics/scripted_moves.py new file mode 100644 index 000000000..d0438166b --- /dev/null +++ b/utama_core/tactics/scripted_moves.py @@ -0,0 +1,100 @@ +"""Fixed moves for measuring a robot: the commands are a script, not a decision. + +`ScriptedMovesTactic` sends `CALIBRATION_SCRIPT`'s raw velocity commands, step by step, to the +one robot it is given, then stands still. Run with a game log being recorded +(`StrategyRunner(game_log_path=...)`), the log holds each command as sent and the robot's +motion as the camera saw it; `tools/robot_calibration.py` turns that into the robot's +numbers (vision noise, delay, speed and turn tracking, acceleration). + +Each move goes and comes back (forward then backward, left then right), so the robot +ends near where it started; the longest is 0.8 m forward. Pauses (HALT, STOP) do not +advance the script, and the step a pause interrupted starts again, so no measurement is +split: halt to put the robot back, then Force Start to carry on. +""" + +from __future__ import annotations + +from typing import NamedTuple, Optional + +from utama_core.engine.context import TickContext +from utama_core.engine.tactic import BaseTactic, RobotId, TacticTag +from utama_core.entities.data.command import RobotCommand +from utama_core.entities.game import Game + + +class Move(NamedTuple): + name: str + seconds: float + forward: float = 0.0 # m/s, robot frame (kicker forward) + left: float = 0.0 # m/s + turn: float = 0.0 # rad/s, anticlockwise + + +_REST = 1.5 # s still between moves, so each response settles +_GAP_S = 0.1 # a tick this long after the last one means play was paused + + +def _there_and_back(name: str, speed: float, distance: float, sideways: bool = False) -> list[Move]: + t = distance / speed + axis = "left" if sideways else "forward" + go, back = Move(name, t, **{axis: speed}), Move(name, t, **{axis: -speed}) + return [go, Move("rest", _REST), back, Move("rest", _REST)] + + +def _turns(rate: float, seconds: float) -> list[Move]: + name = f"turn {rate:g} rad/s" + return [Move(name, seconds, turn=rate), Move("rest", _REST), Move(name, seconds, turn=-rate), Move("rest", _REST)] + + +# About 70 s. Speeds stay within the real robots' caps (REAL_PARAMS: 1 m/s, 4 rad/s). +CALIBRATION_SCRIPT: tuple[Move, ...] = ( + Move("still", 10.0), # vision noise: robot (and a ball placed in view) at rest + *_there_and_back("forward 0.3 m/s", 0.3, 0.6), + *_there_and_back("forward 0.6 m/s", 0.6, 0.8), + *_there_and_back("forward 1 m/s", 1.0, 0.8), + *_there_and_back("sideways 0.5 m/s", 0.5, 0.6, sideways=True), + *_turns(2.0, 3.0), + *_turns(4.0, 1.6), + Move("still", 3.0), +) + + +class ScriptedMovesTactic(BaseTactic[None]): + """Plays `script` on its first robot; others (and the first, once done) stand still.""" + + tag = TacticTag.MIXED + + def __init__(self, script: tuple[Move, ...] = CALIBRATION_SCRIPT): + self.script = script + # Progress lives here, not in `mem`: the kernel may reset `mem` when play resumes. + self._step = 0 + self._step_elapsed = 0.0 + self._last_ts: Optional[float] = None + + def initial_mem(self) -> None: + return None + + @property + def done(self) -> bool: + return self._step >= len(self.script) + + def tick( + self, game: Game, ctx: TickContext, robot_ids: tuple[RobotId, ...], mem: None + ) -> tuple[dict[RobotId, RobotCommand], None]: + still = RobotCommand(0.0, 0.0, 0.0, kick=False, chip=False, dribble=False) + commands = {rid: still for rid in robot_ids} + if not robot_ids: + return commands, mem + dt = 0.0 if self._last_ts is None else game.ts - self._last_ts + self._last_ts = game.ts + if dt > _GAP_S: + self._step_elapsed = 0.0 # resumed after a pause: this step again, from its start + else: + self._step_elapsed += dt + while not self.done and self._step_elapsed >= self.script[self._step].seconds: + self._step_elapsed -= self.script[self._step].seconds + self._step += 1 + if not self.done: + m = self.script[self._step] + commands[robot_ids[0]] = RobotCommand(m.forward, m.left, m.turn, kick=False, chip=False, dribble=False) + return commands, mem diff --git a/utama_core/team_controller/src/controllers/real/real_robot_controller.py b/utama_core/team_controller/src/controllers/real/real_robot_controller.py index 85d49c50a..cfef8808a 100644 --- a/utama_core/team_controller/src/controllers/real/real_robot_controller.py +++ b/utama_core/team_controller/src/controllers/real/real_robot_controller.py @@ -76,6 +76,8 @@ def __init__( self._out_packet = self._empty_command() self._in_packet_size = 1 # size of the feedback packet received from the robots self._robots_info: List[RobotResponse] = [None] * self._n_friendly + # Set by StrategyRunner to record the radio's bytes both ways (`GameLogWriter`). + self.game_log = None if serial_port is None: logger.debug(f"Serial port: {PORT} opened with baudrate: {BAUD_RATE} and timeout {TIMEOUT}") else: @@ -111,7 +113,10 @@ def get_robots_responses(self) -> List[RobotResponse]: # 1. Non-blocking read: Only read if bytes are actually waiting bytes_available = self._serial_port.in_waiting if bytes_available > 0: - self._buffer.extend(self._serial_port.read(bytes_available)) + data = self._serial_port.read(bytes_available) + self._buffer.extend(data) + if self.game_log is not None: + self.game_log.radio_in(bytes(data)) responses = [] responded_ids = set() @@ -179,6 +184,8 @@ def send_robot_commands(self) -> None: f"Only {len(self._assigned_mapping)} out of {self._n_friendly} robots have been assigned commands. Sending empty commands for unassigned robots." ) self._serial_port.write(self._out_packet) + if self.game_log is not None: + self.game_log.radio_out(bytes(self._out_packet)) ### update kick and chip trackers. We persist the kick/chip command for KICKER_PERSIST_TIMESTEPS ### this feature is to combat packet loss and to ensure the robot does not kick within its cooldown period diff --git a/utama_core/tests/analysis/test_robot_calibration.py b/utama_core/tests/analysis/test_robot_calibration.py new file mode 100644 index 000000000..d24e65df3 --- /dev/null +++ b/utama_core/tests/analysis/test_robot_calibration.py @@ -0,0 +1,188 @@ +"""`robot_calibration` recovers a robot's known numbers from a synthetic field log, and the log +it reads is what the radio and the referee really write.""" + +from __future__ import annotations + +import math + +import numpy as np +import pytest + +from utama_core.analysis.robot_calibration import FieldLog, calibrate, decode_radio +from utama_core.entities.data.command import RobotCommand +from utama_core.entities.data.referee import RefereeData +from utama_core.entities.game.team_info import TeamInfo +from utama_core.entities.referee.referee_command import RefereeCommand +from utama_core.entities.referee.stage import Stage +from utama_core.replay.game_log_writer import RADIO_OUT, GameLogWriter +from utama_core.replay.ssl_log import REFEREE, read_records, referee_data +from utama_core.team_controller.src.controllers.real.real_robot_controller import ( + RealRobotController, +) +from utama_core.team_controller.src.generated_code.ssl_gc_referee_message_pb2 import ( + Referee, +) +from utama_core.team_controller.src.generated_code.ssl_vision_wrapper_pb2 import ( + SSL_WrapperPacket, +) + +DELAY, ACCEL, TURN_ACCEL, GAIN = 0.08, 3.0, 30.0, 0.9 +NOISE_M, NOISE_RAD, DROPPED = 0.001, math.radians(0.5), 0.02 +FIRMWARE, VISION_ID = 3, 1 + + +class _Serial: + port = "fake" + in_waiting = 0 + + def __init__(self): + self.written: list[bytes] = [] + + def write(self, data): + self.written.append(bytes(data)) + + +def _radio_packet(controller: RealRobotController, serial: _Serial, command: RobotCommand) -> bytes: + controller.add_robot_commands(command, VISION_ID) + controller.send_robot_commands() + return serial.written[-1] + + +def _vision_packet(t: float, x: float, y: float, th: float) -> bytes: + packet = SSL_WrapperPacket() + d = packet.detection + d.frame_number, d.t_capture, d.t_sent, d.camera_id = int(t * 60), t, t, 0 + r = d.robots_yellow.add() + r.confidence, r.robot_id, r.x, r.y, r.orientation, r.pixel_x, r.pixel_y = ( + 1.0, + VISION_ID, + x * 1000, + y * 1000, + th, + 0, + 0, + ) + return packet.SerializeToString() + + +def _script() -> list[tuple[float, RobotCommand]]: + """(seconds, command): rest, then forward and back, sideways, a turn.""" + still = RobotCommand(0.0, 0.0, 0.0, False, False, False) + moves = [(3.0, still)] + for fwd, left, turn in [(0.5, 0, 0), (-0.5, 0, 0), (0, 0.4, 0), (0, -0.4, 0), (0, 0, 2.0), (0, 0, -2.0)]: + moves += [(1.5, RobotCommand(fwd, left, turn, False, False, False)), (2.0, still)] + return moves + + +@pytest.fixture(scope="module") +def synthetic_log(tmp_path_factory): + """A robot that follows each command `DELAY` late, at `GAIN` of it, accelerating at `ACCEL` + (`TURN_ACCEL` turning), seen at 60 Hz with `NOISE_*` and `DROPPED` detections missing.""" + path = tmp_path_factory.mktemp("field") / "synthetic.log" + log = GameLogWriter(path) + log.run_info({"vision_to_cmd": {"yellow": {str(VISION_ID): FIRMWARE}, "blue": {}}}) + serial = _Serial() + controller = RealRobotController(True, 1, vision_to_cmd_mapping={VISION_ID: FIRMWARE}, serial_port=serial) + rng = np.random.default_rng(0) + + sent = [] # (time, command) + t_end = 0.0 + for seconds, command in _script(): + for k in range(round(seconds * 60)): + sent.append((t_end + k / 60, command)) + t_end += seconds + for t, command in sent: + log.write(RADIO_OUT, _radio_packet(controller, serial, command), ts_ns=int(t * 1e9)) + + dt, x, y, th = 0.001, 0.0, 0.0, 0.3 + v = np.zeros(3) # forward, left, turn + next_frame = 0.008 + times = np.array([t for t, _ in sent]) + for i in range(int(t_end / dt)): + t = i * dt + j = np.searchsorted(times, t - DELAY, side="right") - 1 + target = GAIN * np.array(sent[j][1][:3]) if j >= 0 else np.zeros(3) + limit = np.array([ACCEL, ACCEL, TURN_ACCEL]) * dt + v += np.clip(target - v, -limit, limit) + x += (v[0] * math.cos(th) - v[1] * math.sin(th)) * dt + y += (v[0] * math.sin(th) + v[1] * math.cos(th)) * dt + th += v[2] * dt + if t >= next_frame: + if rng.random() >= DROPPED: + nx, ny, nth = rng.normal(0, [NOISE_M, NOISE_M, NOISE_RAD]) + log.write(4, _vision_packet(t, x + nx, y + ny, th + nth), ts_ns=int(t * 1e9)) + else: # the camera sent the frame, without the robot + packet = SSL_WrapperPacket() + d = packet.detection + d.frame_number, d.t_capture, d.t_sent, d.camera_id = int(t * 60), t, t, 0 + log.write(4, packet.SerializeToString(), ts_ns=int(t * 1e9)) + next_frame += 1 / 60 + log.close() + return path + + +def test_the_writers_format_constants_are_the_readers(): + from utama_core.replay import game_log_writer, ssl_log + + assert (game_log_writer.LOG_HEADER, game_log_writer.REFEREE, game_log_writer.VISION_2014) == ( + ssl_log.LOG_HEADER, + ssl_log.REFEREE, + ssl_log.VISION_2014, + ) + + +def test_the_radio_decoder_reads_what_the_real_controller_sends(): + serial = _Serial() + controller = RealRobotController(True, 1, vision_to_cmd_mapping={VISION_ID: FIRMWARE}, serial_port=serial) + packet = _radio_packet(controller, serial, RobotCommand(0.5, -0.25, 2.0, kick=True, chip=False, dribble=False)) + ((fw, fwd, left, turn, kicker),) = decode_radio(packet) + assert (fw, fwd, left, turn, kicker) == (FIRMWARE, 0.5, -0.25, 2.0, 0xF0) + + +def test_calibration_recovers_the_synthetic_robots_numbers(synthetic_log): + (result,) = calibrate(FieldLog(synthetic_log)) + assert result["robot"] == "yellow 1" and result["firmware_id"] == FIRMWARE + # The onset, not the lag that lines the command up with the response: that one adds about + # half the ramp (0.45 m/s / 3 m/s² / 2 = 75 ms) and would fail this. + assert result["loop_delay_s"] == pytest.approx(DELAY, abs=0.01) + forward, left, turn = (result["summary"][a] for a in ("forward", "left", "turn")) + assert forward["steps"] == 2 and left["steps"] == 2 and turn["steps"] == 2 + for s in (forward, left, turn): + assert s["gain"] == pytest.approx(GAIN, abs=0.02) + assert forward["accel"] == pytest.approx(ACCEL, rel=0.15) + # Turning reaches its speed in ~4 camera frames, so its acceleration is only roughly known. + assert turn["accel"] == pytest.approx(TURN_ACCEL, rel=0.5) + vision = result["vision"] + assert vision["position_mm"] == pytest.approx(1000 * NOISE_M, rel=0.25) + assert vision["heading_deg"] == pytest.approx(math.degrees(NOISE_RAD), rel=0.25) + assert vision["missing_share"] == pytest.approx(DROPPED, abs=0.01) + + +def test_the_referee_record_reads_back_as_the_same_referee_data(tmp_path): + data = RefereeData( + source_identifier="utama", + time_sent=12.5, + time_received=12.5, + referee_command=RefereeCommand.DIRECT_FREE_BLUE, + referee_command_timestamp=10.0, + stage=Stage.NORMAL_FIRST_HALF, + stage_time_left=200.0, + blue_team=TeamInfo("blue", score=1, goalkeeper=0), + yellow_team=TeamInfo("yellow", score=2, goalkeeper=1), + designated_position=(1.5, -0.5), + blue_team_on_positive_half=True, + next_command=RefereeCommand.FORCE_START, + ) + log = GameLogWriter(tmp_path / "r.log") + log.referee(data) + log.referee(data) # unchanged: not written again + log.close() + records = [p for _, kind, p in read_records(tmp_path / "r.log") if kind == REFEREE] + assert len(records) == 1 + msg = Referee() + msg.ParseFromString(records[0]) + back = referee_data(msg, received_s=12.5) + assert back.referee_command == data.referee_command and back.stage == data.stage + assert (back.yellow_team.score, back.blue_team.score) == (2, 1) + assert back.designated_position == pytest.approx(data.designated_position) + assert back.blue_team_on_positive_half is True and back.next_command == RefereeCommand.FORCE_START diff --git a/utama_core/tests/engine/test_scripted_moves_tactic.py b/utama_core/tests/engine/test_scripted_moves_tactic.py new file mode 100644 index 000000000..95abd0ca7 --- /dev/null +++ b/utama_core/tests/engine/test_scripted_moves_tactic.py @@ -0,0 +1,42 @@ +"""`ScriptedMovesTactic` plays its moves in order, restarts a move a pause interrupted, then stands still.""" + +from __future__ import annotations + +from types import SimpleNamespace + +from utama_core.tactics.scripted_moves import Move, ScriptedMovesTactic + +SCRIPT = (Move("go", 0.5, forward=0.3), Move("turn", 0.5, turn=1.0)) + + +def _tick(tactic: ScriptedMovesTactic, ts: float, robot_ids=(4, 7)): + commands, _ = tactic.tick(SimpleNamespace(ts=ts), None, robot_ids, None) + return commands + + +def _run(tactic, t0: float, seconds: float) -> dict: + commands = None + for k in range(round(seconds * 60)): + commands = _tick(tactic, t0 + k / 60) + return commands + + +def test_moves_play_in_order_on_the_first_robot_and_the_others_stand_still(): + tactic = ScriptedMovesTactic(SCRIPT) + first = _run(tactic, 0.0, 0.25) + assert first[4].local_forward_vel == 0.3 and first[7].local_forward_vel == 0.0 + assert _run(tactic, 0.25, 0.5)[4].angular_vel == 1.0 + + +def test_a_pause_restarts_the_interrupted_move_from_its_start(): + tactic = ScriptedMovesTactic(SCRIPT) + _run(tactic, 0.0, 0.4) # 0.4 s into the 0.5 s "go" + # HALT for 3 s: no ticks. Resumed, "go" runs a full 0.5 s again, not the 0.1 s left. + assert _run(tactic, 3.4, 0.45)[4].local_forward_vel == 0.3 + assert _run(tactic, 3.85, 0.1)[4].angular_vel == 1.0 + + +def test_once_done_the_robot_stands_still(): + tactic = ScriptedMovesTactic(SCRIPT) + last = _run(tactic, 0.0, 1.2) + assert tactic.done and last[4] == last[7] and last[4].local_forward_vel == 0.0 and last[4].angular_vel == 0.0 diff --git a/utama_core/tests/replay/test_fingerprint.py b/utama_core/tests/replay/test_fingerprint.py index 1dfb3d503..56c6ae925 100644 --- a/utama_core/tests/replay/test_fingerprint.py +++ b/utama_core/tests/replay/test_fingerprint.py @@ -381,7 +381,8 @@ def test_the_base_holds_the_sim_script_and_referee_profiles(graph): # environment: settings.py reads UTAMA_EXACT_MATH -> ENV_PREFIXES; robosim_wrapper copies # the environment into the sim subprocess -> ENV_PREFIXES covers the numerics ones. # opens files: profile_loader reads profiles/*.yaml -> data_files. The rest write a -# match's outputs (match log, stats, replays) or read old ones for analysis, never during play. +# match's outputs (match log, stats, replays, a camera run's game log) or read old ones for +# analysis, never during play. # subprocess: robosim_wrapper starts robosim_subprocess.py in the robosim pixi env -> # SUBPROCESS_SCRIPTS, and environment_pin's robosim env listing and binary hashes. # getattr on a module by computed name: tools.evaluation.match looks a factory up by name -> @@ -409,6 +410,7 @@ def test_the_base_holds_the_sim_script_and_referee_profiles(graph): "utama_core.engine.match_stats", "utama_core.replay.columnar_reader", "utama_core.replay.columnar_writer", + "utama_core.replay.game_log_writer", "utama_core.replay.replay_writer", ], "subprocess": ["utama_core.rsoccer_simulator.src.Simulators.robosim.robosim_wrapper"], diff --git a/utama_core/tests/strategy_runner/test_game_log_sides.py b/utama_core/tests/strategy_runner/test_game_log_sides.py new file mode 100644 index 000000000..b856688c9 --- /dev/null +++ b/utama_core/tests/strategy_runner/test_game_log_sides.py @@ -0,0 +1,41 @@ +"""The game log's referee records carry the sides: the converter reads them, our referee leaves them out.""" + +from __future__ import annotations + +import dataclasses +from types import SimpleNamespace + +import pytest + +from utama_core.entities.data.referee import RefereeData +from utama_core.entities.game.team_info import TeamInfo +from utama_core.entities.referee.referee_command import RefereeCommand +from utama_core.entities.referee.stage import Stage +from utama_core.run.strategy_runner import StrategyRunner + +DATA = RefereeData( + source_identifier=None, + time_sent=0.0, + time_received=0.0, + referee_command=RefereeCommand.HALT, + referee_command_timestamp=0.0, + stage=Stage.NORMAL_FIRST_HALF, + stage_time_left=0.0, + blue_team=TeamInfo("blue"), + yellow_team=TeamInfo("yellow"), +) + + +@pytest.mark.parametrize( + "yellow, right, blue_on_positive", + [(True, True, False), (True, False, True), (False, True, True), (False, False, False)], +) +def test_sides_missing_from_the_referee_are_ours(yellow, right, blue_on_positive): + runner = SimpleNamespace(my_team_is_yellow=yellow, my_team_is_right=right) + assert StrategyRunner._with_sides(runner, DATA).blue_team_on_positive_half is blue_on_positive + + +def test_sides_the_referee_sent_are_kept(): + runner = SimpleNamespace(my_team_is_yellow=True, my_team_is_right=True) + sent = dataclasses.replace(DATA, blue_team_on_positive_half=True) + assert StrategyRunner._with_sides(runner, sent).blue_team_on_positive_half is True diff --git a/utama_core/tests/strategy_runner/test_runner_misconfig.py b/utama_core/tests/strategy_runner/test_runner_misconfig.py index a963838f7..679784c9b 100644 --- a/utama_core/tests/strategy_runner/test_runner_misconfig.py +++ b/utama_core/tests/strategy_runner/test_runner_misconfig.py @@ -80,7 +80,7 @@ def test_setup_vision_and_referee_starts_vision_only_when_referee_none(monkeypat started = [] class DummyVisionReceiver: - def __init__(self, buffers, on_geometry=None): + def __init__(self, buffers, on_geometry=None, on_packet=None): pass class DummyRefereeReceiver: @@ -113,7 +113,7 @@ def test_setup_vision_and_referee_starts_vision_only_when_referee_custom(monkeyp started = [] class DummyVisionReceiver: - def __init__(self, buffers, on_geometry=None): + def __init__(self, buffers, on_geometry=None, on_packet=None): pass monkeypatch.setattr(runner_mod, "VisionReceiver", DummyVisionReceiver) @@ -140,7 +140,7 @@ def test_setup_vision_and_referee_starts_both_receivers_when_referee_official( started = [] class DummyVisionReceiver: - def __init__(self, buffers, on_geometry=None): + def __init__(self, buffers, on_geometry=None, on_packet=None): pass class DummyRefereeReceiver: From 1fbce32303960452cf4ac3df1f7eb6c133bfc6b6 Mon Sep 17 00:00:00 2001 From: isaac0804 Date: Wed, 21 Oct 2026 20:16:00 +0100 Subject: [PATCH 11/56] feat(dashboard): match timeline in the Replay and Live tabs; 15x smaller replay payload timeline.js draws tools/match_timeline.py's swimlanes in the browser: the referee command band, one lane per friendly robot coloured by its tactic with a strip where it had the ball, each robot's distance to the ball, and the event markers. In the Replay tab it follows the cursor and seeks on click or drag (30 s / 60 s / 3 min / whole match); its tactic spans match match_timeline.tactic_changes exactly on a tournament match. In the Live tab each streamed state extends the lanes, stamped with the frame's game time (now sent with the state), keeping the last 5 minutes. A 13-minute replay's frames were 108 MB of JSON (full 17-digit floats) against a 16 MB .npz. Positions now go to the millimetre, velocities to the centimetre, and responses over 64 KB are gzipped: 7.4 MB. Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_01TJGHdb8Dht67hn4Lohndf5 --- docs/STRATEGY_DEVELOPMENT.md | 2 + docs/roadmap.md | 12 +- utama_core/dashboard/page.py | 1 + utama_core/dashboard/server.py | 8 + utama_core/dashboard/static/app.css | 6 + utama_core/dashboard/static/index.html | 27 +- utama_core/dashboard/static/live.js | 21 + utama_core/dashboard/static/replay.js | 38 +- utama_core/dashboard/static/timeline.js | 418 ++++++++++++++++++ utama_core/dashboard/views/referee.py | 24 +- utama_core/dashboard/views/replay.py | 2 +- .../test_referee_view_serialization.py | 28 ++ .../tests/dashboard/test_server_gzip.py | 41 ++ 13 files changed, 603 insertions(+), 25 deletions(-) create mode 100644 utama_core/dashboard/static/timeline.js create mode 100644 utama_core/tests/dashboard/test_server_gzip.py diff --git a/docs/STRATEGY_DEVELOPMENT.md b/docs/STRATEGY_DEVELOPMENT.md index 107c9a5e1..ae9c2f1bc 100644 --- a/docs/STRATEGY_DEVELOPMENT.md +++ b/docs/STRATEGY_DEVELOPMENT.md @@ -377,6 +377,8 @@ whoever runs it. dark strip where it had the ball), each robot's distance to the ball, and goals, fouls and shots on top. Use it for thrashing (a robot switching tactics every few ticks), a tactic stuck committed, or everyone chasing the ball. Friendly side only has tactic lanes (the intentions log is config_a's). + The dashboard draws the same lanes under the Replay tab's controls (click to seek) and live in + the Live tab. Thrashing in `split_shape` against `tiki_taka_plus`: robots 2-4 flip between the attack and defence slots about every 1.5 s, in pairs of rows at the same tick. It is not split_shape's alone: most strategies that split attack and defence on possession take a tactic for under 1 s and revert about diff --git a/docs/roadmap.md b/docs/roadmap.md index 8624b873f..e51d8be13 100644 --- a/docs/roadmap.md +++ b/docs/roadmap.md @@ -86,12 +86,12 @@ entry by its title, not its number. runner or can run faster than real time headless. CI hardcodes `--ignore-glob "**/*grsim*"` (`ci.yml:41,43`). -7. **Match timeline in the dashboard.** `tools/match_timeline.py` draws one window as swimlanes - (referee band, each robot's tactic, possession, distance to the ball, events). The Replay tab - has only a per-tick marker strip (goals, fouls, stalls, ball losses); the swimlane panel that - follows the replay cursor, with clicking a lane to seek, is what is missing. The data is all - in the replay and its `intentions.jsonl` already (`load_window` computes it without drawing). - Not started (user idea, 2026-10-17). +7. **Match timeline in the dashboard.** Built 2026-10-21 (`dashboard/static/timeline.js`): the + swimlanes of `tools/match_timeline.py` (referee band, each robot's tactic, possession, distance + to the ball, events) under the Replay tab's controls, following the cursor, click or drag to + seek; and in the Live tab, the last minutes up to now. Not yet: switch ticks and shots (the + tool has them), enemy lanes. A replay's frames still take ~10 s to serve, nearly all of it the + ball-loss markers (`turnover_breakdown.analyse_match` re-reads the replay frame by frame). 8. **More tactics from real football vocabulary** (formations, set plays, pressing schemes) rather than variations on existing ones. No new `Tactic` class has been added since this diff --git a/utama_core/dashboard/page.py b/utama_core/dashboard/page.py index 862c1934d..7e4da9402 100644 --- a/utama_core/dashboard/page.py +++ b/utama_core/dashboard/page.py @@ -20,6 +20,7 @@ def build_page() -> str: .replace("__STATUS_PANEL_JS__", (_STATIC_DIR / "status_panel.js").read_text()) .replace("__APP_JS__", (_STATIC_DIR / "app.js").read_text()) .replace("__LIVE_JS__", (_STATIC_DIR / "live.js").read_text()) + .replace("__TIMELINE_JS__", (_STATIC_DIR / "timeline.js").read_text()) .replace("__REPLAY_JS__", (_STATIC_DIR / "replay.js").read_text()) .replace("__TOURNAMENT_JS__", (_STATIC_DIR / "tournament.js").read_text()) ) diff --git a/utama_core/dashboard/server.py b/utama_core/dashboard/server.py index cad64c50f..dd8e40ead 100644 --- a/utama_core/dashboard/server.py +++ b/utama_core/dashboard/server.py @@ -15,6 +15,7 @@ from __future__ import annotations +import gzip import json import threading from http.server import BaseHTTPRequestHandler, ThreadingHTTPServer @@ -24,6 +25,9 @@ CommandHandler = Callable[[dict], None] +_GZIP_MIN_BYTES = 64 * 1024 + + def attach_dashboard(port: int = 8080) -> "DashboardServer": """Start the dashboard's HTTP server in a background daemon thread. @@ -137,6 +141,10 @@ def log_message(self, fmt, *args): def _send_json_bytes(self, body: bytes, status: int = 200) -> None: self.send_response(status) self.send_header("Content-Type", "application/json") + # A replay's frames are tens of MB of JSON; gzip makes them about a tenth. + if len(body) > _GZIP_MIN_BYTES and "gzip" in self.headers.get("Accept-Encoding", ""): + body = gzip.compress(body, compresslevel=5) + self.send_header("Content-Encoding", "gzip") self.send_header("Content-Length", str(len(body))) self.end_headers() self.wfile.write(body) diff --git a/utama_core/dashboard/static/app.css b/utama_core/dashboard/static/app.css index 9cd11b661..13009dd05 100644 --- a/utama_core/dashboard/static/app.css +++ b/utama_core/dashboard/static/app.css @@ -186,6 +186,12 @@ td.num { text-align: right; font-variant-numeric: tabular-nums; } .check-fail { color: #e06a60; font-weight: 600; } .auto-halt { background: var(--halt); color: #fff; padding: 6px 8px; border-radius: var(--radius); font-size: .75rem; } +/* Replay timeline (timeline.js): swimlanes under the transport controls. */ +.timeline-wrap { width: 100%; cursor: pointer; } +.timeline-wrap canvas { display: block; } +.timeline-legend { display: flex; flex-wrap: wrap; gap: 4px 10px; font-size: .65rem; color: var(--muted); } +.timeline-legend i { display: inline-block; width: 9px; height: 9px; margin-right: 3px; border-radius: 2px; vertical-align: -1px; } + /* Replay timeline markers: one tick per goal / foul / stall / ball loss. */ .marker-strip { position: relative; height: 10px; margin: 0 7px; } .marker { position: absolute; top: 0; width: 3px; height: 10px; margin-left: -1px; border-radius: 1px; cursor: pointer; } diff --git a/utama_core/dashboard/static/index.html b/utama_core/dashboard/static/index.html index 152847e62..6816c7af5 100644 --- a/utama_core/dashboard/static/index.html +++ b/utama_core/dashboard/static/index.html @@ -15,9 +15,21 @@
-
+
Field
-
+
+
+
+
+
+ +
+
@@ -84,6 +96,16 @@
+
+
+
+ +
@@ -112,6 +134,7 @@ + diff --git a/utama_core/dashboard/static/live.js b/utama_core/dashboard/static/live.js index 52d990694..349652677 100644 --- a/utama_core/dashboard/static/live.js +++ b/utama_core/dashboard/static/live.js @@ -10,6 +10,8 @@ let fieldView = null; let cfg = null; let currentCmd = null; + let timeline = null; // Timeline (timeline.js), the last minutes up to now + let timelineQueued = false; function send(command, designated) { const body = designated ? { command, designated } : { command }; @@ -53,6 +55,16 @@ renderTacticStatusInto("ref-tactic-entries", d.tactic_status); d.tactic_tags = tacticTagsFromStatus(d.tactic_status); if (fieldView) fieldView.draw(d); + if (timeline && d.ts != null) { + timeline.append(d.ts, d); + if (!timelineQueued) { + timelineQueued = true; // one draw per screen frame, however fast states arrive + requestAnimationFrame(() => { + timelineQueued = false; + timeline.draw(timeline.ts); + }); + } + } } // The runner's field checks (utama_core/run/field_check.py): only the problems are listed, @@ -110,6 +122,15 @@ Dashboard.showView("live"); // a running match is what this page is open for }); + timeline = new Timeline( + document.getElementById("live-timeline"), + document.getElementById("live-timeline-legend"), + null + ); + document.getElementById("live-timeline-window").addEventListener("change", (e) => { + timeline.setWindow(Number(e.target.value)); + }); + const es = new EventSource("/events/referee"); es.addEventListener("referee", (ev) => onEvent(JSON.parse(ev.data))); es.onopen = () => document.getElementById("ref-conn-dot").classList.add("live"); diff --git a/utama_core/dashboard/static/replay.js b/utama_core/dashboard/static/replay.js index 46a5d6451..93d922e40 100644 --- a/utama_core/dashboard/static/replay.js +++ b/utama_core/dashboard/static/replay.js @@ -20,6 +20,7 @@ (function () { let fieldView = null; + let timeline = null; // Timeline (timeline.js) let frames = []; let index = 0; let playing = false; @@ -186,18 +187,21 @@ if (i >= 0) jumpToMarker(i); } - function jumpToEvent(sim_time) { - // Nearest frame whose ts >= sim_time (falls back to the last frame if - // the event is at/after the final recorded ts). - let target = frames.length - 1; - for (let i = 0; i < frames.length; i++) { - if (frames[i].ts >= sim_time) { - target = i; - break; - } + function firstFrameAtOrAfter(sim_time) { + // the last frame if sim_time is at/after the final recorded ts + let lo = 0; + let hi = frames.length - 1; + while (lo < hi) { + const mid = (lo + hi) >> 1; + if (frames[mid].ts < sim_time) lo = mid + 1; + else hi = mid; } + return lo; + } + + function jumpToEvent(sim_time) { stopPlayback(); - setIndex(target); + setIndex(firstFrameAtOrAfter(sim_time)); } function setIndex(i) { @@ -219,6 +223,7 @@ fieldView.setTeamLayout(frame.my_team_is_right, fieldView.myTeamIsYellow); } if (fieldView) fieldView.draw(frame); + if (timeline) timeline.draw(frame.ts); renderRobotStatusInto("replay-status-entries", frame, { hideFeedback: true }); renderTacticStatusInto("replay-tactic-entries", tacticStatus, { emptyLabel: "no tactic data at this frame", @@ -338,6 +343,7 @@ lastIndex = 0; resetEventCursors(); setIndex(0); + if (timeline) timeline.load([], [], [], [], []); setLoading(true); fetch("/replay/frames?path=" + encodeURIComponent(path)) .then((r) => r.json()) @@ -366,6 +372,7 @@ if (refBanner) refBanner.style.display = data.has_referee_data ? "none" : ""; renderIntentionLog(); renderMarkers(); + timeline.load(frames, tacticEvents, refereeEvents, traceEvents, markers); setIndex(0); if (atTime != null) jumpToEvent(atTime - MARKER_LEAD_S); }) @@ -401,6 +408,17 @@ stopPlayback(); setIndex(Number(e.target.value)); }); + timeline = new Timeline( + document.getElementById("replay-timeline"), + document.getElementById("replay-timeline-legend"), + (ts) => { + stopPlayback(); + setIndex(firstFrameAtOrAfter(ts)); + } + ); + document.getElementById("replay-timeline-window").addEventListener("change", (e) => { + timeline.setWindow(Number(e.target.value)); + }); document.getElementById("replay-speed").addEventListener("change", (e) => { speed = Number(e.target.value); }); diff --git a/utama_core/dashboard/static/timeline.js b/utama_core/dashboard/static/timeline.js new file mode 100644 index 000000000..d7f9c0dcb --- /dev/null +++ b/utama_core/dashboard/static/timeline.js @@ -0,0 +1,418 @@ +// Match timeline: a window of the match as swimlanes, the dashboard's version of +// tools/match_timeline.py's figure, in the Replay tab around the viewed frame and in the +// Live tab up to now. Top to bottom, on one time axis: +// - the referee command band (live play, STOP, kickoff, free kick, penalty, placement, HALT); +// - one lane per friendly robot: a span per tactic it was in (the Tactic class from the +// intention log), and a light strip along the bottom where it had the ball; +// - each friendly robot's distance to the ball, 0-3 m; +// - events: goals, fouls, stalls, ball losses (the replay's markers). +// A cursor marks the viewed frame; in a replay, clicking or dragging seeks. A replay is computed +// once from data replay.js already holds (`load`); live, each streamed state extends the lanes +// (`append`) and the last LIVE_KEEP_S are kept. Drawing only clips and paints. + +(function () { + const SLOTS = ["#2a78d6", "#eb6834", "#1baf7a", "#eda100", "#e87ba4", "#008300", "#6250d6"]; + const OTHER = "#5f636c"; // tactics beyond the seven slots + const REF_FILL = { + "live play": "#2b3a30", + STOP: "#5b5f68", + HALT: "#b8615a", + kickoff: "#3d5a80", + "free kick": "#7a5236", + penalty: "#6d4560", + placement: "#6e6436", + other: "#33373f", + }; + const MARKER_FILL = { goal: "#e0c14f", foul: "#d08a4a", stall: "#b8615a", loss: "#8b909b" }; + const KEEPER = "goalkeeper"; // the label the Live view's tactic status gives the keeper + const LIVE_KEEP_S = 300; + const MAX_DIST_M = 3; + + const GUTTER = 44; // left labels + const REF_H = 16; + const LANE_H = 18; + const DIST_H = 46; + const EVENT_H = 12; + const AXIS_H = 14; + const GAP = 4; + + // The legend group of a referee command name, as match_timeline.ref_group. + function refGroup(command) { + if (!command) return "other"; + if (command === "NORMAL_START" || command === "FORCE_START") return "live play"; + if (command === "HALT") return "HALT"; + if (command === "STOP" || command.startsWith("GOAL_") || command.startsWith("TIMEOUT_")) return "STOP"; + if (command.startsWith("PREPARE_KICKOFF")) return "kickoff"; + if (command.startsWith("DIRECT_FREE") || command.startsWith("INDIRECT_FREE")) return "free kick"; + if (command.startsWith("PREPARE_PENALTY")) return "penalty"; + if (command.startsWith("BALL_PLACEMENT")) return "placement"; + return "other"; + } + + // Spans [{t0, t1, label}] tiling [start, end] from change points [[t, label]], merging repeats. + function spansFrom(changes, start, end) { + const out = []; + for (let i = 0; i < changes.length; i++) { + const t0 = Math.max(changes[i][0], start); + const t1 = Math.min(i + 1 < changes.length ? changes[i + 1][0] : end, end); + const label = changes[i][1]; + if (t1 <= t0) continue; + const last = out[out.length - 1]; + if (last && last.label === label) last.t1 = t1; + else out.push({ t0, t1, label }); + } + return out; + } + + // For each robot, the tactic it was in over time, as match_timeline.tactic_changes: a robot's + // tactic is the slot whose latest row lists it, named by the row's note when that is a class + // name; a "referee reset" row clears every slot. + function tacticChanges(events, robotIds) { + const members = new Map(); // slot -> Set(robot ids); insertion order = last update last + const names = {}; + const changes = {}; + for (const id of robotIds) changes[id] = []; + for (const e of events) { + if (e.tactic_id === "referee reset") members.clear(); + else { + if (e.note && /^[A-Za-z_][A-Za-z0-9_]*$/.test(e.note)) names[e.tactic_id] = e.note; + if (!(e.tactic_id in names)) names[e.tactic_id] = e.tactic_id; + members.delete(e.tactic_id); + members.set(e.tactic_id, new Set(e.robot_ids)); + } + const slots = Array.from(members.keys()).reverse(); + for (const id of robotIds) { + const slot = slots.find((s) => members.get(s).has(id)); + const label = slot === undefined ? null : names[slot]; + const points = changes[id]; + const last = points[points.length - 1]; + if (!last && label === null) continue; + if (last && last[1] === label) continue; + if (last && last[0] === e.sim_time) last[1] = label; + else points.push([e.sim_time, label]); + } + } + return changes; + } + + // Runs of `flag(i)` over the frames, as spans. + function runs(frames, flag) { + const out = []; + let start = null; + for (let i = 0; i < frames.length; i++) { + const on = flag(i); + if (on && start === null) start = frames[i].ts; + else if (!on && start !== null) { + out.push({ t0: start, t1: frames[i].ts }); + start = null; + } + } + if (start !== null) out.push({ t0: start, t1: frames[frames.length - 1].ts }); + return out; + } + + function firstAtOrAfter(frames, ts) { + let lo = 0; + let hi = frames.length; + while (lo < hi) { + const mid = (lo + hi) >> 1; + if (frames[mid].ts < ts) lo = mid + 1; + else hi = mid; + } + return lo; + } + + // A live tactic status entry ["[ (committed)]", ""] as a lane label. + function liveLabel(status) { + return status ? status[0].replace(/ \(committed\)$/, "") : null; + } + + class Timeline { + constructor(canvas, legend, onSeek) { + this.canvas = canvas; + this.legend = legend; + this.onSeek = onSeek; + this.windowS = 60; + this.data = null; + this.ts = 0; + this._view = null; // [t0, t1] last drawn + let dragging = false; + const seek = (e) => { + if (!this._view || !this.onSeek) return; + const rect = canvas.getBoundingClientRect(); + const x = e.clientX - rect.left - GUTTER; + const w = rect.width - GUTTER; + if (x < 0 || w <= 0) return; + this.onSeek(this._view[0] + (x / w) * (this._view[1] - this._view[0])); + }; + canvas.addEventListener("mousedown", (e) => { + dragging = true; + seek(e); + }); + window.addEventListener("mousemove", (e) => dragging && seek(e)); + window.addEventListener("mouseup", () => (dragging = false)); + new ResizeObserver(() => this.draw(this.ts)).observe(canvas.parentElement); + } + + setWindow(seconds) { + this.windowS = seconds; + this.draw(this.ts); + } + + load(frames, tacticEvents, refereeEvents, traceEvents, markers) { + if (!frames.length) { + this.data = null; + this.legend.innerHTML = ""; + this.draw(0); + return; + } + const start = frames[0].ts; + const end = frames[frames.length - 1].ts; + const ids = new Set(); + for (const f of frames) for (const r of f.robots.friendly) ids.add(r.id); + const robotIds = Array.from(ids).sort((a, b) => a - b); + + const keepers = new Set(); + for (const e of traceEvents) { + if (e.key === "highlights.goalkeeper" && e.value) for (const k in e.value) keepers.add(Number(k)); + } + const changes = tacticChanges(tacticEvents, robotIds); + const lanes = robotIds.map((id) => { + const points = [[start, keepers.has(id) ? KEEPER : null]].concat(changes[id]); + const tactics = spansFrom(points, start, end).map((s) => + s.label === null && keepers.has(id) ? { ...s, label: KEEPER } : s + ); + const has = (i) => frames[i].robots.friendly.some((r) => r.id === id && r.has_ball); + return { id, tactics, ball: runs(frames, has) }; + }); + + const total = {}; + for (const lane of lanes) { + for (const s of lane.tactics) { + if (s.label && s.label !== KEEPER) total[s.label] = (total[s.label] || 0) + s.t1 - s.t0; + } + } + const ranked = Object.keys(total).sort((a, b) => total[b] - total[a] || a.localeCompare(b)); + const colours = {}; + ranked.forEach((name, i) => (colours[name] = i < SLOTS.length ? SLOTS[i] : OTHER)); + + const refPoints = refereeEvents.map((e) => [e.sim_time, refGroup(e.command)]); + const referee = refPoints.length ? spansFrom(refPoints, start, end) : []; + + // distance to the ball per robot per frame, NaN when either is missing + const dist = {}; + for (const id of robotIds) dist[id] = new Float32Array(frames.length); + frames.forEach((f, i) => { + for (const id of robotIds) { + const r = f.robots.friendly.find((q) => q.id === id); + dist[id][i] = r && f.ball ? Math.hypot(r.x - f.ball.x, r.y - f.ball.y) : NaN; + } + }); + + this.data = { frames, origin: start, start, end, lanes, colours, referee, dist, markers, robotColour: {} }; + robotIds.forEach((id, i) => (this.data.robotColour[id] = SLOTS[i % SLOTS.length])); + this.legend.innerHTML = ranked + .map((name) => `${name.replace(/Tactic$/, "")}`) + .join(""); + this.draw(start); + } + + // Live: one state from the Live view's stream (robots, ball, command, scores, + // tactic_status), at `ts` seconds. Spans are extended in place; the caller draws. + append(ts, state) { + let d = this.data; + // a new match (the clock jumped) starts over; a state no newer than the last is skipped + const fresh = !d || !d.live || ts < d.end - 5 || ts > d.end + 60; + if (!fresh && ts <= d.end) return; + if (fresh) { + d = this.data = { live: true, frames: [], origin: ts, start: ts, end: ts, lanes: [], colours: {}, referee: [] }; + Object.assign(d, { dist: {}, markers: [], robotColour: {}, score: null }); + this.legend.innerHTML = ""; + } + const extend = (spans, label) => { + const last = spans[spans.length - 1]; + if (last) last.t1 = ts; + if (!last || last.label !== label) spans.push({ t0: ts, t1: ts, label }); + }; + const friendly = (state.robots && state.robots.friendly) || []; + for (const r of friendly) { + if (d.dist[r.id]) continue; + d.dist[r.id] = d.frames.map(() => NaN); + d.robotColour[r.id] = SLOTS[Object.keys(d.robotColour).length % SLOTS.length]; + d.lanes.push({ id: r.id, tactics: [], ball: [] }); + d.lanes.sort((a, b) => a.id - b.id); + } + d.frames.push({ ts }); + d.end = ts; + extend(d.referee, refGroup(state.command)); + const status = state.tactic_status || {}; + for (const lane of d.lanes) { + const r = friendly.find((q) => q.id === lane.id); + const label = r ? liveLabel(status[String(lane.id)]) : null; + if (label && label !== KEEPER && !(label in d.colours)) { + const n = Object.keys(d.colours).length; + d.colours[label] = n < SLOTS.length ? SLOTS[n] : OTHER; + this.legend.innerHTML += `${label}`; + } + extend(lane.tactics, label); + extend(lane.ball, Boolean(r && r.has_ball)); + d.dist[lane.id].push(r && state.ball ? Math.hypot(r.x - state.ball.x, r.y - state.ball.y) : NaN); + } + const score = [state.yellow_score, state.blue_score]; + if (d.score && (score[0] > d.score[0] || score[1] > d.score[1])) { + const scorer = score[0] > d.score[0] ? "yellow" : "blue"; + d.markers.push({ sim_time: ts, kind: "goal", label: `Goal ${scorer}, ${score[0]}-${score[1]}` }); + } + d.score = score; + + if (ts - d.start > LIVE_KEEP_S + 30) { + const cut = ts - LIVE_KEEP_S; + const k = firstAtOrAfter(d.frames, cut); + d.frames.splice(0, k); + for (const id in d.dist) d.dist[id].splice(0, k); + const keep = (spans) => spans.filter((s) => s.t1 >= cut); + d.referee = keep(d.referee); + d.markers = d.markers.filter((m) => m.sim_time >= cut); + for (const lane of d.lanes) { + lane.tactics = keep(lane.tactics); + lane.ball = keep(lane.ball); + } + d.start = d.frames[0].ts; + } + this.ts = ts; + } + + _height() { + const lanes = this.data ? this.data.lanes.length : 0; + return REF_H + GAP + lanes * LANE_H + GAP + DIST_H + GAP + EVENT_H + AXIS_H; + } + + draw(ts) { + this.ts = ts; + const canvas = this.canvas; + const dpr = window.devicePixelRatio || 1; + const cssW = canvas.parentElement.clientWidth; + const cssH = this._height(); + if (canvas.width !== Math.round(cssW * dpr) || canvas.height !== Math.round(cssH * dpr)) { + canvas.width = Math.round(cssW * dpr); + canvas.height = Math.round(cssH * dpr); + canvas.style.width = cssW + "px"; + canvas.style.height = cssH + "px"; + } + const ctx = canvas.getContext("2d"); + ctx.setTransform(dpr, 0, 0, dpr, 0, 0); + ctx.clearRect(0, 0, cssW, cssH); + const d = this.data; + if (!d) return; + + const span = Math.min(this.windowS || d.end - d.start, d.end - d.start) || 1; + let t0 = ts - span / 3; + t0 = Math.max(d.start, Math.min(t0, d.end - span)); + const t1 = t0 + span; + this._view = [t0, t1]; + const w = cssW - GUTTER; + const x = (t) => GUTTER + ((t - t0) / span) * w; + ctx.font = "10px ui-monospace, monospace"; + ctx.textBaseline = "middle"; + + const band = (spans, y, h, fill, label) => { + for (const s of spans) { + if (s.t1 < t0 || s.t0 > t1 || s.label === null || s.label === false) continue; + const a = Math.max(x(s.t0), GUTTER); + const b = Math.min(x(s.t1), cssW); + ctx.fillStyle = fill(s); + ctx.fillRect(a, y, Math.max(1, b - a - 1), h); + const text = label && label(s); + if (text && ctx.measureText(text).width + 6 < b - a) { + ctx.fillStyle = "#dfe2e8"; + ctx.fillText(text, a + 3, y + h / 2); + } + } + }; + const rowLabel = (text, y, h) => { + ctx.fillStyle = "#8b909b"; + ctx.fillText(text, 2, y + h / 2); + }; + + let y = 0; + rowLabel("ref", y, REF_H); + band(d.referee, y, REF_H, (s) => REF_FILL[s.label] || REF_FILL.other, (s) => s.label); + y += REF_H + GAP; + + for (const lane of d.lanes) { + rowLabel("#" + lane.id, y, LANE_H); + band( + lane.tactics, + y, + LANE_H - 2, + (s) => (s.label === KEEPER ? OTHER : d.colours[s.label] || OTHER), + (s) => s.label.replace(/Tactic$/, "") + ); + band(lane.ball, y + LANE_H - 5, 3, () => "#f2f2f2"); + y += LANE_H; + } + y += GAP; + + rowLabel("ball m", y, DIST_H); + ctx.strokeStyle = "#33373f"; + ctx.beginPath(); + for (const m of [1, 2]) { + const gy = y + DIST_H * (1 - m / MAX_DIST_M); + ctx.moveTo(GUTTER, gy); + ctx.lineTo(cssW, gy); + } + ctx.stroke(); + const i0 = Math.max(0, firstAtOrAfter(d.frames, t0) - 1); + const i1 = Math.min(d.frames.length, firstAtOrAfter(d.frames, t1) + 1); + const stride = Math.max(1, Math.floor((i1 - i0) / w)); + for (const lane of d.lanes) { + const series = d.dist[lane.id]; + ctx.strokeStyle = d.robotColour[lane.id]; + ctx.beginPath(); + let pen = false; + for (let i = i0; i < i1; i += stride) { + const v = series[i]; + if (Number.isNaN(v)) { + pen = false; + continue; + } + const px = x(d.frames[i].ts); + const py = y + DIST_H * (1 - Math.min(v, MAX_DIST_M) / MAX_DIST_M); + if (pen) ctx.lineTo(px, py); + else ctx.moveTo(px, py); + pen = true; + } + ctx.stroke(); + } + y += DIST_H + GAP; + + rowLabel("events", y, EVENT_H); + for (const m of d.markers) { + if (m.sim_time < t0 || m.sim_time > t1) continue; + ctx.fillStyle = MARKER_FILL[m.kind] || "#8b909b"; + ctx.fillRect(x(m.sim_time) - 1.5, y, 3, EVENT_H); + } + y += EVENT_H; + + // axis: a tick every 5 s or 10 s or 30 s, labelled as match time (m:ss) + const step = span > 180 ? 30 : span > 60 ? 10 : 5; + ctx.fillStyle = "#8b909b"; + ctx.textAlign = "center"; + for (let s = Math.ceil((t0 - d.origin) / step) * step; d.origin + s <= t1; s += step) { + const px = x(d.origin + s); + ctx.fillRect(px, y, 1, 3); + if (px > GUTTER + 12 && px < cssW - 12) { + ctx.fillText(Math.floor(s / 60) + ":" + String(Math.floor(s % 60)).padStart(2, "0"), px, y + 9); + } + } + ctx.textAlign = "start"; + + const cx = x(ts); + ctx.fillStyle = "#dfe2e8"; + ctx.fillRect(cx - 0.5, 0, 1.5, y); + } + } + + if (typeof window !== "undefined") window.Timeline = Timeline; + if (typeof module !== "undefined") module.exports = { refGroup, spansFrom, tacticChanges }; +})(); diff --git a/utama_core/dashboard/views/referee.py b/utama_core/dashboard/views/referee.py index fec6d03cf..180cda7d5 100644 --- a/utama_core/dashboard/views/referee.py +++ b/utama_core/dashboard/views/referee.py @@ -68,6 +68,16 @@ def notify( server.notify(CHANNEL, _serialise_state(ref_data, game_frame, tactic_status, robot_feedback_data, field_check)) +# Millimetres, mrad and cm/s: finer than anything drawn, and a full float's 17 digits made a +# match's replay ~7x its size on disk. +def _mm(value: float) -> float: + return round(value, 3) + + +def _cm(value: float) -> float: + return round(value, 2) + + def _serialise_robots(game_frame) -> dict: if game_frame is None: return {"friendly": [], "enemy": []} @@ -76,11 +86,11 @@ def _robot_list(robots_dict): return [ { "id": r.id, - "x": r.p.x, - "y": r.p.y, - "vx": r.v.x, - "vy": r.v.y, - "orientation": r.orientation, + "x": _mm(r.p.x), + "y": _mm(r.p.y), + "vx": _cm(r.v.x), + "vy": _cm(r.v.y), + "orientation": _mm(r.orientation), "has_ball": r.has_ball, } for r in robots_dict.values() @@ -95,7 +105,8 @@ def _robot_list(robots_dict): def _serialise_ball(game_frame): if game_frame is None or game_frame.ball is None: return None - return {"x": game_frame.ball.p.x, "y": game_frame.ball.p.y, "vx": game_frame.ball.v.x, "vy": game_frame.ball.v.y} + ball = game_frame.ball + return {"x": _mm(ball.p.x), "y": _mm(ball.p.y), "vx": _cm(ball.v.x), "vy": _cm(ball.v.y)} def _serialise_state(ref_data, game_frame=None, tactic_status=None, robot_feedback_data=None, field_check=None) -> dict: @@ -115,6 +126,7 @@ def _serialise_state(ref_data, game_frame=None, tactic_status=None, robot_feedba "blue_score": ref_data.blue_team.score, "designated": designated, "status_message": ref_data.status_message, + "ts": getattr(game_frame, "ts", None), # game time, for the Live timeline "my_team_is_right": getattr(game_frame, "my_team_is_right", False), "my_team_is_yellow": getattr(game_frame, "my_team_is_yellow", True), "robots": _serialise_robots(game_frame), diff --git a/utama_core/dashboard/views/replay.py b/utama_core/dashboard/views/replay.py index 99d68faec..0a10635a4 100644 --- a/utama_core/dashboard/views/replay.py +++ b/utama_core/dashboard/views/replay.py @@ -116,7 +116,7 @@ def _frames_bytes(query: Optional[dict] = None) -> bytes: frames.append( { - "ts": obj.ts, + "ts": round(obj.ts, 4), "my_team_is_right": bool(obj.my_team_is_right), # the teams change ends at half-time "robots": _serialise_robots(obj), "ball": _serialise_ball(obj), diff --git a/utama_core/tests/dashboard/test_referee_view_serialization.py b/utama_core/tests/dashboard/test_referee_view_serialization.py index 36c813b88..7154388fb 100644 --- a/utama_core/tests/dashboard/test_referee_view_serialization.py +++ b/utama_core/tests/dashboard/test_referee_view_serialization.py @@ -45,3 +45,31 @@ def test_serialise_state_includes_robot_feedback_rows(): payload = _serialise_state(_referee_data(), robot_feedback_data=robot_feedback) assert payload["robot_feedback"] == robot_feedback + + +def test_positions_are_sent_to_the_millimetre_and_velocities_to_the_centimetre(): + from utama_core.dashboard.views.referee import _serialise_ball, _serialise_robots + from utama_core.entities.data.vector import Vector2D, Vector3D + from utama_core.entities.game import Ball, GameFrame, Robot + + robot = Robot( + id=1, + is_friendly=True, + has_ball=False, + p=Vector2D(1.23456789, -0.0004999), + v=Vector2D(0.123456, 0.0), + a=Vector2D(0, 0), + orientation=3.14159265, + ) + frame = GameFrame( + ts=0.0, + my_team_is_yellow=True, + my_team_is_right=True, + friendly_robots={1: robot}, + enemy_robots={}, + ball=Ball(p=Vector3D(0.98765, 0, 0), v=Vector3D(2.34567, 0, 0), a=Vector3D(0, 0, 0)), + ) + (r,) = _serialise_robots(frame)["friendly"] + assert (r["x"], r["y"], r["vx"], r["orientation"]) == (1.235, -0.0, 0.12, 3.142) + ball = _serialise_ball(frame) + assert (ball["x"], ball["vx"]) == (0.988, 2.35) diff --git a/utama_core/tests/dashboard/test_server_gzip.py b/utama_core/tests/dashboard/test_server_gzip.py new file mode 100644 index 000000000..cd055f91a --- /dev/null +++ b/utama_core/tests/dashboard/test_server_gzip.py @@ -0,0 +1,41 @@ +"""Large JSON responses (a replay's frames) are gzipped when the browser accepts it; small ones are not.""" + +from __future__ import annotations + +import gzip +import socket +import time +import urllib.request + +from utama_core.dashboard.server import _GZIP_MIN_BYTES, DashboardServer + + +def _free_port() -> int: + with socket.socket() as s: + s.bind(("", 0)) + return s.getsockname()[1] + + +def _get(port: int, path: str, accept: str): + request = urllib.request.Request(f"http://localhost:{port}{path}", headers={"Accept-Encoding": accept}) + with urllib.request.urlopen(request, timeout=5) as response: + return response.headers.get("Content-Encoding"), response.read() + + +def test_only_responses_over_the_threshold_are_gzipped_and_only_when_accepted(): + big = b"[" + b"1," * _GZIP_MIN_BYTES + b"1]" + small = b"[1]" + port = _free_port() + server = DashboardServer(port) + server.add_route("/big", lambda: big) + server.add_route("/small", lambda: small) + server.start() + for _ in range(50): + try: + encoding, body = _get(port, "/big", "gzip, deflate") + break + except OSError: # not listening yet + time.sleep(0.05) + assert encoding == "gzip" and gzip.decompress(body) == big + assert _get(port, "/big", "identity") == (None, big) + assert _get(port, "/small", "gzip") == (None, small) From 92ba5f9398a1d0e03a0721cf4906fca6479dad41 Mon Sep 17 00:00:00 2001 From: isaac0804 Date: Wed, 21 Oct 2026 20:45:57 +0100 Subject: [PATCH 12/56] feat(dashboard): Play list of situations; possession band, switch ticks, shots The Replay side panel's raw event and intention logs were hard to read. A Play list replaces them: one row per situation (a restart going live or a side winning the ball, from goal_ledger), with its third, length, shots and how it ended (the stoppage's reason, or who won the ball and how). Click a row to seek; the raw intention log stays, folded. The timeline gains a possession band, tactic switch ticks, shots and hover tooltips. The slow analysis (ball losses, shots) is kept beside the replay in .timeline.json, and npz frames are built straight from the columns, so a replay serves in ~3 s instead of ~16 s. Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_01TJGHdb8Dht67hn4Lohndf5 --- docs/STRATEGY_DEVELOPMENT.md | 5 +- docs/roadmap.md | 10 +- utama_core/dashboard/static/app.css | 13 +- utama_core/dashboard/static/index.html | 12 +- utama_core/dashboard/static/replay.js | 107 +++++++-- utama_core/dashboard/static/timeline.js | 226 +++++++++++++++--- utama_core/dashboard/views/replay.py | 168 +++++++++++-- .../tests/dashboard/test_replay_view.py | 85 ++++++- 8 files changed, 533 insertions(+), 93 deletions(-) diff --git a/docs/STRATEGY_DEVELOPMENT.md b/docs/STRATEGY_DEVELOPMENT.md index ae9c2f1bc..4b820a9c3 100644 --- a/docs/STRATEGY_DEVELOPMENT.md +++ b/docs/STRATEGY_DEVELOPMENT.md @@ -377,8 +377,9 @@ whoever runs it. dark strip where it had the ball), each robot's distance to the ball, and goals, fouls and shots on top. Use it for thrashing (a robot switching tactics every few ticks), a tactic stuck committed, or everyone chasing the ball. Friendly side only has tactic lanes (the intentions log is config_a's). - The dashboard draws the same lanes under the Replay tab's controls (click to seek) and live in - the Live tab. + The dashboard draws the same lanes under the Replay tab's controls (click to seek; hover for what + each mark is) and live in the Live tab. Beside a replay, its Play list reads the match as + situations: who had the ball from which restart or regain, for how long, and how it ended. Thrashing in `split_shape` against `tiki_taka_plus`: robots 2-4 flip between the attack and defence slots about every 1.5 s, in pairs of rows at the same tick. It is not split_shape's alone: most strategies that split attack and defence on possession take a tactic for under 1 s and revert about diff --git a/docs/roadmap.md b/docs/roadmap.md index e51d8be13..8419fed61 100644 --- a/docs/roadmap.md +++ b/docs/roadmap.md @@ -89,9 +89,13 @@ entry by its title, not its number. 7. **Match timeline in the dashboard.** Built 2026-10-21 (`dashboard/static/timeline.js`): the swimlanes of `tools/match_timeline.py` (referee band, each robot's tactic, possession, distance to the ball, events) under the Replay tab's controls, following the cursor, click or drag to - seek; and in the Live tab, the last minutes up to now. Not yet: switch ticks and shots (the - tool has them), enemy lanes. A replay's frames still take ~10 s to serve, nearly all of it the - ball-loss markers (`turnover_breakdown.analyse_match` re-reads the replay frame by frame). + seek; and in the Live tab, the last minutes up to now. Switch ticks, shots and a possession + band (each situation's side) as in the tool. The Replay side panel's Play list replaces the raw + event and intention logs: one row per situation (`goal_ledger.situations`: a restart going live + or a side winning the ball), with its third, length, shots and how it ended (the stoppage's + reason, or who won the ball and how); the raw intention log stays, folded. The slow analysis is + kept beside the replay (`.timeline.json`), so a replay serves in ~3 s. Not yet: enemy + lanes (no intentions are recorded for the opponent). 8. **More tactics from real football vocabulary** (formations, set plays, pressing schemes) rather than variations on existing ones. No new `Tactic` class has been added since this diff --git a/utama_core/dashboard/static/app.css b/utama_core/dashboard/static/app.css index 13009dd05..ff735f78b 100644 --- a/utama_core/dashboard/static/app.css +++ b/utama_core/dashboard/static/app.css @@ -187,7 +187,18 @@ td.num { text-align: right; font-variant-numeric: tabular-nums; } .auto-halt { background: var(--halt); color: #fff; padding: 6px 8px; border-radius: var(--radius); font-size: .75rem; } /* Replay timeline (timeline.js): swimlanes under the transport controls. */ -.timeline-wrap { width: 100%; cursor: pointer; } +.timeline-wrap { width: 100%; cursor: pointer; position: relative; } +.timeline-tip { position: absolute; display: none; pointer-events: none; z-index: 2; padding: 2px 6px; white-space: nowrap; + font-size: .68rem; background: var(--surface-2); border: 1px solid var(--border); border-radius: var(--radius); } + +/* Replay play list: one row per situation, a bar in the owning team's colour. */ +.play-row { display: flex; gap: 8px; padding: 3px 4px 3px 6px; border-left: 3px solid var(--border); cursor: pointer; } +.play-row:hover { background: var(--surface-2); } +.play-row.current { background: var(--surface-2); } +.play-row.goal .play-ended { color: #e0c14f; font-weight: bold; } +.play-time { min-width: 38px; color: var(--muted); font-variant-numeric: tabular-nums; } +.play-detail { color: var(--muted); } +.raw-log summary { cursor: pointer; font-size: .68rem; margin-top: 8px; } .timeline-wrap canvas { display: block; } .timeline-legend { display: flex; flex-wrap: wrap; gap: 4px 10px; font-size: .65rem; color: var(--muted); } .timeline-legend i { display: inline-block; width: 9px; height: 9px; margin-right: 3px; border-radius: 2px; vertical-align: -1px; } diff --git a/utama_core/dashboard/static/index.html b/utama_core/dashboard/static/index.html index 6816c7af5..fe519edc7 100644 --- a/utama_core/dashboard/static/index.html +++ b/utama_core/dashboard/static/index.html @@ -96,7 +96,7 @@
-
+
diff --git a/utama_core/dashboard/static/replay.js b/utama_core/dashboard/static/replay.js index 42d12abc8..4d8bce3fd 100644 --- a/utama_core/dashboard/static/replay.js +++ b/utama_core/dashboard/static/replay.js @@ -75,6 +75,7 @@ function framesFromColumns(c) { let play = []; // one row per situation (dashboard/views/replay.py play_rows), worded by wordPlay let playIdx = -1; // the row highlighted as the one being played let myTeamIsYellow = true; + let clockAt = () => null; // the referee's match clock at a time (timeline.js matchClock) const MARKER_LEAD_S = 2; // jump a little before an event, to see it happen function frameTs(i) { @@ -128,13 +129,17 @@ function framesFromColumns(c) { function updateReadouts() { const label = document.getElementById("replay-frame-label"); if (!label) return; + const clockEl = document.getElementById("replay-match-clock"); if (!frames.length) { label.textContent = "0:00 / 0:00"; + if (clockEl) clockEl.textContent = ""; return; } const elapsed = frameTs(index) - frameTs(0); const total = frameTs(frames.length - 1) - frameTs(0); label.textContent = fmtTime(elapsed) + " / " + fmtTime(total); + const clock = clockAt(frameTs(index)); + if (clockEl) clockEl.textContent = clock ? "match clock " + fmtTime(clock.played) : ""; } const REPLAY_HEADER_IDS = { @@ -436,6 +441,7 @@ function framesFromColumns(c) { frames = data.columns ? framesFromColumns(data.columns) : data.frames || []; tacticEvents = data.tactic_events || []; refereeEvents = data.referee_events || []; + clockAt = matchClock(refereeEvents); traceEvents = data.trace_events || []; markers = data.markers || []; shots = data.shots || []; diff --git a/utama_core/dashboard/static/timeline.js b/utama_core/dashboard/static/timeline.js index 08c026187..270ae2ae0 100644 --- a/utama_core/dashboard/static/timeline.js +++ b/utama_core/dashboard/static/timeline.js @@ -56,6 +56,43 @@ return "other"; } + // The commands the referee's match clock runs under (referee_command.py's CLOCK_RUNS): it stops in + // STOP, HALT, kickoff and penalty preparation and ball placement, so a 10 min match takes longer. + const CLOCK_RUNS = new Set([ + "NORMAL_START", + "FORCE_START", + "DIRECT_FREE_YELLOW", + "DIRECT_FREE_BLUE", + "INDIRECT_FREE_YELLOW", + "INDIRECT_FREE_BLUE", + ]); + + // The match clock from the referee's change rows [{sim_time, command, stage}], as the referee + // keeps it (state_machine.py's `_stage_played`): a function of time giving the stage and its + // playing time so far, or null before the first row. + function matchClock(referee) { + const played = []; + let total = 0; + for (let i = 0; i < referee.length; i++) { + const prev = referee[i - 1]; + if (prev && CLOCK_RUNS.has(prev.command)) total += referee[i].sim_time - prev.sim_time; + if (prev && referee[i].stage !== prev.stage) total = 0; + played.push(total); + } + return (t) => { + let lo = 0; + let hi = referee.length; + while (lo < hi) { + const mid = (lo + hi) >> 1; + if (referee[mid].sim_time <= t) lo = mid + 1; + else hi = mid; + } + if (lo === 0) return null; + const e = referee[lo - 1]; + return { stage: e.stage, played: played[lo - 1] + (CLOCK_RUNS.has(e.command) ? t - e.sim_time : 0) }; + }; + } + // Spans [{t0, t1, label, text}] tiling [start, end] from change points [[t, label, text]], // merging repeats (a merged span keeps its first text). function spansFrom(changes, start, end) { @@ -278,12 +315,22 @@ team: this._teams(myTeamIsYellow), }; robotIds.forEach((id, i) => (this.data.robotColour[id] = SLOTS[i % SLOTS.length])); - this.legend.innerHTML = ranked - .map((name) => `${short(name)}`) - .join(""); + this.legend.innerHTML = + this._refKey(this.data.referee) + + ranked.map((name) => `${short(name)}`).join(""); this.draw(start); } + // The referee band's colours, for the groups this match has: a short STOP or placement is + // too narrow to carry its name. + _refKey(spans) { + const groups = new Set(spans.map((s) => s.label)); + const present = Object.keys(REF_FILL).filter((g) => groups.has(g)); + if (!present.length) return ""; + const keys = present.map((g) => `${g}`).join(""); + return `ref${keys}tactics`; + } + _teams(myTeamIsYellow) { const friendly = myTeamIsYellow ? "yellow" : "blue"; const enemy = myTeamIsYellow ? "blue" : "yellow"; @@ -470,7 +517,7 @@ y + 2, BALL_H - 3, (s) => d.team[s.label] || OTHER, - (s) => (s.kind && s.kind !== "ball won" ? s.kind : null) + (s) => (s.kind && s.kind !== "ball won" ? "from " + s.kind : null) ); y += BALL_H + GAP; @@ -563,6 +610,6 @@ } } - if (typeof window !== "undefined") window.Timeline = Timeline; - if (typeof module !== "undefined") module.exports = { refGroup, spansFrom, tacticChanges }; + if (typeof window !== "undefined") Object.assign(window, { Timeline, matchClock }); + if (typeof module !== "undefined") module.exports = { refGroup, spansFrom, tacticChanges, matchClock }; })(); diff --git a/utama_core/tests/dashboard/test_timeline_js.py b/utama_core/tests/dashboard/test_timeline_js.py new file mode 100644 index 000000000..fc1f19e62 --- /dev/null +++ b/utama_core/tests/dashboard/test_timeline_js.py @@ -0,0 +1,39 @@ +"""`timeline.js`'s match clock run in node: the replay showed only its own time, which keeps +running through stoppages, so a 10 min match read 12 min with nothing to match the referee's +clock against. Skipped where node is missing.""" + +from __future__ import annotations + +import json +import shutil +import subprocess +from pathlib import Path + +import pytest + +SCRIPT = (Path(__file__).parents[2] / "dashboard" / "static" / "timeline.js").read_text() + +REFEREE = [ + {"sim_time": 0, "command": "PREPARE_KICKOFF_YELLOW", "stage": "NORMAL_FIRST_HALF_PRE"}, + {"sim_time": 5, "command": "NORMAL_START", "stage": "NORMAL_FIRST_HALF"}, + {"sim_time": 8, "command": "STOP", "stage": "NORMAL_FIRST_HALF"}, + {"sim_time": 9, "command": "BALL_PLACEMENT_BLUE", "stage": "NORMAL_FIRST_HALF"}, + {"sim_time": 10, "command": "DIRECT_FREE_BLUE", "stage": "NORMAL_FIRST_HALF"}, + {"sim_time": 12, "command": "NORMAL_START", "stage": "NORMAL_FIRST_HALF"}, + {"sim_time": 20, "command": "STOP", "stage": "NORMAL_HALF_TIME"}, +] + + +@pytest.mark.skipif(shutil.which("node") is None, reason="needs node") +def test_the_match_clock_runs_only_in_play_and_restarts_each_stage(): + js = ( + "const module = { exports: {} };\n" + + SCRIPT + + f"\nconst clock = module.exports.matchClock({json.dumps(REFEREE)});" + + "\nconsole.log(JSON.stringify([-1, 6, 9.5, 11, 15, 21].map(clock)));" + ) + out = json.loads(subprocess.run(["node", "-e", js], capture_output=True, text=True, check=True).stdout) + assert out[0] is None + # 5-8 in play, 8-10 stopped (STOP, placement), 10-20 in play (the free kick counts) + assert [c["played"] for c in out[1:]] == pytest.approx([1, 3, 4, 8, 0]) + assert out[-1]["stage"] == "NORMAL_HALF_TIME" From a68416b763120e9657d49b3fbd1899b1715f98db Mon Sep 17 00:00:00 2001 From: isaac0804 Date: Thu, 22 Oct 2026 16:20:34 +0100 Subject: [PATCH 31/56] fix(dashboard): a command from the Live tab is stamped on the referee's clock The page's commands were stamped with the wall clock, but in rsim the referee runs on sim time: one button press put its match clock (and the STOP and free-kick timers) 1.8e9 s out, shown as "29878093:22 left". They now take the time of the referee's last step (CustomReferee.now). Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_01TJGHdb8Dht67hn4Lohndf5 --- utama_core/custom_referee/custom_referee.py | 8 ++++ utama_core/dashboard/views/referee.py | 7 +++- .../test_referee_view_serialization.py | 37 +++++++++++++++++++ 3 files changed, 50 insertions(+), 2 deletions(-) diff --git a/utama_core/custom_referee/custom_referee.py b/utama_core/custom_referee/custom_referee.py index 189468e3e..72cf93829 100644 --- a/utama_core/custom_referee/custom_referee.py +++ b/utama_core/custom_referee/custom_referee.py @@ -208,6 +208,7 @@ def __init__( self._field_check: Optional[dict] = None self._match_log = None self._match_log_tick = 0 + self._now: Optional[float] = None # the last step's time, see `now` self._last_logged_ref_data: Optional[RefereeData] = None # Last non-stopping foul per team (True = yellow), for §7's goal validity. self._non_stopping_foul_at: dict[bool, float] = {} @@ -266,6 +267,7 @@ def step(self, game_frame: GameFrame, current_time: float) -> RefereeData: effect in `GameStateMachine._handle_foul` — it never touches `command`. """ + self._now = current_time violation: Optional[RuleViolation] = None non_stopping_violation: Optional[RuleViolation] = None for rule in self._rules: @@ -349,6 +351,12 @@ def set_field_check(self, field_check: dict) -> None: """ self._field_check = field_check + @property + def now(self) -> Optional[float]: + """The time of the last `step`, on the clock this referee runs on (sim time in rsim, not + the wall clock); None before the first. A command from outside the loop is stamped with it.""" + return self._now + @property def field_check(self) -> Optional[dict]: """The last `set_field_check` payload; None without a camera.""" diff --git a/utama_core/dashboard/views/referee.py b/utama_core/dashboard/views/referee.py index 3a5bf953a..5416f5c07 100644 --- a/utama_core/dashboard/views/referee.py +++ b/utama_core/dashboard/views/referee.py @@ -44,15 +44,18 @@ def _config_bytes() -> bytes: def _handle_command(payload: dict) -> None: cmd = RefereeCommand[payload["command"]] + # The referee's own clock: in rsim that is sim time, and a wall-clock stamp threw its + # match clock and timers 1.8e9 s out. + now = referee.now if referee.now is not None else time.time() designated = payload.get("designated") if designated is not None and hasattr(referee, "force_command"): target = (float(designated[0]), float(designated[1])) error = placement_target_error(target, referee.geometry, referee.field_check) if error: raise ValueError(error) # the page shows it; nothing is sent to the robots - referee.force_command(cmd, time.time(), ball_placement_target=target) + referee.force_command(cmd, now, ball_placement_target=target) else: - referee.set_command(cmd, time.time()) + referee.set_command(cmd, now) server.add_route("/referee/config", _config_bytes) server.add_command_handler("/referee/command", _handle_command) diff --git a/utama_core/tests/dashboard/test_referee_view_serialization.py b/utama_core/tests/dashboard/test_referee_view_serialization.py index ae6ac3485..33b2662fa 100644 --- a/utama_core/tests/dashboard/test_referee_view_serialization.py +++ b/utama_core/tests/dashboard/test_referee_view_serialization.py @@ -113,3 +113,40 @@ def test_a_refused_placement_is_a_bad_request_and_sends_nothing(monkeypatch): referee.force_command.assert_not_called() handlers["/referee/command"]({"command": "BALL_PLACEMENT_YELLOW", "designated": [1.0, 0.0]}) assert referee.force_command.call_args.kwargs["ball_placement_target"] == (1.0, 0.0) + + +def test_a_command_from_the_page_is_stamped_on_the_referees_clock_not_the_wall_clock(monkeypatch): + """In rsim the referee runs on sim time; a wall-clock stamp put its match clock 1.8e9 s out.""" + from types import SimpleNamespace + + from utama_core.custom_referee import CustomReferee + from utama_core.dashboard.views import referee as referee_view + from utama_core.entities.data.vector import Vector3D + from utama_core.entities.game.ball import Ball + from utama_core.entities.game.game_frame import GameFrame + from utama_core.entities.referee.referee_command import RefereeCommand + + handlers = {} + server = SimpleNamespace( + add_route=lambda path, getter: None, add_command_handler=handlers.__setitem__, notify=lambda *a: None + ) + referee = CustomReferee.from_profile_name("simulation") + monkeypatch.setattr(referee_view, "_build_static_config", lambda profile: {}) + referee_view.attach(server, referee, profile=None) + zero = Vector3D(0, 0, 0) + frame = GameFrame( + ts=12.5, + my_team_is_yellow=True, + my_team_is_right=False, + friendly_robots={}, + enemy_robots={}, + ball=Ball(p=zero, v=zero, a=zero), + referee=None, + ) + referee.seed_clock(0.0, RefereeCommand.HALT) + referee.step(frame, 12.5) + + handlers["/referee/command"]({"command": "STOP"}) + + assert referee._state.command == RefereeCommand.STOP + assert referee._state.command_timestamp == 12.5 From 9669652a9ab93ae81bf49f4ef3e226f98b9c8b2a Mon Sep 17 00:00:00 2001 From: isaac0804 Date: Thu, 22 Oct 2026 16:20:36 +0100 Subject: [PATCH 32/56] feat(dashboard): choose or restart each side's strategy from the Live tab StrategyRunner.replace_strategy runs a new strategy for a side from the next tick, only while halted; vision, the radio and the dashboard carry on. The script lists the choices (dashboard/views/strategy.py) and the Live tab shows a list and Run per side; Run on the one running restarts it. The field example offers its attacker and defender tactics. The example's robot now holds the shot line while its tactic does not apply: press applies only with an opponent near the ball, and picking it anyway made the kernel raise and stop the run. Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_01TJGHdb8Dht67hn4Lohndf5 --- examples/real_attacker_defender.py | 29 ++++++- utama_core/dashboard/static/index.html | 4 + utama_core/dashboard/static/live.js | 37 +++++++++ utama_core/dashboard/views/strategy.py | 50 +++++++++++ utama_core/run/strategy_runner.py | 43 ++++++++++ .../tests/dashboard/test_strategy_view.py | 48 +++++++++++ .../examples/test_examples_run_as_scripts.py | 14 ++++ .../strategy_runner/test_replace_strategy.py | 82 +++++++++++++++++++ 8 files changed, 304 insertions(+), 3 deletions(-) create mode 100644 utama_core/dashboard/views/strategy.py create mode 100644 utama_core/tests/dashboard/test_strategy_view.py create mode 100644 utama_core/tests/strategy_runner/test_replace_strategy.py diff --git a/examples/real_attacker_defender.py b/examples/real_attacker_defender.py index b9c853f5d..448d3a137 100644 --- a/examples/real_attacker_defender.py +++ b/examples/real_attacker_defender.py @@ -16,6 +16,8 @@ each robot plays, and whether the referee's rules are on. - Nothing advances on its own: play only starts and stops from the dashboard. Real mode starts in HALT; Ctrl+C sends stop commands to the robots before exiting. + - To change what a robot plays, or restart it, Halt and use the Live tab's Strategy panel + (Run on the one running restarts it); no need to restart the script. On the dashboard's Live tab, the field check must be all green or amber before Force Start: a red row (robot not seen, outside the camera half, radio lost, wrong field size) halts the @@ -71,6 +73,7 @@ ) from utama_core.dashboard import attach_dashboard from utama_core.dashboard.views import referee as referee_view +from utama_core.dashboard.views import strategy as strategy_view from utama_core.engine.abstract_strategy import AbstractStrategy from utama_core.engine.context import TickContext from utama_core.engine.strategy import Strategy as KernelSchedulerStrategy @@ -161,6 +164,14 @@ def _field_bounds(field: FieldDimensions, half: str) -> FieldBounds: raise ValueError(f"CAMERA_HALF must be 'left', 'right' or 'full', got {half!r}") +def _play_or_hold(game, free_robots, prev_partition, available_tactic_ids) -> dict: + """The robot plays its tactic; while that does not apply (press: no opponent near the ball), + it holds the shot line instead of the kernel rejecting the pick.""" + if not free_robots: + return {} + return {("play" if "play" in available_tactic_ids else "hold"): free_robots} + + def _one_robot_strategy(robot_id: int, tactic_cls) -> AbstractStrategy: """`robot_id` plays `tactic_cls`, or keeps goal when `tactic_cls` is None.""" outfield = () if tactic_cls is None else (robot_id,) @@ -168,8 +179,8 @@ def _one_robot_strategy(robot_id: int, tactic_cls) -> AbstractStrategy: def _build(motion_controller) -> KernelSchedulerStrategy: # A Strategy needs one registered tactic even with no outfield robot to run it. return KernelSchedulerStrategy( - tactics={"play": (tactic_cls or DefenseTactic)()}, - partitioner=KernelSchedulerStrategy.single_tactic_picker(lambda game, active: "play"), + tactics={"play": (tactic_cls or DefenseTactic)(), "hold": DefenseTactic()}, + partitioner=_play_or_hold, outfield_robot_ids=outfield, ctx=TickContext(motion_controller=motion_controller), ) @@ -249,7 +260,7 @@ def build_runner(mode: str, calibrate: Optional[str] = None) -> StrategyRunner: ) if game_log: print(f"Recording to {game_log}") - return StrategyRunner( + runner = StrategyRunner( game_log_path=game_log, strategy=_one_robot_strategy(attacker_id, attacker_tactic), opp_strategy=_one_robot_strategy(defender_id, defender_tactic), @@ -268,6 +279,18 @@ def build_runner(mode: str, calibrate: Optional[str] = None) -> StrategyRunner: yellow_robot_profiles=attacker_profiles if attacker_is_yellow else defender_profiles, blue_robot_profiles=defender_profiles if attacker_is_yellow else attacker_profiles, ) + if calibrate is None: + # The Live tab's Strategy panel: change either robot's tactic, or restart it, while halted. + strategy_view.attach( + server, + runner, + choices={ + "my": {n: (lambda t=t: _one_robot_strategy(attacker_id, t)) for n, t in ATTACKER_TACTICS.items()}, + "opp": {n: (lambda t=t: _one_robot_strategy(defender_id, t)) for n, t in DEFENDER_TACTICS.items()}, + }, + running={"my": ATTACKER_TACTIC, "opp": DEFENDER_TACTIC}, + ) + return runner def main() -> None: diff --git a/utama_core/dashboard/static/index.html b/utama_core/dashboard/static/index.html index b27415110..7adedec77 100644 --- a/utama_core/dashboard/static/index.html +++ b/utama_core/dashboard/static/index.html @@ -55,6 +55,10 @@
+
Tactics
diff --git a/utama_core/dashboard/static/live.js b/utama_core/dashboard/static/live.js index 8ce419db6..d63cffbd9 100644 --- a/utama_core/dashboard/static/live.js +++ b/utama_core/dashboard/static/live.js @@ -209,7 +209,44 @@ box.querySelector("#ref-check-ok").addEventListener("toggle", (e) => (okOpen = e.target.open)); } + // What each side plays, when the script offers choices (dashboard/views/strategy.py): a list + // and Run per side. The server refuses unless halted, and the refusal shows under the buttons. + function mountStrategy() { + fetch("/strategy/config") + .then((r) => (r.ok ? r.json() : null)) + .then((sides) => { + if (!sides) return; + const rows = document.getElementById("strategy-rows"); + rows.innerHTML = ""; + for (const side in sides) { + const s = sides[side]; + const row = document.createElement("div"); + row.className = "row"; + row.style.alignItems = "center"; + const options = s.choices + .map((n) => `${n}`) + .join(""); + row.innerHTML = + `${s.team}` + + ""; + row.querySelector("button").addEventListener("click", () => { + const name = row.querySelector("select").value; + fetch("/strategy/run", { + method: "POST", + headers: { "Content-Type": "application/json" }, + body: JSON.stringify({ side, name }), + }) + .then((r) => (r.ok ? null : r.text().then((t) => showSendError(name, t || "HTTP " + r.status)))) + .catch((err) => showSendError(name, err.message || "no connection")); + }); + rows.appendChild(row); + } + document.getElementById("strategy-run").style.display = ""; + }); + } + function mount() { + mountStrategy(); fetch("/referee/config") .then((r) => (r.ok ? r.json() : null)) .then((c) => { diff --git a/utama_core/dashboard/views/strategy.py b/utama_core/dashboard/views/strategy.py new file mode 100644 index 000000000..fa3d1f152 --- /dev/null +++ b/utama_core/dashboard/views/strategy.py @@ -0,0 +1,50 @@ +"""Strategy view — choose, or restart, what each side of a running match plays, from the Live tab. + +The script that builds the runner says what can be chosen: for each side ("my", "opp"), names +mapped to functions that build a fresh `AbstractStrategy`, and the name it started with. Running a +choice (the one running restarts it) hands a new strategy to `StrategyRunner.replace_strategy`, +which takes it at the next tick and only while HALTed; a refusal comes back to the page as a 400. +""" + +from __future__ import annotations + +import json +from typing import TYPE_CHECKING, Callable, Dict + +from utama_core.dashboard.server import DashboardServer + +if TYPE_CHECKING: + from utama_core.engine.abstract_strategy import AbstractStrategy + from utama_core.run import StrategyRunner + +Choices = Dict[str, Callable[[], "AbstractStrategy"]] + + +def attach( + server: DashboardServer, + runner: "StrategyRunner", + choices: Dict[str, Choices], + running: Dict[str, str], +) -> None: + """`choices`: side ("my"/"opp") -> {name: build a fresh strategy}; `running`: side -> the + name each side started with.""" + running = dict(running) + + def _config_bytes() -> bytes: + yellow = {"my": runner.my_team_is_yellow, "opp": not runner.my_team_is_yellow} + return json.dumps( + { + side: {"team": "yellow" if yellow[side] else "blue", "choices": list(names), "running": running[side]} + for side, names in choices.items() + } + ).encode() + + def _handle_run(payload: dict) -> None: + side, name = payload["side"], payload["name"] + if side not in choices or name not in choices[side]: + raise ValueError(f"no strategy {name!r} for {side!r}") + runner.replace_strategy(side, choices[side][name]()) + running[side] = name + + server.add_route("/strategy/config", _config_bytes) + server.add_command_handler("/strategy/run", _handle_run) diff --git a/utama_core/run/strategy_runner.py b/utama_core/run/strategy_runner.py index 403dc5391..e81cca051 100644 --- a/utama_core/run/strategy_runner.py +++ b/utama_core/run/strategy_runner.py @@ -431,6 +431,7 @@ def __init__( self.field_check = self._make_field_check() self._field_check_rows: list[dict] = [] self._auto_halt: Optional[str] = None # the auto-HALT in force's reason, shown on the dashboard + self._pending_strategies: dict[str, AbstractStrategy] = {} # side -> its replacement, see replace_strategy ### functions below rely on self.my and self.opp ### @@ -956,6 +957,47 @@ def _make_field_check(self) -> Optional[FieldCheck]: expected = [(colour, rid) for colour, ids in driven.items() for rid in ids] return FieldCheck(expected, self.field_bounds, self.full_field_dims) + def replace_strategy(self, side: str, strategy: AbstractStrategy) -> None: + """Run `strategy` for `side` ("my" or "opp") from the next tick, in place of the one + running: a different one, or a fresh copy of the same to restart it. Vision, the radio and + the dashboard carry on. Only while HALTed, so no robot is mid-move when its strategy + changes: raises ValueError otherwise (the dashboard shows it). Safe from any thread.""" + runtime = self.my if side == "my" else self.opp if side == "opp" else None + if runtime is None: + raise ValueError(f"no {side!r} side in this run") + if not self._halted(runtime): + raise ValueError("press Halt first: a strategy is only changed while halted") + if strategy.control_scheme not in (None, runtime.control_scheme): + raise ValueError( + f"needs control scheme {strategy.control_scheme!r}, this run uses {runtime.control_scheme!r}" + ) + self._pending_strategies[side] = strategy + + @staticmethod + def _halted(runtime: SideRuntime) -> bool: + referee = runtime.game.referee if runtime.game is not None else None + return referee is not None and referee.referee_command == RefereeCommand.HALT + + def _swap_in_pending_strategy(self, runtime: SideRuntime, side: str) -> None: + """Called on the control loop, just before `runtime`'s strategy steps.""" + new = self._pending_strategies.pop(side, None) + if new is None: + return + if not self._halted(runtime): # play restarted between the request and this tick + self.logger.warning("Strategy change for %s dropped: no longer halted", side) + return + old = runtime.strategy + if old.rsim_env is not None: + new.load_rsim_env(old.rsim_env) + new.load_robot_controller(old.robot_controller) + new.load_motion_controller(old.motion_controller) + new.load_game(old.game) + kernel_strategy = getattr(new, "_kernel_strategy", None) + if side == "my" and self.match_log is not None and kernel_strategy is not None: + kernel_strategy.match_log = self.match_log + runtime.strategy = new + self.logger.info("Strategy for %s replaced", side) + def _tick_field_check(self, vision_frames: List[Optional[RawVisionData]], now: float) -> None: """Judge the field against the setup; on real robots, HALT while any check fails. @@ -1989,4 +2031,5 @@ def _step_game( side.game.add_game_frame(new_game_frame) if not running_opp and self.match_stats is not None: self.match_stats.record_tick(new_game_frame, committed_tactics=self._committed_tactics()) + self._swap_in_pending_strategy(side, "opp" if running_opp else "my") side.strategy.step() diff --git a/utama_core/tests/dashboard/test_strategy_view.py b/utama_core/tests/dashboard/test_strategy_view.py new file mode 100644 index 000000000..bfdc45d00 --- /dev/null +++ b/utama_core/tests/dashboard/test_strategy_view.py @@ -0,0 +1,48 @@ +"""The Live tab's Strategy panel: what each side may play, and Run handing a fresh strategy to +the runner, which refuses unless halted.""" + +from __future__ import annotations + +import json +from types import SimpleNamespace + +import pytest + +from utama_core.dashboard.views import strategy as strategy_view + + +def _attach(replace): + routes, handlers = {}, {} + server = SimpleNamespace(add_route=routes.__setitem__, add_command_handler=handlers.__setitem__) + runner = SimpleNamespace(my_team_is_yellow=False, replace_strategy=replace) + built = [] + choices = { + "my": {"lead": lambda: built.append("lead") or "lead", "press": lambda: built.append("press") or "press"}, + "opp": {"goalkeeper": lambda: "goalkeeper"}, + } + strategy_view.attach(server, runner, choices, running={"my": "lead", "opp": "goalkeeper"}) + return routes["/strategy/config"], handlers["/strategy/run"], built + + +def test_run_builds_a_fresh_strategy_for_the_runner_and_the_config_shows_it_running(): + replaced = [] + config, run, built = _attach(lambda side, s: replaced.append((side, s))) + run({"side": "my", "name": "press"}) + run({"side": "my", "name": "press"}) # a restart: built afresh again + assert replaced == [("my", "press"), ("my", "press")] and built == ["press", "press"] + assert json.loads(config()) == { + "my": {"team": "blue", "choices": ["lead", "press"], "running": "press"}, + "opp": {"team": "yellow", "choices": ["goalkeeper"], "running": "goalkeeper"}, + } + + +def test_a_refused_run_keeps_what_is_shown_running(): + def refuse(side, s): + raise ValueError("press Halt first") + + config, run, _ = _attach(refuse) + with pytest.raises(ValueError, match="Halt"): + run({"side": "my", "name": "press"}) + with pytest.raises(ValueError, match="no strategy"): + run({"side": "opp", "name": "press"}) + assert json.loads(config())["my"]["running"] == "lead" diff --git a/utama_core/tests/examples/test_examples_run_as_scripts.py b/utama_core/tests/examples/test_examples_run_as_scripts.py index 8dd32686a..9428cedbf 100644 --- a/utama_core/tests/examples/test_examples_run_as_scripts.py +++ b/utama_core/tests/examples/test_examples_run_as_scripts.py @@ -20,3 +20,17 @@ def test_an_example_with_options_prints_its_help_run_by_path(name, tmp_path): ) assert done.returncode == 0, done.stderr[-2000:] assert "usage:" in done.stdout + + +def test_the_field_example_holds_while_its_tactic_does_not_apply(): + """`press` applies only with an opponent near the ball; picking it anyway made the kernel + raise, stopping the run the first time the defender played it.""" + sys.path.insert(0, str(EXAMPLES)) + try: + import real_attacker_defender as example + finally: + sys.path.remove(str(EXAMPLES)) + robots = frozenset({1}) + assert example._play_or_hold(None, robots, None, frozenset({"hold"})) == {"hold": robots} + assert example._play_or_hold(None, robots, None, frozenset({"play", "hold"})) == {"play": robots} + assert example._play_or_hold(None, frozenset(), None, frozenset({"play", "hold"})) == {} diff --git a/utama_core/tests/strategy_runner/test_replace_strategy.py b/utama_core/tests/strategy_runner/test_replace_strategy.py new file mode 100644 index 000000000..9c93f3b8d --- /dev/null +++ b/utama_core/tests/strategy_runner/test_replace_strategy.py @@ -0,0 +1,82 @@ +"""`StrategyRunner.replace_strategy`: the dashboard's Strategy panel changes, or restarts, what a +side plays while the run carries on — only while HALTed, so no robot is mid-move.""" + +from __future__ import annotations + +import pytest + +from utama_core.custom_referee import CustomReferee +from utama_core.custom_referee.profiles.profile_loader import load_profile +from utama_core.engine.abstract_strategy import AbstractStrategy +from utama_core.engine.context import TickContext +from utama_core.engine.match_log import MatchLog +from utama_core.engine.strategy import Strategy +from utama_core.entities.referee.referee_command import RefereeCommand +from utama_core.run import StrategyRunner +from utama_core.tactics.defense import DefenseTactic + + +def _strategy(ticks: list) -> AbstractStrategy: + def build(motion_controller) -> Strategy: + kernel = Strategy( + tactics={"play": DefenseTactic()}, + partitioner=Strategy.single_tactic_picker(lambda game, active: "play"), + outfield_robot_ids=(1,), + ctx=TickContext(motion_controller=motion_controller), + ) + tick = kernel.tick + kernel.tick = lambda game: ticks.append(game.referee.referee_command) or tick(game) + return kernel + + return AbstractStrategy(build_kernel_strategy=build) + + +@pytest.fixture +def runner(): + referee = CustomReferee(load_profile("simulation"), n_robots_yellow=2, n_robots_blue=0) + r = StrategyRunner( + strategy=_strategy([]), + my_team_is_yellow=True, + my_team_is_right=False, + mode="rsim", + exp_friendly=2, + exp_enemy=0, + referee=referee, + referee_initial_command=RefereeCommand.HALT, + ) + r.match_log = MatchLog() + yield r + r.close() + + +def test_a_replacement_runs_from_the_next_tick_and_logs_to_the_match_log(runner): + runner._run_step() + new_ticks: list = [] + runner.replace_strategy("my", _strategy(new_ticks)) + assert not new_ticks + runner._run_step() + assert new_ticks == [RefereeCommand.HALT] + assert runner.my.strategy._kernel_strategy.match_log is runner.match_log + + +def test_a_replacement_is_refused_unless_halted(runner): + runner._run_step() + runner.referee.set_command(RefereeCommand.FORCE_START, runner.my.current_game_frame.ts) + runner._run_step() + with pytest.raises(ValueError, match="Halt"): + runner.replace_strategy("my", _strategy([])) + + +def test_a_replacement_queued_while_halted_is_dropped_if_play_starts_first(runner): + runner._run_step() + old = runner.my.strategy + runner.replace_strategy("my", _strategy([])) + runner.referee.set_command(RefereeCommand.FORCE_START, runner.my.current_game_frame.ts) + runner._run_step() + assert runner.my.strategy is old + + +def test_there_is_no_opp_side_to_replace_without_an_opponent(runner): + runner._run_step() + with pytest.raises(ValueError, match="opp"): + runner.replace_strategy("opp", _strategy([])) From 2266a64cf52377c8390c2e4ce126b482e4f80704 Mon Sep 17 00:00:00 2001 From: isaac0804 Date: Thu, 22 Oct 2026 16:20:50 +0100 Subject: [PATCH 33/56] docs: the Live tab's Strategy panel Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_01TJGHdb8Dht67hn4Lohndf5 --- docs/custom_referee_gui.md | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/docs/custom_referee_gui.md b/docs/custom_referee_gui.md index ce944e734..76421a6f1 100644 --- a/docs/custom_referee_gui.md +++ b/docs/custom_referee_gui.md @@ -41,6 +41,22 @@ with everything outside `field_bounds` dimmed, so a half-field test shows which expects. Space halts; nothing on the keyboard starts the robots. The Kickoff and Free Kick buttons are hidden when the profile has no rule that sets up a restart (goal detection only). +## Strategy panel + +A script can let the page change what each side plays, or restart it, without restarting the +run (`utama_core/dashboard/views/strategy.py`): + +```python +strategy_view.attach(server, runner, + choices={"my": {"lead": make_lead, "press": make_press}}, # name -> builds a fresh AbstractStrategy + running={"my": "lead"}) +``` + +The Live tab then lists the choices per side with a Run button; Run on the one running restarts +it. `StrategyRunner.replace_strategy` takes the new strategy at the next tick and only while +halted, so the page refuses otherwise. `examples/real_attacker_defender.py` offers its attacker +and defender tactics this way. + ## Commands and what robots do | Button | Command | Use | Robots | From cbcb1e3ca2b3934e0cb43306f6de406e49344171 Mon Sep 17 00:00:00 2001 From: isaac0804 Date: Thu, 22 Oct 2026 16:34:20 +0100 Subject: [PATCH 34/56] fix(run,dashboard): an operator's HALT lasts in the simulators; a taken dashboard port fails at start In rsim and grSim the runner resumes a HALT after 5 s, for tournaments with nobody to end a HALT the rules call. It resumed the operator's Halt too, and the field example's starting HALT, so a grSim test was playing before anyone pressed Start and the Strategy panel had 5 s to act. A HALT called from the dashboard (or a run started halted with the dashboard attached) now lasts until the operator ends it; the rules' HALTs still resume. A dashboard whose port was taken failed in its server thread and the match ran on without one, the open page showing the other dashboard. It now raises when started. Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_01TJGHdb8Dht67hn4Lohndf5 --- examples/real_attacker_defender.py | 2 +- utama_core/custom_referee/custom_referee.py | 16 +++++++++++++++- utama_core/dashboard/server.py | 10 +++++++--- utama_core/dashboard/views/referee.py | 4 ++-- utama_core/run/strategy_runner.py | 6 ++++-- .../custom_referee/test_custom_referee.py | 18 ++++++++++++++++++ .../tests/dashboard/test_server_port_taken.py | 19 +++++++++++++++++++ .../strategy_runner/test_replace_strategy.py | 13 +++++++++++++ 8 files changed, 79 insertions(+), 9 deletions(-) create mode 100644 utama_core/tests/dashboard/test_server_port_taken.py diff --git a/examples/real_attacker_defender.py b/examples/real_attacker_defender.py index 448d3a137..b245fc22b 100644 --- a/examples/real_attacker_defender.py +++ b/examples/real_attacker_defender.py @@ -14,7 +14,7 @@ - Everything to edit for a test day is in the "Test-day configuration" block: the field, which part of it the camera sees, each robot's IDs and working hardware, what each robot plays, and whether the referee's rules are on. - - Nothing advances on its own: play only starts and stops from the dashboard. Real + - Nothing advances on its own: play only starts and stops from the dashboard. Every mode starts in HALT; Ctrl+C sends stop commands to the robots before exiting. - To change what a robot plays, or restart it, Halt and use the Live tab's Strategy panel (Run on the one running restarts it); no need to restart the script. diff --git a/utama_core/custom_referee/custom_referee.py b/utama_core/custom_referee/custom_referee.py index 72cf93829..3e964f876 100644 --- a/utama_core/custom_referee/custom_referee.py +++ b/utama_core/custom_referee/custom_referee.py @@ -209,6 +209,7 @@ def __init__( self._match_log = None self._match_log_tick = 0 self._now: Optional[float] = None # the last step's time, see `now` + self._operator_halt = False # see halted_by_operator self._last_logged_ref_data: Optional[RefereeData] = None # Last non-stopping foul per team (True = yellow), for §7's goal validity. self._non_stopping_foul_at: dict[bool, float] = {} @@ -351,6 +352,13 @@ def set_field_check(self, field_check: dict) -> None: """ self._field_check = field_check + @property + def halted_by_operator(self) -> bool: + """In a HALT an operator called (the dashboard's Halt, or a run started halted with the + dashboard there to end it), as against one the rules called: the simulators resume + only the latter on their own.""" + return self._operator_halt and self._state.command == RefereeCommand.HALT + @property def now(self) -> Optional[float]: """The time of the last `step`, on the clock this referee runs on (sim time in rsim, not @@ -372,6 +380,7 @@ def seed_clock(self, timestamp: float, initial_command: RefereeCommand = Referee (wall-clock timestamps that may be far from 0). """ self._state.seed_clock(timestamp) + self._operator_halt = initial_command == RefereeCommand.HALT and self._dashboard_notifier is not None if initial_command != RefereeCommand.HALT: self._state.set_command(initial_command, timestamp) @@ -382,6 +391,7 @@ def set_command( designated_position: Optional[tuple[float, float]] = None, next_command: Optional[RefereeCommand] = None, status_message: Optional[str] = None, + by_operator: bool = False, ) -> None: """Manual override for operator use or test scripting. @@ -393,12 +403,14 @@ def set_command( next_command: Optional queued command to expose through ``RefereeData.next_command``. status_message: Optional human-readable referee status. + by_operator: A person called it (the dashboard); see `halted_by_operator`. The optional fields are intentionally thin wrappers around the custom referee state machine so integration tests and scenario runners do not need to reach into private ``_state`` attributes. """ self._state.set_command(command, timestamp) + self._operator_halt = by_operator and command == RefereeCommand.HALT if designated_position is not None: self._state.ball_placement_target = designated_position if next_command is not None: @@ -411,9 +423,11 @@ def force_command( command: "RefereeCommand", timestamp: float, ball_placement_target=None, + by_operator: bool = False, ) -> None: - """God-mode override — bypasses the STOP-first guard.""" + """God-mode override — bypasses the STOP-first guard. `by_operator` as in `set_command`.""" self._state.force_command(command, timestamp, ball_placement_target) + self._operator_halt = by_operator and command == RefereeCommand.HALT def resume_from_halt(self, timestamp: float) -> bool: """HALT → STOP, keeping any queued restart (`GameStateMachine.resume_from_halt`).""" diff --git a/utama_core/dashboard/server.py b/utama_core/dashboard/server.py index 70cc38fcd..49cefcfc3 100644 --- a/utama_core/dashboard/server.py +++ b/utama_core/dashboard/server.py @@ -145,10 +145,14 @@ def notify(self, channel: str, state: dict) -> None: # ---- thread entry point ---- + def start(self) -> None: + # Bound here, on the caller's thread: a port already taken (another dashboard) raises + # at once, instead of in the server thread while the match runs on with no dashboard. + self._http_server = ThreadingHTTPServer(("", self._port), self._make_handler_class()) + super().start() + def run(self) -> None: - handler_factory = self._make_handler_class() - http_server = ThreadingHTTPServer(("", self._port), handler_factory) - http_server.serve_forever() + self._http_server.serve_forever() def _make_handler_class(self): server = self diff --git a/utama_core/dashboard/views/referee.py b/utama_core/dashboard/views/referee.py index 5416f5c07..dfe746490 100644 --- a/utama_core/dashboard/views/referee.py +++ b/utama_core/dashboard/views/referee.py @@ -53,9 +53,9 @@ def _handle_command(payload: dict) -> None: error = placement_target_error(target, referee.geometry, referee.field_check) if error: raise ValueError(error) # the page shows it; nothing is sent to the robots - referee.force_command(cmd, now, ball_placement_target=target) + referee.force_command(cmd, now, ball_placement_target=target, by_operator=True) else: - referee.set_command(cmd, now) + referee.set_command(cmd, now, by_operator=True) server.add_route("/referee/config", _config_bytes) server.add_command_handler("/referee/command", _handle_command) diff --git a/utama_core/run/strategy_runner.py b/utama_core/run/strategy_runner.py index e81cca051..00e052ff7 100644 --- a/utama_core/run/strategy_runner.py +++ b/utama_core/run/strategy_runner.py @@ -1712,13 +1712,15 @@ def _run_step(self): if ref_data.referee_command not in _BALL_PLACEMENT_COMMANDS: self._pending_placement_teleport = None self._tick_pending_placement_teleport() - if self.sim_controller is not None: + if self.sim_controller is not None and not self.referee.halted_by_operator: # HALT (e.g. DefenseAreaStoppageRule's 2nd-foul escalation) has # no auto-advance in GameStateMachine by design — on a real # pitch it requires a human referee/GameController to resume. # An automated sim/tournament run has neither, so left alone # a HALT here would freeze the match forever. Auto-resume to - # NORMAL_START after a grace period, sim-only. + # NORMAL_START after a grace period, sim-only. Not a HALT an + # operator called from the dashboard: they end it (the Strategy + # panel needs one that lasts). if ref_data.referee_command == RefereeCommand.HALT: if self._prev_custom_ref_command != RefereeCommand.HALT: self._halt_entered_at = self.my.current_game_frame.ts diff --git a/utama_core/tests/custom_referee/test_custom_referee.py b/utama_core/tests/custom_referee/test_custom_referee.py index 9b8c66ba0..77ae529fb 100644 --- a/utama_core/tests/custom_referee/test_custom_referee.py +++ b/utama_core/tests/custom_referee/test_custom_referee.py @@ -1406,3 +1406,21 @@ def test_legacy_stop_to_prepare_kickoff_key_is_still_loaded(self, tmp_path): def test_unknown_profile_raises(self): with pytest.raises(FileNotFoundError): load_profile("nonexistent_profile") + + +def test_only_a_halt_an_operator_called_counts_as_the_operators(): + """The simulators resume a HALT the rules call; one the operator called waits for them.""" + referee = CustomReferee.from_profile_name("simulation") + referee.seed_clock(0.0, RefereeCommand.HALT) + assert not referee.halted_by_operator # no dashboard: nobody to end it + referee.set_command(RefereeCommand.STOP, 1.0, by_operator=True) + assert not referee.halted_by_operator + referee.force_command(RefereeCommand.HALT, 2.0, by_operator=True) + assert referee.halted_by_operator + referee.force_command(RefereeCommand.HALT, 3.0) # the runner's or a rule's + assert not referee.halted_by_operator + + referee = CustomReferee.from_profile_name("simulation") + referee.attach_dashboard_notifier(lambda *args: None) + referee.seed_clock(0.0, RefereeCommand.HALT) # started halted, with a page to end it + assert referee.halted_by_operator diff --git a/utama_core/tests/dashboard/test_server_port_taken.py b/utama_core/tests/dashboard/test_server_port_taken.py new file mode 100644 index 000000000..f02a8c2d7 --- /dev/null +++ b/utama_core/tests/dashboard/test_server_port_taken.py @@ -0,0 +1,19 @@ +"""A dashboard whose port is taken (another dashboard already running) must fail at start: it +used to fail in its server thread, and the match ran on with no Live tab while the page open on +that port showed the other dashboard.""" + +from __future__ import annotations + +import socket + +import pytest + +from utama_core.dashboard.server import DashboardServer + + +def test_starting_a_dashboard_on_a_taken_port_raises(): + with socket.socket() as taken: + taken.bind(("", 0)) + taken.listen() + with pytest.raises(OSError): + DashboardServer(taken.getsockname()[1]).start() diff --git a/utama_core/tests/strategy_runner/test_replace_strategy.py b/utama_core/tests/strategy_runner/test_replace_strategy.py index 9c93f3b8d..2093cbb24 100644 --- a/utama_core/tests/strategy_runner/test_replace_strategy.py +++ b/utama_core/tests/strategy_runner/test_replace_strategy.py @@ -80,3 +80,16 @@ def test_there_is_no_opp_side_to_replace_without_an_opponent(runner): runner._run_step() with pytest.raises(ValueError, match="opp"): runner.replace_strategy("opp", _strategy([])) + + +def test_in_the_simulator_an_operators_halt_lasts_until_they_end_it(runner): + """A HALT the rules call resumes on its own after 5 s in sim (no operator in a tournament); + one pressed on the dashboard waited only that long, so the Strategy panel had 5 s to act.""" + runner._run_step() + runner.referee.set_command(RefereeCommand.FORCE_START, runner.my.current_game_frame.ts) + runner._run_step() + runner.referee.set_command(RefereeCommand.HALT, runner.my.current_game_frame.ts, by_operator=True) + start = runner.my.current_game_frame.ts + while runner.my.current_game_frame.ts - start < 12: # past the 5 s resume and 4 s clearing + runner._run_step() + assert runner.referee._state.command == RefereeCommand.HALT From e514eb9ba363febdbb76105b40405dbf6f679ae8 Mon Sep 17 00:00:00 2001 From: isaac0804 Date: Thu, 22 Oct 2026 16:46:58 +0100 Subject: [PATCH 35/56] fix(run): on camera runs our robots stop at the edge of the area in play A tactic chasing a ball out of the camera's half drove its robot after it (0.9 m past the half-way line in a grSim test of real_attacker_defender), out of sight of vision. The planners' walls at field_bounds are obstacles to plan around, not a limit, and the field check only halts once a robot is 0.3 m out. FencedRobotController sits under the strategy in grSim and real mode and caps each robot's speed towards each edge to what it can stop from before the line (2 m/s^2, 0.1 s delay); speed away from an edge is never limited, so a robot outside drives back in. In grSim, a ball rolled out repeatedly for 25 s: 0.52 m outside without it, 0.001 m with. Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_01TJGHdb8Dht67hn4Lohndf5 --- docs/custom_referee_gui.md | 6 ++ utama_core/run/field_fence.py | 76 +++++++++++++++++++ utama_core/run/strategy_runner.py | 14 +++- .../tests/strategy_runner/test_field_fence.py | 67 ++++++++++++++++ 4 files changed, 161 insertions(+), 2 deletions(-) create mode 100644 utama_core/run/field_fence.py create mode 100644 utama_core/tests/strategy_runner/test_field_fence.py diff --git a/docs/custom_referee_gui.md b/docs/custom_referee_gui.md index 76421a6f1..48fd7a5a8 100644 --- a/docs/custom_referee_gui.md +++ b/docs/custom_referee_gui.md @@ -33,6 +33,12 @@ With vision from a camera (real robots or grSim) the Live tab also shows the fie | ` ` (any other) | | seen inside `field_bounds` but not configured: our planners cannot see it, often a wrong ID | | Ball | | not seen, or outside `field_bounds` | +Before it gets that far, the runner keeps our robots inside `field_bounds` on camera runs +(`utama_core/run/field_fence.py`): each robot's speed towards an edge is capped to what it can +stop from before the line, whatever tactic sent the command, so a robot chasing a ball out of the +camera's area stops at the line. The planners' walls at `field_bounds` are only obstacles to plan +around, and did not stop it (0.5–0.9 m out in grSim before the fence). + Only the red and amber rows are listed; the green ones fold into an "N ok" line that opens on click. On real robots any red row forces `HALT`, whatever the command and whether or not the page is open; the red banner says why. Pressing Force Start while a row is red halts again at once, so diff --git a/utama_core/run/field_fence.py b/utama_core/run/field_fence.py new file mode 100644 index 000000000..057e607ed --- /dev/null +++ b/utama_core/run/field_fence.py @@ -0,0 +1,76 @@ +"""Keeps our robots inside the area in play (`field_bounds`, what the camera sees) on camera runs. + +A tactic chasing a ball that left the camera's half drove its robot after it, out of sight, +where nothing tracks it (a 2026-10-22 grSim test of `examples/real_attacker_defender.py`: the +attacker 0.9 m past the half-way line). The field check only halts once a robot is already +0.3 m out. `FencedRobotController` sits under the strategy, like `ProfiledRobotController`, and +caps each robot's speed towards each edge of the area to what it can still stop from before the +line; speed away from an edge is never limited, so a robot outside drives back in. It knows the +robots' poses, nothing of the tactics, so it holds whichever tactic or skill made the command. +""" + +from __future__ import annotations + +import math +from typing import Callable, Dict, Optional, Union + +from utama_core.entities.data.command import RobotCommand +from utama_core.entities.game import Game +from utama_core.entities.game.field import FieldBounds + +BRAKE = 2.0 # m/s^2: half the robots' MAX_ACCELERATION, for a floor that grips less +DELAY = 0.1 # s from a command to the robot acting on it (radio, firmware), at full speed + + +def _max_speed_towards(distance: float) -> float: + """The fastest a robot `distance` m from a line may move towards it and still stop before it.""" + if distance <= 0: + return 0.0 + return BRAKE * (-DELAY + math.sqrt(DELAY * DELAY + 2 * distance / BRAKE)) + + +def fence(x: float, y: float, orientation: float, command: RobotCommand, bounds: FieldBounds) -> RobotCommand: + """`command` for a robot at (`x`, `y`) facing `orientation`, its speed towards each edge of + `bounds` capped; unchanged when no cap applies.""" + c, s = math.cos(orientation), math.sin(orientation) + vx = command.local_forward_vel * c - command.local_left_vel * s + vy = command.local_forward_vel * s + command.local_left_vel * c + (left, top), (right, bottom) = bounds.top_left, bounds.bottom_right + fx = min(vx, _max_speed_towards(right - x)) if vx > 0 else max(vx, -_max_speed_towards(x - left)) + fy = min(vy, _max_speed_towards(top - y)) if vy > 0 else max(vy, -_max_speed_towards(y - bottom)) + if fx == vx and fy == vy: + return command + return command._replace(local_forward_vel=fx * c + fy * s, local_left_vel=-fx * s + fy * c) + + +class FencedRobotController: + """A team's robot controller whose robots' commands are fenced to `bounds` (see module + docstring). `game` gives the side's current `Game`, whose poses are in vision coordinates + like `bounds`; a robot it does not see is sent its command unchanged.""" + + def __init__(self, controller, bounds: FieldBounds, game: Callable[[], Optional[Game]]): + self._controller = controller + self._bounds = bounds + self._game = game + + def __getattr__(self, name): + return getattr(self._controller, name) + + def add_robot_commands( + self, + robot_commands: Union[RobotCommand, Dict[int, RobotCommand]], + robot_id: Optional[int] = None, + ) -> None: + if isinstance(robot_commands, RobotCommand): + if robot_id is None: + raise ValueError("robot_id cannot be None type!") + robot_commands = {robot_id: robot_commands} + game = self._game() + robots = game.friendly_robots if game is not None else {} + fenced = {} + for rid, command in robot_commands.items(): + robot = robots.get(rid) + fenced[rid] = ( + command if robot is None else fence(robot.p.x, robot.p.y, robot.orientation, command, self._bounds) + ) + self._controller.add_robot_commands(fenced) diff --git a/utama_core/run/strategy_runner.py b/utama_core/run/strategy_runner.py index 00e052ff7..fdc720b1d 100644 --- a/utama_core/run/strategy_runner.py +++ b/utama_core/run/strategy_runner.py @@ -56,6 +56,7 @@ from utama_core.rsoccer_simulator.src.Utils.sim_noise import SimNoise from utama_core.run import GameGater from utama_core.run.field_check import FieldCheck +from utama_core.run.field_fence import FencedRobotController from utama_core.run.live_status import LiveStatusMixin from utama_core.run.referee_source import OfficialReferee, RefereeSource from utama_core.run.side_runtime import SideRuntime, _mirrored, _turned @@ -1193,7 +1194,7 @@ def _load_robot_controllers(self): my_profiles, opp_profiles = map_colors_to_friendly_enemy( self.my_team_is_yellow, self.yellow_robot_profiles, self.blue_robot_profiles ) - my_robot_controller = self._with_profiles(my_robot_controller, my_profiles) + my_robot_controller = self._fenced(self._with_profiles(my_robot_controller, my_profiles), lambda: self.my.game) self.my.strategy.load_robot_controller(my_robot_controller) self.my.strategy.load_motion_controller(self.my.motion_controller(self.mode, self.rsim_env)) if self.match_log is not None: @@ -1207,7 +1208,9 @@ def _load_robot_controllers(self): if kernel_strategy is not None: kernel_strategy.match_log = self.match_log if self.opp: - opp_robot_controller = self._with_profiles(opp_robot_controller, opp_profiles) + opp_robot_controller = self._fenced( + self._with_profiles(opp_robot_controller, opp_profiles), lambda: self.opp.game + ) self.opp.strategy.load_robot_controller(opp_robot_controller) self.opp.strategy.load_motion_controller(self.opp.motion_controller(self.mode, self.rsim_env)) @@ -1242,6 +1245,13 @@ def _trusted_ir(self, is_yellow: bool, trusted_ir_robots: Optional[FrozenSet[int ) return frozenset(self._driven_ids(is_yellow) - broken) + def _fenced(self, controller, game): + """`controller`, its robots kept inside `field_bounds` when vision comes from a camera + (grSim or real): outside it nothing sees them (`field_fence.py`).""" + if self.mode == Mode.RSIM: + return controller + return FencedRobotController(controller, self.field_bounds, game) + def _with_profiles(self, controller, profiles: dict[int, RobotProfile]): """`controller`, wrapped to fit commands to `profiles` when a kicker or dribbler is missing.""" if all(p.kicker and p.dribbler for p in profiles.values()): diff --git a/utama_core/tests/strategy_runner/test_field_fence.py b/utama_core/tests/strategy_runner/test_field_fence.py new file mode 100644 index 000000000..79ae29c53 --- /dev/null +++ b/utama_core/tests/strategy_runner/test_field_fence.py @@ -0,0 +1,67 @@ +"""Robots on camera runs are kept inside the area in play: a tactic chasing a ball out of the +camera's half drove its robot out of sight (a grSim test, 0.9 m past the half-way line).""" + +from __future__ import annotations + +import math +from types import SimpleNamespace + +import pytest + +from utama_core.entities.data.command import RobotCommand +from utama_core.entities.data.vector import Vector2D +from utama_core.entities.game.field import FieldBounds +from utama_core.run.field_fence import ( + BRAKE, + DELAY, + FencedRobotController, + _max_speed_towards, + fence, +) + +LEFT_HALF = FieldBounds(top_left=(-2.0, 1.5), bottom_right=(0.0, -1.5)) + + +def _cmd(forward=0.0, left=0.0, turn=0.0) -> RobotCommand: + return RobotCommand(forward, left, turn, kick=False, chip=False, dribble=False) + + +@pytest.mark.parametrize("distance", [0.01, 0.1, 0.5, 2.0]) +def test_the_speed_cap_is_what_stops_a_robot_exactly_at_the_line(distance): + v = _max_speed_towards(distance) + assert v * DELAY + v * v / (2 * BRAKE) == pytest.approx(distance) + + +def test_far_from_the_lines_a_command_is_unchanged(): + command = _cmd(1.0, 0.3, 2.0) + assert fence(-1.0, 0.0, 0.0, command, LEFT_HALF) is command + + +def test_towards_the_half_way_line_the_robot_slows_and_on_it_stops(): + near = fence(-0.05, 0.0, 0.0, _cmd(1.0), LEFT_HALF) + assert 0 < near.local_forward_vel < 1.0 + assert fence(0.0, 0.0, 0.0, _cmd(1.0), LEFT_HALF).local_forward_vel == 0.0 + + +def test_only_the_speed_towards_the_line_is_capped_whatever_way_the_robot_faces(): + # Facing +y, its left is -x: a robot on the line driving left-and-forward keeps the +y part. + out = fence(0.0, 0.0, math.pi / 2, _cmd(0.5, -1.0, 1.0), LEFT_HALF) + assert out.local_forward_vel == pytest.approx(0.5) + assert out.local_left_vel == pytest.approx(0.0) + assert out.angular_vel == 1.0 + + +def test_a_robot_outside_may_drive_back_in_but_not_further_out(): + assert fence(0.4, 0.0, 0.0, _cmd(-1.0), LEFT_HALF).local_forward_vel == -1.0 + assert fence(0.4, 0.0, 0.0, _cmd(1.0), LEFT_HALF).local_forward_vel == 0.0 + assert fence(-1.0, -1.7, 0.0, _cmd(left=-1.0), LEFT_HALF).local_left_vel == 0.0 # past the touch line + + +def test_the_controller_fences_the_robots_it_sees_and_passes_the_rest(): + sent = {} + inner = SimpleNamespace(add_robot_commands=sent.update) + game = SimpleNamespace(friendly_robots={0: SimpleNamespace(p=Vector2D(0.0, 0.0), orientation=0.0)}) + controller = FencedRobotController(inner, LEFT_HALF, lambda: game) + controller.add_robot_commands({0: _cmd(1.0), 1: _cmd(1.0)}) + assert sent[0].local_forward_vel == 0.0 + assert sent[1].local_forward_vel == 1.0 From 20ab69fd3725d6158ffc297a5db9b5eba869ca90 Mon Sep 17 00:00:00 2001 From: isaac0804 Date: Thu, 22 Oct 2026 19:05:12 +0100 Subject: [PATCH 36/56] docs,tools: the real-robot test-day procedure; a vision check docs/real_robot_setup.md: ssl-vision on its own laptop, WSL mirrored networking and the Hyper-V firewall rule for vision, the radio through usbipd by hardware ID with auto-attach, teleop, and the field check, with the failures met on the first setup (2026-10-22) and their causes. It replaces setup_external.md's short WSL section. tools/vision_check.py says whether ssl-vision's multicast reaches this machine, per camera, with no radio or runner. tools/ssl-vision-lab-field.patch adds our field as a preset to ssl-vision. Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_01TJGHdb8Dht67hn4Lohndf5 --- docs/README.md | 3 +- docs/real_robot_setup.md | 134 +++++++++++++++++++++++++++++++ docs/setup_external.md | 30 +------ docs/tools.md | 4 +- tools/ssl-vision-lab-field.patch | 117 +++++++++++++++++++++++++++ tools/vision_check.py | 72 +++++++++++++++++ 6 files changed, 331 insertions(+), 29 deletions(-) create mode 100644 docs/real_robot_setup.md create mode 100644 tools/ssl-vision-lab-field.patch create mode 100644 tools/vision_check.py diff --git a/docs/README.md b/docs/README.md index 12b989340..54678a6ef 100644 --- a/docs/README.md +++ b/docs/README.md @@ -12,7 +12,8 @@ Every doc in this folder, by kind. New to the repo: the [README](../README.md) f - [motion_planning_results.md](motion_planning_results.md): its latest full run. - [tools.md](tools.md): every script and pixi task, by purpose. - [visual_guide.md](visual_guide.md): the game drawn on the pitch from real replays (how each strategy plays, one match over time, a restart, the motion planner), what each picture has shown us, and how to draw it. -- [setup_external.md](setup_external.md): grSim, the official GameController and AutoReferee, SSL Vision for real robots. +- [setup_external.md](setup_external.md): grSim, the official GameController and AutoReferee. +- [real_robot_setup.md](real_robot_setup.md): a test day with real robots: ssl-vision on its own laptop, WSL networking and USB, the radio, and the checks before a robot moves. - [contributing.md](contributing.md): editor setup, commits, pull requests and releases. ## Design rationale diff --git a/docs/real_robot_setup.md b/docs/real_robot_setup.md new file mode 100644 index 000000000..85f40f054 --- /dev/null +++ b/docs/real_robot_setup.md @@ -0,0 +1,134 @@ +# Real robots: setting up and checking vision and radio + +The procedure for a test day with real robots: ssl-vision on its own Linux laptop sending vision +over the network, and our stack on a Windows laptop in WSL with the radio dongle. Each part is +checked on its own before anything drives a robot: vision with `tools/vision_check.py`, the radio +with teleop, then both together in the field check of `examples/real_attacker_defender.py`. + +``` +vision laptop (Linux) robot laptop (Windows + WSL) + cameras -> ssl-vision --multicast--> tools/vision_check.py, then the runner + 224.5.23.2:10006 radio dongle (USB, passed into WSL) -> robots +``` + +## Once per machine + +### Vision laptop (Linux) + +1. Build [ssl-vision](https://github.com/RoboCup-SSL/ssl-vision) (its README). For a FLIR + camera install FLIR's **Spinnaker SDK** first: on the + [download page](https://www.flir.com/products/spinnaker-sdk) take "Spinnaker SDK, 64-bit" + for your Ubuntu (not ARM64 unless the laptop is ARM). Unpack it, install the libraries its + README lists, then `sudo sh install_spinnaker.sh`: yes to the udev entry (add your user to + `flirimaging`) and to USB-FS 1000 MB. Build ssl-vision with + `cmake -B build -DUSE_SPINNAKER=true -DUSE_V4L=true && make -C build`. +2. Add our field as a preset: `git apply tools/ssl-vision-lab-field.patch` in the ssl-vision + checkout (copy the file over), then rebuild. Field Configuration gets a **Lab Field** button + (3 m x 2.25 m, goal 0.8 m, defense area 1 m x 0.5 m, two cameras). Do not use the Division B + button with two cameras: it sets the camera count to 1. +3. USB webcams (the RER camera is one) need your user in `video`: + `sudo usermod -aG video $USER`, then log in again. + +### Robot laptop (Windows + WSL) + +1. **USB into WSL:** in admin PowerShell, `winget install --exact dorssel.usbipd-win`. The + `usbipd` you can run inside WSL is an unrelated Linux tool: always run it in PowerShell. +2. **Share the radio once, by its hardware ID** (stays the same whatever port it is in, unlike + the bus ID): `usbipd list`, note the radio's `VID:PID`, then + `usbipd bind --hardware-id `. +3. **WSL on the real network:** in `%USERPROFILE%\.wslconfig`, under `[wsl2]`, add + `networkingMode=mirrored`. Without it WSL sits on a private network that never sees the + vision laptop's multicast. +4. **Let vision in:** in admin PowerShell, open UDP 10006 to WSL in the Hyper-V firewall: + ``` + New-NetFirewallHyperVRule -Name SSLVision -DisplayName "SSL Vision multicast" -Direction Inbound -VMCreatorId '{40E0AC32-46A5-438A-A0B2-2B479E8F2E90}' -Protocol UDP -LocalPorts 10006 + ``` + (`40E0AC32-...` is WSL's fixed ID in that firewall.) Then `wsl --shutdown`. +5. **Radio permission in WSL:** `sudo usermod -aG dialout $USER`, then `newgrp dialout` in the + terminal you run from (or restart WSL). + +## Each test day + +### 1. Network + +Both laptops on the same network, by Ethernet to one switch or router if possible: many Wi-Fi +routers drop or throttle multicast. If the robot laptop has two networks (Wi-Fi for internet, +Ethernet to the switch), our receiver listens on the one Windows uses for internet: use one. + +### 2. Vision + +On the vision laptop: `./bin/vision -c 2` (the number of cameras) in the ssl-vision checkout. +For each camera under **Image Capture**: the capture module (**Spinnaker** for FLIR, **Video 4 +Linux** for a USB webcam), then **Capture Control → Start**. Settings save on a clean exit or +with **Save Settings**, not on a crash. + +- **USB webcam:** cam idx is the `/dev/video` number of its image; a camera shows up as two + nodes and the second is metadata ("Inappropriate ioctl" means the wrong one). Use **MJPEG**: + the RER camera's uncompressed YUYV is 5 fps at 1280x960, MJPEG 60-90 fps. Its controls: + `v4l2-ctl -d /dev/video0 --list-ctrls`. Let auto white balance settle + (`--set-ctrl=white_balance_automatic=1`), then lock it (`=0` and + `--set-ctrl=white_balance_temperature=`): colours that drift break the colour + calibration. Keep exposure under about 10 ms (`exposure_time_absolute=100`) so the ball is + not smeared. +- **FLIR:** exposure and gain are in its Spinnaker node and change live. +- Field Configuration: **Lab Field → Apply**, then each camera's calibration, then colours per + camera. +- `./bin/graphicalClient` on the vision laptop shows what ssl-vision sends, before any network. + +On the robot laptop, in WSL: + +``` +pixi run python tools/vision_check.py +``` + +Expect each camera at about its frame rate and the field at 3000 x 2250 mm. Nothing at all +means the network or the firewall (steps 3-4 above, step 1 here); one camera missing means its +capture is not running. + +### 3. Radio + +Plug the dongle in, then in admin PowerShell (leave it open: it re-attaches after a replug): + +``` +usbipd attach --wsl --hardware-id --auto-attach +``` + +In WSL `ls /dev/ttyACM*` shows it; the code uses `PORT` in `utama_core/config/settings.py` +(`/dev/ttyACM0`). Then drive each robot by hand: + +``` +pixi run debug-robots +``` + +Number keys choose the robot by its **radio (firmware) ID**, WASD moves, Q/E turns, Shift is +fast (0.5 m/s, else 0.1), Space dribbles, B/C kick/chip, Esc stops every robot and quits. Hold +the robot off the floor for the first presses and check each key moves it the way it says. +Note each robot's radio ID and the pattern on top (its vision ID): the runner needs both. + +One radio drives every robot, of either colour: robots are addressed by radio ID only, so the +radio IDs must differ across all robots on it. + +### 4. Both together + +Set the "Test-day configuration" block of `examples/real_attacker_defender.py`: `FIELD`, +`CAMERA_HALF` (`"full"` when the cameras cover the whole field), each robot's vision and radio +ID and working hardware. Then `pixi run python examples/real_attacker_defender.py`, open +http://localhost:8080 and go through the Live tab's field check +([custom_referee_gui.md](custom_referee_gui.md#field-check-camera-runs)): every row green or +amber, each robot under its own ID and colour, a ball in each corner inside the area in play, +and a robot pushed by hand across the cameras' overlap moving smoothly (a jump there means the +two calibrations disagree). Only then Force Start. + +## When something fails + +| Symptom | Cause | +|---|---| +| `usbipd` in WSL talks about `linux-tools` packages | It is the Linux tool; run `usbipd` in PowerShell | +| `usbipd list` shows "Unknown USB device (device descriptor request failed)", `0000:0002` | Windows cannot read the device: other port, no hub, other cable | +| No `/dev/video*` after attaching a webcam | `sudo modprobe uvcvideo` | +| ssl-vision: "unable to obtain instance of camera" / permission | User not in `video` (or `flirimaging`); `newgrp video` | +| ssl-vision: "Could not set format" on `/dev/video1` | cam idx points at the metadata node: use the other one | +| `tee: .../usbfs_memory_mb: No such file` | WSL loads USB support only once a device is attached; `/etc/modprobe.d/usbfs.conf` with `options usbcore usbfs_memory_mb=1000` sets it then | +| Whole image tinted one colour | White balance stuck at an extreme (2800 K): auto, then lock | +| `vision_check` sees nothing | Different networks, Wi-Fi multicast, `.wslconfig` not mirrored, or the firewall rule missing | +| Dashboard: "Address already in use" | Another dashboard (`dashboard_server.py`) holds 8080: stop it | diff --git a/docs/setup_external.md b/docs/setup_external.md index 450f5e44b..fcb63d6df 100644 --- a/docs/setup_external.md +++ b/docs/setup_external.md @@ -50,31 +50,7 @@ cd AutoReferee && ./gradlew run Then run your own strategy. The GameController's web UI is at http://localhost:8081/#/match (this repo's dashboard is :8080). -## SSL Vision for real testing (WSL) +## SSL Vision and the radio for real robots -1. Connect the vision Linux laptop and your own laptop to the same network (an external - hotspot). -2. Allow inbound UDP packets through the vision port. With admin privileges, in PowerShell: - - ``` - New-NetFirewallRule -DisplayName "Allow Multicast UDP 10006" -Direction Inbound -Protocol UDP -LocalPort 10006 -Action Allow - ``` - -3. Type `%USERPROFILE%` into Windows + R and add a `.wslconfig` file there (file type - WSLCONFIG): - - ``` - [wsl2] - networkingMode=mirrored - ``` - -4. Restart WSL with `wsl --shutdown`, then check the connection: - - ``` - sudo tcpdump -i eth1 -n host 224.5.23.2 and udp port 10006 - ``` - - If you see UDP packets, everything is working. - -For the robot radio and controllers see -[`utama_core/team_controller/README.md`](../utama_core/team_controller/README.md). +The whole procedure (vision laptop, WSL networking and USB, radio, the checks before a robot +moves) is in [real_robot_setup.md](real_robot_setup.md). diff --git a/docs/tools.md b/docs/tools.md index a5424a407..f90dad12d 100644 --- a/docs/tools.md +++ b/docs/tools.md @@ -16,7 +16,7 @@ Every script in the repository, by what it is for. Run them from the repository | `build-planner` | build the C++ fast-path planner (`utama_core/motion_planning/src/fastpathplanning/cpp`) into `build/fpp` with cmake | | `replay [-n NAME] [-p]` | play a legacy pickle replay `replays/NAME.pkl` in the rSoccer viewer (newest if no name; `-p` step by step). Today's `.npz` replays open in the dashboard instead | | `runs` | list the tournament runs in `replays/`: start time, commit, matches, stalls, arguments | -| `debug-robots` | keyboard/click teleoperation GUI for real robots over the serial radio | +| `debug-robots` | keyboard/click teleoperation GUI for real robots over the serial radio (robots chosen by radio ID; [real_robot_setup.md](real_robot_setup.md)) | ## Worktrees @@ -113,6 +113,8 @@ described in [STRATEGY_DEVELOPMENT.md](STRATEGY_DEVELOPMENT.md). | `examples/demo_referee_feedback_gui.py` | The referee UI's controller-feedback panel with fake feedback, no hardware | | `examples/demo_exhibition_road.py` | The Exhibition Road festival demo on the 4 m x 3 m field, over rsim | | `examples/real_attacker_defender.py` | One attacker against one defender on real robots (or `--mode rsim`/`grsim`): field, camera half, IDs and each robot's working hardware in one config block; the Live tab's field check gates the start. Records every camera run as an SSL game log in `replays/field/`; `--calibrate attacker\|defender` plays fixed moves for measuring that robot | +| `tools/vision_check.py` | Does ssl-vision's multicast reach this machine: sender, packets per second per camera, detections and the field size, in 5 s with no radio or runner. The first check of a test day ([real_robot_setup.md](real_robot_setup.md)) | +| `tools/ssl-vision-lab-field.patch` | Not a script: adds a **Lab Field** preset (our 3 m x 2.25 m field, two cameras) to an ssl-vision checkout; `git apply` it there and rebuild | | `tools/robot_calibration.py` | A robot's numbers from a recorded field test (`replays/field/*.log`): loop delay, speed gain and acceleration per axis (the sim's move model fitted to the camera's positions), vision noise at rest and missed detections, kick speed. Writes `.calibration.json`. The same log converts to a replay with `tools/convert_ssl_log.py` | | `examples/demo_dribbler_test.py` | One robot dribbling round a rectangle on the Exhibition Road field | | `examples/demo_split_shape_match.py` | Visible grSim 6v6 of `split_shape` against itself | diff --git a/tools/ssl-vision-lab-field.patch b/tools/ssl-vision-lab-field.patch new file mode 100644 index 000000000..3231d5c72 --- /dev/null +++ b/tools/ssl-vision-lab-field.patch @@ -0,0 +1,117 @@ +diff --git a/src/shared/util/field.cpp b/src/shared/util/field.cpp +index 7eff4d0..8d59b8f 100644 +--- a/src/shared/util/field.cpp ++++ b/src/shared/util/field.cpp +@@ -371,6 +371,7 @@ RoboCupField::RoboCupField() { + updateShapes = new VarTrigger("Field Lines/Arcs","Update"); + applyDivisionA = new VarTrigger("Division A", "Apply"); + applyDivisionB = new VarTrigger("Division B", "Apply"); ++ applyLab = new VarTrigger("Lab Field", "Apply"); + var_num_lines = new VarInt("Number of Line Segments", 0); + var_num_arcs = new VarInt("Number of Arcs", 0); + field_lines_list = new VarList("Field Lines"); +@@ -381,6 +382,7 @@ RoboCupField::RoboCupField() { + settings->addChild(field_models->settings); + settings->addChild(applyDivisionA); + settings->addChild(applyDivisionB); ++ settings->addChild(applyLab); + settings->addChild(field_length); + settings->addChild(field_width); + settings->addChild(goal_width); +@@ -409,6 +411,7 @@ RoboCupField::RoboCupField() { + connect(updateShapes,SIGNAL(wasEdited(VarType*)),this,SLOT(updateFieldLinesAndArcs())); + connect(applyDivisionA,SIGNAL(wasEdited(VarType*)),this,SLOT(applyGeometryDivisionA())); + connect(applyDivisionB,SIGNAL(wasEdited(VarType*)),this,SLOT(applyGeometryDivisionB())); ++ connect(applyLab,SIGNAL(wasEdited(VarType*)),this,SLOT(applyGeometryLab())); + connect(var_num_lines, SIGNAL(wasEdited(VarType*)), + this, SLOT(ResizeFieldLines())); + connect(var_num_arcs, SIGNAL(wasEdited(VarType*)), +@@ -469,6 +472,7 @@ RoboCupField::~RoboCupField() { + delete field_arcs[i]; + } + delete updateShapes; ++ delete applyLab; + delete settings; + } + +@@ -512,6 +516,27 @@ void RoboCupField::applyGeometryDivisionB() { + num_cameras_local->setDouble(FieldConstantsRoboCup2018B::kNumCamerasLocal); + } + ++void RoboCupField::applyGeometryLab() { ++ field_length->setDouble(FieldConstantsLab::kFieldLength); ++ field_width->setDouble(FieldConstantsLab::kFieldWidth); ++ goal_width->setDouble(FieldConstantsLab::kGoalWidth); ++ goal_depth->setDouble(FieldConstantsLab::kGoalDepth); ++ goal_height->setDouble(FieldConstantsLab::kGoalHeight); ++ boundary_width->setDouble(FieldConstantsLab::kBoundaryWidth); ++ boundary_width_goal_line->setDouble(FieldConstantsLab::kBoundaryWidthGoalLine); ++ line_thickness->setDouble(FieldConstantsLab::kLineThickness); ++ penalty_area_depth->setDouble(FieldConstantsLab::kPenaltyAreaDepth); ++ penalty_area_width->setDouble(FieldConstantsLab::kPenaltyAreaWidth); ++ goal_center_to_penalty_mark->setDouble(FieldConstantsLab::kGoalLineToPenaltyMark); ++ center_circle_radius->setDouble(FieldConstantsLab::kCenterCircleRadius); ++ ball_radius->setDouble(FieldConstantsLab::kBallRadius); ++ max_robot_radius->setDouble(FieldConstantsLab::kMaxRobotRadius); ++ goal_substitution_area_width->setDouble(FieldConstantsLab::kGoalSubstitutionAreaWidth); ++ num_cameras_total->setDouble(FieldConstantsLab::kNumCamerasTotal); ++ num_cameras_local->setDouble(FieldConstantsLab::kNumCamerasLocal); ++ updateFieldLinesAndArcs(); ++} ++ + SSL_FieldShapeType RoboCupField::parseShapeType(const VarStringEnum* value) const { + std::string type = value->getString(); + if(shapeTypeMap.find(type) != shapeTypeMap.end()) { +diff --git a/src/shared/util/field.h b/src/shared/util/field.h +index e60066a..b9b6657 100644 +--- a/src/shared/util/field.h ++++ b/src/shared/util/field.h +@@ -167,6 +167,7 @@ protected: + VarTrigger* updateShapes; + VarTrigger* applyDivisionA; + VarTrigger* applyDivisionB; ++ VarTrigger* applyLab; + public: + VarList* getSettings() const { + return settings; +@@ -224,6 +225,7 @@ protected slots: + void updateFieldLinesAndArcs(); + void applyGeometryDivisionA(); + void applyGeometryDivisionB(); ++ void applyGeometryLab(); + }; + + +diff --git a/src/shared/util/field_default_constants.h b/src/shared/util/field_default_constants.h +index bcf52e5..095282a 100644 +--- a/src/shared/util/field_default_constants.h ++++ b/src/shared/util/field_default_constants.h +@@ -49,6 +49,28 @@ namespace FieldConstantsRoboCup2018A { + const int kNumCamerasLocal = 1; + } + ++// Our lab field (Utama): 3 m x 2.25 m, two cameras. Not a league field; the penalty ++// mark and centre circle are not marked on it. ++namespace FieldConstantsLab { ++ const double kFieldLength = 3000.0; ++ const double kFieldWidth = 2250.0; ++ const double kGoalWidth = 800.0; ++ const double kGoalDepth = 180.0; ++ const double kGoalHeight = 155.0; ++ const double kBoundaryWidth = 300.0; ++ const double kBoundaryWidthGoalLine = 300.0; ++ const double kGoalSubstitutionAreaWidth = 0.0; ++ const double kLineThickness = 10.0; ++ const double kPenaltyAreaDepth = 500.0; ++ const double kPenaltyAreaWidth = 1000.0; ++ const double kCenterCircleRadius = 300.0; ++ const double kGoalLineToPenaltyMark = 1000.0; ++ const double kBallRadius = 21.5; ++ const double kMaxRobotRadius = 90.0; ++ const int kNumCamerasTotal = 2; ++ const int kNumCamerasLocal = 2; ++} ++ + namespace FieldConstantsRoboCup2018B { + const double kFieldLength = 9000.0; + const double kFieldWidth = 6000.0; diff --git a/tools/vision_check.py b/tools/vision_check.py new file mode 100644 index 000000000..b12ab77d3 --- /dev/null +++ b/tools/vision_check.py @@ -0,0 +1,72 @@ +# ruff: noqa: E402 +"""Does ssl-vision's multicast reach this machine? Listens for a few seconds, with no radio or +runner, and prints who sent it, packets per second per camera, what was detected and the field +size ssl-vision announces. The first check on a test day (`docs/real_robot_setup.md`). + + pixi run python tools/vision_check.py [--seconds 5] +""" + +import argparse +import collections +import socket +import struct +import sys +import time +from pathlib import Path + +REPO_ROOT = Path(__file__).resolve().parents[1] +if str(REPO_ROOT) not in sys.path: + sys.path.insert(0, str(REPO_ROOT)) + +from utama_core.config.settings import MULTICAST_GROUP, VISION_PORT +from utama_core.team_controller.src.generated_code.ssl_vision_wrapper_pb2 import ( + SSL_WrapperPacket, +) + + +def main() -> None: + parser = argparse.ArgumentParser(description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter) + parser.add_argument("--seconds", type=float, default=5.0) + seconds = parser.parse_args().seconds + + sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) + sock.bind((MULTICAST_GROUP, VISION_PORT)) + group = struct.pack("4sL", socket.inet_aton(MULTICAST_GROUP), socket.INADDR_ANY) + sock.setsockopt(socket.IPPROTO_IP, socket.IP_ADD_MEMBERSHIP, group) + sock.settimeout(0.5) + + per_camera, seen, senders, field = collections.Counter(), collections.Counter(), set(), None + end = time.time() + seconds + while time.time() < end: + try: + data, sender = sock.recvfrom(65536) + except socket.timeout: + continue + senders.add(sender[0]) + packet = SSL_WrapperPacket() + packet.ParseFromString(data) + if packet.HasField("detection"): + d = packet.detection + per_camera[d.camera_id] += 1 + seen["yellow"] += len(d.robots_yellow) + seen["blue"] += len(d.robots_blue) + seen["balls"] += len(d.balls) + if packet.HasField("geometry"): + field = (packet.geometry.field.field_length, packet.geometry.field.field_width) + + if not senders: + print(f"Nothing on {MULTICAST_GROUP}:{VISION_PORT} in {seconds:.0f} s: see docs/real_robot_setup.md, 'Vision'.") + sys.exit(1) + print(f"From: {', '.join(sorted(senders))}") + for camera, n in sorted(per_camera.items()): + print(f"Camera {camera}: {n / seconds:.0f} packets/s") + print( + "Detections per packet: " + + ", ".join(f"{k} {v / max(1, sum(per_camera.values())):.1f}" for k, v in seen.items()) + ) + print(f"Field: {field[0]:.0f} x {field[1]:.0f} mm" if field else "Field: no geometry packet yet") + + +if __name__ == "__main__": + main() From 34c3f1bb060cb21bd32ad909ee2cc141cf51feec Mon Sep 17 00:00:00 2001 From: isaac0804 Date: Thu, 22 Oct 2026 19:05:13 +0100 Subject: [PATCH 37/56] examples(real_attacker_defender): the lab field, 3 m x 2.25 m Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_01TJGHdb8Dht67hn4Lohndf5 --- examples/real_attacker_defender.py | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/examples/real_attacker_defender.py b/examples/real_attacker_defender.py index b245fc22b..453331ca5 100644 --- a/examples/real_attacker_defender.py +++ b/examples/real_attacker_defender.py @@ -92,13 +92,14 @@ MODE = "real" # "real", "rsim" or "grsim"; --mode overrides it -# PLACEHOLDER: measure the real field. Every value is a half-size in metres -# (half length, half width, ...), as in `STANDARD_FIELD_DIMS`. +# The lab field: 3 m x 2.25 m, goal 0.8 m wide, defense area 1 m wide x 0.5 m deep. Every +# value is a half-size in metres (half length, half width, ...), as in `STANDARD_FIELD_DIMS`; +# ssl-vision's field configuration must match it (lengths there are full sizes, in mm). FIELD = FieldDimensions( - full_field_half_length=2.0, - full_field_half_width=1.5, - half_defense_area_depth=0.3, - half_defense_area_width=0.6, + full_field_half_length=1.5, + full_field_half_width=1.125, + half_defense_area_depth=0.25, + half_defense_area_width=0.5, half_goal_width=0.4, center_circle_radius=0.3, ) From c577b79ee4949ed96955562b86e0b2a9b7047f7d Mon Sep 17 00:00:00 2001 From: isaac0804 Date: Thu, 22 Oct 2026 19:44:07 +0100 Subject: [PATCH 38/56] tools(vision_check): each robot and ball where each camera puts it, and the gap Two cameras over the whole field disagreed on robot positions (graphicalClient showed them jumping between two spots); the gap in mm shows whether a calibration change helped. Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_01TJGHdb8Dht67hn4Lohndf5 --- tools/vision_check.py | 19 +++++++++++++++++++ 1 file changed, 19 insertions(+) diff --git a/tools/vision_check.py b/tools/vision_check.py index b12ab77d3..b86e983a9 100644 --- a/tools/vision_check.py +++ b/tools/vision_check.py @@ -3,6 +3,10 @@ runner, and prints who sent it, packets per second per camera, what was detected and the field size ssl-vision announces. The first check on a test day (`docs/real_robot_setup.md`). +Each robot and ball is also listed where each camera puts it (mean over the run) and, when +several cameras see it, the largest gap between them: cameras whose calibrations agree are a few +cm apart. Rerun after each calibration change. + pixi run python tools/vision_check.py [--seconds 5] """ @@ -12,6 +16,7 @@ import struct import sys import time +from itertools import combinations from pathlib import Path REPO_ROOT = Path(__file__).resolve().parents[1] @@ -37,6 +42,7 @@ def main() -> None: sock.settimeout(0.5) per_camera, seen, senders, field = collections.Counter(), collections.Counter(), set(), None + positions = collections.defaultdict(list) # (thing, camera) -> [(x, y)] in mm end = time.time() + seconds while time.time() < end: try: @@ -52,6 +58,11 @@ def main() -> None: seen["yellow"] += len(d.robots_yellow) seen["blue"] += len(d.robots_blue) seen["balls"] += len(d.balls) + for colour, robots in (("yellow", d.robots_yellow), ("blue", d.robots_blue)): + for r in robots: + positions[(f"{colour} {r.robot_id}", d.camera_id)].append((r.x, r.y)) + if len(d.balls) == 1: # with several balls in one image, which is which is unknown + positions[("ball", d.camera_id)].append((d.balls[0].x, d.balls[0].y)) if packet.HasField("geometry"): field = (packet.geometry.field.field_length, packet.geometry.field.field_width) @@ -67,6 +78,14 @@ def main() -> None: ) print(f"Field: {field[0]:.0f} x {field[1]:.0f} mm" if field else "Field: no geometry packet yet") + by_thing = collections.defaultdict(dict) + for (thing, camera), xys in positions.items(): + by_thing[thing][camera] = tuple(sum(c) / len(xys) for c in zip(*xys)) + for thing, cameras in sorted(by_thing.items()): + where = ", ".join(f"camera {c} ({x:.0f}, {y:.0f})" for c, (x, y) in sorted(cameras.items())) + gaps = [((a[0] - b[0]) ** 2 + (a[1] - b[1]) ** 2) ** 0.5 for a, b in combinations(cameras.values(), 2)] + print(f"{thing}: {where}" + (f", {max(gaps):.0f} mm apart" if gaps else "")) + if __name__ == "__main__": main() From eb8c3d21bc8ae832008466f604b407491765edf5 Mon Sep 17 00:00:00 2001 From: isaac0804 Date: Thu, 22 Oct 2026 19:57:04 +0100 Subject: [PATCH 39/56] fix(vision): weight each camera by how steeply it sees a robot; drop one that disagrees Two cameras over the whole lab field (2026-10-22), each behind a goal, put a robot 17 cm apart at the far end; CameraCombiner averaged them, so the robot sat 9 cm off both and jumped 9 cm whenever a camera missed a frame. The receiver now keeps each camera's position from ssl-vision's geometry calibrations and stamps it on that camera's frames; the combiner weights each detection by cos^4 of its camera's view angle and drops one further than CAMERA_AGREEMENT (0.1 m) from the best-placed camera's. Without positions (rsim, grSim, an uncalibrated camera) every camera counts the same, as before. Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_01TJGHdb8Dht67hn4Lohndf5 --- utama_core/config/settings.py | 2 + .../receivers/vision_receiver.py | 13 +++- .../data_processing/refiners/position.py | 68 ++++++++++++------- utama_core/entities/data/raw_vision.py | 4 +- .../receivers/vision_receiver_unit_test.py | 30 ++++++++ .../tests/refiners/camera_combiner_test.py | 47 +++++++++++++ 6 files changed, 138 insertions(+), 26 deletions(-) diff --git a/utama_core/config/settings.py b/utama_core/config/settings.py index 9b2477223..00fe7fada 100644 --- a/utama_core/config/settings.py +++ b/utama_core/config/settings.py @@ -46,6 +46,8 @@ ### Refiners ### BALL_MERGE_THRESHOLD = 0.05 # CameraCombiner: distance threshold to merge balls (m) +# CameraCombiner: a robot's detection further than this from the best-placed camera's is dropped (m) +CAMERA_AGREEMENT = 0.1 VISION_BOUNDS_BUFFER = 1.0 # CameraCombiner: buffer around field bounds to include in vision (m) # An unseen ball is assumed to roll on at this deceleration (m/s^2): official Div B logs, 1 s or # more after a kick (docs/sim_realism.md). A field's own value comes from its free-rolling balls. diff --git a/utama_core/data_processing/receivers/vision_receiver.py b/utama_core/data_processing/receivers/vision_receiver.py index 5d533171a..12467b076 100644 --- a/utama_core/data_processing/receivers/vision_receiver.py +++ b/utama_core/data_processing/receivers/vision_receiver.py @@ -1,6 +1,6 @@ import logging import threading -from typing import Callable, Deque, List, Optional +from typing import Callable, Deque, Dict, List, Optional, Tuple from utama_core.config.settings import MULTICAST_GROUP, VISION_PORT from utama_core.entities.data.raw_vision import RawBallData, RawRobotData, RawVisionData @@ -29,6 +29,8 @@ def __init__( self._geometry_fired = False self._stop_event = stop_event self._on_packet = on_packet # every packet as received, before parsing (`GameLogWriter`) + # camera id -> (x, y, height) in m, from every geometry packet's calibrations + self._camera_positions: Dict[int, Tuple[float, float, float]] = {} # Parked exception from the geometry callback — re-raised in the main # thread by StrategyRunner._run_step() so the process exits cleanly. self.thread_exception: Optional[BaseException] = None @@ -70,6 +72,14 @@ def _handle_packet(self, vision_packet: SSL_WrapperPacket, data: bytes) -> bool: self._stop_event.set() return False self._geometry_fired = True + if vision_packet.HasField("geometry"): + for calib in vision_packet.geometry.calib: + if calib.HasField("derived_camera_world_tz"): + self._camera_positions[calib.camera_id] = ( + calib.derived_camera_world_tx / 1000, + calib.derived_camera_world_ty / 1000, + calib.derived_camera_world_tz / 1000, + ) # SSL-Vision and grSim send geometry in packets of its own: an absent detection reads as an # empty camera-0 frame captured at t=0, which would drag the combined frame's timestamp down. if vision_packet.HasField("detection"): @@ -104,4 +114,5 @@ def _process_packet(self, detection_frame: object): # detection_frame = protobu for ball in detection_frame.balls ], camera_id=detection_frame.camera_id, + camera_position=self._camera_positions.get(detection_frame.camera_id), ) diff --git a/utama_core/data_processing/refiners/position.py b/utama_core/data_processing/refiners/position.py index 9cd52ff5e..8b6ff19c4 100644 --- a/utama_core/data_processing/refiners/position.py +++ b/utama_core/data_processing/refiners/position.py @@ -1,3 +1,4 @@ +import math from collections import defaultdict from dataclasses import dataclass, replace from functools import partial @@ -6,7 +7,11 @@ import numpy as np from utama_core.config.field_params import FieldDimensions -from utama_core.config.settings import BALL_MERGE_THRESHOLD, VISION_BOUNDS_BUFFER +from utama_core.config.settings import ( + BALL_MERGE_THRESHOLD, + CAMERA_AGREEMENT, + VISION_BOUNDS_BUFFER, +) from utama_core.data_processing.refiners.base_refiner import BaseRefiner from utama_core.data_processing.refiners.filters.kalman import ( KalmanFilter, @@ -383,6 +388,14 @@ def filter_running(self) -> bool: class CameraCombiner: + """Merges the cameras' latest frames into one. A robot seen by several cameras is placed at + the average of their detections, each weighted by how steeply its camera looks at the robot + when ssl-vision sends where the cameras are: a camera's error on the floor grows as + 1/cos^2 of its view angle (a pixel covers more floor, a wrong robot height shifts the robot + more), so the weight is cos^4. A detection further than `CAMERA_AGREEMENT` from the + best-placed camera's is dropped, so a camera that disagrees (a poor calibration far from it) + does not pull the robot away. Without camera positions every camera counts the same.""" + def combine_cameras( self, frames: List[RawVisionData], @@ -399,7 +412,7 @@ def combine_cameras( """ ts = [] - # maps robot id to list of frames seen for that robot + # maps robot id to the (detection, its camera's position) of every camera that saw it yellow_captured = defaultdict(list) blue_captured = defaultdict(list) balls_captured = defaultdict(list) @@ -408,11 +421,11 @@ def combine_cameras( for frame_ind, frame in enumerate(frames): for yr in frame.yellow_robots: if self._bounds_check(yr.x, yr.y, bounds): - yellow_captured[yr.id].append(yr) + yellow_captured[yr.id].append((yr, frame.camera_position)) for br in frame.blue_robots: if self._bounds_check(br.x, br.y, bounds): - blue_captured[br.id].append(br) + blue_captured[br.id].append((br, frame.camera_position)) for b in frame.balls: if self._bounds_check(b.x, b.y, bounds): @@ -426,28 +439,35 @@ def combine_cameras( return VisionData(sum(ts) / len(ts), avg_yellows, avg_blues, balls) - def _avg_robots(self, rs: List[RawRobotData]) -> Optional[VisionRobotData]: + @staticmethod + def _view_weight(x: float, y: float, camera: Optional[Tuple[float, float, float]]) -> float: + """cos^4 of the angle from the vertical at which `camera` sees (`x`, `y`); 1 when unknown.""" + if camera is None: + return 1.0 + cx, cy, height = camera + h2 = height * height + return (h2 / (h2 + (x - cx) ** 2 + (y - cy) ** 2)) ** 2 + + def _avg_robots(self, seen: List[Tuple[RawRobotData, Optional[Tuple[float, float, float]]]]) -> VisionRobotData: # All these robots should have the same id - if not rs: - return None + rs = [r for r, _ in seen] base_id = rs[0].id - tx, ty, tc = 0, 0, 0 - - sum_orientation_x_component = 0.0 - sum_orientation_y_component = 0.0 - for r in rs: - assert base_id == r.id - tx += r.x - ty += r.y - sum_orientation_x_component += np.cos(r.orientation) - sum_orientation_y_component += np.sin(r.orientation) - tc += r.confidence - - avg_orientation_x = sum_orientation_x_component / len(rs) - avg_orientation_y = sum_orientation_y_component / len(rs) - avg_orientation = np.atan2(avg_orientation_y, avg_orientation_x) - - return VisionRobotData(base_id, tx / len(rs), ty / len(rs), avg_orientation) + assert all(r.id == base_id for r in rs) + if all(camera is not None for _, camera in seen): + weights = [self._view_weight(r.x, r.y, camera) for r, camera in seen] + best = rs[int(np.argmax(weights))] + kept = [(r, w) for r, w in zip(rs, weights) if math.hypot(r.x - best.x, r.y - best.y) <= CAMERA_AGREEMENT] + else: + kept = [(r, 1.0) for r in rs] + + total = sum(w for _, w in kept) + x = sum(r.x * w for r, w in kept) / total + y = sum(r.y * w for r, w in kept) / total + avg_orientation = math.atan2( + sum(math.sin(r.orientation) * w for r, w in kept), + sum(math.cos(r.orientation) * w for r, w in kept), + ) + return VisionRobotData(base_id, x, y, avg_orientation) def _combine_balls_by_proximity(self, bs: Dict[int, List[RawBallData]]) -> List[VisionBallData]: combined_balls: List[VisionBallData] = [] diff --git a/utama_core/entities/data/raw_vision.py b/utama_core/entities/data/raw_vision.py index f173af3db..d42ce05c0 100644 --- a/utama_core/entities/data/raw_vision.py +++ b/utama_core/entities/data/raw_vision.py @@ -1,5 +1,5 @@ from dataclasses import dataclass -from typing import List +from typing import List, Optional, Tuple # Unit: m @@ -28,3 +28,5 @@ class RawVisionData: blue_robots: List[RawRobotData] balls: List[RawBallData] camera_id: int + # Where that camera is (x, y, height), from ssl-vision's calibration; None when not sent (rsim, grSim). + camera_position: Optional[Tuple[float, float, float]] = None diff --git a/utama_core/tests/receivers/vision_receiver_unit_test.py b/utama_core/tests/receivers/vision_receiver_unit_test.py index b50e29c67..4531c9fee 100644 --- a/utama_core/tests/receivers/vision_receiver_unit_test.py +++ b/utama_core/tests/receivers/vision_receiver_unit_test.py @@ -146,3 +146,33 @@ def test_geometry_only_packet_adds_no_frame(): assert v._handle_packet(SSL_WrapperPacket(), with_detection.SerializeToString()) assert len(buffers[0]) == 0 assert buffers[1][0].ts == 5.0 + + +def test_a_cameras_position_from_geometry_rides_on_its_frames(): + # CameraCombiner weights each camera by how steeply it sees a robot: it needs where ssl-vision + # calibrated each camera to be, which only the geometry packets carry. + buffers = [deque(maxlen=1) for _ in range(2)] + v = VisionReceiver(buffers) + geometry = SSL_WrapperPacket() + field = geometry.geometry.field + field.field_length, field.field_width, field.goal_width, field.goal_depth, field.boundary_width = ( + 3000, + 2250, + 800, + 180, + 300, + ) + calib = geometry.geometry.calib.add() + calib.camera_id, calib.focal_length, calib.principal_point_x, calib.principal_point_y = 0, 700, 640, 480 + calib.distortion, calib.q0, calib.q1, calib.q2, calib.q3, calib.tx, calib.ty, calib.tz = (0,) * 8 + calib.derived_camera_world_tx, calib.derived_camera_world_ty, calib.derived_camera_world_tz = -1728, -57, 2078 + assert v._handle_packet(SSL_WrapperPacket(), geometry.SerializeToString()) + + for camera_id in (0, 1): + packet = SSL_WrapperPacket() + d = packet.detection + d.camera_id, d.t_capture, d.t_sent, d.frame_number = camera_id, 1.0, 1.0, 1 + assert v._handle_packet(SSL_WrapperPacket(), packet.SerializeToString()) + + assert buffers[0][0].camera_position == (-1.728, -0.057, 2.078) + assert buffers[1][0].camera_position is None # camera 1 sent no calibration diff --git a/utama_core/tests/refiners/camera_combiner_test.py b/utama_core/tests/refiners/camera_combiner_test.py index feb641324..38ada4cac 100644 --- a/utama_core/tests/refiners/camera_combiner_test.py +++ b/utama_core/tests/refiners/camera_combiner_test.py @@ -159,3 +159,50 @@ def test_only_in_bounds_camera_contributes_to_average(): test_combine_with_both_camera_empty_gives_empty() test_combine_proximity_multiple_balls() test_combine_filters_low_confidence() + + +# Two cameras over the whole lab field, each behind one goal (as on 2026-10-22). +CAMERA_LEFT, CAMERA_RIGHT = (-1.73, 0.0, 2.08), (1.73, 0.0, 2.08) + + +def _robot_seen_at(left_xy, right_xy, left=CAMERA_LEFT, right=CAMERA_RIGHT): + frames = [ + RawVisionData(0, [RawRobotData(2, *left_xy, 0, 1)], [], [], 0, left), + RawVisionData(0, [RawRobotData(2, *right_xy, 0, 1)], [], [], 1, right), + ] + return CameraCombiner().combine_cameras(frames, infinite_bounds).yellow_robots[0] + + +def test_cameras_that_disagree_give_the_robot_where_the_steeper_camera_sees_it(): + # The 2026-10-22 test: at x = 0.8 the camera behind the far goal put a robot 17 cm from where + # the camera above that end did, and the average left it 9 cm off both, jumping 9 cm whenever + # a camera missed a frame. Beyond CAMERA_AGREEMENT (0.1 m) only the steeper camera counts. + robot = _robot_seen_at((0.80, 0.07), (0.97, 0.13)) + assert math.isclose(robot.x, 0.97) and math.isclose(robot.y, 0.13) + + robot = _robot_seen_at((0.80, 0.07), (0.889, 0.07)) # 8.9 cm apart: both count + assert 0.80 < robot.x < 0.889 + + +def test_cameras_that_agree_are_weighted_towards_the_steeper_one(): + robot = _robot_seen_at((0.80, 0.0), (0.86, 0.0)) + assert robot.x > 0.83 # past the plain average, towards the camera above that end + + robot = _robot_seen_at((-0.03, 0.0), (0.03, 0.0)) # halfway between the cameras: equal weight + assert math.isclose(robot.x, 0.0, abs_tol=1e-12) + + +def test_without_camera_positions_every_camera_counts_the_same(): + robot = _robot_seen_at((0.80, 0.0), (0.97, 0.0), left=None, right=None) + assert math.isclose(robot.x, 0.885) + robot = _robot_seen_at((0.80, 0.0), (0.97, 0.0), right=None) # one camera unknown: no weighting + assert math.isclose(robot.x, 0.885) + + +def test_robot_orientation_is_the_weighted_circular_mean(): + frames = [ + RawVisionData(0, [RawRobotData(2, 0.0, 0.0, math.pi - 0.1, 1)], [], [], 0, CAMERA_LEFT), + RawVisionData(0, [RawRobotData(2, 0.0, 0.0, -math.pi + 0.1, 1)], [], [], 1, CAMERA_RIGHT), + ] + robot = CameraCombiner().combine_cameras(frames, infinite_bounds).yellow_robots[0] + assert math.isclose(abs(robot.orientation), math.pi) From 357e1028982be5975222963708be32a0844aebd8 Mon Sep 17 00:00:00 2001 From: isaac0804 Date: Thu, 22 Oct 2026 20:03:40 +0100 Subject: [PATCH 40/56] examples: real_score_goal, one robot alone takes the ball and scores For a test day with a single robot ready. Field and camera view come from real_attacker_defender.py so the two agree; starts in HALT with the field check and dashboard as there. Scores in rsim within 5 s of Force Start. Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_01TJGHdb8Dht67hn4Lohndf5 --- examples/real_score_goal.py | 95 +++++++++++++++++++ .../examples/test_examples_run_as_scripts.py | 2 +- 2 files changed, 96 insertions(+), 1 deletion(-) create mode 100644 examples/real_score_goal.py diff --git a/examples/real_score_goal.py b/examples/real_score_goal.py new file mode 100644 index 000000000..ec1540511 --- /dev/null +++ b/examples/real_score_goal.py @@ -0,0 +1,95 @@ +# ruff: noqa: E402 +"""examples/real_score_goal.py — one robot, no opponent: it takes the ball and scores. + +Run: + pixi run python examples/real_score_goal.py # real robot + pixi run python examples/real_score_goal.py --mode rsim + # the dashboard is at http://localhost:8080: press Force Start to play, Halt to freeze + +The field and the camera's view are `FIELD` and `CAMERA_HALF` in `real_attacker_defender.py`, +so the two examples always agree; the robot is set below. It attacks the goal at +x (the right +one in ssl-vision's coordinates) with `LeadAndSupportTactic`: alone, it goes to the ball, turns +to the open corner and shoots. It starts in HALT, and the field check gates Force Start as in +`real_attacker_defender.py`. Put the ball back and Force Start again for the next shot. +""" + +import sys +from pathlib import Path + +sys.path.insert(0, str(Path(__file__).resolve().parents[1])) + +import argparse +import time + +from real_attacker_defender import ( + CAMERA_HALF, + DASHBOARD_PORT, + FIELD, + RECORD, + TestRobot, + _field_bounds, + _one_robot_strategy, + _referee_profile, +) + +from utama_core.custom_referee import CustomReferee +from utama_core.dashboard import attach_dashboard +from utama_core.dashboard.views import referee as referee_view +from utama_core.entities.referee.referee_command import RefereeCommand +from utama_core.run import StrategyRunner +from utama_core.tactics.lead_and_support import LeadAndSupportTactic + +# --------------------------------------------------------------------------- +# Test-day configuration +# --------------------------------------------------------------------------- + +SHOOTER_COLOUR = "blue" +SHOOTER = TestRobot(vision_id=2, firmware_id=0) + +# --------------------------------------------------------------------------- + + +def build_runner(mode: str) -> StrategyRunner: + real = mode == "real" + shooter_id = SHOOTER.vision_id if real else 0 # in the simulators robots are numbered from 0 + is_yellow = SHOOTER_COLOUR == "yellow" + if SHOOTER_COLOUR not in ("yellow", "blue"): + raise ValueError(f"SHOOTER_COLOUR must be 'yellow' or 'blue', got {SHOOTER_COLOUR!r}") + mapping = {SHOOTER.vision_id: SHOOTER.firmware_id} if real else None + + profile = _referee_profile() + referee = CustomReferee(profile, n_robots_yellow=int(is_yellow), n_robots_blue=int(not is_yellow)) + server = attach_dashboard(port=DASHBOARD_PORT) + referee_view.attach(server, referee, profile) + + game_log = f"replays/field/{time.strftime('%Y%m%d_%H%M%S')}_{mode}_score_goal.log" if RECORD and real else None + if game_log: + print(f"Recording to {game_log}") + return StrategyRunner( + game_log_path=game_log, + strategy=_one_robot_strategy(shooter_id, LeadAndSupportTactic), + my_team_is_yellow=is_yellow, + my_team_is_right=False, # own goal at -x: it attacks the goal at +x + mode=mode, + exp_friendly=1, + exp_enemy=0, + full_field_dims=FIELD, + field_bounds=_field_bounds(FIELD, CAMERA_HALF), + referee=referee, + referee_initial_command=RefereeCommand.HALT, + show_live_status=not real, + yellow_vision_to_cmd_mapping=mapping if is_yellow else None, + blue_vision_to_cmd_mapping=None if is_yellow else mapping, + yellow_robot_profiles={shooter_id: SHOOTER.profile} if is_yellow else None, + blue_robot_profiles=None if is_yellow else {shooter_id: SHOOTER.profile}, + ) + + +def main() -> None: + parser = argparse.ArgumentParser(description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter) + parser.add_argument("--mode", choices=("real", "rsim", "grsim"), default="real") + build_runner(parser.parse_args().mode).run() + + +if __name__ == "__main__": + main() diff --git a/utama_core/tests/examples/test_examples_run_as_scripts.py b/utama_core/tests/examples/test_examples_run_as_scripts.py index 9428cedbf..a2123af1a 100644 --- a/utama_core/tests/examples/test_examples_run_as_scripts.py +++ b/utama_core/tests/examples/test_examples_run_as_scripts.py @@ -13,7 +13,7 @@ EXAMPLES = Path(__file__).resolve().parents[3] / "examples" -@pytest.mark.parametrize("name", ["real_attacker_defender.py", "demo_referee_feedback_gui.py"]) +@pytest.mark.parametrize("name", ["real_attacker_defender.py", "real_score_goal.py", "demo_referee_feedback_gui.py"]) def test_an_example_with_options_prints_its_help_run_by_path(name, tmp_path): done = subprocess.run( [sys.executable, str(EXAMPLES / name), "--help"], cwd=tmp_path, capture_output=True, text=True, timeout=120 From 0c146de596726fed5dcafc0554491ef46a72ab65 Mon Sep 17 00:00:00 2001 From: isaac0804 Date: Thu, 22 Oct 2026 20:17:04 +0100 Subject: [PATCH 41/56] fix(run): a team alone in real mode tracks only its own mapped robots, whatever its colour The roster allowlists assumed our team was yellow: with our blue robot alone (real_score_goal.py, 2026-10-22) blue went unfiltered and yellow was filtered, so misread patterns (blue 1, blue 5 beside our blue 2) became our robots and lived on in the Kalman filter as ghosts. A colour we drive is now filtered to its mapping's IDs; the opponent's colour without an opponent strategy stays unfiltered. The field check still reads raw vision, so it keeps listing such detections as 'not configured'. Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_01TJGHdb8Dht67hn4Lohndf5 --- utama_core/run/strategy_runner.py | 32 +++++++++++++++---- .../strategy_runner/test_runner_misconfig.py | 22 +++++++++++-- 2 files changed, 44 insertions(+), 10 deletions(-) diff --git a/utama_core/run/strategy_runner.py b/utama_core/run/strategy_runner.py index fdc720b1d..9d546748d 100644 --- a/utama_core/run/strategy_runner.py +++ b/utama_core/run/strategy_runner.py @@ -408,13 +408,12 @@ def __init__( self.yellow_vision_to_cmd_mapping, self.blue_vision_to_cmd_mapping ) - # Derive per-color roster allowlists from the validated mappings (real mode only). - # Any robot ID seen by vision that is not in the allowlist is silently dropped so that - # stray detections from robots not in play never pollute the game state. - # Blue filtering is only applied when there is an opponent strategy — in single-team - # mode blue robots are tracked as enemies and must not be filtered out. - _allowed_yellow = frozenset(self.yellow_vision_to_cmd_mapping) or None - _allowed_blue = (frozenset(self.blue_vision_to_cmd_mapping) or None) if opp_strategy is not None else None + _allowed_yellow, _allowed_blue = self._roster_allowlists( + self.yellow_vision_to_cmd_mapping, + self.blue_vision_to_cmd_mapping, + my_team_is_yellow, + has_opp=opp_strategy is not None, + ) self.my, self.opp = self._setup_sides_data( strategy, @@ -533,6 +532,25 @@ def __init__( self.profiler_name = profiler_name self.profiler = cProfile.Profile() if profiler_name else None + @staticmethod + def _roster_allowlists( + yellow_mapping: dict[int, int], blue_mapping: dict[int, int], my_team_is_yellow: bool, has_opp: bool + ) -> Tuple[Optional[frozenset[int]], Optional[frozenset[int]]]: + """Per-colour roster allowlists from the validated mappings (real mode only; empty + elsewhere, so None). A robot ID vision reports that is not in its colour's allowlist is + dropped, so a misread pattern (a ghost blue 5 beside our blue 2) never becomes a robot + the Kalman filter then keeps alive. A colour we drive (ours, and the opponent's with an + opponent strategy) is filtered; the opponent's colour without one is the enemies we + track, unfiltered.""" + + def allowlist(mapping: dict[int, int], driven: bool) -> Optional[frozenset[int]]: + return (frozenset(mapping) or None) if driven else None + + return ( + allowlist(yellow_mapping, my_team_is_yellow or has_opp), + allowlist(blue_mapping, not my_team_is_yellow or has_opp), + ) + def _validate_vision_to_cmd_mapping(self, mapping: Optional[dict[int, int]], is_yellow: bool) -> dict[int, int]: if self.mode == Mode.REAL: is_my_team_color = is_yellow == self.my_team_is_yellow diff --git a/utama_core/tests/strategy_runner/test_runner_misconfig.py b/utama_core/tests/strategy_runner/test_runner_misconfig.py index 679784c9b..201ca43b9 100644 --- a/utama_core/tests/strategy_runner/test_runner_misconfig.py +++ b/utama_core/tests/strategy_runner/test_runner_misconfig.py @@ -495,9 +495,25 @@ def test_blue_allowlist_not_set_in_single_team_real_mode(): # Validate mapping returns {} for blue (no opp, mapping=None) blue_mapping = StrategyRunner._validate_vision_to_cmd_mapping(runner, None, is_yellow=False) assert blue_mapping == {} - # Simulate the allowlist derivation: empty mapping with no opp → None, not frozenset() - _allowed_blue = (frozenset(blue_mapping) or None) if runner.opp is not None else None - assert _allowed_blue is None + assert StrategyRunner._roster_allowlists({0: 3}, blue_mapping, my_team_is_yellow=True, has_opp=False) == ( + frozenset({0}), + None, + ) + + +def test_a_blue_team_alone_tracks_only_its_own_robots(): + # 2026-10-22, real_score_goal.py with our blue 2 alone: misread patterns came through as blue 1 + # and blue 5, became our robots and lived on in the Kalman filter as ghosts. The allowlists + # assumed our team was yellow, so a blue team alone was never filtered (and yellow was). + assert StrategyRunner._roster_allowlists({}, {2: 0}, my_team_is_yellow=False, has_opp=False) == ( + None, + frozenset({2}), + ) + # Both teams driven: both filtered. + assert StrategyRunner._roster_allowlists({1: 1}, {2: 0}, my_team_is_yellow=False, has_opp=True) == ( + frozenset({1}), + frozenset({2}), + ) def test_trusted_ir_robots_both_teams_pvp(): From 08e2acf4a7d6a44bd9f473ef24b9380f0e2c6da6 Mon Sep 17 00:00:00 2001 From: isaac0804 Date: Thu, 22 Oct 2026 20:23:53 +0100 Subject: [PATCH 42/56] examples: real_dribble_and_shoot; fix(dribble): letting go backs away instead of waiting DribbleTactic stood still after releasing the ball until it was 13 cm away; a ball that stopped at 11 cm froze the robot for good (rsim, 2026-10-22). It now backs off at 0.2 m/s until the ball is clear. real_dribble_and_shoot: one robot dribbles a lap, then LeadAndSupportTactic shoots; real_score_goal's build_runner takes the tactics, offered in the Strategy panel. In rsim the robot still stalls on the second segment with the ball (the planner's command shrinks to ~0, DribbleTactic's known rsim issue); untested on a real robot. Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_01TJGHdb8Dht67hn4Lohndf5 --- examples/real_dribble_and_shoot.py | 73 +++++++++++++++++++ examples/real_score_goal.py | 23 ++++-- utama_core/tactics/dribble.py | 5 +- .../tests/engine/test_dribble_tactic.py | 45 ++++++++++++ .../examples/test_examples_run_as_scripts.py | 5 +- 5 files changed, 144 insertions(+), 7 deletions(-) create mode 100644 examples/real_dribble_and_shoot.py create mode 100644 utama_core/tests/engine/test_dribble_tactic.py diff --git a/examples/real_dribble_and_shoot.py b/examples/real_dribble_and_shoot.py new file mode 100644 index 000000000..a0d8d19a9 --- /dev/null +++ b/examples/real_dribble_and_shoot.py @@ -0,0 +1,73 @@ +# ruff: noqa: E402 +"""examples/real_dribble_and_shoot.py — one robot dribbles a lap of the field, then scores. + +Run: + pixi run python examples/real_dribble_and_shoot.py # real robot + pixi run python examples/real_dribble_and_shoot.py --mode rsim + # the dashboard is at http://localhost:8080: press Force Start to play, Halt to freeze + +Everything else is `real_score_goal.py`: the robot set there (`SHOOTER`), the field and camera +view from `real_attacker_defender.py`, HALT at start, the field check. The robot fetches the +ball and dribbles it corner to corner round a rectangle at 55% of the field (`DribbleTactic`, +letting go and taking the ball back every 0.8 m, inside the 1 m dribbling rule), and after +`LAPS` laps shoots at the goal at +x (`LeadAndSupportTactic`). For another go: Halt, put the +ball back, and Run in the Live tab's Strategy panel restarts the lap. +""" + +import sys +from pathlib import Path + +sys.path.insert(0, str(Path(__file__).resolve().parents[1])) + +import argparse +from dataclasses import dataclass, field + +from real_score_goal import build_runner + +from utama_core.engine.tactic import BaseTactic, TacticTag +from utama_core.tactics.dribble import DribbleMem, DribbleTactic +from utama_core.tactics.lead_and_support import LeadAndSupportMem, LeadAndSupportTactic + +LAPS = 1 + + +@dataclass +class DribbleThenShootMem: + dribble: DribbleMem + shoot: LeadAndSupportMem = field(default_factory=LeadAndSupportMem) + + +class DribbleThenShootTactic(BaseTactic[DribbleThenShootMem]): + """`DribbleTactic` until it has reached `LAPS` x 4 corners, then `LeadAndSupportTactic`.""" + + tag = TacticTag.ATTACK + + def __init__(self): + self._dribble, self._shoot = DribbleTactic(), LeadAndSupportTactic() + + def initial_mem(self) -> DribbleThenShootMem: + return DribbleThenShootMem(dribble=self._dribble.initial_mem()) + + def _shooting(self, mem: DribbleThenShootMem) -> bool: + return mem.dribble.corner_index >= 4 * LAPS + + def is_committed(self, game, mem: DribbleThenShootMem) -> bool: + return self._shooting(mem) and self._shoot.is_committed(game, mem.shoot) + + def tick(self, game, ctx, robot_ids, mem: DribbleThenShootMem): + if self._shooting(mem): + commands, mem.shoot = self._shoot.tick(game, ctx, robot_ids, mem.shoot) + else: + commands, mem.dribble = self._dribble.tick(game, ctx, robot_ids, mem.dribble) + return commands, mem + + +def main() -> None: + parser = argparse.ArgumentParser(description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter) + parser.add_argument("--mode", choices=("real", "rsim", "grsim"), default="real") + tactics = {"dribble_then_shoot": DribbleThenShootTactic, "dribble": DribbleTactic, "shoot": LeadAndSupportTactic} + build_runner(parser.parse_args().mode, tactics, name="dribble_and_shoot").run() + + +if __name__ == "__main__": + main() diff --git a/examples/real_score_goal.py b/examples/real_score_goal.py index ec1540511..249a62ed6 100644 --- a/examples/real_score_goal.py +++ b/examples/real_score_goal.py @@ -10,7 +10,8 @@ so the two examples always agree; the robot is set below. It attacks the goal at +x (the right one in ssl-vision's coordinates) with `LeadAndSupportTactic`: alone, it goes to the ball, turns to the open corner and shoots. It starts in HALT, and the field check gates Force Start as in -`real_attacker_defender.py`. Put the ball back and Force Start again for the next shot. +`real_attacker_defender.py`. Put the ball back and Force Start again for the next shot; Halt and +Run in the Live tab's Strategy panel restarts the tactic. """ import sys @@ -35,6 +36,7 @@ from utama_core.custom_referee import CustomReferee from utama_core.dashboard import attach_dashboard from utama_core.dashboard.views import referee as referee_view +from utama_core.dashboard.views import strategy as strategy_view from utama_core.entities.referee.referee_command import RefereeCommand from utama_core.run import StrategyRunner from utama_core.tactics.lead_and_support import LeadAndSupportTactic @@ -49,7 +51,10 @@ # --------------------------------------------------------------------------- -def build_runner(mode: str) -> StrategyRunner: +def build_runner(mode: str, tactics=None, name: str = "score_goal") -> StrategyRunner: + """The robot plays `tactics[next(iter(tactics))]`; the Strategy panel switches among `tactics` + ({name: tactic class}, default this example's shooter). `name` tags the recorded log.""" + tactics = tactics or {"shoot": LeadAndSupportTactic} real = mode == "real" shooter_id = SHOOTER.vision_id if real else 0 # in the simulators robots are numbered from 0 is_yellow = SHOOTER_COLOUR == "yellow" @@ -62,12 +67,13 @@ def build_runner(mode: str) -> StrategyRunner: server = attach_dashboard(port=DASHBOARD_PORT) referee_view.attach(server, referee, profile) - game_log = f"replays/field/{time.strftime('%Y%m%d_%H%M%S')}_{mode}_score_goal.log" if RECORD and real else None + game_log = f"replays/field/{time.strftime('%Y%m%d_%H%M%S')}_{mode}_{name}.log" if RECORD and real else None if game_log: print(f"Recording to {game_log}") - return StrategyRunner( + first = next(iter(tactics)) + runner = StrategyRunner( game_log_path=game_log, - strategy=_one_robot_strategy(shooter_id, LeadAndSupportTactic), + strategy=_one_robot_strategy(shooter_id, tactics[first]), my_team_is_yellow=is_yellow, my_team_is_right=False, # own goal at -x: it attacks the goal at +x mode=mode, @@ -83,6 +89,13 @@ def build_runner(mode: str) -> StrategyRunner: yellow_robot_profiles={shooter_id: SHOOTER.profile} if is_yellow else None, blue_robot_profiles=None if is_yellow else {shooter_id: SHOOTER.profile}, ) + strategy_view.attach( + server, + runner, + choices={"my": {n: (lambda t=t: _one_robot_strategy(shooter_id, t)) for n, t in tactics.items()}}, + running={"my": first}, + ) + return runner def main() -> None: diff --git a/utama_core/tactics/dribble.py b/utama_core/tactics/dribble.py index 3f7fddfa4..c58cafc6c 100644 --- a/utama_core/tactics/dribble.py +++ b/utama_core/tactics/dribble.py @@ -35,6 +35,7 @@ _EXPECTED_BALL_ROLL = 0.6 # ball roll at normalised release speed (PID_BUFFER=0.5) _PID_BUFFER = 0.5 # distance past release point for PID deceleration _MIN_LIMIT = 0.2 # minimum dribble segment length +_BACK_OFF_SPEED = 0.2 # m/s backwards while letting go, until the ball is _BALL_SEPARATION_THRESHOLD away def dribbled_enough(distance: float, limit: float) -> bool: @@ -135,7 +136,9 @@ def tick( if mem.releasing or dribbled_enough(mem.dribbled_distance, mem.dribble_limit): mem.releasing = True if not ball_separated(game, robot_id, _BALL_SEPARATION_THRESHOLD): - return {robot_id: empty_command()}, mem + # Back off: standing still waited forever for a ball that stopped short of the + # threshold (rsim, 2026-10-22: 11 cm away, the robot frozen for good). + return {robot_id: empty_command()._replace(local_forward_vel=-_BACK_OFF_SPEED)}, mem mem.dribbled_distance = -1.0 mem.releasing = False return {robot_id: empty_command()}, mem diff --git a/utama_core/tests/engine/test_dribble_tactic.py b/utama_core/tests/engine/test_dribble_tactic.py new file mode 100644 index 000000000..b57680653 --- /dev/null +++ b/utama_core/tests/engine/test_dribble_tactic.py @@ -0,0 +1,45 @@ +"""DribbleTactic's release: it lets go of the ball at the end of each segment.""" + +from __future__ import annotations + +from utama_core.config.field_params import GREAT_EXHIBITION_FIELD_DIMS +from utama_core.engine.context import TickContext +from utama_core.entities.data.vector import Vector2D, Vector3D +from utama_core.entities.game import Field, Game, GameHistory +from utama_core.entities.game.ball import Ball +from utama_core.entities.game.game_frame import GameFrame +from utama_core.entities.game.robot import Robot +from utama_core.tactics.dribble import DribbleMem, DribbleTactic + + +def _game(ball_distance: float) -> Game: + zero2, zero3 = Vector2D(0, 0), Vector3D(0.0, 0.0, 0.0) + robot = Robot(id=0, is_friendly=True, has_ball=False, p=Vector2D(0, 0), v=zero2, a=zero2, orientation=0.0) + frame = GameFrame( + ts=0.0, + my_team_is_yellow=True, + my_team_is_right=True, + friendly_robots={0: robot}, + enemy_robots={}, + ball=Ball(p=Vector3D(ball_distance, 0.0, 0.0), v=zero3, a=zero3), + ) + dims = GREAT_EXHIBITION_FIELD_DIMS + field = Field(my_team_is_right=True, field_dims=dims, field_bounds=dims.full_field_bounds) + return Game(past=GameHistory(max_history=20), current=frame, field=field) + + +def _releasing_mem() -> DribbleMem: + return DribbleMem( + final_target=Vector2D(1.0, 0.6), dribble_limit=0.5, segment_target=Vector2D(1.0, 0.6), releasing=True + ) + + +def test_letting_go_backs_away_from_a_ball_that_stopped_close(): + # rsim, 2026-10-22: the ball stopped 11 cm away, short of the 13 cm the release waits for; + # the robot stood still waiting, for good. It backs away, dribbler off, until the ball is clear. + commands, mem = DribbleTactic().tick(_game(0.11), TickContext(motion_controller=None), (0,), _releasing_mem()) + assert commands[0].local_forward_vel < 0 and not commands[0].dribble + assert mem.releasing + + commands, mem = DribbleTactic().tick(_game(0.14), TickContext(motion_controller=None), (0,), _releasing_mem()) + assert not mem.releasing and mem.dribbled_distance == -1.0 # clear: it goes back for the ball diff --git a/utama_core/tests/examples/test_examples_run_as_scripts.py b/utama_core/tests/examples/test_examples_run_as_scripts.py index a2123af1a..937bc86c5 100644 --- a/utama_core/tests/examples/test_examples_run_as_scripts.py +++ b/utama_core/tests/examples/test_examples_run_as_scripts.py @@ -13,7 +13,10 @@ EXAMPLES = Path(__file__).resolve().parents[3] / "examples" -@pytest.mark.parametrize("name", ["real_attacker_defender.py", "real_score_goal.py", "demo_referee_feedback_gui.py"]) +@pytest.mark.parametrize( + "name", + ["real_attacker_defender.py", "real_score_goal.py", "real_dribble_and_shoot.py", "demo_referee_feedback_gui.py"], +) def test_an_example_with_options_prints_its_help_run_by_path(name, tmp_path): done = subprocess.run( [sys.executable, str(EXAMPLES / name), "--help"], cwd=tmp_path, capture_output=True, text=True, timeout=120 From a17d4d2892346588e7765b2db38f9f8ffbdfe1a9 Mon Sep 17 00:00:00 2001 From: isaac0804 Date: Thu, 22 Oct 2026 21:08:53 +0100 Subject: [PATCH 43/56] fix(vision): fuse the ball across cameras like the robots, and keep a one-camera ball at its robot The robot was averaged across cameras while each camera's ball stayed separate (merged only within 5 cm) and one was picked, so with the cameras ~6 cm apart the ball at the dribbler came out inside the robot. Ball detections within BALL_MERGE_THRESHOLD (now 0.2 m) are fused by the robots' rule, and a ball one camera alone sees (the robot hides it from the other) is moved by that camera's offset at the nearest robot it also sees (BALL_ROBOT_SHIFT_RANGE). Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_01TJGHdb8Dht67hn4Lohndf5 --- utama_core/config/settings.py | 6 +- .../data_processing/refiners/position.py | 110 +++++++++++------- .../tests/refiners/camera_combiner_test.py | 63 +++++++--- 3 files changed, 122 insertions(+), 57 deletions(-) diff --git a/utama_core/config/settings.py b/utama_core/config/settings.py index 00fe7fada..aa8e1b7d8 100644 --- a/utama_core/config/settings.py +++ b/utama_core/config/settings.py @@ -45,9 +45,13 @@ FPS_PRINT_INTERVAL = 1.0 # seconds ### Refiners ### -BALL_MERGE_THRESHOLD = 0.05 # CameraCombiner: distance threshold to merge balls (m) +BALL_MERGE_THRESHOLD = ( + 0.2 # CameraCombiner: ball detections this close are one ball (m); two cameras can disagree by ~0.1 +) # CameraCombiner: a robot's detection further than this from the best-placed camera's is dropped (m) CAMERA_AGREEMENT = 0.1 +# CameraCombiner: a ball this close to a robot its camera sees takes that camera's offset at the robot (m) +BALL_ROBOT_SHIFT_RANGE = 0.3 VISION_BOUNDS_BUFFER = 1.0 # CameraCombiner: buffer around field bounds to include in vision (m) # An unseen ball is assumed to roll on at this deceleration (m/s^2): official Div B logs, 1 s or # more after a kick (docs/sim_realism.md). A field's own value comes from its free-rolling balls. diff --git a/utama_core/data_processing/refiners/position.py b/utama_core/data_processing/refiners/position.py index 8b6ff19c4..55b9a7c91 100644 --- a/utama_core/data_processing/refiners/position.py +++ b/utama_core/data_processing/refiners/position.py @@ -9,6 +9,7 @@ from utama_core.config.field_params import FieldDimensions from utama_core.config.settings import ( BALL_MERGE_THRESHOLD, + BALL_ROBOT_SHIFT_RANGE, CAMERA_AGREEMENT, VISION_BOUNDS_BUFFER, ) @@ -394,7 +395,13 @@ class CameraCombiner: 1/cos^2 of its view angle (a pixel covers more floor, a wrong robot height shifts the robot more), so the weight is cos^4. A detection further than `CAMERA_AGREEMENT` from the best-placed camera's is dropped, so a camera that disagrees (a poor calibration far from it) - does not pull the robot away. Without camera positions every camera counts the same.""" + does not pull the robot away. Without camera positions every camera counts the same. A ball + is fused by the same rule, its detections grouped by being within `BALL_MERGE_THRESHOLD` + of each other: fused one way and the robots another, a ball at a robot's dribbler came out + inside the robot (2026-10-22). Each camera's ball is first moved by that camera's offset at + the nearest robot it also sees (within `BALL_ROBOT_SHIFT_RANGE`): the fused robot minus its + own detection of it. So a ball seen by one camera only (the robot hides it from the other) + stays where that camera put it relative to the robot.""" def combine_cameras( self, @@ -415,10 +422,10 @@ def combine_cameras( # maps robot id to the (detection, its camera's position) of every camera that saw it yellow_captured = defaultdict(list) blue_captured = defaultdict(list) - balls_captured = defaultdict(list) + balls_captured = [] # (detection, its camera's position, ids of that frame's robot objects) # Each frame is from a different camera - for frame_ind, frame in enumerate(frames): + for frame in frames: for yr in frame.yellow_robots: if self._bounds_check(yr.x, yr.y, bounds): yellow_captured[yr.id].append((yr, frame.camera_position)) @@ -429,13 +436,28 @@ def combine_cameras( for b in frame.balls: if self._bounds_check(b.x, b.y, bounds): - balls_captured[frame_ind].append(b) + frame_robots = {id(r) for r in frame.yellow_robots + frame.blue_robots} + balls_captured.append((b, frame.camera_position, frame_robots)) ts.append(frame.ts) avg_yellows = list(map(self._avg_robots, yellow_captured.values())) avg_blues = list(map(self._avg_robots, blue_captured.values())) - # Current strategy is just to take the most confident ball - balls = self._combine_balls_by_proximity(balls_captured) + + # Each ball in the fused robots' frame: shifted by its camera's offset at its nearest robot. + fused = [ + (r, fused_r) + for captured, avg in ((yellow_captured, avg_yellows), (blue_captured, avg_blues)) + for seen, fused_r in zip(captured.values(), avg) + for r, _ in seen + ] + shifted = [] + for b, camera, frame_robots in balls_captured: + near = [(r, f) for r, f in fused if id(r) in frame_robots] + r, f = min(near, key=lambda rf: math.hypot(rf[0].x - b.x, rf[0].y - b.y), default=(None, None)) + if r is not None and math.hypot(r.x - b.x, r.y - b.y) <= BALL_ROBOT_SHIFT_RANGE: + b = RawBallData(b.x + f.x - r.x, b.y + f.y - r.y, b.z, b.confidence) + shifted.append((b, camera)) + balls = self._combine_balls_by_proximity(shifted) return VisionData(sum(ts) / len(ts), avg_yellows, avg_blues, balls) @@ -448,18 +470,23 @@ def _view_weight(x: float, y: float, camera: Optional[Tuple[float, float, float] h2 = height * height return (h2 / (h2 + (x - cx) ** 2 + (y - cy) ** 2)) ** 2 + def _fuse(self, seen: list) -> list: + """(detection, weight) of the detections of one thing, `seen` as (detection, camera + position): those within `CAMERA_AGREEMENT` of the best-placed camera's, weighted by view + angle; every detection at weight 1 when a camera's position is unknown.""" + if any(camera is None for _, camera in seen): + return [(d, 1.0) for d, _ in seen] + weights = [self._view_weight(d.x, d.y, camera) for d, camera in seen] + best = seen[int(np.argmax(weights))][0] + return [ + (d, w) for (d, _), w in zip(seen, weights) if math.hypot(d.x - best.x, d.y - best.y) <= CAMERA_AGREEMENT + ] + def _avg_robots(self, seen: List[Tuple[RawRobotData, Optional[Tuple[float, float, float]]]]) -> VisionRobotData: # All these robots should have the same id - rs = [r for r, _ in seen] - base_id = rs[0].id - assert all(r.id == base_id for r in rs) - if all(camera is not None for _, camera in seen): - weights = [self._view_weight(r.x, r.y, camera) for r, camera in seen] - best = rs[int(np.argmax(weights))] - kept = [(r, w) for r, w in zip(rs, weights) if math.hypot(r.x - best.x, r.y - best.y) <= CAMERA_AGREEMENT] - else: - kept = [(r, 1.0) for r in rs] - + base_id = seen[0][0].id + assert all(r.id == base_id for r, _ in seen) + kept = self._fuse(seen) total = sum(w for _, w in kept) x = sum(r.x * w for r, w in kept) / total y = sum(r.y * w for r, w in kept) / total @@ -469,33 +496,30 @@ def _avg_robots(self, seen: List[Tuple[RawRobotData, Optional[Tuple[float, float ) return VisionRobotData(base_id, x, y, avg_orientation) - def _combine_balls_by_proximity(self, bs: Dict[int, List[RawBallData]]) -> List[VisionBallData]: - combined_balls: List[VisionBallData] = [] - for ball_list in bs.values(): - for b in ball_list: - found = False - for i, cb in enumerate(combined_balls): - if CameraCombiner.ball_merge_predicate(b, cb): - found = True - combined_balls[i] = CameraCombiner.ball_merge(cb, b) - break - - if not found: - # If no ball close enough, must have found a new separate ball - combined_balls.append(b) - return combined_balls + def _combine_balls_by_proximity(self, seen: list) -> List[VisionBallData]: + """One ball per group of detections (`seen`: (detection, camera position)) within + `BALL_MERGE_THRESHOLD` of the group's first, fused like a robot.""" + groups: list = [] + for b, camera in seen: + for group in groups: + if math.hypot(b.x - group[0][0].x, b.y - group[0][0].y) < BALL_MERGE_THRESHOLD: + group.append((b, camera)) + break + else: + groups.append([(b, camera)]) + balls = [] + for group in groups: + kept = self._fuse(group) + total = sum(w for _, w in kept) + balls.append( + RawBallData( + sum(b.x * w for b, w in kept) / total, + sum(b.y * w for b, w in kept) / total, + sum(b.z * w for b, w in kept) / total, + max(b.confidence for b, _ in kept), + ) + ) + return balls def _bounds_check(self, x: float, y: float, bounds: VisionBounds) -> bool: return bounds.x_min <= x <= bounds.x_max and bounds.y_min <= y <= bounds.y_max - - @staticmethod - def ball_merge_predicate(b1: RawBallData, b2: RawBallData) -> bool: - return abs(b1.x - b2.x) + abs(b1.y - b2.y) < BALL_MERGE_THRESHOLD - - @staticmethod - def ball_merge(b1: RawBallData, b2: RawBallData) -> RawBallData: - nx = (b1.x + b2.x) / 2 - ny = (b1.y + b2.y) / 2 - nz = (b1.z + b2.z) / 2 - nc = max(b1.confidence, b2.confidence) - return RawBallData(nx, ny, nz, nc) diff --git a/utama_core/tests/refiners/camera_combiner_test.py b/utama_core/tests/refiners/camera_combiner_test.py index 38ada4cac..3737c61a8 100644 --- a/utama_core/tests/refiners/camera_combiner_test.py +++ b/utama_core/tests/refiners/camera_combiner_test.py @@ -50,14 +50,16 @@ def test_combine_with_one_camera_empty(): def test_combine_proximity_multiple_balls(): - c = CameraCombiner() - - balls = { - 0: [RawBallData(0, 0, 0, 1), RawBallData(5, 5, 0, 1)], - 1: [RawBallData(0.02, 0.03, 0, 1), RawBallData(1, 1, 0, 1)], - } - balls = c._combine_balls_by_proximity(balls) - assert len(balls) == 4 + # (0, 0) and (0.02, 0.03) are one ball seen by two cameras; the others are balls of their own. + seen = [ + (RawBallData(0, 0, 0, 1), None), + (RawBallData(5, 5, 0, 1), None), + (RawBallData(0.02, 0.03, 0, 1), None), + (RawBallData(1, 1, 0, 1), None), + ] + balls = CameraCombiner()._combine_balls_by_proximity(seen) + assert len(balls) == 3 + assert (balls[0].x, balls[0].y) == (0.01, 0.015) def test_combine_with_both_camera_empty_gives_empty(): @@ -71,11 +73,12 @@ def test_combine_with_both_camera_empty_gives_empty(): def test_combine_filters_low_confidence(): - balls = { - 0: [RawBallData(0, 0, 0, 0.05), RawBallData(5, 5, 0, 0.05)], - 1: [RawBallData(0.02, 0.02, 0, 0.05)], - } - assert len(CameraCombiner()._combine_balls_by_proximity(balls)) == 2 + seen = [ + (RawBallData(0, 0, 0, 0.05), None), + (RawBallData(5, 5, 0, 0.05), None), + (RawBallData(0.02, 0.02, 0, 0.05), None), + ] + assert len(CameraCombiner()._combine_balls_by_proximity(seen)) == 2 # --- Bounds filtering tests --- @@ -206,3 +209,37 @@ def test_robot_orientation_is_the_weighted_circular_mean(): ] robot = CameraCombiner().combine_cameras(frames, infinite_bounds).yellow_robots[0] assert math.isclose(abs(robot.orientation), math.pi) + + +def test_a_ball_at_the_dribbler_stays_there_when_the_cameras_disagree(): + # 2026-10-22: the cameras put robot and ball 6 cm apart from each other's view. The robot was + # averaged and the ball (beyond the old 5 cm merge) taken from one camera, so the ball came + # out 3 cm inside the robot. Fused alike, the ball stays 11 cm ahead of the robot's centre. + shift = 0.06 + frames = [ + RawVisionData(0, [], [RawRobotData(2, -0.9, 0.4, 0, 1)], [RawBallData(-0.79, 0.4, 0, 0.9)], 0), + RawVisionData(0, [], [RawRobotData(2, -0.9 + shift, 0.4, 0, 1)], [RawBallData(-0.79 + shift, 0.4, 0, 0.8)], 1), + ] + combined = CameraCombiner().combine_cameras(frames, infinite_bounds) + assert len(combined.balls) == 1 + assert math.isclose(combined.balls[0].x - combined.blue_robots[0].x, 0.11) + + frames[0].camera_position, frames[1].camera_position = CAMERA_LEFT, CAMERA_RIGHT # positions known + combined = CameraCombiner().combine_cameras(frames, infinite_bounds) + assert len(combined.balls) == 1 + assert math.isclose(combined.balls[0].x - combined.blue_robots[0].x, 0.11, abs_tol=0.005) + + +def test_a_ball_only_one_camera_sees_keeps_its_place_at_the_dribbler(): + # 2026-10-22: the robot hid the ball from one camera; the robot was fused from both cameras + # (6 cm apart) and the ball came from the other alone, so it sat 3 cm inside the robot. + shift = 0.06 + frames = [ + RawVisionData(0, [], [RawRobotData(2, -0.9, 0.4, 0, 1)], [RawBallData(-0.79, 0.4, 0, 0.9)], 0), + RawVisionData(0, [], [RawRobotData(2, -0.9 + shift, 0.4, 0, 1)], [], 1), + ] + combined = CameraCombiner().combine_cameras(frames, infinite_bounds) + assert math.isclose(combined.balls[0].x - combined.blue_robots[0].x, 0.11) + + far = [RawVisionData(0, [], [RawRobotData(2, -0.9, 0.4, 0, 1)], [RawBallData(0.5, 0.4, 0, 0.9)], 0), frames[1]] + assert CameraCombiner().combine_cameras(far, infinite_bounds).balls[0].x == 0.5 # no robot near: unmoved From d51514227c65deda8c9616fde1cd5981983cefee Mon Sep 17 00:00:00 2001 From: isaac0804 Date: Thu, 22 Oct 2026 21:18:12 +0100 Subject: [PATCH 44/56] tools(vision_check): --guided checks each camera against 9 known spots and saves the baseline Cameras that agree with each other can both be wrong. The ball (or a robot, facing +x) goes on the corners, edge midpoints and centre of the announced field; each camera's error is printed per spot and as mean, worst and shift, and saved to replays/field/. A stray ball detection far from the spot (camera 0 sees one 824 mm away) is left out. Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_01TJGHdb8Dht67hn4Lohndf5 --- docs/real_robot_setup.md | 8 ++ docs/tools.md | 2 +- tools/vision_check.py | 152 ++++++++++++++++++-- utama_core/tests/tools/test_vision_check.py | 25 ++++ 4 files changed, 172 insertions(+), 15 deletions(-) create mode 100644 utama_core/tests/tools/test_vision_check.py diff --git a/docs/real_robot_setup.md b/docs/real_robot_setup.md index 85f40f054..1ad03f82f 100644 --- a/docs/real_robot_setup.md +++ b/docs/real_robot_setup.md @@ -85,6 +85,14 @@ Expect each camera at about its frame rate and the field at 3000 x 2250 mm. Noth means the network or the firewall (steps 3-4 above, step 1 here); one camera missing means its capture is not running. +After a calibration, check it against known spots: `tools/vision_check.py --guided ball`, then +`--guided "blue 2"` (any robot, facing +x). It asks for the ball or robot on the 9 points +(corners, edge midpoints, centre) and prints each camera's error there; the results land in +`replays/field/*_vision_truth_*.json` to compare with the next setup. The pattern points at the +cause: errors growing towards the edges are the distortion or focal length, the same shift +everywhere the camera's position, more at one end than the other its tilt or height. The ball +right but the robot not is the robot height in ssl-vision. + ### 3. Radio Plug the dongle in, then in admin PowerShell (leave it open: it re-attaches after a replug): diff --git a/docs/tools.md b/docs/tools.md index f90dad12d..8b77a093c 100644 --- a/docs/tools.md +++ b/docs/tools.md @@ -113,7 +113,7 @@ described in [STRATEGY_DEVELOPMENT.md](STRATEGY_DEVELOPMENT.md). | `examples/demo_referee_feedback_gui.py` | The referee UI's controller-feedback panel with fake feedback, no hardware | | `examples/demo_exhibition_road.py` | The Exhibition Road festival demo on the 4 m x 3 m field, over rsim | | `examples/real_attacker_defender.py` | One attacker against one defender on real robots (or `--mode rsim`/`grsim`): field, camera half, IDs and each robot's working hardware in one config block; the Live tab's field check gates the start. Records every camera run as an SSL game log in `replays/field/`; `--calibrate attacker\|defender` plays fixed moves for measuring that robot | -| `tools/vision_check.py` | Does ssl-vision's multicast reach this machine: sender, packets per second per camera, detections and the field size, in 5 s with no radio or runner. The first check of a test day ([real_robot_setup.md](real_robot_setup.md)) | +| `tools/vision_check.py` | Does ssl-vision's multicast reach this machine: sender, packets per second per camera, detections and the field size, in 5 s with no radio or runner. The first check of a test day ([real_robot_setup.md](real_robot_setup.md)). `--guided ball` (or `"blue 2"`): each camera's error on 9 known spots, saved as a baseline | | `tools/ssl-vision-lab-field.patch` | Not a script: adds a **Lab Field** preset (our 3 m x 2.25 m field, two cameras) to an ssl-vision checkout; `git apply` it there and rebuild | | `tools/robot_calibration.py` | A robot's numbers from a recorded field test (`replays/field/*.log`): loop delay, speed gain and acceleration per axis (the sim's move model fitted to the camera's positions), vision noise at rest and missed detections, kick speed. Writes `.calibration.json`. The same log converts to a replay with `tools/convert_ssl_log.py` | | `examples/demo_dribbler_test.py` | One robot dribbling round a rectangle on the Exhibition Road field | diff --git a/tools/vision_check.py b/tools/vision_check.py index b86e983a9..6f2c545a5 100644 --- a/tools/vision_check.py +++ b/tools/vision_check.py @@ -7,11 +7,20 @@ several cameras see it, the largest gap between them: cameras whose calibrations agree are a few cm apart. Rerun after each calibration change. +With `--guided ball` (or `--guided "blue 2"`) it checks the cameras against known spots instead: +it asks for the ball (or that robot, facing +x) on each of 9 points (corners, edge midpoints, +centre of the field ssl-vision announces), averages each camera there for `--seconds`, and prints +each camera's error at each point. Cameras that agree with each other can both be wrong; this +says by how much. The results are saved to `replays/field/` as the baseline for the next setup. + pixi run python tools/vision_check.py [--seconds 5] + pixi run python tools/vision_check.py --guided ball [--seconds 2] """ import argparse import collections +import json +import math import socket import struct import sys @@ -28,21 +37,25 @@ SSL_WrapperPacket, ) +GUIDE_RADIUS = 500 # mm: in the guided check, a detection further than this from the spot is not it -def main() -> None: - parser = argparse.ArgumentParser(description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter) - parser.add_argument("--seconds", type=float, default=5.0) - seconds = parser.parse_args().seconds +def _socket(): sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) sock.bind((MULTICAST_GROUP, VISION_PORT)) group = struct.pack("4sL", socket.inet_aton(MULTICAST_GROUP), socket.INADDR_ANY) sock.setsockopt(socket.IPPROTO_IP, socket.IP_ADD_MEMBERSHIP, group) sock.settimeout(0.5) + return sock + +def listen(sock, seconds: float) -> dict: + """What arrives in `seconds`: senders, packets and detections per camera, the field size and + `positions`, (thing, camera) -> [(x, y, orientation)] in mm and rad (orientation None for a ball), + and `ball_frames`, camera -> every ball of each of its frames ([[(x, y)]]).""" per_camera, seen, senders, field = collections.Counter(), collections.Counter(), set(), None - positions = collections.defaultdict(list) # (thing, camera) -> [(x, y)] in mm + positions, ball_frames = collections.defaultdict(list), collections.defaultdict(list) end = time.time() + seconds while time.time() < end: try: @@ -60,32 +73,143 @@ def main() -> None: seen["balls"] += len(d.balls) for colour, robots in (("yellow", d.robots_yellow), ("blue", d.robots_blue)): for r in robots: - positions[(f"{colour} {r.robot_id}", d.camera_id)].append((r.x, r.y)) + positions[(f"{colour} {r.robot_id}", d.camera_id)].append((r.x, r.y, r.orientation)) + ball_frames[d.camera_id].append([(b.x, b.y) for b in d.balls]) if len(d.balls) == 1: # with several balls in one image, which is which is unknown - positions[("ball", d.camera_id)].append((d.balls[0].x, d.balls[0].y)) + positions[("ball", d.camera_id)].append((d.balls[0].x, d.balls[0].y, None)) if packet.HasField("geometry"): field = (packet.geometry.field.field_length, packet.geometry.field.field_width) + return { + "per_camera": per_camera, + "seen": seen, + "senders": senders, + "field": field, + "positions": positions, + "ball_frames": ball_frames, + } + + +def mean_pose(samples: list) -> tuple: + """Mean (x, y, orientation) of `samples`; the orientation a circular mean, None for a ball.""" + x = sum(s[0] for s in samples) / len(samples) + y = sum(s[1] for s in samples) / len(samples) + if samples[0][2] is None: + return x, y, None + return x, y, math.atan2(sum(math.sin(s[2]) for s in samples), sum(math.cos(s[2]) for s in samples)) + + +def guide_points(length: float, width: float) -> list: + """The 9 spots (mm) the guided check asks for: corners, edge midpoints and centre.""" + return [(x * length / 2, y * width / 2) for y in (1, 0, -1) for x in (-1, 0, 1)] + + +def nearest_balls(frames: list, x: float, y: float, limit: float = GUIDE_RADIUS) -> list: + """Of each frame's balls, the one nearest (`x`, `y`) when within `limit` mm, as + (x, y, None): a stray detection elsewhere (something orange) is left out.""" + picked = [] + for balls in frames: + near = [b for b in balls if math.hypot(b[0] - x, b[1] - y) <= limit] + if near: + bx, by = min(near, key=lambda b: math.hypot(b[0] - x, b[1] - y)) + picked.append((bx, by, None)) + return picked - if not senders: + +def check(seconds: float) -> None: + sock = _socket() + heard = listen(sock, seconds) + if not heard["senders"]: print(f"Nothing on {MULTICAST_GROUP}:{VISION_PORT} in {seconds:.0f} s: see docs/real_robot_setup.md, 'Vision'.") sys.exit(1) - print(f"From: {', '.join(sorted(senders))}") + print(f"From: {', '.join(sorted(heard['senders']))}") + per_camera = heard["per_camera"] for camera, n in sorted(per_camera.items()): print(f"Camera {camera}: {n / seconds:.0f} packets/s") print( "Detections per packet: " - + ", ".join(f"{k} {v / max(1, sum(per_camera.values())):.1f}" for k, v in seen.items()) + + ", ".join(f"{k} {v / max(1, sum(per_camera.values())):.1f}" for k, v in heard["seen"].items()) ) + field = heard["field"] print(f"Field: {field[0]:.0f} x {field[1]:.0f} mm" if field else "Field: no geometry packet yet") by_thing = collections.defaultdict(dict) - for (thing, camera), xys in positions.items(): - by_thing[thing][camera] = tuple(sum(c) / len(xys) for c in zip(*xys)) + for (thing, camera), samples in heard["positions"].items(): + by_thing[thing][camera] = mean_pose(samples) for thing, cameras in sorted(by_thing.items()): - where = ", ".join(f"camera {c} ({x:.0f}, {y:.0f})" for c, (x, y) in sorted(cameras.items())) - gaps = [((a[0] - b[0]) ** 2 + (a[1] - b[1]) ** 2) ** 0.5 for a, b in combinations(cameras.values(), 2)] + where = ", ".join(f"camera {c} ({p[0]:.0f}, {p[1]:.0f})" for c, p in sorted(cameras.items())) + gaps = [math.hypot(a[0] - b[0], a[1] - b[1]) for a, b in combinations(cameras.values(), 2)] print(f"{thing}: {where}" + (f", {max(gaps):.0f} mm apart" if gaps else "")) +def guided(thing: str, seconds: float) -> None: + sock = _socket() + field = None + while field is None: # ssl-vision sends the geometry about once a second + heard = listen(sock, 2.0) + if not heard["senders"]: + print(f"Nothing on {MULTICAST_GROUP}:{VISION_PORT}: run without --guided to diagnose.") + sys.exit(1) + field = heard["field"] + print(f"Field {field[0]:.0f} x {field[1]:.0f} mm. Checking {thing!r} at 9 points, {seconds:.0f} s each.") + if thing != "ball": + print("Face the robot towards +x at each point.") + + results = [] + for i, (x, y) in enumerate(guide_points(*field), 1): + input(f"\n[{i}/9] Put the {thing} at ({x:.0f}, {y:.0f}) mm, then press Enter ") + heard = listen(sock, seconds) + point = {"truth": [x, y], "cameras": {}} + if thing == "ball": + seen = {c: nearest_balls(f, x, y) for c, f in heard["ball_frames"].items()} + else: + seen = {c: s for (name, c), s in heard["positions"].items() if name == thing} + for camera, samples in sorted(seen.items()): + if not samples: + continue + mx, my, mo = mean_pose(samples) + point["cameras"][camera] = {"seen": [mx, my], "orientation": mo, "frames": len(samples)} + line = f" camera {camera}: ({mx:.0f}, {my:.0f}), off by {math.hypot(mx - x, my - y):.0f} mm" + line += f" (dx {mx - x:+.0f}, dy {my - y:+.0f})" + if mo is not None: + line += f", heading {math.degrees(mo):+.1f} deg" + print(line) + if not point["cameras"]: + print(f" no camera saw the {thing}") + results.append(point) + + print("\nPer camera over the points it saw: mean error, worst, mean shift (dx, dy)") + for camera in sorted({c for p in results for c in p["cameras"]}): + errs = [ + (p["cameras"][camera]["seen"][0] - p["truth"][0], p["cameras"][camera]["seen"][1] - p["truth"][1]) + for p in results + if camera in p["cameras"] + ] + dists = [math.hypot(dx, dy) for dx, dy in errs] + print( + f" camera {camera}: {len(errs)}/9 points, {sum(dists) / len(dists):.0f} mm mean, {max(dists):.0f} worst," + f" shift ({sum(e[0] for e in errs) / len(errs):+.0f}, {sum(e[1] for e in errs) / len(errs):+.0f})" + ) + out = ( + REPO_ROOT + / "replays" + / "field" + / f"{time.strftime('%Y%m%d_%H%M%S')}_vision_truth_{thing.replace(' ', '_')}.json" + ) + out.parent.mkdir(parents=True, exist_ok=True) + out.write_text(json.dumps({"thing": thing, "field": field, "seconds": seconds, "points": results}, indent=1)) + print(f"Saved {out.relative_to(REPO_ROOT)}") + + +def main() -> None: + parser = argparse.ArgumentParser(description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter) + parser.add_argument("--seconds", type=float, help="how long to listen (default 5, or 2 per point with --guided)") + parser.add_argument("--guided", metavar="THING", help='"ball" or a robot ("blue 2"): check it on 9 known points') + args = parser.parse_args() + if args.guided: + guided(args.guided, args.seconds or 2.0) + else: + check(args.seconds or 5.0) + + if __name__ == "__main__": main() diff --git a/utama_core/tests/tools/test_vision_check.py b/utama_core/tests/tools/test_vision_check.py new file mode 100644 index 000000000..a86ef26be --- /dev/null +++ b/utama_core/tests/tools/test_vision_check.py @@ -0,0 +1,25 @@ +"""The guided check of `tools/vision_check.py`: the spots it asks for and which ball it takes.""" + +import importlib.util +from pathlib import Path + +spec = importlib.util.spec_from_file_location( + "vision_check", Path(__file__).resolve().parents[3] / "tools" / "vision_check.py" +) +vision_check = importlib.util.module_from_spec(spec) +spec.loader.exec_module(vision_check) + + +def test_the_nine_spots_are_the_corners_edge_midpoints_and_centre(): + spots = vision_check.guide_points(3000, 2250) + assert len(spots) == 9 and (0, 0) in spots + assert {(-1500, 1125), (1500, 1125), (-1500, -1125), (1500, -1125)} <= set(spots) + assert {(0, 1125), (0, -1125), (-1500, 0), (1500, 0)} <= set(spots) + + +def test_a_stray_ball_away_from_the_spot_is_left_out(): + # 2026-10-22: camera 0 also saw "a ball" 824 mm away from the real one. + frames = [[(-365, -517), (1490, 1120)], [(1505, 1130)], [(-365, -517)]] + assert vision_check.nearest_balls(frames, 1500, 1125) == [(1490, 1120, None), (1505, 1130, None)] + assert vision_check.nearest_balls(frames, 1500, 1125, limit=vision_check.GUIDE_RADIUS) != [] + assert vision_check.mean_pose(vision_check.nearest_balls(frames, 1500, 1125)) == (1497.5, 1125, None) From 8ad1ab48ae0cf2a29fead295b3f17db2902fbc7d Mon Sep 17 00:00:00 2001 From: isaac0804 Date: Sat, 24 Oct 2026 14:36:52 +0100 Subject: [PATCH 45/56] docs: plan an in-house vision stack for lab days, with the experiments that decide it The 2026-10-22 test day's vision findings, the options considered, the design (per-session tag calibration, tags on robots, ball inside the field mask, SSL packets out), two experiments with pass marks, and the order of work. Linked from the roadmap and the docs index. Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_01TJGHdb8Dht67hn4Lohndf5 --- docs/README.md | 1 + docs/in_house_vision.md | 202 ++++++++++++++++++++++++++++++++++++++++ docs/roadmap.md | 10 +- 3 files changed, 212 insertions(+), 1 deletion(-) create mode 100644 docs/in_house_vision.md diff --git a/docs/README.md b/docs/README.md index 54678a6ef..100fdee13 100644 --- a/docs/README.md +++ b/docs/README.md @@ -35,6 +35,7 @@ Every doc in this folder, by kind. New to the repo: the [README](../README.md) f - [roadmap.md](roadmap.md): larger open workstreams, and one-line pointers to finished ones. - [sim_realism.md](sim_realism.md): how far rsim is from real matches, the realism scorecard, and the plan to close the gap with measured, randomized physics. +- [in_house_vision.md](in_house_vision.md): our own vision stack for lab days (per-session tag calibration, tags on robots), the experiments that decide it, and the 2026-10-22 test day's vision findings. - [testing_gaps.md](testing_gaps.md): kinds of testing gap that let bugs through, numbered (code cites them by number). - [investigation_default_vs_lowblock_stalemate.md](investigation_default_vs_lowblock_stalemate.md): why `default` vs `low_block` stays 0-0; partly fixed. - [investigation_ball_contact_orientation_divergence.md](investigation_ball_contact_orientation_divergence.md): a robot pinned against the ball rotates away from contact; localized, not fixed. diff --git a/docs/in_house_vision.md b/docs/in_house_vision.md new file mode 100644 index 000000000..cd403e313 --- /dev/null +++ b/docs/in_house_vision.md @@ -0,0 +1,202 @@ +# In-house vision: plan, experiments and findings + +Status: plan, 2026-10-24. Nothing below is built unless it says so. Written after the +2026-10-22 test day, where vision took most of the day. + +The goal is less time spent on vision each test day, and robot positions accurate to about +1 cm instead of 3–9 cm. This applies to **lab and test days only**: at competitions the league +runs ssl-vision with the standard colour patterns, and our stack only receives its packets. + +## What went wrong on 2026-10-22 + +The setup: two cameras, ssl-vision on its own Linux laptop sending 224.5.23.2:10006 at 61 fps +per camera, our stack in WSL on the robot laptop (`docs/real_robot_setup.md`). + +- **The cameras disagree.** The same robot was reported 13–20 cm apart by the two cameras + early on; after the distortion was retuned in ssl-vision, 5 cm, then 85 mm later in the day + (`replays/field/20261022_211142_real_score_goal.log`, constant 83–85 mm in every frame). + Something moved between those checks, and nothing tells us when. +- **Against known spots** (`vision_check.py --guided ball`, + `replays/field/20261022_212332_vision_truth_ball.json`): camera 0 saw 7 of 9 points, 33 mm + mean error, 61 mm worst; camera 1 saw 5 of 9, 55 mm mean, 92 mm worst. Neither saw the ball + at (1500, 0), right in front of the +x goal. +- **Camera height is unknown.** Nobody has a good measurement of it, and ssl-vision's + calibration leans on it. A wrong robot height shifts each camera's robots along its own line + of sight, in opposite directions for the two cameras. +- **Camera 1 never sent its calibration** in the geometry packet, so our camera weighting + (`CameraCombiner`, below) ran with equal weights all day. +- **Colour calibration is slow and noisy at the edges and corners.** Yellow detection + collapsed after a change to the robot tops (1.3 to 0.1 detections per packet), and one + yellow robot came out as IDs 1, 2 and 3 at the same spot. Camera 0 detected no ball at all + in one 2,010-packet run. +- **Stray orange objects became balls.** Camera 0 saw 1–4 fake balls in a line about 15 cm + outside the +y touchline, each at confidence 1.00. Our stack picked a fake 22% of the time + and switched between balls 107 times in 6 s: the flickering ball. +- **Focus and zoom can't be locked** on our cameras, so the lens changes whenever a camera is + packed up, and every calibration starts from zero. + +## What our stack already does about it + +Built during the test day; it hides the cameras' disagreement but cannot remove it. + +- `CameraCombiner` (`utama_core/data_processing/refiners/position.py`) weights each camera by + how steeply it sees a robot (cos⁴ of the view angle) once every camera's position is known, + and drops a detection more than `CAMERA_AGREEMENT` (0.1 m) from the best camera's. +- The ball is fused across cameras by the same rule, and each camera's ball is first moved by + that camera's own error at the robot nearest it (`BALL_ROBOT_SHIFT_RANGE`), so a ball at the + dribbler stays at the dribbler. 2.4% of frames in the log above still put the ball inside the + robot: those where camera 1 sees the ball but loses the robot. +- `tools/vision_check.py` prints where each camera puts each robot and ball and the gap + between them, and with `--guided` checks each camera against 9 known spots and saves the + result as a baseline. + +## Options considered + +| | Time per test day | Build cost | Risk | +|---|---|---|---| +| 1. ssl-vision, more tuning and better fusion | stays high | none | what we have now | +| 2. Our own stack, with tags (and patterns) | low | weeks | most work, but ours to fix | +| 3. Modify ssl-vision's own code | medium | high: a large C++/Qt codebase and a fork to maintain | avoid | + +Improving ssl-vision from the outside (our own calibration tool writing its settings) gets most +of what option 3 would, without a fork. So the real choice is 1 or 2, and two experiments +decide it (below). + +## Decision, under working assumptions + +The plan below assumes three things, each to be confirmed by an experiment: + +1. **The cameras can't be locked** (focus and zoom change between sessions). +2. **Proper calibration alone doesn't remove the position error** (experiment 1). +3. **Tags are better than colour patterns** for robots (experiment 2; early indications say so). + +Under those, **build our own stack, tag-only for robots, and recalibrate it automatically at +the start of every session**. Pattern mode is not rebuilt: when we want to practise with +patterns (as at competitions), we run ssl-vision. If an experiment contradicts an assumption, +revisit this section before building past the step it gates. + +What the assumptions imply: + +- Cameras that can't be locked: the lens changes each session, so calibration must run every + session, in about a minute, with nothing done by hand, and keep checking during the session. +- Calibration that doesn't fix the error: don't rely on a correct camera model. Fit the + image-to-field mapping each session from measurements, then correct what is left. +- Tags better than patterns: identify robots and their heading from tags, so robots need no + colour at all. The ball is the only thing left that needs colour. + +## Design + +``` +cameras -> capture -> [calibration each session] -> tag detector + ball detector -> SSL_WrapperPacket -> our stack (unchanged) +``` + +### 1. Calibration each session: one button + +- About 20 A4 tags (AprilTag 36h11) on the floor; 3 of them on measured spots (the centre and + two corners, lined up with the field lines) that fix the field's axes. The rest go anywhere: + the solver finds where they are. +- A few tags on boxes at robot-top height (about 14 cm). A flat floor alone cannot tell a longer + lens from a higher camera, which is today's camera-height problem; tags off the floor + separate the two, and they calibrate exactly the plane the robot positions come from. +- Press calibrate: one bundle adjustment fits each camera's lens (focal length, image centre, + distortion), its position and tilt, and the loose tags' positions, over hundreds of tag + corners. Both cameras see shared tags, so they agree with each other by construction. +- Because the camera model alone isn't trusted (assumption 2), fit a smooth correction map from + the residual error at the tags: one for robot-top height, one for ball height (about 21 mm). +- It ends by running the guided check itself and prints the error per camera. +- Expected accuracy: the field is about 3 m in roughly 1,300 pixels, 2–3 mm per pixel; tag + corners are found to a fraction of a pixel and averaged over hundreds, so a few mm. + +### 2. Watching for camera movement during the session + +A few tags stay just outside the field lines all session. Every few seconds the tool +re-estimates each camera's position from them, corrects for a small move and raises an alarm +for a large one (a bumped or sagging mount). This answers "something moved between 52 mm and +85 mm" from the test day. + +### 3. Robots: tags + +- One tag per robot top; a config file maps tag ID to team and robot ID. +- `cv2.aruco` has AprilTag 36h11 built in; each detection gives position and heading from the + tag's 4 corners. +- When a moving robot's tag is found but can't be decoded, tracking keeps its ID. +- Short exposure: motion blur is the main weakness of tags. + +### 4. Ball: colour, but the easy kind + +- Only inside the field mask that calibration gives, plus a margin: that alone would have + removed the test day's fake balls. +- Flat-field correction from an empty-field frame (the median over a few seconds), so the ball + looks the same colour in the corners as in the centre. +- Orange by hue, not brightness; blobs on a robot tag are ignored. + +### 5. Output: standard SSL packets + +The same `SSL_WrapperPacket` on the same multicast address, so `CameraCombiner`, the refiners +and everything above stay unchanged. While both run, send on a second port so the two can be +compared side by side on the same scene with `vision_check`. + +### Practical + +- Python and OpenCV; move to C++ only if measured latency forces it. Tag detection at 60 fps is + fine with decimation. +- Runs on the vision laptop. FLIR cameras through PySpin (Spinnaker's Python API), webcams + through OpenCV directly. +- Roughly 2–4 weeks part-time to step 4 of the order below. The calibration tool is the hardest + part and the most valuable. + +## Experiments + +Run on recorded frames where possible, so they need no test-day time beyond the recording. + +### Experiment 1: does proper calibration fix the position error? + +- Checkerboard on A4, about 20 stills per camera, tilted, all over the image; OpenCV computes + focal length and distortion. +- Tags or checkerboards on known spots for each camera's position and height; feed the result + into ssl-vision. +- Rerun `vision_check.py --guided ball` and compare with the 2026-10-22 baseline above. +- **Down to about 1 cm:** calibration was the main problem. Stay on ssl-vision with our + calibration tool (option 1), and assumption 2 is wrong. +- **Not down:** the error is elsewhere (colour, ssl-vision's simpler camera model), which + points to our own stack. + +### Experiment 2: are tags better than colour patterns? + +- An AprilTag (8–10 cm) taped on one robot; record both cameras while pushing it slowly across + the field, corners included. +- Offline, compare the tag detector with ssl-vision's pattern on the same frames: detection + rate, position and heading jitter, ID swaps, corners against the centre, moving against still. +- Whether tags are big enough in pixels depends on the camera resolution; this answers it. + +### Results + +None yet. Record each result here with its date, the recording it used and the numbers. + +## Order of work, each step with a pass mark + +1. **Recorder:** synchronised frames from both cameras to disk. Needed for everything after, + and it moves most of the work off test days. +2. **Calibration tool, offline on recorded frames.** Pass: guided error under 1 cm on both + cameras. +3. **Live tag detector and SSL output.** Pass: beside ssl-vision, fewer dropouts and less + jitter, no ID swaps, and corners as good as the centre. +4. **Ball detector.** Pass: no false balls on an empty field, and the ball tracked while a + robot moves it. +5. Retire ssl-vision on lab days. + +## Risks + +- Tags blur on fast robots, and may be too few pixels at the far corners. Experiment 2's + recordings show both early; the fixes are a shorter exposure or bigger tags. +- A lens that changes during a session (focus hunting) breaks the calibration mid-run; the + movement watch (design 2) is meant to catch it. +- Lab-only: the team still needs ssl-vision and patterns working for competitions, so the + patterns on the robots stay usable alongside the tags. + +## Open questions + +- Camera models, lenses and resolutions of both cameras (the RER webcam is 1280x960 MJPEG). +- Whether the cameras' focus and zoom are software controls (`v4l2-ctl`) that a startup script + can set, even if they can't be locked physically. +- SSH access to the vision laptop, and which ssl-vision commit it runs. diff --git a/docs/roadmap.md b/docs/roadmap.md index fb02e9c10..cad07f0b8 100644 --- a/docs/roadmap.md +++ b/docs/roadmap.md @@ -218,6 +218,14 @@ entry by its title, not its number. not yet rank strategies better than goals, so it explains results rather than saving matches (`sim_realism.md`, "P1"). P2 (structured strategy search scored by it) is not started. -13. **Deferred, revisit only when forced** (minimalism): +13. **In-house vision for lab days (plan, 2026-10-24):** [`in_house_vision.md`](in_house_vision.md). + The 2026-10-22 test day lost most of its time to ssl-vision: cameras 5–8.5 cm apart and 3–9 cm off known spots, colour + calibration slow and noisy at the edges, fake balls, no way to lock focus or zoom. The plan: + per-session tag calibration of both cameras, tags on robots, an orange ball inside the field + mask, sent as standard SSL packets so our stack is unchanged. Lab only; competitions keep + the league's ssl-vision. Two experiments (calibration alone; tags against patterns) come + first; nothing is built yet. + +14. **Deferred, revisit only when forced** (minimalism): - `@pytest.mark.engine` marker for kernel tests — wait for real CI bottleneck data. - `TickContext` responsibilities beyond `motion_controller`/`match_log`. From 52875ce6705f721951db052a1d3c0605df094a4e Mon Sep 17 00:00:00 2001 From: isaac0804 Date: Sat, 24 Oct 2026 14:58:30 +0100 Subject: [PATCH 46/56] fix(run): the fence holds only edges inside the field, and fences a kicker-less push With the cameras on the whole field (and in grSim by default) the fence was the touch and goal lines: a robot could not cross them to fetch a ball out of play, so a free kick or placement there never completed, and the keeper could not step into its goal mouth. Only edges inside the field (the half-way line on a half-field run) are fenced now; none when the cameras see it all. The fence also wrapped the profile layer, which turns a kicker-less robot's kick into a 0.3 m push the fence never saw. The profile is outside it now. Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_01TJGHdb8Dht67hn4Lohndf5 --- utama_core/run/field_fence.py | 20 ++++++++ utama_core/run/strategy_runner.py | 23 +++++---- .../tests/strategy_runner/test_field_fence.py | 51 +++++++++++++++++++ 3 files changed, 85 insertions(+), 9 deletions(-) diff --git a/utama_core/run/field_fence.py b/utama_core/run/field_fence.py index 057e607ed..d7197bfa6 100644 --- a/utama_core/run/field_fence.py +++ b/utama_core/run/field_fence.py @@ -7,6 +7,10 @@ caps each robot's speed towards each edge of the area to what it can still stop from before the line; speed away from an edge is never limited, so a robot outside drives back in. It knows the robots' poses, nothing of the tactics, so it holds whichever tactic or skill made the command. + +Only edges inside the field are fenced (the half-way line on a half-field run): a robot must +cross the touch and goal lines to fetch a ball out of play, or a restart never completes +(`custom_referee/actions.py`, `_clamp_to_field_or_ball`), and the cameras see past them. """ from __future__ import annotations @@ -29,6 +33,22 @@ def _max_speed_towards(distance: float) -> float: return BRAKE * (-DELAY + math.sqrt(DELAY * DELAY + 2 * distance / BRAKE)) +def inner_edges(bounds: FieldBounds, full: FieldBounds) -> Optional[FieldBounds]: + """`bounds` with each edge on `full`'s outer lines moved out to infinity; None when every + edge is on them (the cameras see the whole field: nothing to fence).""" + (left, top), (right, bottom) = bounds.top_left, bounds.bottom_right + (f_left, f_top), (f_right, f_bottom) = full.top_left, full.bottom_right + opened = ( + -math.inf if left <= f_left else left, + math.inf if top >= f_top else top, + math.inf if right >= f_right else right, + -math.inf if bottom <= f_bottom else bottom, + ) + if all(math.isinf(edge) for edge in opened): + return None + return FieldBounds(top_left=(opened[0], opened[1]), bottom_right=(opened[2], opened[3])) + + def fence(x: float, y: float, orientation: float, command: RobotCommand, bounds: FieldBounds) -> RobotCommand: """`command` for a robot at (`x`, `y`) facing `orientation`, its speed towards each edge of `bounds` capped; unchanged when no cap applies.""" diff --git a/utama_core/run/strategy_runner.py b/utama_core/run/strategy_runner.py index 9d546748d..073ef8d65 100644 --- a/utama_core/run/strategy_runner.py +++ b/utama_core/run/strategy_runner.py @@ -56,7 +56,7 @@ from utama_core.rsoccer_simulator.src.Utils.sim_noise import SimNoise from utama_core.run import GameGater from utama_core.run.field_check import FieldCheck -from utama_core.run.field_fence import FencedRobotController +from utama_core.run.field_fence import FencedRobotController, inner_edges from utama_core.run.live_status import LiveStatusMixin from utama_core.run.referee_source import OfficialReferee, RefereeSource from utama_core.run.side_runtime import SideRuntime, _mirrored, _turned @@ -1212,7 +1212,7 @@ def _load_robot_controllers(self): my_profiles, opp_profiles = map_colors_to_friendly_enemy( self.my_team_is_yellow, self.yellow_robot_profiles, self.blue_robot_profiles ) - my_robot_controller = self._fenced(self._with_profiles(my_robot_controller, my_profiles), lambda: self.my.game) + my_robot_controller = self._wrapped(my_robot_controller, my_profiles, lambda: self.my.game) self.my.strategy.load_robot_controller(my_robot_controller) self.my.strategy.load_motion_controller(self.my.motion_controller(self.mode, self.rsim_env)) if self.match_log is not None: @@ -1226,9 +1226,7 @@ def _load_robot_controllers(self): if kernel_strategy is not None: kernel_strategy.match_log = self.match_log if self.opp: - opp_robot_controller = self._fenced( - self._with_profiles(opp_robot_controller, opp_profiles), lambda: self.opp.game - ) + opp_robot_controller = self._wrapped(opp_robot_controller, opp_profiles, lambda: self.opp.game) self.opp.strategy.load_robot_controller(opp_robot_controller) self.opp.strategy.load_motion_controller(self.opp.motion_controller(self.mode, self.rsim_env)) @@ -1263,12 +1261,19 @@ def _trusted_ir(self, is_yellow: bool, trusted_ir_robots: Optional[FrozenSet[int ) return frozenset(self._driven_ids(is_yellow) - broken) + def _wrapped(self, controller, profiles: dict[int, RobotProfile], game): + """`controller` fitted to `profiles`, then fenced: the fence sees the command the robot + is sent, a kicker-less robot's push included.""" + return self._with_profiles(self._fenced(controller, game), profiles) + def _fenced(self, controller, game): - """`controller`, its robots kept inside `field_bounds` when vision comes from a camera - (grSim or real): outside it nothing sees them (`field_fence.py`).""" - if self.mode == Mode.RSIM: + """`controller`, its robots kept inside the edges of `field_bounds` that are inside the + field when vision comes from a camera (grSim or real): past them nothing sees them + (`field_fence.py`).""" + bounds = inner_edges(self.field_bounds, self.full_field_dims.full_field_bounds) + if self.mode == Mode.RSIM or bounds is None: return controller - return FencedRobotController(controller, self.field_bounds, game) + return FencedRobotController(controller, bounds, game) def _with_profiles(self, controller, profiles: dict[int, RobotProfile]): """`controller`, wrapped to fit commands to `profiles` when a kicker or dribbler is missing.""" diff --git a/utama_core/tests/strategy_runner/test_field_fence.py b/utama_core/tests/strategy_runner/test_field_fence.py index 79ae29c53..21b3f184d 100644 --- a/utama_core/tests/strategy_runner/test_field_fence.py +++ b/utama_core/tests/strategy_runner/test_field_fence.py @@ -8,6 +8,8 @@ import pytest +from utama_core.config.enums import Mode +from utama_core.config.field_params import STANDARD_FIELD_DIMS from utama_core.entities.data.command import RobotCommand from utama_core.entities.data.vector import Vector2D from utama_core.entities.game.field import FieldBounds @@ -17,7 +19,10 @@ FencedRobotController, _max_speed_towards, fence, + inner_edges, ) +from utama_core.run.strategy_runner import StrategyRunner +from utama_core.team_controller.src.controllers.common.robot_profile import RobotProfile LEFT_HALF = FieldBounds(top_left=(-2.0, 1.5), bottom_right=(0.0, -1.5)) @@ -65,3 +70,49 @@ def test_the_controller_fences_the_robots_it_sees_and_passes_the_rest(): controller.add_robot_commands({0: _cmd(1.0), 1: _cmd(1.0)}) assert sent[0].local_forward_vel == 0.0 assert sent[1].local_forward_vel == 1.0 + + +FULL = STANDARD_FIELD_DIMS.full_field_bounds # 9 m x 6 m: x in [-4.5, 4.5], y in [-3, 3] + + +def test_only_the_edges_inside_the_field_are_fenced(): + left_half = FieldBounds(top_left=(-4.5, 3.0), bottom_right=(0.0, -3.0)) + assert inner_edges(left_half, FULL) == FieldBounds(top_left=(-math.inf, math.inf), bottom_right=(0.0, -math.inf)) + assert inner_edges(FULL, FULL) is None + + +def test_a_robot_may_cross_the_touch_line_to_fetch_a_ball_out_of_play(): + # The ball 0.2 m past the touch line on a free kick: a fence at the line froze the restart. + left_half = inner_edges(FieldBounds(top_left=(-4.5, 3.0), bottom_right=(0.0, -3.0)), FULL) + assert fence(-2.0, 3.0, 0.0, _cmd(left=1.0), left_half).local_left_vel == 1.0 + assert fence(-4.5, 0.0, 0.0, _cmd(-1.0), left_half).local_forward_vel == -1.0 # into its goal mouth + assert fence(0.0, 0.0, 0.0, _cmd(1.0), left_half).local_forward_vel == 0.0 # the half-way line holds + + +def _runner(mode, field_bounds) -> StrategyRunner: + runner = StrategyRunner.__new__(StrategyRunner) + runner.mode, runner.field_bounds, runner.full_field_dims = mode, field_bounds, STANDARD_FIELD_DIMS + return runner + + +def _robot_on_the_half_way_line(): + return SimpleNamespace(friendly_robots={0: SimpleNamespace(p=Vector2D(0.0, 0.0), orientation=0.0)}) + + +def test_a_kicker_less_robots_push_is_fenced_too(): + # The profile turns a kick into a 1 m/s push; fenced before it, the push drove 0.3 m past the line. + sent = {} + left_half = FieldBounds(top_left=(-4.5, 3.0), bottom_right=(0.0, -3.0)) + controller = _runner(Mode.GRSIM, left_half)._wrapped( + SimpleNamespace(add_robot_commands=sent.update), {0: RobotProfile(kicker=False)}, _robot_on_the_half_way_line + ) + controller.add_robot_commands({0: RobotCommand(0.0, 0.0, 0.0, kick=True, chip=False, dribble=False)}) + assert sent[0].local_forward_vel == 0.0 + + +@pytest.mark.parametrize( + "mode, bounds", [(Mode.RSIM, FieldBounds(top_left=(-4.5, 3.0), bottom_right=(0.0, -3.0))), (Mode.GRSIM, FULL)] +) +def test_no_fence_in_rsim_or_when_the_cameras_see_the_whole_field(mode, bounds): + inner = SimpleNamespace(add_robot_commands=lambda commands: None) + assert _runner(mode, bounds)._fenced(inner, _robot_on_the_half_way_line) is inner From fa1f918f319bbaaf663647fdc694b0004f395814 Mon Sep 17 00:00:00 2001 From: isaac0804 Date: Sat, 24 Oct 2026 14:59:59 +0100 Subject: [PATCH 47/56] fix(vision_check): --guided measures each spot from packets after Enter Packets queued while the operator carried the robot to the next spot were read first, so the previous spot (1.1-1.5 m away) and the carry were averaged into the spot's error and the saved baseline. The socket is drained on Enter. Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_01TJGHdb8Dht67hn4Lohndf5 --- tools/vision_check.py | 17 ++++++++++ utama_core/tests/tools/test_vision_check.py | 37 +++++++++++++++++++++ 2 files changed, 54 insertions(+) diff --git a/tools/vision_check.py b/tools/vision_check.py index 6f2c545a5..cfd942f42 100644 --- a/tools/vision_check.py +++ b/tools/vision_check.py @@ -50,6 +50,22 @@ def _socket(): return sock +def drain(sock) -> int: + """Throws away the packets queued on `sock`, and returns how many: in the guided check, those + from while the thing was carried to the next spot.""" + sock.setblocking(False) + dropped = 0 + try: + while True: + sock.recv(65536) + dropped += 1 + except BlockingIOError: + pass + finally: + sock.settimeout(0.5) + return dropped + + def listen(sock, seconds: float) -> dict: """What arrives in `seconds`: senders, packets and detections per camera, the field size and `positions`, (thing, camera) -> [(x, y, orientation)] in mm and rad (orientation None for a ball), @@ -157,6 +173,7 @@ def guided(thing: str, seconds: float) -> None: results = [] for i, (x, y) in enumerate(guide_points(*field), 1): input(f"\n[{i}/9] Put the {thing} at ({x:.0f}, {y:.0f}) mm, then press Enter ") + drain(sock) heard = listen(sock, seconds) point = {"truth": [x, y], "cameras": {}} if thing == "ball": diff --git a/utama_core/tests/tools/test_vision_check.py b/utama_core/tests/tools/test_vision_check.py index a86ef26be..20466e414 100644 --- a/utama_core/tests/tools/test_vision_check.py +++ b/utama_core/tests/tools/test_vision_check.py @@ -1,8 +1,15 @@ """The guided check of `tools/vision_check.py`: the spots it asks for and which ball it takes.""" import importlib.util +import select +import socket +import time from pathlib import Path +from utama_core.team_controller.src.generated_code.ssl_vision_wrapper_pb2 import ( + SSL_WrapperPacket, +) + spec = importlib.util.spec_from_file_location( "vision_check", Path(__file__).resolve().parents[3] / "tools" / "vision_check.py" ) @@ -23,3 +30,33 @@ def test_a_stray_ball_away_from_the_spot_is_left_out(): assert vision_check.nearest_balls(frames, 1500, 1125) == [(1490, 1120, None), (1505, 1130, None)] assert vision_check.nearest_balls(frames, 1500, 1125, limit=vision_check.GUIDE_RADIUS) != [] assert vision_check.mean_pose(vision_check.nearest_balls(frames, 1500, 1125)) == (1497.5, 1125, None) + + +def _robot_at(x: float) -> bytes: + packet = SSL_WrapperPacket() + d = packet.detection + d.frame_number, d.t_capture, d.t_sent, d.camera_id = 0, 0.0, 0.0, 0 + r = d.robots_blue.add() + r.robot_id, r.x, r.y, r.confidence, r.pixel_x, r.pixel_y = 2, x, 0.0, 1.0, 0.0, 0.0 + return packet.SerializeToString() + + +def test_a_spot_is_measured_from_packets_after_enter_not_those_queued_while_walking(): + # Before, the guided check read the backlog first: the robot still at the last spot, or carried. + receiver, sender = socket.socket(socket.AF_INET, socket.SOCK_DGRAM), socket.socket( + socket.AF_INET, socket.SOCK_DGRAM + ) + receiver.bind(("127.0.0.1", 0)) + receiver.settimeout(0.5) + try: + for _ in range(3): + sender.sendto(_robot_at(-1500.0), receiver.getsockname()) # queued: the previous spot + select.select([receiver], [], [], 1.0) + time.sleep(0.05) # all three delivered + assert vision_check.drain(receiver) == 3 + sender.sendto(_robot_at(0.0), receiver.getsockname()) + heard = vision_check.listen(receiver, 0.3) + finally: + receiver.close() + sender.close() + assert [s[0] for s in heard["positions"][("blue 2", 0)]] == [0.0] From 62b5956bc797a0843f1f6bb607bca0b9cb18f038 Mon Sep 17 00:00:00 2001 From: isaac0804 Date: Sat, 24 Oct 2026 15:00:00 +0100 Subject: [PATCH 48/56] fix(vision): a camera at height 0 counts as one whose position is unknown A camera not calibrated yet reports height 0, which gave every robot it sees a view-angle weight of 0: a robot only it saw divided by zero in CameraCombiner. Also corrects the comment: grSim sends camera positions too, so the weighting is active there. Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_01TJGHdb8Dht67hn4Lohndf5 --- .../receivers/vision_receiver.py | 3 +- utama_core/entities/data/raw_vision.py | 3 +- .../receivers/vision_receiver_unit_test.py | 31 +++++++++++++------ 3 files changed, 26 insertions(+), 11 deletions(-) diff --git a/utama_core/data_processing/receivers/vision_receiver.py b/utama_core/data_processing/receivers/vision_receiver.py index 12467b076..839c01246 100644 --- a/utama_core/data_processing/receivers/vision_receiver.py +++ b/utama_core/data_processing/receivers/vision_receiver.py @@ -74,7 +74,8 @@ def _handle_packet(self, vision_packet: SSL_WrapperPacket, data: bytes) -> bool: self._geometry_fired = True if vision_packet.HasField("geometry"): for calib in vision_packet.geometry.calib: - if calib.HasField("derived_camera_world_tz"): + # A height of 0 is a camera not calibrated yet: its view-angle weights would all be 0. + if calib.HasField("derived_camera_world_tz") and calib.derived_camera_world_tz > 0: self._camera_positions[calib.camera_id] = ( calib.derived_camera_world_tx / 1000, calib.derived_camera_world_ty / 1000, diff --git a/utama_core/entities/data/raw_vision.py b/utama_core/entities/data/raw_vision.py index d42ce05c0..7e68ba4bd 100644 --- a/utama_core/entities/data/raw_vision.py +++ b/utama_core/entities/data/raw_vision.py @@ -28,5 +28,6 @@ class RawVisionData: blue_robots: List[RawRobotData] balls: List[RawBallData] camera_id: int - # Where that camera is (x, y, height), from ssl-vision's calibration; None when not sent (rsim, grSim). + # Where that camera is (x, y, height), from the geometry's calibration (ssl-vision, grSim); None when + # not sent (rsim) or not calibrated yet (height 0). camera_position: Optional[Tuple[float, float, float]] = None diff --git a/utama_core/tests/receivers/vision_receiver_unit_test.py b/utama_core/tests/receivers/vision_receiver_unit_test.py index 4531c9fee..94738d9c8 100644 --- a/utama_core/tests/receivers/vision_receiver_unit_test.py +++ b/utama_core/tests/receivers/vision_receiver_unit_test.py @@ -148,11 +148,7 @@ def test_geometry_only_packet_adds_no_frame(): assert buffers[1][0].ts == 5.0 -def test_a_cameras_position_from_geometry_rides_on_its_frames(): - # CameraCombiner weights each camera by how steeply it sees a robot: it needs where ssl-vision - # calibrated each camera to be, which only the geometry packets carry. - buffers = [deque(maxlen=1) for _ in range(2)] - v = VisionReceiver(buffers) +def _geometry_with_camera_0_at(height_mm: float) -> bytes: geometry = SSL_WrapperPacket() field = geometry.geometry.field field.field_length, field.field_width, field.goal_width, field.goal_depth, field.boundary_width = ( @@ -165,14 +161,31 @@ def test_a_cameras_position_from_geometry_rides_on_its_frames(): calib = geometry.geometry.calib.add() calib.camera_id, calib.focal_length, calib.principal_point_x, calib.principal_point_y = 0, 700, 640, 480 calib.distortion, calib.q0, calib.q1, calib.q2, calib.q3, calib.tx, calib.ty, calib.tz = (0,) * 8 - calib.derived_camera_world_tx, calib.derived_camera_world_ty, calib.derived_camera_world_tz = -1728, -57, 2078 - assert v._handle_packet(SSL_WrapperPacket(), geometry.SerializeToString()) + calib.derived_camera_world_tx, calib.derived_camera_world_ty, calib.derived_camera_world_tz = -1728, -57, height_mm + return geometry.SerializeToString() + +def _frames_after_geometry(height_mm: float) -> list: + buffers = [deque(maxlen=1) for _ in range(2)] + v = VisionReceiver(buffers) + assert v._handle_packet(SSL_WrapperPacket(), _geometry_with_camera_0_at(height_mm)) for camera_id in (0, 1): packet = SSL_WrapperPacket() d = packet.detection d.camera_id, d.t_capture, d.t_sent, d.frame_number = camera_id, 1.0, 1.0, 1 assert v._handle_packet(SSL_WrapperPacket(), packet.SerializeToString()) + return [b[0] for b in buffers] + + +def test_a_cameras_position_from_geometry_rides_on_its_frames(): + # CameraCombiner weights each camera by how steeply it sees a robot: it needs where ssl-vision + # calibrated each camera to be, which only the geometry packets carry. + frames = _frames_after_geometry(2078) + assert frames[0].camera_position == (-1.728, -0.057, 2.078) + assert frames[1].camera_position is None # camera 1 sent no calibration + - assert buffers[0][0].camera_position == (-1.728, -0.057, 2.078) - assert buffers[1][0].camera_position is None # camera 1 sent no calibration +def test_a_camera_at_no_height_counts_as_unknown(): + # An uncalibrated camera's height of 0 weighs every robot it sees at 0: one only it saw + # divided by zero in CameraCombiner. + assert _frames_after_geometry(0)[0].camera_position is None From 71086dcfadb9e3d53a27b2aa8b410a877a8eefee Mon Sep 17 00:00:00 2001 From: isaac0804 Date: Sat, 24 Oct 2026 15:34:56 +0100 Subject: [PATCH 49/56] fix(vision): a camera that loses the robot for a frame keeps the ball at its dribbler When camera 1 kept the ball at the dribbler but lost the robot for a frame or two, the robot came from camera 0 alone, 8.5 cm from camera 1's view, and the ball was not shifted: it sat inside the robot in 95 of 3,971 frames of 20261022_211142_real_score_goal.log. CameraCombiner now remembers each camera's gap to the others at each robot (BALL_ROBOT_OFFSET_MEMORY, 0.5 s), and places a robot a camera has just lost at another camera's detection plus that gap. The refiner keeps one combiner across frames. Replayed, that log has no ball inside the robot; the nearest is 94 mm, as in camera 1's own view. Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_01TJGHdb8Dht67hn4Lohndf5 --- utama_core/config/settings.py | 2 + .../data_processing/refiners/position.py | 65 +++++++++++++------ .../tests/refiners/camera_combiner_test.py | 21 ++++++ .../tests/refiners/position_unit_test.py | 14 ++++ 4 files changed, 82 insertions(+), 20 deletions(-) diff --git a/utama_core/config/settings.py b/utama_core/config/settings.py index aa8e1b7d8..acbfb4d50 100644 --- a/utama_core/config/settings.py +++ b/utama_core/config/settings.py @@ -52,6 +52,8 @@ CAMERA_AGREEMENT = 0.1 # CameraCombiner: a ball this close to a robot its camera sees takes that camera's offset at the robot (m) BALL_ROBOT_SHIFT_RANGE = 0.3 +# CameraCombiner: a camera that just lost a robot still places it from its gap to another camera this long (s) +BALL_ROBOT_OFFSET_MEMORY = 0.5 VISION_BOUNDS_BUFFER = 1.0 # CameraCombiner: buffer around field bounds to include in vision (m) # An unseen ball is assumed to roll on at this deceleration (m/s^2): official Div B logs, 1 s or # more after a kick (docs/sim_realism.md). A field's own value comes from its free-rolling balls. diff --git a/utama_core/data_processing/refiners/position.py b/utama_core/data_processing/refiners/position.py index 55b9a7c91..8ef968bbc 100644 --- a/utama_core/data_processing/refiners/position.py +++ b/utama_core/data_processing/refiners/position.py @@ -9,6 +9,7 @@ from utama_core.config.field_params import FieldDimensions from utama_core.config.settings import ( BALL_MERGE_THRESHOLD, + BALL_ROBOT_OFFSET_MEMORY, BALL_ROBOT_SHIFT_RANGE, CAMERA_AGREEMENT, VISION_BOUNDS_BUFFER, @@ -82,6 +83,7 @@ def __init__( False # Only start filtering once we have valid data to filter (i.e. after the first valid game frame) ) + self._combiner = CameraCombiner() self.exp_ball = exp_ball self.allowed_yellow_ids = allowed_yellow_ids self.allowed_blue_ids = allowed_blue_ids @@ -110,7 +112,7 @@ def refine( # class VisionData: ts: float; yellow_robots: List[VisionRobotData]; blue_robots: List[VisionRobotData]; balls: List[VisionBallData] # class VisionRobotData: id: int; x: float; y: float; orientation: float - combined_vision_data: VisionData = CameraCombiner().combine_cameras( + combined_vision_data: VisionData = self._combiner.combine_cameras( frames, bounds=self.vision_bounds, ) @@ -233,6 +235,7 @@ def reset(self): Should be called at the start of each game to ensure no leakage of information between games. """ self._filter_running = False + self._combiner = CameraCombiner() if self._uses_kalman: self.kalman_filters_yellow = {} self.kalman_filters_blue = {} @@ -401,7 +404,16 @@ class CameraCombiner: inside the robot (2026-10-22). Each camera's ball is first moved by that camera's offset at the nearest robot it also sees (within `BALL_ROBOT_SHIFT_RANGE`): the fused robot minus its own detection of it. So a ball seen by one camera only (the robot hides it from the other) - stays where that camera put it relative to the robot.""" + stays where that camera put it relative to the robot. A camera that has just lost the robot + (the ball at its dribbler still in view) puts it where another camera sees it plus the gap + between the two cameras there in the last `BALL_ROBOT_OFFSET_MEMORY` s: the gap is their + calibrations' disagreement, steady over that time. Without it the ball came out inside the + robot (2.4% of one 2026-10-22 log).""" + + def __init__(self): + # (camera id, other camera id, (colour, robot id)) -> (camera's detection minus the other's + # in x, y; frame time) + self._gaps: Dict[tuple, Tuple[float, float, float]] = {} def combine_cameras( self, @@ -422,7 +434,6 @@ def combine_cameras( # maps robot id to the (detection, its camera's position) of every camera that saw it yellow_captured = defaultdict(list) blue_captured = defaultdict(list) - balls_captured = [] # (detection, its camera's position, ids of that frame's robot objects) # Each frame is from a different camera for frame in frames: @@ -433,30 +444,44 @@ def combine_cameras( for br in frame.blue_robots: if self._bounds_check(br.x, br.y, bounds): blue_captured[br.id].append((br, frame.camera_position)) - - for b in frame.balls: - if self._bounds_check(b.x, b.y, bounds): - frame_robots = {id(r) for r in frame.yellow_robots + frame.blue_robots} - balls_captured.append((b, frame.camera_position, frame_robots)) ts.append(frame.ts) avg_yellows = list(map(self._avg_robots, yellow_captured.values())) avg_blues = list(map(self._avg_robots, blue_captured.values())) + fused = {("yellow", r.id): r for r in avg_yellows} | {("blue", r.id): r for r in avg_blues} # Each ball in the fused robots' frame: shifted by its camera's offset at its nearest robot. - fused = [ - (r, fused_r) - for captured, avg in ((yellow_captured, avg_yellows), (blue_captured, avg_blues)) - for seen, fused_r in zip(captured.values(), avg) - for r, _ in seen - ] + seen_by = defaultdict(dict) # (colour, robot id) -> camera id -> that camera's detection + for frame in frames: + for colour, robots in (("yellow", frame.yellow_robots), ("blue", frame.blue_robots)): + for r in robots: + if self._bounds_check(r.x, r.y, bounds): + seen_by[(colour, r.id)][frame.camera_id] = (r.x, r.y) shifted = [] - for b, camera, frame_robots in balls_captured: - near = [(r, f) for r, f in fused if id(r) in frame_robots] - r, f = min(near, key=lambda rf: math.hypot(rf[0].x - b.x, rf[0].y - b.y), default=(None, None)) - if r is not None and math.hypot(r.x - b.x, r.y - b.y) <= BALL_ROBOT_SHIFT_RANGE: - b = RawBallData(b.x + f.x - r.x, b.y + f.y - r.y, b.z, b.confidence) - shifted.append((b, camera)) + for frame in frames: + own = [] # (where this camera puts a robot, the fused robot there) + for key, f in fused.items(): + detections = seen_by[key] + if frame.camera_id in detections: + x, y = detections[frame.camera_id] + for other, (ox, oy) in detections.items(): + self._gaps[(frame.camera_id, other, key)] = (x - ox, y - oy, frame.ts) + own.append((x, y, f)) + continue + # Lost for a moment: where it would be, from another camera and the gap between them. + for other, (ox, oy) in detections.items(): + gap = self._gaps.get((frame.camera_id, other, key)) + if gap is not None and frame.ts - gap[2] <= BALL_ROBOT_OFFSET_MEMORY: + own.append((ox + gap[0], oy + gap[1], f)) + break + for b in frame.balls: + if not self._bounds_check(b.x, b.y, bounds): + continue + near = min(own, key=lambda o: math.hypot(o[0] - b.x, o[1] - b.y), default=None) + if near is not None and math.hypot(near[0] - b.x, near[1] - b.y) <= BALL_ROBOT_SHIFT_RANGE: + x, y, f = near + b = RawBallData(b.x + f.x - x, b.y + f.y - y, b.z, b.confidence) + shifted.append((b, frame.camera_position)) balls = self._combine_balls_by_proximity(shifted) return VisionData(sum(ts) / len(ts), avg_yellows, avg_blues, balls) diff --git a/utama_core/tests/refiners/camera_combiner_test.py b/utama_core/tests/refiners/camera_combiner_test.py index 3737c61a8..5da6cbab0 100644 --- a/utama_core/tests/refiners/camera_combiner_test.py +++ b/utama_core/tests/refiners/camera_combiner_test.py @@ -243,3 +243,24 @@ def test_a_ball_only_one_camera_sees_keeps_its_place_at_the_dribbler(): far = [RawVisionData(0, [], [RawRobotData(2, -0.9, 0.4, 0, 1)], [RawBallData(0.5, 0.4, 0, 0.9)], 0), frames[1]] assert CameraCombiner().combine_cameras(far, infinite_bounds).balls[0].x == 0.5 # no robot near: unmoved + + +def test_a_camera_that_loses_the_robot_for_a_frame_keeps_the_ball_at_its_dribbler(): + # 2026-10-22: camera 1 kept the ball at the dribbler but lost the robot for a frame or two; the + # robot then came from camera 0 alone, 8.5 cm from camera 1's view, and the ball sat inside it + # (95 of 3,971 frames). Camera 1 now places the robot from camera 0 plus their gap just before. + gap, combiner = 0.085, CameraCombiner() + + def frames(ts, camera_1_sees_robot): + robot_1 = [RawRobotData(2, -0.5 + gap, 0.3, 0, 1)] if camera_1_sees_robot else [] + return [ + RawVisionData(ts, [], [RawRobotData(2, -0.5, 0.3, 0, 1)], [], 0), + RawVisionData(ts, [], robot_1, [RawBallData(-0.5 + gap + 0.115, 0.3, 0, 0.9)], 1), + ] + + combiner.combine_cameras(frames(0.0, True), infinite_bounds) + lost = combiner.combine_cameras(frames(0.016, False), infinite_bounds) + assert math.isclose(lost.balls[0].x - lost.blue_robots[0].x, 0.115) + + stale = combiner.combine_cameras(frames(0.6, False), infinite_bounds) # lost longer than the memory + assert math.isclose(stale.balls[0].x - stale.blue_robots[0].x, gap + 0.115) diff --git a/utama_core/tests/refiners/position_unit_test.py b/utama_core/tests/refiners/position_unit_test.py index 8e63521b9..0ca74f904 100644 --- a/utama_core/tests/refiners/position_unit_test.py +++ b/utama_core/tests/refiners/position_unit_test.py @@ -334,6 +334,20 @@ def test_with_no_filtering_a_seen_ball_passes_through(): assert (g.ball.p.x, g.ball.p.y) == pytest.approx((-1.0, 0.5)) +def test_the_refiner_keeps_a_cameras_gap_between_frames(): + # The combiner remembers each camera's gap to the others at a robot; a fresh combiner per + # frame would forget it, and a ball whose camera just lost the robot lands inside the robot. + p = PositionRefiner(full_field_dims, filtering=False) + g = GameFrame(0, True, True, {}, {}, bfac3d(0, 0)) + for ts, robot_1 in ((0.0, [RawRobotData(2, -0.415, 0.3, 0, 1)]), (0.016, [])): + frames = [ + RawVisionData(ts, [], [RawRobotData(2, -0.5, 0.3, 0, 1)], [], 0), + RawVisionData(ts, [], robot_1, [RawBallData(-0.3, 0.3, 0, 0.9)], 1), + ] + g = p.refine(g, frames) + assert g.ball.p.x - g.enemy_robots[2].p.x == pytest.approx(0.115) + + if __name__ == "__main__": test_combining_single_team_combines_single_robot() test_combining_with_robot_not_in_game_adds() From 62584bd9588e885a28f52e7ca05bbf80c8ee20c1 Mon Sep 17 00:00:00 2001 From: isaac0804 Date: Sat, 24 Oct 2026 15:39:44 +0100 Subject: [PATCH 50/56] fix(refiners): the ball is one in the field or a goal, nearest the last, not the most confident 2026-10-22: camera 0 saw up to four steady "balls" 15 cm past the +y touch line, as confident as the real one. The most confident ball, ties going to list order, was a fake 22% of the time, and the ball flickered between them. PositionRefiner now takes a ball inside the field lines or a goal when there is one, then the one nearest the last ball; with no last ball, the most confident. Replayed over the day's logs, no log has more ball jumps of over 0.3 m in one frame; in 20261022_213347_real_score_goal.log they fall from 5,439 to 4, and frames with the ball off the field from 7,317 to 88. Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_01TJGHdb8Dht67hn4Lohndf5 --- docs/in_house_vision.md | 8 +++-- .../data_processing/refiners/position.py | 22 ++++++++++++-- .../tests/refiners/position_unit_test.py | 30 +++++++++++++++++++ 3 files changed, 56 insertions(+), 4 deletions(-) diff --git a/docs/in_house_vision.md b/docs/in_house_vision.md index cd403e313..9acabd1ed 100644 --- a/docs/in_house_vision.md +++ b/docs/in_house_vision.md @@ -44,8 +44,12 @@ Built during the test day; it hides the cameras' disagreement but cannot remove and drops a detection more than `CAMERA_AGREEMENT` (0.1 m) from the best camera's. - The ball is fused across cameras by the same rule, and each camera's ball is first moved by that camera's own error at the robot nearest it (`BALL_ROBOT_SHIFT_RANGE`), so a ball at the - dribbler stays at the dribbler. 2.4% of frames in the log above still put the ball inside the - robot: those where camera 1 sees the ball but loses the robot. + dribbler stays at the dribbler. When a camera loses the robot for a frame or two, it places + the robot from another camera plus the gap between them just before: the 2.4% of frames of + the log above that put the ball inside the robot are gone. +- `PositionRefiner` takes a ball inside the field or a goal over one outside them, then the one + nearest the last ball. Replayed, `20261022_213347_real_score_goal.log`'s ball jumps of over 0.3 m in one frame went + from 5,439 to 4. - `tools/vision_check.py` prints where each camera puts each robot and ball and the gap between them, and with `--guided` checks each camera against 9 known spots and saves the result as a baseline. diff --git a/utama_core/data_processing/refiners/position.py b/utama_core/data_processing/refiners/position.py index 8ef968bbc..6e31ffae1 100644 --- a/utama_core/data_processing/refiners/position.py +++ b/utama_core/data_processing/refiners/position.py @@ -84,6 +84,7 @@ def __init__( ) self._combiner = CameraCombiner() + self._field_dims = full_field_dims self.exp_ball = exp_ball self.allowed_yellow_ids = allowed_yellow_ids self.allowed_blue_ids = allowed_blue_ids @@ -187,8 +188,7 @@ def refine( combined_vision_data.blue_robots, ) - # After the balls have been combined, take the most confident - new_ball = PositionRefiner._get_most_confident_ball(combined_vision_data.balls) + new_ball = self._pick_ball(combined_vision_data.balls, game_frame.ball) # Skip filtering when there's no ball and we don't expect one if new_ball is not None or self.exp_ball: @@ -331,6 +331,24 @@ def _ball_from_vision(ball_data: VisionBallData) -> Ball: zv = Vector3D(0, 0, 0) return Ball(Vector3D(ball_data.x, ball_data.y, ball_data.z), zv, zv) + def _pick_ball(self, balls: List[VisionBallData], last: Optional[Ball]) -> Optional[Ball]: + """The ball among `balls`: one inside the field lines or a goal when any is (a ball rarely + leaves the field, stray orange things off it do: 2026-10-22, four steady "balls" 15 cm + past the touch line, as confident as the real one), then the nearest to `last`, the ball + the frame before. With no last ball, the most confident.""" + inside = [b for b in balls if self._in_field_or_goal(b.x, b.y)] + candidates = inside or balls + if last is None or not candidates: + return PositionRefiner._get_most_confident_ball(candidates) + nearest = min(candidates, key=lambda b: math.hypot(b.x - last.p.x, b.y - last.p.y)) + return PositionRefiner._ball_from_vision(nearest) + + def _in_field_or_goal(self, x: float, y: float) -> bool: + d = self._field_dims + if abs(y) <= d.full_field_half_width and abs(x) <= d.full_field_half_length: + return True + return abs(y) <= d.half_goal_width and abs(x) <= d.full_field_half_length + d.goal_depth + @staticmethod def _get_most_confident_ball(balls: List[VisionBallData]) -> Ball: balls_by_confidence = sorted(balls, key=lambda ball: ball.confidence, reverse=True) diff --git a/utama_core/tests/refiners/position_unit_test.py b/utama_core/tests/refiners/position_unit_test.py index 0ca74f904..4dbefc491 100644 --- a/utama_core/tests/refiners/position_unit_test.py +++ b/utama_core/tests/refiners/position_unit_test.py @@ -348,6 +348,36 @@ def test_the_refiner_keeps_a_cameras_gap_between_frames(): assert g.ball.p.x - g.enemy_robots[2].p.x == pytest.approx(0.115) +def _picked(p, last, balls): + g = GameFrame(0, True, True, {}, {}, last) + return p.refine(g, [RawVisionData(0.1, [], [], balls, 0)]).ball + + +def test_stray_balls_off_the_field_do_not_take_the_ball(): + # 2026-10-22: camera 0 also saw four steady "balls" 15 cm past the +y touch line, as confident + # as the real one; the most confident, ties in list order, picked a fake 22% of the time. + p = PositionRefiner(full_field_dims, filtering=False) + w = full_field_dims.full_field_half_width + fakes = [RawBallData(x, w + 0.15, 0, 1.0) for x in (-0.705, -0.22, -0.075, 0.079)] + real = RawBallData(-0.8, -0.35, 0, 1.0) + for balls in (fakes + [real], [real] + fakes, fakes[:2] + [real] + fakes[2:]): + ball = _picked(p, bfac3d(-0.8, -0.35), balls) + assert (ball.p.x, ball.p.y) == pytest.approx((-0.8, -0.35)) + alone = _picked(p, bfac3d(0, 0), [fakes[0]]) # the only ball: taken, though off the field + assert alone.p.y == pytest.approx(w + 0.15) + + +def test_among_balls_in_play_the_one_nearest_the_last_is_kept(): + p = PositionRefiner(full_field_dims, filtering=False) + balls = [RawBallData(1.0, 0.0, 0, 1.0), RawBallData(-1.0, 0.0, 0, 0.5)] + assert _picked(p, bfac3d(-0.9, 0.0), balls).p.x == pytest.approx(-1.0) + assert _picked(p, None, balls).p.x == pytest.approx(1.0) # no last ball: the most confident + # A ball in the goal is in play (2026-10-22: a shot past the goal line, a blob elsewhere). + in_goal = full_field_dims.full_field_half_length + 0.05 + balls = [RawBallData(1.0, 0.0, 0, 1.0), RawBallData(in_goal, 0.0, 0, 1.0)] + assert _picked(p, bfac3d(in_goal, 0.0), balls).p.x == pytest.approx(in_goal) + + if __name__ == "__main__": test_combining_single_team_combines_single_robot() test_combining_with_robot_not_in_game_adds() From d21aa29a0a2a21ab0ed0da7ec6ca81431b2dd317 Mon Sep 17 00:00:00 2001 From: isaac0804 Date: Sun, 25 Oct 2026 14:34:34 +0000 Subject: [PATCH 51/56] docs(sim_realism): first measurements of our robot from the 2026-10-22 field logs Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_01TJGHdb8Dht67hn4Lohndf5 --- docs/sim_realism.md | 43 +++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 43 insertions(+) diff --git a/docs/sim_realism.md b/docs/sim_realism.md index 67c6fde8e..7e3ff6a4c 100644 --- a/docs/sim_realism.md +++ b/docs/sim_realism.md @@ -414,6 +414,49 @@ Replace league ranges with measurements of our robots. Tests, each short and rep Ranges for our robots then replace the league ranges for our side only. Opponents keep the league ranges. +#### First measurements of our robot (field test 2026-10-22) + +From the logs in `replays/field/20261022_*_real_*.log`, one robot (blue 2, firmware 0), mostly +`real_score_goal` runs. Of 33 recorded minutes, the robot moved for under 3, dribbled for 3.5 +and kicked 9 times, so these are small samples. The radio log (`RADIO_OUT`) says when the +dribbler and kicker were commanded. Kicks and ball slow-down come from one camera's raw ball +detections, because the two cameras disagreed by several cm. Vision noise comes from +`tools/robot_calibration.py`. + +| Measure | Ours | League / sim | +|---|---|---| +| Full kick, launch speed | 1.95–2.37 m/s (3 kicks on one camera; a 4th, from the fused ball, about 2.5); one weak kick at 1.07 | league full power 4.1–6.3; sim 5 (`KICK_SPD`) | +| Kick misfire: the ball moved under 15 cm | 4 of 9, the IR sensor seeing the ball every time | sim never | +| Kick direction, against the robot's heading | +2.6° to +4.9°: a steady offset, about 1° of spread | sim 0 | +| Kick command to the ball moving (camera seen) | 63–89 ms | sim: next tick | +| Ball slide, first 0.4 s after a kick | 1.0–3.5 m/s² | league 2–4; sim 0.65 | +| Ball rolling, after 0.5 s | 0.22–0.37 m/s² | league 0.29; sim 0.65 | +| Robot position noise at rest | 0.2–0.6 mm (2–8 mm in 4 logs where the robot rocked or the cameras traded off) | league 0.4–1.0 | +| Robot heading noise at rest | 0.3–0.5° | league 0.2–0.3 | +| Robot missing from a camera frame | 3–9% | league 0.1–0.9% | +| Ball at rest, position noise | 0.2–0.5 mm | | +| Ball lost while dribbling | 12 losses, nearly all with ≥ 0.85 m/s or ≥ 2.5 rad/s commanded in the 0.7 s before; slow, straight dribbles held 10–15 s | | +| Loop delay (command to motion seen) | 80 ms, from 3 noisy steps in one log | sim 0.05–0.10 when on | + +- **Our kicker is less than half as fast as the league's and the sim's.** The electronics team + says the capacitor's charge, and so the kick strength, depends on how long it has been + charging, and they can't control it. So the sim should treat every kick as a full kick, at + about 2.1 m/s for our robots, with the league range kept for opponents. +- **Misfires aren't explained by the ball's place in the mouth.** On the misfires, vision put the + ball within 22 mm of where it sits when IR sees it, and within 8 mm sideways; full kicks sat + just as far off. Vision in those logs is only good to about ±2 cm, so a ball a centimetre + short of the kicker can't be ruled out. Off-centre balls didn't explain direction either: the + offset was +3° to +5° whatever the ball's sideways place. It is likely the robot's heading as + vision reads it, or the kicker's mounting. +- **The ball on this floor slows like the league's carpet:** it slides, then rolls at about + 0.3 m/s². This is the two-phase friction found in Phase 1, which rsim's single 0.65 doesn't + reproduce. +- **The IR ball sensor never fired with the ball more than 25 cm away.** Where vision puts the + ball while IR fires shows vision's own error at the dribbler. It was steady (95–99 mm from + the robot's centre) after the cameras were retuned, but spread over 66–104 mm earlier. +- **Still unmeasured:** loop delay and acceleration limits (they need a `--calibrate` run, not + strategy runs), spread across robots (one robot), and kick strength against charging time. + ## P1: Value model on real data The xG model is already a small value model fitted on reality, for shots only. The extension From a2e2d21ee78f553266b8326657e03a36e45c53f2 Mon Sep 17 00:00:00 2001 From: isaac0804 Date: Sun, 25 Oct 2026 15:33:31 +0000 Subject: [PATCH 52/56] docs(in_house_vision): the team chose the in-house stack; plan in order with pass marks Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_01TJGHdb8Dht67hn4Lohndf5 --- docs/in_house_vision.md | 84 ++++++++++++++++++++++++++++++++++------- docs/roadmap.md | 7 ++-- 2 files changed, 75 insertions(+), 16 deletions(-) diff --git a/docs/in_house_vision.md b/docs/in_house_vision.md index 9acabd1ed..1d3aa37a8 100644 --- a/docs/in_house_vision.md +++ b/docs/in_house_vision.md @@ -1,7 +1,8 @@ # In-house vision: plan, experiments and findings -Status: plan, 2026-10-24. Nothing below is built unless it says so. Written after the -2026-10-22 test day, where vision took most of the day. +Status: plan, 2026-10-24; on 2026-10-25 the team chose the full in-house stack (below, +"Plan now"). Nothing below is built unless it says so. Written after the 2026-10-22 test day, +where vision took most of the day. The goal is less time spent on vision each test day, and robot positions accurate to about 1 cm instead of 3–9 cm. This applies to **lab and test days only**: at competitions the league @@ -177,17 +178,74 @@ Run on recorded frames where possible, so they need no test-day time beyond the None yet. Record each result here with its date, the recording it used and the numbers. -## Order of work, each step with a pass mark - -1. **Recorder:** synchronised frames from both cameras to disk. Needed for everything after, - and it moves most of the work off test days. -2. **Calibration tool, offline on recorded frames.** Pass: guided error under 1 cm on both - cameras. -3. **Live tag detector and SSL output.** Pass: beside ssl-vision, fewer dropouts and less - jitter, no ID swaps, and corners as good as the centre. -4. **Ball detector.** Pass: no false balls on an empty field, and the ball tracked while a - robot moves it. -5. Retire ssl-vision on lab days. +## Plan now (team decision, 2026-10-25) + +The team chose the full in-house stack. The guided check by hand (`vision_check.py --guided`) +took too long to run every session, and we have one ball, which rolls off its spot. So +experiments 1 and 2 no longer decide anything: their questions become pass marks below. Every +step is measured on recordings, so most of the work happens off test days. + +### Before the next test day (no hardware) + +- **Facts to collect:** + - the camera models, resolution, frame rate and interface (USB/V4L2 or FLIR Spinnaker); + - whether we can SSH into the vision laptop and run Python there. + + Resolution decides whether robot tags are big enough. A robot top fits a tag of about + 100 mm; at 2–3 mm per pixel that is 35–50 pixels, 4–5 pixels per tag cell, which is + marginal. Exposure decides motion blur: 1 m/s for 10 ms is 1 cm, about 4 pixels, so + exposure must be about 2 ms. +- **Vision scorecard** (`tools/vision_bench.py`, to build): replays `replays/field/*.log` + through our vision layer and gives one table: + - robot and ball jitter at rest; + - dropped frames; + - camera disagreement where the cameras overlap; + - how far rolling balls stray from a straight line; + - the spread of the IR-on ball offset (ball in the mouth); + - ball jumps, balls off the field, balls inside a robot, ID swaps. + + Its first run is the ssl-vision baseline from 2026-10-22 that every step below must beat. + Our own stack sends the same `SSL_WrapperPacket`s, so the same scorecard judges it. +- **Calibration solver, offline:** `cv2.aruco` AprilTag 36h11 detection, then a least-squares + fit of each camera's lens, position and tilt and of every loose tag's position (design 1). + It is tested on synthetic images rendered from a known camera, so it is right before it + meets a real one. +- **Print** about 20 A4 tags, 3 with crosshairs for the anchor spots (centre and two + corners), plus a few robot-top tags and boxes about 14 cm high for raised tags. + +### Next test day (about 30 minutes of vision) + +1. **Recorder:** a small script on the vision laptop that saves frames from both cameras to + disk, with ssl-vision stopped, since a camera opens for one program at a time. Needs the + facts above. +2. **Record:** + - the tag layout, still: calibration; + - one robot with a top tag driving a scripted path over the whole field: tag + detection while moving, blur and corners; + - the ball rolled across the field: ball detection and straight-line checks; + - the empty field: flat-field correction. + +### Then, offline on those recordings, each with a pass mark + +3. **Calibration from the tag frames.** Pass: + - the anchors land within 3 mm of their spots; + - the two cameras agree within 1 cm on shared tags; + - raised tags fit as well as floor tags. + + This answers experiment 1 for our own model. +4. **Robot tag detector, sending SSL packets** on a second port, replayed from the robot + recording. Pass: beats the ssl-vision baseline on the scorecard for jitter, dropouts and + ID swaps, corners included. This is experiment 2's question. +5. **Ball detector**, inside the field mask, after flat-field correction. Pass: + - no false balls on the empty field; + - the ball tracked while a robot moves it; + - straight-line error under 1 cm. +6. **Live on a test day**, side by side with ssl-vision, then retire ssl-vision on lab days. + Calibration and the movement watch (design 2) then run every session. + +The IR offset correction (`docs/sim_realism.md`, the IR finding) sits above either stack and +is judged by the same scorecard. It waits until our own calibration shows how much offset is +left. ## Risks diff --git a/docs/roadmap.md b/docs/roadmap.md index cad07f0b8..36107f948 100644 --- a/docs/roadmap.md +++ b/docs/roadmap.md @@ -218,13 +218,14 @@ entry by its title, not its number. not yet rank strategies better than goals, so it explains results rather than saving matches (`sim_realism.md`, "P1"). P2 (structured strategy search scored by it) is not started. -13. **In-house vision for lab days (plan, 2026-10-24):** [`in_house_vision.md`](in_house_vision.md). +13. **In-house vision for lab days (team decision 2026-10-25):** [`in_house_vision.md`](in_house_vision.md). The 2026-10-22 test day lost most of its time to ssl-vision: cameras 5–8.5 cm apart and 3–9 cm off known spots, colour calibration slow and noisy at the edges, fake balls, no way to lock focus or zoom. The plan: per-session tag calibration of both cameras, tags on robots, an orange ball inside the field mask, sent as standard SSL packets so our stack is unchanged. Lab only; competitions keep - the league's ssl-vision. Two experiments (calibration alone; tags against patterns) come - first; nothing is built yet. + the league's ssl-vision. The team chose to build it (2026-10-25). Next: a vision scorecard on the + recorded logs (the ssl-vision baseline to beat), an offline tag calibration solver, then + recording camera frames on the next test day; nothing is built yet. 14. **Deferred, revisit only when forced** (minimalism): - `@pytest.mark.engine` marker for kernel tests — wait for real CI bottleneck data. From 3253fe53e9051dc82732e72c896824f05a233d53 Mon Sep 17 00:00:00 2001 From: isaac0804 Date: Sun, 25 Oct 2026 15:43:56 +0000 Subject: [PATCH 53/56] docs(in_house_vision): how calibration tells floor and raised tags apart, a session, the code Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_01TJGHdb8Dht67hn4Lohndf5 --- docs/in_house_vision.md | 48 ++++++++++++++++++++++++++++++++++++++--- 1 file changed, 45 insertions(+), 3 deletions(-) diff --git a/docs/in_house_vision.md b/docs/in_house_vision.md index 1d3aa37a8..fbafccb89 100644 --- a/docs/in_house_vision.md +++ b/docs/in_house_vision.md @@ -103,9 +103,31 @@ cameras -> capture -> [calibration each session] -> tag detector + ball detector - A few tags on boxes at robot-top height (about 14 cm). A flat floor alone cannot tell a longer lens from a higher camera, which is today's camera-height problem; tags off the floor separate the two, and they calibrate exactly the plane the robot positions come from. -- Press calibrate: one bundle adjustment fits each camera's lens (focal length, image centre, - distortion), its position and tilt, and the loose tags' positions, over hundreds of tag - corners. Both cameras see shared tags, so they agree with each other by construction. +- Tags are told apart by ID. A small config file gives each tag's height and names the + anchors, for example tags 0–19 on the floor (z = 0), tags 20–24 on boxes (z = 0.143 m, + box plus paper, measured once with a ruler), tags 0–2 on the anchor spots. Only the + height is given: the solver finds each loose tag's x, y and rotation. The box height and + the printed tag size are the only things measured by hand. +- Press calibrate: one fit of all the numbers at once (bundle adjustment). Unknowns: + - per camera, about 11: focal length, image centre, 1–2 distortion terms, position + (x, y, height) and tilt; + - per loose tag, 3: x, y and rotation on its plane. + + Measurements: each tag's 4 corners in each image, about 200 coordinates per camera for 25 + tags. The fit moves every number until each tag, projected from its place through its + camera, lands on the corners seen. Both cameras see shared tags, so they agree with each + other by construction. Put a box or two where the cameras overlap, so they are tied at + robot height too. +- Why raised tags fix the camera height: a raised tag is nearer the camera, so it appears + pushed outward, away from the spot under the camera. A tag at height h, a distance d out, + looks like a floor point at about d × H / (H − h). With the camera at H = 3 m, a 14 cm box + 1.5 m out appears about 73 mm further out; were the camera 10 cm higher, about 2.5 mm less. + Corners are found to about 1 mm, so the fit pins the height down. With the camera model + known, a pixel's line of sight gives the field position at any height: robots at the tag's + height (about 14 cm), the ball at its centre (about 2 cm). Tags at a single height, all at + robot height for example, would leave the ball's height poorly fixed. +- The fit prints each tag's error (where its corners land against where they were seen), so + a tag set up wrongly, at a height the config doesn't say, stands out straight away. - Because the camera model alone isn't trusted (assumption 2), fit a smooth correction map from the residual error at the tags: one for robot-top height, one for ball height (about 21 mm). - It ends by running the guided check itself and prints the error per camera. @@ -141,6 +163,26 @@ The same `SSL_WrapperPacket` on the same multicast address, so `CameraCombiner`, and everything above stay unchanged. While both run, send on a second port so the two can be compared side by side on the same scene with `vision_check`. +### A session, start to finish + +1. Set up the cameras; the program sets fixed exposure (about 2 ms, so moving tags don't + blur), gain and white balance, and timestamps each frame at capture. +2. Lay out the tags, press calibrate (about a minute), check the error it prints, and pick + up the floor tags, leaving the few just outside the field lines for the movement watch. +3. Play: per frame, robots from tags and the ball from colour inside the field mask, sent as + SSL packets. Calibration also outputs the field mask and the empty-field picture for + flat-field correction. + +### Code + +A handful of Python modules on OpenCV and numpy, roughly 1,000–1,500 lines: +- `capture` (one backend per camera type); +- `calibrate` (the hardest part); +- `robots`; +- `ball`; +- `send`; +- a config holding camera settings, tag heights and anchors, and the tag-to-robot map. + ### Practical - Python and OpenCV; move to C++ only if measured latency forces it. Tag detection at 60 fps is From 538ba1c549fe415210f842868bcf0e4f34f50fde Mon Sep 17 00:00:00 2001 From: isaac0804 Date: Sun, 25 Oct 2026 15:59:53 +0000 Subject: [PATCH 54/56] feat(game_log): record which code ran, operator and strategy-change notes, and the world beside the log RUN_INFO now holds the command line, the git commit and whether the tree was dirty, and the field bounds. A NOTE record (103) carries notes typed in the Live tab and the runner's own strategy changes (and dropped ones). A run with a game log also writes the filtered world as a columnar replay beside it, saved once at close: a checkpoint took 385 ms ten minutes in. Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_01TJGHdb8Dht67hn4Lohndf5 --- docs/real_robot_setup.md | 6 +++ utama_core/dashboard/static/index.html | 4 ++ utama_core/dashboard/static/live.js | 23 +++++++++- utama_core/dashboard/views/strategy.py | 22 +++++++-- utama_core/replay/game_log_writer.py | 26 ++++++++++- utama_core/run/strategy_runner.py | 45 +++++++++++++++++-- .../tests/dashboard/test_strategy_view.py | 34 +++++++++----- utama_core/tests/replay/test_fingerprint.py | 7 ++- .../tests/replay/test_game_log_writer.py | 41 +++++++++++++++++ .../strategy_runner/test_replace_strategy.py | 14 ++++++ 10 files changed, 200 insertions(+), 22 deletions(-) create mode 100644 utama_core/tests/replay/test_game_log_writer.py diff --git a/docs/real_robot_setup.md b/docs/real_robot_setup.md index 1ad03f82f..179d7d144 100644 --- a/docs/real_robot_setup.md +++ b/docs/real_robot_setup.md @@ -127,6 +127,12 @@ amber, each robot under its own ID and colour, a ball in each corner inside the and a robot pushed by hand across the cameras' overlap moving smoothly (a jump there means the two calibrations disagree). Only then Force Start. +Each run records `replays/field/
+
Tactics
diff --git a/utama_core/dashboard/static/live.js b/utama_core/dashboard/static/live.js index d63cffbd9..486e6411b 100644 --- a/utama_core/dashboard/static/live.js +++ b/utama_core/dashboard/static/live.js @@ -211,11 +211,14 @@ // What each side plays, when the script offers choices (dashboard/views/strategy.py): a list // and Run per side. The server refuses unless halted, and the refusal shows under the buttons. + // A run that records a game log also gets a note box: Enter writes the note into the log. function mountStrategy() { fetch("/strategy/config") .then((r) => (r.ok ? r.json() : null)) - .then((sides) => { - if (!sides) return; + .then((cfg) => { + if (!cfg) return; + const sides = cfg.sides; + if (cfg.notes) mountNote(); const rows = document.getElementById("strategy-rows"); rows.innerHTML = ""; for (const side in sides) { @@ -245,6 +248,22 @@ }); } + function mountNote() { + const input = document.getElementById("log-note"); + document.getElementById("log-note-row").style.display = ""; + input.addEventListener("keydown", (e) => { + if (e.key !== "Enter" || !input.value.trim()) return; + const text = input.value; + fetch("/log/note", { + method: "POST", + headers: { "Content-Type": "application/json" }, + body: JSON.stringify({ text }), + }) + .then((r) => (r.ok ? (input.value = "") : r.text().then((t) => showSendError("note", t || "HTTP " + r.status)))) + .catch((err) => showSendError("note", err.message || "no connection")); + }); + } + function mount() { mountStrategy(); fetch("/referee/config") diff --git a/utama_core/dashboard/views/strategy.py b/utama_core/dashboard/views/strategy.py index fa3d1f152..250ad93e7 100644 --- a/utama_core/dashboard/views/strategy.py +++ b/utama_core/dashboard/views/strategy.py @@ -4,6 +4,7 @@ mapped to functions that build a fresh `AbstractStrategy`, and the name it started with. Running a choice (the one running restarts it) hands a new strategy to `StrategyRunner.replace_strategy`, which takes it at the next tick and only while HALTed; a refusal comes back to the page as a 400. +When the run records a game log, a note box writes the operator's notes into it (`GameLogWriter.note`). """ from __future__ import annotations @@ -34,8 +35,15 @@ def _config_bytes() -> bytes: yellow = {"my": runner.my_team_is_yellow, "opp": not runner.my_team_is_yellow} return json.dumps( { - side: {"team": "yellow" if yellow[side] else "blue", "choices": list(names), "running": running[side]} - for side, names in choices.items() + "sides": { + side: { + "team": "yellow" if yellow[side] else "blue", + "choices": list(names), + "running": running[side], + } + for side, names in choices.items() + }, + "notes": runner.game_log is not None, } ).encode() @@ -43,8 +51,16 @@ def _handle_run(payload: dict) -> None: side, name = payload["side"], payload["name"] if side not in choices or name not in choices[side]: raise ValueError(f"no strategy {name!r} for {side!r}") - runner.replace_strategy(side, choices[side][name]()) + runner.replace_strategy(side, choices[side][name](), name) running[side] = name + def _handle_note(payload: dict) -> None: + text = str(payload.get("text", "")).strip() + if runner.game_log is None: + raise ValueError("this run records no game log") + if text: + runner.game_log.note(text) + server.add_route("/strategy/config", _config_bytes) server.add_command_handler("/strategy/run", _handle_run) + server.add_command_handler("/log/note", _handle_note) diff --git a/utama_core/replay/game_log_writer.py b/utama_core/replay/game_log_writer.py index c4fd6cb40..3d4cd96ab 100644 --- a/utama_core/replay/game_log_writer.py +++ b/utama_core/replay/game_log_writer.py @@ -14,7 +14,11 @@ the radio (12 bytes per robot, `RealRobotController._generate_command_buffer`) and the feedback bytes read back. Commanded against seen motion gives the control delay. - `RUN_INFO` (ours): one JSON object at the start: mode, colours, sides, each team's vision - ID -> firmware ID mapping and robot profiles, so the radio's IDs can be matched to vision's. + ID -> firmware ID mapping and robot profiles, so the radio's IDs can be matched to vision's; + the command line, the git commit (and whether the tree had uncommitted changes) and the field + bounds, so a log can be replayed through the code that ran it. +- `NOTE` (ours): `{"text": ...}`, a note from the operator (the dashboard's note box) or the + runner (a strategy change), so a log's idle stretches and tests can be told apart. Every record is stamped with the receive (or send) time on this machine's clock, `time.time_ns()`, so vision and radio share one clock: their difference is the whole loop, @@ -26,6 +30,7 @@ import json import struct +import subprocess import threading import time from pathlib import Path @@ -40,10 +45,24 @@ # in the whole reader). LOG_HEADER = b"SSL_LOG_FILE" REFEREE, VISION_2014 = 3, 4 -RADIO_OUT, RADIO_IN, RUN_INFO = 100, 101, 102 +RADIO_OUT, RADIO_IN, RUN_INFO, NOTE = 100, 101, 102, 103 _FLUSH_S = 1.0 +def code_version() -> Optional[dict]: + """The git commit this code runs from and whether the tree has uncommitted changes; None + outside a git checkout (an installed package).""" + repo = Path(__file__).resolve().parent + try: + commit = subprocess.run(["git", "rev-parse", "HEAD"], cwd=repo, capture_output=True, text=True, timeout=5) + status = subprocess.run(["git", "status", "--porcelain"], cwd=repo, capture_output=True, text=True, timeout=5) + except (OSError, subprocess.SubprocessError): + return None + if commit.returncode != 0: + return None + return {"commit": commit.stdout.strip(), "dirty": bool(status.stdout.strip())} + + def referee_message(data: RefereeData, command_counter: int) -> Referee: """`data` as the game controller would have sent it (times in us, positions in mm).""" msg = Referee( @@ -100,6 +119,9 @@ def write(self, msg_type: int, payload: bytes, ts_ns: Optional[int] = None) -> N def run_info(self, info: dict) -> None: self.write(RUN_INFO, json.dumps(info).encode()) + def note(self, text: str) -> None: + self.write(NOTE, json.dumps({"text": text}).encode()) + def vision(self, packet: bytes) -> None: self.write(VISION_2014, packet) diff --git a/utama_core/run/strategy_runner.py b/utama_core/run/strategy_runner.py index 073ef8d65..24e44f57b 100644 --- a/utama_core/run/strategy_runner.py +++ b/utama_core/run/strategy_runner.py @@ -3,11 +3,13 @@ import json import logging import signal +import sys import threading import time import warnings from collections import deque from dataclasses import dataclass +from pathlib import Path from typing import TYPE_CHECKING, FrozenSet, List, Optional, Tuple, Union from rich.live import Live @@ -19,6 +21,7 @@ from utama_core.config.settings import ( MAX_CAMERAS, MAX_GAME_HISTORY, + REPLAY_BASE_PATH, ROBOT_FEEDBACK_CONNECTION_TIMEOUT_SECONDS, TIMESTEP, ) @@ -49,7 +52,7 @@ ColumnarReplayWriter, ColumnarReplayWriterConfig, ) -from utama_core.replay.game_log_writer import GameLogWriter +from utama_core.replay.game_log_writer import GameLogWriter, code_version from utama_core.replay.replay_writer import ReplayWriter, ReplayWriterConfig from utama_core.rsoccer_simulator.src.ssl.envs import SSLStandardEnv from utama_core.rsoccer_simulator.src.Utils.gaussian_noise import RsimGaussianNoise @@ -125,6 +128,16 @@ class _RobotPortFeedbackState: last_seen: float +def _replay_name(game_log_path: str) -> str: + """The replay's name for a game log: the same path without `.log`, under `REPLAY_BASE_PATH` + when the log is (`replays/field/X.log` -> `field/X`), else the log's own name.""" + stem = Path(game_log_path).resolve().with_suffix("") + try: + return str(stem.relative_to(REPLAY_BASE_PATH.resolve())) + except ValueError: + return stem.name + + def _record_robot_feedback_responses( feedback_by_port_id: dict[int, _RobotPortFeedbackState], responses: List[RobotResponse], @@ -265,7 +278,9 @@ class StrategyRunner(SimRecoveryMixin, VisionStreamStatusMixin, LiveStatusMixin) blue_robot_profiles (dict[int, RobotProfile], optional): Same as ``yellow_robot_profiles`` for the blue team. game_log_path (str, optional): Record the run as an SSL game log (`.log`): every vision packet, the referee's state and, on real robots, the radio's bytes both ways; see `GameLogWriter`. - grSim and real mode only (rsim has no packets). + grSim and real mode only (rsim has no packets). Unless ``replay_writer_config`` is given, the + world the strategy saw is also saved as a columnar replay beside it, at close + (`replays/field/X.log` -> `replays/field/X.npz`). """ def __init__( @@ -394,8 +409,22 @@ def __init__( "yellow": {k: dataclasses.asdict(p) for k, p in self.yellow_robot_profiles.items()}, "blue": {k: dataclasses.asdict(p) for k, p in self.blue_robot_profiles.items()}, }, + "command": sys.argv, + "code": code_version(), + "field_bounds": { + "top_left": [float(v) for v in self.field_bounds.top_left], + "bottom_right": [float(v) for v in self.field_bounds.bottom_right], + }, } ) + # The world as the strategy saw it (after the refiners and filters), beside the log: + # the log holds only what went in and out. Saved once, at close: a checkpoint rewrites the + # whole replay on the control loop (385 ms ten minutes in), and a crash loses only what + # replaying the log rebuilds. + if replay_writer_config is None: + replay_writer_config = ColumnarReplayWriterConfig( + replay_name=_replay_name(game_log_path), checkpoint_every_s=0 + ) for is_yellow in (True, False): self._check_profiles_are_driven(is_yellow) yellow_trusted_ir_robots = self._trusted_ir(True, yellow_trusted_ir_robots) @@ -432,6 +461,7 @@ def __init__( self._field_check_rows: list[dict] = [] self._auto_halt: Optional[str] = None # the auto-HALT in force's reason, shown on the dashboard self._pending_strategies: dict[str, AbstractStrategy] = {} # side -> its replacement, see replace_strategy + self._pending_strategy_names: dict[str, str] = {} ### functions below rely on self.my and self.opp ### @@ -976,11 +1006,12 @@ def _make_field_check(self) -> Optional[FieldCheck]: expected = [(colour, rid) for colour, ids in driven.items() for rid in ids] return FieldCheck(expected, self.field_bounds, self.full_field_dims) - def replace_strategy(self, side: str, strategy: AbstractStrategy) -> None: + def replace_strategy(self, side: str, strategy: AbstractStrategy, name: str = "") -> None: """Run `strategy` for `side` ("my" or "opp") from the next tick, in place of the one running: a different one, or a fresh copy of the same to restart it. Vision, the radio and the dashboard carry on. Only while HALTed, so no robot is mid-move when its strategy - changes: raises ValueError otherwise (the dashboard shows it). Safe from any thread.""" + changes: raises ValueError otherwise (the dashboard shows it). Safe from any thread. + `name` is what the game log's note calls it (default: the strategy's class).""" runtime = self.my if side == "my" else self.opp if side == "opp" else None if runtime is None: raise ValueError(f"no {side!r} side in this run") @@ -991,6 +1022,7 @@ def replace_strategy(self, side: str, strategy: AbstractStrategy) -> None: f"needs control scheme {strategy.control_scheme!r}, this run uses {runtime.control_scheme!r}" ) self._pending_strategies[side] = strategy + self._pending_strategy_names[side] = name @staticmethod def _halted(runtime: SideRuntime) -> bool: @@ -1002,9 +1034,14 @@ def _swap_in_pending_strategy(self, runtime: SideRuntime, side: str) -> None: new = self._pending_strategies.pop(side, None) if new is None: return + name = self._pending_strategy_names.pop(side, "") or type(new).__name__ if not self._halted(runtime): # play restarted between the request and this tick self.logger.warning("Strategy change for %s dropped: no longer halted", side) + if self.game_log is not None: + self.game_log.note(f"{side} strategy change to {name} dropped: no longer halted") return + if self.game_log is not None: + self.game_log.note(f"{side} strategy: {name}") old = runtime.strategy if old.rsim_env is not None: new.load_rsim_env(old.rsim_env) diff --git a/utama_core/tests/dashboard/test_strategy_view.py b/utama_core/tests/dashboard/test_strategy_view.py index bfdc45d00..00fb084ef 100644 --- a/utama_core/tests/dashboard/test_strategy_view.py +++ b/utama_core/tests/dashboard/test_strategy_view.py @@ -11,38 +11,52 @@ from utama_core.dashboard.views import strategy as strategy_view -def _attach(replace): +def _attach(replace, game_log=None): routes, handlers = {}, {} server = SimpleNamespace(add_route=routes.__setitem__, add_command_handler=handlers.__setitem__) - runner = SimpleNamespace(my_team_is_yellow=False, replace_strategy=replace) + runner = SimpleNamespace(my_team_is_yellow=False, replace_strategy=replace, game_log=game_log) built = [] choices = { "my": {"lead": lambda: built.append("lead") or "lead", "press": lambda: built.append("press") or "press"}, "opp": {"goalkeeper": lambda: "goalkeeper"}, } strategy_view.attach(server, runner, choices, running={"my": "lead", "opp": "goalkeeper"}) - return routes["/strategy/config"], handlers["/strategy/run"], built + return routes["/strategy/config"], handlers["/strategy/run"], built, handlers["/log/note"] def test_run_builds_a_fresh_strategy_for_the_runner_and_the_config_shows_it_running(): replaced = [] - config, run, built = _attach(lambda side, s: replaced.append((side, s))) + config, run, built, _ = _attach(lambda side, s, name: replaced.append((side, s, name))) run({"side": "my", "name": "press"}) run({"side": "my", "name": "press"}) # a restart: built afresh again - assert replaced == [("my", "press"), ("my", "press")] and built == ["press", "press"] + assert replaced == [("my", "press", "press"), ("my", "press", "press")] and built == ["press", "press"] assert json.loads(config()) == { - "my": {"team": "blue", "choices": ["lead", "press"], "running": "press"}, - "opp": {"team": "yellow", "choices": ["goalkeeper"], "running": "goalkeeper"}, + "sides": { + "my": {"team": "blue", "choices": ["lead", "press"], "running": "press"}, + "opp": {"team": "yellow", "choices": ["goalkeeper"], "running": "goalkeeper"}, + }, + "notes": False, } def test_a_refused_run_keeps_what_is_shown_running(): - def refuse(side, s): + def refuse(side, s, name): raise ValueError("press Halt first") - config, run, _ = _attach(refuse) + config, run, _, _ = _attach(refuse) with pytest.raises(ValueError, match="Halt"): run({"side": "my", "name": "press"}) with pytest.raises(ValueError, match="no strategy"): run({"side": "opp", "name": "press"}) - assert json.loads(config())["my"]["running"] == "lead" + assert json.loads(config())["sides"]["my"]["running"] == "lead" + + +def test_a_note_goes_into_the_game_log_and_needs_one(): + notes = [] + config, _, _, note = _attach(lambda *a: None, game_log=SimpleNamespace(note=notes.append)) + note({"text": " ball placed by hand "}) + note({"text": ""}) # an empty box writes nothing + assert notes == ["ball placed by hand"] and json.loads(config())["notes"] is True + _, _, _, note = _attach(lambda *a: None) + with pytest.raises(ValueError, match="no game log"): + note({"text": "x"}) diff --git a/utama_core/tests/replay/test_fingerprint.py b/utama_core/tests/replay/test_fingerprint.py index 56c6ae925..d62ccabf8 100644 --- a/utama_core/tests/replay/test_fingerprint.py +++ b/utama_core/tests/replay/test_fingerprint.py @@ -385,6 +385,8 @@ def test_the_base_holds_the_sim_script_and_referee_profiles(graph): # analysis, never during play. # subprocess: robosim_wrapper starts robosim_subprocess.py in the robosim pixi env -> # SUBPROCESS_SCRIPTS, and environment_pin's robosim env listing and binary hashes. +# game_log_writer asks git for the commit it records, only for a camera run's game log +# (never in rsim), so no match result depends on it. # getattr on a module by computed name: tools.evaluation.match looks a factory up by name -> # the config names are the fingerprint's own inputs. # module-level state: code in the closure, so already hashed; its values must not carry @@ -413,7 +415,10 @@ def test_the_base_holds_the_sim_script_and_referee_profiles(graph): "utama_core.replay.game_log_writer", "utama_core.replay.replay_writer", ], - "subprocess": ["utama_core.rsoccer_simulator.src.Simulators.robosim.robosim_wrapper"], + "subprocess": [ + "utama_core.replay.game_log_writer", + "utama_core.rsoccer_simulator.src.Simulators.robosim.robosim_wrapper", + ], } diff --git a/utama_core/tests/replay/test_game_log_writer.py b/utama_core/tests/replay/test_game_log_writer.py new file mode 100644 index 000000000..e99ea861e --- /dev/null +++ b/utama_core/tests/replay/test_game_log_writer.py @@ -0,0 +1,41 @@ +"""What a field test's game log records besides the packets: notes, and which code ran.""" + +from __future__ import annotations + +import json +import subprocess + +from utama_core.config.settings import REPLAY_BASE_PATH +from utama_core.replay import game_log_writer +from utama_core.replay.game_log_writer import NOTE, GameLogWriter, code_version +from utama_core.replay.ssl_log import read_records +from utama_core.run.strategy_runner import _replay_name + + +def test_a_note_is_a_record_in_the_log(tmp_path): + log = GameLogWriter(tmp_path / "run.log") + log.note("ball placed by hand") + log.close() + assert [(k, json.loads(d)) for _, k, d in read_records(tmp_path / "run.log")] == [ + (NOTE, {"text": "ball placed by hand"}) + ] + + +def test_the_code_version_is_this_checkouts_commit_or_none_without_git(monkeypatch): + version = code_version() + assert len(version["commit"]) == 40 and isinstance(version["dirty"], bool) + + def no_git(*args, **kwargs): + raise FileNotFoundError("git") + + monkeypatch.setattr(game_log_writer.subprocess, "run", no_git) + assert code_version() is None + monkeypatch.setattr( + game_log_writer.subprocess, "run", lambda *a, **k: subprocess.CompletedProcess(a, 128, "", "not a repo") + ) + assert code_version() is None + + +def test_the_replay_sits_beside_the_log(): + assert _replay_name(str(REPLAY_BASE_PATH / "field" / "20261022_213347_real.log")) == "field/20261022_213347_real" + assert _replay_name("/somewhere/else/run.log") == "run" diff --git a/utama_core/tests/strategy_runner/test_replace_strategy.py b/utama_core/tests/strategy_runner/test_replace_strategy.py index 2093cbb24..7677497b2 100644 --- a/utama_core/tests/strategy_runner/test_replace_strategy.py +++ b/utama_core/tests/strategy_runner/test_replace_strategy.py @@ -76,6 +76,20 @@ def test_a_replacement_queued_while_halted_is_dropped_if_play_starts_first(runne assert runner.my.strategy is old +def test_a_recorded_run_notes_each_strategy_change_and_each_dropped_one(runner): + # A field test's log said nothing of a strategy changed from the dashboard, so its tests + # couldn't be told apart; a dropped change looked like one that ran. + notes = [] + runner.game_log = type("Log", (), {"note": lambda self, text: notes.append(text), "close": lambda self: None})() + runner._run_step() + runner.replace_strategy("my", _strategy([]), "press") + runner._run_step() + runner.replace_strategy("my", _strategy([]), "lead") + runner.referee.set_command(RefereeCommand.FORCE_START, runner.my.current_game_frame.ts) + runner._run_step() + assert notes == ["my strategy: press", "my strategy change to lead dropped: no longer halted"] + + def test_there_is_no_opp_side_to_replace_without_an_opponent(runner): runner._run_step() with pytest.raises(ValueError, match="opp"): From 4992933c663ccd4ed4a00dccd1e3de16a7948599 Mon Sep 17 00:00:00 2001 From: isaac0804 Date: Sun, 25 Oct 2026 17:23:01 +0000 Subject: [PATCH 55/56] fix(strategy_view): show running only what the runner swapped in; queue a strategy with its name The Strategy panel showed a change as running as soon as it was queued, so one dropped because play restarted first still showed. It now shows StrategyRunner.strategy_names, set at the swap. The dashboard thread queued the strategy and its name in two writes; a swap between them noted the class name and left the chosen name to label the next change. One entry now holds both. Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_01TJGHdb8Dht67hn4Lohndf5 --- utama_core/dashboard/views/strategy.py | 6 +++--- utama_core/run/strategy_runner.py | 19 +++++++++++-------- .../tests/dashboard/test_strategy_view.py | 17 ++++++++++------- .../strategy_runner/test_replace_strategy.py | 18 ++++++++++++++++++ 4 files changed, 42 insertions(+), 18 deletions(-) diff --git a/utama_core/dashboard/views/strategy.py b/utama_core/dashboard/views/strategy.py index 250ad93e7..8a71e68b5 100644 --- a/utama_core/dashboard/views/strategy.py +++ b/utama_core/dashboard/views/strategy.py @@ -4,6 +4,8 @@ mapped to functions that build a fresh `AbstractStrategy`, and the name it started with. Running a choice (the one running restarts it) hands a new strategy to `StrategyRunner.replace_strategy`, which takes it at the next tick and only while HALTed; a refusal comes back to the page as a 400. +What the panel shows running is what the runner swapped in (`StrategyRunner.strategy_names`), so a +change dropped because play restarted first never shows as running. When the run records a game log, a note box writes the operator's notes into it (`GameLogWriter.note`). """ @@ -29,7 +31,6 @@ def attach( ) -> None: """`choices`: side ("my"/"opp") -> {name: build a fresh strategy}; `running`: side -> the name each side started with.""" - running = dict(running) def _config_bytes() -> bytes: yellow = {"my": runner.my_team_is_yellow, "opp": not runner.my_team_is_yellow} @@ -39,7 +40,7 @@ def _config_bytes() -> bytes: side: { "team": "yellow" if yellow[side] else "blue", "choices": list(names), - "running": running[side], + "running": runner.strategy_names.get(side, running[side]), } for side, names in choices.items() }, @@ -52,7 +53,6 @@ def _handle_run(payload: dict) -> None: if side not in choices or name not in choices[side]: raise ValueError(f"no strategy {name!r} for {side!r}") runner.replace_strategy(side, choices[side][name](), name) - running[side] = name def _handle_note(payload: dict) -> None: text = str(payload.get("text", "")).strip() diff --git a/utama_core/run/strategy_runner.py b/utama_core/run/strategy_runner.py index 24e44f57b..4a7fdd8a8 100644 --- a/utama_core/run/strategy_runner.py +++ b/utama_core/run/strategy_runner.py @@ -460,8 +460,10 @@ def __init__( self.field_check = self._make_field_check() self._field_check_rows: list[dict] = [] self._auto_halt: Optional[str] = None # the auto-HALT in force's reason, shown on the dashboard - self._pending_strategies: dict[str, AbstractStrategy] = {} # side -> its replacement, see replace_strategy - self._pending_strategy_names: dict[str, str] = {} + # side -> (its replacement, its name), see replace_strategy: one entry, so the control loop + # never takes a strategy without its name. + self._pending_strategies: dict[str, tuple[AbstractStrategy, str]] = {} + self.strategy_names: dict[str, str] = {} # side -> the name of the strategy swapped in last ### functions below rely on self.my and self.opp ### @@ -1011,7 +1013,7 @@ def replace_strategy(self, side: str, strategy: AbstractStrategy, name: str = "" running: a different one, or a fresh copy of the same to restart it. Vision, the radio and the dashboard carry on. Only while HALTed, so no robot is mid-move when its strategy changes: raises ValueError otherwise (the dashboard shows it). Safe from any thread. - `name` is what the game log's note calls it (default: the strategy's class).""" + `name` is what the game log's note and `strategy_names` call it (default: the strategy's class).""" runtime = self.my if side == "my" else self.opp if side == "opp" else None if runtime is None: raise ValueError(f"no {side!r} side in this run") @@ -1021,8 +1023,7 @@ def replace_strategy(self, side: str, strategy: AbstractStrategy, name: str = "" raise ValueError( f"needs control scheme {strategy.control_scheme!r}, this run uses {runtime.control_scheme!r}" ) - self._pending_strategies[side] = strategy - self._pending_strategy_names[side] = name + self._pending_strategies[side] = (strategy, name) @staticmethod def _halted(runtime: SideRuntime) -> bool: @@ -1031,10 +1032,11 @@ def _halted(runtime: SideRuntime) -> bool: def _swap_in_pending_strategy(self, runtime: SideRuntime, side: str) -> None: """Called on the control loop, just before `runtime`'s strategy steps.""" - new = self._pending_strategies.pop(side, None) - if new is None: + pending = self._pending_strategies.pop(side, None) + if pending is None: return - name = self._pending_strategy_names.pop(side, "") or type(new).__name__ + new, name = pending + name = name or type(new).__name__ if not self._halted(runtime): # play restarted between the request and this tick self.logger.warning("Strategy change for %s dropped: no longer halted", side) if self.game_log is not None: @@ -1052,6 +1054,7 @@ def _swap_in_pending_strategy(self, runtime: SideRuntime, side: str) -> None: if side == "my" and self.match_log is not None and kernel_strategy is not None: kernel_strategy.match_log = self.match_log runtime.strategy = new + self.strategy_names[side] = name self.logger.info("Strategy for %s replaced", side) def _tick_field_check(self, vision_frames: List[Optional[RawVisionData]], now: float) -> None: diff --git a/utama_core/tests/dashboard/test_strategy_view.py b/utama_core/tests/dashboard/test_strategy_view.py index 00fb084ef..aa38dbfb4 100644 --- a/utama_core/tests/dashboard/test_strategy_view.py +++ b/utama_core/tests/dashboard/test_strategy_view.py @@ -14,22 +14,25 @@ def _attach(replace, game_log=None): routes, handlers = {}, {} server = SimpleNamespace(add_route=routes.__setitem__, add_command_handler=handlers.__setitem__) - runner = SimpleNamespace(my_team_is_yellow=False, replace_strategy=replace, game_log=game_log) + runner = SimpleNamespace(my_team_is_yellow=False, replace_strategy=replace, game_log=game_log, strategy_names={}) built = [] choices = { "my": {"lead": lambda: built.append("lead") or "lead", "press": lambda: built.append("press") or "press"}, "opp": {"goalkeeper": lambda: "goalkeeper"}, } strategy_view.attach(server, runner, choices, running={"my": "lead", "opp": "goalkeeper"}) - return routes["/strategy/config"], handlers["/strategy/run"], built, handlers["/log/note"] + return routes["/strategy/config"], handlers["/strategy/run"], built, handlers["/log/note"], runner -def test_run_builds_a_fresh_strategy_for_the_runner_and_the_config_shows_it_running(): +def test_run_builds_a_fresh_strategy_for_the_runner_and_the_config_shows_it_once_swapped_in(): replaced = [] - config, run, built, _ = _attach(lambda side, s, name: replaced.append((side, s, name))) + config, run, built, _, runner = _attach(lambda side, s, name: replaced.append((side, s, name))) run({"side": "my", "name": "press"}) run({"side": "my", "name": "press"}) # a restart: built afresh again assert replaced == [("my", "press", "press"), ("my", "press", "press")] and built == ["press", "press"] + # Queued, not yet swapped in (or dropped because play restarted first): still the old one. + assert json.loads(config())["sides"]["my"]["running"] == "lead" + runner.strategy_names["my"] = "press" assert json.loads(config()) == { "sides": { "my": {"team": "blue", "choices": ["lead", "press"], "running": "press"}, @@ -43,7 +46,7 @@ def test_a_refused_run_keeps_what_is_shown_running(): def refuse(side, s, name): raise ValueError("press Halt first") - config, run, _, _ = _attach(refuse) + config, run, _, _, _ = _attach(refuse) with pytest.raises(ValueError, match="Halt"): run({"side": "my", "name": "press"}) with pytest.raises(ValueError, match="no strategy"): @@ -53,10 +56,10 @@ def refuse(side, s, name): def test_a_note_goes_into_the_game_log_and_needs_one(): notes = [] - config, _, _, note = _attach(lambda *a: None, game_log=SimpleNamespace(note=notes.append)) + config, _, _, note, _ = _attach(lambda *a: None, game_log=SimpleNamespace(note=notes.append)) note({"text": " ball placed by hand "}) note({"text": ""}) # an empty box writes nothing assert notes == ["ball placed by hand"] and json.loads(config())["notes"] is True - _, _, _, note = _attach(lambda *a: None) + _, _, _, note, _ = _attach(lambda *a: None) with pytest.raises(ValueError, match="no game log"): note({"text": "x"}) diff --git a/utama_core/tests/strategy_runner/test_replace_strategy.py b/utama_core/tests/strategy_runner/test_replace_strategy.py index 7677497b2..4dd1d09a0 100644 --- a/utama_core/tests/strategy_runner/test_replace_strategy.py +++ b/utama_core/tests/strategy_runner/test_replace_strategy.py @@ -88,6 +88,24 @@ def test_a_recorded_run_notes_each_strategy_change_and_each_dropped_one(runner): runner.referee.set_command(RefereeCommand.FORCE_START, runner.my.current_game_frame.ts) runner._run_step() assert notes == ["my strategy: press", "my strategy change to lead dropped: no longer halted"] + assert runner.strategy_names == {"my": "press"} # what the dashboard shows running + + +def test_a_swap_between_the_dashboards_writes_still_takes_the_chosen_name(runner): + # The dashboard's thread queued the strategy, then its name, in two writes; the control loop + # swapping in between noted the class name, and the chosen name labelled the next change. + notes = [] + runner.game_log = type("Log", (), {"note": lambda self, text: notes.append(text), "close": lambda self: None})() + runner._run_step() + + class SwapOnWrite(dict): # the control loop takes its turn right after each write + def __setitem__(self, key, value): + super().__setitem__(key, value) + runner._swap_in_pending_strategy(runner.my, key) + + runner._pending_strategies = SwapOnWrite() + runner.replace_strategy("my", _strategy([]), "press") + assert notes == ["my strategy: press"] and runner.strategy_names == {"my": "press"} def test_there_is_no_opp_side_to_replace_without_an_opponent(runner): From 770ae020a60ce5e8af24836c080a20c4027c14b9 Mon Sep 17 00:00:00 2001 From: Fred Huang <60901360+fred-huang122@users.noreply.github.com> Date: Tue, 27 Oct 2026 13:15:07 +0000 Subject: [PATCH 56/56] fix(vision): preserve plausible nearby ball observations (#150) * fix(vision): preserve plausible nearby ball observations Prefer plausible nearby alternatives before per-camera confidence selection, and keep each camera from overweighting close duplicate observations. Preserve observed ball/robot geometry when independent smoothing would move the ball inside its robot, while retaining the existing estimate during camera loss. Add proximity-boundary, confidence, field/goal, camera-loss and minimized real-log regression tests. * fix(vision): retain valid boundary observations and reset dimensions Filter original robot detections by vision bounds before averaging same-camera repeats, and retain field dimensions when resetting the camera combiner. Add 28 regression cases covering both teams, all four vision boundaries, position bias, and repeated resets with smoothing enabled and disabled. All 168 refiner tests pass; formatting and lint checks pass. * fix(ci): separate push and pull request concurrency groups Include the event type in each branch's concurrency group so the push and pull-request checks cannot cancel each other on the same commit. Continue canceling superseded runs for each event and branch. * fix(vision): keep single-camera ticks in fused frame instead of post-Kalman clamp --------- Co-authored-by: isaac0804 --- .../data_processing/refiners/position.py | 167 +++++++-- .../tests/refiners/camera_combiner_test.py | 6 +- .../tests/refiners/one_camera_ball_repro.json | 107 ++++++ .../refiners/test_local_camera_consistency.py | 348 ++++++++++++++++++ 4 files changed, 598 insertions(+), 30 deletions(-) create mode 100644 utama_core/tests/refiners/one_camera_ball_repro.json create mode 100644 utama_core/tests/refiners/test_local_camera_consistency.py diff --git a/utama_core/data_processing/refiners/position.py b/utama_core/data_processing/refiners/position.py index 6e31ffae1..1a443258b 100644 --- a/utama_core/data_processing/refiners/position.py +++ b/utama_core/data_processing/refiners/position.py @@ -2,11 +2,12 @@ from collections import defaultdict from dataclasses import dataclass, replace from functools import partial -from typing import Dict, List, Optional, Tuple +from typing import Dict, List, Optional, Sequence, Tuple import numpy as np from utama_core.config.field_params import FieldDimensions +from utama_core.config.physical_constants import BALL_RADIUS, ROBOT_RADIUS from utama_core.config.settings import ( BALL_MERGE_THRESHOLD, BALL_ROBOT_OFFSET_MEMORY, @@ -26,6 +27,12 @@ from utama_core.global_utils.mapping_utils import map_friendly_enemy_to_colors +def _in_field_or_goal(x: float, y: float, dimensions: FieldDimensions) -> bool: + if abs(y) <= dimensions.full_field_half_width and abs(x) <= dimensions.full_field_half_length: + return True + return abs(y) <= dimensions.half_goal_width and abs(x) <= dimensions.full_field_half_length + dimensions.goal_depth + + @dataclass class VisionBounds: x_min: float @@ -83,7 +90,7 @@ def __init__( False # Only start filtering once we have valid data to filter (i.e. after the first valid game frame) ) - self._combiner = CameraCombiner() + self._combiner = CameraCombiner(full_field_dims) self._field_dims = full_field_dims self.exp_ball = exp_ball self.allowed_yellow_ids = allowed_yellow_ids @@ -105,14 +112,24 @@ def refine( game_frame: GameFrame, data: List[RawVisionData], ) -> GameFrame: - frames = [frame for frame in data if frame is not None] + frames = [ + replace( + frame, + yellow_robots=[ + r for r in frame.yellow_robots if self.allowed_yellow_ids is None or r.id in self.allowed_yellow_ids + ], + blue_robots=[ + r for r in frame.blue_robots if self.allowed_blue_ids is None or r.id in self.allowed_blue_ids + ], + ) + for frame in data + if frame is not None + ] # If no information just return the original if not frames: return game_frame - # class VisionData: ts: float; yellow_robots: List[VisionRobotData]; blue_robots: List[VisionRobotData]; balls: List[VisionBallData] - # class VisionRobotData: id: int; x: float; y: float; orientation: float combined_vision_data: VisionData = self._combiner.combine_cameras( frames, bounds=self.vision_bounds, @@ -235,7 +252,7 @@ def reset(self): Should be called at the start of each game to ensure no leakage of information between games. """ self._filter_running = False - self._combiner = CameraCombiner() + self._combiner = CameraCombiner(self._field_dims) if self._uses_kalman: self.kalman_filters_yellow = {} self.kalman_filters_blue = {} @@ -336,19 +353,13 @@ def _pick_ball(self, balls: List[VisionBallData], last: Optional[Ball]) -> Optio leaves the field, stray orange things off it do: 2026-10-22, four steady "balls" 15 cm past the touch line, as confident as the real one), then the nearest to `last`, the ball the frame before. With no last ball, the most confident.""" - inside = [b for b in balls if self._in_field_or_goal(b.x, b.y)] + inside = [b for b in balls if _in_field_or_goal(b.x, b.y, self._field_dims)] candidates = inside or balls if last is None or not candidates: return PositionRefiner._get_most_confident_ball(candidates) nearest = min(candidates, key=lambda b: math.hypot(b.x - last.p.x, b.y - last.p.y)) return PositionRefiner._ball_from_vision(nearest) - def _in_field_or_goal(self, x: float, y: float) -> bool: - d = self._field_dims - if abs(y) <= d.full_field_half_width and abs(x) <= d.full_field_half_length: - return True - return abs(y) <= d.half_goal_width and abs(x) <= d.full_field_half_length + d.goal_depth - @staticmethod def _get_most_confident_ball(balls: List[VisionBallData]) -> Ball: balls_by_confidence = sorted(balls, key=lambda ball: ball.confidence, reverse=True) @@ -418,7 +429,7 @@ class CameraCombiner: best-placed camera's is dropped, so a camera that disagrees (a poor calibration far from it) does not pull the robot away. Without camera positions every camera counts the same. A ball is fused by the same rule, its detections grouped by being within `BALL_MERGE_THRESHOLD` - of each other: fused one way and the robots another, a ball at a robot's dribbler came out + of every other member: fused one way and the robots another, a ball at a robot's dribbler came out inside the robot (2026-10-22). Each camera's ball is first moved by that camera's offset at the nearest robot it also sees (within `BALL_ROBOT_SHIFT_RANGE`): the fused robot minus its own detection of it. So a ball seen by one camera only (the robot hides it from the other) @@ -428,10 +439,15 @@ class CameraCombiner: calibrations' disagreement, steady over that time. Without it the ball came out inside the robot (2.4% of one 2026-10-22 log).""" - def __init__(self): + def __init__(self, full_field_dims: Optional[FieldDimensions] = None): + self._field_dims = full_field_dims # (camera id, other camera id, (colour, robot id)) -> (camera's detection minus the other's # in x, y; frame time) self._gaps: Dict[tuple, Tuple[float, float, float]] = {} + # (colour, robot id) -> (fused robot, {camera id: (x, y)}, frame time) + self._remembered: Dict[Tuple[str, int], Tuple[VisionRobotData, Dict[int, Tuple[float, float]], float]] = {} + # (camera id, (colour, robot id)) -> (x, y, frame time, camera position) + self._last_seen: Dict[tuple, Tuple[float, float, float, Optional[Tuple[float, float, float]]]] = {} def combine_cameras( self, @@ -448,6 +464,18 @@ def combine_cameras( VisionData: A combined VisionData object containing averaged robot positions and the most confident ball position. """ + frames = [ + replace( + frame, + yellow_robots=self._nearby_robot_duplicates( + [r for r in frame.yellow_robots if self._bounds_check(r.x, r.y, bounds)] + ), + blue_robots=self._nearby_robot_duplicates( + [r for r in frame.blue_robots if self._bounds_check(r.x, r.y, bounds)] + ), + ) + for frame in frames + ] ts = [] # maps robot id to the (detection, its camera's position) of every camera that saw it yellow_captured = defaultdict(list) @@ -464,6 +492,7 @@ def combine_cameras( blue_captured[br.id].append((br, frame.camera_position)) ts.append(frame.ts) + frame_ts = sum(ts) / len(ts) avg_yellows = list(map(self._avg_robots, yellow_captured.values())) avg_blues = list(map(self._avg_robots, blue_captured.values())) fused = {("yellow", r.id): r for r in avg_yellows} | {("blue", r.id): r for r in avg_blues} @@ -475,11 +504,46 @@ def combine_cameras( for r in robots: if self._bounds_check(r.x, r.y, bounds): seen_by[(colour, r.id)][frame.camera_id] = (r.x, r.y) + + # Keep single-camera ticks in the fused frame using the last multi-camera bias. + for key, detections in seen_by.items(): + f = fused[key] + if len(detections) >= 2: + self._remembered[key] = (f, dict(detections), frame_ts) + elif len(detections) == 1: + cam_id, (x, y) = next(iter(detections.items())) + rem = self._remembered.get(key) + if rem is None or frame_ts - rem[2] > BALL_ROBOT_OFFSET_MEMORY: + for (other, last_key), (ox, oy, ot, o_pos) in list(self._last_seen.items()): + if last_key == key and other != cam_id and abs(frame_ts - ot) <= 0.05: + captured = yellow_captured[key[1]] if key[0] == "yellow" else blue_captured[key[1]] + f = self._avg_robots(captured + [(RawRobotData(key[1], ox, oy, f.orientation, 1.0), o_pos)]) + self._remembered[key] = (f, {cam_id: (x, y), other: (ox, oy)}, frame_ts) + self._gaps[(cam_id, other, key)] = (x - ox, y - oy, frame_ts) + self._gaps[(other, cam_id, key)] = (ox - x, oy - y, frame_ts) + rem = self._remembered[key] + break + if rem is not None and frame_ts - rem[2] <= BALL_ROBOT_OFFSET_MEMORY and cam_id in rem[1]: + rx, ry = rem[1][cam_id] + fused[key] = VisionRobotData(f.id, x + rem[0].x - rx, y + rem[0].y - ry, f.orientation) + + for frame in frames: + for colour, robots in (("yellow", frame.yellow_robots), ("blue", frame.blue_robots)): + for r in robots: + self._last_seen[(frame.camera_id, (colour, r.id))] = (r.x, r.y, frame.ts, frame.camera_position) + + avg_yellows = [fused[("yellow", r.id)] for r in avg_yellows] + avg_blues = [fused[("blue", r.id)] for r in avg_blues] + shifted = [] + all_keys = set(fused) | { + k for k, rem in self._remembered.items() if frame_ts - rem[2] <= BALL_ROBOT_OFFSET_MEMORY + } for frame in frames: own = [] # (where this camera puts a robot, the fused robot there) - for key, f in fused.items(): - detections = seen_by[key] + for key in all_keys: + f = fused.get(key) or self._remembered[key][0] + detections = seen_by.get(key, {}) if frame.camera_id in detections: x, y = detections[frame.camera_id] for other, (ox, oy) in detections.items(): @@ -492,6 +556,11 @@ def combine_cameras( if gap is not None and frame.ts - gap[2] <= BALL_ROBOT_OFFSET_MEMORY: own.append((ox + gap[0], oy + gap[1], f)) break + else: + rem = self._remembered.get(key) + if rem is not None and frame.ts - rem[2] <= BALL_ROBOT_OFFSET_MEMORY and frame.camera_id in rem[1]: + rx, ry = rem[1][frame.camera_id] + own.append((f.x - (rem[0].x - rx), f.y - (rem[0].y - ry), f)) for b in frame.balls: if not self._bounds_check(b.x, b.y, bounds): continue @@ -499,10 +568,33 @@ def combine_cameras( if near is not None and math.hypot(near[0] - b.x, near[1] - b.y) <= BALL_ROBOT_SHIFT_RANGE: x, y, f = near b = RawBallData(b.x + f.x - x, b.y + f.y - y, b.z, b.confidence) - shifted.append((b, frame.camera_position)) - balls = self._combine_balls_by_proximity(shifted) + shifted.append((b, frame.camera_position, frame.camera_id)) + balls = self._combine_balls_by_proximity(shifted, avg_yellows + avg_blues) - return VisionData(sum(ts) / len(ts), avg_yellows, avg_blues, balls) + return VisionData(frame_ts, avg_yellows, avg_blues, balls) + + @staticmethod + def _nearby_robot_duplicates(robots: List[RawRobotData]) -> List[RawRobotData]: + """Nearby repeats of one ID in an image are one camera observation.""" + by_id = defaultdict(list) + for robot in robots: + by_id[robot.id].append(robot) + result = [] + for robot_id, seen in by_id.items(): + if len(seen) < 2 or any(math.hypot(a.x - b.x, a.y - b.y) > CAMERA_AGREEMENT for a in seen for b in seen): + result.extend(seen) + continue + n = len(seen) + result.append( + RawRobotData( + robot_id, + sum(r.x for r in seen) / n, + sum(r.y for r in seen) / n, + math.atan2(sum(math.sin(r.orientation) for r in seen), sum(math.cos(r.orientation) for r in seen)), + max(r.confidence for r in seen), + ) + ) + return result @staticmethod def _view_weight(x: float, y: float, camera: Optional[Tuple[float, float, float]]) -> float: @@ -539,20 +631,39 @@ def _avg_robots(self, seen: List[Tuple[RawRobotData, Optional[Tuple[float, float ) return VisionRobotData(base_id, x, y, avg_orientation) - def _combine_balls_by_proximity(self, seen: list) -> List[VisionBallData]: - """One ball per group of detections (`seen`: (detection, camera position)) within - `BALL_MERGE_THRESHOLD` of the group's first, fused like a robot.""" + def _plausible_ball(self, ball: RawBallData, robots: Sequence[VisionRobotData]) -> bool: + in_field = self._field_dims is None or _in_field_or_goal(ball.x, ball.y, self._field_dims) + clear_of_robots = ball.z > BALL_RADIUS or all( + math.hypot(ball.x - robot.x, ball.y - robot.y) >= ROBOT_RADIUS for robot in robots + ) + return in_field and clear_of_robots + + def _combine_balls_by_proximity( + self, + seen: list[tuple[RawBallData, Optional[tuple[float, float, float]], int]], + robots: Sequence[VisionRobotData] = (), + ) -> List[VisionBallData]: + """Fuse close observations, using the strongest candidate per camera in each group.""" groups: list = [] - for b, camera in seen: + for detection in seen: + ball = detection[0] for group in groups: - if math.hypot(b.x - group[0][0].x, b.y - group[0][0].y) < BALL_MERGE_THRESHOLD: - group.append((b, camera)) + if all(math.hypot(ball.x - b.x, ball.y - b.y) < BALL_MERGE_THRESHOLD for b, _, _ in group): + group.append(detection) break else: - groups.append([(b, camera)]) + groups.append([detection]) balls = [] for group in groups: - kept = self._fuse(group) + # Prefer a plausible nearby alternative before confidence can discard it. + # If every candidate is uncertain, retain the existing measurement path. + candidates = [observation for observation in group if self._plausible_ball(observation[0], robots)] or group + by_camera = {} + for ball, camera, camera_id in candidates: + previous = by_camera.get(camera_id) + if previous is None or ball.confidence > previous[0].confidence: + by_camera[camera_id] = (ball, camera) + kept = self._fuse(list(by_camera.values())) total = sum(w for _, w in kept) balls.append( RawBallData( diff --git a/utama_core/tests/refiners/camera_combiner_test.py b/utama_core/tests/refiners/camera_combiner_test.py index 5da6cbab0..18678c749 100644 --- a/utama_core/tests/refiners/camera_combiner_test.py +++ b/utama_core/tests/refiners/camera_combiner_test.py @@ -57,7 +57,7 @@ def test_combine_proximity_multiple_balls(): (RawBallData(0.02, 0.03, 0, 1), None), (RawBallData(1, 1, 0, 1), None), ] - balls = CameraCombiner()._combine_balls_by_proximity(seen) + balls = CameraCombiner()._combine_balls_by_proximity([(b, camera, i) for i, (b, camera) in enumerate(seen)]) assert len(balls) == 3 assert (balls[0].x, balls[0].y) == (0.01, 0.015) @@ -78,7 +78,9 @@ def test_combine_filters_low_confidence(): (RawBallData(5, 5, 0, 0.05), None), (RawBallData(0.02, 0.02, 0, 0.05), None), ] - assert len(CameraCombiner()._combine_balls_by_proximity(seen)) == 2 + assert ( + len(CameraCombiner()._combine_balls_by_proximity([(b, camera, i) for i, (b, camera) in enumerate(seen)])) == 2 + ) # --- Bounds filtering tests --- diff --git a/utama_core/tests/refiners/one_camera_ball_repro.json b/utama_core/tests/refiners/one_camera_ball_repro.json new file mode 100644 index 000000000..7b5565a00 --- /dev/null +++ b/utama_core/tests/refiners/one_camera_ball_repro.json @@ -0,0 +1,107 @@ +[ + [ + { + "ts": 1792699911.262031, + "camera_id": 0, + "yellow_robots": [], + "blue_robots": [], + "balls": [], + "camera_position": [ + -1.728633544921875, + -0.0574510383605957, + 2.077995849609375 + ] + }, + { + "ts": 1792699911.259359, + "camera_id": 1, + "yellow_robots": [], + "blue_robots": [ + { + "id": 2, + "x": -0.6380708618164063, + "y": 0.2897994384765625, + "orientation": -0.20379391312599182, + "confidence": 0.9494209885597229 + } + ], + "balls": [ + { + "x": -0.5209290161132812, + "y": 0.290102294921875, + "z": 0.0, + "confidence": 0.8251418471336365 + } + ], + "camera_position": null + } + ], + [ + { + "ts": 1792699911.295273, + "camera_id": 0, + "yellow_robots": [], + "blue_robots": [ + { + "id": 2, + "x": -0.5537928466796875, + "y": 0.28197738647460935, + "orientation": -0.19114355742931366, + "confidence": 0.9518543481826782 + } + ], + "balls": [], + "camera_position": [ + -1.728633544921875, + -0.0574510383605957, + 2.077995849609375 + ] + } + ], + [], + [ + { + "ts": 1792699911.328597, + "camera_id": 0, + "yellow_robots": [], + "blue_robots": [ + { + "id": 2, + "x": -0.5540905151367187, + "y": 0.2820794677734375, + "orientation": -0.19513890147209167, + "confidence": 0.9517627358436584 + } + ], + "balls": [], + "camera_position": [ + -1.728633544921875, + -0.0574510383605957, + 2.077995849609375 + ] + }, + { + "ts": 1792699911.327261, + "camera_id": 1, + "yellow_robots": [], + "blue_robots": [ + { + "id": 2, + "x": -0.638071533203125, + "y": 0.28938409423828126, + "orientation": -0.1888488084077835, + "confidence": 0.9497724175453186 + } + ], + "balls": [ + { + "x": -0.5207621459960937, + "y": 0.2896724853515625, + "z": 0.0, + "confidence": 0.8302320837974548 + } + ], + "camera_position": null + } + ] +] \ No newline at end of file diff --git a/utama_core/tests/refiners/test_local_camera_consistency.py b/utama_core/tests/refiners/test_local_camera_consistency.py new file mode 100644 index 000000000..567478ecb --- /dev/null +++ b/utama_core/tests/refiners/test_local_camera_consistency.py @@ -0,0 +1,348 @@ +"""Local camera consistency: one physical object, one contribution per camera.""" + +import json +import math +from pathlib import Path + +import pytest + +from utama_core.config.field_params import FieldDimensions +from utama_core.config.physical_constants import ROBOT_RADIUS +from utama_core.data_processing.refiners import PositionRefiner, VelocityRefiner +from utama_core.data_processing.refiners.position import CameraCombiner, VisionBounds +from utama_core.entities.data.raw_vision import RawBallData, RawRobotData, RawVisionData +from utama_core.entities.game import GameFrame, GameHistory + +BOUNDS = VisionBounds(-10, 10, -10, 10) + + +def test_nearby_ball_duplicates_use_the_strongest_detection_per_camera(): + frames = [ + RawVisionData(1, [], [], [RawBallData(0, 0, 0, 0.4), RawBallData(0.02, 0, 0, 0.95)], 0), + RawVisionData(1, [], [], [RawBallData(0.1, 0, 0, 1)], 1), + ] + result = CameraCombiner().combine_cameras(frames, BOUNDS) + assert len(result.balls) == 1 + assert result.balls[0].x == pytest.approx(0.06) # camera 0 winner .02; camera 1 .10 + + +def test_nearby_same_id_robot_duplicates_do_not_give_one_camera_extra_weight(): + frames = [ + RawVisionData(1, [], [RawRobotData(2, 0, 0, 0, 1), RawRobotData(2, 0.02, 0, 0, 1)], [], 0), + RawVisionData(1, [], [RawRobotData(2, 0.1, 0, 0, 1)], [], 1), + ] + result = CameraCombiner().combine_cameras(frames, BOUNDS) + assert len(result.blue_robots) == 1 + assert result.blue_robots[0].x == pytest.approx(0.055) + + +@pytest.mark.parametrize("colour", ["yellow", "blue"]) +@pytest.mark.parametrize("axis,sign", [("x", -1), ("x", 1), ("y", -1), ("y", 1)]) +@pytest.mark.parametrize("inside", [2.45, 2.49, 2.5]) +def test_outside_robot_repeat_cannot_erase_or_bias_an_inside_observation(colour, axis, sign, inside): + bounds = VisionBounds(-2.5, 2.5, -2.5, 2.5) + x, y = (sign * inside, 0) if axis == "x" else (0, sign * inside) + outside_x, outside_y = (sign * 2.55, 0) if axis == "x" else (0, sign * 2.55) + robots = [RawRobotData(2, x, y, 0.4, 0.9), RawRobotData(2, outside_x, outside_y, 1, 0.99)] + frame = RawVisionData(1, robots if colour == "yellow" else [], robots if colour == "blue" else [], [], 0) + + result = CameraCombiner().combine_cameras([frame], bounds) + + combined = result.yellow_robots if colour == "yellow" else result.blue_robots + assert len(combined) == 1 + assert combined[0].id == 2 + assert (combined[0].x, combined[0].y, combined[0].orientation) == pytest.approx((x, y, 0.4)) + + +def test_close_different_robot_identities_are_not_silently_combined(): + frame = RawVisionData(1, [], [RawRobotData(2, 0, 0, 0, 1), RawRobotData(3, 0.08, 0, 0, 1)], [], 0) + assert {r.id for r in CameraCombiner().combine_cameras([frame], BOUNDS).blue_robots} == {2, 3} + + +def test_real_single_camera_ball_does_not_move_inside_robot_after_filtering(): + # Four minimized control inputs from 20261022_211142; only the far camera sees the ball. + steps = json.loads((Path(__file__).with_name("one_camera_ball_repro.json")).read_text()) + position = PositionRefiner( + FieldDimensions(1.5, 1.125, 0.25, 0.5, 0.4, 0.4), + allowed_blue_ids=frozenset({2}), + allowed_yellow_ids=frozenset(), + ) + velocity = VelocityRefiner() + history = GameHistory(20) + current = GameFrame(0, False, False, {}, {}, None) + for step in steps: + frames = [ + RawVisionData( + s["ts"], + [RawRobotData(**r) for r in s["yellow_robots"]], + [RawRobotData(**r) for r in s["blue_robots"]], + [RawBallData(**b) for b in s["balls"]], + s["camera_id"], + s["camera_position"], + ) + for s in step + ] + new = position.refine(current, frames) + if position.filter_running: + new = velocity.refine(history, new) + elif new.ball is not None: + position.start_filtering() + if current.ts: + history.add_game_frame(current) + current = new + ball = current.ball.p + robot = current.friendly_robots[2].p + assert math.hypot(ball.x - robot.x, ball.y - robot.y) >= 0.09 + # The Kalman state must follow the correction rather than springing back next tick. + assert tuple(position.kalman_filter_ball.state[:2]) == pytest.approx((ball.x, ball.y)) + + +def test_ignored_nearby_robot_id_cannot_displace_the_ball_from_active_robot(): + position = PositionRefiner( + FieldDimensions(1.5, 1.125, 0.25, 0.5, 0.4, 0.4), + filtering=False, + allowed_blue_ids=frozenset({2}), + allowed_yellow_ids=frozenset(), + ) + frames = [ + RawVisionData( + 1, [], [RawRobotData(2, 0, 0, 0, 1), RawRobotData(3, 0.01, 0, 0, 1)], [RawBallData(0.115, 0, 0, 1)], 0 + ), + RawVisionData(1, [], [RawRobotData(2, 0.08, 0, 0, 1)], [], 1), + ] + result = position.refine(GameFrame(0, False, False, {}, {}, None), frames) + assert set(result.friendly_robots) == {2} + assert result.ball.p.x - result.friendly_robots[2].p.x == pytest.approx(0.115) + + +def test_far_camera_ball_keeps_offset_when_nearer_camera_dominates_robot_pose(): + frames = [ + RawVisionData(1, [], [RawRobotData(2, 0.8, 0, 0, 1)], [RawBallData(0.915, 0, 0, 1)], 0, (-1.73, 0, 2.08)), + RawVisionData(1, [], [RawRobotData(2, 0.885, 0, 0, 1)], [], 1, (1.73, 0, 2.08)), + ] + result = CameraCombiner().combine_cameras(frames, BOUNDS) + assert result.blue_robots[0].x > 0.8425 + assert result.balls[0].x - result.blue_robots[0].x == pytest.approx(0.115) + assert frames[0].balls[0].x == pytest.approx(0.915) # original camera observation stays intact + + +@pytest.mark.parametrize("reverse", [False, True]) +def test_a_stronger_distant_candidate_cannot_displace_the_nearby_track(reverse): + position = PositionRefiner(FieldDimensions(1.5, 1.125, 0.25, 0.5, 0.4, 0.4), filtering=False) + start = RawVisionData(1, [], [], [RawBallData(0, 0, 0, 0.8)], 0) + current = position.refine(GameFrame(0, False, False, {}, {}, None), [start]) + candidates = [RawBallData(0.02, 0, 0, 0.8), RawBallData(0.04, 0, 0, 0.9), RawBallData(1.4, 0, 0, 1)] + if reverse: + candidates.reverse() + frame = RawVisionData(1.02, [], [], candidates, 0) + result = position.refine(current, [frame]) + assert result.ball.p.x == pytest.approx(0.04) + assert len(frame.balls) == 3 # do not alter original camera observations + + +def test_a_chain_of_nearby_candidates_cannot_merge_two_distant_endpoints(): + frames = [RawVisionData(1, [], [], [RawBallData(x, 0, 0, 1)], i) for i, x in enumerate((0, -0.19, 0.19))] + balls = CameraCombiner().combine_cameras(frames, BOUNDS).balls + assert len(balls) == 2 # endpoints are 380 mm apart, beyond the 200 mm merge limit + + +def test_a_camera_that_loses_the_ball_stops_contributing_to_fusion(): + combiner = CameraCombiner() + combiner.combine_cameras( + [ + RawVisionData(1, [], [], [RawBallData(0, 0, 0, 1)], 0), + RawVisionData(1, [], [], [RawBallData(0.1, 0, 0, 1)], 1), + ], + BOUNDS, + ) + result = combiner.combine_cameras( + [ + RawVisionData(1.02, [], [], [], 0), + RawVisionData(1.02, [], [], [RawBallData(0.12, 0, 0, 1)], 1), + ], + BOUNDS, + ) + assert result.balls[0].x == pytest.approx(0.12) + missing = combiner.combine_cameras([RawVisionData(1.04, [], [], [], 0), RawVisionData(1.04, [], [], [], 1)], BOUNDS) + assert missing.balls == [] + + +@pytest.mark.parametrize("filtering", [False, True]) +def test_strategy_keeps_a_ball_estimate_when_both_cameras_lose_it(filtering): + position = PositionRefiner(FieldDimensions(1.5, 1.125, 0.25, 0.5, 0.4, 0.4), filtering=filtering) + start = RawVisionData(1, [], [], [RawBallData(0.4, 0.2, 0, 1)], 0) + current = position.refine(GameFrame(0, False, False, {}, {}, None), [start]) + position.start_filtering() + for ts in (1.02, 1.04, 2): + result = position.refine(current, [RawVisionData(ts, [], [], [], 0), RawVisionData(ts, [], [], [], 1)]) + assert result.ball is not None + assert (result.ball.p.x, result.ball.p.y) == pytest.approx((0.4, 0.2)) + current = result + assert position.refine(current, [None, None]).ball is current.ball + + +@pytest.mark.parametrize("distance,count", [(0.1999, 1), (0.2, 2), (0.2001, 2)]) +def test_ball_merge_limit_is_strictly_200mm(distance, count): + frames = [ + RawVisionData(1, [], [], [RawBallData(0, 0, 0, 1)], 0), + RawVisionData(1, [], [], [RawBallData(distance, 0, 0, 1)], 1), + ] + assert len(CameraCombiner().combine_cameras(frames, BOUNDS).balls) == count + + +@pytest.mark.parametrize("filtering", [False, True]) +@pytest.mark.parametrize("reverse", [False, True]) +@pytest.mark.parametrize("cameras", [1, 2]) +def test_robot_pattern_cannot_replace_a_plausible_nearby_ball(filtering, reverse, cameras): + position = PositionRefiner( + FieldDimensions(1.5, 1.125, 0.25, 0.5, 0.4, 0.4), filtering=filtering, allowed_blue_ids=frozenset({2}) + ) + + def frames(ts, candidates): + result = [RawVisionData(ts, [], [RawRobotData(2, 0, 0, 0, 1)], candidates, 0)] + if cameras == 2: + result.append(RawVisionData(ts, [], [RawRobotData(2, 0.08, 0, 0, 1)], [], 1)) + return result + + current = position.refine(GameFrame(0, False, False, {}, {}, None), frames(1, [RawBallData(0.115, 0, 0, 0.9)])) + position.start_filtering() + candidates = [RawBallData(0.115, 0, 0, 0.9), RawBallData(0, 0, 0, 0.99)] + if reverse: + candidates.reverse() + observation = frames(1.02, candidates) + result = position.refine(current, observation) + assert result.ball.p.x - result.friendly_robots[2].p.x == pytest.approx(0.115) + assert observation[0].balls == candidates # raw observations remain intact + + +@pytest.mark.parametrize("filtering", [False, True]) +@pytest.mark.parametrize("reverse", [False, True]) +def test_in_field_ball_is_not_discarded_for_a_stronger_nearby_outside_candidate(filtering, reverse): + position = PositionRefiner(FieldDimensions(1.5, 1.125, 0.25, 0.5, 0.4, 0.4), filtering=filtering) + current = position.refine( + GameFrame(0, False, False, {}, {}, None), [RawVisionData(1, [], [], [RawBallData(1.4, 0.8, 0, 0.9)], 0)] + ) + position.start_filtering() + candidates = [RawBallData(1.4, 0.8, 0, 0.9), RawBallData(1.55, 0.8, 0, 0.99)] + if reverse: + candidates.reverse() + result = position.refine(current, [RawVisionData(1.02, [], [], candidates, 0)]) + assert result.ball.p.x == pytest.approx(1.4) + + +@pytest.mark.parametrize("filtering", [False, True]) +@pytest.mark.parametrize("reverse", [False, True]) +def test_reset_preserves_preference_for_an_in_field_ball(filtering, reverse): + position = PositionRefiner(FieldDimensions(1.5, 1.125, 0.25, 0.5, 0.4, 0.4), filtering=filtering) + candidates = [RawBallData(1.4, 0.8, 0, 0.9), RawBallData(1.55, 0.8, 0, 0.99)] + if reverse: + candidates.reverse() + + for _ in range(3): + current = position.refine(GameFrame(0, False, False, {}, {}, None), [RawVisionData(1, [], [], candidates, 0)]) + assert (current.ball.p.x, current.ball.p.y) == pytest.approx((1.4, 0.8)) + position.start_filtering() + result = position.refine(current, [RawVisionData(1.02, [], [], candidates, 0)]) + assert (result.ball.p.x, result.ball.p.y) == pytest.approx((1.4, 0.8)) + position.reset() + + +def test_a_ball_inside_the_goal_is_a_plausible_candidate(): + position = PositionRefiner(FieldDimensions(1.5, 1.125, 0.25, 0.5, 0.4, 0.4), filtering=False) + candidates = [RawBallData(1.4, 0, 0, 0.9), RawBallData(1.55, 0, 0, 0.99)] + result = position.refine(GameFrame(0, False, False, {}, {}, None), [RawVisionData(1, [], [], candidates, 0)]) + assert result.ball.p.x == pytest.approx(1.55) + + +def test_a_raised_ball_is_not_rejected_by_a_flat_robot_footprint(): + position = PositionRefiner(FieldDimensions(1.5, 1.125, 0.25, 0.5, 0.4, 0.4), filtering=False) + frame = RawVisionData( + 1, [], [RawRobotData(2, 0, 0, 0, 1)], [RawBallData(0.115, 0, 0, 0.9), RawBallData(0, 0, 0.2, 0.99)], 0 + ) + result = position.refine(GameFrame(0, False, False, {}, {}, None), [frame]) + assert (result.ball.p.x, result.ball.p.z) == pytest.approx((0, 0.2)) + + +def test_a_single_uncertain_ball_measurement_is_not_erased_by_a_shape_proxy(): + position = PositionRefiner(FieldDimensions(1.5, 1.125, 0.25, 0.5, 0.4, 0.4), filtering=False) + frame = RawVisionData(1, [], [RawRobotData(2, 0, 0, 0, 1)], [RawBallData(0.075, 0, 0, 0.9)], 0) + result = position.refine(GameFrame(0, False, False, {}, {}, None), [frame]) + assert result.ball.p.x == pytest.approx(0.075) + + +def test_a_second_camera_robot_pattern_cannot_pull_a_valid_ball_inside_the_robot(): + position = PositionRefiner(FieldDimensions(1.5, 1.125, 0.25, 0.5, 0.4, 0.4), filtering=False) + frames = [ + RawVisionData(1, [], [RawRobotData(2, 0, 0, 0, 1)], [RawBallData(0.115, 0, 0, 0.9)], 0), + RawVisionData(1, [], [RawRobotData(2, 0.08, 0, 0, 1)], [RawBallData(0.08, 0, 0, 0.99)], 1), + ] + result = position.refine(GameFrame(0, False, False, {}, {}, None), frames) + assert result.ball.p.x - result.friendly_robots[2].p.x == pytest.approx(0.115) + + +def test_a_second_camera_outside_candidate_cannot_pull_a_valid_ball_out_of_field(): + position = PositionRefiner(FieldDimensions(1.5, 1.125, 0.25, 0.5, 0.4, 0.4), filtering=False) + frames = [ + RawVisionData(1, [], [], [RawBallData(1.4, 0.8, 0, 0.9)], 0), + RawVisionData(1, [], [], [RawBallData(1.55, 0.8, 0, 0.99)], 1), + ] + result = position.refine(GameFrame(0, False, False, {}, {}, None), frames) + assert result.ball.p.x == pytest.approx(1.4) + + +def test_single_camera_tick_after_two_camera_fusion_does_not_jump_robot_onto_ball(): + cam_positions = {0: (-1.1, 0.0, 1.73), 1: (1.1, 0.0, 1.73)} + + cam0_t1 = RawVisionData( + ts=1.0, + yellow_robots=[], + blue_robots=[RawRobotData(2, -0.554, 1.084, 0.0, 1.0)], + balls=[], + camera_id=0, + camera_position=cam_positions[0], + ) + cam1_t1 = RawVisionData( + ts=1.0, + yellow_robots=[], + blue_robots=[RawRobotData(2, -0.638, 1.093, 0.0, 1.0)], + balls=[RawBallData(-0.521, 1.076, 0.0, 1.0)], + camera_id=1, + camera_position=cam_positions[1], + ) + + cam0_t2 = RawVisionData( + ts=1.016, + yellow_robots=[], + blue_robots=[RawRobotData(2, -0.554, 1.084, 0.0, 1.0)], + balls=[], + camera_id=0, + camera_position=cam_positions[0], + ) + + combiner = CameraCombiner() + fused_t1 = combiner.combine_cameras([cam0_t1, cam1_t1], BOUNDS) + assert len(fused_t1.blue_robots) == 1 + assert fused_t1.blue_robots[0].x == pytest.approx(-0.577, abs=0.01) + assert fused_t1.blue_robots[0].y == pytest.approx(1.086, abs=0.01) + + fused_t2 = combiner.combine_cameras([cam0_t2], BOUNDS) + assert len(fused_t2.blue_robots) == 1 + assert fused_t2.blue_robots[0].x == pytest.approx(-0.577, abs=0.01) + assert fused_t2.blue_robots[0].y == pytest.approx(1.086, abs=0.01) + assert abs(fused_t2.blue_robots[0].x - (-0.554)) > 0.015 + + position = PositionRefiner( + FieldDimensions(1.5, 1.125, 0.25, 0.5, 0.4, 0.4), + allowed_blue_ids=frozenset({2}), + allowed_yellow_ids=frozenset(), + ) + current = GameFrame(0, False, False, {}, {}, None) + frame1 = position.refine(current, [cam0_t1, cam1_t1]) + position.start_filtering() + + frame2 = position.refine(frame1, [cam0_t2]) + assert frame2.ball is not None + assert 2 in frame2.friendly_robots + dist = math.hypot(frame2.ball.p.x - frame2.friendly_robots[2].p.x, frame2.ball.p.y - frame2.friendly_robots[2].p.y) + assert dist >= ROBOT_RADIUS + assert dist > 0.11