Draw Virtual Obstacles

src/proto/visualization.proto

@@ -63,6 +63,11 @@ message ObstacleList
     repeated Obstacle obstacles = 1;
 }
 
+message VirtualObstacles
+{
+    repeated Obstacle obstacles = 1;
+}
+
 message Shape
 {
     oneof shape

src/software/ai/hl/stp/tactic/move_primitive.cpp

@@ -1,5 +1,6 @@
 #include "software/ai/hl/stp/tactic/move_primitive.h"
 
+#include "proto/message_translation/tbots_geometry.h"
 #include "proto/message_translation/tbots_protobuf.h"
 #include "proto/primitive/primitive_msg_factory.h"
 #include "software/ai/navigator/trajectory/bang_bang_trajectory_1d_angular.h"
@@ -184,9 +185,26 @@ void MovePrimitive::updateObstacles(
     // Separately store the non-robot + non-ball obstacles
     field_obstacles =
         obstacle_factory.createObstaclesFromMotionConstraints(motion_constraints, world);
-
     obstacles = field_obstacles;
 
+    // adding virtual obstalces
+    auto virtual_obstacles = world.getVirtualObstacles().obstacles();
+    for (TbotsProto::Obstacle &obstacle : virtual_obstacles)
+    {
+        if (!obstacle.has_polygon())
+        {
+            LOG(WARNING)
+                << "Virtual Obstacles contain obstacle that is not a polygon. Shape ignored";
+
+            continue;
+        }
+
+        Polygon obstacle_polygon     = createPolygon(obstacle.polygon());
+        ObstaclePtr current_obstacle = obstacle_factory.createFromShape(obstacle_polygon);
+        obstacles.push_back(current_obstacle);
+    }
+
+
     for (const Robot &enemy : world.enemyTeam().getAllRobots())
     {
         if (obstacle_avoidance_mode == TbotsProto::SAFE)

src/software/backend/backend.cpp

@@ -2,6 +2,8 @@
 
 #include "proto/message_translation/tbots_protobuf.h"
 #include "proto/sensor_msg.pb.h"
+#include "software/multithreading/subject.hpp"
+
 
 void Backend::receiveRobotStatus(TbotsProto::RobotStatus msg)
 {
@@ -31,3 +33,8 @@ void Backend::receiveSensorProto(SensorProto sensor_msg)
 {
     Subject<SensorProto>::sendValueToObservers(sensor_msg);
 }
+
+void Backend::receiveObstacleList(TbotsProto::VirtualObstacles new_obstacle_list)
+{
+    Subject<TbotsProto::VirtualObstacles>::sendValueToObservers(new_obstacle_list);
+}

src/software/backend/backend.h

@@ -15,6 +15,7 @@
  * "Subject". Please see the implementation of those classes for details.
  */
 class Backend : public Subject<SensorProto>,
+                public Subject<TbotsProto::VirtualObstacles>,
                 public FirstInFirstOutThreadedObserver<World>,
                 public FirstInFirstOutThreadedObserver<TbotsProto::PrimitiveSet>
 {
@@ -33,4 +34,12 @@ class Backend : public Subject<SensorProto>,
     void receiveSSLWrapperPacket(SSLProto::SSL_WrapperPacket msg);
     void receiveSSLReferee(SSLProto::Referee msg);
     void receiveSensorProto(SensorProto sensor_msg);
+
+    /**
+     * Callback function that receive a list of new virtual obstacles from
+     * Subject<VirtualObstacles>
+     *
+     * @param new_obstacles_list a new obstacles list we are having
+     */
+    void receiveObstacleList(TbotsProto::VirtualObstacles new_obstacle_list);
 };

src/software/backend/unix_simulator_backend.cpp

@@ -1,13 +1,15 @@
 #include "software/backend/unix_simulator_backend.h"
 
 #include "proto/message_translation/ssl_wrapper.h"
+#include "proto/message_translation/tbots_geometry.h"
 #include "proto/message_translation/tbots_protobuf.h"
 #include "proto/parameters.pb.h"
 #include "proto/robot_log_msg.pb.h"
 #include "proto/sensor_msg.pb.h"
 #include "shared/constants.h"
 #include "software/constants.h"
 #include "software/logger/logger.h"
+#include "software/multithreading/subject.hpp"
 #include "software/util/generic_factory/generic_factory.h"
 
 UnixSimulatorBackend::UnixSimulatorBackend(
@@ -38,6 +40,13 @@ UnixSimulatorBackend::UnixSimulatorBackend(
             [&](TbotsProto::ThunderbotsConfig& msg) { receiveThunderbotsConfig(msg); },
             proto_logger));
 
+    // external obstacles for bang bang trajectory planner
+    external_obstacles_list_.reset(
+        new ThreadedProtoUnixListener<TbotsProto::VirtualObstacles>(
+            runtime_dir + VIRTUAL_OBSTACLES_UNIX_PATH,
+            [&](TbotsProto::VirtualObstacles& msg) { receiveObstacleList(msg); },
+            proto_logger));
+
     // The following listeners have an empty callback since their values are
     // only used by proto_logger for replay purposes.
     validation_proto_set_listener.reset(

src/software/backend/unix_simulator_backend.h

@@ -37,6 +37,8 @@ class UnixSimulatorBackend : public Backend, public Subject<TbotsProto::Thunderb
 
     // ThreadedProtoUnix** to communicate with Thunderscope
     // Inputs
+    std::unique_ptr<ThreadedProtoUnixListener<TbotsProto::VirtualObstacles>>
+        external_obstacles_list_;
     std::unique_ptr<ThreadedProtoUnixListener<TbotsProto::RobotStatus>>
         robot_status_input;
     std::unique_ptr<ThreadedProtoUnixListener<SSLProto::SSL_WrapperPacket>>

src/software/constants.h

@@ -35,6 +35,7 @@ const std::string ROBOT_CRASH_PATH                       = "/robot_crash";
 const std::string DYNAMIC_PARAMETER_UPDATE_REQUEST_PATH  = "/dynamic_parameter_request";
 const std::string DYNAMIC_PARAMETER_UPDATE_RESPONSE_PATH = "/dynamic_parameter_response";
 const std::string WORLD_STATE_RECEIVED_TRIGGER_PATH = "/world_state_received_trigger";
+const std::string VIRTUAL_OBSTACLES_UNIX_PATH       = "/virtual_obstacles";
 
 const unsigned UNIX_BUFFER_SIZE = 20000;
 

src/software/py_constants.cpp

@@ -8,6 +8,7 @@ PYBIND11_MODULE(py_constants, m)
 {
     m.attr("BALL_MAX_SPEED_METERS_PER_SECOND") = BALL_MAX_SPEED_METERS_PER_SECOND;
     m.attr("ROBOT_MAX_HEIGHT_METERS")          = ROBOT_MAX_HEIGHT_METERS;
+    m.attr("VIRTUAL_OBSTACLES_UNIX_PATH")      = VIRTUAL_OBSTACLES_UNIX_PATH;
     m.attr("ROBOT_MAX_RADIUS_METERS")          = ROBOT_MAX_RADIUS_METERS;
     m.attr("ROBOT_MAX_HEIGHT_MILLIMETERS") =
         ROBOT_MAX_HEIGHT_METERS * MILLIMETERS_PER_METER;

src/software/sensor_fusion/sensor_fusion.cpp

@@ -38,6 +38,7 @@ std::optional<World> SensorFusion::getWorld() const
             new_world.updateRefereeStage(*referee_stage);
         }
 
+        new_world.setVirtualObstacles(virtual_obstacles_);
         return new_world;
     }
     else
@@ -437,3 +438,8 @@ void SensorFusion::resetWorldComponents()
     enemy_team_filter    = RobotTeamFilter();
     possession           = TeamPossession::FRIENDLY_TEAM;
 }
+
+void SensorFusion::setVirtualObstacles(TbotsProto::VirtualObstacles virtual_obstacles)
+{
+    virtual_obstacles_ = virtual_obstacles;
+}

src/software/sensor_fusion/sensor_fusion.h

@@ -48,6 +48,13 @@ class SensorFusion
      */
     std::optional<World> getWorld() const;
 
+    /**
+     * Set the virtual obstacles in the world
+     *
+     * @param virtual_obstacles a list of virtual obstacles
+     */
+    void setVirtualObstacles(TbotsProto::VirtualObstacles virtual_obstacles);
+
     // Number of vision packets to indicate that the vision client most likely reset,
     // determined experimentally with the simulator
     static constexpr unsigned int VISION_PACKET_RESET_COUNT_THRESHOLD = 5;
@@ -182,4 +189,6 @@ class SensorFusion
 
     // The timestamp, in seconds, of the most recently received vision packet
     double last_t_capture;
+
+    TbotsProto::VirtualObstacles virtual_obstacles_;
 };

src/software/sensor_fusion/threaded_sensor_fusion.cpp

@@ -39,3 +39,8 @@ void ThreadedSensorFusion::onValueReceived(SensorProto sensor_msg)
         }
     }
 }
+
+void ThreadedSensorFusion::onValueReceived(TbotsProto::VirtualObstacles virtual_obstacles)
+{
+    sensor_fusion.setVirtualObstacles(virtual_obstacles);
+}

src/software/sensor_fusion/threaded_sensor_fusion.h

@@ -10,7 +10,9 @@
 class ThreadedSensorFusion
     : public Subject<World>,
       public FirstInFirstOutThreadedObserver<SensorProto>,
-      public FirstInFirstOutThreadedObserver<TbotsProto::ThunderbotsConfig>
+      public FirstInFirstOutThreadedObserver<TbotsProto::ThunderbotsConfig>,
+      public FirstInFirstOutThreadedObserver<TbotsProto::VirtualObstacles>
+
 {
    public:
     explicit ThreadedSensorFusion(TbotsProto::SensorFusionConfig config);
@@ -19,6 +21,7 @@ class ThreadedSensorFusion
    private:
     void onValueReceived(SensorProto sensor_msg) override;
     void onValueReceived(TbotsProto::ThunderbotsConfig config) override;
+    void onValueReceived(TbotsProto::VirtualObstacles virtual_obstacles) override;
 
     SensorFusion sensor_fusion;
     TbotsProto::SensorFusionConfig sensor_fusion_config;

src/software/thunderscope/BUILD

@@ -86,6 +86,7 @@ py_library(
         "//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_draw_polygon_obstacle",
         "//software/thunderscope/gl/layers:gl_obstacle_layer",
         "//software/thunderscope/gl/layers:gl_passing_layer",
         "//software/thunderscope/gl/layers:gl_path_layer",

src/software/thunderscope/binary_context_managers/full_system.py

@@ -6,6 +6,7 @@
 import threading
 
 from subprocess import Popen, TimeoutExpired
+from software.thunderscope.gl.layers.gl_obstacle_layer import ObstacleList
 from software.thunderscope.proto_unix_io import ProtoUnixIO
 from software.python_bindings import *
 from proto.import_all_protos import *
@@ -215,5 +216,6 @@ def setup_proto_unix_io(self, proto_unix_io: ProtoUnixIO) -> None:
             (VALIDATION_PROTO_SET_PATH, ValidationProtoSet),
             (ROBOT_LOG_PATH, RobotLog),
             (ROBOT_CRASH_PATH, RobotCrash),
+            (VIRTUAL_OBSTACLES_UNIX_PATH, VirtualObstacles),
         ]:
             proto_unix_io.attach_unix_sender(self.full_system_runtime_dir, *arg)

src/software/thunderscope/constants.py

@@ -170,6 +170,10 @@ class EstopMode(IntEnum):
     <b><code>Ctrl + R:</code></b> Remove the current layout file and reset the layout<br><br>
     Layout file (on save) is located at {SAVED_LAYOUT_PATH}<br>
 
+    <b><code>Shift+Double Click</code></b> Push the current obstacle to the obstacle stack. FriendlyObstacleLayer must be enabled.<br><br>
+    <b><code>c</code></b> Clear the virtual obstacle stack FriendlyObstacleLayer must be enabled. <br><br>
+
+
     """
 )
 

src/software/thunderscope/gl/BUILD

@@ -12,6 +12,8 @@ py_library(
         "//software/thunderscope/gl/layers:gl_measure_layer",
         "//software/thunderscope/gl/widgets:gl_field_toolbar",
         "//software/thunderscope/gl/widgets:gl_gamecontroller_toolbar",
+        "//software/thunderscope/gl/widgets:gl_multilayer_toolbar",
+        "//software/thunderscope/gl/widgets:gl_shift_toolbar",
         "//software/thunderscope/replay:replay_controls",
         requirement("pyqtgraph"),
     ],

src/software/thunderscope/gl/gl_widget.py

@@ -11,10 +11,12 @@
 
 from software.thunderscope.constants import *
 
+from software.thunderscope.gl.widgets.gl_multilayer_toolbar import MultilayerToolbar
+from software.thunderscope.gl.widgets.gl_shift_toolbar import ShiftButtonToolbar
 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.widgets.gl_field_toolbar import GLFieldToolbar, MultiLayerToolbar
 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 *
@@ -83,21 +85,35 @@ def __init__(
         self.setLayout(self.layout)
         self.layout.addWidget(self.gl_view_widget)
 
+        # setup multi layer toolbar
+        self.multi_layer_toolbar = MultilayerToolbar(
+            parent=self.gl_view_widget, toolbars=[]
+        )
+
+        self.shift_button_toolbar = ShiftButtonToolbar(parent=self.multi_layer_toolbar)
+
         # Setup toolbar
         self.measure_mode_enabled = False
         self.measure_layer = None
         self.layers_menu = QMenu()
         self.toolbars_menu = QMenu()
         self.layers_menu_actions = {}
+
+
         self.simulation_control_toolbar = GLFieldToolbar(
-            parent=self.gl_view_widget,
+            parent=self.multi_layer_toolbar,
             on_camera_view_change=self.set_camera_view,
             on_measure_mode=self.toggle_measure_mode,
             layers_menu=self.layers_menu,
             toolbars_menu=self.toolbars_menu,
             sandbox_mode=sandbox_mode,
         )
 
+
+        layers = [self.simulation_control_toolbar]
+        self.multilayer_toolbar = MultiLayerToolbar(self.gl_view_widget, layers)
+        self.multi_layer_toolbar.add_toolbar(self.simulation_control_toolbar)
+
         # Setup gamecontroller toolbar
         self.gamecontroller_toolbar = GLGamecontrollerToolbar(
             parent=self.gl_view_widget,
@@ -123,6 +139,10 @@ def __init__(
 
         self.set_camera_view(CameraView.LANDSCAPE_HIGH_ANGLE)
 
+    def get_shift_button_toolbar(self):
+        """Get the ShiftButtonToolbar"""
+        return self.shift_button_toolbar
+
     def get_sim_control_toolbar(self):
         """Returns the simulation control toolbar"""
         return self.simulation_control_toolbar

src/software/thunderscope/gl/layers/BUILD

@@ -156,3 +156,13 @@ py_library(
         requirement("pyqtgraph"),
     ],
 )
+
+py_library(
+    name = "gl_draw_polygon_obstacle",
+    srcs = ["gl_draw_polygon_obstacle.py"],
+    deps = [
+        ":gl_layer",
+        "//software/thunderscope/gl/graphics:gl_polygon",
+        requirement("pyqtgraph"),
+    ],
+)