Project

General

Profile

Revision 458

Added by Jason knichel about 16 years ago

added some really simple support for keep track of which position is which robot over time

View differences:

trunk/code/projects/colonet/ColonetServer/includes/PositionMonitor.h
14 14
using namespace std;
15 15

  
16 16
#define MAX_POSITIONS 20
17
#define MAX_DISTANCE 5
17 18

  
18 19
using namespace std;
19 20

  
......
32 33

  
33 34
 private:
34 35
  map<int, VisionPosition> positionMap;
36
  bool isProbablySameRobot(VisionPosition p1, VisionPosition p2);
37
  int newIdToAssign;
35 38
};
36 39

  
37 40
#endif
trunk/code/projects/colonet/ColonetServer/PositionMonitor.cpp
21 21
  //TODO: don't hardcode this file name
22 22
  //TODO: check for error returned from init
23 23
  vision_init("/var/www/colonet.jpg");
24
  newIdToAssign = 0;
24 25
}
25 26

  
26 27
PositionMonitor::~PositionMonitor() {
......
42 43
  //TODO: check for error returned
43 44
  int numPositions = vision_get_robot_positions(&positions);
44 45

  
46
  /*
45 47
  //TODO: compare current set of positions to previous set of
46 48
  // positions to match up who is who
47 49
  positionMap.clear();
48 50

  
49 51
  int i;
50 52
  for (i = 0; i < numPositions; i++) {
53
    //TODO: would this insert by copy or just point to the memory in positions[i]?
54
    //      If this just points to the memory in positions[i] then we have a bug
51 55
    positionMap[i] = positions[i];
52 56
  }
57
  */
58
  
59
  //TODO: also remove robots that might have disappeared
60
  int i;
61
  for (i = 0; i < numPositions; i++) {
62
    VisionPosition newPos = positions[i];
63
    map<int, VisionPosition>::iterator iter;
64
    for (iter = positionMap.begin(); iter != positionMap.end(); iter++) {
65
      VisionPosition oldPos = iter->second;
53 66

  
67
      if (isProbablySameRobot(newPos, oldPos)) {
68
	//TODO: is this the right use of an iterator?
69
	iter->second = newPos;
70
	break;
71
      }
72
    }
73
    
74
    if (iter == positionMap.end()) {
75
      //a position was found that probably isn't a known robot so add it in case a new robot entered the field
76
      positionMap.insert(make_pair(newIdToAssign, newPos));
77
      newIdToAssign++;
78
    }
79
  }
80

  
54 81
  //TODO: remove this debug information
55 82
  map<int, VisionPosition>::iterator iter;
56 83
  for (iter = positionMap.begin(); iter != positionMap.end(); iter++) {
......
59 86
  
60 87
  if (positions)
61 88
    free(positions);
89

  
62 90
  return 0;
63 91
}
64 92

  
......
95 123

  
96 124
  return 0;
97 125
}
126

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

Also available in: Unified diff