From a4e4af814bbd95c991669b21d2a638a6f9011119 Mon Sep 17 00:00:00 2001 From: Joel Date: Fri, 20 Feb 2026 21:43:48 +0000 Subject: [PATCH 1/5] decouple pull and send robot data in real and drop timeout --- utama_core/run/strategy_runner.py | 17 ++++---- .../common/robot_controller_abstract.py | 10 ++++- .../controllers/real/real_robot_controller.py | 39 +++++++++---------- 3 files changed, 36 insertions(+), 30 deletions(-) diff --git a/utama_core/run/strategy_runner.py b/utama_core/run/strategy_runner.py index fe6b73a5..9ed9b8fb 100644 --- a/utama_core/run/strategy_runner.py +++ b/utama_core/run/strategy_runner.py @@ -100,7 +100,7 @@ def __init__( print_real_fps: bool = False, # Turn this on for RSim profiler_name: Optional[str] = None, rsim_noise: RsimGaussianNoise = RsimGaussianNoise(), - rsim_vanishing: float = 0 + rsim_vanishing: float = 0, ): self.logger = logging.getLogger(__name__) @@ -224,14 +224,12 @@ def start_threads(self, vision_receiver: VisionReceiver): # , referee_receiver) # referee_thread.start() def _load_sim( - self, - rsim_noise: RsimGaussianNoise, - rsim_vanishing: float + self, rsim_noise: RsimGaussianNoise, rsim_vanishing: float ) -> Tuple[Optional[SSLStandardEnv], Optional[AbstractSimController]]: """Mode RSIM: Loads the RSim environment with the expected number of robots and corresponding sim controller. Mode GRSIM: Loads corresponding sim controller and teleports robots in GRSim to ensure the expected number of robots is met. - + Args: rsim_noise (RsimGaussianNoise, optional): When running in rsim, add Gaussian noise to balls and robots with the given standard deviation. The 3 parameters are for x (in m), y (in m), and orientation (in degrees) respectively. @@ -245,7 +243,13 @@ def _load_sim( """ if self.mode == Mode.RSIM: n_yellow, n_blue = map_friendly_enemy_to_colors(self.my_team_is_yellow, self.exp_friendly, self.exp_enemy) - rsim_env = SSLStandardEnv(n_robots_yellow=n_yellow, n_robots_blue=n_blue, render_mode=None, gaussian_noise=rsim_noise, vanishing=rsim_vanishing) + rsim_env = SSLStandardEnv( + n_robots_yellow=n_yellow, + n_robots_blue=n_blue, + render_mode=None, + gaussian_noise=rsim_noise, + vanishing=rsim_vanishing, + ) if self.opp_strategy: self.opp_strategy.load_rsim_env(rsim_env) @@ -434,7 +438,6 @@ def _reset_game(self): current game and history objects (useful between episodes or after resets). """ _ = self.my_strategy.robot_controller.get_robots_responses() - ( self.my_game_history, self.my_current_game_frame, diff --git a/utama_core/team_controller/src/controllers/common/robot_controller_abstract.py b/utama_core/team_controller/src/controllers/common/robot_controller_abstract.py index 1d7509ba..9e9041e8 100644 --- a/utama_core/team_controller/src/controllers/common/robot_controller_abstract.py +++ b/utama_core/team_controller/src/controllers/common/robot_controller_abstract.py @@ -55,5 +55,11 @@ def _add_robot_command(self, command: RobotCommand, robot_id: int) -> None: @abc.abstractmethod def get_robots_responses(self) -> Optional[List[RobotResponse]]: - """Returns the robot response from the last sent commands.""" - return self._robots_info.popleft() if self._robots_info else None + """ + Pull the latest robot response. + + Returns: + A list of RobotResponse objects (each containing the robot ID + and its ball-contact state), or None if no response was received. + """ + pass diff --git a/utama_core/team_controller/src/controllers/real/real_robot_controller.py b/utama_core/team_controller/src/controllers/real/real_robot_controller.py index c93ff98d..8906ba66 100644 --- a/utama_core/team_controller/src/controllers/real/real_robot_controller.py +++ b/utama_core/team_controller/src/controllers/real/real_robot_controller.py @@ -47,10 +47,10 @@ class RealRobotController(AbstractRobotController): def __init__(self, is_team_yellow: bool, n_friendly: int): super().__init__(is_team_yellow, n_friendly) self._serial_port = self._init_serial() - self._rbt_cmd_size = 10 # packet size for one robot + self._rbt_cmd_size = 10 # packet size (bytes) for one robot + self._robot_info_size = 2 # number of bytes in feedback packet for all robots self._out_packet = self._empty_command() self._in_packet_size = 1 # size of the feedback packet received from the robots - self._robots_info: List[RobotResponse] = [None] * self._n_friendly logger.debug(f"Serial port: {PORT} opened with baudrate: {BAUD_RATE} and timeout {TIMEOUT}") self._assigned_mapping = {} # mapping of robot_id to index in the out_packet @@ -58,8 +58,21 @@ def __init__(self, is_team_yellow: bool, n_friendly: int): self._kicker_tracker: Dict[int, KickTrackerEntry] = {} def get_robots_responses(self) -> Optional[List[RobotResponse]]: - ### TODO: Not implemented yet - return None + # Check if anything is available (instant, non-blocking) + waiting = self._serial_port.in_waiting + if waiting < self._robot_info_size: + return None + + data_in = self._serial_port.read(waiting) + + if len(data_in) < self._robot_info_size: + return None + + # TODO: complete implementation + # Take the newest 2 bytes only + # packet = data_in[-self._robot_info_size :] + + # return packet def send_robot_commands(self) -> None: """Sends the robot commands to the appropriate team (yellow or blue).""" @@ -71,10 +84,6 @@ def send_robot_commands(self) -> None: f"Only {len(self._assigned_mapping)} out of {self._n_friendly} robots have been assigned commands. Sending incomplete command packet." ) self._serial_port.write(self._out_packet) - self._serial_port.read_all() - # data_in = self._serial.read_all() - # print(data_in) - # TODO: add receiving feedback from the robots ### update kick and chip trackers. We persist the kick/chip command for KICKER_PERSIST_TIMESTEPS ### this feature is to combat packet loss and to ensure the robot does not kick within its cooldown period @@ -145,18 +154,6 @@ def _add_robot_command(self, command: RobotCommand, robot_id: int) -> None: is_kick=False, ) - # def _populate_robots_info(self, data_in: bytes) -> None: - # """ - # Populates the robots_info list with the data received from the robots. - # """ - # for i in range(self._n_friendly): - # has_ball = False - # if data_in[0] & 0b10000000: - # has_ball = True - # info = RobotInfo(has_ball=has_ball) - # self._robots_info[i] = info - # data_in = data_in << 1 # shift to the next robot's data - def _generate_command_buffer(self, robot_id: int, c_command: RobotCommand) -> bytes: """Generates the command buffer to be sent to the robot.""" assert robot_id < 6, "Invalid robot_id. Must be between 0 and 5." @@ -287,7 +284,7 @@ def _init_serial(self) -> Serial: bytesize=EIGHTBITS, # 8 data bits parity=PARITY_EVEN, # Even parity (makes it 9 bits total) stopbits=STOPBITS_TWO, # 2 stop bits - timeout=0.1, + timeout=0, ) return serial_port except Exception as e: From 0080884b16992d1bbe409f5461d89af5a0f41580 Mon Sep 17 00:00:00 2001 From: Joel Date: Fri, 20 Feb 2026 21:57:26 +0000 Subject: [PATCH 2/5] add rx buffer to manage partial reads --- .../controllers/real/real_robot_controller.py | 28 ++++++++++++------- 1 file changed, 18 insertions(+), 10 deletions(-) diff --git a/utama_core/team_controller/src/controllers/real/real_robot_controller.py b/utama_core/team_controller/src/controllers/real/real_robot_controller.py index 8906ba66..5adc92a4 100644 --- a/utama_core/team_controller/src/controllers/real/real_robot_controller.py +++ b/utama_core/team_controller/src/controllers/real/real_robot_controller.py @@ -47,6 +47,7 @@ class RealRobotController(AbstractRobotController): def __init__(self, is_team_yellow: bool, n_friendly: int): super().__init__(is_team_yellow, n_friendly) self._serial_port = self._init_serial() + self._rx_buffer = bytearray() self._rbt_cmd_size = 10 # packet size (bytes) for one robot self._robot_info_size = 2 # number of bytes in feedback packet for all robots self._out_packet = self._empty_command() @@ -58,21 +59,28 @@ def __init__(self, is_team_yellow: bool, n_friendly: int): self._kicker_tracker: Dict[int, KickTrackerEntry] = {} def get_robots_responses(self) -> Optional[List[RobotResponse]]: - # Check if anything is available (instant, non-blocking) - waiting = self._serial_port.in_waiting - if waiting < self._robot_info_size: - return None + # Non-blocking read + data = self._serial_port.read(self._serial_port.in_waiting) - data_in = self._serial_port.read(waiting) + if data: + self._rx_buffer.extend(data) - if len(data_in) < self._robot_info_size: + if len(self._rx_buffer) < self._robot_info_size: return None - # TODO: complete implementation - # Take the newest 2 bytes only - # packet = data_in[-self._robot_info_size :] + # Keep only the last complete packet + packet_count = len(self._rx_buffer) // self._robot_info_size + start = (packet_count - 1) * self._robot_info_size + end = start + self._robot_info_size + + packet = self._rx_buffer[start:end] + + # Clear buffer after last full packet + self._rx_buffer = bytearray() - # return packet + # TODO complete implementation to parse packet into RobotResponse. + packet + return None def send_robot_commands(self) -> None: """Sends the robot commands to the appropriate team (yellow or blue).""" From 00182bfb7a57986b6efaa5889a43eee7f40ee343 Mon Sep 17 00:00:00 2001 From: Joel Date: Fri, 20 Feb 2026 21:58:54 +0000 Subject: [PATCH 3/5] fully remove timeout --- utama_core/config/settings.py | 1 - .../src/controllers/real/real_robot_controller.py | 3 +-- 2 files changed, 1 insertion(+), 3 deletions(-) diff --git a/utama_core/config/settings.py b/utama_core/config/settings.py index e2a2bce3..7ecba5b4 100644 --- a/utama_core/config/settings.py +++ b/utama_core/config/settings.py @@ -30,7 +30,6 @@ ### REAL CONTROLLER SETTINGS ### BAUD_RATE = 115200 PORT = "/dev/ttyUSB0" -TIMEOUT = 0.1 KICKER_COOLDOWN_TIME = 10 # in seconds to prevent kicker from being actuated too frequently KICKER_COOLDOWN_TIMESTEPS = int(KICKER_COOLDOWN_TIME * CONTROL_FREQUENCY) # in timesteps KICKER_PERSIST_TIMESTEPS = 10 # in timesteps to persist the kick command diff --git a/utama_core/team_controller/src/controllers/real/real_robot_controller.py b/utama_core/team_controller/src/controllers/real/real_robot_controller.py index 5adc92a4..b8403e67 100644 --- a/utama_core/team_controller/src/controllers/real/real_robot_controller.py +++ b/utama_core/team_controller/src/controllers/real/real_robot_controller.py @@ -13,7 +13,6 @@ KICKER_COOLDOWN_TIMESTEPS, KICKER_PERSIST_TIMESTEPS, PORT, - TIMEOUT, TIMESTEP, ) from utama_core.entities.data.command import RobotCommand, RobotResponse @@ -52,7 +51,7 @@ def __init__(self, is_team_yellow: bool, n_friendly: int): self._robot_info_size = 2 # number of bytes in feedback packet for all robots self._out_packet = self._empty_command() self._in_packet_size = 1 # size of the feedback packet received from the robots - logger.debug(f"Serial port: {PORT} opened with baudrate: {BAUD_RATE} and timeout {TIMEOUT}") + logger.debug(f"Serial port: {PORT} opened with baudrate: {BAUD_RATE}.") self._assigned_mapping = {} # mapping of robot_id to index in the out_packet # track last kick time for each robot to transmit kick as HIGH for n timesteps after command From 439254d4936417cd8ef5697f31e25863c0c56183 Mon Sep 17 00:00:00 2001 From: Joel Date: Sat, 21 Feb 2026 23:31:21 +0000 Subject: [PATCH 4/5] add get_robot_response for when its implemented in hardware --- .../controllers/real/real_robot_controller.py | 78 +++++++++++++------ 1 file changed, 55 insertions(+), 23 deletions(-) diff --git a/utama_core/team_controller/src/controllers/real/real_robot_controller.py b/utama_core/team_controller/src/controllers/real/real_robot_controller.py index b8403e67..38f72063 100644 --- a/utama_core/team_controller/src/controllers/real/real_robot_controller.py +++ b/utama_core/team_controller/src/controllers/real/real_robot_controller.py @@ -23,7 +23,6 @@ logger = logging.getLogger(__name__) -# NB: A major assumption is that the robot IDs are 0-5 for the friendly team. MAX_VEL = REAL_PARAMS.MAX_VEL MAX_ANGULAR_VEL = REAL_PARAMS.MAX_ANGULAR_VEL @@ -43,6 +42,9 @@ class RealRobotController(AbstractRobotController): n_robots (int): The number of robots in the team. Directly affects output buffer size. Default is 6. """ + HEADER = 0xAA + TAIL = 0x55 + def __init__(self, is_team_yellow: bool, n_friendly: int): super().__init__(is_team_yellow, n_friendly) self._serial_port = self._init_serial() @@ -57,29 +59,59 @@ def __init__(self, is_team_yellow: bool, n_friendly: int): # track last kick time for each robot to transmit kick as HIGH for n timesteps after command self._kicker_tracker: Dict[int, KickTrackerEntry] = {} - def get_robots_responses(self) -> Optional[List[RobotResponse]]: - # Non-blocking read - data = self._serial_port.read(self._serial_port.in_waiting) - - if data: - self._rx_buffer.extend(data) - - if len(self._rx_buffer) < self._robot_info_size: - return None - - # Keep only the last complete packet - packet_count = len(self._rx_buffer) // self._robot_info_size - start = (packet_count - 1) * self._robot_info_size - end = start + self._robot_info_size - - packet = self._rx_buffer[start:end] + def get_robots_responses(self) -> Optional[Dict[int, RobotResponse]]: + """Reads the robot responses from the serial port and returns a dictionary mapping robot IDs to their responses.""" + # For now, we are not processing feedback from the robots, but this method can be implemented similarly to the GRSimRobotController if needed in the future. + self._serial_port.read_all() # Clear the input buffer to avoid overflow + return None - # Clear buffer after last full packet - self._rx_buffer = bytearray() + # TODO: implement when feedback is actually working + def get_robots_responses_real(self) -> Optional[Dict[int, RobotResponse]]: + FRAME_SIZE = 1 + self._robot_info_size + 1 + + # --- Step 1: Non-blocking read --- + waiting = self._serial_port.in_waiting + if waiting: + self._rx_buffer.extend(self._serial_port.read(waiting)) + + latest_payload = None + + # --- Step 2: Forward scan to find the newest COMPLETE frame --- + while len(self._rx_buffer) >= FRAME_SIZE: + if self._rx_buffer[0] != self.HEADER: + # Fast-forward to the next possible header to save CPU cycles + next_header = self._rx_buffer.find(bytes([self.HEADER])) + if next_header == -1: + self._rx_buffer.clear() + break + self._rx_buffer = self._rx_buffer[next_header:] + continue + + # We have a header and enough bytes. Validate tail. + if self._rx_buffer[FRAME_SIZE - 1] == self.TAIL: + # Valid frame found! Save it, but keep looping to see if there's a newer one. + latest_payload = self._rx_buffer[1 : FRAME_SIZE - 1] + self._rx_buffer = self._rx_buffer[FRAME_SIZE:] + else: + # Fake header. Drop it so we don't get stuck, and resync. + self._rx_buffer.pop(0) + + # --- Step 3: If no complete frames were found, check for a partial one to block for --- + if not latest_payload and len(self._rx_buffer) > 0 and self._rx_buffer[0] == self.HEADER: + remaining = FRAME_SIZE - len(self._rx_buffer) + chunk = self._serial_port.read(remaining) # Blocks + if chunk: + self._rx_buffer.extend(chunk) + + # Validate the newly completed frame + if len(self._rx_buffer) == FRAME_SIZE and self._rx_buffer[FRAME_SIZE - 1] == self.TAIL: + latest_payload = self._rx_buffer[1 : FRAME_SIZE - 1] + self._rx_buffer.clear() + else: + # Fake header or timeout. Drop the bad byte. + self._rx_buffer.pop(0) - # TODO complete implementation to parse packet into RobotResponse. - packet - return None + return None # Placeholder until we implement actual parsing of latest_payload def send_robot_commands(self) -> None: """Sends the robot commands to the appropriate team (yellow or blue).""" @@ -276,7 +308,7 @@ def _empty_command(self) -> bytearray: # * first byte is robot ID (here INVALID_RBT_ID) # * remaining (self._rbt_cmd_size - 1) bytes are zeros # - 0x55: end byte - cmd = bytearray([0xAA] + [INVALID_RBT_ID] + [0] * (self._rbt_cmd_size - 1) + [0x55]) + cmd = bytearray([self.HEADER] + [INVALID_RBT_ID] + [0] * (self._rbt_cmd_size - 1) + [self.TAIL]) commands.extend(cmd) self._cached_empty_command = commands return self._cached_empty_command.copy() From e875cdd31123a16a9a679bad5cc981abaf44064f Mon Sep 17 00:00:00 2001 From: Joel Date: Sat, 21 Feb 2026 23:44:15 +0000 Subject: [PATCH 5/5] make it non-blocking even on partial read --- .../controllers/real/real_robot_controller.py | 32 ++++--------------- .../controllers/sim/grsim_robot_controller.py | 2 +- 2 files changed, 8 insertions(+), 26 deletions(-) diff --git a/utama_core/team_controller/src/controllers/real/real_robot_controller.py b/utama_core/team_controller/src/controllers/real/real_robot_controller.py index 38f72063..3c1f70e9 100644 --- a/utama_core/team_controller/src/controllers/real/real_robot_controller.py +++ b/utama_core/team_controller/src/controllers/real/real_robot_controller.py @@ -59,14 +59,13 @@ def __init__(self, is_team_yellow: bool, n_friendly: int): # track last kick time for each robot to transmit kick as HIGH for n timesteps after command self._kicker_tracker: Dict[int, KickTrackerEntry] = {} - def get_robots_responses(self) -> Optional[Dict[int, RobotResponse]]: - """Reads the robot responses from the serial port and returns a dictionary mapping robot IDs to their responses.""" + def get_robots_responses(self) -> Optional[list[RobotResponse]]: # For now, we are not processing feedback from the robots, but this method can be implemented similarly to the GRSimRobotController if needed in the future. self._serial_port.read_all() # Clear the input buffer to avoid overflow return None # TODO: implement when feedback is actually working - def get_robots_responses_real(self) -> Optional[Dict[int, RobotResponse]]: + def get_robots_responses_real(self) -> Optional[list[RobotResponse]]: FRAME_SIZE = 1 + self._robot_info_size + 1 # --- Step 1: Non-blocking read --- @@ -76,10 +75,9 @@ def get_robots_responses_real(self) -> Optional[Dict[int, RobotResponse]]: latest_payload = None - # --- Step 2: Forward scan to find the newest COMPLETE frame --- + # --- Step 2: Forward scan for complete frames --- while len(self._rx_buffer) >= FRAME_SIZE: if self._rx_buffer[0] != self.HEADER: - # Fast-forward to the next possible header to save CPU cycles next_header = self._rx_buffer.find(bytes([self.HEADER])) if next_header == -1: self._rx_buffer.clear() @@ -87,31 +85,15 @@ def get_robots_responses_real(self) -> Optional[Dict[int, RobotResponse]]: self._rx_buffer = self._rx_buffer[next_header:] continue - # We have a header and enough bytes. Validate tail. if self._rx_buffer[FRAME_SIZE - 1] == self.TAIL: - # Valid frame found! Save it, but keep looping to see if there's a newer one. latest_payload = self._rx_buffer[1 : FRAME_SIZE - 1] self._rx_buffer = self._rx_buffer[FRAME_SIZE:] else: - # Fake header. Drop it so we don't get stuck, and resync. - self._rx_buffer.pop(0) - - # --- Step 3: If no complete frames were found, check for a partial one to block for --- - if not latest_payload and len(self._rx_buffer) > 0 and self._rx_buffer[0] == self.HEADER: - remaining = FRAME_SIZE - len(self._rx_buffer) - chunk = self._serial_port.read(remaining) # Blocks - if chunk: - self._rx_buffer.extend(chunk) - - # Validate the newly completed frame - if len(self._rx_buffer) == FRAME_SIZE and self._rx_buffer[FRAME_SIZE - 1] == self.TAIL: - latest_payload = self._rx_buffer[1 : FRAME_SIZE - 1] - self._rx_buffer.clear() - else: - # Fake header or timeout. Drop the bad byte. - self._rx_buffer.pop(0) + # Partial frame — keep it in buffer until next read + break - return None # Placeholder until we implement actual parsing of latest_payload + latest_payload + return None # TODO: parse latest_payload into list of RobotResponse and return. def send_robot_commands(self) -> None: """Sends the robot commands to the appropriate team (yellow or blue).""" diff --git a/utama_core/team_controller/src/controllers/sim/grsim_robot_controller.py b/utama_core/team_controller/src/controllers/sim/grsim_robot_controller.py index c81954ba..62338ff4 100644 --- a/utama_core/team_controller/src/controllers/sim/grsim_robot_controller.py +++ b/utama_core/team_controller/src/controllers/sim/grsim_robot_controller.py @@ -110,7 +110,7 @@ def _update_robot_info(self, data: bytes) -> None: # ignore robot_info for robots that are not expected (ie deactivated since the start of the game) self._robots_info.append(robots_info) - def get_robots_responses(self) -> Optional[RobotResponse]: + def get_robots_responses(self) -> Optional[list[RobotResponse]]: if self._robots_info is None or len(self._robots_info) == 0: for i in range(self._n_friendly): self.add_robot_commands(RobotCommand(0, 0, 0, False, False, False), i)