Skip to content

Commit 59e3fc7

Browse files
nycratnycrat
committed
Merge branch 'master' into avah/move_pytests_to_own_directory
2 parents e043d7c + fd47670 commit 59e3fc7

14 files changed

Lines changed: 380 additions & 4 deletions

File tree

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

Lines changed: 26 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -113,6 +113,7 @@ cc_library(
113113
],
114114
deps = [
115115
":primitive",
116+
":vis_proto_deduper",
116117
"//proto/message_translation:tbots_protobuf",
117118
"//proto/primitive:primitive_msg_factory",
118119
"//software/ai/navigator/trajectory:trajectory_planner",
@@ -142,3 +143,28 @@ cc_test(
142143
"//software/test_util",
143144
],
144145
)
146+
147+
cc_library(
148+
name = "vis_proto_deduper",
149+
srcs = ["vis_proto_deduper.cpp"],
150+
hdrs = [
151+
"vis_proto_deduper.h",
152+
],
153+
deps = [
154+
":primitive",
155+
"//proto/message_translation:tbots_protobuf",
156+
"//software/ai/navigator/obstacle:robot_navigation_obstacle_factory",
157+
"//software/util/hash:hash_combine",
158+
],
159+
)
160+
161+
cc_test(
162+
name = "vis_proto_deduper_test",
163+
srcs = ["vis_proto_deduper_test.cpp"],
164+
deps = [
165+
":primitive",
166+
":vis_proto_deduper",
167+
"//shared/test_util:tbots_gtest_main",
168+
"//software/test_util",
169+
],
170+
)

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

Lines changed: 5 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -8,6 +8,7 @@
88
#include "software/ai/navigator/trajectory/bang_bang_trajectory_1d_angular.h"
99
#include "software/geom/algorithms/end_in_obstacle_sample.h"
1010

11+
1112
MovePrimitive::MovePrimitive(
1213
const Robot &robot, const Point &destination, const Angle &final_angle,
1314
const TbotsProto::MaxAllowedSpeedMode &max_allowed_speed_mode,
@@ -263,10 +264,10 @@ void MovePrimitive::getVisualizationProtos(
263264
TbotsProto::ObstacleList &obstacle_list_out,
264265
TbotsProto::PathVisualization &path_visualization_out) const
265266
{
266-
for (const auto &obstacle : obstacles)
267-
{
268-
obstacle_list_out.add_obstacles()->CopyFrom(obstacle->createObstacleProto());
269-
}
267+
// If we are sending lots of duplicated obstacles, then it will cause the system
268+
// network buffer overflow. Therefore, we selectively populate some of the obstacles.
269+
// See the implementation of VisProtoDeduper
270+
vis_proto_deduper.dedupeAndFill(obstacles, obstacle_list_out);
270271

271272
TbotsProto::Path path;
272273
if (traj_path.has_value())

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

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -3,6 +3,7 @@
33

44
#include "proto/primitive/primitive_types.h"
55
#include "software/ai/hl/stp/tactic/primitive.h"
6+
#include "software/ai/hl/stp/tactic/vis_proto_deduper.h"
67
#include "software/ai/navigator/trajectory/bang_bang_trajectory_1d_angular.h"
78
#include "software/ai/navigator/trajectory/bang_bang_trajectory_2d.h"
89
#include "software/ai/navigator/trajectory/trajectory_planner.h"
@@ -100,4 +101,7 @@ class MovePrimitive : public Primitive
100101
TrajectoryPlanner planner;
101102

102103
constexpr static unsigned int NUM_TRAJECTORY_VISUALIZATION_POINTS = 10;
104+
constexpr static unsigned int PROTO_DEDUPER_WINDOW_SIZE = 5;
105+
106+
inline static VisProtoDeduper vis_proto_deduper{PROTO_DEDUPER_WINDOW_SIZE};
103107
};

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

Lines changed: 35 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,35 @@
1+
#include "vis_proto_deduper.h"
2+
3+
VisProtoDeduper::VisProtoDeduper(unsigned int window_size) : window_size_(window_size) {}
4+
5+
void VisProtoDeduper::dedupeAndFill(const std::vector<ObstaclePtr> &obstacle_list,
6+
TbotsProto::ObstacleList &obstacle_list_out)
7+
{
8+
// lazily evict the ObstacleList from the deque
9+
if (sent_queue_.size() > window_size_)
10+
{
11+
const std::vector<std::size_t> &popped_hashes = sent_queue_.front();
12+
for (const auto &obstacle_hash : popped_hashes)
13+
{
14+
sent_set_.erase(obstacle_hash);
15+
}
16+
17+
sent_queue_.pop_front();
18+
}
19+
20+
// computing hashes of the current obstacle list and compare with the window
21+
std::vector<std::size_t> current_hashes;
22+
for (const auto &obstacle : obstacle_list)
23+
{
24+
std::size_t hash_val = obstacle_hasher_(*obstacle);
25+
// only push to the output if this packet has not been seen in the window
26+
if (sent_set_.count(hash_val) == 0)
27+
{
28+
TbotsProto::Obstacle proto = obstacle->createObstacleProto();
29+
sent_set_.insert(hash_val);
30+
obstacle_list_out.add_obstacles()->CopyFrom(proto);
31+
current_hashes.push_back(hash_val);
32+
}
33+
}
34+
sent_queue_.push_back(std::move(current_hashes));
35+
}

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

Lines changed: 60 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,60 @@
1+
#pragma once
2+
3+
#include <deque>
4+
#include <unordered_set>
5+
6+
#include "software/ai/navigator/obstacle/obstacle.hpp"
7+
8+
/**
9+
* The VisProtoDeduper maintains a rolling history of obstacles that have already been
10+
* transmitted. By using a combination of a sliding window (deque) and a fast lookup (hash
11+
* set), it ensures that only "new" or "expired" information is added to the outgoing
12+
* protobuf message.
13+
*
14+
* For example:
15+
* TIME STEP [t] INTERNAL STATE
16+
* ------------------------------------------- ------------------------------
17+
* Incoming Obstacles List: [ A, B, C ] sent_set: { A, B, C }
18+
* Action: All are NEW. sent_queue: [ {A,B,C} ]
19+
* Output Proto: { A, B, C }
20+
*
21+
* TIME STEP [t+1]
22+
* ------------------------------------------- sent_set: { A, B, C, D }
23+
* Incoming ObstaclesList: [ A, D ] sent_queue: [ {A,B,C}, {D} ]
24+
* Action: A is DUPE, D is NEW.
25+
* Output Proto: { D }
26+
*
27+
* TIME STEP [t+2] (Window Size = 2)
28+
* ------------------------------------------- sent_set: { D, E }
29+
* Incoming ObstaclesList: [ A, E ] sent_queue: [ {D}, {E} ]
30+
* Action: A was EVICTED from window, ( {A,B,C} was popped )
31+
* so A is NEW again. E is NEW.
32+
* Output Proto: { A, E }
33+
*/
34+
class VisProtoDeduper
35+
{
36+
public:
37+
/**
38+
* Creates a sliding window deduplicater
39+
*
40+
* @param window_size size of the sliding window
41+
*/
42+
VisProtoDeduper(unsigned int window_size);
43+
44+
/**
45+
* Given an input obstacle list
46+
*
47+
* @param obstacle_list input list of obstacle
48+
* @param obstacle_list_out output list of obstacle after filtered
49+
*/
50+
void dedupeAndFill(const std::vector<ObstaclePtr>& obstacle_list,
51+
TbotsProto::ObstacleList& obstacle_list_out);
52+
53+
54+
private:
55+
unsigned int window_size_;
56+
std::unordered_set<std::size_t> sent_set_;
57+
std::deque<std::vector<std::size_t>> sent_queue_;
58+
59+
std::hash<Obstacle> obstacle_hasher_;
60+
};

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

Lines changed: 150 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,150 @@
1+
2+
#include "software/ai/hl/stp/tactic/vis_proto_deduper.h"
3+
4+
#include <gtest/gtest.h>
5+
6+
#include <memory>
7+
#include <vector>
8+
9+
#include "software/ai/navigator/obstacle/obstacle.hpp"
10+
#include "software/ai/navigator/obstacle/robot_navigation_obstacle_factory.h"
11+
#include "software/geom/point.h"
12+
#include "software/geom/polygon.h"
13+
14+
15+
16+
class VisProtoDeduperTest : public ::testing::Test
17+
{
18+
protected:
19+
RobotNavigationObstacleFactory obstacle_factory =
20+
RobotNavigationObstacleFactory(TbotsProto::RobotNavigationObstacleConfig());
21+
22+
// Helper to extract the list of obstacles from the proto message for easy
23+
// verification
24+
std::vector<TbotsProto::Obstacle> getObstaclesFromProto(
25+
const TbotsProto::ObstacleList& msg)
26+
{
27+
std::vector<TbotsProto::Obstacle> obstacles;
28+
for (const auto& obs : msg.obstacles())
29+
{
30+
obstacles.push_back(obs);
31+
}
32+
return obstacles;
33+
}
34+
35+
// Helper to create a unique obstacle based on a position offset
36+
// This ensures we have distinct geometries to hash.
37+
ObstaclePtr createTestObstacle(double x, double y)
38+
{
39+
auto polygon = Polygon({Point(x, y), Point(x + 1.0, y), Point(x, y + 1.0)});
40+
return obstacle_factory.createFromShape(polygon);
41+
}
42+
};
43+
44+
TEST_F(VisProtoDeduperTest, DeduplicatesRepeatedObstacles)
45+
{
46+
VisProtoDeduper deduper(5);
47+
TbotsProto::ObstacleList output_msg;
48+
49+
auto obs1 = createTestObstacle(10, 10);
50+
std::vector<ObstaclePtr> input = {obs1};
51+
52+
// First pass: Obstacle is new
53+
deduper.dedupeAndFill(input, output_msg);
54+
EXPECT_EQ(output_msg.obstacles_size(), 1);
55+
56+
// Clear output for next step
57+
output_msg.Clear();
58+
59+
// Second pass: Same obstacle passed immediately again
60+
deduper.dedupeAndFill(input, output_msg);
61+
EXPECT_EQ(output_msg.obstacles_size(), 0)
62+
<< "Should filter out recently sent obstacle";
63+
}
64+
65+
TEST_F(VisProtoDeduperTest, HandlesMixedNewAndOldObstacles)
66+
{
67+
VisProtoDeduper deduper(5);
68+
TbotsProto::ObstacleList output_msg;
69+
70+
auto obs_old = createTestObstacle(10, 10);
71+
auto obs_new = createTestObstacle(20, 20);
72+
73+
// Step 1: Send first obstacle
74+
deduper.dedupeAndFill({obs_old}, output_msg);
75+
EXPECT_EQ(output_msg.obstacles_size(), 1);
76+
output_msg.Clear();
77+
78+
// Step 2: Send both. 'obs_old' should be deduped, 'obs_new' should pass.
79+
deduper.dedupeAndFill({obs_old, obs_new}, output_msg);
80+
81+
ASSERT_EQ(output_msg.obstacles_size(), 1);
82+
}
83+
84+
TEST_F(VisProtoDeduperTest, WindowEvictionLogic)
85+
{
86+
// Window size of 2
87+
// Frame 0: Send A (Stored in queue index 0)
88+
// Frame 1: Send empty (Stored in queue index 1)
89+
// Frame 2: Send empty (Stored in queue index 2) -> Window exceeded?
90+
// Logic check: if queue.size() > window.
91+
// After Frame 0: size 1.
92+
// After Frame 1: size 2.
93+
// After Frame 2: size 3. (3 > 2, so Frame 0 is evicted).
94+
95+
VisProtoDeduper deduper(2);
96+
TbotsProto::ObstacleList output_msg;
97+
auto obs = createTestObstacle(5, 5);
98+
99+
deduper.dedupeAndFill({obs}, output_msg);
100+
EXPECT_EQ(output_msg.obstacles_size(), 1);
101+
output_msg.Clear();
102+
103+
deduper.dedupeAndFill({}, output_msg);
104+
EXPECT_EQ(output_msg.obstacles_size(), 0);
105+
106+
deduper.dedupeAndFill({}, output_msg);
107+
EXPECT_EQ(output_msg.obstacles_size(), 0);
108+
109+
deduper.dedupeAndFill({obs}, output_msg);
110+
EXPECT_EQ(output_msg.obstacles_size(), 1)
111+
<< "Obstacle should be resent after window expiration";
112+
}
113+
114+
TEST_F(VisProtoDeduperTest, ZeroWindowAlwaysSends)
115+
{
116+
// If window size is 0, it should behave like a pass-through (or evict immediately)
117+
VisProtoDeduper deduper(0);
118+
TbotsProto::ObstacleList output_msg;
119+
auto obs = createTestObstacle(1, 1);
120+
121+
// Pass 1
122+
deduper.dedupeAndFill({obs}, output_msg);
123+
EXPECT_EQ(output_msg.obstacles_size(), 1);
124+
output_msg.Clear();
125+
126+
// Pass 2 - Should send again because window size is 0 (immediate eviction)
127+
deduper.dedupeAndFill({obs}, output_msg);
128+
EXPECT_EQ(output_msg.obstacles_size(), 1);
129+
}
130+
131+
TEST_F(VisProtoDeduperTest, MultipleDistinctObstaclesInOneBatch)
132+
{
133+
VisProtoDeduper deduper(5);
134+
TbotsProto::ObstacleList output_msg;
135+
136+
auto obs1 = createTestObstacle(1, 1);
137+
auto obs2 = createTestObstacle(2, 2);
138+
auto obs3 = createTestObstacle(3, 3);
139+
140+
// Send 3 unique obstacles at once
141+
deduper.dedupeAndFill({obs1, obs2, obs3}, output_msg);
142+
EXPECT_EQ(output_msg.obstacles_size(), 3);
143+
output_msg.Clear();
144+
145+
// Send 2 old, 1 new
146+
auto obs4 = createTestObstacle(4, 4);
147+
deduper.dedupeAndFill({obs1, obs3, obs4}, output_msg);
148+
149+
ASSERT_EQ(output_msg.obstacles_size(), 1);
150+
}

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

Lines changed: 9 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,7 @@
11
#pragma once
22

3+
#include <functional>
4+
35
#include "software/ai/navigator/obstacle/obstacle.hpp"
46
#include "software/geom/algorithms/closest_point.h"
57
#include "software/geom/algorithms/contains.h"
@@ -30,6 +32,7 @@ class GeomObstacle : public Obstacle
3032
std::string toString(void) const override;
3133
void accept(ObstacleVisitor& visitor) const override;
3234
std::vector<Point> rasterize(const double resolution_size) const override;
35+
std::size_t hash() const override;
3336

3437
/**
3538
* Gets the underlying GEOM_TYPE
@@ -116,3 +119,9 @@ void GeomObstacle<GEOM_TYPE>::accept(ObstacleVisitor& visitor) const
116119
{
117120
visitor.visit(*this);
118121
}
122+
123+
template <typename GEOM_TYPE>
124+
std::size_t GeomObstacle<GEOM_TYPE>::hash() const
125+
{
126+
return std::hash<GEOM_TYPE>{}(geom_);
127+
}

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

Lines changed: 16 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -112,6 +112,13 @@ class Obstacle
112112
* @param visitor An Obstacle Visitor
113113
*/
114114
virtual void accept(ObstacleVisitor& visitor) const = 0;
115+
116+
/**
117+
* Computes the hash of the current obstacle object
118+
*
119+
* @return hash value
120+
*/
121+
virtual std::size_t hash() const = 0;
115122
};
116123

117124
/**
@@ -144,3 +151,12 @@ inline std::ostream& operator<<(std::ostream& os, const ObstaclePtr& obstacle_pt
144151
os << obstacle_ptr->toString();
145152
return os;
146153
}
154+
155+
template <>
156+
struct std::hash<Obstacle>
157+
{
158+
std::size_t operator()(const Obstacle& obstacle) const
159+
{
160+
return obstacle.hash();
161+
}
162+
};

‎src/software/geom/BUILD‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -64,6 +64,7 @@ cc_library(
6464
deps = [
6565
":segment",
6666
":shape",
67+
"//software/util/hash:hash_combine",
6768
],
6869
)
6970

0 commit comments

Comments
 (0)