Project

General

Profile

Statistics
| Revision:

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

History | View | Annotate | Download (3.36 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::stopMonitoring() {
40
  //TODO: fill this in when this becomes asynchronous
41
  return 0;
42
}
43

    
44
int PositionMonitor::updatePositions() {
45
  VisionPosition * positions = NULL;
46

    
47
  //TODO: check for error returned
48
  int numPositions = vision_get_robot_positions(&positions);
49
  printf("numPositions is %d\n", numPositions);
50
  for (int i = 0; i < numPositions; i++) {
51
    printf("{%d,%d} ", positions[i].x, positions[i].y);
52
  }
53
  printf("\n");
54

    
55
  map<int, VisionPosition> newPositionMap;
56

    
57
  //TODO: also remove robots that might have disappeared
58
  int i;
59
  for (i = 0; i < numPositions; i++) {
60
    VisionPosition newPos = positions[i];
61
    map<int, VisionPosition>::iterator iter;
62
    for (iter = positionMap.begin(); iter != positionMap.end(); iter++) {
63
      VisionPosition oldPos = iter->second;
64

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

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

    
83
  positionMap = newPositionMap;
84

    
85
  //TODO: remove this debug information
86
  map<int, VisionPosition>::iterator iter;
87
  for (iter = positionMap.begin(); iter != positionMap.end(); iter++) {
88
    printf("%d has position (%d, %d)\n", iter->first, iter->second.x, iter->second.y);
89
  }
90
  
91
  if (positions)
92
    free(positions);
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
  map<int,VisionPosition>::iterator iter = positionMap.find(old_id);
101

    
102
  if (iter == positionMap.end()) {
103
    fprintf(stderr, "assignRealId: old_id not found\n");
104
    return -1;
105
  }
106

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

    
110
  return 0;
111
}
112

    
113
map<int, VisionPosition> PositionMonitor::getAllRobotPositions() {
114
  return positionMap;
115
}
116

    
117
int PositionMonitor::getRobotPosition(int robot_id, int* xbuf, int* ybuf) {
118
  //TODO: figure out what a map returns if the element doesn't exist
119
  if (positionMap.find(robot_id) == positionMap.end()){
120
    return -1;
121
  }
122

    
123
  VisionPosition pos = positionMap[robot_id];
124

    
125
  *xbuf = pos.x;
126
  *ybuf = pos.y;
127

    
128
  return 0;
129
}
130

    
131
bool PositionMonitor::isProbablySameRobot(VisionPosition p1, VisionPosition p2) {
132
  int xDiff = p1.x - p2.x;
133
  int yDiff = p1.y - p2.y;
134
  return (xDiff*xDiff + yDiff*yDiff < MAX_DISTANCE*MAX_DISTANCE);
135
}