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