Various Thunderscope visualization improvements (UBC-Thunderbots#3198)

* Add attacker tactic visualizations, thunderscope sim speed control, update debug shapes layer

* Add function docs

* Move attacker tactic visualization to attacker_tactic.cpp

* address comments

* Add new autokick/chip graphic and address comment

* Remove extra TODO

* fix cpp test viz

* update debug shapes

Author: nimazareian

src/proto/message_translation/BUILD

@@ -14,6 +14,7 @@ cc_library(
         ":tbots_geometry",
         "//proto:tbots_cc_proto",
         "//proto:visualization_cc_proto",
+        "//software/ai/evaluation:shot",
         "//software/ai/navigator/trajectory:bang_bang_trajectory_1d_angular",
         "//software/ai/navigator/trajectory:trajectory_path",
         "//software/ai/passing:pass_with_rating",

src/proto/message_translation/tbots_protobuf.cpp

@@ -331,14 +331,12 @@ std::unique_ptr<TbotsProto::PlotJugglerValue> createPlotJugglerValue(
     return plot_juggler_value_msg;
 }
 
-std::unique_ptr<TbotsProto::DebugShapesMap> createDebugShapesMap(
-    const std::map<std::string, TbotsProto::Shape>& named_shapes)
+std::unique_ptr<TbotsProto::DebugShapes> createDebugShapes(
+    const std::vector<TbotsProto::DebugShapes::DebugShape>& debug_shapes)
 {
-    auto debug_shape_list_msg = std::make_unique<TbotsProto::DebugShapesMap>();
-    for (auto const& [name, shape_proto] : named_shapes)
-    {
-        (*debug_shape_list_msg->mutable_named_shapes())[name] = shape_proto;
-    }
+    auto debug_shape_list_msg = std::make_unique<TbotsProto::DebugShapes>();
+    (*debug_shape_list_msg->mutable_debug_shapes()) = {debug_shapes.begin(),
+                                                       debug_shapes.end()};
     return debug_shape_list_msg;
 }
 

src/proto/message_translation/tbots_protobuf.h

@@ -127,23 +127,47 @@ std::unique_ptr<TbotsProto::PlotJugglerValue> createPlotJugglerValue(
     const std::map<std::string, double>& values);
 
 /**
- * Returns a TbotsProto::DebugShapesMap proto containing the name
- * shape pairs.
+ * Returns a TbotsProto::DebugShapes::DebugShape proto for a given shape,
+ * unique id, and an optional debug text.
+ *
+ * @tparam Shape A shape which has a matching createShapeProto function
+ *
+ * @param shape The shape to create the protobuf for
+ * @param unique_id A unique string used to differentiate this shape
+ * from others. Note that this ID should remain the same for the same shape
+ * to avoid it from being plotted multiple times.
+ * @param debug_text An optional string to display on the shape
+ *
+ * @return The unique_ptr to a TbotsProto::DebugShapes::DebugShape proto
+ */
+template <class Shape>
+std::unique_ptr<TbotsProto::DebugShapes::DebugShape> createDebugShape(
+    const Shape& shape, const std::string& unique_id, const std::string& debug_text = "")
+{
+    auto debug_shape = std::make_unique<TbotsProto::DebugShapes::DebugShape>();
+    (*debug_shape->mutable_shape()) = *createShapeProto(shape);
+    debug_shape->set_unique_id(unique_id);
+    debug_shape->set_debug_text(debug_text);
+    return debug_shape;
+};
+
+/**
+ * Returns a TbotsProto::DebugShapes proto containing the debug shapes
  *
  * Could use LOG(VISUALIZE) to plot these values. Example:
- *  LOG(VISUALIZE) << *createDebugShapesMap({
- *      {"circle_name", *createShapeProto(circle_object)},
- *      {"stadium_name", *createShapeProto(stadium_object)},
- *      {"polygon_name", *createShapeProto(polygon_object)}
+ *  LOG(VISUALIZE) << *createDebugShapes({
+ *       *createDebugShape(circle, unique_id1, optional_text),
+ *       *createDebugShape(polygon, unique_id2, optional_text),
+ *       *createDebugShape(stadium, unique_id3, optional_text)
  *  });
  *
- * @param named_shapes The map of name shape proto pairs to plot
+ * @param debug_shapes A list of debug shapes proto to plot
  *
- * @return The unique_ptr to a TbotsProto::DebugShapesMap proto containing data with
+ * @return The unique_ptr to a TbotsProto::DebugShapes proto containing data with
  *        specified names and shapes
  */
-std::unique_ptr<TbotsProto::DebugShapesMap> createDebugShapesMap(
-    const std::map<std::string, TbotsProto::Shape>& named_shapes);
+std::unique_ptr<TbotsProto::DebugShapes> createDebugShapes(
+    const std::vector<TbotsProto::DebugShapes::DebugShape>& debug_shapes);
 
 /**
  * Returns a TbotsProto::Shape proto given a shape.

src/proto/visualization.proto

@@ -33,6 +33,14 @@ message PassVisualization
     repeated PassWithRating best_passes = 1;
 }
 
+message AttackerVisualization
+{
+    optional Pass pass_        = 1;  // needs the _ because pass is a keyword in python
+    bool pass_committed        = 2;
+    optional Shot shot         = 3;
+    optional Point chip_target = 4;
+}
+
 message CostVisualization
 {
     uint32 num_rows      = 1;
@@ -65,7 +73,17 @@ message Shape
     }
 }
 
-message DebugShapesMap
+message DebugShapes
 {
-    map<string, Shape> named_shapes = 1;
+    message DebugShape
+    {
+        Shape shape = 1;
+        // Unique ID to identify the shape
+        string unique_id = 2;
+        // Text to display next to the shape
+        string debug_text = 3;
+    }
+
+    // Unique ID to a named shape
+    repeated DebugShape debug_shapes = 1;
 }

src/proto/world.proto

@@ -56,8 +56,10 @@ message SimulatorTick
 
 message SimulationState
 {
-    required bool is_playing = 1 [default = true];
+    required bool is_playing         = 1 [default = true];
+    required double simulation_speed = 2 [default = 1.0];
 }
+
 message Pass
 {
     // The location of the passer
@@ -70,6 +72,13 @@ message Pass
     required double pass_speed_m_per_s = 3;
 }
 
+message Shot
+{
+    required Point shot_origin = 1;
+    required Point shot_target = 2;
+    required Angle open_angle  = 3;
+}
+
 message EnemyThreat
 {
     // The enemy robot

src/software/ai/hl/stp/tactic/attacker/attacker_tactic.cpp

@@ -1,5 +1,6 @@
 #include "software/ai/hl/stp/tactic/attacker/attacker_tactic.h"
 
+#include "proto/message_translation/tbots_geometry.h"
 #include "shared/constants.h"
 #include "software/ai/evaluation/calc_best_shot.h"
 #include "software/logger/logger.h"
@@ -67,4 +68,44 @@ void AttackerTactic::updatePrimitive(const TacticUpdate& tactic_update, bool res
 
     fsm_map.at(tactic_update.robot.id())
         ->process_event(AttackerFSM::Update(control_params, tactic_update));
+
+    visualizeControlParams(*tactic_update.world_ptr, control_params);
+}
+
+void AttackerTactic::visualizeControlParams(
+    const World& world, const AttackerFSM::ControlParams& control_params)
+{
+    TbotsProto::AttackerVisualization pass_visualization_msg;
+
+    if (control_params.best_pass_so_far.has_value())
+    {
+        TbotsProto::Pass pass_msg;
+        *(pass_msg.mutable_passer_point()) =
+            *createPointProto(control_params.best_pass_so_far->passerPoint());
+        *(pass_msg.mutable_receiver_point()) =
+            *createPointProto(control_params.best_pass_so_far->receiverPoint());
+        pass_msg.set_pass_speed_m_per_s(control_params.best_pass_so_far->speed());
+        *(pass_visualization_msg.mutable_pass_()) = pass_msg;
+    }
+
+    pass_visualization_msg.set_pass_committed(pass_committed);
+
+    if (control_params.shot.has_value())
+    {
+        TbotsProto::Shot shot_msg;
+        *(shot_msg.mutable_shot_origin()) = *createPointProto(world.ball().position());
+        *(shot_msg.mutable_shot_target()) =
+            *createPointProto(control_params.shot->getPointToShootAt());
+        *(shot_msg.mutable_open_angle()) =
+            *createAngleProto(control_params.shot->getOpenAngle());
+        *(pass_visualization_msg.mutable_shot()) = shot_msg;
+    }
+
+    if (chip_target.has_value())
+    {
+        *(pass_visualization_msg.mutable_chip_target()) =
+            *createPointProto(chip_target.value());
+    }
+
+    LOG(VISUALIZE) << pass_visualization_msg;
 }

src/software/ai/hl/stp/tactic/attacker/attacker_tactic.h

@@ -46,6 +46,15 @@ class AttackerTactic : public Tactic
    private:
     void updatePrimitive(const TacticUpdate& tactic_update, bool reset_fsm) override;
 
+    /**
+     * Log visualize the control parameters for this AttackerTactic
+     *
+     * @param world Current state of the world
+     * @param control_params The control parameters to visualize
+     */
+    void visualizeControlParams(const World& world,
+                                const AttackerFSM::ControlParams& control_params);
+
     std::map<RobotId, std::unique_ptr<FSM<AttackerFSM>>> fsm_map;
 
     // The pass to execute

src/software/thunderscope/BUILD

@@ -77,6 +77,7 @@ py_library(
         "//software/thunderscope/common:proto_configuration_widget",
         "//software/thunderscope/common:proto_plotter",
         "//software/thunderscope/gl:gl_widget",
+        "//software/thunderscope/gl/layers:gl_attacker_layer",
         "//software/thunderscope/gl/layers:gl_cost_vis_layer",
         "//software/thunderscope/gl/layers:gl_debug_shapes_layer",
         "//software/thunderscope/gl/layers:gl_obstacle_layer",

src/software/thunderscope/binary_context_managers/full_system.py

@@ -170,11 +170,12 @@ def setup_proto_unix_io(self, proto_unix_io: ProtoUnixIO) -> None:
         for proto_class in [
             PathVisualization,
             PassVisualization,
+            AttackerVisualization,
             CostVisualization,
             NamedValue,
             PlayInfo,
             ObstacleList,
-            DebugShapesMap,
+            DebugShapes,
         ]:
             proto_unix_io.attach_unix_receiver(
                 runtime_dir=self.full_system_runtime_dir,

src/software/thunderscope/constants.py

@@ -1,4 +1,5 @@
 from pyqtgraph.Qt import QtCore, QtGui
+from OpenGL.GL import *
 from proto.import_all_protos import *
 from enum import Enum, IntEnum
 from proto.robot_log_msg_pb2 import LogLevel
@@ -159,6 +160,8 @@ class EstopMode(IntEnum):
     f"{SAVED_LAYOUT_PATH}/last_opened_tscope_layout.{LAYOUT_FILE_EXTENSION}"
 )
 
+SIMULATION_SPEEDS = [2, 1, 0.5, 0.2, 0.1, 0.05]
+
 THUNDERSCOPE_HELP_TEXT = textwrap.dedent(
     f"""
     <h3>General Controls</h3><br>
@@ -167,6 +170,8 @@ class EstopMode(IntEnum):
     <b><code>M:</code></b> Toggle measure mode<br>
     <b><code>S:</code></b> Toggle visibility of robot/ball speed visualization<br>
     <b><code>Ctrl + Space:</code></b> Stop AI vs AI simulation<br>
+    <b><code>Ctrl + Up:</code></b> Increment simulation speed<br>
+    <b><code>Ctrl + Down:</code></b> Decrement simulation speed<br>
     <b><code>Number Keys:</code></b> Position camera to preset view<br>
     <b><code>Shift + Left Click:</code></b> Place the ball at the cursor<br>
     <b><code>Shift + Left Click Drag:</code></b> Place the ball at the cursor and kick it<br>
@@ -277,7 +282,13 @@ class Colors(object):
     NAVIGATOR_OBSTACLE_COLOR = QtGui.QColor(255, 80, 0, 100)
     DEBUG_SHAPES_COLOR = QtGui.QColor(190, 50, 235, 255)
     PASS_VISUALIZATION_COLOR = QtGui.QColor(255, 0, 0, 80)
+    UNCOMMITTED_PASS_VISUALIZATION_COLOR = QtGui.QColor(255, 0, 0, 80)
+    COMMITTED_PASS_VISUALIZATION_COLOR = QtGui.QColor(0, 255, 255, 255)
+    SHOT_VISUALIZATION_COLOR = QtGui.QColor(255, 0, 0, 255)
+    CHIP_TARGET_VISUALIZATION_COLOR = QtGui.QColor(255, 0, 0, 255)
     BREAKBEAM_TRIPPED_COLOR = QtGui.QColor(255, 0, 0, 255)
+    AUTO_CHIP_ENABLED_COLOR = QtGui.QColor(215, 0, 200, 255)
+    AUTO_KICK_ENABLED_COLOR = QtGui.QColor(255, 0, 0, 255)
 
     VALIDATION_PASSED_COLOR = QtGui.QColor(0, 200, 0, 255)
     VALIDATION_FAILED_COLOR = QtGui.QColor(200, 0, 0, 255)
@@ -323,3 +334,21 @@ class TrailValues:
 
     DEFAULT_TRAIL_LENGTH = 20
     DEFAULT_TRAIL_SAMPLING_RATE = 0
+
+
+class CustomGLOptions:
+    """
+    Custom OpenGL Rendering modes that could be used in addition to
+    the ones provided by PyQtGraph in GLGraphicsItem.py GLOptions.
+    """
+
+    # Opaque rendering (i.e. overlapping colors are not blended) while
+    # also allowing for custom depth values to be set.
+    # This is useful when the graphics are overlaid on top of (e.g.) a
+    # yellow robot where the blended colors would not be easily visible.
+    OPAQUE_WITH_OUT_DEPTH_TEST = {
+        GL_DEPTH_TEST: False,
+        GL_BLEND: False,
+        GL_ALPHA_TEST: False,
+        GL_CULL_FACE: False,
+    }