Profile — loading…
@@ -1004,6 +1052,80 @@ def _build_static_config(profile: "RefereeProfile") -> dict:
return `
`;
}
+// --- God mode ---
+let _godMode = false;
+let _ctxFieldPos = null; // {x, y} in field coords at last right-click
+
+function toggleGod() {
+ _godMode = !_godMode;
+ const btn = document.getElementById('god-btn');
+ btn.classList.toggle('active', _godMode);
+ btn.title = _godMode
+ ? 'God mode ON — right-click on the field canvas to move the ball.'
+ : 'God mode: right-click anywhere on the field to move the ball or issue ball placement.';
+}
+
+function _canvasToField(canvas, clientX, clientY) {
+ if (!_cfg) return null;
+ const rect = canvas.getBoundingClientRect();
+ const cw = canvas.width, ch = canvas.height;
+ const M = 12;
+ const scale = (cw - 2 * M) / (2 * _cfg.half_length);
+ const px = (clientX - rect.left) * (cw / rect.width);
+ const py = (clientY - rect.top) * (ch / rect.height);
+ // Inverse of: toX(fx) = M + (fx + half_length) * scale
+ const fx = (px - M) / scale - _cfg.half_length;
+ // Inverse of: toY(fy) = CH - M - (fy + half_width) * scale
+ const fy = (ch - M - py) / scale - _cfg.half_width;
+ return { x: fx, y: fy };
+}
+
+function _hideCtxMenu() {
+ document.getElementById('ctx-menu').style.display = 'none';
+}
+
+document.addEventListener('click', _hideCtxMenu);
+document.addEventListener('keydown', e => { if (e.key === 'Escape') _hideCtxMenu(); });
+
+document.getElementById('field-canvas').addEventListener('contextmenu', function(e) {
+ if (!_godMode) return;
+ e.preventDefault();
+ _ctxFieldPos = _canvasToField(this, e.clientX, e.clientY);
+ if (!_ctxFieldPos) return;
+ const menu = document.getElementById('ctx-menu');
+ menu.style.display = 'block';
+ const mx = Math.min(e.clientX, window.innerWidth - menu.offsetWidth - 4);
+ const my = Math.min(e.clientY, window.innerHeight - menu.offsetHeight - 4);
+ menu.style.left = mx + 'px';
+ menu.style.top = my + 'px';
+});
+
+function ctxPlaceBallYellow() {
+ _hideCtxMenu();
+ if (!_ctxFieldPos) return;
+ fetch('/command', {
+ method: 'POST',
+ headers: { 'Content-Type': 'application/json' },
+ body: JSON.stringify({
+ command: 'BALL_PLACEMENT_YELLOW',
+ designated: [_ctxFieldPos.x, _ctxFieldPos.y],
+ }),
+ }).catch(err => console.error('command error:', err));
+}
+
+function ctxPlaceBallBlue() {
+ _hideCtxMenu();
+ if (!_ctxFieldPos) return;
+ fetch('/command', {
+ method: 'POST',
+ headers: { 'Content-Type': 'application/json' },
+ body: JSON.stringify({
+ command: 'BALL_PLACEMENT_BLUE',
+ designated: [_ctxFieldPos.x, _ctxFieldPos.y],
+ }),
+ }).catch(err => console.error('command error:', err));
+}
+
fetch('/config').then(r => r.json()).then(c => {
document.getElementById('page-title').textContent = 'Custom Referee — ' + c.profile_name;
document.getElementById('cfg-title').textContent = 'Profile: ' + c.profile_name;
diff --git a/utama_core/custom_referee/state_machine.py b/utama_core/custom_referee/state_machine.py
index f194bdcd..aa465de7 100644
--- a/utama_core/custom_referee/state_machine.py
+++ b/utama_core/custom_referee/state_machine.py
@@ -1,633 +1,710 @@
-"""GameStateMachine: owns all mutable game state for the CustomReferee."""
-
-from __future__ import annotations
-
-import copy
-import logging
-import math
-from typing import Optional
-
-from utama_core.config.field_params import STANDARD_FIELD_DIMS
-from utama_core.custom_referee.geometry import RefereeGeometry
-from utama_core.custom_referee.profiles.profile_loader import AutoAdvanceConfig
-from utama_core.custom_referee.rules.base_rule import RuleViolation
-from utama_core.entities.data.referee import RefereeData
-from utama_core.entities.game.game_frame import GameFrame
-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
-
-logger = logging.getLogger(__name__)
-
-_TRANSITION_COOLDOWN = 0.3 # seconds — prevents command oscillation
-_BALL_CLEAR_DIST = 0.5 # metres — all robots must be this far from ball before queued restart
-_KICKER_READY_DIST = 0.3 # metres — kicker must be within this distance to trigger free kick start
-_PLACEMENT_DONE_DIST = 0.15 # metres — ball within this dist of target → placement complete
-_AUTO_ADVANCE_DELAY = 2.0 # seconds — readiness must be sustained this long before play starts
-
-
-class GameStateMachine:
- """Owns score, command, and stage. Produces ``RefereeData`` each tick."""
-
- def __init__(
- self,
- half_duration_seconds: float,
- kickoff_team: str,
- n_robots_yellow: int,
- n_robots_blue: int,
- initial_stage: Stage = Stage.NORMAL_FIRST_HALF_PRE,
- force_start_after_goal: bool = False,
- stop_duration_seconds: float = 3.0,
- prepare_duration_seconds: float = 3.0,
- kickoff_timeout_seconds: float = 10.0,
- geometry: Optional[RefereeGeometry] = None,
- auto_advance: Optional[AutoAdvanceConfig] = None,
- ) -> None:
- self.command = RefereeCommand.HALT
- self.command_counter = 0
- self.command_timestamp = 0.0
-
- self.stage = initial_stage
- # Seeded by seed_clock() after the first valid game frame is available.
- self.stage_start_time: Optional[float] = None
- self.stage_duration = half_duration_seconds
-
- self.yellow_team = TeamInfo(
- name="Yellow",
- score=0,
- red_cards=0,
- yellow_card_times=[],
- yellow_cards=0,
- timeouts=4,
- timeout_time=300,
- goalkeeper=0,
- foul_counter=0,
- ball_placement_failures=0,
- can_place_ball=True,
- max_allowed_bots=n_robots_yellow,
- bot_substitution_intent=False,
- bot_substitution_allowed=True,
- bot_substitutions_left=5,
- )
- self.blue_team = TeamInfo(
- name="Blue",
- score=0,
- red_cards=0,
- yellow_card_times=[],
- yellow_cards=0,
- timeouts=4,
- timeout_time=300,
- goalkeeper=0,
- foul_counter=0,
- ball_placement_failures=0,
- can_place_ball=True,
- max_allowed_bots=n_robots_blue,
- bot_substitution_intent=False,
- bot_substitution_allowed=True,
- bot_substitutions_left=5,
- )
-
- self.next_command: Optional[RefereeCommand] = None
- self.ball_placement_target: Optional[tuple[float, float]] = None
- self.status_message: Optional[str] = None
-
- # Kickoff team initialised from profile.
- self._kickoff_team_is_yellow = kickoff_team.lower() == "yellow"
-
- # Arcade auto-advance: after stop_duration_seconds in STOP following a
- # goal, automatically issue FORCE_START instead of waiting for operator.
- self._force_start_after_goal = force_start_after_goal
- self._stop_duration_seconds = stop_duration_seconds
- self._stop_entered_time: float = -math.inf # wall time when STOP was last entered
-
- # Auto-advance timings.
- self._prepare_duration_seconds = prepare_duration_seconds
- self._kickoff_timeout_seconds = kickoff_timeout_seconds
- self._prepare_entered_time: float = -math.inf # wall time when PREPARE_KICKOFF was entered
- self._normal_start_time: float = -math.inf # wall time when NORMAL_START was entered
-
- # Ball position snapshot at NORMAL_START — used to detect if the ball has moved.
- self._ball_pos_at_normal_start: Optional[tuple[float, float]] = None
-
- # Timestamps for sustained-readiness countdown before play-starting advances.
- # Set to math.inf when condition is not yet met; fire when elapsed >= _AUTO_ADVANCE_DELAY.
- self._advance2_ready_since: float = math.inf # PREPARE_* → NORMAL_START
- self._advance3_ready_since: float = math.inf # DIRECT_FREE_* → NORMAL_START
- self._advance4_ready_since: float = math.inf # BALL_PLACEMENT_* → next_command
-
- # Field geometry (used for readiness checks).
- self._geometry: Optional[RefereeGeometry] = geometry
-
- # Cooldown: don't process a new violation within this window.
- self._last_transition_time: float = -math.inf
-
- # Per-transition enable flags (default: all on).
- self._auto_advance = auto_advance if auto_advance is not None else AutoAdvanceConfig()
-
- # ------------------------------------------------------------------
- # Public API
- # ------------------------------------------------------------------
-
- _PREPARE_KICKOFF_COMMANDS = frozenset(
- {
- RefereeCommand.PREPARE_KICKOFF_YELLOW,
- RefereeCommand.PREPARE_KICKOFF_BLUE,
- }
- )
- _DIRECT_FREE_COMMANDS = frozenset(
- {
- RefereeCommand.DIRECT_FREE_YELLOW,
- RefereeCommand.DIRECT_FREE_BLUE,
- }
- )
- _PREPARE_PENALTY_COMMANDS = frozenset(
- {
- RefereeCommand.PREPARE_PENALTY_YELLOW,
- RefereeCommand.PREPARE_PENALTY_BLUE,
- }
- )
- _BALL_PLACEMENT_COMMANDS = frozenset(
- {
- RefereeCommand.BALL_PLACEMENT_YELLOW,
- RefereeCommand.BALL_PLACEMENT_BLUE,
- }
- )
-
- def step(
- self,
- current_time: float,
- violation: Optional[RuleViolation],
- game_frame: Optional["GameFrame"] = None,
- ) -> RefereeData:
- """Process one tick. Apply violation if not in cooldown. Return RefereeData."""
- if self.stage_start_time is None:
- self.stage_start_time = current_time
-
- if violation is not None and self._can_transition(current_time):
- self._apply_violation(violation, current_time)
-
- # ----------------------------------------------------------------
- # Auto-advance 1: STOP → next queued restart
- # Fires when all robots are ≥ _BALL_CLEAR_DIST from the ball.
- # ----------------------------------------------------------------
- if (
- self._auto_advance.stop_to_next_command
- and self.command == RefereeCommand.STOP
- and self.next_command in self._NEEDS_STOP_FIRST
- and game_frame is not None
- and self._all_robots_clear(game_frame)
- ):
- logger.info("All robots clear — auto-advancing STOP → %s", self.next_command.name)
- self.command = self.next_command
- self.command_counter += 1
- self.command_timestamp = current_time
- if self.command in self._BALL_PLACEMENT_COMMANDS:
- self.next_command = RefereeCommand.NORMAL_START
- self._advance4_ready_since = math.inf
- elif self.command in self._DIRECT_FREE_COMMANDS:
- self.next_command = RefereeCommand.NORMAL_START
- self._advance3_ready_since = math.inf
- elif self.command in self._PREPARE_KICKOFF_COMMANDS or self.command in self._PREPARE_PENALTY_COMMANDS:
- self.next_command = RefereeCommand.NORMAL_START
- self._prepare_entered_time = current_time
- self._advance2_ready_since = math.inf
- self._last_transition_time = current_time
-
- # ----------------------------------------------------------------
- # Auto-advance 2a: PREPARE_KICKOFF_* → NORMAL_START
- # Fires after prepare_duration_seconds AND one attacker is inside
- # the centre circle, sustained for _AUTO_ADVANCE_DELAY seconds.
- # ----------------------------------------------------------------
- elif self._auto_advance.prepare_kickoff_to_normal and self.command in self._PREPARE_KICKOFF_COMMANDS:
- ready = (
- (current_time - self._prepare_entered_time) >= self._prepare_duration_seconds
- and game_frame is not None
- and self._kicker_in_centre_circle(self.command, game_frame)
- )
- if ready:
- if self._advance2_ready_since == math.inf:
- self._advance2_ready_since = current_time
- logger.debug("Advance 2 countdown started (%s)", self.command.name)
- elif (current_time - self._advance2_ready_since) >= _AUTO_ADVANCE_DELAY:
- logger.info(
- "Kicker in centre circle — auto-advancing %s → NORMAL_START",
- self.command.name,
- )
- self.command = RefereeCommand.NORMAL_START
- self.command_counter += 1
- self.command_timestamp = current_time
- self.next_command = None
- self.status_message = None
- self._normal_start_time = current_time
- self._ball_pos_at_normal_start = (
- (game_frame.ball.p.x, game_frame.ball.p.y) if game_frame.ball is not None else None
- )
- self._advance2_ready_since = math.inf
- self._last_transition_time = current_time
- else:
- self._advance2_ready_since = math.inf
-
- # ----------------------------------------------------------------
- # Auto-advance 2b: PREPARE_PENALTY_* → NORMAL_START
- # Fires after prepare_duration_seconds when the kicker reaches the
- # penalty mark, sustained for _AUTO_ADVANCE_DELAY seconds.
- # ----------------------------------------------------------------
- elif self._auto_advance.prepare_penalty_to_normal and self.command in self._PREPARE_PENALTY_COMMANDS:
- ready = (
- (current_time - self._prepare_entered_time) >= self._prepare_duration_seconds
- and game_frame is not None
- and self._penalty_kicker_ready(self.command, game_frame)
- )
- if ready:
- if self._advance2_ready_since == math.inf:
- self._advance2_ready_since = current_time
- logger.debug("Advance 2 countdown started (%s)", self.command.name)
- elif (current_time - self._advance2_ready_since) >= _AUTO_ADVANCE_DELAY:
- logger.info(
- "Kicker at penalty mark — auto-advancing %s → NORMAL_START",
- self.command.name,
- )
- self.command = RefereeCommand.NORMAL_START
- self.command_counter += 1
- self.command_timestamp = current_time
- self.next_command = None
- self.status_message = None
- self._normal_start_time = current_time
- self._ball_pos_at_normal_start = (
- (game_frame.ball.p.x, game_frame.ball.p.y) if game_frame.ball is not None else None
- )
- self._advance2_ready_since = math.inf
- self._last_transition_time = current_time
- else:
- self._advance2_ready_since = math.inf
-
- # ----------------------------------------------------------------
- # Auto-advance 3: DIRECT_FREE_* → NORMAL_START
- # Fires when the kicker is within _KICKER_READY_DIST of the ball
- # AND all defending robots are ≥ _BALL_CLEAR_DIST away, sustained
- # for _AUTO_ADVANCE_DELAY seconds.
- # ----------------------------------------------------------------
- elif self._auto_advance.direct_free_to_normal and self.command in self._DIRECT_FREE_COMMANDS:
- ready = game_frame is not None and self._free_kick_ready(self.command, game_frame)
- if ready:
- if self._advance3_ready_since == math.inf:
- self._advance3_ready_since = current_time
- logger.debug("Advance 3 countdown started (%s)", self.command.name)
- elif (current_time - self._advance3_ready_since) >= _AUTO_ADVANCE_DELAY:
- logger.info("Free kick ready — auto-advancing %s → NORMAL_START", self.command.name)
- self.command = RefereeCommand.NORMAL_START
- self.command_counter += 1
- self.command_timestamp = current_time
- self.next_command = None
- self.status_message = None
- self._normal_start_time = current_time
- self._ball_pos_at_normal_start = (
- (game_frame.ball.p.x, game_frame.ball.p.y) if game_frame.ball is not None else None
- )
- self._advance3_ready_since = math.inf
- self._last_transition_time = current_time
- else:
- self._advance3_ready_since = math.inf
-
- # ----------------------------------------------------------------
- # Auto-advance 4: BALL_PLACEMENT_* → next_command
- # Fires when ball reaches within _PLACEMENT_DONE_DIST of target,
- # sustained for _AUTO_ADVANCE_DELAY seconds.
- # ----------------------------------------------------------------
- elif self._auto_advance.ball_placement_to_next and self.command in self._BALL_PLACEMENT_COMMANDS:
- ready = self.next_command is not None and game_frame is not None and self._ball_placement_done(game_frame)
- if ready:
- if self._advance4_ready_since == math.inf:
- self._advance4_ready_since = current_time
- logger.debug("Advance 4 countdown started (%s)", self.command.name)
- elif (current_time - self._advance4_ready_since) >= _AUTO_ADVANCE_DELAY:
- logger.info(
- "Ball placement complete — auto-advancing %s → %s",
- self.command.name,
- self.next_command.name,
- )
- self.command = self.next_command
- self.command_counter += 1
- self.command_timestamp = current_time
- self.next_command = None
- self._advance4_ready_since = math.inf
- self.status_message = None
- self._last_transition_time = current_time
- else:
- self._advance4_ready_since = math.inf
-
- # ----------------------------------------------------------------
- # Auto-advance 5: NORMAL_START → FORCE_START
- # Fires after kickoff_timeout_seconds if the ball hasn't moved ≥5 cm.
- # ----------------------------------------------------------------
- elif (
- self._auto_advance.normal_start_to_force
- and self.command == RefereeCommand.NORMAL_START
- and self._ball_pos_at_normal_start is not None
- and (current_time - self._normal_start_time) >= self._kickoff_timeout_seconds
- and game_frame is not None
- and game_frame.ball is not None
- and not self._ball_has_moved(game_frame)
- ):
- logger.info("Kickoff/free-kick timeout — auto-advancing NORMAL_START → FORCE_START")
- self.command = RefereeCommand.FORCE_START
- self.command_counter += 1
- self.command_timestamp = current_time
- self.next_command = None
- self.status_message = None
- self._ball_pos_at_normal_start = None
- self._last_transition_time = current_time
-
- # ----------------------------------------------------------------
- # Legacy force-start path: STOP → FORCE_START after goal
- # ----------------------------------------------------------------
- elif (
- self._force_start_after_goal
- and self.command == RefereeCommand.STOP
- and self.next_command in self._PREPARE_KICKOFF_COMMANDS
- and (current_time - self._stop_entered_time) >= self._stop_duration_seconds
- ):
- self.command = RefereeCommand.FORCE_START
- self.command_counter += 1
- self.command_timestamp = current_time
- self.next_command = None
- self.status_message = None
- self._last_transition_time = current_time
- logger.info("Auto-advanced STOP → FORCE_START after goal (force-start profile mode)")
-
- return self._generate_referee_data(current_time)
-
- def _all_robots_clear(self, game_frame: "GameFrame") -> bool:
- """Return True if every robot on both teams is ≥ _BALL_CLEAR_DIST from the ball."""
- ball = game_frame.ball
- if ball is None:
- return True
- bx, by = ball.p.x, ball.p.y
- for r in list(game_frame.friendly_robots.values()) + list(game_frame.enemy_robots.values()):
- if math.hypot(r.p.x - bx, r.p.y - by) < _BALL_CLEAR_DIST:
- return False
- return True
-
- def _kicker_in_centre_circle(self, command: RefereeCommand, game_frame: "GameFrame") -> bool:
- """Return True if at least one robot of the attacking team is inside the centre circle."""
- r = self._geometry.center_circle_radius if self._geometry is not None else 0.5 # fallback for standalone use
- kicking_is_yellow = command == RefereeCommand.PREPARE_KICKOFF_YELLOW
- attackers = (
- game_frame.friendly_robots if kicking_is_yellow == game_frame.my_team_is_yellow else game_frame.enemy_robots
- )
- return any(math.hypot(robot.p.x, robot.p.y) <= r for robot in attackers.values())
-
- def _penalty_kicker_ready(self, command: RefereeCommand, game_frame: "GameFrame") -> bool:
- """Return True when an attacking robot is within the ready radius of the penalty mark."""
- kicking_is_yellow = command == RefereeCommand.PREPARE_PENALTY_YELLOW
- attackers = (
- game_frame.friendly_robots if kicking_is_yellow == game_frame.my_team_is_yellow else game_frame.enemy_robots
- )
- if not attackers:
- return False
-
- half_length = (
- self._geometry.half_length if self._geometry is not None else STANDARD_FIELD_DIMS.full_field_half_length
- )
- yellow_is_right = game_frame.my_team_is_right == game_frame.my_team_is_yellow
- if kicking_is_yellow:
- goal_sign = -1.0 if yellow_is_right else 1.0
- else:
- goal_sign = 1.0 if yellow_is_right else -1.0
- penalty_mark_x = goal_sign * half_length * 0.5
-
- closest = min(math.hypot(robot.p.x - penalty_mark_x, robot.p.y) for robot in attackers.values())
- return closest <= _KICKER_READY_DIST
-
- def _free_kick_ready(self, command: RefereeCommand, game_frame: "GameFrame") -> bool:
- """Return True when a free kick is ready to start:
- - The kicker (closest attacker to ball) is within _KICKER_READY_DIST of the ball.
- - All defending robots are ≥ _BALL_CLEAR_DIST from the ball.
- """
- ball = game_frame.ball
- if ball is None:
- return False
- bx, by = ball.p.x, ball.p.y
-
- kicking_is_yellow = command == RefereeCommand.DIRECT_FREE_YELLOW
- attackers = (
- game_frame.friendly_robots if kicking_is_yellow == game_frame.my_team_is_yellow else game_frame.enemy_robots
- )
- defenders = (
- game_frame.enemy_robots if kicking_is_yellow == game_frame.my_team_is_yellow else game_frame.friendly_robots
- )
-
- # Check defending robots are all clear.
- if any(math.hypot(r.p.x - bx, r.p.y - by) < _BALL_CLEAR_DIST for r in defenders.values()):
- return False
-
- # Check at least one attacker is close to the ball (kicker in position).
- if not attackers:
- return False
- closest = min(math.hypot(r.p.x - bx, r.p.y - by) for r in attackers.values())
- return closest <= _KICKER_READY_DIST
-
- def _ball_has_moved(self, game_frame: "GameFrame") -> bool:
- """Return True if the ball has moved ≥ 0.05 m since NORMAL_START."""
- if self._ball_pos_at_normal_start is None or game_frame.ball is None:
- return False
- ox, oy = self._ball_pos_at_normal_start
- return math.hypot(game_frame.ball.p.x - ox, game_frame.ball.p.y - oy) >= 0.05
-
- def _ball_placement_done(self, game_frame: "GameFrame") -> bool:
- """Return True when the ball is within _PLACEMENT_DONE_DIST of the placement target."""
- if self.ball_placement_target is None or game_frame.ball is None:
- return False
- tx, ty = self.ball_placement_target
- return math.hypot(game_frame.ball.p.x - tx, game_frame.ball.p.y - ty) <= _PLACEMENT_DONE_DIST
-
- # Commands that require robots to clear the ball before they take effect.
- # In a real match these are always preceded by STOP.
- _NEEDS_STOP_FIRST = frozenset(
- {
- RefereeCommand.PREPARE_KICKOFF_YELLOW,
- RefereeCommand.PREPARE_KICKOFF_BLUE,
- RefereeCommand.DIRECT_FREE_YELLOW,
- RefereeCommand.DIRECT_FREE_BLUE,
- RefereeCommand.PREPARE_PENALTY_YELLOW,
- RefereeCommand.PREPARE_PENALTY_BLUE,
- RefereeCommand.BALL_PLACEMENT_YELLOW,
- RefereeCommand.BALL_PLACEMENT_BLUE,
- }
- )
-
- def seed_clock(self, timestamp: float) -> None:
- """Align all internal timers to *timestamp*.
-
- Resets every timer that was initialised before the real vision clock
- was known (construction time) so that durations are measured from the
- correct timebase. Safe to call only once, immediately after the first
- valid game frame is available.
- """
- if self.stage_start_time is None:
- self.stage_start_time = timestamp
- self.command_timestamp = timestamp
- # Push all "entered-at" sentinels to the new timebase so that
- # cooldowns / durations are not immediately satisfied.
- self._last_transition_time = timestamp - _TRANSITION_COOLDOWN # allow transitions immediately
- self._stop_entered_time = timestamp - self._stop_duration_seconds # won't auto-advance yet
- self._prepare_entered_time = timestamp - self._prepare_duration_seconds # same
-
- def set_command(self, command: RefereeCommand, timestamp: float) -> None:
- """Manual override — for operator use or test scripting.
-
- If *command* is a set-piece command (kickoff, free kick, penalty,
- ball placement) and the game is not already in STOP or HALT, a STOP
- is issued first and the requested command is stored as ``next_command``
- so the operator (or a script) can advance to it after robots have
- cleared the ball. This mirrors real-match game-controller behaviour
- and prevents robots from receiving a PREPARE_KICKOFF while they are
- still within the keep-out zone around the ball.
- """
- _ALREADY_STOPPED = (RefereeCommand.STOP, RefereeCommand.HALT)
-
- if command in self._NEEDS_STOP_FIRST and self.command not in _ALREADY_STOPPED:
- # Insert STOP; park the real command as next_command.
- logger.info("Inserting STOP before %s so robots can clear the ball", command.name)
- self.command = RefereeCommand.STOP
- self.command_counter += 1
- self.command_timestamp = timestamp
- self.next_command = command
- self.status_message = None
- self._stop_entered_time = timestamp
- return
-
- # NORMAL_START while in STOP with a pending set-piece: advance to the
- # set-piece first so robots can form up. Auto-advance will then issue
- # NORMAL_START after prepare_duration_seconds.
- if (
- command == RefereeCommand.NORMAL_START
- and self.command == RefereeCommand.STOP
- and self.next_command in self._NEEDS_STOP_FIRST
- ):
- logger.info(
- "Manually advancing STOP → %s (auto NORMAL_START in %.1f s)",
- self.next_command.name,
- self._prepare_duration_seconds,
- )
- self.command = self.next_command
- self.command_counter += 1
- self.command_timestamp = timestamp
- self.next_command = RefereeCommand.NORMAL_START
- self.status_message = None
- self._prepare_entered_time = timestamp
- return
-
- self.command = command
- self.command_counter += 1
- self.command_timestamp = timestamp
- self.status_message = None
- self._advance2_ready_since = math.inf
- self._advance3_ready_since = math.inf
- self._advance4_ready_since = math.inf
-
- if command in (RefereeCommand.STOP, RefereeCommand.HALT):
- self._stop_entered_time = timestamp
- elif command in (
- RefereeCommand.PREPARE_KICKOFF_YELLOW,
- RefereeCommand.PREPARE_KICKOFF_BLUE,
- RefereeCommand.PREPARE_PENALTY_YELLOW,
- RefereeCommand.PREPARE_PENALTY_BLUE,
- ):
- self._prepare_entered_time = timestamp
-
- # Advance PRE stages to their active counterpart when play begins.
- _PRE_TO_ACTIVE = {
- Stage.NORMAL_FIRST_HALF_PRE: Stage.NORMAL_FIRST_HALF,
- Stage.NORMAL_SECOND_HALF_PRE: Stage.NORMAL_SECOND_HALF,
- Stage.EXTRA_FIRST_HALF_PRE: Stage.EXTRA_FIRST_HALF,
- Stage.EXTRA_SECOND_HALF_PRE: Stage.EXTRA_SECOND_HALF,
- }
- if command in (RefereeCommand.NORMAL_START, RefereeCommand.FORCE_START):
- active = _PRE_TO_ACTIVE.get(self.stage)
- if active is not None:
- self.advance_stage(active, timestamp)
-
- logger.info("Referee command manually set to: %s", command.name)
-
- def advance_stage(self, new_stage: Stage, timestamp: float) -> None:
- """Advance the game stage."""
- logger.info("Stage %s → %s", self.stage.name, new_stage.name)
- self.stage = new_stage
- self.stage_start_time = timestamp
-
- # ------------------------------------------------------------------
- # Internal helpers
- # ------------------------------------------------------------------
-
- def _can_transition(self, current_time: float) -> bool:
- return (current_time - self._last_transition_time) >= _TRANSITION_COOLDOWN
-
- def _apply_violation(self, violation: RuleViolation, current_time: float) -> None:
- """Update state in response to a detected violation."""
- if violation.rule_name == "goal":
- self._handle_goal(violation, current_time)
- else:
- self._handle_foul(violation, current_time)
-
- self._last_transition_time = current_time
-
- def _handle_goal(self, violation: RuleViolation, current_time: float) -> None:
- # Determine scorer from next_command (loser gets the kickoff).
- if violation.next_command == RefereeCommand.PREPARE_KICKOFF_BLUE:
- # Blue gets kickoff → yellow scored.
- self.yellow_team.increment_score()
- logger.info(
- "Goal by Yellow! Score: Yellow %d – Blue %d",
- self.yellow_team.score,
- self.blue_team.score,
- )
- elif violation.next_command == RefereeCommand.PREPARE_KICKOFF_YELLOW:
- # Yellow gets kickoff → blue scored.
- self.blue_team.increment_score()
- logger.info(
- "Goal by Blue! Score: Yellow %d – Blue %d",
- self.yellow_team.score,
- self.blue_team.score,
- )
-
- self.command = RefereeCommand.STOP
- self.command_counter += 1
- self.command_timestamp = current_time
- self.next_command = violation.next_command
- self.ball_placement_target = (0.0, 0.0)
- self.status_message = violation.status_message
- self._stop_entered_time = current_time
-
- def _handle_foul(self, violation: RuleViolation, current_time: float) -> None:
- self.command = violation.suggested_command
- self.command_counter += 1
- self.command_timestamp = current_time
- self.next_command = violation.next_command
- self.ball_placement_target = violation.designated_position
- self.status_message = violation.status_message
- logger.info(
- "Foul detected: %s → %s (next: %s)",
- violation.rule_name,
- violation.suggested_command.name,
- violation.next_command.name if violation.next_command else "None",
- )
-
- def _generate_referee_data(self, current_time: float) -> RefereeData:
- stage_time_left = max(0.0, self.stage_duration - (current_time - self.stage_start_time))
- return RefereeData(
- source_identifier="custom_referee",
- time_sent=current_time,
- time_received=current_time,
- referee_command=self.command,
- referee_command_timestamp=self.command_timestamp,
- stage=self.stage,
- stage_time_left=stage_time_left,
- blue_team=copy.copy(self.blue_team),
- yellow_team=copy.copy(self.yellow_team),
- designated_position=self.ball_placement_target,
- blue_team_on_positive_half=None,
- next_command=self.next_command,
- current_action_time_remaining=None,
- status_message=self.status_message,
- )
+"""GameStateMachine: owns all mutable game state for the CustomReferee."""
+
+from __future__ import annotations
+
+import copy
+import logging
+import math
+from typing import Optional
+
+from utama_core.config.field_params import STANDARD_FIELD_DIMS
+from utama_core.custom_referee.geometry import RefereeGeometry
+from utama_core.custom_referee.profiles.profile_loader import AutoAdvanceConfig
+from utama_core.custom_referee.rules.base_rule import RuleViolation
+from utama_core.entities.data.referee import RefereeData
+from utama_core.entities.game.game_frame import GameFrame
+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
+
+logger = logging.getLogger(__name__)
+
+_TRANSITION_COOLDOWN = 0.3 # seconds — prevents command oscillation
+_BALL_CLEAR_DIST = 0.5 # metres — all robots must be this far from ball before queued restart
+_KICKER_READY_DIST = 0.3 # metres — kicker must be within this distance to trigger free kick start
+_PLACEMENT_DONE_DIST = 0.15 # metres — ball within this dist of target → placement complete
+_AUTO_ADVANCE_DELAY = 2.0 # seconds — readiness must be sustained this long before play starts
+
+
+class GameStateMachine:
+ """Owns score, command, and stage. Produces ``RefereeData`` each tick."""
+
+ def __init__(
+ self,
+ half_duration_seconds: float,
+ kickoff_team: str,
+ n_robots_yellow: int,
+ n_robots_blue: int,
+ initial_stage: Stage = Stage.NORMAL_FIRST_HALF_PRE,
+ force_start_after_goal: bool = False,
+ stop_duration_seconds: float = 3.0,
+ prepare_duration_seconds: float = 3.0,
+ kickoff_timeout_seconds: float = 10.0,
+ geometry: Optional[RefereeGeometry] = None,
+ auto_advance: Optional[AutoAdvanceConfig] = None,
+ ) -> None:
+ self.command = RefereeCommand.HALT
+ self.command_counter = 0
+ self.command_timestamp = 0.0
+
+ self.stage = initial_stage
+ # Seeded by seed_clock() after the first valid game frame is available.
+ self.stage_start_time: Optional[float] = None
+ self.stage_duration = half_duration_seconds
+
+ self.yellow_team = TeamInfo(
+ name="Yellow",
+ score=0,
+ red_cards=0,
+ yellow_card_times=[],
+ yellow_cards=0,
+ timeouts=4,
+ timeout_time=300,
+ goalkeeper=0,
+ foul_counter=0,
+ ball_placement_failures=0,
+ can_place_ball=True,
+ max_allowed_bots=n_robots_yellow,
+ bot_substitution_intent=False,
+ bot_substitution_allowed=True,
+ bot_substitutions_left=5,
+ )
+ self.blue_team = TeamInfo(
+ name="Blue",
+ score=0,
+ red_cards=0,
+ yellow_card_times=[],
+ yellow_cards=0,
+ timeouts=4,
+ timeout_time=300,
+ goalkeeper=0,
+ foul_counter=0,
+ ball_placement_failures=0,
+ can_place_ball=True,
+ max_allowed_bots=n_robots_blue,
+ bot_substitution_intent=False,
+ bot_substitution_allowed=True,
+ bot_substitutions_left=5,
+ )
+
+ self.next_command: Optional[RefereeCommand] = None
+ self.ball_placement_target: Optional[tuple[float, float]] = None
+ self._post_ball_placement_command: Optional[RefereeCommand] = None
+ self.status_message: Optional[str] = None
+
+ # Kickoff team initialised from profile.
+ self._kickoff_team_is_yellow = kickoff_team.lower() == "yellow"
+
+ # Arcade auto-advance: after stop_duration_seconds in STOP following a
+ # goal, automatically issue FORCE_START instead of waiting for operator.
+ self._force_start_after_goal = force_start_after_goal
+ self._stop_duration_seconds = stop_duration_seconds
+ self._stop_entered_time: float = -math.inf # wall time when STOP was last entered
+
+ # Auto-advance timings.
+ self._prepare_duration_seconds = prepare_duration_seconds
+ self._kickoff_timeout_seconds = kickoff_timeout_seconds
+ self._prepare_entered_time: float = -math.inf # wall time when PREPARE_KICKOFF was entered
+ self._normal_start_time: float = -math.inf # wall time when NORMAL_START was entered
+
+ # Ball position snapshot at NORMAL_START — used to detect if the ball has moved.
+ self._ball_pos_at_normal_start: Optional[tuple[float, float]] = None
+
+ # Timestamps for sustained-readiness countdown before play-starting advances.
+ # Set to math.inf when condition is not yet met; fire when elapsed >= _AUTO_ADVANCE_DELAY.
+ self._advance2_ready_since: float = math.inf # PREPARE_* → NORMAL_START
+ self._advance3_ready_since: float = math.inf # DIRECT_FREE_* → NORMAL_START
+ self._advance4_ready_since: float = math.inf # BALL_PLACEMENT_* → next_command
+
+ # Field geometry (used for readiness checks).
+ self._geometry: Optional[RefereeGeometry] = geometry
+
+ # Cooldown: don't process a new violation within this window.
+ self._last_transition_time: float = -math.inf
+
+ # Per-transition enable flags (default: all on).
+ self._auto_advance = auto_advance if auto_advance is not None else AutoAdvanceConfig()
+
+ # ------------------------------------------------------------------
+ # Public API
+ # ------------------------------------------------------------------
+
+ _PREPARE_KICKOFF_COMMANDS = frozenset(
+ {
+ RefereeCommand.PREPARE_KICKOFF_YELLOW,
+ RefereeCommand.PREPARE_KICKOFF_BLUE,
+ }
+ )
+ _DIRECT_FREE_COMMANDS = frozenset(
+ {
+ RefereeCommand.DIRECT_FREE_YELLOW,
+ RefereeCommand.DIRECT_FREE_BLUE,
+ }
+ )
+ _PREPARE_PENALTY_COMMANDS = frozenset(
+ {
+ RefereeCommand.PREPARE_PENALTY_YELLOW,
+ RefereeCommand.PREPARE_PENALTY_BLUE,
+ }
+ )
+ _BALL_PLACEMENT_COMMANDS = frozenset(
+ {
+ RefereeCommand.BALL_PLACEMENT_YELLOW,
+ RefereeCommand.BALL_PLACEMENT_BLUE,
+ }
+ )
+
+ def step(
+ self,
+ current_time: float,
+ violation: Optional[RuleViolation],
+ game_frame: Optional["GameFrame"] = None,
+ ) -> RefereeData:
+ """Process one tick. Apply violation if not in cooldown. Return RefereeData."""
+ if self.stage_start_time is None:
+ self.stage_start_time = current_time
+
+ if violation is not None and self._can_transition(current_time):
+ self._apply_violation(violation, current_time)
+
+ # ----------------------------------------------------------------
+ # Auto-advance 1: STOP → next queued restart
+ # Fires when all robots are ≥ _BALL_CLEAR_DIST from the ball.
+ # ----------------------------------------------------------------
+ if (
+ self._auto_advance.stop_to_next_command
+ and self.command == RefereeCommand.STOP
+ and self.next_command in self._NEEDS_STOP_FIRST
+ and game_frame is not None
+ and self._all_robots_clear(game_frame)
+ ):
+ logger.info("All robots clear — auto-advancing STOP → %s", self.next_command.name)
+ self.command = self.next_command
+ self.command_counter += 1
+ self.command_timestamp = current_time
+ if self.command in self._BALL_PLACEMENT_COMMANDS:
+ self.next_command = self._post_ball_placement_command or RefereeCommand.NORMAL_START
+ self._post_ball_placement_command = None
+ self._advance4_ready_since = math.inf
+ elif self.command in self._DIRECT_FREE_COMMANDS:
+ self.next_command = RefereeCommand.NORMAL_START
+ self._advance3_ready_since = math.inf
+ elif self.command in self._PREPARE_KICKOFF_COMMANDS or self.command in self._PREPARE_PENALTY_COMMANDS:
+ self.next_command = RefereeCommand.NORMAL_START
+ self._prepare_entered_time = current_time
+ self._advance2_ready_since = math.inf
+ self._last_transition_time = current_time
+
+ # ----------------------------------------------------------------
+ # Auto-advance 2a: PREPARE_KICKOFF_* → NORMAL_START
+ # Fires after prepare_duration_seconds AND one attacker is inside
+ # the centre circle, sustained for _AUTO_ADVANCE_DELAY seconds.
+ # ----------------------------------------------------------------
+ elif self._auto_advance.prepare_kickoff_to_normal and self.command in self._PREPARE_KICKOFF_COMMANDS:
+ ready = (
+ (current_time - self._prepare_entered_time) >= self._prepare_duration_seconds
+ and game_frame is not None
+ and self._kicker_in_centre_circle(self.command, game_frame)
+ )
+ if ready:
+ if self._advance2_ready_since == math.inf:
+ self._advance2_ready_since = current_time
+ logger.debug("Advance 2 countdown started (%s)", self.command.name)
+ elif (current_time - self._advance2_ready_since) >= _AUTO_ADVANCE_DELAY:
+ logger.info(
+ "Kicker in centre circle — auto-advancing %s → NORMAL_START",
+ self.command.name,
+ )
+ self.command = RefereeCommand.NORMAL_START
+ self.command_counter += 1
+ self.command_timestamp = current_time
+ self.next_command = None
+ self.status_message = None
+ self._normal_start_time = current_time
+ self._ball_pos_at_normal_start = (
+ (game_frame.ball.p.x, game_frame.ball.p.y) if game_frame.ball is not None else None
+ )
+ self._advance2_ready_since = math.inf
+ self._last_transition_time = current_time
+ else:
+ self._advance2_ready_since = math.inf
+
+ # ----------------------------------------------------------------
+ # Auto-advance 2b: PREPARE_PENALTY_* → NORMAL_START
+ # Fires after prepare_duration_seconds when the kicker reaches the
+ # penalty mark, sustained for _AUTO_ADVANCE_DELAY seconds.
+ # ----------------------------------------------------------------
+ elif self._auto_advance.prepare_penalty_to_normal and self.command in self._PREPARE_PENALTY_COMMANDS:
+ ready = (
+ (current_time - self._prepare_entered_time) >= self._prepare_duration_seconds
+ and game_frame is not None
+ and self._penalty_kicker_ready(self.command, game_frame)
+ )
+ if ready:
+ if self._advance2_ready_since == math.inf:
+ self._advance2_ready_since = current_time
+ logger.debug("Advance 2 countdown started (%s)", self.command.name)
+ elif (current_time - self._advance2_ready_since) >= _AUTO_ADVANCE_DELAY:
+ logger.info(
+ "Kicker at penalty mark — auto-advancing %s → NORMAL_START",
+ self.command.name,
+ )
+ self.command = RefereeCommand.NORMAL_START
+ self.command_counter += 1
+ self.command_timestamp = current_time
+ self.next_command = None
+ self.status_message = None
+ self._normal_start_time = current_time
+ self._ball_pos_at_normal_start = (
+ (game_frame.ball.p.x, game_frame.ball.p.y) if game_frame.ball is not None else None
+ )
+ self._advance2_ready_since = math.inf
+ self._last_transition_time = current_time
+ else:
+ self._advance2_ready_since = math.inf
+
+ # ----------------------------------------------------------------
+ # Auto-advance 3: DIRECT_FREE_* → NORMAL_START
+ # Fires when the kicker is within _KICKER_READY_DIST of the ball
+ # AND all defending robots are ≥ _BALL_CLEAR_DIST away, sustained
+ # for _AUTO_ADVANCE_DELAY seconds.
+ # ----------------------------------------------------------------
+ elif self._auto_advance.direct_free_to_normal and self.command in self._DIRECT_FREE_COMMANDS:
+ ready = game_frame is not None and self._free_kick_ready(self.command, game_frame)
+ if ready:
+ if self._advance3_ready_since == math.inf:
+ self._advance3_ready_since = current_time
+ logger.debug("Advance 3 countdown started (%s)", self.command.name)
+ elif (current_time - self._advance3_ready_since) >= _AUTO_ADVANCE_DELAY:
+ logger.info("Free kick ready — auto-advancing %s → NORMAL_START", self.command.name)
+ self.command = RefereeCommand.NORMAL_START
+ self.command_counter += 1
+ self.command_timestamp = current_time
+ self.next_command = None
+ self.status_message = None
+ self._normal_start_time = current_time
+ self._ball_pos_at_normal_start = (
+ (game_frame.ball.p.x, game_frame.ball.p.y) if game_frame.ball is not None else None
+ )
+ self._advance3_ready_since = math.inf
+ self._last_transition_time = current_time
+ else:
+ self._advance3_ready_since = math.inf
+
+ # ----------------------------------------------------------------
+ # Auto-advance 4: BALL_PLACEMENT_* → next_command
+ # Fires when ball reaches within _PLACEMENT_DONE_DIST of target,
+ # sustained for _AUTO_ADVANCE_DELAY seconds.
+ # ----------------------------------------------------------------
+ elif self._auto_advance.ball_placement_to_next and self.command in self._BALL_PLACEMENT_COMMANDS:
+ ready = self.next_command is not None and game_frame is not None and self._ball_placement_done(game_frame)
+ if ready:
+ if self._advance4_ready_since == math.inf:
+ self._advance4_ready_since = current_time
+ logger.debug("Advance 4 countdown started (%s)", self.command.name)
+ elif (current_time - self._advance4_ready_since) >= _AUTO_ADVANCE_DELAY:
+ logger.info(
+ "Ball placement complete — auto-advancing %s → %s",
+ self.command.name,
+ self.next_command.name,
+ )
+ completed_command = self.next_command
+ self.command = completed_command
+ self.command_counter += 1
+ self.command_timestamp = current_time
+ if completed_command in self._DIRECT_FREE_COMMANDS:
+ self.next_command = RefereeCommand.NORMAL_START
+ self._advance3_ready_since = math.inf
+ elif (
+ completed_command in self._PREPARE_KICKOFF_COMMANDS
+ or completed_command in self._PREPARE_PENALTY_COMMANDS
+ ):
+ self.next_command = RefereeCommand.NORMAL_START
+ self._prepare_entered_time = current_time
+ self._advance2_ready_since = math.inf
+ else:
+ self.next_command = None
+ self._advance4_ready_since = math.inf
+ self.status_message = None
+ self._last_transition_time = current_time
+ else:
+ self._advance4_ready_since = math.inf
+
+ # ----------------------------------------------------------------
+ # Auto-advance 5: NORMAL_START → FORCE_START
+ # Fires after kickoff_timeout_seconds if the ball hasn't moved ≥5 cm.
+ # ----------------------------------------------------------------
+ elif (
+ self._auto_advance.normal_start_to_force
+ and self.command == RefereeCommand.NORMAL_START
+ and self._ball_pos_at_normal_start is not None
+ and (current_time - self._normal_start_time) >= self._kickoff_timeout_seconds
+ and game_frame is not None
+ and game_frame.ball is not None
+ and not self._ball_has_moved(game_frame)
+ ):
+ logger.info("Kickoff/free-kick timeout — auto-advancing NORMAL_START → FORCE_START")
+ self.command = RefereeCommand.FORCE_START
+ self.command_counter += 1
+ self.command_timestamp = current_time
+ self.next_command = None
+ self.status_message = None
+ self._ball_pos_at_normal_start = None
+ self._last_transition_time = current_time
+
+ # ----------------------------------------------------------------
+ # Legacy force-start path: STOP → FORCE_START after goal
+ # ----------------------------------------------------------------
+ elif (
+ self._force_start_after_goal
+ and self.command == RefereeCommand.STOP
+ and self.next_command in self._PREPARE_KICKOFF_COMMANDS
+ and (current_time - self._stop_entered_time) >= self._stop_duration_seconds
+ ):
+ self.command = RefereeCommand.FORCE_START
+ self.command_counter += 1
+ self.command_timestamp = current_time
+ self.next_command = None
+ self.status_message = None
+ self._last_transition_time = current_time
+ logger.info("Auto-advanced STOP → FORCE_START after goal (force-start profile mode)")
+
+ return self._generate_referee_data(current_time)
+
+ def _all_robots_clear(self, game_frame: "GameFrame") -> bool:
+ """Return True if every robot on both teams is ≥ _BALL_CLEAR_DIST from the ball."""
+ ball = game_frame.ball
+ if ball is None:
+ return True
+ bx, by = ball.p.x, ball.p.y
+ for r in list(game_frame.friendly_robots.values()) + list(game_frame.enemy_robots.values()):
+ if math.hypot(r.p.x - bx, r.p.y - by) < _BALL_CLEAR_DIST:
+ return False
+ return True
+
+ def _kicker_in_centre_circle(self, command: RefereeCommand, game_frame: "GameFrame") -> bool:
+ """Return True if at least one robot of the attacking team is inside the centre circle."""
+ r = self._geometry.center_circle_radius if self._geometry is not None else 0.5 # fallback for standalone use
+ kicking_is_yellow = command == RefereeCommand.PREPARE_KICKOFF_YELLOW
+ attackers = (
+ game_frame.friendly_robots if kicking_is_yellow == game_frame.my_team_is_yellow else game_frame.enemy_robots
+ )
+ return any(math.hypot(robot.p.x, robot.p.y) <= r for robot in attackers.values())
+
+ def _penalty_kicker_ready(self, command: RefereeCommand, game_frame: "GameFrame") -> bool:
+ """Return True when an attacking robot is within the ready radius of the penalty mark."""
+ kicking_is_yellow = command == RefereeCommand.PREPARE_PENALTY_YELLOW
+ attackers = (
+ game_frame.friendly_robots if kicking_is_yellow == game_frame.my_team_is_yellow else game_frame.enemy_robots
+ )
+ if not attackers:
+ return False
+
+ half_length = (
+ self._geometry.half_length if self._geometry is not None else STANDARD_FIELD_DIMS.full_field_half_length
+ )
+ yellow_is_right = game_frame.my_team_is_right == game_frame.my_team_is_yellow
+ if kicking_is_yellow:
+ goal_sign = -1.0 if yellow_is_right else 1.0
+ else:
+ goal_sign = 1.0 if yellow_is_right else -1.0
+ penalty_mark_x = goal_sign * half_length * 0.5
+
+ closest = min(math.hypot(robot.p.x - penalty_mark_x, robot.p.y) for robot in attackers.values())
+ return closest <= _KICKER_READY_DIST
+
+ def _free_kick_ready(self, command: RefereeCommand, game_frame: "GameFrame") -> bool:
+ """Return True when a free kick is ready to start:
+ - The kicker (closest attacker to ball) is within _KICKER_READY_DIST of the ball.
+ - All defending robots are ≥ _BALL_CLEAR_DIST from the ball.
+ """
+ ball = game_frame.ball
+ if ball is None:
+ return False
+ bx, by = ball.p.x, ball.p.y
+
+ kicking_is_yellow = command == RefereeCommand.DIRECT_FREE_YELLOW
+ attackers = (
+ game_frame.friendly_robots if kicking_is_yellow == game_frame.my_team_is_yellow else game_frame.enemy_robots
+ )
+ defenders = (
+ game_frame.enemy_robots if kicking_is_yellow == game_frame.my_team_is_yellow else game_frame.friendly_robots
+ )
+
+ # Check defending robots are all clear.
+ if any(math.hypot(r.p.x - bx, r.p.y - by) < _BALL_CLEAR_DIST for r in defenders.values()):
+ return False
+
+ # Check at least one attacker is close to the ball (kicker in position).
+ if not attackers:
+ return False
+ closest = min(math.hypot(r.p.x - bx, r.p.y - by) for r in attackers.values())
+ return closest <= _KICKER_READY_DIST
+
+ def _ball_has_moved(self, game_frame: "GameFrame") -> bool:
+ """Return True if the ball has moved ≥ 0.05 m since NORMAL_START."""
+ if self._ball_pos_at_normal_start is None or game_frame.ball is None:
+ return False
+ ox, oy = self._ball_pos_at_normal_start
+ return math.hypot(game_frame.ball.p.x - ox, game_frame.ball.p.y - oy) >= 0.05
+
+ def _ball_placement_done(self, game_frame: "GameFrame") -> bool:
+ """Return True when the ball is within _PLACEMENT_DONE_DIST of the placement target."""
+ if self.ball_placement_target is None or game_frame.ball is None:
+ return False
+ tx, ty = self.ball_placement_target
+ return math.hypot(game_frame.ball.p.x - tx, game_frame.ball.p.y - ty) <= _PLACEMENT_DONE_DIST
+
+ # Commands that require robots to clear the ball before they take effect.
+ # In a real match these are always preceded by STOP.
+ _NEEDS_STOP_FIRST = frozenset(
+ {
+ RefereeCommand.PREPARE_KICKOFF_YELLOW,
+ RefereeCommand.PREPARE_KICKOFF_BLUE,
+ RefereeCommand.DIRECT_FREE_YELLOW,
+ RefereeCommand.DIRECT_FREE_BLUE,
+ RefereeCommand.PREPARE_PENALTY_YELLOW,
+ RefereeCommand.PREPARE_PENALTY_BLUE,
+ RefereeCommand.BALL_PLACEMENT_YELLOW,
+ RefereeCommand.BALL_PLACEMENT_BLUE,
+ }
+ )
+
+ @staticmethod
+ def _ball_placement_command_for(restart_command: Optional[RefereeCommand]) -> Optional[RefereeCommand]:
+ """Return the ball-placement command for the team that owns a restart."""
+ if restart_command in (
+ RefereeCommand.PREPARE_KICKOFF_YELLOW,
+ RefereeCommand.PREPARE_PENALTY_YELLOW,
+ RefereeCommand.DIRECT_FREE_YELLOW,
+ RefereeCommand.INDIRECT_FREE_YELLOW,
+ ):
+ return RefereeCommand.BALL_PLACEMENT_YELLOW
+ if restart_command in (
+ RefereeCommand.PREPARE_KICKOFF_BLUE,
+ RefereeCommand.PREPARE_PENALTY_BLUE,
+ RefereeCommand.DIRECT_FREE_BLUE,
+ RefereeCommand.INDIRECT_FREE_BLUE,
+ ):
+ return RefereeCommand.BALL_PLACEMENT_BLUE
+ return None
+
+ def seed_clock(self, timestamp: float) -> None:
+ """Align all internal timers to *timestamp*.
+
+ Resets every timer that was initialised before the real vision clock
+ was known (construction time) so that durations are measured from the
+ correct timebase. Safe to call only once, immediately after the first
+ valid game frame is available.
+ """
+ if self.stage_start_time is None:
+ self.stage_start_time = timestamp
+ self.command_timestamp = timestamp
+ # Push all "entered-at" sentinels to the new timebase so that
+ # cooldowns / durations are not immediately satisfied.
+ self._last_transition_time = timestamp - _TRANSITION_COOLDOWN # allow transitions immediately
+ self._stop_entered_time = timestamp - self._stop_duration_seconds # won't auto-advance yet
+ self._prepare_entered_time = timestamp - self._prepare_duration_seconds # same
+
+ def set_command(self, command: RefereeCommand, timestamp: float) -> None:
+ """Manual override — for operator use or test scripting.
+
+ If *command* is a set-piece command (kickoff, free kick, penalty,
+ ball placement) and the game is not already in STOP or HALT, a STOP
+ is issued first and the requested command is stored as ``next_command``
+ so the operator (or a script) can advance to it after robots have
+ cleared the ball. This mirrors real-match game-controller behaviour
+ and prevents robots from receiving a PREPARE_KICKOFF while they are
+ still within the keep-out zone around the ball.
+ """
+ _ALREADY_STOPPED = (RefereeCommand.STOP, RefereeCommand.HALT)
+
+ if command in self._NEEDS_STOP_FIRST and self.command not in _ALREADY_STOPPED:
+ # Insert STOP; park the real command as next_command.
+ logger.info("Inserting STOP before %s so robots can clear the ball", command.name)
+ self.command = RefereeCommand.STOP
+ self.command_counter += 1
+ self.command_timestamp = timestamp
+ self.next_command = command
+ self._post_ball_placement_command = None
+ self.status_message = None
+ self._stop_entered_time = timestamp
+ return
+
+ # NORMAL_START while in STOP with a pending set-piece: advance to the
+ # set-piece first so robots can form up. Auto-advance will then issue
+ # NORMAL_START after prepare_duration_seconds.
+ if (
+ command == RefereeCommand.NORMAL_START
+ and self.command == RefereeCommand.STOP
+ and self.next_command in self._NEEDS_STOP_FIRST
+ ):
+ logger.info(
+ "Manually advancing STOP → %s (auto NORMAL_START in %.1f s)",
+ self.next_command.name,
+ self._prepare_duration_seconds,
+ )
+ self.command = self.next_command
+ self.command_counter += 1
+ self.command_timestamp = timestamp
+ if self.command in self._BALL_PLACEMENT_COMMANDS:
+ self.next_command = self._post_ball_placement_command or RefereeCommand.NORMAL_START
+ self._post_ball_placement_command = None
+ self._advance4_ready_since = math.inf
+ else:
+ self.next_command = RefereeCommand.NORMAL_START
+ self.status_message = None
+ self._prepare_entered_time = timestamp
+ return
+
+ self.command = command
+ self.command_counter += 1
+ self.command_timestamp = timestamp
+ self._post_ball_placement_command = None
+ self.status_message = None
+ self._advance2_ready_since = math.inf
+ self._advance3_ready_since = math.inf
+ self._advance4_ready_since = math.inf
+
+ if command in (RefereeCommand.STOP, RefereeCommand.HALT):
+ self._stop_entered_time = timestamp
+ elif command in (
+ RefereeCommand.PREPARE_KICKOFF_YELLOW,
+ RefereeCommand.PREPARE_KICKOFF_BLUE,
+ RefereeCommand.PREPARE_PENALTY_YELLOW,
+ RefereeCommand.PREPARE_PENALTY_BLUE,
+ ):
+ self._prepare_entered_time = timestamp
+
+ # Advance PRE stages to their active counterpart when play begins.
+ _PRE_TO_ACTIVE = {
+ Stage.NORMAL_FIRST_HALF_PRE: Stage.NORMAL_FIRST_HALF,
+ Stage.NORMAL_SECOND_HALF_PRE: Stage.NORMAL_SECOND_HALF,
+ Stage.EXTRA_FIRST_HALF_PRE: Stage.EXTRA_FIRST_HALF,
+ Stage.EXTRA_SECOND_HALF_PRE: Stage.EXTRA_SECOND_HALF,
+ }
+ if command in (RefereeCommand.NORMAL_START, RefereeCommand.FORCE_START):
+ active = _PRE_TO_ACTIVE.get(self.stage)
+ if active is not None:
+ self.advance_stage(active, timestamp)
+
+ logger.info("Referee command manually set to: %s", command.name)
+
+ def force_command(
+ self,
+ command: RefereeCommand,
+ timestamp: float,
+ ball_placement_target: Optional[tuple[float, float]] = None,
+ ) -> None:
+ """Directly set the command, bypassing the STOP-first guard.
+
+ For god-mode / test use only — skips the normal safety interlock that
+ inserts STOP before set-piece commands.
+ """
+ self.command = command
+ self.command_counter += 1
+ self.command_timestamp = timestamp
+ self._post_ball_placement_command = None
+ self.status_message = None
+ self._advance2_ready_since = math.inf
+ self._advance3_ready_since = math.inf
+ self._advance4_ready_since = math.inf
+ if ball_placement_target is not None:
+ self.ball_placement_target = ball_placement_target
+ if command in self._BALL_PLACEMENT_COMMANDS:
+ # Auto-advance 4 requires next_command to be set.
+ self.next_command = RefereeCommand.NORMAL_START
+ else:
+ self.next_command = None
+ logger.info("Referee command force-set to: %s", command.name)
+
+ def advance_stage(self, new_stage: Stage, timestamp: float) -> None:
+ """Advance the game stage."""
+ logger.info("Stage %s → %s", self.stage.name, new_stage.name)
+ self.stage = new_stage
+ self.stage_start_time = timestamp
+
+ # ------------------------------------------------------------------
+ # Internal helpers
+ # ------------------------------------------------------------------
+
+ def _can_transition(self, current_time: float) -> bool:
+ return (current_time - self._last_transition_time) >= _TRANSITION_COOLDOWN
+
+ def _apply_violation(self, violation: RuleViolation, current_time: float) -> None:
+ """Update state in response to a detected violation."""
+ if violation.rule_name == "goal":
+ self._handle_goal(violation, current_time)
+ else:
+ self._handle_foul(violation, current_time)
+
+ self._last_transition_time = current_time
+
+ def _handle_goal(self, violation: RuleViolation, current_time: float) -> None:
+ # Determine scorer from next_command (loser gets the kickoff).
+ if violation.next_command == RefereeCommand.PREPARE_KICKOFF_BLUE:
+ # Blue gets kickoff → yellow scored.
+ self.yellow_team.increment_score()
+ logger.info(
+ "Goal by Yellow! Score: Yellow %d – Blue %d",
+ self.yellow_team.score,
+ self.blue_team.score,
+ )
+ elif violation.next_command == RefereeCommand.PREPARE_KICKOFF_YELLOW:
+ # Yellow gets kickoff → blue scored.
+ self.blue_team.increment_score()
+ logger.info(
+ "Goal by Blue! Score: Yellow %d – Blue %d",
+ self.yellow_team.score,
+ self.blue_team.score,
+ )
+
+ self.command = RefereeCommand.STOP
+ self.command_counter += 1
+ self.command_timestamp = current_time
+ self.next_command = self._ball_placement_command_for(violation.next_command) or violation.next_command
+ self._post_ball_placement_command = (
+ violation.next_command if self.next_command in self._BALL_PLACEMENT_COMMANDS else None
+ )
+ self.ball_placement_target = (0.0, 0.0)
+ self.status_message = violation.status_message
+ self._stop_entered_time = current_time
+
+ def _handle_foul(self, violation: RuleViolation, current_time: float) -> None:
+ self.command = violation.suggested_command
+ self.command_counter += 1
+ self.command_timestamp = current_time
+ placement_command = (
+ self._ball_placement_command_for(violation.next_command)
+ if violation.designated_position is not None
+ else None
+ )
+ self.next_command = placement_command or violation.next_command
+ self._post_ball_placement_command = violation.next_command if placement_command is not None else None
+ self.ball_placement_target = violation.designated_position
+ self.status_message = violation.status_message
+ logger.info(
+ "Foul detected: %s → %s (next: %s)",
+ violation.rule_name,
+ violation.suggested_command.name,
+ violation.next_command.name if violation.next_command else "None",
+ )
+
+ def _generate_referee_data(self, current_time: float) -> RefereeData:
+ stage_time_left = max(0.0, self.stage_duration - (current_time - self.stage_start_time))
+ return RefereeData(
+ source_identifier="custom_referee",
+ time_sent=current_time,
+ time_received=current_time,
+ referee_command=self.command,
+ referee_command_timestamp=self.command_timestamp,
+ stage=self.stage,
+ stage_time_left=stage_time_left,
+ blue_team=copy.copy(self.blue_team),
+ yellow_team=copy.copy(self.yellow_team),
+ designated_position=self.ball_placement_target,
+ blue_team_on_positive_half=None,
+ next_command=self.next_command,
+ current_action_time_remaining=None,
+ status_message=self.status_message,
+ )
diff --git a/utama_core/data_processing/refiners/robot_info.py b/utama_core/data_processing/refiners/robot_info.py
index 5b519cdb..315ff25f 100644
--- a/utama_core/data_processing/refiners/robot_info.py
+++ b/utama_core/data_processing/refiners/robot_info.py
@@ -1,26 +1,65 @@
import warnings
from dataclasses import replace
-from typing import List
+from typing import FrozenSet, List, Optional
+from utama_core.config.physical_constants import ROBOT_RADIUS
from utama_core.data_processing.refiners.base_refiner import BaseRefiner
from utama_core.entities.data.command import RobotResponse
from utama_core.entities.game.game_frame import GameFrame
# TODO: current doesn't handle has_ball for enemy robots. In future, implement using vision data
+# Distance threshold for vision-based has_ball inference: robot centre + small buffer.
+_BALL_CAPTURE_DIST = ROBOT_RADIUS + 0.04 # ~0.13 m
+
class RobotInfoRefiner(BaseRefiner):
+ """Merges IR-sensor robot responses into the game frame.
+
+ Args:
+ trusted_ir_robots: Set of robot IDs (vision IDs) whose IR sensor is known to be
+ working. Robots in this set use the raw sensor reading directly. Any robot
+ ID **not** in this set has its has_ball inferred from vision proximity
+ (~0.13 m threshold) instead. Pass ``None`` (default) to trust every
+ robot's IR sensor — this is the normal stable-hardware behaviour and can
+ be restored by simply removing the argument.
+ """
+
+ def __init__(self, trusted_ir_robots: Optional[FrozenSet[int]] = None):
+ self._trusted_ir_robots = trusted_ir_robots
+
def refine(self, game_frame: GameFrame, robot_responses: List[RobotResponse]):
- if robot_responses is None or len(robot_responses) == 0:
+ if self._trusted_ir_robots is None and (robot_responses is None or len(robot_responses) == 0):
return game_frame
friendly_robots = game_frame.friendly_robots.copy()
- for robot_response in robot_responses:
- id = robot_response.id
- if id in friendly_robots:
- robot = friendly_robots[id]
- friendly_robots[id] = replace(robot, has_ball=robot_response.has_ball)
- else:
- warnings.warn(f"Robot ID {id} in robot responses not found in friendly robots. ")
+
+ # When a trust list is active, first apply vision-proximity inference for every
+ # untrusted robot. This ensures that robots which did not return a response this
+ # tick (non-blocking real-mode polling can skip robots) do not keep stale values.
+ if self._trusted_ir_robots is not None:
+ for robot_id, robot in friendly_robots.items():
+ if robot_id not in self._trusted_ir_robots:
+ friendly_robots[robot_id] = replace(robot, has_ball=self._infer_has_ball(game_frame, robot))
+
+ # Overlay IR readings for trusted robots that responded this tick.
+ if robot_responses:
+ for robot_response in robot_responses:
+ rid = robot_response.id
+ if rid not in friendly_robots:
+ warnings.warn(f"Robot ID {rid} in robot responses not found in friendly robots. ")
+ continue
+
+ if self._trusted_ir_robots is None or rid in self._trusted_ir_robots:
+ friendly_robots[rid] = replace(friendly_robots[rid], has_ball=robot_response.has_ball)
+
new_game_frame = replace(game_frame, friendly_robots=friendly_robots)
return new_game_frame
+
+ @staticmethod
+ def _infer_has_ball(game_frame: GameFrame, robot) -> bool:
+ """Proximity-based has_ball: True when robot is within capture distance of ball."""
+ if game_frame.ball is None:
+ return False
+ ball_2d = game_frame.ball.p.to_2d()
+ return robot.p.distance_to(ball_2d) < _BALL_CAPTURE_DIST
diff --git a/utama_core/global_utils/mapping_utils.py b/utama_core/global_utils/mapping_utils.py
index 5d5971a0..3ce236f4 100644
--- a/utama_core/global_utils/mapping_utils.py
+++ b/utama_core/global_utils/mapping_utils.py
@@ -23,6 +23,23 @@ def map_friendly_enemy_to_colors(my_team_is_yellow: bool, friendly_item: T, enem
return yellow_item, blue_item
+# NB: actually map_friendly_enemy_to_colors and map_colors_to_friendly_enemy have the same logic
+
+
+def map_colors_to_friendly_enemy(my_team_is_yellow: bool, yellow_item: T, blue_item: T) -> Tuple[T, T]:
+ """Map yellow and blue items to their respective friendly and enemy items based on my team color.
+
+ Args:
+ my_team_is_yellow (bool): True if the team is yellow, False if blue.
+ yellow_item (T): Any item from the yellow team (int, list, etc.)
+ blue_item (T): Any item from the blue team (int, list, etc.)
+
+ Returns:
+ Tuple[T, T]: A tuple of (friendly_item, enemy_item).
+ """
+ return map_friendly_enemy_to_colors(my_team_is_yellow, yellow_item, blue_item)
+
+
def map_left_right_to_colors(my_team_is_yellow: bool, my_team_is_right: bool, right_item: T, left_item: T):
"""Map left and right items to their respective colors based on my team color and position.
diff --git a/utama_core/run/strategy_runner.py b/utama_core/run/strategy_runner.py
index c47cc35d..f748517b 100644
--- a/utama_core/run/strategy_runner.py
+++ b/utama_core/run/strategy_runner.py
@@ -6,7 +6,7 @@
import warnings
from collections import deque
from dataclasses import dataclass, field
-from typing import TYPE_CHECKING, List, Optional, Tuple
+from typing import TYPE_CHECKING, FrozenSet, List, Optional, Tuple
from rich.live import Live
from rich.text import Text
@@ -14,7 +14,7 @@
from utama_core.config.enums import Mode, mode_str_to_enum
from utama_core.config.field_params import STANDARD_FIELD_DIMS, FieldDimensions
from utama_core.config.formations import FormationType, get_formations
-from utama_core.config.physical_constants import MAX_ROBOTS
+from utama_core.config.physical_constants import MAX_ROBOT_ID, MAX_ROBOTS
from utama_core.config.settings import (
FPS_PRINT_INTERVAL,
MAX_CAMERAS,
@@ -29,12 +29,13 @@
RobotInfoRefiner,
VelocityRefiner,
)
-from utama_core.entities.data.command import RobotCommand
+from utama_core.entities.data.command import RobotCommand, RobotResponse
from utama_core.entities.data.raw_vision import RawVisionData
from utama_core.entities.game import Game, GameFrame, GameHistory
from utama_core.entities.game.field import Field, FieldBounds
from utama_core.entities.referee.referee_command import RefereeCommand
from utama_core.global_utils.mapping_utils import (
+ map_colors_to_friendly_enemy,
map_friendly_enemy_to_colors,
map_left_right_to_colors,
)
@@ -135,6 +136,14 @@ class StrategyRunner:
instance to use the in-process referee, ``OfficialReferee()`` to consume
commands from the SSL game-controller over the network, or ``None``
(default) to run without any referee input.
+ yellow_vision_to_cmd_mapping (dict[int, int], optional): Mapping from vision robot IDs to command robot IDs for the yellow team.
+ Used only in real mode. In real PVP/shared-transmitter mode, mappings are required for both teams and must include all expected robots.
+ blue_vision_to_cmd_mapping (dict[int, int], optional): Mapping from vision robot IDs to command robot IDs for the blue team.
+ Used only in real mode. In real PVP/shared-transmitter mode, mappings are required for both teams and must include all expected robots.
+ yellow_trusted_ir_robots (FrozenSet[int], optional): Vision IDs of yellow-team robots whose IR (has_ball) sensor
+ 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.
"""
def __init__(
@@ -160,6 +169,10 @@ def __init__(
filtering: bool = False,
referee: RefereeSource = None,
formation_type: Optional[FormationType] = None,
+ yellow_vision_to_cmd_mapping: Optional[dict[int, int]] = None,
+ 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,
):
self.logger = logging.getLogger(__name__)
@@ -193,8 +206,16 @@ 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
self.my, self.opp = self._setup_sides_data(
- strategy, opp_strategy, filtering, control_scheme, opp_control_scheme
+ strategy,
+ opp_strategy,
+ filtering,
+ control_scheme,
+ opp_control_scheme,
+ my_trusted_ir_robots=my_trusted_ir,
+ opp_trusted_ir_robots=opp_trusted_ir,
)
### functions below rely on self.my and self.opp ###
@@ -202,6 +223,21 @@ def __init__(
self.rsim_env, self.sim_controller = self._load_sim(rsim_noise, rsim_vanishing)
self._assert_exp_robots_and_ball(exp_friendly, exp_enemy, exp_ball)
+ # mapping for mismatch between vision and cmd ids
+ self.yellow_vision_to_cmd_mapping = self._validate_vision_to_cmd_mapping(
+ yellow_vision_to_cmd_mapping, is_yellow=True
+ )
+ self.blue_vision_to_cmd_mapping = self._validate_vision_to_cmd_mapping(
+ blue_vision_to_cmd_mapping, is_yellow=False
+ )
+ 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()}
+
+ 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
+ )
+
self._load_robot_controllers()
# Remove Rsim ball. Rsim does not have the flexibilty to start without a ball.
@@ -259,6 +295,77 @@ def __init__(
self.profiler_name = profiler_name
self.profiler = cProfile.Profile() if profiler_name else None
+ def _validate_vision_to_cmd_mapping(self, mapping: Optional[dict[int, int]], is_yellow: bool) -> dict[int, int]:
+ if self.mode == Mode.REAL:
+ explicitly_provided = mapping is not None
+ if mapping is None:
+ if self.opp:
+ raise ValueError(
+ "explicit vision_to_cmd_mapping is required for both teams in real PVP/shared-transmitter mode."
+ )
+ return {}
+
+ if not isinstance(mapping, dict):
+ raise TypeError(
+ f"vision_to_cmd_mapping must be a dictionary mapping vision robot IDs to command robot IDs; got {type(mapping).__name__}."
+ )
+
+ # if we are not running an opp strat, but mapping provided, warn that it will be ignored
+ if self.opp is None and self.my_team_is_yellow ^ is_yellow:
+ warnings.warn(
+ "vision_to_cmd_mapping is provided but will be ignored since the opponent team is not being controlled."
+ )
+
+ if self.opp and explicitly_provided:
+ if is_yellow ^ self.my_team_is_yellow:
+ if len(mapping) != self.exp_enemy:
+ raise ValueError(
+ "vision_to_cmd_mapping for opponent team must include all expected opponent robots for shared transmission."
+ )
+ else:
+ if len(mapping) != self.exp_friendly:
+ raise ValueError(
+ "vision_to_cmd_mapping for friendly team must include all expected friendly robots for shared transmission."
+ )
+
+ for vision_id, cmd_id in mapping.items():
+ if not isinstance(vision_id, int) or not isinstance(cmd_id, int):
+ raise TypeError(
+ f"vision_to_cmd_mapping must map integers to integers; got key type {type(vision_id).__name__} and value type {type(cmd_id).__name__}."
+ )
+ if vision_id < 0 or cmd_id < 0:
+ raise ValueError(
+ f"vision_to_cmd_mapping cannot have negative IDs; got vision ID {vision_id} and command ID {cmd_id}."
+ )
+ if vision_id > MAX_ROBOT_ID:
+ raise ValueError(
+ f"vision_to_cmd_mapping cannot have vision IDs greater than {MAX_ROBOT_ID}; got vision ID {vision_id}."
+ )
+ if cmd_id > 0xFF:
+ raise ValueError(
+ f"vision_to_cmd_mapping cannot have command IDs greater than 255 (1 byte limit); got command ID {cmd_id}."
+ )
+ return mapping
+ else:
+ if mapping is not None:
+ raise ValueError(
+ "vision_to_cmd_mapping should not be provided in simulation modes; robot ID mapping is only needed in real mode."
+ )
+ return {}
+
+ def _check_no_cmd_duplicate_if_transmission_sharing(
+ self, yellow_mapping: dict[int, int], blue_mapping: dict[int, int]
+ ):
+ seen = set()
+ dicts = [yellow_mapping, blue_mapping]
+ for d in dicts:
+ for v in d.values():
+ if v in seen:
+ raise ValueError(
+ f"vision_to_cmd_mapping for friendly and opponent teams cannot have overlapping command IDs since commands are transmitted together; duplicate command ID: {v}."
+ )
+ seen.add(v)
+
def _handle_sigint(self, sig, frame):
self._stop_event.set()
signal.default_int_handler(sig, frame)
@@ -343,6 +450,8 @@ def _setup_sides_data(
filtering: bool,
control_scheme: str,
opp_control_scheme: Optional[str],
+ my_trusted_ir_robots: Optional[FrozenSet[int]] = None,
+ opp_trusted_ir_robots: Optional[FrozenSet[int]] = None,
) -> Tuple[SideRuntime, Optional[SideRuntime]]:
"""Setup the data structures for both sides (my team and opponent)
Args:
@@ -351,6 +460,8 @@ def _setup_sides_data(
filtering (bool): Whether to use filtering in the position refiners.
control_scheme (str): Name of the motion control scheme to use for the friendly team.
opp_control_scheme (Optional[str]): Name of the motion control scheme to use for the opponent team. If not set, uses same as friendly.
+ my_trusted_ir_robots (FrozenSet[int], optional): Vision IDs of friendly robots whose IR sensor is trusted.
+ opp_trusted_ir_robots (FrozenSet[int], optional): Vision IDs of opponent robots whose IR sensor is trusted.
Side effect: Initializes the SideRuntime for both friendly and opponent sides, including their strategies, refiners, and motion controllers.
@@ -359,7 +470,10 @@ def _setup_sides_data(
"""
opp_side = None
my_pos_ref, my_vel_ref, my_robot_ref = self._init_refiners(
- self.full_field_dims, filtering=filtering, exp_ball=self.exp_ball
+ self.full_field_dims,
+ filtering=filtering,
+ exp_ball=self.exp_ball,
+ trusted_ir_robots=my_trusted_ir_robots,
)
my_motion_controller = get_control_scheme(control_scheme)
my_strategy.setup_strategy_blackboard(is_opp_strat=False)
@@ -373,7 +487,10 @@ def _setup_sides_data(
if opp_strategy is not None:
opp_pos_ref, opp_vel_ref, opp_robot_ref = self._init_refiners(
- self.full_field_dims, filtering=filtering, exp_ball=self.exp_ball
+ self.full_field_dims,
+ filtering=filtering,
+ exp_ball=self.exp_ball,
+ trusted_ir_robots=opp_trusted_ir_robots,
)
opp_motion_controller = (
get_control_scheme(opp_control_scheme) if opp_control_scheme is not None else my_motion_controller
@@ -389,6 +506,41 @@ def _setup_sides_data(
return my_side, opp_side
+ def _split_robot_responses_by_team(
+ self, responses: List[RobotResponse]
+ ) -> Tuple[List[RobotResponse], List[RobotResponse]]:
+ """Split a list of RobotResponse objects into separate lists for the friendly and opponent teams
+ based on the robot IDs and the vision.
+ """
+ friendly_responses = []
+ opponent_responses = []
+
+ for response in responses:
+ cmd_id = response.id
+
+ if self.my_team_is_yellow:
+ vision_id = self.yellow_cmd_to_vision_mapping.get(cmd_id)
+ if vision_id is not None:
+ friendly_responses.append(RobotResponse(vision_id, response.has_ball))
+ else:
+ opp_vision_id = self.blue_cmd_to_vision_mapping.get(cmd_id)
+ if opp_vision_id is not None:
+ opponent_responses.append(RobotResponse(opp_vision_id, response.has_ball))
+ else:
+ self.logger.warning(f"RobotResponse cmd_id={cmd_id} not found in either yellow or blue mapping")
+ else:
+ vision_id = self.blue_cmd_to_vision_mapping.get(cmd_id)
+ if vision_id is not None:
+ friendly_responses.append(RobotResponse(vision_id, response.has_ball))
+ else:
+ opp_vision_id = self.yellow_cmd_to_vision_mapping.get(cmd_id)
+ if opp_vision_id is not None:
+ opponent_responses.append(RobotResponse(opp_vision_id, response.has_ball))
+ else:
+ self.logger.warning(f"RobotResponse cmd_id={cmd_id} not found in either blue or yellow mapping")
+
+ return friendly_responses, opponent_responses
+
def _remove_rsim_ball(self):
"""Removes the ball from the RSim environment by teleporting it off-field."""
self.sim_controller.remove_ball()
@@ -632,12 +784,23 @@ def _load_robot_controllers(self):
)
elif self.mode == Mode.REAL:
+ my_viz_to_cmd_mapping, opp_viz_to_cmd_mapping = map_colors_to_friendly_enemy(
+ self.my_team_is_yellow,
+ self.yellow_vision_to_cmd_mapping,
+ self.blue_vision_to_cmd_mapping,
+ )
my_robot_controller = RealRobotController(
- is_team_yellow=self.my_team_is_yellow, n_friendly=self.exp_friendly
+ is_team_yellow=self.my_team_is_yellow,
+ n_friendly=self.exp_friendly,
+ vision_to_cmd_mapping=my_viz_to_cmd_mapping,
)
if self.opp:
+ serial = my_robot_controller.serial_port # share serial connection for efficiency
opp_robot_controller = RealRobotController(
- is_team_yellow=not self.my_team_is_yellow, n_friendly=self.exp_enemy
+ is_team_yellow=not self.my_team_is_yellow,
+ n_friendly=self.exp_enemy,
+ vision_to_cmd_mapping=opp_viz_to_cmd_mapping,
+ serial_port=serial,
)
else:
@@ -654,14 +817,17 @@ def _init_refiners(
field_dims: FieldDimensions,
filtering: bool,
exp_ball: bool = True,
+ trusted_ir_robots: Optional[FrozenSet[int]] = None,
) -> tuple[PositionRefiner, VelocityRefiner, RobotInfoRefiner]:
"""
Initialize the position, velocity, and robot info refiners.
Args:
- field_bounds (FieldBounds): The bounds of the field.
+ field_dims (FieldDimensions): The field dimensions.
filtering (bool): Whether to use filtering in the position refiner.
exp_ball (bool): Whether the ball is expected. When False, the position refiner is
allowed to return None if no ball is detected in raw vision data.
+ trusted_ir_robots (FrozenSet[int], optional): Vision IDs of robots whose IR sensor is trusted.
+ See RobotInfoRefiner for details.
Returns:
tuple: The initialized PositionRefiner, VelocityRefiner, and RobotInfoRefiner.
"""
@@ -671,7 +837,7 @@ def _init_refiners(
exp_ball=exp_ball,
)
velocity_refiner = VelocityRefiner()
- robot_info_refiner = RobotInfoRefiner()
+ robot_info_refiner = RobotInfoRefiner(trusted_ir_robots=trusted_ir_robots)
return position_refiner, velocity_refiner, robot_info_refiner
@@ -928,15 +1094,36 @@ def _run_step(self):
self._last_referee_data = self.ref_buffer.popleft()
referee_data = self._last_referee_data
+ friendly_res, opp_res = None, None
+ if self.mode == Mode.REAL:
+ responses = self.my.strategy.robot_controller.get_robots_responses()
+ if self.opp:
+ friendly_res, opp_res = self._split_robot_responses_by_team(responses)
+ else:
+ cmd_to_vision = (
+ self.yellow_cmd_to_vision_mapping if self.my_team_is_yellow else self.blue_cmd_to_vision_mapping
+ )
+ if cmd_to_vision:
+ friendly_res = []
+ for r in responses:
+ vision_id = cmd_to_vision.get(r.id)
+ if vision_id is None:
+ self.logger.warning(f"RobotResponse cmd_id={r.id} not found in mapping for controlled team")
+ continue
+ friendly_res.append(RobotResponse(vision_id, r.has_ball))
+ else:
+ friendly_res = responses
+
# alternate between opp and friendly playing
+ real = self.mode == Mode.REAL
if self.toggle_opp_first:
if self.opp:
- self._step_game(vision_frames, referee_data, True)
- self._step_game(vision_frames, referee_data, False)
+ self._step_game(vision_frames, referee_data, True, real_responses=opp_res if real else None)
+ self._step_game(vision_frames, referee_data, False, real_responses=friendly_res if real else None)
else:
- self._step_game(vision_frames, referee_data, False)
+ self._step_game(vision_frames, referee_data, False, real_responses=friendly_res if real else None)
if self.opp:
- self._step_game(vision_frames, referee_data, True)
+ self._step_game(vision_frames, referee_data, True, real_responses=opp_res if real else None)
self.toggle_opp_first = not self.toggle_opp_first
# --- rate limiting ---
@@ -1020,6 +1207,7 @@ def _step_game(
vision_frames: List[RawVisionData],
referee_data,
running_opp: bool,
+ real_responses: Optional[List[RobotResponse]] = None,
):
"""Step the game for the robot controller and strategy.
@@ -1027,11 +1215,16 @@ def _step_game(
vision_frames (List[RawVisionData]): The vision frames.
referee_data: The referee data from RSim or network receiver.
running_opp (bool): Whether to run the opponent strategy.
+ real_responses (Optional[List[RobotResponse]]): The robot responses pulled for real.
+ We use a shared transmitter, so it cannot be pulled per side.
"""
side = self.opp if running_opp else self.my
# Pull responses from robot controller
- responses = side.strategy.robot_controller.get_robots_responses()
+ if self.mode != Mode.REAL:
+ responses = side.strategy.robot_controller.get_robots_responses()
+ else:
+ responses = real_responses if real_responses is not None else []
# Update game frame with refined information
new_game_frame = side.position_refiner.refine(side.current_game_frame, vision_frames)
diff --git a/utama_core/strategy/common/abstract_strategy.py b/utama_core/strategy/common/abstract_strategy.py
index 091acdfc..cddfa89b 100644
--- a/utama_core/strategy/common/abstract_strategy.py
+++ b/utama_core/strategy/common/abstract_strategy.py
@@ -203,6 +203,7 @@ def setup_strategy_blackboard(self, is_opp_strat: bool):
Setups the blackboard based on if is_opp_strat.
"""
+ self._is_opp_strat = is_opp_strat
self.blackboard = self._setup_blackboard(is_opp_strat)
def setup_behaviour_tree(self, is_opp_strat: bool):
diff --git a/utama_core/strategy/examples/__init__.py b/utama_core/strategy/examples/__init__.py
index a59b9b55..d85dcc67 100644
--- a/utama_core/strategy/examples/__init__.py
+++ b/utama_core/strategy/examples/__init__.py
@@ -1,4 +1,11 @@
+from utama_core.strategy.examples.ball_placement_and_kick_strategy import (
+ BallPlacementAndKickStrategy,
+ KickAfterDirectFreeStep,
+)
from utama_core.strategy.examples.defense_strategy import DefenceStrategy
+from utama_core.strategy.examples.deliberate_out_of_bounds_strategy import (
+ DeliberateOutOfBoundsStrategy,
+)
from utama_core.strategy.examples.go_to_ball_ex import GoToBallExampleStrategy
from utama_core.strategy.examples.motion_planning.multi_robot_navigation_strategy import (
MultiRobotNavigationStrategy,
diff --git a/utama_core/strategy/examples/ball_placement_and_kick_strategy.py b/utama_core/strategy/examples/ball_placement_and_kick_strategy.py
new file mode 100644
index 00000000..d26e3270
--- /dev/null
+++ b/utama_core/strategy/examples/ball_placement_and_kick_strategy.py
@@ -0,0 +1,116 @@
+from typing import Optional
+
+import py_trees
+
+from utama_core.entities.data.command import RobotCommand
+from utama_core.entities.referee.referee_command import RefereeCommand
+from utama_core.strategy.common import AbstractBehaviour
+from utama_core.strategy.examples.ball_placement_strategy import BallPlacementStrategy
+
+_DIRECT_FREE_COMMANDS = frozenset(
+ {
+ RefereeCommand.DIRECT_FREE_YELLOW,
+ RefereeCommand.DIRECT_FREE_BLUE,
+ }
+)
+
+
+class _CommandTracker:
+ """Mutable container updated by BallPlacementAndKickStrategy.step() every tick.
+
+ Because KickAfterDirectFreeStep is not ticked while the referee-override tree
+ handles DIRECT_FREE_* commands, it cannot observe that command transition
+ directly. The strategy's step() method samples the referee command before
+ each tree tick and records it here so the behaviour can read it on the next
+ NORMAL_START tick.
+ """
+
+ def __init__(self):
+ self.prev_command: Optional[RefereeCommand] = None
+
+
+class KickAfterDirectFreeStep(AbstractBehaviour):
+ """Kick once on each NORMAL_START that follows a direct-free command."""
+
+ _KICK_DISTANCE = 0.22
+
+ def __init__(self, name: str, tracker: _CommandTracker):
+ super().__init__(name=name)
+ self._tracker = tracker
+
+ def setup_(self):
+ self._active_restart_timestamp: float | None = None
+ self._kick_sent_for_timestamp: float | None = None
+
+ def update(self) -> py_trees.common.Status:
+ game = self.blackboard.game
+ ref = game.referee
+
+ if ref is None:
+ return py_trees.common.Status.RUNNING
+
+ current_command = ref.referee_command
+
+ if current_command != RefereeCommand.NORMAL_START:
+ return py_trees.common.Status.RUNNING
+
+ # Only kick if we transitioned here from a direct free (not from a kickoff
+ # seed / force-start at game start). _tracker.prev_command is maintained
+ # by the strategy's step() so it reflects commands seen during ticks when
+ # this node was overridden by the RefereeOverride subtree.
+ if self._tracker.prev_command not in _DIRECT_FREE_COMMANDS:
+ return py_trees.common.Status.RUNNING
+
+ restart_timestamp = ref.referee_command_timestamp
+ if restart_timestamp != self._active_restart_timestamp:
+ self._active_restart_timestamp = restart_timestamp
+ self._kick_sent_for_timestamp = None
+
+ if self._kick_sent_for_timestamp == restart_timestamp:
+ return py_trees.common.Status.RUNNING
+
+ if game.ball is None or not game.friendly_robots:
+ return py_trees.common.Status.RUNNING
+
+ kicker_id = min(
+ game.friendly_robots,
+ key=lambda rid: game.friendly_robots[rid].p.distance_to(game.ball.p),
+ )
+ kicker = game.friendly_robots[kicker_id]
+ if kicker.p.distance_to(game.ball.p) > self._KICK_DISTANCE:
+ return py_trees.common.Status.RUNNING
+
+ self.blackboard.cmd_map[kicker_id] = RobotCommand(
+ local_forward_vel=0,
+ local_left_vel=0,
+ angular_vel=0,
+ kick=1,
+ chip=0,
+ dribble=0,
+ )
+ self._kick_sent_for_timestamp = restart_timestamp
+
+ return py_trees.common.Status.RUNNING
+
+
+class BallPlacementAndKickStrategy(BallPlacementStrategy):
+ """Use referee ball placement/direct-free setup, then kick once on NORMAL_START."""
+
+ def __init__(self):
+ self._tracker = _CommandTracker()
+ super().__init__()
+
+ def create_behaviour_tree(self) -> py_trees.behaviour.Behaviour:
+ root = py_trees.composites.Sequence(name="PlacementRestartKickRoot", memory=False)
+ root.add_child(KickAfterDirectFreeStep(name="KickAfterDirectFree", tracker=self._tracker))
+ return root
+
+ def step(self):
+ # Sample the referee command *before* the tree tick so that transitions
+ # through DIRECT_FREE_* (which are handled by the RefereeOverride subtree
+ # and never reach KickAfterDirectFreeStep) are still recorded in the tracker.
+ game = self.blackboard.game
+ ref = game.referee
+ if ref is not None:
+ self._tracker.prev_command = ref.referee_command
+ super().step()
diff --git a/utama_core/strategy/examples/ball_placement_strategy.py b/utama_core/strategy/examples/ball_placement_strategy.py
new file mode 100644
index 00000000..f22611f5
--- /dev/null
+++ b/utama_core/strategy/examples/ball_placement_strategy.py
@@ -0,0 +1,182 @@
+"""ball_placement_strategy.py — Skeleton for the ball placement feature.
+
+Your task
+---------
+Implement ``BallPlacementStep.update()`` so that, when the referee issues
+BALL_PLACEMENT_YELLOW (our team places), one robot picks up the ball with its
+dribbler and carries it to ``game.referee.designated_position``, while the
+other robot waits clear of the ball.
+
+The referee integration (deciding *when* to use this behaviour) is already
+wired up in the referee override tree — you do not need to touch the referee
+config. Your work is entirely inside this file's strategy and behaviour tree.
+
+How ball placement works
+------------------------
+1. Referee issues BALL_PLACEMENT_YELLOW with a ``designated_position`` (x, y).
+2. One robot (the "placer") drives to the ball with its dribbler on to capture it.
+3. Once it has the ball (``robot.has_ball`` is True), it carries it to
+ ``designated_position`` and stops.
+4. When the ball is within ``BALL_PLACEMENT_DONE_DISTANCE`` of the target the
+ referee auto-advances to DIRECT_FREE_YELLOW → NORMAL_START.
+5. All other robots must stay at least ``BALL_KEEP_OUT_DISTANCE`` from the ball.
+
+Useful references
+-----------------
+- ``game.referee.designated_position`` — (x, y) tuple, the placement target.
+- ``game.ball.p`` — current ball position (Vector2D-like).
+- ``robot.has_ball`` — True when the robot's IR dribbler sensor
+ detects the ball.
+- ``move(game, motion_controller, robot_id, target, orientation, dribbling=True)``
+ — send a motion command; pass ``dribbling=True`` to activate the dribbler.
+- ``empty_command(False)`` — stop a robot in place.
+- ``BALL_PLACEMENT_DONE_DISTANCE`` — 0.15 m; ball must be this close to target.
+- ``BALL_KEEP_OUT_DISTANCE`` — 0.8 m; clearance for non-placer robots.
+
+Running the demo
+----------------
+ pixi run python demo_ball_placement.py
+
+The RSim window opens and the browser GUI at http://localhost:8080 lets you
+issue BALL_PLACEMENT_YELLOW commands from the "Manual Commands" panel and watch
+your strategy respond.
+
+Running the tests
+-----------------
+ pixi run pytest utama_core/tests/strategy_runner/test_ball_placement_rsim.py -v
+
+There are three tests that must pass before the feature is considered complete:
+ 1. placer robot drives toward the ball after the command is issued.
+ 2. placer carries the ball toward the designated position (dribbler on).
+ 3. non-placer robot stays outside the keep-out radius throughout.
+"""
+
+import math
+from typing import Optional
+
+import py_trees
+
+from utama_core.config.referee_constants import (
+ BALL_KEEP_OUT_DISTANCE,
+ BALL_PLACEMENT_DONE_DISTANCE,
+)
+from utama_core.entities.data.vector import Vector2D
+from utama_core.skills.src.utils.move_utils import empty_command, move, turn_on_spot
+from utama_core.strategy.common import AbstractBehaviour, AbstractStrategy
+from utama_core.strategy.referee.actions import _clear_to_legal_positions
+
+
+class BallPlacementStep(AbstractBehaviour):
+ """Move one robot to place the ball at the referee's designated position.
+
+ Implementation guide
+ --------------------
+ The ``update()`` method is called every simulation tick while the referee
+ command is BALL_PLACEMENT_YELLOW. It must:
+
+ 1. Read ``game.referee.designated_position`` — the (x, y) target tuple.
+ 2. If the ball is already at the target (within ``BALL_PLACEMENT_DONE_DISTANCE``),
+ stop all robots and return RUNNING (the referee will advance the state).
+ 3. Pick the robot closest to the ball as the placer.
+ 4. If the placer does not yet have the ball (``robot.has_ball`` is False),
+ drive it to the ball with the dribbler on.
+ 5. Once the placer has the ball, drive it to ``designated_position`` with
+ the dribbler still on.
+ 6. Keep all non-placer robots at least ``BALL_KEEP_OUT_DISTANCE`` away from
+ the ball by calling ``_clear_to_legal_positions``.
+
+ Return ``py_trees.common.Status.RUNNING`` on every tick (the referee
+ override tree holds here until the command changes).
+
+ Tips
+ ----
+ - Use ``Vector2D(target[0], target[1])`` to convert the tuple to a Vector2D.
+ - Orientation: ``robot.p.angle_to(target_pos)`` gives the heading angle.
+ - Call ``move(..., dribbling=True)`` to activate the dribbler.
+ - Write commands to ``self.blackboard.cmd_map[robot_id]`` for each robot.
+ """
+
+ def update(self) -> py_trees.common.Status:
+ game = self.blackboard.game
+ ref = game.referee
+ motion_controller = self.blackboard.motion_controller
+
+ if ref is None or game.ball is None or not game.friendly_robots:
+ return self._stop_all()
+
+ target = ref.designated_position
+ if target is None:
+ return self._stop_all()
+
+ target_pos = Vector2D(target[0], target[1])
+ ball = game.ball
+
+ if ball.p.distance_to(target_pos) <= BALL_PLACEMENT_DONE_DISTANCE:
+ return self._stop_all()
+
+ placer_id = min(
+ game.friendly_robots,
+ key=lambda rid: game.friendly_robots[rid].p.distance_to(ball.p),
+ )
+ placer = game.friendly_robots[placer_id]
+
+ _FACE_READY_ANGLE = 0.2
+
+ if placer.has_ball:
+ orientation = placer.p.angle_to(target_pos)
+ face_error = math.atan2(
+ math.sin(orientation - placer.orientation), math.cos(orientation - placer.orientation)
+ )
+ if abs(face_error) > _FACE_READY_ANGLE:
+ self.blackboard.cmd_map[placer_id] = turn_on_spot(
+ game, motion_controller, placer_id, orientation, dribbling=True
+ )
+ else:
+ self.blackboard.cmd_map[placer_id] = move(
+ game, motion_controller, placer_id, target_pos, orientation, dribbling=True
+ )
+ else:
+ target_for_move = Vector2D(ball.p.x, ball.p.y)
+ orientation = placer.p.angle_to(target_for_move)
+ self.blackboard.cmd_map[placer_id] = move(
+ game, motion_controller, placer_id, target_for_move, orientation, dribbling=True
+ )
+
+ _clear_to_legal_positions(
+ self.blackboard,
+ ball_keep_dist=BALL_KEEP_OUT_DISTANCE,
+ exempt_robot_ids={placer_id},
+ )
+ return py_trees.common.Status.RUNNING
+
+ def _stop_all(self) -> py_trees.common.Status:
+ game = self.blackboard.game
+ for robot_id in game.friendly_robots:
+ self.blackboard.cmd_map[robot_id] = empty_command(False)
+ return py_trees.common.Status.RUNNING
+
+
+class BallPlacementStrategy(AbstractStrategy):
+ """2v2 ball placement strategy for the Exhibition Road field.
+
+ The referee override tree (built into AbstractStrategy) automatically handles
+ HALT, STOP, kickoff, and penalty commands — you only need to implement what
+ happens during BALL_PLACEMENT_YELLOW (i.e., in ``BallPlacementStep``).
+
+ During NORMAL_START / FORCE_START the ``IdleRoot`` sequence at the bottom of
+ the tree runs — replace it with your regular game strategy once placement works.
+ """
+
+ def create_behaviour_tree(self) -> py_trees.behaviour.Behaviour:
+ root = py_trees.composites.Sequence(name="BallPlacementRoot", memory=False)
+ root.add_child(BallPlacementStep(name="PlaceBall"))
+ return root
+
+ def assert_exp_robots(self, n_runtime_friendly: int, n_runtime_enemy: int) -> bool:
+ return n_runtime_friendly >= 1
+
+ def assert_exp_goals(self, includes_my_goal_line: bool, includes_opp_goal_line: bool) -> bool:
+ return True
+
+ def get_min_bounding_req(self) -> Optional[object]:
+ return None
diff --git a/utama_core/strategy/examples/deliberate_out_of_bounds_strategy.py b/utama_core/strategy/examples/deliberate_out_of_bounds_strategy.py
new file mode 100644
index 00000000..db696d52
--- /dev/null
+++ b/utama_core/strategy/examples/deliberate_out_of_bounds_strategy.py
@@ -0,0 +1,215 @@
+"""Opponent strategy that deliberately creates out-of-bounds ball-placement events."""
+
+import math
+
+import py_trees
+
+from utama_core.config.field_params import FieldDimensions
+from utama_core.config.physical_constants import ROBOT_RADIUS
+from utama_core.entities.data.command import RobotCommand
+from utama_core.entities.data.vector import Vector2D
+from utama_core.skills.src.utils.move_utils import empty_command, move
+from utama_core.strategy.common import AbstractBehaviour, AbstractStrategy
+
+
+def _angle_delta(target: float, current: float) -> float:
+ return math.atan2(math.sin(target - current), math.cos(target - current))
+
+
+class DeliberateOutOfBoundsStep(AbstractBehaviour):
+ """Line up behind the ball, shove once toward a boundary, then stop."""
+
+ _TARGETS = ("top", "bottom", "left", "right", "yellow_goal")
+ _BOUNDARY_EPSILON = 0.04
+ _GOAL_EPSILON = 0.06
+ _GOAL_MOUTH_CLEARANCE = 0.28
+ _APPROACH_BACKOFF = ROBOT_RADIUS + 0.10
+ _APPROACH_TOLERANCE = 0.08
+ _ALIGN_TOLERANCE_RAD = 0.14
+ _PUSH_SPEED = 1.9
+ _PUSH_SECONDS = 0.34
+ _STOP_SECONDS = 0.35
+
+ def __init__(self, field_dims: FieldDimensions, name: str = "DeliberateOutOfBoundsStep"):
+ super().__init__(name=name)
+ self._field_dims = field_dims
+ self._target_index = 0
+ self._phase = "approach"
+ self._phase_started_at = 0.0
+ self._waiting_for_restart = False
+
+ def update(self) -> py_trees.common.Status:
+ game = self.blackboard.game
+ motion_controller = self.blackboard.motion_controller
+
+ if game.ball is None or not game.friendly_robots:
+ return self._stop_all()
+
+ ball_pos = Vector2D(game.ball.p.x, game.ball.p.y)
+ half_length = self._field_dims.full_field_half_length
+ half_width = self._field_dims.full_field_half_width
+
+ if self._waiting_for_restart:
+ if abs(ball_pos.x) < half_length - 0.25 and abs(ball_pos.y) < half_width - 0.25:
+ self._waiting_for_restart = False
+ self._phase = "approach"
+ else:
+ return self._stop_all()
+
+ target_name = self._TARGETS[self._target_index]
+ if self._has_reached_target(ball_pos, target_name, game, half_length, half_width):
+ if self._target_index < len(self._TARGETS) - 1:
+ self._target_index += 1
+ self._waiting_for_restart = True
+ self._phase = "approach"
+ return self._stop_all()
+
+ target_point = self._target_point(target_name, ball_pos, game, half_length, half_width)
+ direction = target_point - ball_pos
+ if direction.mag() == 0.0:
+ return self._stop_all()
+ direction = Vector2D(direction.x / direction.mag(), direction.y / direction.mag())
+
+ pusher_id = min(
+ game.friendly_robots,
+ key=lambda rid: game.friendly_robots[rid].p.distance_to(ball_pos),
+ )
+ pusher = game.friendly_robots[pusher_id]
+ setup_point = Vector2D(
+ ball_pos.x - direction.x * self._APPROACH_BACKOFF,
+ ball_pos.y - direction.y * self._APPROACH_BACKOFF,
+ )
+ push_oren = math.atan2(direction.y, direction.x)
+
+ for robot_id in game.friendly_robots:
+ if robot_id != pusher_id:
+ self.blackboard.cmd_map[robot_id] = empty_command(False)
+ continue
+
+ if self._phase == "approach":
+ if pusher.p.distance_to(setup_point) <= self._APPROACH_TOLERANCE:
+ self._phase = "align"
+ self._phase_started_at = game.ts
+ self.blackboard.cmd_map[robot_id] = move(
+ game,
+ motion_controller,
+ robot_id,
+ setup_point,
+ push_oren,
+ dribbling=False,
+ )
+ continue
+
+ if self._phase == "align":
+ aligned = abs(_angle_delta(push_oren, pusher.orientation)) <= self._ALIGN_TOLERANCE_RAD
+ near_setup = pusher.p.distance_to(setup_point) <= self._APPROACH_TOLERANCE * 1.8
+ if aligned and near_setup:
+ self._phase = "push"
+ self._phase_started_at = game.ts
+ else:
+ self.blackboard.cmd_map[robot_id] = move(
+ game,
+ motion_controller,
+ robot_id,
+ setup_point,
+ push_oren,
+ dribbling=False,
+ )
+ continue
+
+ if self._phase == "push":
+ if game.ts - self._phase_started_at <= self._PUSH_SECONDS:
+ self.blackboard.cmd_map[robot_id] = RobotCommand(
+ local_forward_vel=self._PUSH_SPEED,
+ local_left_vel=0,
+ angular_vel=0,
+ kick=0,
+ chip=0,
+ dribble=0,
+ )
+ continue
+ self._phase = "stop"
+ self._phase_started_at = game.ts
+
+ if self._phase == "stop":
+ self.blackboard.cmd_map[robot_id] = empty_command(False)
+ if game.ts - self._phase_started_at >= self._STOP_SECONDS:
+ self._phase = "approach"
+
+ return py_trees.common.Status.RUNNING
+
+ def _target_point(
+ self,
+ target_name: str,
+ ball_pos: Vector2D,
+ game,
+ half_length: float,
+ half_width: float,
+ ) -> Vector2D:
+ if target_name == "top":
+ return Vector2D(ball_pos.x, half_width + self._BOUNDARY_EPSILON)
+ if target_name == "bottom":
+ return Vector2D(ball_pos.x, -half_width - self._BOUNDARY_EPSILON)
+ if target_name == "left":
+ return Vector2D(-half_length - self._BOUNDARY_EPSILON, self._safe_touchline_y(ball_pos.y, game))
+ if target_name == "right":
+ return Vector2D(half_length + self._BOUNDARY_EPSILON, self._safe_touchline_y(ball_pos.y, game))
+
+ yellow_is_right = game.my_team_is_right == game.my_team_is_yellow
+ yellow_goal_x = half_length + self._GOAL_EPSILON if yellow_is_right else -half_length - self._GOAL_EPSILON
+ return Vector2D(yellow_goal_x, 0.0)
+
+ def _safe_touchline_y(self, current_y: float, game) -> float:
+ min_abs_y = game.field.half_goal_width + self._GOAL_MOUTH_CLEARANCE
+ sign = 1.0 if current_y >= 0.0 else -1.0
+ return sign * max(abs(current_y), min_abs_y)
+
+ def _has_reached_target(
+ self,
+ ball_pos: Vector2D,
+ target_name: str,
+ game,
+ half_length: float,
+ half_width: float,
+ ) -> bool:
+ if target_name == "top":
+ return ball_pos.y >= half_width - self._BOUNDARY_EPSILON
+ if target_name == "bottom":
+ return ball_pos.y <= -half_width + self._BOUNDARY_EPSILON
+ if target_name == "left":
+ return ball_pos.x <= -half_length + self._BOUNDARY_EPSILON and abs(ball_pos.y) > game.field.half_goal_width
+ if target_name == "right":
+ return ball_pos.x >= half_length - self._BOUNDARY_EPSILON and abs(ball_pos.y) > game.field.half_goal_width
+
+ yellow_is_right = game.my_team_is_right == game.my_team_is_yellow
+ if yellow_is_right:
+ return ball_pos.x >= half_length - self._GOAL_EPSILON and abs(ball_pos.y) < game.field.half_goal_width
+ return ball_pos.x <= -half_length + self._GOAL_EPSILON and abs(ball_pos.y) < game.field.half_goal_width
+
+ def _stop_all(self) -> py_trees.common.Status:
+ game = self.blackboard.game
+ for robot_id in game.friendly_robots:
+ self.blackboard.cmd_map[robot_id] = empty_command(False)
+ return py_trees.common.Status.RUNNING
+
+
+class DeliberateOutOfBoundsStrategy(AbstractStrategy):
+ """Opponent strategy that pushes the ball out once per side, then scores on yellow."""
+
+ def __init__(self, field_dims: FieldDimensions) -> None:
+ self._field_dims = field_dims
+ super().__init__()
+
+ def assert_exp_robots(self, n_runtime_friendly: int, n_runtime_enemy: int) -> bool:
+ return n_runtime_friendly >= 1
+
+ def assert_exp_goals(self, includes_my_goal_line: bool, includes_opp_goal_line: bool) -> bool:
+ return True
+
+ def get_min_bounding_req(self):
+ return None
+
+ def create_behaviour_tree(self) -> py_trees.behaviour.Behaviour:
+ root = py_trees.composites.Sequence(name="DeliberateOutOfBoundsRoot", memory=False)
+ root.add_child(DeliberateOutOfBoundsStep(self._field_dims))
+ return root
diff --git a/utama_core/strategy/referee/actions.py b/utama_core/strategy/referee/actions.py
index facc9093..09b19df8 100644
--- a/utama_core/strategy/referee/actions.py
+++ b/utama_core/strategy/referee/actions.py
@@ -27,7 +27,7 @@
)
from utama_core.entities.data.vector import Vector2D
from utama_core.entities.referee.referee_command import RefereeCommand
-from utama_core.skills.src.utils.move_utils import empty_command, move
+from utama_core.skills.src.utils.move_utils import empty_command, move, turn_on_spot
from utama_core.strategy.common.abstract_behaviour import AbstractBehaviour
@@ -301,6 +301,16 @@ class BallPlacementOursStep(AbstractBehaviour):
designated position. All other robots clear away from the ball.
"""
+ _RELEASE_DELAY_SECONDS = 0.25
+
+ def setup_(self):
+ self._release_started_at: float | None = None
+ self._placer_id: int | None = None
+
+ def _reset_release(self) -> None:
+ self._release_started_at = None
+ self._placer_id = None
+
def update(self) -> py_trees.common.Status:
game = self.blackboard.game
ref = game.referee
@@ -309,37 +319,67 @@ def update(self) -> py_trees.common.Status:
# Determine which team is ours
our_team = ref.yellow_team if game.my_team_is_yellow else ref.blue_team
if getattr(our_team, "can_place_ball", None) is False:
+ self._reset_release()
return _all_stop(self.blackboard)
target = ref.designated_position
if target is None:
+ self._reset_release()
return _all_stop(self.blackboard)
target_pos = Vector2D(target[0], target[1])
ball = game.ball
if ball is None:
+ self._reset_release()
return _all_stop(self.blackboard)
if ball.p.distance_to(target_pos) <= BALL_PLACEMENT_DONE_DISTANCE:
- return _all_stop(self.blackboard)
+ if self._release_started_at is None:
+ self._release_started_at = game.ts
+
+ placer_id = self._placer_id
+ if placer_id not in game.friendly_robots:
+ placer_id = min(
+ game.friendly_robots,
+ key=lambda rid: game.friendly_robots[rid].p.distance_to(ball.p),
+ )
+
+ hold_dribbler = game.ts - self._release_started_at < self._RELEASE_DELAY_SECONDS
+ for robot_id in game.friendly_robots:
+ self.blackboard.cmd_map[robot_id] = empty_command(hold_dribbler and robot_id == placer_id)
+ return py_trees.common.Status.RUNNING
+
+ self._release_started_at = None
# Pick the placer: robot closest to the ball
placer_id = min(
game.friendly_robots,
key=lambda rid: game.friendly_robots[rid].p.distance_to(ball.p),
)
+ self._placer_id = placer_id
+
+ _FACE_READY_ANGLE = 0.2
for robot_id in game.friendly_robots:
if robot_id == placer_id:
robot = game.friendly_robots[robot_id]
if robot.has_ball:
- target_for_move = target_pos
+ oren = robot.p.angle_to(target_pos)
+ face_error = math.atan2(math.sin(oren - robot.orientation), math.cos(oren - robot.orientation))
+ if abs(face_error) > _FACE_READY_ANGLE:
+ self.blackboard.cmd_map[robot_id] = turn_on_spot(
+ game, motion_controller, robot_id, oren, dribbling=True
+ )
+ else:
+ self.blackboard.cmd_map[robot_id] = move(
+ game, motion_controller, robot_id, target_pos, oren, dribbling=True
+ )
else:
target_for_move = Vector2D(ball.p.x, ball.p.y)
- oren = robot.p.angle_to(target_for_move)
- self.blackboard.cmd_map[robot_id] = move(
- game, motion_controller, robot_id, target_for_move, oren, dribbling=True
- )
+ oren = robot.p.angle_to(target_for_move)
+ self.blackboard.cmd_map[robot_id] = move(
+ game, motion_controller, robot_id, target_for_move, oren, dribbling=True
+ )
return _clear_to_legal_positions(
self.blackboard,
ball_keep_dist=BALL_KEEP_OUT_DISTANCE,
@@ -547,8 +587,26 @@ class DirectFreeOursStep(AbstractBehaviour):
All other robots stop in place.
"""
- # How far infield from the ball the approach point is placed
- _APPROACH_OFFSET = 0.15
+ # How close the robot center should get behind the ball before turning/kicking.
+ _APPROACH_OFFSET = ROBOT_RADIUS + 0.03
+ _APPROACH_READY_DISTANCE = 0.04
+ _KICK_READY_DISTANCE = 0.16
+ _FACE_READY_ANGLE = 0.18
+
+ @staticmethod
+ def _angle_error(current: float, target: float) -> float:
+ return math.atan2(math.sin(target - current), math.cos(target - current))
+
+ def _nearest_enemy_to_ball(self, game, ball_pos: Vector2D):
+ if not game.enemy_robots:
+ return None, None
+ enemy_id = min(game.enemy_robots, key=lambda rid: game.enemy_robots[rid].p.distance_to(ball_pos))
+ return enemy_id, game.enemy_robots[enemy_id]
+
+ def _kick_target_enemy(self, game, ball_pos: Vector2D):
+ if 1 in game.enemy_robots:
+ return 1, game.enemy_robots[1]
+ return self._nearest_enemy_to_ball(game, ball_pos)
def update(self) -> py_trees.common.Status:
game = self.blackboard.game
@@ -564,22 +622,33 @@ def update(self) -> py_trees.common.Status:
if robot_id == kicker_id and ball:
robot = game.friendly_robots[robot_id]
ball_pos = Vector2D(ball.p.x, ball.p.y)
+ distance_to_ball = robot.p.distance_to(ball_pos)
+ _, enemy = self._kick_target_enemy(game, ball_pos)
- # Compute an approach point offset inward from the nearest boundary,
- # so the robot never drives through the ball toward the edge.
- half_length = _field_half_length(game)
- half_width = _field_half_width(game)
- # Inward direction: push away from whichever boundary is closest
- offset_x = 0.0
- offset_y = 0.0
- if abs(ball_pos.x) > half_length - 0.5:
- offset_x = self._APPROACH_OFFSET * (-1.0 if ball_pos.x > 0 else 1.0)
- if abs(ball_pos.y) > half_width - 0.5:
- offset_y = self._APPROACH_OFFSET * (-1.0 if ball_pos.y > 0 else 1.0)
-
- approach = Vector2D(ball_pos.x + offset_x, ball_pos.y + offset_y)
- oren = robot.p.angle_to(approach)
- self.blackboard.cmd_map[robot_id] = move(game, motion_controller, robot_id, approach, oren)
+ if enemy is None:
+ target_oren = robot.p.angle_to(ball_pos)
+ else:
+ target_oren = ball_pos.angle_to(enemy.p)
+
+ kick_dir = Vector2D(math.cos(target_oren), math.sin(target_oren))
+ approach = Vector2D(
+ ball_pos.x - kick_dir.x * self._APPROACH_OFFSET,
+ ball_pos.y - kick_dir.y * self._APPROACH_OFFSET,
+ )
+ approach = _clamp_to_field(approach, game)
+ distance_to_approach = robot.p.distance_to(approach)
+ face_error = self._angle_error(robot.orientation, target_oren)
+
+ if distance_to_approach > self._APPROACH_READY_DISTANCE:
+ self.blackboard.cmd_map[robot_id] = move(game, motion_controller, robot_id, approach, target_oren)
+ elif abs(face_error) > self._FACE_READY_ANGLE:
+ self.blackboard.cmd_map[robot_id] = turn_on_spot(
+ game, motion_controller, robot_id, target_oren, dribbling=False
+ )
+ elif distance_to_ball > self._KICK_READY_DISTANCE:
+ self.blackboard.cmd_map[robot_id] = move(game, motion_controller, robot_id, approach, target_oren)
+ else:
+ self.blackboard.cmd_map[robot_id] = empty_command(False)
else:
self.blackboard.cmd_map[robot_id] = empty_command(False)
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 278d7f0a..63feaf3d 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
@@ -16,6 +16,13 @@
TIMEOUT,
TIMESTEP,
)
+
+# Leaky-bucket dribbler thermal limiter.
+# The bucket accumulates real seconds while dribbling and drains at the same
+# rate while off. When full the dribbler is forced off until the bucket drains
+# to DRIBBLER_RESUME_SECONDS (hysteresis), preventing rapid on/off oscillation.
+DRIBBLER_MAX_ON_SECONDS: float = 30.0
+DRIBBLER_RESUME_SECONDS: float = 15.0 # must drain to 50% before re-enabling
from utama_core.entities.data.command import RobotCommand, RobotResponse
from utama_core.skills.src.utils.move_utils import empty_command
from utama_core.team_controller.src.controllers.common.robot_controller_abstract import (
@@ -24,7 +31,6 @@
logger = logging.getLogger(__name__)
-# NB: A major assumption is that the robot IDs are 0-5 for the friendly team.
MAX_VEL = REAL_PARAMS.MAX_VEL
MAX_ANGULAR_VEL = REAL_PARAMS.MAX_ANGULAR_VEL
@@ -44,19 +50,42 @@ class RealRobotController(AbstractRobotController):
n_robots (int): The number of robots in the team. Directly affects output buffer size. Default is 6.
"""
- def __init__(self, is_team_yellow: bool, n_friendly: int):
+ def __init__(
+ self,
+ is_team_yellow: bool,
+ n_friendly: int,
+ vision_to_cmd_mapping: Optional[Dict[int, int]] = None,
+ serial_port: Optional[Serial] = None,
+ ):
super().__init__(is_team_yellow, n_friendly)
- self._serial_port = self._init_serial()
+ self._serial_port = self._init_serial() if serial_port is None else serial_port
+ self._sharing_friendly_transmitter = serial_port is not None
self._rbt_cmd_size = 10 # packet size for one robot
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
- logger.debug(f"Serial port: {PORT} opened with baudrate: {BAUD_RATE} and timeout {TIMEOUT}")
+ if serial_port is None:
+ logger.debug(f"Serial port: {PORT} opened with baudrate: {BAUD_RATE} and timeout {TIMEOUT}")
+ else:
+ logger.debug(f"Sharing serial port: {self._serial_port.port}")
self._assigned_mapping = {} # mapping of robot_id to index in the out_packet
+ self._vision_to_cmd_mapping = vision_to_cmd_mapping if vision_to_cmd_mapping is not None else {}
+ cmd_to_vision_mapping = {v: k for k, v in self._vision_to_cmd_mapping.items()}
+ if len(cmd_to_vision_mapping) != len(self._vision_to_cmd_mapping):
+ raise ValueError("vision_to_cmd_mapping must be one-to-one: duplicate command IDs are not allowed.")
+ self._cmd_to_vision_mapping = cmd_to_vision_mapping
# track last kick time for each robot to transmit kick as HIGH for n timesteps after command
self._kicker_tracker: Dict[int, KickTrackerEntry] = {}
+ # leaky-bucket dribbler thermal limiter: accumulated wall-clock seconds per
+ # robot (cmd ID). Fills while dribbling, drains while off, clamped to
+ # [0, DRIBBLER_MAX_ON_SECONDS]. Dribbler is forced off when bucket is full.
+ self._dribbler_seconds: Dict[int, float] = {}
+ self._dribbler_last_tick: Dict[int, float] = {} # time.monotonic() of last call
+ self._dribbler_limit_warned: set = set() # robots that have already been warned this event
+ self._dribbler_throttled: set = set() # robots currently in post-limit cooldown (hysteresis)
+
def get_robots_responses(self) -> List[RobotResponse]:
HEADER = 0xAA
FOOTER = 0x55
@@ -72,6 +101,7 @@ def get_robots_responses(self) -> List[RobotResponse]:
self._buffer.extend(self._serial_port.read(bytes_available))
responses = []
+ responded_ids = set()
while True:
# 2. Look for header
@@ -112,7 +142,14 @@ def get_robots_responses(self) -> List[RobotResponse]:
# Guard against IndexError just in case, though validated by 'length' check
if len(data) >= 1:
- responses.append(RobotResponse(robot_id, has_ball=(data[0] & 0x01) != 0))
+ # cmd IDs are returned as-is; StrategyRunner remaps them to vision IDs via _split_robot_responses_by_team
+ if robot_id in responded_ids:
+ warnings.warn(
+ f"Received multiple responses for robot ID {robot_id} in the same cycle. Ignoring subsequent responses."
+ )
+ else:
+ responses.append(RobotResponse(robot_id, has_ball=(data[0] & 0x01) != 0))
+ responded_ids.add(robot_id)
# 8. Clear parsed packet from buffer
del self._buffer[:packet_len]
@@ -163,6 +200,12 @@ def _add_robot_command(self, command: RobotCommand, robot_id: int) -> None:
robot_id (int): The ID of the robot.
command (RobotCommand): A named tuple containing the robot command with keys: 'local_forward_vel', 'local_left_vel', 'angular_vel', 'kick', 'chip', 'dribble'.
"""
+ if robot_id in self._vision_to_cmd_mapping:
+ robot_id = self._vision_to_cmd_mapping[robot_id]
+ elif self._sharing_friendly_transmitter:
+ warnings.warn(
+ f"Robot ID {robot_id} has no entry in vision_to_cmd_mapping on a shared transmitter setup; command will be sent using the raw vision ID."
+ )
if robot_id in self._assigned_mapping:
warnings.warn(
f"Robot ID {robot_id} has already been assigned a command in this cycle. Overwriting previous command."
@@ -211,10 +254,54 @@ def _add_robot_command(self, command: RobotCommand, robot_id: int) -> None:
# self._robots_info[i] = info
# data_in = data_in << 1 # shift to the next robot's data
+ def _update_dribbler_bucket(self, robot_id: int, requested: bool) -> bool:
+ """Leaky-bucket thermal limiter for the dribbler.
+
+ Returns True if the dribbler should actually be turned on this call.
+ The bucket accumulates real elapsed seconds while dribbling and drains
+ at the same rate while off, so intermittent use is accounted for
+ proportionally regardless of control-loop frequency.
+ """
+ now = time.monotonic()
+ dt = now - self._dribbler_last_tick.get(robot_id, now)
+ self._dribbler_last_tick[robot_id] = now
+
+ current = self._dribbler_seconds.get(robot_id, 0.0)
+ at_limit = current >= DRIBBLER_MAX_ON_SECONDS
+
+ if at_limit:
+ self._dribbler_throttled.add(robot_id)
+
+ throttled = robot_id in self._dribbler_throttled
+
+ if requested and not throttled:
+ self._dribbler_seconds[robot_id] = min(current + dt, DRIBBLER_MAX_ON_SECONDS)
+ self._dribbler_limit_warned.discard(robot_id)
+ return True
+ elif requested and throttled:
+ if robot_id not in self._dribbler_limit_warned:
+ warnings.warn(
+ f"Robot {robot_id}: dribbler thermal limit reached "
+ f"({DRIBBLER_MAX_ON_SECONDS:.0f}s). "
+ f"Forcing dribbler off until bucket drains to {DRIBBLER_RESUME_SECONDS:.0f}s."
+ )
+ self._dribbler_limit_warned.add(robot_id)
+ # still drain while forced off
+ self._dribbler_seconds[robot_id] = max(0.0, current - dt)
+ if self._dribbler_seconds[robot_id] <= DRIBBLER_RESUME_SECONDS:
+ self._dribbler_throttled.discard(robot_id)
+ self._dribbler_limit_warned.discard(robot_id)
+ return False
+ else:
+ # Not requested: drain the bucket
+ self._dribbler_seconds[robot_id] = max(0.0, current - dt)
+ if self._dribbler_seconds[robot_id] <= DRIBBLER_RESUME_SECONDS:
+ self._dribbler_throttled.discard(robot_id)
+ self._dribbler_limit_warned.discard(robot_id)
+ return False
+
def _generate_command_buffer(self, robot_id: int, c_command: RobotCommand) -> bytes:
"""Generates the command buffer to be sent to the robot."""
- assert robot_id < 6, "Invalid robot_id. Must be between 0 and 5."
-
# endianness: little endian
packet = bytearray(
[
@@ -228,8 +315,10 @@ def _generate_command_buffer(self, robot_id: int, c_command: RobotCommand) -> by
]
)
+ dribble_allowed = self._update_dribbler_bucket(robot_id, c_command.dribble)
+
dribbler_speed = 0
- if c_command.dribble:
+ if dribble_allowed:
dribbler_speed = 0xC000 # set bits 15:14 to 11
dribbler_speed |= 4095 & 0x3FFF # set bits 13:0 to 4095
@@ -372,6 +461,18 @@ def n_friendly(self) -> int:
def in_packet_size(self) -> int:
return self._in_packet_size
+ @property
+ def vision_to_cmd_mapping(self) -> Dict[int, int]:
+ return self._vision_to_cmd_mapping
+
+ @property
+ def cmd_to_vision_mapping(self) -> Dict[int, int]:
+ return self._cmd_to_vision_mapping
+
+ @property
+ def sharing_friendly_transmitter(self) -> bool:
+ return self._sharing_friendly_transmitter
+
if __name__ == "__main__":
robot_controller = RealRobotController(is_team_yellow=True, n_friendly=1)
diff --git a/utama_core/team_controller/src/debug_utils/telop_gui.py b/utama_core/team_controller/src/debug_utils/telop_gui.py
index 79ce2ab3..5da99738 100644
--- a/utama_core/team_controller/src/debug_utils/telop_gui.py
+++ b/utama_core/team_controller/src/debug_utils/telop_gui.py
@@ -53,6 +53,7 @@ class RealRobotController:
def __init__(self, is_team_yellow=True, n_friendly=2):
self.n_friendly = n_friendly
+ self._dribbler_seconds: dict = {}
print("\n[DUMMY CONTROLLER ENABLED]")
print("No serial/network/hardware required.\n")
@@ -89,6 +90,7 @@ def send_robot_commands(self):
KICK_C = "#d94a4a"
ON_C = "#4ad97a"
BALL_C = "#d9a84a"
+HEAT_C = "#d94a4a" # Red when dribbler near/at thermal limit
DUMMY_C = "#d9a84a" # Orange for dummy warning
@@ -287,6 +289,8 @@ def _build_ui(self):
self._fb_ball_vars: dict[int, tk.StringVar] = {}
self._fb_ball_lbls: dict[int, tk.Label] = {}
+ self._fb_heat_vars: dict[int, tk.StringVar] = {}
+ self._fb_heat_lbls: dict[int, tk.Label] = {}
self._fb_status_vars: dict[int, tk.StringVar] = {}
for i in range(N_FRIENDLY):
row = tk.Frame(fb_frame, bg=SURFACE)
@@ -315,6 +319,18 @@ def _build_ui(self):
)
ball_lbl.pack(side="left", padx=(8, 0))
+ heat_var = tk.StringVar(value="heat: 0%")
+ heat_lbl = tk.Label(
+ row,
+ textvariable=heat_var,
+ bg=SURFACE,
+ fg=MUTED,
+ font=("monospace", 11),
+ width=10,
+ anchor="w",
+ )
+ heat_lbl.pack(side="left", padx=(8, 0))
+
status_var = tk.StringVar(value="no data")
tk.Label(
row,
@@ -327,6 +343,8 @@ def _build_ui(self):
self._fb_ball_vars[i] = ball_var
self._fb_ball_lbls[i] = ball_lbl
+ self._fb_heat_vars[i] = heat_var
+ self._fb_heat_lbls[i] = heat_lbl
self._fb_status_vars[i] = status_var
# --- Command readout ---
@@ -485,6 +503,10 @@ def _control_loop(self):
time.sleep(max(0.0, dt - elapsed))
def _update_feedback(self, responses):
+ from utama_core.team_controller.src.controllers.real.real_robot_controller import (
+ DRIBBLER_MAX_ON_SECONDS,
+ )
+
for resp in responses:
if resp.id not in self._fb_ball_vars:
continue
@@ -496,6 +518,13 @@ def _update_feedback(self, responses):
self._fb_ball_lbls[resp.id].configure(fg=MUTED)
self._fb_status_vars[resp.id].set("connected")
+ # dribbler heat display
+ bucket = self.controller._dribbler_seconds.get(resp.id, 0.0)
+ pct = int(bucket / DRIBBLER_MAX_ON_SECONDS * 100)
+ self._fb_heat_vars[resp.id].set(f"heat: {pct:3d}%")
+ heat_colour = HEAT_C if pct >= 80 else (BALL_C if pct >= 50 else MUTED)
+ self._fb_heat_lbls[resp.id].configure(fg=heat_colour)
+
def _update_readout(self, cmd: RobotCommand):
self._metrics["fwd"].set(f"{cmd.local_forward_vel:+.2f}")
self._metrics["left"].set(f"{cmd.local_left_vel:+.2f}")
diff --git a/utama_core/tests/controller/test_dribbler_limiter.py b/utama_core/tests/controller/test_dribbler_limiter.py
new file mode 100644
index 00000000..2ed97bcd
--- /dev/null
+++ b/utama_core/tests/controller/test_dribbler_limiter.py
@@ -0,0 +1,177 @@
+"""Tests for the leaky-bucket dribbler thermal limiter in RealRobotController."""
+
+from unittest.mock import MagicMock, patch
+
+import pytest
+
+from utama_core.team_controller.src.controllers.real.real_robot_controller import (
+ DRIBBLER_MAX_ON_SECONDS,
+ DRIBBLER_RESUME_SECONDS,
+ RealRobotController,
+)
+
+_MONOTONIC = "utama_core.team_controller.src.controllers.real.real_robot_controller.time.monotonic"
+
+
+@pytest.fixture
+def controller():
+ """RealRobotController with a mock serial port (no real hardware needed)."""
+ mock_serial = MagicMock()
+ mock_serial.in_waiting = 0
+ return RealRobotController(is_team_yellow=True, n_friendly=3, serial_port=mock_serial)
+
+
+def _tick(controller, robot_id: int, requested: bool, dt: float) -> bool:
+ """Call _update_dribbler_bucket with a controlled time delta.
+
+ Seeds _dribbler_last_tick so that time.monotonic() - last_tick == dt exactly.
+ """
+ now = controller._dribbler_last_tick.get(robot_id, 0.0) + dt
+ controller._dribbler_last_tick[robot_id] = now - dt
+ with patch(_MONOTONIC, return_value=now):
+ return controller._update_dribbler_bucket(robot_id, requested)
+
+
+# ---------------------------------------------------------------------------
+# Basic bucket mechanics
+# ---------------------------------------------------------------------------
+
+
+def test_dribbler_allowed_when_bucket_empty(controller):
+ assert _tick(controller, 0, requested=True, dt=1.0) is True
+
+
+def test_bucket_fills_by_elapsed_seconds(controller):
+ _tick(controller, 0, requested=True, dt=5.0)
+ assert controller._dribbler_seconds[0] == pytest.approx(5.0)
+
+
+def test_bucket_drains_by_elapsed_seconds(controller):
+ controller._dribbler_seconds[0] = 20.0
+ _tick(controller, 0, requested=False, dt=7.0)
+ assert controller._dribbler_seconds[0] == pytest.approx(13.0)
+
+
+def test_bucket_floors_at_zero(controller):
+ controller._dribbler_seconds[0] = 2.0
+ _tick(controller, 0, requested=False, dt=10.0)
+ assert controller._dribbler_seconds[0] == pytest.approx(0.0)
+
+
+def test_dribbler_off_returns_false(controller):
+ assert _tick(controller, 0, requested=False, dt=1.0) is False
+
+
+def test_bucket_does_not_overfill(controller):
+ _tick(controller, 0, requested=True, dt=DRIBBLER_MAX_ON_SECONDS * 2)
+ assert controller._dribbler_seconds[0] == pytest.approx(DRIBBLER_MAX_ON_SECONDS)
+
+
+# ---------------------------------------------------------------------------
+# Limit enforcement
+# ---------------------------------------------------------------------------
+
+
+def test_dribbler_forced_off_at_limit(controller):
+ controller._dribbler_seconds[0] = DRIBBLER_MAX_ON_SECONDS
+ assert _tick(controller, 0, requested=True, dt=1.0) is False
+
+
+def test_dribbler_forced_off_emits_warning_once(controller):
+ controller._dribbler_seconds[0] = DRIBBLER_MAX_ON_SECONDS
+ # first tick at limit: warning
+ with pytest.warns(UserWarning, match="thermal limit"):
+ _tick(controller, 0, requested=True, dt=1.0)
+ # subsequent ticks: no more warnings
+ import warnings as _warnings
+
+ with _warnings.catch_warnings():
+ _warnings.simplefilter("error")
+ _tick(controller, 0, requested=True, dt=1.0) # must not raise
+
+
+# ---------------------------------------------------------------------------
+# Hysteresis: no 1s-on / 1s-off oscillation at the limit
+# ---------------------------------------------------------------------------
+
+
+def test_throttled_robot_stays_off_until_resume_threshold(controller):
+ # fill to limit
+ controller._dribbler_seconds[0] = DRIBBLER_MAX_ON_SECONDS
+ _tick(controller, 0, requested=True, dt=0.0) # trigger throttle flag
+
+ # drain to just above resume threshold — still blocked
+ controller._dribbler_seconds[0] = DRIBBLER_RESUME_SECONDS + 1.0
+ assert _tick(controller, 0, requested=True, dt=0.0) is False
+
+
+def test_throttled_robot_resumes_at_resume_threshold(controller):
+ controller._dribbler_seconds[0] = DRIBBLER_MAX_ON_SECONDS
+ _tick(controller, 0, requested=True, dt=0.0) # trigger throttle
+
+ # drain to exactly the resume threshold
+ _tick(controller, 0, requested=False, dt=DRIBBLER_MAX_ON_SECONDS - DRIBBLER_RESUME_SECONDS)
+ assert controller._dribbler_seconds[0] == pytest.approx(DRIBBLER_RESUME_SECONDS)
+ # now requesting dribble should be allowed
+ assert _tick(controller, 0, requested=True, dt=1.0) is True
+
+
+def test_bucket_drains_while_throttled(controller):
+ # robot should still drain even while strategy keeps requesting dribble
+ controller._dribbler_seconds[0] = DRIBBLER_MAX_ON_SECONDS
+ _tick(controller, 0, requested=True, dt=0.0) # throttle
+ _tick(controller, 0, requested=True, dt=5.0)
+ assert controller._dribbler_seconds[0] == pytest.approx(DRIBBLER_MAX_ON_SECONDS - 5.0)
+
+
+def test_no_oscillation_at_limit(controller):
+ # fill to limit, then alternate on/off — robot must stay off until resume threshold
+ controller._dribbler_seconds[0] = DRIBBLER_MAX_ON_SECONDS
+ results = []
+ for i in range(10):
+ requested = i % 2 == 0 # alternate on/off
+ results.append(_tick(controller, 0, requested=requested, dt=1.0))
+ # all must be False until bucket drains below DRIBBLER_RESUME_SECONDS
+ assert all(r is False for r in results)
+
+
+# ---------------------------------------------------------------------------
+# Recovery after full drain
+# ---------------------------------------------------------------------------
+
+
+def test_dribbler_fully_recovers_after_drain(controller):
+ controller._dribbler_seconds[0] = DRIBBLER_MAX_ON_SECONDS
+ _tick(controller, 0, requested=True, dt=0.0) # throttle
+ _tick(controller, 0, requested=False, dt=DRIBBLER_MAX_ON_SECONDS)
+ assert controller._dribbler_seconds[0] == pytest.approx(0.0)
+ assert _tick(controller, 0, requested=True, dt=1.0) is True
+
+
+def test_limit_hit_at_correct_wall_time(controller):
+ # 29s → allowed, fills to 29s
+ assert _tick(controller, 0, requested=True, dt=29.0) is True
+ assert controller._dribbler_seconds[0] == pytest.approx(29.0)
+ # 1s more fills to 30s (cap) → still allowed (not yet at limit at entry)
+ assert _tick(controller, 0, requested=True, dt=1.0) is True
+ assert controller._dribbler_seconds[0] == pytest.approx(DRIBBLER_MAX_ON_SECONDS)
+ # next tick: at limit → forced off
+ assert _tick(controller, 0, requested=True, dt=1.0) is False
+
+
+# ---------------------------------------------------------------------------
+# Per-robot isolation
+# ---------------------------------------------------------------------------
+
+
+def test_buckets_are_independent_per_robot(controller):
+ controller._dribbler_seconds[0] = DRIBBLER_MAX_ON_SECONDS
+ assert _tick(controller, 0, requested=True, dt=0.0) is False # throttle robot 0
+ assert _tick(controller, 1, requested=True, dt=1.0) is True
+
+
+def test_draining_one_robot_does_not_affect_another(controller):
+ controller._dribbler_seconds[0] = 20.0
+ controller._dribbler_seconds[1] = 10.0
+ _tick(controller, 0, requested=False, dt=5.0)
+ assert controller._dribbler_seconds[1] == pytest.approx(10.0)
diff --git a/utama_core/tests/custom_referee/test_custom_referee.py b/utama_core/tests/custom_referee/test_custom_referee.py
index 1fb17557..9819cdfd 100644
--- a/utama_core/tests/custom_referee/test_custom_referee.py
+++ b/utama_core/tests/custom_referee/test_custom_referee.py
@@ -1,581 +1,582 @@
-"""Unit tests for the CustomReferee system."""
-
-from __future__ import annotations
-
-import math
-from unittest.mock import MagicMock
-
-import pytest
-
-from utama_core.config.field_params import STANDARD_FIELD_DIMS
-from utama_core.custom_referee.custom_referee import CustomReferee
-from utama_core.custom_referee.geometry import RefereeGeometry
-from utama_core.custom_referee.profiles.profile_loader import load_profile
-from utama_core.custom_referee.rules.defense_area_rule import DefenseAreaRule
-from utama_core.custom_referee.rules.goal_rule import GoalRule
-from utama_core.custom_referee.rules.keep_out_rule import KeepOutRule
-from utama_core.custom_referee.rules.out_of_bounds_rule import OutOfBoundsRule
-from utama_core.custom_referee.state_machine import GameStateMachine
-from utama_core.entities.data.referee import RefereeData
-from utama_core.entities.data.vector import Vector2D, Vector3D
-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.entities.referee.referee_command import RefereeCommand
-from utama_core.entities.referee.stage import Stage
-
-# ---------------------------------------------------------------------------
-# Helpers
-# ---------------------------------------------------------------------------
-
-GEO = RefereeGeometry.from_field_dims(STANDARD_FIELD_DIMS)
-
-
-def _ball(x: float, y: float, z: float = 0.0) -> Ball:
- return Ball(p=Vector3D(x, y, z), v=Vector3D(0, 0, 0), a=Vector3D(0, 0, 0))
-
-
-def _robot(robot_id: int, x: float, y: float, is_friendly: bool, has_ball: bool = False) -> Robot:
- return Robot(
- id=robot_id,
- is_friendly=is_friendly,
- has_ball=has_ball,
- p=Vector2D(x, y),
- v=Vector2D(0, 0),
- a=Vector2D(0, 0),
- orientation=0.0,
- )
-
-
-def _frame(
- ball: Ball,
- friendly_robots: dict | None = None,
- enemy_robots: dict | None = None,
- my_team_is_yellow: bool = True,
- my_team_is_right: bool = False,
- ts: float = 10.0,
-) -> GameFrame:
- return GameFrame(
- ts=ts,
- my_team_is_yellow=my_team_is_yellow,
- my_team_is_right=my_team_is_right,
- friendly_robots=friendly_robots or {},
- enemy_robots=enemy_robots or {},
- ball=ball,
- referee=None,
- )
-
-
-def _state_machine() -> GameStateMachine:
- sm = GameStateMachine(
- half_duration_seconds=300.0,
- kickoff_team="yellow",
- n_robots_yellow=3,
- n_robots_blue=3,
- )
- sm.seed_clock(0.0)
- sm.set_command(RefereeCommand.NORMAL_START, 0.0)
- return sm
-
-
-# ---------------------------------------------------------------------------
-# GoalRule
-# ---------------------------------------------------------------------------
-
-
-class TestGoalRule:
- def test_right_goal_blue_scores_when_yellow_is_right(self):
- # Yellow defends right goal → ball in right goal → blue scored → yellow kicks off.
- rule = GoalRule(cooldown_seconds=1.0)
- frame = _frame(ball=_ball(5.0, 0.0), my_team_is_yellow=True, my_team_is_right=True)
- violation = rule.check(frame, GEO, RefereeCommand.NORMAL_START)
- assert violation is not None
- assert violation.rule_name == "goal"
- assert violation.status_message == "Goal by Blue"
- assert violation.next_command == RefereeCommand.PREPARE_KICKOFF_YELLOW
-
- def test_left_goal_yellow_scores_when_yellow_is_right(self):
- # Blue defends left goal → ball in left goal → yellow scored → blue kicks off.
- rule = GoalRule(cooldown_seconds=1.0)
- frame = _frame(ball=_ball(-5.0, 0.0), my_team_is_yellow=True, my_team_is_right=True)
- violation = rule.check(frame, GEO, RefereeCommand.NORMAL_START)
- assert violation is not None
- assert violation.status_message == "Goal by Yellow"
- assert violation.next_command == RefereeCommand.PREPARE_KICKOFF_BLUE
-
- def test_right_goal_yellow_scores_when_yellow_is_left(self):
- # Blue defends right goal → ball in right goal → yellow scored → blue kicks off.
- rule = GoalRule(cooldown_seconds=1.0)
- frame = _frame(ball=_ball(5.0, 0.0), my_team_is_yellow=True, my_team_is_right=False)
- violation = rule.check(frame, GEO, RefereeCommand.NORMAL_START)
- assert violation is not None
- assert violation.status_message == "Goal by Yellow"
- assert violation.next_command == RefereeCommand.PREPARE_KICKOFF_BLUE
-
- def test_no_goal_wide_shot(self):
- rule = GoalRule()
- frame = _frame(ball=_ball(5.0, 1.0)) # y=1.0 > half_goal_width=0.5
- assert rule.check(frame, GEO, RefereeCommand.NORMAL_START) is None
-
- def test_goal_respects_cooldown(self):
- rule = GoalRule(cooldown_seconds=2.0)
- frame1 = _frame(ball=_ball(5.0, 0.0), my_team_is_right=True, ts=10.0)
- v1 = rule.check(frame1, GEO, RefereeCommand.NORMAL_START)
- assert v1 is not None
-
- # Second detection within cooldown window — must be suppressed.
- frame2 = _frame(ball=_ball(5.0, 0.0), my_team_is_right=True, ts=10.5)
- v2 = rule.check(frame2, GEO, RefereeCommand.NORMAL_START)
- assert v2 is None
-
- # After cooldown expires — should fire again.
- frame3 = _frame(ball=_ball(5.0, 0.0), my_team_is_right=True, ts=13.0)
- v3 = rule.check(frame3, GEO, RefereeCommand.NORMAL_START)
- assert v3 is not None
-
- def test_no_detection_during_stop(self):
- rule = GoalRule()
- frame = _frame(ball=_ball(5.0, 0.0))
- assert rule.check(frame, GEO, RefereeCommand.STOP) is None
-
-
-# ---------------------------------------------------------------------------
-# OutOfBoundsRule
-# ---------------------------------------------------------------------------
-
-
-class TestOutOfBoundsRule:
- def test_ball_out_top(self):
- rule = OutOfBoundsRule()
- frame = _frame(ball=_ball(0.0, 3.5), my_team_is_yellow=True)
- violation = rule.check(frame, GEO, RefereeCommand.NORMAL_START)
- assert violation is not None
- assert violation.rule_name == "out_of_bounds"
-
- def test_ball_out_right_side(self):
- rule = OutOfBoundsRule()
- frame = _frame(ball=_ball(5.0, 1.0)) # wide — not in goal (y=1.0 > 0.5)
- violation = rule.check(frame, GEO, RefereeCommand.NORMAL_START)
- assert violation is not None
-
- def test_ball_in_field_no_violation(self):
- rule = OutOfBoundsRule()
- frame = _frame(ball=_ball(0.0, 0.0))
- assert rule.check(frame, GEO, RefereeCommand.NORMAL_START) is None
-
- def test_ball_in_goal_no_out_of_bounds(self):
- rule = OutOfBoundsRule()
- frame = _frame(ball=_ball(5.0, 0.0)) # in right goal
- # GoalRule handles this; OutOfBoundsRule must not also fire.
- assert rule.check(frame, GEO, RefereeCommand.NORMAL_START) is None
-
- def test_free_kick_assigned_to_non_touching_team_friendly_yellow(self):
- """Friendly (yellow) last touched → enemy (blue) gets free kick."""
- rule = OutOfBoundsRule()
- friendly = {0: _robot(0, 4.4, 2.9, is_friendly=True, has_ball=True)}
- frame_before = _frame(ball=_ball(4.4, 2.9), friendly_robots=friendly, my_team_is_yellow=True, ts=9.9)
- rule.check(frame_before, GEO, RefereeCommand.NORMAL_START)
-
- frame_out = _frame(ball=_ball(0.0, 3.5), my_team_is_yellow=True, ts=10.0)
- violation = rule.check(frame_out, GEO, RefereeCommand.NORMAL_START)
- assert violation is not None
- assert violation.next_command == RefereeCommand.DIRECT_FREE_BLUE
-
- def test_designated_position_is_infield(self):
- rule = OutOfBoundsRule()
- frame = _frame(ball=_ball(0.0, 3.5))
- violation = rule.check(frame, GEO, RefereeCommand.NORMAL_START)
- assert violation is not None
- px, py = violation.designated_position
- assert abs(py) < GEO.half_width # placed infield
-
-
-# ---------------------------------------------------------------------------
-# DefenseAreaRule
-# ---------------------------------------------------------------------------
-
-
-class TestDefenseAreaRule:
- def _frame_with_attacker_in_defense(self, my_team_is_right: bool = False) -> GameFrame:
- # Enemy robot inside my (left) defense area.
- enemy = {0: _robot(0, -4.3, 0.5, is_friendly=False)}
- return _frame(
- ball=_ball(0, 0),
- enemy_robots=enemy,
- my_team_is_right=my_team_is_right,
- )
-
- def test_fires_during_normal_start(self):
- rule = DefenseAreaRule()
- frame = self._frame_with_attacker_in_defense(my_team_is_right=False)
- v = rule.check(frame, GEO, RefereeCommand.NORMAL_START)
- assert v is not None
- assert v.rule_name == "defense_area"
-
- def test_does_not_fire_during_stop(self):
- rule = DefenseAreaRule()
- frame = self._frame_with_attacker_in_defense()
- assert rule.check(frame, GEO, RefereeCommand.STOP) is None
-
- def test_does_not_fire_during_force_start_no_actually_fires(self):
- rule = DefenseAreaRule()
- frame = self._frame_with_attacker_in_defense(my_team_is_right=False)
- v = rule.check(frame, GEO, RefereeCommand.FORCE_START)
- assert v is not None
-
- def test_too_many_defenders(self):
- rule = DefenseAreaRule(max_defenders=1)
- # Two friendly robots in own (left) defense area.
- friendly = {
- 0: _robot(0, -4.3, 0.0, is_friendly=True),
- 1: _robot(1, -4.3, 0.5, is_friendly=True),
- }
- frame = _frame(ball=_ball(0, 0), friendly_robots=friendly, my_team_is_right=False, my_team_is_yellow=True)
- v = rule.check(frame, GEO, RefereeCommand.NORMAL_START)
- assert v is not None
- assert v.next_command == RefereeCommand.DIRECT_FREE_BLUE
-
-
-# ---------------------------------------------------------------------------
-# KeepOutRule
-# ---------------------------------------------------------------------------
-
-
-class TestKeepOutRule:
- def test_no_trigger_before_persistence_threshold(self):
- rule = KeepOutRule(radius_meters=0.5, violation_persistence_frames=5)
- friendly = {0: _robot(0, 0.2, 0.0, is_friendly=True)}
- # Enemy is kicking → check friendly.
- for _ in range(4):
- frame = _frame(ball=_ball(0.0, 0.0), friendly_robots=friendly)
- v = rule.check(frame, GEO, RefereeCommand.DIRECT_FREE_BLUE)
- assert v is None
-
- def test_triggers_after_persistence_threshold(self):
- rule = KeepOutRule(radius_meters=0.5, violation_persistence_frames=5)
- friendly = {0: _robot(0, 0.2, 0.0, is_friendly=True)}
- v = None
- for _ in range(5):
- frame = _frame(ball=_ball(0.0, 0.0), friendly_robots=friendly)
- v = rule.check(frame, GEO, RefereeCommand.DIRECT_FREE_BLUE)
- assert v is not None
- assert v.rule_name == "keep_out"
-
- def test_resets_on_non_violation_frame(self):
- rule = KeepOutRule(radius_meters=0.5, violation_persistence_frames=5)
- friendly = {0: _robot(0, 0.2, 0.0, is_friendly=True)}
- for _ in range(4):
- frame = _frame(ball=_ball(0.0, 0.0), friendly_robots=friendly)
- rule.check(frame, GEO, RefereeCommand.DIRECT_FREE_BLUE)
-
- # Robot moves away — count resets.
- far_frame = _frame(
- ball=_ball(0.0, 0.0),
- friendly_robots={0: _robot(0, 2.0, 0.0, is_friendly=True)},
- )
- rule.check(far_frame, GEO, RefereeCommand.DIRECT_FREE_BLUE)
-
- # Needs another full persistence run to trigger.
- v = None
- for _ in range(5):
- frame = _frame(ball=_ball(0.0, 0.0), friendly_robots=friendly)
- v = rule.check(frame, GEO, RefereeCommand.DIRECT_FREE_BLUE)
- assert v is not None
-
- def test_inactive_during_normal_start(self):
- rule = KeepOutRule(radius_meters=0.5, violation_persistence_frames=1)
- friendly = {0: _robot(0, 0.1, 0.0, is_friendly=True)}
- frame = _frame(ball=_ball(0.0, 0.0), friendly_robots=friendly)
- assert rule.check(frame, GEO, RefereeCommand.NORMAL_START) is None
-
-
-# ---------------------------------------------------------------------------
-# GameStateMachine
-# ---------------------------------------------------------------------------
-
-
-class TestGameStateMachine:
- def test_goal_increments_yellow_score(self):
- from utama_core.custom_referee.rules.base_rule import RuleViolation
-
- sm = _state_machine()
- violation = RuleViolation(
- rule_name="goal",
- suggested_command=RefereeCommand.STOP,
- next_command=RefereeCommand.PREPARE_KICKOFF_BLUE,
- status_message="Goal by Yellow",
- )
- data = sm.step(current_time=10.0, violation=violation)
- assert sm.yellow_team.score == 1
- assert sm.blue_team.score == 0
- assert data.referee_command == RefereeCommand.STOP
- assert data.next_command == RefereeCommand.PREPARE_KICKOFF_BLUE
-
- def test_goal_increments_blue_score(self):
- from utama_core.custom_referee.rules.base_rule import RuleViolation
-
- sm = _state_machine()
- violation = RuleViolation(
- rule_name="goal",
- suggested_command=RefereeCommand.STOP,
- next_command=RefereeCommand.PREPARE_KICKOFF_YELLOW,
- status_message="Goal by Blue",
- )
- sm.step(current_time=10.0, violation=violation)
- assert sm.blue_team.score == 1
- assert sm.yellow_team.score == 0
-
- def test_transition_cooldown_suppresses_duplicate(self):
- from utama_core.custom_referee.rules.base_rule import RuleViolation
-
- sm = _state_machine()
- violation = RuleViolation(
- rule_name="goal",
- suggested_command=RefereeCommand.STOP,
- next_command=RefereeCommand.PREPARE_KICKOFF_BLUE,
- status_message="Goal",
- )
- sm.step(current_time=10.0, violation=violation)
- assert sm.yellow_team.score == 1
-
- # Second goal within cooldown window — must be suppressed.
- sm.step(current_time=10.1, violation=violation)
- assert sm.yellow_team.score == 1 # still 1
-
- def test_goal_sets_designated_position_to_centre(self):
- from utama_core.custom_referee.rules.base_rule import RuleViolation
-
- sm = _state_machine()
- violation = RuleViolation(
- rule_name="goal",
- suggested_command=RefereeCommand.STOP,
- next_command=RefereeCommand.PREPARE_KICKOFF_BLUE,
- status_message="Goal by Yellow",
- )
- data = sm.step(current_time=10.0, violation=violation)
- assert data.designated_position == (0.0, 0.0)
-
- def test_goal_status_message_is_propagated_into_referee_data(self):
- from utama_core.custom_referee.rules.base_rule import RuleViolation
-
- sm = _state_machine()
- violation = RuleViolation(
- rule_name="goal",
- suggested_command=RefereeCommand.STOP,
- next_command=RefereeCommand.PREPARE_KICKOFF_BLUE,
- status_message="Goal by Yellow",
- )
- data = sm.step(current_time=10.0, violation=violation)
- assert data.status_message == "Goal by Yellow"
-
- def test_manual_command_clears_status_message(self):
- from utama_core.custom_referee.rules.base_rule import RuleViolation
-
- sm = _state_machine()
- violation = RuleViolation(
- rule_name="goal",
- suggested_command=RefereeCommand.STOP,
- next_command=RefereeCommand.PREPARE_KICKOFF_BLUE,
- status_message="Goal by Yellow",
- )
- sm.step(current_time=10.0, violation=violation)
-
- sm.set_command(RefereeCommand.NORMAL_START, timestamp=11.0)
- data = sm.step(current_time=11.0, violation=None)
- assert data.status_message is None
-
- def test_manual_set_command(self):
- sm = _state_machine()
- sm.set_command(RefereeCommand.NORMAL_START, timestamp=5.0)
- assert sm.command == RefereeCommand.NORMAL_START
-
- def test_manual_ball_placement_auto_advances_from_stop_and_then_to_normal_start(self):
- sm = _state_machine()
- sm.set_command(RefereeCommand.BALL_PLACEMENT_YELLOW, timestamp=1.0)
- sm.ball_placement_target = (0.0, 0.0)
-
- clear_frame = _frame(ball=_ball(2.0, 0.0), ts=10.0)
- data = sm.step(current_time=10.0, violation=None, game_frame=clear_frame)
+"""Unit tests for the CustomReferee system."""
+
+from __future__ import annotations
+
+import math
+from unittest.mock import MagicMock
+
+import pytest
+
+from utama_core.config.field_params import STANDARD_FIELD_DIMS
+from utama_core.custom_referee.custom_referee import CustomReferee
+from utama_core.custom_referee.geometry import RefereeGeometry
+from utama_core.custom_referee.profiles.profile_loader import load_profile
+from utama_core.custom_referee.rules.defense_area_rule import DefenseAreaRule
+from utama_core.custom_referee.rules.goal_rule import GoalRule
+from utama_core.custom_referee.rules.keep_out_rule import KeepOutRule
+from utama_core.custom_referee.rules.out_of_bounds_rule import OutOfBoundsRule
+from utama_core.custom_referee.state_machine import GameStateMachine
+from utama_core.entities.data.referee import RefereeData
+from utama_core.entities.data.vector import Vector2D, Vector3D
+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.entities.referee.referee_command import RefereeCommand
+from utama_core.entities.referee.stage import Stage
+
+# ---------------------------------------------------------------------------
+# Helpers
+# ---------------------------------------------------------------------------
+
+GEO = RefereeGeometry.from_field_dims(STANDARD_FIELD_DIMS)
+
+
+def _ball(x: float, y: float, z: float = 0.0) -> Ball:
+ return Ball(p=Vector3D(x, y, z), v=Vector3D(0, 0, 0), a=Vector3D(0, 0, 0))
+
+
+def _robot(robot_id: int, x: float, y: float, is_friendly: bool, has_ball: bool = False) -> Robot:
+ return Robot(
+ id=robot_id,
+ is_friendly=is_friendly,
+ has_ball=has_ball,
+ p=Vector2D(x, y),
+ v=Vector2D(0, 0),
+ a=Vector2D(0, 0),
+ orientation=0.0,
+ )
+
+
+def _frame(
+ ball: Ball,
+ friendly_robots: dict | None = None,
+ enemy_robots: dict | None = None,
+ my_team_is_yellow: bool = True,
+ my_team_is_right: bool = False,
+ ts: float = 10.0,
+) -> GameFrame:
+ return GameFrame(
+ ts=ts,
+ my_team_is_yellow=my_team_is_yellow,
+ my_team_is_right=my_team_is_right,
+ friendly_robots=friendly_robots or {},
+ enemy_robots=enemy_robots or {},
+ ball=ball,
+ referee=None,
+ )
+
+
+def _state_machine() -> GameStateMachine:
+ sm = GameStateMachine(
+ half_duration_seconds=300.0,
+ kickoff_team="yellow",
+ n_robots_yellow=3,
+ n_robots_blue=3,
+ )
+ sm.seed_clock(0.0)
+ sm.set_command(RefereeCommand.NORMAL_START, 0.0)
+ return sm
+
+
+# ---------------------------------------------------------------------------
+# GoalRule
+# ---------------------------------------------------------------------------
+
+
+class TestGoalRule:
+ def test_right_goal_blue_scores_when_yellow_is_right(self):
+ # Yellow defends right goal → ball in right goal → blue scored → yellow kicks off.
+ rule = GoalRule(cooldown_seconds=1.0)
+ frame = _frame(ball=_ball(5.0, 0.0), my_team_is_yellow=True, my_team_is_right=True)
+ violation = rule.check(frame, GEO, RefereeCommand.NORMAL_START)
+ assert violation is not None
+ assert violation.rule_name == "goal"
+ assert violation.status_message == "Goal by Blue"
+ assert violation.next_command == RefereeCommand.PREPARE_KICKOFF_YELLOW
+
+ def test_left_goal_yellow_scores_when_yellow_is_right(self):
+ # Blue defends left goal → ball in left goal → yellow scored → blue kicks off.
+ rule = GoalRule(cooldown_seconds=1.0)
+ frame = _frame(ball=_ball(-5.0, 0.0), my_team_is_yellow=True, my_team_is_right=True)
+ violation = rule.check(frame, GEO, RefereeCommand.NORMAL_START)
+ assert violation is not None
+ assert violation.status_message == "Goal by Yellow"
+ assert violation.next_command == RefereeCommand.PREPARE_KICKOFF_BLUE
+
+ def test_right_goal_yellow_scores_when_yellow_is_left(self):
+ # Blue defends right goal → ball in right goal → yellow scored → blue kicks off.
+ rule = GoalRule(cooldown_seconds=1.0)
+ frame = _frame(ball=_ball(5.0, 0.0), my_team_is_yellow=True, my_team_is_right=False)
+ violation = rule.check(frame, GEO, RefereeCommand.NORMAL_START)
+ assert violation is not None
+ assert violation.status_message == "Goal by Yellow"
+ assert violation.next_command == RefereeCommand.PREPARE_KICKOFF_BLUE
+
+ def test_no_goal_wide_shot(self):
+ rule = GoalRule()
+ frame = _frame(ball=_ball(5.0, 1.0)) # y=1.0 > half_goal_width=0.5
+ assert rule.check(frame, GEO, RefereeCommand.NORMAL_START) is None
+
+ def test_goal_respects_cooldown(self):
+ rule = GoalRule(cooldown_seconds=2.0)
+ frame1 = _frame(ball=_ball(5.0, 0.0), my_team_is_right=True, ts=10.0)
+ v1 = rule.check(frame1, GEO, RefereeCommand.NORMAL_START)
+ assert v1 is not None
+
+ # Second detection within cooldown window — must be suppressed.
+ frame2 = _frame(ball=_ball(5.0, 0.0), my_team_is_right=True, ts=10.5)
+ v2 = rule.check(frame2, GEO, RefereeCommand.NORMAL_START)
+ assert v2 is None
+
+ # After cooldown expires — should fire again.
+ frame3 = _frame(ball=_ball(5.0, 0.0), my_team_is_right=True, ts=13.0)
+ v3 = rule.check(frame3, GEO, RefereeCommand.NORMAL_START)
+ assert v3 is not None
+
+ def test_no_detection_during_stop(self):
+ rule = GoalRule()
+ frame = _frame(ball=_ball(5.0, 0.0))
+ assert rule.check(frame, GEO, RefereeCommand.STOP) is None
+
+
+# ---------------------------------------------------------------------------
+# OutOfBoundsRule
+# ---------------------------------------------------------------------------
+
+
+class TestOutOfBoundsRule:
+ def test_ball_out_top(self):
+ rule = OutOfBoundsRule()
+ frame = _frame(ball=_ball(0.0, 3.5), my_team_is_yellow=True)
+ violation = rule.check(frame, GEO, RefereeCommand.NORMAL_START)
+ assert violation is not None
+ assert violation.rule_name == "out_of_bounds"
+
+ def test_ball_out_right_side(self):
+ rule = OutOfBoundsRule()
+ frame = _frame(ball=_ball(5.0, 1.0)) # wide — not in goal (y=1.0 > 0.5)
+ violation = rule.check(frame, GEO, RefereeCommand.NORMAL_START)
+ assert violation is not None
+
+ def test_ball_in_field_no_violation(self):
+ rule = OutOfBoundsRule()
+ frame = _frame(ball=_ball(0.0, 0.0))
+ assert rule.check(frame, GEO, RefereeCommand.NORMAL_START) is None
+
+ def test_ball_in_goal_no_out_of_bounds(self):
+ rule = OutOfBoundsRule()
+ frame = _frame(ball=_ball(5.0, 0.0)) # in right goal
+ # GoalRule handles this; OutOfBoundsRule must not also fire.
+ assert rule.check(frame, GEO, RefereeCommand.NORMAL_START) is None
+
+ def test_free_kick_assigned_to_non_touching_team_friendly_yellow(self):
+ """Friendly (yellow) last touched → enemy (blue) gets free kick."""
+ rule = OutOfBoundsRule()
+ friendly = {0: _robot(0, 4.4, 2.9, is_friendly=True, has_ball=True)}
+ frame_before = _frame(ball=_ball(4.4, 2.9), friendly_robots=friendly, my_team_is_yellow=True, ts=9.9)
+ rule.check(frame_before, GEO, RefereeCommand.NORMAL_START)
+
+ frame_out = _frame(ball=_ball(0.0, 3.5), my_team_is_yellow=True, ts=10.0)
+ violation = rule.check(frame_out, GEO, RefereeCommand.NORMAL_START)
+ assert violation is not None
+ assert violation.next_command == RefereeCommand.DIRECT_FREE_BLUE
+
+ def test_designated_position_is_infield(self):
+ rule = OutOfBoundsRule()
+ frame = _frame(ball=_ball(0.0, 3.5))
+ violation = rule.check(frame, GEO, RefereeCommand.NORMAL_START)
+ assert violation is not None
+ px, py = violation.designated_position
+ assert abs(py) < GEO.half_width # placed infield
+
+
+# ---------------------------------------------------------------------------
+# DefenseAreaRule
+# ---------------------------------------------------------------------------
+
+
+class TestDefenseAreaRule:
+ def _frame_with_attacker_in_defense(self, my_team_is_right: bool = False) -> GameFrame:
+ # Enemy robot inside my (left) defense area.
+ enemy = {0: _robot(0, -4.3, 0.5, is_friendly=False)}
+ return _frame(
+ ball=_ball(0, 0),
+ enemy_robots=enemy,
+ my_team_is_right=my_team_is_right,
+ )
+
+ def test_fires_during_normal_start(self):
+ rule = DefenseAreaRule()
+ frame = self._frame_with_attacker_in_defense(my_team_is_right=False)
+ v = rule.check(frame, GEO, RefereeCommand.NORMAL_START)
+ assert v is not None
+ assert v.rule_name == "defense_area"
+
+ def test_does_not_fire_during_stop(self):
+ rule = DefenseAreaRule()
+ frame = self._frame_with_attacker_in_defense()
+ assert rule.check(frame, GEO, RefereeCommand.STOP) is None
+
+ def test_does_not_fire_during_force_start_no_actually_fires(self):
+ rule = DefenseAreaRule()
+ frame = self._frame_with_attacker_in_defense(my_team_is_right=False)
+ v = rule.check(frame, GEO, RefereeCommand.FORCE_START)
+ assert v is not None
+
+ def test_too_many_defenders(self):
+ rule = DefenseAreaRule(max_defenders=1)
+ # Two friendly robots in own (left) defense area.
+ friendly = {
+ 0: _robot(0, -4.3, 0.0, is_friendly=True),
+ 1: _robot(1, -4.3, 0.5, is_friendly=True),
+ }
+ frame = _frame(ball=_ball(0, 0), friendly_robots=friendly, my_team_is_right=False, my_team_is_yellow=True)
+ v = rule.check(frame, GEO, RefereeCommand.NORMAL_START)
+ assert v is not None
+ assert v.next_command == RefereeCommand.DIRECT_FREE_BLUE
+
+
+# ---------------------------------------------------------------------------
+# KeepOutRule
+# ---------------------------------------------------------------------------
+
+
+class TestKeepOutRule:
+ def test_no_trigger_before_persistence_threshold(self):
+ rule = KeepOutRule(radius_meters=0.5, violation_persistence_frames=5)
+ friendly = {0: _robot(0, 0.2, 0.0, is_friendly=True)}
+ # Enemy is kicking → check friendly.
+ for _ in range(4):
+ frame = _frame(ball=_ball(0.0, 0.0), friendly_robots=friendly)
+ v = rule.check(frame, GEO, RefereeCommand.DIRECT_FREE_BLUE)
+ assert v is None
+
+ def test_triggers_after_persistence_threshold(self):
+ rule = KeepOutRule(radius_meters=0.5, violation_persistence_frames=5)
+ friendly = {0: _robot(0, 0.2, 0.0, is_friendly=True)}
+ v = None
+ for _ in range(5):
+ frame = _frame(ball=_ball(0.0, 0.0), friendly_robots=friendly)
+ v = rule.check(frame, GEO, RefereeCommand.DIRECT_FREE_BLUE)
+ assert v is not None
+ assert v.rule_name == "keep_out"
+
+ def test_resets_on_non_violation_frame(self):
+ rule = KeepOutRule(radius_meters=0.5, violation_persistence_frames=5)
+ friendly = {0: _robot(0, 0.2, 0.0, is_friendly=True)}
+ for _ in range(4):
+ frame = _frame(ball=_ball(0.0, 0.0), friendly_robots=friendly)
+ rule.check(frame, GEO, RefereeCommand.DIRECT_FREE_BLUE)
+
+ # Robot moves away — count resets.
+ far_frame = _frame(
+ ball=_ball(0.0, 0.0),
+ friendly_robots={0: _robot(0, 2.0, 0.0, is_friendly=True)},
+ )
+ rule.check(far_frame, GEO, RefereeCommand.DIRECT_FREE_BLUE)
+
+ # Needs another full persistence run to trigger.
+ v = None
+ for _ in range(5):
+ frame = _frame(ball=_ball(0.0, 0.0), friendly_robots=friendly)
+ v = rule.check(frame, GEO, RefereeCommand.DIRECT_FREE_BLUE)
+ assert v is not None
+
+ def test_inactive_during_normal_start(self):
+ rule = KeepOutRule(radius_meters=0.5, violation_persistence_frames=1)
+ friendly = {0: _robot(0, 0.1, 0.0, is_friendly=True)}
+ frame = _frame(ball=_ball(0.0, 0.0), friendly_robots=friendly)
+ assert rule.check(frame, GEO, RefereeCommand.NORMAL_START) is None
+
+
+# ---------------------------------------------------------------------------
+# GameStateMachine
+# ---------------------------------------------------------------------------
+
+
+class TestGameStateMachine:
+ def test_goal_increments_yellow_score(self):
+ from utama_core.custom_referee.rules.base_rule import RuleViolation
+
+ sm = _state_machine()
+ violation = RuleViolation(
+ rule_name="goal",
+ suggested_command=RefereeCommand.STOP,
+ next_command=RefereeCommand.PREPARE_KICKOFF_BLUE,
+ status_message="Goal by Yellow",
+ )
+ data = sm.step(current_time=10.0, violation=violation)
+ assert sm.yellow_team.score == 1
+ assert sm.blue_team.score == 0
+ assert data.referee_command == RefereeCommand.STOP
+ assert data.next_command == RefereeCommand.BALL_PLACEMENT_BLUE
+
+ def test_goal_increments_blue_score(self):
+ from utama_core.custom_referee.rules.base_rule import RuleViolation
+
+ sm = _state_machine()
+ violation = RuleViolation(
+ rule_name="goal",
+ suggested_command=RefereeCommand.STOP,
+ next_command=RefereeCommand.PREPARE_KICKOFF_YELLOW,
+ status_message="Goal by Blue",
+ )
+ sm.step(current_time=10.0, violation=violation)
+ assert sm.blue_team.score == 1
+ assert sm.yellow_team.score == 0
+
+ def test_transition_cooldown_suppresses_duplicate(self):
+ from utama_core.custom_referee.rules.base_rule import RuleViolation
+
+ sm = _state_machine()
+ violation = RuleViolation(
+ rule_name="goal",
+ suggested_command=RefereeCommand.STOP,
+ next_command=RefereeCommand.PREPARE_KICKOFF_BLUE,
+ status_message="Goal",
+ )
+ sm.step(current_time=10.0, violation=violation)
+ assert sm.yellow_team.score == 1
+
+ # Second goal within cooldown window — must be suppressed.
+ sm.step(current_time=10.1, violation=violation)
+ assert sm.yellow_team.score == 1 # still 1
+
+ def test_goal_sets_designated_position_to_centre(self):
+ from utama_core.custom_referee.rules.base_rule import RuleViolation
+
+ sm = _state_machine()
+ violation = RuleViolation(
+ rule_name="goal",
+ suggested_command=RefereeCommand.STOP,
+ next_command=RefereeCommand.PREPARE_KICKOFF_BLUE,
+ status_message="Goal by Yellow",
+ )
+ data = sm.step(current_time=10.0, violation=violation)
+ assert data.designated_position == (0.0, 0.0)
+
+ def test_goal_status_message_is_propagated_into_referee_data(self):
+ from utama_core.custom_referee.rules.base_rule import RuleViolation
+
+ sm = _state_machine()
+ violation = RuleViolation(
+ rule_name="goal",
+ suggested_command=RefereeCommand.STOP,
+ next_command=RefereeCommand.PREPARE_KICKOFF_BLUE,
+ status_message="Goal by Yellow",
+ )
+ data = sm.step(current_time=10.0, violation=violation)
+ assert data.status_message == "Goal by Yellow"
+
+ def test_manual_command_clears_status_message(self):
+ from utama_core.custom_referee.rules.base_rule import RuleViolation
+
+ sm = _state_machine()
+ violation = RuleViolation(
+ rule_name="goal",
+ suggested_command=RefereeCommand.STOP,
+ next_command=RefereeCommand.PREPARE_KICKOFF_BLUE,
+ status_message="Goal by Yellow",
+ )
+ sm.step(current_time=10.0, violation=violation)
+
+ sm.set_command(RefereeCommand.NORMAL_START, timestamp=11.0)
+ data = sm.step(current_time=11.0, violation=None)
+ assert data.status_message is None
+
+ def test_manual_set_command(self):
+ sm = _state_machine()
+ sm.set_command(RefereeCommand.NORMAL_START, timestamp=5.0)
+ assert sm.command == RefereeCommand.NORMAL_START
+
+ def test_manual_ball_placement_auto_advances_from_stop_and_then_to_normal_start(self):
+ sm = _state_machine()
+ sm.set_command(RefereeCommand.BALL_PLACEMENT_YELLOW, timestamp=1.0)
+ sm.ball_placement_target = (0.0, 0.0)
+
+ clear_frame = _frame(ball=_ball(2.0, 0.0), ts=10.0)
+ data = sm.step(current_time=10.0, violation=None, game_frame=clear_frame)
+ assert data.referee_command == RefereeCommand.BALL_PLACEMENT_YELLOW
+ assert data.next_command == RefereeCommand.NORMAL_START
+
+ placed_frame = _frame(ball=_ball(0.0, 0.0), ts=20.0)
+ sm.step(current_time=20.0, violation=None, game_frame=placed_frame)
+ data = sm.step(current_time=23.0, violation=None, game_frame=placed_frame)
+ assert data.referee_command == RefereeCommand.NORMAL_START
+ assert data.next_command is None
+
+ def test_manual_penalty_auto_advances_from_stop_and_then_to_normal_start(self):
+ sm = _state_machine()
+ sm.set_command(RefereeCommand.PREPARE_PENALTY_YELLOW, timestamp=1.0)
+
+ clear_frame = _frame(ball=_ball(0.0, 0.0), ts=10.0)
+ data = sm.step(current_time=10.0, violation=None, game_frame=clear_frame)
+ assert data.referee_command == RefereeCommand.PREPARE_PENALTY_YELLOW
+ assert data.next_command == RefereeCommand.NORMAL_START
+
+ ready_attackers = {0: _robot(0, 2.25, 0.0, is_friendly=True)}
+ ready_frame = _frame(ball=_ball(0.0, 0.0), friendly_robots=ready_attackers, ts=14.0)
+ sm.step(current_time=14.0, violation=None, game_frame=ready_frame)
+ data = sm.step(current_time=17.0, violation=None, game_frame=ready_frame)
+ assert data.referee_command == RefereeCommand.NORMAL_START
+ assert data.next_command is None
+
+
+# ---------------------------------------------------------------------------
+# CustomReferee integration
+# ---------------------------------------------------------------------------
+
+
+class TestCustomReferee:
+ def test_returns_valid_referee_data(self):
+ referee = CustomReferee.from_profile_name("simulation")
+ frame = _frame(ball=_ball(0.0, 0.0))
+ data = referee.step(frame, current_time=10.0)
+ assert isinstance(data, RefereeData)
+ assert data.source_identifier == "custom_referee"
+
+ def test_simulation_goal_auto_advances_to_ball_placement_and_scores(self):
+ referee = CustomReferee.from_profile_name("simulation")
+ referee.set_command(RefereeCommand.NORMAL_START, timestamp=0.0)
+
+ # Yellow is on the RIGHT — ball in right goal means yellow conceded, blue scored.
+ frame = _frame(ball=_ball(5.0, 0.0), my_team_is_yellow=True, my_team_is_right=True, ts=10.0)
+ data = referee.step(frame, current_time=10.0)
+
assert data.referee_command == RefereeCommand.BALL_PLACEMENT_YELLOW
- assert data.next_command == RefereeCommand.NORMAL_START
-
- placed_frame = _frame(ball=_ball(0.0, 0.0), ts=20.0)
- sm.step(current_time=20.0, violation=None, game_frame=placed_frame)
- data = sm.step(current_time=23.0, violation=None, game_frame=placed_frame)
- assert data.referee_command == RefereeCommand.NORMAL_START
- assert data.next_command is None
-
- def test_manual_penalty_auto_advances_from_stop_and_then_to_normal_start(self):
- sm = _state_machine()
- sm.set_command(RefereeCommand.PREPARE_PENALTY_YELLOW, timestamp=1.0)
-
- clear_frame = _frame(ball=_ball(0.0, 0.0), ts=10.0)
- data = sm.step(current_time=10.0, violation=None, game_frame=clear_frame)
- assert data.referee_command == RefereeCommand.PREPARE_PENALTY_YELLOW
- assert data.next_command == RefereeCommand.NORMAL_START
-
- ready_attackers = {0: _robot(0, 2.25, 0.0, is_friendly=True)}
- ready_frame = _frame(ball=_ball(0.0, 0.0), friendly_robots=ready_attackers, ts=14.0)
- sm.step(current_time=14.0, violation=None, game_frame=ready_frame)
- data = sm.step(current_time=17.0, violation=None, game_frame=ready_frame)
- assert data.referee_command == RefereeCommand.NORMAL_START
- assert data.next_command is None
-
-
-# ---------------------------------------------------------------------------
-# CustomReferee integration
-# ---------------------------------------------------------------------------
-
-
-class TestCustomReferee:
- def test_returns_valid_referee_data(self):
- referee = CustomReferee.from_profile_name("simulation")
- frame = _frame(ball=_ball(0.0, 0.0))
- data = referee.step(frame, current_time=10.0)
- assert isinstance(data, RefereeData)
- assert data.source_identifier == "custom_referee"
-
- def test_simulation_goal_auto_advances_to_prepare_kickoff_and_scores(self):
- referee = CustomReferee.from_profile_name("simulation")
- referee.set_command(RefereeCommand.NORMAL_START, timestamp=0.0)
-
- # Yellow is on the RIGHT — ball in right goal means yellow conceded, blue scored.
- frame = _frame(ball=_ball(5.0, 0.0), my_team_is_yellow=True, my_team_is_right=True, ts=10.0)
- data = referee.step(frame, current_time=10.0)
-
- assert data.referee_command == RefereeCommand.PREPARE_KICKOFF_YELLOW
assert data.blue_team.score == 1
assert data.yellow_team.score == 0
- assert data.next_command == RefereeCommand.NORMAL_START
-
- def test_human_profile_no_oob(self):
- """Human profile disables out-of-bounds — ball outside must not trigger."""
- referee = CustomReferee.from_profile_name("human")
- referee.set_command(RefereeCommand.NORMAL_START, timestamp=0.0)
- frame = _frame(ball=_ball(0.0, 4.0), ts=10.0) # ball outside field width
- data = referee.step(frame, current_time=10.0)
- assert data.referee_command == RefereeCommand.NORMAL_START
-
- def test_simulation_oob_exposes_status_message(self):
- referee = CustomReferee.from_profile_name("simulation")
- referee.set_command(RefereeCommand.NORMAL_START, timestamp=0.0)
- frame = _frame(ball=_ball(0.0, 4.0), ts=10.0)
- data = referee.step(frame, current_time=10.0)
- assert data.status_message == "Ball out of bounds"
-
- def test_human_stays_in_stop_after_goal_until_operator_advances(self):
- """Human mode keeps the game in STOP after a goal for operator control."""
- referee = CustomReferee.from_profile_name("human")
- referee.set_command(RefereeCommand.NORMAL_START, timestamp=0.0)
-
- # Score a goal (yellow on right, blue scores).
- goal_frame = _frame(ball=_ball(5.0, 0.0), my_team_is_yellow=True, my_team_is_right=True, ts=10.0)
- data = referee.step(goal_frame, current_time=10.0)
- assert data.referee_command == RefereeCommand.STOP
+ assert data.next_command == RefereeCommand.PREPARE_KICKOFF_YELLOW
+
+ def test_human_profile_no_oob(self):
+ """Human profile disables out-of-bounds — ball outside must not trigger."""
+ referee = CustomReferee.from_profile_name("human")
+ referee.set_command(RefereeCommand.NORMAL_START, timestamp=0.0)
+ frame = _frame(ball=_ball(0.0, 4.0), ts=10.0) # ball outside field width
+ data = referee.step(frame, current_time=10.0)
+ assert data.referee_command == RefereeCommand.NORMAL_START
+
+ def test_simulation_oob_exposes_status_message(self):
+ referee = CustomReferee.from_profile_name("simulation")
+ referee.set_command(RefereeCommand.NORMAL_START, timestamp=0.0)
+ frame = _frame(ball=_ball(0.0, 4.0), ts=10.0)
+ data = referee.step(frame, current_time=10.0)
+ assert data.status_message == "Ball out of bounds"
+
+ def test_human_stays_in_stop_after_goal_until_operator_advances(self):
+ """Human mode keeps the game in STOP after a goal for operator control."""
+ referee = CustomReferee.from_profile_name("human")
+ referee.set_command(RefereeCommand.NORMAL_START, timestamp=0.0)
+
+ # Score a goal (yellow on right, blue scores).
+ goal_frame = _frame(ball=_ball(5.0, 0.0), my_team_is_yellow=True, my_team_is_right=True, ts=10.0)
+ data = referee.step(goal_frame, current_time=10.0)
+ assert data.referee_command == RefereeCommand.STOP
+ assert data.next_command == RefereeCommand.BALL_PLACEMENT_YELLOW
+
+ # Still in STOP later — operator must choose the next command.
+ still_stop = referee.step(_frame(ball=_ball(0.0, 0.0), ts=70.0), current_time=70.0)
+ assert still_stop.referee_command == RefereeCommand.STOP
+
+ def test_simulation_stays_in_ball_placement_after_goal_until_ball_is_placed(self):
+ """Simulation mode auto-advances into BALL_PLACEMENT and waits for the ball at centre."""
+ referee = CustomReferee.from_profile_name("simulation")
+ referee.set_command(RefereeCommand.NORMAL_START, timestamp=0.0)
+
+ goal_frame = _frame(ball=_ball(5.0, 0.0), my_team_is_yellow=True, my_team_is_right=True, ts=10.0)
+ referee.step(goal_frame, current_time=10.0)
+
+ data = referee.step(_frame(ball=_ball(1.0, 0.0), ts=70.0), current_time=70.0)
+ assert data.referee_command == RefereeCommand.BALL_PLACEMENT_YELLOW
assert data.next_command == RefereeCommand.PREPARE_KICKOFF_YELLOW
- # Still in STOP later — operator must choose the next command.
- still_stop = referee.step(_frame(ball=_ball(0.0, 0.0), ts=70.0), current_time=70.0)
- assert still_stop.referee_command == RefereeCommand.STOP
-
- def test_simulation_stays_in_prepare_kickoff_after_goal_without_ready_kicker(self):
- """Simulation mode auto-advances into PREPARE_KICKOFF and waits there until ready."""
- referee = CustomReferee.from_profile_name("simulation")
- referee.set_command(RefereeCommand.NORMAL_START, timestamp=0.0)
-
- goal_frame = _frame(ball=_ball(5.0, 0.0), my_team_is_yellow=True, my_team_is_right=True, ts=10.0)
- referee.step(goal_frame, current_time=10.0)
-
- # With no kicker in the centre circle, the state remains in PREPARE_KICKOFF.
- data = referee.step(_frame(ball=_ball(0.0, 0.0), ts=70.0), current_time=70.0)
- assert data.referee_command == RefereeCommand.PREPARE_KICKOFF_YELLOW
-
- def test_simulation_oob_auto_advances_to_direct_free_and_then_normal_start(self):
- referee = CustomReferee.from_profile_name("simulation")
- referee.set_command(RefereeCommand.NORMAL_START, timestamp=0.0)
-
- touch_frame = _frame(
- ball=_ball(4.4, 2.9),
- friendly_robots={0: _robot(0, 4.4, 2.9, is_friendly=True, has_ball=True)},
- my_team_is_yellow=True,
- ts=9.9,
- )
- referee.step(touch_frame, current_time=9.9)
+ def test_simulation_oob_auto_advances_to_ball_placement_then_direct_free(self):
+ referee = CustomReferee.from_profile_name("simulation")
+ referee.set_command(RefereeCommand.NORMAL_START, timestamp=0.0)
+
+ touch_frame = _frame(
+ ball=_ball(4.4, 2.9),
+ friendly_robots={0: _robot(0, 4.4, 2.9, is_friendly=True, has_ball=True)},
+ my_team_is_yellow=True,
+ ts=9.9,
+ )
+ referee.step(touch_frame, current_time=9.9)
+
+ out_frame = _frame(ball=_ball(0.0, 3.5), my_team_is_yellow=True, ts=10.0)
+ data = referee.step(out_frame, current_time=10.0)
+ assert data.referee_command == RefereeCommand.BALL_PLACEMENT_BLUE
+ assert data.next_command == RefereeCommand.DIRECT_FREE_BLUE
- out_frame = _frame(ball=_ball(0.0, 3.5), my_team_is_yellow=True, ts=10.0)
- data = referee.step(out_frame, current_time=10.0)
+ ready_frame = _frame(
+ ball=_ball(0.0, 2.9),
+ friendly_robots={0: _robot(0, 1.0, 0.0, is_friendly=True)},
+ enemy_robots={0: _robot(0, 0.1, 0.0, is_friendly=False)},
+ my_team_is_yellow=True,
+ ts=20.0,
+ )
+ referee.step(ready_frame, current_time=20.0)
+ data = referee.step(ready_frame, current_time=23.0)
assert data.referee_command == RefereeCommand.DIRECT_FREE_BLUE
assert data.next_command == RefereeCommand.NORMAL_START
-
- ready_frame = _frame(
- ball=_ball(0.0, 0.0),
- friendly_robots={0: _robot(0, 1.0, 0.0, is_friendly=True)},
- enemy_robots={0: _robot(0, 0.1, 0.0, is_friendly=False)},
- my_team_is_yellow=True,
- ts=20.0,
- )
- referee.step(ready_frame, current_time=20.0)
- data = referee.step(ready_frame, current_time=23.0)
- assert data.referee_command == RefereeCommand.NORMAL_START
-
- def test_human_manual_direct_free_stays_in_stop_until_operator_advances(self):
- referee = CustomReferee.from_profile_name("human")
- referee.set_command(RefereeCommand.NORMAL_START, timestamp=0.0)
- referee.set_command(RefereeCommand.DIRECT_FREE_BLUE, timestamp=1.0)
-
- data = referee.step(_frame(ball=_ball(0.0, 0.0), my_team_is_yellow=True, ts=10.0), current_time=10.0)
- assert data.referee_command == RefereeCommand.STOP
- assert data.next_command == RefereeCommand.DIRECT_FREE_BLUE
-
- still_stop = referee.step(_frame(ball=_ball(0.0, 0.0), ts=40.0), current_time=40.0)
- assert still_stop.referee_command == RefereeCommand.STOP
-
- def test_human_manual_penalty_stays_in_stop_until_operator_advances(self):
- referee = CustomReferee.from_profile_name("human")
- referee.set_command(RefereeCommand.NORMAL_START, timestamp=0.0)
- referee.set_command(RefereeCommand.PREPARE_PENALTY_YELLOW, timestamp=1.0)
-
- data = referee.step(_frame(ball=_ball(0.0, 0.0), ts=10.0), current_time=10.0)
- assert data.referee_command == RefereeCommand.STOP
- assert data.next_command == RefereeCommand.PREPARE_PENALTY_YELLOW
-
-
-# ---------------------------------------------------------------------------
-# Profile loader
-# ---------------------------------------------------------------------------
-
-
-class TestProfileLoader:
- def test_simulation_loads(self):
- profile = load_profile("simulation")
- assert profile.profile_name == "simulation"
- assert profile.rules.goal_detection.enabled is True
- assert profile.rules.keep_out.radius_meters == 0.5
-
- def test_human_loads(self):
- profile = load_profile("human")
- assert profile.profile_name == "human"
- assert profile.rules.out_of_bounds.enabled is False
- assert profile.game.force_start_after_goal is False
- assert profile.game.auto_advance.stop_to_next_command is False
- assert profile.game.auto_advance.prepare_penalty_to_normal is False
-
- def test_legacy_stop_to_prepare_kickoff_key_is_still_loaded(self, tmp_path):
- profile_path = tmp_path / "legacy_profile.yaml"
- profile_path.write_text(
- """
-profile_name: "legacy"
-geometry: {}
-rules: {}
-game:
- auto_advance:
- stop_to_prepare_kickoff: false
-""".strip()
- )
-
- profile = load_profile(str(profile_path))
- assert profile.game.auto_advance.stop_to_next_command is False
-
- def test_unknown_profile_raises(self):
- with pytest.raises(FileNotFoundError):
- load_profile("nonexistent_profile")
+
+ def test_human_manual_direct_free_stays_in_stop_until_operator_advances(self):
+ referee = CustomReferee.from_profile_name("human")
+ referee.set_command(RefereeCommand.NORMAL_START, timestamp=0.0)
+ referee.set_command(RefereeCommand.DIRECT_FREE_BLUE, timestamp=1.0)
+
+ data = referee.step(_frame(ball=_ball(0.0, 0.0), my_team_is_yellow=True, ts=10.0), current_time=10.0)
+ assert data.referee_command == RefereeCommand.STOP
+ assert data.next_command == RefereeCommand.DIRECT_FREE_BLUE
+
+ still_stop = referee.step(_frame(ball=_ball(0.0, 0.0), ts=40.0), current_time=40.0)
+ assert still_stop.referee_command == RefereeCommand.STOP
+
+ def test_human_manual_penalty_stays_in_stop_until_operator_advances(self):
+ referee = CustomReferee.from_profile_name("human")
+ referee.set_command(RefereeCommand.NORMAL_START, timestamp=0.0)
+ referee.set_command(RefereeCommand.PREPARE_PENALTY_YELLOW, timestamp=1.0)
+
+ data = referee.step(_frame(ball=_ball(0.0, 0.0), ts=10.0), current_time=10.0)
+ assert data.referee_command == RefereeCommand.STOP
+ assert data.next_command == RefereeCommand.PREPARE_PENALTY_YELLOW
+
+
+# ---------------------------------------------------------------------------
+# Profile loader
+# ---------------------------------------------------------------------------
+
+
+class TestProfileLoader:
+ def test_simulation_loads(self):
+ profile = load_profile("simulation")
+ assert profile.profile_name == "simulation"
+ assert profile.rules.goal_detection.enabled is True
+ assert profile.rules.keep_out.radius_meters == 0.5
+
+ def test_human_loads(self):
+ profile = load_profile("human")
+ assert profile.profile_name == "human"
+ assert profile.rules.out_of_bounds.enabled is False
+ assert profile.game.force_start_after_goal is False
+ assert profile.game.auto_advance.stop_to_next_command is False
+ assert profile.game.auto_advance.prepare_penalty_to_normal is False
+
+ def test_legacy_stop_to_prepare_kickoff_key_is_still_loaded(self, tmp_path):
+ profile_path = tmp_path / "legacy_profile.yaml"
+ profile_path.write_text(
+ """
+profile_name: "legacy"
+geometry: {}
+rules: {}
+game:
+ auto_advance:
+ stop_to_prepare_kickoff: false
+""".strip()
+ )
+
+ profile = load_profile(str(profile_path))
+ assert profile.game.auto_advance.stop_to_next_command is False
+
+ def test_unknown_profile_raises(self):
+ with pytest.raises(FileNotFoundError):
+ load_profile("nonexistent_profile")
diff --git a/utama_core/tests/referee/test_referee_unit.py b/utama_core/tests/referee/test_referee_unit.py
index ce99eb69..dc07184c 100644
--- a/utama_core/tests/referee/test_referee_unit.py
+++ b/utama_core/tests/referee/test_referee_unit.py
@@ -450,16 +450,27 @@ def fake_move(game, motion_controller, robot_id, target_coords, target_oren, dri
assert cmd_map[1] is not None
def test_robot_with_ball_moves_to_designated_position(self, monkeypatch):
+ import math
+
from utama_core.strategy.referee import actions as referee_actions
- captured = []
+ move_captured = []
+ turn_captured = []
def fake_move(game, motion_controller, robot_id, target_coords, target_oren, dribbling=False):
- captured.append((robot_id, target_coords, dribbling))
+ move_captured.append((robot_id, target_coords, dribbling))
return ("move", robot_id)
+ def fake_turn_on_spot(game, motion_controller, robot_id, target_oren, dribbling=False):
+ turn_captured.append((robot_id, dribbling))
+ return ("turn", robot_id)
+
monkeypatch.setattr(referee_actions, "move", fake_move)
+ monkeypatch.setattr(referee_actions, "turn_on_spot", fake_turn_on_spot)
+ target = (1.5, -0.5)
+ # Orient the robot to face the target so face_error < threshold → move branch.
+ facing = math.atan2(target[1], target[0])
robots = {
0: Robot(
id=0,
@@ -468,13 +479,13 @@ def fake_move(game, motion_controller, robot_id, target_coords, target_oren, dri
p=Vector2D(0.0, 0.0),
v=Vector2D(0.0, 0.0),
a=Vector2D(0.0, 0.0),
- orientation=0.0,
+ orientation=facing,
)
}
referee = _make_referee_data(
command=RefereeCommand.BALL_PLACEMENT_YELLOW,
)
- referee.designated_position = (1.5, -0.5)
+ referee.designated_position = target
frame = GameFrame(
ts=0.0,
my_team_is_yellow=True,
@@ -501,9 +512,11 @@ def fake_move(game, motion_controller, robot_id, target_coords, target_oren, dri
status = node.update()
assert status == py_trees.common.Status.RUNNING
- assert captured[0][0] == 0
- assert captured[0][1] == Vector2D(1.5, -0.5)
- assert captured[0][2] is True
+ # Robot already faces target → should call move (not turn_on_spot).
+ assert len(move_captured) >= 1
+ assert move_captured[0][0] == 0
+ assert move_captured[0][1] == Vector2D(1.5, -0.5)
+ assert move_captured[0][2] is True
def test_non_placing_teammate_clears_from_ball(self, monkeypatch):
from utama_core.strategy.referee import actions as referee_actions
@@ -1253,8 +1266,12 @@ def fake_move(game, motion_controller, robot_id, target_coords, target_oren, dri
kicker_entries = [entry for entry in captured if entry[0] == 1]
assert len(kicker_entries) == 1
- assert kicker_entries[0][1].x == pytest.approx(1.0)
- assert kicker_entries[0][1].y == pytest.approx(0.0)
+ # Target is the approach point behind the ball, not the ball itself —
+ # verify it is closer to the ball than robot 1's starting position.
+ ball_pos = Vector2D(1.0, 0.0)
+ robot1_start = Vector2D(1.2, 0.0)
+ target = kicker_entries[0][1]
+ assert target.distance_to(ball_pos) < robot1_start.distance_to(ball_pos)
def test_kicker_moves_toward_ball(self, monkeypatch):
from utama_core.strategy.referee import actions as referee_actions
@@ -1294,8 +1311,12 @@ def fake_move(game, motion_controller, robot_id, target_coords, target_oren, dri
assert len(captured) == 1
assert captured[0][0] == 0
- assert captured[0][1].x == pytest.approx(2.0)
- assert captured[0][1].y == pytest.approx(1.0)
+ # Target is the approach point behind the ball, not the ball itself —
+ # verify it is closer to the ball than the robot's starting position.
+ ball_pos = Vector2D(2.0, 1.0)
+ robot_start = Vector2D(0.5, 0.0)
+ target = captured[0][1]
+ assert target.distance_to(ball_pos) < robot_start.distance_to(ball_pos)
def test_non_kicker_robots_get_stop_command(self, monkeypatch):
from utama_core.strategy.referee import actions as referee_actions
diff --git a/utama_core/tests/refiners/robot_info_test.py b/utama_core/tests/refiners/robot_info_test.py
new file mode 100644
index 00000000..27271bec
--- /dev/null
+++ b/utama_core/tests/refiners/robot_info_test.py
@@ -0,0 +1,170 @@
+import pytest
+
+from utama_core.data_processing.refiners.robot_info import (
+ _BALL_CAPTURE_DIST,
+ RobotInfoRefiner,
+)
+from utama_core.entities.data.command import RobotResponse
+from utama_core.entities.data.vector import Vector2D, Vector3D
+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
+
+
+def _make_robot(robot_id: int, x: float, y: float, has_ball: bool = False) -> Robot:
+ return Robot(
+ id=robot_id,
+ is_friendly=True,
+ has_ball=has_ball,
+ p=Vector2D(x, y),
+ v=None,
+ a=None,
+ orientation=0,
+ )
+
+
+def _make_frame(robots: dict, ball_pos=None) -> GameFrame:
+ ball = Ball(Vector3D(*ball_pos, 0), Vector3D(0, 0, 0), None) if ball_pos else None
+ return GameFrame(
+ ts=0.0,
+ my_team_is_yellow=True,
+ my_team_is_right=True,
+ friendly_robots=robots,
+ enemy_robots={},
+ ball=ball,
+ )
+
+
+# ---------------------------------------------------------------------------
+# Default behaviour (trusted_ir_robots=None — trust all)
+# ---------------------------------------------------------------------------
+
+
+def test_default_uses_ir_sensor_true():
+ refiner = RobotInfoRefiner()
+ frame = _make_frame({0: _make_robot(0, 0.0, 0.0, has_ball=False)}, ball_pos=(1.0, 0.0))
+ result = refiner.refine(frame, [RobotResponse(id=0, has_ball=True)])
+ assert result.friendly_robots[0].has_ball is True
+
+
+def test_default_uses_ir_sensor_false():
+ refiner = RobotInfoRefiner()
+ # Robot is right on top of the ball but IR says False — should respect IR.
+ frame = _make_frame({0: _make_robot(0, 0.0, 0.0, has_ball=True)}, ball_pos=(0.0, 0.0))
+ result = refiner.refine(frame, [RobotResponse(id=0, has_ball=False)])
+ assert result.friendly_robots[0].has_ball is False
+
+
+def test_empty_responses_returns_frame_unchanged():
+ refiner = RobotInfoRefiner()
+ frame = _make_frame({0: _make_robot(0, 0.0, 0.0, has_ball=True)})
+ assert refiner.refine(frame, []) is frame
+ assert refiner.refine(frame, None) is frame
+
+
+def test_unknown_robot_id_warns(recwarn):
+ refiner = RobotInfoRefiner()
+ frame = _make_frame({0: _make_robot(0, 0.0, 0.0)})
+ refiner.refine(frame, [RobotResponse(id=99, has_ball=True)])
+ assert any("99" in str(w.message) for w in recwarn.list)
+
+
+# ---------------------------------------------------------------------------
+# Trusted-IR allowlist: trusted robot uses IR, untrusted uses vision proximity
+# ---------------------------------------------------------------------------
+
+
+def test_trusted_robot_uses_ir_even_when_far_from_ball():
+ refiner = RobotInfoRefiner(trusted_ir_robots=frozenset({0}))
+ # Robot is 2 m from ball — no proximity, but IR says True.
+ frame = _make_frame({0: _make_robot(0, 0.0, 0.0)}, ball_pos=(2.0, 0.0))
+ result = refiner.refine(frame, [RobotResponse(id=0, has_ball=True)])
+ assert result.friendly_robots[0].has_ball is True
+
+
+def test_trusted_robot_respects_ir_false():
+ refiner = RobotInfoRefiner(trusted_ir_robots=frozenset({0}))
+ # Robot is touching the ball but IR says False — should respect IR.
+ frame = _make_frame({0: _make_robot(0, 0.0, 0.0)}, ball_pos=(0.0, 0.0))
+ result = refiner.refine(frame, [RobotResponse(id=0, has_ball=False)])
+ assert result.friendly_robots[0].has_ball is False
+
+
+def test_untrusted_robot_infers_true_when_close():
+ refiner = RobotInfoRefiner(trusted_ir_robots=frozenset()) # no robots trusted
+ # Place robot within capture distance; IR reports False (broken sensor).
+ frame = _make_frame({0: _make_robot(0, 0.0, 0.0)}, ball_pos=(0.05, 0.0))
+ result = refiner.refine(frame, [RobotResponse(id=0, has_ball=False)])
+ assert result.friendly_robots[0].has_ball is True
+
+
+def test_untrusted_robot_infers_false_when_far():
+ refiner = RobotInfoRefiner(trusted_ir_robots=frozenset())
+ # Robot is far from ball; IR reports True (broken sensor firing randomly).
+ frame = _make_frame({0: _make_robot(0, 0.0, 0.0)}, ball_pos=(1.0, 0.0))
+ result = refiner.refine(frame, [RobotResponse(id=0, has_ball=True)])
+ assert result.friendly_robots[0].has_ball is False
+
+
+def test_untrusted_robot_at_exact_capture_boundary():
+ refiner = RobotInfoRefiner(trusted_ir_robots=frozenset())
+ just_inside = _BALL_CAPTURE_DIST - 0.001
+ just_outside = _BALL_CAPTURE_DIST + 0.001
+
+ frame_in = _make_frame({0: _make_robot(0, 0.0, 0.0)}, ball_pos=(just_inside, 0.0))
+ assert refiner.refine(frame_in, [RobotResponse(id=0, has_ball=False)]).friendly_robots[0].has_ball is True
+
+ frame_out = _make_frame({0: _make_robot(0, 0.0, 0.0)}, ball_pos=(just_outside, 0.0))
+ assert refiner.refine(frame_out, [RobotResponse(id=0, has_ball=False)]).friendly_robots[0].has_ball is False
+
+
+def test_untrusted_robot_no_ball_in_frame_returns_false():
+ refiner = RobotInfoRefiner(trusted_ir_robots=frozenset())
+ frame = _make_frame({0: _make_robot(0, 0.0, 0.0)}, ball_pos=None)
+ result = refiner.refine(frame, [RobotResponse(id=0, has_ball=True)])
+ assert result.friendly_robots[0].has_ball is False
+
+
+# ---------------------------------------------------------------------------
+# Mixed team: one robot trusted, one not
+# ---------------------------------------------------------------------------
+
+
+def test_mixed_team_trusted_uses_ir_untrusted_uses_vision():
+ refiner = RobotInfoRefiner(trusted_ir_robots=frozenset({0}))
+ robots = {
+ 0: _make_robot(0, 0.0, 0.0), # trusted — far from ball
+ 1: _make_robot(1, 0.05, 0.0), # untrusted — close to ball
+ }
+ frame = _make_frame(robots, ball_pos=(0.0, 0.0))
+ responses = [
+ RobotResponse(id=0, has_ball=True), # IR says yes → trust it even though far
+ RobotResponse(id=1, has_ball=False), # IR says no → ignore, use proximity
+ ]
+ result = refiner.refine(frame, responses)
+ assert result.friendly_robots[0].has_ball is True # from IR
+ assert result.friendly_robots[1].has_ball is True # from vision (close enough)
+
+
+def test_untrusted_robot_missing_from_responses_uses_vision():
+ """Untrusted robot absent from responses this tick must not keep stale has_ball."""
+ refiner = RobotInfoRefiner(trusted_ir_robots=frozenset({0}))
+ robots = {
+ 0: _make_robot(0, 2.0, 0.0, has_ball=False), # trusted
+ 1: _make_robot(1, 0.05, 0.0, has_ball=True), # untrusted, stale True
+ }
+ frame = _make_frame(robots, ball_pos=(0.0, 0.0))
+ # Robot 1 did not respond this tick (non-blocking poll returned nothing for it).
+ responses = [RobotResponse(id=0, has_ball=False)]
+ result = refiner.refine(frame, responses)
+ # Robot 1 is close to ball → vision inference gives True (not stale, but correct)
+ assert result.friendly_robots[1].has_ball is True
+
+ # Now robot 1 is far from ball; stale value is True but inference should correct it.
+ robots_far = {
+ 0: _make_robot(0, 2.0, 0.0, has_ball=False),
+ 1: _make_robot(1, 1.0, 0.0, has_ball=True), # stale True, but now far
+ }
+ frame_far = _make_frame(robots_far, ball_pos=(0.0, 0.0))
+ result_far = refiner.refine(frame_far, [RobotResponse(id=0, has_ball=False)])
+ assert result_far.friendly_robots[1].has_ball is False # stale cleared by vision
diff --git a/utama_core/tests/strategy_runner/test_ball_placement_rsim.py b/utama_core/tests/strategy_runner/test_ball_placement_rsim.py
new file mode 100644
index 00000000..7274ea3a
--- /dev/null
+++ b/utama_core/tests/strategy_runner/test_ball_placement_rsim.py
@@ -0,0 +1,451 @@
+"""Integration tests for the ball placement feature.
+
+These tests verify that ``BallPlacementStrategy`` (and the underlying
+``BallPlacementStep`` behaviour) satisfies the core requirements of automatic
+ball placement:
+
+ 1. **Approach** — after BALL_PLACEMENT_YELLOW is issued, the placer robot
+ genuinely closes distance to the ball (robot starts far away so the test
+ cannot pass before the strategy acts).
+ 2. **Carry** — once the robot captures the ball (``robot.has_ball`` or
+ proximity), it moves toward ``designated_position`` with the dribbler on.
+ 3. **Clearance** — non-placer robots stay outside ``BALL_KEEP_OUT_DISTANCE``
+ throughout the placement phase. The placer ID is fixed to what was set in
+ reset_field so the check is independent of the implementation's own
+ selection logic.
+ 4. **Placer selection** — when robot 1 is closer to the ball than robot 0,
+ robot 1 is selected as the placer and robot 0 stays away.
+
+Setup
+-----
+- Exhibition Road field (``GREAT_EXHIBITION_FIELD_DIMS``, 4 m × 3 m).
+- 2v2 — two yellow robots (our team) controlled by ``BallPlacementStrategy``;
+ no enemy robots (they would interfere with deterministic positioning).
+- CustomReferee with the "simulation" profile so auto-advance fires and the
+ full BALL_PLACEMENT → DIRECT_FREE → NORMAL_START cycle can complete.
+
+Running
+-------
+ pixi run pytest utama_core/tests/strategy_runner/test_ball_placement_rsim.py -v
+
+Known gap
+---------
+The carry/dribbler phase transition (approach → has_ball → carry) is not tested
+end-to-end because the PID motion controller decelerates before physically
+touching the dribbler geometry, so ``robot.infrared`` (and therefore
+``robot.has_ball``) may not fire reliably. This requires motion controller
+tuning (slower final approach, behind-ball offset) before it can be tested.
+Test 2 currently covers carry progress as a proxy but does not assert dribbler
+state directly.
+
+All four tests are independent: they each start a fresh rsim episode, teleport
+robots and ball to known positions, issue BALL_PLACEMENT_YELLOW directly, and
+then observe the strategy's response over time.
+"""
+
+from __future__ import annotations
+
+import math
+from typing import Optional
+
+import py_trees
+
+from utama_core.config.field_params import GREAT_EXHIBITION_FIELD_DIMS
+from utama_core.config.referee_constants import BALL_KEEP_OUT_DISTANCE
+from utama_core.custom_referee import CustomReferee
+from utama_core.entities.game import Game
+from utama_core.entities.game.field import FieldBounds
+from utama_core.entities.referee.referee_command import RefereeCommand
+from utama_core.run.strategy_runner import StrategyRunner
+from utama_core.strategy.examples.ball_placement_strategy import BallPlacementStrategy
+from utama_core.team_controller.src.controllers import AbstractSimController
+from utama_core.tests.common.abstract_test_manager import (
+ AbstractTestManager,
+ TestingStatus,
+)
+
+# ---------------------------------------------------------------------------
+# Constants
+# ---------------------------------------------------------------------------
+
+# Designated placement target used across all tests (inside exhibition field)
+_TARGET = (0.8, 0.5)
+
+# Tolerance within which a robot is considered to have "approached" the ball
+_APPROACH_TOLERANCE = 0.5 # metres
+
+# Tolerance within which the placer is considered to be "heading to target"
+_TARGET_TOLERANCE = 0.6 # metres
+
+# Clearance margin: robots must be at least this far from the ball
+_CLEAR_MARGIN = 0.05 # metres — allowed slack inside keep-out boundary
+
+
+# ---------------------------------------------------------------------------
+# Shared runner factory
+# ---------------------------------------------------------------------------
+
+
+def _make_runner(referee: CustomReferee) -> StrategyRunner:
+ """Build a 2v2 StrategyRunner on the Exhibition Road field."""
+ return StrategyRunner(
+ strategy=BallPlacementStrategy(),
+ my_team_is_yellow=True,
+ my_team_is_right=True,
+ mode="rsim",
+ exp_friendly=2,
+ exp_enemy=0,
+ exp_ball=True,
+ full_field_dims=GREAT_EXHIBITION_FIELD_DIMS,
+ referee=referee,
+ )
+
+
+# ---------------------------------------------------------------------------
+# Test 1: placer robot drives toward the ball
+# ---------------------------------------------------------------------------
+
+
+class _ApproachBallManager(AbstractTestManager):
+ """Verify that after BALL_PLACEMENT_YELLOW the placer genuinely closes on the ball.
+
+ Robot 0 starts 1.5 m from the ball — well outside _APPROACH_TOLERANCE — so
+ the test cannot pass before the strategy acts. We record the initial distance
+ and require it to decrease by at least _APPROACH_PROGRESS metres, proving the
+ robot is actively moving toward the ball rather than already being close.
+ """
+
+ n_episodes = 1
+ # Robot 0 starts this far from the ball; must be > _APPROACH_TOLERANCE so the
+ # test cannot pass before the strategy has done anything.
+ _INITIAL_DIST = 1.5 # metres
+ # How much closer robot 0 must get before the test passes.
+ _APPROACH_PROGRESS = 0.8 # metres
+
+ def __init__(self, referee: CustomReferee) -> None:
+ super().__init__()
+ self._referee = referee
+ self.command_seen: bool = False
+ self.robot_approached_ball: bool = False
+ self._initial_dist: Optional[float] = None
+
+ def reset_field(self, sim_controller: AbstractSimController, game: Game) -> None:
+ # Ball at centre-left; robot 0 starts 1.5 m away along x so it must
+ # travel a meaningful distance before the test can pass.
+ sim_controller.teleport_ball(-0.5, 0.0)
+ sim_controller.teleport_robot(game.my_team_is_yellow, 0, 1.0, 0.0) # 1.5 m from ball
+ sim_controller.teleport_robot(game.my_team_is_yellow, 1, 0.8, -0.8) # kept away
+
+ self._referee.set_command(RefereeCommand.BALL_PLACEMENT_YELLOW, game.ts)
+ self._referee._state.ball_placement_target = _TARGET
+
+ def eval_status(self, game: Game) -> TestingStatus:
+ ref = game.referee
+ if ref is None:
+ return TestingStatus.IN_PROGRESS
+
+ if ref.referee_command == RefereeCommand.BALL_PLACEMENT_YELLOW:
+ self.command_seen = True
+
+ if not self.command_seen:
+ return TestingStatus.IN_PROGRESS
+
+ ball = game.ball
+ robot0 = game.friendly_robots.get(0)
+ if ball is None or robot0 is None:
+ return TestingStatus.IN_PROGRESS
+
+ dist = math.hypot(robot0.p.x - ball.p.x, robot0.p.y - ball.p.y)
+
+ if self._initial_dist is None:
+ self._initial_dist = dist
+ return TestingStatus.IN_PROGRESS
+
+ if self._initial_dist - dist >= self._APPROACH_PROGRESS:
+ self.robot_approached_ball = True
+ return TestingStatus.SUCCESS
+
+ return TestingStatus.IN_PROGRESS
+
+
+def test_placer_approaches_ball_after_command(headless: bool) -> None:
+ """After BALL_PLACEMENT_YELLOW, the placer robot closes at least 0.8 m toward the ball."""
+ referee = CustomReferee.from_profile_name("simulation")
+ runner = _make_runner(referee)
+ tm = _ApproachBallManager(referee)
+
+ passed = runner.run_test(tm, episode_timeout=20.0, rsim_headless=headless)
+
+ assert tm.command_seen, "BALL_PLACEMENT_YELLOW was never seen in game.referee"
+ assert tm.robot_approached_ball, (
+ f"Robot 0 did not close {_ApproachBallManager._APPROACH_PROGRESS} m toward the ball "
+ f"(started {_ApproachBallManager._INITIAL_DIST} m away)"
+ )
+ assert passed
+
+
+# ---------------------------------------------------------------------------
+# Test 2: placer moves toward designated_position (carry phase)
+# ---------------------------------------------------------------------------
+
+
+class _CarryToTargetManager(AbstractTestManager):
+ """Verify that the placer moves toward the designated position.
+
+ Robot 0 starts far from the ball, which sits between the robot and the
+ target. Because rsim's infrared sensor does not reliably fire, we test
+ the approach phase: the robot drives toward the ball (and therefore toward
+ the target), and must close at least _PROGRESS_THRESHOLD metres on the
+ target before the episode times out.
+ """
+
+ n_episodes = 1
+
+ # How much closer the placer must get to the target over the observation window
+ _PROGRESS_THRESHOLD = 0.2 # metres
+
+ def __init__(self, referee: CustomReferee) -> None:
+ super().__init__()
+ self._referee = referee
+ self.command_seen: bool = False
+ self.placer_made_progress: bool = False
+ self._initial_dist_to_target: Optional[float] = None
+
+ def reset_field(self, sim_controller: AbstractSimController, game: Game) -> None:
+ # Ball between robot 0 and the target so approach motion reduces
+ # distance to target, making progress measurable without has_ball.
+ sim_controller.teleport_ball(-0.6, 0.0)
+ sim_controller.teleport_robot(game.my_team_is_yellow, 0, -1.3, 0.0)
+ sim_controller.teleport_robot(game.my_team_is_yellow, 1, 0.6, -0.6)
+
+ self._referee.set_command(RefereeCommand.BALL_PLACEMENT_YELLOW, game.ts)
+ self._referee._state.ball_placement_target = _TARGET
+
+ def eval_status(self, game: Game) -> TestingStatus:
+ ref = game.referee
+ if ref is None:
+ return TestingStatus.IN_PROGRESS
+
+ if ref.referee_command == RefereeCommand.BALL_PLACEMENT_YELLOW:
+ self.command_seen = True
+
+ if not self.command_seen:
+ return TestingStatus.IN_PROGRESS
+
+ robot0 = game.friendly_robots.get(0)
+ if robot0 is None:
+ return TestingStatus.IN_PROGRESS
+
+ tx, ty = _TARGET
+ dist = math.hypot(robot0.p.x - tx, robot0.p.y - ty)
+
+ if self._initial_dist_to_target is None:
+ self._initial_dist_to_target = dist
+ return TestingStatus.IN_PROGRESS
+
+ improvement = self._initial_dist_to_target - dist
+ if improvement >= self._PROGRESS_THRESHOLD:
+ self.placer_made_progress = True
+ return TestingStatus.SUCCESS
+
+ return TestingStatus.IN_PROGRESS
+
+
+def test_placer_moves_toward_designated_position(headless: bool) -> None:
+ """After capturing the ball, the placer robot moves toward the designated position."""
+ referee = CustomReferee.from_profile_name("simulation")
+ runner = _make_runner(referee)
+ tm = _CarryToTargetManager(referee)
+
+ passed = runner.run_test(tm, episode_timeout=25.0, rsim_headless=headless)
+
+ assert tm.command_seen, "BALL_PLACEMENT_YELLOW was never seen in game.referee"
+ assert tm.placer_made_progress, "Placer robot did not make sufficient progress toward the designated position"
+ assert passed
+
+
+# ---------------------------------------------------------------------------
+# Test 3: non-placer robot stays outside keep-out distance
+# ---------------------------------------------------------------------------
+
+
+class _ClearanceManager(AbstractTestManager):
+ """Verify that non-placer robots respect the ball keep-out distance.
+
+ Robot 0 is placed closest to the ball and is therefore the expected placer.
+ Robot 1 starts inside the keep-out radius. The placer ID is fixed to 0
+ based on what reset_field sets up — it is NOT re-derived from the game state
+ using the same logic as the implementation, which would make the check
+ tautological. Robot 1 must clear and hold outside the keep-out zone.
+ """
+
+ n_episodes = 1
+ _PLACER_ID = 0 # fixed: robot 0 is closest to the ball in reset_field
+ _FRAMES_REQUIRED = 60
+
+ def __init__(self, referee: CustomReferee) -> None:
+ super().__init__()
+ self._referee = referee
+ self.command_seen: bool = False
+ self.clearance_achieved: bool = False
+ self._clear_frame_count: int = 0
+
+ def reset_field(self, sim_controller: AbstractSimController, game: Game) -> None:
+ # Ball at centre so there is plenty of room in all directions for robot 1
+ # to reach 0.8 m clearance. Robot 0 is 0.2 m from the ball → it is the
+ # placer. Robot 1 starts 0.3 m from the ball (inside keep-out) → must clear.
+ sim_controller.teleport_ball(0.0, 0.0)
+ sim_controller.teleport_robot(game.my_team_is_yellow, 0, 0.2, 0.0)
+ sim_controller.teleport_robot(game.my_team_is_yellow, 1, 0.0, 0.3) # within keep-out
+
+ self._referee.set_command(RefereeCommand.BALL_PLACEMENT_YELLOW, game.ts)
+ self._referee._state.ball_placement_target = _TARGET
+
+ def eval_status(self, game: Game) -> TestingStatus:
+ ref = game.referee
+ if ref is None:
+ return TestingStatus.IN_PROGRESS
+
+ if ref.referee_command == RefereeCommand.BALL_PLACEMENT_YELLOW:
+ self.command_seen = True
+
+ if not self.command_seen:
+ return TestingStatus.IN_PROGRESS
+
+ ball = game.ball
+ if ball is None:
+ return TestingStatus.IN_PROGRESS
+
+ threshold = BALL_KEEP_OUT_DISTANCE - _CLEAR_MARGIN
+
+ non_placers_clear = all(
+ math.hypot(robot.p.x - ball.p.x, robot.p.y - ball.p.y) >= threshold
+ for rid, robot in game.friendly_robots.items()
+ if rid != self._PLACER_ID
+ )
+
+ if non_placers_clear:
+ self._clear_frame_count += 1
+ else:
+ self._clear_frame_count = 0
+
+ if self._clear_frame_count >= self._FRAMES_REQUIRED:
+ self.clearance_achieved = True
+ return TestingStatus.SUCCESS
+
+ return TestingStatus.IN_PROGRESS
+
+
+def test_non_placer_clears_ball_keep_out_zone(headless: bool) -> None:
+ """Non-placer robot (id=1) clears and holds outside the ball keep-out radius."""
+ referee = CustomReferee.from_profile_name("simulation")
+ runner = _make_runner(referee)
+ tm = _ClearanceManager(referee)
+
+ passed = runner.run_test(tm, episode_timeout=20.0, rsim_headless=headless)
+
+ assert tm.command_seen, "BALL_PLACEMENT_YELLOW was never seen in game.referee"
+ assert tm.clearance_achieved, (
+ f"Robot 1 did not sustain clearance outside "
+ f"BALL_KEEP_OUT_DISTANCE ({BALL_KEEP_OUT_DISTANCE} m) "
+ f"for {_ClearanceManager._FRAMES_REQUIRED} consecutive frames"
+ )
+ assert passed
+
+
+# ---------------------------------------------------------------------------
+# Test 4: placer selection — robot 1 closer to ball is chosen as placer
+# ---------------------------------------------------------------------------
+
+
+class _PlacerSelectionManager(AbstractTestManager):
+ """Verify that the robot closest to the ball is selected as the placer.
+
+ Robot 1 starts closer to the ball than robot 0. The correct behaviour is:
+ - Robot 1 (the placer) closes distance to the ball.
+ - Robot 0 (the non-placer) does NOT drive toward the ball — it should either
+ hold position or move away to clear the keep-out zone.
+
+ This test exercises the min(distance_to_ball) selection logic with robot 0
+ as the non-placer, which the original tests never covered.
+ """
+
+ n_episodes = 1
+ _PLACER_ID = 1 # robot 1 is closer to the ball in reset_field
+ _NON_PLACER_ID = 0
+ _PLACER_PROGRESS = 0.2 # metres robot 1 must close toward the ball
+ _NON_PLACER_GRACE = 0.3 # seconds before we start checking robot 0
+
+ def __init__(self, referee: CustomReferee) -> None:
+ super().__init__()
+ self._referee = referee
+ self.command_seen: bool = False
+ self.placer_approached: bool = False
+ self.non_placer_stayed_away: bool = False
+ self._placer_initial_dist: Optional[float] = None
+ self._non_placer_initial_dist: Optional[float] = None
+ self._command_ts: Optional[float] = None
+
+ def reset_field(self, sim_controller: AbstractSimController, game: Game) -> None:
+ # Ball at centre; robot 1 is 0.3 m from ball, robot 0 is 1.2 m away.
+ sim_controller.teleport_ball(0.0, 0.0)
+ sim_controller.teleport_robot(game.my_team_is_yellow, 0, -1.2, 0.0) # far — non-placer
+ sim_controller.teleport_robot(game.my_team_is_yellow, 1, 0.3, 0.0) # close — placer
+
+ self._referee.set_command(RefereeCommand.BALL_PLACEMENT_YELLOW, game.ts)
+ self._referee._state.ball_placement_target = _TARGET
+
+ def eval_status(self, game: Game) -> TestingStatus:
+ ref = game.referee
+ if ref is None:
+ return TestingStatus.IN_PROGRESS
+
+ if ref.referee_command == RefereeCommand.BALL_PLACEMENT_YELLOW:
+ if not self.command_seen:
+ self.command_seen = True
+ self._command_ts = game.ts
+
+ if not self.command_seen:
+ return TestingStatus.IN_PROGRESS
+
+ ball = game.ball
+ robot1 = game.friendly_robots.get(self._PLACER_ID)
+ robot0 = game.friendly_robots.get(self._NON_PLACER_ID)
+ if ball is None or robot1 is None or robot0 is None:
+ return TestingStatus.IN_PROGRESS
+
+ dist1 = math.hypot(robot1.p.x - ball.p.x, robot1.p.y - ball.p.y)
+ dist0 = math.hypot(robot0.p.x - ball.p.x, robot0.p.y - ball.p.y)
+
+ if self._placer_initial_dist is None:
+ self._placer_initial_dist = dist1
+ self._non_placer_initial_dist = dist0
+
+ # Placer (robot 1) must close on the ball.
+ if self._placer_initial_dist - dist1 >= self._PLACER_PROGRESS:
+ self.placer_approached = True
+
+ # Non-placer (robot 0) must not move closer to the ball than it started.
+ # Allow a grace period for the command to propagate, then check.
+ grace_elapsed = (game.ts - self._command_ts) >= self._NON_PLACER_GRACE
+ if grace_elapsed and dist0 >= self._non_placer_initial_dist - 0.1:
+ self.non_placer_stayed_away = True
+
+ if self.placer_approached and self.non_placer_stayed_away:
+ return TestingStatus.SUCCESS
+
+ return TestingStatus.IN_PROGRESS
+
+
+def test_closer_robot_selected_as_placer(headless: bool) -> None:
+ """Robot 1 (closer to ball) is selected as placer; robot 0 does not drive toward ball."""
+ referee = CustomReferee.from_profile_name("simulation")
+ runner = _make_runner(referee)
+ tm = _PlacerSelectionManager(referee)
+
+ passed = runner.run_test(tm, episode_timeout=20.0, rsim_headless=headless)
+
+ assert tm.command_seen, "BALL_PLACEMENT_YELLOW was never seen in game.referee"
+ assert tm.placer_approached, "Robot 1 (closer to ball) did not drive toward the ball — wrong placer selected"
+ assert tm.non_placer_stayed_away, "Robot 0 (non-placer) moved toward the ball — it should clear away"
+ assert passed
diff --git a/utama_core/tests/strategy_runner/test_referee_rsim.py b/utama_core/tests/strategy_runner/test_referee_rsim.py
index 67f3877e..8389538e 100644
--- a/utama_core/tests/strategy_runner/test_referee_rsim.py
+++ b/utama_core/tests/strategy_runner/test_referee_rsim.py
@@ -177,15 +177,10 @@ def test_ball_placement_robot_approaches_designated_position(headless):
class _DirectFreeOursManager(AbstractTestManager):
- """Ball exits the side boundary with no friendly robot nearby → DIRECT_FREE_YELLOW (ours).
-
- With no robot close enough to the ball to register a friendly last-touch,
- OutOfBoundsRule defaults to DIRECT_FREE_YELLOW. We verify the kicker
- drives toward the (now out-of-bounds) ball.
- """
+ """DIRECT_FREE_YELLOW is injected directly; kicker must drive toward the ball."""
n_episodes = 1
- APPROACH_TOLERANCE = 0.6 # slightly wider: ball may be just outside boundary
+ APPROACH_TOLERANCE = 0.6
def __init__(self, referee: CustomReferee):
super().__init__()
@@ -194,13 +189,12 @@ def __init__(self, referee: CustomReferee):
self.robot_near_ball: bool = False
def reset_field(self, sim_controller: AbstractSimController, game: Game):
- # Keep all robots well away from the ball so last-touch is unknown → DIRECT_FREE_YELLOW
sim_controller.teleport_robot(game.my_team_is_yellow, 0, -1.5, 0.0)
sim_controller.teleport_robot(game.my_team_is_yellow, 1, -2.0, 0.5)
sim_controller.teleport_robot(game.my_team_is_yellow, 2, -2.0, -0.5)
- # Ball heading out the top sideline
- sim_controller.teleport_ball(0.0, 2.5, vx=0.0, vy=2.5)
- self._referee.set_command(RefereeCommand.FORCE_START, game.ts)
+ sim_controller.teleport_ball(0.0, 0.0)
+ # Inject directly — bypass OOB detection which now routes through ball placement first.
+ self._referee.force_command(RefereeCommand.DIRECT_FREE_YELLOW, game.ts)
def eval_status(self, game: Game) -> TestingStatus:
ref = game.referee
@@ -245,34 +239,26 @@ def test_direct_free_kick_ours_robot_drives_to_ball(headless):
class _DirectFreeTheirsManager(AbstractTestManager):
- """A robot starts inside the keep-out radius; DIRECT_FREE_BLUE (theirs) is issued.
-
- We place robot 0 right next to the ball before it exits so last-touch registers
- as friendly → OutOfBoundsRule issues DIRECT_FREE_BLUE (opponent's free kick).
- The test verifies that all robots end up outside the keep-out radius.
- """
+ """DIRECT_FREE_BLUE is injected directly; all robots must clear the keep-out radius."""
n_episodes = 1
- # All robots must clear beyond this radius from the ball position.
- CLEAR_TOLERANCE = 0.1 # allowed margin inside keep-out (robots should be well clear)
+ CLEAR_TOLERANCE = 0.1
def __init__(self, referee: CustomReferee):
super().__init__()
self._referee = referee
self.direct_free_seen: bool = False
self.robots_cleared: bool = False
- # We record the ball position when DIRECT_FREE_BLUE fires to check clearing.
self._ball_pos_at_call: Optional[tuple[float, float]] = None
def reset_field(self, sim_controller: AbstractSimController, game: Game):
- # Robot 0 is placed right next to the ball — it will register as last-toucher.
- sim_controller.teleport_robot(game.my_team_is_yellow, 0, 0.0, 2.4)
- # Robots 1 and 2 also start near the ball path — both inside keep-out.
- sim_controller.teleport_robot(game.my_team_is_yellow, 1, 0.1, 2.3)
- sim_controller.teleport_robot(game.my_team_is_yellow, 2, -0.1, 2.3)
- # Ball heading out the top sideline; robot 0 is close enough for last-touch
- sim_controller.teleport_ball(0.0, 2.5, vx=0.0, vy=2.5)
- self._referee.set_command(RefereeCommand.FORCE_START, game.ts)
+ # All robots start inside keep-out radius around the ball.
+ sim_controller.teleport_robot(game.my_team_is_yellow, 0, 0.0, 0.3)
+ sim_controller.teleport_robot(game.my_team_is_yellow, 1, 0.1, -0.3)
+ sim_controller.teleport_robot(game.my_team_is_yellow, 2, -0.1, 0.3)
+ sim_controller.teleport_ball(0.0, 0.0)
+ # Inject directly — bypass OOB detection which now routes through ball placement first.
+ self._referee.force_command(RefereeCommand.DIRECT_FREE_BLUE, game.ts)
def eval_status(self, game: Game) -> TestingStatus:
ref = game.referee
diff --git a/utama_core/tests/strategy_runner/test_runner_misconfig.py b/utama_core/tests/strategy_runner/test_runner_misconfig.py
index 1a04f9e4..3e458ad0 100644
--- a/utama_core/tests/strategy_runner/test_runner_misconfig.py
+++ b/utama_core/tests/strategy_runner/test_runner_misconfig.py
@@ -132,7 +132,9 @@ def __init__(self, buffers, on_geometry=None):
assert len(started[0]) == 1
-def test_setup_vision_and_referee_starts_both_receivers_when_referee_official(monkeypatch):
+def test_setup_vision_and_referee_starts_both_receivers_when_referee_official(
+ monkeypatch,
+):
from utama_core.run import strategy_runner as runner_mod
started = []
@@ -262,3 +264,194 @@ def test_strategy_runner_bounds_outside_non_standard_field_dims():
full_field_dims=GREAT_EXHIBITION_FIELD_DIMS,
field_bounds=too_large_for_custom_dims,
)
+
+
+def test_validate_vision_to_cmd_mapping_real_not_controlled():
+ runner = SimpleNamespace(
+ mode=Mode.REAL,
+ my_team_is_yellow=True,
+ opp=None,
+ exp_friendly=3,
+ )
+ result = StrategyRunner._validate_vision_to_cmd_mapping(runner, None, True)
+ assert result == {}
+
+
+def test_validate_vision_to_cmd_mapping_pvp_opp_mapping_missing():
+ runner = SimpleNamespace(
+ mode=Mode.REAL,
+ my_team_is_yellow=True,
+ opp=True,
+ exp_friendly=3,
+ exp_enemy=3,
+ )
+ with pytest.raises(ValueError, match="required for both teams in real PVP"):
+ StrategyRunner._validate_vision_to_cmd_mapping(runner, None, False)
+
+
+def test_validate_vision_to_cmd_mapping_pvp_friendly_mapping_missing():
+ runner = SimpleNamespace(
+ mode=Mode.REAL,
+ my_team_is_yellow=True,
+ opp=True,
+ exp_friendly=3,
+ exp_enemy=3,
+ )
+ with pytest.raises(ValueError, match="required for both teams in real PVP"):
+ StrategyRunner._validate_vision_to_cmd_mapping(runner, None, True)
+
+
+def test_validate_vision_to_cmd_mapping_type_error():
+ runner = SimpleNamespace(
+ mode=Mode.REAL,
+ my_team_is_yellow=True,
+ opp=None,
+ exp_friendly=3,
+ )
+ with pytest.raises(TypeError, match="must be a dictionary"):
+ StrategyRunner._validate_vision_to_cmd_mapping(runner, [], True)
+
+
+def test_validate_vision_to_cmd_mapping_ignored_warning():
+ import warnings
+
+ runner = SimpleNamespace(
+ mode=Mode.REAL,
+ my_team_is_yellow=True,
+ opp=None,
+ exp_friendly=3,
+ )
+ with pytest.warns(UserWarning, match="vision_to_cmd_mapping is provided but will be ignored"):
+ StrategyRunner._validate_vision_to_cmd_mapping(runner, {0: 0}, False)
+
+
+def test_validate_vision_to_cmd_mapping_incorrect_length_friendly():
+ runner = SimpleNamespace(
+ mode=Mode.REAL,
+ my_team_is_yellow=True,
+ opp=True,
+ exp_friendly=3,
+ exp_enemy=3,
+ )
+ with pytest.raises(ValueError, match="must include all expected friendly robots"):
+ StrategyRunner._validate_vision_to_cmd_mapping(runner, {0: 0, 1: 1}, True)
+
+
+def test_validate_vision_to_cmd_mapping_incorrect_length_enemy():
+ runner = SimpleNamespace(
+ mode=Mode.REAL,
+ my_team_is_yellow=True,
+ opp=True,
+ exp_friendly=3,
+ exp_enemy=3,
+ )
+ with pytest.raises(ValueError, match="must include all expected opponent robots"):
+ StrategyRunner._validate_vision_to_cmd_mapping(runner, {0: 0, 1: 1}, False)
+
+
+def test_validate_vision_to_cmd_mapping_invalid_ids():
+ from utama_core.config.physical_constants import MAX_ROBOT_ID
+
+ runner = SimpleNamespace(
+ mode=Mode.REAL,
+ my_team_is_yellow=True,
+ opp=None,
+ exp_friendly=3,
+ )
+ with pytest.raises(TypeError, match="must map integers to integers"):
+ StrategyRunner._validate_vision_to_cmd_mapping(runner, {0: "0"}, True)
+ with pytest.raises(ValueError, match="cannot have negative IDs"):
+ StrategyRunner._validate_vision_to_cmd_mapping(runner, {-1: 0}, True)
+ with pytest.raises(ValueError, match="cannot have vision IDs greater than"):
+ StrategyRunner._validate_vision_to_cmd_mapping(runner, {MAX_ROBOT_ID + 1: 0}, True)
+ with pytest.raises(ValueError, match="cannot have command IDs greater than 255"):
+ StrategyRunner._validate_vision_to_cmd_mapping(runner, {0: 256}, True)
+
+
+def test_validate_vision_to_cmd_mapping_sim_mode_raises():
+ runner = SimpleNamespace(
+ mode=Mode.RSIM,
+ my_team_is_yellow=True,
+ opp=None,
+ )
+ with pytest.raises(ValueError, match="should not be provided in simulation modes"):
+ StrategyRunner._validate_vision_to_cmd_mapping(runner, {0: 0}, True)
+
+
+def test_trusted_ir_robots_yellow_team_is_my_team():
+ """yellow_trusted_ir_robots routes to my refiner when my_team_is_yellow=True."""
+ runner = StrategyRunner(
+ strategy=DummyStrategy(),
+ my_team_is_yellow=True,
+ my_team_is_right=True,
+ mode="rsim",
+ exp_friendly=3,
+ exp_enemy=3,
+ yellow_trusted_ir_robots=frozenset({0, 1}),
+ )
+ assert runner.my.robot_info_refiner._trusted_ir_robots == frozenset({0, 1})
+ assert runner.opp is None
+
+
+def test_trusted_ir_robots_blue_team_is_my_team():
+ """blue_trusted_ir_robots routes to my refiner when my_team_is_yellow=False."""
+ runner = StrategyRunner(
+ strategy=DummyStrategy(),
+ my_team_is_yellow=False,
+ my_team_is_right=True,
+ mode="rsim",
+ exp_friendly=3,
+ exp_enemy=3,
+ blue_trusted_ir_robots=frozenset({2}),
+ )
+ assert runner.my.robot_info_refiner._trusted_ir_robots == frozenset({2})
+
+
+def test_trusted_ir_robots_both_teams_pvp():
+ """Both colour params route correctly in PVP mode."""
+ runner = StrategyRunner(
+ strategy=DummyStrategy(),
+ opp_strategy=DummyStrategy(),
+ my_team_is_yellow=True,
+ my_team_is_right=True,
+ mode="rsim",
+ exp_friendly=3,
+ exp_enemy=3,
+ yellow_trusted_ir_robots=frozenset({0}),
+ blue_trusted_ir_robots=frozenset({1}),
+ )
+ assert runner.my.robot_info_refiner._trusted_ir_robots == frozenset({0})
+ assert runner.opp.robot_info_refiner._trusted_ir_robots == frozenset({1})
+
+
+def test_trusted_ir_robots_none_by_default():
+ """Default None means trust all IR sensors (backwards-compatible)."""
+ runner = StrategyRunner(
+ strategy=DummyStrategy(),
+ my_team_is_yellow=True,
+ my_team_is_right=True,
+ mode="rsim",
+ exp_friendly=3,
+ exp_enemy=3,
+ )
+ assert runner.my.robot_info_refiner._trusted_ir_robots is None
+
+
+def test_check_no_cmd_duplicate_if_transmission_sharing():
+ runner = SimpleNamespace()
+
+ # Test valid non-overlapping mappings
+ yellow_mapping = {0: 1, 1: 2}
+ blue_mapping = {0: 3, 1: 4}
+ StrategyRunner._check_no_cmd_duplicate_if_transmission_sharing(runner, yellow_mapping, blue_mapping)
+
+ # Test collision in same team
+ yellow_mapping_duplicate = {0: 1, 1: 1}
+ with pytest.raises(ValueError, match="cannot have overlapping command IDs"):
+ StrategyRunner._check_no_cmd_duplicate_if_transmission_sharing(runner, yellow_mapping_duplicate, blue_mapping)
+
+ # Test collision across teams
+ yellow_mapping = {0: 1, 1: 2}
+ blue_mapping_overlap = {0: 2, 1: 4}
+ with pytest.raises(ValueError, match="cannot have overlapping command IDs"):
+ StrategyRunner._check_no_cmd_duplicate_if_transmission_sharing(runner, yellow_mapping, blue_mapping_overlap)