diff --git a/main.py b/main.py index bfb4c138..c7f60a68 100644 --- a/main.py +++ b/main.py @@ -1,3 +1,4 @@ +from utama_core.config.field_params import GREAT_EXHIBITION_FIELD_DIMS from utama_core.entities.game.field import FieldBounds from utama_core.replay import ReplayWriterConfig from utama_core.rsoccer_simulator.src.Utils.gaussian_noise import RsimGaussianNoise @@ -15,7 +16,7 @@ def main(): # Setup for real testing # Custom field size based setup in real - custom_bounds = FieldBounds(top_left=(2.25, 1.5), bottom_right=(4.5, -1.5)) + custom_bounds = FieldBounds(top_left=(-2, 1.5), bottom_right=(1, -1.5)) runner = StrategyRunner( strategy=RandomMovementStrategy(n_robots=2, field_bounds=custom_bounds, endpoint_tolerance=0.1, seed=42), @@ -25,6 +26,8 @@ def main(): exp_friendly=2, exp_enemy=0, replay_writer_config=ReplayWriterConfig(replay_name="test_replay", overwrite_existing=True), + field_bounds=custom_bounds, + full_field_dims=GREAT_EXHIBITION_FIELD_DIMS, print_real_fps=True, profiler_name=None, ) diff --git a/utama_core/config/field_params.py b/utama_core/config/field_params.py new file mode 100644 index 00000000..3f230557 --- /dev/null +++ b/utama_core/config/field_params.py @@ -0,0 +1,148 @@ +from dataclasses import dataclass +from functools import cached_property + +import numpy as np + + +@dataclass(frozen=True) +class FieldBounds: + top_left: tuple[float, float] + bottom_right: tuple[float, float] + + @property + def center(self) -> tuple[float, float]: + """Calculates the geometric center of the field bounds.""" + cx = (self.top_left[0] + self.bottom_right[0]) / 2.0 + cy = (self.top_left[1] + self.bottom_right[1]) / 2.0 + return (cx, cy) + + +@dataclass(frozen=True) +class FieldDimensions: + """Holds field dimensions and derives all geometric shapes.""" + + full_field_half_length: float + full_field_half_width: float + half_defense_area_depth: float + half_defense_area_width: float + half_goal_width: float + + # --- Bounds --- + + @cached_property + def full_field_bounds(self): + return FieldBounds( + top_left=(-self.full_field_half_length, self.full_field_half_width), + bottom_right=(self.full_field_half_length, -self.full_field_half_width), + ) + + # --- Full field polygon --- + + @cached_property + def full_field(self) -> np.ndarray: + L = self.full_field_half_length + W = self.full_field_half_width + return np.array( + [ + (L, W), + (L, -W), + (-L, -W), + (-L, W), + ] + ) + + # --- Goal lines --- + + @cached_property + def right_goal_line(self) -> np.ndarray: + L = self.full_field_half_length + G = self.half_goal_width + return np.array( + [ + (L, G), + (L, -G), + ] + ) + + @cached_property + def left_goal_line(self) -> np.ndarray: + L = self.full_field_half_length + G = self.half_goal_width + return np.array( + [ + (-L, G), + (-L, -G), + ] + ) + + # --- Defense areas --- + + @cached_property + def right_defense_area(self) -> np.ndarray: + L = self.full_field_half_length + D = self.half_defense_area_depth + W = self.half_defense_area_width + return np.array( + [ + (L, W), + (L - 2 * D, W), + (L - 2 * D, -W), + (L, -W), + ] + ) + + @cached_property + def left_defense_area(self) -> np.ndarray: + L = self.full_field_half_length + D = self.half_defense_area_depth + W = self.half_defense_area_width + return np.array( + [ + (-L, W), + (-L + 2 * D, W), + (-L + 2 * D, -W), + (-L, -W), + ] + ) + + def __post_init__(self): + L = self.full_field_half_length + W = self.full_field_half_width + D = self.half_defense_area_depth + DW = self.half_defense_area_width + G = self.half_goal_width + + # --- Positivity --- + if not (L > 0 and W > 0): + raise ValueError("Field length/width must be positive") + if not (D > 0 and DW > 0 and G > 0): + raise ValueError("Goal/defense measurements must be positive") + + # --- Fit constraints --- + if 2 * D > L: + raise ValueError(f"Defense depth {2*D} exceeds field length {L}") + if DW > W: + raise ValueError(f"Defense width {DW} exceeds field width {W}") + if G > W: + raise ValueError(f"Goal width {G} exceeds field width {W}") + + # --- Optional semantic constraint --- + if G > DW: + raise ValueError(f"Goal width {G} should not exceed defense width {DW}") + + +STANDARD_FIELD_DIMS = FieldDimensions( + full_field_half_length=4.5, + full_field_half_width=3.0, + half_defense_area_depth=0.5, + half_defense_area_width=1, + half_goal_width=0.5, +) + +GREAT_EXHIBITION_FIELD_DIMS = FieldDimensions( + full_field_half_length=2.0, + full_field_half_width=1.5, + half_defense_area_depth=0.4, + half_defense_area_width=0.8, + half_goal_width=0.5, +) diff --git a/utama_core/config/formations.py b/utama_core/config/formations.py index 1ab30264..0a03fc63 100644 --- a/utama_core/config/formations.py +++ b/utama_core/config/formations.py @@ -1,21 +1,177 @@ +import math +from enum import Enum +from typing import NamedTuple + import numpy as np +from utama_core.config.field_params import FieldBounds, FieldDimensions +from utama_core.config.physical_constants import MAX_ROBOTS, ROBOT_RADIUS +from utama_core.global_utils.math_utils import normalise_heading + + +class FormationEntry(NamedTuple): + x: float + y: float + theta: float + + # Starting positions for right team -RIGHT_START_ONE = [ - (4.2000, 0.0, np.pi), - (3.4000, -0.2000, np.pi), - (3.4000, 0.2000, np.pi), - (0.7000, 0.0, np.pi), - (0.7000, 2.2500, np.pi), - (0.7000, -2.2500, np.pi), -] - -# Starting positions for left team -LEFT_START_ONE = [ - (-4.2000, 0.0, 0), - (-3.4000, 0.2000, 0), - (-3.4000, -0.2000, 0), - (-0.7000, 0.0, 0), - (-0.7000, -2.2500, 0), - (-0.7000, 2.2500, 0), -] + +#################### INSERT FORMATIONS HERE ###################### +# Normalised right team formation that will be scaled, +# then mirrored for left. + +# Anisotropic normalised scaling to field half-length, half-width +# e.g +# 0.75 = 3.375 (actual scale of x-coord) / 4.5 (field half-length) +# -0.06 = -0.18 (actual scale of y-coord) / 3.0 (field half-width) +################################################################## + + +class FormationType(Enum): + START_ONE = "START_ONE" + + +FORMATIONS = { + FormationType.START_ONE: [ + FormationEntry(0.9, 0.0, np.pi), + FormationEntry(0.75, -0.2, np.pi), + FormationEntry(0.75, 0.2, np.pi), + FormationEntry(0.16, 0.0, np.pi), + FormationEntry(0.16, 0.75, np.pi), + FormationEntry(0.16, -0.75, np.pi), + ], +} + +################## END OF FORMATIONS ########################## + + +def _mirror(formation: list[FormationEntry], bounds: FieldBounds) -> list[FormationEntry]: + cx, _ = bounds.center + + return [ + FormationEntry( + 2 * cx - entry.x, + entry.y, + normalise_heading(np.pi - entry.theta), + ) + for entry in formation + ] + + +def _scale(norm_formation, bounds: FieldBounds) -> list[FormationEntry]: + x_min = bounds.top_left[0] + x_max = bounds.bottom_right[0] + y_max = bounds.top_left[1] + y_min = bounds.bottom_right[1] + + L = (x_max - x_min) / 2 + W = (y_max - y_min) / 2 + + cx, cy = bounds.center + + return [FormationEntry(cx + x * L, cy + y * W, theta) for x, y, theta in norm_formation] + + +def _validate_bounds_and_intra_team_collision( + formation: list[FormationEntry], + bounds: FieldBounds, +) -> None: + """ + Validate that the formation fits inside the bounding box and that robots do not overlap. + The robot center may touch the edges. + + Raises ValueError if the formation cannot fit. + + Args: + formation: List of (x, y, theta) tuples for robots. + bounds: FieldBounds object with top_left and bottom_right coordinates. + """ + # --- Bounding box edges (center can touch boundary) --- + x_min = bounds.top_left[0] + x_max = bounds.bottom_right[0] + y_max = bounds.top_left[1] + y_min = bounds.bottom_right[1] + + # --- Check bounds for each robot --- + for i, (x, y, _) in enumerate(formation): + if not (x_min <= x <= x_max): + raise ValueError(f"Robot {i} x-position out of bounds: {x}, allowed: [{x_min}, {x_max}]") + if not (y_min <= y <= y_max): + raise ValueError(f"Robot {i} y-position out of bounds: {y}, allowed: [{y_min}, {y_max}]") + + # --- Check pairwise collisions --- + n = len(formation) + for i in range(n): + x1, y1, _ = formation[i] + for j in range(i + 1, n): + x2, y2, _ = formation[j] + dist = math.hypot(x1 - x2, y1 - y2) + if dist < 2 * ROBOT_RADIUS: + raise ValueError( + f"Could not fit all robots in provided FieldBounds/FieldDimensions. Robots {i} and {j} overlap (distance={dist:.3f})" + ) + + +def _validate_team_separation(left, right): + """ + Validate that the left and right teams are sufficiently separated to avoid collisions. + """ + max_left_x = -np.inf + min_right_x = np.inf + if left: + max_left_x = max(x for x, _, _ in left) + if right: + min_right_x = min(x for x, _, _ in right) + + gap = min_right_x - max_left_x + required = 2 * ROBOT_RADIUS + + if gap < required: + raise ValueError(f"Teams not sufficiently separated: gap={gap:.3f}, required={required:.3f}") + + +# TODO: can consider a fitting algorithm that can optimise robot placement so that the chance of running out of space is reduced. + + +def get_formations( + bounds: FieldBounds, + n_left: int, + n_right: int, + formation_type: FormationType, +) -> tuple[list[FormationEntry], list[FormationEntry]]: + """ + Returns the starting formations for both teams based on the provided field dimensions. + The formations are defined as lists of FormationEntry objects, which contain the x and y coordinates + + Args: + bounds: FieldBounds object defining the top-left and bottom-right corners of the field. + n_left: Number of robots on the left team. + n_right: Number of robots on the right team. + formation_type: The type of formation to generate (e.g., START_ONE). + + Returns: + tuple[list[FormationEntry], list[FormationEntry]]: A tuple containing two lists of FormationEntry objects. + left and right team formations respectively. + """ + if n_left > MAX_ROBOTS or n_right > MAX_ROBOTS: + raise ValueError( + f"Number of robots per team cannot exceed {MAX_ROBOTS}. Got n_left={n_left}, n_right={n_right}." + ) + if formation_type not in FORMATIONS: + raise ValueError(f"Formation '{formation_type.value}' not found. Available: {list(FORMATIONS.keys())}") + + base = FORMATIONS[formation_type] + if n_left > len(base) or n_right > len(base): + raise ValueError( + f"Formation '{formation_type.value}' only defines {len(base)} positions, but got n_left={n_left}, n_right={n_right}." + ) + + left = _mirror(_scale(base[:n_left], bounds), bounds) + right = _scale(base[:n_right], bounds) + + _validate_bounds_and_intra_team_collision(left, bounds) + _validate_bounds_and_intra_team_collision(right, bounds) + _validate_team_separation(left, right) + + return left, right diff --git a/utama_core/config/physical_constants.py b/utama_core/config/physical_constants.py index 440be96a..624d793e 100644 --- a/utama_core/config/physical_constants.py +++ b/utama_core/config/physical_constants.py @@ -1,2 +1,3 @@ ROBOT_RADIUS = 0.09 MAX_ROBOTS = 6 +BALL_RADIUS = 0.0215 diff --git a/utama_core/data_processing/refiners/position.py b/utama_core/data_processing/refiners/position.py index 5ff1a2d8..9036efe4 100644 --- a/utama_core/data_processing/refiners/position.py +++ b/utama_core/data_processing/refiners/position.py @@ -5,6 +5,7 @@ import numpy as np +from utama_core.config.field_params import FieldDimensions from utama_core.config.settings import BALL_MERGE_THRESHOLD, VISION_BOUNDS_BUFFER from utama_core.data_processing.refiners.base_refiner import BaseRefiner from utama_core.data_processing.refiners.filters.kalman import ( @@ -44,6 +45,11 @@ class PositionRefiner(BaseRefiner): Refiner that combines vision data from multiple cameras, applies bounds filtering, and optionally applies Kalman filtering for smoothing and imputing vanished robots. + Args: + full_field_dims: The dimensions of the full field, used to set bounds for vision data inclusion. + filtering: Whether to apply Kalman filtering for smoothing and imputing vanished robots. + exp_ball: Whether to expect a ball on the field. + Important: when exp_ball set to False, the refiner could return either Ball | None type ball value when exp_ball set to True, the refiner will ALWAYS return a Ball type ball value (will impute missing frames) @@ -51,17 +57,20 @@ class PositionRefiner(BaseRefiner): def __init__( self, - field_bounds: FieldBounds, + full_field_dims: FieldDimensions, filtering: bool = True, exp_ball: bool = True, ): # alpha=0 means no change in angle (inf smoothing), alpha=1 means no smoothing self.angle_smoother = AngleSmoother(alpha=1) + top_left = full_field_dims.full_field_bounds.top_left + bottom_right = full_field_dims.full_field_bounds.bottom_right + self.vision_bounds = VisionBounds( - x_min=field_bounds.top_left[0] - VISION_BOUNDS_BUFFER, # expand left - x_max=field_bounds.bottom_right[0] + VISION_BOUNDS_BUFFER, # expand right - y_min=field_bounds.bottom_right[1] - VISION_BOUNDS_BUFFER, # expand bottom - y_max=field_bounds.top_left[1] + VISION_BOUNDS_BUFFER, # expand top + x_min=top_left[0] - VISION_BOUNDS_BUFFER, + x_max=bottom_right[0] + VISION_BOUNDS_BUFFER, + y_min=bottom_right[1] - VISION_BOUNDS_BUFFER, + y_max=top_left[1] + VISION_BOUNDS_BUFFER, ) # For Kalman filtering and imputing vanished values. @@ -79,7 +88,11 @@ def __init__( self.kalman_filter_ball = KalmanFilterBall() # Primary function for the Refiner interface - def refine(self, game_frame: GameFrame, data: List[RawVisionData]) -> GameFrame: + def refine( + self, + game_frame: GameFrame, + data: List[RawVisionData], + ) -> GameFrame: frames = [frame for frame in data if frame is not None] # If no information just return the original @@ -88,7 +101,10 @@ def refine(self, game_frame: GameFrame, data: List[RawVisionData]) -> GameFrame: # class VisionData: ts: float; yellow_robots: List[VisionRobotData]; blue_robots: List[VisionRobotData]; balls: List[VisionBallData] # class VisionRobotData: id: int; x: float; y: float; orientation: float - combined_vision_data: VisionData = CameraCombiner().combine_cameras(frames, bounds=self.vision_bounds) + combined_vision_data: VisionData = CameraCombiner().combine_cameras( + frames, + bounds=self.vision_bounds, + ) time_elapsed = combined_vision_data.ts - game_frame.ts @@ -196,7 +212,7 @@ def reset(self): def start_filtering(self): """ - Start filtering after first valid frame is received from GameGater. + Start filtering (imputation and interpolation) after first valid frame is received from GameGater. """ self._filter_running = True @@ -333,7 +349,11 @@ def filter_running(self) -> bool: class CameraCombiner: - def combine_cameras(self, frames: List[RawVisionData], bounds: VisionBounds) -> VisionData: + def combine_cameras( + self, + frames: List[RawVisionData], + bounds: VisionBounds, + ) -> VisionData: """ Combines the vision data from multiple cameras into a single coherent VisionData object. Also, removes any robot detections that are out of the specified bounds. diff --git a/utama_core/entities/game/ball.py b/utama_core/entities/game/ball.py index 75089148..07c79b8b 100644 --- a/utama_core/entities/game/ball.py +++ b/utama_core/entities/game/ball.py @@ -8,14 +8,3 @@ class Ball: p: Vector3D v: Vector3D a: Vector3D - - def is_ball_in_goal(self, right_goal: bool) -> bool: - ball_pos = self.p - return ( - ball_pos.x < -self.field.half_length - and (ball_pos.y < self.field.HALF_GOAL_WIDTH and ball_pos.y > -self.field.HALF_GOAL_WIDTH) - and not right_goal - or ball_pos.x > self.field.half_length - and (ball_pos.y < self.field.HALF_GOAL_WIDTH and ball_pos.y > -self.field.HALF_GOAL_WIDTH) - and right_goal - ) diff --git a/utama_core/entities/game/field.py b/utama_core/entities/game/field.py index 5e9692db..500c2ad5 100644 --- a/utama_core/entities/game/field.py +++ b/utama_core/entities/game/field.py @@ -2,26 +2,7 @@ import numpy as np - -class ClassProperty: - def __init__(self, getter): - self.getter = getter - - def __get__(self, instance, owner): - return self.getter(owner) - - -@dataclass(frozen=True) -class FieldBounds: - top_left: tuple[float, float] - bottom_right: tuple[float, float] - - @property - def center(self) -> tuple[float, float]: - """Calculates the geometric center of the field bounds.""" - cx = (self.top_left[0] + self.bottom_right[0]) / 2.0 - cy = (self.top_left[1] + self.bottom_right[1]) / 2.0 - return (cx, cy) +from utama_core.config.field_params import FieldBounds, FieldDimensions class Field: @@ -30,73 +11,14 @@ class Field: Call the class properties to get the field information """ - # Class constants refer to the standard SSL field (9m x 6m) - - _HALF_GOAL_WIDTH = 0.5 - _HALF_DEFENSE_AREA_LENGTH = 0.5 - _HALF_DEFENSE_AREA_WIDTH = 1 - - _FULL_FIELD_HALF_WIDTH = 3.0 - _FULL_FIELD_HALF_LENGTH = 4.5 - - _RIGHT_GOAL_LINE = np.array( - [ - (_FULL_FIELD_HALF_LENGTH, _HALF_GOAL_WIDTH), - (_FULL_FIELD_HALF_LENGTH, -_HALF_GOAL_WIDTH), - ] - ) - - _LEFT_GOAL_LINE = np.array( - [ - (-_FULL_FIELD_HALF_LENGTH, _HALF_GOAL_WIDTH), - (-_FULL_FIELD_HALF_LENGTH, -_HALF_GOAL_WIDTH), - ] - ) - - _RIGHT_DEFENSE_AREA = np.array( - [ - (_FULL_FIELD_HALF_LENGTH, _HALF_DEFENSE_AREA_WIDTH), - ( - _FULL_FIELD_HALF_LENGTH - 2 * _HALF_DEFENSE_AREA_LENGTH, - _HALF_DEFENSE_AREA_WIDTH, - ), - ( - _FULL_FIELD_HALF_LENGTH - 2 * _HALF_DEFENSE_AREA_LENGTH, - -_HALF_DEFENSE_AREA_WIDTH, - ), - (_FULL_FIELD_HALF_LENGTH, -_HALF_DEFENSE_AREA_WIDTH), - (_FULL_FIELD_HALF_LENGTH, _HALF_DEFENSE_AREA_WIDTH), - ] - ) - - _LEFT_DEFENSE_AREA = np.array( - [ - (-_FULL_FIELD_HALF_LENGTH, _HALF_DEFENSE_AREA_WIDTH), - ( - -_FULL_FIELD_HALF_LENGTH + 2 * _HALF_DEFENSE_AREA_LENGTH, - _HALF_DEFENSE_AREA_WIDTH, - ), - ( - -_FULL_FIELD_HALF_LENGTH + 2 * _HALF_DEFENSE_AREA_LENGTH, - -_HALF_DEFENSE_AREA_WIDTH, - ), - (-_FULL_FIELD_HALF_LENGTH, -_HALF_DEFENSE_AREA_WIDTH), - (-_FULL_FIELD_HALF_LENGTH, _HALF_DEFENSE_AREA_WIDTH), - ] - ) - - _FULL_FIELD = np.array( - [ - (-_FULL_FIELD_HALF_LENGTH, -_FULL_FIELD_HALF_WIDTH), - (-_FULL_FIELD_HALF_LENGTH, _FULL_FIELD_HALF_WIDTH), - (_FULL_FIELD_HALF_LENGTH, _FULL_FIELD_HALF_WIDTH), - (_FULL_FIELD_HALF_LENGTH, -_FULL_FIELD_HALF_WIDTH), - ] - ) - - def __init__(self, my_team_is_right: bool, field_bounds: FieldBounds): + def __init__( + self, + my_team_is_right: bool, + field_dims: FieldDimensions, + field_bounds: FieldBounds, + ): self.my_team_is_right = my_team_is_right - + self._field_dims = field_dims self._field_bounds = field_bounds self._half_length = (field_bounds.bottom_right[0] - field_bounds.top_left[0]) / 2 @@ -104,16 +26,16 @@ def __init__(self, my_team_is_right: bool, field_bounds: FieldBounds): @property def includes_left_goal(self) -> bool: - return self._field_bounds.top_left[0] == -self._FULL_FIELD_HALF_LENGTH and ( - self._field_bounds.top_left[1] >= self._HALF_GOAL_WIDTH - and self._field_bounds.bottom_right[1] <= -self._HALF_GOAL_WIDTH + return self._field_bounds.top_left[0] == -self._field_dims.full_field_half_length and ( + self._field_bounds.top_left[1] >= self._field_dims.half_goal_width + and self._field_bounds.bottom_right[1] <= -self._field_dims.half_goal_width ) @property def includes_right_goal(self) -> bool: - return self._field_bounds.bottom_right[0] == self._FULL_FIELD_HALF_LENGTH and ( - self._field_bounds.top_left[1] >= self._HALF_GOAL_WIDTH - and self._field_bounds.bottom_right[1] <= -self._HALF_GOAL_WIDTH + return self._field_bounds.bottom_right[0] == self._field_dims.full_field_half_length and ( + self._field_bounds.top_left[1] >= self._field_dims.half_goal_width + and self._field_bounds.bottom_right[1] <= -self._field_dims.half_goal_width ) @property @@ -133,30 +55,30 @@ def includes_opp_goal_line(self) -> bool: @property def my_goal_line(self) -> np.ndarray: if self.my_team_is_right: - return self._RIGHT_GOAL_LINE + return self._field_dims.right_goal_line else: - return self._LEFT_GOAL_LINE + return self._field_dims.left_goal_line @property def enemy_goal_line(self) -> np.ndarray: if self.my_team_is_right: - return self._LEFT_GOAL_LINE + return self._field_dims.left_goal_line else: - return self._RIGHT_GOAL_LINE + return self._field_dims.right_goal_line @property def my_defense_area(self) -> np.ndarray: if self.my_team_is_right: - return self._RIGHT_DEFENSE_AREA + return self._field_dims.right_defense_area else: - return self._LEFT_DEFENSE_AREA + return self._field_dims.left_defense_area @property def enemy_defense_area(self) -> np.ndarray: if self.my_team_is_right: - return self._LEFT_DEFENSE_AREA + return self._field_dims.left_defense_area else: - return self._RIGHT_DEFENSE_AREA + return self._field_dims.right_defense_area @property def half_length(self) -> float: @@ -176,41 +98,38 @@ def center(self) -> tuple[float, float]: ### Class Properties for standard field dimensions ### - @ClassProperty - def HALF_GOAL_WIDTH(cls) -> float: - return cls._HALF_GOAL_WIDTH + @property + def full_field_half_length(self) -> float: + return self._field_dims.full_field_half_length - @ClassProperty - def LEFT_GOAL_LINE(cls) -> np.ndarray: - return cls._LEFT_GOAL_LINE + @property + def full_field_half_width(self) -> float: + return self._field_dims.full_field_half_width - @ClassProperty - def RIGHT_GOAL_LINE(cls) -> np.ndarray: - return cls._RIGHT_GOAL_LINE + @property + def half_goal_width(self) -> float: + return self._field_dims.half_goal_width - @ClassProperty - def LEFT_DEFENSE_AREA(cls) -> np.ndarray: - return cls._LEFT_DEFENSE_AREA + @property + def left_goal_line(self) -> np.ndarray: + return self._field_dims.left_goal_line - @ClassProperty - def RIGHT_DEFENSE_AREA(cls) -> np.ndarray: - return cls._RIGHT_DEFENSE_AREA + @property + def right_goal_line(self) -> np.ndarray: + return self._field_dims.right_goal_line - @ClassProperty - def FULL_FIELD_HALF_LENGTH(cls) -> float: - return cls._FULL_FIELD_HALF_LENGTH + @property + def left_defense_area(self) -> np.ndarray: + return self._field_dims.left_defense_area - @ClassProperty - def FULL_FIELD_HALF_WIDTH(cls) -> float: - return cls._FULL_FIELD_HALF_WIDTH + @property + def right_defense_area(self) -> np.ndarray: + return self._field_dims.right_defense_area - @ClassProperty - def FULL_FIELD(cls) -> np.ndarray: - return cls._FULL_FIELD + @property + def full_field(self) -> np.ndarray: + return self._field_dims.full_field - @ClassProperty - def FULL_FIELD_BOUNDS(cls) -> FieldBounds: - return FieldBounds( - top_left=(-cls._FULL_FIELD_HALF_LENGTH, cls._FULL_FIELD_HALF_WIDTH), - bottom_right=(cls._FULL_FIELD_HALF_LENGTH, -cls._FULL_FIELD_HALF_WIDTH), - ) + @property + def full_field_bounds(self) -> FieldBounds: + return self._field_dims.full_field_bounds diff --git a/utama_core/entities/game/game_frame.py b/utama_core/entities/game/game_frame.py index d7e62a69..f30162bc 100644 --- a/utama_core/entities/game/game_frame.py +++ b/utama_core/entities/game/game_frame.py @@ -18,14 +18,3 @@ class GameFrame: friendly_robots: Dict[int, Robot] enemy_robots: Dict[int, Robot] ball: Optional[Ball] - - def is_ball_in_goal(self, right_goal: bool) -> bool: - ball_pos = self.ball.p - return ( - ball_pos.x < -self.field.half_length - and (ball_pos.y < self.field.HALF_GOAL_WIDTH and ball_pos.y > -self.field.HALF_GOAL_WIDTH) - and not right_goal - or ball_pos.x > self.field.half_length - and (ball_pos.y < self.field.HALF_GOAL_WIDTH and ball_pos.y > -self.field.HALF_GOAL_WIDTH) - and right_goal - ) diff --git a/utama_core/global_utils/math_utils.py b/utama_core/global_utils/math_utils.py index f0db48e0..0915c99b 100644 --- a/utama_core/global_utils/math_utils.py +++ b/utama_core/global_utils/math_utils.py @@ -130,14 +130,43 @@ def in_field_bounds(point: Tuple[float, float] | Vector2D, bounding_box: FieldBo ) -def assert_valid_bounding_box(bb: FieldBounds): - """Asserts that a FieldBounds object is valid, raising an AssertionError if not.""" - fx, fy = Field._FULL_FIELD_HALF_LENGTH, Field._FULL_FIELD_HALF_WIDTH +def assert_valid_bounding_box( + bb: FieldBounds, + full_field_half_length: float, + full_field_half_width: float, +): + """Validate a bounding box is well-formed and within full field limits.""" + fx, fy = full_field_half_length, full_field_half_width x0, y0 = bb.top_left x1, y1 = bb.bottom_right - assert x0 <= x1, f"top-left x {x0} must be <= bottom-right x {x1}" - assert y0 >= y1, f"top-left y {y0} must be >= bottom-right y {y1}" - # Also ensure within full field - assert -fx <= x0 <= fx and -fx <= x1 <= fx, f"x coordinates out of full field bounds ±{fx}" - assert -fy <= y0 <= fy and -fy <= y1 <= fy, f"y coordinates out of full field bounds ±{fy}" + + # Shape validity + if x0 > x1: + raise ValueError(f"top-left x {x0} must be <= bottom-right x {x1}") + if y0 < y1: + raise ValueError(f"top-left y {y0} must be >= bottom-right y {y1}") + + # Within global field bounds + if not (-fx <= x0 <= fx and -fx <= x1 <= fx): + raise ValueError(f"x coordinates out of full field bounds +/-{fx}") + if not (-fy <= y0 <= fy and -fy <= y1 <= fy): + raise ValueError(f"y coordinates out of full field bounds +/-{fy}") + + +def assert_contains(outer: FieldBounds, inner: FieldBounds): + """Validate that one bounding box fully contains another.""" + ox0, oy0 = outer.top_left + ox1, oy1 = outer.bottom_right + + ix0, iy0 = inner.top_left + ix1, iy1 = inner.bottom_right + + if ox0 > ix0: + raise ValueError(f"Outer left {ox0} does not contain inner left {ix0}") + if oy0 < iy0: + raise ValueError(f"Outer top {oy0} does not contain inner top {iy0}") + if ox1 < ix1: + raise ValueError(f"Outer right {ox1} does not contain inner right {ix1}") + if oy1 > iy1: + raise ValueError(f"Outer bottom {oy1} does not contain inner bottom {iy1}") diff --git a/utama_core/motion_planning/src/planning/controller.py b/utama_core/motion_planning/src/planning/controller.py deleted file mode 100644 index 963deb60..00000000 --- a/utama_core/motion_planning/src/planning/controller.py +++ /dev/null @@ -1,115 +0,0 @@ -from enum import Enum -from typing import Tuple - -from utama_core.entities.game.field import Field -from utama_core.entities.game.game_frame import GameFrame -from utama_core.motion_planning.src.planning.exit_strategies import ExitStrategy -from utama_core.motion_planning.src.planning.other_path_planners import ( - BisectorPlanner, - DynamicWindowPlanner, -) - - -class TempObstacleType(Enum): - NONE = [] - FIELD = [Field.FULL_FIELD] - DEFENCE_ZONES = [Field.LEFT_DEFENSE_AREA, Field.RIGHT_DEFENSE_AREA] - ALL = [Field.LEFT_DEFENSE_AREA, Field.RIGHT_DEFENSE_AREA, Field.FULL_FIELD] - - -class TimedSwitchController: - """Takes two planners, one run per frame and one run per N frames, idea is that the slower planner gives more - accurate global guidance.""" - - DEFAULT_RUN = 60 # SLow planner is invoked once every DEFAULT_RUN frames - - def __init__( - self, - num_robots: int, - game: GameFrame, - exit_strategy: ExitStrategy, - friendly_colour, - env, - ): - self.num_robots = num_robots - self._exit_strategy = exit_strategy - self._slow_planner = BisectorPlanner(game, friendly_colour, env) - self._game = game - self._fast_planner = DynamicWindowPlanner(game) - self._real_targets = [None for _ in range(num_robots)] - self._intermediate_target = [None for _ in range(num_robots)] - self._exit_points = [None for _ in range(num_robots)] - self._last_slow_frame = [0 for _ in range(num_robots)] - - # DEBUG ONLY - self._env = env - - def path_to( - self, - target: Tuple[float, float], - robot_id: int, - temporary_obstacles_enum: TempObstacleType, - ) -> Tuple[float, float]: - """Computes the path to the given target for the specified robot, considering temporary obstacles such as - defence zones, field or None. - - Args: - target (Tuple[float, float]): The target coordinates (x, y) to which the robot should navigate. - robot_id (int): The identifier of the robot for which the path is being computed. - temporary_obstacles_enum (TempObstacleType): An enumeration indicating the type of temporary obstacles to consider. - - Returns: - Tuple[float, float]: The next coordinates (x, y) in the path to the target. - """ - robot_position = self._game.friendly_robots[robot_id].p - - if self._exit_points[robot_id] is None: - required_exit_point = self._exit_strategy.get_exit_point( - (robot_position.x, robot_position.y), temporary_obstacles_enum.value - ) - if required_exit_point is not None: - # Should not be too far inside the obstacle, use the fast planner with no temporary obstacles - # to give a safe path to the edge of the obstacle - self._exit_points[robot_id] = required_exit_point - - if self._exit_points[robot_id] is not None: - print("ALREADY trying to exit", self._exit_points[robot_id]) - if ExitStrategy.is_close_enough_to_exit_point( - (robot_position.x, robot_position.y), self._exit_points[robot_id] - ): - self._exit_points[robot_id] = None - else: - plan = self._fast_planner.path_to( - robot_id, - self._exit_points[robot_id], - temporary_obstacles=TempObstacleType.NONE.value, - ) - if plan is None: - return self._exit_points[robot_id] - return plan.waypoint - - if target == self._real_targets[robot_id]: - if self._last_slow_frame[robot_id] == 0: - # Invoke the slow planner and reset the counter - self._intermediate_target[robot_id] = self._slow_planner.path_to( - robot_id, target, temporary_obstacles=temporary_obstacles_enum.value - ) - self._last_slow_frame[robot_id] = self.DEFAULT_RUN - else: - # Count down until the next slow frame - self._last_slow_frame[robot_id] -= 1 - else: - self._real_targets[robot_id] = target - self._intermediate_target[robot_id] = self._slow_planner.path_to( - robot_id, target, temporary_obstacles=temporary_obstacles_enum.value - ) - self._last_slow_frame[robot_id] = self.DEFAULT_RUN - - plan = self._fast_planner.path_to( - robot_id, - self._intermediate_target[robot_id], - temporary_obstacles=temporary_obstacles_enum.value, - ) - if plan is None: - return self._intermediate_target[robot_id] - return plan.waypoint diff --git a/utama_core/rsoccer_simulator/src/Render/field.py b/utama_core/rsoccer_simulator/src/Render/field.py index 3583de27..25febcbb 100644 --- a/utama_core/rsoccer_simulator/src/Render/field.py +++ b/utama_core/rsoccer_simulator/src/Render/field.py @@ -263,6 +263,38 @@ class SSLRenderField(VSSRenderField): corner_arc_r = 0.01 _scale = 100 + def __init__( + self, + length: float | None = None, + width: float | None = None, + penalty_length: float | None = None, + penalty_width: float | None = None, + goal_width: float | None = None, + goal_depth: float | None = None, + margin: float | None = None, + center_circle_r: float | None = None, + scale: float | None = None, + ): + if length is not None: + self.length = length + if width is not None: + self.width = width + if penalty_length is not None: + self.penalty_length = penalty_length + if penalty_width is not None: + self.penalty_width = penalty_width + if goal_width is not None: + self.goal_width = goal_width + if goal_depth is not None: + self.goal_depth = goal_depth + if margin is not None: + self.margin = margin + if center_circle_r is not None: + self.center_circle_r = center_circle_r + if scale is not None: + self._scale = scale + super().__init__() + if __name__ == "__main__": field = Sim2DRenderField() diff --git a/utama_core/rsoccer_simulator/src/ssl/envs/standard_ssl.py b/utama_core/rsoccer_simulator/src/ssl/envs/standard_ssl.py index 1eb026da..f3e81723 100644 --- a/utama_core/rsoccer_simulator/src/ssl/envs/standard_ssl.py +++ b/utama_core/rsoccer_simulator/src/ssl/envs/standard_ssl.py @@ -1,10 +1,11 @@ import logging import random -from typing import List, Tuple +from typing import List, Optional, Tuple from numpy.random import normal -from utama_core.config.formations import LEFT_START_ONE, RIGHT_START_ONE +from utama_core.config.field_params import STANDARD_FIELD_DIMS, FieldDimensions +from utama_core.config.formations import FormationEntry, FormationType, get_formations from utama_core.config.robot_params import RSIM_PARAMS from utama_core.config.settings import ( MAX_BALL_SPEED, @@ -80,24 +81,52 @@ def __init__( n_robots_blue: int = 6, n_robots_yellow: int = 6, time_step: float = TIMESTEP, - blue_starting_formation: list[tuple] = None, - yellow_starting_formation: list[tuple] = None, + blue_starting_formation: Optional[list[FormationEntry]] = None, + yellow_starting_formation: Optional[list[FormationEntry]] = None, + full_field_dims: Optional[FieldDimensions] = None, + ball_starting_position: Optional[Tuple[float, float]] = None, gaussian_noise: RsimGaussianNoise = RsimGaussianNoise(), vanishing: float = 0, ): + render_field_overrides = None + if full_field_dims is not None: + render_field_overrides = { + "length": 2 * full_field_dims.full_field_half_length, + "width": 2 * full_field_dims.full_field_half_width, + "penalty_length": 2 * full_field_dims.half_defense_area_depth, + "penalty_width": 2 * full_field_dims.half_defense_area_width, + "goal_width": 2 * full_field_dims.half_goal_width, + } + super().__init__( field_type=field_type, n_robots_blue=n_robots_blue, n_robots_yellow=n_robots_yellow, time_step=time_step, render_mode=render_mode, + render_field_overrides=render_field_overrides, ) - # Note: observation_space and action_space removed - not needed for non-RL use + # NOTE: observation_space and action_space removed - not needed for non-RL use + + self.blue_formation = blue_starting_formation + self.yellow_formation = yellow_starting_formation + + # NOTE: in a normal strategy, formations will never be None + # This implementation is to allow rsim to be used as standalone + # Makes the implicit assumption of blue on left and yellow on right if formation not given + # This assumption is faulty for normal strats, but it is necessary if rsim is spawned in isolation with no strat context + if blue_starting_formation is None or yellow_starting_formation is None: + self.blue_formation, self.yellow_formation = get_formations( + STANDARD_FIELD_DIMS.full_field_bounds, + n_left=n_robots_blue, + n_right=n_robots_yellow, + formation_type=FormationType.START_ONE, + ) - # set starting formation style for - self.blue_formation = LEFT_START_ONE if not blue_starting_formation else blue_starting_formation - self.yellow_formation = RIGHT_START_ONE if not yellow_starting_formation else yellow_starting_formation + # Ball start position is expressed in normal field coordinates used by + # StrategyRunner/game state (not simulator-internal y-sign convention). + self.ball_starting_position = ball_starting_position if ball_starting_position is not None else (0.0, 0.0) # Track dribbler state across steps so we can model ball release when # the dribbler turns off in a way that depends on robot speed. @@ -368,7 +397,8 @@ def _get_initial_positions_frame(self, ball_exists: bool = True) -> Frame: pos_frame.robots_yellow[i] = Robot(id=i, x=x, y=-y, theta=-rad_to_deg(heading)) if ball_exists: - pos_frame.ball = Ball(x=0, y=0) + bx, by = self.ball_starting_position + pos_frame.ball = Ball(x=bx, y=-by) else: pos_frame.ball = Ball( x=self.OFF_FIELD_OFFSET + self.field.length, 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 6f023518..dec6ad79 100644 --- a/utama_core/rsoccer_simulator/src/ssl/ssl_gym_base.py +++ b/utama_core/rsoccer_simulator/src/ssl/ssl_gym_base.py @@ -4,7 +4,7 @@ # - To create your wrapper from env to communcation, use inherit from this class! """ -from typing import List +from typing import List, Optional import numpy as np import pygame @@ -42,6 +42,7 @@ def __init__( n_robots_yellow: int, time_step: float, render_mode=None, + render_field_overrides: Optional[dict[str, float]] = None, ): # Initialize Simulator self.render_mode = render_mode @@ -73,7 +74,7 @@ def __init__( self.overlay: list[OverlayObject] = [] # Render - self.field_renderer = SSLRenderField() + self.field_renderer = SSLRenderField(**render_field_overrides) if render_field_overrides else SSLRenderField() self.window_surface = None self.window_size = self.field_renderer.window_size self.clock = None diff --git a/utama_core/run/game_gater.py b/utama_core/run/game_gater.py index 867156d8..6e6a7318 100644 --- a/utama_core/run/game_gater.py +++ b/utama_core/run/game_gater.py @@ -29,15 +29,27 @@ def wait_until_game_valid( A tuple containing the refined game frame for the player's team and the opponent's team (if is_pvp is True). """ + def print_current_vision(game_frame: GameFrame): + print("Waiting for valid game frame...") + print(f"Friendly robots: {len(game_frame.friendly_robots)}/{exp_friendly}") + print(f"Enemy robots: {len(game_frame.enemy_robots)}/{exp_enemy}") + print(f"Ball present: {game_frame.ball is not None} (exp: {exp_ball})\n") + def _add_frame(my_game_frame: GameFrame, opp_game_frame: GameFrame) -> Tuple[GameFrame, Optional[GameFrame]]: if rsim_env: obs = rsim_env.step_noop() # Step the environment without action to get the latest observation vision_frames = [obs] else: vision_frames = [buffer.popleft() if buffer else None for buffer in vision_buffers] - my_game_frame = position_refiner.refine(my_game_frame, vision_frames) + my_game_frame = position_refiner.refine( + my_game_frame, + vision_frames, + ) if is_pvp: - opp_game_frame = position_refiner.refine(opp_game_frame, vision_frames) + opp_game_frame = position_refiner.refine( + opp_game_frame, + vision_frames, + ) return my_game_frame, opp_game_frame @@ -59,17 +71,14 @@ def _add_frame(my_game_frame: GameFrame, opp_game_frame: GameFrame) -> Tuple[Gam ): if time.time() - start_time > wait_before_warn: start_time = time.time() - print("Waiting for valid game frame...") - print(f"Friendly robots: {len(my_game_frame.friendly_robots)}/{exp_friendly}") - print(f"Enemy robots: {len(my_game_frame.enemy_robots)}/{exp_enemy}") - print(f"Ball present: {my_game_frame.ball is not None} (exp: {exp_ball})\n") - + print_current_vision(my_game_frame) # nothing will change in rsim if we don't step it. # if no valid frame, likely misconfigured. - if rsim_env: - raise TimeoutError( - f"Rsim environment did not produce a valid game frame after {wait_before_warn} seconds. Check the environment setup and vision data." - ) + if rsim_env: + print_current_vision(my_game_frame) + raise TimeoutError( + f"Rsim environment did not produce a valid game frame after {wait_before_warn} seconds. Check the environment setup and vision data." + ) time.sleep(0.05) my_game_frame, opp_game_frame = _add_frame(my_game_frame, opp_game_frame) diff --git a/utama_core/run/strategy_runner.py b/utama_core/run/strategy_runner.py index f2b1f602..2d1a8838 100644 --- a/utama_core/run/strategy_runner.py +++ b/utama_core/run/strategy_runner.py @@ -12,7 +12,8 @@ from rich.text import Text from utama_core.config.enums import Mode, mode_str_to_enum -from utama_core.config.formations import LEFT_START_ONE, RIGHT_START_ONE +from utama_core.config.field_params import STANDARD_FIELD_DIMS, FieldDimensions +from utama_core.config.formations import FormationType, get_formations from utama_core.config.physical_constants import MAX_ROBOTS from utama_core.config.settings import ( FPS_PRINT_INTERVAL, @@ -102,7 +103,8 @@ class StrategyRunner: exp_ball (bool): Whether the ball is expected to be present. Only raises error when strategy expects ball but runtime does not provide it. Defaults to True. - field_bounds (FieldBounds): Configuration of the field. Defaults to standard field. + full_field_dims (FieldDimensions): The dimensions of the full field. Defaults to standard field dimensions. + field_bounds (FieldBounds): Bounds of the subset of the full field being used. Defaults to None (ie full field). opp_strategy (AbstractStrategy, optional): Opponent strategy for pvp. Defaults to None for single player. control_scheme (str, optional): Name of the motion control scheme to use. opp_control_scheme (str, optional): Name of the opponent motion control scheme to use. If not set, uses same as friendly. @@ -126,7 +128,8 @@ def __init__( exp_friendly: int, exp_enemy: int, exp_ball: bool = True, - field_bounds: FieldBounds = Field.FULL_FIELD_BOUNDS, + full_field_dims: FieldDimensions = STANDARD_FIELD_DIMS, + field_bounds: Optional[FieldBounds] = None, opp_strategy: Optional[AbstractStrategy] = None, control_scheme: str = "pid", # This is also the default control scheme used in the motion planning tests opp_control_scheme: Optional[str] = None, @@ -145,11 +148,17 @@ def __init__( self.exp_friendly = exp_friendly self.exp_enemy = exp_enemy self.exp_ball = exp_ball - self.field_bounds = field_bounds + self.full_field_dims = full_field_dims + # if field bounds not provided, default to full field dimensions + self.field_bounds = field_bounds if field_bounds else full_field_dims.full_field_bounds self.vision_buffers, self.ref_buffer = self._setup_vision_and_referee() - assert_valid_bounding_box(self.field_bounds) + assert_valid_bounding_box( + self.field_bounds, + full_field_dims.full_field_half_length, + full_field_dims.full_field_half_width, + ) self.my, self.opp = self._setup_sides_data( strategy, opp_strategy, filtering, control_scheme, opp_control_scheme @@ -169,9 +178,12 @@ def __init__( if self.rsim_env and not self.exp_ball: self._remove_rsim_ball() + # Load all game related data self._load_game() - self._assert_exp_goals() + self.my.strategy.setup_behaviour_tree(is_opp_strat=False) + if self.opp: + self.opp.strategy.setup_behaviour_tree(is_opp_strat=True) self.toggle_opp_first = False # used to alternate the order of opp and friendly in run @@ -276,10 +288,10 @@ def _setup_sides_data( """ opp_side = None my_pos_ref, my_vel_ref, my_robot_ref = self._init_refiners( - self.field_bounds, filtering=filtering, exp_ball=self.exp_ball + self.full_field_dims, filtering=filtering, exp_ball=self.exp_ball ) my_motion_controller = get_control_scheme(control_scheme) - my_strategy.setup_behaviour_tree(is_opp_strat=False) + my_strategy.setup_strategy_blackboard(is_opp_strat=False) my_side = SideRuntime( strategy=my_strategy, position_refiner=my_pos_ref, @@ -290,12 +302,12 @@ def _setup_sides_data( if opp_strategy is not None: opp_pos_ref, opp_vel_ref, opp_robot_ref = self._init_refiners( - self.field_bounds, filtering=filtering, exp_ball=self.exp_ball + self.full_field_dims, filtering=filtering, exp_ball=self.exp_ball ) opp_motion_controller = ( get_control_scheme(opp_control_scheme) if opp_control_scheme is not None else my_motion_controller ) - opp_strategy.setup_behaviour_tree(is_opp_strat=True) + opp_strategy.setup_strategy_blackboard(is_opp_strat=True) opp_side = SideRuntime( strategy=opp_strategy, position_refiner=opp_pos_ref, @@ -331,12 +343,31 @@ def _load_sim( SSLBaseEnv: The RSim environment (Otherwise None). AbstractSimController: The simulation controller for the environment (Otherwise None). """ + # No sim to load for real mode. + if self.mode == Mode.REAL: + return None, None + + left_start, right_start = get_formations( + bounds=self.field_bounds, + n_left=self.exp_enemy if self.my_team_is_right else self.exp_friendly, + n_right=self.exp_friendly if self.my_team_is_right else self.exp_enemy, + formation_type=FormationType.START_ONE, + ) + + yellow_start, blue_start = map_left_right_to_colors( + self.my_team_is_yellow, self.my_team_is_right, right_start, left_start + ) + if self.mode == Mode.RSIM: n_yellow, n_blue = map_friendly_enemy_to_colors(self.my_team_is_yellow, self.exp_friendly, self.exp_enemy) rsim_env = SSLStandardEnv( n_robots_yellow=n_yellow, n_robots_blue=n_blue, render_mode=None, + blue_starting_formation=blue_start, + yellow_starting_formation=yellow_start, + full_field_dims=self.full_field_dims, + ball_starting_position=self.field_bounds.center, gaussian_noise=rsim_noise, vanishing=rsim_vanishing, ) @@ -347,7 +378,8 @@ def _load_sim( return rsim_env, RSimController(field_bounds=self.field_bounds, exp_ball=self.exp_ball, env=rsim_env) - elif self.mode == Mode.GRSIM: + # GRSIM Mode + else: # can consider baking all of these directly into sim controller sim_controller = GRSimController(self.field_bounds, self.exp_ball) n_yellow, n_blue = map_friendly_enemy_to_colors(self.my_team_is_yellow, self.exp_friendly, self.exp_enemy) @@ -362,12 +394,6 @@ def _load_sim( y_to_keep = [i for i in range(n_yellow)] b_to_keep = [i for i in range(n_blue)] - yellow_start, blue_start = map_left_right_to_colors( - self.my_team_is_yellow, - self.my_team_is_right, - RIGHT_START_ONE, - LEFT_START_ONE, - ) for y in y_to_keep: sim_controller.set_robot_presence(y, True, True) y_start = yellow_start[y] @@ -378,15 +404,12 @@ def _load_sim( sim_controller.teleport_robot(False, b, b_start[0], b_start[1], b_start[2]) if self.exp_ball: - sim_controller.teleport_ball(0, 0) + sim_controller.teleport_ball(self.field_bounds.center[0], self.field_bounds.center[1]) else: sim_controller.remove_ball() return None, sim_controller - else: - return None, None - def _setup_vision_and_referee(self) -> Tuple[deque, deque]: """Setup the vision and referee buffers. @@ -457,15 +480,17 @@ def _assert_exp_robots_and_ball( def _assert_exp_goals(self): """Assert the expected number of goals.""" - assert self.my.strategy.assert_exp_goals( + if not self.my.strategy.assert_exp_goals( self.my.game.field.includes_my_goal_line, self.my.game.field.includes_opp_goal_line, - ), "Field does not match expected goals for my strategy." + ): + raise RuntimeError("Field does not match expected goals for my strategy.") if self.opp: - assert self.opp.strategy.assert_exp_goals( + if not self.opp.strategy.assert_exp_goals( self.opp.game.field.includes_my_goal_line, self.opp.game.field.includes_opp_goal_line, - ), "Field does not match expected goals for opponent strategy." + ): + raise RuntimeError("Field does not match expected goals for opponent strategy.") def _load_robot_controllers(self): """ @@ -524,7 +549,7 @@ def _load_robot_controllers(self): def _init_refiners( self, - field_bounds: FieldBounds, + field_dims: FieldDimensions, filtering: bool, exp_ball: bool = True, ) -> tuple[PositionRefiner, VelocityRefiner, RobotInfoRefiner]: @@ -539,7 +564,7 @@ def _init_refiners( tuple: The initialized PositionRefiner, VelocityRefiner, and RobotInfoRefiner. """ position_refiner = PositionRefiner( - field_bounds, + field_dims, filtering=filtering, exp_ball=exp_ball, ) @@ -570,13 +595,13 @@ def _load_game(self): if self.opp: self.opp.position_refiner.start_filtering() - my_field = Field(self.my_team_is_right, self.field_bounds) + my_field = Field(self.my_team_is_right, self.full_field_dims, self.field_bounds) self.my.game_history = GameHistory(MAX_GAME_HISTORY) self.my.game = Game(self.my.game_history, my_current_game_frame, field=my_field) self.my.current_game_frame = my_current_game_frame if self.opp: - opp_field = Field(not self.my_team_is_right, self.field_bounds) + opp_field = Field(not self.my_team_is_right, self.full_field_dims, self.field_bounds) self.opp.game_history = GameHistory(MAX_GAME_HISTORY) self.opp.game = Game(self.opp.game_history, opp_current_game_frame, field=opp_field) self.opp.current_game_frame = opp_current_game_frame @@ -599,53 +624,39 @@ def _reset_game(self): self.opp.position_refiner.reset() self._load_game() - def _reset_robots(self): - """Send zero-velocity commands to all robots to stop them. - - Ensures both friendly and opponent robots (if present) receive - zeroed commands and that those commands are sent immediately. - """ - for i in self.my.current_game_frame.friendly_robots.keys(): - self.my.strategy.robot_controller.add_robot_commands(RobotCommand(0, 0, 0, 0, 0, 0), i) - self.my.strategy.robot_controller.send_robot_commands() - - if self.opp and self.opp.current_game_frame: - for i in self.opp.current_game_frame.friendly_robots.keys(): - self.opp.strategy.robot_controller.add_robot_commands(RobotCommand(0, 0, 0, 0, 0, 0), i) - self.opp.strategy.robot_controller.send_robot_commands() - - def _stop_robots(self, stop_command_mult: int): + def _stop_robots(self, repeat: int = 1): """ - Send a series of stop commands to all robots to ensure they come to a halt. + Send stop commands to the robots. Args: - stop_command_mult (int): Number of times to send the stop command. + repeat (int): Number of times to send the stop command. """ - my_stop_commands = { - robot_id: RobotCommand(0, 0, 0, 0, 0, 0) for robot_id in self.my.game.friendly_robots.keys() - } - if self.opp and self.opp.game: - opp_stop_commands = { - robot_id: RobotCommand(0, 0, 0, 0, 0, 0) for robot_id in self.opp.game.friendly_robots.keys() - } - - for _ in range(stop_command_mult): - self.my.strategy.robot_controller.add_robot_commands(my_stop_commands) - self.my.strategy.robot_controller.send_robot_commands() - if self.opp and self.opp.game: - self.opp.strategy.robot_controller.add_robot_commands(opp_stop_commands) + + def build_commands(team: SideRuntime) -> dict[int, RobotCommand]: + return {robot_id: RobotCommand(0, 0, 0, 0, 0, 0) for robot_id in team.game.friendly_robots.keys()} + + my_cmds = build_commands(self.my) if self.my.game is not None else None + opp_cmds = build_commands(self.opp) if self.opp and self.opp.game is not None else None + + for _ in range(repeat): + if my_cmds: + self.my.strategy.robot_controller.add_robot_commands(my_cmds) + self.my.strategy.robot_controller.send_robot_commands() + + if opp_cmds: + self.opp.strategy.robot_controller.add_robot_commands(opp_cmds) self.opp.strategy.robot_controller.send_robot_commands() - def close(self, stop_command_mult: int = 20): + def close(self, stop_command_repeat: int = 20): """ Close resources used by the StrategyRunner and stop robots if in real mode. Args: - stop_command_mult (int): Number of times to send the stop command to robots. + stop_command_repeat (int): Number of times to send the stop command to robots. """ self.logger.info("Cleaning up resources...") if self.mode == Mode.REAL: try: - self._stop_robots(stop_command_mult) + self._stop_robots(repeat=stop_command_repeat) except Exception: self.logger.exception("Was unable to stop robots cleanly.") if self.profiler: @@ -721,10 +732,10 @@ def run_test( if status == TestingStatus.FAILURE: passed = False - self._reset_robots() + self._stop_robots() break elif status == TestingStatus.SUCCESS: - self._reset_robots() + self._stop_robots() break if self._stop_event.is_set(): @@ -773,6 +784,8 @@ def _run_step(self): No return value; updates internal game state and controllers. """ frame_start = time.perf_counter() + self._draw_rsim_field_bounds_overlay() + if self.mode == Mode.RSIM: vision_frames = [self.rsim_env._frame_to_observations()[0]] else: @@ -814,6 +827,26 @@ def _run_step(self): self.elapsed_time = 0.0 self.num_frames_elapsed = 0 + def _draw_rsim_field_bounds_overlay(self) -> None: + """Draw active field bounds overlay in RSIM human render mode.""" + if self.mode != Mode.RSIM or not self.rsim_env: + return + + # Overlays are cleared during render; only enqueue when the frame will be rendered. + if self.rsim_env.render_mode != "human": + return + + top_left = self.field_bounds.top_left + bottom_right = self.field_bounds.bottom_right + + bounds_polygon = [ + (top_left[0], top_left[1]), + (bottom_right[0], top_left[1]), + (bottom_right[0], bottom_right[1]), + (top_left[0], bottom_right[1]), + ] + self.rsim_env.draw_polygon(bounds_polygon, color="PINK", width=2) + def _step_game( self, vision_frames: List[RawVisionData], diff --git a/utama_core/skills/src/goalkeep.py b/utama_core/skills/src/goalkeep.py index 888d1209..6b5ac57e 100644 --- a/utama_core/skills/src/goalkeep.py +++ b/utama_core/skills/src/goalkeep.py @@ -2,6 +2,7 @@ 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 @@ -10,6 +11,8 @@ from utama_core.skills.src.go_to_point import go_to_point from utama_core.skills.src.utils.move_utils import face_ball, move +# TODO: instead of checking number of friendly, should check roles + def goalkeep( game: Game, @@ -17,26 +20,27 @@ def goalkeep( robot_id: int, env: Optional[SSLStandardEnv] = None, ): - if game.my_team_is_right: - target = predict_ball_pos_at_x(game, 4.5) - else: - target = predict_ball_pos_at_x(game, -4.5) + 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) stop_y = 0.0 - def intersection_with_vertical_line(a, b, x_line=4.5): + def intersection_with_vertical_line(a, b): xa, ya = a xb, yb = b - if xb < xa: - return a + + if xb == xa: + return a # Line is vertical, return the point itself k = (yb - ya) / (xb - xa) - y_intersect = ya + k * (x_line - xa) - if y_intersect < -0.5: - return (x_line, -0.5) - elif y_intersect > 0.5: - return (x_line, 0.5) - return (x_line, y_intersect) + 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: try: @@ -45,10 +49,31 @@ def intersection_with_vertical_line(a, b, x_line=4.5): not game.my_team_is_right and game.friendly_robots[1].p.x < game.ball.p.x ) if defender_between: - _, yy = intersection_with_vertical_line( - (game.ball.p.x, game.ball.p.y), (game.friendly_robots[1].p.x, game.friendly_robots[1].p.y + 0.1) + # 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, + ), ) - stop_y = (yy + 0.5) / 2 + _, 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, + ), + ) + + # 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 @@ -63,17 +88,25 @@ def intersection_with_vertical_line(a, b, x_line=4.5): ) 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 + 0.1) + (game.ball.p.x, game.ball.p.y), + ( + game.friendly_robots[1].p.x, + game.friendly_robots[1].p.y + EDGE_OFFSET, + ), ) _, 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 - 0.1) + (game.ball.p.x, game.ball.p.y), + ( + game.friendly_robots[2].p.x, + game.friendly_robots[2].p.y - EDGE_OFFSET, + ), ) 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]) > 0.5: - target = Vector2D(4.5 if game.my_team_is_right else -4.5, stop_y) + 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) diff --git a/utama_core/strategy/common/__init__.py b/utama_core/strategy/common/__init__.py index e5598519..c8290df5 100644 --- a/utama_core/strategy/common/__init__.py +++ b/utama_core/strategy/common/__init__.py @@ -1,3 +1,6 @@ from utama_core.strategy.common.abstract_behaviour import AbstractBehaviour -from utama_core.strategy.common.abstract_strategy import AbstractStrategy +from utama_core.strategy.common.abstract_strategy import ( + AbstractStrategy, + SpaceRequirements, +) from utama_core.strategy.common.base_blackboard import BaseBlackboard diff --git a/utama_core/strategy/common/abstract_strategy.py b/utama_core/strategy/common/abstract_strategy.py index 5a1a4dd7..9444554b 100644 --- a/utama_core/strategy/common/abstract_strategy.py +++ b/utama_core/strategy/common/abstract_strategy.py @@ -1,5 +1,6 @@ import logging from abc import ABC, abstractmethod +from dataclasses import dataclass from typing import Optional, cast import py_trees @@ -11,7 +12,10 @@ 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.global_utils.math_utils import assert_valid_bounding_box +from utama_core.global_utils.math_utils import ( + assert_contains, + assert_valid_bounding_box, +) 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 @@ -55,6 +59,21 @@ def prune_nodes(container: pydot.Dot) -> None: prune_nodes(graph) +@dataclass(slots=True, frozen=True) +class SpaceRequirements: + """ + Represents minimum space requirements for a strategy. + + Attributes: + min_length (float): Minimum required length of the field region. + min_width (float): Minimum required width of the field region. + """ + + min_length: float + min_width: float + + +@dataclass class AbstractStrategy(ABC): """ Base class for team strategies backed by behaviour trees. @@ -124,7 +143,7 @@ def assert_exp_goals(self, includes_my_goal_line: bool, includes_opp_goal_line: ... @abstractmethod - def get_min_bounding_zone(self) -> Optional[FieldBounds]: + def get_min_bounding_req(self) -> Optional[FieldBounds | SpaceRequirements]: """ Return the minimum field region required by the strategy. @@ -139,7 +158,9 @@ def get_min_bounding_zone(self) -> Optional[FieldBounds]: The bounding zone should be defined in field coordinates (i.e., absolute positions). Returns: - Optional[FieldBounds]: A `FieldBounds` specifying the minimum bounding region, or `None`. + Optional[FieldBounds | SpaceRequirements]: + A `FieldBounds` object specifying the required region, a `SpaceRequirements` + object specifying minimum length and width, or `None` if no specific region is required. """ ... @@ -163,13 +184,20 @@ def execute_default_action(self, game: Game, role: Role, robot_id: int) -> Robot ### END OF STRATEGY IMPLEMENTATION ### + def setup_strategy_blackboard(self, is_opp_strat: bool): + """ + Must be called before blackboard can be used. + + Setups the blackboard based on if is_opp_strat. + """ + self.blackboard = self._setup_blackboard(is_opp_strat) + def setup_behaviour_tree(self, is_opp_strat: bool): """ Must be called before strategy can be run. - Setups the tree and blackboard based on if is_opp_strat + Setups the behaviour tree based on if is_opp_strat. """ - self.blackboard = self._setup_blackboard(is_opp_strat) self.behaviour_tree.setup(is_opp_strat=is_opp_strat) def load_rsim_env(self, env: SSLBaseEnv): @@ -192,29 +220,41 @@ def load_motion_controller(self, motion_controller: MotionController): self.blackboard.set("motion_controller", motion_controller, overwrite=True) self.blackboard.register_key(key="motion_controller", access=py_trees.common.Access.READ) - def assert_field_requirements(self): + def assert_field_requirements(self, game: Game): """ Assert that the actual field size meets the strategy's requirements, that both actual field and min_bounding_zone are within the full field, and that bounding boxes are well-formed (top-left above/left of bottom-right). """ - actual_field_size = self.blackboard.game.field.field_bounds - min_bounding_zone = self.get_min_bounding_zone() + actual_field_bounds = game.field.field_bounds + min_bounding_req = self.get_min_bounding_req() # --- Validate min bounding zone --- - if min_bounding_zone is not None: - assert_valid_bounding_box(min_bounding_zone) - - # --- Check that actual field contains min_bounding_zone --- - ax0, ay0 = actual_field_size.top_left - ax1, ay1 = actual_field_size.bottom_right - mx0, my0 = min_bounding_zone.top_left - mx1, my1 = min_bounding_zone.bottom_right - - assert ax0 <= mx0, f"Field top-left x {ax0} smaller than required {mx0}" - assert ay0 >= my0, f"Field top-left y {ay0} smaller than required {my0}" - assert ax1 >= mx1, f"Field bottom-right x {ax1} smaller than required {mx1}" - assert ay1 <= my1, f"Field bottom-right y {ay1} smaller than required {my1}" + if min_bounding_req is not None: + # Validate required zone + if isinstance(min_bounding_req, FieldBounds): + assert_valid_bounding_box( + min_bounding_req, + game.field.full_field_half_length, + game.field.full_field_half_width, + ) + # Check containment + assert_contains(actual_field_bounds, min_bounding_req) + elif isinstance(min_bounding_req, SpaceRequirements): + # Check if the actual field is large enough + actual_length = actual_field_bounds.bottom_right[0] - actual_field_bounds.top_left[0] + actual_width = actual_field_bounds.top_left[1] - actual_field_bounds.bottom_right[1] + if actual_length < min_bounding_req.min_length: + raise ValueError( + "Field bound length too small for strategy. " + f"Actual length: {actual_length}, required minimum length: {min_bounding_req.min_length}." + ) + + if actual_width < min_bounding_req.min_width: + raise ValueError( + "Field bound width too small for strategy. " + f"Actual width: {actual_width}, required minimum width: {min_bounding_req.min_width}." + ) def load_game(self, game: Game): """ @@ -223,7 +263,7 @@ def load_game(self, game: Game): We do not set to READ after, as we TestManager may reset the game object for the new episode. """ self.blackboard.set("game", game, overwrite=True) - self.assert_field_requirements() + self.assert_field_requirements(game) def step(self): # start_time = time.time() diff --git a/utama_core/strategy/examples/defense_strategy.py b/utama_core/strategy/examples/defense_strategy.py index 62ce6073..ca394c5d 100644 --- a/utama_core/strategy/examples/defense_strategy.py +++ b/utama_core/strategy/examples/defense_strategy.py @@ -201,7 +201,7 @@ def assert_exp_robots(self, n_runtime_friendly: int, n_runtime_enemy: int): def assert_exp_goals(self, includes_my_goal_line, includes_opp_goal_line): return True # No specific goal line requirements - def get_min_bounding_zone(self): + def get_min_bounding_req(self): return None # No specific bounding zone requirements def execute_default_action(self, game: Game, role: Role, robot_id: int): diff --git a/utama_core/strategy/examples/go_to_ball_ex.py b/utama_core/strategy/examples/go_to_ball_ex.py index 36b707f2..d98bb103 100644 --- a/utama_core/strategy/examples/go_to_ball_ex.py +++ b/utama_core/strategy/examples/go_to_ball_ex.py @@ -146,7 +146,7 @@ def assert_exp_robots(self, n_runtime_friendly: int, n_runtime_enemy: int): def assert_exp_goals(self, includes_my_goal_line: bool, includes_opp_goal_line: bool): return True - def get_min_bounding_zone(self): + def get_min_bounding_req(self): return None def create_behaviour_tree(self) -> py_trees.behaviour.Behaviour: diff --git a/utama_core/strategy/examples/motion_planning/multi_robot_navigation_strategy.py b/utama_core/strategy/examples/motion_planning/multi_robot_navigation_strategy.py index 0e2a1bc5..6e521ad6 100644 --- a/utama_core/strategy/examples/motion_planning/multi_robot_navigation_strategy.py +++ b/utama_core/strategy/examples/motion_planning/multi_robot_navigation_strategy.py @@ -37,8 +37,7 @@ def assert_exp_robots(self, n_runtime_friendly: int, n_runtime_enemy: int): def assert_exp_goals(self, includes_my_goal_line: bool, includes_opp_goal_line: bool): return True - def get_min_bounding_zone(self) -> Optional[FieldBounds]: - """Calculate bounding box for all robot targets.""" + def get_min_bounding_req(self): if not self.robot_targets: return None diff --git a/utama_core/strategy/examples/motion_planning/oscillating_obstacle_strategy.py b/utama_core/strategy/examples/motion_planning/oscillating_obstacle_strategy.py index b02afea8..df5d34aa 100644 --- a/utama_core/strategy/examples/motion_planning/oscillating_obstacle_strategy.py +++ b/utama_core/strategy/examples/motion_planning/oscillating_obstacle_strategy.py @@ -147,8 +147,7 @@ def assert_exp_robots(self, n_runtime_friendly: int, n_runtime_enemy: int): def assert_exp_goals(self, includes_my_goal_line: bool, includes_opp_goal_line: bool): return True - def get_min_bounding_zone(self) -> Optional[FieldBounds]: - """Calculate bounding box for all oscillating obstacles.""" + def get_min_bounding_req(self): if not self.obstacle_configs: return None diff --git a/utama_core/strategy/examples/motion_planning/random_movement_strategy.py b/utama_core/strategy/examples/motion_planning/random_movement_strategy.py index 0194a088..770dec38 100644 --- a/utama_core/strategy/examples/motion_planning/random_movement_strategy.py +++ b/utama_core/strategy/examples/motion_planning/random_movement_strategy.py @@ -153,7 +153,7 @@ def assert_exp_robots(self, n_runtime_friendly: int, n_runtime_enemy: int): def assert_exp_goals(self, includes_my_goal_line: bool, includes_opp_goal_line: bool): return True - def get_min_bounding_zone(self) -> Optional[FieldBounds]: + def get_min_bounding_req(self): return self.field_bounds def create_behaviour_tree(self) -> py_trees.behaviour.Behaviour: diff --git a/utama_core/strategy/examples/motion_planning/simple_navigation_strategy.py b/utama_core/strategy/examples/motion_planning/simple_navigation_strategy.py index 4ebd161d..5c6372a6 100644 --- a/utama_core/strategy/examples/motion_planning/simple_navigation_strategy.py +++ b/utama_core/strategy/examples/motion_planning/simple_navigation_strategy.py @@ -94,8 +94,7 @@ def assert_exp_robots(self, n_runtime_friendly: int, n_runtime_enemy: int): def assert_exp_goals(self, includes_my_goal_line: bool, includes_opp_goal_line: bool): return True - def get_min_bounding_zone(self) -> Optional[FieldBounds]: - """Return None to allow full field navigation.""" + def get_min_bounding_req(self): return None def create_behaviour_tree(self) -> py_trees.behaviour.Behaviour: diff --git a/utama_core/strategy/examples/one_robot_placement_strategy.py b/utama_core/strategy/examples/one_robot_placement_strategy.py index 8c570731..aa92fd84 100644 --- a/utama_core/strategy/examples/one_robot_placement_strategy.py +++ b/utama_core/strategy/examples/one_robot_placement_strategy.py @@ -13,7 +13,10 @@ from utama_core.strategy.common.abstract_behaviour import AbstractBehaviour # from robot_control.src.tests.utils import one_robot_placement -from utama_core.strategy.common.abstract_strategy import AbstractStrategy +from utama_core.strategy.common.abstract_strategy import ( + AbstractStrategy, + SpaceRequirements, +) from utama_core.strategy.examples.utils import ( CalculateFieldCenter, SetBlackboardVariable, @@ -117,28 +120,27 @@ def update(self) -> py_trees.common.Status: class RobotPlacementStrategy(AbstractStrategy): - def __init__(self, robot_id: int, field_bounds: Optional[FieldBounds] = None): + def __init__(self, robot_id: int): """ Initializes the RobotPlacementStrategy with a specific robot ID. + Robot placement oscillates the specified robot between two points centered around the middle of the field bounds. :param robot_id: The ID of the robot this strategy will control. :param field_bounds: The bounds of the field to operate within. """ self.robot_id = robot_id - self.field_bounds = field_bounds if field_bounds else Field.FULL_FIELD_BOUNDS super().__init__() def assert_exp_robots(self, n_runtime_friendly: int, n_runtime_enemy: int): - if 1 <= n_runtime_friendly <= 6: + if n_runtime_friendly == 1 and n_runtime_enemy == 0: return True return False def assert_exp_goals(self, includes_my_goal_line: bool, includes_opp_goal_line: bool): return True # No specific goal line requirements - def get_min_bounding_zone(self) -> Optional[FieldBounds]: - # toggles robot between (1, -1) and (1, 1) - return FieldBounds(top_left=(-1, 1), bottom_right=(1, -1)) + def get_min_bounding_req(self): + return SpaceRequirements(min_length=1.0, min_width=1.0) # Require at least a 1x1 area to allow for oscillation def create_behaviour_tree(self) -> py_trees.behaviour.Behaviour: """Factory function to create a complete behaviour tree.""" @@ -157,7 +159,7 @@ def create_behaviour_tree(self) -> py_trees.behaviour.Behaviour: ### Assemble the tree ### # Calculate Field Center from custom field_bounds - calc_center = CalculateFieldCenter(field_bounds=self.field_bounds, output_key=field_center_key) + calc_center = CalculateFieldCenter(output_key=field_center_key) coach_root.add_children( [ diff --git a/utama_core/strategy/examples/startup_strategy.py b/utama_core/strategy/examples/startup_strategy.py index 7080d3d6..e27d3d67 100644 --- a/utama_core/strategy/examples/startup_strategy.py +++ b/utama_core/strategy/examples/startup_strategy.py @@ -1,9 +1,10 @@ import py_trees from py_trees.composites import Sequence -from utama_core.config.formations import LEFT_START_ONE, RIGHT_START_ONE +from utama_core.config.field_params import STANDARD_FIELD_DIMS +from utama_core.config.formations import FormationType, get_formations +from utama_core.config.physical_constants import MAX_ROBOTS from utama_core.entities.data.vector import Vector2D -from utama_core.entities.game.field import FieldBounds from utama_core.global_utils.math_utils import compute_bounding_zone_from_points from utama_core.skills.src.go_to_point import go_to_point from utama_core.strategy.common import AbstractBehaviour, AbstractStrategy @@ -13,7 +14,13 @@ def generate_starting_positions(is_right_team: bool): """ Generate starting and target formations based on team side. """ - start_formation = RIGHT_START_ONE if is_right_team else LEFT_START_ONE + left_formation, right_formation = get_formations( + STANDARD_FIELD_DIMS.full_field_bounds, + MAX_ROBOTS, + MAX_ROBOTS, + formation_type=FormationType.START_ONE, + ) + start_formation = right_formation if is_right_team else left_formation target_formation = start_formation.copy() target_formation.reverse() return start_formation, target_formation @@ -55,17 +62,8 @@ def assert_exp_robots(self, n_runtime_friendly: int, n_runtime_enemy: int): def assert_exp_goals(self, includes_my_goal_line: bool, includes_opp_goal_line: bool): return True # No specific goal line requirements - def get_min_bounding_zone(self) -> FieldBounds: - all_points = [] - start_formation, target_formation = generate_starting_positions(self.blackboard.game.my_team_is_right) - - for robot_id in self.blackboard.game.friendly_robots: - fx, fy, _ = start_formation[robot_id] - all_points.append((fx, fy)) - tx, ty, _ = target_formation[robot_id] - all_points.append((tx, ty)) - - return compute_bounding_zone_from_points(all_points) + def get_min_bounding_req(self): + return STANDARD_FIELD_DIMS.full_field_bounds # enforce full field required def create_behaviour_tree(self) -> py_trees.behaviour.Behaviour: """Factory function to create a complete behaviour tree.""" diff --git a/utama_core/strategy/examples/two_robot_placement.py b/utama_core/strategy/examples/two_robot_placement.py index 32b7ff38..4fc4d50e 100644 --- a/utama_core/strategy/examples/two_robot_placement.py +++ b/utama_core/strategy/examples/two_robot_placement.py @@ -6,11 +6,14 @@ from py_trees.composites import Parallel, Sequence from utama_core.config.settings import TIMESTEP -from utama_core.entities.game.field import Field, FieldBounds +from utama_core.entities.game.field import FieldBounds from utama_core.global_utils.math_utils import Vector2D from utama_core.skills.src.utils.move_utils import move from utama_core.strategy.common.abstract_behaviour import AbstractBehaviour -from utama_core.strategy.common.abstract_strategy import AbstractStrategy +from utama_core.strategy.common.abstract_strategy import ( + AbstractStrategy, + SpaceRequirements, +) from utama_core.strategy.examples.utils import ( CalculateFieldCenter, SetBlackboardVariable, @@ -154,29 +157,24 @@ def __init__( self, first_robot_id: int, second_robot_id: int, - field_bounds: Optional[FieldBounds] = None, ): """ Initialize the TwoRobotPlacementStrategy with two robot IDs and optional field bounds. """ self.first_robot_id = first_robot_id self.second_robot_id = second_robot_id - self.field_bounds = field_bounds if field_bounds else Field.FULL_FIELD_BOUNDS super().__init__() def assert_exp_robots(self, n_runtime_friendly: int, n_runtime_enemy: int): - if n_runtime_friendly == 2: + if n_runtime_friendly == 2 and n_runtime_enemy == 0: return True return False def assert_exp_goals(self, includes_my_goal_line: bool, includes_opp_goal_line: bool): return True # No specific goal line requirements - def get_min_bounding_zone(self) -> Optional[FieldBounds]: - # toggles robot between (1, -1) and (1, 1) - # Using full field bounds logic now, but maybe should return specific bounds if needed. - # For now, keeping consistent with previous simple bounds or updating if needed. - return FieldBounds(top_left=(-1, 1), bottom_right=(1, -1)) + def get_min_bounding_req(self): + return SpaceRequirements(min_length=1.0, min_width=1.0) # Require at least a 1x1 area to allow for oscillation def create_behaviour_tree(self) -> py_trees.behaviour.Behaviour: """Factory function to create a complete behaviour tree.""" @@ -197,7 +195,7 @@ def create_behaviour_tree(self) -> py_trees.behaviour.Behaviour: set_turn = SetBlackboardVariable(name="InitTurn", variable_name=turn_key, value=0) # Calculate Field Center from custom field_bounds - calc_center = CalculateFieldCenter(field_bounds=self.field_bounds, output_key=field_center_key) + calc_center = CalculateFieldCenter(output_key=field_center_key) # Robot 1 (X-mover): Centered at (center_x, center_y), move range +/- 0.5 in X move_robot1 = RobotPlacementStep( diff --git a/utama_core/strategy/examples/utils.py b/utama_core/strategy/examples/utils.py index 5d833e71..2f1d559f 100644 --- a/utama_core/strategy/examples/utils.py +++ b/utama_core/strategy/examples/utils.py @@ -34,13 +34,13 @@ class CalculateFieldCenter(AbstractBehaviour): Calculates the center of the provided field bounds and writes it to the blackboard. """ - def __init__(self, field_bounds: FieldBounds, output_key: str = "FieldCenter"): + def __init__(self, output_key: str = "FieldCenter"): super().__init__(name="CalculateFieldCenter") self.output_key = output_key - self.field_bounds = field_bounds self.calculated = False def setup_(self): + self.field_bounds = self.blackboard.game.field.field_bounds self.blackboard.register_key(key=self.output_key, access=py_trees.common.Access.WRITE) def update(self) -> py_trees.common.Status: diff --git a/utama_core/team_controller/src/controllers/sim/grsim_controller.py b/utama_core/team_controller/src/controllers/sim/grsim_controller.py index f8b0ad7f..87879c51 100644 --- a/utama_core/team_controller/src/controllers/sim/grsim_controller.py +++ b/utama_core/team_controller/src/controllers/sim/grsim_controller.py @@ -1,7 +1,6 @@ import time from typing import Tuple -from utama_core.config import formations from utama_core.config.settings import ( ADD_Y_COORD, LOCAL_HOST, @@ -72,16 +71,6 @@ def _create_teleport_ball_command(self, x: float, y: float, vx: float, vy: float sim_control.teleport_ball.CopyFrom(tele_ball) return sim_control - def reset(self): - for idx, x in enumerate(formations.RIGHT_START_ONE): - self.teleport_robot(True, idx, x[0], x[1], x[2]) - for idx, x in enumerate(formations.LEFT_START_ONE): - self.teleport_robot(False, idx, x[0], x[1], x[2]) - if self.exp_ball: - self.teleport_ball(0, 0, 0, 0) - else: - self.remove_ball() - def _do_teleport_robot_unrestricted( self, is_team_yellow: bool, diff --git a/utama_core/tests/abstract_strategy/test_assertions.py b/utama_core/tests/abstract_strategy/test_assertions.py index cfe8201b..85bcf729 100644 --- a/utama_core/tests/abstract_strategy/test_assertions.py +++ b/utama_core/tests/abstract_strategy/test_assertions.py @@ -2,19 +2,34 @@ import pytest -from utama_core.entities.game.field import FieldBounds -from utama_core.strategy.common import AbstractBehaviour, AbstractStrategy +from utama_core.config.field_params import STANDARD_FIELD_DIMS +from utama_core.entities.game.field import Field, FieldBounds +from utama_core.strategy.common import ( + AbstractBehaviour, + AbstractStrategy, + SpaceRequirements, +) # Dummy blackboard helper def make_dummy_blackboard(actual_field_bounds): bb = SimpleNamespace() bb.game = SimpleNamespace() - bb.game.field = SimpleNamespace() - bb.game.field.field_bounds = actual_field_bounds + bb.game.field = Field( + my_team_is_right=True, + field_dims=STANDARD_FIELD_DIMS, + field_bounds=actual_field_bounds, + ) return bb +# Dummy game object +def make_dummy_game(field_bounds): + game = SimpleNamespace() + game.field = Field(my_team_is_right=True, field_dims=STANDARD_FIELD_DIMS, field_bounds=field_bounds) + return game + + # Helper strategy class DummyStrategy(AbstractStrategy): exp_ball = True # Not relevant for these tests @@ -32,7 +47,7 @@ def __init__(self, min_bb=None): super().__init__() self._min_bb = min_bb - def get_min_bounding_zone(self): + def get_min_bounding_req(self): return self._min_bb @@ -44,14 +59,16 @@ def test_normal_case(): min_bb = FieldBounds(top_left=(-4.0, 2.5), bottom_right=(4.0, -2.5)) strategy = DummyStrategy(min_bb=min_bb) strategy.blackboard = make_dummy_blackboard(actual_field) - strategy.assert_field_requirements() # should pass + game = make_dummy_game(actual_field) + strategy.assert_field_requirements(game) # should pass def test_min_bb_none(): actual_field = FieldBounds(top_left=(-4.5, 3.0), bottom_right=(4.5, -3.0)) strategy = DummyStrategy(min_bb=None) strategy.blackboard = make_dummy_blackboard(actual_field) - strategy.assert_field_requirements() # should pass + game = make_dummy_game(actual_field) + strategy.assert_field_requirements(game) # should pass def test_min_bb_outside_field(): @@ -59,8 +76,9 @@ def test_min_bb_outside_field(): min_bb = FieldBounds(top_left=(-5.0, 3.5), bottom_right=(4.0, -2.5)) strategy = DummyStrategy(min_bb=min_bb) strategy.blackboard = make_dummy_blackboard(actual_field) - with pytest.raises(AssertionError): - strategy.assert_field_requirements() + game = make_dummy_game(actual_field) + with pytest.raises(ValueError): + strategy.assert_field_requirements(game) def test_crossed_bounding_box(): @@ -68,8 +86,9 @@ def test_crossed_bounding_box(): min_bb = FieldBounds(top_left=(1.0, -1.0), bottom_right=(-1.0, 1.0)) # crossed strategy = DummyStrategy(min_bb=min_bb) strategy.blackboard = make_dummy_blackboard(actual_field) - with pytest.raises(AssertionError): - strategy.assert_field_requirements() + game = make_dummy_game(actual_field) + with pytest.raises(ValueError): + strategy.assert_field_requirements(game) def test_min_bb_exceeds_full_field(): @@ -77,5 +96,25 @@ def test_min_bb_exceeds_full_field(): min_bb = FieldBounds(top_left=(-5.0, 4.0), bottom_right=(5.0, -4.0)) strategy = DummyStrategy(min_bb=min_bb) strategy.blackboard = make_dummy_blackboard(actual_field) - with pytest.raises(AssertionError): - strategy.assert_field_requirements() + game = make_dummy_game(actual_field) + with pytest.raises(ValueError): + strategy.assert_field_requirements(game) + + +def test_min_bb_not_contained_in_actual_field(): + actual_field = FieldBounds(top_left=(-2.0, 2.0), bottom_right=(2.0, -2.0)) + min_bb = FieldBounds(top_left=(-3.0, 1.5), bottom_right=(1.0, -1.5)) + strategy = DummyStrategy(min_bb=min_bb) + strategy.blackboard = make_dummy_blackboard(actual_field) + game = make_dummy_game(actual_field) + with pytest.raises(ValueError, match="does not contain"): + strategy.assert_field_requirements(game) + + +def test_space_requirements_rejected_when_field_too_small(): + actual_field = FieldBounds(top_left=(-2.0, 1.0), bottom_right=(2.0, -1.0)) + strategy = DummyStrategy(min_bb=SpaceRequirements(min_length=4.5, min_width=2.5)) + strategy.blackboard = make_dummy_blackboard(actual_field) + game = make_dummy_game(actual_field) + with pytest.raises(ValueError, match="too small for strategy"): + strategy.assert_field_requirements(game) diff --git a/utama_core/tests/config/test_field_dimensions.py b/utama_core/tests/config/test_field_dimensions.py new file mode 100644 index 00000000..e677a8af --- /dev/null +++ b/utama_core/tests/config/test_field_dimensions.py @@ -0,0 +1,197 @@ +import numpy as np +import pytest +from numpy.testing import assert_array_equal + +from utama_core.config.field_params import ( + GREAT_EXHIBITION_FIELD_DIMS, + STANDARD_FIELD_DIMS, + FieldBounds, + FieldDimensions, +) +from utama_core.entities.game.field import Field + + +@pytest.mark.parametrize( + "dims", + [ + STANDARD_FIELD_DIMS, + GREAT_EXHIBITION_FIELD_DIMS, + FieldDimensions( + full_field_half_length=6.0, + full_field_half_width=4.0, + half_defense_area_depth=0.5, + half_defense_area_width=1.0, + half_goal_width=0.5, + ), + ], +) +def test_full_field_bounds_follow_resized_dimensions(dims: FieldDimensions): + bounds = dims.full_field_bounds + assert bounds.top_left == (-dims.full_field_half_length, dims.full_field_half_width) + assert bounds.bottom_right == ( + dims.full_field_half_length, + -dims.full_field_half_width, + ) + assert bounds.center == (0.0, 0.0) + + +@pytest.mark.parametrize( + "dims", + [ + STANDARD_FIELD_DIMS, + GREAT_EXHIBITION_FIELD_DIMS, + FieldDimensions(5.2, 3.6, 0.7, 1.4, 0.6), + ], +) +def test_full_field_polygon_matches_dimensions(dims: FieldDimensions): + length = dims.full_field_half_length + width = dims.full_field_half_width + expected = np.array([(length, width), (length, -width), (-length, -width), (-length, width)]) + assert_array_equal(dims.full_field, expected) + + +def test_goal_lines_shift_when_full_field_is_resized(): + small = FieldDimensions(3.0, 2.0, 0.5, 1.0, 0.5) + large = FieldDimensions(6.0, 2.0, 0.5, 1.0, 0.5) + + assert_array_equal(small.right_goal_line, np.array([(3.0, 0.5), (3.0, -0.5)])) + assert_array_equal(large.right_goal_line, np.array([(6.0, 0.5), (6.0, -0.5)])) + assert_array_equal(small.left_goal_line, np.array([(-3.0, 0.5), (-3.0, -0.5)])) + assert_array_equal(large.left_goal_line, np.array([(-6.0, 0.5), (-6.0, -0.5)])) + + +def test_defense_areas_track_resized_full_length(): + dims = FieldDimensions(5.0, 3.0, 0.75, 1.25, 0.5) + + expected_right = np.array([(5.0, 1.25), (3.5, 1.25), (3.5, -1.25), (5.0, -1.25)]) + expected_left = np.array([(-5.0, 1.25), (-3.5, 1.25), (-3.5, -1.25), (-5.0, -1.25)]) + + assert_array_equal(dims.right_defense_area, expected_right) + assert_array_equal(dims.left_defense_area, expected_left) + + +@pytest.mark.parametrize("team_is_right", [True, False]) +def test_field_goal_lines_match_resized_dimensions(team_is_right: bool): + dims = FieldDimensions(6.0, 4.0, 0.5, 1.0, 0.5) + field = Field( + my_team_is_right=team_is_right, + field_dims=dims, + field_bounds=dims.full_field_bounds, + ) + + if team_is_right: + assert_array_equal(field.my_goal_line, dims.right_goal_line) + assert_array_equal(field.enemy_goal_line, dims.left_goal_line) + else: + assert_array_equal(field.my_goal_line, dims.left_goal_line) + assert_array_equal(field.enemy_goal_line, dims.right_goal_line) + + +@pytest.mark.parametrize("team_is_right", [True, False]) +def test_field_reports_goal_lines_present_on_full_resized_bounds(team_is_right: bool): + dims = FieldDimensions(6.0, 4.0, 0.5, 1.0, 0.5) + field = Field( + my_team_is_right=team_is_right, + field_dims=dims, + field_bounds=dims.full_field_bounds, + ) + + assert field.includes_my_goal_line + assert field.includes_opp_goal_line + + +@pytest.mark.parametrize("team_is_right", [True, False]) +def test_field_reports_goal_lines_absent_when_bounds_crop_goal_width( + team_is_right: bool, +): + dims = FieldDimensions(6.0, 4.0, 0.5, 1.0, 0.5) + cropped_bounds = FieldBounds(top_left=(-6.0, 0.4), bottom_right=(6.0, -0.4)) + field = Field( + my_team_is_right=team_is_right, + field_dims=dims, + field_bounds=cropped_bounds, + ) + + assert not field.includes_my_goal_line + assert not field.includes_opp_goal_line + + +@pytest.mark.parametrize( + ("kwargs", "error_pattern"), + [ + ( + { + "full_field_half_length": 0.0, + "full_field_half_width": 3.0, + "half_defense_area_depth": 0.5, + "half_defense_area_width": 1.0, + "half_goal_width": 0.5, + }, + "Field length/width must be positive", + ), + ( + { + "full_field_half_length": 4.5, + "full_field_half_width": 3.0, + "half_defense_area_depth": 0.0, + "half_defense_area_width": 1.0, + "half_goal_width": 0.5, + }, + "Goal/defense measurements must be positive", + ), + ( + { + "full_field_half_length": 1.0, + "full_field_half_width": 3.0, + "half_defense_area_depth": 0.6, + "half_defense_area_width": 1.0, + "half_goal_width": 0.5, + }, + "exceeds field length", + ), + ( + { + "full_field_half_length": 4.5, + "full_field_half_width": 1.0, + "half_defense_area_depth": 0.5, + "half_defense_area_width": 1.1, + "half_goal_width": 0.5, + }, + "Defense width .* exceeds field width", + ), + ( + { + "full_field_half_length": 4.5, + "full_field_half_width": 1.0, + "half_defense_area_depth": 0.5, + "half_defense_area_width": 1.0, + "half_goal_width": 1.1, + }, + "Goal width .* exceeds field width", + ), + ( + { + "full_field_half_length": 4.5, + "full_field_half_width": 2.0, + "half_defense_area_depth": 0.5, + "half_defense_area_width": 0.6, + "half_goal_width": 0.8, + }, + "should not exceed defense width", + ), + ], +) +def test_invalid_field_dimensions_raise_value_errors(kwargs, error_pattern: str): + with pytest.raises(ValueError, match=error_pattern): + FieldDimensions(**kwargs) + + +def test_cached_geometry_properties_return_same_objects(): + dims = FieldDimensions(4.5, 3.0, 0.5, 1.0, 0.5) + + assert dims.full_field is dims.full_field + assert dims.full_field_bounds is dims.full_field_bounds + assert dims.left_goal_line is dims.left_goal_line + assert dims.right_goal_line is dims.right_goal_line + assert dims.left_defense_area is dims.left_defense_area + assert dims.right_defense_area is dims.right_defense_area diff --git a/utama_core/tests/controller/test_sim_controller.py b/utama_core/tests/controller/test_sim_controller.py index 4d721d60..b0bd48b6 100644 --- a/utama_core/tests/controller/test_sim_controller.py +++ b/utama_core/tests/controller/test_sim_controller.py @@ -1,10 +1,9 @@ """Tests for AbstractSimController bounds-checking behaviour.""" -from unittest.mock import MagicMock, call, patch - import pytest -from utama_core.entities.game.field import Field, FieldBounds +from utama_core.config.field_params import STANDARD_FIELD_DIMS, FieldBounds +from utama_core.config.settings import OFF_PITCH_OFFSET from utama_core.team_controller.src.controllers.common.sim_controller_abstract import ( AbstractSimController, ) @@ -37,11 +36,14 @@ def set_robot_presence(self, robot_id, is_team_yellow, should_robot_be_present): # --------------------------------------------------------------------------- # Standard SSL full-field bounds: x ∈ [-4.5, 4.5], y ∈ [-3.0, 3.0] -FULL_BOUNDS = Field.FULL_FIELD_BOUNDS +FULL_BOUNDS = STANDARD_FIELD_DIMS.full_field_bounds # A small custom bounds used for many tests: x ∈ [0, 2], y ∈ [-1, 1] CUSTOM_BOUNDS = FieldBounds(top_left=(0.0, 1.0), bottom_right=(2.0, -1.0)) +# A shifted non-centered bounds: x ∈ [1.5, 3.5], y ∈ [2.0, 4.0] +SHIFTED_BOUNDS = FieldBounds(top_left=(1.5, 4.0), bottom_right=(3.5, 2.0)) + @pytest.fixture def ctrl(): @@ -61,6 +63,12 @@ def ctrl_full(): return StubSimController(FULL_BOUNDS, exp_ball=True) +@pytest.fixture +def ctrl_shifted(): + """Controller using shifted non-centered bounds.""" + return StubSimController(SHIFTED_BOUNDS, exp_ball=True) + + # =========================================================================== # teleport_ball # =========================================================================== @@ -81,6 +89,13 @@ def test_center_of_bounds_accepted(self, ctrl): ctrl.teleport_ball(1.0, 0.0) # center of CUSTOM_BOUNDS assert len(ctrl.teleport_ball_calls) == 1 + def test_shifted_bounds_use_shifted_coordinates(self, ctrl_shifted): + # Center of SHIFTED_BOUNDS is (2.5, 3.0), not (0, 0) + ctrl_shifted.teleport_ball(2.5, 3.0) + assert ctrl_shifted.teleport_ball_calls == [(2.5, 3.0, 0, 0)] + with pytest.raises(ValueError, match=r"outside of the field boundaries"): + ctrl_shifted.teleport_ball(0.0, 0.0) + @pytest.mark.parametrize( "x, y", [ @@ -187,6 +202,12 @@ def test_theta_none_forwarded(self, ctrl): ctrl.teleport_robot(True, 1, 1.0, 0.0) assert ctrl.teleport_robot_calls == [(True, 1, 1.0, 0.0, None)] + def test_shifted_bounds_robot_teleport_is_not_origin_based(self, ctrl_shifted): + ctrl_shifted.teleport_robot(True, 0, 2.5, 3.0, 0.0) + assert ctrl_shifted.teleport_robot_calls == [(True, 0, 2.5, 3.0, 0.0)] + with pytest.raises(ValueError, match=r"outside of the field boundaries"): + ctrl_shifted.teleport_robot(True, 0, 0.0, 0.0, 0.0) + @pytest.mark.parametrize( "x, y", [ @@ -264,6 +285,12 @@ def test_remove_ball_places_outside_field(self, ctrl): assert not in_field_bounds((x, y), ctrl.field_bounds) + def test_remove_ball_uses_bounds_relative_offset(self, ctrl_shifted): + ctrl_shifted.remove_ball() + x, y, _, _ = ctrl_shifted.teleport_ball_calls[0] + assert x == SHIFTED_BOUNDS.bottom_right[0] + OFF_PITCH_OFFSET + assert y == SHIFTED_BOUNDS.bottom_right[1] + OFF_PITCH_OFFSET + def test_remove_ball_works_when_ball_not_expected(self, ctrl_no_ball): """remove_ball bypasses exp_ball guard (it calls the unrestricted method directly).""" ctrl_no_ball.remove_ball() diff --git a/utama_core/tests/global_utils/test_math_utils.py b/utama_core/tests/global_utils/test_math_utils.py index a37f9fa2..4f62a0d9 100644 --- a/utama_core/tests/global_utils/test_math_utils.py +++ b/utama_core/tests/global_utils/test_math_utils.py @@ -184,7 +184,7 @@ def test_compute_bounding_zone_from_points_with_vector2d(): def test_assert_valid_bounding_box_valid(): bb = FieldBounds(top_left=(-4.5, 3.0), bottom_right=(4.5, -3.0)) - assert_valid_bounding_box(bb) # Should not raise + assert_valid_bounding_box(bb, 4.5, 3.0) # Should not raise @pytest.mark.parametrize( @@ -198,5 +198,5 @@ def test_assert_valid_bounding_box_valid(): ) def test_assert_valid_bounding_box_invalid(top_left, bottom_right): bb = FieldBounds(top_left, bottom_right) - with pytest.raises(AssertionError): - assert_valid_bounding_box(bb) + with pytest.raises(ValueError): + assert_valid_bounding_box(bb, 4.5, 3.0) diff --git a/utama_core/tests/motion_planning/random_movement_test.py b/utama_core/tests/motion_planning/random_movement_test.py index 2f364406..2bc5901f 100644 --- a/utama_core/tests/motion_planning/random_movement_test.py +++ b/utama_core/tests/motion_planning/random_movement_test.py @@ -5,6 +5,7 @@ from dataclasses import dataclass from typing import Dict +from utama_core.config.field_params import STANDARD_FIELD_DIMS from utama_core.config.physical_constants import ROBOT_RADIUS from utama_core.entities.data.vector import Vector2D from utama_core.entities.game import Game @@ -125,12 +126,12 @@ def test_random_movement_same_team( # use small bounds to increase chance of collision small_bounds = FieldBounds( top_left=( - -Field.FULL_FIELD_HALF_LENGTH + 1, - Field.FULL_FIELD_HALF_WIDTH - 1, + -STANDARD_FIELD_DIMS.full_field_half_length + 1, + STANDARD_FIELD_DIMS.full_field_half_width - 1, ), bottom_right=( - -Field.FULL_FIELD_HALF_LENGTH + 3, - Field.FULL_FIELD_HALF_WIDTH - 3, + -STANDARD_FIELD_DIMS.full_field_half_length + 3, + STANDARD_FIELD_DIMS.full_field_half_width - 3, ), ) diff --git a/utama_core/tests/refiners/position_refiner_integration_test.py b/utama_core/tests/refiners/position_refiner_integration_test.py index 188483e5..d3e5a928 100644 --- a/utama_core/tests/refiners/position_refiner_integration_test.py +++ b/utama_core/tests/refiners/position_refiner_integration_test.py @@ -3,10 +3,10 @@ import time from collections import deque +from utama_core.config.field_params import STANDARD_FIELD_DIMS from utama_core.config.settings import TIMESTEP from utama_core.data_processing.receivers.vision_receiver import VisionReceiver from utama_core.data_processing.refiners import PositionRefiner -from utama_core.entities.game import Field from utama_core.run import GameGater logger = logging.getLogger(__name__) @@ -28,7 +28,7 @@ def main(): # Runs the vision receiver, vision_buffers = [deque(maxlen=1) for _ in range(4)] vision_receiver = VisionReceiver(vision_buffers) - position_refiner = PositionRefiner(Field.FULL_FIELD_BOUNDS) + position_refiner = PositionRefiner(STANDARD_FIELD_DIMS) start_threads(vision_receiver) NUM_FRIENDLY = 1 diff --git a/utama_core/tests/refiners/position_unit_test.py b/utama_core/tests/refiners/position_unit_test.py index 1de23f00..e33d5b6f 100644 --- a/utama_core/tests/refiners/position_unit_test.py +++ b/utama_core/tests/refiners/position_unit_test.py @@ -1,12 +1,13 @@ +from utama_core.config.field_params import STANDARD_FIELD_DIMS from utama_core.data_processing.refiners import PositionRefiner from utama_core.entities.data.raw_vision import RawBallData, RawRobotData, RawVisionData from utama_core.entities.data.vector import Vector2D from utama_core.entities.data.vision import VisionBallData, VisionRobotData -from utama_core.entities.game import Ball, Field, FieldBounds, GameFrame +from utama_core.entities.game import Ball, GameFrame from utama_core.entities.game.robot import Robot -full_field = Field.FULL_FIELD_BOUNDS -position_refiner = PositionRefiner(full_field) +full_field_dims = STANDARD_FIELD_DIMS +position_refiner = PositionRefiner(full_field_dims) def test_combining_single_team_combines_single_robot(): @@ -76,7 +77,7 @@ def base_refine(is_yellow: bool): raw_balls = [RawBallData(0, 0, 0, 0)] raw_vision_data_cam1 = RawVisionData(0, raw_yellow, raw_blue, raw_balls, 0) raw_vision_data_cam2 = RawVisionData(0, raw_yellow, raw_blue, raw_balls, 1) - p = PositionRefiner(full_field) + p = PositionRefiner(full_field_dims) g = GameFrame(0, is_yellow, True, friendly, enemy, bfac(0, 0)) result = p.refine(g, [raw_vision_data_cam1, raw_vision_data_cam2]) fr = result.friendly_robots[0] @@ -108,7 +109,7 @@ def test_refine_for_multiple_yellow(): raw_balls = [RawBallData(0, 0, 0, 0)] raw_vision_data_cam1 = RawVisionData(0, raw_yellow, [], raw_balls, 0) raw_vision_data_cam2 = RawVisionData(0, raw_yellow, [], raw_balls, 1) - p = PositionRefiner(full_field) + p = PositionRefiner(full_field_dims) g = GameFrame(0, True, True, friendly, {}, bfac(0, 0)) result = p.refine(g, [raw_vision_data_cam1, raw_vision_data_cam2]) @@ -127,7 +128,7 @@ def test_refine_nones(): raw_balls = [RawBallData(0, 0, 0, 0)] raw_vision_data_cam1 = RawVisionData(0, raw_yellow, [], raw_balls, 0) raw_vision_data_cam2 = RawVisionData(0, raw_yellow, [], raw_balls, 1) - p = PositionRefiner(full_field) + p = PositionRefiner(full_field_dims) g = GameFrame(0, True, True, friendly, {}, bfac(0, 0)) result = p.refine(g, [raw_vision_data_cam1, raw_vision_data_cam2, None, None]) @@ -142,12 +143,12 @@ def test_refine_nones(): def test_out_of_bounds_does_not_update_existing_robot(): # Existing friendly robot at origin friendly = {0: rfac(0, True, 0, 0)} - # Vision sees same robot far outside bounds (x beyond 5.5) + # Vision sees same robot far outside bounds (x beyond full field) raw_yellow = [RawRobotData(0, 10.0, 0.0, 0.0, 1.0)] raw_balls = [RawBallData(0, 0, 0, 0)] frames = [RawVisionData(0, raw_yellow, [], raw_balls, 0)] - p = PositionRefiner(full_field) + p = PositionRefiner(full_field_dims) g = GameFrame(0, True, True, friendly, {}, bfac(0, 0)) result = p.refine(g, frames) @@ -160,12 +161,12 @@ def test_out_of_bounds_does_not_update_existing_robot(): def test_out_of_bounds_enemy_not_added(): # No enemy robots initially friendly = {0: rfac(0, True, 0, 0)} - # Vision sees a blue robot outside bounds (y beyond 4.0) + # Vision sees a blue robot outside full field bounds raw_blue = [RawRobotData(1, 0.0, 10.0, 0.0, 1.0)] raw_balls = [RawBallData(0, 0, 0, 0)] frames = [RawVisionData(0, [], raw_blue, raw_balls, 0)] - p = PositionRefiner(full_field) + p = PositionRefiner(full_field_dims) g = GameFrame(0, True, True, friendly, {}, bfac(0, 0)) result = p.refine(g, frames) @@ -174,12 +175,12 @@ def test_out_of_bounds_enemy_not_added(): def test_out_of_bounds_friendly_not_added(): - # Vision sees a yellow robot outside bounds (y beyond 4.0) - raw_yellow = [RawRobotData(1, 3.0, 3.1, 0.0, 1.0)] + # Vision sees a yellow robot clearly outside full field bounds (+ buffer) + raw_yellow = [RawRobotData(1, 3.0, 10.0, 0.0, 1.0)] raw_balls = [RawBallData(0, 0, 0, 0)] frames = [RawVisionData(0, raw_yellow, [], raw_balls, 0)] - p = PositionRefiner(FieldBounds(top_left=(-1, 1.0), bottom_right=(1.0, -1.0))) + p = PositionRefiner(full_field_dims) g = GameFrame(0, True, True, {}, {}, bfac(0, 0)) result = p.refine(g, frames) diff --git a/utama_core/tests/strategy_examples/test_placement_coords.py b/utama_core/tests/strategy_examples/test_placement_coords.py index 21053065..f474b9d1 100644 --- a/utama_core/tests/strategy_examples/test_placement_coords.py +++ b/utama_core/tests/strategy_examples/test_placement_coords.py @@ -11,13 +11,8 @@ import pytest -from utama_core.config.formations import LEFT_START_ONE, RIGHT_START_ONE from utama_core.entities.game import Game from utama_core.entities.game.field import FieldBounds -from utama_core.global_utils.mapping_utils import ( - map_friendly_enemy_to_colors, - map_left_right_to_colors, -) from utama_core.run import StrategyRunner from utama_core.strategy.examples.one_robot_placement_strategy import ( RobotPlacementStrategy, @@ -45,24 +40,10 @@ def __init__(self, expected_center: tuple[float, float], tolerance: float = 0.15 def reset_field(self, sim_controller: AbstractSimController, game: Game): """Reset robot and ball positions for the test.""" - ini_yellow, ini_blue = map_left_right_to_colors( - game.my_team_is_yellow, - game.my_team_is_right, - RIGHT_START_ONE, - LEFT_START_ONE, - ) - - y_robots, b_robots = map_friendly_enemy_to_colors( - game.my_team_is_yellow, game.friendly_robots, game.enemy_robots - ) - - for i in b_robots.keys(): - sim_controller.teleport_robot(False, i, ini_blue[i][0], ini_blue[i][1], ini_blue[i][2]) - for j in y_robots.keys(): - sim_controller.teleport_robot(True, j, ini_yellow[j][0], ini_yellow[j][1], ini_yellow[j][2]) + centre = game.field.field_bounds.center + sim_controller.teleport_robot(game.my_team_is_yellow, self.my_strategy.robot_id, centre[0], centre[1]) - sim_controller.teleport_robot(game.my_team_is_yellow, self.my_strategy.robot_id, 0, 0) - sim_controller.teleport_ball(3, 3) + sim_controller.teleport_ball(centre[0] + 0.5, centre[1] + 0.5) def eval_status(self, game: Game) -> TestingStatus: """Verify robot reaches both oscillation targets.""" @@ -84,12 +65,13 @@ def eval_status(self, game: Game) -> TestingStatus: def _run_placement_test(field_bounds: Optional[FieldBounds], expected_center: tuple[float, float]): """Helper to run a placement strategy test with given bounds.""" - strategy = RobotPlacementStrategy(robot_id=0, field_bounds=field_bounds) + strategy = RobotPlacementStrategy(robot_id=0) runner = StrategyRunner( strategy=strategy, my_team_is_yellow=True, my_team_is_right=False, + field_bounds=field_bounds, mode="rsim", exp_friendly=1, exp_enemy=0, diff --git a/utama_core/tests/strategy_examples/test_two_robot_placement_coords.py b/utama_core/tests/strategy_examples/test_two_robot_placement_coords.py index 40c892a9..0a8af483 100644 --- a/utama_core/tests/strategy_examples/test_two_robot_placement_coords.py +++ b/utama_core/tests/strategy_examples/test_two_robot_placement_coords.py @@ -13,13 +13,8 @@ import pytest -from utama_core.config.formations import LEFT_START_ONE, RIGHT_START_ONE from utama_core.entities.game import Game from utama_core.entities.game.field import FieldBounds -from utama_core.global_utils.mapping_utils import ( - map_friendly_enemy_to_colors, - map_left_right_to_colors, -) from utama_core.run import StrategyRunner from utama_core.strategy.examples.two_robot_placement import TwoRobotPlacementStrategy from utama_core.team_controller.src.controllers import AbstractSimController @@ -61,27 +56,13 @@ def __init__( def reset_field(self, sim_controller: AbstractSimController, game: Game): """Reset robot and ball positions for the test.""" - ini_yellow, ini_blue = map_left_right_to_colors( - game.my_team_is_yellow, - game.my_team_is_right, - RIGHT_START_ONE, - LEFT_START_ONE, - ) - - y_robots, b_robots = map_friendly_enemy_to_colors( - game.my_team_is_yellow, game.friendly_robots, game.enemy_robots - ) - - for i in b_robots.keys(): - sim_controller.teleport_robot(False, i, ini_blue[i][0], ini_blue[i][1], ini_blue[i][2]) - for j in y_robots.keys(): - sim_controller.teleport_robot(True, j, ini_yellow[j][0], ini_yellow[j][1], ini_yellow[j][2]) - # Position robots near the center for faster test convergence cx, cy = self.expected_center - sim_controller.teleport_robot(game.my_team_is_yellow, self.first_robot_id, cx, cy) - sim_controller.teleport_robot(game.my_team_is_yellow, self.second_robot_id, cx, cy) - sim_controller.teleport_ball(3, 3) + sim_controller.teleport_robot(game.my_team_is_yellow, self.first_robot_id, cx - 0.4, cy) + sim_controller.teleport_robot(game.my_team_is_yellow, self.second_robot_id, cx, cy - 0.4) + sim_controller.teleport_ball( + cx + 0.5, cy + 0.5 + ) # Place ball at corner to avoid interference with robot movement def eval_status(self, game: Game) -> TestingStatus: """Verify both robots reach their oscillation targets.""" @@ -121,13 +102,13 @@ def _run_two_robot_placement_test( strategy = TwoRobotPlacementStrategy( first_robot_id=first_robot_id, second_robot_id=second_robot_id, - field_bounds=field_bounds, ) runner = StrategyRunner( strategy=strategy, my_team_is_yellow=True, my_team_is_right=False, + field_bounds=field_bounds, mode="rsim", exp_friendly=2, exp_enemy=0, diff --git a/utama_core/tests/strategy_runner/integration_test.py b/utama_core/tests/strategy_runner/integration_test.py deleted file mode 100644 index 646344fb..00000000 --- a/utama_core/tests/strategy_runner/integration_test.py +++ /dev/null @@ -1,18 +0,0 @@ -from utama_core.entities.game.field import FieldBounds -from utama_core.run.strategy_runner import StrategyRunner -from utama_core.tests.strategy_runner.strat_runner_test_utils import DummyStrategy - - -def test_position_refiner_config(): - runner = StrategyRunner( - strategy=DummyStrategy(), - my_team_is_yellow=True, - my_team_is_right=True, - mode="rsim", - exp_friendly=3, - exp_enemy=0, - field_bounds=FieldBounds(top_left=(0, 3), bottom_right=(4.5, -3)), - ) - - assert runner.my.game.field.half_length == 2.25 - assert runner.my.game.field.half_width == 3.0 diff --git a/utama_core/tests/strategy_runner/strat_runner_test_utils.py b/utama_core/tests/strategy_runner/strat_runner_test_utils.py index 5cad1a1d..04250042 100644 --- a/utama_core/tests/strategy_runner/strat_runner_test_utils.py +++ b/utama_core/tests/strategy_runner/strat_runner_test_utils.py @@ -7,9 +7,12 @@ def assert_exp_robots(self, exp_friendly, exp_enemy): def assert_exp_goals(self, my_goal, opp_goal): return True - def get_min_bounding_zone(self): + def get_min_bounding_req(self): return None + def setup_strategy_blackboard(self, is_opp_strat): + pass + def setup_behaviour_tree(self, is_opp_strat): pass diff --git a/utama_core/tests/strategy_runner/teleport_position_accuracy_test.py b/utama_core/tests/strategy_runner/teleport_position_accuracy_test.py index b47315f8..78e4f76a 100644 --- a/utama_core/tests/strategy_runner/teleport_position_accuracy_test.py +++ b/utama_core/tests/strategy_runner/teleport_position_accuracy_test.py @@ -42,7 +42,7 @@ def assert_exp_robots(self, n_runtime_friendly: int, n_runtime_enemy: int) -> bo def assert_exp_goals(self, includes_my_goal_line: bool, includes_opp_goal_line: bool) -> bool: return True - def get_min_bounding_zone(self) -> Optional[FieldBounds]: + def get_min_bounding_req(self): return None diff --git a/utama_core/tests/strategy_runner/test_error_handling.py b/utama_core/tests/strategy_runner/test_error_handling.py index 404d2de6..340c7fe0 100644 --- a/utama_core/tests/strategy_runner/test_error_handling.py +++ b/utama_core/tests/strategy_runner/test_error_handling.py @@ -23,6 +23,17 @@ def mock_runner(): runner._stop_event = MagicMock() runner._stop_event.is_set.return_value = False + # run() now performs pre-run game setup; keep these tests focused on + # stop/exception handling by bypassing that heavy integration step. + runner._pre_run_setup = MagicMock() + runner.toggle_opp_first = False + runner.my_team_is_yellow = True + runner.my_team_is_right = False + runner.exp_friendly = 2 + runner.exp_enemy = 0 + runner.exp_ball = True + runner.vision_buffers = [] + # Opp side runner.opp = None @@ -39,8 +50,40 @@ def mock_runner(): class TestStopRobotsOnClose: """Tests for _stop_robots behavior when close() is called.""" + def test_stop_robots_sends_commands_when_game_exists(self, mock_runner): + mock_runner._stop_robots(repeat=2) + + controller = mock_runner.my.strategy.robot_controller + assert controller.add_robot_commands.call_count == 2 + assert controller.send_robot_commands.call_count == 2 + + def test_stop_robots_handles_missing_game_gracefully(self, mock_runner): + mock_runner.my.game = None + + mock_runner._stop_robots(repeat=3) + + controller = mock_runner.my.strategy.robot_controller + controller.add_robot_commands.assert_not_called() + controller.send_robot_commands.assert_not_called() + + def test_stop_robots_only_sends_for_sides_with_game(self, mock_runner): + opp = MagicMock() + opp.game = None + opp.strategy = MagicMock() + opp.strategy.robot_controller = MagicMock() + mock_runner.opp = opp + + mock_runner._stop_robots(repeat=1) + + my_controller = mock_runner.my.strategy.robot_controller + opp_controller = mock_runner.opp.strategy.robot_controller + assert my_controller.add_robot_commands.call_count == 1 + assert my_controller.send_robot_commands.call_count == 1 + opp_controller.add_robot_commands.assert_not_called() + opp_controller.send_robot_commands.assert_not_called() + def test_stop_commands_sent_in_real_mode(self, mock_runner): - mock_runner.close(stop_command_mult=5) + mock_runner.close(stop_command_repeat=5) controller = mock_runner.my.strategy.robot_controller assert controller.add_robot_commands.call_count == 5 @@ -53,7 +96,7 @@ def test_stop_commands_have_zero_velocity(self, mock_runner): 2: MagicMock(), } - mock_runner._stop_robots(stop_command_mult=1) + mock_runner._stop_robots(repeat=1) controller = mock_runner.my.strategy.robot_controller commands_dict = controller.add_robot_commands.call_args[0][0] diff --git a/utama_core/tests/strategy_runner/test_exp_ball.py b/utama_core/tests/strategy_runner/test_exp_ball.py index 3049117a..2d2d75ed 100644 --- a/utama_core/tests/strategy_runner/test_exp_ball.py +++ b/utama_core/tests/strategy_runner/test_exp_ball.py @@ -45,7 +45,7 @@ def assert_exp_robots(self, n_runtime_friendly: int, n_runtime_enemy: int) -> bo def assert_exp_goals(self, includes_my_goal_line: bool, includes_opp_goal_line: bool) -> bool: return True - def get_min_bounding_zone(self) -> Optional[FieldBounds]: + def get_min_bounding_req(self): return None @@ -63,7 +63,7 @@ def assert_exp_robots(self, n_runtime_friendly: int, n_runtime_enemy: int) -> bo def assert_exp_goals(self, includes_my_goal_line: bool, includes_opp_goal_line: bool) -> bool: return True - def get_min_bounding_zone(self) -> Optional[FieldBounds]: + def get_min_bounding_req(self): return None diff --git a/utama_core/tests/strategy_runner/test_grsim_sim_setup.py b/utama_core/tests/strategy_runner/test_grsim_sim_setup.py new file mode 100644 index 00000000..0c64dc32 --- /dev/null +++ b/utama_core/tests/strategy_runner/test_grsim_sim_setup.py @@ -0,0 +1,164 @@ +from unittest.mock import patch + +from utama_core.config.formations import FormationType, get_formations +from utama_core.entities.game.field import FieldBounds +from utama_core.global_utils.mapping_utils import ( + map_friendly_enemy_to_colors, + map_left_right_to_colors, +) +from utama_core.run.strategy_runner import StrategyRunner +from utama_core.tests.strategy_runner.strat_runner_test_utils import DummyStrategy + + +class _FakeGRSimController: + def __init__(self, field_bounds, exp_ball): + self.field_bounds = field_bounds + self.exp_ball = exp_ball + self.set_robot_presence_calls: list[tuple[int, bool, bool]] = [] + self.teleport_robot_calls: list[tuple[bool, int, float, float, float | None]] = [] + self.teleport_ball_calls: list[tuple[float, float]] = [] + self.remove_ball_calls = 0 + + def set_robot_presence(self, robot_id, is_team_yellow, should_robot_be_present): + self.set_robot_presence_calls.append((robot_id, is_team_yellow, should_robot_be_present)) + + def teleport_robot(self, is_team_yellow, robot_id, x, y, theta=None): + self.teleport_robot_calls.append((is_team_yellow, robot_id, x, y, theta)) + + def teleport_ball(self, x, y): + self.teleport_ball_calls.append((x, y)) + + def remove_ball(self): + self.remove_ball_calls += 1 + + +class _FakeGRSimRobotController: + def __init__(self, is_team_yellow, n_friendly): + self.is_team_yellow = is_team_yellow + self.n_friendly = n_friendly + + +def test_grsim_spawn_positions_and_ball_use_field_bounds_center(): + bounds = FieldBounds(top_left=(-2.0, 2.4), bottom_right=(4.0, -1.6)) + my_team_is_yellow = True + my_team_is_right = False + exp_friendly = 2 + exp_enemy = 1 + + with ( + patch( + "utama_core.run.strategy_runner.GRSimController", + _FakeGRSimController, + ), + patch( + "utama_core.run.strategy_runner.GRSimRobotController", + _FakeGRSimRobotController, + ), + patch.object( + StrategyRunner, + "start_threads", + lambda self, vision_receiver: None, + ), + patch.object( + StrategyRunner, + "_load_game", + lambda self: None, + ), + patch.object( + StrategyRunner, + "_assert_exp_goals", + lambda self: None, + ), + ): + runner = StrategyRunner( + strategy=DummyStrategy(), + my_team_is_yellow=my_team_is_yellow, + my_team_is_right=my_team_is_right, + mode="grsim", + exp_friendly=exp_friendly, + exp_enemy=exp_enemy, + field_bounds=bounds, + exp_ball=True, + ) + + assert isinstance(runner.sim_controller, _FakeGRSimController) + + left_start, right_start = get_formations( + bounds=bounds, + n_left=exp_friendly, + n_right=exp_enemy, + formation_type=FormationType.START_ONE, + ) + expected_yellow, expected_blue = map_left_right_to_colors( + my_team_is_yellow, + my_team_is_right, + right_start, + left_start, + ) + + n_yellow, n_blue = map_friendly_enemy_to_colors( + my_team_is_yellow, + exp_friendly, + exp_enemy, + ) + + expected_calls: dict[tuple[bool, int], tuple[float, float, float]] = {} + for y in range(n_yellow): + e = expected_yellow[y] + expected_calls[(True, y)] = (e.x, e.y, e.theta) + for b in range(n_blue): + e = expected_blue[b] + expected_calls[(False, b)] = (e.x, e.y, e.theta) + + actual_calls = { + (is_yellow, robot_id): (x, y, theta) + for is_yellow, robot_id, x, y, theta in runner.sim_controller.teleport_robot_calls + } + + assert actual_calls == expected_calls + assert runner.sim_controller.teleport_ball_calls == [bounds.center] + + +def test_grsim_exp_ball_false_removes_ball_not_center_teleport(): + bounds = FieldBounds(top_left=(-2.0, 2.4), bottom_right=(4.0, -1.6)) + strategy = DummyStrategy() + strategy.exp_ball = False + + with ( + patch( + "utama_core.run.strategy_runner.GRSimController", + _FakeGRSimController, + ), + patch( + "utama_core.run.strategy_runner.GRSimRobotController", + _FakeGRSimRobotController, + ), + patch.object( + StrategyRunner, + "start_threads", + lambda self, vision_receiver: None, + ), + patch.object( + StrategyRunner, + "_load_game", + lambda self: None, + ), + patch.object( + StrategyRunner, + "_assert_exp_goals", + lambda self: None, + ), + ): + runner = StrategyRunner( + strategy=strategy, + my_team_is_yellow=True, + my_team_is_right=False, + mode="grsim", + exp_friendly=2, + exp_enemy=1, + field_bounds=bounds, + exp_ball=False, + ) + + assert runner.sim_controller.remove_ball_calls == 1 + assert runner.sim_controller.teleport_ball_calls == [] diff --git a/utama_core/tests/strategy_runner/test_rsim_formations.py b/utama_core/tests/strategy_runner/test_rsim_formations.py new file mode 100644 index 00000000..618513c4 --- /dev/null +++ b/utama_core/tests/strategy_runner/test_rsim_formations.py @@ -0,0 +1,173 @@ +import os + +import py_trees +import pytest + +from utama_core.config.field_params import GREAT_EXHIBITION_FIELD_DIMS +from utama_core.config.formations import FormationType, get_formations +from utama_core.entities.game.field import FieldBounds +from utama_core.global_utils.mapping_utils import map_left_right_to_colors +from utama_core.run.strategy_runner import StrategyRunner +from utama_core.strategy.common.abstract_strategy import AbstractStrategy +from utama_core.tests.common.abstract_test_manager import ( + AbstractTestManager, + TestingStatus, +) + +os.environ["SDL_VIDEO_WINDOW_POS"] = "100,100" + +POSITION_TOLERANCE = 0.15 + + +class _IdleStrategy(AbstractStrategy): + """Minimal strategy used only to drive runner lifecycle.""" + + exp_ball: bool = True + + def create_behaviour_tree(self) -> py_trees.behaviour.Behaviour: + return py_trees.behaviours.Success(name="Idle") + + def assert_exp_robots(self, n_runtime_friendly: int, n_runtime_enemy: int) -> bool: + return True + + 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 + + +class _CaptureFirstFrameManager(AbstractTestManager): + n_episodes = 1 + + def __init__(self): + super().__init__() + self.friendly_positions: dict[int, tuple[float, float]] = {} + self.enemy_positions: dict[int, tuple[float, float]] = {} + self.ball_position: tuple[float, float] | None = None + + def reset_field(self, sim_controller, game): + # Keep RSIM initial spawn positions generated by the formation allocator. + return None + + def eval_status(self, game): + if not self.friendly_positions: + self.friendly_positions = {rid: (robot.p.x, robot.p.y) for rid, robot in game.friendly_robots.items()} + self.enemy_positions = {rid: (robot.p.x, robot.p.y) for rid, robot in game.enemy_robots.items()} + if game.ball is not None: + self.ball_position = (game.ball.p.x, game.ball.p.y) + return TestingStatus.SUCCESS + + +@pytest.mark.parametrize( + "team_is_yellow,team_on_right,n_friendly,n_enemy", + [ + (True, False, 3, 1), + (False, True, 3, 1), + ], +) +def test_rsim_formation_allocation_and_spawn_positions( + team_is_yellow: bool, + team_on_right: bool, + n_friendly: int, + n_enemy: int, +): + runner = StrategyRunner( + strategy=_IdleStrategy(), + my_team_is_yellow=team_is_yellow, + my_team_is_right=team_on_right, + mode="rsim", + exp_friendly=n_friendly, + exp_enemy=n_enemy, + ) + + left_start, right_start = get_formations( + bounds=runner.field_bounds, + n_left=n_enemy if team_on_right else n_friendly, + n_right=n_friendly if team_on_right else n_enemy, + formation_type=FormationType.START_ONE, + ) + expected_yellow, expected_blue = map_left_right_to_colors( + team_is_yellow, + team_on_right, + right_start, + left_start, + ) + + # Allocation behavior in StrategyRunner._load_sim + assert runner.rsim_env.yellow_formation == expected_yellow + assert runner.rsim_env.blue_formation == expected_blue + + manager = _CaptureFirstFrameManager() + passed = runner.run_test(manager, episode_timeout=5.0, rsim_headless=True) + assert passed + + expected_friendly = expected_yellow if team_is_yellow else expected_blue + expected_enemy = expected_blue if team_is_yellow else expected_yellow + + assert len(manager.friendly_positions) == n_friendly + assert len(manager.enemy_positions) == n_enemy + + for rid, expected in enumerate(expected_friendly): + actual_x, actual_y = manager.friendly_positions[rid] + assert abs(actual_x - expected.x) <= POSITION_TOLERANCE + assert abs(actual_y - expected.y) <= POSITION_TOLERANCE + + for rid, expected in enumerate(expected_enemy): + actual_x, actual_y = manager.enemy_positions[rid] + assert abs(actual_x - expected.x) <= POSITION_TOLERANCE + assert abs(actual_y - expected.y) <= POSITION_TOLERANCE + + +def test_rsim_spawn_respects_shifted_bounds_and_ball_center(): + shifted_bounds = FieldBounds(top_left=(1.5, 2.0), bottom_right=(4.5, -2.0)) + + runner = StrategyRunner( + strategy=_IdleStrategy(), + my_team_is_yellow=True, + my_team_is_right=False, + mode="rsim", + exp_friendly=1, + exp_enemy=0, + field_bounds=shifted_bounds, + ) + + manager = _CaptureFirstFrameManager() + passed = runner.run_test(manager, episode_timeout=5.0, rsim_headless=True) + assert passed + + expected_x, expected_y = shifted_bounds.center + assert manager.ball_position is not None + assert abs(manager.ball_position[0] - expected_x) <= POSITION_TOLERANCE + assert abs(manager.ball_position[1] - expected_y) <= POSITION_TOLERANCE + + +def test_rsim_renderer_resizes_with_non_standard_field_dimensions(): + dims = GREAT_EXHIBITION_FIELD_DIMS + runner = StrategyRunner( + strategy=_IdleStrategy(), + my_team_is_yellow=True, + my_team_is_right=False, + mode="rsim", + exp_friendly=1, + exp_enemy=0, + full_field_dims=dims, + ) + + renderer = runner.rsim_env.field_renderer + scale = renderer.scale + + assert renderer.length == pytest.approx(2 * dims.full_field_half_length * scale) + assert renderer.width == pytest.approx(2 * dims.full_field_half_width * scale) + assert renderer.penalty_length == pytest.approx(2 * dims.half_defense_area_depth * scale) + assert renderer.penalty_width == pytest.approx(2 * dims.half_defense_area_width * scale) + assert renderer.goal_width == pytest.approx(2 * dims.half_goal_width * scale) + + # Goal line and boundary should be centered and align with resized geometry. + goal_top = (renderer.screen_height - renderer.goal_width) / 2 + goal_bottom = goal_top + renderer.goal_width + + assert renderer.margin == pytest.approx(renderer.center_x - renderer.length / 2) + assert renderer.margin == pytest.approx(renderer.center_y - renderer.width / 2) + assert goal_top == pytest.approx(renderer.center_y - dims.half_goal_width * scale) + assert goal_bottom == pytest.approx(renderer.center_y + dims.half_goal_width * scale) diff --git a/utama_core/tests/strategy_runner/test_rsim_render_bounds.py b/utama_core/tests/strategy_runner/test_rsim_render_bounds.py new file mode 100644 index 00000000..89999281 --- /dev/null +++ b/utama_core/tests/strategy_runner/test_rsim_render_bounds.py @@ -0,0 +1,35 @@ +from unittest.mock import MagicMock, patch + +from utama_core.config.enums import Mode +from utama_core.entities.game.field import FieldBounds +from utama_core.run.strategy_runner import StrategyRunner + + +def _make_runner_for_overlay_tests(render_mode: str | None): + with patch.object(StrategyRunner, "__init__", lambda self: None): + runner = StrategyRunner() + runner.mode = Mode.RSIM + runner.field_bounds = FieldBounds(top_left=(-1.0, 2.0), bottom_right=(3.0, -4.0)) + runner.rsim_env = MagicMock() + runner.rsim_env.render_mode = render_mode + return runner + + +def test_draw_rsim_field_bounds_overlay_draws_expected_polygon(): + runner = _make_runner_for_overlay_tests(render_mode="human") + + runner._draw_rsim_field_bounds_overlay() + + runner.rsim_env.draw_polygon.assert_called_once_with( + [(-1.0, 2.0), (3.0, 2.0), (3.0, -4.0), (-1.0, -4.0)], + color="PINK", + width=2, + ) + + +def test_draw_rsim_field_bounds_overlay_not_drawn_when_not_human(): + runner = _make_runner_for_overlay_tests(render_mode=None) + + runner._draw_rsim_field_bounds_overlay() + + runner.rsim_env.draw_polygon.assert_not_called() diff --git a/utama_core/tests/strategy_runner/test_runner_misconfig.py b/utama_core/tests/strategy_runner/test_runner_misconfig.py index 417e4a1a..ae14e4c6 100644 --- a/utama_core/tests/strategy_runner/test_runner_misconfig.py +++ b/utama_core/tests/strategy_runner/test_runner_misconfig.py @@ -1,6 +1,9 @@ +from unittest.mock import MagicMock + import pytest from utama_core.config.enums import Mode +from utama_core.config.field_params import GREAT_EXHIBITION_FIELD_DIMS from utama_core.entities.game.field import FieldBounds from utama_core.run.strategy_runner import StrategyRunner from utama_core.tests.strategy_runner.strat_runner_test_utils import DummyStrategy @@ -51,7 +54,12 @@ def test_assert_exp_robots_too_many_enemy(base_runner): def test_assert_exp_goals_fails(base_runner): # Mock the strategy to return False on assert_exp_goals base_runner.my.strategy.assert_exp_goals = lambda *a, **k: False - with pytest.raises(AssertionError): + base_runner.my.game = MagicMock() + base_runner.my.game.field = MagicMock( + includes_my_goal_line=True, + includes_opp_goal_line=True, + ) + with pytest.raises(RuntimeError, match="Field does not match expected goals"): base_runner._assert_exp_goals() @@ -77,10 +85,10 @@ def test_strategy_runner_valid_bounds(): def test_strategy_runner_invalid_bounds(): - """Should raise AssertionError when bounds are invalid.""" + """Should raise ValueError when bounds are invalid.""" invalid_bounds = FieldBounds(top_left=(3, 2), bottom_right=(-3, -2)) - with pytest.raises(AssertionError): + with pytest.raises(ValueError): StrategyRunner( strategy=DummyStrategy(), my_team_is_yellow=True, @@ -90,3 +98,21 @@ def test_strategy_runner_invalid_bounds(): exp_enemy=3, field_bounds=invalid_bounds, ) + + +def test_strategy_runner_bounds_outside_non_standard_field_dims(): + """Should raise when bounds exceed a custom full field size.""" + # Valid in standard SSL dimensions, but outside GREAT_EXHIBITION_FIELD_DIMS. + too_large_for_custom_dims = FieldBounds(top_left=(-4.5, 3.0), bottom_right=(4.5, -3.0)) + + with pytest.raises(ValueError, match="out of full field bounds"): + StrategyRunner( + strategy=DummyStrategy(), + my_team_is_yellow=True, + my_team_is_right=True, + mode="rsim", + exp_friendly=3, + exp_enemy=3, + full_field_dims=GREAT_EXHIBITION_FIELD_DIMS, + field_bounds=too_large_for_custom_dims, + )