From 01541caa1fcfe68db186f093db128ea88b2eb434 Mon Sep 17 00:00:00 2001 From: isaac0804 Date: Wed, 27 Nov 2024 02:00:46 +0000 Subject: [PATCH 01/14] Use queue for referee message receiver --- entities/data/referee.py | 7 + entities/game/game.py | 158 +++++++++++++------ main.py | 137 +++++++++------- team_controller/src/data/referee_receiver.py | 30 +++- 4 files changed, 219 insertions(+), 113 deletions(-) create mode 100644 entities/data/referee.py diff --git a/entities/data/referee.py b/entities/data/referee.py new file mode 100644 index 00000000..21c6f06b --- /dev/null +++ b/entities/data/referee.py @@ -0,0 +1,7 @@ +from collections import namedtuple + + +RefereeData = namedtuple( + "RefereeData", + ["t", "referee_command", "stage", "blue_team_info", "yellow_team_info"], +) diff --git a/entities/game/game.py b/entities/game/game.py index 8cf9d339..2a046ce4 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 @@ -10,6 +11,7 @@ from entities.game.ball import Ball 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 import logging, warnings @@ -17,6 +19,7 @@ # Configure logging logger = logging.getLogger(__name__) + class Game: """ Class containing states of the entire game and field information. @@ -25,16 +28,21 @@ class Game: def __init__(self, my_team_is_yellow=True): self._my_team_is_yellow = my_team_is_yellow self._field = Field() - + self._records: List[FrameData] = [] self._predicted_next_frame: PredictedFrame = None - - self._friendly_robots: List[Robot] = [Robot(id, is_friendly=True) for id in range(6)] - self._enemy_robots: List[Robot] = [Robot(id, is_friendly=False) for id in range(6)] + + self._friendly_robots: List[Robot] = [ + Robot(id, is_friendly=True) for id in range(6) + ] + self._enemy_robots: List[Robot] = [ + Robot(id, is_friendly=False) for id in range(6) + ] self._ball: Ball = Ball() - + self._yellow_score = 0 self._blue_score = 0 + self._referee_records = [] @property def field(self) -> Field: @@ -52,7 +60,9 @@ def records(self) -> List[FrameData]: @property def yellow_score(self) -> int: - return self._yellow_score + return ( + self._yellow_score + ) # TODO, read directly from _referee_records or store as class variable? @property def blue_score(self) -> int: @@ -65,57 +75,71 @@ def my_team_is_yellow(self) -> bool: @property def predicted_next_frame(self) -> PredictedFrame: return self._predicted_next_frame - + @property def friendly_robots(self) -> List[Robot]: return self._friendly_robots - + @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 - + self._friendly_robots[robot_id].robot_data = robot_data + @property def enemy_robots(self) -> List[Robot]: return self._enemy_robots - + @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 - + @property 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): self._ball.ball_data = value - + def is_ball_in_goal(self, left_goal: bool): ball_pos = self.get_ball_pos()[0] - return (ball_pos.x < -self.field.HALF_LENGTH and (ball_pos.y < self.field.HALF_GOAL_WIDTH and ball_pos.y > -self.field.HALF_GOAL_WIDTH) and left_goal - or ball_pos.x > self.field.HALF_LENGTH and (ball_pos.y < self.field.HALF_GOAL_WIDTH and ball_pos.y > -self.field.HALF_GOAL_WIDTH) and not left_goal) + return ( + ball_pos.x < -self.field.HALF_LENGTH + and ( + ball_pos.y < self.field.HALF_GOAL_WIDTH + and ball_pos.y > -self.field.HALF_GOAL_WIDTH + ) + and left_goal + or ball_pos.x > self.field.HALF_LENGTH + and ( + ball_pos.y < self.field.HALF_GOAL_WIDTH + and ball_pos.y > -self.field.HALF_GOAL_WIDTH + ) + and not left_goal + ) ### Game state management ### def add_new_state(self, frame_data: FrameData) -> None: if isinstance(frame_data, FrameData): self._records.append(frame_data) - self._predicted_next_frame = self._reorganise_frame(self.predict_frame_after(TIMESTEP)) + self._predicted_next_frame = self._reorganise_frame( + self.predict_frame_after(TIMESTEP) + ) self._update_data(frame_data) else: raise ValueError("Invalid frame data.") - + def add_robot_info(self, robots_info: List[RobotInfo]) -> None: for robot_id, robot_info in enumerate(robots_info): self._friendly_robots[robot_id].has_ball = robot_info.has_ball # Extensible with more info (remeber to add the property in robot.py) - + def _update_data(self, frame_data: FrameData) -> None: if self.my_team_is_yellow: self.friendly_robots = frame_data.yellow_robots @@ -130,7 +154,11 @@ def get_robots_pos(self, is_yellow: bool) -> List[RobotData]: if not self._records: return None record = self._records[-1] - warnings.warn("Use game.friendly_robots/enemy_robots instead", DeprecationWarning, stacklevel=2) + warnings.warn( + "Use game.friendly_robots/enemy_robots instead", + DeprecationWarning, + stacklevel=2, + ) return record.yellow_robots if is_yellow else record.blue_robots def get_robot_pos(self, is_yellow: bool, robot_id: int) -> RobotData: @@ -147,15 +175,14 @@ def get_robots_velocity(self, is_yellow: bool) -> List[tuple]: # TODO: potential namespace conflict when robot (robot.py) entity is reintroduced. Think about integrating the two return [ self.get_object_velocity(RobotEntity(i, Colour.YELLOW)) - for i in range(len(self.get_robots_pos(True)) + for i in range( + len(self.get_robots_pos(True)) ) # TODO: This is a bit of a hack, we should be able to get the number of robots from the field ] else: return [ self.get_object_velocity(RobotEntity(i, Colour.BLUE)) - for i in range( - len(self.get_robots_pos(False)) - ) + for i in range(len(self.get_robots_pos(False))) ] ### Ball Data retrieval ### @@ -177,14 +204,20 @@ def get_latest_frame(self) -> Optional[FrameData]: return None return self._records[-1] - def get_my_latest_frame(self, my_team_is_yellow: bool) -> tuple[RobotData, RobotData, BallData]: + def get_my_latest_frame( + self, my_team_is_yellow: bool + ) -> tuple[RobotData, RobotData, BallData]: """ FrameData rearranged as Tuple(friendly_robots, enemy_robots, balls) based on provided _my_team_is_yellow field """ if not self._records: return None latest_frame = self.get_latest_frame() - warnings.warn("Use game.friendly/enemy.x/y/orentation instead", DeprecationWarning, stacklevel=2) + warnings.warn( + "Use game.friendly/enemy.x/y/orentation instead", + DeprecationWarning, + stacklevel=2, + ) return self._reorganise_frame_data(latest_frame, my_team_is_yellow) def predict_next_frame(self) -> FrameData: @@ -193,15 +226,19 @@ def predict_next_frame(self) -> FrameData: """ return self._predicted_next_frame - def predict_my_next_frame(self,my_team_is_yellow: bool) -> tuple[RobotData, RobotData, BallData]: + def predict_my_next_frame( + self, my_team_is_yellow: bool + ) -> tuple[RobotData, RobotData, BallData]: """ FrameData rearranged as (friendly_robots, enemy_robots, balls) based on my_team_is_yellow """ if self._predicted_next_frame is None: return None - warnings.warn("Use game.predicted_next_frame instead", DeprecationWarning, stacklevel=2) - return self._reorganise_frame_data(self._predicted_next_frame) - + warnings.warn( + "Use game.predicted_next_frame instead", DeprecationWarning, stacklevel=2 + ) + 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. @@ -224,7 +261,7 @@ def predict_frame_after(self, t: float) -> FrameData: list(map(lambda pos: RobotData(pos[0], pos[1], 0), blue_pos)), [BallData(ball_pos[0], ball_pos[1], 0)], # TODO : Support z axis ) - + def _reorganise_frame(self, frame: FrameData) -> Optional[PredictedFrame]: if frame: ts, yellow_pos, blue_pos, ball_pos = frame @@ -243,7 +280,7 @@ def _reorganise_frame(self, frame: FrameData) -> Optional[PredictedFrame]: ball_pos, ) return None - + def _reorganise_frame_data( self, frame_data: FrameData, my_team_is_yellow: bool ) -> tuple[RobotData, RobotData, BallData]: @@ -261,7 +298,7 @@ def predict_object_pos_after(self, t: float, object: GameObject) -> Optional[tup # If t is after the object has stopped we return the position at which object stopped. sx = 0 sy = 0 - + acceleration = self.get_object_acceleration(object) if acceleration is None: @@ -274,7 +311,7 @@ def predict_object_pos_after(self, t: float, object: GameObject) -> Optional[tup ux, uy = None, None else: ux, uy = vels - + if object is Ball: ball = self.get_ball_pos() start_x, start_y = ball[0].x, ball[0].y @@ -284,10 +321,10 @@ def predict_object_pos_after(self, t: float, object: GameObject) -> Optional[tup if ax and ux: sx = self._calculate_displacement(ux, ax, t) - + if ay and uy: sy = self._calculate_displacement(uy, ay, t) - + return ( start_x + sx, start_y + sy, @@ -299,24 +336,26 @@ def _calculate_displacement(self, u, a, t): else: stop_time = -u / a effective_time = min(t, stop_time) - displacement = (u * effective_time) + (0.5 * a * effective_time ** 2) - logger.debug(f"Displacement: {displacement} for time: {effective_time}, stop time: {stop_time}") + displacement = (u * effective_time) + (0.5 * a * effective_time**2) + logger.debug( + f"Displacement: {displacement} for time: {effective_time}, stop time: {stop_time}" + ) return displacement - + def predict_ball_pos_at_x(self, x: float) -> Optional[tuple]: vel = self.get_ball_velocity() - + if not vel or not vel[0] or not vel[0]: return None - + ux, uy = vel pos = self.get_ball_pos()[0] bx = pos.x by = pos.y - - if (uy == 0): + + if uy == 0: return (bx, by) - + t = (x - bx) / ux y = by + uy * t return (x, y) @@ -364,7 +403,11 @@ def _get_object_velocity_at_frame( # 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", time_received, previous_time_received) + logger.warning( + "Timestamps out of order for vision data %f should be after %f", + time_received, + previous_time_received, + ) return None dt_secs = time_received - previous_time_received @@ -381,7 +424,7 @@ def get_object_acceleration(self, object: GameObject) -> Optional[tuple]: N_WINDOWS = 3 iter = 0 missing_velocities = 0 - + if len(self._records) < WINDOW * N_WINDOWS + 1: return None @@ -389,7 +432,7 @@ def get_object_acceleration(self, object: GameObject) -> Optional[tuple]: missing_velocities = 0 averageVelocity = [0, 0] windowStart = 1 + (i * WINDOW) - windowEnd = windowStart + WINDOW # Excluded + windowEnd = windowStart + WINDOW # Excluded windowMiddle = (windowStart + windowEnd) // 2 for j in range(windowStart, windowEnd): @@ -401,7 +444,7 @@ def get_object_acceleration(self, object: GameObject) -> Optional[tuple]: averageVelocity[0] += curr_vel[0] averageVelocity[1] += curr_vel[1] else: - missing_velocities += 1 + missing_velocities += 1 averageVelocity[0] /= WINDOW - missing_velocities averageVelocity[1] /= WINDOW - missing_velocities @@ -411,7 +454,9 @@ def get_object_acceleration(self, object: GameObject) -> Optional[tuple]: self._records[-windowMiddle + WINDOW].ts - self._records[-windowMiddle].ts ) - accelX = (futureAverageVelocity[0] - averageVelocity[0]) / dt # TODO vec + accelX = ( + futureAverageVelocity[0] - averageVelocity[0] + ) / dt # TODO vec accelY = (futureAverageVelocity[1] - averageVelocity[1]) / dt totalX += accelX totalY += accelY @@ -421,9 +466,20 @@ def get_object_acceleration(self, object: GameObject) -> Optional[tuple]: return (totalX / iter, totalY / iter) + def add_new_referee_data(self, referee_data: RefereeData) -> None: + # This function only updates referee records when something changed. + + if not self._referee_records: + self._referee_records.append(referee_data) + + # TODO: investigate potential namedtuple __eq__ issue + if referee_data[1:] != self._referee_records[-1][1:]: + self._referee_records.append(referee_data) + + if __name__ == "__main__": logging.basicConfig(level=logging.INFO) - + game = Game() print(game.ball.x) print(game.ball.y) diff --git a/main.py b/main.py index e93b744e..35179093 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,28 +36,33 @@ def main(): time.sleep(0.2) message_queue = queue.SimpleQueue() - 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() + referee_receiver = RefereeMessageReceiver(message_queue, debug=False) + vision_receiver = VisionDataReceiver(message_queue, debug=False) + decision_maker = StartUpController(game, debug=False) - TIME = 1/60 * 10 # frames in seconds - FRAMES_IN_TIME = round(60 * TIME) + # 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) - # TODO: Not implemented - # referee_thread = threading.Thread(target=referee_receiver.pull_referee_data) - # referee_thread.daemon = True - # referee_thread.start() + # 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) frames = 0 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 @@ -64,36 +70,30 @@ def main(): if message_type == MessageType.VISION: frames += 1 game.add_new_state(message) - + 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 @@ -110,45 +110,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) @@ -168,45 +172,58 @@ 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" + ) + 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}") - - + print(f"Robot {i} has ball: {game.friendly_robots[i].has_ball}") + ### 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") - - # to just demonstrate the print statements - break + + # to just demonstrate the print statements + break except KeyboardInterrupt: print("Stopping main program.") + if __name__ == "__main__": main1() diff --git a/team_controller/src/data/referee_receiver.py b/team_controller/src/data/referee_receiver.py index 4267b96e..4d91d009 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 = [] @@ -149,6 +163,18 @@ def _update_data(self, referee_packet: Referee) -> None: ) # Convert microseconds to seconds self.yellow_info.parse_referee_packet(referee_packet.yellow) self.blue_info.parse_referee_packet(referee_packet.blue) + self._message_queue.put_nowait( + ( + MessageType.REF, + RefereeData( + self.time_received, + self.command, + self.stage, + self.yellow_info, + self.blue_info, + ), + ) + ) def check_new_message(self) -> bool: """ From e866f4a5633e08a3e387da6cfb055b9b7af2bb88 Mon Sep 17 00:00:00 2001 From: fred-huang122 Date: Thu, 28 Nov 2024 11:09:29 +0000 Subject: [PATCH 02/14] added comments --- entities/referee/referee_command.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/entities/referee/referee_command.py b/entities/referee/referee_command.py index 84ad94fd..2a8bca44 100644 --- a/entities/referee/referee_command.py +++ b/entities/referee/referee_command.py @@ -23,8 +23,8 @@ def from_id(command_id: int): 7: "PREPARE_PENALTY_BLUE", 8: "DIRECT_FREE_YELLOW", 9: "DIRECT_FREE_BLUE", - 10: "INDIRECT_FREE_YELLOW", - 11: "INDIRECT_FREE_BLUE", + 10: "INDIRECT_FREE_YELLOW", # depreceated + 11: "INDIRECT_FREE_BLUE", # depreceated 12: "TIMEOUT_YELLOW", 13: "TIMEOUT_BLUE", 14: "GOAL_YELLOW", From f0750094b028c9f1ab266a88427913327583a135 Mon Sep 17 00:00:00 2001 From: fred-huang122 Date: Thu, 28 Nov 2024 11:54:51 +0000 Subject: [PATCH 03/14] changed and deletedout dated functions --- team_controller/src/data/referee_receiver.py | 29 -------------------- 1 file changed, 29 deletions(-) diff --git a/team_controller/src/data/referee_receiver.py b/team_controller/src/data/referee_receiver.py index 4d91d009..5519dd32 100644 --- a/team_controller/src/data/referee_receiver.py +++ b/team_controller/src/data/referee_receiver.py @@ -176,23 +176,6 @@ def _update_data(self, referee_packet: Referee) -> None: ) ) - 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) - - if serialized_data != self.old_serialized_data: - self.referee.ParseFromString(data) - self.old_serialized_data = serialized_data - return True - return False - def check_new_command(self) -> bool: """ Check if a new command has been received and update the command history. @@ -200,7 +183,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: @@ -213,17 +195,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. From 496e9c1985cf1037b4be82ab125b85cf0c1fea2c Mon Sep 17 00:00:00 2001 From: fred-huang122 Date: Thu, 28 Nov 2024 11:55:11 +0000 Subject: [PATCH 04/14] updated files --- team_controller/src/utils/network_utils.py | 1 + 1 file changed, 1 insertion(+) diff --git a/team_controller/src/utils/network_utils.py b/team_controller/src/utils/network_utils.py index 3ff361d4..1e74cf5f 100644 --- a/team_controller/src/utils/network_utils.py +++ b/team_controller/src/utils/network_utils.py @@ -41,6 +41,7 @@ def setup_socket( # 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, From aadc395282bd8982b69c0f49ac04199728ebd3b1 Mon Sep 17 00:00:00 2001 From: isaac0804 Date: Thu, 5 Dec 2024 19:23:11 +0000 Subject: [PATCH 05/14] More referee data and team info, function transfer --- .gitignore | 2 +- entities/data/referee.py | 56 +++++++++- entities/game/team_info.py | 52 ++++++++- main.py | 2 + team_controller/src/data/referee_receiver.py | 112 ++++++------------- 5 files changed, 139 insertions(+), 85 deletions(-) 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/entities/data/referee.py b/entities/data/referee.py index 21c6f06b..f9e09c70 100644 --- a/entities/data/referee.py +++ b/entities/data/referee.py @@ -1,7 +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 -RefereeData = namedtuple( - "RefereeData", - ["t", "referee_command", "stage", "blue_team_info", "yellow_team_info"], -) + +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/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/main.py b/main.py index 35179093..3985b294 100644 --- a/main.py +++ b/main.py @@ -1,5 +1,6 @@ import threading import queue +import time from entities.game import Game import time import math @@ -57,6 +58,7 @@ def main(): FRAMES_IN_TIME = round(60 * TIME) frames = 0 + begin = time.time() try: logger.debug("LOCATED BALL") logger.debug( diff --git a/team_controller/src/data/referee_receiver.py b/team_controller/src/data/referee_receiver.py index 5519dd32..cd5a15af 100644 --- a/team_controller/src/data/referee_receiver.py +++ b/team_controller/src/data/referee_receiver.py @@ -50,7 +50,7 @@ def __init__( # 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 @@ -151,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 = ( @@ -163,19 +163,42 @@ def _update_data(self, referee_packet: Referee) -> None: ) # Convert microseconds to seconds self.yellow_info.parse_referee_packet(referee_packet.yellow) self.blue_info.parse_referee_packet(referee_packet.blue) - self._message_queue.put_nowait( - ( - MessageType.REF, - RefereeData( - self.time_received, - self.command, - self.stage, - self.yellow_info, - self.blue_info, - ), + + # 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, ) + + # 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: """ Check if a new command has been received and update the command history. @@ -205,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. @@ -298,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. From 7a7a50f2a05a75cc7044c6398eb63563fd4bcc82 Mon Sep 17 00:00:00 2001 From: isaac0804 Date: Thu, 5 Dec 2024 19:34:24 +0000 Subject: [PATCH 06/14] More stage checking functions --- entities/game/game.py | 186 ++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 186 insertions(+) diff --git a/entities/game/game.py b/entities/game/game.py index 2a046ce4..e0932b01 100644 --- a/entities/game/game.py +++ b/entities/game/game.py @@ -10,6 +10,8 @@ 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 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 @@ -476,6 +478,190 @@ def add_new_referee_data(self, referee_data: RefereeData) -> None: if referee_data[1:] != self._referee_records[-1][1:]: self._referee_records.append(referee_data) + @property + def source_identifier(self) -> Optional[str]: + """Get the source identifier.""" + if self._referee_records: + return self._referee_records[-1].source_identifier + return None + + @property + def last_time_sent(self) -> float: + """Get the time sent.""" + if self._referee_records: + return self._referee_records[-1].time_sent + return 0.0 + + @property + def last_time_received(self) -> float: + """Get the time received.""" + if self._referee_records: + return self._referee_records[-1].time_received + return 0.0 + + @property + def last_command(self) -> RefereeCommand: + """Get the last command.""" + if self._referee_records: + return self._referee_records[-1].referee_command + return RefereeCommand.HALT + + @property + def last_command_timestamp(self) -> float: + """Get the command timestamp.""" + if self._referee_records: + return self._referee_records[-1].referee_command_timestamp + return 0.0 + + @property + def stage(self) -> Stage: + """Get the current stage.""" + if self._referee_records: + return self._referee_records[-1].stage + return Stage.NORMAL_FIRST_HALF_PRE + + @property + def stage_time_left(self) -> float: + """Get the time left in the current stage.""" + if self._referee_records: + return self._referee_records[-1].stage_time_left + return 0.0 + + @property + def blue_team(self) -> TeamInfo: + """Get the blue team info.""" + if self._referee_records: + return self._referee_records[-1].blue_team + return 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: + """Get the yellow team info.""" + if self._referee_records: + return self._referee_records[-1].yellow_team + return 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]]: + """Get the designated position.""" + if self._referee_records: + return self._referee_records[-1].designated_position + return None + + @property + def blue_team_on_positive_half(self) -> Optional[bool]: + """Get the blue team on positive half.""" + if self._referee_records: + return self._referee_records[-1].blue_team_on_positive_half + return None + + @property + def next_command(self) -> Optional[RefereeCommand]: + """Get the next command.""" + if self._referee_records: + return self._referee_records[-1].next_command + return None + + @property + def current_action_time_remaining(self) -> Optional[int]: + """Get the current action time remaining.""" + if self._referee_records: + return self._referee_records[-1].current_action_time_remaining + return None + + def is_halt(self) -> bool: + """Check if the command is HALT.""" + return self.referee_data_handler.last_command == RefereeCommand.HALT + + def is_stop(self) -> bool: + """Check if the command is STOP.""" + return self.referee_data_handler.last_command == RefereeCommand.STOP + + def is_normal_start(self) -> bool: + """Check if the command is NORMAL_START.""" + return self.referee_data_handler.last_command == RefereeCommand.NORMAL_START + + def is_force_start(self) -> bool: + """Check if the command is FORCE_START.""" + return self.referee_data_handler.last_command == RefereeCommand.FORCE_START + + def is_prepare_kickoff_yellow(self) -> bool: + """Check if the command is PREPARE_KICKOFF_YELLOW.""" + return ( + self.referee_data_handler.last_command + == RefereeCommand.PREPARE_KICKOFF_YELLOW + ) + + def is_prepare_kickoff_blue(self) -> bool: + """Check if the command is PREPARE_KICKOFF_BLUE.""" + return ( + self.referee_data_handler.last_command + == RefereeCommand.PREPARE_KICKOFF_BLUE + ) + + def is_prepare_penalty_yellow(self) -> bool: + """Check if the command is PREPARE_PENALTY_YELLOW.""" + return ( + self.referee_data_handler.last_command + == RefereeCommand.PREPARE_PENALTY_YELLOW + ) + + def is_prepare_penalty_blue(self) -> bool: + """Check if the command is PREPARE_PENALTY_BLUE.""" + return ( + self.referee_data_handler.last_command + == RefereeCommand.PREPARE_PENALTY_BLUE + ) + + def is_direct_free_yellow(self) -> bool: + """Check if the command is DIRECT_FREE_YELLOW.""" + return ( + self.referee_data_handler.last_command == RefereeCommand.DIRECT_FREE_YELLOW + ) + + def is_direct_free_blue(self) -> bool: + """Check if the command is DIRECT_FREE_BLUE.""" + return self.referee_data_handler.last_command == RefereeCommand.DIRECT_FREE_BLUE + + def is_timeout_yellow(self) -> bool: + """Check if the command is TIMEOUT_YELLOW.""" + return self.referee_data_handler.last_command == RefereeCommand.TIMEOUT_YELLOW + + def is_timeout_blue(self) -> bool: + """Check if the command is TIMEOUT_BLUE.""" + return self.referee_data_handler.last_command == RefereeCommand.TIMEOUT_BLUE + + def is_ball_placement_yellow(self) -> bool: + """Check if the command is BALL_PLACEMENT_YELLOW.""" + return ( + self.referee_data_handler.last_command + == RefereeCommand.BALL_PLACEMENT_YELLOW + ) + + def is_ball_placement_blue(self) -> bool: + """Check if the command is BALL_PLACEMENT_BLUE.""" + return ( + self.referee_data_handler.last_command == RefereeCommand.BALL_PLACEMENT_BLUE + ) + if __name__ == "__main__": logging.basicConfig(level=logging.INFO) From 383bfd5490415abe9a952db0aaa92957eb831a15 Mon Sep 17 00:00:00 2001 From: isaac0804 Date: Fri, 6 Dec 2024 10:33:21 +0000 Subject: [PATCH 07/14] Change RefereeCommand and Stage to enum class --- entities/game/game.py | 216 +++++++++++++++--- entities/referee/referee_command.py | 59 +++-- entities/referee/stage.py | 57 ++--- main.py | 22 +- start_test_env.sh | 4 +- .../grsim_robot_controller_startup_test.py | 3 + 6 files changed, 268 insertions(+), 93 deletions(-) diff --git a/entities/game/game.py b/entities/game/game.py index e0932b01..fed4f166 100644 --- a/entities/game/game.py +++ b/entities/game/game.py @@ -3,6 +3,7 @@ from entities.game.field import Field from entities.data.vision import FrameData, RobotData, BallData, PredictedFrame from entities.data.referee import RefereeData +<<<<<<< HEAD from entities.data.command import RobotInfo from entities.game.game_object import Colour, GameObject, Robot @@ -12,6 +13,13 @@ from entities.game.team_info import TeamInfo from entities.referee.referee_command import RefereeCommand +======= +from entities.game.game_object import Ball, Colour, GameObject, Robot +from entities.game.team_info import TeamInfo +from entities.referee.referee_command import RefereeCommand +from entities.referee.stage import Stage + +>>>>>>> 31d8bcc (Change RefereeCommand and Stage to enum class) 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 @@ -478,7 +486,6 @@ def add_new_referee_data(self, referee_data: RefereeData) -> None: if referee_data[1:] != self._referee_records[-1][1:]: self._referee_records.append(referee_data) - @property def source_identifier(self) -> Optional[str]: """Get the source identifier.""" if self._referee_records: @@ -587,78 +594,202 @@ def current_action_time_remaining(self) -> Optional[int]: return self._referee_records[-1].current_action_time_remaining return None + @property def is_halt(self) -> bool: """Check if the command is HALT.""" - return self.referee_data_handler.last_command == RefereeCommand.HALT + return self.last_command == RefereeCommand.HALT + @property def is_stop(self) -> bool: """Check if the command is STOP.""" - return self.referee_data_handler.last_command == RefereeCommand.STOP + return self.last_command == RefereeCommand.STOP + @property def is_normal_start(self) -> bool: """Check if the command is NORMAL_START.""" - return self.referee_data_handler.last_command == RefereeCommand.NORMAL_START + return self.last_command == RefereeCommand.NORMAL_START + @property def is_force_start(self) -> bool: """Check if the command is FORCE_START.""" - return self.referee_data_handler.last_command == RefereeCommand.FORCE_START + return self.last_command == RefereeCommand.FORCE_START + @property def is_prepare_kickoff_yellow(self) -> bool: """Check if the command is PREPARE_KICKOFF_YELLOW.""" - return ( - self.referee_data_handler.last_command - == RefereeCommand.PREPARE_KICKOFF_YELLOW - ) + return self.last_command == RefereeCommand.PREPARE_KICKOFF_YELLOW + @property def is_prepare_kickoff_blue(self) -> bool: """Check if the command is PREPARE_KICKOFF_BLUE.""" - return ( - self.referee_data_handler.last_command - == RefereeCommand.PREPARE_KICKOFF_BLUE - ) + return self.last_command == RefereeCommand.PREPARE_KICKOFF_BLUE + @property def is_prepare_penalty_yellow(self) -> bool: """Check if the command is PREPARE_PENALTY_YELLOW.""" - return ( - self.referee_data_handler.last_command - == RefereeCommand.PREPARE_PENALTY_YELLOW - ) + return self.last_command == RefereeCommand.PREPARE_PENALTY_YELLOW + @property def is_prepare_penalty_blue(self) -> bool: """Check if the command is PREPARE_PENALTY_BLUE.""" - return ( - self.referee_data_handler.last_command - == RefereeCommand.PREPARE_PENALTY_BLUE - ) + return self.last_command == RefereeCommand.PREPARE_PENALTY_BLUE + @property def is_direct_free_yellow(self) -> bool: """Check if the command is DIRECT_FREE_YELLOW.""" - return ( - self.referee_data_handler.last_command == RefereeCommand.DIRECT_FREE_YELLOW - ) + return self.last_command == RefereeCommand.DIRECT_FREE_YELLOW + @property def is_direct_free_blue(self) -> bool: """Check if the command is DIRECT_FREE_BLUE.""" - return self.referee_data_handler.last_command == RefereeCommand.DIRECT_FREE_BLUE + return self.last_command == RefereeCommand.DIRECT_FREE_BLUE + @property def is_timeout_yellow(self) -> bool: """Check if the command is TIMEOUT_YELLOW.""" - return self.referee_data_handler.last_command == RefereeCommand.TIMEOUT_YELLOW + return self.last_command == RefereeCommand.TIMEOUT_YELLOW + @property def is_timeout_blue(self) -> bool: """Check if the command is TIMEOUT_BLUE.""" - return self.referee_data_handler.last_command == RefereeCommand.TIMEOUT_BLUE + return self.last_command == RefereeCommand.TIMEOUT_BLUE + @property def is_ball_placement_yellow(self) -> bool: """Check if the command is BALL_PLACEMENT_YELLOW.""" - return ( - self.referee_data_handler.last_command - == RefereeCommand.BALL_PLACEMENT_YELLOW - ) + return self.last_command == RefereeCommand.BALL_PLACEMENT_YELLOW + @property def is_ball_placement_blue(self) -> bool: """Check if the command is BALL_PLACEMENT_BLUE.""" + return self.last_command == RefereeCommand.BALL_PLACEMENT_BLUE + + def get_object_velocity(self, object: GameObject): + 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 + elif isinstance(object, Robot): + if object.colour == Colour.YELLOW: + return self._records[frame].yellow_robots[object.id] + else: + return 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: + # Cannot provide velocity at frame that does not exist + print(frame) + return None + + # Otherwise get the previous and current frames + previous_frame = self._records[frame - 1] + current_frame = self._records[frame] + + previous_pos = self._get_object_position_at_frame(frame - 1, object) + current_pos = self._get_object_position_at_frame(frame, object) + + 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: + # TODO log a warning + print("Timestamps out of order for vision data ") + return None + + dt_secs = time_received - previous_time_received + + vx = (current_pos.x - previous_pos.x) / dt_secs + vy = (current_pos.y - previous_pos.y) / dt_secs + + return (vx, vy) + + def get_object_acceleration(self, object: GameObject) -> Optional[tuple]: + totalX = 0 + totalY = 0 + WINDOW = 5 + N_WINDOWS = 6 + iter = 0 + + if len(self._records) < WINDOW * N_WINDOWS + 1: + return None + + for i in range(N_WINDOWS): + averageVelocity = [0, 0] + windowStart = 1 + i * WINDOW + windowEnd = windowStart + WINDOW # Excluded + windowMiddle = (windowStart + windowEnd) // 2 + + for j in range(windowStart, windowEnd): + curr_vel = self._get_object_velocity_at_frame( + len(self._records) - j, object + ) + averageVelocity[0] += curr_vel[0] + averageVelocity[1] += curr_vel[1] + + averageVelocity[0] /= WINDOW + averageVelocity[1] /= WINDOW + + if i != 0: + dt = ( + self._records[-windowMiddle + WINDOW].ts + - self._records[-windowMiddle].ts + ) + accX = (futureAverageVelocity[0] - averageVelocity[0]) / dt # TODO vec + accY = (futureAverageVelocity[1] - averageVelocity[1]) / dt + totalX += accX + totalY += accY + iter += 1 + + futureAverageVelocity = tuple(averageVelocity) + + return (totalX / iter, totalY / iter) + + def predict_object_pos_after(self, t: float, object: GameObject) -> Optional[tuple]: + # If t is after the object has stopped we return the position at which object stopped. + + acc = self.get_object_acceleration(object) + + if acc is None: + return None + + ax, ay = acc + ux, uy = self.get_object_velocity(object) + + if object is Ball: + ball = self.get_ball_pos() + start_x, start_y = ball[0].x, ball[0].y + else: + posn = self._get_object_position_at_frame(len(self._records) - 1, object) + start_x, start_y = posn.x, posn.y + + if ax == 0: # Due to friction, if acc = 0 then stopped. + sx = 0 # TODO: Not sure what to do about robots with respect to friction - we never know if they are slowing down to stop or if they are slowing down to change direction + else: + tx_stop = -ux / ax + tx = min(t, tx_stop) + sx = ux * tx + 0.5 * ax * tx * tx + + if ay == 0: + sy = 0 + else: + ty_stop = -uy / ay + ty = min(t, ty_stop) + sy = uy * ty + 0.5 * ay * ty * ty + return ( +<<<<<<< HEAD self.referee_data_handler.last_command == RefereeCommand.BALL_PLACEMENT_BLUE ) @@ -674,3 +805,26 @@ def is_ball_placement_blue(self) -> bool: print(game.ball.x) print(game.ball.y) print(game.ball.z) +======= + start_x + sx, + start_y + sy, + ) # TODO: Doesn't take into account spin / angular vel + + def predict_frame_after(self, t: float): + yellow_pos = [ + self.predict_object_pos_after(t, Robot(Colour.YELLOW, i)) for i in range(6) + ] + blue_pos = [ + self.predict_object_pos_after(t, Robot(Colour.BLUE, i)) for i in range(6) + ] + ball_pos = self.predict_object_pos_after(t, Ball) + if ball_pos is None or None in yellow_pos or None in blue_pos: + return None + else: + return FrameData( + self._records[-1].ts + t, + list(map(lambda pos: RobotData(pos[0], pos[1], 0), yellow_pos)), + list(map(lambda pos: RobotData(pos[0], pos[1], 0), blue_pos)), + [BallData(ball_pos[0], ball_pos[1], 0)], # TODO : Support z axis + ) +>>>>>>> 31d8bcc (Change RefereeCommand and Stage to enum class) diff --git a/entities/referee/referee_command.py b/entities/referee/referee_command.py index 2a8bca44..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", # depreceated - 11: "INDIRECT_FREE_BLUE", # depreceated - 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 3985b294..b5441e87 100644 --- a/main.py +++ b/main.py @@ -32,7 +32,7 @@ def data_update_listener(receiver: VisionDataReceiver): def main(): - game = Game(my_team_is_yellow=True) + game = Game() GRSimController().teleport_ball(0, 0, 2, 2.5) time.sleep(0.2) @@ -54,8 +54,10 @@ def main(): vision_thread.start() referee_thread.start() - TIME = 1 / 60 * 10 # frames in seconds + TIME = 0.5 FRAMES_IN_TIME = round(60 * TIME) + +>>>>>>> 05bd7f6 (Change RefereeCommand and Stage to enum class) frames = 0 begin = time.time() @@ -72,14 +74,15 @@ 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 ): +<<<<<<< HEAD logger.debug( "Ball prediction inaccuracy delta (cm): " - + "{:.5f}".format( + + "{:.20f}".format( 100 * math.sqrt( (game.ball.x - predictions[-FRAMES_IN_TIME].ball[0].x) @@ -87,6 +90,7 @@ def main(): + (game.ball.y - predictions[-FRAMES_IN_TIME].ball[0].y) ** 2 ) +<<<<<<< HEAD ) ) for i in range(6): @@ -135,11 +139,21 @@ def main(): ) ) ) +======= + ), + ) + # for i in range(6): + # print(f"Blue robot {i} prediction inaccuracy delta (cm): ", '{:.20f}'.format(100 * math.sqrt((actual.blue_robots[i].x - predictions[-FRAMES_IN_TIME].blue_robots[i].x)**2 + (actual.blue_robots[i].y - predictions[-FRAMES_IN_TIME].blue_robots[i].y)**2))) + # for i in range(6): + # print(f"Yellow robot {i} prediction inaccuracy delta (cm): ", '{:.20f}'.format(100 * math.sqrt((actual.yellow_robots[i].x - predictions[-FRAMES_IN_TIME].yellow_robots[i].x)**2 + (actual.yellow_robots[i].y - predictions[-FRAMES_IN_TIME].yellow_robots[i].y)**2))) +>>>>>>> 05bd7f6 (Change RefereeCommand and Stage to enum class) predictions.append(game.predicted_next_frame) elif message_type == MessageType.REF: game.add_new_referee_data(message) + print(game.last_command) + print(game.stage) decision_maker.make_decision() diff --git a/start_test_env.sh b/start_test_env.sh index 682b2922..6187acc2 100755 --- a/start_test_env.sh +++ b/start_test_env.sh @@ -23,9 +23,9 @@ cd .. # Change to the AutoReferee directory and run the run.sh script cd AutoReferee/ -./run.sh & +./run.sh -a & AUTOREFEREE_PID=$! cd .. # Wait for all background processes to finish -wait \ No newline at end of file +wait 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..07e984e3 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 == "HALT": + continue self.sim_robot_controller.add_robot_commands(command, robot_id) logger.debug(out_packet) From d4bdaad9efeb06b6d7b307018419909b6ff3bfb9 Mon Sep 17 00:00:00 2001 From: isaac0804 Date: Fri, 6 Dec 2024 14:24:47 +0000 Subject: [PATCH 08/14] Modify start test env script --- main.py | 24 +----- start_test_env.sh | 73 +++++++++++++++++-- .../grsim_robot_controller_startup_test.py | 2 +- 3 files changed, 70 insertions(+), 29 deletions(-) diff --git a/main.py b/main.py index b5441e87..35179093 100644 --- a/main.py +++ b/main.py @@ -1,6 +1,5 @@ import threading import queue -import time from entities.game import Game import time import math @@ -32,7 +31,7 @@ def data_update_listener(receiver: VisionDataReceiver): def main(): - game = Game() + game = Game(my_team_is_yellow=True) GRSimController().teleport_ball(0, 0, 2, 2.5) time.sleep(0.2) @@ -54,13 +53,10 @@ def main(): vision_thread.start() referee_thread.start() - TIME = 0.5 + TIME = 1 / 60 * 10 # frames in seconds FRAMES_IN_TIME = round(60 * TIME) - ->>>>>>> 05bd7f6 (Change RefereeCommand and Stage to enum class) frames = 0 - begin = time.time() try: logger.debug("LOCATED BALL") logger.debug( @@ -74,15 +70,14 @@ 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 ): -<<<<<<< HEAD logger.debug( "Ball prediction inaccuracy delta (cm): " - + "{:.20f}".format( + + "{:.5f}".format( 100 * math.sqrt( (game.ball.x - predictions[-FRAMES_IN_TIME].ball[0].x) @@ -90,7 +85,6 @@ def main(): + (game.ball.y - predictions[-FRAMES_IN_TIME].ball[0].y) ** 2 ) -<<<<<<< HEAD ) ) for i in range(6): @@ -139,21 +133,11 @@ def main(): ) ) ) -======= - ), - ) - # for i in range(6): - # print(f"Blue robot {i} prediction inaccuracy delta (cm): ", '{:.20f}'.format(100 * math.sqrt((actual.blue_robots[i].x - predictions[-FRAMES_IN_TIME].blue_robots[i].x)**2 + (actual.blue_robots[i].y - predictions[-FRAMES_IN_TIME].blue_robots[i].y)**2))) - # for i in range(6): - # print(f"Yellow robot {i} prediction inaccuracy delta (cm): ", '{:.20f}'.format(100 * math.sqrt((actual.yellow_robots[i].x - predictions[-FRAMES_IN_TIME].yellow_robots[i].x)**2 + (actual.yellow_robots[i].y - predictions[-FRAMES_IN_TIME].yellow_robots[i].y)**2))) ->>>>>>> 05bd7f6 (Change RefereeCommand and Stage to enum class) predictions.append(game.predicted_next_frame) elif message_type == MessageType.REF: game.add_new_referee_data(message) - print(game.last_command) - print(game.stage) decision_maker.make_decision() diff --git a/start_test_env.sh b/start_test_env.sh index 6187acc2..86555792 100755 --- a/start_test_env.sh +++ b/start_test_env.sh @@ -3,29 +3,86 @@ # Function to handle cleanup on script exit cleanup() { echo "Caught SIGINT signal! Cleaning up..." - kill $GRSIM_PID $GAME_CONTROLLER_PID 2>/dev/null - pkill -f "AutoReferee" + + # Kill grSim, game controller, and AutoReferee processes if they exist + if [ ! -z "$GRSIM_PID" ]; then + echo "Stopping grSim..." + kill $GRSIM_PID 2>/dev/null + fi + + if [ ! -z "$GAME_CONTROLLER_PID" ]; then + echo "Stopping game controller..." + kill $GAME_CONTROLLER_PID 2>/dev/null + fi + + if [ ! -z "$AUTOREFEREE_PID" ]; then + echo "Stopping AutoReferee..." + kill $AUTOREFEREE_PID 2>/dev/null + fi + + echo "Cleanup complete. Exiting." exit } # Trap SIGINT (Ctrl+C) and call the cleanup function trap cleanup SIGINT -# Start grSim in the background -grSim & +# Open the website based on the operating system +echo "Opening website..." + +if [[ "$OSTYPE" == "linux-gnu"* ]]; then + # Linux + xdg-open "http://localhost:8081/#/match" & + +elif [[ "$OSTYPE" == "darwin"* ]]; then + # macOS + open "http://localhost:8081/#/match" & + +elif [[ "$OSTYPE" == "cygwin" || "$OSTYPE" == "msys" || "$OSTYPE" == "win32" ]]; then + # Windows + start "" "http://localhost:8081/#/match" & + +else + echo "Unsupported operating system. Cannot open the website." + exit 1 +fi + +# Start grSim in the background, suppressing output +echo "Starting grSim..." +grSim > /dev/null 2>&1 & GRSIM_PID=$! -# Change to the ssl-game-controller directory and run the game controller +# Check if grSim started successfully +if [ $? -ne 0 ]; then + echo "Failed to start grSim. Exiting." + exit 1 +fi + +# Change to the ssl-game-controller directory and run the game controller, suppressing output +echo "Starting game controller..." cd ssl-game-controller/ -./ssl-game-controller_v3.12.7_linux_amd64 & +./ssl-game-controller_v3.12.7_linux_amd64 > /dev/null 2>&1 & GAME_CONTROLLER_PID=$! cd .. -# Change to the AutoReferee directory and run the run.sh script +# Check if the game controller started successfully +if [ $? -ne 0 ]; then + echo "Failed to start game controller. Exiting." + cleanup +fi + +# Change to the AutoReferee directory and run the run.sh script, suppressing output +echo "Starting AutoReferee..." cd AutoReferee/ -./run.sh -a & +./gradlew run > /dev/null 2>&1 & AUTOREFEREE_PID=$! cd .. +# Check if AutoReferee started successfully +if [ $? -ne 0 ]; then + echo "Failed to start AutoReferee. Exiting." + cleanup +fi + # Wait for all background processes to finish wait 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 07e984e3..081072a3 100644 --- a/team_controller/src/tests/grsim_robot_controller_startup_test.py +++ b/team_controller/src/tests/grsim_robot_controller_startup_test.py @@ -53,7 +53,7 @@ def make_decision(self): robot_id, target_coords, robots, balls, face_ball=True ) - if self.game.last_command.name == "HALT": + if self.game.last_command.name in ["HALT", "STOP"]: continue self.sim_robot_controller.add_robot_commands(command, robot_id) From b40737abcbc7a1bd5344d7ef340309833d97bed0 Mon Sep 17 00:00:00 2001 From: isaac0804 Date: Mon, 13 Jan 2025 16:14:47 +0000 Subject: [PATCH 09/14] remove autokill test env script --- start_test_env.sh | 23 ++++------------------- 1 file changed, 4 insertions(+), 19 deletions(-) diff --git a/start_test_env.sh b/start_test_env.sh index 86555792..fdd5bdfd 100755 --- a/start_test_env.sh +++ b/start_test_env.sh @@ -27,25 +27,10 @@ cleanup() { # Trap SIGINT (Ctrl+C) and call the cleanup function trap cleanup SIGINT -# Open the website based on the operating system -echo "Opening website..." - -if [[ "$OSTYPE" == "linux-gnu"* ]]; then - # Linux - xdg-open "http://localhost:8081/#/match" & - -elif [[ "$OSTYPE" == "darwin"* ]]; then - # macOS - open "http://localhost:8081/#/match" & - -elif [[ "$OSTYPE" == "cygwin" || "$OSTYPE" == "msys" || "$OSTYPE" == "win32" ]]; then - # Windows - start "" "http://localhost:8081/#/match" & - -else - echo "Unsupported operating system. Cannot open the website." - exit 1 -fi +# Output reminder to open the website manually +echo "Reminder: Please open the following website in your browser:" +echo "http://localhost:8081/#/match" +echo "Once the website is opened, the script will continue..." # Start grSim in the background, suppressing output echo "Starting grSim..." From 6420083773f9921aa8479774c4ea75df61b59b75 Mon Sep 17 00:00:00 2001 From: isaac0804 Date: Tue, 14 Jan 2025 00:26:34 +0000 Subject: [PATCH 10/14] demo version --- main.py | 5 +++-- team_controller/src/utils/network_utils.py | 10 ++++++---- 2 files changed, 9 insertions(+), 6 deletions(-) diff --git a/main.py b/main.py index 35179093..4922b3b0 100644 --- a/main.py +++ b/main.py @@ -38,8 +38,8 @@ def main(): message_queue = queue.SimpleQueue() referee_receiver = RefereeMessageReceiver(message_queue, debug=False) - vision_receiver = VisionDataReceiver(message_queue, debug=False) - decision_maker = StartUpController(game, debug=False) + vision_receiver = VisionDataReceiver(message_queue) + decision_maker = StartUpController(game) # Start the data receiving in separate threads vision_thread = threading.Thread(target=vision_receiver.pull_game_data) @@ -71,6 +71,7 @@ def main(): 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 diff --git a/team_controller/src/utils/network_utils.py b/team_controller/src/utils/network_utils.py index 1e74cf5f..a7b52eb2 100644 --- a/team_controller/src/utils/network_utils.py +++ b/team_controller/src/utils/network_utils.py @@ -8,6 +8,7 @@ logger = logging.getLogger(__name__) + def setup_socket( sock: socket.socket, address: Tuple[str, int], bind_socket: bool = False ) -> socket.socket: @@ -40,7 +41,6 @@ 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, @@ -79,7 +79,9 @@ def receive_data(sock: socket.socket) -> Optional[bytes]: return 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,8 +91,8 @@ 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` From 68b473ca4f639e644ee93297ff4618269e7c3479 Mon Sep 17 00:00:00 2001 From: isaac0804 Date: Tue, 14 Jan 2025 00:53:25 +0000 Subject: [PATCH 11/14] Robot and RobotEntity in game.py breaking things --- entities/game/game.py | 39 ++++++++++++++------------------------- main.py | 4 ++-- 2 files changed, 16 insertions(+), 27 deletions(-) diff --git a/entities/game/game.py b/entities/game/game.py index fed4f166..02afefe0 100644 --- a/entities/game/game.py +++ b/entities/game/game.py @@ -3,7 +3,6 @@ from entities.game.field import Field from entities.data.vision import FrameData, RobotData, BallData, PredictedFrame from entities.data.referee import RefereeData -<<<<<<< HEAD from entities.data.command import RobotInfo from entities.game.game_object import Colour, GameObject, Robot @@ -11,15 +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.game.game_object import Ball, Colour, GameObject, Robot from entities.game.team_info import TeamInfo from entities.referee.referee_command import RefereeCommand from entities.referee.stage import Stage ->>>>>>> 31d8bcc (Change RefereeCommand and Stage to enum class) 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 @@ -670,7 +664,7 @@ def get_object_velocity(self, object: GameObject): 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 - elif isinstance(object, Robot): + elif isinstance(object, RobotEntity): if object.colour == Colour.YELLOW: return self._records[frame].yellow_robots[object.id] else: @@ -789,23 +783,6 @@ def predict_object_pos_after(self, t: float, object: GameObject) -> Optional[tup sy = uy * ty + 0.5 * ay * ty * ty return ( -<<<<<<< HEAD - self.referee_data_handler.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) -======= start_x + sx, start_y + sy, ) # TODO: Doesn't take into account spin / angular vel @@ -827,4 +804,16 @@ def predict_frame_after(self, t: float): list(map(lambda pos: RobotData(pos[0], pos[1], 0), blue_pos)), [BallData(ball_pos[0], ball_pos[1], 0)], # TODO : Support z axis ) ->>>>>>> 31d8bcc (Change RefereeCommand and Stage to enum class) + + +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/main.py b/main.py index 4922b3b0..7878c85e 100644 --- a/main.py +++ b/main.py @@ -160,7 +160,7 @@ def main1(): time.sleep(0.2) message_queue = queue.SimpleQueue() - receiver = VisionDataReceiver(message_queue, debug=False) + receiver = VisionDataReceiver(message_queue) # Start the data receiving in a separate thread data_thread = threading.Thread(target=data_update_listener, args=(receiver,)) @@ -227,4 +227,4 @@ def main1(): if __name__ == "__main__": - main1() + main() From 33b4956083397f0023aad2510824a804512befd1 Mon Sep 17 00:00:00 2001 From: isaac0804 Date: Tue, 14 Jan 2025 01:16:04 +0000 Subject: [PATCH 12/14] Fix main.py, demo runs now --- entities/game/game.py | 530 ++++++++++-------------------------------- main.py | 3 +- 2 files changed, 130 insertions(+), 403 deletions(-) diff --git a/entities/game/game.py b/entities/game/game.py index 02afefe0..00a4ea16 100644 --- a/entities/game/game.py +++ b/entities/game/game.py @@ -16,11 +16,8 @@ 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 - import logging, warnings -# Configure logging logger = logging.getLogger(__name__) @@ -58,15 +55,11 @@ 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: - return ( - self._yellow_score - ) # TODO, read directly from _referee_records or store as class variable? + return self._yellow_score @property def blue_score(self) -> int: @@ -87,7 +80,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 @@ -98,7 +90,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 @@ -107,7 +98,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): self._ball.ball_data = value @@ -128,7 +118,6 @@ def is_ball_in_goal(self, left_goal: bool): and not left_goal ) - ### Game state management ### def add_new_state(self, frame_data: FrameData) -> None: if isinstance(frame_data, FrameData): self._records.append(frame_data) @@ -142,7 +131,6 @@ def add_new_state(self, frame_data: FrameData) -> None: def add_robot_info(self, robots_info: List[RobotInfo]) -> None: for robot_id, robot_info in enumerate(robots_info): self._friendly_robots[robot_id].has_ball = robot_info.has_ball - # Extensible with more info (remeber to add the property in robot.py) def _update_data(self, frame_data: FrameData) -> None: if self.my_team_is_yellow: @@ -151,9 +139,8 @@ def _update_data(self, frame_data: FrameData) -> None: else: self.friendly_robots = frame_data.blue_robots self.enemy_robots = frame_data.yellow_robots - self._ball = frame_data.ball[0] # TODO: Don't always take first ball pos + self._ball = frame_data.ball[0] - ### Robot data retrieval ### def get_robots_pos(self, is_yellow: bool) -> List[RobotData]: if not self._records: return None @@ -170,26 +157,15 @@ 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: - # TODO: potential namespace conflict when robot (robot.py) entity is reintroduced. Think about integrating the two - return [ - self.get_object_velocity(RobotEntity(i, Colour.YELLOW)) - for i in range( - len(self.get_robots_pos(True)) - ) # TODO: This is a bit of a hack, we should be able to get the number of robots from the field - ] - else: - return [ - self.get_object_velocity(RobotEntity(i, Colour.BLUE)) - for i in range(len(self.get_robots_pos(False))) - ] + return [ + self.get_object_velocity( + RobotEntity(i, Colour.YELLOW if is_yellow else Colour.BLUE) + ) + for i in range(len(self.get_robots_pos(is_yellow))) + ] - ### Ball Data retrieval ### def get_ball_pos(self) -> List[BallData]: if not self._records: return None @@ -197,23 +173,14 @@ 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 ) -> tuple[RobotData, RobotData, BallData]: - """ - FrameData rearranged as Tuple(friendly_robots, enemy_robots, balls) based on provided _my_team_is_yellow field - """ if not self._records: return None latest_frame = self.get_latest_frame() @@ -225,28 +192,21 @@ 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( self, my_team_is_yellow: bool ) -> tuple[RobotData, RobotData, BallData]: - """ - FrameData rearranged as (friendly_robots, enemy_robots, balls) based on my_team_is_yellow - """ if self._predicted_next_frame is None: return None warnings.warn( "Use game.predicted_next_frame instead", DeprecationWarning, stacklevel=2 ) - return self._reorganise_frame_data(self._predicted_next_frame) + return self._reorganise_frame_data( + self._predicted_next_frame, my_team_is_yellow + ) 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))) @@ -263,139 +223,48 @@ def predict_frame_after(self, t: float) -> FrameData: self._records[-1].ts + t, list(map(lambda pos: RobotData(pos[0], pos[1], 0), yellow_pos)), list(map(lambda pos: RobotData(pos[0], pos[1], 0), blue_pos)), - [BallData(ball_pos[0], ball_pos[1], 0)], # TODO : Support z axis + [BallData(ball_pos[0], ball_pos[1], 0)], ) 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 - else: - return blue_robots, yellow_robots, balls - - ### General Object Position Prediction ### - def predict_object_pos_after(self, t: float, object: GameObject) -> Optional[tuple]: - # If t is after the object has stopped we return the position at which object stopped. - sx = 0 - sy = 0 - - acceleration = self.get_object_acceleration(object) - - if acceleration is None: - return None - - ax, ay = acceleration - vels = self.get_object_velocity(object) - - if vels is None: - ux, uy = None, None - else: - ux, uy = vels - - if object is Ball: - ball = self.get_ball_pos() - start_x, start_y = ball[0].x, ball[0].y - else: - posn = self._get_object_position_at_frame(len(self._records) - 1, object) - start_x, start_y = posn.x, posn.y - - if ax and ux: - sx = self._calculate_displacement(ux, ax, t) - - if ay and uy: - sy = self._calculate_displacement(uy, ay, t) - return ( - start_x + sx, - start_y + sy, - ) # TODO: Doesn't take into account spin / angular vel - - def _calculate_displacement(self, u, a, t): - if a == 0: # Handle zero acceleration case - return u * t - else: - stop_time = -u / a - effective_time = min(t, stop_time) - displacement = (u * effective_time) + (0.5 * a * effective_time**2) - logger.debug( - f"Displacement: {displacement} for time: {effective_time}, stop time: {stop_time}" - ) - return displacement - - def predict_ball_pos_at_x(self, x: float) -> Optional[tuple]: - vel = self.get_ball_velocity() - - if not vel or not vel[0] or not vel[0]: - return None - - ux, uy = vel - pos = self.get_ball_pos()[0] - bx = pos.x - by = pos.y - - if uy == 0: - return (bx, by) - - t = (x - bx) / ux - y = by + uy * t - return (x, y) + (yellow_robots, blue_robots, balls) + if my_team_is_yellow + else (blue_robots, yellow_robots, balls) + ) 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] @@ -405,13 +274,8 @@ 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", - time_received, - previous_time_received, - ) + logger.warning("Timestamps out of order for vision data") return None dt_secs = time_received - previous_time_received @@ -436,11 +300,10 @@ def get_object_acceleration(self, object: GameObject) -> Optional[tuple]: missing_velocities = 0 averageVelocity = [0, 0] windowStart = 1 + (i * WINDOW) - windowEnd = windowStart + WINDOW # Excluded + windowEnd = windowStart + WINDOW 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 ) @@ -458,9 +321,7 @@ def get_object_acceleration(self, object: GameObject) -> Optional[tuple]: self._records[-windowMiddle + WINDOW].ts - self._records[-windowMiddle].ts ) - accelX = ( - futureAverageVelocity[0] - averageVelocity[0] - ) / dt # TODO vec + accelX = (futureAverageVelocity[0] - averageVelocity[0]) / dt accelY = (futureAverageVelocity[1] - averageVelocity[1]) / dt totalX += accelX totalY += accelY @@ -470,341 +331,206 @@ def get_object_acceleration(self, object: GameObject) -> Optional[tuple]: return (totalX / iter, totalY / iter) - def add_new_referee_data(self, referee_data: RefereeData) -> None: - # This function only updates referee records when something changed. + def predict_object_pos_after(self, t: float, object: GameObject) -> Optional[tuple]: + acc = self.get_object_acceleration(object) + if acc is None: + return None + ax, ay = acc + ux, uy = self.get_object_velocity(object) + + if object is Ball: + ball = self.get_ball_pos() + start_x, start_y = ball[0].x, ball[0].y + else: + posn = self._get_object_position_at_frame(len(self._records) - 1, object) + start_x, start_y = posn.x, posn.y + + if ax == 0: + sx = 0 + else: + tx_stop = -ux / ax + tx = min(t, tx_stop) + sx = ux * tx + 0.5 * ax * tx * tx + + if ay == 0: + sy = 0 + else: + ty_stop = -uy / ay + ty = min(t, ty_stop) + sy = uy * ty + 0.5 * ay * ty * ty + + return (start_x + sx, start_y + sy) + + def add_new_referee_data(self, referee_data: RefereeData) -> None: if not self._referee_records: self._referee_records.append(referee_data) - - # TODO: investigate potential namedtuple __eq__ issue - if referee_data[1:] != self._referee_records[-1][1:]: + elif referee_data[1:] != self._referee_records[-1][1:]: self._referee_records.append(referee_data) def source_identifier(self) -> Optional[str]: - """Get the source identifier.""" - if self._referee_records: - return self._referee_records[-1].source_identifier - return None + return ( + self._referee_records[-1].source_identifier + if self._referee_records + else None + ) @property def last_time_sent(self) -> float: - """Get the time sent.""" - if self._referee_records: - return self._referee_records[-1].time_sent - return 0.0 + return self._referee_records[-1].time_sent if self._referee_records else 0.0 @property def last_time_received(self) -> float: - """Get the time received.""" - if self._referee_records: - return self._referee_records[-1].time_received - return 0.0 + return self._referee_records[-1].time_received if self._referee_records else 0.0 @property def last_command(self) -> RefereeCommand: - """Get the last command.""" - if self._referee_records: - return self._referee_records[-1].referee_command - return RefereeCommand.HALT + return ( + self._referee_records[-1].referee_command + if self._referee_records + else RefereeCommand.HALT + ) @property def last_command_timestamp(self) -> float: - """Get the command timestamp.""" - if self._referee_records: - return self._referee_records[-1].referee_command_timestamp - return 0.0 + return ( + self._referee_records[-1].referee_command_timestamp + if self._referee_records + else 0.0 + ) @property def stage(self) -> Stage: - """Get the current stage.""" - if self._referee_records: - return self._referee_records[-1].stage - return Stage.NORMAL_FIRST_HALF_PRE + return ( + self._referee_records[-1].stage + if self._referee_records + else Stage.NORMAL_FIRST_HALF_PRE + ) @property def stage_time_left(self) -> float: - """Get the time left in the current stage.""" - if self._referee_records: - return self._referee_records[-1].stage_time_left - return 0.0 + return ( + self._referee_records[-1].stage_time_left if self._referee_records else 0.0 + ) @property def blue_team(self) -> TeamInfo: - """Get the blue team info.""" - if self._referee_records: - return self._referee_records[-1].blue_team - return TeamInfo( - name="", - score=0, - red_cards=0, - yellow_card_times=[], - yellow_cards=0, - timeouts=0, - timeout_time=0, - goalkeeper=0, + 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: - """Get the yellow team info.""" - if self._referee_records: - return self._referee_records[-1].yellow_team - return TeamInfo( - name="", - score=0, - red_cards=0, - yellow_card_times=[], - yellow_cards=0, - timeouts=0, - timeout_time=0, - goalkeeper=0, + 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]]: - """Get the designated position.""" - if self._referee_records: - return self._referee_records[-1].designated_position - return None + return ( + self._referee_records[-1].designated_position + if self._referee_records + else None + ) @property def blue_team_on_positive_half(self) -> Optional[bool]: - """Get the blue team on positive half.""" - if self._referee_records: - return self._referee_records[-1].blue_team_on_positive_half - return None + return ( + self._referee_records[-1].blue_team_on_positive_half + if self._referee_records + else None + ) @property def next_command(self) -> Optional[RefereeCommand]: - """Get the next command.""" - if self._referee_records: - return self._referee_records[-1].next_command - return None + return self._referee_records[-1].next_command if self._referee_records else None @property def current_action_time_remaining(self) -> Optional[int]: - """Get the current action time remaining.""" - if self._referee_records: - return self._referee_records[-1].current_action_time_remaining - return None + return ( + self._referee_records[-1].current_action_time_remaining + if self._referee_records + else None + ) @property def is_halt(self) -> bool: - """Check if the command is HALT.""" return self.last_command == RefereeCommand.HALT @property def is_stop(self) -> bool: - """Check if the command is STOP.""" return self.last_command == RefereeCommand.STOP @property def is_normal_start(self) -> bool: - """Check if the command is NORMAL_START.""" return self.last_command == RefereeCommand.NORMAL_START @property def is_force_start(self) -> bool: - """Check if the command is FORCE_START.""" return self.last_command == RefereeCommand.FORCE_START @property def is_prepare_kickoff_yellow(self) -> bool: - """Check if the command is PREPARE_KICKOFF_YELLOW.""" return self.last_command == RefereeCommand.PREPARE_KICKOFF_YELLOW @property def is_prepare_kickoff_blue(self) -> bool: - """Check if the command is PREPARE_KICKOFF_BLUE.""" return self.last_command == RefereeCommand.PREPARE_KICKOFF_BLUE @property def is_prepare_penalty_yellow(self) -> bool: - """Check if the command is PREPARE_PENALTY_YELLOW.""" return self.last_command == RefereeCommand.PREPARE_PENALTY_YELLOW @property def is_prepare_penalty_blue(self) -> bool: - """Check if the command is PREPARE_PENALTY_BLUE.""" return self.last_command == RefereeCommand.PREPARE_PENALTY_BLUE @property def is_direct_free_yellow(self) -> bool: - """Check if the command is DIRECT_FREE_YELLOW.""" return self.last_command == RefereeCommand.DIRECT_FREE_YELLOW @property def is_direct_free_blue(self) -> bool: - """Check if the command is DIRECT_FREE_BLUE.""" return self.last_command == RefereeCommand.DIRECT_FREE_BLUE @property def is_timeout_yellow(self) -> bool: - """Check if the command is TIMEOUT_YELLOW.""" return self.last_command == RefereeCommand.TIMEOUT_YELLOW @property def is_timeout_blue(self) -> bool: - """Check if the command is TIMEOUT_BLUE.""" return self.last_command == RefereeCommand.TIMEOUT_BLUE @property def is_ball_placement_yellow(self) -> bool: - """Check if the command is BALL_PLACEMENT_YELLOW.""" return self.last_command == RefereeCommand.BALL_PLACEMENT_YELLOW @property def is_ball_placement_blue(self) -> bool: - """Check if the command is BALL_PLACEMENT_BLUE.""" return self.last_command == RefereeCommand.BALL_PLACEMENT_BLUE - def get_object_velocity(self, object: GameObject): - 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 - 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] - - 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: - # Cannot provide velocity at frame that does not exist - print(frame) - return None - - # Otherwise get the previous and current frames - previous_frame = self._records[frame - 1] - current_frame = self._records[frame] - - previous_pos = self._get_object_position_at_frame(frame - 1, object) - current_pos = self._get_object_position_at_frame(frame, object) - - 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: - # TODO log a warning - print("Timestamps out of order for vision data ") - return None - - dt_secs = time_received - previous_time_received - - vx = (current_pos.x - previous_pos.x) / dt_secs - vy = (current_pos.y - previous_pos.y) / dt_secs - - return (vx, vy) - - def get_object_acceleration(self, object: GameObject) -> Optional[tuple]: - totalX = 0 - totalY = 0 - WINDOW = 5 - N_WINDOWS = 6 - iter = 0 - - if len(self._records) < WINDOW * N_WINDOWS + 1: - return None - - for i in range(N_WINDOWS): - averageVelocity = [0, 0] - windowStart = 1 + i * WINDOW - windowEnd = windowStart + WINDOW # Excluded - windowMiddle = (windowStart + windowEnd) // 2 - - for j in range(windowStart, windowEnd): - curr_vel = self._get_object_velocity_at_frame( - len(self._records) - j, object - ) - averageVelocity[0] += curr_vel[0] - averageVelocity[1] += curr_vel[1] - - averageVelocity[0] /= WINDOW - averageVelocity[1] /= WINDOW - - if i != 0: - dt = ( - self._records[-windowMiddle + WINDOW].ts - - self._records[-windowMiddle].ts - ) - accX = (futureAverageVelocity[0] - averageVelocity[0]) / dt # TODO vec - accY = (futureAverageVelocity[1] - averageVelocity[1]) / dt - totalX += accX - totalY += accY - iter += 1 - - futureAverageVelocity = tuple(averageVelocity) - - return (totalX / iter, totalY / iter) - - def predict_object_pos_after(self, t: float, object: GameObject) -> Optional[tuple]: - # If t is after the object has stopped we return the position at which object stopped. - - acc = self.get_object_acceleration(object) - - if acc is None: - return None - - ax, ay = acc - ux, uy = self.get_object_velocity(object) - - if object is Ball: - ball = self.get_ball_pos() - start_x, start_y = ball[0].x, ball[0].y - else: - posn = self._get_object_position_at_frame(len(self._records) - 1, object) - start_x, start_y = posn.x, posn.y - - if ax == 0: # Due to friction, if acc = 0 then stopped. - sx = 0 # TODO: Not sure what to do about robots with respect to friction - we never know if they are slowing down to stop or if they are slowing down to change direction - else: - tx_stop = -ux / ax - tx = min(t, tx_stop) - sx = ux * tx + 0.5 * ax * tx * tx - - if ay == 0: - sy = 0 - else: - ty_stop = -uy / ay - ty = min(t, ty_stop) - sy = uy * ty + 0.5 * ay * ty * ty - - return ( - start_x + sx, - start_y + sy, - ) # TODO: Doesn't take into account spin / angular vel - - def predict_frame_after(self, t: float): - yellow_pos = [ - self.predict_object_pos_after(t, Robot(Colour.YELLOW, i)) for i in range(6) - ] - blue_pos = [ - self.predict_object_pos_after(t, Robot(Colour.BLUE, i)) for i in range(6) - ] - ball_pos = self.predict_object_pos_after(t, Ball) - if ball_pos is None or None in yellow_pos or None in blue_pos: - return None - else: - return FrameData( - self._records[-1].ts + t, - list(map(lambda pos: RobotData(pos[0], pos[1], 0), yellow_pos)), - list(map(lambda pos: RobotData(pos[0], pos[1], 0), blue_pos)), - [BallData(ball_pos[0], ball_pos[1], 0)], # TODO : Support z axis - ) - if __name__ == "__main__": logging.basicConfig(level=logging.INFO) diff --git a/main.py b/main.py index 7878c85e..1e9fde8f 100644 --- a/main.py +++ b/main.py @@ -32,7 +32,8 @@ def data_update_listener(receiver: VisionDataReceiver): def main(): game = Game(my_team_is_yellow=True) - GRSimController().teleport_ball(0, 0, 2, 2.5) + # GRSimController().teleport_ball(0, 0, 2, 2.5) + GRSimController().teleport_ball(0, 0, 0, 0) time.sleep(0.2) message_queue = queue.SimpleQueue() From 6fa89e3f019fd44de0c76c40b779d7f2f4b1c3e8 Mon Sep 17 00:00:00 2001 From: isaac0804 Date: Tue, 14 Jan 2025 01:35:28 +0000 Subject: [PATCH 13/14] README update --- README.md | 45 ++++++++++++++++++++++++++++++++++----------- start_test_env.sh | 2 +- 2 files changed, 35 insertions(+), 12 deletions(-) diff --git a/README.md b/README.md index 6c6d29c6..ee9e502b 100644 --- a/README.md +++ b/README.md @@ -2,23 +2,42 @@ ## 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 + ![field_guide](assets/images/field_guide.jpg) + 1. All coordinates and velocities will be in meters or meters per second. 2. All angular properties will be in radians or radians per second, normalised between [pi, -pi]. A heading of radian 0 indicates a robot facing towards the positive x-axis (ie left to right). 3. Unless otherwise stated, the coordinate system is aligned such that blue robots are on the left and yellow are on the right. - ## Guidelines #### Folder Hierarchy + 1. `decision_maker`: higher level control from above roles to plays and tactics [**No other folder should be importing from this folder**] 2. `robot_control`: lower level control for individual robots spanning skills to roles [**utility folder for decision_maker**] 3. `motion_planning`: control algorithms for movement and path planning [**utility folder for robot_control and other folders**] @@ -30,23 +49,27 @@ If you are still struggling with local import errors (ie importing our local mod 9. `replay`: replay system for storing played games in a .pkl file that can be reconstructed in rSoccer sim [**imports from rsoccer**] #### Code Writing + 1. Use typing for all functions. 2. Please please document your code on the subfolder's `README.md`. 3. Download and install `Black Formatter` for code formatting - 1. For VScode, go to View > Command Palette and search `Open User Settings (JSON)` - 2. Find the `"[python]"` field and add the following lines: - ```yaml - "[python]": { - "editor.defaultFormatter": "ms-python.black-formatter", # add this - "editor.formatOnSave": true # and add this - } - ``` + 1. For VScode, go to View > Command Palette and search `Open User Settings (JSON)` + 2. Find the `"[python]"` field and add the following lines: + + ```yaml + "[python]": { + "editor.defaultFormatter": "ms-python.black-formatter", # add this + "editor.formatOnSave": true, # and add this + } + ``` #### Push and Commit + 1. Each team should be working within your own branch of the repository. 2. Inform your lead when ready to push to main. 3. We aim to merge at different releases, so that it is easier for version control. ## Milestones -- 2024 November 20 - First goal in grSim (featuring Ray casting) \ No newline at end of file + +- 2024 November 20 - First goal in grSim (featuring Ray casting) diff --git a/start_test_env.sh b/start_test_env.sh index fdd5bdfd..475434f0 100755 --- a/start_test_env.sh +++ b/start_test_env.sh @@ -46,7 +46,7 @@ fi # Change to the ssl-game-controller directory and run the game controller, suppressing output echo "Starting game controller..." cd ssl-game-controller/ -./ssl-game-controller_v3.12.7_linux_amd64 > /dev/null 2>&1 & +./ssl-game-controller > /dev/null 2>&1 & GAME_CONTROLLER_PID=$! cd .. From 33fe7f03775aa0d1b20a01105b9a141d4a18bdd3 Mon Sep 17 00:00:00 2001 From: isaac0804 Date: Wed, 26 Feb 2025 15:19:30 +0000 Subject: [PATCH 14/14] game --- entities/game/game.py | 20 +++----------------- 1 file changed, 3 insertions(+), 17 deletions(-) diff --git a/entities/game/game.py b/entities/game/game.py index 21ee3ce4..491198b3 100644 --- a/entities/game/game.py +++ b/entities/game/game.py @@ -15,12 +15,9 @@ from entities.referee.stage import Stage from team_controller.src.config.settings import TIMESTEP -<<<<<<< HEAD -======= # TODO : ^ I don't like this circular import logic. Wondering if we should store this constant somewhere else # TODO: Namespace conflict for robot. We need to resolve this ASAP. ->>>>>>> main import logging, warnings @@ -40,31 +37,18 @@ def __init__( num_enemy_robots: int = 6, ): self._my_team_is_yellow = my_team_is_yellow -<<<<<<< HEAD - self._field = Field() -======= self._field = Field(my_team_is_yellow, my_team_is_right) ->>>>>>> main self._records: List[FrameData] = [] self._predicted_next_frame: PredictedFrame = None self._friendly_robots: List[Robot] = [ -<<<<<<< HEAD - Robot(id, is_friendly=True) for id in range(6) - ] - self._enemy_robots: List[Robot] = [ - Robot(id, is_friendly=False) for id in range(6) - ] - self._ball: Ball = Ball() -======= Robot(id, is_friendly=True) for id in range(num_friendly_robots) ] self._enemy_robots: List[Robot] = [ Robot(id, is_friendly=False) for id in range(num_enemy_robots) ] self._ball: Ball = Ball(BallData(0, 0, 0, 1)) ->>>>>>> main self._yellow_score = 0 self._blue_score = 0 @@ -440,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