Revision 627
checking for error from vision function in server
PositionMonitor.cpp | ||
---|---|---|
6 | 6 |
* @date 2/4/08 |
7 | 7 |
*/ |
8 | 8 |
|
9 |
#include <assert.h> |
|
9 | 10 |
#include <map> |
10 | 11 |
#include <PositionMonitor.h> |
11 | 12 |
#include <stdio.h> |
... | ... | |
33 | 34 |
|
34 | 35 |
void PositionMonitor::run() { |
35 | 36 |
while (1) { |
36 |
updatePositions(); |
|
37 |
if (updatePositions() == -1) { |
|
38 |
fprintf(stderr, "updatePositions failed.\n"); |
|
39 |
} |
|
40 |
|
|
37 | 41 |
usleep(CAM_UPDATE_PERIOD); |
38 | 42 |
} |
39 | 43 |
} |
40 | 44 |
|
41 | 45 |
int PositionMonitor::updatePositions() { |
42 |
VisionPosition * positions = NULL; |
|
46 |
VisionPosition* positions = NULL; |
|
47 |
int numPositions = vision_get_robot_positions(&positions); |
|
43 | 48 |
|
44 |
//TODO: check for error returned |
|
45 |
int numPositions = vision_get_robot_positions(&positions); |
|
46 | 49 |
/* |
47 |
printf("numPositions is %d\n", numPositions);
|
|
50 |
printf("numPositions is %d\n", numPositions); |
|
48 | 51 |
for (int i = 0; i < numPositions; i++) { |
49 | 52 |
printf("{%d,%d} ", positions[i].x, positions[i].y); |
50 | 53 |
} |
51 | 54 |
printf("\n"); |
52 | 55 |
*/ |
53 | 56 |
|
54 |
map<int, VisionPosition> newPositionMap; |
|
57 |
if (numPositions == -1) { |
|
58 |
fprintf(stderr, "%s: vision_get_robot_positions failed.\n", __FUNCTION__); |
|
59 |
return -1; |
|
60 |
} else { |
|
61 |
map<int, VisionPosition> newPositionMap; |
|
55 | 62 |
|
56 |
pthread_mutex_lock(&position_map_lock); |
|
63 |
pthread_mutex_lock(&position_map_lock);
|
|
57 | 64 |
|
58 |
//TODO: also remove robots that might have disappeared |
|
59 |
int i; |
|
60 |
for (i = 0; i < numPositions; i++) { |
|
61 |
VisionPosition newPos = positions[i]; |
|
62 |
map<int, VisionPosition>::iterator iter; |
|
63 |
for (iter = positionMap.begin(); iter != positionMap.end(); iter++) { |
|
64 |
VisionPosition oldPos = iter->second; |
|
65 |
//TODO: also remove robots that might have disappeared |
|
66 |
for (int i = 0; i < numPositions; i++) { |
|
67 |
VisionPosition newPos = positions[i]; |
|
68 |
map<int, VisionPosition>::iterator iter; |
|
69 |
for (iter = positionMap.begin(); iter != positionMap.end(); iter++) { |
|
70 |
VisionPosition oldPos = iter->second; |
|
65 | 71 |
|
66 |
if (isProbablySameRobot(newPos, oldPos)) { |
|
67 |
//TODO: is this the right use of an iterator? |
|
68 |
newPositionMap.insert(make_pair(iter->first, newPos)); |
|
69 |
deleteBufferMap.erase(iter->first); |
|
70 |
deleteBufferMap.insert(make_pair(iter->first, ROBOT_DELETE_BUFFER)); |
|
71 |
break; |
|
72 |
if (isProbablySameRobot(newPos, oldPos)) { |
|
73 |
//TODO: is this the right use of an iterator? |
|
74 |
newPositionMap.insert(make_pair(iter->first, newPos)); |
|
75 |
deleteBufferMap.erase(iter->first); |
|
76 |
deleteBufferMap.insert(make_pair(iter->first, ROBOT_DELETE_BUFFER)); |
|
77 |
break; |
|
78 |
} |
|
72 | 79 |
} |
73 |
} |
|
74 | 80 |
|
75 |
if (iter == positionMap.end()) { |
|
76 |
//a position was found that probably isn't a known |
|
77 |
// robot so add it in case a new robot entered the field |
|
78 |
printf("Inserting new robot: %d (%d,%d)\n", newIdToAssign, newPos.x, newPos.y); |
|
81 |
if (iter == positionMap.end()) {
|
|
82 |
//a position was found that probably isn't a known
|
|
83 |
// robot so add it in case a new robot entered the field
|
|
84 |
printf("Inserting new robot: %d (%d,%d)\n", newIdToAssign, newPos.x, newPos.y);
|
|
79 | 85 |
|
80 |
//a position was found that probably isn't a known robot so add it in case a new robot entered the field |
|
81 |
newPositionMap.insert(make_pair(newIdToAssign, newPos)); |
|
82 |
deleteBufferMap.insert(make_pair(newIdToAssign, ROBOT_DELETE_BUFFER)); |
|
83 |
newIdToAssign--; |
|
86 |
//a position was found that probably isn't a known robot so add it in case a new robot entered the field |
|
87 |
newPositionMap.insert(make_pair(newIdToAssign, newPos)); |
|
88 |
deleteBufferMap.insert(make_pair(newIdToAssign, ROBOT_DELETE_BUFFER)); |
|
89 |
newIdToAssign--; |
|
90 |
} |
|
84 | 91 |
} |
85 |
} |
|
86 | 92 |
|
87 |
map<int, VisionPosition>::iterator iter2; |
|
88 |
for (iter2 = positionMap.begin(); iter2 != positionMap.end(); iter2++) { |
|
89 |
int currId = iter2->first; |
|
90 |
map<int, VisionPosition>::iterator checkContains = newPositionMap.find(currId); |
|
91 |
if (checkContains == newPositionMap.end()) { |
|
92 |
int bufferValue = deleteBufferMap[iter2->first]; |
|
93 |
bufferValue--; |
|
94 |
if (bufferValue > 0) { |
|
95 |
newPositionMap.insert(make_pair(currId, iter2->second)); |
|
96 |
deleteBufferMap.erase(currId); |
|
97 |
deleteBufferMap.insert(make_pair(currId, bufferValue)); |
|
98 |
} else { |
|
99 |
deleteBufferMap.erase(currId); |
|
93 |
map<int, VisionPosition>::iterator iter2; |
|
94 |
for (iter2 = positionMap.begin(); iter2 != positionMap.end(); iter2++) { |
|
95 |
int currId = iter2->first; |
|
96 |
map<int, VisionPosition>::iterator checkContains = newPositionMap.find(currId); |
|
97 |
if (checkContains == newPositionMap.end()) { |
|
98 |
int bufferValue = deleteBufferMap[iter2->first]; |
|
99 |
bufferValue--; |
|
100 |
if (bufferValue > 0) { |
|
101 |
newPositionMap.insert(make_pair(currId, iter2->second)); |
|
102 |
deleteBufferMap.erase(currId); |
|
103 |
deleteBufferMap.insert(make_pair(currId, bufferValue)); |
|
104 |
} else { |
|
105 |
deleteBufferMap.erase(currId); |
|
106 |
} |
|
100 | 107 |
} |
101 | 108 |
} |
102 |
} |
|
103 | 109 |
|
104 |
positionMap = newPositionMap; |
|
110 |
positionMap = newPositionMap;
|
|
105 | 111 |
|
106 |
// printf("\npositionMap size is %d and deleteBufferMap size is %d\n", positionMap.size(), deleteBufferMap.size()); |
|
112 |
// printf("\npositionMap size is %d and deleteBufferMap size is %d\n", positionMap.size(), deleteBufferMap.size());
|
|
107 | 113 |
|
108 |
/* |
|
109 |
//TODO: remove this debug information |
|
110 |
map<int, VisionPosition>::iterator iter; |
|
111 |
for (iter = positionMap.begin(); iter != positionMap.end(); iter++) { |
|
114 |
/*
|
|
115 |
//TODO: remove this debug information
|
|
116 |
map<int, VisionPosition>::iterator iter;
|
|
117 |
for (iter = positionMap.begin(); iter != positionMap.end(); iter++) {
|
|
112 | 118 |
printf("%d has position (%d, %d) with delete buffer %d\n", iter->first, iter->second.x, iter->second.y, deleteBufferMap[iter->first]); |
113 |
} |
|
114 |
*/ |
|
119 |
}
|
|
120 |
*/
|
|
115 | 121 |
|
116 | 122 |
|
117 |
pthread_mutex_unlock(&position_map_lock); |
|
123 |
pthread_mutex_unlock(&position_map_lock);
|
|
118 | 124 |
|
119 |
if (positions) {
|
|
125 |
assert(positions != NULL);
|
|
120 | 126 |
free(positions); |
127 |
|
|
128 |
return 0; |
|
121 | 129 |
} |
122 |
|
|
123 |
return 0; |
|
124 | 130 |
} |
125 | 131 |
|
126 | 132 |
int PositionMonitor::assignRealId(int old_id, int real_id) { |
Also available in: Unified diff