Revision 627
checking for error from vision function in server
| trunk/code/projects/colonet/server/vision/vision_driver.c | ||
|---|---|---|
| 21 | 21 |
VisionPosition* positions; |
| 22 | 22 |
int num_positions = vision_get_robot_positions(&positions); |
| 23 | 23 |
|
| 24 |
if (num_positions == -1) {
|
|
| 25 |
fprintf(stderr, "vision_get_robot_positions failed.\n"); |
|
| 26 |
return 1; |
|
| 27 |
} |
|
| 28 |
|
|
| 24 | 29 |
printf("Got %d positions.\n", num_positions);
|
| 25 | 30 |
|
| 26 | 31 |
int i; |
| trunk/code/projects/colonet/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