diff --git a/src/software/ai/hl/stp/tactic/BUILD b/src/software/ai/hl/stp/tactic/BUILD index c69db8f0a3..80802c82d7 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", ], ) @@ -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", + ] +) + diff --git a/src/software/ai/hl/stp/tactic/move_primitive.cpp b/src/software/ai/hl/stp/tactic/move_primitive.cpp index e77bb450d2..a744f592b8 100644 --- a/src/software/ai/hl/stp/tactic/move_primitive.cpp +++ b/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()) diff --git a/src/software/ai/hl/stp/tactic/move_primitive.h b/src/software/ai/hl/stp/tactic/move_primitive.h index 2149686aea..c639eb5ed2 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" @@ -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}; }; 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..1ed93cf561 --- /dev/null +++ b/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 &obstacle_list, TbotsProto::ObstacleList& obstacle_list_out) { + // 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_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) { + 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)); +} + 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..964a883fa4 --- /dev/null +++ b/src/software/ai/hl/stp/tactic/vis_proto_deduper.h @@ -0,0 +1,58 @@ +#pragma once + +#include +#include + +#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& obstacle_list, TbotsProto::ObstacleList& obstacle_list_out); + + +private: + unsigned int window_size_; + std::unordered_set sent_set_; + std::deque> sent_queue_; + + 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 new file mode 100644 index 0000000000..a014226945 --- /dev/null +++ b/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 +#include +#include + + + +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) + { + std::vector 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 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); +} + 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..55dde7381b 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 +{ + std::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..1e9dcf79ce 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 +{ + std::size_t operator()(const Polygon &polygon) const + { + std::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..889c3bf531 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 +{ + std::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/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"], +) + 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); +} +