Revision 518
fixed mutex unlock bug. made it so that if there is just one robot seen, it is assumed to be the robot requesting
trunk/code/projects/colonet/server/colonet_wireless.cpp | ||
---|---|---|
106 | 106 |
} |
107 | 107 |
|
108 | 108 |
if (dest == GLOBAL_DEST) { |
109 |
printf("sending to global dest\n"); |
|
109 |
//printf("sending to global dest\n");
|
|
110 | 110 |
if (wl_send_global_packet(COLONET_PACKET_GROUP_ID, (char)msg_type, (char*)(&pkt), |
111 | 111 |
sizeof(ColonetRobotServerPacket), 0) != 0) { |
112 | 112 |
return -1; |
113 | 113 |
} |
114 | 114 |
} else { |
115 |
printf("sending to specific robot: %d.\n", dest);
|
|
115 |
//("sending to specific robot: %d.\n", dest);
|
|
116 | 116 |
if (wl_send_robot_to_robot_global_packet(COLONET_PACKET_GROUP_ID, (char)msg_type, (char*)(&pkt), |
117 | 117 |
sizeof(ColonetRobotServerPacket), dest, COLONET_RESPONSE_PACKET_FRAME_ID) != 0) { |
118 | 118 |
return -1; |
... | ... | |
180 | 180 |
//TODO: These are just here to get rid of compiler warnings |
181 | 181 |
type = type; |
182 | 182 |
|
183 |
printf("!!!***Got packet from robots***!!!\n");
|
|
183 |
//printf("!!!***Got packet from robot***!!!\n");
|
|
184 | 184 |
|
185 | 185 |
ColonetRobotServerPacket* pkt = (ColonetRobotServerPacket*)data; |
186 | 186 |
|
... | ... | |
191 | 191 |
if (pkt->msg_code == ROBOT_REQUEST_POSITION_FROM_SERVER) { |
192 | 192 |
/* Robot has requested its position. */ |
193 | 193 |
int robot_x, robot_y; |
194 |
if (colonet_server.getPositionMonitor()->getRobotPosition(source, &robot_x, &robot_y) != 0) { |
|
194 |
PositionMonitor* pm = colonet_server.getPositionMonitor(); |
|
195 |
int num_robots = pm->getNumVisibleRobots(); |
|
196 |
int ret = pm->getRobotPosition(source, &robot_x, &robot_y); |
|
197 |
if (ret != 0 && num_robots != 1) { |
|
195 | 198 |
fprintf(stderr, "Robot %d requested position, but its position is not known.\n", source); |
196 | 199 |
} else { |
200 |
if (ret != 0 /* && num_robots == 1 */) { |
|
201 |
VisionPosition pos = pm->getFirstPosition(); |
|
202 |
robot_x = pos.x; |
|
203 |
robot_y = pos.y; |
|
204 |
} |
|
205 |
|
|
197 | 206 |
unsigned char response[80]; |
198 |
response[0] = robot_x & 0xFF;
|
|
199 |
response[1] = (robot_x >> 8) & 0xFF;
|
|
200 |
response[2] = robot_y & 0xFF;
|
|
201 |
response[3] = (robot_y >> 8) & 0xFF;
|
|
207 |
response[0] = (robot_x >> 8) & 0xFF;
|
|
208 |
response[1] = robot_x & 0xFF;
|
|
209 |
response[2] = (robot_y >> 8) & 0xFF;
|
|
210 |
response[3] = robot_y & 0xFF;
|
|
202 | 211 |
|
212 |
fprintf(stderr, "Robot %d requested position. Its position is %d,%d.\n", source, robot_x, robot_y); |
|
213 |
|
|
203 | 214 |
if (colonet_wl_send(-1, source, COLONET_COMMAND, SERVER_REPORT_POSITION_TO_ROBOT, response) != 0) { |
204 | 215 |
fprintf(stderr, "colonet_wl_send failed!\n"); |
205 | 216 |
exit(1); |
trunk/code/projects/colonet/server/includes/PositionMonitor.h | ||
---|---|---|
30 | 30 |
int assignRealId(int old_id, int real_id); |
31 | 31 |
map<int, VisionPosition> getAllRobotPositions(void); |
32 | 32 |
int getRobotPosition(int robot_id, int* xbuf, int* ybuf); |
33 |
int getNumVisibleRobots(void); |
|
34 |
VisionPosition getFirstPosition(void); |
|
33 | 35 |
|
34 | 36 |
private: |
35 | 37 |
map<int, VisionPosition> positionMap; |
trunk/code/projects/colonet/server/PositionMonitor.cpp | ||
---|---|---|
119 | 119 |
return positionMap; |
120 | 120 |
} |
121 | 121 |
|
122 |
int PositionMonitor::getNumVisibleRobots() { |
|
123 |
return positionMap.size(); |
|
124 |
} |
|
125 |
|
|
126 |
VisionPosition PositionMonitor::getFirstPosition(void) { |
|
127 |
return positionMap.begin()->second; |
|
128 |
} |
|
129 |
|
|
122 | 130 |
int PositionMonitor::getRobotPosition(int robot_id, int* xbuf, int* ybuf) { |
123 | 131 |
//TODO: figure out what a map returns if the element doesn't exist |
124 | 132 |
|
125 | 133 |
pthread_mutex_lock(&position_map_lock); |
126 | 134 |
|
127 | 135 |
if (positionMap.find(robot_id) == positionMap.end()){ |
136 |
pthread_mutex_unlock(&position_map_lock); |
|
128 | 137 |
return -1; |
129 |
} |
|
138 |
} else { |
|
139 |
VisionPosition pos = positionMap[robot_id]; |
|
130 | 140 |
|
131 |
VisionPosition pos = positionMap[robot_id];
|
|
141 |
pthread_mutex_unlock(&position_map_lock);
|
|
132 | 142 |
|
133 |
pthread_mutex_unlock(&position_map_lock); |
|
143 |
*xbuf = pos.x; |
|
144 |
*ybuf = pos.y; |
|
134 | 145 |
|
135 |
*xbuf = pos.x; |
|
136 |
*ybuf = pos.y; |
|
137 |
|
|
138 |
return 0; |
|
146 |
return 0; |
|
147 |
} |
|
139 | 148 |
} |
140 | 149 |
|
141 | 150 |
bool PositionMonitor::isProbablySameRobot(VisionPosition p1, VisionPosition p2) { |
Also available in: Unified diff