[Thunderscope] Remove Extra Robots and Add Missing Robots during Simulator + Fixes

src/proto/message_translation/ssl_referee.cpp

@@ -2,6 +2,14 @@
 
 #include "shared/constants.h"
 
+// this set contains an ongoing list of deprecated SSLProto::Referee_Commands
+const static std::unordered_set<SSLProto::Referee::Command> deprecated_commands = {
+    SSLProto::Referee_Command_GOAL_YELLOW,
+    SSLProto::Referee_Command_GOAL_BLUE,
+    SSLProto::Referee_Command_INDIRECT_FREE_YELLOW,
+    SSLProto::Referee_Command_INDIRECT_FREE_BLUE,
+};
+
 // this maps a protobuf SSLProto::Referee_Command enum to its equivalent internal type
 // this map is used when we are on the blue team
 const static std::unordered_map<SSLProto::Referee::Command, RefereeCommand>
@@ -62,6 +70,8 @@ const static std::unordered_map<SSLProto::Referee::Command, RefereeCommand>
 RefereeCommand createRefereeCommand(const SSLProto::Referee &packet,
                                     TeamColour team_colour)
 {
+    if (deprecated_commands.contains(packet.command()))
+        return RefereeCommand::HALT;
     if (team_colour == TeamColour::YELLOW)
     {
         return yellow_team_command_map.at(packet.command());

src/proto/message_translation/ssl_referee_test.cpp

@@ -124,17 +124,6 @@ INSTANTIATE_TEST_CASE_P(
         std::make_tuple(RefereeCommand::DIRECT_FREE_THEM,
                         SSLProto::Referee_Command_DIRECT_FREE_YELLOW, TeamColour::BLUE),
 
-        // indirect free
-        std::make_tuple(RefereeCommand::INDIRECT_FREE_US,
-                        SSLProto::Referee_Command_INDIRECT_FREE_YELLOW,
-                        TeamColour::YELLOW),
-        std::make_tuple(RefereeCommand::INDIRECT_FREE_US,
-                        SSLProto::Referee_Command_INDIRECT_FREE_BLUE, TeamColour::BLUE),
-        std::make_tuple(RefereeCommand::INDIRECT_FREE_THEM,
-                        SSLProto::Referee_Command_INDIRECT_FREE_BLUE, TeamColour::YELLOW),
-        std::make_tuple(RefereeCommand::INDIRECT_FREE_THEM,
-                        SSLProto::Referee_Command_INDIRECT_FREE_YELLOW, TeamColour::BLUE),
-
         // timeout
         std::make_tuple(RefereeCommand::TIMEOUT_US,
                         SSLProto::Referee_Command_TIMEOUT_YELLOW, TeamColour::YELLOW),

src/software/sensor_fusion/sensor_fusion_test.cpp

@@ -25,8 +25,8 @@ class SensorFusionTest : public ::testing::Test
           robot_status_msg_dribble_motor_hot(initDribbleMotorHotErrorCode()),
           robot_status_msg_multiple_error_codes(initMultipleErrorCode()),
           robot_status_msg_no_error_code(initNoErrorCode()),
-          referee_indirect_yellow(initRefereeIndirectYellow()),
-          referee_indirect_blue(initRefereeIndirectBlue()),
+          referee_direct_yellow(initRefereeDirectYellow()),
+          referee_direct_blue(initRefereeDirectBlue()),
           referee_normal_start(initRefereeNormalStart()),
           referee_ball_placement_yellow(initRefereeBallPlacementYellow()),
           referee_ball_placement_blue(initRefereeBallPlacementBlue()),
@@ -49,8 +49,8 @@ class SensorFusionTest : public ::testing::Test
     std::unique_ptr<TbotsProto::RobotStatus> robot_status_msg_dribble_motor_hot;
     std::unique_ptr<TbotsProto::RobotStatus> robot_status_msg_multiple_error_codes;
     std::unique_ptr<TbotsProto::RobotStatus> robot_status_msg_no_error_code;
-    std::unique_ptr<SSLProto::Referee> referee_indirect_yellow;
-    std::unique_ptr<SSLProto::Referee> referee_indirect_blue;
+    std::unique_ptr<SSLProto::Referee> referee_direct_yellow;
+    std::unique_ptr<SSLProto::Referee> referee_direct_blue;
     std::unique_ptr<SSLProto::Referee> referee_normal_start;
     std::unique_ptr<SSLProto::Referee> referee_ball_placement_yellow;
     std::unique_ptr<SSLProto::Referee> referee_ball_placement_blue;
@@ -282,17 +282,17 @@ class SensorFusionTest : public ::testing::Test
         return robot_msg;
     }
 
-    std::unique_ptr<SSLProto::Referee> initRefereeIndirectYellow()
+    std::unique_ptr<SSLProto::Referee> initRefereeDirectYellow()
     {
         auto ref_msg = std::make_unique<SSLProto::Referee>();
-        ref_msg->set_command(SSLProto::Referee_Command_INDIRECT_FREE_YELLOW);
+        ref_msg->set_command(SSLProto::Referee_Command_DIRECT_FREE_YELLOW);
         return ref_msg;
     }
 
-    std::unique_ptr<SSLProto::Referee> initRefereeIndirectBlue()
+    std::unique_ptr<SSLProto::Referee> initRefereeDirectBlue()
     {
         auto ref_msg = std::make_unique<SSLProto::Referee>();
-        ref_msg->set_command(SSLProto::Referee_Command_INDIRECT_FREE_BLUE);
+        ref_msg->set_command(SSLProto::Referee_Command_DIRECT_FREE_BLUE);
         return ref_msg;
     }
 
@@ -545,7 +545,7 @@ TEST_F(SensorFusionTest, test_complete_wrapper_with_robot_status_msg_2_at_a_time
 TEST_F(SensorFusionTest, test_referee_yellow_then_normal)
 {
     GameState expected_1;
-    expected_1.updateRefereeCommand(RefereeCommand::INDIRECT_FREE_US);
+    expected_1.updateRefereeCommand(RefereeCommand::DIRECT_FREE_US);
 
     GameState expected_2 = expected_1;
     expected_2.updateRefereeCommand(RefereeCommand::NORMAL_START);
@@ -555,7 +555,7 @@ TEST_F(SensorFusionTest, test_referee_yellow_then_normal)
         createSSLWrapperPacket(std::move(geom_data), initDetectionFrame());
     // set vision msg so that world is valid
     *(sensor_msg_1.mutable_ssl_vision_msg())  = *ssl_wrapper_packet;
-    *(sensor_msg_1.mutable_ssl_referee_msg()) = *referee_indirect_yellow;
+    *(sensor_msg_1.mutable_ssl_referee_msg()) = *referee_direct_yellow;
     sensor_fusion.processSensorProto(sensor_msg_1);
     World result_1 = *sensor_fusion.getWorld();
     EXPECT_EQ(expected_1, result_1.gameState());
@@ -570,7 +570,7 @@ TEST_F(SensorFusionTest, test_referee_yellow_then_normal)
 TEST_F(SensorFusionTest, test_referee_blue_then_normal)
 {
     GameState expected_1;
-    expected_1.updateRefereeCommand(RefereeCommand::INDIRECT_FREE_THEM);
+    expected_1.updateRefereeCommand(RefereeCommand::DIRECT_FREE_THEM);
 
     GameState expected_2 = expected_1;
     expected_2.updateRefereeCommand(RefereeCommand::NORMAL_START);
@@ -580,7 +580,7 @@ TEST_F(SensorFusionTest, test_referee_blue_then_normal)
         createSSLWrapperPacket(std::move(geom_data), initDetectionFrame());
     // set vision msg so that world is valid
     *(sensor_msg_1.mutable_ssl_vision_msg())  = *ssl_wrapper_packet;
-    *(sensor_msg_1.mutable_ssl_referee_msg()) = *referee_indirect_blue;
+    *(sensor_msg_1.mutable_ssl_referee_msg()) = *referee_direct_blue;
     sensor_fusion.processSensorProto(sensor_msg_1);
     World result_1 = *sensor_fusion.getWorld();
     EXPECT_EQ(expected_1, result_1.gameState());

src/software/thunderscope/binary_context_managers/BUILD

@@ -22,6 +22,7 @@ py_library(
     deps = [
         "//proto:import_all_protos",
         "//software/networking:ssl_proto_communication",
+        "//software/thunderscope/common:thread_safe_circular_buffer",
     ],
 )
 

src/software/thunderscope/binary_context_managers/game_controller.py

@@ -1,5 +1,6 @@
 from __future__ import annotations
 
+import queue
 import random
 import logging
 import os
@@ -17,8 +18,12 @@
 from software.py_constants import *
 from software.thunderscope.binary_context_managers.util import *
 from software.thunderscope.thread_safe_buffer import ThreadSafeBuffer
+from software.thunderscope.common.thread_safe_circular_buffer import (
+    ThreadSafeCircularBuffer,
+)
 
 logger = logging.getLogger(__name__)
+import itertools
 
 
 class Gamecontroller:
@@ -29,7 +34,10 @@ class Gamecontroller:
     CI_MODE_OUTPUT_RECEIVE_BUFFER_SIZE = 9000
 
     def __init__(
-        self, suppress_logs: bool = False, use_conventional_port: bool = False
+        self,
+        suppress_logs: bool = False,
+        use_conventional_port: bool = False,
+        simulator_proto_unix_io: ProtoUnixIO = None,
     ) -> None:
         """Run Gamecontroller
 
@@ -54,6 +62,14 @@ def __init__(
             buffer_size=2, protobuf_type=ManualGCCommand
         )
 
+        self.simulator_proto_unix_io = simulator_proto_unix_io
+        self.blue_team_world_buffer = ThreadSafeCircularBuffer(
+            buffer_size=1, protobuf_type=World
+        )
+        self.latest_world = None
+        self.blue_removed_robot_ids = queue.Queue()
+        self.yellow_removed_robot_ids = queue.Queue()
+
     def get_referee_port(self) -> int:
         """Sometimes, the port that we are using changes depending on context.
         We want a getter function that returns the port we are using.
@@ -95,7 +111,6 @@ def __exit__(self, type, value, traceback) -> None:
         """
         self.gamecontroller_proc.terminate()
         self.gamecontroller_proc.wait()
-
         self.ci_socket.close()
 
     def refresh(self):
@@ -119,6 +134,104 @@ def refresh(self):
             )
             manual_command = self.command_override_buffer.get(return_cached=False)
 
+    @staticmethod
+    def __update_robot_count(
+        robot_states,
+        team: Team,
+        max_robots: int,
+        removed_robot_ids: queue.Queue[int],
+        field_edge_y_meters: float,
+    ) -> None:
+        """Static method for updating the number of robots in the world state (i.e. robot_states) for a given Team.
+        This method is team & side agnostic.
+
+        :param robot_states: WorldState <Robot ID, RobotState> map protobuf to be updated.
+        :param team: the Team of robots currently in play
+        :param max_robots: The number of robots we should have on the field. Must be >= 0
+        :param removed_robot_ids: The robots (IDs) which have been removed already and can safely be re-added
+        :param field_edge_y_meters: Places new robots at this y position along the centerline
+        :return:
+        """
+        # build the robot state for placing robots at the edge of field
+        place_state = RobotState(
+            global_position=Point(x_meters=0, y_meters=field_edge_y_meters)
+        )
+        # Remove robots, as we have too many. Set robot velocities to zero to avoid any drift
+        for count, robot in enumerate(team.team_robots, start=1):
+            if count <= max_robots:
+                robot_states[robot.id].CopyFrom(robot.current_state)
+                velocity = robot_states[robot.id].global_velocity
+                velocity.x_component_meters = 0
+                velocity.y_component_meters = 0
+            else:
+                removed_robot_ids.put(robot.id)
+        # Add robots, since we are missing some
+        robots_diff: int = max_robots - len(team.team_robots)
+        if robots_diff <= 0:
+            return
+        for _ in range(robots_diff):
+            try:
+                robot_states[removed_robot_ids.get_nowait()].CopyFrom(place_state)
+            except queue.Empty:
+                return
+
+    def handle_referee(self, referee: Referee) -> None:
+        """Updates the world state based on the referee message
+        :param referee: the referee protobuf message
+        """
+        # Check that we are running with the simulator and have access to its
+        # proto unix io
+        if self.simulator_proto_unix_io is None:
+            return
+
+        # Convert the latest blue world into a WorldState we can send to the simulator and update the robots
+        self.latest_world = self.blue_team_world_buffer.get(
+            block=False, return_cached=True
+        )
+
+        max_allowed_bots_yellow: int = referee.yellow.max_allowed_bots
+        max_allowed_bots_blue: int = referee.blue.max_allowed_bots
+        # Ignore if nothing needs to be updated
+        if (
+            len(self.latest_world.friendly_team.team_robots) == max_allowed_bots_blue
+            and len(self.latest_world.enemy_team.team_robots) == max_allowed_bots_yellow
+        ):
+            return
+
+        # Populate a blank WorldState with new updated robot information
+        world_state = WorldState()
+        field_edge_y_meters: Final[int] = (
+            self.latest_world.field.field_y_length
+            - self.latest_world.field.boundary_buffer_size
+        )
+
+        self.__update_robot_count(
+            world_state.blue_robots,
+            self.latest_world.friendly_team,
+            max_allowed_bots_blue,
+            self.blue_removed_robot_ids,
+            field_edge_y_meters,
+        )
+        self.__update_robot_count(
+            world_state.yellow_robots,
+            self.latest_world.enemy_team,
+            max_allowed_bots_yellow,
+            self.yellow_removed_robot_ids,
+            -field_edge_y_meters,
+        )
+
+        # Check if we need to invert the world state
+        if referee.blue_team_on_positive_half:
+            for robot in itertools.chain(
+                world_state.blue_robots, world_state.yellow_robots
+            ):
+                robot.current_state.global_position.x_meters *= -1
+                robot.current_state.global_position.y_meters *= -1
+                robot.current_state.global_orientation.radians += math.pi
+
+        # Send out updated world state
+        self.simulator_proto_unix_io.send_proto(WorldState, world_state)
+
     def is_valid_port(self, port):
         """Determine whether or not a given port is valid
 
@@ -168,6 +281,7 @@ def __send_referee_command(data: Referee) -> None:
 
             :param data: The referee command to send
             """
+            self.handle_referee(data)
             blue_full_system_proto_unix_io.send_proto(Referee, data)
             yellow_full_system_proto_unix_io.send_proto(Referee, data)
             if autoref_proto_unix_io is not None:
@@ -187,6 +301,9 @@ def __send_referee_command(data: Referee) -> None:
         yellow_full_system_proto_unix_io.register_observer(
             ManualGCCommand, self.command_override_buffer
         )
+        blue_full_system_proto_unix_io.register_observer(
+            World, self.blue_team_world_buffer
+        )
 
     def send_gc_command(
         self,

src/software/thunderscope/common/BUILD

@@ -52,3 +52,11 @@ py_library(
         requirement("pyqt-toast-notification"),
     ],
 )
+
+py_library(
+    name = "thread_safe_circular_buffer",
+    srcs = ["thread_safe_circular_buffer.py"],
+    deps = [
+        "//software/thunderscope:thread_safe_buffer",
+    ],
+)

src/software/thunderscope/common/proto_plotter.py

@@ -100,7 +100,7 @@ def refresh(self) -> None:
         # Dump the entire buffer into a deque. This operation is fast because
         # its just consuming data from the buffer and appending it to a deque.
         for proto_class, buffer in self.buffers.items():
-            for _ in range(buffer.queue.qsize()):
+            for _ in range(buffer.size()):
                 data = self.configuration[proto_class](buffer.get(block=False))
 
                 # If named_value is new, create a plot and for the new value and