Revision 710
Tweaked vision and various colonet fixes
PositionMonitor.cpp | ||
---|---|---|
16 | 16 |
using namespace std; |
17 | 17 |
|
18 | 18 |
#define MAX_POSITIONS 20 |
19 |
#define SAME_ROBOT_DISTANCE_THRESHOLD 150
|
|
19 |
#define SAME_ROBOT_DISTANCE_THRESHOLD 15 |
|
20 | 20 |
#define ROBOT_DELETE_BUFFER 10 |
21 | 21 |
#define CAM_UPDATE_PERIOD 100000 |
22 | 22 |
|
... | ... | |
28 | 28 |
//TODO: check for error returned from init |
29 | 29 |
vision_init("/var/www/colonet.jpg"); |
30 | 30 |
newIdToAssign = -1; |
31 |
|
|
31 |
|
|
32 | 32 |
pthread_mutex_init(&position_map_lock, NULL); |
33 | 33 |
} |
34 | 34 |
|
... | ... | |
61 | 61 |
int numPositions = vision_get_robot_positions(&positions); |
62 | 62 |
|
63 | 63 |
//debug output code |
64 |
/*
|
|
65 |
printf("numPositions is %d\n", numPositions); |
|
64 |
/* |
|
65 |
printf("\nnumPositions is %d\n", numPositions);
|
|
66 | 66 |
for (int i = 0; i < numPositions; i++) { |
67 | 67 |
printf("{%d,%d} ", positions[i].x, positions[i].y); |
68 | 68 |
} |
... | ... | |
81 | 81 |
for (int i = 0; i < numPositions; i++) { |
82 | 82 |
VisionPosition newPos = positions[i]; |
83 | 83 |
map<int, VisionPosition>::iterator iter; |
84 |
|
|
84 | 85 |
//go through all the positions we currently have in the position map |
85 | 86 |
for (iter = positionMap.begin(); iter != positionMap.end(); iter++) { |
86 | 87 |
VisionPosition oldPos = iter->second; |
87 | 88 |
|
88 | 89 |
//figure out if the new point is probably the same robot as one of the old positions |
89 | 90 |
if (isProbablySameRobot(newPos, oldPos)) { |
91 |
//DEBUG |
|
92 |
//printf("probably same robot: new = (%d, %d), old = (%d, %d)\n", newPos.x, newPos.y, oldPos.x, oldPos.y); |
|
93 |
|
|
90 | 94 |
//if the new positions is the same robot as an old one, insert this new position into the new map |
91 | 95 |
newPositionMap.insert(make_pair(iter->first, newPos)); |
92 | 96 |
|
... | ... | |
99 | 103 |
} |
100 | 104 |
|
101 | 105 |
if (iter == positionMap.end()) { |
102 |
//a position was found that probably isn't a known
|
|
106 |
//a position was found that probably isn't a known |
|
103 | 107 |
// robot so add it in case a new robot entered the field |
104 | 108 |
printf("Inserting new robot: %d (%d,%d)\n", newIdToAssign, newPos.x, newPos.y); |
105 |
|
|
109 |
|
|
106 | 110 |
//a position was found that probably isn't a known robot so add it in case a new robot entered the field |
107 | 111 |
newPositionMap.insert(make_pair(newIdToAssign, newPos)); |
108 | 112 |
deleteBufferMap.insert(make_pair(newIdToAssign, ROBOT_DELETE_BUFFER)); |
... | ... | |
110 | 114 |
} |
111 | 115 |
} |
112 | 116 |
|
113 |
|
|
117 |
|
|
114 | 118 |
map<int, VisionPosition>::iterator iter2; |
115 | 119 |
for (iter2 = positionMap.begin(); iter2 != positionMap.end(); iter2++) { |
116 | 120 |
int currId = iter2->first; |
... | ... | |
130 | 134 |
|
131 | 135 |
positionMap = newPositionMap; |
132 | 136 |
|
133 |
// printf("\npositionMap size is %d and deleteBufferMap size is %d\n", positionMap.size(), deleteBufferMap.size()); |
|
134 |
|
|
135 | 137 |
/* |
136 |
//TODO: remove this debug information |
|
138 |
//DEBUG PRINT STATEMENTS |
|
139 |
printf("positionMap size is %d and deleteBufferMap size is %d\n", positionMap.size(), deleteBufferMap.size()); |
|
137 | 140 |
map<int, VisionPosition>::iterator iter; |
138 | 141 |
for (iter = positionMap.begin(); iter != positionMap.end(); iter++) { |
139 |
printf("%d has position (%d, %d) with delete buffer %d\n", iter->first, iter->second.x, iter->second.y, deleteBufferMap[iter->first]); |
|
142 |
printf("%d has position (%d, %d) with delete buffer %d\n", iter->first, iter->second.x, iter->second.y, deleteBufferMap[iter->first]);
|
|
140 | 143 |
} |
141 | 144 |
*/ |
142 | 145 |
|
... | ... | |
193 | 196 |
pthread_mutex_lock(&position_map_lock); |
194 | 197 |
|
195 | 198 |
if (positionMap.find(robot_id) == positionMap.end()){ |
196 |
pthread_mutex_unlock(&position_map_lock);
|
|
199 |
pthread_mutex_unlock(&position_map_lock); |
|
197 | 200 |
return -1; |
198 | 201 |
} else { |
199 | 202 |
VisionPosition pos = positionMap[robot_id]; |
Also available in: Unified diff