Revision 550
failed attempt to fix colonet robot code
PositionMonitor.cpp | ||
---|---|---|
6 | 6 |
* @date 2/4/08 |
7 | 7 |
*/ |
8 | 8 |
|
9 |
//TODO: make this file asynchronous |
|
10 |
|
|
9 |
#include <map> |
|
11 | 10 |
#include <PositionMonitor.h> |
11 |
#include <stdio.h> |
|
12 | 12 |
#include <stdlib.h> |
13 | 13 |
#include <vision.h> |
14 | 14 |
|
15 |
#include <stdio.h> |
|
16 |
|
|
17 |
#include <map> |
|
18 | 15 |
using namespace std; |
19 | 16 |
|
17 |
#define MAX_POSITIONS 20 |
|
18 |
#define SAME_ROBOT_DISTANCE_THRESHOLD 110 |
|
19 |
#define ROBOT_DELETE_BUFFER 10 |
|
20 |
#define CAM_UPDATE_PERIOD 100000 |
|
21 |
|
|
20 | 22 |
PositionMonitor::PositionMonitor() { |
21 | 23 |
//TODO: don't hardcode this file name |
22 | 24 |
//TODO: check for error returned from init |
... | ... | |
32 | 34 |
void PositionMonitor::run() { |
33 | 35 |
while (1) { |
34 | 36 |
updatePositions(); |
35 |
usleep(500000);
|
|
37 |
usleep(CAM_UPDATE_PERIOD);
|
|
36 | 38 |
} |
37 | 39 |
} |
38 | 40 |
|
... | ... | |
62 | 64 |
if (isProbablySameRobot(newPos, oldPos)) { |
63 | 65 |
//TODO: is this the right use of an iterator? |
64 | 66 |
newPositionMap.insert(make_pair(iter->first, newPos)); |
65 |
deleteBufferMap.erase(iter->first);
|
|
66 |
deleteBufferMap.insert(make_pair(iter->first, ROBOT_DELETE_BUFFER));
|
|
67 |
deleteBufferMap.erase(iter->first);
|
|
68 |
deleteBufferMap.insert(make_pair(iter->first, ROBOT_DELETE_BUFFER));
|
|
67 | 69 |
break; |
68 | 70 |
} |
69 | 71 |
} |
... | ... | |
88 | 90 |
int bufferValue = deleteBufferMap[iter2->first]; |
89 | 91 |
bufferValue--; |
90 | 92 |
if (bufferValue > 0) { |
91 |
newPositionMap.insert(make_pair(currId, iter2->second));
|
|
92 |
deleteBufferMap.erase(currId);
|
|
93 |
deleteBufferMap.insert(make_pair(currId, bufferValue));
|
|
93 |
newPositionMap.insert(make_pair(currId, iter2->second));
|
|
94 |
deleteBufferMap.erase(currId);
|
|
95 |
deleteBufferMap.insert(make_pair(currId, bufferValue));
|
|
94 | 96 |
} else { |
95 |
deleteBufferMap.erase(currId);
|
|
97 |
deleteBufferMap.erase(currId);
|
|
96 | 98 |
} |
97 | 99 |
} |
98 | 100 |
} |
Also available in: Unified diff