diff --git a/.gitignore b/.gitignore index c3be0a5a..ceaaf7af 100644 --- a/.gitignore +++ b/.gitignore @@ -194,5 +194,5 @@ cython_debug/ # option (not recommended) you can uncomment the following to ignore the entire idea folder. #.idea/ +AutoReferee/ ssl-game-controller/ -AutoReferee/ \ No newline at end of file diff --git a/README.md b/README.md index c4bf2999..ee9e502b 100644 --- a/README.md +++ b/README.md @@ -2,12 +2,29 @@ ## Setup Guidelines +### Setup Utama + 1. cd to root folder `/Utama` and type `pip install -e .` to install all dependencies. 2. Note that this also installs all modules with `__init__.py` (so you need to run it again when you add an `__init__.py`) If you are still struggling with local import errors (ie importing our local modules), you can add `export PYTHONPATH=/path/to/folder/Utama` to your `~/.bashrc`. To do this, run `sudo nano ~/.bashrc` and then add the export line at the bottom of the document. Finally, close and save. -###### Warning: the above workaround is quite hackish and may result in source conflicts if you are working on other projects in the same Linux machine. Be advised. +> Warning: the above workaround is quite hackish and may result in source conflicts if you are working on other projects in the same Linux machine. Be advised. + +### Setup Autoreferee + +1. Make sure `grSim` is setup properly and can be called through terminal. +2. `git clone` from [AutoReferee repo](https://github.com/TIGERs-Mannheim/AutoReferee) in a folder named `/AutoReferee` in root directory. +3. Change `DIV_A` in `/AutoReferee/config/moduli/moduli.xml` to `DIV_B`. + +```xml + + ROBOCUP + DIV_B + +``` + +4. Get the latest [compiled game controller](https://github.com/RoboCup-SSL/ssl-game-controller/releases/) and rename it to `ssl_game_controller`. Save it in `/ssl-game-controller` directory. ### Field Guide diff --git a/entities/data/referee.py b/entities/data/referee.py new file mode 100644 index 00000000..f9e09c70 --- /dev/null +++ b/entities/data/referee.py @@ -0,0 +1,55 @@ +from collections import namedtuple +from typing import NamedTuple, Optional, Tuple +from enum import Enum + +from entities.game.team_info import TeamInfo +from entities.referee.referee_command import RefereeCommand +from entities.referee.stage import Stage + + +class RefereeData(NamedTuple): + """Namedtuple for referee data.""" + + source_identifier: Optional[str] + time_sent: float + time_received: float + referee_command: RefereeCommand + referee_command_timestamp: float + stage: Stage + stage_time_left: float + blue_team: TeamInfo + yellow_team: TeamInfo + designated_position: Optional[Tuple[float]] = None + + # Information about the direction of play. + # True, if the blue team will have it's goal on the positive x-axis of the ssl-vision coordinate system. + # Obviously, the yellow team will play on the opposite half. + blue_team_on_positive_half: Optional[bool] = None + + # The command that will be issued after the current stoppage and ball placement to continue the game. + next_command: Optional[RefereeCommand] = None + + # The time in microseconds that is remaining until the current action times out + # The time will not be reset. It can get negative. + # An autoRef would raise an appropriate event, if the time gets negative. + # Possible actions where this time is relevant: + # * free kicks + # * kickoff, penalty kick, force start + # * ball placement + current_action_time_remaining: Optional[int] = None + + def __eq__(self, other): + if not isinstance(other, RefereeData): + return NotImplemented + return ( + self.stage == other.stage + and self.referee_command == other.referee_command + and self.referee_command_timestamp == other.referee_command_timestamp + and self.yellow_team == other.yellow_team + and self.blue_team == other.blue_team + and self.designated_position == other.designated_position + and self.blue_team_on_positive_half == other.blue_team_on_positive_half + and self.next_command == other.next_command + and self.current_action_time_remaining + == other.current_action_time_remaining + ) diff --git a/entities/game/game.py b/entities/game/game.py index 1d887bec..491198b3 100644 --- a/entities/game/game.py +++ b/entities/game/game.py @@ -1,7 +1,8 @@ from typing import List, Optional, NamedTuple - +from entities.game import game_object from entities.game.field import Field from entities.data.vision import FrameData, RobotData, BallData, PredictedFrame +from entities.data.referee import RefereeData from entities.data.command import RobotInfo from entities.game.game_object import Colour, GameObject, Robot @@ -9,6 +10,10 @@ from entities.game.robot import Robot from entities.game.ball import Ball +from entities.game.team_info import TeamInfo +from entities.referee.referee_command import RefereeCommand +from entities.referee.stage import Stage + from team_controller.src.config.settings import TIMESTEP # TODO : ^ I don't like this circular import logic. Wondering if we should store this constant somewhere else @@ -16,7 +21,6 @@ import logging, warnings -# Configure logging logger = logging.getLogger(__name__) @@ -48,6 +52,7 @@ def __init__( self._yellow_score = 0 self._blue_score = 0 + self._referee_records = [] @property def field(self) -> Field: @@ -59,9 +64,7 @@ def current_state(self) -> FrameData: @property def records(self) -> List[FrameData]: - if not self._records: - return None - return self._records + return self._records if self._records else None @property def yellow_score(self) -> int: @@ -86,7 +89,6 @@ def friendly_robots(self) -> List[Robot]: @friendly_robots.setter def friendly_robots(self, value: List[RobotData]): for robot_id, robot_data in enumerate(value): - # TODO: temporary fix for robot data being None if robot_data is not None: self._friendly_robots[robot_id].robot_data = robot_data @@ -97,7 +99,6 @@ def enemy_robots(self) -> List[Robot]: @enemy_robots.setter def enemy_robots(self, value: List[RobotData]): for robot_id, robot_data in enumerate(value): - # TODO: temporary fix for robot data being None if robot_data is not None: self._enemy_robots[robot_id].robot_data = robot_data @@ -106,7 +107,6 @@ def ball(self) -> Ball: return self._ball @ball.setter - # TODO: can always make a "setter" which copies the object and returns a new object with the changed value def ball(self, value: BallData): # Temporary fix for when the ball is None if value is not None: @@ -129,7 +129,6 @@ def is_ball_in_goal(self, right_goal: bool): and right_goal ) - ### Game state management ### def add_new_state(self, frame_data: FrameData) -> None: if isinstance(frame_data, FrameData): # if self.my_team_is_yellow: @@ -168,7 +167,6 @@ def _update_data(self, frame_data: FrameData) -> None: self._ball = frame_data.ball[0] # TODO: Don't always take first ball pos # BUG: self._ball is of type Ball, frame_data.ball[0] is of type BallData! - ### Robot data retrieval ### def get_robots_pos(self, is_yellow: bool) -> List[RobotData]: if not self._records: return None @@ -185,9 +183,6 @@ def get_robot_pos(self, is_yellow: bool, robot_id: int) -> RobotData: return None if not all else all[robot_id] def get_robots_velocity(self, is_yellow: bool) -> List[tuple]: - """ - Returns (vx, vy) of all robots on a team at the latest frame. None if no data available. - """ if len(self._records) <= 1: return None if is_yellow: @@ -204,7 +199,6 @@ def get_robots_velocity(self, is_yellow: bool) -> List[tuple]: for i in range(len(self.get_robots_pos(False))) ] - ### Ball Data retrieval ### def get_ball_pos(self) -> List[BallData]: if not self._records: return None @@ -212,16 +206,10 @@ def get_ball_pos(self) -> List[BallData]: return self._records[-1].ball def get_ball_velocity(self) -> Optional[tuple]: - """ - Returns (vx, vy) of the ball at the latest frame. None if no data available. - """ return self.get_object_velocity(Ball) - ### Frame Data retrieval ### def get_latest_frame(self) -> Optional[FrameData]: - if not self._records: - return None - return self._records[-1] + return self._records[-1] if self._records else None def get_my_latest_frame( self, my_team_is_yellow: bool @@ -240,9 +228,6 @@ def get_my_latest_frame( return self._reorganise_frame_data(latest_frame, my_team_is_yellow) def predict_next_frame(self) -> FrameData: - """ - Predicts the next frame based on the latest frame. - """ return self._predicted_next_frame def predict_my_next_frame( @@ -259,9 +244,6 @@ def predict_my_next_frame( return self._reorganise_frame_data(self._predicted_next_frame) def predict_frame_after(self, t: float) -> FrameData: - """ - Predicts frame in t seconds from the latest frame. - """ yellow_pos = [ self.predict_object_pos_after(t, RobotEntity(Colour.YELLOW, i)) for i in range(len(self.get_robots_pos(True))) @@ -285,27 +267,14 @@ def _reorganise_frame(self, frame: FrameData) -> Optional[PredictedFrame]: if frame: ts, yellow_pos, blue_pos, ball_pos = frame if self.my_team_is_yellow: - return PredictedFrame( - ts, - yellow_pos, - blue_pos, - ball_pos, - ) + return PredictedFrame(ts, yellow_pos, blue_pos, ball_pos) else: - return PredictedFrame( - ts, - blue_pos, - yellow_pos, - ball_pos, - ) + return PredictedFrame(ts, blue_pos, yellow_pos, ball_pos) return None def _reorganise_frame_data( self, frame_data: FrameData, my_team_is_yellow: bool ) -> tuple[RobotData, RobotData, BallData]: - """ - *Deprecated* reorganises frame data to be (friendly_robots, enemy_robots, balls) - """ _, yellow_robots, blue_robots, balls = frame_data if my_team_is_yellow: return yellow_robots, blue_robots, balls @@ -382,37 +351,25 @@ def predict_ball_pos_at_x(self, x: float) -> Optional[tuple]: return (x, y) def get_object_velocity(self, object: GameObject) -> Optional[tuple]: - velocities = self._get_object_velocity_at_frame(len(self._records) - 1, object) - if velocities is None: - return None return self._get_object_velocity_at_frame(len(self._records) - 1, object) def _get_object_position_at_frame(self, frame: int, object: GameObject): if object == Ball: - return self._records[frame].ball[0] # TODO don't always take first ball pos + return self._records[frame].ball[0] elif isinstance(object, RobotEntity): - if object.colour == Colour.YELLOW: - return self._records[frame].yellow_robots[object.id] - else: - return self._records[frame].blue_robots[object.id] + return ( + self._records[frame].yellow_robots[object.id] + if object.colour == Colour.YELLOW + else self._records[frame].blue_robots[object.id] + ) def _get_object_velocity_at_frame( self, frame: int, object: GameObject ) -> Optional[tuple]: - """ - Calculates the object's velocity based on position changes over time, - at frame f. - - Returns: - tuple: The velocity components (vx, vy). - - """ if frame >= len(self._records) or frame == 0: logger.warning("Cannot provide velocity at a frame that does not exist") - logger.info("See frame: %s", str(frame)) return None - # Otherwise get the previous and current frames previous_frame = self._records[frame - 1] current_frame = self._records[frame] @@ -426,7 +383,6 @@ def _get_object_velocity_at_frame( previous_time_received = previous_frame.ts time_received = current_frame.ts - # Latest frame should always be ahead of last one if time_received < previous_time_received: logger.warning( "Timestamps out of order for vision data %f should be after %f", @@ -461,7 +417,6 @@ def get_object_acceleration(self, object: GameObject) -> Optional[tuple]: windowMiddle = (windowStart + windowEnd) // 2 for j in range(windowStart, windowEnd): - # TODO: Handle when curr_vell is not when (time_received < previous_time_received) curr_vel = self._get_object_velocity_at_frame( len(self._records) - j, object ) @@ -469,7 +424,9 @@ def get_object_acceleration(self, object: GameObject) -> Optional[tuple]: averageVelocity[0] += curr_vel[0] averageVelocity[1] += curr_vel[1] elif missing_velocities == WINDOW - 1: - logging.warning(f"No velocity data to calculate acceleration for frame {len(self._records) - j}") + logging.warning( + f"No velocity data to calculate acceleration for frame {len(self._records) - j}" + ) return None else: missing_velocities += 1 @@ -493,3 +450,185 @@ def get_object_acceleration(self, object: GameObject) -> Optional[tuple]: futureAverageVelocity = tuple(averageVelocity) return (totalX / iter, totalY / iter) + + def add_new_referee_data(self, referee_data: RefereeData) -> None: + if not self._referee_records: + self._referee_records.append(referee_data) + elif referee_data[1:] != self._referee_records[-1][1:]: + self._referee_records.append(referee_data) + + def source_identifier(self) -> Optional[str]: + return ( + self._referee_records[-1].source_identifier + if self._referee_records + else None + ) + + @property + def last_time_sent(self) -> float: + return self._referee_records[-1].time_sent if self._referee_records else 0.0 + + @property + def last_time_received(self) -> float: + return self._referee_records[-1].time_received if self._referee_records else 0.0 + + @property + def last_command(self) -> RefereeCommand: + return ( + self._referee_records[-1].referee_command + if self._referee_records + else RefereeCommand.HALT + ) + + @property + def last_command_timestamp(self) -> float: + return ( + self._referee_records[-1].referee_command_timestamp + if self._referee_records + else 0.0 + ) + + @property + def stage(self) -> Stage: + return ( + self._referee_records[-1].stage + if self._referee_records + else Stage.NORMAL_FIRST_HALF_PRE + ) + + @property + def stage_time_left(self) -> float: + return ( + self._referee_records[-1].stage_time_left if self._referee_records else 0.0 + ) + + @property + def blue_team(self) -> TeamInfo: + return ( + self._referee_records[-1].blue_team + if self._referee_records + else TeamInfo( + name="", + score=0, + red_cards=0, + yellow_card_times=[], + yellow_cards=0, + timeouts=0, + timeout_time=0, + goalkeeper=0, + ) + ) + + @property + def yellow_team(self) -> TeamInfo: + return ( + self._referee_records[-1].yellow_team + if self._referee_records + else TeamInfo( + name="", + score=0, + red_cards=0, + yellow_card_times=[], + yellow_cards=0, + timeouts=0, + timeout_time=0, + goalkeeper=0, + ) + ) + + @property + def designated_position(self) -> Optional[tuple[float]]: + return ( + self._referee_records[-1].designated_position + if self._referee_records + else None + ) + + @property + def blue_team_on_positive_half(self) -> Optional[bool]: + return ( + self._referee_records[-1].blue_team_on_positive_half + if self._referee_records + else None + ) + + @property + def next_command(self) -> Optional[RefereeCommand]: + return self._referee_records[-1].next_command if self._referee_records else None + + @property + def current_action_time_remaining(self) -> Optional[int]: + return ( + self._referee_records[-1].current_action_time_remaining + if self._referee_records + else None + ) + + @property + def is_halt(self) -> bool: + return self.last_command == RefereeCommand.HALT + + @property + def is_stop(self) -> bool: + return self.last_command == RefereeCommand.STOP + + @property + def is_normal_start(self) -> bool: + return self.last_command == RefereeCommand.NORMAL_START + + @property + def is_force_start(self) -> bool: + return self.last_command == RefereeCommand.FORCE_START + + @property + def is_prepare_kickoff_yellow(self) -> bool: + return self.last_command == RefereeCommand.PREPARE_KICKOFF_YELLOW + + @property + def is_prepare_kickoff_blue(self) -> bool: + return self.last_command == RefereeCommand.PREPARE_KICKOFF_BLUE + + @property + def is_prepare_penalty_yellow(self) -> bool: + return self.last_command == RefereeCommand.PREPARE_PENALTY_YELLOW + + @property + def is_prepare_penalty_blue(self) -> bool: + return self.last_command == RefereeCommand.PREPARE_PENALTY_BLUE + + @property + def is_direct_free_yellow(self) -> bool: + return self.last_command == RefereeCommand.DIRECT_FREE_YELLOW + + @property + def is_direct_free_blue(self) -> bool: + return self.last_command == RefereeCommand.DIRECT_FREE_BLUE + + @property + def is_timeout_yellow(self) -> bool: + return self.last_command == RefereeCommand.TIMEOUT_YELLOW + + @property + def is_timeout_blue(self) -> bool: + return self.last_command == RefereeCommand.TIMEOUT_BLUE + + @property + def is_ball_placement_yellow(self) -> bool: + return self.last_command == RefereeCommand.BALL_PLACEMENT_YELLOW + + @property + def is_ball_placement_blue(self) -> bool: + return self.last_command == RefereeCommand.BALL_PLACEMENT_BLUE + + +if __name__ == "__main__": + logging.basicConfig(level=logging.INFO) + + game = Game() + print(game.ball.x) + print(game.ball.y) + print(game.ball.z) + game.ball = BallData(1, 2, 3) + print(game.ball.x) + print(game.ball.y) + print(game.ball.z) diff --git a/entities/game/team_info.py b/entities/game/team_info.py index c31f4362..555c5d3c 100644 --- a/entities/game/team_info.py +++ b/entities/game/team_info.py @@ -1,3 +1,6 @@ +from typing import Optional, List + + class TeamInfo: """ Class containing information about a team. @@ -8,17 +11,29 @@ def __init__( name: str, score: int = 0, red_cards: int = 0, + yellow_card_times: List[int] = [], yellow_cards: int = 0, timeouts: int = 0, timeout_time: int = 0, + goalkeeper: int = 0, + foul_counter: Optional[int] = None, + ball_placement_failures: Optional[int] = None, + can_place_ball: Optional[bool] = None, + max_allowed_bots: Optional[int] = None, + bot_substitution_intent: Optional[bool] = None, + ball_placement_failures_reached: Optional[bool] = None, + bot_substitution_allowed: Optional[bool] = None, + bot_substitutions_left: Optional[int] = None, + bot_substitution_time_left: Optional[int] = None, ): # The team's name (empty string if operator has not typed anything). self.name = name # The number of goals scored by the team during normal play and overtime. self.score = score - # The number of red cards issued to the team since the beginning of the - # game. + # The number of red cards issued to the team since the beginning of the game. self.red_cards = red_cards + # The amount of time (in microseconds) left on each yellow card issued to the team. + self.yellow_card_times = yellow_card_times # The total number of yellow cards ever issued to the team. self.yellow_cards = yellow_cards # The number of timeouts this team can still call. @@ -26,6 +41,26 @@ def __init__( self.timeouts = timeouts # The number of microseconds of timeout this team can use. self.timeout_time = timeout_time + # The pattern number of this team's goalkeeper. + self.goalkeeper = goalkeeper + # The total number of countable fouls that act towards yellow cards + self.foul_counter = foul_counter + # The number of consecutive ball placement failures of this team + self.ball_placement_failures = ball_placement_failures + # Indicate if the team is able and allowed to place the ball + self.can_place_ball = can_place_ball + # The maximum number of bots allowed on the field based on division and cards + self.max_allowed_bots = max_allowed_bots + # The team has submitted an intent to substitute one or more robots at the next chance + self.bot_substitution_intent = bot_substitution_intent + # Indicate if the team reached the maximum allowed ball placement failures and is thus not allowed to place the ball anymore + self.ball_placement_failures_reached = ball_placement_failures_reached + # The team is allowed to substitute one or more robots currently + self.bot_substitution_allowed = bot_substitution_allowed + # The number of bot substitutions left by the team in this halftime + self.bot_substitutions_left = bot_substitutions_left + # The number of microseconds left for current bot substitution + self.bot_substitution_time_left = bot_substitution_time_left def __repr__(self): return ( @@ -47,9 +82,20 @@ def parse_referee_packet(self, packet): self.name = packet.name self.score = packet.score self.red_cards = packet.red_cards + self.yellow_card_times = list(packet.yellow_card_times) self.yellow_cards = packet.yellow_cards - self.timeouts_left = packet.timeouts + self.timeouts = packet.timeouts self.timeout_time = packet.timeout_time + self.goalkeeper = packet.goalkeeper + self.foul_counter = packet.foul_counter + self.ball_placement_failures = packet.ball_placement_failures + self.can_place_ball = packet.can_place_ball + self.max_allowed_bots = packet.max_allowed_bots + self.bot_substitution_intent = packet.bot_substitution_intent + self.ball_placement_failures_reached = packet.ball_placement_failures_reached + self.bot_substitution_allowed = packet.bot_substitution_allowed + self.bot_substitutions_left = packet.bot_substitutions_left + self.bot_substitution_time_left = packet.bot_substitution_time_left def increment_score(self): self.score += 1 diff --git a/entities/referee/referee_command.py b/entities/referee/referee_command.py index 84ad94fd..63661ba3 100644 --- a/entities/referee/referee_command.py +++ b/entities/referee/referee_command.py @@ -1,36 +1,35 @@ -class RefereeCommand: +from enum import Enum + + +class RefereeCommand(Enum): """ - Class representing a referee command. + Enum representing a referee command. """ - def __init__(self, command_id: int, name: str): - self.command_id = command_id - self.name = name - - def __repr__(self): - return f"RefereeCommand(id={self.command_id}, name={self.name})" + HALT = 0 + STOP = 1 + NORMAL_START = 2 + FORCE_START = 3 + PREPARE_KICKOFF_YELLOW = 4 + PREPARE_KICKOFF_BLUE = 5 + PREPARE_PENALTY_YELLOW = 6 + PREPARE_PENALTY_BLUE = 7 + DIRECT_FREE_YELLOW = 8 + DIRECT_FREE_BLUE = 9 + INDIRECT_FREE_YELLOW = 10 # deprecated + INDIRECT_FREE_BLUE = 11 # deprecated + TIMEOUT_YELLOW = 12 + TIMEOUT_BLUE = 13 + GOAL_YELLOW = 14 # deprecated + GOAL_BLUE = 15 # deprecated + BALL_PLACEMENT_YELLOW = 16 + BALL_PLACEMENT_BLUE = 17 @staticmethod def from_id(command_id: int): - command_map = { - 0: "HALT", - 1: "STOP", - 2: "NORMAL_START", - 3: "FORCE_START", - 4: "PREPARE_KICKOFF_YELLOW", - 5: "PREPARE_KICKOFF_BLUE", - 6: "PREPARE_PENALTY_YELLOW", - 7: "PREPARE_PENALTY_BLUE", - 8: "DIRECT_FREE_YELLOW", - 9: "DIRECT_FREE_BLUE", - 10: "INDIRECT_FREE_YELLOW", - 11: "INDIRECT_FREE_BLUE", - 12: "TIMEOUT_YELLOW", - 13: "TIMEOUT_BLUE", - 14: "GOAL_YELLOW", - 15: "GOAL_BLUE", - 16: "BALL_PLACEMENT_YELLOW", - 17: "BALL_PLACEMENT_BLUE", - } - name = command_map.get(command_id, "UNKNOWN") - return RefereeCommand(command_id, name) + for command in RefereeCommand: + if ( + command.value == command_id + ): # Check the enum's value (which is the command_id) + return command + raise ValueError(f"Invalid referee command ID: {command_id}") diff --git a/entities/referee/stage.py b/entities/referee/stage.py index ba42da1e..6506f912 100644 --- a/entities/referee/stage.py +++ b/entities/referee/stage.py @@ -1,32 +1,37 @@ -class Stage: +from enum import Enum + + +class Stage(Enum): """ - Class representing a game stage. + Enum representing a game stage. """ - def __init__(self, stage_id: int, name: str): - self.stage_id = stage_id - self.name = name - - def __repr__(self): - return f"Stage(id={self.stage_id}, name={self.name})" + NORMAL_FIRST_HALF_PRE = 0 + NORMAL_FIRST_HALF = 1 + NORMAL_HALF_TIME = 2 + NORMAL_SECOND_HALF_PRE = 3 + NORMAL_SECOND_HALF = 4 + EXTRA_TIME_BREAK = 5 + EXTRA_FIRST_HALF_PRE = 6 + EXTRA_FIRST_HALF = 7 + EXTRA_HALF_TIME = 8 + EXTRA_SECOND_HALF_PRE = 9 + EXTRA_SECOND_HALF = 10 + PENALTY_SHOOTOUT_BREAK = 11 + PENALTY_SHOOTOUT = 12 + POST_GAME = 13 @staticmethod def from_id(stage_id: int): - stage_map = { - 0: "NORMAL_FIRST_HALF_PRE", - 1: "NORMAL_FIRST_HALF", - 2: "NORMAL_HALF_TIME", - 3: "NORMAL_SECOND_HALF_PRE", - 4: "NORMAL_SECOND_HALF", - 5: "EXTRA_TIME_BREAK", - 6: "EXTRA_FIRST_HALF_PRE", - 7: "EXTRA_FIRST_HALF", - 8: "EXTRA_HALF_TIME", - 9: "EXTRA_SECOND_HALF_PRE", - 10: "EXTRA_SECOND_HALF", - 11: "PENALTY_SHOOTOUT_BREAK", - 12: "PENALTY_SHOOTOUT", - 13: "POST_GAME", - } - name = stage_map.get(stage_id, "UNKNOWN") - return Stage(stage_id, name) + try: + return Stage(stage_id) + except ValueError: + raise ValueError(f"Invalid stage ID: {stage_id}") + + @property + def name(self): + return self.name + + @property + def stage_id(self): + return self.value diff --git a/main.py b/main.py index 60332198..f56524f5 100644 --- a/main.py +++ b/main.py @@ -24,6 +24,7 @@ # Enable all warnings, including DeprecationWarning warnings.simplefilter("default", DeprecationWarning) + def data_update_listener(receiver: VisionDataReceiver): # Start receiving game data; this will run in a separate thread. receiver.pull_game_data() @@ -35,15 +36,24 @@ def main(): time.sleep(0.2) message_queue = queue.SimpleQueue() - receiver = VisionDataReceiver(message_queue) + + referee_receiver = RefereeMessageReceiver(message_queue, debug=False) + vision_receiver = VisionDataReceiver(message_queue) decision_maker = StartUpController(game) - # Start the data receiving in a separate thread - data_thread = threading.Thread(target=data_update_listener, args=(receiver,)) - data_thread.daemon = True # Allows the thread to close when the main program exits - data_thread.start() + # Start the data receiving in separate threads + vision_thread = threading.Thread(target=vision_receiver.pull_game_data) + referee_thread = threading.Thread(target=referee_receiver.pull_referee_data) - TIME = 1/60 * 10 # frames in seconds + # Allows the thread to close when the main program exits + vision_thread.daemon = True + referee_thread.daemon = True + + # Start both thread + vision_thread.start() + referee_thread.start() + + TIME = 1 / 60 * 10 # frames in seconds FRAMES_IN_TIME = round(60 * TIME) # TODO: Not implemented @@ -57,8 +67,10 @@ def main(): try: logger.debug("LOCATED BALL") - logger.debug(f"Predicting robot position with {FRAMES_IN_TIME} frames of motion") - + logger.debug( + f"Predicting robot position with {FRAMES_IN_TIME} frames of motion" + ) + predictions: List[PredictedFrame] = [] while True: (message_type, message) = message_queue.get() # Infinite timeout for now @@ -66,36 +78,31 @@ def main(): if message_type == MessageType.VISION: frames += 1 game.add_new_state(message) - + + actual = game.records[-1] # JUST FOR TESTING - don't do this irl if ( len(predictions) >= FRAMES_IN_TIME and predictions[-FRAMES_IN_TIME] != None - ): + ): logger.debug( - "Ball prediction inaccuracy delta (cm): " + - "{:.5f}".format( + "Ball prediction inaccuracy delta (cm): " + + "{:.5f}".format( 100 * math.sqrt( - ( - game.ball.x - - predictions[-FRAMES_IN_TIME].ball[0].x - ) + (game.ball.x - predictions[-FRAMES_IN_TIME].ball[0].x) ** 2 - + ( - game.ball.y - - predictions[-FRAMES_IN_TIME].ball[0].y - ) + + (game.ball.y - predictions[-FRAMES_IN_TIME].ball[0].y) ** 2 ) ) ) for i in range(6): logger.debug( - f"Enemy(Blue) robot {i} prediction inaccuracy delta (cm): " + - "{:.5f}".format( + f"Enemy(Blue) robot {i} prediction inaccuracy delta (cm): " + + "{:.5f}".format( 100 * math.sqrt( - ( + ( # proposed implementation game.enemy_robots[i].x - predictions[-FRAMES_IN_TIME].enemy_robots[i].x @@ -112,45 +119,49 @@ def main(): ) for i in range(6): logger.debug( - f"Friendly(Yellow) robot {i} prediction inaccuracy delta (cm): " + - "{:.5f}".format( + f"Friendly(Yellow) robot {i} prediction inaccuracy delta (cm): " + + "{:.5f}".format( 100 * math.sqrt( ( # original implementation game.friendly_robots[i].x - - predictions[-FRAMES_IN_TIME].friendly_robots[i].x + - predictions[-FRAMES_IN_TIME] + .friendly_robots[i] + .x ) ** 2 + ( # proposed implementation game.friendly_robots[i].y - - predictions[-FRAMES_IN_TIME].friendly_robots[i].y + - predictions[-FRAMES_IN_TIME] + .friendly_robots[i] + .y ) ** 2 ) ) ) - predictions.append(game.predicted_next_frame) - + elif message_type == MessageType.REF: - pass - + game.add_new_referee_data(message) + decision_maker.make_decision() - + except KeyboardInterrupt: print("Stopping main program.") + def main1(): """ This is a test function to demonstrate the use of the Game class and the Robot class. - - In terms of RobotInfo and the way it is currently implemented is not confirmed and may change. - (have the robot info update game directly in the main thread as robot commands are sent on the main thread) - - We will need to implement a way to update the robot info with grsim and rsim in the controllers + + In terms of RobotInfo and the way it is currently implemented is not confirmed and may change. + (have the robot info update game directly in the main thread as robot commands are sent on the main thread) + + We will need to implement a way to update the robot info with grsim and rsim in the controllers """ ### Standard setup for the game ### game = Game(my_team_is_yellow=True) @@ -170,41 +181,55 @@ def main1(): # referee_thread.start() #### Demo #### - + ### Creates the made up robot info message ### - madeup_recieved_message = [RobotInfo(True), RobotInfo(False), RobotInfo(False), RobotInfo(False), RobotInfo(False), RobotInfo(False)] + madeup_recieved_message = [ + RobotInfo(True), + RobotInfo(False), + RobotInfo(False), + RobotInfo(False), + RobotInfo(False), + RobotInfo(False), + ] message_type = MessageType.ROBOT_INFO message_queue.put((message_type, madeup_recieved_message)) try: while True: - (message_type, message) = message_queue.get() + (message_type, message) = message_queue.get() if message_type == MessageType.VISION: game.add_new_state(message) - + ### for demo purposes (displays when vision is received) ### - print(f"Before robot is_active( {game.friendly_robots[0].inactive} ) coords: {game.friendly_robots[0].x}, {game.friendly_robots[0].y}") + print( + f"Before robot is_active( {game.friendly_robots[0].inactive} ) coords: {game.friendly_robots[0].x}, {game.friendly_robots[0].y}" + ) # TODO: create a check with referee to see if robot is inactive game.friendly_robots[0].inactive = True - print(f"After robot is_active( {game.friendly_robots[0].inactive} ) Coords: {game.friendly_robots[0].x}, {game.friendly_robots[0].y}\n") + print( + f"After robot is_active( {game.friendly_robots[0].inactive} ) Coords: {game.friendly_robots[0].x}, {game.friendly_robots[0].y}\n" + ) ### Getting coordinate data ### - print(f"Friendly(Yellow) Robot 1 coords: {game.friendly_robots[0].x}, {game.friendly_robots[0].y}, {game.friendly_robots[0].orientation}") + print( + f"Friendly(Yellow) Robot 1 coords: {game.friendly_robots[0].x}, {game.friendly_robots[0].y}, {game.friendly_robots[0].orientation}" + ) print(f"Ball coords: {game.ball.x}, {game.ball.y}, {game.ball.z}\n\n") if message_type == MessageType.REF: pass - - if message_type == MessageType.ROBOT_INFO: + + if message_type == MessageType.ROBOT_INFO: game.add_robot_info(message) - + ### for demo purposes (displays when robot info is received) #### for i in range(6): - print(f"Robot {i} has ball: {game.friendly_robots[i].has_ball}\n\n") + print(f"Robot {i} has ball: {game.friendly_robots[i].has_ball}\n\n") except KeyboardInterrupt: print("Stopping main program.") + if __name__ == "__main__": main() diff --git a/team_controller/src/data/referee_receiver.py b/team_controller/src/data/referee_receiver.py index 4267b96e..cd5a15af 100644 --- a/team_controller/src/data/referee_receiver.py +++ b/team_controller/src/data/referee_receiver.py @@ -1,10 +1,14 @@ import threading import time +import queue from typing import Tuple, Optional, List from entities.referee.referee_command import RefereeCommand from entities.referee.stage import Stage from entities.game.team_info import TeamInfo +from entities.data.referee import RefereeData +from team_controller.src.data.base_receiver import BaseReceiver +from team_controller.src.data.message_enum import MessageType from team_controller.src.utils import network_manager from team_controller.src.config.settings import MULTICAST_GROUP_REFEREE, REFEREE_PORT @@ -13,7 +17,8 @@ logger = logging.getLogger(__name__) -class RefereeMessageReceiver: + +class RefereeMessageReceiver(BaseReceiver): """ A class responsible for receiving and managing referee messages in a multi-robot game environment. The class interfaces with a network manager to receive packets, which contain game state information, @@ -23,7 +28,16 @@ class RefereeMessageReceiver: ip (str): The IP address for receiving multicast referee data. Defaults to MULTICAST_GROUP_REFEREE. port (int): The port for receiving referee data. Defaults to REFEREE_PORT. """ - def __init__(self, ip=MULTICAST_GROUP_REFEREE, port=REFEREE_PORT): # TODO: add message queue + + def __init__( + self, + message_queue: queue.SimpleQueue, + ip=MULTICAST_GROUP_REFEREE, + port=REFEREE_PORT, + debug=False, + ): + super().__init__(message_queue) + self.net = network_manager.NetworkManager(address=(ip, port), bind_socket=True) self.prev_command_counter = -1 self.command_history = [] @@ -36,7 +50,7 @@ def __init__(self, ip=MULTICAST_GROUP_REFEREE, port=REFEREE_PORT): # TODO: add m # Initialize state variables self.stage = None self.command = None - self.sent_time = None + self.time_sent = None self.stage_time_left = None self.command_counter = None self.command_timestamp = None @@ -137,7 +151,7 @@ def _update_data(self, referee_packet: Referee) -> None: # Update state variables self.stage = Stage.from_id(referee_packet.stage) self.command = RefereeCommand.from_id(referee_packet.command) - self.sent_time = ( + self.time_sent = ( referee_packet.packet_timestamp / 1e6 ) # Convert microseconds to seconds self.stage_time_left = ( @@ -150,22 +164,40 @@ def _update_data(self, referee_packet: Referee) -> None: self.yellow_info.parse_referee_packet(referee_packet.yellow) self.blue_info.parse_referee_packet(referee_packet.blue) - def check_new_message(self) -> bool: - """ - Check if a new referee message has been received. - - Returns: - bool: True if a new message has been received, False otherwise. - """ - data = self.net.receive_data() - if data: - serialized_data = self._serialize_relevant_fields(data) + # Construct the designated position tuple if available + designated_position = None + if referee_packet.HasField("designated_position"): + designated_position = ( + referee_packet.designated_position.x, + referee_packet.designated_position.y, + ) - if serialized_data != self.old_serialized_data: - self.referee.ParseFromString(data) - self.old_serialized_data = serialized_data - return True - return False + # Construct the RefereeData instance + referee_data = RefereeData( + source_identifier=referee_packet.source_identifier, + time_sent=self.time_sent, + time_received=self.time_received, + referee_command=self.command, + referee_command_timestamp=self.command_timestamp, + stage=self.stage, + stage_time_left=self.stage_time_left, + blue_team=self.blue_info, + yellow_team=self.yellow_info, + designated_position=designated_position, + blue_team_on_positive_half=referee_packet.blue_team_on_positive_half, + next_command=( + RefereeCommand.from_id(referee_packet.next_command) + if referee_packet.HasField("next_command") + else None + ), + current_action_time_remaining=( + referee_packet.current_action_time_remaining + if referee_packet.HasField("current_action_time_remaining") + else None + ), + ) + + self._message_queue.put_nowait((MessageType.REF, referee_data)) def check_new_command(self) -> bool: """ @@ -174,7 +206,6 @@ def check_new_command(self) -> bool: Returns: bool: True if a new command has been received, False otherwise. """ - data = self.net.receive_data() history_length = 5 if data: @@ -187,17 +218,6 @@ def check_new_command(self) -> bool: return True return False - def get_latest_command(self) -> Tuple[int, Tuple[float, float]]: - """ - Get the latest command and its designated position. - - Returns: - Tuple[int, Tuple[float, float]]: The latest command and its designated position. - """ - command = self.referee.command - designated_position = self.referee.designated_position - return command, (designated_position.x, designated_position.y) - def get_latest_message(self) -> Referee: """ Retrieves the current referee data. @@ -208,62 +228,6 @@ def get_latest_message(self) -> Referee: with self.lock: return self.referee - def get_stage_time_left(self) -> float: - """ - Get the time left in the current stage in seconds. - - Returns: - float: The time left in the current stage in seconds. - """ - return self.stage_time_left - - def get_packet_timestamp(self) -> float: - """ - Get the packet timestamp in seconds. - - Returns: - float: The packet timestamp in seconds. - """ - return self.sent_time - - def yellow_team_info(self) -> TeamInfo: - """ - Get the information for the yellow team. - - Returns: - TeamInfo: The yellow team information. - """ - return self.yellow_info - - def blue_team_info(self) -> TeamInfo: - """ - Get the information for the blue team. - - Returns: - TeamInfo: The blue team information. - """ - return self.blue_info - - def get_stage(self) -> Optional[Stage]: - """ - Get the current state. - - Returns: - Optional[Stage]: Current state, otherwise None - """ - return self.stage - - def get_next_command(self) -> Optional[RefereeCommand]: - """ - Get the next command if available. - - Returns: - Optional[int]: The next command if available, None otherwise. - """ - if self.referee.next_command: - return RefereeCommand.from_id(self.referee.next_command) - return None - def get_designated_position(self) -> Optional[Tuple[float, float]]: """ Get the designated position if available. @@ -301,15 +265,6 @@ def check_command_sequence(self, sequence: List[RefereeCommand]) -> bool: return False return self.command_history[-len(sequence) :] == sequence - def get_time_received(self) -> float: - """ - Retrieves the time at which the most recent referee data was received. - - Returns: - float: The time at which the most recent referee data was received. - """ - return self.time_received - def wait_for_update(self, timeout: float = None) -> bool: """ Waits for the data to be updated, returning True if an update occurs within the timeout. diff --git a/team_controller/src/tests/grsim_robot_controller_startup_test.py b/team_controller/src/tests/grsim_robot_controller_startup_test.py index 4fc7eb5b..081072a3 100644 --- a/team_controller/src/tests/grsim_robot_controller_startup_test.py +++ b/team_controller/src/tests/grsim_robot_controller_startup_test.py @@ -52,6 +52,9 @@ def make_decision(self): command = self._calculate_robot_velocities( robot_id, target_coords, robots, balls, face_ball=True ) + + if self.game.last_command.name in ["HALT", "STOP"]: + continue self.sim_robot_controller.add_robot_commands(command, robot_id) logger.debug(out_packet) diff --git a/team_controller/src/utils/network_utils.py b/team_controller/src/utils/network_utils.py index 84c7b840..0960f47c 100644 --- a/team_controller/src/utils/network_utils.py +++ b/team_controller/src/utils/network_utils.py @@ -9,6 +9,7 @@ logger = logging.getLogger(__name__) + def setup_socket( sock: socket.socket, address: Tuple[str, int], bind_socket: bool = False ) -> socket.socket: @@ -41,7 +42,7 @@ def setup_socket( sock.setsockopt(socket.IPPROTO_IP, socket.IP_ADD_MEMBERSHIP, mreq) # sock.settimeout(0.005) # Set timeout to 1 frame period (60 FPS) - logger.info( + logging.info( "Socket setup completed with address %s and bind_socket=%s", address, bind_socket, @@ -78,8 +79,13 @@ def receive_data(sock: socket.socket) -> Optional[bytes]: logger.exception("Unexpected error receiving data") return None + send_sock = None -def send_command(address: Tuple[str, int], command: object, is_sim_robot_cmd: bool = False) -> Optional[bytes]: + + +def send_command( + address: Tuple[str, int], command: object, is_sim_robot_cmd: bool = False +) -> Optional[bytes]: """ Sends a command to the specified address over a UDP socket. @@ -89,28 +95,28 @@ def send_command(address: Tuple[str, int], command: object, is_sim_robot_cmd: bo is_sim_robot_cmd (bool): If True, the function will attempt to receive a response from the server. Returns: - Optional[bytes]: The data received, or None if no data is received or if an error occurs. - + Optional[bytes]: The data received, or None if no data is received or if an error occurs. + This function creates a temporary UDP socket, serializes the command, and sends it to the specified address. If the command being sent is a RobotControl packet there will be a response packet which will be received. Errors during serialization or socket operations are logged, with specific handling if the `SerializeToString` method is missing. """ - global send_sock + global send_sock try: if not send_sock: - send_sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) - + send_sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + start = time.time() - serialized_command = command.SerializeToString() + serialized_command = command.SerializeToString() send_sock.sendto(serialized_command, address) - + if is_sim_robot_cmd: data = receive_data(send_sock) return data logger.info("Command sent to %s", address) - + except AttributeError: logger.error("Command object has no SerializeToString method") except socket.error as e: