diff --git a/src/proto/visualization.proto b/src/proto/visualization.proto index 2dc1165ac2..d222060143 100644 --- a/src/proto/visualization.proto +++ b/src/proto/visualization.proto @@ -41,6 +41,11 @@ message AttackerVisualization optional Point chip_target = 4; } +message BallPlacementVisualization +{ + Point ball_placement_point = 1; +} + message CostVisualization { uint32 num_rows = 1; diff --git a/src/shared/constants.h b/src/shared/constants.h index a235bdfb3d..6f5a1aadcc 100644 --- a/src/shared/constants.h +++ b/src/shared/constants.h @@ -78,6 +78,14 @@ static const double BALL_MAX_RADIUS_METERS = 0.0215; // cover more than 20% of the ball static const double MAX_FRACTION_OF_BALL_COVERED_BY_ROBOT = 0.2; +// The radius of a circle region where ball placement is acceptable (in meters). +static const double BALL_PLACEMENT_TOLERANCE_RADIUS_METERS = 0.15; +// The radius of the outer region where robots are not allowed to be during ball +// placement (in meters) +static const double BALL_PLACEMENT_ROBOT_AVOID_RADIUS_METERS = 0.5; +// The time limit for ball placement in seconds +static const int BALL_PLACEMENT_TIME_LIMIT_S = 30; + // The mass of a standard golf ball, as defined by https://en.wikipedia.org/wiki/Golf_ball constexpr double BALL_MASS_KG = 0.004593; // The max allowed speed of the robot when the stop command is issued, in meters per diff --git a/src/software/ai/hl/stp/play/ball_placement/ball_placement_play.cpp b/src/software/ai/hl/stp/play/ball_placement/ball_placement_play.cpp index 0bc65ae98c..2e59ef17a6 100644 --- a/src/software/ai/hl/stp/play/ball_placement/ball_placement_play.cpp +++ b/src/software/ai/hl/stp/play/ball_placement/ball_placement_play.cpp @@ -1,5 +1,6 @@ #include "software/ai/hl/stp/play/ball_placement/ball_placement_play.h" +#include "proto/message_translation/tbots_geometry.h" #include "software/util/generic_factory/generic_factory.h" @@ -17,7 +18,18 @@ void BallPlacementPlay::getNextTactics(TacticCoroutine::push_type &yield, void BallPlacementPlay::updateTactics(const PlayUpdate &play_update) { - fsm.process_event(BallPlacementPlayFSM::Update(control_params, play_update)); + auto event = BallPlacementPlayFSM::Update(control_params, play_update); + fsm.process_event(event); + + auto placement_point = event.common.world_ptr->gameState().getBallPlacementPoint(); + if (placement_point.has_value()) + { + TbotsProto::BallPlacementVisualization ball_placement_vis_msg; + *(ball_placement_vis_msg.mutable_ball_placement_point()) = + *createPointProto(placement_point.value()); + + LOG(VISUALIZE) << ball_placement_vis_msg; + } } std::vector BallPlacementPlay::getState() diff --git a/src/software/py_constants.cpp b/src/software/py_constants.cpp index 4a7d3dfa28..bda0addfc6 100644 --- a/src/software/py_constants.cpp +++ b/src/software/py_constants.cpp @@ -17,6 +17,11 @@ PYBIND11_MODULE(py_constants, m) m.attr("BALL_MAX_RADIUS_METERS") = BALL_MAX_RADIUS_METERS; m.attr("BALL_MAX_RADIUS_MILLIMETERS") = BALL_MAX_RADIUS_METERS * MILLIMETERS_PER_METER; + m.attr("BALL_PLACEMENT_TOLERANCE_RADIUS_METERS") = + BALL_PLACEMENT_TOLERANCE_RADIUS_METERS; + m.attr("BALL_PLACEMENT_ROBOT_AVOID_RADIUS_METERS") = + BALL_PLACEMENT_ROBOT_AVOID_RADIUS_METERS; + m.attr("BALL_PLACEMENT_TIME_LIMIT_S") = BALL_PLACEMENT_TIME_LIMIT_S; m.attr("MAX_FRACTION_OF_BALL_COVERED_BY_ROBOT") = MAX_FRACTION_OF_BALL_COVERED_BY_ROBOT; diff --git a/src/software/thunderscope/BUILD b/src/software/thunderscope/BUILD index e188c0d202..06c5121e6e 100644 --- a/src/software/thunderscope/BUILD +++ b/src/software/thunderscope/BUILD @@ -89,6 +89,7 @@ py_library( "//software/thunderscope/gl/layers:gl_obstacle_layer", "//software/thunderscope/gl/layers:gl_passing_layer", "//software/thunderscope/gl/layers:gl_path_layer", + "//software/thunderscope/gl/layers:gl_referee_info_layer", "//software/thunderscope/gl/layers:gl_sandbox_world_layer", "//software/thunderscope/gl/layers:gl_simulator_layer", "//software/thunderscope/gl/layers:gl_tactic_layer", diff --git a/src/software/thunderscope/binary_context_managers/full_system.py b/src/software/thunderscope/binary_context_managers/full_system.py index 240a06354a..d6aa8b5f1d 100644 --- a/src/software/thunderscope/binary_context_managers/full_system.py +++ b/src/software/thunderscope/binary_context_managers/full_system.py @@ -184,6 +184,7 @@ def setup_proto_unix_io(self, proto_unix_io: ProtoUnixIO) -> None: PlayInfo, ObstacleList, DebugShapes, + BallPlacementVisualization, ]: proto_unix_io.attach_unix_receiver( runtime_dir=self.full_system_runtime_dir, diff --git a/src/software/thunderscope/constants.py b/src/software/thunderscope/constants.py index 40f5e34aef..b6191f3637 100644 --- a/src/software/thunderscope/constants.py +++ b/src/software/thunderscope/constants.py @@ -265,6 +265,7 @@ class Colors: ROBOT_MIDDLE_BLUE = QtGui.QColor(0, 0, 255, 255) PINK = QtGui.QColor(255, 0, 255) GREEN = QtGui.QColor(0, 255, 0) + RED = QtGui.QColor(255, 0, 0, 255) # Creates a default vision pattern lookup with the actual colors used on the robots VISION_PATTERN_LOOKUP = create_vision_pattern_lookup(PINK, GREEN) diff --git a/src/software/thunderscope/gl/graphics/BUILD b/src/software/thunderscope/gl/graphics/BUILD index 2b79290562..927612c58b 100644 --- a/src/software/thunderscope/gl/graphics/BUILD +++ b/src/software/thunderscope/gl/graphics/BUILD @@ -106,3 +106,12 @@ py_library( ":gl_shape", ], ) + +py_library( + name = "gl_label", + srcs = ["gl_label.py"], + deps = [ + requirement("pyqtgraph"), + ":gl_shape", + ], +) diff --git a/src/software/thunderscope/gl/graphics/gl_label.py b/src/software/thunderscope/gl/graphics/gl_label.py new file mode 100644 index 0000000000..93d418dc6f --- /dev/null +++ b/src/software/thunderscope/gl/graphics/gl_label.py @@ -0,0 +1,78 @@ +from PyQt6.QtGui import QFont, QColor +from pyqtgraph.opengl.GLGraphicsItem import GLGraphicsItem +from pyqtgraph.Qt import QtCore, QtGui + +from typing import Optional + +from software.thunderscope.gl.graphics.gl_painter import GLPainter +from software.thunderscope.constants import Colors + + +class GLLabel(GLPainter): + """Displays a 2D text label on the viewport""" + + def __init__( + self, + parent_item: Optional[GLGraphicsItem] = None, + font: QFont = QFont("Roboto", 8), + text_color: QColor = Colors.PRIMARY_TEXT_COLOR, + offset: tuple[int, int] = (0, 0), + text: str = "", + ) -> None: + """Initialize the GLLabel + + :param parent_item: The parent item of the graphic + :param font: The font using to render the text + :param text_color: The color for rendering the text. + :param offset: The offset (x, y) from the viewport left and top edge + to use when positioning the label. + If x is negative then the x offset is |x| pixels from + the viewport right edge. + If y is negative then the y offset is |y| pixels from + the viewport bottom edge. + :param text: The optional title to display above the legend + """ + super().__init__(parent_item=parent_item) + + self.text_pen = QtGui.QPen(text_color) + self.font = font + self.offset = offset + self.text = text + + self.add_draw_function(self.draw_label) + + def draw_label(self, painter: QtGui.QPainter, viewport_rect: QtCore.QRect) -> None: + """Draw the label + + :param painter: The QPainter to perform drawing operations with + :param viewport_rect: The QRect indicating the viewport dimensions + """ + # calculate width and height of the label + painter.setFont(self.font) + bounds = painter.boundingRect( + QtCore.QRectF(0, 0, 0, 0), + QtCore.Qt.AlignmentFlag.AlignLeft | QtCore.Qt.AlignmentFlag.AlignVCenter, + str(self.text), + ) + + width = round(bounds.width()) + height = round(bounds.height()) + + # Determine x and y coordinates of the label + if self.offset[0] < 0: + x = viewport_rect.right() + self.offset[0] - width + else: + x = viewport_rect.left() + self.offset[0] + if self.offset[1] < 0: + y = viewport_rect.bottom() + self.offset[1] - height + else: + y = viewport_rect.top() + self.offset[1] + + if self.text: + painter.drawText(QtCore.QPoint(x, y), self.text) + + def set_text(self, new_text: str) -> None: + """Update the text being displayed + :param new_text: new text being displayed + """ + self.text = new_text diff --git a/src/software/thunderscope/gl/layers/BUILD b/src/software/thunderscope/gl/layers/BUILD index 33d1da8093..50d4614455 100644 --- a/src/software/thunderscope/gl/layers/BUILD +++ b/src/software/thunderscope/gl/layers/BUILD @@ -156,3 +156,13 @@ py_library( requirement("pyqtgraph"), ], ) + +py_library( + name = "gl_referee_info_layer", + srcs = ["gl_referee_info_layer.py"], + deps = [ + ":gl_layer", + "//software/thunderscope/gl/graphics:gl_label", + requirement("pyqtgraph"), + ], +) diff --git a/src/software/thunderscope/gl/layers/gl_referee_info_layer.py b/src/software/thunderscope/gl/layers/gl_referee_info_layer.py new file mode 100644 index 0000000000..2071fccbcf --- /dev/null +++ b/src/software/thunderscope/gl/layers/gl_referee_info_layer.py @@ -0,0 +1,249 @@ +import math +import time +from typing import Optional + +from PyQt6 import QtGui +from google.protobuf.json_format import MessageToDict +from pyqtgraph.opengl.items.GLTextItem import GLTextItem + +from proto.import_all_protos import * +from software.py_constants import * +from software.thunderscope.constants import DepthValues, Colors +from software.thunderscope.gl.graphics.gl_circle import GLCircle +from software.thunderscope.gl.graphics.gl_label import GLLabel +from software.thunderscope.gl.helpers.observable_list import ObservableList +from software.thunderscope.gl.layers.gl_layer import GLLayer +from software.thunderscope.thread_safe_buffer import ThreadSafeBuffer + + +class GLRefereeInfoLayer(GLLayer): + """GLLayer that visualizes referee info""" + + REFEREE_COMMAND_PREFIX = "Command: " + GAMESTATE_PREFIX = "Game State: " + + # outline color of the avoid area for ball placement + BALL_PLACEMENT_ROBOT_AVOID_AREA_VISUALIZATION_COLOR = Colors.RED + # outline color of the tolerance region for ball placement (when the ball is inside) + BALL_PLACEMENT_TOLERANCE_VISUALIZATION_COLOR_ON = Colors.GREEN + # outline color of the tolerance region for ball placement (when the ball is outside) + BALL_PLACEMENT_TOLERANCE_VISUALIZATION_COLOR_OFF = Colors.RED + # outline color of the target mark + BALL_PLACEMENT_TARGET_VISUALIZATION_COLOR = Colors.FIELD_LINE_COLOR + # text color for count down + COUNT_DOWN_TEXT_COLOR = Colors.RED + + @staticmethod + def is_point_in_circle( + point: tuple[float | int, float | int], + center: tuple[float | int, float | int], + radius: float | int, + ) -> bool: + """Returns true if the point is in the circle. + + :param point: coordinates of a point in xy plane. + :param center: coordinates of the circle center in xy plane. + :param radius: radius of the circle + :return: true if the point is in the circle. + """ + return math.dist(point, center) < radius + + def __init__(self, name: str, buffer_size: int = 1) -> None: + """Initialize the GLRefereeInfoLayer + + :param name: The displayed name of the layer + :param buffer_size: the buffer size, set higher for smoother plots. + Set lower for more realtime plots. Default is arbitrary. + """ + super().__init__(name) + self.setDepthValue(DepthValues.OVERLAY_DEPTH) + self.referee_vis_buffer = ThreadSafeBuffer(buffer_size, Referee) + self.ball_placement_vis_buffer = ThreadSafeBuffer( + buffer_size, BallPlacementVisualization + ) + self.world_buffer = ThreadSafeBuffer(buffer_size, World) + + self.cached_world = None + self.cached_referee_info = None + + self.referee_text_graphics = ObservableList(self._graphics_changed) + + self.placement_tolerance_graphic = GLCircle( + parent_item=self, + radius=BALL_PLACEMENT_TOLERANCE_RADIUS_METERS, + outline_color=self.BALL_PLACEMENT_TOLERANCE_VISUALIZATION_COLOR_OFF, + ) + + self.placement_target_graphic = GLCircle( + parent_item=self, + radius=BALL_PLACEMENT_TOLERANCE_RADIUS_METERS, + outline_color=self.BALL_PLACEMENT_TARGET_VISUALIZATION_COLOR, + ) + + self.robot_avoid_circle_graphic = GLCircle( + parent_item=self, + radius=BALL_PLACEMENT_ROBOT_AVOID_RADIUS_METERS, + outline_color=self.BALL_PLACEMENT_ROBOT_AVOID_AREA_VISUALIZATION_COLOR, + ) + + # initialize the two text items to display + self.gamestate_type_text: Optional[GLLabel] = None + self.command_type_text: Optional[GLLabel] = None + + self.ball_placement_countdown_graphic: Optional[GLTextItem] = None + + self.ball_placement_point = None + self.ball_placement_point_hidden = False + self.shrink_target = True + self.placement_start_time = 0 + + def __update_ball_placement(self) -> None: + """Update ball placement visuals""" + ball_placement_vis_proto = self.ball_placement_vis_buffer.get( + block=False, return_cached=False + ) + + if not self.ball_placement_countdown_graphic: + self.ball_placement_countdown_graphic = GLTextItem( + parentItem=self, + font=QtGui.QFont("Roboto", 7, weight=700), + color=self.COUNT_DOWN_TEXT_COLOR, + ) + + if not ball_placement_vis_proto and not self.ball_placement_point_hidden: + self.placement_tolerance_graphic.hide() + self.placement_target_graphic.hide() + self.robot_avoid_circle_graphic.hide() + self.ball_placement_countdown_graphic.hide() + + self.ball_placement_point_hidden = True + self.shrink_target = True + return + + if ball_placement_vis_proto: + # move the ball placement graphics to the new point + new_placement_point = ball_placement_vis_proto.ball_placement_point + + # update the color of the target circle according to the position of the ball. + # if the ball lies inside the tolerance circle, the circle will be green, otherwise red. + ball_state = self.cached_world.ball.current_state + if not self.ball_placement_point_hidden: + if GLRefereeInfoLayer.is_point_in_circle( + ( + ball_state.global_position.x_meters, + ball_state.global_position.y_meters, + ), + (new_placement_point.x_meters, new_placement_point.y_meters), + BALL_PLACEMENT_TOLERANCE_RADIUS_METERS, + ): + self.placement_tolerance_graphic.set_outline_color( + self.BALL_PLACEMENT_TOLERANCE_VISUALIZATION_COLOR_ON + ) + else: + self.placement_tolerance_graphic.set_outline_color( + self.BALL_PLACEMENT_TOLERANCE_VISUALIZATION_COLOR_OFF + ) + + if self.ball_placement_point_hidden: + self.ball_placement_point = new_placement_point + + self.placement_tolerance_graphic.set_position( + self.ball_placement_point.x_meters, + self.ball_placement_point.y_meters, + ) + self.placement_tolerance_graphic.show() + + self.placement_target_graphic.set_position( + self.ball_placement_point.x_meters, + self.ball_placement_point.y_meters, + ) + self.placement_target_graphic.set_radius( + BALL_PLACEMENT_TOLERANCE_RADIUS_METERS + ) + self.placement_target_graphic.show() + + self.robot_avoid_circle_graphic.set_position( + self.ball_placement_point.x_meters, + self.ball_placement_point.y_meters, + ) + self.robot_avoid_circle_graphic.show() + + self.ball_placement_countdown_graphic.setData( + text=f"{BALL_PLACEMENT_TIME_LIMIT_S}s", + pos=[ + self.ball_placement_point.x_meters + + BALL_PLACEMENT_ROBOT_AVOID_RADIUS_METERS / 2, + self.ball_placement_point.y_meters + + BALL_PLACEMENT_ROBOT_AVOID_RADIUS_METERS + + 0.1, + 0, + ], + ) + self.ball_placement_countdown_graphic.show() + + self.placement_start_time = time.time() + self.ball_placement_point_hidden = False + + # shrinking or expanding placement target graphic + if self.shrink_target: + self.placement_target_graphic.set_radius( + self.placement_target_graphic.radius - 0.01 + ) + self.shrink_target = self.placement_target_graphic.radius >= 0 + else: + self.placement_target_graphic.set_radius( + self.placement_target_graphic.radius + 0.01 + ) + self.shrink_target = ( + self.placement_target_graphic.radius + >= BALL_PLACEMENT_TOLERANCE_RADIUS_METERS + ) + + # update the count-down graphics + time_left = max( + int(self.cached_referee_info["currentActionTimeRemaining"]) // 1000000, + 0, + ) + self.ball_placement_countdown_graphic.setData(text=f"{time_left}s") + + def __update_referee_info(self): + """Update gamestate and command info text displays""" + referee_proto = self.referee_vis_buffer.get(block=False, return_cached=False) + if not referee_proto: + return + + referee_msg_dict = MessageToDict(referee_proto) + if not referee_msg_dict: + return + self.cached_referee_info = referee_msg_dict + + if not self.gamestate_type_text: + self.gamestate_type_text = GLLabel( + parent_item=self, + offset=(-10, 50), + text=GLRefereeInfoLayer.GAMESTATE_PREFIX + referee_msg_dict["stage"], + ) + self.referee_text_graphics.append(self.gamestate_type_text) + else: + self.gamestate_type_text.set_text( + GLRefereeInfoLayer.GAMESTATE_PREFIX + referee_msg_dict["stage"] + ) + + if not self.command_type_text: + self.command_type_text = GLLabel( + parent_item=self, + offset=(-10, 70), + text=GLRefereeInfoLayer.GAMESTATE_PREFIX + referee_msg_dict["command"], + ) + self.referee_text_graphics.append(self.command_type_text) + else: + self.command_type_text.set_text( + GLRefereeInfoLayer.REFEREE_COMMAND_PREFIX + referee_msg_dict["command"] + ) + + def refresh_graphics(self) -> None: + """Refresh all displays in the layer""" + self.cached_world = self.world_buffer.get(block=False, return_cached=True) + + self.__update_referee_info() + self.__update_ball_placement() diff --git a/src/software/thunderscope/thunderscope_main.py b/src/software/thunderscope/thunderscope_main.py index 78b70a4e32..8d42f71d73 100644 --- a/src/software/thunderscope/thunderscope_main.py +++ b/src/software/thunderscope/thunderscope_main.py @@ -263,6 +263,7 @@ {"proto_class": PrimitiveSet}, {"proto_class": World}, {"proto_class": PlayInfo}, + {"proto_class": BallPlacementVisualization}, ]: proto_unix_io.attach_unix_receiver( runtime_dir, from_log_visualize=True, **arg diff --git a/src/software/thunderscope/widget_setup_functions.py b/src/software/thunderscope/widget_setup_functions.py index d7d77055c9..0d6d72c424 100644 --- a/src/software/thunderscope/widget_setup_functions.py +++ b/src/software/thunderscope/widget_setup_functions.py @@ -27,6 +27,7 @@ gl_tactic_layer, gl_cost_vis_layer, gl_trail_layer, + gl_referee_info_layer, ) @@ -129,6 +130,9 @@ def setup_gl_widget( ) tactic_layer = gl_tactic_layer.GLTacticLayer("Tactics", visualization_buffer_size) trail_layer = gl_trail_layer.GLTrailLayer("Trail", visualization_buffer_size) + referee_layer = gl_referee_info_layer.GLRefereeInfoLayer( + "Referee Info", visualization_buffer_size + ) gl_widget.add_layer(world_layer) gl_widget.add_layer(simulator_layer, False) @@ -141,6 +145,7 @@ def setup_gl_widget( gl_widget.add_layer(validation_layer) gl_widget.add_layer(trail_layer, False) gl_widget.add_layer(debug_shapes_layer, True) + gl_widget.add_layer(referee_layer, False) simulation_control_toolbar = gl_widget.get_sim_control_toolbar() simulation_control_toolbar.set_speed_callback(world_layer.set_simulation_speed) @@ -189,6 +194,9 @@ def setup_gl_widget( (CostVisualization, cost_vis_layer.cost_visualization_buffer), (World, trail_layer.world_buffer), (DebugShapes, debug_shapes_layer.debug_shapes_buffer), + (Referee, referee_layer.referee_vis_buffer), + (BallPlacementVisualization, referee_layer.ball_placement_vis_buffer), + (World, referee_layer.world_buffer), ]: full_system_proto_unix_io.register_observer(*arg)