Project

General

Profile

Statistics
| Revision:

root / trunk / code / projects / colonet / server / PositionMonitor.cpp @ 518

History | View | Annotate | Download (3.86 KB)

1
/**
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
#include <PositionMonitor.h>
12
#include <stdlib.h>
13
#include <vision.h>
14

    
15
#include <stdio.h>
16

    
17
#include <map>
18
using namespace std;
19

    
20
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
  newIdToAssign = -1;
25
  
26
  pthread_mutex_init(&position_map_lock, NULL);
27
}
28

    
29
PositionMonitor::~PositionMonitor() {
30
}
31

    
32
void PositionMonitor::run() {
33
  while (1) {
34
    updatePositions();
35
    usleep(500000);
36
  }
37
}
38

    
39
int PositionMonitor::updatePositions() {
40
  VisionPosition * positions = NULL;
41

    
42
  //TODO: check for error returned
43
  int numPositions = vision_get_robot_positions(&positions);
44
  printf("numPositions is %d\n", numPositions);
45
  for (int i = 0; i < numPositions; i++) {
46
    printf("{%d,%d} ", positions[i].x, positions[i].y);
47
  }
48
  printf("\n");
49

    
50
  map<int, VisionPosition> newPositionMap;
51

    
52
  pthread_mutex_lock(&position_map_lock);
53

    
54
  //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

    
62
      if (isProbablySameRobot(newPos, oldPos)) {
63
        //TODO: is this the right use of an iterator?
64
        newPositionMap.insert(make_pair(iter->first, newPos));
65
        break;
66
      }
67
    }
68

    
69
    if (iter == positionMap.end()) {
70
      //a position was found that probably isn't a known 
71
      //  robot so add it in case a new robot entered the field
72
      printf("Inserting new robot: %d (%d,%d)", newIdToAssign, newPos.x, newPos.y);
73
      
74
      //a position was found that probably isn't a known robot so add it in case a new robot entered the field
75
      newPositionMap.insert(make_pair(newIdToAssign, newPos));
76
      newIdToAssign--;
77
    }
78
  }
79

    
80
  positionMap = newPositionMap;
81

    
82
  //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

    
88
  pthread_mutex_unlock(&position_map_lock);
89

    
90
  if (positions) {
91
    free(positions);
92
  }
93

    
94
  return 0;
95
}
96

    
97
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
  pthread_mutex_lock(&position_map_lock);
101

    
102
  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
    return -1;
107
  }
108

    
109
  positionMap.insert(make_pair(real_id, iter->second));
110
  positionMap.erase(old_id);
111

    
112
  pthread_mutex_unlock(&position_map_lock);
113

    
114
  return 0;
115
}
116

    
117
map<int, VisionPosition> PositionMonitor::getAllRobotPositions() {
118
  // TODO return a copy instead of the actual map for synch purposes
119
  return positionMap;
120
}
121

    
122
int PositionMonitor::getNumVisibleRobots() {
123
  return positionMap.size();
124
}
125

    
126
VisionPosition PositionMonitor::getFirstPosition(void) {
127
  return positionMap.begin()->second;
128
}
129

    
130
int PositionMonitor::getRobotPosition(int robot_id, int* xbuf, int* ybuf) {
131
  //TODO: figure out what a map returns if the element doesn't exist
132

    
133
  pthread_mutex_lock(&position_map_lock);
134

    
135
  if (positionMap.find(robot_id) == positionMap.end()){
136
    pthread_mutex_unlock(&position_map_lock);    
137
    return -1;
138
  } else {
139
    VisionPosition pos = positionMap[robot_id];
140

    
141
    pthread_mutex_unlock(&position_map_lock);
142

    
143
    *xbuf = pos.x;
144
    *ybuf = pos.y;
145

    
146
    return 0;
147
  }
148
}
149

    
150
bool PositionMonitor::isProbablySameRobot(VisionPosition p1, VisionPosition p2) {
151
  int xDiff = p1.x - p2.x;
152
  int yDiff = p1.y - p2.y;
153
  return (xDiff*xDiff + yDiff*yDiff < SAME_ROBOT_DISTANCE_THRESHOLD*SAME_ROBOT_DISTANCE_THRESHOLD);
154
}