Add robot has_ball feedback

pixi.toml

@@ -50,6 +50,7 @@ lint = "pre-commit run --all-files"
 test = "pytest utama_core/tests/"
 main = "python -m main"
 replay = "python -m utama_core.replay.replay_player"
+debug-robots = "python -m utama_core.team_controller.src.debug_utils.telop_gui"
 precommit-install = "pre-commit install && pixi run pre-commit install --hook-type pre-commit"
 precommit-uninstall = "pre-commit uninstall"
 

utama_core/team_controller/src/controllers/real/real_robot_controller.py

@@ -57,9 +57,53 @@ 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]]:
-        ### TODO: Not implemented yet
-        return None
+    def get_robots_responses(self) -> List[RobotResponse]:
+        HEADER = 0xAA
+        FOOTER = 0x55
+        if not hasattr(self, "_buffer"):
+            self._buffer = bytearray()
+
+        # Read whatever is available
+        self._buffer.extend(self._serial_port.read(self._serial_port.in_waiting or 1))
+
+        responses = []
+
+        while True:
+            # Look for header
+            if len(self._buffer) < 1:
+                break
+
+            if self._buffer[0] != HEADER:
+                self._buffer.pop(0)
+                continue
+
+            # Need at least header + id + length
+            if len(self._buffer) < 3:
+                break
+
+            robot_id = self._buffer[1]
+            length = self._buffer[2]
+
+            packet_len = 1 + 1 + 1 + length + 1  # header + id + len + data + footer
+
+            if len(self._buffer) < packet_len:
+                break  # wait for more data
+
+            # Validate footer
+            if self._buffer[packet_len - 1] != FOOTER:
+                # Corrupted packet → resync
+                self._buffer.pop(0)
+                continue
+
+            # Extract packet
+            data = self._buffer[3 : 3 + length]
+
+            responses.append(RobotResponse(robot_id, has_ball=(data[0] & 0x01) != 0))
+
+            # Remove parsed packet
+            del self._buffer[:packet_len]
+
+        return responses
 
     def send_robot_commands(self) -> None:
         """Sends the robot commands to the appropriate team (yellow or blue)."""
@@ -68,13 +112,9 @@ def send_robot_commands(self) -> None:
         # print(binary_representation)
         if len(self._assigned_mapping) != self._n_friendly:
             warnings.warn(
-                f"Only {len(self._assigned_mapping)} out of {self._n_friendly} robots have been assigned commands. Sending incomplete command packet."
+                f"Only {len(self._assigned_mapping)} out of {self._n_friendly} robots have been assigned commands. Sending empty commands for unassigned robots."
             )
         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