Revision 655
added some more documentation
| PositionMonitor.cpp | ||
|---|---|---|
| 20 | 20 |
#define ROBOT_DELETE_BUFFER 10 |
| 21 | 21 |
#define CAM_UPDATE_PERIOD 100000 |
| 22 | 22 |
|
| 23 |
/** |
|
| 24 |
* @brief Constructor for the PositionMonitor class |
|
| 25 |
*/ |
|
| 23 | 26 |
PositionMonitor::PositionMonitor() {
|
| 24 | 27 |
//TODO: don't hardcode this file name |
| 25 | 28 |
//TODO: check for error returned from init |
| ... | ... | |
| 29 | 32 |
pthread_mutex_init(&position_map_lock, NULL); |
| 30 | 33 |
} |
| 31 | 34 |
|
| 35 |
/** |
|
| 36 |
* @brief Destructor for the PositionMonitor class |
|
| 37 |
*/ |
|
| 32 | 38 |
PositionMonitor::~PositionMonitor() {
|
| 33 | 39 |
} |
| 34 | 40 |
|
| 41 |
/** |
|
| 42 |
* @brief This function updates the positions of things in the image |
|
| 43 |
* |
|
| 44 |
* @return Void |
|
| 45 |
*/ |
|
| 35 | 46 |
void PositionMonitor::run() {
|
| 36 | 47 |
while (1) {
|
| 37 | 48 |
if (updatePositions() == -1) {
|
| ... | ... | |
| 42 | 53 |
} |
| 43 | 54 |
} |
| 44 | 55 |
|
| 56 |
/** |
|
| 57 |
* @brief This function asks the vision code for the position of things it identifies |
|
| 58 |
*/ |
|
| 45 | 59 |
int PositionMonitor::updatePositions() {
|
| 46 | 60 |
VisionPosition* positions = NULL; |
| 47 | 61 |
int numPositions = vision_get_robot_positions(&positions); |
| 48 | 62 |
|
| 63 |
//debug output code |
|
| 49 | 64 |
/* |
| 50 | 65 |
printf("numPositions is %d\n", numPositions);
|
| 51 | 66 |
for (int i = 0; i < numPositions; i++) {
|
| ... | ... | |
| 62 | 77 |
|
| 63 | 78 |
pthread_mutex_lock(&position_map_lock); |
| 64 | 79 |
|
| 65 |
//TODO: also remove robots that might have disappeared
|
|
| 80 |
//go through each of the positions the vision system identified
|
|
| 66 | 81 |
for (int i = 0; i < numPositions; i++) {
|
| 67 | 82 |
VisionPosition newPos = positions[i]; |
| 68 | 83 |
map<int, VisionPosition>::iterator iter; |
| 84 |
//go through all the positions we currently have in the position map |
|
| 69 | 85 |
for (iter = positionMap.begin(); iter != positionMap.end(); iter++) {
|
| 70 | 86 |
VisionPosition oldPos = iter->second; |
| 71 | 87 |
|
| 88 |
//figure out if the new point is probably the same robot as one of the old positions |
|
| 72 | 89 |
if (isProbablySameRobot(newPos, oldPos)) {
|
| 73 |
//TODO: is this the right use of an iterator?
|
|
| 90 |
//if the new positions is the same robot as an old one, insert this new position into the new map
|
|
| 74 | 91 |
newPositionMap.insert(make_pair(iter->first, newPos)); |
| 92 |
|
|
| 93 |
//erase this robot from the map that is used to smooth over several frames in case of problems identifying robots |
|
| 75 | 94 |
deleteBufferMap.erase(iter->first); |
| 95 |
//insert this robot back into the map in order to refresh its delete buffer |
|
| 76 | 96 |
deleteBufferMap.insert(make_pair(iter->first, ROBOT_DELETE_BUFFER)); |
| 77 | 97 |
break; |
| 78 | 98 |
} |
| ... | ... | |
| 90 | 110 |
} |
| 91 | 111 |
} |
| 92 | 112 |
|
| 113 |
|
|
| 93 | 114 |
map<int, VisionPosition>::iterator iter2; |
| 94 | 115 |
for (iter2 = positionMap.begin(); iter2 != positionMap.end(); iter2++) {
|
| 95 | 116 |
int currId = iter2->first; |
Also available in: Unified diff