Draw Virtual Obstacles (UBC-Thunderbots#3406)

Mr-Anyone · pre-commit-ci-lite[bot] · itsarune · web-flow · commit 9975f654cbbb · 2025-05-15T20:57:11.000-04:00
* added feature to draw virtual obstacles

* fixed a refactor

* removed debugging comments

* [pre-commit.ci lite] apply automatic fixes

* following conventions after reading on a few pr

* [pre-commit.ci lite] apply automatic fixes

* fixed tests?

* fixed minor bugs

* [pre-commit.ci lite] apply automatic fixes

* made some changes

* [pre-commit.ci lite] apply automatic fixes

* should work

* [pre-commit.ci lite] apply automatic fixes

* fixed typo

* made some simple changes

* made some changes to the logic

* remove unnecessary import

* [pre-commit.ci lite] apply automatic fixes

* kind of implemented double click

* better double click and now handles single click

* added more docs

* removed commentsq

* added menu

* fixed sizing

* basically done, need to do cleanup

* [pre-commit.ci lite] apply automatic fixes

* updated documentation

* moved stuff into different file

* made some changes

* final clean up I hope

* [pre-commit.ci lite] apply automatic fixes

* Update src/software/thunderscope/constants.py

Co-authored-by: itsarune &lt;42703774+itsarune@users.noreply.github.com&gt;

* Update src/software/thunderscope/constants.py

Co-authored-by: itsarune &lt;42703774+itsarune@users.noreply.github.com&gt;

* made some small changes

* removed a todo

* [pre-commit.ci lite] apply automatic fixes

* fixed build?

* remove all toolbar related code

* update docs

* Formatting

* [pre-commit.ci lite] apply automatic fixes

* Update src/software/world/world.h

Co-authored-by: William Ha &lt;60044853+williamckha@users.noreply.github.com&gt;

* hopefully this fixed it?

* Refactored GLDrawPolygonObstacleLayer and fixed GLPolygon not rendering outline correctly

* [pre-commit.ci lite] apply automatic fixes

* Fix small nitpicks

* [pre-commit.ci lite] apply automatic fixes

* address comments

---------

Co-authored-by: pre-commit-ci-lite[bot] &lt;117423508+pre-commit-ci-lite[bot]@users.noreply.github.com&gt;
Co-authored-by: itsarune &lt;42703774+itsarune@users.noreply.github.com&gt;
Co-authored-by: williamckha &lt;williamha@outlook.com&gt;
Co-authored-by: William Ha &lt;60044853+williamckha@users.noreply.github.com&gt;
diff --git a/src/proto/visualization.proto b/src/proto/visualization.proto
@@ -68,6 +68,11 @@ message ObstacleList
     repeated Obstacle obstacles = 1;
 }
 
+message VirtualObstacles
+{
+    repeated Obstacle obstacles = 1;
+}
+
 message Shape
 {
     oneof shape
diff --git a/src/software/ai/hl/stp/tactic/move_primitive.cpp b/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)
diff --git a/src/software/backend/backend.cpp b/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);
+}
diff --git a/src/software/backend/backend.h b/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);
 };
diff --git a/src/software/backend/unix_simulator_backend.cpp b/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(
@@ -37,6 +39,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(
diff --git a/src/software/backend/unix_simulator_backend.h b/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>>
diff --git a/src/software/constants.h b/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;
 
diff --git a/src/software/py_constants.cpp b/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;
diff --git a/src/software/sensor_fusion/sensor_fusion.cpp b/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
@@ -512,3 +513,8 @@ void SensorFusion::resetWorldComponents()
     possession           = TeamPossession::FRIENDLY_TEAM;
     dribble_displacement = std::nullopt;
 }
+
+void SensorFusion::setVirtualObstacles(TbotsProto::VirtualObstacles virtual_obstacles)
+{
+    virtual_obstacles_ = virtual_obstacles;
+}
diff --git a/src/software/sensor_fusion/sensor_fusion.h b/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_;
 };
diff --git a/src/software/sensor_fusion/threaded_sensor_fusion.cpp b/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);
+}
diff --git a/src/software/sensor_fusion/threaded_sensor_fusion.h b/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;
diff --git a/src/software/thunderscope/BUILD b/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_movement_field_test_layer",
         "//software/thunderscope/gl/layers:gl_obstacle_layer",
diff --git a/src/software/thunderscope/binary_context_managers/full_system.py b/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)
diff --git a/src/software/thunderscope/constants.py b/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>
+
     <h3>GL Field Movement Test Layer</h3>
     <b><code>Shift+Alt+Control+Click</code></b> Select the robot that is going to be moved<br>
     <b><code>Shift+Alt+Click</code></b>Move the selected robot to the clicked point<br>
diff --git a/src/software/thunderscope/gl/gl_widget.py b/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,
diff --git a/src/software/thunderscope/gl/graphics/gl_polygon.py b/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:
diff --git a/src/software/thunderscope/gl/layers/BUILD b/src/software/thunderscope/gl/layers/BUILD
@@ -181,6 +181,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"],
diff --git a/src/software/thunderscope/gl/layers/gl_draw_polygon_obstacle.py b/src/software/thunderscope/gl/layers/gl_draw_polygon_obstacle.py
diff --git a/src/software/thunderscope/widget_setup_functions.py b/src/software/thunderscope/widget_setup_functions.py
diff --git a/src/software/unix_full_system_main.cpp b/src/software/unix_full_system_main.cpp
diff --git a/src/software/world/world.cpp b/src/software/world/world.cpp
diff --git a/src/software/world/world.h b/src/software/world/world.h

Original file line number	Diff line number	Diff line change
`@@ -68,6 +68,11 @@ message ObstacleList`
`68`	`68`	`repeated Obstacle obstacles = 1;`
`69`	`69`	`}`
`70`	`70`
	`71`	`+message VirtualObstacles`
	`72`	`+{`
	`73`	`+ repeated Obstacle obstacles = 1;`
	`74`	`+}`
	`75`	`+`
`71`	`76`	`message Shape`
`72`	`77`	`{`
`73`	`78`	`oneof shape`
Original file line number	Diff line number	Diff line change
`@@ -2,6 +2,8 @@`
`2`	`2`
`3`	`3`	`#include "proto/message_translation/tbots_protobuf.h"`
`4`	`4`	`#include "proto/sensor_msg.pb.h"`
	`5`	`+#include "software/multithreading/subject.hpp"`
	`6`	`+`
`5`	`7`
`6`	`8`	`void Backend::receiveRobotStatus(TbotsProto::RobotStatus msg)`
`7`	`9`	`{`
`@@ -31,3 +33,8 @@ void Backend::receiveSensorProto(SensorProto sensor_msg)`
`31`	`33`	`{`
`32`	`34`	`Subject<SensorProto>::sendValueToObservers(sensor_msg);`
`33`	`35`	`}`
	`36`	`+`
	`37`	`+void Backend::receiveObstacleList(TbotsProto::VirtualObstacles new_obstacle_list)`
	`38`	`+{`
	`39`	`+ Subject<TbotsProto::VirtualObstacles>::sendValueToObservers(new_obstacle_list);`
	`40`	`+}`
Original file line number	Diff line number	Diff line change
`@@ -8,6 +8,7 @@ PYBIND11_MODULE(py_constants, m)`
`8`	`8`	`{`
`9`	`9`	`m.attr("BALL_MAX_SPEED_METERS_PER_SECOND") = BALL_MAX_SPEED_METERS_PER_SECOND;`
`10`	`10`	`m.attr("ROBOT_MAX_HEIGHT_METERS") = ROBOT_MAX_HEIGHT_METERS;`
	`11`	`+ m.attr("VIRTUAL_OBSTACLES_UNIX_PATH") = VIRTUAL_OBSTACLES_UNIX_PATH;`
`11`	`12`	`m.attr("ROBOT_MAX_RADIUS_METERS") = ROBOT_MAX_RADIUS_METERS;`
`12`	`13`	`m.attr("ROBOT_MAX_HEIGHT_MILLIMETERS") =`
`13`	`14`	`ROBOT_MAX_HEIGHT_METERS * MILLIMETERS_PER_METER;`
Original file line number	Diff line number	Diff line change
`@@ -40,6 +40,7 @@ std::optional<World> SensorFusion::getWorld() const`
`40`	`40`	`new_world.updateRefereeStage(*referee_stage);`
`41`	`41`	`}`
`42`	`42`
	`43`	`+ new_world.setVirtualObstacles(virtual_obstacles_);`
`43`	`44`	`return new_world;`
`44`	`45`	`}`
`45`	`46`	`else`
`@@ -512,3 +513,8 @@ void SensorFusion::resetWorldComponents()`
`512`	`513`	`possession = TeamPossession::FRIENDLY_TEAM;`
`513`	`514`	`dribble_displacement = std::nullopt;`
`514`	`515`	`}`
	`516`	`+`
	`517`	`+void SensorFusion::setVirtualObstacles(TbotsProto::VirtualObstacles virtual_obstacles)`
	`518`	`+{`
	`519`	`+ virtual_obstacles_ = virtual_obstacles;`
	`520`	`+}`