decouple pull and send robot data in real and drop timeout

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

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,

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

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
@@ -47,18 +46,39 @@ 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._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()
         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}")
+        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
         self._kicker_tracker: Dict[int, KickTrackerEntry] = {}
 
     def get_robots_responses(self) -> Optional[List[RobotResponse]]:
-        ### TODO: Not implemented yet
+        # 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]
+
+        # Clear buffer after last full packet
+        self._rx_buffer = bytearray()
+
+        # TODO complete implementation to parse packet into RobotResponse.
+        packet
         return None
 
     def send_robot_commands(self) -> None:
@@ -71,10 +91,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 +161,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 +291,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: