Visualization Proto Deduplicator

src/software/ai/hl/stp/tactic/BUILD

@@ -117,6 +117,7 @@ cc_library(
         "//proto/primitive:primitive_msg_factory",
         "//software/ai/navigator/trajectory:trajectory_planner",
         "//software/geom/algorithms:end_in_obstacle_sample",
+        ":vis_proto_deduper",
     ],
 )
 
@@ -142,3 +143,29 @@ cc_test(
         "//software/test_util",
     ],
 )
+
+cc_library(
+    name = "vis_proto_deduper",
+    srcs = ["vis_proto_deduper.cpp"],
+    hdrs = [
+        "vis_proto_deduper.h",
+    ],
+    deps = [
+        ":primitive",
+        "//proto/message_translation:tbots_protobuf",
+        "//software/util/hash:hash_combine",
+        "//software/ai/navigator/obstacle:robot_navigation_obstacle_factory",
+    ]
+)
+
+cc_test(
+    name = "vis_proto_deduper_test",
+    srcs = ["vis_proto_deduper_test.cpp"],
+    deps = [
+        ":primitive",
+        "//shared/test_util:tbots_gtest_main",
+        "//software/test_util",
+        ":vis_proto_deduper",
+    ]
+)
+

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

@@ -8,6 +8,7 @@
 #include "software/ai/navigator/trajectory/bang_bang_trajectory_1d_angular.h"
 #include "software/geom/algorithms/end_in_obstacle_sample.h"
 
+
 MovePrimitive::MovePrimitive(
     const Robot &robot, const Point &destination, const Angle &final_angle,
     const TbotsProto::MaxAllowedSpeedMode &max_allowed_speed_mode,
@@ -265,10 +266,10 @@ void MovePrimitive::getVisualizationProtos(
     TbotsProto::ObstacleList &obstacle_list_out,
     TbotsProto::PathVisualization &path_visualization_out) const
 {
-    for (const auto &obstacle : obstacles)
-    {
-        obstacle_list_out.add_obstacles()->CopyFrom(obstacle->createObstacleProto());
-    }
+    // If we are sending lots of duplicated obstacles, then it will cause the system network buffer
+    // overflow. Therefore, we selectively populate some of the obstacles. See the implementation of 
+    // VisProtoDeduper
+    vis_proto_deduper.dedupeAndFill(obstacles, obstacle_list_out);
 
     TbotsProto::Path path;
     if (traj_path.has_value())

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

@@ -3,6 +3,7 @@
 
 #include "proto/primitive/primitive_types.h"
 #include "software/ai/hl/stp/tactic/primitive.h"
+#include "software/ai/hl/stp/tactic/vis_proto_deduper.h"
 #include "software/ai/navigator/trajectory/bang_bang_trajectory_1d_angular.h"
 #include "software/ai/navigator/trajectory/bang_bang_trajectory_2d.h"
 #include "software/ai/navigator/trajectory/trajectory_planner.h"
@@ -100,4 +101,7 @@ class MovePrimitive : public Primitive
     TrajectoryPlanner planner;
 
     constexpr static unsigned int NUM_TRAJECTORY_VISUALIZATION_POINTS = 10;
+    constexpr static unsigned int PROTO_DEDUPER_WINDOW_SIZE = 5;
+
+    inline static VisProtoDeduper vis_proto_deduper{PROTO_DEDUPER_WINDOW_SIZE};
 };

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

@@ -0,0 +1,31 @@
+#include "vis_proto_deduper.h"
+
+VisProtoDeduper::VisProtoDeduper(unsigned int window_size):
+    window_size_(window_size) {}
+
+void VisProtoDeduper::dedupeAndFill(const std::vector<ObstaclePtr> &obstacle_list, TbotsProto::ObstacleList& obstacle_list_out) {
+    // lazily evict the ObstacleList from the deque
+    if (sent_queue_.size() > window_size_) {
+        const std::vector<std::size_t>& popped_hashes = sent_queue_.front();
+        for (const auto &obstacle_hash : popped_hashes) {
+            sent_set_.erase(obstacle_hash);
+        }
+
+        sent_queue_.pop_front();
+    }
+
+    // computing hashes of the current obstacle list and compare with the window
+    std::vector<std::size_t> current_hashes;
+    for (const auto &obstacle : obstacle_list) {
+        std::size_t hash_val = obstacle_hasher_(*obstacle);
+        // only push to the output if this packet has not been seen in the window
+        if (sent_set_.count(hash_val) == 0) {
+            TbotsProto::Obstacle proto = obstacle->createObstacleProto();
+            sent_set_.insert(hash_val);
+            obstacle_list_out.add_obstacles()->CopyFrom(proto);
+            current_hashes.push_back(hash_val);
+        }
+    }
+    sent_queue_.push_back(std::move(current_hashes));
+}
+

src/software/ai/hl/stp/tactic/vis_proto_deduper.h

@@ -0,0 +1,58 @@
+#pragma once
+
+#include <unordered_set>
+#include <deque>
+
+#include "software/ai/navigator/obstacle/obstacle.hpp"
+
+/**
+ * The VisProtoDeduper maintains a rolling history of obstacles that have already been transmitted. 
+ * By using a combination of a sliding window (deque) and a fast lookup (hash set), 
+ * it ensures that only "new" or "expired" information is added to the outgoing protobuf message.
+ *
+ *  For example:
+ *  TIME STEP [t]                                     INTERNAL STATE
+ *  -------------------------------------------       ------------------------------
+ *  Incoming Obstacles List: [ A, B, C ]              sent_set: { A, B, C }
+ *  Action: All are NEW.                              sent_queue: [ {A,B,C} ]
+ *  Output Proto: { A, B, C }
+ *
+ *  TIME STEP [t+1]
+ *  -------------------------------------------       sent_set: { A, B, C, D }
+ *  Incoming ObstaclesList: [ A, D ]                  sent_queue: [ {A,B,C}, {D} ]
+ *  Action: A is DUPE, D is NEW.
+ *  Output Proto: { D }
+ *
+ *  TIME STEP [t+2] (Window Size = 2)
+ *  -------------------------------------------       sent_set: { D, E }
+ *  Incoming ObstaclesList: [ A, E ]                  sent_queue: [ {D}, {E} ]
+ *  Action: A was EVICTED from window,                ( {A,B,C} was popped )
+ *       so A is NEW again. E is NEW.
+ *  Output Proto: { A, E }
+ */
+class VisProtoDeduper {
+public:
+    /**
+    * Creates a sliding window deduplicater
+    * 
+    * @param window_size size of the sliding window
+    */
+    VisProtoDeduper(unsigned int window_size);
+
+    /**
+    * Given an input obstacle list 
+    * 
+    * @param obstacle_list input list of obstacle
+    * @param obstacle_list_out output list of obstacle after filtered
+    */
+    void dedupeAndFill(const std::vector<ObstaclePtr>& obstacle_list, TbotsProto::ObstacleList& obstacle_list_out);
+
+
+private:
+    unsigned int window_size_;
+    std::unordered_set<std::size_t> sent_set_;
+    std::deque<std::vector<std::size_t>> sent_queue_;
+
+    std::hash<Obstacle> obstacle_hasher_;
+};
+

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

@@ -0,0 +1,145 @@
+
+#include "software/ai/hl/stp/tactic/vis_proto_deduper.h"
+#include "software/ai/navigator/obstacle/obstacle.hpp"
+#include "software/ai/navigator/obstacle/robot_navigation_obstacle_factory.h"
+#include "software/geom/polygon.h"
+#include "software/geom/point.h"
+
+#include <gtest/gtest.h>
+#include <memory>
+#include <vector>
+
+
+
+class VisProtoDeduperTest : public ::testing::Test
+{
+protected:
+    RobotNavigationObstacleFactory obstacle_factory =
+        RobotNavigationObstacleFactory(TbotsProto::RobotNavigationObstacleConfig());
+
+    // Helper to extract the list of obstacles from the proto message for easy verification
+    std::vector<TbotsProto::Obstacle> getObstaclesFromProto(const TbotsProto::ObstacleList& msg)
+    {
+        std::vector<TbotsProto::Obstacle> obstacles;
+        for (const auto& obs : msg.obstacles())
+        {
+            obstacles.push_back(obs);
+        }
+        return obstacles;;
+    }
+
+    // Helper to create a unique obstacle based on a position offset
+    // This ensures we have distinct geometries to hash.
+    ObstaclePtr createTestObstacle(double x, double y)
+    {
+        auto polygon = Polygon({Point(x, y), Point(x + 1.0, y), Point(x, y + 1.0)});
+        return obstacle_factory.createFromShape(polygon);
+    }
+};
+
+TEST_F(VisProtoDeduperTest, DeduplicatesRepeatedObstacles)
+{
+    VisProtoDeduper deduper(5);
+    TbotsProto::ObstacleList output_msg;
+
+    auto obs1 = createTestObstacle(10, 10);
+    std::vector<ObstaclePtr> input = {obs1};
+
+    // First pass: Obstacle is new
+    deduper.dedupeAndFill(input, output_msg);
+    EXPECT_EQ(output_msg.obstacles_size(), 1);
+
+    // Clear output for next step
+    output_msg.Clear();
+
+    // Second pass: Same obstacle passed immediately again
+    deduper.dedupeAndFill(input, output_msg);
+    EXPECT_EQ(output_msg.obstacles_size(), 0) << "Should filter out recently sent obstacle";
+}
+
+TEST_F(VisProtoDeduperTest, HandlesMixedNewAndOldObstacles)
+{
+    VisProtoDeduper deduper(5);
+    TbotsProto::ObstacleList output_msg;
+
+    auto obs_old = createTestObstacle(10, 10);
+    auto obs_new = createTestObstacle(20, 20);
+
+    // Step 1: Send first obstacle
+    deduper.dedupeAndFill({obs_old}, output_msg);
+    EXPECT_EQ(output_msg.obstacles_size(), 1);
+    output_msg.Clear();
+
+    // Step 2: Send both. 'obs_old' should be deduped, 'obs_new' should pass.
+    deduper.dedupeAndFill({obs_old, obs_new}, output_msg);
+
+    ASSERT_EQ(output_msg.obstacles_size(), 1);
+}
+
+TEST_F(VisProtoDeduperTest, WindowEvictionLogic)
+{
+    // Window size of 2
+    // Frame 0: Send A (Stored in queue index 0)
+    // Frame 1: Send empty (Stored in queue index 1)
+    // Frame 2: Send empty (Stored in queue index 2) -> Window exceeded? 
+    // Logic check: if queue.size() > window. 
+    // After Frame 0: size 1. 
+    // After Frame 1: size 2. 
+    // After Frame 2: size 3. (3 > 2, so Frame 0 is evicted).
+
+    VisProtoDeduper deduper(2);
+    TbotsProto::ObstacleList output_msg;
+    auto obs = createTestObstacle(5, 5);
+
+    deduper.dedupeAndFill({obs}, output_msg);
+    EXPECT_EQ(output_msg.obstacles_size(), 1);
+    output_msg.Clear();
+
+    deduper.dedupeAndFill({}, output_msg);
+    EXPECT_EQ(output_msg.obstacles_size(), 0);
+
+    deduper.dedupeAndFill({}, output_msg);
+    EXPECT_EQ(output_msg.obstacles_size(), 0);
+
+    deduper.dedupeAndFill({obs}, output_msg);
+    EXPECT_EQ(output_msg.obstacles_size(), 1) << "Obstacle should be resent after window expiration";
+}
+
+TEST_F(VisProtoDeduperTest, ZeroWindowAlwaysSends)
+{
+    // If window size is 0, it should behave like a pass-through (or evict immediately)
+    VisProtoDeduper deduper(0);
+    TbotsProto::ObstacleList output_msg;
+    auto obs = createTestObstacle(1, 1);
+
+    // Pass 1
+    deduper.dedupeAndFill({obs}, output_msg);
+    EXPECT_EQ(output_msg.obstacles_size(), 1);
+    output_msg.Clear();
+
+    // Pass 2 - Should send again because window size is 0 (immediate eviction)
+    deduper.dedupeAndFill({obs}, output_msg);
+    EXPECT_EQ(output_msg.obstacles_size(), 1);
+}
+
+TEST_F(VisProtoDeduperTest, MultipleDistinctObstaclesInOneBatch)
+{
+    VisProtoDeduper deduper(5);
+    TbotsProto::ObstacleList output_msg;
+
+    auto obs1 = createTestObstacle(1, 1);
+    auto obs2 = createTestObstacle(2, 2);
+    auto obs3 = createTestObstacle(3, 3);
+
+    // Send 3 unique obstacles at once
+    deduper.dedupeAndFill({obs1, obs2, obs3}, output_msg);
+    EXPECT_EQ(output_msg.obstacles_size(), 3);
+    output_msg.Clear();
+
+    // Send 2 old, 1 new
+    auto obs4 = createTestObstacle(4, 4);
+    deduper.dedupeAndFill({obs1, obs3, obs4}, output_msg);
+
+    ASSERT_EQ(output_msg.obstacles_size(), 1);
+}
+

src/software/ai/navigator/obstacle/geom_obstacle.hpp

@@ -7,6 +7,8 @@
 #include "software/geom/algorithms/intersects.h"
 #include "software/geom/algorithms/rasterize.h"
 
+#include <functional>
+
 template <typename GEOM_TYPE>
 class GeomObstacle : public Obstacle
 {
@@ -30,6 +32,7 @@ class GeomObstacle : public Obstacle
     std::string toString(void) const override;
     void accept(ObstacleVisitor& visitor) const override;
     std::vector<Point> rasterize(const double resolution_size) const override;
+    std::size_t hash() const override;
 
     /**
      * Gets the underlying GEOM_TYPE
@@ -116,3 +119,10 @@ void GeomObstacle<GEOM_TYPE>::accept(ObstacleVisitor& visitor) const
 {
     visitor.visit(*this);
 }
+
+template <typename GEOM_TYPE>
+std::size_t GeomObstacle<GEOM_TYPE>::hash() const
+{
+    return std::hash<GEOM_TYPE>{}(geom_);
+}
+

src/software/ai/navigator/obstacle/obstacle.hpp

@@ -112,6 +112,13 @@ class Obstacle
      * @param visitor An Obstacle Visitor
      */
     virtual void accept(ObstacleVisitor& visitor) const = 0;
+
+    /**
+     * Computes the hash of the current obstacle object
+     *
+     * @return hash value
+     */
+    virtual std::size_t hash() const = 0;
 };
 
 /**
@@ -144,3 +151,13 @@ inline std::ostream& operator<<(std::ostream& os, const ObstaclePtr& obstacle_pt
     os << obstacle_ptr->toString();
     return os;
 }
+
+template <>
+struct std::hash<Obstacle>
+{
+    std::size_t operator()(const Obstacle &obstacle) const
+    {
+        return obstacle.hash();
+    }
+};
+

src/software/geom/BUILD

@@ -64,6 +64,7 @@ cc_library(
     deps = [
         ":segment",
         ":shape",
+        "//software/util/hash:hash_combine",
     ],
 )