Merge pull request #4 from ICRS-RoboCup-SSL/use_queue

Use queue branch merge

Author: fred-huang122

entities/game/game.py

@@ -1,6 +1,6 @@
+from typing import List
 from entities.game.field import Field
-from entities.data.vision import FrameData
-
+from entities.data.vision import FrameData, RobotData, BallData
 
 class Game:
     """
@@ -38,3 +38,10 @@ def add_new_state(self, frame_data: FrameData) -> None:
             self._records.append(frame_data)
         else:
             raise ValueError("Invalid frame data.")
+    
+    def get_robots_pos(self, is_yellow: bool) -> List[RobotData]:
+        record = self._records[-1] 
+        return record.yellow_robots if is_yellow else record.blue_robots
+
+    def get_ball_pos(self) -> BallData:
+        return self._records[-1].ball

main.py

@@ -1,7 +1,11 @@
 import threading
+import queue
 from entities.game import Game
-from team_controller.src.data.referee_receiver import RefereeMessageReceiver
+
+from team_controller.src.controllers.robot_startup_controller import StartUpController
 from team_controller.src.data.vision_receiver import VisionDataReceiver
+from team_controller.src.data.message_enum import MessageType
+
 
 
 def data_update_listener(receiver: VisionDataReceiver):
@@ -11,9 +15,10 @@ def data_update_listener(receiver: VisionDataReceiver):
 
 def main():
     game = Game()
-    # Initialize the VisionDataReceiver
-    receiver = VisionDataReceiver(debug=False)
-    referee_receiver = RefereeMessageReceiver(debug=True)
+
+    message_queue = queue.SimpleQueue()
+    receiver = VisionDataReceiver(message_queue, debug=False)
+    decision_maker = StartUpController(game, debug=False)
 
     # Start the data receiving in a separate thread
     data_thread = threading.Thread(target=data_update_listener, args=(receiver,))
@@ -26,26 +31,27 @@ def main():
 
     try:
         while True:
-            # Wait for the update event with a timeout (optional)
-            if receiver.wait_for_update(timeout=0.1):
-                # An update has occurred, so process the updated data
-                frame_data = receiver.get_frame_data()
-                game.add_new_state(frame_data)
+            (message_type, message) = message_queue.get() # Infinite timeout for now
 
+            if message_type == MessageType.VISION:
+                # message = FrameData(...)
+                game.add_new_state(message)
                 # access current state data
                 # print(
                 #     game.current_state.yellow_robots[0].x,
                 #     game.current_state.yellow_robots[0].y,
                 # )
 
                 # access game records from -x number of frames ago
-                # print(game.records[-1].ts, game.records[-1].ball[0].x)
-            else:
-                print("No data update received within the timeout period.")
+                print(game.records[-1].ts, game.records[-1].ball[0].x)
+
+            elif message_type == MessageType.REF:
+                pass
+        
+            decision_maker.make_decision()
 
     except KeyboardInterrupt:
         print("Stopping main program.")
 
-
 if __name__ == "__main__":
     main()

rationale.md

@@ -0,0 +1,35 @@
+- Running additional computations off main thread for some reason
+- Ability to prioritise / wait on only one queue vs waiting on all queues -> Claim is that you have to process both and load is constant so if you fall behind, you are falling behind consistently and we need to fix it separately because it won't work properly.
+
+We claim can't wait on multiple successively i.e. ref then image then ref etc because packet drop or no ref message or lost connection etc - missing other data. Can't skip through.
+
+
+
+Single thread - Doesn't support the "long running computation" - Would get less performance if we added it. 
+Queues with no blocking - main spins in fast event loop
+
+
+1 big queue with blocking
+  -> Advantage is that you get slightly better latency because the other threads are running and you can put that in another thread.
+  -> Kind of depends if they want to do long running other computations.
+
+
+
+
+
+
+If the queue is empty then what does main need to do? Is it nothing?
+
+
+Problems with current approach:
+ - Sequential Approach
+ - Separation of concerns for the entities and last 5
+        -> Enables receivers to have no concept of state which is good as they shouldn't be responsible for knowing what we want to track about the game
+ - Threading complexity and potential race conditions  
+ - Another advantage is that the controller is only summoned when decision to be amde - no random sleep lengths
+
+
+-> "Entities Data Structure" -> Plan is just have main manage it after retrieving from the thread safe queue. Only 1 thread; no race conditions. Main give that data to whoever.
+
+Priorities: If you wanted to you could but we think you don't need to because you need to process everything anyway
+

team_controller/README.md

@@ -59,7 +59,7 @@ This folder contains all the files which perform the initial processing of data
     - `Ball`: A named tuple representing the ball's position with fields `x`, `y`, and `z`.
     - `Robot`: A named tuple representing a robot's position with fields `x`, `y`, and `orientation`.
   - **Methods**:
-    - `get_robots_pos(is_yellow: bool) -> List[Robot]`: Retrieves the current position data for robots on the specified team. If `is_yellow` is `True`, it retrieves data for the yellow team; otherwise, it retrieves data for the blue team.
+    - DOCTODO `get_robots_pos(is_yellow: bool) -> List[Robot]`: Retrieves the current position data for robots on the specified team. If `is_yellow` is `True`, it retrieves data for the yellow team; otherwise, it retrieves data for the blue team.
     - `get_ball_pos() -> Ball`: Retrieves the current position data for the ball.
     - `pull_game_data() -> None`: Continuously receives vision data packets and updates the internal data structures for the game state. This method runs indefinitely and should typically be started in a separate thread.
 

team_controller/proto/ssl_gc_referee_message.proto

@@ -210,7 +210,7 @@ message Referee {
     //  * kickoff, penalty kick, force start
     //  * ball placement
     optional int64 current_action_time_remaining = 15;
-
+ 
     // A message that can be displayed to the spectators, like a reason for a stoppage.
     optional string status_message = 20;
 }

team_controller/src/controllers/robot_startup_controller.py

@@ -3,6 +3,7 @@
 import numpy as np
 from typing import Tuple, Optional, Dict, Union, List
 
+from entities.game.game import Game
 from global_utils.math_utils import rotate_vector
 from entities.data.command import RobotSimCommand
 from entities.data.vision import RobotData, BallData
@@ -13,55 +14,46 @@
     PID_PARAMS,
     YELLOW_START,
 )
+from team_controller.src.generated_code.ssl_simulation_robot_control_pb2 import (
+    RobotControl,
+)
 
 # TODO: To be moved to a High-level Descision making repo
-
 class StartUpController:
     def __init__(
         self,
-        vision_receiver: VisionDataReceiver,
+        game: Game,
         debug=False,
     ):
-        self.vision_receiver = vision_receiver
-        self.sim_robot_controller = SimRobotController(is_team_yellow=True, debug=debug)
+        self.game = game
+        self.sim_robot_controller = SimRobotController(debug=debug)
 
         # TODO: Tune PID parameters further when going from sim to real(it works for Grsim)
         # potentially have a set tunig parameters for each robot
         self.pid_oren = PID(0.0167, 8, -8, 5, 0, 0.03, num_robots=6)
         self.pid_trans = PID(0.0167, 1.5, -1.5, 5, 0, 0.02, num_robots=6)
 
-        self.lock = threading.Lock()
-
         self.debug = debug
 
-    def startup(self):
-        while True:
-            start_time = time.time()
-
-            robots, balls = self._get_positions()
-
-            if robots and balls:
-                for robot_id, robot_data in enumerate(robots):
-                    if robot_data is None:
-                        continue
-                    target_coords = YELLOW_START[robot_id]
-                    command = self._calculate_robot_velocities(
-                        robot_id, target_coords, robots, balls, face_ball=True
-                    )
-                    self.sim_robot_controller.add_robot_commands(command, robot_id)
-                    
-                self.sim_robot_controller.send_robot_commands()
-                self.sim_robot_controller.robot_has_ball(robot_id=3)
-
-            time_to_sleep = max(0, 0.0167 - (time.time() - start_time))
-            time.sleep(time_to_sleep)
-
-    def _get_positions(self) -> tuple:
-        # Fetch the latest positions of robots and balls with thread locking.
-        with self.lock:
-            robots = self.vision_receiver.get_robots_pos(is_yellow=True)
-            balls = self.vision_receiver.get_ball_pos()
-        return robots, balls
+    def make_decision(self):
+        robots = self.game.get_robots_pos(is_yellow=True)
+        balls = self.game.get_ball_pos()
+
+        if robots and balls:
+            out_packet = RobotControl()
+            for robot_id, robot_data in enumerate(robots):
+                if robot_data is None:
+                    continue
+                target_coords = YELLOW_START[robot_id]
+                command = self._calculate_robot_velocities(
+                    robot_id, target_coords, robots, balls, face_ball=True
+                )
+                self.sim_robot_controller.add_robot_command(command)
+
+            if self.debug:
+                print(out_packet)
+            self.sim_robot_controller.send_robot_commands(team_is_yellow=True)
+            self.sim_robot_controller.robot_has_ball(robot_id=3, team_is_yellow=True)
 
 def _calculate_robot_velocities(
         self,

team_controller/src/data/base_receiver.py

@@ -0,0 +1,10 @@
+import queue
+import abc
+
+
+class BaseReceiver(abc.ABC):
+    """Interface for receivers. Every receiver should add messages to the passed-in message queue which 
+    will notify main of something to do."""
+
+    def __init__(self, message_queue:queue.SimpleQueue):
+        self._message_queue = message_queue

team_controller/src/data/message_enum.py

@@ -0,0 +1,7 @@
+from enum import Enum
+
+
+class MessageType(Enum):
+    """Describes the type of message added to the global message queue by the receivers"""    
+    VISION = 1
+    REF = 2

team_controller/src/data/referee_receiver.py

@@ -22,8 +22,7 @@ class RefereeMessageReceiver:
         port (int): The port for receiving referee data. Defaults to REFEREE_PORT.
         debug (bool): Whether to print debug information. Defaults to False.
     """
-
-    def __init__(self, ip=MULTICAST_GROUP_REFEREE, port=REFEREE_PORT, debug=False):
+    def __init__(self, ip=MULTICAST_GROUP_REFEREE, port=REFEREE_PORT, debug=False): # TODO: add message queue
         self.net = network_manager.NetworkManager(address=(ip, port), bind_socket=True)
         self.prev_command_counter = -1
         self.command_history = []