diff --git a/demo_dribbler_test.py b/demo_dribbler_test.py new file mode 100644 index 00000000..61a96e33 --- /dev/null +++ b/demo_dribbler_test.py @@ -0,0 +1,39 @@ +"""demo_dribbler_test.py — Test the dribbler with one robot on the Exhibition Road field. + +Run: + pixi run python demo_dribbler_test.py + +What this does +-------------- +- 1 yellow robot, 0 opponents, Exhibition Road field (3 m × 2.25 m) +- DribblerTestStrategy: robot fetches the ball, carries it forward with the + dribbler on, then stops +- Blue target point drawn in RSim during carry +""" + +from utama_core.config.field_params import GREAT_EXHIBITION_FIELD_DIMS +from utama_core.run import StrategyRunner +from utama_core.strategy.examples.dribbler_test_strategy import DribblerTestStrategy + +N_FRIENDLY = 1 +MY_TEAM_IS_YELLOW = True +MY_TEAM_IS_RIGHT = True + + +def main() -> None: + runner = StrategyRunner( + strategy=DribblerTestStrategy(robot_id=0), + my_team_is_yellow=MY_TEAM_IS_YELLOW, + my_team_is_right=MY_TEAM_IS_RIGHT, + mode="rsim", + exp_friendly=N_FRIENDLY, + exp_enemy=0, + full_field_dims=GREAT_EXHIBITION_FIELD_DIMS, + show_live_status=True, + ) + + runner.run() + + +if __name__ == "__main__": + main() diff --git a/demo_dribbler_test2.py b/demo_dribbler_test2.py new file mode 100644 index 00000000..9d68de8c --- /dev/null +++ b/demo_dribbler_test2.py @@ -0,0 +1,34 @@ +"""demo_dribbler_test2.py — Dribbler sequence test: forward → left → right → back → stop. + +Run: + pixi run python demo_dribbler_test2.py +""" + +from utama_core.config.field_params import GREAT_EXHIBITION_FIELD_DIMS +from utama_core.run import StrategyRunner +from utama_core.strategy.examples.dribbler_test_strategy2 import ( + DribblerSequenceStrategy, +) + +N_FRIENDLY = 1 +MY_TEAM_IS_YELLOW = True +MY_TEAM_IS_RIGHT = True + + +def main() -> None: + runner = StrategyRunner( + strategy=DribblerSequenceStrategy(robot_id=0), + my_team_is_yellow=MY_TEAM_IS_YELLOW, + my_team_is_right=MY_TEAM_IS_RIGHT, + mode="rsim", + exp_friendly=N_FRIENDLY, + exp_enemy=0, + full_field_dims=GREAT_EXHIBITION_FIELD_DIMS, + show_live_status=True, + ) + + runner.run() + + +if __name__ == "__main__": + main() diff --git a/demo_kicker_test.py b/demo_kicker_test.py new file mode 100644 index 00000000..01bb91cd --- /dev/null +++ b/demo_kicker_test.py @@ -0,0 +1,48 @@ +"""demo_kicker_test.py — Test the kicker with one robot on the Exhibition Road field. + +Run: + pixi run python demo_kicker_test.py + +What this does +-------------- +- 1 yellow robot, 0 opponents, Exhibition Road field (3 m × 2.25 m) +- KickerTestStrategy: robot fetches ball, aligns to face enemy goal, kicks, + waits, then repeats +- Red goal center drawn in RSim +""" + +from utama_core.config.field_params import GREAT_EXHIBITION_FIELD_DIMS +from utama_core.run import StrategyRunner +from utama_core.strategy.examples.kicker_test_strategy import KickerTestStrategy + +N_FRIENDLY = 1 +MY_TEAM_IS_YELLOW = True +MY_TEAM_IS_RIGHT = True + + +# Vision ID → firmware command ID mapping. +# Update before running on real hardware: vision ID is the sticker on the robot, +# command ID is baked into the robot's firmware. +YELLOW_VISION_TO_CMD = { + 0: 0, # vision ID 0 → cmd ID 0 ← update with actual firmware ID +} + + +def main() -> None: + runner = StrategyRunner( + strategy=KickerTestStrategy(robot_id=0), + my_team_is_yellow=MY_TEAM_IS_YELLOW, + my_team_is_right=MY_TEAM_IS_RIGHT, + mode="real", + exp_friendly=N_FRIENDLY, + exp_enemy=0, + full_field_dims=GREAT_EXHIBITION_FIELD_DIMS, + show_live_status=True, + yellow_vision_to_cmd_mapping=YELLOW_VISION_TO_CMD, + ) + + runner.run() + + +if __name__ == "__main__": + main() diff --git a/demo_one_robot_placement.py b/demo_one_robot_placement.py new file mode 100644 index 00000000..ca00e171 --- /dev/null +++ b/demo_one_robot_placement.py @@ -0,0 +1,47 @@ +"""demo_one_robot_placement.py — Entry point for testing RobotPlacementStrategy with one robot. + +Run: + pixi run python demo_one_robot_placement.py + +What this sets up +----------------- +- Exhibition Road field (3 m × 2.25 m, ``GREAT_EXHIBITION_FIELD_DIMS``) +- 1 yellow robot, 0 opponents +- RobotPlacementStrategy: robot oscillates vertically around the field center + while facing the ball +""" + +from utama_core.config.field_params import GREAT_EXHIBITION_FIELD_DIMS +from utama_core.run import StrategyRunner +from utama_core.strategy.examples import RobotPlacementStrategy + +N_FRIENDLY = 1 +MY_TEAM_IS_YELLOW = True +MY_TEAM_IS_RIGHT = True + +# Vision ID → firmware command ID mapping. +# Update before running on real hardware: vision ID is the sticker on the robot, +# command ID is baked into the robot's firmware. +YELLOW_VISION_TO_CMD = { + 0: 0, # vision ID 0 → cmd ID 0 ← update with actual firmware ID +} + + +def main() -> None: + runner = StrategyRunner( + strategy=RobotPlacementStrategy(robot_id=0), + my_team_is_yellow=MY_TEAM_IS_YELLOW, + my_team_is_right=MY_TEAM_IS_RIGHT, + mode="real", + exp_friendly=N_FRIENDLY, + exp_enemy=0, + full_field_dims=GREAT_EXHIBITION_FIELD_DIMS, + show_live_status=True, + yellow_vision_to_cmd_mapping=YELLOW_VISION_TO_CMD, + ) + + runner.run() + + +if __name__ == "__main__": + main() diff --git a/utama_core/config/field_params.py b/utama_core/config/field_params.py index 3af87bd5..2b99e91c 100644 --- a/utama_core/config/field_params.py +++ b/utama_core/config/field_params.py @@ -149,10 +149,10 @@ def __post_init__(self): ) 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, + full_field_half_length=1.5, + full_field_half_width=1.125, + half_defense_area_depth=0.25, + half_defense_area_width=0.5, + half_goal_width=0.4, center_circle_radius=0.3, ) diff --git a/utama_core/config/physical_constants.py b/utama_core/config/physical_constants.py index 624d793e..ce45f84d 100644 --- a/utama_core/config/physical_constants.py +++ b/utama_core/config/physical_constants.py @@ -1,3 +1,4 @@ ROBOT_RADIUS = 0.09 MAX_ROBOTS = 6 +MAX_ROBOT_ID = 15 # valid IDs from ssl small ruleset BALL_RADIUS = 0.0215 diff --git a/utama_core/config/settings.py b/utama_core/config/settings.py index d255f4aa..17c16f5d 100644 --- a/utama_core/config/settings.py +++ b/utama_core/config/settings.py @@ -29,7 +29,7 @@ ### REAL CONTROLLER SETTINGS ### BAUD_RATE = 115200 -PORT = "/dev/ttyUSB0" +PORT = "/dev/ttyACM0" TIMEOUT = 0.1 KICKER_COOLDOWN_TIME = 10 # in seconds to prevent kicker from being actuated too frequently KICKER_COOLDOWN_TIMESTEPS = int(KICKER_COOLDOWN_TIME * CONTROL_FREQUENCY) # in timesteps diff --git a/utama_core/data_processing/refiners/robot_info.py b/utama_core/data_processing/refiners/robot_info.py index 5b519cdb..4ade245d 100644 --- a/utama_core/data_processing/refiners/robot_info.py +++ b/utama_core/data_processing/refiners/robot_info.py @@ -1,26 +1,67 @@ import warnings from dataclasses import replace -from typing import List +from typing import FrozenSet, List, Optional +from utama_core.config.physical_constants import ROBOT_RADIUS from utama_core.data_processing.refiners.base_refiner import BaseRefiner from utama_core.entities.data.command import RobotResponse from utama_core.entities.game.game_frame import GameFrame # TODO: current doesn't handle has_ball for enemy robots. In future, implement using vision data +# Distance threshold for vision-based has_ball inference: robot centre + small buffer. +_BALL_CAPTURE_DIST = ROBOT_RADIUS + 0.04 # ~0.13 m + class RobotInfoRefiner(BaseRefiner): + """Merges IR-sensor robot responses into the game frame. + + Args: + trusted_ir_robots: Set of robot IDs (vision IDs) whose IR sensor is known to be + working. Robots in this set use the raw sensor reading directly. Any robot + ID **not** in this set has its has_ball inferred from vision proximity + (~0.13 m threshold) instead. Pass ``None`` (default) to trust every + robot's IR sensor — this is the normal stable-hardware behaviour and can + be restored by simply removing the argument. + """ + + def __init__(self, trusted_ir_robots: Optional[FrozenSet[int]] = None): + self._trusted_ir_robots = trusted_ir_robots + def refine(self, game_frame: GameFrame, robot_responses: List[RobotResponse]): - if robot_responses is None or len(robot_responses) == 0: + friendly_robots = game_frame.friendly_robots.copy() + + # When an allowlist is active, first infer has_ball for every untrusted + # robot from vision proximity. This covers frames where the robot drops + # its serial response entirely — without this pass, has_ball would stay + # frozen at the previous frame's value instead of being inferred. + if self._trusted_ir_robots is not None: + for robot_id, robot in friendly_robots.items(): + if robot_id not in self._trusted_ir_robots: + friendly_robots[robot_id] = replace(robot, has_ball=self._infer_has_ball(game_frame, robot)) + + # Then overlay IR sensor readings for robots that sent a response. + if robot_responses: + for robot_response in robot_responses: + rid = robot_response.id + if rid not in friendly_robots: + warnings.warn(f"Robot ID {rid} in robot responses not found in friendly robots. ") + continue + + robot = friendly_robots[rid] + if self._trusted_ir_robots is None or rid in self._trusted_ir_robots: + # Trusted (or trust-all mode): use raw IR reading + friendly_robots[rid] = replace(robot, has_ball=robot_response.has_ball) + # Untrusted robots were already handled by the vision-proximity pass above + + if friendly_robots == game_frame.friendly_robots: return game_frame + return replace(game_frame, friendly_robots=friendly_robots) - friendly_robots = game_frame.friendly_robots.copy() - for robot_response in robot_responses: - id = robot_response.id - if id in friendly_robots: - robot = friendly_robots[id] - friendly_robots[id] = replace(robot, has_ball=robot_response.has_ball) - else: - warnings.warn(f"Robot ID {id} in robot responses not found in friendly robots. ") - new_game_frame = replace(game_frame, friendly_robots=friendly_robots) - return new_game_frame + @staticmethod + def _infer_has_ball(game_frame: GameFrame, robot) -> bool: + """Proximity-based has_ball: True when robot is within capture distance of ball.""" + if game_frame.ball is None: + return False + ball_2d = game_frame.ball.p.to_2d() + return robot.p.distance_to(ball_2d) < _BALL_CAPTURE_DIST diff --git a/utama_core/global_utils/mapping_utils.py b/utama_core/global_utils/mapping_utils.py index 5d5971a0..3ce236f4 100644 --- a/utama_core/global_utils/mapping_utils.py +++ b/utama_core/global_utils/mapping_utils.py @@ -23,6 +23,23 @@ def map_friendly_enemy_to_colors(my_team_is_yellow: bool, friendly_item: T, enem return yellow_item, blue_item +# NB: actually map_friendly_enemy_to_colors and map_colors_to_friendly_enemy have the same logic + + +def map_colors_to_friendly_enemy(my_team_is_yellow: bool, yellow_item: T, blue_item: T) -> Tuple[T, T]: + """Map yellow and blue items to their respective friendly and enemy items based on my team color. + + Args: + my_team_is_yellow (bool): True if the team is yellow, False if blue. + yellow_item (T): Any item from the yellow team (int, list, etc.) + blue_item (T): Any item from the blue team (int, list, etc.) + + Returns: + Tuple[T, T]: A tuple of (friendly_item, enemy_item). + """ + return map_friendly_enemy_to_colors(my_team_is_yellow, yellow_item, blue_item) + + def map_left_right_to_colors(my_team_is_yellow: bool, my_team_is_right: bool, right_item: T, left_item: T): """Map left and right items to their respective colors based on my team color and position. diff --git a/utama_core/run/strategy_runner.py b/utama_core/run/strategy_runner.py index c47cc35d..35b4cf53 100644 --- a/utama_core/run/strategy_runner.py +++ b/utama_core/run/strategy_runner.py @@ -6,7 +6,7 @@ import warnings from collections import deque from dataclasses import dataclass, field -from typing import TYPE_CHECKING, List, Optional, Tuple +from typing import TYPE_CHECKING, FrozenSet, List, Optional, Tuple from rich.live import Live from rich.text import Text @@ -14,7 +14,7 @@ from utama_core.config.enums import Mode, mode_str_to_enum from utama_core.config.field_params import STANDARD_FIELD_DIMS, FieldDimensions from utama_core.config.formations import FormationType, get_formations -from utama_core.config.physical_constants import MAX_ROBOTS +from utama_core.config.physical_constants import MAX_ROBOT_ID, MAX_ROBOTS from utama_core.config.settings import ( FPS_PRINT_INTERVAL, MAX_CAMERAS, @@ -29,12 +29,13 @@ RobotInfoRefiner, VelocityRefiner, ) -from utama_core.entities.data.command import RobotCommand +from utama_core.entities.data.command import RobotCommand, RobotResponse from utama_core.entities.data.raw_vision import RawVisionData from utama_core.entities.game import Game, GameFrame, GameHistory from utama_core.entities.game.field import Field, FieldBounds from utama_core.entities.referee.referee_command import RefereeCommand from utama_core.global_utils.mapping_utils import ( + map_colors_to_friendly_enemy, map_friendly_enemy_to_colors, map_left_right_to_colors, ) @@ -135,6 +136,14 @@ class StrategyRunner: instance to use the in-process referee, ``OfficialReferee()`` to consume commands from the SSL game-controller over the network, or ``None`` (default) to run without any referee input. + yellow_vision_to_cmd_mapping (dict[int, int], optional): Mapping from vision robot IDs to command robot IDs for the yellow team. + Used only in real mode. In real PVP/shared-transmitter mode, mappings are required for both teams and must include all expected robots. + blue_vision_to_cmd_mapping (dict[int, int], optional): Mapping from vision robot IDs to command robot IDs for the blue team. + Used only in real mode. In real PVP/shared-transmitter mode, mappings are required for both teams and must include all expected robots. + yellow_trusted_ir_robots (FrozenSet[int], optional): Vision IDs of yellow-team robots whose IR (has_ball) sensor + is confirmed working. Robots NOT in this set fall back to vision-proximity inference (~0.13 m). + Pass ``None`` (default) to trust all IR sensors — remove this argument once sensors are stable. + blue_trusted_ir_robots (FrozenSet[int], optional): Same as ``yellow_trusted_ir_robots`` for the blue team. """ def __init__( @@ -160,6 +169,10 @@ def __init__( filtering: bool = False, referee: RefereeSource = None, formation_type: Optional[FormationType] = None, + yellow_vision_to_cmd_mapping: Optional[dict[int, int]] = None, + blue_vision_to_cmd_mapping: Optional[dict[int, int]] = None, + yellow_trusted_ir_robots: Optional[FrozenSet[int]] = None, + blue_trusted_ir_robots: Optional[FrozenSet[int]] = None, ): self.logger = logging.getLogger(__name__) @@ -193,8 +206,16 @@ def __init__( ) self.referee_refiner = RefereeRefiner() + my_trusted_ir = yellow_trusted_ir_robots if my_team_is_yellow else blue_trusted_ir_robots + opp_trusted_ir = blue_trusted_ir_robots if my_team_is_yellow else yellow_trusted_ir_robots self.my, self.opp = self._setup_sides_data( - strategy, opp_strategy, filtering, control_scheme, opp_control_scheme + strategy, + opp_strategy, + filtering, + control_scheme, + opp_control_scheme, + my_trusted_ir_robots=my_trusted_ir, + opp_trusted_ir_robots=opp_trusted_ir, ) ### functions below rely on self.my and self.opp ### @@ -202,6 +223,21 @@ def __init__( self.rsim_env, self.sim_controller = self._load_sim(rsim_noise, rsim_vanishing) self._assert_exp_robots_and_ball(exp_friendly, exp_enemy, exp_ball) + # mapping for mismatch between vision and cmd ids + self.yellow_vision_to_cmd_mapping = self._validate_vision_to_cmd_mapping( + yellow_vision_to_cmd_mapping, is_yellow=True + ) + self.blue_vision_to_cmd_mapping = self._validate_vision_to_cmd_mapping( + blue_vision_to_cmd_mapping, is_yellow=False + ) + self.yellow_cmd_to_vision_mapping = {v: k for k, v in self.yellow_vision_to_cmd_mapping.items()} + self.blue_cmd_to_vision_mapping = {v: k for k, v in self.blue_vision_to_cmd_mapping.items()} + + if self.opp and self.mode == Mode.REAL: + self._check_no_cmd_duplicate_if_transmission_sharing( + self.yellow_vision_to_cmd_mapping, self.blue_vision_to_cmd_mapping + ) + self._load_robot_controllers() # Remove Rsim ball. Rsim does not have the flexibilty to start without a ball. @@ -259,6 +295,106 @@ def __init__( self.profiler_name = profiler_name self.profiler = cProfile.Profile() if profiler_name else None + def _validate_vision_to_cmd_mapping(self, mapping: Optional[dict[int, int]], is_yellow: bool) -> dict[int, int]: + if self.mode == Mode.REAL: + explicitly_provided = mapping is not None + if mapping is None: + if self.opp: + raise ValueError( + "explicit vision_to_cmd_mapping is required for both teams in real PVP/shared-transmitter mode." + ) + return {} + + if not isinstance(mapping, dict): + raise TypeError( + f"vision_to_cmd_mapping must be a dictionary mapping vision robot IDs to command robot IDs; got {type(mapping).__name__}." + ) + + # if we are not running an opp strat, but mapping provided, warn that it will be ignored + if self.opp is None and self.my_team_is_yellow ^ is_yellow: + warnings.warn( + "vision_to_cmd_mapping is provided but will be ignored since the opponent team is not being controlled." + ) + + if self.opp and explicitly_provided: + if is_yellow ^ self.my_team_is_yellow: + exp_count = self.exp_enemy + team_label = "opponent" + else: + exp_count = self.exp_friendly + team_label = "friendly" + + # At init time we only know how many robots to expect, not their + # actual vision IDs (those are non-contiguous in some deployments). + # Check count here; key-coverage against observed IDs happens in + # _validate_mapping_covers_game_frame() after _load_game(). + if len(mapping) != exp_count: + raise ValueError( + f"vision_to_cmd_mapping for {team_label} team has {len(mapping)} entries but " + f"{exp_count} robots are expected. Every robot must have a mapping entry " + "in shared-transmitter mode." + ) + + for vision_id, cmd_id in mapping.items(): + if not isinstance(vision_id, int) or not isinstance(cmd_id, int): + raise TypeError( + f"vision_to_cmd_mapping must map integers to integers; got key type {type(vision_id).__name__} and value type {type(cmd_id).__name__}." + ) + if vision_id < 0 or cmd_id < 0: + raise ValueError( + f"vision_to_cmd_mapping cannot have negative IDs; got vision ID {vision_id} and command ID {cmd_id}." + ) + if vision_id > MAX_ROBOT_ID: + raise ValueError( + f"vision_to_cmd_mapping cannot have vision IDs greater than {MAX_ROBOT_ID}; got vision ID {vision_id}." + ) + if cmd_id > 0xFF: + raise ValueError( + f"vision_to_cmd_mapping cannot have command IDs greater than 255 (1 byte limit); got command ID {cmd_id}." + ) + return mapping + else: + if mapping is not None: + raise ValueError( + "vision_to_cmd_mapping should not be provided in simulation modes; robot ID mapping is only needed in real mode." + ) + return {} + + def _validate_mapping_covers_game_frame(self, mapping: dict[int, int], observed_ids: set[int], team_label: str): + """After the first game frame loads, verify the mapping keys match the observed vision IDs. + + Called from _load_game() so we can validate against actual IDs rather than + assuming the contiguous 0..n-1 range (which fails non-contiguous deployments). + """ + if not mapping: + return + missing = observed_ids - mapping.keys() + extra = mapping.keys() - observed_ids + if missing or extra: + parts = [] + if missing: + parts.append(f"missing entries for observed IDs {sorted(missing)}") + if extra: + parts.append(f"extra entries for unseen IDs {sorted(extra)}") + raise ValueError( + f"vision_to_cmd_mapping for {team_label} team does not match observed vision IDs: " + + "; ".join(parts) + + f". Observed IDs: {sorted(observed_ids)}." + ) + + def _check_no_cmd_duplicate_if_transmission_sharing( + self, yellow_mapping: dict[int, int], blue_mapping: dict[int, int] + ): + seen = set() + dicts = [yellow_mapping, blue_mapping] + for d in dicts: + for v in d.values(): + if v in seen: + raise ValueError( + f"vision_to_cmd_mapping for friendly and opponent teams cannot have overlapping command IDs since commands are transmitted together; duplicate command ID: {v}." + ) + seen.add(v) + def _handle_sigint(self, sig, frame): self._stop_event.set() signal.default_int_handler(sig, frame) @@ -343,6 +479,8 @@ def _setup_sides_data( filtering: bool, control_scheme: str, opp_control_scheme: Optional[str], + my_trusted_ir_robots: Optional[FrozenSet[int]] = None, + opp_trusted_ir_robots: Optional[FrozenSet[int]] = None, ) -> Tuple[SideRuntime, Optional[SideRuntime]]: """Setup the data structures for both sides (my team and opponent) Args: @@ -351,6 +489,8 @@ def _setup_sides_data( filtering (bool): Whether to use filtering in the position refiners. control_scheme (str): Name of the motion control scheme to use for the friendly team. opp_control_scheme (Optional[str]): Name of the motion control scheme to use for the opponent team. If not set, uses same as friendly. + my_trusted_ir_robots (FrozenSet[int], optional): Vision IDs of friendly robots whose IR sensor is trusted. + opp_trusted_ir_robots (FrozenSet[int], optional): Vision IDs of opponent robots whose IR sensor is trusted. Side effect: Initializes the SideRuntime for both friendly and opponent sides, including their strategies, refiners, and motion controllers. @@ -359,7 +499,10 @@ def _setup_sides_data( """ opp_side = None my_pos_ref, my_vel_ref, my_robot_ref = self._init_refiners( - self.full_field_dims, filtering=filtering, exp_ball=self.exp_ball + self.full_field_dims, + filtering=filtering, + exp_ball=self.exp_ball, + trusted_ir_robots=my_trusted_ir_robots, ) my_motion_controller = get_control_scheme(control_scheme) my_strategy.setup_strategy_blackboard(is_opp_strat=False) @@ -373,7 +516,10 @@ def _setup_sides_data( if opp_strategy is not None: opp_pos_ref, opp_vel_ref, opp_robot_ref = self._init_refiners( - self.full_field_dims, filtering=filtering, exp_ball=self.exp_ball + self.full_field_dims, + filtering=filtering, + exp_ball=self.exp_ball, + trusted_ir_robots=opp_trusted_ir_robots, ) opp_motion_controller = ( get_control_scheme(opp_control_scheme) if opp_control_scheme is not None else my_motion_controller @@ -389,6 +535,41 @@ def _setup_sides_data( return my_side, opp_side + def _split_robot_responses_by_team( + self, responses: List[RobotResponse] + ) -> Tuple[List[RobotResponse], List[RobotResponse]]: + """Split a list of RobotResponse objects into separate lists for the friendly and opponent teams + based on the robot IDs and the vision. + """ + friendly_responses = [] + opponent_responses = [] + + for response in responses: + cmd_id = response.id + + if self.my_team_is_yellow: + vision_id = self.yellow_cmd_to_vision_mapping.get(cmd_id) + if vision_id is not None: + friendly_responses.append(RobotResponse(vision_id, response.has_ball)) + else: + opp_vision_id = self.blue_cmd_to_vision_mapping.get(cmd_id) + if opp_vision_id is not None: + opponent_responses.append(RobotResponse(opp_vision_id, response.has_ball)) + else: + self.logger.warning(f"RobotResponse cmd_id={cmd_id} not found in either yellow or blue mapping") + else: + vision_id = self.blue_cmd_to_vision_mapping.get(cmd_id) + if vision_id is not None: + friendly_responses.append(RobotResponse(vision_id, response.has_ball)) + else: + opp_vision_id = self.yellow_cmd_to_vision_mapping.get(cmd_id) + if opp_vision_id is not None: + opponent_responses.append(RobotResponse(opp_vision_id, response.has_ball)) + else: + self.logger.warning(f"RobotResponse cmd_id={cmd_id} not found in either blue or yellow mapping") + + return friendly_responses, opponent_responses + def _remove_rsim_ball(self): """Removes the ball from the RSim environment by teleporting it off-field.""" self.sim_controller.remove_ball() @@ -632,12 +813,23 @@ def _load_robot_controllers(self): ) elif self.mode == Mode.REAL: + my_viz_to_cmd_mapping, opp_viz_to_cmd_mapping = map_colors_to_friendly_enemy( + self.my_team_is_yellow, + self.yellow_vision_to_cmd_mapping, + self.blue_vision_to_cmd_mapping, + ) my_robot_controller = RealRobotController( - is_team_yellow=self.my_team_is_yellow, n_friendly=self.exp_friendly + is_team_yellow=self.my_team_is_yellow, + n_friendly=self.exp_friendly, + vision_to_cmd_mapping=my_viz_to_cmd_mapping, ) if self.opp: + serial = my_robot_controller.serial_port # share serial connection for efficiency opp_robot_controller = RealRobotController( - is_team_yellow=not self.my_team_is_yellow, n_friendly=self.exp_enemy + is_team_yellow=not self.my_team_is_yellow, + n_friendly=self.exp_enemy, + vision_to_cmd_mapping=opp_viz_to_cmd_mapping, + serial_port=serial, ) else: @@ -654,14 +846,17 @@ def _init_refiners( field_dims: FieldDimensions, filtering: bool, exp_ball: bool = True, + trusted_ir_robots: Optional[FrozenSet[int]] = None, ) -> tuple[PositionRefiner, VelocityRefiner, RobotInfoRefiner]: """ Initialize the position, velocity, and robot info refiners. Args: - field_bounds (FieldBounds): The bounds of the field. + field_dims (FieldDimensions): The field dimensions. filtering (bool): Whether to use filtering in the position refiner. exp_ball (bool): Whether the ball is expected. When False, the position refiner is allowed to return None if no ball is detected in raw vision data. + trusted_ir_robots (FrozenSet[int], optional): Vision IDs of robots whose IR sensor is trusted. + See RobotInfoRefiner for details. Returns: tuple: The initialized PositionRefiner, VelocityRefiner, and RobotInfoRefiner. """ @@ -671,7 +866,7 @@ def _init_refiners( exp_ball=exp_ball, ) velocity_refiner = VelocityRefiner() - robot_info_refiner = RobotInfoRefiner() + robot_info_refiner = RobotInfoRefiner(trusted_ir_robots=trusted_ir_robots) return position_refiner, velocity_refiner, robot_info_refiner @@ -708,6 +903,22 @@ def _load_game(self): self.opp.game = Game(self.opp.game_history, opp_current_game_frame, field=opp_field) self.opp.current_game_frame = opp_current_game_frame + # Validate mapping key coverage against the real observed vision IDs now + # that we have a game frame (at init we only knew the expected count). + if self.opp: + my_mapping = ( + self.yellow_vision_to_cmd_mapping if self.my_team_is_yellow else self.blue_vision_to_cmd_mapping + ) + opp_mapping = ( + self.blue_vision_to_cmd_mapping if self.my_team_is_yellow else self.yellow_vision_to_cmd_mapping + ) + self._validate_mapping_covers_game_frame( + my_mapping, set(my_current_game_frame.friendly_robots.keys()), "friendly" + ) + self._validate_mapping_covers_game_frame( + opp_mapping, set(opp_current_game_frame.friendly_robots.keys()), "opponent" + ) + self.my.strategy.load_game(self.my.game) if self.opp: self.opp.strategy.load_game(self.opp.game) @@ -928,15 +1139,36 @@ def _run_step(self): self._last_referee_data = self.ref_buffer.popleft() referee_data = self._last_referee_data + friendly_res, opp_res = None, None + if self.mode == Mode.REAL: + responses = self.my.strategy.robot_controller.get_robots_responses() + if self.opp: + friendly_res, opp_res = self._split_robot_responses_by_team(responses) + else: + cmd_to_vision = ( + self.yellow_cmd_to_vision_mapping if self.my_team_is_yellow else self.blue_cmd_to_vision_mapping + ) + if cmd_to_vision: + friendly_res = [] + for r in responses: + vision_id = cmd_to_vision.get(r.id) + if vision_id is None: + self.logger.warning(f"RobotResponse cmd_id={r.id} not found in mapping for controlled team") + continue + friendly_res.append(RobotResponse(vision_id, r.has_ball)) + else: + friendly_res = responses + # alternate between opp and friendly playing + real = self.mode == Mode.REAL if self.toggle_opp_first: if self.opp: - self._step_game(vision_frames, referee_data, True) - self._step_game(vision_frames, referee_data, False) + self._step_game(vision_frames, referee_data, True, real_responses=opp_res if real else None) + self._step_game(vision_frames, referee_data, False, real_responses=friendly_res if real else None) else: - self._step_game(vision_frames, referee_data, False) + self._step_game(vision_frames, referee_data, False, real_responses=friendly_res if real else None) if self.opp: - self._step_game(vision_frames, referee_data, True) + self._step_game(vision_frames, referee_data, True, real_responses=opp_res if real else None) self.toggle_opp_first = not self.toggle_opp_first # --- rate limiting --- @@ -1020,6 +1252,7 @@ def _step_game( vision_frames: List[RawVisionData], referee_data, running_opp: bool, + real_responses: Optional[List[RobotResponse]] = None, ): """Step the game for the robot controller and strategy. @@ -1027,11 +1260,16 @@ def _step_game( vision_frames (List[RawVisionData]): The vision frames. referee_data: The referee data from RSim or network receiver. running_opp (bool): Whether to run the opponent strategy. + real_responses (Optional[List[RobotResponse]]): The robot responses pulled for real. + We use a shared transmitter, so it cannot be pulled per side. """ side = self.opp if running_opp else self.my # Pull responses from robot controller - responses = side.strategy.robot_controller.get_robots_responses() + if self.mode != Mode.REAL: + responses = side.strategy.robot_controller.get_robots_responses() + else: + responses = real_responses if real_responses is not None else [] # Update game frame with refined information new_game_frame = side.position_refiner.refine(side.current_game_frame, vision_frames) diff --git a/utama_core/strategy/examples/dribbler_test_strategy.py b/utama_core/strategy/examples/dribbler_test_strategy.py new file mode 100644 index 00000000..42f6f8c7 --- /dev/null +++ b/utama_core/strategy/examples/dribbler_test_strategy.py @@ -0,0 +1,100 @@ +"""Strategy that tests the dribbler: fetch ball, carry it forward, done.""" + +import math + +import py_trees +from py_trees.composites import Sequence + +from utama_core.global_utils.math_utils import Vector2D +from utama_core.skills.src.go_to_ball import go_to_ball +from utama_core.skills.src.utils.move_utils import empty_command, move +from utama_core.strategy.common.abstract_behaviour import AbstractBehaviour +from utama_core.strategy.common.abstract_strategy import ( + AbstractStrategy, + SpaceRequirements, +) +from utama_core.strategy.examples.utils import SetBlackboardVariable + +_CARRY_DIST = 0.5 # metres to carry the ball forward +_ARRIVE_TOL = 0.10 # metres — done + + +class DribblerStep(AbstractBehaviour): + """ + Fetch ball without dribbler, carry it forward with dribbler on, then stop. + + States: + FETCH — approach ball until has_ball, dribbler off + CARRY — dribbler on, drive forward _CARRY_DIST metres from fetch position + DONE — dribbler off, idle + """ + + def __init__(self, robot_id_key: str): + super().__init__() + self.robot_id_key = robot_id_key + self._state = "FETCH" + self._target: Vector2D | None = None + + def setup_(self): + self.blackboard.register_key(key=self.robot_id_key, access=py_trees.common.Access.READ) + + def update(self) -> py_trees.common.Status: + game = self.blackboard.game + rsim_env = self.blackboard.rsim_env + robot_id: int = self.blackboard.get(self.robot_id_key) + + robot = game.friendly_robots[robot_id] + ball = game.ball.p.to_2d() + + if self._state == "FETCH": + cmd = go_to_ball(game, self.blackboard.motion_controller, robot_id, dribble_when_near=False) + if robot.has_ball: + # Target is _CARRY_DIST forward (toward enemy goal) from current ball position + goal_x = game.field.enemy_goal_line[0][0] + forward = 1.0 if goal_x > 0 else -1.0 + self._target = Vector2D(ball.x + forward * _CARRY_DIST, ball.y) + self._state = "CARRY" + + elif self._state == "CARRY": + target = self._target + if rsim_env: + rsim_env.draw_point(target.x, target.y, color="blue") + face_oren = robot.p.angle_to(target) + cmd = move(game, self.blackboard.motion_controller, robot_id, target, face_oren, dribbling=True) + if math.dist((robot.p.x, robot.p.y), (target.x, target.y)) < _ARRIVE_TOL: + self._state = "DONE" + + else: # DONE + cmd = empty_command(dribbler_on=False) + + self.blackboard.cmd_map[robot_id] = cmd + return py_trees.common.Status.RUNNING + + +class DribblerTestStrategy(AbstractStrategy): + """1-robot strategy: fetch ball → carry forward with dribbler → stop.""" + + def __init__(self, robot_id: int = 0): + self.robot_id = robot_id + super().__init__() + + def assert_exp_robots(self, n_runtime_friendly: int, n_runtime_enemy: int) -> bool: + return n_runtime_friendly == 1 and n_runtime_enemy == 0 + + def assert_exp_goals(self, includes_my_goal_line: bool, includes_opp_goal_line: bool) -> bool: + return True + + def get_min_bounding_req(self) -> SpaceRequirements: + return SpaceRequirements(min_length=1.0, min_width=0.5) + + def create_behaviour_tree(self) -> py_trees.behaviour.Behaviour: + robot_id_key = "target_robot_id" + + root = Sequence(name="DribblerRoot", memory=False) + root.add_children( + [ + SetBlackboardVariable("SetRobotID", robot_id_key, self.robot_id), + DribblerStep(robot_id_key=robot_id_key), + ] + ) + return root diff --git a/utama_core/strategy/examples/dribbler_test_strategy2.py b/utama_core/strategy/examples/dribbler_test_strategy2.py new file mode 100644 index 00000000..98f86587 --- /dev/null +++ b/utama_core/strategy/examples/dribbler_test_strategy2.py @@ -0,0 +1,117 @@ +"""Strategy that tests the dribbler through a fixed sequence: forward → left → right → back → stop.""" + +import math + +import py_trees +from py_trees.composites import Sequence + +from utama_core.global_utils.math_utils import Vector2D +from utama_core.skills.src.go_to_ball import go_to_ball +from utama_core.skills.src.utils.move_utils import empty_command, move +from utama_core.strategy.common.abstract_behaviour import AbstractBehaviour +from utama_core.strategy.common.abstract_strategy import ( + AbstractStrategy, + SpaceRequirements, +) +from utama_core.strategy.examples.utils import SetBlackboardVariable + +_CARRY_DIST = 0.4 # metres per leg +_ARRIVE_TOL = 0.10 + + +class DribblerSequenceStep(AbstractBehaviour): + """ + Fetch ball until has_ball, then carry through a fixed sequence of legs, dribbler off when done. + + Sequence after fetch: + 1. Forward (toward enemy goal) + 2. Left (+y) + 3. Right (-y, back across) + 4. Backward (away from enemy goal) + 5. DONE — dribbler off, idle + """ + + def __init__(self, robot_id_key: str): + super().__init__() + self.robot_id_key = robot_id_key + self._state = "FETCH" + self._legs: list[Vector2D] = [] + self._leg_idx = 0 + self._origin: Vector2D | None = None + + def setup_(self): + self.blackboard.register_key(key=self.robot_id_key, access=py_trees.common.Access.READ) + + def _build_legs(self, fetch_pos: Vector2D, forward: float) -> list[Vector2D]: + """Absolute waypoints for each carry leg, computed once at fetch time.""" + x0, y0 = fetch_pos.x, fetch_pos.y + p1 = Vector2D(x0 + forward * _CARRY_DIST, y0) # forward + p2 = Vector2D(x0 + forward * _CARRY_DIST, y0 + _CARRY_DIST) # left + p3 = Vector2D(x0 + forward * _CARRY_DIST, y0 - _CARRY_DIST) # right + p4 = Vector2D(x0, y0 - _CARRY_DIST) # back + return [p1, p2, p3, p4] + + def update(self) -> py_trees.common.Status: + game = self.blackboard.game + rsim_env = self.blackboard.rsim_env + robot_id: int = self.blackboard.get(self.robot_id_key) + + robot = game.friendly_robots[robot_id] + ball = game.ball.p.to_2d() + + if self._state == "FETCH": + cmd = go_to_ball(game, self.blackboard.motion_controller, robot_id, dribble_when_near=False) + if robot.has_ball: + goal_x = game.field.enemy_goal_line[0][0] + forward = 1.0 if goal_x > 0 else -1.0 + self._legs = self._build_legs(ball, forward) + self._leg_idx = 0 + self._state = "CARRY" + + elif self._state == "CARRY": + target = self._legs[self._leg_idx] + if rsim_env: + rsim_env.draw_point(target.x, target.y, color="blue") + + face_oren = robot.p.angle_to(target) + cmd = move(game, self.blackboard.motion_controller, robot_id, target, face_oren, dribbling=True) + + if math.dist((robot.p.x, robot.p.y), (target.x, target.y)) < _ARRIVE_TOL: + self._leg_idx += 1 + if self._leg_idx >= len(self._legs): + self._state = "DONE" + + else: # DONE + cmd = empty_command(dribbler_on=False) + + self.blackboard.cmd_map[robot_id] = cmd + return py_trees.common.Status.RUNNING + + +class DribblerSequenceStrategy(AbstractStrategy): + """1-robot strategy: fetch → carry forward/left/right/back → stop.""" + + def __init__(self, robot_id: int = 0): + self.robot_id = robot_id + super().__init__() + + def assert_exp_robots(self, n_runtime_friendly: int, n_runtime_enemy: int) -> bool: + return n_runtime_friendly == 1 and n_runtime_enemy == 0 + + def assert_exp_goals(self, includes_my_goal_line: bool, includes_opp_goal_line: bool) -> bool: + return True + + def get_min_bounding_req(self) -> SpaceRequirements: + return SpaceRequirements(min_length=1.0, min_width=1.0) + + def create_behaviour_tree(self) -> py_trees.behaviour.Behaviour: + robot_id_key = "target_robot_id" + + root = Sequence(name="DribblerSeqRoot", memory=False) + root.add_children( + [ + SetBlackboardVariable("SetRobotID", robot_id_key, self.robot_id), + DribblerSequenceStep(robot_id_key=robot_id_key), + ] + ) + return root diff --git a/utama_core/strategy/examples/kicker_test_strategy.py b/utama_core/strategy/examples/kicker_test_strategy.py new file mode 100644 index 00000000..21eca184 --- /dev/null +++ b/utama_core/strategy/examples/kicker_test_strategy.py @@ -0,0 +1,114 @@ +"""Strategy that tests the kicker: go to ball, align to goal, kick.""" + +import math + +import py_trees +from py_trees.composites import Sequence + +from utama_core.global_utils.math_utils import Vector2D +from utama_core.skills.src.go_to_ball import go_to_ball +from utama_core.skills.src.utils.move_utils import empty_command, kick, turn_on_spot +from utama_core.strategy.common.abstract_behaviour import AbstractBehaviour +from utama_core.strategy.common.abstract_strategy import ( + AbstractStrategy, + SpaceRequirements, +) +from utama_core.strategy.examples.utils import SetBlackboardVariable + +_ALIGN_TOL = 0.06 # radians +_KICK_TICKS = 3 +_WAIT_TICKS = 40 + + +class KickerStep(AbstractBehaviour): + """ + Go to ball, turn to face goal, kick, wait, repeat. + + States: + FETCH — go_to_ball until has_ball + ALIGN — turn on spot to face enemy goal + KICK — issue kick for _KICK_TICKS ticks + WAIT — idle while ball travels + """ + + def __init__(self, robot_id_key: str): + super().__init__() + self.robot_id_key = robot_id_key + self._state = "FETCH" + self._kick_ticks = 0 + self._wait_ticks = 0 + + def setup_(self): + self.blackboard.register_key(key=self.robot_id_key, access=py_trees.common.Access.READ) + + def update(self) -> py_trees.common.Status: + game = self.blackboard.game + rsim_env = self.blackboard.rsim_env + robot_id: int = self.blackboard.get(self.robot_id_key) + + robot = game.friendly_robots[robot_id] + + goal_x = game.field.enemy_goal_line[0][0] + goal = Vector2D(goal_x, 0.0) + ball = game.ball.p.to_2d() # + Vector2D(0, 0.05) # aim slightly above center for better ball contact + shoot_oren = ball.angle_to(goal) + + if rsim_env: + rsim_env.draw_point(goal.x, goal.y, color="red") + + if self._state == "FETCH": + cmd = go_to_ball(game, self.blackboard.motion_controller, robot_id, dribble_when_near=True) + if robot.has_ball: + self._state = "ALIGN" + + elif self._state == "ALIGN": + cmd = turn_on_spot(game, self.blackboard.motion_controller, robot_id, shoot_oren, dribbling=True) + oren_err = abs((robot.orientation - shoot_oren + math.pi) % (2 * math.pi) - math.pi) + if oren_err < _ALIGN_TOL: + self._kick_ticks = 0 + self._state = "KICK" + + elif self._state == "KICK": + cmd = kick() + self._kick_ticks += 1 + if self._kick_ticks >= _KICK_TICKS: + self._wait_ticks = 0 + self._state = "WAIT" + + else: # WAIT + cmd = empty_command() + self._wait_ticks += 1 + if self._wait_ticks > _WAIT_TICKS: + self._state = "FETCH" + + self.blackboard.cmd_map[robot_id] = cmd + return py_trees.common.Status.RUNNING + + +class KickerTestStrategy(AbstractStrategy): + """1-robot strategy: fetch ball → align to goal → kick → repeat.""" + + def __init__(self, robot_id: int = 0): + self.robot_id = robot_id + super().__init__() + + def assert_exp_robots(self, n_runtime_friendly: int, n_runtime_enemy: int) -> bool: + return n_runtime_friendly == 1 and n_runtime_enemy == 0 + + def assert_exp_goals(self, includes_my_goal_line: bool, includes_opp_goal_line: bool) -> bool: + return True + + def get_min_bounding_req(self) -> SpaceRequirements: + return SpaceRequirements(min_length=2.0, min_width=1.0) + + def create_behaviour_tree(self) -> py_trees.behaviour.Behaviour: + robot_id_key = "target_robot_id" + + root = Sequence(name="KickerRoot", memory=False) + root.add_children( + [ + SetBlackboardVariable("SetRobotID", robot_id_key, self.robot_id), + KickerStep(robot_id_key=robot_id_key), + ] + ) + return root diff --git a/utama_core/strategy/examples/one_robot_placement_strategy.py b/utama_core/strategy/examples/one_robot_placement_strategy.py index aa92fd84..4098af6a 100644 --- a/utama_core/strategy/examples/one_robot_placement_strategy.py +++ b/utama_core/strategy/examples/one_robot_placement_strategy.py @@ -1,18 +1,13 @@ import math -import random -from typing import Any, Optional import numpy as np import py_trees from py_trees.composites import Sequence from utama_core.config.settings import TIMESTEP -from utama_core.entities.game.field import Field, 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 robot_control.src.tests.utils import one_robot_placement from utama_core.strategy.common.abstract_strategy import ( AbstractStrategy, SpaceRequirements, @@ -22,100 +17,81 @@ SetBlackboardVariable, ) +_ARRIVE_TOL = 0.15 # metres +_MARGIN = 0.2 # metres inset from field bounds edges for waypoints + class RobotPlacementStep(AbstractBehaviour): """ - A behaviour that commands a robot to move between two specific positions on the field. - - **Args:** - invert (bool): Whether to invert the robot's movement direction. - **Blackboard Interaction:** - Reads: - - `rd_robot_id` (int): The ID of the robot to check for ball possession. Typically from the `SetBlackboardVariable` node. + Cycles a robot through a 3x3 grid of waypoints covering the field bounds, + facing the ball at each step. - **Returns:** - - `py_trees.common.Status.RUNNING`: The behaviour is actively commanding the robot to move. + Blackboard reads: + - robot_id_key (int): robot to control + - field_center_key (tuple): center of the active field bounds """ def __init__(self, rd_robot_id: str, field_center_key: str = "FieldCenter"): super().__init__() self.field_center_key = field_center_key self.robot_id_key = rd_robot_id - self.initialized = False - self.center_x = 0.0 - self.center_y = 0.0 - self.tx = 0.0 - self.ty = 0.0 + self._waypoints: list[Vector2D] = [] + self._wp_idx = 0 def setup_(self): self.blackboard.register_key(key=self.robot_id_key, access=py_trees.common.Access.READ) self.blackboard.register_key(key=self.field_center_key, access=py_trees.common.Access.READ) + def _build_waypoints(self) -> list[Vector2D]: + """3x3 grid inset from the active field bounds, in a snake pattern.""" + bounds = self.blackboard.game.field.field_bounds + x_min = bounds.top_left[0] + _MARGIN + x_max = bounds.bottom_right[0] - _MARGIN + y_min = bounds.bottom_right[1] + _MARGIN + y_max = bounds.top_left[1] - _MARGIN + + xs = [x_min, (x_min + x_max) / 2, x_max] + ys = [y_min, (y_min + y_max) / 2, y_max] + + # Snake order: alternate y direction per column to minimise travel + points = [] + for i, x in enumerate(xs): + col_ys = ys if i % 2 == 0 else reversed(ys) + for y in col_ys: + points.append(Vector2D(x, y)) + return points + def update(self) -> py_trees.common.Status: - """Closure which advances the simulation by one step.""" + game = self.blackboard.game + rsim_env = self.blackboard.rsim_env + robot_id: int = self.blackboard.get(self.robot_id_key) - # Initialize targets if not ready - if not self.initialized: + if not self._waypoints: try: - center = self.blackboard.get(self.field_center_key) - if center: - self.center_x, self.center_y = center - self.tx = self.center_x - self.ty = self.center_y + 0.5 - self.initialized = True + self.blackboard.get(self.field_center_key) # wait until center is ready except KeyError: - # Center not yet available return py_trees.common.Status.FAILURE + self._waypoints = self._build_waypoints() - if not self.initialized: - return py_trees.common.Status.FAILURE + target = self._waypoints[self._wp_idx] + robot = game.friendly_robots[robot_id] + ball = game.ball - game = self.blackboard.game - rsim_env = self.blackboard.rsim_env - id: int = self.blackboard.get(self.robot_id_key) - - friendly_robots = game.friendly_robots - bx, by = game.ball.p.x, game.ball.p.y - rp = friendly_robots[id].p - cx, cy = rp.x, rp.y - error = math.dist((self.tx, self.ty), (cx, cy)) - - if game.friendly_robots and game.ball is not None: - friendly_robots = game.friendly_robots - bx, by = game.ball.p.x, game.ball.p.y - rp = friendly_robots[id].p - cx, cy, _ = rp.x, rp.y, friendly_robots[id].orientation - error = math.dist((self.tx, self.ty), (cx, cy)) - - # Ensure target x is always the center x - self.tx = self.center_x - - switch = error < 0.1 - if switch: - upper_target = self.center_y + 0.5 - lower_target = self.center_y - 0.5 - - if math.isclose(self.ty, lower_target, abs_tol=0.1): - self.ty = upper_target - else: - self.ty = lower_target - - # changed so the robot tracks the ball while moving - oren = np.atan2(by - cy, bx - cx) - cmd = move( - game, - self.blackboard.motion_controller, - id, - Vector2D(self.tx, self.ty), - oren, - ) - if rsim_env: - rsim_env.draw_point(self.tx, self.ty, color="red") - v = game.friendly_robots[id].v - p = game.friendly_robots[id].p - rsim_env.draw_point(p.x + v.x * TIMESTEP * 5, p.y + v.y * TIMESTEP * 5, color="green") - - self.blackboard.cmd_map[id] = cmd + bx, by = ball.p.x, ball.p.y + cx, cy = robot.p.x, robot.p.y + oren = np.atan2(by - cy, bx - cx) + + cmd = move(game, self.blackboard.motion_controller, robot_id, target, oren) + + if rsim_env: + rsim_env.draw_point(target.x, target.y, color="red") + v = robot.v + rsim_env.draw_point(cx + v.x * TIMESTEP * 5, cy + v.y * TIMESTEP * 5, color="green") + + if math.dist((cx, cy), (target.x, target.y)) < _ARRIVE_TOL: + self._wp_idx = (self._wp_idx + 1) % len(self._waypoints) + + self.blackboard.cmd_map[robot_id] = cmd return py_trees.common.Status.RUNNING diff --git a/utama_core/team_controller/src/controllers/real/real_robot_controller.py b/utama_core/team_controller/src/controllers/real/real_robot_controller.py index 278d7f0a..c93a7666 100644 --- a/utama_core/team_controller/src/controllers/real/real_robot_controller.py +++ b/utama_core/team_controller/src/controllers/real/real_robot_controller.py @@ -16,6 +16,13 @@ TIMEOUT, TIMESTEP, ) + +# Leaky-bucket dribbler thermal limiter. +# The bucket accumulates real seconds while dribbling and drains at the same +# rate while off. When full the dribbler is forced off until the bucket drains +# to DRIBBLER_RESUME_SECONDS (hysteresis), preventing rapid on/off oscillation. +DRIBBLER_MAX_ON_SECONDS: float = 30.0 +DRIBBLER_RESUME_SECONDS: float = 15.0 # must drain to 50% before re-enabling from utama_core.entities.data.command import RobotCommand, RobotResponse from utama_core.skills.src.utils.move_utils import empty_command from utama_core.team_controller.src.controllers.common.robot_controller_abstract import ( @@ -24,7 +31,6 @@ logger = logging.getLogger(__name__) -# NB: A major assumption is that the robot IDs are 0-5 for the friendly team. MAX_VEL = REAL_PARAMS.MAX_VEL MAX_ANGULAR_VEL = REAL_PARAMS.MAX_ANGULAR_VEL @@ -44,19 +50,42 @@ class RealRobotController(AbstractRobotController): n_robots (int): The number of robots in the team. Directly affects output buffer size. Default is 6. """ - def __init__(self, is_team_yellow: bool, n_friendly: int): + def __init__( + self, + is_team_yellow: bool, + n_friendly: int, + vision_to_cmd_mapping: Optional[Dict[int, int]] = None, + serial_port: Optional[Serial] = None, + ): super().__init__(is_team_yellow, n_friendly) - self._serial_port = self._init_serial() + self._serial_port = self._init_serial() if serial_port is None else serial_port + self._sharing_friendly_transmitter = serial_port is not None self._rbt_cmd_size = 10 # packet size for one robot self._out_packet = self._empty_command() self._in_packet_size = 1 # size of the feedback packet received from the robots self._robots_info: List[RobotResponse] = [None] * self._n_friendly - logger.debug(f"Serial port: {PORT} opened with baudrate: {BAUD_RATE} and timeout {TIMEOUT}") + if serial_port is None: + logger.debug(f"Serial port: {PORT} opened with baudrate: {BAUD_RATE} and timeout {TIMEOUT}") + else: + logger.debug(f"Sharing serial port: {self._serial_port.port}") self._assigned_mapping = {} # mapping of robot_id to index in the out_packet + self._vision_to_cmd_mapping = vision_to_cmd_mapping if vision_to_cmd_mapping is not None else {} + cmd_to_vision_mapping = {v: k for k, v in self._vision_to_cmd_mapping.items()} + if len(cmd_to_vision_mapping) != len(self._vision_to_cmd_mapping): + raise ValueError("vision_to_cmd_mapping must be one-to-one: duplicate command IDs are not allowed.") + self._cmd_to_vision_mapping = cmd_to_vision_mapping # track last kick time for each robot to transmit kick as HIGH for n timesteps after command self._kicker_tracker: Dict[int, KickTrackerEntry] = {} + # leaky-bucket dribbler thermal limiter: accumulated wall-clock seconds per + # robot (cmd ID). Fills while dribbling, drains while off, clamped to + # [0, DRIBBLER_MAX_ON_SECONDS]. Dribbler is forced off when bucket is full. + self._dribbler_seconds: Dict[int, float] = {} + self._dribbler_last_tick: Dict[int, float] = {} # time.monotonic() of last call + self._dribbler_limit_warned: set[int] = set() # robots that have already been warned this event + self._dribbler_throttled: set[int] = set() # robots currently in post-limit cooldown (hysteresis) + def get_robots_responses(self) -> List[RobotResponse]: HEADER = 0xAA FOOTER = 0x55 @@ -72,6 +101,7 @@ def get_robots_responses(self) -> List[RobotResponse]: self._buffer.extend(self._serial_port.read(bytes_available)) responses = [] + responded_ids = set() while True: # 2. Look for header @@ -112,7 +142,14 @@ def get_robots_responses(self) -> List[RobotResponse]: # Guard against IndexError just in case, though validated by 'length' check if len(data) >= 1: - responses.append(RobotResponse(robot_id, has_ball=(data[0] & 0x01) != 0)) + # cmd IDs are returned as-is; StrategyRunner remaps them to vision IDs via _split_robot_responses_by_team + if robot_id in responded_ids: + warnings.warn( + f"Received multiple responses for robot ID {robot_id} in the same cycle. Ignoring subsequent responses." + ) + else: + responses.append(RobotResponse(robot_id, has_ball=(data[0] & 0x01) != 0)) + responded_ids.add(robot_id) # 8. Clear parsed packet from buffer del self._buffer[:packet_len] @@ -163,6 +200,12 @@ def _add_robot_command(self, command: RobotCommand, robot_id: int) -> None: robot_id (int): The ID of the robot. command (RobotCommand): A named tuple containing the robot command with keys: 'local_forward_vel', 'local_left_vel', 'angular_vel', 'kick', 'chip', 'dribble'. """ + if robot_id in self._vision_to_cmd_mapping: + robot_id = self._vision_to_cmd_mapping[robot_id] + elif self._sharing_friendly_transmitter: + warnings.warn( + f"Robot ID {robot_id} has no entry in vision_to_cmd_mapping on a shared transmitter setup; command will be sent using the raw vision ID." + ) if robot_id in self._assigned_mapping: warnings.warn( f"Robot ID {robot_id} has already been assigned a command in this cycle. Overwriting previous command." @@ -211,10 +254,54 @@ def _add_robot_command(self, command: RobotCommand, robot_id: int) -> None: # self._robots_info[i] = info # data_in = data_in << 1 # shift to the next robot's data + def _update_dribbler_bucket(self, robot_id: int, requested: bool) -> bool: + """Leaky-bucket thermal limiter for the dribbler. + + Returns True if the dribbler should actually be turned on this call. + The bucket accumulates real elapsed seconds while dribbling and drains + at the same rate while off, so intermittent use is accounted for + proportionally regardless of control-loop frequency. + """ + now = time.monotonic() + dt = now - self._dribbler_last_tick.get(robot_id, now) + self._dribbler_last_tick[robot_id] = now + + current = self._dribbler_seconds.get(robot_id, 0.0) + at_limit = current >= DRIBBLER_MAX_ON_SECONDS + + if at_limit: + self._dribbler_throttled.add(robot_id) + + throttled = robot_id in self._dribbler_throttled + + if requested and not throttled: + self._dribbler_seconds[robot_id] = min(current + dt, DRIBBLER_MAX_ON_SECONDS) + self._dribbler_limit_warned.discard(robot_id) + return True + elif requested and throttled: + if robot_id not in self._dribbler_limit_warned: + warnings.warn( + f"Robot {robot_id}: dribbler thermal limit reached " + f"({DRIBBLER_MAX_ON_SECONDS:.0f}s). " + f"Forcing dribbler off until bucket drains to {DRIBBLER_RESUME_SECONDS:.0f}s." + ) + self._dribbler_limit_warned.add(robot_id) + # still drain while forced off + self._dribbler_seconds[robot_id] = max(0.0, current - dt) + if self._dribbler_seconds[robot_id] <= DRIBBLER_RESUME_SECONDS: + self._dribbler_throttled.discard(robot_id) + self._dribbler_limit_warned.discard(robot_id) + return False + else: + # Not requested: drain the bucket + self._dribbler_seconds[robot_id] = max(0.0, current - dt) + if self._dribbler_seconds[robot_id] <= DRIBBLER_RESUME_SECONDS: + self._dribbler_throttled.discard(robot_id) + self._dribbler_limit_warned.discard(robot_id) + return False + def _generate_command_buffer(self, robot_id: int, c_command: RobotCommand) -> bytes: """Generates the command buffer to be sent to the robot.""" - assert robot_id < 6, "Invalid robot_id. Must be between 0 and 5." - # endianness: little endian packet = bytearray( [ @@ -228,8 +315,10 @@ def _generate_command_buffer(self, robot_id: int, c_command: RobotCommand) -> by ] ) + dribble_allowed = self._update_dribbler_bucket(robot_id, c_command.dribble) + dribbler_speed = 0 - if c_command.dribble: + if dribble_allowed: dribbler_speed = 0xC000 # set bits 15:14 to 11 dribbler_speed |= 4095 & 0x3FFF # set bits 13:0 to 4095 @@ -372,6 +461,18 @@ def n_friendly(self) -> int: def in_packet_size(self) -> int: return self._in_packet_size + @property + def vision_to_cmd_mapping(self) -> Dict[int, int]: + return self._vision_to_cmd_mapping + + @property + def cmd_to_vision_mapping(self) -> Dict[int, int]: + return self._cmd_to_vision_mapping + + @property + def sharing_friendly_transmitter(self) -> bool: + return self._sharing_friendly_transmitter + if __name__ == "__main__": robot_controller = RealRobotController(is_team_yellow=True, n_friendly=1) diff --git a/utama_core/team_controller/src/debug_utils/telop_gui.py b/utama_core/team_controller/src/debug_utils/telop_gui.py index 79ce2ab3..5da99738 100644 --- a/utama_core/team_controller/src/debug_utils/telop_gui.py +++ b/utama_core/team_controller/src/debug_utils/telop_gui.py @@ -53,6 +53,7 @@ class RealRobotController: def __init__(self, is_team_yellow=True, n_friendly=2): self.n_friendly = n_friendly + self._dribbler_seconds: dict = {} print("\n[DUMMY CONTROLLER ENABLED]") print("No serial/network/hardware required.\n") @@ -89,6 +90,7 @@ def send_robot_commands(self): KICK_C = "#d94a4a" ON_C = "#4ad97a" BALL_C = "#d9a84a" +HEAT_C = "#d94a4a" # Red when dribbler near/at thermal limit DUMMY_C = "#d9a84a" # Orange for dummy warning @@ -287,6 +289,8 @@ def _build_ui(self): self._fb_ball_vars: dict[int, tk.StringVar] = {} self._fb_ball_lbls: dict[int, tk.Label] = {} + self._fb_heat_vars: dict[int, tk.StringVar] = {} + self._fb_heat_lbls: dict[int, tk.Label] = {} self._fb_status_vars: dict[int, tk.StringVar] = {} for i in range(N_FRIENDLY): row = tk.Frame(fb_frame, bg=SURFACE) @@ -315,6 +319,18 @@ def _build_ui(self): ) ball_lbl.pack(side="left", padx=(8, 0)) + heat_var = tk.StringVar(value="heat: 0%") + heat_lbl = tk.Label( + row, + textvariable=heat_var, + bg=SURFACE, + fg=MUTED, + font=("monospace", 11), + width=10, + anchor="w", + ) + heat_lbl.pack(side="left", padx=(8, 0)) + status_var = tk.StringVar(value="no data") tk.Label( row, @@ -327,6 +343,8 @@ def _build_ui(self): self._fb_ball_vars[i] = ball_var self._fb_ball_lbls[i] = ball_lbl + self._fb_heat_vars[i] = heat_var + self._fb_heat_lbls[i] = heat_lbl self._fb_status_vars[i] = status_var # --- Command readout --- @@ -485,6 +503,10 @@ def _control_loop(self): time.sleep(max(0.0, dt - elapsed)) def _update_feedback(self, responses): + from utama_core.team_controller.src.controllers.real.real_robot_controller import ( + DRIBBLER_MAX_ON_SECONDS, + ) + for resp in responses: if resp.id not in self._fb_ball_vars: continue @@ -496,6 +518,13 @@ def _update_feedback(self, responses): self._fb_ball_lbls[resp.id].configure(fg=MUTED) self._fb_status_vars[resp.id].set("connected") + # dribbler heat display + bucket = self.controller._dribbler_seconds.get(resp.id, 0.0) + pct = int(bucket / DRIBBLER_MAX_ON_SECONDS * 100) + self._fb_heat_vars[resp.id].set(f"heat: {pct:3d}%") + heat_colour = HEAT_C if pct >= 80 else (BALL_C if pct >= 50 else MUTED) + self._fb_heat_lbls[resp.id].configure(fg=heat_colour) + def _update_readout(self, cmd: RobotCommand): self._metrics["fwd"].set(f"{cmd.local_forward_vel:+.2f}") self._metrics["left"].set(f"{cmd.local_left_vel:+.2f}") diff --git a/utama_core/tests/controller/test_dribbler_limiter.py b/utama_core/tests/controller/test_dribbler_limiter.py new file mode 100644 index 00000000..2ed97bcd --- /dev/null +++ b/utama_core/tests/controller/test_dribbler_limiter.py @@ -0,0 +1,177 @@ +"""Tests for the leaky-bucket dribbler thermal limiter in RealRobotController.""" + +from unittest.mock import MagicMock, patch + +import pytest + +from utama_core.team_controller.src.controllers.real.real_robot_controller import ( + DRIBBLER_MAX_ON_SECONDS, + DRIBBLER_RESUME_SECONDS, + RealRobotController, +) + +_MONOTONIC = "utama_core.team_controller.src.controllers.real.real_robot_controller.time.monotonic" + + +@pytest.fixture +def controller(): + """RealRobotController with a mock serial port (no real hardware needed).""" + mock_serial = MagicMock() + mock_serial.in_waiting = 0 + return RealRobotController(is_team_yellow=True, n_friendly=3, serial_port=mock_serial) + + +def _tick(controller, robot_id: int, requested: bool, dt: float) -> bool: + """Call _update_dribbler_bucket with a controlled time delta. + + Seeds _dribbler_last_tick so that time.monotonic() - last_tick == dt exactly. + """ + now = controller._dribbler_last_tick.get(robot_id, 0.0) + dt + controller._dribbler_last_tick[robot_id] = now - dt + with patch(_MONOTONIC, return_value=now): + return controller._update_dribbler_bucket(robot_id, requested) + + +# --------------------------------------------------------------------------- +# Basic bucket mechanics +# --------------------------------------------------------------------------- + + +def test_dribbler_allowed_when_bucket_empty(controller): + assert _tick(controller, 0, requested=True, dt=1.0) is True + + +def test_bucket_fills_by_elapsed_seconds(controller): + _tick(controller, 0, requested=True, dt=5.0) + assert controller._dribbler_seconds[0] == pytest.approx(5.0) + + +def test_bucket_drains_by_elapsed_seconds(controller): + controller._dribbler_seconds[0] = 20.0 + _tick(controller, 0, requested=False, dt=7.0) + assert controller._dribbler_seconds[0] == pytest.approx(13.0) + + +def test_bucket_floors_at_zero(controller): + controller._dribbler_seconds[0] = 2.0 + _tick(controller, 0, requested=False, dt=10.0) + assert controller._dribbler_seconds[0] == pytest.approx(0.0) + + +def test_dribbler_off_returns_false(controller): + assert _tick(controller, 0, requested=False, dt=1.0) is False + + +def test_bucket_does_not_overfill(controller): + _tick(controller, 0, requested=True, dt=DRIBBLER_MAX_ON_SECONDS * 2) + assert controller._dribbler_seconds[0] == pytest.approx(DRIBBLER_MAX_ON_SECONDS) + + +# --------------------------------------------------------------------------- +# Limit enforcement +# --------------------------------------------------------------------------- + + +def test_dribbler_forced_off_at_limit(controller): + controller._dribbler_seconds[0] = DRIBBLER_MAX_ON_SECONDS + assert _tick(controller, 0, requested=True, dt=1.0) is False + + +def test_dribbler_forced_off_emits_warning_once(controller): + controller._dribbler_seconds[0] = DRIBBLER_MAX_ON_SECONDS + # first tick at limit: warning + with pytest.warns(UserWarning, match="thermal limit"): + _tick(controller, 0, requested=True, dt=1.0) + # subsequent ticks: no more warnings + import warnings as _warnings + + with _warnings.catch_warnings(): + _warnings.simplefilter("error") + _tick(controller, 0, requested=True, dt=1.0) # must not raise + + +# --------------------------------------------------------------------------- +# Hysteresis: no 1s-on / 1s-off oscillation at the limit +# --------------------------------------------------------------------------- + + +def test_throttled_robot_stays_off_until_resume_threshold(controller): + # fill to limit + controller._dribbler_seconds[0] = DRIBBLER_MAX_ON_SECONDS + _tick(controller, 0, requested=True, dt=0.0) # trigger throttle flag + + # drain to just above resume threshold — still blocked + controller._dribbler_seconds[0] = DRIBBLER_RESUME_SECONDS + 1.0 + assert _tick(controller, 0, requested=True, dt=0.0) is False + + +def test_throttled_robot_resumes_at_resume_threshold(controller): + controller._dribbler_seconds[0] = DRIBBLER_MAX_ON_SECONDS + _tick(controller, 0, requested=True, dt=0.0) # trigger throttle + + # drain to exactly the resume threshold + _tick(controller, 0, requested=False, dt=DRIBBLER_MAX_ON_SECONDS - DRIBBLER_RESUME_SECONDS) + assert controller._dribbler_seconds[0] == pytest.approx(DRIBBLER_RESUME_SECONDS) + # now requesting dribble should be allowed + assert _tick(controller, 0, requested=True, dt=1.0) is True + + +def test_bucket_drains_while_throttled(controller): + # robot should still drain even while strategy keeps requesting dribble + controller._dribbler_seconds[0] = DRIBBLER_MAX_ON_SECONDS + _tick(controller, 0, requested=True, dt=0.0) # throttle + _tick(controller, 0, requested=True, dt=5.0) + assert controller._dribbler_seconds[0] == pytest.approx(DRIBBLER_MAX_ON_SECONDS - 5.0) + + +def test_no_oscillation_at_limit(controller): + # fill to limit, then alternate on/off — robot must stay off until resume threshold + controller._dribbler_seconds[0] = DRIBBLER_MAX_ON_SECONDS + results = [] + for i in range(10): + requested = i % 2 == 0 # alternate on/off + results.append(_tick(controller, 0, requested=requested, dt=1.0)) + # all must be False until bucket drains below DRIBBLER_RESUME_SECONDS + assert all(r is False for r in results) + + +# --------------------------------------------------------------------------- +# Recovery after full drain +# --------------------------------------------------------------------------- + + +def test_dribbler_fully_recovers_after_drain(controller): + controller._dribbler_seconds[0] = DRIBBLER_MAX_ON_SECONDS + _tick(controller, 0, requested=True, dt=0.0) # throttle + _tick(controller, 0, requested=False, dt=DRIBBLER_MAX_ON_SECONDS) + assert controller._dribbler_seconds[0] == pytest.approx(0.0) + assert _tick(controller, 0, requested=True, dt=1.0) is True + + +def test_limit_hit_at_correct_wall_time(controller): + # 29s → allowed, fills to 29s + assert _tick(controller, 0, requested=True, dt=29.0) is True + assert controller._dribbler_seconds[0] == pytest.approx(29.0) + # 1s more fills to 30s (cap) → still allowed (not yet at limit at entry) + assert _tick(controller, 0, requested=True, dt=1.0) is True + assert controller._dribbler_seconds[0] == pytest.approx(DRIBBLER_MAX_ON_SECONDS) + # next tick: at limit → forced off + assert _tick(controller, 0, requested=True, dt=1.0) is False + + +# --------------------------------------------------------------------------- +# Per-robot isolation +# --------------------------------------------------------------------------- + + +def test_buckets_are_independent_per_robot(controller): + controller._dribbler_seconds[0] = DRIBBLER_MAX_ON_SECONDS + assert _tick(controller, 0, requested=True, dt=0.0) is False # throttle robot 0 + assert _tick(controller, 1, requested=True, dt=1.0) is True + + +def test_draining_one_robot_does_not_affect_another(controller): + controller._dribbler_seconds[0] = 20.0 + controller._dribbler_seconds[1] = 10.0 + _tick(controller, 0, requested=False, dt=5.0) + assert controller._dribbler_seconds[1] == pytest.approx(10.0) diff --git a/utama_core/tests/refiners/robot_info_test.py b/utama_core/tests/refiners/robot_info_test.py new file mode 100644 index 00000000..b9e2ef88 --- /dev/null +++ b/utama_core/tests/refiners/robot_info_test.py @@ -0,0 +1,157 @@ +import pytest + +from utama_core.data_processing.refiners.robot_info import ( + _BALL_CAPTURE_DIST, + RobotInfoRefiner, +) +from utama_core.entities.data.command import RobotResponse +from utama_core.entities.data.vector import Vector2D, Vector3D +from utama_core.entities.game.ball import Ball +from utama_core.entities.game.game_frame import GameFrame +from utama_core.entities.game.robot import Robot + + +def _make_robot(robot_id: int, x: float, y: float, has_ball: bool = False) -> Robot: + return Robot( + id=robot_id, + is_friendly=True, + has_ball=has_ball, + p=Vector2D(x, y), + v=None, + a=None, + orientation=0, + ) + + +def _make_frame(robots: dict, ball_pos=None) -> GameFrame: + ball = Ball(Vector3D(*ball_pos, 0), Vector3D(0, 0, 0), None) if ball_pos else None + return GameFrame( + ts=0.0, + my_team_is_yellow=True, + my_team_is_right=True, + friendly_robots=robots, + enemy_robots={}, + ball=ball, + ) + + +# --------------------------------------------------------------------------- +# Default behaviour (trusted_ir_robots=None — trust all) +# --------------------------------------------------------------------------- + + +def test_default_uses_ir_sensor_true(): + refiner = RobotInfoRefiner() + frame = _make_frame({0: _make_robot(0, 0.0, 0.0, has_ball=False)}, ball_pos=(1.0, 0.0)) + result = refiner.refine(frame, [RobotResponse(id=0, has_ball=True)]) + assert result.friendly_robots[0].has_ball is True + + +def test_default_uses_ir_sensor_false(): + refiner = RobotInfoRefiner() + # Robot is right on top of the ball but IR says False — should respect IR. + frame = _make_frame({0: _make_robot(0, 0.0, 0.0, has_ball=True)}, ball_pos=(0.0, 0.0)) + result = refiner.refine(frame, [RobotResponse(id=0, has_ball=False)]) + assert result.friendly_robots[0].has_ball is False + + +def test_empty_responses_returns_frame_unchanged(): + refiner = RobotInfoRefiner() + frame = _make_frame({0: _make_robot(0, 0.0, 0.0, has_ball=True)}) + assert refiner.refine(frame, []) is frame + assert refiner.refine(frame, None) is frame + + +def test_untrusted_robot_inferred_even_when_response_dropped(): + # If a broken robot sends no packet at all, has_ball must still be inferred + # from vision proximity rather than left frozen at the previous value. + refiner = RobotInfoRefiner(trusted_ir_robots=frozenset()) # no robots trusted + # Robot is close to ball; previous has_ball=True (stale) — empty responses this frame + frame = _make_frame({0: _make_robot(0, 0.0, 0.0, has_ball=True)}, ball_pos=(2.0, 0.0)) + result = refiner.refine(frame, []) # no response arrived + # Robot is 2 m away — should be inferred as False, not left as stale True + assert result.friendly_robots[0].has_ball is False + + +def test_unknown_robot_id_warns(recwarn): + refiner = RobotInfoRefiner() + frame = _make_frame({0: _make_robot(0, 0.0, 0.0)}) + refiner.refine(frame, [RobotResponse(id=99, has_ball=True)]) + assert any("99" in str(w.message) for w in recwarn.list) + + +# --------------------------------------------------------------------------- +# Trusted-IR allowlist: trusted robot uses IR, untrusted uses vision proximity +# --------------------------------------------------------------------------- + + +def test_trusted_robot_uses_ir_even_when_far_from_ball(): + refiner = RobotInfoRefiner(trusted_ir_robots=frozenset({0})) + # Robot is 2 m from ball — no proximity, but IR says True. + frame = _make_frame({0: _make_robot(0, 0.0, 0.0)}, ball_pos=(2.0, 0.0)) + result = refiner.refine(frame, [RobotResponse(id=0, has_ball=True)]) + assert result.friendly_robots[0].has_ball is True + + +def test_trusted_robot_respects_ir_false(): + refiner = RobotInfoRefiner(trusted_ir_robots=frozenset({0})) + # Robot is touching the ball but IR says False — should respect IR. + frame = _make_frame({0: _make_robot(0, 0.0, 0.0)}, ball_pos=(0.0, 0.0)) + result = refiner.refine(frame, [RobotResponse(id=0, has_ball=False)]) + assert result.friendly_robots[0].has_ball is False + + +def test_untrusted_robot_infers_true_when_close(): + refiner = RobotInfoRefiner(trusted_ir_robots=frozenset()) # no robots trusted + # Place robot within capture distance; IR reports False (broken sensor). + frame = _make_frame({0: _make_robot(0, 0.0, 0.0)}, ball_pos=(0.05, 0.0)) + result = refiner.refine(frame, [RobotResponse(id=0, has_ball=False)]) + assert result.friendly_robots[0].has_ball is True + + +def test_untrusted_robot_infers_false_when_far(): + refiner = RobotInfoRefiner(trusted_ir_robots=frozenset()) + # Robot is far from ball; IR reports True (broken sensor firing randomly). + frame = _make_frame({0: _make_robot(0, 0.0, 0.0)}, ball_pos=(1.0, 0.0)) + result = refiner.refine(frame, [RobotResponse(id=0, has_ball=True)]) + assert result.friendly_robots[0].has_ball is False + + +def test_untrusted_robot_at_exact_capture_boundary(): + refiner = RobotInfoRefiner(trusted_ir_robots=frozenset()) + just_inside = _BALL_CAPTURE_DIST - 0.001 + just_outside = _BALL_CAPTURE_DIST + 0.001 + + frame_in = _make_frame({0: _make_robot(0, 0.0, 0.0)}, ball_pos=(just_inside, 0.0)) + assert refiner.refine(frame_in, [RobotResponse(id=0, has_ball=False)]).friendly_robots[0].has_ball is True + + frame_out = _make_frame({0: _make_robot(0, 0.0, 0.0)}, ball_pos=(just_outside, 0.0)) + assert refiner.refine(frame_out, [RobotResponse(id=0, has_ball=False)]).friendly_robots[0].has_ball is False + + +def test_untrusted_robot_no_ball_in_frame_returns_false(): + refiner = RobotInfoRefiner(trusted_ir_robots=frozenset()) + frame = _make_frame({0: _make_robot(0, 0.0, 0.0)}, ball_pos=None) + result = refiner.refine(frame, [RobotResponse(id=0, has_ball=True)]) + assert result.friendly_robots[0].has_ball is False + + +# --------------------------------------------------------------------------- +# Mixed team: one robot trusted, one not +# --------------------------------------------------------------------------- + + +def test_mixed_team_trusted_uses_ir_untrusted_uses_vision(): + refiner = RobotInfoRefiner(trusted_ir_robots=frozenset({0})) + robots = { + 0: _make_robot(0, 0.0, 0.0), # trusted — far from ball + 1: _make_robot(1, 0.05, 0.0), # untrusted — close to ball + } + frame = _make_frame(robots, ball_pos=(0.0, 0.0)) + responses = [ + RobotResponse(id=0, has_ball=True), # IR says yes → trust it even though far + RobotResponse(id=1, has_ball=False), # IR says no → ignore, use proximity + ] + result = refiner.refine(frame, responses) + assert result.friendly_robots[0].has_ball is True # from IR + assert result.friendly_robots[1].has_ball is True # from vision (close enough) diff --git a/utama_core/tests/strategy_examples/test_placement_coords.py b/utama_core/tests/strategy_examples/test_placement_coords.py index f474b9d1..a3f9bf44 100644 --- a/utama_core/tests/strategy_examples/test_placement_coords.py +++ b/utama_core/tests/strategy_examples/test_placement_coords.py @@ -1,9 +1,9 @@ """Integration tests for RobotPlacementStrategy using AbstractTestManager. These tests verify that: -1. The robot moves to positions based on the provided field_bounds center -2. The robot oscillates between the expected start and end points -3. Custom field_bounds correctly shift the placement region +1. The robot moves to positions within the provided field_bounds +2. The robot visits waypoints from the 3x3 grid generated for those bounds +3. Custom field_bounds correctly constrain the placement region """ import math @@ -11,10 +11,15 @@ import pytest +from utama_core.config.field_params import STANDARD_FIELD_DIMS from utama_core.entities.game import Game from utama_core.entities.game.field import FieldBounds from utama_core.run import StrategyRunner from utama_core.strategy.examples.one_robot_placement_strategy import ( + _ARRIVE_TOL as _STRATEGY_ARRIVE_TOL, +) +from utama_core.strategy.examples.one_robot_placement_strategy import ( + _MARGIN, RobotPlacementStrategy, ) from utama_core.team_controller.src.controllers import AbstractSimController @@ -23,48 +28,76 @@ TestingStatus, ) +# Use the same tolerance as the strategy so the test only marks a waypoint reached +# when the strategy itself would have advanced to the next one. +_ARRIVE_TOL = _STRATEGY_ARRIVE_TOL + + +def _grid_waypoints(bounds: FieldBounds) -> list[tuple[float, float]]: + """Mirror the strategy's _build_waypoints logic for expected-value computation.""" + x_min = bounds.top_left[0] + _MARGIN + x_max = bounds.bottom_right[0] - _MARGIN + y_min = bounds.bottom_right[1] + _MARGIN + y_max = bounds.top_left[1] - _MARGIN + xs = [x_min, (x_min + x_max) / 2, x_max] + ys = [y_min, (y_min + y_max) / 2, y_max] + points = [] + for i, x in enumerate(xs): + col_ys = ys if i % 2 == 0 else list(reversed(ys)) + for y in col_ys: + points.append((x, y)) + return points + class RobotPlacementTestManager(AbstractTestManager): - """Test manager that verifies robot oscillates around the expected center.""" + """Verify the robot visits at least two distinct grid waypoints.""" n_episodes = 1 - def __init__(self, expected_center: tuple[float, float], tolerance: float = 0.15): + def __init__(self, waypoints: list[tuple[float, float]], tolerance: float = _ARRIVE_TOL): super().__init__() - self.expected_center = expected_center - self.expected_upper = (expected_center[0], expected_center[1] + 0.5) - self.expected_lower = (expected_center[0], expected_center[1] - 0.5) - self.reached_upper = False - self.reached_lower = False + self.waypoints = waypoints self.tolerance = tolerance + # Track which waypoints the robot has visited + self.visited: set[int] = set() + + @property + def reached_a(self) -> bool: + return 0 in self.visited + + @property + def reached_b(self) -> bool: + return len(self.visited) >= 2 + + @property + def target_a(self) -> tuple[float, float]: + return self.waypoints[0] + + @property + def target_b(self) -> tuple[float, float]: + return self.waypoints[1] if len(self.waypoints) > 1 else self.waypoints[0] def reset_field(self, sim_controller: AbstractSimController, game: Game): - """Reset robot and ball positions for the test.""" 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_ball(centre[0] + 0.5, centre[1] + 0.5) def eval_status(self, game: Game) -> TestingStatus: - """Verify robot reaches both oscillation targets.""" robot = game.friendly_robots.get(self.my_strategy.robot_id) if not robot: return TestingStatus.IN_PROGRESS - robot_pos = (robot.p.x, robot.p.y) + pos = (robot.p.x, robot.p.y) + for i, wp in enumerate(self.waypoints): + if math.dist(pos, wp) < self.tolerance: + self.visited.add(i) - if math.dist(robot_pos, self.expected_upper) < self.tolerance: - self.reached_upper = True - if math.dist(robot_pos, self.expected_lower) < self.tolerance: - self.reached_lower = True - - if self.reached_upper and self.reached_lower: + if len(self.visited) >= 2: return TestingStatus.SUCCESS return TestingStatus.IN_PROGRESS -def _run_placement_test(field_bounds: Optional[FieldBounds], expected_center: tuple[float, float]): - """Helper to run a placement strategy test with given bounds.""" +def _run_placement_test(field_bounds: Optional[FieldBounds]): strategy = RobotPlacementStrategy(robot_id=0) runner = StrategyRunner( @@ -77,8 +110,12 @@ def _run_placement_test(field_bounds: Optional[FieldBounds], expected_center: tu exp_enemy=0, ) - test_manager = RobotPlacementTestManager(expected_center=expected_center) - passed = runner.run_test(test_manager=test_manager, episode_timeout=15, rsim_headless=True) + # Compute expected waypoints from the actual bounds that will be used + effective_bounds = field_bounds if field_bounds is not None else STANDARD_FIELD_DIMS.full_field_bounds + waypoints = _grid_waypoints(effective_bounds) + + test_manager = RobotPlacementTestManager(waypoints=waypoints) + passed = runner.run_test(test_manager=test_manager, episode_timeout=30, rsim_headless=True) return passed, test_manager @@ -87,17 +124,14 @@ class TestFieldBoundsCenter: """Tests for FieldBounds center calculation.""" def test_full_field_center_is_origin(self): - """Full field bounds should have center at (0, 0).""" bounds = FieldBounds(top_left=(-4.5, 3.0), bottom_right=(4.5, -3.0)) assert bounds.center == (0.0, 0.0) def test_custom_bounds_center(self): - """Custom bounds should correctly calculate center.""" bounds = FieldBounds(top_left=(1.0, 2.0), bottom_right=(3.0, 0.0)) assert bounds.center == (2.0, 1.0) def test_custom_differs_from_default(self): - """Custom and default bounds must have different centers.""" default = FieldBounds(top_left=(-4.5, 3.0), bottom_right=(4.5, -3.0)) custom = FieldBounds(top_left=(1.0, 2.0), bottom_right=(3.0, 0.0)) assert default.center != custom.center @@ -107,24 +141,18 @@ class TestRobotPlacementStrategy: """Integration tests for RobotPlacementStrategy behavior.""" def test_oscillation_with_custom_bounds(self): - """Robot oscillates around custom bounds center (2.0, 1.0).""" + """Robot visits the first two grid waypoints within custom bounds (1,2)→(3,0).""" bounds = FieldBounds(top_left=(1.0, 2.0), bottom_right=(3.0, 0.0)) - expected_center = bounds.center - - passed, manager = _run_placement_test(bounds, expected_center) - - assert manager.reached_upper, f"Never reached upper target {manager.expected_upper}" - assert manager.reached_lower, f"Never reached lower target {manager.expected_lower}" + passed, manager = _run_placement_test(bounds) + assert manager.reached_a, f"Never reached first waypoint {manager.target_a}" + assert manager.reached_b, f"Never reached second waypoint {manager.target_b}" assert passed def test_oscillation_with_default_bounds(self): - """Robot oscillates around default field center (0, 0).""" - expected_center = (0.0, 0.0) - - passed, manager = _run_placement_test(field_bounds=None, expected_center=expected_center) - - assert manager.reached_upper, f"Never reached upper target {manager.expected_upper}" - assert manager.reached_lower, f"Never reached lower target {manager.expected_lower}" + """Robot visits the first two grid waypoints on the full standard field.""" + passed, manager = _run_placement_test(field_bounds=None) + assert manager.reached_a, f"Never reached first waypoint {manager.target_a}" + assert manager.reached_b, f"Never reached second waypoint {manager.target_b}" assert passed diff --git a/utama_core/tests/strategy_runner/test_runner_misconfig.py b/utama_core/tests/strategy_runner/test_runner_misconfig.py index 1a04f9e4..540f81ad 100644 --- a/utama_core/tests/strategy_runner/test_runner_misconfig.py +++ b/utama_core/tests/strategy_runner/test_runner_misconfig.py @@ -132,7 +132,9 @@ def __init__(self, buffers, on_geometry=None): assert len(started[0]) == 1 -def test_setup_vision_and_referee_starts_both_receivers_when_referee_official(monkeypatch): +def test_setup_vision_and_referee_starts_both_receivers_when_referee_official( + monkeypatch, +): from utama_core.run import strategy_runner as runner_mod started = [] @@ -262,3 +264,235 @@ def test_strategy_runner_bounds_outside_non_standard_field_dims(): full_field_dims=GREAT_EXHIBITION_FIELD_DIMS, field_bounds=too_large_for_custom_dims, ) + + +def test_validate_vision_to_cmd_mapping_real_not_controlled(): + runner = SimpleNamespace( + mode=Mode.REAL, + my_team_is_yellow=True, + opp=None, + exp_friendly=3, + ) + result = StrategyRunner._validate_vision_to_cmd_mapping(runner, None, True) + assert result == {} + + +def test_validate_vision_to_cmd_mapping_pvp_opp_mapping_missing(): + runner = SimpleNamespace( + mode=Mode.REAL, + my_team_is_yellow=True, + opp=True, + exp_friendly=3, + exp_enemy=3, + ) + with pytest.raises(ValueError, match="required for both teams in real PVP"): + StrategyRunner._validate_vision_to_cmd_mapping(runner, None, False) + + +def test_validate_vision_to_cmd_mapping_pvp_friendly_mapping_missing(): + runner = SimpleNamespace( + mode=Mode.REAL, + my_team_is_yellow=True, + opp=True, + exp_friendly=3, + exp_enemy=3, + ) + with pytest.raises(ValueError, match="required for both teams in real PVP"): + StrategyRunner._validate_vision_to_cmd_mapping(runner, None, True) + + +def test_validate_vision_to_cmd_mapping_type_error(): + runner = SimpleNamespace( + mode=Mode.REAL, + my_team_is_yellow=True, + opp=None, + exp_friendly=3, + ) + with pytest.raises(TypeError, match="must be a dictionary"): + StrategyRunner._validate_vision_to_cmd_mapping(runner, [], True) + + +def test_validate_vision_to_cmd_mapping_ignored_warning(): + import warnings + + runner = SimpleNamespace( + mode=Mode.REAL, + my_team_is_yellow=True, + opp=None, + exp_friendly=3, + ) + with pytest.warns(UserWarning, match="vision_to_cmd_mapping is provided but will be ignored"): + StrategyRunner._validate_vision_to_cmd_mapping(runner, {0: 0}, False) + + +def test_validate_vision_to_cmd_mapping_incorrect_length_friendly(): + runner = SimpleNamespace( + mode=Mode.REAL, + my_team_is_yellow=True, + opp=True, + exp_friendly=3, + exp_enemy=3, + ) + # 2 entries when 3 are expected + with pytest.raises(ValueError, match="has 2 entries but 3 robots are expected"): + StrategyRunner._validate_vision_to_cmd_mapping(runner, {0: 0, 1: 1}, True) + + +def test_validate_vision_to_cmd_mapping_incorrect_length_enemy(): + runner = SimpleNamespace( + mode=Mode.REAL, + my_team_is_yellow=True, + opp=True, + exp_friendly=3, + exp_enemy=3, + ) + # 2 entries when 3 are expected + with pytest.raises(ValueError, match="has 2 entries but 3 robots are expected"): + StrategyRunner._validate_vision_to_cmd_mapping(runner, {0: 0, 1: 1}, False) + + +def test_validate_vision_to_cmd_mapping_correct_count_non_contiguous_ids_passes_init(): + # Non-contiguous vision IDs (e.g. real field robots numbered 5,6,7) with the right + # count must PASS at init time — coverage against observed IDs is validated later + # in _validate_mapping_covers_game_frame() after _load_game(). + runner = SimpleNamespace( + mode=Mode.REAL, + my_team_is_yellow=True, + opp=True, + exp_friendly=3, + exp_enemy=3, + ) + result = StrategyRunner._validate_vision_to_cmd_mapping(runner, {5: 0, 6: 1, 7: 2}, True) + assert result == {5: 0, 6: 1, 7: 2} + + +def test_validate_mapping_covers_game_frame_mismatch_raises(): + from utama_core.run.strategy_runner import StrategyRunner + + runner = SimpleNamespace() + with pytest.raises(ValueError, match="missing entries for observed IDs"): + StrategyRunner._validate_mapping_covers_game_frame(runner, {0: 0, 1: 1, 2: 2}, {5, 6, 7}, "friendly") + + +def test_validate_mapping_covers_game_frame_match_passes(): + from utama_core.run.strategy_runner import StrategyRunner + + runner = SimpleNamespace() + # Should not raise + StrategyRunner._validate_mapping_covers_game_frame(runner, {5: 0, 6: 1, 7: 2}, {5, 6, 7}, "friendly") + + +def test_validate_mapping_covers_game_frame_empty_mapping_passes(): + from utama_core.run.strategy_runner import StrategyRunner + + runner = SimpleNamespace() + # Empty mapping (non-PVP mode) always passes + StrategyRunner._validate_mapping_covers_game_frame(runner, {}, {0, 1, 2}, "friendly") + + +def test_validate_vision_to_cmd_mapping_invalid_ids(): + from utama_core.config.physical_constants import MAX_ROBOT_ID + + runner = SimpleNamespace( + mode=Mode.REAL, + my_team_is_yellow=True, + opp=None, + exp_friendly=3, + ) + with pytest.raises(TypeError, match="must map integers to integers"): + StrategyRunner._validate_vision_to_cmd_mapping(runner, {0: "0"}, True) + with pytest.raises(ValueError, match="cannot have negative IDs"): + StrategyRunner._validate_vision_to_cmd_mapping(runner, {-1: 0}, True) + with pytest.raises(ValueError, match="cannot have vision IDs greater than"): + StrategyRunner._validate_vision_to_cmd_mapping(runner, {MAX_ROBOT_ID + 1: 0}, True) + with pytest.raises(ValueError, match="cannot have command IDs greater than 255"): + StrategyRunner._validate_vision_to_cmd_mapping(runner, {0: 256}, True) + + +def test_validate_vision_to_cmd_mapping_sim_mode_raises(): + runner = SimpleNamespace( + mode=Mode.RSIM, + my_team_is_yellow=True, + opp=None, + ) + with pytest.raises(ValueError, match="should not be provided in simulation modes"): + StrategyRunner._validate_vision_to_cmd_mapping(runner, {0: 0}, True) + + +def test_trusted_ir_robots_yellow_team_is_my_team(): + """yellow_trusted_ir_robots routes to my refiner when my_team_is_yellow=True.""" + runner = StrategyRunner( + strategy=DummyStrategy(), + my_team_is_yellow=True, + my_team_is_right=True, + mode="rsim", + exp_friendly=3, + exp_enemy=3, + yellow_trusted_ir_robots=frozenset({0, 1}), + ) + assert runner.my.robot_info_refiner._trusted_ir_robots == frozenset({0, 1}) + assert runner.opp is None + + +def test_trusted_ir_robots_blue_team_is_my_team(): + """blue_trusted_ir_robots routes to my refiner when my_team_is_yellow=False.""" + runner = StrategyRunner( + strategy=DummyStrategy(), + my_team_is_yellow=False, + my_team_is_right=True, + mode="rsim", + exp_friendly=3, + exp_enemy=3, + blue_trusted_ir_robots=frozenset({2}), + ) + assert runner.my.robot_info_refiner._trusted_ir_robots == frozenset({2}) + + +def test_trusted_ir_robots_both_teams_pvp(): + """Both colour params route correctly in PVP mode.""" + runner = StrategyRunner( + strategy=DummyStrategy(), + opp_strategy=DummyStrategy(), + my_team_is_yellow=True, + my_team_is_right=True, + mode="rsim", + exp_friendly=3, + exp_enemy=3, + yellow_trusted_ir_robots=frozenset({0}), + blue_trusted_ir_robots=frozenset({1}), + ) + assert runner.my.robot_info_refiner._trusted_ir_robots == frozenset({0}) + assert runner.opp.robot_info_refiner._trusted_ir_robots == frozenset({1}) + + +def test_trusted_ir_robots_none_by_default(): + """Default None means trust all IR sensors (backwards-compatible).""" + runner = StrategyRunner( + strategy=DummyStrategy(), + my_team_is_yellow=True, + my_team_is_right=True, + mode="rsim", + exp_friendly=3, + exp_enemy=3, + ) + assert runner.my.robot_info_refiner._trusted_ir_robots is None + + +def test_check_no_cmd_duplicate_if_transmission_sharing(): + runner = SimpleNamespace() + + # Test valid non-overlapping mappings + yellow_mapping = {0: 1, 1: 2} + blue_mapping = {0: 3, 1: 4} + StrategyRunner._check_no_cmd_duplicate_if_transmission_sharing(runner, yellow_mapping, blue_mapping) + + # Test collision in same team + yellow_mapping_duplicate = {0: 1, 1: 1} + with pytest.raises(ValueError, match="cannot have overlapping command IDs"): + StrategyRunner._check_no_cmd_duplicate_if_transmission_sharing(runner, yellow_mapping_duplicate, blue_mapping) + + # Test collision across teams + yellow_mapping = {0: 1, 1: 2} + blue_mapping_overlap = {0: 2, 1: 4} + with pytest.raises(ValueError, match="cannot have overlapping command IDs"): + StrategyRunner._check_no_cmd_duplicate_if_transmission_sharing(runner, yellow_mapping, blue_mapping_overlap)