Revision 468
made it so it removes robots it does not have new positions for
| trunk/code/projects/colonet/ColonetServer/PositionMonitor.cpp | ||
|---|---|---|
| 48 | 48 |
} |
| 49 | 49 |
printf("\n");
|
| 50 | 50 |
|
| 51 |
map<int, VisionPosition> newPositionMap; |
|
| 51 | 52 |
|
| 52 |
/* |
|
| 53 |
//TODO: compare current set of positions to previous set of |
|
| 54 |
// positions to match up who is who |
|
| 55 |
positionMap.clear(); |
|
| 56 |
|
|
| 57 |
int i; |
|
| 58 |
for (i = 0; i < numPositions; i++) {
|
|
| 59 |
//TODO: would this insert by copy or just point to the memory in positions[i]? |
|
| 60 |
// If this just points to the memory in positions[i] then we have a bug |
|
| 61 |
positionMap[i] = positions[i]; |
|
| 62 |
} |
|
| 63 |
*/ |
|
| 64 |
|
|
| 65 | 53 |
//TODO: also remove robots that might have disappeared |
| 66 | 54 |
int i; |
| 67 | 55 |
for (i = 0; i < numPositions; i++) {
|
| ... | ... | |
| 72 | 60 |
|
| 73 | 61 |
if (isProbablySameRobot(newPos, oldPos)) {
|
| 74 | 62 |
//TODO: is this the right use of an iterator? |
| 75 |
iter->second = newPos;
|
|
| 63 |
newPositionMap.insert(make_pair(iter->first, newPos));
|
|
| 76 | 64 |
break; |
| 77 | 65 |
} |
| 78 | 66 |
} |
| 79 | 67 |
|
| 80 | 68 |
if (iter == positionMap.end()) {
|
| 69 |
//a position was found that probably isn't a known |
|
| 70 |
// robot so add it in case a new robot entered the field |
|
| 81 | 71 |
printf("Inserting new robot: %d (%d,%d)", newIdToAssign, newPos.x, newPos.y);
|
| 82 | 72 |
|
| 83 | 73 |
//a position was found that probably isn't a known robot so add it in case a new robot entered the field |
| 84 |
positionMap.insert(make_pair(newIdToAssign, newPos));
|
|
| 74 |
newPositionMap.insert(make_pair(newIdToAssign, newPos));
|
|
| 85 | 75 |
newIdToAssign--; |
| 86 | 76 |
} |
| 87 | 77 |
} |
| 88 | 78 |
|
| 79 |
positionMap = newPositionMap; |
|
| 80 |
|
|
| 89 | 81 |
//TODO: remove this debug information |
| 90 | 82 |
map<int, VisionPosition>::iterator iter; |
| 91 | 83 |
for (iter = positionMap.begin(); iter != positionMap.end(); iter++) {
|
Also available in: Unified diff