diff --git a/src/proto/world.proto b/src/proto/world.proto index 287caacbab..f8a0dc1dcc 100644 --- a/src/proto/world.proto +++ b/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 diff --git a/src/software/sensor_fusion/sensor_fusion.cpp b/src/software/sensor_fusion/sensor_fusion.cpp index 42296afa2d..7b8e5d5dc1 100644 --- a/src/software/sensor_fusion/sensor_fusion.cpp +++ b/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()) diff --git a/src/software/sensor_fusion/sensor_fusion.h b/src/software/sensor_fusion/sensor_fusion.h index 63f47cbada..4ff47e153f 100644 --- a/src/software/sensor_fusion/sensor_fusion.h +++ b/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 diff --git a/src/software/sensor_fusion/sensor_fusion_test.cpp b/src/software/sensor_fusion/sensor_fusion_test.cpp index 5d40f74d71..80ea873a95 100644 --- a/src/software/sensor_fusion/sensor_fusion_test.cpp +++ b/src/software/sensor_fusion/sensor_fusion_test.cpp @@ -174,7 +174,10 @@ 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 enemy_robots; for (const auto& state : initBlueRobotStates()) @@ -182,7 +185,10 @@ class SensorFusionTest : public ::testing::Test 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,7 +203,9 @@ 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 enemy_robots; for (const auto& state : initInvertedBlueRobotStates()) @@ -205,7 +213,9 @@ class SensorFusionTest : public ::testing::Test 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; diff --git a/src/software/thunderscope/common/common_widgets.py b/src/software/thunderscope/common/common_widgets.py index c7f7fcdb98..dd6c33bed8 100644 --- a/src/software/thunderscope/common/common_widgets.py +++ b/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"}; + }} + """ + ) diff --git a/src/software/thunderscope/constants.py b/src/software/thunderscope/constants.py index e1b590239d..809858eeec 100644 --- a/src/software/thunderscope/constants.py +++ b/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( + """ +

Sandbox Mode Controls


+
+ All Sandbox Mode controls need Ctrl + Shift to work.
+
+ Double Click: Add a new robot or remove an existing one
+ Click and Drag: Move an existing robot around the field
+
+ Undo: Undoes the last operation
+ Redo: Redoes the last operation
+ Clear Field: Removes all robots from field
+ """ +) + + +class TeamToAdd(StrEnum): + BLUE = "Blue" + YELLOW = "Yellow" + + THUNDERSCOPE_UI_FONT_NAME = "Roboto" diff --git a/src/software/thunderscope/gl/BUILD b/src/software/thunderscope/gl/BUILD index 8bfb8a6d48..2a00ea58e7 100644 --- a/src/software/thunderscope/gl/BUILD +++ b/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"), ], diff --git a/src/software/thunderscope/gl/gl_widget.py b/src/software/thunderscope/gl/gl_widget.py index 469c741b9e..a7eb043a16 100644 --- a/src/software/thunderscope/gl/gl_widget.py +++ b/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,7 +42,6 @@ def __init__( friendly_color_yellow: bool, frame_swap_counter: Optional[FrameTimeCounter] = None, player: Optional[ProtoPlayer] = None, - sandbox_mode: bool = False, ) -> None: """Initialize the GLWidget @@ -49,7 +49,6 @@ def __init__( :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: diff --git a/src/software/thunderscope/gl/layers/BUILD b/src/software/thunderscope/gl/layers/BUILD index eef31df0e2..ff6b8d5229 100644 --- a/src/software/thunderscope/gl/layers/BUILD +++ b/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"), ], ) diff --git a/src/software/thunderscope/gl/layers/gl_sandbox_world_layer.py b/src/software/thunderscope/gl/layers/gl_sandbox_world_layer.py index 3ad7371ecf..73247e98b9 100644 --- a/src/software/thunderscope/gl/layers/gl_sandbox_world_layer.py +++ b/src/software/thunderscope/gl/layers/gl_sandbox_world_layer.py @@ -1,6 +1,9 @@ import math -from typing import Optional, override +from abc import ABC, abstractmethod +from typing import Callable, Tuple, Optional, override +from dataclasses import dataclass from proto.import_all_protos import * +from proto.ssl_gc_common_pb2 import Team from pyqtgraph.Qt.QtCore import * from pyqtgraph.Qt.QtGui import * from pyqtgraph.opengl import * @@ -8,40 +11,153 @@ from software.thunderscope.gl.layers.gl_world_layer import GLWorldLayer from software.thunderscope.gl.helpers.extended_gl_view_widget import MouseInSceneEvent from software.thunderscope.proto_unix_io import ProtoUnixIO +from software.thunderscope.gl.sandbox.local_robot_state_provider import ( + local_robot_state_provider_instance, +) +from software.thunderscope.constants import Colors, DepthValues -class RobotOperation: +class Operation(ABC): + """Interface for operations that can be undone/redone""" + + @abstractmethod + def reverse(self, friendly_next_id: int, enemy_next_id: int) -> "Operation": + """Returns the reverse of this operation + + :param friendly_next_id: the next friendly robot id to use in the reversed operation + :param enemy_next_id: the next enemy robot id to use in the reversed operation + :return: the reverse of this operation + """ + pass + + +@dataclass +class RobotOperation(Operation): """An operation that changes the state of the robots on the field Contains the id of the robot to change, its new and previous positions if applicable And the id of the next robot to add after this operation completes """ - def __init__( - self, - id: int, - prev_pos: Optional[tuple[int, int]], - pos: Optional[tuple[int, int]], - next_id: int, - ): - self.type = type - self.id = id - self.prev_pos = prev_pos - self.pos = pos - self.next_id = next_id + id: int + prev_pos: Optional[QVector3D] + pos: Optional[QVector3D] + next_id: int + is_friendly: bool + + @override + def reverse(self, friendly_next_id: int, enemy_next_id: int) -> Operation: + return RobotOperation( + id=self.id, + prev_pos=self.pos, + pos=self.prev_pos, + next_id=friendly_next_id if self.is_friendly else enemy_next_id, + is_friendly=self.is_friendly, + ) + + +@dataclass +class GroupOperation(Operation): + """A group of RobotOperations that should be applied together""" + + operations: list[RobotOperation] + + @override + def reverse(self, friendly_next_id: int, enemy_next_id: int) -> Operation: + return GroupOperation( + operations=[ + RobotOperation( + id=op.id, + prev_pos=op.pos, + pos=op.prev_pos, + next_id=friendly_next_id if op.is_friendly else enemy_next_id, + is_friendly=op.is_friendly, + ) + for op in self.operations + ] + ) class EnemyAtMousePositionError(Exception): pass +class LastRobotRemoveError(Exception): + pass + + class GLSandboxWorldLayer(GLWorldLayer): """GLWorldLayer that adds functionality to add, remove, and change the state of the robots on the field""" + class SandboxWorldState: + """Wrapper around WorldState that automatically handles coordinate frame + inversion when adding robots by delegating to the parent layer's + _invert_position_if_defending_negative_half method. + """ + + def __init__( + self, + invert_fn: Callable[[QVector3D, float], Tuple[QVector3D, float]], + friendly_colour_yellow: bool, + ): + self._proto = WorldState() + self._invert_fn = invert_fn + self._friendly_colour_yellow = friendly_colour_yellow + + @property + def proto(self) -> WorldState: + """Returns the underlying WorldState proto""" + return self._proto + + def set_ball_state(self, ball_state: BallState) -> None: + """Sets the ball state in the world state + + :param ball_state: the ball state to set + """ + self._proto.ball_state.CopyFrom(ball_state) + + def add_robot( + self, robot_id: int, pos: QVector3D, orientation: float, is_friendly: bool + ) -> None: + """Adds a robot to the world state, automatically inverting the + position and orientation if the coordinate frame should be inverted + + :param robot_id: the id of the robot to add + :param pos: the position of the robot + :param orientation: the orientation of the robot (radians) + """ + converted_pos, converted_orientation = self._invert_fn( + QVector3D(pos.x(), pos.y(), 0), orientation + ) + + robot_state = RobotState( + global_position=Point( + x_meters=converted_pos.x(), + y_meters=converted_pos.y(), + ), + global_orientation=Angle(radians=converted_orientation), + ) + if self._friendly_colour_yellow == is_friendly: + self._proto.yellow_robots[robot_id].CopyFrom(robot_state) + else: + self._proto.blue_robots[robot_id].CopyFrom(robot_state) + + def remove_robot(self, robot_id: int, is_friendly: bool) -> None: + """Removes a robot from the world state + + :param robot_id: the id of the robot to remove + """ + if self._friendly_colour_yellow == is_friendly: + del self._proto.yellow_robots[robot_id] + else: + del self._proto.blue_robots[robot_id] + undo_toggle_enabled_signal = pyqtSignal(bool) redo_toggle_enabled_signal = pyqtSignal(bool) DEFAULT_ROBOT_ANGLE = 0 + MIN_ROBOT_ID = 0 + def __init__( self, name: str, @@ -64,31 +180,37 @@ def __init__( self.robot_remove_double_click = None # the selected robot for moving - self.selected_robot_id = None + self.selected_robot: Tuple[int, bool] = None self.selected_robot_pos = None self.selected_robot_plane = None self.move_in_progress = False # the currently added robots and the next id to add - self.next_id = 0 - self.curr_robot_ids = set() - # if the world has robots already, update curr_robot_ids on the first tick + self.friendly_next_id = 0 + self.enemy_next_id = 0 + + self.friendly_curr_robots: set[int] = set() + self.enemy_curr_robots: set[int] = set() + self.curr_robot_ids_map: dict[bool, set[int]] = { + True: self.friendly_curr_robots, + False: self.enemy_curr_robots, + } + # if the world has robots already, update curr robot ids on the first tick self.should_init_curr_robot_ids = True - # the local state of robots (if simulator is paused) - # map of robot id to a tuple with the robot coordinates and orientation - # or None if the robot has been removed already - # (easier to keep track of robots rather than removing the entry entirely) - self.local_robot_positions: dict[int, tuple[QVector3D, float]] = {} - - # the state of robots before running the simulator - # the robot state if only manual moves are considered - self.pre_sim_robot_positions: dict[int, tuple[QVector3D, float]] = {} - # stacks for undo and redo operations self.undo_operations = [] self.redo_operations = [] + self.sandbox_mode_enabled = False + self.should_add_friendly = True + + self._referee_defined = False + + local_robot_state_provider_instance.register_callback( + self._update_robots_graphics + ) + @override def mouse_in_scene_pressed(self, event: MouseInSceneEvent) -> None: """Requires Ctrl + Shift to be pressed along with mouse click @@ -102,23 +224,28 @@ def mouse_in_scene_pressed(self, event: MouseInSceneEvent) -> None: # forward event to super method for ball placement super().mouse_in_scene_pressed(event) + # if sandbox mode is disabled, don't do anything + if not self.sandbox_mode_enabled: + return + # only allow robot editing if Ctrl + Shift is pressed to avoid conflicting with the ball placement if not event.mouse_event.modifiers() & Qt.KeyboardModifier.ControlModifier: return - try: - # determine whether a robot was clicked - robot_id, index = self.__identify_robot(event.multi_plane_points) - except EnemyAtMousePositionError: - # if enemy robot is already at mouse position, return - return + # determine whether a robot was clicked + robot_id, index, is_friendly = self.__identify_robot(event.multi_plane_points) if robot_id is None: # if no robot was clicked self.__handle_new_robot_event(event) else: # if a robot was clicked - self.__handle_existing_robot_event(event, robot_id, index) + try: + self.__handle_existing_robot_event(event, robot_id, index, is_friendly) + except LastRobotRemoveError: + # if the user attempted to remove the last robot + # self.__display_last_remove_warning(event) + return @override def mouse_in_scene_dragged(self, event: MouseInSceneEvent) -> None: @@ -133,21 +260,23 @@ def mouse_in_scene_dragged(self, event: MouseInSceneEvent) -> None: """ super().mouse_in_scene_dragged(event) + # if sandbox mode is disabled, don't do anything + if not self.sandbox_mode_enabled: + return + # only allow robot editing if Ctrl + Shift is pressed to avoid conflicting with the ball placement if not event.mouse_event.modifiers() & Qt.KeyboardModifier.ControlModifier: return # if robot is selected - if self.selected_robot_id is not None and self.selected_robot_plane is not None: + if self.selected_robot is not None and self.selected_robot_plane is not None: + selected_robot_id, is_friendly = self.selected_robot + # get the new position on the plane that the robot was initially selected on point_on_current_plane = event.multi_plane_points[self.selected_robot_plane] # check if the mouse position is free or not - try: - robot_id, _ = self.__identify_robot([point_on_current_plane]) - except EnemyAtMousePositionError: - # if enemy robot is already at mouse position, return - return + robot_id, _, _ = self.__identify_robot([point_on_current_plane]) # skip if new position has a robot already if robot_id is not None: @@ -162,10 +291,11 @@ def mouse_in_scene_dragged(self, event: MouseInSceneEvent) -> None: # add an undo operation to restore the robot to the position before moving self.__add_undo_operation( RobotOperation( - self.selected_robot_id, + selected_robot_id, point_on_current_plane, self.selected_robot_pos, - self.next_id, + self.friendly_next_id, + is_friendly, ) ) self.undo_toggle_enabled_signal.emit(len(self.undo_operations) != 0) @@ -174,7 +304,10 @@ def mouse_in_scene_dragged(self, event: MouseInSceneEvent) -> None: # update selected robot position self.__update_world_state( - self.selected_robot_id, point_on_current_plane, self.DEFAULT_ROBOT_ANGLE + selected_robot_id, + point_on_current_plane, + self.DEFAULT_ROBOT_ANGLE, + is_friendly=is_friendly, ) @override @@ -185,12 +318,22 @@ def mouse_in_scene_released(self, event: MouseInSceneEvent) -> None: """ super().mouse_in_scene_released(event) + # if sandbox mode is disabled, don't do anything + if not self.sandbox_mode_enabled: + return + # ends the currently happening move - self.selected_robot_id = None + self.selected_robot = None self.selected_robot_pos = None self.selected_robot_plane = None self.move_in_progress = False + @override + def _get_cached_teams_from_proto(self) -> None: + super()._get_cached_teams_from_proto() + + self.__check_referee_status() + @override def refresh_graphics(self) -> None: """Calls the super class refresh graphics @@ -205,19 +348,24 @@ def refresh_graphics(self) -> None: if self.should_init_curr_robot_ids: # for robots in the world, add the ids them to curr robots for robot in self.cached_world.friendly_team.team_robots: - self.curr_robot_ids.add(robot.id) - self.pre_sim_robot_positions[robot.id] = ( - QVector3D( - robot.current_state.global_position.x_meters, - robot.current_state.global_position.y_meters, - 0, - ), - robot.current_state.global_orientation.radians, - ) + self.friendly_curr_robots.add(robot.id) - self.next_id = len(self.curr_robot_ids) + for robot in self.cached_world.enemy_team.team_robots: + self.enemy_curr_robots.add(robot.id) + + self.friendly_next_id = len(self.friendly_curr_robots) + self.enemy_next_id = len(self.enemy_curr_robots) self.should_init_curr_robot_ids = False + def __check_referee_status(self) -> None: + """Checks if a referee message has been received. + Once a referee message is received and has content, sets a flag + permanently to True indicating we know which half we are defending. + """ + referee = self.referee_buffer.get(block=False) + if referee and referee.IsInitialized(): + self._referee_defined = True + def undo(self) -> None: """Undoes the last operation Adds a corresponding opposite move to the redo list so we can redo if necessary @@ -229,23 +377,18 @@ def undo(self) -> None: # get the operation which undoes the previous one operation = self.undo_operations.pop() - # add an opposite operation to the redo list with pos and prev_pos swapped + # add the reverse operation to the redo list self.redo_operations.append( - RobotOperation( - id=operation.id, - prev_pos=operation.pos, - pos=operation.prev_pos, - next_id=self.next_id, - ) + operation.reverse(self.friendly_next_id, self.enemy_next_id) ) + # apply the operation + self.__undo_redo_internal(operation) + # enable / disable the undo and redo buttons self.undo_toggle_enabled_signal.emit(len(self.undo_operations) != 0) self.redo_toggle_enabled_signal.emit(len(self.redo_operations) != 0) - # apply the operation - self.__undo_redo_internal(operation) - def redo(self) -> None: """Redoes the last undo operation Adds a corresponding opposite move to the undo list so we can undo if necessary @@ -257,22 +400,82 @@ def redo(self) -> None: # get the operation operation = self.redo_operations.pop() - # add an opposite operation to the undo list with pos and prev_pos swapped + # add the reverse operation to the undo list self.undo_operations.append( - RobotOperation( - id=operation.id, - prev_pos=operation.pos, - pos=operation.prev_pos, - next_id=self.next_id, - ) + operation.reverse(self.friendly_next_id, self.enemy_next_id) ) + # apply the operation + self.__undo_redo_internal(operation) + # enable / disable the undo and redo buttons self.undo_toggle_enabled_signal.emit(len(self.undo_operations) != 0) self.redo_toggle_enabled_signal.emit(len(self.redo_operations) != 0) - # apply the operation - self.__undo_redo_internal(operation) + def clear_field(self) -> None: + """Removes all robots from the field. + Adds a GroupOperation to the undo list that can restore all robots. + """ + # collect current robot positions into a GroupOperation for undo + operations = {} + + # check the cached world state first + for robot_ in self.cached_world.friendly_team.team_robots: + # get the coordinates from the robot state + pos_x = robot_.current_state.global_position.x_meters + pos_y = robot_.current_state.global_position.y_meters + + operations[robot_.id] = RobotOperation( + id=robot_.id, + prev_pos=None, + pos=QVector3D( + robot_.current_state.global_position.x_meters, + robot_.current_state.global_position.y_meters, + 0, + ), + next_id=self.friendly_next_id, + is_friendly=True, + ) + + # then, update with local state + for ( + robot_id, + pos_and_orient, + ) in local_robot_state_provider_instance.get_team_state( + self.friendly_colour_yellow + ).items(): + # if the local robot has already been removed, skip it + if pos_and_orient is None: + continue + + position, _ = pos_and_orient + + operations[robot_id] = RobotOperation( + id=robot_id, + prev_pos=None, + pos=position, + next_id=self.friendly_next_id, + is_friendly=True, + ) + + if operations: + self.undo_operations.append(GroupOperation(operations=operations.values())) + self.undo_toggle_enabled_signal.emit(len(self.undo_operations) != 0) + + # clear internal state + self.friendly_curr_robots.clear() + self.enemy_curr_robots.clear() + local_robot_state_provider_instance.clear_team(self.friendly_colour_yellow) + local_robot_state_provider_instance.clear_team(not self.friendly_colour_yellow) + + # clear redo list since this is a new action + self.redo_operations.clear() + self.redo_toggle_enabled_signal.emit(False) + + # send out empty world state + world_state = self.__get_empty_world_state() + + self.simulator_io.send_proto(WorldState, world_state.proto) @override def toggle_play_state(self) -> bool: @@ -284,14 +487,38 @@ def toggle_play_state(self) -> bool: curr_play_state = super().toggle_play_state() # reset the local state - self.local_robot_positions = {} + local_robot_state_provider_instance.clear_team(self.friendly_colour_yellow) + local_robot_state_provider_instance.clear_team(not self.friendly_colour_yellow) return curr_play_state - def reset_to_pre_sim(self) -> None: - """Resets all robot positions to what they were before the simulator ran""" - for robot_id, state in self.pre_sim_robot_positions.items(): - self.__update_world_state(robot_id, state[0], state[1]) + def toggle_sandbox_mode(self) -> bool: + """Toggles sandbox mode on/off, sends a SandboxModeState proto, and syncs undo/redo enable state + + :return: the current sandbox mode state + """ + self.sandbox_mode_enabled = not self.sandbox_mode_enabled + + self.simulator_io.send_proto( + SandboxModeState, + SandboxModeState(is_enabled=self.sandbox_mode_enabled), + ) + + # resync undo / redo enabled state once sandbox mode is enabled + # as enabling sandbox mode enables the buttons + if self.sandbox_mode_enabled: + self.undo_toggle_enabled_signal.emit(len(self.undo_operations) != 0) + self.redo_toggle_enabled_signal.emit(len(self.redo_operations) != 0) + + return self.sandbox_mode_enabled + + def set_adding_team(self, team: Team) -> None: + """Updates which team new robots will be added to based on the + selected team and the friendly colour. + + :param team: the Team enum value (Team.Yellow or Team.Blue) + """ + self.should_add_friendly = (team == Team.YELLOW) == self.friendly_colour_yellow def __add_undo_operation(self, operation: RobotOperation) -> None: """Adds an undo operation to the list and emits the toggle enable signal @@ -301,23 +528,90 @@ def __add_undo_operation(self, operation: RobotOperation) -> None: self.undo_operations.append(operation) self.undo_toggle_enabled_signal.emit(len(self.undo_operations) != 0) - def __undo_redo_internal(self, operation: RobotOperation) -> None: - """Helper method to apply a RobotOperation + def __undo_redo_internal(self, operation: Operation) -> None: + """Helper method to apply an Operation Updates robot positions and the next id :param operation: the operation to apply """ - self.next_id = operation.next_id - self.__update_world_state( - operation.id, operation.pos, self.DEFAULT_ROBOT_ANGLE, clear_redo=False + if isinstance(operation, GroupOperation): + self.friendly_next_id = float("inf") + self.enemy_next_id = float("inf") + + world_state = self.__get_curr_world_state() + + for inner_op in operation.operations: + if inner_op.is_friendly: + self.friendly_next_id = int( + min(self.friendly_next_id, inner_op.next_id) + ) + else: + self.enemy_next_id = int(min(self.enemy_next_id, inner_op.next_id)) + world_state = self.__update_with_new_position( + world_state, + inner_op.id, + inner_op.pos, + self.DEFAULT_ROBOT_ANGLE, + inner_op.is_friendly, + ) + + # send out world state + self.simulator_io.send_proto(WorldState, world_state.proto) + else: + if operation.is_friendly: + self.friendly_next_id = operation.next_id + else: + self.enemy_next_id = operation.next_id + self.__update_world_state( + operation.id, + operation.pos, + self.DEFAULT_ROBOT_ANGLE, + clear_redo=False, + is_friendly=operation.is_friendly, + ) + + def __get_empty_world_state(self) -> SandboxWorldState: + """Constructs a SandboxWorldState with just the ball state filled in + from the cached world state and 1 robot placed at + the edge of the center circle on the half line + + Replaces all local states as well + + :return: the sandbox world state with ball state and 1 robot + """ + world_state = GLSandboxWorldLayer.SandboxWorldState( + self.__invert_robot_if_defending_negative_half, self.friendly_colour_yellow ) + world_state.set_ball_state(self.cached_world.ball.current_state) + + center_circle_radius = self.cached_world.field.center_circle_radius + robot_pos = QVector3D(-center_circle_radius, 0, 0) + + local_robot_state_provider_instance.clear_team(self.friendly_colour_yellow) + local_robot_state_provider_instance.clear_team(not self.friendly_colour_yellow) + + world_state = self.__update_with_new_position( + world_state, + self.MIN_ROBOT_ID, + robot_pos, + self.DEFAULT_ROBOT_ANGLE, + is_friendly=True, + ) + + self.friendly_next_id = self.MIN_ROBOT_ID + 1 + + return world_state # # # # # # # # # # # # # # # # # # # # # # # # # # ADD / REMOVE / MOVE ROBOT METHODS # # # # # # # # # # # # # # # # # # # # # # # # # # def __handle_existing_robot_event( - self, event: MouseInSceneEvent, robot_id: int, index: int + self, + event: MouseInSceneEvent, + robot_id: int, + index: int, + is_friendly: bool, ) -> None: """Handles a mouse event when a position where a robot is present is clicked Marks the robot as selected (for drag moving) @@ -327,10 +621,11 @@ def __handle_existing_robot_event( :param event: the event containing the xy-plane and other plane coordinates :param robot_id: the id of the robot that was clicked on :param index: the plane index that the robot was selected on + :param is_friendly: whether the robot is on the friendly team """ # marks the robot as selected along with the plane index that the mouse click intersected with # and its current position on that plane - self.selected_robot_id = robot_id + self.selected_robot = robot_id, is_friendly self.selected_robot_pos = event.multi_plane_points[index] self.selected_robot_plane = index @@ -339,25 +634,51 @@ def __handle_existing_robot_event( self.robot_remove_double_click and self.robot_remove_double_click == event.multi_plane_points[index] ): + # prevent removing the last robot + if len(self.curr_robot_ids_map[is_friendly]) <= 1: + self.__toggle_robot_remove_double_click() + raise LastRobotRemoveError("Trying to remove the final robot!") + # add an undo operation to add back the robot self.__add_undo_operation( RobotOperation( robot_id, None, event.multi_plane_points[index], - self.next_id, + self.friendly_next_id if is_friendly else self.enemy_next_id, + is_friendly, ) ) # remove the robot - self.__update_world_state(robot_id, None, self.DEFAULT_ROBOT_ANGLE) + self.__update_world_state( + robot_id, + None, + self.DEFAULT_ROBOT_ANGLE, + is_friendly=is_friendly, + ) # set next id to the lowest free id - self.next_id = min(self.next_id, robot_id) + if is_friendly: + self.friendly_next_id = min(self.friendly_next_id, robot_id) + else: + self.enemy_next_id = min(self.enemy_next_id, robot_id) self.__toggle_robot_remove_double_click() else: # start a remove double click self.robot_remove_double_click = event.multi_plane_points[index] QTimer.singleShot(500, self.__toggle_robot_remove_double_click) + def __display_last_remove_warning(self, event: MouseInSceneEvent) -> None: + warning = GLTextItem(font=GLWorldLayer.TEXT_GRAPHICS_QFONT, color=Colors.RED) + warning.show() + warning.setDepthValue(DepthValues.ABOVE_FOREGROUND_DEPTH) + warning.setData( + text="Can't remove last robot!", + pos=[ + event.position().x() - int(warning.width() / 2), + event.position().y() + int(warning.height() * 1.1), + ], + ) + def __handle_new_robot_event(self, event: MouseInSceneEvent) -> None: """Handles a mouse event when an empty position is clicked If double clicked, adds a new robot at that position @@ -370,32 +691,57 @@ def __handle_new_robot_event(self, event: MouseInSceneEvent) -> None: self.robot_add_double_click and self.robot_add_double_click == event.point_in_scene ): + curr_next_id = ( + self.friendly_next_id + if self.should_add_friendly + else self.enemy_next_id + ) + # add an undo operation to remove the robot that is being added self.__add_undo_operation( - RobotOperation(self.next_id, event.point_in_scene, None, self.next_id) + RobotOperation( + curr_next_id, + event.point_in_scene, + None, + curr_next_id, + self.should_add_friendly, + ) ) # add the robot self.__update_world_state( - self.next_id, event.point_in_scene, self.DEFAULT_ROBOT_ANGLE + curr_next_id, + event.point_in_scene, + self.DEFAULT_ROBOT_ANGLE, + is_friendly=self.should_add_friendly, ) - self.next_id = self.__get_next_robot_id(self.next_id) + + new_next_id = self.__get_next_robot_id( + curr_next_id, self.curr_robot_ids_map[self.should_add_friendly] + ) + + if self.should_add_friendly: + self.friendly_next_id = new_next_id + else: + self.enemy_next_id = new_next_id + self.__toggle_robot_add_double_click() else: # start a double click self.robot_add_double_click = event.point_in_scene QTimer.singleShot(500, self.__toggle_robot_add_double_click) - def __get_next_robot_id(self, curr_next_id: int) -> int: + def __get_next_robot_id(self, curr_next_id: int, curr_robot_ids: set[int]) -> int: """Gets the id of the next robot to add based on the currently added robot ids :param curr_next_id: the current next id to add + :param curr_robot_ids: the set of currently used robot ids for this team """ # start with the default next id next_id = curr_next_id + 1 # loops until a free id is found - while next_id in self.curr_robot_ids: + while next_id in curr_robot_ids: next_id += 1 return next_id @@ -410,102 +756,81 @@ def __toggle_robot_remove_double_click(self) -> None: if self.robot_remove_double_click: self.robot_remove_double_click = None - def __add_robot_to_state( - self, world_state: WorldState, id: int, pos: QVector3D, orientation: float - ) -> WorldState: - """Adds a robot with the given state and id to the given world state - To the right team based on current team color - Converts position and orientation if needed - - :param world_state: the world state to add robot to - :param id: the id of the robot to add - :param pos: the new QVector3D position of the robot - :param orientation: the new orientation of the robot (radians) - """ - # convert position and orientation if needed - ( - converted_pos, - converted_orientation, - ) = self.__invert_robot_if_defending_negative_half(pos, orientation) - # build the robot state - robot_state = RobotState( - global_position=Point( - x_meters=converted_pos.x(), - y_meters=converted_pos.y(), - ), - global_orientation=Angle(radians=converted_orientation), - ) - - if self.friendly_colour_yellow: - world_state.yellow_robots[id].CopyFrom(robot_state) - else: - world_state.blue_robots[id].CopyFrom(robot_state) - return world_state - - def __remove_robot_from_state(self, world_state: WorldState, id: int) -> WorldState: - """Removes a robot with the given id from the right team in the given world state - Based on current team color - - :param world_state: the world state to remove robot from - :param id: the id of the robot to remove - """ - if self.friendly_colour_yellow: - del world_state.yellow_robots[id] - else: - del world_state.blue_robots[id] - return world_state - - def __identify_robot( - self, multi_plane_points: list[QVector3D] + def __identify_robot_with_locals( + self, + multi_plane_points: list[QVector3D], + team: Team, + local_state_map: dict[int, tuple[QVector3D, float] | None], ) -> tuple[Optional[int], Optional[int]]: - """Identify which robot was clicked on the team + """Check local state first, then team robots, for a robot at the given position - :param multi_plane_points: points on the x-y plane and planes above it corresponding to the mouse click - :return: The robot if one is present at the mouse position, - along with the index of the plane it was identified on, - else None, None + :param multi_plane_points: points on the x-y plane and planes above it + :param team: the team to check (friendly or enemy) + :param local_state_map: local robot position map (id -> (QVector3D, float) or None) + :return: the robot id and plane index if found, else None, None """ - # first, check if there's any enemy robots being clicked on - # return None immediately if so (since we can't edit enemy robot state) - for robot_ in self.cached_world.enemy_team.team_robots: - # get the coordinates from the robot state - pos_x = robot_.current_state.global_position.x_meters - pos_y = robot_.current_state.global_position.y_meters - - # check if robot in position and return if found - index = self.__identify_robot_helper(multi_plane_points, pos_x, pos_y) - - if index is not None: - raise EnemyAtMousePositionError("Enemy robot at this position already!") - - # look for robot in local state - for robot_id, pos in self.local_robot_positions.items(): - # if the local robot has already been removed, skip it + # check local state first + for robot_id, pos in local_state_map.items(): if pos is None: continue - # check if robot in position and return if found - index = self.__identify_robot_helper( + index = self.__identify_robot_with_pos( multi_plane_points, pos[0].x(), pos[0].y() ) if index is not None: return robot_id, index - # if not found, look for robot in cached world state - for robot_ in self.cached_world.friendly_team.team_robots: - # get the coordinates from the robot state + # then check team robots + for robot_ in team.team_robots: pos_x = robot_.current_state.global_position.x_meters pos_y = robot_.current_state.global_position.y_meters - # check if robot in position and return if found - index = self.__identify_robot_helper(multi_plane_points, pos_x, pos_y) + index = self.__identify_robot_with_pos(multi_plane_points, pos_x, pos_y) if index is not None: return robot_.id, index + return None, None - def __identify_robot_helper( + def __identify_robot( + self, multi_plane_points: list[QVector3D] + ) -> tuple[Optional[int], Optional[int], bool]: + """Identify which robot was clicked on the field + + :param multi_plane_points: points on the x-y plane and planes above it corresponding to the mouse click + :return: The robot id if one is present at the mouse position, + along with the index of the plane it was identified on, + and whether the robot is friendly (True) or enemy (False), + else None, None, False + """ + # check friendly team first (with local state) + robot_id, index = self.__identify_robot_with_locals( + multi_plane_points, + self.cached_world.friendly_team, + local_robot_state_provider_instance.get_team_state( + self.friendly_colour_yellow + ), + ) + + if robot_id is not None: + return robot_id, index, True + + # check enemy team (with local state) + robot_id, index = self.__identify_robot_with_locals( + multi_plane_points, + self.cached_world.enemy_team, + local_robot_state_provider_instance.get_team_state( + not self.friendly_colour_yellow + ), + ) + + if robot_id is not None: + return robot_id, index, False + + return None, None, False + + def __identify_robot_with_pos( self, multi_plane_points, pos_x, pos_y ) -> Optional[int]: """Loops over the multi plane points given and checks if any of them are within the radius of the given robot @@ -524,51 +849,76 @@ def __identify_robot_helper( return index return None - def __update_world_state( - self, - new_robot_id: int, - new_pos: Optional[QVector3D], - new_orientation: float, - clear_redo=True, - ) -> None: - """Send out a WorldState proto with the existing robots - If new position is provided, adds a robot with the given id at the given position - Else, removes the robot with the given id from the robot state + def __get_curr_world_state(self) -> SandboxWorldState: + world_state = GLSandboxWorldLayer.SandboxWorldState( + self.__invert_robot_if_defending_negative_half, self.friendly_colour_yellow + ) - :param new_robot_id: the id of the robot to add / remove / move - :param new_pos: the new QVector3D position of the robot (None if robot to be removed) - :param new_orientation: the new orientation of the robot (radians) - :param clear_redo: If True, indicates a new action instead of an action from the undo/redo list. - clears redo list if True - """ - world_state = WorldState() + world_state = self.__get_current_world_state_for_team(world_state, True) + world_state = self.__get_current_world_state_for_team(world_state, False) + + return world_state + + def __get_current_world_state_for_team( + self, world_state: SandboxWorldState, is_friendly: bool + ) -> SandboxModeState: + team_robots = ( + self.cached_world.friendly_team.team_robots + if is_friendly + else self.cached_world.enemy_team.team_robots + ) # copy over existing robots for the current team - for robot_ in self.cached_world.friendly_team.team_robots: - world_state = self.__add_robot_to_state( - world_state, + for robot_ in team_robots: + world_state.add_robot( robot_.id, - QVector3D( + pos=QVector3D( robot_.current_state.global_position.x_meters, robot_.current_state.global_position.y_meters, 0, ), - robot_.current_state.global_orientation.radians, + orientation=robot_.current_state.global_orientation.radians, + is_friendly=is_friendly, ) # copy over any local state robots if sim is paused if not self.is_playing: - for robot_id, pos in self.local_robot_positions.items(): + for robot_id, pos in local_robot_state_provider_instance.get_team_state( + self.friendly_colour_yellow == is_friendly + ).items(): # if the robot has already been removed, skip it if pos is None: continue - world_state = self.__add_robot_to_state( - world_state, robot_id, pos[0], pos[1] + world_state.add_robot( + robot_id, pos=pos[0], orientation=pos[1], is_friendly=is_friendly ) + return world_state + + def __update_world_state( + self, + new_robot_id: int, + new_pos: Optional[QVector3D], + new_orientation: float, + clear_redo=True, + is_friendly: bool = True, + ) -> None: + """Send out a WorldState proto with the existing robots + If new position is provided, adds a robot with the given id at the given position + Else, removes the robot with the given id from the robot state + + :param new_robot_id: the id of the robot to add / remove / move + :param new_pos: the new QVector3D position of the robot (None if robot to be removed) + :param new_orientation: the new orientation of the robot (radians) + :param clear_redo: If True, indicates a new action instead of an action from the undo/redo list. + clears redo list if True + :param is_friendly: whether the robot is on the friendly team + """ + world_state = self.__get_curr_world_state() + world_state = self.__update_with_new_position( - world_state, new_robot_id, new_pos, new_orientation + world_state, new_robot_id, new_pos, new_orientation, is_friendly ) # if we've just done a new action (not undone an old action) @@ -577,15 +927,16 @@ def __update_world_state( self.redo_operations.clear() # send out world state - self.simulator_io.send_proto(WorldState, world_state) + self.simulator_io.send_proto(WorldState, world_state.proto) def __update_with_new_position( self, - world_state: WorldState, + world_state: SandboxWorldState, robot_id: int, new_pos: Optional[QVector3D], new_orientation: float = 0, - ) -> WorldState: + is_friendly: bool = True, + ) -> SandboxWorldState: """Updates the world state with the new robot position for the given id New position is defined if adding / moving a robot and None if removing one @@ -593,30 +944,37 @@ def __update_with_new_position( :param robot_id: the id of thr robot to add / move / remove :param new_pos: the new position of the robot, or None if removing :param new_orientation: the new orientation of the robot if needed (radians) + :param is_friendly: whether the robot is on the friendly team :return: the updated world state """ if new_pos: - self.curr_robot_ids.add(robot_id) - world_state = self.__add_robot_to_state( - world_state, robot_id, new_pos, new_orientation + self.curr_robot_ids_map[is_friendly].add(robot_id) + world_state.add_robot(robot_id, new_pos, new_orientation, is_friendly) + + # saves the state to local dict + local_robot_state_provider_instance.update_robot( + robot_id, + new_pos, + new_orientation, + self.friendly_colour_yellow == is_friendly, ) - - # saves the state to local and pre-sim dicts - self.pre_sim_robot_positions[robot_id] = (new_pos, new_orientation) - if not self.is_playing: - # update the local state to the converted position - self.local_robot_positions[robot_id] = ( - new_pos, - new_orientation, - ) else: # remove an existing robot - self.curr_robot_ids.remove(robot_id) - del self.pre_sim_robot_positions[robot_id] - world_state = self.__remove_robot_from_state(world_state, robot_id) + self.curr_robot_ids_map[is_friendly].remove(robot_id) + local_robot_state_provider_instance.remove_robot( + robot_id, self.friendly_colour_yellow == is_friendly + ) + world_state.remove_robot(robot_id, is_friendly) return world_state + @override + def _should_invert_coordinate_frame(self) -> bool: + if not self._referee_defined: + return False + + return super()._should_invert_coordinate_frame() + def __invert_robot_if_defending_negative_half( self, point: QVector3D, orientation: float ) -> tuple[QVector3D, float]: @@ -638,17 +996,43 @@ def __invert_robot_if_defending_negative_half( # GRAPHICS UPDATE METHODS # # # # # # # # # # # # # # # # # # # # # + def __override_cache_with_locals( + self, + local_positions: dict[int, tuple[QVector3D, float] | None], + cached_team: dict[int, tuple[float, float, float]], + ) -> None: + """Override a cached team dict with local robot positions + + :param local_positions: map of robot id to (position, orientation) or None if removed + :param cached_team: the cached team dict to override (e.g. self._cached_friendly_team) + """ + for robot_id, pos in local_positions.items(): + if pos is None and robot_id in cached_team: + del cached_team[robot_id] + elif pos is not None: + cached_team[robot_id] = ( + pos[0].x(), + pos[0].y(), + pos[1], + ) + @override def _update_robots_graphics(self) -> None: """Overrides the _update_robots_graphics method in the super class - Adds local state robots to the friendly team cache before updating the robot graphics + Adds local state robots to the team caches before updating the robot graphics """ - for robot_id, pos in self.local_robot_positions.items(): - if pos is None and robot_id in self._cached_friendly_team: - # if removed in local state, remove from dict - del self._cached_friendly_team[robot_id] - elif pos is not None: - # override position using local pos - self._cached_friendly_team[robot_id] = (pos[0].x(), pos[0].y(), pos[1]) + if not self.is_playing: + self.__override_cache_with_locals( + local_robot_state_provider_instance.get_team_state( + self.friendly_colour_yellow + ), + self._cached_friendly_team, + ) + self.__override_cache_with_locals( + local_robot_state_provider_instance.get_team_state( + not self.friendly_colour_yellow + ), + self._cached_enemy_team, + ) super()._update_robots_graphics() diff --git a/src/software/thunderscope/gl/sandbox/BUILD b/src/software/thunderscope/gl/sandbox/BUILD new file mode 100644 index 0000000000..10b257bfe6 --- /dev/null +++ b/src/software/thunderscope/gl/sandbox/BUILD @@ -0,0 +1,21 @@ +load("@thunderscope_deps//:requirements.bzl", "requirement") + +package(default_visibility = ["//visibility:public"]) + +py_library( + name = "gl_sandbox_sidebar", + srcs = ["gl_sandbox_sidebar.py"], + deps = [ + "//software/thunderscope/common:common_widgets", + requirement("pyqtgraph"), + requirement("qtawesome"), + ], +) + +py_library( + name = "local_robot_state_provider", + srcs = ["local_robot_state_provider.py"], + deps = [ + requirement("pyqtgraph"), + ], +) diff --git a/src/software/thunderscope/gl/sandbox/gl_sandbox_sidebar.py b/src/software/thunderscope/gl/sandbox/gl_sandbox_sidebar.py new file mode 100644 index 0000000000..460a56f9e4 --- /dev/null +++ b/src/software/thunderscope/gl/sandbox/gl_sandbox_sidebar.py @@ -0,0 +1,280 @@ +from typing import Callable, override +from pyqtgraph.Qt import QtCore +from pyqtgraph.Qt.QtWidgets import * +from proto.import_all_protos import * +from proto.ssl_gc_common_pb2 import Team +from software.thunderscope.thread_safe_buffer import ThreadSafeBuffer +from software.thunderscope.proto_unix_io import ProtoUnixIO +import qtawesome as qta +from software.thunderscope.common.common_widgets import ToggleableButton, StyledButton +from software.thunderscope.constants import SANDBOX_MODE_HELP_TEXT + + +class GLSandboxSidebar(QWidget): + """Sidebar widget for the sandbox mode + + Absolutely positioned to the right of its parent, overlaying on top, + taking the full vertical space. Uses a QVBoxLayout to arrange sandbox + controls vertically. + """ + + CONTENT_COLOR = "white" + BACKGROUND_COLOR = "black" + POSITION_PADDING_MULTIPLIER = 0.1 + SIDEBAR_WIDTH_RATIO = 0.2 + + def __init__( + self, parent: QWidget, widget_above: QWidget, simulator_io: ProtoUnixIO + ): + """Set up the sandbox sidebar + + :param parent: the parent widget to attach this sidebar to + :param widget_above: the widget above the sidebar for placement + :param simulator_io: the ProtoUnixIO to send/receive protos on + """ + super().__init__(parent=parent) + self.widget_above = widget_above + self.simulator_io = simulator_io + + # Setup sidebar with a vertical layout + self.setLayout(QVBoxLayout()) + + self.sidebar_enabled = False + self.sidebar_rendered = False + self._sandbox_toggle_callback: Callable[[], None] | None = None + self._toggle_add_team_callback: Callable[[Team], None] | None = None + + # Create a container widget to hold all sidebar contents + self.sidebar_container = QWidget() + self.sidebar_container.setLayout(QVBoxLayout()) + self.sidebar_container.setObjectName("sidebarContainer") + + # Style with a dark background + border so it's visible when overlaying + self.sidebar_container.setStyleSheet( + f"#sidebarContainer {{" + f" background-color: {self.BACKGROUND_COLOR};" + f" border: 2px solid {self.CONTENT_COLOR};" + f" border-radius: 5px;" + f" padding: 15px 10px;" + f" padding-top: 10px;" + f"}}" + ) + self.sidebar_container.setAttribute( + QtCore.Qt.WidgetAttribute.WA_StyledBackground + ) + self.sidebar_container.setSizePolicy( + QSizePolicy.Policy.MinimumExpanding, QSizePolicy.Policy.Preferred + ) + + # Setup sandbox mode toggle checkbox + self.checkbox_layout = QHBoxLayout() + self.checkbox_layout.setAlignment(QtCore.Qt.AlignmentFlag.AlignCenter) + self.checkbox_layout.addWidget(QLabel("Enable Sandbox Mode: ")) + + self.sandbox_mode_checkbox = QCheckBox() + self.sandbox_mode_checkbox.setChecked(False) + self.sandbox_mode_checkbox.stateChanged.connect(self.toggle_sandbox_mode) + self.checkbox_layout.addWidget(self.sandbox_mode_checkbox) + self.sidebar_container.layout().addStretch() + self.sidebar_container.layout().addLayout(self.checkbox_layout) + + self.help_layout = QHBoxLayout() + self.help_layout.setAlignment(QtCore.Qt.AlignmentFlag.AlignCenter) + self.help_layout.addWidget(QLabel("How to Use: ")) + + self.help_button = StyledButton() + self.help_button.setToolTip("Help") + self.help_button.setIcon(qta.icon("mdi6.help-circle", color=self.CONTENT_COLOR)) + self.help_layout.addWidget(self.help_button) + self.help_button.clicked.connect( + lambda: QMessageBox.information( + self.window(), "Help", SANDBOX_MODE_HELP_TEXT + ) + ) + self.sidebar_container.layout().addStretch() + self.sidebar_container.layout().addLayout(self.help_layout) + + # Setup pause button + self.pause_button = ToggleableButton(False) + self.toggle_pause_button(True) + # buffer for the simulator pause / play state + self.simulation_state_buffer = ThreadSafeBuffer(5, SimulationState) + # buffer for the sandbox mode enabled state + self.sandbox_mode_state_buffer = ThreadSafeBuffer(5, SandboxModeState) + + # Setup Undo button + self.undo_button_enabled = False + self.undo_button = ToggleableButton(False) + self.undo_button.setToolTip("Undo") + self.undo_button.setIcon( + qta.icon("mdi6.undo-variant", color=self.CONTENT_COLOR) + ) + + # Setup Redo button + self.redo_button_enabled = False + self.redo_button = ToggleableButton(False) + self.redo_button.setToolTip("Redo") + self.redo_button.setIcon( + qta.icon("mdi6.redo-variant", color=self.CONTENT_COLOR) + ) + + button_layout = QHBoxLayout() + button_layout.addWidget(self.pause_button) + button_layout.addWidget(self.undo_button) + button_layout.addWidget(self.redo_button) + self.sidebar_container.layout().addStretch() + self.sidebar_container.layout().addLayout(button_layout) + self.sidebar_container.layout().addStretch() + + # Setup team selector for adding robots + self.team_label = QLabel("Add to Team:") + self.team_label.setAlignment(QtCore.Qt.AlignmentFlag.AlignCenter) + self.sidebar_container.layout().addWidget(self.team_label) + + self.team_group = QButtonGroup() + + team_buttons = [] + for team in [Team.BLUE, Team.YELLOW]: + radio_button = QRadioButton(Team.Name(team)) + self.team_group.addButton(radio_button, team) + self.sidebar_container.layout().addWidget(radio_button) + team_buttons.append(radio_button) + + self.team_group.idClicked.connect(self.set_adding_team) + team_buttons[0].setChecked(True) + + self.team_combo = QComboBox() + self.team_combo.addItem(Team.Name(Team.BLUE), Team.BLUE) + self.team_combo.addItem(Team.Name(Team.YELLOW), Team.YELLOW) + self.team_combo.currentIndexChanged.connect(self.set_adding_team) + self.sidebar_container.layout().addWidget(self.team_combo) + + # Setup Clear Field button + self.clear_field_button = ToggleableButton(False) + self.clear_field_button.setToolTip("Clears all robots from the field") + self.clear_field_button.setText("Clear Field") + self.sidebar_container.layout().addStretch() + self.sidebar_container.layout().addWidget(self.clear_field_button) + + # Add the container to the main layout, with stretch underneath + self.layout().addWidget(self.sidebar_container) + self.layout().addStretch() + + self.hide() + + # Listen for parent resize events to reposition + parent.installEventFilter(self) + + def refresh(self) -> None: + """Refreshes the UI to move the sidebar and update the pause button and sandbox mode state""" + if not self.sidebar_rendered: + if self.sidebar_enabled: + self.reposition() + self.show() + else: + self.hide() + self.sidebar_rendered = True + + # update the pause button state + simulation_state = self.simulation_state_buffer.get( + block=False, return_cached=False + ) + if simulation_state: + self.toggle_pause_button(simulation_state.is_playing) + + # update the sandbox mode state + sandbox_mode_state = self.sandbox_mode_state_buffer.get( + block=False, return_cached=False + ) + if sandbox_mode_state: + self.sandbox_mode_checkbox.setChecked(sandbox_mode_state.is_enabled) + self.pause_button.toggle_enabled(sandbox_mode_state.is_enabled) + self.clear_field_button.toggle_enabled(sandbox_mode_state.is_enabled) + self.undo_button.toggle_enabled( + self.undo_button_enabled and sandbox_mode_state.is_enabled + ) + self.redo_button.toggle_enabled( + self.redo_button_enabled and sandbox_mode_state.is_enabled + ) + + def set_adding_team(self, index: int) -> None: + """Called when the team combo box selection changes. + Invokes the registered callback with the selected Team enum value. + + :param index: the index of the selected team + """ + if self._toggle_add_team_callback: + self._toggle_add_team_callback(self.team_combo.currentData()) + + def set_add_team_callback(self, callback: Callable[[Team], None]) -> None: + """Sets the callback to call when the team selection changes. + + :param callback: A callable that takes the selected Team enum value. + """ + self._toggle_add_team_callback = callback + + def set_sandbox_toggle_callback(self, callback: Callable[[], None]) -> None: + """Sets the callback to call when sandbox mode is toggled. + + :param callback: A callable with no arguments that toggles sandbox mode. + """ + self._sandbox_toggle_callback = callback + + def toggle_sandbox_mode(self) -> None: + """Toggle sandbox mode on/off by calling the toggle callback.""" + if self._sandbox_toggle_callback: + self._sandbox_toggle_callback() + + def toggle_pause_button(self, is_playing: bool) -> None: + """Toggles the state of the pause button by updating its text and icon + + :param is_playing: True if the button is in the Play state, False if its in the Pause state + """ + self.pause_button.setToolTip("Pause" if is_playing else "Play") + self.pause_button.setIcon( + qta.icon( + "fa6s.pause" if is_playing else "fa5s.play", + color=self.CONTENT_COLOR, + ) + ) + + def toggle_visibility(self, enabled: bool): + """Toggle the sidebar visibility""" + self.sidebar_enabled = enabled + self.sidebar_rendered = False + + def reposition(self): + """Position to the right edge of the parent, taking full height""" + parent = self.parent() + width = int(parent.width() * self.SIDEBAR_WIDTH_RATIO) + self.setGeometry( + parent.geometry().right() - width, + self.widget_above.height(), + width, + parent.height(), + ) + + @override + def eventFilter(self, obj, event): + """Reposition when the parent is resized""" + if obj is self.parent() and event.type() == QtCore.QEvent.Resize: + self.reposition() + return super().eventFilter(obj, event) + + def toggle_undo_enabled(self, enabled: bool) -> None: + """Callback function to enable / disable the undo button based on the given state + + :param enabled: if the undo button is enabled or not + """ + self.undo_button_enabled = enabled + self.undo_button.toggle_enabled(enabled) + self.undo_button.repaint() + + def toggle_redo_enabled(self, enabled: bool) -> None: + """Callback function to enable / disable the redo button based on the given state + + :param enabled: if the redo button is enabled or not + """ + self.redo_button_enabled = enabled + self.redo_button.toggle_enabled(enabled) + self.redo_button.repaint() diff --git a/src/software/thunderscope/gl/sandbox/local_robot_state_provider.py b/src/software/thunderscope/gl/sandbox/local_robot_state_provider.py new file mode 100644 index 0000000000..a7825821db --- /dev/null +++ b/src/software/thunderscope/gl/sandbox/local_robot_state_provider.py @@ -0,0 +1,96 @@ +from typing import Callable + +from pyqtgraph.Qt.QtGui import QVector3D + + +class LocalRobotStateProvider: + """A singleton class that provides local robot state management + + Used to maintain the state of robots when the simulator is paused, + so sandbox edits (add, remove, move robots) persist while paused. + """ + + def __init__(self) -> None: + """Constructs a LocalRobotStateProvider with empty state""" + # map of robot id to a tuple with the robot coordinates and orientation + # or None if the robot has been removed already + self.yellow_robot_states: dict[int, tuple[QVector3D, float] | None] = {} + self.blue_robot_states: dict[int, tuple[QVector3D, float] | None] = {} + self._callbacks: list[Callable[[], None]] = [] + + def register_callback(self, callback: Callable[[], None]) -> None: + """Register a callback to be invoked when local robot state changes + + :param callback: function to call when robot state changes + """ + self._callbacks.append(callback) + + def _invoke_callbacks(self) -> None: + """Invoke all registered callbacks""" + for callback in self._callbacks: + callback() + + def update_robot( + self, + robot_id: int, + position: QVector3D, + orientation: float, + is_yellow: bool, + ) -> None: + """Add or update a robot's local state + + :param robot_id: the id of the robot + :param position: the new position of the robot + :param orientation: the new orientation of the robot (radians) + :param is_yellow: whether the robot is on the yellow team + """ + if is_yellow: + self.yellow_robot_states[robot_id] = (position, orientation) + else: + self.blue_robot_states[robot_id] = (position, orientation) + + self._invoke_callbacks() + + def remove_robot(self, robot_id: int, is_yellow: bool) -> None: + """Mark a robot as removed in local state + + :param robot_id: the id of the robot to remove + :param is_yellow: whether the robot is on the yellow team + """ + if is_yellow: + self.yellow_robot_states[robot_id] = None + else: + self.blue_robot_states[robot_id] = None + + self._invoke_callbacks() + + def get_team_state( + self, is_yellow: bool + ) -> dict[int, tuple[QVector3D, float] | None]: + """Get the full local state dict for a team + + :param is_yellow: whether to return the yellow team state + :return: the dict of robot id to (position, orientation) or None + """ + if is_yellow: + return self.yellow_robot_states + else: + return self.blue_robot_states + + def clear_team(self, is_yellow: bool) -> None: + """Clear all local state for a team + + :param is_yellow: whether to clear the yellow team state + """ + if is_yellow: + self.yellow_robot_states.clear() + else: + self.blue_robot_states.clear() + + def clear_all(self) -> None: + """Clear all local robot state""" + self.yellow_robot_states.clear() + self.blue_robot_states.clear() + + +local_robot_state_provider_instance = LocalRobotStateProvider() diff --git a/src/software/thunderscope/gl/toolbars/BUILD b/src/software/thunderscope/gl/toolbars/BUILD new file mode 100644 index 0000000000..d1cbd46628 --- /dev/null +++ b/src/software/thunderscope/gl/toolbars/BUILD @@ -0,0 +1,38 @@ +load("@thunderscope_deps//:requirements.bzl", "requirement") + +package(default_visibility = ["//visibility:public"]) + +py_library( + name = "gl_toolbar", + srcs = ["gl_toolbar.py"], + deps = [ + requirement("pyqtgraph"), + ], +) + +py_library( + name = "gl_field_toolbar", + srcs = ["gl_field_toolbar.py"], + deps = [ + ":gl_toolbar", + "//software/thunderscope/common:common_widgets", + requirement("pyqtgraph"), + requirement("qtawesome"), + ], +) + +py_library( + name = "gl_gamecontroller_toolbar", + srcs = ["gl_gamecontroller_toolbar.py"], + deps = [ + ":gl_toolbar", + "//proto:import_all_protos", + "//software/networking:ssl_proto_communication", + "//software/thunderscope/binary_context_managers:game_controller", + "//software/thunderscope/common:common_widgets", + "//software/thunderscope/gl/widgets:gl_runtime_installer", + "//software/thunderscope/gl/widgets:gl_runtime_selector", + requirement("pyqtgraph"), + requirement("qtawesome"), + ], +) diff --git a/src/software/thunderscope/gl/widgets/gl_field_toolbar.py b/src/software/thunderscope/gl/toolbars/gl_field_toolbar.py similarity index 53% rename from src/software/thunderscope/gl/widgets/gl_field_toolbar.py rename to src/software/thunderscope/gl/toolbars/gl_field_toolbar.py index 962992b995..2f9ff2064b 100644 --- a/src/software/thunderscope/gl/widgets/gl_field_toolbar.py +++ b/src/software/thunderscope/gl/toolbars/gl_field_toolbar.py @@ -8,10 +8,16 @@ THUNDERSCOPE_HELP_TEXT, SIMULATION_SPEEDS, ) -from software.thunderscope.common.common_widgets import ToggleableButton -from software.thunderscope.gl.widgets.gl_toolbar import GLToolbar +from software.thunderscope.common.common_widgets import ( + StyledButton, +) +from software.thunderscope.gl.toolbars.gl_toolbar import GLToolbar import qtawesome as qta +# Sandbox status label colors +ON_COLOR = "#00D000" +OFF_COLOR = "#FF0000" + class GLFieldToolbar(GLToolbar): """Toolbar for the GL Field Widget @@ -34,9 +40,6 @@ def __init__( """Set up the toolbar with these buttons: - Layers select menu - - Undo - - Pause - - Redo - Help - Measure Mode Toggle - Camera View Select menu @@ -47,100 +50,74 @@ def __init__( :param on_measure_mode: the callback function for when measure mode is toggled :param layers_menu: the QMenu for the layers menu selection :param toolbars_menu: the QMenu for the toolbars menu selection - :param sandbox_mode: if sandbox mode should be enabled :param replay_mode: if replay mode is enabled :param on_add_bookmark: the callback function when adding a bookmark """ super(GLFieldToolbar, self).__init__(parent=parent) # Setup Layers button for toggling visibility of layers - self.layers_button = QPushButton() + self.layers_button = StyledButton() self.layers_button.setText("Layers") - self.layers_button.setStyleSheet(self.get_button_style()) self.layers_button.setMenu(layers_menu) # Set up View button for setting the camera position to standard views - self.camera_view_button = QPushButton() + self.camera_view_button = StyledButton() self.camera_view_button.setToolTip("View") self.camera_view_button.setIcon( qta.icon("msc.device-camera-video", color=self.BUTTON_ICON_COLOR) ) - self.camera_view_button.setStyleSheet(self.get_button_style()) self.camera_view_menu = QMenu() self.camera_view_button.setMenu(self.camera_view_menu) - self.camera_view_actions = [ - QtGui.QAction("[1] Orthographic Top Down"), - QtGui.QAction("[2] Landscape High Angle"), - QtGui.QAction("[3] Left Half High Angle"), - QtGui.QAction("[4] Right Half High Angle"), - ] - self.camera_view_actions[0].triggered.connect( - lambda: on_camera_view_change(CameraView.ORTHOGRAPHIC) - ) - self.camera_view_actions[1].triggered.connect( - lambda: on_camera_view_change(CameraView.LANDSCAPE_HIGH_ANGLE) - ) - self.camera_view_actions[2].triggered.connect( - lambda: on_camera_view_change(CameraView.LEFT_HALF_HIGH_ANGLE) - ) - self.camera_view_actions[3].triggered.connect( - lambda: on_camera_view_change(CameraView.RIGHT_HALF_HIGH_ANGLE) - ) - for camera_view_action in self.camera_view_actions: + for idx, camera_view in enumerate(CameraView): + camera_view_action = QtGui.QAction(f"[{idx}] {camera_view.value}") + camera_view_action.triggered.connect( + lambda: on_camera_view_change(camera_view) + ) self.camera_view_menu.addAction(camera_view_action) # Setup Measure button for enabling/disabling measure mode - self.measure_button = QPushButton() + self.measure_button = StyledButton() self.measure_button.setToolTip("Measure") self.measure_button.setIcon( qta.icon("ph.ruler-light", color=self.BUTTON_ICON_COLOR) ) - self.measure_button.setStyleSheet(self.get_button_style()) self.measure_button.setShortcut("m") self.measure_button.clicked.connect(lambda: on_measure_mode()) # Setup Help button - self.help_button = QPushButton() + self.help_button = StyledButton() self.help_button.setToolTip("Help") self.help_button.setIcon( qta.icon("mdi.help-circle", color=self.BUTTON_ICON_COLOR) ) - self.help_button.setStyleSheet(self.get_button_style()) self.help_button.clicked.connect( lambda: QMessageBox.information( self.window(), "Help", THUNDERSCOPE_HELP_TEXT ) ) - # Setup pause button - self.pause_button = QPushButton() - self.pause_button.setStyleSheet(self.get_button_style()) - self.toggle_pause_button(True) - # buffer for the simulator pause / play state + # buffer for the simulator state self.simulation_state_buffer = ThreadSafeBuffer(5, SimulationState) # Setup Toolbars button for toggling visibility of toolbars - self.toolbars_button = QPushButton() + self.toolbars_button = StyledButton() self.toolbars_button.setText("Toolbars") self.toolbars_menu = QMenu() self.toolbars_menu_checkboxes = {} self.toolbars_button.setMenu(toolbars_menu) - self.toolbars_button.setStyleSheet(self.get_button_style()) if not replay_mode: - self.bookmark_button = QPushButton() + self.bookmark_button = StyledButton() self.bookmark_button.setIcon( qta.icon(("fa6.bookmark"), color=self.BUTTON_ICON_COLOR) ) self.bookmark_button.setShortcut("b") - self.bookmark_button.setStyleSheet(self.get_button_style()) self.bookmark_button.clicked.connect(on_add_bookmark) # Setup simulation speed button and menu self.sim_speed_menu = QMenu() - self.sim_speed_button = QPushButton() + self.sim_speed_button = StyledButton() self.sim_speed_button.setText("Speed: 1.00x") - self.sim_speed_button.setStyleSheet(self.get_button_style()) self.sim_speed_button.setMenu(self.sim_speed_menu) self.sim_speed_button.setToolTip("Simulation Speed") @@ -154,44 +131,41 @@ def __init__( lambda new_speed=speed: self.speed_callback(new_speed), ) - # if sandbox mode, set up the sandbox control buttons - if sandbox_mode: - # Setup Undo button - self.undo_button = ToggleableButton(False) - self.undo_button.setToolTip("Undo") - self.undo_button.setIcon( - qta.icon("mdi6.undo-variant", color=self.BUTTON_ICON_COLOR) - ) - self.undo_button.setStyleSheet(self.get_button_style(False)) + self.sandbox_sidebar_visible = False + self.sidebar_visibility_callback = None - # Setup Redo button - self.redo_button = ToggleableButton(False) - self.redo_button.setToolTip("Redo") - self.redo_button.setIcon( - qta.icon("mdi6.redo-variant", color=self.BUTTON_ICON_COLOR) - ) - self.redo_button.setStyleSheet(self.get_button_style(False)) - - self.reset_button = QPushButton() - self.reset_button.setToolTip("Reset") - self.reset_button.setIcon( - qta.icon( - "ph.arrow-counter-clockwise-fill", color=self.BUTTON_ICON_COLOR - ) - ) - self.reset_button.setStyleSheet(self.get_button_style()) + # buffer for the sandbox mode enabled state + self.sandbox_mode_state_buffer = ThreadSafeBuffer(5, SandboxModeState) + + self.sidebar_button_container = QWidget() + sidebar_button_layout = QHBoxLayout() + sidebar_button_layout.setContentsMargins(0, 0, 0, 0) + sidebar_button_layout.setSpacing(4) + + # sandbox mode title + sidebar_button_layout.addWidget(QLabel("Sandbox Mode")) + + # label to indicate sandbox mode state + self.sandbox_sidebar_label = QLabel() + sidebar_button_layout.addWidget(self.sandbox_sidebar_label) + + # button to show sidebar + self.sidebar_open_button = StyledButton() + self.sidebar_open_button.setToolTip("Toggle Sandbox Sidebar") + self.sidebar_open_button.clicked.connect(self.toggle_sidebar_visibility) + self.__update_sidebar_open_button() + sidebar_button_layout.addWidget(self.sidebar_open_button) + + self.sidebar_button_container.setLayout(sidebar_button_layout) + + # turn off sandbox mode as default + self.set_sandbox_mode_enabled(False) # Setup toolbar self.layout().addWidget(self.layers_button) + self.add_separator(self.layout()) self.layout().addWidget(self.toolbars_button) - self.layout().addStretch() - if sandbox_mode: - self.layout().addWidget(self.sim_speed_button) - self.layout().addWidget(self.reset_button) - self.layout().addWidget(self.undo_button) - self.layout().addWidget(self.pause_button) - self.layout().addWidget(self.redo_button) - + self.add_separator(self.layout()) self.layout().addWidget(self.help_button) self.layout().addWidget(self.measure_button) self.layout().addWidget(self.camera_view_button) @@ -199,29 +173,25 @@ def __init__( if not replay_mode: self.layout().addWidget(self.bookmark_button) + self.layout().addStretch() + self.layout().addWidget(self.sidebar_button_container) + @override def refresh(self) -> None: - """Refreshes the UI for all the toolbar icons and updates toolbar position""" - # update the pause button state + """Updates the sim speed and sandbox mode state""" simulation_state = self.simulation_state_buffer.get( block=False, return_cached=False ) if simulation_state: - self.toggle_pause_button(simulation_state.is_playing) self.update_simulation_speed(simulation_state.simulation_speed) - def toggle_pause_button(self, is_playing: bool) -> None: - """Toggles the state of the pause button by updating its text and icon - - :param is_playing: True if the button is in the Play state, False if its in the Pause state - """ - self.pause_button.setToolTip("Pause" if is_playing else "Play") - self.pause_button.setIcon( - qta.icon( - "fa6s.pause" if is_playing else "fa5s.play", - color=self.BUTTON_ICON_COLOR, - ) + sandbox_mode_state = self.sandbox_mode_state_buffer.get( + block=False, return_cached=False ) + if sandbox_mode_state: + self.set_sandbox_mode_enabled(sandbox_mode_state.is_enabled) + + self.setGeometry(0, 0, self.parentWidget().width(), self.height()) def update_simulation_speed(self, speed: float) -> None: """Updates the simulation speed label @@ -230,27 +200,59 @@ def update_simulation_speed(self, speed: float) -> None: """ self.sim_speed_button.setText(f"Speed: {speed:.2f}x") - def toggle_undo_enabled(self, enabled: bool) -> None: - """Callback function to enable / disable the undo button based on the given state + def set_speed_callback(self, callback: Callable[[float], None]) -> None: + """Sets the callback function for updating the simulation speed - :param enabled: if the undo button is enabled or not + :param callback: the callback function to update the simulation speed """ - self.undo_button.toggle_enabled(enabled) - self.undo_button.setStyleSheet(self.get_button_style(enabled)) - self.undo_button.repaint() + self.speed_callback = callback - def toggle_redo_enabled(self, enabled: bool) -> None: - """Callback function to enable / disable the redo button based on the given state + def set_sidebar_visibility_callback( + self, callback: Callable[[float], None] + ) -> None: + """Sets the callback function for toggling sidebar visibility - :param enabled: if the redo button is enabled or not + :param callback: the callback function for toggling sidebar visibility """ - self.redo_button.toggle_enabled(enabled) - self.redo_button.setStyleSheet(self.get_button_style(enabled)) - self.redo_button.repaint() + self.sidebar_visibility_callback = callback - def set_speed_callback(self, callback: Callable[[float], None]) -> None: - """Sets the callback function for updating the simulation speed + def toggle_sidebar_visibility(self) -> None: + """Toggles the sandbox sidebar visibility between shown and hidden - :param callback: the callback function to update the simulation speed + Flips the internal visibility state, updates the sidebar open button + icon to reflect the new state, and invokes the sidebar visibility + callback if one has been set. """ - self.speed_callback = callback + self.sandbox_sidebar_visible = not self.sandbox_sidebar_visible + + self.__update_sidebar_open_button() + + if self.sidebar_visibility_callback: + self.sidebar_visibility_callback(self.sandbox_sidebar_visible) + + def __update_sidebar_open_button(self) -> None: + """Updates the sidebar open button icon to reflect current visibility + + Shows an up icon when the sidebar is visible, and a + down icon when it is hidden. + """ + self.sidebar_open_button.setIcon( + qta.icon( + ( + "fa6s.chevron-up" + if self.sandbox_sidebar_visible + else "fa6s.chevron-down" + ), + color=self.BUTTON_ICON_COLOR, + ) + ) + + def set_sandbox_mode_enabled(self, enabled: bool) -> None: + """Sets the sandbox enabled state and updates the label + + :param enabled: whether sandbox mode is enabled + """ + self.sandbox_enabled = enabled + color = ON_COLOR if enabled else OFF_COLOR + self.sandbox_sidebar_label.setText("On" if enabled else "Off") + self.sandbox_sidebar_label.setStyleSheet(f"color: {color}; font-weight: bold;") diff --git a/src/software/thunderscope/gl/widgets/gl_gamecontroller_toolbar.py b/src/software/thunderscope/gl/toolbars/gl_gamecontroller_toolbar.py similarity index 91% rename from src/software/thunderscope/gl/widgets/gl_gamecontroller_toolbar.py rename to src/software/thunderscope/gl/toolbars/gl_gamecontroller_toolbar.py index d1aed796fd..ee2fb5f46a 100644 --- a/src/software/thunderscope/gl/widgets/gl_gamecontroller_toolbar.py +++ b/src/software/thunderscope/gl/toolbars/gl_gamecontroller_toolbar.py @@ -5,11 +5,12 @@ from typing import Callable, override import webbrowser from software.thunderscope.gl.widgets.gl_runtime_selector import GLRuntimeSelectorDialog -from software.thunderscope.gl.widgets.gl_toolbar import GLToolbar +from software.thunderscope.gl.toolbars.gl_toolbar import GLToolbar from software.thunderscope.proto_unix_io import ProtoUnixIO from software.thunderscope.gl.widgets.gl_runtime_installer import ( GLRuntimeInstallerDialog, ) +from software.thunderscope.common.common_widgets import ToggleableButton, StyledButton import qtawesome as qta @@ -82,9 +83,8 @@ def __init__( # set up the menu for selecting plays self.plays_menu = QMenu() - self.plays_menu_button = QPushButton() + self.plays_menu_button = StyledButton() self.plays_menu_button.setText("Plays") - self.plays_menu_button.setStyleSheet(self.get_button_style()) self.plays_menu_button.setMenu(self.plays_menu) # add play items for each team color @@ -118,17 +118,17 @@ def __init__( self.__toggle_normal_start_button() self.layout().addWidget(QLabel("Gamecontroller")) - self.__add_separator(self.layout()) + self.add_separator(self.layout()) self.layout().addWidget(self.stop_button) self.layout().addWidget(self.halt_button) self.layout().addWidget(self.force_start_button) - self.__add_separator(self.layout()) + self.add_separator(self.layout()) self.layout().addWidget(self.plays_menu_button) self.layout().addWidget(self.normal_start_button) - self.__add_separator(self.layout()) + self.add_separator(self.layout()) self.layout().addWidget(self.gc_browser_button) self.layout().addStretch() - self.__add_separator(self.layout()) + self.add_separator(self.layout()) self.layout().addWidget(self.runtime_installer_button) self.layout().addWidget(self.runtime_selector_button) @@ -137,15 +137,6 @@ def refresh(self) -> None: """Refreshes the UI to update toolbar position""" self.move(0, self.parentWidget().geometry().bottom() - self.height()) - def __add_separator(self, layout: QBoxLayout) -> None: - """Adds a separator line with enough spacing to the given layout - - :param layout: the layout to add the separator to - """ - layout.addSpacing(10) - layout.addWidget(QLabel("|")) - layout.addSpacing(10) - def __add_plays_menu_items(self, is_blue: bool) -> None: """Initializes the plays menu with the available plays for the given team @@ -198,9 +189,7 @@ def __plays_menu_handler( def __toggle_normal_start_button(self) -> None: """Toggles the enabled / disabled state of the Normal Start button""" self.normal_start_enabled = not self.normal_start_enabled - self.normal_start_button.setStyleSheet( - self.get_button_style(self.normal_start_enabled) - ) + self.normal_start_button.toggle_enabled(self.normal_start_enabled) self.normal_start_button.setIcon( qta.icon( "fa5s.play", @@ -216,7 +205,7 @@ def __setup_icon_button( tooltip: str, callback: Callable[[], None], display_text: str = None, - ) -> QPushButton: + ) -> StyledButton: """Sets up a button with the given name and callback :param icon: the icon displayed on the button @@ -225,14 +214,14 @@ def __setup_icon_button( :param display_text: optional param if button needs both text and an icon :return: the button """ - button = QPushButton() + button = ToggleableButton(enabled=True) button.setIcon(icon) button.setToolTip(tooltip) - button.setStyleSheet(self.get_button_style()) button.clicked.connect(callback) if display_text: button.setText(display_text) + return button def __send_stop_command(self) -> None: diff --git a/src/software/thunderscope/gl/toolbars/gl_toolbar.py b/src/software/thunderscope/gl/toolbars/gl_toolbar.py new file mode 100644 index 0000000000..466b9fda4b --- /dev/null +++ b/src/software/thunderscope/gl/toolbars/gl_toolbar.py @@ -0,0 +1,41 @@ +from pyqtgraph.Qt import QtCore +from pyqtgraph.Qt.QtWidgets import * + + +class GLToolbar(QWidget): + """Base class for a toolbar in our UI + + Has a refresh method to update UI + Has button colors / styles and initializes base formatting + """ + + BUTTON_ICON_COLOR = "white" + DISABLED_BUTTON_ICON_COLOR = "#969696" + + def __init__(self, parent: QWidget): + """Sets the base formatting for a toolbar + + :param parent: the parent to overlay this toolbar over + """ + super(GLToolbar, self).__init__(parent=parent) + + # Setup toolbar + self.setSizePolicy( + QSizePolicy.Policy.MinimumExpanding, QSizePolicy.Policy.Fixed + ) + self.setStyleSheet("background-color: rgba(0,0,0,0);" "padding: 0px;") + self.setAttribute(QtCore.Qt.WidgetAttribute.WA_StyledBackground) + self.setLayout(QHBoxLayout()) + + def refresh(self) -> None: + """Refreshes the UI (overridden by child classes)""" + raise NotImplementedError("Subclasses must implement this method!") + + def add_separator(self, layout: QBoxLayout) -> None: + """Adds a separator line with enough spacing to the given layout + + :param layout: the layout to add the separator to + """ + layout.addSpacing(10) + layout.addWidget(QLabel("|")) + layout.addSpacing(10) diff --git a/src/software/thunderscope/gl/widgets/BUILD b/src/software/thunderscope/gl/widgets/BUILD index 1033dacfe6..ca4c1b29fb 100644 --- a/src/software/thunderscope/gl/widgets/BUILD +++ b/src/software/thunderscope/gl/widgets/BUILD @@ -20,14 +20,6 @@ py_library( ], ) -py_library( - name = "gl_toolbar", - srcs = ["gl_toolbar.py"], - deps = [ - requirement("pyqtgraph"), - ], -) - py_library( name = "icon_loader", srcs = ["icon_loader.py"], @@ -35,30 +27,3 @@ py_library( requirement("pyqtgraph"), ], ) - -py_library( - name = "gl_field_toolbar", - srcs = ["gl_field_toolbar.py"], - deps = [ - ":gl_toolbar", - "//software/thunderscope/common:common_widgets", - requirement("pyqtgraph"), - requirement("qtawesome"), - ], -) - -py_library( - name = "gl_gamecontroller_toolbar", - srcs = ["gl_gamecontroller_toolbar.py"], - deps = [ - ":gl_runtime_installer", - ":gl_runtime_selector", - ":gl_toolbar", - "//proto:import_all_protos", - "//software/networking:ssl_proto_communication", - "//software/thunderscope/binary_context_managers:game_controller", - "//software/thunderscope/common:common_widgets", - requirement("pyqtgraph"), - requirement("qtawesome"), - ], -) diff --git a/src/software/thunderscope/gl/widgets/gl_toolbar.py b/src/software/thunderscope/gl/widgets/gl_toolbar.py deleted file mode 100644 index 06963ae8da..0000000000 --- a/src/software/thunderscope/gl/widgets/gl_toolbar.py +++ /dev/null @@ -1,56 +0,0 @@ -import textwrap -from pyqtgraph.Qt import QtCore -from pyqtgraph.Qt.QtWidgets import * - - -class GLToolbar(QWidget): - """Base class for a toolbar in our UI - - Has a refresh method to update UI - Has button colors / styles and initializes base formatting - """ - - BUTTON_ICON_COLOR = "white" - DISABLED_BUTTON_ICON_COLOR = "#969696" - - def __init__(self, parent: QWidget): - """Sets the base formatting for a toolbar - - :param parent: the parent to overlay this toolbar over - """ - super(GLToolbar, self).__init__(parent=parent) - - # Setup toolbar - self.setSizePolicy(QSizePolicy.Policy.Preferred, QSizePolicy.Policy.Fixed) - self.setStyleSheet("background-color: rgba(0,0,0,0);" "padding: 0px;") - self.setAttribute(QtCore.Qt.WidgetAttribute.WA_StyledBackground) - self.setLayout(QHBoxLayout()) - - def refresh(self) -> None: - """Refreshes the UI (overridden by child classes)""" - raise NotImplementedError("Subclasses must implement this method!") - - def get_button_style(self, 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"}; - }} - """ - ) diff --git a/src/software/thunderscope/thunderscope_config.py b/src/software/thunderscope/thunderscope_config.py index 4816e0268b..d2142e36a1 100644 --- a/src/software/thunderscope/thunderscope_config.py +++ b/src/software/thunderscope/thunderscope_config.py @@ -124,7 +124,6 @@ def configure_base_fullsystem( full_system_proto_unix_io: ProtoUnixIO, sim_proto_unix_io: ProtoUnixIO, friendly_colour_yellow: bool, - sandbox_mode: bool = False, replay: bool = False, replay_log: os.PathLike = None, visualization_buffer_size: int = 5, @@ -138,7 +137,6 @@ def configure_base_fullsystem( :param full_system_proto_unix_io: the proto unix io to configure widgets with :param sim_proto_unix_io: the proto unix io for the simulator :param friendly_colour_yellow: if this is Yellow FullSystem (True) or Blue (False) - :param sandbox_mode: if sandbox mode should be enabled :param replay: True if in replay mode, False if not :param replay_log: the file path of the replay protos :param visualization_buffer_size: The size of the visualization buffer. @@ -153,7 +151,6 @@ def configure_base_fullsystem( TScopeWidget( name="Field", widget=setup_gl_widget( - sandbox_mode=sandbox_mode, replay=replay, replay_log=replay_log, full_system_proto_unix_io=full_system_proto_unix_io, @@ -353,7 +350,6 @@ def configure_two_ai_gamecontroller_view( sim_proto_unix_io=proto_unix_io_map[ProtoUnixIOTypes.SIM], friendly_colour_yellow=False, visualization_buffer_size=visualization_buffer_size, - sandbox_mode=True, extra_widgets=[], frame_swap_counter=blue_frame_swap_frametime_counter, refresh_counter=blue_refresh_func_frametime_counter, @@ -369,7 +365,6 @@ def configure_two_ai_gamecontroller_view( sim_proto_unix_io=proto_unix_io_map[ProtoUnixIOTypes.SIM], friendly_colour_yellow=True, visualization_buffer_size=visualization_buffer_size, - sandbox_mode=True, extra_widgets=[], frame_swap_counter=yellow_frame_swap_frametime_counter, refresh_counter=yellow_refresh_func_frametime_counter, diff --git a/src/software/thunderscope/widget_setup_functions.py b/src/software/thunderscope/widget_setup_functions.py index 59b4c565ae..48bb5f01a2 100644 --- a/src/software/thunderscope/widget_setup_functions.py +++ b/src/software/thunderscope/widget_setup_functions.py @@ -25,7 +25,6 @@ gl_passing_layer, gl_attacker_layer, gl_sandbox_world_layer, - gl_world_layer, gl_debug_shapes_layer, gl_simulator_layer, gl_tactic_layer, @@ -60,7 +59,6 @@ def setup_gl_widget( full_system_proto_unix_io: ProtoUnixIO, friendly_colour_yellow: bool, visualization_buffer_size: int, - sandbox_mode: bool = False, replay: bool = False, replay_log: os.PathLike = None, frame_swap_counter: Optional[FrameTimeCounter] = None, @@ -72,7 +70,6 @@ def setup_gl_widget( :param full_system_proto_unix_io: The proto unix io object for the full system :param friendly_colour_yellow: Whether the friendly colour is yellow :param visualization_buffer_size: How many packets to buffer while rendering - :param sandbox_mode: if sandbox mode should be enabled :param replay: Whether replay mode is currently enabled :param replay_log: The file path of the replay log :param frame_swap_counter: FrameTimeCounter to keep track of the time between @@ -89,7 +86,6 @@ def setup_gl_widget( friendly_color_yellow=friendly_colour_yellow, frame_swap_counter=frame_swap_counter, player=player, - sandbox_mode=sandbox_mode, ) # Create layers @@ -113,20 +109,11 @@ def setup_gl_widget( cost_vis_layer = gl_cost_vis_layer.GLCostVisLayer( "Passing Cost", visualization_buffer_size ) - world_layer = ( - gl_sandbox_world_layer.GLSandboxWorldLayer( - "Vision", - sim_proto_unix_io, - friendly_colour_yellow, - visualization_buffer_size, - ) - if sandbox_mode - else gl_world_layer.GLWorldLayer( - "Vision", - sim_proto_unix_io, - friendly_colour_yellow, - visualization_buffer_size, - ) + world_layer = gl_sandbox_world_layer.GLSandboxWorldLayer( + "Vision", + sim_proto_unix_io, + friendly_colour_yellow, + visualization_buffer_size, ) field_movement_layer = gl_movement_field_test_layer.GLMovementFieldTestLayer( "Field Movement Layer", full_system_proto_unix_io @@ -164,33 +151,40 @@ def setup_gl_widget( gl_widget.add_layer(referee_layer) simulation_control_toolbar = gl_widget.get_sim_control_toolbar() + + sandbox_sidebar = gl_widget.get_sandbox_sidebar() + + sim_proto_unix_io.register_observer( + SimulationState, simulation_control_toolbar.simulation_state_buffer + ) + sim_proto_unix_io.register_observer( + SimulationState, world_layer.simulation_state_buffer + ) + sim_proto_unix_io.register_observer( + SimulationState, gl_widget.simulation_state_buffer + ) + sim_proto_unix_io.register_observer( + SimulationState, sandbox_sidebar.simulation_state_buffer + ) + + sim_proto_unix_io.register_observer( + SandboxModeState, simulation_control_toolbar.sandbox_mode_state_buffer + ) + sim_proto_unix_io.register_observer( + SandboxModeState, sandbox_sidebar.sandbox_mode_state_buffer + ) + simulation_control_toolbar.set_speed_callback(world_layer.set_simulation_speed) - # connect all sandbox controls if using sandbox mode - if sandbox_mode: - simulation_control_toolbar.undo_button.clicked.connect(world_layer.undo) - simulation_control_toolbar.redo_button.clicked.connect(world_layer.redo) - simulation_control_toolbar.reset_button.clicked.connect( - world_layer.reset_to_pre_sim - ) - world_layer.undo_toggle_enabled_signal.connect( - simulation_control_toolbar.toggle_undo_enabled - ) - world_layer.redo_toggle_enabled_signal.connect( - simulation_control_toolbar.toggle_redo_enabled - ) - simulation_control_toolbar.pause_button.clicked.connect( - world_layer.toggle_play_state - ) - sim_proto_unix_io.register_observer( - SimulationState, simulation_control_toolbar.simulation_state_buffer - ) - sim_proto_unix_io.register_observer( - SimulationState, world_layer.simulation_state_buffer - ) - sim_proto_unix_io.register_observer( - SimulationState, gl_widget.simulation_state_buffer - ) + # connect all sandbox controls + sandbox_sidebar.set_sandbox_toggle_callback(world_layer.toggle_sandbox_mode) + sandbox_sidebar.set_add_team_callback(world_layer.set_adding_team) + sandbox_sidebar.pause_button.clicked.connect(world_layer.toggle_play_state) + sandbox_sidebar.undo_button.clicked.connect(world_layer.undo) + sandbox_sidebar.redo_button.clicked.connect(world_layer.redo) + sandbox_sidebar.clear_field_button.clicked.connect(world_layer.clear_field) + world_layer.undo_toggle_enabled_signal.connect(sandbox_sidebar.toggle_undo_enabled) + world_layer.redo_toggle_enabled_signal.connect(sandbox_sidebar.toggle_redo_enabled) for arg in [ (World, world_layer.world_buffer),