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 obstalce that is not a polygon. Shape igonred";
+
+            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,5 @@ class Backend : public Subject<SensorProto>,
     void receiveSSLWrapperPacket(SSLProto::SSL_WrapperPacket msg);
     void receiveSSLReferee(SSLProto::Referee msg);
     void receiveSensorProto(SensorProto sensor_msg);
+    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,12 @@ class SensorFusion
      */
     std::optional<World> getWorld() const;
 
+    /**
+     * Set the virtual obstacles in the world
+     *
+     */
+    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 +188,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 list)
+{
+    sensor_fusion.setVirtualObstacles(list);
+}

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 list) 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/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"),
+    ],
+)

src/software/thunderscope/gl/layers/gl_draw_polygon_obstacle.py

@@ -0,0 +1,146 @@
+from software.thunderscope.binary_context_managers.full_system import ProtoUnixIO
+from software.thunderscope.gl.graphics.gl_polygon import GLPolygon
+from pyqtgraph.Qt import QtGui
+from pyqtgraph.Qt.QtCore import Qt
+from pyqtgraph.opengl import *
+
+from proto.import_all_protos import *
+from software.py_constants import *
+from software.thunderscope.gl.helpers.observable_list import ObservableList
+from software.thunderscope.proto_unix_io import ProtoUnixIO
+
+from software.thunderscope.gl.layers.gl_layer import GLLayer
+from software.thunderscope.gl.helpers.extended_gl_view_widget import MouseInSceneEvent
+
+
+class GLDrawPolygonObstacleLayer(GLLayer):
+    """A layer used to draw polygons that are going to represent obstacles for the trajectroy planner
+    to avoid.
+    """
+
+    def __init__(
+        self, name, blue_fs_io: ProtoUnixIO, yellow_fs_io: ProtoUnixIO
+    ) -> None:
+        """Initial this layer
+
+        :param blue_fs_io: the blue full system Protounix io
+        :param yellow_fs_io: the yellow full system Protounix io
+        """
+        super().__init__(name)
+
+        self.blue_fu_io: ProtoUnixIO = blue_fs_io
+        self.yellow_fs_io: ProtoUnixIO = yellow_fs_io
+
+        self.current_polygon: GLPolygon = GLPolygon(parent_item=self, line_width=2)
+        # Tuple[float, float] represents a point (x,y)
+        self.points: List[Tuple[float, float]] = []
+
+        self.obstacles: List[Obstacle] = []
+
+        # used for keeping track and rendering multiple polygons
+        self.rendering_polygons: ObservableList = ObservableList(self._graphics_changed)
+
+    def keyPressEvent(self, event: QtGui.QKeyEvent) -> None:
+        """Responding to key events that are going to push obstacles to the stack or add point
+
+        :param event: a qt key event indicating the button that are pressed
+        """
+        if event.key() == Qt.Key.Key_Q:
+            self.push_polygon_to_list()
+
+        if event.key() == Qt.Key.Key_W:
+            self.clear_polygons()
+
+    def clear_polygons(self):
+        """Clearing the obstacles"""
+        self.points.clear()
+        self.obstacles.clear()
+
+        for polygon in self.rendering_polygons:
+            polygon.hide()
+        self.rendering_polygons.resize(0, lambda: {})
+        self.current_polygon.hide()
+        self.current_polygon = GLPolygon(parent_item=self, line_width=2)
+
+        self._send_to_fs()
+
+    def push_polygon_to_list(self):
+        """Pushing the fully drawn polygon to the stack"""
+        points = [
+            Point(x_meters=point[0], y_meters=point[1]) for point in self.points[:-1]
+        ]
+
+        if len(points) <= 2:
+            print("Cannot push polygon to stack as there as less that points two.")
+            return
+
+        polygon = Polygon(points=points)
+        obstacle = Obstacle(polygon=polygon)
+        self.obstacles.append(obstacle)
+        self.points.clear()
+
+        self.rendering_polygons.append(self.current_polygon)
+        self.current_polygon = GLPolygon(parent_item=self, line_width=2)
+        self._send_to_fs()
+
+    def _add_one_point(self, point: tuple[float, float]):
+        """Adding one points to a polygon
+
+        :param point: represent the point (x,y) that is added to the polygon
+        """
+        # trying to create a line
+        if len(self.points) < 2:
+            self.points.append(point)
+            self.current_polygon.set_points(self.points)
+            return
+
+        # creating a triangle
+        if len(self.points) == 2:
+            start_point = self.points[0]
+
+            self.points.append(point)
+            self.points.append(start_point)
+            self.current_polygon.set_points(self.points)
+
+        # creating a general polygon
+        start_point = self.points[0]
+        self.points.pop()  # removing the start point since the last point is always the start point
+
+        self.points.append(point)
+        self.points.append(start_point)
+        self.current_polygon.set_points(self.points)
+        self._send_to_fs()
+
+    def _send_to_fs(self):
+        """Sending a list of virtual obstacles to full system"""
+        obstacles = self.obstacles.copy()
+
+        points = [
+            Point(x_meters=point[0], y_meters=point[1]) for point in self.points[:-1]
+        ]
+
+        # only send to full system when the points form a valid polygon
+        if len(points) >= 3:
+            polygon = Polygon(points=points)
+            obstacle = Obstacle(polygon=polygon)
+            obstacles.append(obstacle)
+
+        self.blue_fu_io.send_proto(
+            VirtualObstacles, VirtualObstacles(obstacles=obstacles)
+        )
+        self.yellow_fs_io.send_proto(
+            VirtualObstacles, VirtualObstacles(obstacles=obstacles)
+        )
+
+    def mouse_in_scene_pressed(self, event: MouseInSceneEvent) -> None:
+        """Adding the point in scene
+
+        :param event: the mouse event
+        """
+        point = event.point_in_scene
+        self._add_one_point((point.x(), point.y()))
+
+        return super().mouse_in_scene_pressed(event)
+
+    def refresh_graphics(self) -> None:
+        return super().refresh_graphics()