Project

General

Profile

Statistics
| Revision:

root / trunk / code / projects / colonet / ColonetServer / PositionMonitor.cpp @ 468

History | View | Annotate | Download (3.38 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

    
27
PositionMonitor::~PositionMonitor() {
28
}
29

    
30
int PositionMonitor::startMonitoring() {
31
  //TODO: fill this in when this becomes asynchronous
32
  return 0;
33
}
34

    
35
int PositionMonitor::stopMonitoring() {
36
  //TODO: fill this in when this becomes asynchronous
37
  return 0;
38
}
39

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

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

    
51
  map<int, VisionPosition> newPositionMap;
52

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

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

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

    
79
  positionMap = newPositionMap;
80

    
81
  //TODO: remove this debug information
82
  map<int, VisionPosition>::iterator iter;
83
  for (iter = positionMap.begin(); iter != positionMap.end(); iter++) {
84
    printf("%d has position (%d, %d)\n", iter->first, iter->second.x, iter->second.y);
85
  }
86
  
87
  if (positions)
88
    free(positions);
89

    
90
  return 0;
91
}
92

    
93
int PositionMonitor::assignRealId(int old_id, int real_id) {
94
  printf("assigning real_id %d to old_id %d\n", real_id, old_id);
95

    
96
  map<int,VisionPosition>::iterator iter = positionMap.find(old_id);
97

    
98
  if (iter == positionMap.end()) {
99
    fprintf(stderr, "assignRealId: old_id not found\n");
100
    return -1;
101
  }
102

    
103
  positionMap.insert(make_pair(real_id, iter->second));
104
  positionMap.erase(old_id);
105

    
106
  return 0;
107
}
108

    
109
map<int, VisionPosition> PositionMonitor::getAllRobotPositions() {
110
  //TODO do this in another thread!
111
  updatePositions();
112

    
113
  return positionMap;
114
}
115

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

    
122
  VisionPosition pos = positionMap[robot_id];
123

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

    
127
  return 0;
128
}
129

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