Revision 466
fixed vision bug
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 |
newIdToAssign = -1;
|
|
25 | 25 |
} |
26 | 26 |
|
27 | 27 |
PositionMonitor::~PositionMonitor() { |
... | ... | |
42 | 42 |
|
43 | 43 |
//TODO: check for error returned |
44 | 44 |
int numPositions = vision_get_robot_positions(&positions); |
45 |
printf("numPositions is %d\n", numPositions); |
|
45 | 46 |
|
46 | 47 |
/* |
47 | 48 |
//TODO: compare current set of positions to previous set of |
... | ... | |
55 | 56 |
positionMap[i] = positions[i]; |
56 | 57 |
} |
57 | 58 |
*/ |
58 |
|
|
59 |
|
|
59 | 60 |
//TODO: also remove robots that might have disappeared |
60 | 61 |
int i; |
61 | 62 |
for (i = 0; i < numPositions; i++) { |
... | ... | |
65 | 66 |
VisionPosition oldPos = iter->second; |
66 | 67 |
|
67 | 68 |
if (isProbablySameRobot(newPos, oldPos)) { |
68 |
//TODO: is this the right use of an iterator?
|
|
69 |
iter->second = newPos;
|
|
70 |
break;
|
|
69 |
//TODO: is this the right use of an iterator?
|
|
70 |
iter->second = newPos;
|
|
71 |
break;
|
|
71 | 72 |
} |
72 | 73 |
} |
73 |
|
|
74 |
|
|
74 | 75 |
if (iter == positionMap.end()) { |
76 |
printf("Inserting new robot: %d (%d,%d)", newIdToAssign, newPos.x, newPos.y); |
|
77 |
|
|
75 | 78 |
//a position was found that probably isn't a known robot so add it in case a new robot entered the field |
76 | 79 |
positionMap.insert(make_pair(newIdToAssign, newPos)); |
77 |
newIdToAssign++;
|
|
80 |
newIdToAssign--;
|
|
78 | 81 |
} |
79 | 82 |
} |
80 | 83 |
|
... | ... | |
107 | 110 |
} |
108 | 111 |
|
109 | 112 |
map<int, VisionPosition> PositionMonitor::getAllRobotPositions() { |
113 |
//TODO do this in another thread! |
|
114 |
updatePositions(); |
|
115 |
|
|
110 | 116 |
return positionMap; |
111 | 117 |
} |
112 | 118 |
|
Also available in: Unified diff