Revision 449
added some more stuff to the position monitor
| trunk/code/projects/colonet/ColonetServer/includes/PositionMonitor.h | ||
|---|---|---|
| 11 | 11 |
|
| 12 | 12 |
#include <map> |
| 13 | 13 |
#include <vision.h> |
| 14 |
#include <map> |
|
| 15 |
using namespace std; |
|
| 14 | 16 |
|
| 15 | 17 |
#define MAX_POSITIONS 20 |
| 16 | 18 |
|
| ... | ... | |
| 29 | 31 |
int getRobotPosition(int robot_id, int* xbuf, int* ybuf); |
| 30 | 32 |
|
| 31 | 33 |
private: |
| 32 |
VisionPosition * positions;
|
|
| 34 |
map<int, VisionPosition> positionMap;
|
|
| 33 | 35 |
}; |
| 34 | 36 |
|
| 35 | 37 |
#endif |
| trunk/code/projects/colonet/ColonetServer/PositionMonitor.cpp | ||
|---|---|---|
| 12 | 12 |
#include <stdlib.h> |
| 13 | 13 |
#include <vision.h> |
| 14 | 14 |
|
| 15 |
#include <stdio.h> |
|
| 16 |
|
|
| 17 |
#include <map> |
|
| 18 |
using namespace std; |
|
| 19 |
|
|
| 20 |
|
|
| 21 |
|
|
| 15 | 22 |
PositionMonitor::PositionMonitor() {
|
| 16 | 23 |
//TODO: don't hardcode this file name |
| 17 | 24 |
//TODO: check for error returned from init |
| 18 | 25 |
vision_init("/var/www/colonet.jpg");
|
| 19 |
positions = NULL; |
|
| 20 | 26 |
} |
| 21 | 27 |
|
| 22 | 28 |
PositionMonitor::~PositionMonitor() {
|
| 23 | 29 |
} |
| 24 | 30 |
|
| 25 | 31 |
int PositionMonitor::startMonitoring() {
|
| 32 |
//TODO: fill this in when this becomes asynchronous |
|
| 26 | 33 |
return 0; |
| 27 | 34 |
} |
| 28 | 35 |
|
| 29 | 36 |
int PositionMonitor::stopMonitoring() {
|
| 37 |
//TODO: fill this in when this becomes asynchronous |
|
| 30 | 38 |
return 0; |
| 31 | 39 |
} |
| 32 | 40 |
|
| 33 | 41 |
int PositionMonitor::updatePositions() {
|
| 34 |
if (positions) |
|
| 35 |
free(positions); |
|
| 42 |
VisionPosition * positions = NULL; |
|
| 36 | 43 |
|
| 37 | 44 |
//TODO: check for error returned |
| 38 |
vision_get_robot_positions(&positions); |
|
| 45 |
int numPositions = vision_get_robot_positions(&positions);
|
|
| 39 | 46 |
|
| 47 |
//TODO: compare current set of positions to previous set of |
|
| 48 |
// positions to match up who is who |
|
| 49 |
positionMap.clear(); |
|
| 50 |
|
|
| 51 |
int i; |
|
| 52 |
for (i = 0; i < numPositions; i++) {
|
|
| 53 |
positionMap[i] = positions[i]; |
|
| 54 |
} |
|
| 55 |
|
|
| 56 |
//TODO: remove this debug information |
|
| 57 |
map<int, VisionPosition>::iterator iter; |
|
| 58 |
for (iter = positionMap.begin(); iter != positionMap.end(); iter++) {
|
|
| 59 |
printf("%d has position (%d, %d)\n", iter->first, iter->second.x, iter->second.y);
|
|
| 60 |
} |
|
| 61 |
|
|
| 62 |
if (positions) |
|
| 63 |
free(positions); |
|
| 40 | 64 |
return 0; |
| 41 | 65 |
} |
| 42 | 66 |
|
| ... | ... | |
| 45 | 69 |
} |
| 46 | 70 |
|
| 47 | 71 |
int PositionMonitor::getRobotPosition(int robot_id, int* xbuf, int* ybuf) {
|
| 48 |
// TODO - fill this in. |
|
| 72 |
//TODO: figure out what a map returns if the element doesn't exist |
|
| 73 |
if (positionMap[robot_id] == map<int, VisionPosition>::end){
|
|
| 74 |
return -1; |
|
| 75 |
} |
|
| 76 |
|
|
| 77 |
VisionPosition pos = positionMap[robot_id]; |
|
| 49 | 78 |
|
| 50 |
*xbuf = 1;
|
|
| 51 |
*ybuf = 1;
|
|
| 79 |
*xbuf = pos.x;
|
|
| 80 |
*ybuf = pos.y;
|
|
| 52 | 81 |
|
| 53 | 82 |
return 0; |
| 54 | 83 |
} |
Also available in: Unified diff