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