Sync Sandbox Mode across FullSystems

src/proto/world.proto

@@ -61,6 +61,11 @@ message SimulationState
     required double simulation_speed = 2 [default = 1.0];
 }
 
+message SandboxModeState
+{
+    required bool is_enabled = 1 [default = false];
+}
+
 message Pass
 {
     // The location of the passer

src/software/sensor_fusion/sensor_fusion.cpp

@@ -63,20 +63,24 @@ void SensorFusion::processSensorProto(const SensorProto& sensor_msg)
 
     updateWorld(sensor_msg.robot_status_msgs());
 
-    friendly_team.assignGoalie(friendly_goalie_id);
-    enemy_team.assignGoalie(enemy_goalie_id);
+    unsigned int unresolved_friendly_goalie_id = friendly_goalie_id;
+    unsigned int unresolved_enemy_goalie_id    = enemy_goalie_id;
 
     if (sensor_fusion_config.override_game_controller_friendly_goalie_id())
     {
         RobotId friendly_goalie_id_override = sensor_fusion_config.friendly_goalie_id();
-        friendly_team.assignGoalie(friendly_goalie_id_override);
+        unresolved_friendly_goalie_id       = friendly_goalie_id_override;
     }
 
     if (sensor_fusion_config.override_game_controller_enemy_goalie_id())
     {
         RobotId enemy_goalie_id_override = sensor_fusion_config.enemy_goalie_id();
-        enemy_team.assignGoalie(enemy_goalie_id_override);
+        unresolved_enemy_goalie_id       = enemy_goalie_id_override;
     }
+
+    friendly_team.assignGoalie(
+        resolveGoalieId(friendly_team, unresolved_friendly_goalie_id));
+    enemy_team.assignGoalie(resolveGoalieId(enemy_team, unresolved_enemy_goalie_id));
 }
 
 
@@ -470,6 +474,33 @@ BallDetection SensorFusion::invert(BallDetection ball_detection) const
     return ball_detection;
 }
 
+unsigned int SensorFusion::resolveGoalieId(const Team& team, unsigned int goalie_id)
+{
+    // If the desired goalie exists on the team, use it
+    if (team.getRobotById(goalie_id).has_value())
+    {
+        return goalie_id;
+    }
+
+    // Otherwise, find the lowest robot ID present on the team
+    const auto& all_robots = team.getAllRobots();
+    if (!all_robots.empty())
+    {
+        RobotId lowest_id = all_robots.front().id();
+        for (const auto& robot : all_robots)
+        {
+            if (robot.id() < lowest_id)
+            {
+                lowest_id = robot.id();
+            }
+        }
+        return lowest_id;
+    }
+
+    // If there are no robots on the team yet, fall back to the desired goalie ID
+    return goalie_id;
+}
+
 bool SensorFusion::teamHasBall(const Team& team, const Ball& ball)
 {
     for (const auto& robot : team.getAllRobots())

src/software/sensor_fusion/sensor_fusion.h

@@ -157,6 +157,18 @@ class SensorFusion
      */
     static bool teamHasBall(const Team& team, const Ball& ball);
 
+    /**
+     * Given a team and a desired goalie ID, returns the desired ID if a robot with
+     * that ID exists on the team. Otherwise, returns the lowest robot ID present on
+     * the team. If the team has no robots, returns the desired goalie ID as a fallback.
+     *
+     * @param team The team to check
+     * @param goalie_id The desired goalie ID
+     *
+     * @return The goalie ID to use
+     */
+    static unsigned int resolveGoalieId(const Team& team, unsigned int goalie_id);
+
     /**
      * This function controls the behavior of how the ball is being updated. If this
      * returns True, we use the position of the robot that triggers the breakbeam instead

src/software/sensor_fusion/sensor_fusion_test.cpp

@@ -174,15 +174,21 @@ class SensorFusionTest : public ::testing::Test
             friendly_robots.emplace_back(state.id, state.robot_state, current_time);
         }
         friendly_team.updateRobots(friendly_robots);
-        friendly_team.assignGoalie(0);
+        // This must be a robot ID that actually exists on the team, since
+        // SensorFusion::resolveGoalieId validates the goalie ID exists before assigning
+        // it
+        friendly_team.assignGoalie(1);
         Team enemy_team;
         std::vector<Robot> enemy_robots;
         for (const auto& state : initBlueRobotStates())
         {
             enemy_robots.emplace_back(state.id, state.robot_state, current_time);
         }
         enemy_team.updateRobots(enemy_robots);
-        enemy_team.assignGoalie(0);
+        // This must be a robot ID that actually exists on the team, since
+        // SensorFusion::resolveGoalieId validates the goalie ID exists before assigning
+        // it
+        enemy_team.assignGoalie(1);
         return World(field, ball, friendly_team, enemy_team);
     }
 
@@ -197,15 +203,19 @@ class SensorFusionTest : public ::testing::Test
             friendly_robots.emplace_back(state.id, state.robot_state, current_time);
         }
         friendly_team.updateRobots(friendly_robots);
-        friendly_team.assignGoalie(0);
+        // This must be a robot ID that actually exists on the team, since
+        // SensorFusion::resolveGoalieId validates the goalie ID before assigning it
+        friendly_team.assignGoalie(1);
         Team enemy_team;
         std::vector<Robot> enemy_robots;
         for (const auto& state : initInvertedBlueRobotStates())
         {
             enemy_robots.emplace_back(state.id, state.robot_state, current_time);
         }
         enemy_team.updateRobots(enemy_robots);
-        enemy_team.assignGoalie(0);
+        // This must be a robot ID that actually exists on the team, since
+        // SensorFusion::resolveGoalieId validates the goalie ID before assigning it
+        enemy_team.assignGoalie(1);
         return World(field, ball, friendly_team, enemy_team);
     }
 
@@ -787,8 +797,9 @@ TEST_F(SensorFusionTest, test_sensor_fusion_reset_behaviour_trigger_reset)
 {
     config.set_override_game_controller_friendly_goalie_id(true);
     config.set_override_game_controller_enemy_goalie_id(true);
-    config.set_friendly_goalie_id(0);
-    config.set_enemy_goalie_id(0);
+    // Must use a robot ID that exists on both teams (1 is the lowest ID in our test data)
+    config.set_friendly_goalie_id(1);
+    config.set_enemy_goalie_id(1);
     sensor_fusion = SensorFusion(config);
 
     SensorProto sensor_msg;

src/software/thunderscope/common/common_widgets.py

@@ -4,6 +4,7 @@
 from software.py_constants import *
 from software.thunderscope.util import color_from_gradient
 from typing import override
+import textwrap
 
 
 class FloatSlider(QSlider):
@@ -198,9 +199,24 @@ def value(self) -> float:
         return float(super(ColorProgressBar, self).value()) / self.decimals
 
 
+class StyledButton(QPushButton):
+    """A QPushButton with the toolbar button stylesheet pre-applied.
+
+    This button automatically applies the toggle button style used by toolbar
+    buttons, so callers don't need to manually call setStyleSheet with
+    get_toggle_button_style(). StyledButton defaults to the enabled state, with
+    hover highlighting applied.
+    """
+
+    def __init__(self, parent: QWidget = None):
+        super().__init__(parent)
+        self.setStyleSheet(get_toggle_button_style())
+
+
 class ToggleableButton(QPushButton):
     """A QPushButton which can be enabled or disabled
     Indicates with cursor if it is enabled or disabled
+    Auto-updates the toolbar button stylesheet based on enabled state
     """
 
     def __init__(self, enabled: bool):
@@ -210,12 +226,18 @@ def __init__(self, enabled: bool):
         """
         super(ToggleableButton, self).__init__()
         self.enabled = enabled
+        self.setStyleSheet(get_toggle_button_style(enabled))
 
     def toggle_enabled(self, enabled: bool):
-        """Toggles the enabled state of the button
+        """Toggles the enabled state of the button and updates the stylesheet
+
         :param enabled: the new enabled state
         """
         self.enabled = enabled
+        self.setStyleSheet(get_toggle_button_style(enabled))
+
+    def is_enabled(self):
+        return self.enabled
 
     @override
     def enterEvent(self, event) -> None:
@@ -498,3 +520,29 @@ def display_tooltip(event, tooltip_text):
         )
     elif str(event.type()) == "Type.Leave":
         QToolTip.hideText()
+
+
+def get_toggle_button_style(is_enabled: bool = True) -> str:
+    """Returns the stylesheet for a QPushButton based on if it's enabled or not
+
+    :param is_enabled: True if button is enabled, False if not
+    :return: the corresponding stylesheet indicating the button state
+    """
+    # the style for each toolbar button
+    return textwrap.dedent(
+        f"""
+        QPushButton {{
+            color: #969696;
+            background-color: transparent;
+            border-color: transparent;
+            icon-size: 22px;
+            border-width: 4px;
+            border-radius: 4px;
+            height: 16px;
+        }}
+        QPushButton:hover {{
+            background-color: {"#363636" if is_enabled else "transparent"};
+            border-color: {"#363636" if is_enabled else "transparent"};
+        }}
+        """
+    )

src/software/thunderscope/constants.py

@@ -25,13 +25,13 @@ class IndividualRobotMode(IntEnum):
     AI = 2
 
 
-class CameraView(Enum):
+class CameraView(StrEnum):
     """Enum for preset camera views in the 3D visualizer"""
 
-    ORTHOGRAPHIC = 1
-    LANDSCAPE_HIGH_ANGLE = 2
-    LEFT_HALF_HIGH_ANGLE = 3
-    RIGHT_HALF_HIGH_ANGLE = 4
+    ORTHOGRAPHIC = "Orthographic Top Down"
+    LANDSCAPE_HIGH_ANGLE = "Landscape High Angle"
+    LEFT_HALF_HIGH_ANGLE = "Left Half High Angle"
+    RIGHT_HALF_HIGH_ANGLE = "Right Half High Angle"
 
 
 class EstopMode(IntEnum):
@@ -194,6 +194,27 @@ class EstopMode(IntEnum):
     """
 )
 
+SANDBOX_MODE_HELP_TEXT = textwrap.dedent(
+    """
+    <h3>Sandbox Mode Controls</h3><br>
+    <br>
+    <b>All Sandbox Mode controls need <code>Ctrl + Shift</code> to work.<br>
+    <br>
+    <b><code>Double Click: </code></b>Add a new robot or remove an existing one<br>
+    <b><code>Click and Drag: </code></b>Move an existing robot around the field<br>
+    <br>
+    <b><code>Undo: </code><b>Undoes the last operation<br>
+    <b><code>Redo: </code><b>Redoes the last operation<br>
+    <b><code>Clear Field: </code><b>Removes all robots from field<br>
+    """
+)
+
+
+class TeamToAdd(StrEnum):
+    BLUE = "Blue"
+    YELLOW = "Yellow"
+
+
 THUNDERSCOPE_UI_FONT_NAME = "Roboto"
 
 

src/software/thunderscope/gl/BUILD

@@ -11,8 +11,9 @@ py_library(
         "//software/thunderscope/gl/helpers:extended_gl_view_widget",
         "//software/thunderscope/gl/layers:gl_layer",
         "//software/thunderscope/gl/layers:gl_measure_layer",
-        "//software/thunderscope/gl/widgets:gl_field_toolbar",
-        "//software/thunderscope/gl/widgets:gl_gamecontroller_toolbar",
+        "//software/thunderscope/gl/sandbox:gl_sandbox_sidebar",
+        "//software/thunderscope/gl/toolbars:gl_field_toolbar",
+        "//software/thunderscope/gl/toolbars:gl_gamecontroller_toolbar",
         "//software/thunderscope/replay:replay_controls",
         requirement("pyqtgraph"),
     ],

src/software/thunderscope/gl/gl_widget.py

@@ -14,11 +14,11 @@
 from software.thunderscope.proto_unix_io import ProtoUnixIO
 from software.thunderscope.gl.layers.gl_layer import GLLayer
 from software.thunderscope.gl.layers.gl_measure_layer import GLMeasureLayer
-from software.thunderscope.gl.widgets.gl_field_toolbar import GLFieldToolbar
+from software.thunderscope.gl.toolbars.gl_field_toolbar import GLFieldToolbar
 from software.thunderscope.replay.proto_player import ProtoPlayer
 from software.thunderscope.replay.replay_controls import ReplayControls
 from software.thunderscope.gl.helpers.extended_gl_view_widget import *
-from software.thunderscope.gl.widgets.gl_gamecontroller_toolbar import (
+from software.thunderscope.gl.toolbars.gl_gamecontroller_toolbar import (
     GLGamecontrollerToolbar,
 )
 from software.thunderscope.thread_safe_buffer import ThreadSafeBuffer
@@ -27,6 +27,7 @@
 from proto.tbots_timestamp_msg_pb2 import Timestamp
 
 from software.thunderscope.common.toast_msg_helper import success_toast
+from software.thunderscope.gl.sandbox.gl_sandbox_sidebar import GLSandboxSidebar
 from typing import override
 
 
@@ -41,15 +42,13 @@ def __init__(
         friendly_color_yellow: bool,
         frame_swap_counter: Optional[FrameTimeCounter] = None,
         player: Optional[ProtoPlayer] = None,
-        sandbox_mode: bool = False,
     ) -> None:
         """Initialize the GLWidget
 
         :param proto_unix_io: The ProtoUnixIO to send protos to
         :param friendly_color_yellow: Whether the friendly team is yellow (true) or blue (false)
         :param frame_swap_counter: A FrameTimeCounter to track the time between frame swaps
         :param player: The replay player to optionally display media controls for
-        :param sandbox_mode: Whether sandbox mode should be enabled
         """
         super().__init__()
 
@@ -101,11 +100,21 @@ def __init__(
             on_measure_mode=self.toggle_measure_mode,
             layers_menu=self.layers_menu,
             toolbars_menu=self.toolbars_menu,
-            sandbox_mode=sandbox_mode,
             replay_mode=player is not None,
             on_add_bookmark=self.add_bookmark,
         )
 
+        # Setup sandbox sidebar
+        self.sandbox_sidebar = GLSandboxSidebar(
+            parent=self.gl_view_widget,
+            widget_above=self.simulation_control_toolbar,
+            simulator_io=proto_unix_io,
+        )
+        # let toolbar update the sidebar visibility
+        self.simulation_control_toolbar.set_sidebar_visibility_callback(
+            self.sandbox_sidebar.toggle_visibility
+        )
+
         # Setup gamecontroller toolbar
         self.gamecontroller_toolbar = GLGamecontrollerToolbar(
             parent=self.gl_view_widget,
@@ -136,6 +145,10 @@ def get_sim_control_toolbar(self):
         """Returns the simulation control toolbar"""
         return self.simulation_control_toolbar
 
+    def get_sandbox_sidebar(self):
+        """Returns the sandbox mode sidebar"""
+        return self.sandbox_sidebar
+
     @override
     def keyPressEvent(self, event: QtGui.QKeyEvent) -> None:
         """Detect when a key has been pressed
@@ -270,8 +283,13 @@ def refresh(self) -> None:
 
         if self.simulation_control_toolbar:
             self.simulation_control_toolbar.refresh()
+
+        if self.gamecontroller_toolbar:
             self.gamecontroller_toolbar.refresh()
 
+        if self.sandbox_sidebar:
+            self.sandbox_sidebar.refresh()
+
         simulation_state = self.simulation_state_buffer.get(block=False)
         # Don't refresh the layers if the simulation is paused
         if simulation_state.is_playing:

src/software/thunderscope/gl/layers/BUILD

@@ -92,6 +92,7 @@ py_library(
     srcs = ["gl_sandbox_world_layer.py"],
     deps = [
         ":gl_world_layer",
+        "//software/thunderscope/gl/sandbox:local_robot_state_provider",
         requirement("pyqtgraph"),
     ],
 )