root / trunk / code / projects / colonet / server / PositionMonitor.cpp @ 581
History | View | Annotate | Download (5.15 KB)
1 | 436 | jknichel | /**
|
---|---|---|---|
2 | * @file PositionMonitor.cpp
|
||
3 | *
|
||
4 | * @author Jason Knichel
|
||
5 | *
|
||
6 | * @date 2/4/08
|
||
7 | */
|
||
8 | |||
9 | 550 | emarinel | #include <map> |
10 | 443 | emarinel | #include <PositionMonitor.h> |
11 | 550 | emarinel | #include <stdio.h> |
12 | 436 | jknichel | #include <stdlib.h> |
13 | #include <vision.h> |
||
14 | |||
15 | 449 | jknichel | using namespace std; |
16 | |||
17 | 550 | emarinel | #define MAX_POSITIONS 20 |
18 | #define SAME_ROBOT_DISTANCE_THRESHOLD 110 |
||
19 | #define ROBOT_DELETE_BUFFER 10 |
||
20 | #define CAM_UPDATE_PERIOD 100000 |
||
21 | |||
22 | 436 | jknichel | PositionMonitor::PositionMonitor() { |
23 | //TODO: don't hardcode this file name
|
||
24 | //TODO: check for error returned from init
|
||
25 | vision_init("/var/www/colonet.jpg");
|
||
26 | 466 | emarinel | newIdToAssign = -1;
|
27 | 508 | emarinel | |
28 | pthread_mutex_init(&position_map_lock, NULL);
|
||
29 | 436 | jknichel | } |
30 | |||
31 | PositionMonitor::~PositionMonitor() { |
||
32 | } |
||
33 | |||
34 | 508 | emarinel | void PositionMonitor::run() {
|
35 | while (1) { |
||
36 | updatePositions(); |
||
37 | 550 | emarinel | usleep(CAM_UPDATE_PERIOD); |
38 | 508 | emarinel | } |
39 | 436 | jknichel | } |
40 | |||
41 | 447 | emarinel | int PositionMonitor::updatePositions() {
|
42 | 449 | jknichel | VisionPosition * positions = NULL;
|
43 | 436 | jknichel | |
44 | //TODO: check for error returned
|
||
45 | 449 | jknichel | int numPositions = vision_get_robot_positions(&positions);
|
46 | 581 | jknichel | /*
|
47 | printf("numPositions is %d\n", numPositions);
|
||
48 | 467 | emarinel | for (int i = 0; i < numPositions; i++) {
|
49 | printf("{%d,%d} ", positions[i].x, positions[i].y);
|
||
50 | }
|
||
51 | printf("\n");
|
||
52 | 581 | jknichel | */
|
53 | 436 | jknichel | |
54 | 468 | jknichel | map<int, VisionPosition> newPositionMap;
|
55 | 467 | emarinel | |
56 | 509 | emarinel | pthread_mutex_lock(&position_map_lock); |
57 | |||
58 | 458 | jknichel | //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 | 449 | jknichel | |
66 | 458 | jknichel | if (isProbablySameRobot(newPos, oldPos)) {
|
67 | 466 | emarinel | //TODO: is this the right use of an iterator?
|
68 | 509 | emarinel | newPositionMap.insert(make_pair(iter->first, newPos)); |
69 | 550 | emarinel | deleteBufferMap.erase(iter->first); |
70 | deleteBufferMap.insert(make_pair(iter->first, ROBOT_DELETE_BUFFER)); |
||
71 | 466 | emarinel | break;
|
72 | 458 | jknichel | } |
73 | } |
||
74 | 466 | emarinel | |
75 | 458 | jknichel | if (iter == positionMap.end()) {
|
76 | 468 | jknichel | //a position was found that probably isn't a known
|
77 | // robot so add it in case a new robot entered the field
|
||
78 | 466 | emarinel | printf("Inserting new robot: %d (%d,%d)", newIdToAssign, newPos.x, newPos.y);
|
79 | |||
80 | 458 | jknichel | //a position was found that probably isn't a known robot so add it in case a new robot entered the field
|
81 | 468 | jknichel | newPositionMap.insert(make_pair(newIdToAssign, newPos)); |
82 | 534 | jknichel | deleteBufferMap.insert(make_pair(newIdToAssign, ROBOT_DELETE_BUFFER)); |
83 | 466 | emarinel | newIdToAssign--; |
84 | 458 | jknichel | } |
85 | } |
||
86 | |||
87 | 534 | jknichel | 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 | 550 | emarinel | newPositionMap.insert(make_pair(currId, iter2->second)); |
96 | deleteBufferMap.erase(currId); |
||
97 | deleteBufferMap.insert(make_pair(currId, bufferValue)); |
||
98 | 534 | jknichel | } else {
|
99 | 550 | emarinel | deleteBufferMap.erase(currId); |
100 | 534 | jknichel | } |
101 | } |
||
102 | } |
||
103 | |||
104 | 468 | jknichel | positionMap = newPositionMap; |
105 | |||
106 | 581 | jknichel | // printf("\npositionMap size is %d and deleteBufferMap size is %d\n", positionMap.size(), deleteBufferMap.size());
|
107 | 534 | jknichel | |
108 | 581 | jknichel | /*
|
109 | 449 | jknichel | //TODO: remove this debug information
|
110 | map<int, VisionPosition>::iterator iter;
|
||
111 | for (iter = positionMap.begin(); iter != positionMap.end(); iter++) {
|
||
112 | 537 | jknichel | printf("%d has position (%d, %d) with delete buffer %d\n", iter->first, iter->second.x, iter->second.y, deleteBufferMap[iter->first]);
|
113 | 449 | jknichel | }
|
114 | 581 | jknichel | */
|
115 | 509 | emarinel | |
116 | 581 | jknichel | |
117 | 509 | emarinel | pthread_mutex_unlock(&position_map_lock); |
118 | |||
119 | if (positions) {
|
||
120 | 449 | jknichel | free(positions); |
121 | 509 | emarinel | } |
122 | 458 | jknichel | |
123 | 436 | jknichel | return 0; |
124 | } |
||
125 | 443 | emarinel | |
126 | 455 | emarinel | int PositionMonitor::assignRealId(int old_id, int real_id) { |
127 | printf("assigning real_id %d to old_id %d\n", real_id, old_id);
|
||
128 | |||
129 | 509 | emarinel | pthread_mutex_lock(&position_map_lock); |
130 | |||
131 | 455 | emarinel | map<int,VisionPosition>::iterator iter = positionMap.find(old_id);
|
132 | |||
133 | if (iter == positionMap.end()) {
|
||
134 | fprintf(stderr, "assignRealId: old_id not found\n");
|
||
135 | 520 | emarinel | pthread_mutex_unlock(&position_map_lock); |
136 | 455 | emarinel | return -1; |
137 | } |
||
138 | |||
139 | positionMap.insert(make_pair(real_id, iter->second)); |
||
140 | positionMap.erase(old_id); |
||
141 | 534 | jknichel | int oldDeleteBuffer = deleteBufferMap[old_id];
|
142 | deleteBufferMap.insert(make_pair(real_id, oldDeleteBuffer)); |
||
143 | deleteBufferMap.erase(old_id); |
||
144 | 455 | emarinel | |
145 | 509 | emarinel | pthread_mutex_unlock(&position_map_lock); |
146 | |||
147 | 455 | emarinel | return 0; |
148 | } |
||
149 | |||
150 | 447 | emarinel | map<int, VisionPosition> PositionMonitor::getAllRobotPositions() {
|
151 | 509 | emarinel | // TODO return a copy instead of the actual map for synch purposes
|
152 | 447 | emarinel | return positionMap;
|
153 | } |
||
154 | |||
155 | 518 | emarinel | int PositionMonitor::getNumVisibleRobots() {
|
156 | return positionMap.size();
|
||
157 | } |
||
158 | |||
159 | VisionPosition PositionMonitor::getFirstPosition(void) {
|
||
160 | return positionMap.begin()->second;
|
||
161 | } |
||
162 | |||
163 | 443 | emarinel | int PositionMonitor::getRobotPosition(int robot_id, int* xbuf, int* ybuf) { |
164 | 449 | jknichel | //TODO: figure out what a map returns if the element doesn't exist
|
165 | 513 | emarinel | |
166 | 509 | emarinel | pthread_mutex_lock(&position_map_lock); |
167 | |||
168 | 451 | jknichel | if (positionMap.find(robot_id) == positionMap.end()){
|
169 | 518 | emarinel | pthread_mutex_unlock(&position_map_lock); |
170 | 449 | jknichel | return -1; |
171 | 518 | emarinel | } else {
|
172 | VisionPosition pos = positionMap[robot_id]; |
||
173 | 449 | jknichel | |
174 | 518 | emarinel | pthread_mutex_unlock(&position_map_lock); |
175 | 455 | emarinel | |
176 | 518 | emarinel | *xbuf = pos.x; |
177 | *ybuf = pos.y; |
||
178 | 509 | emarinel | |
179 | 518 | emarinel | return 0; |
180 | } |
||
181 | 443 | emarinel | } |
182 | 458 | jknichel | |
183 | bool PositionMonitor::isProbablySameRobot(VisionPosition p1, VisionPosition p2) {
|
||
184 | int xDiff = p1.x - p2.x;
|
||
185 | int yDiff = p1.y - p2.y;
|
||
186 | 513 | emarinel | return (xDiff*xDiff + yDiff*yDiff < SAME_ROBOT_DISTANCE_THRESHOLD*SAME_ROBOT_DISTANCE_THRESHOLD);
|
187 | 458 | jknichel | } |