Draw Virtual Obstacles

src/proto/visualization.proto

@@ -68,6 +68,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 obstacles
+    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 receives a list of new virtual obstacles to send
+     * to Observers
+     *
+     * @param new_obstacles_list the received virtual obstacles list
+     */
+    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

@@ -39,6 +39,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

@@ -36,6 +36,7 @@ const std::string REPLAY_BOOKMARK_PATH                   = "/replay_bookmark";
 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

@@ -40,6 +40,7 @@ std::optional<World> SensorFusion::getWorld() const
             new_world.updateRefereeStage(*referee_stage);
         }
 
+        new_world.setVirtualObstacles(virtual_obstacles_);
         return new_world;
     }
     else
@@ -510,3 +511,8 @@ void SensorFusion::resetWorldComponents()
     possession           = TeamPossession::FRIENDLY_TEAM;
     dribble_displacement = std::nullopt;
 }
+
+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;
@@ -205,4 +212,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

@@ -87,6 +87,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_max_dribble_layer",
         "//software/thunderscope/gl/layers:gl_obstacle_layer",
         "//software/thunderscope/gl/layers:gl_passing_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 *
@@ -216,6 +217,7 @@ 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),
             (REPLAY_BOOKMARK_PATH, ReplayBookmark),
         ]:
             proto_unix_io.attach_unix_sender(self.full_system_runtime_dir, *arg)

src/software/thunderscope/constants.py

@@ -183,6 +183,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+Alt+Double Click</code></b> Complete the current virtual obstacle. The Draw Obstacle Layer must be enabled.<br><br>
+    <b><code>Shift+Alt+Single Click</code></b> Add a point to the current virtual obstacle. The Draw Obstacle Layer must be enabled.<br><br>
+    <b><code>c</code></b> Clear all virtual obstacles. The Draw Obstacle Layer must be enabled.<br><br>
+
     """
 )
 

src/software/thunderscope/gl/gl_widget.py

@@ -11,7 +11,6 @@
 from software.thunderscope.common.frametime_counter import FrameTimeCounter
 
 from software.thunderscope.constants import *
-
 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
@@ -94,6 +93,7 @@ def __init__(
         self.layers_menu = QMenu()
         self.toolbars_menu = QMenu()
         self.layers_menu_actions = {}
+
         self.simulation_control_toolbar = GLFieldToolbar(
             parent=self.gl_view_widget,
             on_camera_view_change=self.set_camera_view,

src/software/thunderscope/gl/graphics/gl_polygon.py

@@ -61,7 +61,7 @@ def _update_shape_data(self) -> None:
         if not self.points:
             return
 
-        vertices = [(point[0], point[1], 0) for point in self.points]
+        vertices = [(point[0], point[1], 0) for point in self.points + [self.points[0]]]
         self.setData(pos=vertices)
 
         if self.fill_graphic:

src/software/thunderscope/gl/layers/BUILD

@@ -169,6 +169,17 @@ py_library(
     ],
 )
 
+py_library(
+    name = "gl_draw_polygon_obstacle",
+    srcs = ["gl_draw_polygon_obstacle.py"],
+    deps = [
+        ":gl_layer",
+        "//software/thunderscope:util",
+        "//software/thunderscope/gl/graphics:gl_polygon",
+        requirement("pyqtgraph"),
+    ],
+)
+
 py_library(
     name = "gl_referee_info_layer",
     srcs = ["gl_referee_info_layer.py"],