Revision 458
added some really simple support for keep track of which position is which robot over time
trunk/code/projects/colonet/ColonetServer/includes/PositionMonitor.h | ||
---|---|---|
14 | 14 |
using namespace std; |
15 | 15 |
|
16 | 16 |
#define MAX_POSITIONS 20 |
17 |
#define MAX_DISTANCE 5 |
|
17 | 18 |
|
18 | 19 |
using namespace std; |
19 | 20 |
|
... | ... | |
32 | 33 |
|
33 | 34 |
private: |
34 | 35 |
map<int, VisionPosition> positionMap; |
36 |
bool isProbablySameRobot(VisionPosition p1, VisionPosition p2); |
|
37 |
int newIdToAssign; |
|
35 | 38 |
}; |
36 | 39 |
|
37 | 40 |
#endif |
trunk/code/projects/colonet/ColonetServer/PositionMonitor.cpp | ||
---|---|---|
21 | 21 |
//TODO: don't hardcode this file name |
22 | 22 |
//TODO: check for error returned from init |
23 | 23 |
vision_init("/var/www/colonet.jpg"); |
24 |
newIdToAssign = 0; |
|
24 | 25 |
} |
25 | 26 |
|
26 | 27 |
PositionMonitor::~PositionMonitor() { |
... | ... | |
42 | 43 |
//TODO: check for error returned |
43 | 44 |
int numPositions = vision_get_robot_positions(&positions); |
44 | 45 |
|
46 |
/* |
|
45 | 47 |
//TODO: compare current set of positions to previous set of |
46 | 48 |
// positions to match up who is who |
47 | 49 |
positionMap.clear(); |
48 | 50 |
|
49 | 51 |
int i; |
50 | 52 |
for (i = 0; i < numPositions; i++) { |
53 |
//TODO: would this insert by copy or just point to the memory in positions[i]? |
|
54 |
// If this just points to the memory in positions[i] then we have a bug |
|
51 | 55 |
positionMap[i] = positions[i]; |
52 | 56 |
} |
57 |
*/ |
|
58 |
|
|
59 |
//TODO: also remove robots that might have disappeared |
|
60 |
int i; |
|
61 |
for (i = 0; i < numPositions; i++) { |
|
62 |
VisionPosition newPos = positions[i]; |
|
63 |
map<int, VisionPosition>::iterator iter; |
|
64 |
for (iter = positionMap.begin(); iter != positionMap.end(); iter++) { |
|
65 |
VisionPosition oldPos = iter->second; |
|
53 | 66 |
|
67 |
if (isProbablySameRobot(newPos, oldPos)) { |
|
68 |
//TODO: is this the right use of an iterator? |
|
69 |
iter->second = newPos; |
|
70 |
break; |
|
71 |
} |
|
72 |
} |
|
73 |
|
|
74 |
if (iter == positionMap.end()) { |
|
75 |
//a position was found that probably isn't a known robot so add it in case a new robot entered the field |
|
76 |
positionMap.insert(make_pair(newIdToAssign, newPos)); |
|
77 |
newIdToAssign++; |
|
78 |
} |
|
79 |
} |
|
80 |
|
|
54 | 81 |
//TODO: remove this debug information |
55 | 82 |
map<int, VisionPosition>::iterator iter; |
56 | 83 |
for (iter = positionMap.begin(); iter != positionMap.end(); iter++) { |
... | ... | |
59 | 86 |
|
60 | 87 |
if (positions) |
61 | 88 |
free(positions); |
89 |
|
|
62 | 90 |
return 0; |
63 | 91 |
} |
64 | 92 |
|
... | ... | |
95 | 123 |
|
96 | 124 |
return 0; |
97 | 125 |
} |
126 |
|
|
127 |
bool PositionMonitor::isProbablySameRobot(VisionPosition p1, VisionPosition p2) { |
|
128 |
int xDiff = p1.x - p2.x; |
|
129 |
int yDiff = p1.y - p2.y; |
|
130 |
return (xDiff*xDiff + yDiff*yDiff < MAX_DISTANCE*MAX_DISTANCE); |
|
131 |
} |
Also available in: Unified diff