From 1d833e2525dada2543067ffac34dcb0ee7540d95 Mon Sep 17 00:00:00 2001 From: Minghao Li Date: Fri, 23 Jan 2026 16:28:00 -0800 Subject: [PATCH 1/9] fix obstacle proto --- .../ai/hl/stp/tactic/vis_proto_deduper_test.cpp | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) create mode 100644 src/software/ai/hl/stp/tactic/vis_proto_deduper_test.cpp diff --git a/src/software/ai/hl/stp/tactic/vis_proto_deduper_test.cpp b/src/software/ai/hl/stp/tactic/vis_proto_deduper_test.cpp new file mode 100644 index 0000000000..ed85e3c985 --- /dev/null +++ b/src/software/ai/hl/stp/tactic/vis_proto_deduper_test.cpp @@ -0,0 +1,16 @@ +#include "software/ai/hl/stp/tactic/vis_proto_deduper.h" +#include "software/ai/navigator/obstacle/obstacle.hpp" + +#include + +TEST(VisProtoDeduperTest, hash_test) +{ + VisProtoDeduper deduper(10); + + auto polygon1 = Polygon({Point(4.20, 4.20), Point(1.0, 1.1), Point(-1.0, -153.52)}); + auto polygon2 = Polygon({Point(4.20, 4.20), Point(1.0, 1.1), Point(-1.0, -153.52)}); + + auto polygon1_msg = createObstacleProto(polygon1); + auto polygon2_msg = createObstacleProto(polygon2); + EXPECT_EQ(deduper.hash(polygon1_msg), deduper.hash(polygon2_msg)); +} From f056154687479c2e649f3c299bc550b077bbdcbd Mon Sep 17 00:00:00 2001 From: Minghao Li Date: Thu, 22 Jan 2026 13:13:51 -0800 Subject: [PATCH 2/9] make deduper static --- .../ai/hl/stp/tactic/move_primitive.cpp | 2 + .../ai/hl/stp/tactic/move_primitive.h | 2 + .../ai/hl/stp/tactic/vis_proto_deduper.cpp | 82 +++++++++++++++++++ 3 files changed, 86 insertions(+) create mode 100644 src/software/ai/hl/stp/tactic/vis_proto_deduper.cpp diff --git a/src/software/ai/hl/stp/tactic/move_primitive.cpp b/src/software/ai/hl/stp/tactic/move_primitive.cpp index e77bb450d2..920a9f0089 100644 --- a/src/software/ai/hl/stp/tactic/move_primitive.cpp +++ b/src/software/ai/hl/stp/tactic/move_primitive.cpp @@ -8,6 +8,8 @@ #include "software/ai/navigator/trajectory/bang_bang_trajectory_1d_angular.h" #include "software/geom/algorithms/end_in_obstacle_sample.h" +VisProtoDeduper MovePrimitive::vis_proto_deduper(100); + MovePrimitive::MovePrimitive( const Robot &robot, const Point &destination, const Angle &final_angle, const TbotsProto::MaxAllowedSpeedMode &max_allowed_speed_mode, diff --git a/src/software/ai/hl/stp/tactic/move_primitive.h b/src/software/ai/hl/stp/tactic/move_primitive.h index 2149686aea..5e4d86831e 100644 --- a/src/software/ai/hl/stp/tactic/move_primitive.h +++ b/src/software/ai/hl/stp/tactic/move_primitive.h @@ -99,5 +99,7 @@ class MovePrimitive : public Primitive BangBangTrajectory1DAngular angular_trajectory; TrajectoryPlanner planner; + static VisProtoDeduper vis_proto_deduper; + constexpr static unsigned int NUM_TRAJECTORY_VISUALIZATION_POINTS = 10; }; diff --git a/src/software/ai/hl/stp/tactic/vis_proto_deduper.cpp b/src/software/ai/hl/stp/tactic/vis_proto_deduper.cpp new file mode 100644 index 0000000000..8db6e5a102 --- /dev/null +++ b/src/software/ai/hl/stp/tactic/vis_proto_deduper.cpp @@ -0,0 +1,82 @@ +#include "vis_proto_deduper.h" +#include + +VisProtoDeduper::VisProtoDeduper(unsigned int window_size): + window_size(window_size) {} + +void VisProtoDeduper::loadDistinct(const std::vector &obstacle_list, TbotsProto::ObstacleList& obstacle_list_out) { + // Lazily evit the ObstacleList from the deque + if (sent_queue.size() > window_size) { + const std::vector& popped_hashes = sent_queue.front(); + for (const auto &obstacle_hash : popped_hashes) { + sent_set.erase(obstacle_hash); + } + + sent_queue.pop_front(); + } + + std::vector current_hashes; + for (const auto &obstacle : obstacle_list) { + // TODO: may optimize later to get rid of obstacle->createObstacleProto(); + TbotsProto::Obstacle proto = obstacle->createObstacleProto(); + std::size_t hash_val = hash(proto); + if (sent_set.count(hash_val) == 0) { + 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)); +} + +void VisProtoDeduper::hashCombine(std::size_t &seed, std::size_t value) const { + seed ^= value + 0x9e3779b9 + (seed << 6) + (seed >> 2); +} + +// TODO: rewrite the following function generated by AI, move each hash function to each class +std::size_t VisProtoDeduper::hash(const TbotsProto::Obstacle &obstacle) const { + std::size_t seed = 0; + auto mix_float = [&](double val) { + long long quantized = static_cast(val * 10000.0); + hashCombine(seed, std::hash{}(quantized)); + }; + + if (obstacle.has_circle()) { + hashCombine(seed, 1); + const auto& circle = obstacle.circle(); + + if (circle.has_origin()) { + mix_float(circle.origin().x_meters()); + mix_float(circle.origin().y_meters()); + } + mix_float(circle.radius()); + } + else if (obstacle.has_stadium()) { + hashCombine(seed, 2); + const auto& stadium = obstacle.stadium(); + + if (stadium.has_segment()) { + const auto& seg = stadium.segment(); + if (seg.has_start()) { + mix_float(seg.start().x_meters()); + mix_float(seg.start().y_meters()); + } + if (seg.has_end()) { + mix_float(seg.end().x_meters()); + mix_float(seg.end().y_meters()); + } + } + mix_float(stadium.radius()); + } + else if (obstacle.has_polygon()) { + hashCombine(seed, 3); + const auto& poly = obstacle.polygon(); + + for (const auto& point : poly.points()) { + mix_float(point.x_meters()); + mix_float(point.y_meters()); + } + } + + return seed; +} From a434ce05f57da82764016c581e04c052777283f9 Mon Sep 17 00:00:00 2001 From: Minghao Li Date: Thu, 22 Jan 2026 16:14:45 -0800 Subject: [PATCH 3/9] refactor code --- .../ai/hl/stp/tactic/move_primitive.cpp | 7 ++--- .../ai/hl/stp/tactic/vis_proto_deduper.cpp | 7 +++-- .../ai/hl/stp/tactic/vis_proto_deduper.h | 28 +++++++++++++++++++ 3 files changed, 34 insertions(+), 8 deletions(-) create mode 100644 src/software/ai/hl/stp/tactic/vis_proto_deduper.h diff --git a/src/software/ai/hl/stp/tactic/move_primitive.cpp b/src/software/ai/hl/stp/tactic/move_primitive.cpp index 920a9f0089..ca42863070 100644 --- a/src/software/ai/hl/stp/tactic/move_primitive.cpp +++ b/src/software/ai/hl/stp/tactic/move_primitive.cpp @@ -8,7 +8,7 @@ #include "software/ai/navigator/trajectory/bang_bang_trajectory_1d_angular.h" #include "software/geom/algorithms/end_in_obstacle_sample.h" -VisProtoDeduper MovePrimitive::vis_proto_deduper(100); +VisProtoDeduper MovePrimitive::vis_proto_deduper(30); MovePrimitive::MovePrimitive( const Robot &robot, const Point &destination, const Angle &final_angle, @@ -267,10 +267,7 @@ 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()); - } + vis_proto_deduper.dedupeAndFill(obstacles, obstacle_list_out); TbotsProto::Path path; if (traj_path.has_value()) diff --git a/src/software/ai/hl/stp/tactic/vis_proto_deduper.cpp b/src/software/ai/hl/stp/tactic/vis_proto_deduper.cpp index 8db6e5a102..c39c074ced 100644 --- a/src/software/ai/hl/stp/tactic/vis_proto_deduper.cpp +++ b/src/software/ai/hl/stp/tactic/vis_proto_deduper.cpp @@ -4,7 +4,7 @@ VisProtoDeduper::VisProtoDeduper(unsigned int window_size): window_size(window_size) {} -void VisProtoDeduper::loadDistinct(const std::vector &obstacle_list, TbotsProto::ObstacleList& obstacle_list_out) { +void VisProtoDeduper::dedupeAndFill(const std::vector &obstacle_list, TbotsProto::ObstacleList& obstacle_list_out) { // Lazily evit the ObstacleList from the deque if (sent_queue.size() > window_size) { const std::vector& popped_hashes = sent_queue.front(); @@ -24,6 +24,8 @@ void VisProtoDeduper::loadDistinct(const std::vector &obstacle_list sent_set.insert(hash_val); obstacle_list_out.add_obstacles()->CopyFrom(proto); current_hashes.push_back(hash_val); + } else { + // std::cout << "Duplicate" << std::endl; } } sent_queue.push_back(std::move(current_hashes)); @@ -37,8 +39,7 @@ void VisProtoDeduper::hashCombine(std::size_t &seed, std::size_t value) const { std::size_t VisProtoDeduper::hash(const TbotsProto::Obstacle &obstacle) const { std::size_t seed = 0; auto mix_float = [&](double val) { - long long quantized = static_cast(val * 10000.0); - hashCombine(seed, std::hash{}(quantized)); + hashCombine(seed, std::hash{}(val)); }; if (obstacle.has_circle()) { diff --git a/src/software/ai/hl/stp/tactic/vis_proto_deduper.h b/src/software/ai/hl/stp/tactic/vis_proto_deduper.h new file mode 100644 index 0000000000..10e00ee266 --- /dev/null +++ b/src/software/ai/hl/stp/tactic/vis_proto_deduper.h @@ -0,0 +1,28 @@ +#pragma once + +#include +#include + +#include "software/ai/navigator/obstacle/obstacle.hpp" + +class VisProtoDeduper { +public: + /** + * Creates a Visualization Proto Sliding Window Deduplicator + * + * @param window_size size of the sliding window + */ + VisProtoDeduper(unsigned int window_size); + + void dedupeAndFill(const std::vector& obstacle_list, TbotsProto::ObstacleList& obstacle_list_out); + + std::size_t hash(const TbotsProto::Obstacle& obstacle) const; + +private: + unsigned int window_size; + std::unordered_set sent_set; + std::deque> sent_queue; + + void hashCombine(std::size_t& seed, std::size_t value) const; +}; + From 1ce77a5fabb682875126fec0e0de317dff02b0ed Mon Sep 17 00:00:00 2001 From: Minghao Li Date: Fri, 23 Jan 2026 16:17:08 -0800 Subject: [PATCH 4/9] refactor hash computation --- src/software/ai/hl/stp/tactic/BUILD | 25 ++++++++ .../ai/hl/stp/tactic/move_primitive.cpp | 2 +- .../ai/hl/stp/tactic/vis_proto_deduper.cpp | 59 +------------------ .../ai/hl/stp/tactic/vis_proto_deduper.h | 3 +- .../ai/navigator/obstacle/geom_obstacle.hpp | 10 ++++ .../ai/navigator/obstacle/obstacle.hpp | 17 ++++++ src/software/geom/BUILD | 1 + src/software/geom/polygon.h | 20 +++++++ src/software/geom/rectangle.h | 19 ++++++ src/software/geom/stadium.h | 13 ++++ src/software/util/hash/hash_combine.h | 21 +++++++ 11 files changed, 131 insertions(+), 59 deletions(-) create mode 100644 src/software/util/hash/hash_combine.h diff --git a/src/software/ai/hl/stp/tactic/BUILD b/src/software/ai/hl/stp/tactic/BUILD index c69db8f0a3..efc42aad27 100644 --- a/src/software/ai/hl/stp/tactic/BUILD +++ b/src/software/ai/hl/stp/tactic/BUILD @@ -142,3 +142,28 @@ 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", + ] +) + +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", + ] +) + diff --git a/src/software/ai/hl/stp/tactic/move_primitive.cpp b/src/software/ai/hl/stp/tactic/move_primitive.cpp index ca42863070..229dd50ebb 100644 --- a/src/software/ai/hl/stp/tactic/move_primitive.cpp +++ b/src/software/ai/hl/stp/tactic/move_primitive.cpp @@ -8,7 +8,7 @@ #include "software/ai/navigator/trajectory/bang_bang_trajectory_1d_angular.h" #include "software/geom/algorithms/end_in_obstacle_sample.h" -VisProtoDeduper MovePrimitive::vis_proto_deduper(30); +VisProtoDeduper MovePrimitive::vis_proto_deduper(5); MovePrimitive::MovePrimitive( const Robot &robot, const Point &destination, const Angle &final_angle, diff --git a/src/software/ai/hl/stp/tactic/vis_proto_deduper.cpp b/src/software/ai/hl/stp/tactic/vis_proto_deduper.cpp index c39c074ced..c9d6a8fc11 100644 --- a/src/software/ai/hl/stp/tactic/vis_proto_deduper.cpp +++ b/src/software/ai/hl/stp/tactic/vis_proto_deduper.cpp @@ -1,5 +1,4 @@ #include "vis_proto_deduper.h" -#include VisProtoDeduper::VisProtoDeduper(unsigned int window_size): window_size(window_size) {} @@ -15,69 +14,17 @@ void VisProtoDeduper::dedupeAndFill(const std::vector &obstacle_lis sent_queue.pop_front(); } + // Computing hashes of the current obstacle list and compare with the window std::vector current_hashes; for (const auto &obstacle : obstacle_list) { - // TODO: may optimize later to get rid of obstacle->createObstacleProto(); - TbotsProto::Obstacle proto = obstacle->createObstacleProto(); - std::size_t hash_val = hash(proto); + std::size_t hash_val = obstacle_hasher(*obstacle); 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); - } else { - // std::cout << "Duplicate" << std::endl; } } sent_queue.push_back(std::move(current_hashes)); } -void VisProtoDeduper::hashCombine(std::size_t &seed, std::size_t value) const { - seed ^= value + 0x9e3779b9 + (seed << 6) + (seed >> 2); -} - -// TODO: rewrite the following function generated by AI, move each hash function to each class -std::size_t VisProtoDeduper::hash(const TbotsProto::Obstacle &obstacle) const { - std::size_t seed = 0; - auto mix_float = [&](double val) { - hashCombine(seed, std::hash{}(val)); - }; - - if (obstacle.has_circle()) { - hashCombine(seed, 1); - const auto& circle = obstacle.circle(); - - if (circle.has_origin()) { - mix_float(circle.origin().x_meters()); - mix_float(circle.origin().y_meters()); - } - mix_float(circle.radius()); - } - else if (obstacle.has_stadium()) { - hashCombine(seed, 2); - const auto& stadium = obstacle.stadium(); - - if (stadium.has_segment()) { - const auto& seg = stadium.segment(); - if (seg.has_start()) { - mix_float(seg.start().x_meters()); - mix_float(seg.start().y_meters()); - } - if (seg.has_end()) { - mix_float(seg.end().x_meters()); - mix_float(seg.end().y_meters()); - } - } - mix_float(stadium.radius()); - } - else if (obstacle.has_polygon()) { - hashCombine(seed, 3); - const auto& poly = obstacle.polygon(); - - for (const auto& point : poly.points()) { - mix_float(point.x_meters()); - mix_float(point.y_meters()); - } - } - - return seed; -} diff --git a/src/software/ai/hl/stp/tactic/vis_proto_deduper.h b/src/software/ai/hl/stp/tactic/vis_proto_deduper.h index 10e00ee266..7e1ffcb3ec 100644 --- a/src/software/ai/hl/stp/tactic/vis_proto_deduper.h +++ b/src/software/ai/hl/stp/tactic/vis_proto_deduper.h @@ -16,13 +16,12 @@ class VisProtoDeduper { void dedupeAndFill(const std::vector& obstacle_list, TbotsProto::ObstacleList& obstacle_list_out); - std::size_t hash(const TbotsProto::Obstacle& obstacle) const; private: unsigned int window_size; std::unordered_set sent_set; std::deque> sent_queue; - void hashCombine(std::size_t& seed, std::size_t value) const; + std::hash obstacle_hasher; }; diff --git a/src/software/ai/navigator/obstacle/geom_obstacle.hpp b/src/software/ai/navigator/obstacle/geom_obstacle.hpp index f83a1938c1..7623d5565b 100644 --- a/src/software/ai/navigator/obstacle/geom_obstacle.hpp +++ b/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 + template 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 rasterize(const double resolution_size) const override; + std::size_t hash() const override; /** * Gets the underlying GEOM_TYPE @@ -116,3 +119,10 @@ void GeomObstacle::accept(ObstacleVisitor& visitor) const { visitor.visit(*this); } + +template +std::size_t GeomObstacle::hash() const +{ + return std::hash{}(geom_); +} + diff --git a/src/software/ai/navigator/obstacle/obstacle.hpp b/src/software/ai/navigator/obstacle/obstacle.hpp index b1a0477ffc..508c9b610b 100644 --- a/src/software/ai/navigator/obstacle/obstacle.hpp +++ b/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 +{ + size_t operator()(const Obstacle &obstacle) const + { + return obstacle.hash(); + } +}; + diff --git a/src/software/geom/BUILD b/src/software/geom/BUILD index 7788c52f10..3b25c08bd8 100644 --- a/src/software/geom/BUILD +++ b/src/software/geom/BUILD @@ -64,6 +64,7 @@ cc_library( deps = [ ":segment", ":shape", + "//software/util/hash:hash_combine", ], ) diff --git a/src/software/geom/polygon.h b/src/software/geom/polygon.h index 206df68a18..2806413ee8 100644 --- a/src/software/geom/polygon.h +++ b/src/software/geom/polygon.h @@ -4,6 +4,7 @@ #include "software/geom/segment.h" #include "software/geom/shape.h" +#include "software/util/hash/hash_combine.h" /** * A shape composed of line segments. @@ -100,3 +101,22 @@ bool operator!=(const Polygon& poly1, const Polygon& poly2); * @return The output stream with the string representation of the class appended */ std::ostream& operator<<(std::ostream& os, const Polygon& poly); + + +template <> +struct std::hash +{ + size_t operator()(const Polygon &polygon) const + { + size_t seed = 0; + for (const auto& point : polygon.getPoints()) + { + hashCombine(seed, point); + } + for (const auto& segment: polygon.getSegments()) + { + hashCombine(seed, segment); + } + return seed; + } +}; diff --git a/src/software/geom/rectangle.h b/src/software/geom/rectangle.h index 06b26b0890..7de83690dc 100644 --- a/src/software/geom/rectangle.h +++ b/src/software/geom/rectangle.h @@ -1,6 +1,9 @@ #pragma once +#include + #include "software/geom/convex_polygon.h" +#include "software/util/hash/hash_combine.h" /** * A rectangle is a ConvexPolygon of four Points with the invariant that two sides are @@ -125,3 +128,19 @@ class Rectangle : public ConvexPolygon bool operator==(const Rectangle &p) const; }; + +template <> +struct std::hash +{ + size_t operator()(const Rectangle &rectangle) const + { + std::hash hasher; + std::size_t seed = 0; + hashCombine(seed, hasher(rectangle.posXPosYCorner())); + hashCombine(seed, hasher(rectangle.negXPosYCorner())); + hashCombine(seed, hasher(rectangle.negXNegYCorner())); + hashCombine(seed, hasher(rectangle.posXNegYCorner())); + return seed; + } +}; + diff --git a/src/software/geom/stadium.h b/src/software/geom/stadium.h index aa32da9448..4bed63c69f 100644 --- a/src/software/geom/stadium.h +++ b/src/software/geom/stadium.h @@ -99,3 +99,16 @@ bool operator!=(const Stadium &s1, const Stadium &s2); * @return */ std::ostream &operator<<(std::ostream &os, const Stadium &stadium); + +template <> +struct std::hash +{ + std::size_t operator()(const Stadium& stadium) + { + std::size_t seed = 0; + hashCombine(seed, std::hash{}(stadium.segment())); + hashCombine(seed, std::hash{}(stadium.radius())); + return seed; + } +}; + diff --git a/src/software/util/hash/hash_combine.h b/src/software/util/hash/hash_combine.h new file mode 100644 index 0000000000..d3aec2824e --- /dev/null +++ b/src/software/util/hash/hash_combine.h @@ -0,0 +1,21 @@ +#pragma once + +#include +#include + + +/** +* Combine hash values of multiple objects +* +* @param seed current hash value to be updated +* @param v object to add to the current hash value +* NOTE: needs to ensure type T has std::hash template function implemented +* +* https://stackoverflow.com/questions/7222143/unordered-map-hash-function-c +*/ +template +inline void hashCombine(std::size_t& seed, const T& v) { + std::hash hasher; + seed ^= hasher(v) + 0x9e3779b9 + (seed << 6) + (seed >> 2); +} + From deb8158c25fb45a79cfa41d487b0cb90d0973f06 Mon Sep 17 00:00:00 2001 From: Minghao Li Date: Fri, 23 Jan 2026 17:35:02 -0800 Subject: [PATCH 5/9] add BUILD for hash tools --- src/software/util/hash/BUILD | 8 ++++++++ 1 file changed, 8 insertions(+) create mode 100644 src/software/util/hash/BUILD diff --git a/src/software/util/hash/BUILD b/src/software/util/hash/BUILD new file mode 100644 index 0000000000..c58138e5ba --- /dev/null +++ b/src/software/util/hash/BUILD @@ -0,0 +1,8 @@ +package(default_visibility = ["//visibility:public"]) + +cc_library( + name = "hash_combine", + srcs = [], + hdrs = ["hash_combine.h"], +) + From 4fff698360b2d761974298896adfb4ce61d95474 Mon Sep 17 00:00:00 2001 From: Minghao Li Date: Sat, 24 Jan 2026 12:11:55 -0800 Subject: [PATCH 6/9] bug fix --- src/software/ai/hl/stp/tactic/BUILD | 1 + src/software/ai/hl/stp/tactic/move_primitive.h | 1 + 2 files changed, 2 insertions(+) diff --git a/src/software/ai/hl/stp/tactic/BUILD b/src/software/ai/hl/stp/tactic/BUILD index efc42aad27..c87bf72239 100644 --- a/src/software/ai/hl/stp/tactic/BUILD +++ b/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", ], ) diff --git a/src/software/ai/hl/stp/tactic/move_primitive.h b/src/software/ai/hl/stp/tactic/move_primitive.h index 5e4d86831e..1d2581db98 100644 --- a/src/software/ai/hl/stp/tactic/move_primitive.h +++ b/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" From 54bf7ca1c55f005d33e843838ef794ea183725cf Mon Sep 17 00:00:00 2001 From: Minghao Li Date: Mon, 26 Jan 2026 12:23:48 -0800 Subject: [PATCH 7/9] formalize code --- .../ai/hl/stp/tactic/move_primitive.cpp | 6 ++- .../ai/hl/stp/tactic/move_primitive.h | 5 ++- .../ai/hl/stp/tactic/vis_proto_deduper.cpp | 23 ++++++----- .../ai/hl/stp/tactic/vis_proto_deduper.h | 41 ++++++++++++++++--- .../hl/stp/tactic/vis_proto_deduper_test.cpp | 1 - .../ai/navigator/obstacle/obstacle.hpp | 2 +- src/software/geom/polygon.h | 4 +- src/software/geom/rectangle.h | 2 +- 8 files changed, 59 insertions(+), 25 deletions(-) diff --git a/src/software/ai/hl/stp/tactic/move_primitive.cpp b/src/software/ai/hl/stp/tactic/move_primitive.cpp index 229dd50ebb..a744f592b8 100644 --- a/src/software/ai/hl/stp/tactic/move_primitive.cpp +++ b/src/software/ai/hl/stp/tactic/move_primitive.cpp @@ -8,7 +8,6 @@ #include "software/ai/navigator/trajectory/bang_bang_trajectory_1d_angular.h" #include "software/geom/algorithms/end_in_obstacle_sample.h" -VisProtoDeduper MovePrimitive::vis_proto_deduper(5); MovePrimitive::MovePrimitive( const Robot &robot, const Point &destination, const Angle &final_angle, @@ -267,7 +266,10 @@ void MovePrimitive::getVisualizationProtos( TbotsProto::ObstacleList &obstacle_list_out, TbotsProto::PathVisualization &path_visualization_out) const { - vis_proto_deduper.dedupeAndFill(obstacles, obstacle_list_out); + // 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()) diff --git a/src/software/ai/hl/stp/tactic/move_primitive.h b/src/software/ai/hl/stp/tactic/move_primitive.h index 1d2581db98..c639eb5ed2 100644 --- a/src/software/ai/hl/stp/tactic/move_primitive.h +++ b/src/software/ai/hl/stp/tactic/move_primitive.h @@ -100,7 +100,8 @@ class MovePrimitive : public Primitive BangBangTrajectory1DAngular angular_trajectory; TrajectoryPlanner planner; - static VisProtoDeduper vis_proto_deduper; - 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}; }; diff --git a/src/software/ai/hl/stp/tactic/vis_proto_deduper.cpp b/src/software/ai/hl/stp/tactic/vis_proto_deduper.cpp index c9d6a8fc11..1ed93cf561 100644 --- a/src/software/ai/hl/stp/tactic/vis_proto_deduper.cpp +++ b/src/software/ai/hl/stp/tactic/vis_proto_deduper.cpp @@ -1,30 +1,31 @@ #include "vis_proto_deduper.h" VisProtoDeduper::VisProtoDeduper(unsigned int window_size): - window_size(window_size) {} + window_size_(window_size) {} void VisProtoDeduper::dedupeAndFill(const std::vector &obstacle_list, TbotsProto::ObstacleList& obstacle_list_out) { - // Lazily evit the ObstacleList from the deque - if (sent_queue.size() > window_size) { - const std::vector& popped_hashes = sent_queue.front(); + // lazily evict the ObstacleList from the deque + if (sent_queue_.size() > window_size_) { + const std::vector& popped_hashes = sent_queue_.front(); for (const auto &obstacle_hash : popped_hashes) { - sent_set.erase(obstacle_hash); + sent_set_.erase(obstacle_hash); } - sent_queue.pop_front(); + sent_queue_.pop_front(); } - // Computing hashes of the current obstacle list and compare with the window + // computing hashes of the current obstacle list and compare with the window std::vector current_hashes; for (const auto &obstacle : obstacle_list) { - std::size_t hash_val = obstacle_hasher(*obstacle); - if (sent_set.count(hash_val) == 0) { + 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); + 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)); + sent_queue_.push_back(std::move(current_hashes)); } diff --git a/src/software/ai/hl/stp/tactic/vis_proto_deduper.h b/src/software/ai/hl/stp/tactic/vis_proto_deduper.h index 7e1ffcb3ec..964a883fa4 100644 --- a/src/software/ai/hl/stp/tactic/vis_proto_deduper.h +++ b/src/software/ai/hl/stp/tactic/vis_proto_deduper.h @@ -5,23 +5,54 @@ #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 Visualization Proto Sliding Window Deduplicator + * 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& obstacle_list, TbotsProto::ObstacleList& obstacle_list_out); private: - unsigned int window_size; - std::unordered_set sent_set; - std::deque> sent_queue; + unsigned int window_size_; + std::unordered_set sent_set_; + std::deque> sent_queue_; - std::hash obstacle_hasher; + std::hash obstacle_hasher_; }; diff --git a/src/software/ai/hl/stp/tactic/vis_proto_deduper_test.cpp b/src/software/ai/hl/stp/tactic/vis_proto_deduper_test.cpp index ed85e3c985..2cfcdd9725 100644 --- a/src/software/ai/hl/stp/tactic/vis_proto_deduper_test.cpp +++ b/src/software/ai/hl/stp/tactic/vis_proto_deduper_test.cpp @@ -12,5 +12,4 @@ TEST(VisProtoDeduperTest, hash_test) auto polygon1_msg = createObstacleProto(polygon1); auto polygon2_msg = createObstacleProto(polygon2); - EXPECT_EQ(deduper.hash(polygon1_msg), deduper.hash(polygon2_msg)); } diff --git a/src/software/ai/navigator/obstacle/obstacle.hpp b/src/software/ai/navigator/obstacle/obstacle.hpp index 508c9b610b..55dde7381b 100644 --- a/src/software/ai/navigator/obstacle/obstacle.hpp +++ b/src/software/ai/navigator/obstacle/obstacle.hpp @@ -155,7 +155,7 @@ inline std::ostream& operator<<(std::ostream& os, const ObstaclePtr& obstacle_pt template <> struct std::hash { - size_t operator()(const Obstacle &obstacle) const + std::size_t operator()(const Obstacle &obstacle) const { return obstacle.hash(); } diff --git a/src/software/geom/polygon.h b/src/software/geom/polygon.h index 2806413ee8..1e9dcf79ce 100644 --- a/src/software/geom/polygon.h +++ b/src/software/geom/polygon.h @@ -106,9 +106,9 @@ std::ostream& operator<<(std::ostream& os, const Polygon& poly); template <> struct std::hash { - size_t operator()(const Polygon &polygon) const + std::size_t operator()(const Polygon &polygon) const { - size_t seed = 0; + std::size_t seed = 0; for (const auto& point : polygon.getPoints()) { hashCombine(seed, point); diff --git a/src/software/geom/rectangle.h b/src/software/geom/rectangle.h index 7de83690dc..889c3bf531 100644 --- a/src/software/geom/rectangle.h +++ b/src/software/geom/rectangle.h @@ -132,7 +132,7 @@ class Rectangle : public ConvexPolygon template <> struct std::hash { - size_t operator()(const Rectangle &rectangle) const + std::size_t operator()(const Rectangle &rectangle) const { std::hash hasher; std::size_t seed = 0; From 882df2a69c1d9496745859223b8d52a930af472b Mon Sep 17 00:00:00 2001 From: Minghao Li Date: Mon, 26 Jan 2026 12:35:09 -0800 Subject: [PATCH 8/9] add unit tests --- .../hl/stp/tactic/vis_proto_deduper_test.cpp | 136 +++++++++++++++++- 1 file changed, 130 insertions(+), 6 deletions(-) diff --git a/src/software/ai/hl/stp/tactic/vis_proto_deduper_test.cpp b/src/software/ai/hl/stp/tactic/vis_proto_deduper_test.cpp index 2cfcdd9725..b91015e5fc 100644 --- a/src/software/ai/hl/stp/tactic/vis_proto_deduper_test.cpp +++ b/src/software/ai/hl/stp/tactic/vis_proto_deduper_test.cpp @@ -1,15 +1,139 @@ + #include "software/ai/hl/stp/tactic/vis_proto_deduper.h" #include "software/ai/navigator/obstacle/obstacle.hpp" +#include "software/geom/polygon.h" +#include "software/geom/point.h" #include +#include +#include + +// 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 + 1.0), Point(x - 1.0, y - 1.0)}); + return std::make_shared(polygon); +} + +class VisProtoDeduperTest : public ::testing::Test +{ +protected: + // Helper to extract the list of obstacles from the proto message for easy verification + std::vector getObstaclesFromProto(const TbotsProto::ObstacleList& msg) + { + std::vector obstacles; + for (const auto& obs : msg.obstacles()) + { + obstacles.push_back(obs); + } + return obstacles; + } +}; + +TEST_F(VisProtoDeduperTest, DeduplicatesRepeatedObstacles) +{ + VisProtoDeduper deduper(5); + TbotsProto::ObstacleList output_msg; + + auto obs1 = createTestObstacle(10, 10); + std::vector 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); -TEST(VisProtoDeduperTest, hash_test) + 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) { - VisProtoDeduper deduper(10); + // 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); - auto polygon1 = Polygon({Point(4.20, 4.20), Point(1.0, 1.1), Point(-1.0, -153.52)}); - auto polygon2 = Polygon({Point(4.20, 4.20), Point(1.0, 1.1), Point(-1.0, -153.52)}); + // Pass 1 + deduper.dedupeAndFill({obs}, output_msg); + EXPECT_EQ(output_msg.obstacles_size(), 1); + output_msg.Clear(); - auto polygon1_msg = createObstacleProto(polygon1); - auto polygon2_msg = createObstacleProto(polygon2); + // 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); +} + From 9c3788871ccffe016facd02cc3cb3fef6439d035 Mon Sep 17 00:00:00 2001 From: Minghao Li Date: Sat, 7 Feb 2026 20:31:37 +0000 Subject: [PATCH 9/9] fix unit tests --- src/software/ai/hl/stp/tactic/BUILD | 1 + .../hl/stp/tactic/vis_proto_deduper_test.cpp | 22 ++++++++++++------- 2 files changed, 15 insertions(+), 8 deletions(-) diff --git a/src/software/ai/hl/stp/tactic/BUILD b/src/software/ai/hl/stp/tactic/BUILD index c87bf72239..80802c82d7 100644 --- a/src/software/ai/hl/stp/tactic/BUILD +++ b/src/software/ai/hl/stp/tactic/BUILD @@ -154,6 +154,7 @@ cc_library( ":primitive", "//proto/message_translation:tbots_protobuf", "//software/util/hash:hash_combine", + "//software/ai/navigator/obstacle:robot_navigation_obstacle_factory", ] ) diff --git a/src/software/ai/hl/stp/tactic/vis_proto_deduper_test.cpp b/src/software/ai/hl/stp/tactic/vis_proto_deduper_test.cpp index b91015e5fc..a014226945 100644 --- a/src/software/ai/hl/stp/tactic/vis_proto_deduper_test.cpp +++ b/src/software/ai/hl/stp/tactic/vis_proto_deduper_test.cpp @@ -1,6 +1,7 @@ #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" @@ -8,17 +9,14 @@ #include #include -// 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 + 1.0), Point(x - 1.0, y - 1.0)}); - return std::make_shared(polygon); -} + 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 getObstaclesFromProto(const TbotsProto::ObstacleList& msg) { @@ -27,7 +25,15 @@ class VisProtoDeduperTest : public ::testing::Test { obstacles.push_back(obs); } - return obstacles; + 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); } };