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/rules/out_of_bounds_rule.py b/utama_core/custom_referee/rules/out_of_bounds_rule.py
index 3f40adf2..c8e7a98b 100644
--- a/utama_core/custom_referee/rules/out_of_bounds_rule.py
+++ b/utama_core/custom_referee/rules/out_of_bounds_rule.py
@@ -15,7 +15,7 @@
RefereeCommand.FORCE_START,
}
-_INFIELD_OFFSET = 0.1 # metres inside the boundary for free-kick placement
+_INFIELD_OFFSET = 0.25 # metres inside the boundary for a playable free-kick placement
class OutOfBoundsRule(BaseRule):
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/predictors/position.py b/utama_core/data_processing/predictors/position.py
index 3ce5ae65..8a1162b4 100644
--- a/utama_core/data_processing/predictors/position.py
+++ b/utama_core/data_processing/predictors/position.py
@@ -3,94 +3,24 @@
from utama_core.entities.data.vector import Vector2D
from utama_core.entities.game import Game
-### Incomplete Code Snippet ###
-# def predict_frame_after(self, t: float) -> VisionData:
-# yellow_pos = [
-# self.predict_object_pos_after(t, RobotEntity(Colour.YELLOW, i))
-# for i in range(len(self.get_robots_pos(True)))
-# ]
-# blue_pos = [
-# self.predict_object_pos_after(t, RobotEntity(Colour.BLUE, i))
-# for i in range(len(self.get_robots_pos(False)))
-# ]
-# ball_pos = self.predict_object_pos_after(t, Ball)
-# if ball_pos is None or None in yellow_pos or None in blue_pos:
-# return None
-# else:
-# return VisionData(
-# self._records[-1].ts + t,
-# list(map(lambda pos: VisionRobotData(pos[0], pos[1], 0), yellow_pos)),
-# list(map(lambda pos: VisionRobotData(pos[0], pos[1], 0), blue_pos)),
-# [VisionBallData(ball_pos[0], ball_pos[1], 0, 1)], # TODO : Support z axis
-# )
+def predict_ball_pos_at_x(game: Game, x: float) -> Optional[Vector2D]:
+ """Predict where the ball will cross a given x-coordinate.
-# ### General Object Position Prediction ###
-# def predict_object_pos_after(self, t: float, object: GameObject) -> Optional[tuple]:
-# # If t is after the object has stopped we return the position at which object stopped.
-# sx = 0
-# sy = 0
-
-# acceleration = self.get_object_acceleration(object)
-
-# if acceleration is None:
-# return None
-
-# ax, ay = acceleration
-# vels = self.get_object_velocity(object)
-
-# if vels is None:
-# ux, uy = None, None
-# else:
-# ux, uy = vels
-
-# if object is Ball:
-# ball = self.get_ball_pos()
-# start_x, start_y = ball[0].x, ball[0].y
-# else:
-# posn = self._get_object_position_at_frame(len(self._records) - 1, object)
-# start_x, start_y = posn.x, posn.y
-
-# if ax and ux:
-# sx = self._calculate_displacement(ux, ax, t)
-
-# if ay and uy:
-# sy = self._calculate_displacement(uy, ay, t)
-
-# return (
-# start_x + sx,
-# start_y + sy,
-# ) # TODO: Doesn't take into account spin / angular vel
-
-# def _calculate_displacement(self, u, a, t) -> float:
-# if a == 0: # Handle zero acceleration case
-# return u * t
-# else:
-# stop_time = -u / a
-# if stop_time < 0:
-# stop_time = float("inf")
-# effective_time = min(t, stop_time)
-# displacement = (u * effective_time) + (0.5 * a * effective_time**2)
-# logger.debug(
-# f"Displacement: {displacement} for time: {effective_time}, stop time: {stop_time}"
-# )
-# return displacement
-
-
-def predict_ball_pos_at_x(game: Game, x: float) -> Optional[tuple]:
+ Returns None if the ball is not moving toward ``x`` (stationary along x,
+ or moving away). Callers should fall back to their own positioning logic
+ (e.g. goalkeep uses shadow-based ``stop_y``).
+ """
vel = game.ball.v.to_2d()
- if not vel or not vel[0] or not vel[1]:
- return None
+ if abs(vel.x) < 1e-12:
+ return None # Ball not moving along x — cannot predict arrival
- ux, uy = vel
- pos = game.ball.p
- bx = pos.x
- by = pos.y
+ pos = game.ball.p.to_2d()
+ t = (x - pos.x) / vel.x
- if uy == 0:
- return (bx, by)
+ if t < 0:
+ return None # Ball moving away from target x
- t = (x - bx) / ux
- y = by + uy * t
+ y = pos.y + vel.y * t
return Vector2D(x, y)
diff --git a/utama_core/motion_planning/src/controllers/fastpathplanning.py b/utama_core/motion_planning/src/controllers/fastpathplanning.py
index 2807f21b..d4f07342 100644
--- a/utama_core/motion_planning/src/controllers/fastpathplanning.py
+++ b/utama_core/motion_planning/src/controllers/fastpathplanning.py
@@ -1,12 +1,6 @@
-from abc import ABC, abstractmethod
-
-import numpy as np
-
from utama_core.config.enums import Mode
-from utama_core.config.physical_constants import ROBOT_RADIUS
from utama_core.entities.data.vector import Vector2D
from utama_core.entities.game import Game
-from utama_core.entities.game.field import Field
from utama_core.motion_planning.src.common.motion_controller import MotionController
from utama_core.motion_planning.src.fastpathplanning.planner import FastPathPlanner
from utama_core.motion_planning.src.pid.pid import get_pids
@@ -15,10 +9,9 @@
class FastPathPlanningController(MotionController):
def __init__(self, mode: Mode, rsim_env: SSLStandardEnv | None = None):
- self.mode = mode
- self.rsim_env: SSLStandardEnv | None = rsim_env
+ super().__init__(mode, rsim_env)
self.pid_oren, self.pid_trans = get_pids(mode)
- self.fpp = FastPathPlanner(env=self.rsim_env)
+ self.fpp = FastPathPlanner(env=rsim_env)
def calculate(
self,
diff --git a/utama_core/motion_planning/src/fastpathplanning/planner.py b/utama_core/motion_planning/src/fastpathplanning/planner.py
index 84079c17..0ec7424b 100644
--- a/utama_core/motion_planning/src/fastpathplanning/planner.py
+++ b/utama_core/motion_planning/src/fastpathplanning/planner.py
@@ -21,7 +21,7 @@
class FastPathPlanner:
- def __init__(self, env: SSLStandardEnv):
+ def __init__(self, env: SSLStandardEnv | None):
self._env = env
self.config = config
self.OBSTACLE_CLEARANCE = self.config.OBSTACLE_CLEARANCE
@@ -62,7 +62,7 @@ def _get_obstacles(
obstacle_list.append(obstacle_segment)
- # DRAWING: Show the projected velocity line in Red
+ # DRAWING: Show the projected velocity line in Red when an RSim renderer is available.
if self._env is not None:
self._env.draw_line(obstacle_segment, color="Red")
@@ -271,7 +271,7 @@ def _path_to(
# 4. Plan geometric path
final_trajectory, _ = self.check_segment((our_pos, safe_target), obstacles, 0, safe_target, field_bounds)
- # 5. Draw the resulting safe path segments
+ # 5. Draw the resulting safe path segments when an RSim renderer is available.
if self._env is not None:
for i in final_trajectory:
self._env.draw_line(i)
diff --git a/utama_core/rsoccer_simulator/src/Simulators/robosim/robosim_wrapper.py b/utama_core/rsoccer_simulator/src/Simulators/robosim/robosim_wrapper.py
index becd4787..321cb560 100644
--- a/utama_core/rsoccer_simulator/src/Simulators/robosim/robosim_wrapper.py
+++ b/utama_core/rsoccer_simulator/src/Simulators/robosim/robosim_wrapper.py
@@ -50,6 +50,7 @@ def __init__(self, sim_type, n_blue, n_yellow, field_type, time_step_ms):
text=True,
bufsize=1,
env=env,
+ cwd=project_root,
)
def step(self, commands: np.ndarray):
diff --git a/utama_core/rsoccer_simulator/src/ssl/ssl_gym_base.py b/utama_core/rsoccer_simulator/src/ssl/ssl_gym_base.py
index dec6ad79..ff579d12 100644
--- a/utama_core/rsoccer_simulator/src/ssl/ssl_gym_base.py
+++ b/utama_core/rsoccer_simulator/src/ssl/ssl_gym_base.py
@@ -4,6 +4,7 @@
# - To create your wrapper from env to communcation, use inherit from this class!
"""
+import random
from typing import List, Optional
import numpy as np
@@ -99,6 +100,10 @@ def step(self, action):
return observation, reward, done, False, {}
def reset(self, *, seed=None, options=None):
+ if seed is not None:
+ random.seed(seed)
+ np.random.seed(seed)
+
self.steps = 0
self.last_frame = None
self.sent_commands = None
diff --git a/utama_core/run/game_gater.py b/utama_core/run/game_gater.py
index 0634805f..96174001 100644
--- a/utama_core/run/game_gater.py
+++ b/utama_core/run/game_gater.py
@@ -119,20 +119,4 @@ def _add_frame(my_game_frame: GameFrame, opp_game_frame: GameFrame) -> Tuple[Gam
if len(my_game_frame.enemy_robots) > exp_enemy:
raise ValueError(f"Too many enemy robots: {len(my_game_frame.enemy_robots)} > {exp_enemy}")
- # Validate that the vision→cmd mapping keys exactly match the observed robot IDs.
- # Done here (after first valid frame) because actual vision IDs are non-contiguous
- # in some deployments and are not known until a real frame arrives.
- if my_vision_to_cmd_mapping is not None:
- GameGater._validate_mapping_covers_game_frame(
- my_vision_to_cmd_mapping,
- set(my_game_frame.friendly_robots.keys()),
- "friendly",
- )
- if opp_vision_to_cmd_mapping is not None and opp_game_frame is not None:
- GameGater._validate_mapping_covers_game_frame(
- opp_vision_to_cmd_mapping,
- set(opp_game_frame.friendly_robots.keys()),
- "opponent",
- )
-
return my_game_frame, opp_game_frame
diff --git a/utama_core/run/strategy_runner.py b/utama_core/run/strategy_runner.py
index a882a2c4..ae1c761a 100644
--- a/utama_core/run/strategy_runner.py
+++ b/utama_core/run/strategy_runner.py
@@ -232,8 +232,10 @@ def __init__(
# Derive per-color roster allowlists from the validated mappings (real mode only).
# Any robot ID seen by vision that is not in the allowlist is silently dropped so that
# stray detections from robots not in play never pollute the game state.
+ # Blue filtering is only applied when there is an opponent strategy — in single-team
+ # mode blue robots are tracked as enemies and must not be filtered out.
_allowed_yellow = frozenset(self.yellow_vision_to_cmd_mapping) or None
- _allowed_blue = frozenset(self.blue_vision_to_cmd_mapping) or None
+ _allowed_blue = (frozenset(self.blue_vision_to_cmd_mapping) or None) if opp_strategy is not None else None
self.my, self.opp = self._setup_sides_data(
strategy,
@@ -395,8 +397,16 @@ def _check_no_cmd_duplicate_if_transmission_sharing(
seen.add(v)
def _handle_sigint(self, sig, frame):
+ if self._stop_event.is_set():
+ signal.default_int_handler(sig, frame)
self._stop_event.set()
- signal.default_int_handler(sig, frame)
+ self._stop_fps_live()
+ print("\nStopping gracefully. Press Ctrl+C again to force quit.", flush=True)
+
+ def _stop_fps_live(self):
+ if self._fps_live:
+ self._fps_live.stop()
+ self._fps_live = None
def _load_mode(self, mode_str: str) -> Mode:
"""Convert a mode string to a Mode enum value.
@@ -988,8 +998,7 @@ def close(self, stop_command_repeat: int = 20):
self.replay_writer.close()
if self.rsim_env:
self.rsim_env.close()
- if self._fps_live:
- self._fps_live.stop()
+ self._stop_fps_live()
def run_test(
self,
@@ -1074,7 +1083,7 @@ def run(self):
"""Run the main loop, stepping the game until interrupted.
If an RSim environment is present, it ensures rendering is on. The loop
- continues until a KeyboardInterrupt is received, after which resources
+ continues until interrupted via SIGINT stop event, after which resources
(such as replay writer and rsim env) are closed.
"""
signal.signal(signal.SIGINT, self._handle_sigint)
@@ -1101,6 +1110,16 @@ def run(self):
finally:
self.close()
+ def step_once(self):
+ """Advance the runner by one strategy/simulation tick.
+
+ This is a public wrapper around the internal single-step loop for
+ deterministic harnesses and scenario runners that need to apply events
+ or assertions between ticks without taking ownership of the runner's
+ game-loop internals.
+ """
+ self._run_step()
+
def _run_step(self):
"""Perform one tick of the overall game loop.
diff --git a/utama_core/skills/src/defend_parameter.py b/utama_core/skills/src/defend_parameter.py
index bd472e84..995ec1d1 100644
--- a/utama_core/skills/src/defend_parameter.py
+++ b/utama_core/skills/src/defend_parameter.py
@@ -1,14 +1,112 @@
-import math
from typing import Optional
-import numpy as np
-
+from utama_core.config.physical_constants import BALL_RADIUS, ROBOT_RADIUS
from utama_core.data_processing.predictors.position import predict_ball_pos_at_x
from utama_core.entities.data.vector import Vector2D
from utama_core.entities.game import Game
from utama_core.motion_planning.src.common.motion_controller import MotionController
from utama_core.rsoccer_simulator.src.ssl.envs.standard_ssl import SSLStandardEnv
from utama_core.skills.src.go_to_point import go_to_point
+from utama_core.skills.src.utils.defense_utils import (
+ clamp_y,
+ intersection_with_x_line,
+ single_defender_stop_y,
+)
+
+
+def _defender_target_y(ball_pos, keeper_x, goal_frame_y, edge_offset, defender_x, defense_half_width):
+ """Compute the defender y-position for a given goal_frame_y assignment."""
+ offset = edge_offset if goal_frame_y >= 0 else -edge_offset
+ goal_point_y = goal_frame_y + offset
+
+ dx = keeper_x - ball_pos.x
+ if abs(dx) > 1e-12:
+ t = (defender_x - ball_pos.x) / dx
+ t = max(0.0, min(1.0, t))
+ target_y = ball_pos.y + t * (goal_point_y - ball_pos.y)
+ else:
+ target_y = goal_frame_y
+
+ return max(-defense_half_width, min(defense_half_width, target_y))
+
+
+def _shadow_width(ball_pos, defender_pos, keeper_x, post_limit, edge_offset):
+ """Width of the shadow interval a defender casts on the keeper line."""
+ _, yy_top = intersection_with_x_line(
+ (ball_pos.x, ball_pos.y),
+ (defender_pos.x, defender_pos.y + edge_offset),
+ keeper_x,
+ post_limit,
+ )
+ _, yy_bottom = intersection_with_x_line(
+ (ball_pos.x, ball_pos.y),
+ (defender_pos.x, defender_pos.y - edge_offset),
+ keeper_x,
+ post_limit,
+ )
+ return abs(yy_top - yy_bottom)
+
+
+def _choose_defender_side(
+ game,
+ ball_pos,
+ keeper_x,
+ post_limit,
+ edge_offset,
+ defender_x,
+ defense_half_width,
+ robot_id,
+):
+ """For a single defender (2-robot team), dynamically pick the better post side."""
+ # Keeper reference: predicted intercept > current keeper y > centre
+ keeper_ref = 0.0
+ try:
+ predicted = predict_ball_pos_at_x(game, keeper_x)
+ if predicted is not None:
+ keeper_ref = clamp_y(predicted.y, post_limit)
+ else:
+ keeper_ref = clamp_y(game.friendly_robots[0].p.y, post_limit)
+ except (IndexError, KeyError, AttributeError):
+ pass
+
+ candidates = [post_limit, -post_limit]
+ best_side = post_limit if robot_id == 1 else -post_limit
+ best_score = None
+
+ for side in candidates:
+ target_y = _defender_target_y(
+ ball_pos,
+ keeper_x,
+ side,
+ edge_offset,
+ defender_x,
+ defense_half_width,
+ )
+ defender_pos = Vector2D(defender_x, target_y)
+ stop_y = single_defender_stop_y(
+ ball_pos,
+ defender_pos,
+ keeper_x,
+ post_limit,
+ edge_offset,
+ )
+ dist = abs(stop_y - keeper_ref)
+ shadow_w = _shadow_width(
+ ball_pos,
+ defender_pos,
+ keeper_x,
+ post_limit,
+ edge_offset,
+ )
+ # Lower is better: closest keeper stop_y, then largest shadow, then id convention
+ id_tiebreak = 0 if (robot_id == 1) == (side > 0) else 1
+ score = (dist, -shadow_w, id_tiebreak)
+
+ if best_score is None or score < best_score:
+ best_score = score
+ best_side = side
+
+ return best_side
def defend_parameter(
@@ -16,93 +114,44 @@ def defend_parameter(
motion_controller: MotionController,
robot_id: int,
env: Optional[SSLStandardEnv] = None,
+ goal_frame_y: Optional[float] = None,
):
- defenseing_friendly = game.friendly_robots[robot_id]
- vel = game.ball.v.to_2d()
- if len(game.friendly_robots) > 2:
- if game.ball.p.y >= -0.5 and robot_id == 1:
- target_pos = [3.0 if game.my_team_is_right else -3.0, -1.2]
- return go_to_point(
- game,
- motion_controller,
- robot_id,
- Vector2D(target_pos[0], target_pos[1]),
- )
- elif game.ball.p.y < -0.5 and robot_id == 2:
- target_pos = [3.0 if game.my_team_is_right else -3.0, 1.2]
- return go_to_point(
+ ball_pos = game.ball.p.to_2d()
+
+ goal_x = game.field.my_goal_line[0][0]
+ goal_half_width = game.field.half_goal_width
+ defense_area = game.field.my_defense_area
+ defense_front_x = float(defense_area[1][0])
+ defender_x = defense_front_x + (ROBOT_RADIUS if not game.my_team_is_right else -ROBOT_RADIUS)
+ defense_half_width = abs(float(defense_area[0][1]))
+
+ keeper_x = goal_x + (ROBOT_RADIUS if not game.my_team_is_right else -ROBOT_RADIUS)
+ post_limit = goal_half_width - ROBOT_RADIUS
+ edge_offset = BALL_RADIUS + ROBOT_RADIUS
+
+ if goal_frame_y is None:
+ if len(game.friendly_robots) == 2:
+ goal_frame_y = _choose_defender_side(
game,
- motion_controller,
+ ball_pos,
+ keeper_x,
+ post_limit,
+ edge_offset,
+ defender_x,
+ defense_half_width,
robot_id,
- Vector2D(target_pos[0], target_pos[1]),
)
- if robot_id == 1:
- goal_frame = 0.5
- else:
- goal_frame = -0.5
-
- def positions_to_defend_parameter(x2, y2):
- x1, y1 = game.ball.p.x, game.ball.p.y
- x3, y3 = defenseing_friendly.p.x, defenseing_friendly.p.y
- t = ((x3 - x1) * (x2 - x1) + (y3 - y1) * (y2 - y1)) / ((x2 - x1) ** 2 + (y2 - y1) ** 2)
- x4 = x1 + t * (x2 - x1)
- y4 = y1 + t * (y2 - y1)
-
- def cal_xy5(xa, ya, xb, yb, w, x4, y4):
- x5 = xa - w
- dx = xb - xa
- if abs(dx) < 1e-12:
- return x4, y4
- t = (x5 - xa) / dx
- y5 = ya + t * (yb - ya)
- return x5, y5
-
- def distance(x1, y1, x2, y2):
- return math.sqrt((x2 - x1) ** 2 + (y2 - y1) ** 2)
-
- if abs(x2 - x4) < 1 and -1 < y4 < 1:
- hori_x, hori_y = cal_xy5(x2, y2, x1, y1, 1.0, x4, y4)
- ver_x, ver_y = cal_xy5(y2, x2, y1, x1, 2.0, x4, y4)
- if distance(x4, y4, hori_x, hori_y) < distance(x4, y4, ver_x, ver_y):
- x4, y4 = hori_x, hori_y
- else:
- x4, y4 = ver_y, ver_x
-
- return x3, y3, x4, y4
-
- ball_y_at_baseline = predict_ball_pos_at_x(game, 4.5 if game.my_team_is_right else -4.5)
- ball_y_at_robo = predict_ball_pos_at_x(game, defenseing_friendly.p.x)
- if (
- vel[0] ** 2 + vel[1] ** 2 > 0.05
- and (ball_y_at_baseline is not None and ball_y_at_baseline[1] < 0.5 and ball_y_at_baseline[1] > -0.5)
- and (ball_y_at_robo is not None and abs(ball_y_at_robo[1] - defenseing_friendly.p.y) > 0.1)
- ):
- x2, y2 = 4.5 if game.my_team_is_right else -4.5, (goal_frame + 0.2 if robot_id == 1 else goal_frame - 0.2)
- x3, y3, x4, y4 = positions_to_defend_parameter(x2, y2)
- target_pos = np.array([x4, y4])
-
- else:
- robot_rad = 0.09
- x2, y2 = 4.5 if game.my_team_is_right else -4.5, -goal_frame
- x3, y3, x4, y4 = positions_to_defend_parameter(x2, y2)
- vec_to_target = np.array(
- [
- x4 - x3,
- y4 + y3,
- ]
- )
- dist_to_target = np.linalg.norm(vec_to_target)
-
- if dist_to_target > 0:
- vec_dir = vec_to_target / dist_to_target
else:
- vec_dir = np.array([0.0, 0.0])
- target_pos = np.array([x4, y4]) - vec_dir * robot_rad
-
- return go_to_point(
- game,
- motion_controller,
- robot_id,
- Vector2D(target_pos[0], target_pos[1]),
- dribbling=True,
+ goal_frame_y = post_limit if robot_id == 1 else -post_limit
+
+ target_y = _defender_target_y(
+ ball_pos,
+ keeper_x,
+ goal_frame_y,
+ edge_offset,
+ defender_x,
+ defense_half_width,
)
+ target = Vector2D(defender_x, target_y)
+
+ return go_to_point(game, motion_controller, robot_id, target, dribbling=True)
diff --git a/utama_core/skills/src/go_to_ball.py b/utama_core/skills/src/go_to_ball.py
index 51b68336..b9ad93bd 100644
--- a/utama_core/skills/src/go_to_ball.py
+++ b/utama_core/skills/src/go_to_ball.py
@@ -1,3 +1,5 @@
+import math
+
from utama_core.entities.data.command import RobotCommand
from utama_core.entities.game import Game
from utama_core.motion_planning.src.common.motion_controller import MotionController
@@ -14,7 +16,8 @@ def go_to_ball(
ball = game.ball.p.to_2d()
robot = game.friendly_robots[robot_id].p
- target_oren = robot.angle_to(ball)
+ # Kicker/dribbler is on the back of the robot; approach with back facing ball.
+ target_oren = (robot.angle_to(ball) + math.pi) % (2 * math.pi) - math.pi
if dribble_when_near:
distance = robot.distance_to(ball)
diff --git a/utama_core/skills/src/goalkeep.py b/utama_core/skills/src/goalkeep.py
index 6b5ac57e..ce182c8e 100644
--- a/utama_core/skills/src/goalkeep.py
+++ b/utama_core/skills/src/goalkeep.py
@@ -1,7 +1,5 @@
from typing import Optional
-import numpy as np
-
from utama_core.config.physical_constants import BALL_RADIUS, ROBOT_RADIUS
from utama_core.data_processing.predictors.position import predict_ball_pos_at_x
from utama_core.entities.data.vector import Vector2D
@@ -9,7 +7,11 @@
from utama_core.motion_planning.src.common.motion_controller import MotionController
from utama_core.rsoccer_simulator.src.ssl.envs.standard_ssl import SSLStandardEnv
from utama_core.skills.src.go_to_point import go_to_point
-from utama_core.skills.src.utils.move_utils import face_ball, move
+from utama_core.skills.src.utils.defense_utils import (
+ clamp_y,
+ intersection_with_x_line,
+ single_defender_stop_y,
+)
# TODO: instead of checking number of friendly, should check roles
@@ -20,110 +22,78 @@ def goalkeep(
robot_id: int,
env: Optional[SSLStandardEnv] = None,
):
- EDGE_OFFSET = BALL_RADIUS + ROBOT_RADIUS
+ if game.ball is None:
+ return None
+
+ edge_offset = BALL_RADIUS + ROBOT_RADIUS
goal_x = game.field.my_goal_line[0][0]
- half_goal_width = game.field.half_goal_width
- target = predict_ball_pos_at_x(game, goal_x)
+ keeper_x = goal_x + (ROBOT_RADIUS if not game.my_team_is_right else -ROBOT_RADIUS)
+ goal_half_width = game.field.half_goal_width
+ post_limit = goal_half_width - ROBOT_RADIUS
+ ball_pos = game.ball.p.to_2d()
+ target = predict_ball_pos_at_x(game, keeper_x)
stop_y = 0.0
- def intersection_with_vertical_line(a, b):
- xa, ya = a
- xb, yb = b
-
- if xb == xa:
- return a # Line is vertical, return the point itself
-
- k = (yb - ya) / (xb - xa)
- y_intersect = ya + k * (goal_x - xa)
- if y_intersect < -half_goal_width:
- return (goal_x, -half_goal_width)
- elif y_intersect > half_goal_width:
- return (goal_x, half_goal_width)
- return (goal_x, y_intersect)
-
- if len(game.friendly_robots) == 2:
+ if len(game.friendly_robots) == 1:
+ stop_y = clamp_y(ball_pos.y, post_limit)
+ elif len(game.friendly_robots) == 2:
try:
# Check if defender is between ball and goal (side-aware)
- defender_between = (game.my_team_is_right and game.friendly_robots[1].p.x > game.ball.p.x) or (
- not game.my_team_is_right and game.friendly_robots[1].p.x < game.ball.p.x
+ defender_between = (game.my_team_is_right and game.friendly_robots[1].p.x > ball_pos.x) or (
+ not game.my_team_is_right and game.friendly_robots[1].p.x < ball_pos.x
)
if defender_between:
- # 1. Project BOTH edges to find the defender's shadow on the goal line
- _, yy_top = intersection_with_vertical_line(
- (game.ball.p.x, game.ball.p.y),
- (
- game.friendly_robots[1].p.x,
- game.friendly_robots[1].p.y + EDGE_OFFSET,
- ),
- )
- _, yy_bottom = intersection_with_vertical_line(
- (game.ball.p.x, game.ball.p.y),
- (
- game.friendly_robots[1].p.x,
- game.friendly_robots[1].p.y - EDGE_OFFSET,
- ),
+ stop_y = single_defender_stop_y(
+ ball_pos,
+ game.friendly_robots[1].p,
+ keeper_x,
+ post_limit,
+ edge_offset,
)
-
- # 2. Calculate the size of the gaps (using max(0, ...) to ignore negative space if shadow is outside the goal)
- top_gap_size = max(0, half_goal_width - yy_top)
- bottom_gap_size = max(0, yy_bottom - (-half_goal_width))
-
- # 3. Position the goalie in the middle of the LARGEST gap
- if top_gap_size > bottom_gap_size:
- stop_y = (yy_top + half_goal_width) / 2
- else:
- stop_y = (yy_bottom - half_goal_width) / 2
except (IndexError, KeyError):
# If robot with ID 1 is not available, keep default stop_y
pass
elif len(game.friendly_robots) >= 3:
try:
# Check if both defenders are between ball and goal (side-aware)
- defender1_between = (game.my_team_is_right and game.friendly_robots[1].p.x > game.ball.p.x) or (
- not game.my_team_is_right and game.friendly_robots[1].p.x < game.ball.p.x
+ defender1_between = (game.my_team_is_right and game.friendly_robots[1].p.x > ball_pos.x) or (
+ not game.my_team_is_right and game.friendly_robots[1].p.x < ball_pos.x
)
- defender2_between = (game.my_team_is_right and game.friendly_robots[2].p.x > game.ball.p.x) or (
- not game.my_team_is_right and game.friendly_robots[2].p.x < game.ball.p.x
+ defender2_between = (game.my_team_is_right and game.friendly_robots[2].p.x > ball_pos.x) or (
+ not game.my_team_is_right and game.friendly_robots[2].p.x < ball_pos.x
)
if defender1_between and defender2_between:
- _, yy1 = intersection_with_vertical_line(
- (game.ball.p.x, game.ball.p.y),
- (
- game.friendly_robots[1].p.x,
- game.friendly_robots[1].p.y + EDGE_OFFSET,
- ),
+ _, yy1 = intersection_with_x_line(
+ (ball_pos.x, ball_pos.y),
+ (game.friendly_robots[1].p.x, game.friendly_robots[1].p.y + edge_offset),
+ keeper_x,
+ post_limit,
)
- _, yy2 = intersection_with_vertical_line(
- (game.ball.p.x, game.ball.p.y),
- (
- game.friendly_robots[2].p.x,
- game.friendly_robots[2].p.y - EDGE_OFFSET,
- ),
+ _, yy2 = intersection_with_x_line(
+ (ball_pos.x, ball_pos.y),
+ (game.friendly_robots[2].p.x, game.friendly_robots[2].p.y - edge_offset),
+ keeper_x,
+ post_limit,
)
stop_y = (yy1 + yy2) / 2
except (IndexError, KeyError):
# If robots with IDs 1 or 2 are not available, keep existing stop_y
pass
- if not target or abs(target[1]) > half_goal_width:
- target = Vector2D(goal_x, stop_y)
-
- # shooters_data = find_likely_enemy_shooter(game.enemy_robots, game.ball)
-
- if target:
- cmd = go_to_point(
- game,
- motion_controller,
- robot_id,
- target,
- dribbling=True,
- )
+ if target is None:
+ target = Vector2D(keeper_x, stop_y)
+ elif abs(target.y) > goal_half_width:
+ # Ball heading toward goal but predicted wide -- clamp to nearest post
+ # instead of snapping to stop_y, so the keeper stays reactive to the shot
+ clamped_y = max(-post_limit, min(post_limit, target.y))
+ target = Vector2D(keeper_x, clamped_y)
else:
- cmd = move(
- game,
- motion_controller,
- robot_id,
- Vector2D(None, None), # No specific target
- face_ball(game.friendly_robots[robot_id].p, game.ball.p),
- )
- return cmd
+ target = Vector2D(keeper_x, target.y)
+
+ return go_to_point(
+ game,
+ motion_controller,
+ robot_id,
+ target,
+ dribbling=True,
+ )
diff --git a/utama_core/skills/src/utils/defense_utils.py b/utama_core/skills/src/utils/defense_utils.py
index c9d654a0..ef98ed4a 100644
--- a/utama_core/skills/src/utils/defense_utils.py
+++ b/utama_core/skills/src/utils/defense_utils.py
@@ -1,4 +1,4 @@
-from typing import Dict, List, Optional, Tuple
+from typing import Dict, List, NamedTuple, Optional, Tuple
import numpy as np
@@ -8,6 +8,37 @@
EPS = 1e-5
+CURVE_MARGIN = 0.1
+CURVE_SHAPE_R = 2.1
+
+
+class DefenseGeometry(NamedTuple):
+ goal_x: float
+ goal_half_width: float
+ defense_front_x: float
+ defense_depth: float
+ defense_half_width: float
+
+
+def _defense_geometry(game: Game) -> DefenseGeometry:
+ """Derive all field-shaped defense values from ``game.field``."""
+ goal_line = game.field.my_goal_line
+ goal_x = float(goal_line[0][0])
+ goal_half_width = abs(float(goal_line[0][1]))
+
+ defense_area = game.field.my_defense_area
+ defense_front_x = float(defense_area[1][0])
+ defense_depth = abs(goal_x - defense_front_x)
+ defense_half_width = abs(float(defense_area[0][1]))
+
+ return DefenseGeometry(
+ goal_x=goal_x,
+ goal_half_width=goal_half_width,
+ defense_front_x=defense_front_x,
+ defense_depth=defense_depth,
+ defense_half_width=defense_half_width,
+ )
+
def align_defenders(
game: Game,
@@ -24,8 +55,8 @@ def align_defenders(
# Start by getting the current position of the defenders
defender_pos = calculate_defense_area(game, defender_parametric_pos)
- # logger.debug(f"DEFENDER {dx} {dy}")
- goal_centre_x = game.field.my_goal_line[0][0]
+ geo = _defense_geometry(game)
+ goal_centre_x = geo.goal_x
if attacker_orientation is None or attacker_orientation == 0:
# In case there is no ball velocity or attackers, use centre of goal
@@ -33,7 +64,10 @@ def align_defenders(
else:
predicted_goal_position = Vector2D(
goal_centre_x,
- clamp_to_goal_width(predict_goal_y_location(game, attacker_position, attacker_orientation)),
+ clamp_to_goal_width(
+ predict_goal_y_location(game, attacker_position, attacker_orientation),
+ geo.goal_half_width,
+ ),
)
if env:
@@ -85,21 +119,16 @@ def calculate_defense_area(game: Game, t: float) -> Vector2D:
cos_t = np.cos(t)
sin_t = np.sin(t)
- a, r = 1.1, 2.1
+ geo = _defense_geometry(game)
+ front_extent = geo.defense_depth + CURVE_MARGIN
+ vertical_extent = geo.defense_half_width + CURVE_MARGIN
+ r = CURVE_SHAPE_R
rp = Vector2D(
- a * ((1 - r) * (abs(cos_t) * cos_t) + r * cos_t),
- a * ((1 - r) * (abs(sin_t) * sin_t) + r * sin_t),
+ front_extent * ((1 - r) * (abs(cos_t) * cos_t) + r * cos_t),
+ vertical_extent * ((1 - r) * (abs(sin_t) * sin_t) + r * sin_t),
)
- # This slows everything down sooo much that everything breaks (ask Fred if still confused)
- # goal_centre_x, _ = game.field.my_goal_line[0]
-
- if game.my_team_is_right:
- goal_centre_x = 4.5
- else:
- goal_centre_x = -4.5
-
- return make_relative_to_goal_centre(goal_centre_x, rp)
+ return make_relative_to_goal_centre(geo.goal_x, rp)
def make_relative_to_goal_centre(goal_centre_x: float, p: Vector2D) -> Vector2D:
@@ -166,8 +195,8 @@ def step_curve(t: float, direction: int):
return direction * STEP_SIZE + t
-def clamp_to_goal_width(y: float) -> float:
- return max(min(y, 0.5), -0.5)
+def clamp_to_goal_width(y: float, goal_half_width: float) -> float:
+ return max(min(y, goal_half_width), -goal_half_width)
def clamp_to_parametric(t: float) -> float:
@@ -204,3 +233,69 @@ def velocity_to_orientation(p: Tuple[float, float]) -> float:
if res < 0:
res += 2 * np.pi
return res
+
+
+# ---------------------------------------------------------------------------
+# Shared shadow-geometry helpers (used by both goalkeep and defend_parameter)
+# ---------------------------------------------------------------------------
+
+
+def clamp_y(y: float, limit: float) -> float:
+ """Clamp *y* to ``[-limit, limit]``."""
+ return max(-limit, min(limit, y))
+
+
+def intersection_with_x_line(a, b, target_x: float, y_limit: float):
+ """Where line *a* -> *b* crosses ``x = target_x``, y clamped to +/-*y_limit*.
+
+ Returns ``(target_x, clamped_y)``. If the line is vertical (dx ~ 0),
+ returns the y of *b* clamped. If the crossing is behind *a* (t < 0),
+ returns the y of *a* clamped.
+ """
+ xa, ya = a
+ xb, yb = b
+ dx = xb - xa
+
+ if abs(dx) < 1e-12:
+ return (target_x, clamp_y(yb, y_limit))
+
+ t = (target_x - xa) / dx
+ if t < 0:
+ return (target_x, clamp_y(ya, y_limit))
+
+ y_intersect = ya + t * (yb - ya)
+ return (target_x, clamp_y(y_intersect, y_limit))
+
+
+def single_defender_stop_y(
+ ball_pos,
+ defender_pos,
+ keeper_x: float,
+ post_limit: float,
+ edge_offset: float,
+) -> float:
+ """Keeper y-position given a single defender's shadow on the keeper line.
+
+ Projects both edges of the defender to find the shadow interval, then
+ positions the keeper in the centre of the largest uncovered gap.
+ """
+ _, yy_top = intersection_with_x_line(
+ (ball_pos.x, ball_pos.y),
+ (defender_pos.x, defender_pos.y + edge_offset),
+ keeper_x,
+ post_limit,
+ )
+ _, yy_bottom = intersection_with_x_line(
+ (ball_pos.x, ball_pos.y),
+ (defender_pos.x, defender_pos.y - edge_offset),
+ keeper_x,
+ post_limit,
+ )
+
+ top_gap_size = max(0, post_limit - yy_top)
+ bottom_gap_size = max(0, yy_bottom - (-post_limit))
+
+ if top_gap_size > bottom_gap_size:
+ return (yy_top + post_limit) / 2
+
+ return (yy_bottom - post_limit) / 2
diff --git a/utama_core/strategy/common/__init__.py b/utama_core/strategy/common/__init__.py
index c8290df5..4ae22305 100644
--- a/utama_core/strategy/common/__init__.py
+++ b/utama_core/strategy/common/__init__.py
@@ -4,3 +4,10 @@
SpaceRequirements,
)
from utama_core.strategy.common.base_blackboard import BaseBlackboard
+from utama_core.strategy.common.blackboard_contract import (
+ BlackboardKeySpec,
+ declared_contract,
+ register_blackboard_contract,
+ resolve_runtime_key,
+ validate_blackboard_contracts,
+)
diff --git a/utama_core/strategy/common/abstract_behaviour.py b/utama_core/strategy/common/abstract_behaviour.py
index 1f14b5ee..128f02b5 100644
--- a/utama_core/strategy/common/abstract_behaviour.py
+++ b/utama_core/strategy/common/abstract_behaviour.py
@@ -4,6 +4,7 @@
from utama_core.config.settings import BLACKBOARD_NAMESPACE_MAP
from utama_core.strategy.common.base_blackboard import BaseBlackboard
+from utama_core.strategy.common.blackboard_contract import register_blackboard_contract
class AbstractBehaviour(py_trees.behaviour.Behaviour):
@@ -83,6 +84,7 @@ def setup(self, **kwargs: Any) -> None:
key="tactic",
access=py_trees.common.Access.READ,
)
+ register_blackboard_contract(self)
self.setup_()
# prevent overriding of setup method
diff --git a/utama_core/strategy/common/abstract_strategy.py b/utama_core/strategy/common/abstract_strategy.py
index 091acdfc..bdf1d7ea 100644
--- a/utama_core/strategy/common/abstract_strategy.py
+++ b/utama_core/strategy/common/abstract_strategy.py
@@ -12,6 +12,7 @@
from utama_core.entities.data.command import RobotCommand
from utama_core.entities.game import Game
from utama_core.entities.game.field import Field, FieldBounds
+from utama_core.entities.referee.referee_command import RefereeCommand
from utama_core.global_utils.math_utils import (
assert_contains,
assert_valid_bounding_box,
@@ -19,6 +20,7 @@
from utama_core.motion_planning.src.common.motion_controller import MotionController
from utama_core.rsoccer_simulator.src.ssl.ssl_gym_base import SSLBaseEnv
from utama_core.skills.src.utils.move_utils import empty_command
+from utama_core.strategy.common.abstract_behaviour import AbstractBehaviour
from utama_core.strategy.common.base_blackboard import BaseBlackboard
from utama_core.team_controller.src.controllers.common.robot_controller_abstract import (
AbstractRobotController,
@@ -73,6 +75,55 @@ class SpaceRequirements:
min_width: float
+_REFEREE_STOPPAGE_COMMANDS = frozenset(
+ {
+ RefereeCommand.HALT,
+ RefereeCommand.STOP,
+ RefereeCommand.PREPARE_KICKOFF_YELLOW,
+ RefereeCommand.PREPARE_KICKOFF_BLUE,
+ RefereeCommand.PREPARE_PENALTY_YELLOW,
+ RefereeCommand.PREPARE_PENALTY_BLUE,
+ RefereeCommand.DIRECT_FREE_YELLOW,
+ RefereeCommand.DIRECT_FREE_BLUE,
+ RefereeCommand.INDIRECT_FREE_YELLOW,
+ RefereeCommand.INDIRECT_FREE_BLUE,
+ RefereeCommand.TIMEOUT_YELLOW,
+ RefereeCommand.TIMEOUT_BLUE,
+ RefereeCommand.GOAL_YELLOW,
+ RefereeCommand.GOAL_BLUE,
+ RefereeCommand.BALL_PLACEMENT_YELLOW,
+ RefereeCommand.BALL_PLACEMENT_BLUE,
+ }
+)
+
+
+class _ResetStrategyOnRefereeStoppage(AbstractBehaviour):
+ """Invalidate a strategy subtree once for each new referee stoppage token."""
+
+ def __init__(self, target: py_trees.behaviour.Behaviour, name: str = "ResetStrategyOnRefereeStoppage"):
+ super().__init__(name=name)
+ self.target = target
+ self._last_reset_token: tuple[RefereeCommand, float | None] | None = None
+
+ def update(self) -> py_trees.common.Status:
+ game = self.blackboard.game
+ referee = getattr(game, "referee", None)
+ if referee is None:
+ return py_trees.common.Status.SUCCESS
+
+ command = getattr(referee, "referee_command", None)
+ if command not in _REFEREE_STOPPAGE_COMMANDS:
+ return py_trees.common.Status.SUCCESS
+
+ token = (command, getattr(referee, "referee_command_timestamp", None))
+ if token != self._last_reset_token:
+ if self.target.status != py_trees.common.Status.INVALID:
+ self.target.stop(py_trees.common.Status.INVALID)
+ self._last_reset_token = token
+
+ return py_trees.common.Status.SUCCESS
+
+
@dataclass
class AbstractStrategy(ABC):
"""
@@ -95,10 +146,19 @@ def __init__(self):
strategy_subtree = self.create_behaviour_tree()
# Wrap the user's strategy tree with the referee override layer (Option B).
- # The root Selector checks referee commands first; if none match (e.g. NORMAL_START
- # or FORCE_START), it falls through to the strategy subtree.
+ # The reset guard invalidates any running strategy memory as soon as a
+ # stoppage command arrives. The root Selector checks referee commands next;
+ # if none match (e.g. NORMAL_START or FORCE_START), it falls through to the
+ # freshly reset strategy subtree.
root = py_trees.composites.Selector(name="Root", memory=False)
- root.add_children([build_referee_override_tree(), strategy_subtree])
+ referee_reset_and_override = py_trees.composites.Sequence(name="RefereeResetAndOverride", memory=False)
+ referee_reset_and_override.add_children(
+ [
+ _ResetStrategyOnRefereeStoppage(strategy_subtree),
+ build_referee_override_tree(),
+ ]
+ )
+ root.add_children([referee_reset_and_override, strategy_subtree])
self.behaviour_tree = py_trees.trees.BehaviourTree(root)
@@ -203,6 +263,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/common/blackboard_contract.py b/utama_core/strategy/common/blackboard_contract.py
new file mode 100644
index 00000000..0ff85cef
--- /dev/null
+++ b/utama_core/strategy/common/blackboard_contract.py
@@ -0,0 +1,242 @@
+from __future__ import annotations
+
+from dataclasses import dataclass
+from typing import Any, Iterable
+
+import py_trees
+from py_trees.blackboard import Blackboard
+
+
+@dataclass(frozen=True)
+class BlackboardKeySpec:
+ """Declared blackboard contract for a behaviour constructor key."""
+
+ name: str
+ access: str | py_trees.common.Access
+ key_attr: str | None = None
+ key: str | None = None
+ required: bool = True
+ type_name: str = "unknown"
+ meaning: str = ""
+ default: str | None = None
+ bounds: tuple[float | None, float | None] | None = None
+
+ def runtime_key(self, node: Any | None = None) -> str | None:
+ if self.key is not None:
+ return self.key
+ if node is not None and self.key_attr is not None:
+ if hasattr(node, self.key_attr):
+ value = getattr(node, self.key_attr)
+ return None if value is None else str(value)
+ return self.name
+
+ def to_dict(self, node: Any | None = None, *, resolve: bool = False) -> dict[str, Any]:
+ key = self.runtime_key(node)
+ payload = {
+ "name": self.name,
+ "key": key,
+ "key_attr": self.key_attr,
+ "access": _access_name(self.access),
+ "required": self.required,
+ "type": self.type_name,
+ "meaning": self.meaning,
+ "default": self.default,
+ "bounds": self.bounds,
+ }
+ if resolve and node is not None and key is not None:
+ payload["resolved_key"] = resolve_runtime_key(node, key)
+ return payload
+
+
+def declared_contract(target: Any, *, resolve: bool = False) -> list[dict[str, Any]]:
+ return [spec.to_dict(target, resolve=resolve) for spec in _contract_specs(target)]
+
+
+def resolve_runtime_key(node: Any, key: str) -> str:
+ blackboards = getattr(node, "blackboards", ())
+ for client in blackboards:
+ try:
+ return client.absolute_name(key)
+ except KeyError:
+ continue
+ client = getattr(node, "blackboard", None)
+ namespace = getattr(client, "namespace", None) if client is not None else None
+ return Blackboard.absolute_name(namespace, key)
+
+
+def register_blackboard_contract(node: Any) -> None:
+ """Register a behaviour's declared blackboard keys on its py_trees client."""
+
+ specs = _contract_specs(node)
+ if not specs:
+ return
+
+ blackboard = getattr(node, "blackboard", None)
+ if blackboard is None:
+ raise RuntimeError(f"{_node_label(node)} has blackboard contracts but no attached blackboard client")
+
+ for spec in specs:
+ key = _registration_key(node, spec)
+ if key is None:
+ continue
+ blackboard.register_key(key=key, access=_normalise_access(spec.access, node, spec))
+
+
+def validate_blackboard_contracts(
+ root: Any,
+ *,
+ seeded_keys: Iterable[str] | None = None,
+ include_base_keys: bool = True,
+) -> dict[str, Any]:
+ """Validate declared read contracts against known writers and seeded inputs.
+
+ This deliberately validates the declared contract graph, not py_trees runtime
+ required keys. Many strategy keys are written by earlier behaviour nodes.
+ """
+
+ available_keys = {_comparison_key(key) for key in (seeded_keys or ())}
+ if include_base_keys:
+ from utama_core.strategy.common.base_blackboard import BaseBlackboard
+
+ available_keys.update(BaseBlackboard.base_keys())
+
+ reads: list[dict[str, Any]] = []
+ writes: set[str] = set()
+ errors: list[dict[str, Any]] = []
+
+ for node in _iter_nodes(root):
+ for spec in _contract_specs(node):
+ try:
+ key = _registration_key(node, spec)
+ if key is None:
+ continue
+ access = _normalise_access(spec.access, node, spec)
+ except (TypeError, ValueError) as exc:
+ errors.append(_validation_error("malformed_contract", node, spec, None, str(exc)))
+ continue
+
+ item = {
+ "node": getattr(node, "name", node.__class__.__name__),
+ "class_name": node.__class__.__name__,
+ "name": spec.name,
+ "key": key,
+ "access": access.name,
+ }
+ if access == py_trees.common.Access.READ:
+ reads.append(item)
+ elif access in {py_trees.common.Access.WRITE, py_trees.common.Access.EXCLUSIVE_WRITE}:
+ writes.add(_comparison_key(key))
+
+ available_keys.update(writes)
+ for item in reads:
+ if _comparison_key(item["key"]) not in available_keys:
+ errors.append(
+ {
+ "code": "missing_writer",
+ **item,
+ "message": f"Required READ key {item['key']!r} has no declared writer or seeded input.",
+ }
+ )
+
+ return {
+ "status": "valid" if not errors else "invalid",
+ "errors": errors,
+ "read_keys": sorted({_comparison_key(item["key"]) for item in reads}),
+ "write_keys": sorted(writes),
+ "available_keys": sorted(available_keys),
+ }
+
+
+def _contract_specs(target: Any) -> tuple[BlackboardKeySpec, ...]:
+ specs = getattr(target, "BLACKBOARD_CONTRACT", ())
+ if specs is None:
+ return ()
+ if not isinstance(specs, tuple):
+ raise TypeError(f"{_node_label(target)} BLACKBOARD_CONTRACT must be a tuple")
+ for spec in specs:
+ if not isinstance(spec, BlackboardKeySpec):
+ raise TypeError(f"{_node_label(target)} BLACKBOARD_CONTRACT entries must be BlackboardKeySpec")
+ return specs
+
+
+def _registration_key(node: Any, spec: BlackboardKeySpec) -> str | None:
+ if spec.key is not None:
+ key = spec.key
+ elif spec.key_attr is not None:
+ if not hasattr(node, spec.key_attr):
+ if spec.required:
+ raise ValueError(
+ f"{_node_label(node)} contract {spec.name!r} references missing attribute {spec.key_attr!r}"
+ )
+ return None
+ value = getattr(node, spec.key_attr)
+ if value is None:
+ if spec.required:
+ raise ValueError(f"{_node_label(node)} contract {spec.name!r} resolved to None")
+ return None
+ key = str(value)
+ else:
+ key = spec.name
+
+ if key == "":
+ if spec.required:
+ raise ValueError(f"{_node_label(node)} contract {spec.name!r} resolved to an empty key")
+ return None
+ return key
+
+
+def _normalise_access(access: Any, node: Any, spec: BlackboardKeySpec) -> py_trees.common.Access:
+ if isinstance(access, py_trees.common.Access):
+ return access
+ if isinstance(access, str):
+ try:
+ return py_trees.common.Access[access]
+ except KeyError as exc:
+ valid = ", ".join(item.name for item in py_trees.common.Access)
+ raise ValueError(
+ f"{_node_label(node)} contract {spec.name!r} has invalid access {access!r}; expected one of {valid}"
+ ) from exc
+ raise TypeError(f"{_node_label(node)} contract {spec.name!r} access must be a string or py_trees Access")
+
+
+def _iter_nodes(root: Any) -> Iterable[Any]:
+ root = getattr(root, "root", root)
+ if hasattr(root, "iterate"):
+ yield from root.iterate()
+ else:
+ yield root
+
+
+def _validation_error(
+ code: str,
+ node: Any,
+ spec: BlackboardKeySpec,
+ key: str | None,
+ message: str,
+) -> dict[str, Any]:
+ return {
+ "code": code,
+ "node": getattr(node, "name", node.__class__.__name__),
+ "class_name": node.__class__.__name__,
+ "name": spec.name,
+ "key": key,
+ "access": _access_name(spec.access),
+ "message": message,
+ }
+
+
+def _comparison_key(key: str) -> str:
+ key = str(key)
+ if key.startswith("/"):
+ return key.rsplit("/", 1)[-1]
+ return key
+
+
+def _node_label(node: Any) -> str:
+ return getattr(node, "name", getattr(node, "__name__", node.__class__.__name__))
+
+
+def _access_name(access: Any) -> str:
+ if hasattr(access, "name"):
+ return access.name
+ return str(access)
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/sim/rsim_controller.py b/utama_core/team_controller/src/controllers/sim/rsim_controller.py
index e6ab263f..8879cdec 100644
--- a/utama_core/team_controller/src/controllers/sim/rsim_controller.py
+++ b/utama_core/team_controller/src/controllers/sim/rsim_controller.py
@@ -43,3 +43,8 @@ def _get_teleport_location(self, robot_id: int, is_team_yellow: bool, add: bool)
@property
def env(self):
return self._env
+
+ @property
+ def latest_sent_commands(self):
+ """Most recent low-level rSim commands, if a frame has been stepped."""
+ return self._env.sent_commands
diff --git a/utama_core/team_controller/src/controllers/sim/rsim_robot_controller.py b/utama_core/team_controller/src/controllers/sim/rsim_robot_controller.py
index bb1504bd..fb2f75fd 100644
--- a/utama_core/team_controller/src/controllers/sim/rsim_robot_controller.py
+++ b/utama_core/team_controller/src/controllers/sim/rsim_robot_controller.py
@@ -143,6 +143,11 @@ def is_team_yellow(self):
def env(self):
return self._env
+ @property
+ def latest_sent_commands(self):
+ """Most recent low-level rSim commands, if a frame has been stepped."""
+ return self._env.sent_commands
+
@property
def robots_info(self):
return self._robots_info
diff --git a/utama_core/tests/abstract_strategy/test_assertions.py b/utama_core/tests/abstract_strategy/test_assertions.py
index 85bcf729..c37bffeb 100644
--- a/utama_core/tests/abstract_strategy/test_assertions.py
+++ b/utama_core/tests/abstract_strategy/test_assertions.py
@@ -1,14 +1,20 @@
from types import SimpleNamespace
+import py_trees
import pytest
from utama_core.config.field_params import STANDARD_FIELD_DIMS
from utama_core.entities.game.field import Field, FieldBounds
+from utama_core.entities.referee.referee_command import RefereeCommand
from utama_core.strategy.common import (
AbstractBehaviour,
AbstractStrategy,
SpaceRequirements,
)
+from utama_core.strategy.common.abstract_strategy import (
+ _REFEREE_STOPPAGE_COMMANDS,
+ _ResetStrategyOnRefereeStoppage,
+)
# Dummy blackboard helper
@@ -118,3 +124,119 @@ def test_space_requirements_rejected_when_field_too_small():
game = make_dummy_game(actual_field)
with pytest.raises(ValueError, match="too small for strategy"):
strategy.assert_field_requirements(game)
+
+
+def test_strategy_subtree_resets_once_when_referee_enters_stoppage():
+ strategy = ResetRecorderStrategy()
+ strategy.setup_strategy_blackboard(is_opp_strat=False)
+ strategy.setup_behaviour_tree(is_opp_strat=False)
+ strategy.blackboard.game = make_referee_game(RefereeCommand.FORCE_START, timestamp=0.0)
+ strategy.blackboard.cmd_map = {}
+
+ strategy.behaviour_tree.tick()
+ assert strategy.recorder.initialise_count == 1
+ assert strategy.recorder.status == py_trees.common.Status.RUNNING
+
+ strategy.blackboard.game.referee = make_referee_data(RefereeCommand.HALT, timestamp=1.0)
+ strategy.behaviour_tree.tick()
+
+ assert strategy.recorder.invalidated_count == 1
+ assert strategy.recorder.status == py_trees.common.Status.INVALID
+
+ strategy.behaviour_tree.tick()
+ assert strategy.recorder.invalidated_count == 1
+
+ strategy.blackboard.game.referee = make_referee_data(RefereeCommand.FORCE_START, timestamp=2.0)
+ strategy.behaviour_tree.tick()
+
+ assert strategy.recorder.initialise_count == 2
+ assert strategy.recorder.status == py_trees.common.Status.RUNNING
+
+
+def test_active_referee_start_commands_do_not_reset_strategy_subtree():
+ strategy = ResetRecorderStrategy()
+ strategy.setup_strategy_blackboard(is_opp_strat=False)
+ strategy.setup_behaviour_tree(is_opp_strat=False)
+ strategy.blackboard.game = make_referee_game(RefereeCommand.NORMAL_START, timestamp=0.0)
+ strategy.blackboard.cmd_map = {}
+
+ strategy.behaviour_tree.tick()
+ strategy.blackboard.game.referee = make_referee_data(RefereeCommand.FORCE_START, timestamp=1.0)
+ strategy.behaviour_tree.tick()
+
+ assert strategy.recorder.initialise_count == 1
+ assert strategy.recorder.invalidated_count == 0
+
+
+@pytest.mark.parametrize("command", sorted(_REFEREE_STOPPAGE_COMMANDS, key=lambda item: item.value))
+def test_reset_guard_invalidates_for_every_stoppage_command(command):
+ target = RunningRecorder(name="Target")
+ target.status = py_trees.common.Status.RUNNING
+ guard = _ResetStrategyOnRefereeStoppage(target=target)
+ guard.blackboard = SimpleNamespace(game=make_referee_game(command, timestamp=1.0))
+
+ assert guard.update() == py_trees.common.Status.SUCCESS
+ assert target.invalidated_count == 1
+
+
+def test_reset_guard_ignores_active_start_commands():
+ target = RunningRecorder(name="Target")
+ target.status = py_trees.common.Status.RUNNING
+ guard = _ResetStrategyOnRefereeStoppage(target=target)
+ guard.blackboard = SimpleNamespace(game=make_referee_game(RefereeCommand.NORMAL_START, timestamp=1.0))
+
+ assert guard.update() == py_trees.common.Status.SUCCESS
+ assert target.invalidated_count == 0
+
+
+class ResetRecorderStrategy(AbstractStrategy):
+ exp_ball = True
+
+ def __init__(self):
+ self.recorder = RunningRecorder(name="Recorder")
+ super().__init__()
+
+ def create_behaviour_tree(self):
+ return self.recorder
+
+ def assert_exp_robots(self, n_runtime_friendly: int, n_runtime_enemy: int):
+ return True
+
+ def assert_exp_goals(self, includes_my_goal_line: bool, includes_opp_goal_line: bool):
+ return True
+
+ def get_min_bounding_req(self):
+ return None
+
+
+class RunningRecorder(py_trees.behaviour.Behaviour):
+ def __init__(self, name):
+ super().__init__(name=name)
+ self.initialise_count = 0
+ self.invalidated_count = 0
+
+ def initialise(self):
+ self.initialise_count += 1
+
+ def update(self):
+ return py_trees.common.Status.RUNNING
+
+ def terminate(self, new_status):
+ if new_status == py_trees.common.Status.INVALID:
+ self.invalidated_count += 1
+
+
+def make_referee_game(command, timestamp):
+ return SimpleNamespace(
+ referee=make_referee_data(command, timestamp),
+ friendly_robots={},
+ my_team_is_yellow=True,
+ my_team_is_right=False,
+ )
+
+
+def make_referee_data(command, timestamp):
+ return SimpleNamespace(
+ referee_command=command,
+ referee_command_timestamp=timestamp,
+ )
diff --git a/utama_core/tests/common/test_blackboard_contract.py b/utama_core/tests/common/test_blackboard_contract.py
new file mode 100644
index 00000000..8eebc0f7
--- /dev/null
+++ b/utama_core/tests/common/test_blackboard_contract.py
@@ -0,0 +1,123 @@
+from __future__ import annotations
+
+import py_trees
+import pytest
+from py_trees.blackboard import Blackboard
+
+from utama_core.strategy.common import AbstractBehaviour
+from utama_core.strategy.common.blackboard_contract import (
+ BlackboardKeySpec,
+ declared_contract,
+ register_blackboard_contract,
+ validate_blackboard_contracts,
+)
+
+
+class ContractOnlyBehaviour(AbstractBehaviour):
+ BLACKBOARD_CONTRACT = (
+ BlackboardKeySpec(
+ name="robot_id",
+ key_attr="robot_id_key",
+ access="READ",
+ type_name="int",
+ ),
+ BlackboardKeySpec(
+ name="target_orientation",
+ key_attr="target_orientation_key",
+ access=py_trees.common.Access.WRITE,
+ type_name="float",
+ ),
+ BlackboardKeySpec(
+ name="optional_target",
+ key_attr="optional_target_key",
+ access="READ",
+ required=False,
+ ),
+ )
+
+ def __init__(self):
+ super().__init__(name="ContractOnly")
+ self.robot_id_key = "robot_id"
+ self.target_orientation_key = "target_orientation"
+ self.optional_target_key = None
+
+ def setup_(self):
+ pass
+
+ def update(self):
+ return py_trees.common.Status.SUCCESS
+
+
+def test_runtime_key_resolves_direct_key_key_attr_optional_and_class_level_docs():
+ node = ContractOnlyBehaviour()
+
+ assert BlackboardKeySpec(name="direct", key="actual", access="READ").runtime_key(node) == "actual"
+ assert BlackboardKeySpec(name="robot_id", key_attr="robot_id_key", access="READ").runtime_key(node) == "robot_id"
+ assert (
+ BlackboardKeySpec(name="optional", key_attr="optional_target_key", access="READ", required=False).runtime_key(
+ node
+ )
+ is None
+ )
+ assert BlackboardKeySpec(name="robot_id", key_attr="robot_id_key", access="READ").runtime_key() == "robot_id"
+
+
+def test_declared_contract_is_doc_friendly_without_instance():
+ contracts = declared_contract(ContractOnlyBehaviour)
+
+ assert contracts[0]["key"] == "robot_id"
+ assert contracts[0]["access"] == "READ"
+ assert contracts[1]["access"] == "WRITE"
+
+
+def test_setup_auto_registers_contract_keys():
+ Blackboard.clear()
+ node = ContractOnlyBehaviour()
+
+ node.setup(is_opp_strat=False)
+
+ assert "/My/robot_id" in node.blackboard.read
+ assert "/My/target_orientation" in node.blackboard.write
+ assert "/My/optional_target" not in node.blackboard.read
+ Blackboard.clear()
+
+
+def test_register_blackboard_contract_rejects_missing_required_attr():
+ Blackboard.clear()
+ node = ContractOnlyBehaviour()
+ node.BLACKBOARD_CONTRACT = (BlackboardKeySpec(name="missing", key_attr="missing_key", access="READ"),)
+ node.blackboard = py_trees.blackboard.Client(name="Test", namespace="/My")
+
+ with pytest.raises(ValueError, match="missing attribute 'missing_key'"):
+ register_blackboard_contract(node)
+ Blackboard.clear()
+
+
+def test_register_blackboard_contract_rejects_invalid_access():
+ Blackboard.clear()
+ node = ContractOnlyBehaviour()
+ node.BLACKBOARD_CONTRACT = (BlackboardKeySpec(name="robot_id", key_attr="robot_id_key", access="MUTATE"),)
+ node.blackboard = py_trees.blackboard.Client(name="Test", namespace="/My")
+
+ with pytest.raises(ValueError, match="invalid access 'MUTATE'"):
+ register_blackboard_contract(node)
+ Blackboard.clear()
+
+
+def test_validate_blackboard_contracts_reports_missing_read_writer():
+ node = ContractOnlyBehaviour()
+
+ validation = validate_blackboard_contracts(node)
+
+ assert validation["status"] == "invalid"
+ assert {error["code"] for error in validation["errors"]} == {"missing_writer"}
+ assert {error["key"] for error in validation["errors"]} == {"robot_id"}
+
+
+def test_validate_blackboard_contracts_accepts_seeded_inputs():
+ node = ContractOnlyBehaviour()
+
+ validation = validate_blackboard_contracts(node, seeded_keys={"robot_id"})
+
+ assert validation["status"] == "valid"
+ assert validation["errors"] == []
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
index b9e2ef88..707e9931 100644
--- a/utama_core/tests/refiners/robot_info_test.py
+++ b/utama_core/tests/refiners/robot_info_test.py
@@ -155,3 +155,27 @@ def test_mixed_team_trusted_uses_ir_untrusted_uses_vision():
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/skills/test_defend_parameter.py b/utama_core/tests/skills/test_defend_parameter.py
new file mode 100644
index 00000000..635f925b
--- /dev/null
+++ b/utama_core/tests/skills/test_defend_parameter.py
@@ -0,0 +1,464 @@
+from types import SimpleNamespace
+
+import numpy as np
+import pytest
+
+import utama_core.skills.src.defend_parameter as dp
+from utama_core.config.physical_constants import BALL_RADIUS, ROBOT_RADIUS
+from utama_core.entities.data.vector import Vector2D, Vector3D
+
+EDGE_OFFSET = BALL_RADIUS + ROBOT_RADIUS
+
+# ---------------------------------------------------------------------------
+# Standard-field stubs (9x6 field)
+# goal_x = +/-4.5, goal_half_width = 0.5
+# defense front at +/-3.5, defense_half_width = 1.0
+# defender_x (left) = -3.5 + ROBOT_RADIUS, defender_x (right) = 3.5 - ROBOT_RADIUS
+# keeper_x (left) = -4.5 + ROBOT_RADIUS, keeper_x (right) = 4.5 - ROBOT_RADIUS
+# post_limit = 0.5 - ROBOT_RADIUS
+# ---------------------------------------------------------------------------
+_STD_LEFT_GOAL_LINE = np.array([(-4.5, 0.5), (-4.5, -0.5)])
+_STD_RIGHT_GOAL_LINE = np.array([(4.5, 0.5), (4.5, -0.5)])
+_STD_LEFT_DEFENSE_AREA = np.array(
+ [
+ (-4.5, 1.0),
+ (-3.5, 1.0),
+ (-3.5, -1.0),
+ (-4.5, -1.0),
+ (-4.5, 1.0),
+ ]
+)
+_STD_RIGHT_DEFENSE_AREA = np.array(
+ [
+ (4.5, 1.0),
+ (3.5, 1.0),
+ (3.5, -1.0),
+ (4.5, -1.0),
+ (4.5, 1.0),
+ ]
+)
+
+# Defender stands one ROBOT_RADIUS in front of the defense area
+_LEFT_DEFENDER_X = -3.5 + ROBOT_RADIUS
+_RIGHT_DEFENDER_X = 3.5 - ROBOT_RADIUS
+
+# Keeper-line references
+_LEFT_KEEPER_X = -4.5 + ROBOT_RADIUS
+_RIGHT_KEEPER_X = 4.5 - ROBOT_RADIUS
+_STD_POST_LIMIT = 0.5 - ROBOT_RADIUS
+
+
+def _std_field(my_team_is_right: bool):
+ if my_team_is_right:
+ return SimpleNamespace(
+ my_goal_line=_STD_RIGHT_GOAL_LINE,
+ my_defense_area=_STD_RIGHT_DEFENSE_AREA,
+ half_goal_width=0.5,
+ )
+ return SimpleNamespace(
+ my_goal_line=_STD_LEFT_GOAL_LINE,
+ my_defense_area=_STD_LEFT_DEFENSE_AREA,
+ half_goal_width=0.5,
+ )
+
+
+def _custom_field(goal_x, goal_half_width, defense_front_x, defense_half_width):
+ """Create a field stub with arbitrary goal line and defense area."""
+ return SimpleNamespace(
+ my_goal_line=np.array(
+ [
+ (goal_x, goal_half_width),
+ (goal_x, -goal_half_width),
+ ]
+ ),
+ my_defense_area=np.array(
+ [
+ (goal_x, defense_half_width),
+ (defense_front_x, defense_half_width),
+ (defense_front_x, -defense_half_width),
+ (goal_x, -defense_half_width),
+ (goal_x, defense_half_width),
+ ]
+ ),
+ half_goal_width=goal_half_width,
+ )
+
+
+def _make_game(team_is_right, field, friendly_robots, ball_pos):
+ return SimpleNamespace(
+ my_team_is_right=team_is_right,
+ field=field,
+ friendly_robots=friendly_robots,
+ ball=SimpleNamespace(
+ p=Vector3D(ball_pos[0], ball_pos[1], 0.0),
+ v=Vector3D(0.0, 0.0, 0.0),
+ ),
+ )
+
+
+def _capture_go_to_point(monkeypatch):
+ captured = {}
+
+ def fake(game, motion_controller, robot_id, target, dribbling=False):
+ captured["target"] = target
+ captured["dribbling"] = dribbling
+ captured["robot_id"] = robot_id
+ return "sentinel-command"
+
+ monkeypatch.setattr(dp, "go_to_point", fake)
+ return captured
+
+
+# ---------------------------------------------------------------------------
+# Core geometry: defender on ball-to-post line using keeper-x reference
+# ---------------------------------------------------------------------------
+
+
+def test_defender_on_ball_to_post_line_left_team(monkeypatch):
+ """Left team, robot 1 covers top post. Ball at origin."""
+ game = _make_game(
+ team_is_right=False,
+ field=_std_field(False),
+ friendly_robots={1: SimpleNamespace(p=Vector2D(-3.0, 0.0))},
+ ball_pos=(0.0, 0.0),
+ )
+ captured = _capture_go_to_point(monkeypatch)
+
+ dp.defend_parameter(game, motion_controller=object(), robot_id=1)
+
+ # goal_point = (keeper_x, post_limit + EDGE_OFFSET)
+ # t = (defender_x - 0) / (keeper_x - 0)
+ t = _LEFT_DEFENDER_X / _LEFT_KEEPER_X
+ expected_y = t * (_STD_POST_LIMIT + EDGE_OFFSET)
+ assert captured["target"].x == pytest.approx(_LEFT_DEFENDER_X)
+ assert captured["target"].y == pytest.approx(expected_y)
+ assert captured["dribbling"] is True
+
+
+def test_defender_on_ball_to_post_line_right_team(monkeypatch):
+ """Right team, robot 1 covers top post. Ball at origin."""
+ game = _make_game(
+ team_is_right=True,
+ field=_std_field(True),
+ friendly_robots={1: SimpleNamespace(p=Vector2D(3.0, 0.0))},
+ ball_pos=(0.0, 0.0),
+ )
+ captured = _capture_go_to_point(monkeypatch)
+
+ dp.defend_parameter(game, motion_controller=object(), robot_id=1)
+
+ # goal_point = (keeper_x, post_limit + EDGE_OFFSET)
+ t = _RIGHT_DEFENDER_X / _RIGHT_KEEPER_X
+ expected_y = t * (_STD_POST_LIMIT + EDGE_OFFSET)
+ assert captured["target"].x == pytest.approx(_RIGHT_DEFENDER_X)
+ assert captured["target"].y == pytest.approx(expected_y)
+
+
+def test_defender_robot2_covers_bottom_post(monkeypatch):
+ """Robot 2 defaults to the bottom post (-post_limit)."""
+ game = _make_game(
+ team_is_right=False,
+ field=_std_field(False),
+ friendly_robots={2: SimpleNamespace(p=Vector2D(-3.0, 0.0))},
+ ball_pos=(0.0, 0.0),
+ )
+ captured = _capture_go_to_point(monkeypatch)
+
+ dp.defend_parameter(game, motion_controller=object(), robot_id=2)
+
+ t = _LEFT_DEFENDER_X / _LEFT_KEEPER_X
+ expected_y = t * (-_STD_POST_LIMIT - EDGE_OFFSET)
+ assert captured["target"].x == pytest.approx(_LEFT_DEFENDER_X)
+ assert captured["target"].y == pytest.approx(expected_y)
+
+
+# ---------------------------------------------------------------------------
+# goal_frame_y=0.0 targets centre of goal (explicit override still works)
+# ---------------------------------------------------------------------------
+
+
+def test_goal_frame_y_zero_targets_centre(monkeypatch):
+ game = _make_game(
+ team_is_right=False,
+ field=_std_field(False),
+ friendly_robots={1: SimpleNamespace(p=Vector2D(-3.0, 0.0))},
+ ball_pos=(0.0, 0.5),
+ )
+ captured = _capture_go_to_point(monkeypatch)
+
+ dp.defend_parameter(game, motion_controller=object(), robot_id=1, goal_frame_y=0.0)
+
+ # goal_point = (keeper_x, EDGE_OFFSET)
+ t = _LEFT_DEFENDER_X / _LEFT_KEEPER_X
+ expected_y = 0.5 + t * (EDGE_OFFSET - 0.5)
+ assert captured["target"].x == pytest.approx(_LEFT_DEFENDER_X)
+ assert captured["target"].y == pytest.approx(expected_y)
+ assert captured["dribbling"] is True
+
+
+# ---------------------------------------------------------------------------
+# Multi-robot: both defenders dynamically track (no static positions)
+# ---------------------------------------------------------------------------
+
+
+def test_three_robots_both_defenders_dynamic(monkeypatch):
+ """With 3+ robots, both defenders should track the ball-to-post line."""
+ field = _std_field(False)
+ robots = {
+ 0: SimpleNamespace(p=Vector2D(-4.5, 0.0)),
+ 1: SimpleNamespace(p=Vector2D(-3.0, 0.3)),
+ 2: SimpleNamespace(p=Vector2D(-3.0, -0.3)),
+ }
+ captured_calls = []
+
+ def fake(game, motion_controller, robot_id, target, dribbling=False):
+ captured_calls.append({"robot_id": robot_id, "target": target})
+ return "sentinel-command"
+
+ monkeypatch.setattr(dp, "go_to_point", fake)
+
+ game1 = _make_game(False, field, robots, (0.0, 0.0))
+ dp.defend_parameter(game1, motion_controller=object(), robot_id=1)
+
+ game2 = _make_game(False, field, robots, (0.0, 0.0))
+ dp.defend_parameter(game2, motion_controller=object(), robot_id=2)
+
+ assert len(captured_calls) == 2
+ # Robot 1 covers top post -> positive y
+ assert captured_calls[0]["target"].y > 0
+ # Robot 2 covers bottom post -> negative y
+ assert captured_calls[1]["target"].y < 0
+ # Both just in front of defense area
+ assert captured_calls[0]["target"].x == pytest.approx(_LEFT_DEFENDER_X)
+ assert captured_calls[1]["target"].x == pytest.approx(_LEFT_DEFENDER_X)
+
+
+# ---------------------------------------------------------------------------
+# Y clamped to defense_half_width
+# ---------------------------------------------------------------------------
+
+
+def test_target_y_clamped_to_defense_half_width(monkeypatch):
+ """When ball-to-post line exits defense area vertically, y is clamped."""
+ game = _make_game(
+ team_is_right=False,
+ field=_std_field(False),
+ friendly_robots={1: SimpleNamespace(p=Vector2D(-3.0, 0.0))},
+ ball_pos=(0.0, 3.0),
+ )
+ captured = _capture_go_to_point(monkeypatch)
+
+ dp.defend_parameter(game, motion_controller=object(), robot_id=1)
+
+ assert captured["target"].y == pytest.approx(1.0)
+ assert captured["target"].x == pytest.approx(_LEFT_DEFENDER_X)
+
+
+# ---------------------------------------------------------------------------
+# Custom geometry
+# ---------------------------------------------------------------------------
+
+
+def test_custom_geometry_uses_field_values(monkeypatch):
+ """Defender positioning adapts to non-standard field dimensions."""
+ field = _custom_field(-6.0, 0.8, -4.5, 1.5)
+ keeper_x = -6.0 + ROBOT_RADIUS
+ post_limit = 0.8 - ROBOT_RADIUS
+ defender_x = -4.5 + ROBOT_RADIUS # left team
+ game = _make_game(
+ team_is_right=False,
+ field=field,
+ friendly_robots={1: SimpleNamespace(p=Vector2D(-4.0, 0.0))},
+ ball_pos=(0.0, 0.0),
+ )
+ captured = _capture_go_to_point(monkeypatch)
+
+ dp.defend_parameter(game, motion_controller=object(), robot_id=1)
+
+ # goal_point = (keeper_x, post_limit + EDGE_OFFSET)
+ t = defender_x / keeper_x
+ expected_y = t * (post_limit + EDGE_OFFSET)
+ assert captured["target"].x == pytest.approx(defender_x)
+ assert captured["target"].y == pytest.approx(expected_y)
+
+
+def test_custom_geometry_default_goal_frame_y(monkeypatch):
+ """Default goal_frame_y uses post_limit from field, not hardcoded 0.5."""
+ field = _custom_field(-4.5, 0.8, -3.5, 1.0)
+ post_limit = 0.8 - ROBOT_RADIUS
+ game = _make_game(
+ team_is_right=False,
+ field=field,
+ friendly_robots={1: SimpleNamespace(p=Vector2D(-3.0, 0.0))},
+ ball_pos=(0.0, 0.0),
+ )
+ captured = _capture_go_to_point(monkeypatch)
+
+ dp.defend_parameter(game, motion_controller=object(), robot_id=1)
+
+ # goal_frame_y = post_limit (not 0.5 or 0.8)
+ # goal_point = (keeper_x, post_limit + EDGE_OFFSET)
+ keeper_x = -4.5 + ROBOT_RADIUS
+ t = _LEFT_DEFENDER_X / keeper_x
+ expected_y = t * (post_limit + EDGE_OFFSET)
+ assert captured["target"].y == pytest.approx(expected_y)
+
+
+def test_custom_geometry_y_clamped_to_custom_defense_half_width(monkeypatch):
+ """Y clamping uses custom defense_half_width, not hardcoded 1.0."""
+ field = _custom_field(-4.5, 0.5, -3.5, 0.3)
+ game = _make_game(
+ team_is_right=False,
+ field=field,
+ friendly_robots={1: SimpleNamespace(p=Vector2D(-3.0, 0.0))},
+ ball_pos=(0.0, 0.0),
+ )
+ captured = _capture_go_to_point(monkeypatch)
+
+ dp.defend_parameter(game, motion_controller=object(), robot_id=1)
+
+ # Unclamped y > 0.3 -> clamped
+ assert captured["target"].y == pytest.approx(0.3)
+
+
+# ---------------------------------------------------------------------------
+# Radius-aware target geometry
+# ---------------------------------------------------------------------------
+
+
+def test_shadow_target_uses_keeper_x_not_goal_x(monkeypatch):
+ """The shadow target x-coordinate is keeper_x, not the raw goal line."""
+ game = _make_game(
+ team_is_right=False,
+ field=_std_field(False),
+ friendly_robots={1: SimpleNamespace(p=Vector2D(-3.0, 0.0))},
+ ball_pos=(0.0, 0.0),
+ )
+ captured = _capture_go_to_point(monkeypatch)
+
+ dp.defend_parameter(game, motion_controller=object(), robot_id=1, goal_frame_y=0.3)
+
+ # If using keeper_x (-4.41) vs goal_x (-4.5), the t parameter differs.
+ # Verify by checking y matches the keeper_x-based formula:
+ t = _LEFT_DEFENDER_X / _LEFT_KEEPER_X
+ expected_y = t * (0.3 + EDGE_OFFSET)
+ assert captured["target"].y == pytest.approx(expected_y)
+
+
+# ---------------------------------------------------------------------------
+# Single-defender dynamic side choice (2-robot case)
+# ---------------------------------------------------------------------------
+
+
+def test_single_defender_picks_side_for_off_centre_ball(monkeypatch):
+ """When ball is clearly off-centre, the defender should pick the correct side."""
+ # Ball far to the positive-y side: defender should cover +post_limit
+ # so that the keeper doesn't have to travel far to cover the near post
+ game = _make_game(
+ team_is_right=False,
+ field=_std_field(False),
+ friendly_robots={
+ 0: SimpleNamespace(p=Vector2D(-4.41, 0.0)), # goalkeeper
+ 1: SimpleNamespace(p=Vector2D(-3.0, 0.0)), # defender
+ },
+ ball_pos=(0.0, 2.0),
+ )
+ captured = _capture_go_to_point(monkeypatch)
+ monkeypatch.setattr(dp, "predict_ball_pos_at_x", lambda game, x: None)
+
+ dp.defend_parameter(game, motion_controller=object(), robot_id=1)
+
+ # Defender should be shifted toward the ball side (positive y)
+ assert captured["target"].y > 0
+
+
+def test_single_defender_picks_opposite_side_for_negative_ball(monkeypatch):
+ """Mirror: ball far negative-y, defender should cover -post_limit."""
+ game = _make_game(
+ team_is_right=False,
+ field=_std_field(False),
+ friendly_robots={
+ 0: SimpleNamespace(p=Vector2D(-4.41, 0.0)),
+ 1: SimpleNamespace(p=Vector2D(-3.0, 0.0)),
+ },
+ ball_pos=(0.0, -2.0),
+ )
+ captured = _capture_go_to_point(monkeypatch)
+ monkeypatch.setattr(dp, "predict_ball_pos_at_x", lambda game, x: None)
+
+ dp.defend_parameter(game, motion_controller=object(), robot_id=1)
+
+ assert captured["target"].y < 0
+
+
+def test_single_defender_centred_ball_uses_keeper_ref_tiebreak(monkeypatch):
+ """With a centred ball, side choice considers keeper reference position."""
+ game = _make_game(
+ team_is_right=False,
+ field=_std_field(False),
+ friendly_robots={
+ 0: SimpleNamespace(p=Vector2D(-4.41, 0.2)), # keeper slightly positive
+ 1: SimpleNamespace(p=Vector2D(-3.0, 0.0)),
+ },
+ ball_pos=(0.0, 0.0),
+ )
+ captured_a = _capture_go_to_point(monkeypatch)
+ monkeypatch.setattr(dp, "predict_ball_pos_at_x", lambda game, x: None)
+
+ dp.defend_parameter(game, motion_controller=object(), robot_id=1)
+ y_a = captured_a["target"].y
+
+ # Now mirror: keeper slightly negative
+ game2 = _make_game(
+ team_is_right=False,
+ field=_std_field(False),
+ friendly_robots={
+ 0: SimpleNamespace(p=Vector2D(-4.41, -0.2)),
+ 1: SimpleNamespace(p=Vector2D(-3.0, 0.0)),
+ },
+ ball_pos=(0.0, 0.0),
+ )
+ captured_b = _capture_go_to_point(monkeypatch)
+
+ dp.defend_parameter(game2, motion_controller=object(), robot_id=1)
+ y_b = captured_b["target"].y
+
+ # The two should mirror: keeper-positive -> defender-negative and vice versa
+ assert y_a * y_b < 0 # opposite signs
+
+
+def test_single_defender_uses_predicted_intercept_for_keeper_ref(monkeypatch):
+ """When a ball prediction is available, it is used as the keeper reference."""
+ game = _make_game(
+ team_is_right=False,
+ field=_std_field(False),
+ friendly_robots={
+ 0: SimpleNamespace(p=Vector2D(-4.41, 0.0)), # keeper at centre
+ 1: SimpleNamespace(p=Vector2D(-3.0, 0.0)),
+ },
+ ball_pos=(0.0, 0.0),
+ )
+ # Predict ball heading to positive y
+ monkeypatch.setattr(
+ dp,
+ "predict_ball_pos_at_x",
+ lambda game, x: Vector2D(x, 0.3),
+ )
+ captured = _capture_go_to_point(monkeypatch)
+
+ dp.defend_parameter(game, motion_controller=object(), robot_id=1)
+ y_with_positive_prediction = captured["target"].y
+
+ # Predict ball heading to negative y
+ monkeypatch.setattr(
+ dp,
+ "predict_ball_pos_at_x",
+ lambda game, x: Vector2D(x, -0.3),
+ )
+ captured2 = _capture_go_to_point(monkeypatch)
+
+ dp.defend_parameter(game, motion_controller=object(), robot_id=1)
+ y_with_negative_prediction = captured2["target"].y
+
+ # Different predictions should produce different (mirrored) side choices
+ assert y_with_positive_prediction * y_with_negative_prediction < 0
diff --git a/utama_core/tests/skills/test_defense_utils.py b/utama_core/tests/skills/test_defense_utils.py
new file mode 100644
index 00000000..77d207bc
--- /dev/null
+++ b/utama_core/tests/skills/test_defense_utils.py
@@ -0,0 +1,174 @@
+from types import SimpleNamespace
+
+import numpy as np
+import pytest
+
+from utama_core.skills.src.utils.defense_utils import (
+ DefenseGeometry,
+ _defense_geometry,
+ calculate_defense_area,
+ clamp_to_goal_width,
+)
+
+
+# ---------------------------------------------------------------------------
+# Field stubs
+# ---------------------------------------------------------------------------
+def _make_field(goal_x, goal_half_width, defense_front_x, defense_half_width, my_team_is_right):
+ return SimpleNamespace(
+ my_goal_line=np.array(
+ [
+ (goal_x, goal_half_width),
+ (goal_x, -goal_half_width),
+ ]
+ ),
+ my_defense_area=np.array(
+ [
+ (goal_x, defense_half_width),
+ (defense_front_x, defense_half_width),
+ (defense_front_x, -defense_half_width),
+ (goal_x, -defense_half_width),
+ (goal_x, defense_half_width),
+ ]
+ ),
+ )
+
+
+def _make_game(my_team_is_right, field):
+ return SimpleNamespace(my_team_is_right=my_team_is_right, field=field)
+
+
+# ---------------------------------------------------------------------------
+# _defense_geometry – standard fields
+# ---------------------------------------------------------------------------
+
+
+def test_defense_geometry_left_goal():
+ field = _make_field(-4.5, 0.5, -3.5, 1.0, False)
+ game = _make_game(False, field)
+ geo = _defense_geometry(game)
+
+ assert geo.goal_x == pytest.approx(-4.5)
+ assert geo.goal_half_width == pytest.approx(0.5)
+ assert geo.defense_front_x == pytest.approx(-3.5)
+ assert geo.defense_depth == pytest.approx(1.0)
+ assert geo.defense_half_width == pytest.approx(1.0)
+
+
+def test_defense_geometry_right_goal():
+ field = _make_field(4.5, 0.5, 3.5, 1.0, True)
+ game = _make_game(True, field)
+ geo = _defense_geometry(game)
+
+ assert geo.goal_x == pytest.approx(4.5)
+ assert geo.goal_half_width == pytest.approx(0.5)
+ assert geo.defense_front_x == pytest.approx(3.5)
+ assert geo.defense_depth == pytest.approx(1.0)
+ assert geo.defense_half_width == pytest.approx(1.0)
+
+
+def test_defense_geometry_custom():
+ field = _make_field(-6.0, 0.8, -4.5, 1.5, False)
+ game = _make_game(False, field)
+ geo = _defense_geometry(game)
+
+ assert geo.goal_x == pytest.approx(-6.0)
+ assert geo.goal_half_width == pytest.approx(0.8)
+ assert geo.defense_front_x == pytest.approx(-4.5)
+ assert geo.defense_depth == pytest.approx(1.5)
+ assert geo.defense_half_width == pytest.approx(1.5)
+
+
+# ---------------------------------------------------------------------------
+# clamp_to_goal_width
+# ---------------------------------------------------------------------------
+
+
+@pytest.mark.parametrize(
+ ("y", "half_width", "expected"),
+ [
+ (0.3, 0.5, 0.3),
+ (0.7, 0.5, 0.5),
+ (-0.9, 0.5, -0.5),
+ (0.3, 0.8, 0.3),
+ (0.9, 0.8, 0.8),
+ (-1.0, 0.8, -0.8),
+ ],
+)
+def test_clamp_to_goal_width(y, half_width, expected):
+ assert clamp_to_goal_width(y, half_width) == pytest.approx(expected)
+
+
+# ---------------------------------------------------------------------------
+# calculate_defense_area – standard field
+# ---------------------------------------------------------------------------
+
+
+def test_calculate_defense_area_standard_field_left_frontmost():
+ """At t=pi (front of curve), the x should be goal_x - (defense_depth + 0.1)."""
+ field = _make_field(-4.5, 0.5, -3.5, 1.0, False)
+ game = _make_game(False, field)
+ pos = calculate_defense_area(game, np.pi)
+
+ # Front extent = 1.0 + 0.1 = 1.1, r=2.1
+ # At t=pi: cos=-1, sin=0 → x_raw = 1.1 * ((1-2.1)*1*(-1) + 2.1*(-1)) = 1.1 * (1.1 - 2.1) = 1.1 * (-1) = -1.1
+ # Left goal: goal_x - x_raw = -4.5 - (-1.1) = -3.4
+ assert pos.x == pytest.approx(-3.4)
+ assert pos.y == pytest.approx(0.0)
+
+
+def test_calculate_defense_area_standard_field_right_frontmost():
+ """At t=pi (front of curve), for right goal."""
+ field = _make_field(4.5, 0.5, 3.5, 1.0, True)
+ game = _make_game(True, field)
+ pos = calculate_defense_area(game, np.pi)
+
+ # Right goal: goal_x + x_raw = 4.5 + (-1.1) = 3.4
+ assert pos.x == pytest.approx(3.4)
+ assert pos.y == pytest.approx(0.0)
+
+
+def test_calculate_defense_area_standard_field_top_bottom():
+ """At t=pi/2 and t=3pi/2, the y extents should be ±(defense_half_width + 0.1)."""
+ field = _make_field(-4.5, 0.5, -3.5, 1.0, False)
+ game = _make_game(False, field)
+
+ top = calculate_defense_area(game, np.pi / 2)
+ bottom = calculate_defense_area(game, 3 * np.pi / 2)
+
+ # At t=pi/2: cos=0, sin=1 → y_raw = 1.1 * ((1-2.1)*1*1 + 2.1*1) = 1.1 * 1 = 1.1
+ assert top.y == pytest.approx(1.1)
+ assert bottom.y == pytest.approx(-1.1)
+ # x at top/bottom should be at goal line
+ assert top.x == pytest.approx(-4.5)
+ assert bottom.x == pytest.approx(-4.5)
+
+
+# ---------------------------------------------------------------------------
+# calculate_defense_area – custom geometry
+# ---------------------------------------------------------------------------
+
+
+def test_calculate_defense_area_custom_geometry_front_extent():
+ """Custom defense_depth=2.0 should produce front_extent=2.1."""
+ field = _make_field(-6.0, 0.8, -4.0, 1.5, False)
+ game = _make_game(False, field)
+ pos = calculate_defense_area(game, np.pi)
+
+ # defense_depth = 2.0, front_extent = 2.1
+ # x_raw at t=pi: 2.1 * (-1) = -2.1
+ # left goal: -6.0 - (-2.1) = -3.9
+ assert pos.x == pytest.approx(-3.9)
+ assert pos.y == pytest.approx(0.0)
+
+
+def test_calculate_defense_area_custom_geometry_vertical_extent():
+ """Custom defense_half_width=1.5 should produce vertical_extent=1.6."""
+ field = _make_field(-6.0, 0.8, -4.0, 1.5, False)
+ game = _make_game(False, field)
+
+ top = calculate_defense_area(game, np.pi / 2)
+
+ # vertical_extent = 1.5 + 0.1 = 1.6
+ assert top.y == pytest.approx(1.6)
+ assert top.x == pytest.approx(-6.0)
diff --git a/utama_core/tests/skills/test_goalkeep.py b/utama_core/tests/skills/test_goalkeep.py
new file mode 100644
index 00000000..37ef3f38
--- /dev/null
+++ b/utama_core/tests/skills/test_goalkeep.py
@@ -0,0 +1,430 @@
+from types import SimpleNamespace
+
+import numpy as np
+import pytest
+
+import utama_core.skills.src.goalkeep as gk
+from utama_core.config.physical_constants import BALL_RADIUS, ROBOT_RADIUS
+from utama_core.entities.data.vector import Vector2D, Vector3D
+from utama_core.skills.src.utils.defense_utils import (
+ intersection_with_x_line,
+ single_defender_stop_y,
+)
+
+EDGE_OFFSET = BALL_RADIUS + ROBOT_RADIUS
+
+# ---------------------------------------------------------------------------
+# Standard-field stub (9x6 field: goal_x = +/-4.5, goal_half_width = 0.5)
+# ---------------------------------------------------------------------------
+_STD_LEFT_GOAL_LINE = np.array([(-4.5, 0.5), (-4.5, -0.5)])
+_STD_RIGHT_GOAL_LINE = np.array([(4.5, 0.5), (4.5, -0.5)])
+
+_LEFT_KEEPER_X = -4.5 + ROBOT_RADIUS
+_RIGHT_KEEPER_X = 4.5 - ROBOT_RADIUS
+_STD_POST_LIMIT = 0.5 - ROBOT_RADIUS
+
+
+def _std_field(my_team_is_right: bool):
+ goal_line = _STD_RIGHT_GOAL_LINE if my_team_is_right else _STD_LEFT_GOAL_LINE
+ return SimpleNamespace(my_goal_line=goal_line, half_goal_width=0.5)
+
+
+# ---------------------------------------------------------------------------
+# Shared helper unit tests (intersection_with_x_line)
+# ---------------------------------------------------------------------------
+
+
+def test_intersection_with_x_line_basic():
+ intersection = intersection_with_x_line(
+ (-1.0, 0.0),
+ (-3.0, 0.1),
+ -4.5,
+ 0.5,
+ )
+
+ assert intersection == pytest.approx((-4.5, 0.175))
+
+
+@pytest.mark.parametrize(
+ ("a", "b", "target_x", "expected"),
+ [
+ ((-3.0, 0.8), (-3.0, 0.9), -4.5, (-4.5, 0.5)),
+ ((-1.0, 0.2), (1.0, -0.4), -4.5, (-4.5, 0.2)),
+ ],
+)
+def test_intersection_with_x_line_handles_vertical_and_moving_away_cases(a, b, target_x, expected):
+ intersection = intersection_with_x_line(a, b, target_x, 0.5)
+
+ assert intersection == pytest.approx(expected)
+
+
+# ---------------------------------------------------------------------------
+# Shared helper unit tests (single_defender_stop_y)
+# ---------------------------------------------------------------------------
+
+
+@pytest.mark.parametrize(
+ ("ball_pos", "defender_pos", "keeper_x", "post_limit", "expected_sign"),
+ [
+ # Defender shifted positive-y relative to ball → keeper covers negative gap
+ (Vector2D(-1.0, -0.3), Vector2D(-3.0, 0.2), -4.5, 0.5, -1),
+ # Mirror: defender shifted negative-y → keeper covers positive gap
+ (Vector2D(-1.0, 0.3), Vector2D(-3.0, -0.2), -4.5, 0.5, 1),
+ ],
+)
+def test_single_defender_stop_y_targets_open_half(ball_pos, defender_pos, keeper_x, post_limit, expected_sign):
+ stop_y = single_defender_stop_y(ball_pos, defender_pos, keeper_x, post_limit, EDGE_OFFSET)
+
+ # The keeper should be on the opposite side of the defender's offset
+ assert stop_y * expected_sign > 0
+
+
+def test_single_defender_stop_y_with_custom_post_limit():
+ """Wider post_limit changes the midpoint used for shadow-based stop_y."""
+ ball_pos = Vector2D(-1.0, -0.3)
+ defender_pos = Vector2D(-3.0, 0.2)
+ keeper_x = -4.5
+ narrow = single_defender_stop_y(ball_pos, defender_pos, keeper_x, 0.5, EDGE_OFFSET)
+ wide = single_defender_stop_y(ball_pos, defender_pos, keeper_x, 1.0, EDGE_OFFSET)
+ # Wider post_limit means larger uncovered region → keeper shifts further
+ assert abs(wide) > abs(narrow)
+
+
+# ---------------------------------------------------------------------------
+# Goalkeep integration tests
+# ---------------------------------------------------------------------------
+
+
+def test_goalkeep_fallback_uses_side_aware_shadow_target(monkeypatch):
+ game = SimpleNamespace(
+ my_team_is_right=False,
+ field=_std_field(False),
+ friendly_robots={
+ 0: SimpleNamespace(p=Vector2D(-4.2, 0.0)),
+ 1: SimpleNamespace(p=Vector2D(-3.0, 0.2)),
+ },
+ ball=SimpleNamespace(
+ p=Vector3D(-1.0, -0.3, 0.0),
+ v=Vector3D(1.0, 0.0, 0.0),
+ ),
+ )
+
+ captured = {}
+
+ monkeypatch.setattr(gk, "predict_ball_pos_at_x", lambda game, x: None)
+
+ def fake_go_to_point(game, motion_controller, robot_id, target, dribbling=False):
+ captured["robot_id"] = robot_id
+ captured["target"] = target
+ captured["dribbling"] = dribbling
+ return "sentinel-command"
+
+ monkeypatch.setattr(gk, "go_to_point", fake_go_to_point)
+
+ result = gk.goalkeep(game, motion_controller=object(), robot_id=0)
+
+ assert result == "sentinel-command"
+ assert captured["robot_id"] == 0
+ assert captured["dribbling"] is True
+ assert captured["target"].x == pytest.approx(_LEFT_KEEPER_X)
+ # Keeper should be in the negative-y gap (defender is at y=0.2, ball at y=-0.3)
+ assert captured["target"].y < 0
+
+
+def test_goalkeep_uses_predicted_intercept_inside_goal(monkeypatch):
+ game = SimpleNamespace(
+ my_team_is_right=False,
+ field=_std_field(False),
+ friendly_robots={
+ 0: SimpleNamespace(p=Vector2D(-4.2, 0.0)),
+ 1: SimpleNamespace(p=Vector2D(-3.0, 0.2)),
+ },
+ ball=SimpleNamespace(
+ p=Vector3D(-1.0, 0.0, 0.0),
+ v=Vector3D(-1.0, 0.0, 0.0),
+ ),
+ )
+ captured = {}
+
+ monkeypatch.setattr(gk, "predict_ball_pos_at_x", lambda game, x: Vector2D(x, 0.2))
+
+ def fake_go_to_point(game, motion_controller, robot_id, target, dribbling=False):
+ captured["robot_id"] = robot_id
+ captured["target"] = target
+ captured["dribbling"] = dribbling
+ return "sentinel-command"
+
+ monkeypatch.setattr(gk, "go_to_point", fake_go_to_point)
+
+ result = gk.goalkeep(game, motion_controller=object(), robot_id=0)
+
+ assert result == "sentinel-command"
+ assert captured["robot_id"] == 0
+ assert captured["dribbling"] is True
+ assert captured["target"] == Vector2D(_LEFT_KEEPER_X, 0.2)
+
+
+def test_goalkeep_single_keeper_no_prediction_tracks_ball_y(monkeypatch):
+ game = SimpleNamespace(
+ my_team_is_right=False,
+ field=_std_field(False),
+ friendly_robots={
+ 0: SimpleNamespace(p=Vector2D(-4.2, 0.0)),
+ },
+ ball=SimpleNamespace(
+ p=Vector3D(-1.0, 0.2, 0.0),
+ v=Vector3D(1.0, 0.0, 0.0),
+ ),
+ )
+ captured = {}
+
+ monkeypatch.setattr(gk, "predict_ball_pos_at_x", lambda game, x: None)
+
+ def fake_go_to_point(game, motion_controller, robot_id, target, dribbling=False):
+ captured["target"] = target
+ return "sentinel-command"
+
+ monkeypatch.setattr(gk, "go_to_point", fake_go_to_point)
+
+ result = gk.goalkeep(game, motion_controller=object(), robot_id=0)
+
+ assert result == "sentinel-command"
+ assert captured["target"] == Vector2D(_LEFT_KEEPER_X, 0.2)
+
+
+def test_goalkeep_single_keeper_no_prediction_clamps_to_upper_post(monkeypatch):
+ game = SimpleNamespace(
+ my_team_is_right=False,
+ field=_custom_field(-1.5, 0.4),
+ friendly_robots={
+ 0: SimpleNamespace(p=Vector2D(-1.35, 0.0)),
+ },
+ ball=SimpleNamespace(
+ p=Vector3D(-0.679, 0.623, 0.0),
+ v=Vector3D(0.078, -2.35, 0.0),
+ ),
+ )
+ captured = {}
+ keeper_x = -1.5 + ROBOT_RADIUS
+ post_limit = 0.4 - ROBOT_RADIUS
+
+ monkeypatch.setattr(gk, "predict_ball_pos_at_x", lambda game, x: None)
+
+ def fake_go_to_point(game, motion_controller, robot_id, target, dribbling=False):
+ captured["target"] = target
+ return "sentinel-command"
+
+ monkeypatch.setattr(gk, "go_to_point", fake_go_to_point)
+
+ gk.goalkeep(game, motion_controller=object(), robot_id=0)
+
+ assert captured["target"].x == pytest.approx(keeper_x)
+ assert captured["target"].y == pytest.approx(post_limit)
+
+
+def test_goalkeep_single_keeper_no_prediction_clamps_to_lower_post(monkeypatch):
+ game = SimpleNamespace(
+ my_team_is_right=False,
+ field=_custom_field(-1.5, 0.4),
+ friendly_robots={
+ 0: SimpleNamespace(p=Vector2D(-1.35, 0.0)),
+ },
+ ball=SimpleNamespace(
+ p=Vector3D(-0.626, -0.495, 0.0),
+ v=Vector3D(0.002, 0.001, 0.0),
+ ),
+ )
+ captured = {}
+ keeper_x = -1.5 + ROBOT_RADIUS
+ post_limit = 0.4 - ROBOT_RADIUS
+
+ monkeypatch.setattr(gk, "predict_ball_pos_at_x", lambda game, x: None)
+
+ def fake_go_to_point(game, motion_controller, robot_id, target, dribbling=False):
+ captured["target"] = target
+ return "sentinel-command"
+
+ monkeypatch.setattr(gk, "go_to_point", fake_go_to_point)
+
+ gk.goalkeep(game, motion_controller=object(), robot_id=0)
+
+ assert captured["target"].x == pytest.approx(keeper_x)
+ assert captured["target"].y == pytest.approx(-post_limit)
+
+
+def test_goalkeep_three_robots_uses_midpoint_of_two_shadow_edges(monkeypatch):
+ game = SimpleNamespace(
+ my_team_is_right=False,
+ field=_std_field(False),
+ friendly_robots={
+ 0: SimpleNamespace(p=Vector2D(-4.2, 0.0)),
+ 1: SimpleNamespace(p=Vector2D(-3.0, 0.0)),
+ 2: SimpleNamespace(p=Vector2D(-3.0, 0.4)),
+ },
+ ball=SimpleNamespace(
+ p=Vector3D(-1.0, -0.2, 0.0),
+ v=Vector3D(1.0, 0.0, 0.0),
+ ),
+ )
+ captured = {}
+
+ monkeypatch.setattr(gk, "predict_ball_pos_at_x", lambda game, x: None)
+
+ def fake_go_to_point(game, motion_controller, robot_id, target, dribbling=False):
+ captured["target"] = target
+ return "sentinel-command"
+
+ monkeypatch.setattr(gk, "go_to_point", fake_go_to_point)
+
+ result = gk.goalkeep(game, motion_controller=object(), robot_id=0)
+
+ assert result == "sentinel-command"
+ assert captured["target"].x == pytest.approx(_LEFT_KEEPER_X)
+ # Compute expected: intersection of two shadow edges at keeper_x with post_limit clamp
+ _, yy1 = intersection_with_x_line(
+ (-1.0, -0.2),
+ (-3.0, 0.0 + EDGE_OFFSET),
+ _LEFT_KEEPER_X,
+ _STD_POST_LIMIT,
+ )
+ _, yy2 = intersection_with_x_line(
+ (-1.0, -0.2),
+ (-3.0, 0.4 - EDGE_OFFSET),
+ _LEFT_KEEPER_X,
+ _STD_POST_LIMIT,
+ )
+ assert captured["target"].y == pytest.approx((yy1 + yy2) / 2)
+
+
+def test_goalkeep_missing_expected_defender_id_falls_back_to_centre(monkeypatch):
+ game = SimpleNamespace(
+ my_team_is_right=False,
+ field=_std_field(False),
+ friendly_robots={
+ 0: SimpleNamespace(p=Vector2D(-4.2, 0.0)),
+ 2: SimpleNamespace(p=Vector2D(-3.0, 0.2)),
+ },
+ ball=SimpleNamespace(
+ p=Vector3D(-1.0, 0.0, 0.0),
+ v=Vector3D(1.0, 0.0, 0.0),
+ ),
+ )
+ captured = {}
+
+ monkeypatch.setattr(gk, "predict_ball_pos_at_x", lambda game, x: None)
+
+ def fake_go_to_point(game, motion_controller, robot_id, target, dribbling=False):
+ captured["target"] = target
+ return "sentinel-command"
+
+ monkeypatch.setattr(gk, "go_to_point", fake_go_to_point)
+
+ result = gk.goalkeep(game, motion_controller=object(), robot_id=0)
+
+ assert result == "sentinel-command"
+ assert captured["target"] == Vector2D(_LEFT_KEEPER_X, 0.0)
+
+
+# ---------------------------------------------------------------------------
+# Non-standard geometry tests
+# ---------------------------------------------------------------------------
+
+
+def _custom_field(goal_x: float, goal_half_width: float):
+ """Create a field stub with arbitrary goal line."""
+ return SimpleNamespace(
+ my_goal_line=np.array([(goal_x, goal_half_width), (goal_x, -goal_half_width)]),
+ half_goal_width=goal_half_width,
+ )
+
+
+def test_goalkeep_custom_goal_line_changes_intercept_x(monkeypatch):
+ """With a wider goal at x=-6.0, the keeper should target keeper_x = -6.0 + ROBOT_RADIUS."""
+ keeper_x = -6.0 + ROBOT_RADIUS
+ game = SimpleNamespace(
+ my_team_is_right=False,
+ field=_custom_field(-6.0, 0.8),
+ friendly_robots={
+ 0: SimpleNamespace(p=Vector2D(-5.5, 0.0)),
+ 1: SimpleNamespace(p=Vector2D(-4.0, 0.2)),
+ },
+ ball=SimpleNamespace(
+ p=Vector3D(-1.0, -0.3, 0.0),
+ v=Vector3D(1.0, 0.0, 0.0),
+ ),
+ )
+ captured = {}
+
+ monkeypatch.setattr(gk, "predict_ball_pos_at_x", lambda game, x: None)
+
+ def fake_go_to_point(game, motion_controller, robot_id, target, dribbling=False):
+ captured["target"] = target
+ return "sentinel-command"
+
+ monkeypatch.setattr(gk, "go_to_point", fake_go_to_point)
+
+ gk.goalkeep(game, motion_controller=object(), robot_id=0)
+
+ assert captured["target"].x == pytest.approx(keeper_x)
+
+
+def test_goalkeep_custom_goal_width_changes_clamp_range(monkeypatch):
+ """With goal_half_width=0.8, a prediction at y=0.7 should be kept (not clamped)."""
+ game = SimpleNamespace(
+ my_team_is_right=False,
+ field=_custom_field(-4.5, 0.8),
+ friendly_robots={
+ 0: SimpleNamespace(p=Vector2D(-4.2, 0.0)),
+ },
+ ball=SimpleNamespace(
+ p=Vector3D(-1.0, 0.0, 0.0),
+ v=Vector3D(-1.0, 0.0, 0.0),
+ ),
+ )
+ captured = {}
+ keeper_x = -4.5 + ROBOT_RADIUS
+
+ monkeypatch.setattr(gk, "predict_ball_pos_at_x", lambda game, x: Vector2D(x, 0.7))
+
+ def fake_go_to_point(game, motion_controller, robot_id, target, dribbling=False):
+ captured["target"] = target
+ return "sentinel-command"
+
+ monkeypatch.setattr(gk, "go_to_point", fake_go_to_point)
+
+ gk.goalkeep(game, motion_controller=object(), robot_id=0)
+
+ # 0.7 < 0.8 half-width, so prediction is used directly
+ assert captured["target"] == Vector2D(keeper_x, 0.7)
+
+
+def test_goalkeep_wide_shot_clamps_to_post_limit(monkeypatch):
+ """With goal_half_width=0.3, a prediction at y=0.4 is wide -> clamp to post_limit."""
+ game = SimpleNamespace(
+ my_team_is_right=False,
+ field=_custom_field(-4.5, 0.3),
+ friendly_robots={
+ 0: SimpleNamespace(p=Vector2D(-4.2, 0.0)),
+ },
+ ball=SimpleNamespace(
+ p=Vector3D(-1.0, 0.0, 0.0),
+ v=Vector3D(-1.0, 0.0, 0.0),
+ ),
+ )
+ captured = {}
+ keeper_x = -4.5 + ROBOT_RADIUS
+ post_limit = 0.3 - ROBOT_RADIUS
+
+ monkeypatch.setattr(gk, "predict_ball_pos_at_x", lambda game, x: Vector2D(x, 0.4))
+
+ def fake_go_to_point(game, motion_controller, robot_id, target, dribbling=False):
+ captured["target"] = target
+ return "sentinel-command"
+
+ monkeypatch.setattr(gk, "go_to_point", fake_go_to_point)
+
+ gk.goalkeep(game, motion_controller=object(), robot_id=0)
+
+ # abs(0.4) > 0.3 half-width -> clamp to post_limit
+ assert captured["target"].x == pytest.approx(keeper_x)
+ assert captured["target"].y == pytest.approx(post_limit)
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_error_handling.py b/utama_core/tests/strategy_runner/test_error_handling.py
index 1e5e9329..09b1b1ad 100644
--- a/utama_core/tests/strategy_runner/test_error_handling.py
+++ b/utama_core/tests/strategy_runner/test_error_handling.py
@@ -1,5 +1,7 @@
"""Tests for error handling and safety mechanisms in strategies (REAL mode only)."""
+import signal
+import threading
from unittest.mock import MagicMock, patch
import pytest
@@ -48,6 +50,17 @@ def mock_runner():
yield runner
+def assert_zero_velocity_commands(commands_dict, expected_robot_ids):
+ assert set(commands_dict.keys()) == expected_robot_ids
+ for cmd in commands_dict.values():
+ assert cmd.local_forward_vel == 0
+ assert cmd.local_left_vel == 0
+ assert cmd.angular_vel == 0
+ assert not cmd.kick
+ assert not cmd.chip
+ assert not cmd.dribble
+
+
class TestStopRobotsOnClose:
"""Tests for _stop_robots behavior when close() is called."""
@@ -101,14 +114,7 @@ def test_stop_commands_have_zero_velocity(self, mock_runner):
controller = mock_runner.my.strategy.robot_controller
commands_dict = controller.add_robot_commands.call_args[0][0]
-
- for robot_id, cmd in commands_dict.items():
- assert cmd.local_forward_vel == 0
- assert cmd.local_left_vel == 0
- assert cmd.angular_vel == 0
- assert not cmd.kick
- assert not cmd.chip
- assert not cmd.dribble
+ assert_zero_velocity_commands(commands_dict, {0, 1, 2})
def test_stop_not_called_in_rsim_mode(self, mock_runner):
mock_runner.mode = Mode.RSIM
@@ -123,6 +129,28 @@ def test_stop_not_called_in_rsim_mode(self, mock_runner):
class TestStopRobotsOnError:
"""Tests for stop behavior when errors occur during execution."""
+ def test_first_sigint_sets_stop_event_stops_fps_live_and_prints_message(self, mock_runner, capsys):
+ mock_runner._stop_event = threading.Event()
+ fps_live = MagicMock()
+ mock_runner._fps_live = fps_live
+
+ mock_runner._handle_sigint(signal.SIGINT, None)
+
+ assert mock_runner._stop_event.is_set()
+ fps_live.stop.assert_called_once()
+ assert mock_runner._fps_live is None
+ captured = capsys.readouterr()
+ assert "Stopping gracefully" in captured.out
+ assert "Press Ctrl+C again to force quit" in captured.out
+
+ def test_second_sigint_raises_keyboard_interrupt(self, mock_runner):
+ mock_runner._stop_event = threading.Event()
+
+ mock_runner._handle_sigint(signal.SIGINT, None)
+
+ with pytest.raises(KeyboardInterrupt):
+ mock_runner._handle_sigint(signal.SIGINT, None)
+
def test_stop_on_runtime_exception(self, mock_runner):
call_count = {"value": 0}
@@ -172,6 +200,23 @@ def signaled_run_step():
controller = mock_runner.my.strategy.robot_controller
assert controller.add_robot_commands.call_count >= 1
+ def test_run_sends_stop_robot_commands_on_first_sigint_shutdown(self, mock_runner):
+ mock_runner._stop_event = threading.Event()
+
+ def signaled_run_step():
+ mock_runner._handle_sigint(signal.SIGINT, None)
+
+ mock_runner._run_step = signaled_run_step
+
+ mock_runner.run()
+
+ controller = mock_runner.my.strategy.robot_controller
+ assert controller.add_robot_commands.call_count == 20
+ assert controller.send_robot_commands.call_count == 20
+
+ commands_dict = controller.add_robot_commands.call_args[0][0]
+ assert_zero_velocity_commands(commands_dict, {0, 1})
+
if __name__ == "__main__":
pytest.main([__file__, "-v"])
diff --git a/utama_core/tests/strategy_runner/test_referee_rsim.py b/utama_core/tests/strategy_runner/test_referee_rsim.py
index 67f3877e..1d4ae3b2 100644
--- a/utama_core/tests/strategy_runner/test_referee_rsim.py
+++ b/utama_core/tests/strategy_runner/test_referee_rsim.py
@@ -37,13 +37,19 @@
import py_trees
+from utama_core.config.field_params import STANDARD_FIELD_DIMS
from utama_core.config.referee_constants import BALL_KEEP_OUT_DISTANCE
from utama_core.custom_referee import CustomReferee
+from utama_core.custom_referee.geometry import RefereeGeometry
+from utama_core.custom_referee.rules.out_of_bounds_rule import OutOfBoundsRule
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.skills.src.go_to_ball import go_to_ball
+from utama_core.strategy.common.abstract_behaviour import AbstractBehaviour
from utama_core.strategy.common.abstract_strategy import AbstractStrategy
+from utama_core.strategy.examples.utils import SetBlackboardVariable
from utama_core.team_controller.src.controllers import AbstractSimController
from utama_core.tests.common.abstract_test_manager import (
AbstractTestManager,
@@ -86,6 +92,69 @@ def get_min_bounding_req(self) -> Optional[FieldBounds]:
return None
+class _StateHasBall(AbstractBehaviour):
+ def __init__(self, robot_id_key: str, name: str = "StateHasBall"):
+ super().__init__(name=name)
+ self.robot_id_key = robot_id_key
+
+ def setup_(self):
+ self.blackboard.register_key(key=self.robot_id_key, access=py_trees.common.Access.READ)
+
+ def update(self) -> py_trees.common.Status:
+ robot_id = self.blackboard.get(self.robot_id_key)
+ robot = self.blackboard.game.friendly_robots[robot_id]
+ return py_trees.common.Status.SUCCESS if robot.has_ball else py_trees.common.Status.FAILURE
+
+
+class _GoToBallUntilStatePossession(AbstractBehaviour):
+ def __init__(self, robot_id_key: str, name: str = "GoToBallUntilStatePossession"):
+ super().__init__(name=name)
+ self.robot_id_key = robot_id_key
+
+ def setup_(self):
+ self.blackboard.register_key(key=self.robot_id_key, access=py_trees.common.Access.READ)
+
+ def update(self) -> py_trees.common.Status:
+ robot_id = self.blackboard.get(self.robot_id_key)
+ self.blackboard.cmd_map[robot_id] = go_to_ball(self.blackboard.game, self.blackboard.motion_controller, robot_id)
+ return py_trees.common.Status.RUNNING
+
+
+class _StateGoToBallStrategy(AbstractStrategy):
+ exp_ball: bool = True
+
+ def __init__(self, robot_id: int):
+ self.robot_id = robot_id
+ self.robot_id_key = "robot_id"
+ super().__init__()
+
+ def create_behaviour_tree(self) -> py_trees.behaviour.Behaviour:
+ root = py_trees.composites.Sequence(name="StateGoToBallRoot", memory=True)
+ go_to_ball_until_possession = py_trees.composites.Selector(name="StateGoToBall", memory=False)
+ go_to_ball_until_possession.add_children(
+ [
+ _StateHasBall(robot_id_key=self.robot_id_key),
+ _GoToBallUntilStatePossession(robot_id_key=self.robot_id_key),
+ ]
+ )
+ root.add_children(
+ [
+ SetBlackboardVariable(name="SetRobotID", variable_name=self.robot_id_key, value=self.robot_id),
+ go_to_ball_until_possession,
+ ]
+ )
+ 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[FieldBounds]:
+ return None
+
+
# ---------------------------------------------------------------------------
# Helper
# ---------------------------------------------------------------------------
@@ -177,15 +246,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 +258,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 +308,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
@@ -387,6 +442,69 @@ def test_prepare_kickoff_robots_form_on_own_half_outside_circle(headless):
assert passed, "Robots did not sustain a legal kickoff formation for the required number of frames"
+# ---------------------------------------------------------------------------
+# Scenario 4: out-of-bounds restart spot can be played
+# ---------------------------------------------------------------------------
+
+
+class _RestartSpotCaptureManager(AbstractTestManager):
+ """Places the ball at the out-of-bounds restart spot and verifies capture.
+
+ This guards against restart positions that are technically infield but too
+ close to the wall for the robot/dribbler geometry to acquire the ball.
+ """
+
+ n_episodes = 1
+
+ def __init__(self, restart_spot: tuple[float, float]):
+ super().__init__()
+ self.restart_spot = restart_spot
+ self.robot_has_ball = False
+ self.min_distance_to_ball = math.inf
+
+ def reset_field(self, sim_controller: AbstractSimController, game: Game):
+ target_x, target_y = self.restart_spot
+ sim_controller.teleport_robot(game.my_team_is_yellow, 0, target_x, target_y - 0.9, math.pi / 2)
+ sim_controller.teleport_ball(target_x, target_y)
+
+ def eval_status(self, game: Game) -> TestingStatus:
+ robot = game.friendly_robots[0]
+ ball = game.ball
+ self.min_distance_to_ball = min(
+ self.min_distance_to_ball,
+ math.hypot(robot.p.x - ball.p.x, robot.p.y - ball.p.y),
+ )
+ if robot.has_ball:
+ self.robot_has_ball = True
+ return TestingStatus.SUCCESS
+ return TestingStatus.IN_PROGRESS
+
+
+def test_out_of_bounds_restart_spot_is_capturable_by_go_to_ball(headless):
+ geometry = RefereeGeometry.from_field_dims(STANDARD_FIELD_DIMS)
+ restart_spot = OutOfBoundsRule._nearest_infield_point(0.0, geometry.half_width + 0.5, geometry)
+
+ runner = StrategyRunner(
+ strategy=_StateGoToBallStrategy(robot_id=0),
+ my_team_is_yellow=True,
+ my_team_is_right=False,
+ mode="rsim",
+ exp_friendly=1,
+ exp_enemy=0,
+ exp_ball=True,
+ referee=None,
+ )
+ tm = _RestartSpotCaptureManager(restart_spot)
+
+ passed = runner.run_test(tm, episode_timeout=5.0, rsim_headless=headless)
+
+ assert tm.robot_has_ball, (
+ "Robot could not acquire the out-of-bounds restart ball at "
+ f"{restart_spot}; closest approach was {tm.min_distance_to_ball:.3f} m"
+ )
+ assert passed
+
+
# ---------------------------------------------------------------------------
# Future work: full out-of-bounds sequence integration test
#
diff --git a/utama_core/tests/strategy_runner/test_runner_misconfig.py b/utama_core/tests/strategy_runner/test_runner_misconfig.py
index 9aa805b4..a963838f 100644
--- a/utama_core/tests/strategy_runner/test_runner_misconfig.py
+++ b/utama_core/tests/strategy_runner/test_runner_misconfig.py
@@ -389,8 +389,8 @@ def test_validate_vision_to_cmd_mapping_incorrect_length_enemy():
def test_validate_vision_to_cmd_mapping_correct_count_non_contiguous_ids_passes_init():
# Non-contiguous vision IDs (e.g. real field robots numbered 5,6,7) with the right
- # count must PASS at init time — coverage against observed IDs is validated later
- # in _validate_mapping_covers_game_frame() after _load_game().
+ # count must PASS at init time — the allowlist derived from the mapping ensures only
+ # those IDs are accepted from vision at runtime.
runner = SimpleNamespace(
mode=Mode.REAL,
my_team_is_yellow=True,
@@ -482,6 +482,24 @@ def test_trusted_ir_robots_blue_team_is_my_team():
assert runner.my.robot_info_refiner._trusted_ir_robots == frozenset({2})
+def test_blue_allowlist_not_set_in_single_team_real_mode():
+ # In single-team real mode, blue robots are tracked as enemies and must not be filtered.
+ # _allowed_blue must be None even when blue_vision_to_cmd_mapping resolves to {}.
+ runner = SimpleNamespace(
+ mode=Mode.REAL,
+ my_team_is_yellow=True,
+ opp=None,
+ exp_friendly=1,
+ exp_enemy=0,
+ )
+ # Validate mapping returns {} for blue (no opp, mapping=None)
+ blue_mapping = StrategyRunner._validate_vision_to_cmd_mapping(runner, None, is_yellow=False)
+ assert blue_mapping == {}
+ # Simulate the allowlist derivation: empty mapping with no opp → None, not frozenset()
+ _allowed_blue = (frozenset(blue_mapping) or None) if runner.opp is not None else None
+ assert _allowed_blue is None
+
+
def test_trusted_ir_robots_both_teams_pvp():
"""Both colour params route correctly in PVP mode."""
runner = StrategyRunner(
diff --git a/utama_core/tests/strategy_runner/test_scenario_hooks.py b/utama_core/tests/strategy_runner/test_scenario_hooks.py
new file mode 100644
index 00000000..867e15d0
--- /dev/null
+++ b/utama_core/tests/strategy_runner/test_scenario_hooks.py
@@ -0,0 +1,53 @@
+from unittest.mock import Mock
+
+from utama_core.custom_referee import CustomReferee
+from utama_core.entities.referee.referee_command import RefereeCommand
+from utama_core.rsoccer_simulator.src.ssl import ssl_gym_base
+from utama_core.run.strategy_runner import StrategyRunner
+
+
+def test_strategy_runner_step_once_delegates_to_single_step_loop():
+ runner = StrategyRunner.__new__(StrategyRunner)
+ runner._run_step = Mock()
+
+ runner.step_once()
+
+ runner._run_step.assert_called_once_with()
+
+
+def test_custom_referee_set_command_accepts_scripted_metadata():
+ referee = CustomReferee.from_profile_name("simulation")
+
+ referee.set_command(
+ RefereeCommand.BALL_PLACEMENT_YELLOW,
+ timestamp=3.0,
+ designated_position=(1.0, -0.5),
+ next_command=RefereeCommand.NORMAL_START,
+ status_message="scripted placement",
+ )
+ data = referee.step(game_frame=None, current_time=3.0)
+
+ assert data.referee_command == RefereeCommand.BALL_PLACEMENT_YELLOW
+ assert data.designated_position == (1.0, -0.5)
+ assert data.next_command == RefereeCommand.NORMAL_START
+ assert data.status_message == "scripted placement"
+
+
+def test_ssl_base_env_reset_seeds_random_generators(monkeypatch):
+ random_seed = Mock()
+ numpy_seed = Mock()
+ monkeypatch.setattr(ssl_gym_base.random, "seed", random_seed)
+ monkeypatch.setattr(ssl_gym_base.np.random, "seed", numpy_seed)
+
+ env = ssl_gym_base.SSLBaseEnv.__new__(ssl_gym_base.SSLBaseEnv)
+ env.render_mode = None
+ env.rsim = Mock()
+ env.rsim.get_frame.return_value = object()
+ env._get_initial_positions_frame = Mock(return_value=object())
+ env._frame_to_observations = Mock(return_value=("obs",))
+
+ result = ssl_gym_base.SSLBaseEnv.reset(env, seed=42)
+
+ random_seed.assert_called_once_with(42)
+ numpy_seed.assert_called_once_with(42)
+ assert result == (("obs",), {})