Revision 474
renaming colonetserver to server
trunk/code/projects/colonet/ColonetServer/PositionMonitor.cpp | ||
---|---|---|
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 |
} |
trunk/code/projects/colonet/ColonetServer/Makefile | ||
---|---|---|
1 |
# ColonetServer makefile |
|
2 |
|
|
3 |
CC = g++ |
|
4 |
CFLAGS = -Wall -Wshadow -Wextra -g |
|
5 |
VISIONFLAGS = -ggdb `pkg-config opencv --cflags --libs` |
|
6 |
|
|
7 |
COLONETCPPFILES = Main.cpp ColonetServer.cpp ConnectionPool.cpp Command.cpp colonet_wireless.cpp PositionMonitor.cpp |
|
8 |
COLONETCPPOBJECTS = $(COLONETCPPFILES:.cpp=.o) |
|
9 |
COLONETFILES = options.c |
|
10 |
COLONETOBJECTS = $(COLONETFILES:.c=.o) |
|
11 |
LOGGINGFILES = Log.cpp |
|
12 |
LOGGINGOBJECTS = $(LOGGINGFILES:.cpp=.o) |
|
13 |
|
|
14 |
VISIONOBJECTS = ../vision/vision.o |
|
15 |
|
|
16 |
COLONETHEADERFILES = includes/*.h |
|
17 |
|
|
18 |
VPATH = ../lib:../vision |
|
19 |
INCLUDE_DIRS = ../lib ../../libwireless/lib includes ../vision |
|
20 |
LIBRARY_DIRS = ../lib ../../libwireless/lib ../vision |
|
21 |
|
|
22 |
#this takes the include directory and puts a -I in front of each directory name before being used in a gcc statement |
|
23 |
INCLUDE_DIRS_FOR_GCC = $(patsubst %, -I %, $(INCLUDE_DIRS)) |
|
24 |
#this takes the library directory and puts a -L in front of each directory name so it can be used in a gcc statement |
|
25 |
LIBRARY_DIRS_FOR_GCC = $(patsubst %,-L%, $(LIBRARY_DIRS)) |
|
26 |
|
|
27 |
.PHONY : all clean run |
|
28 |
|
|
29 |
all: ColonetServer |
|
30 |
|
|
31 |
run: |
|
32 |
cd ../../libwireless/lib; make colonet |
|
33 |
make; ./ColonetServer |
|
34 |
|
|
35 |
../../libwireless/lib/libwireless_colonet.a: ../../libwireless/lib/*.c ../../libwireless/lib/*.h |
|
36 |
cd ../../libwireless/lib; make colonet |
|
37 |
|
|
38 |
ColonetServer: ../../libwireless/lib/libwireless_colonet.a $(COLONETCPPFILES) $(COLONETFILES) $(LOGGINGFILES) $(COLONETHEADERFILES) |
|
39 |
@echo "-----------------------" |
|
40 |
@echo "---begin compilation---" |
|
41 |
@echo "" |
|
42 |
@echo "---create object files---" |
|
43 |
$(CC) $(CFLAGS) -c $(COLONETFILES) $(INCLUDE_DIRS_FOR_GCC) |
|
44 |
$(CC) $(CFLAGS) -c $(COLONETCPPFILES) $(INCLUDE_DIRS_FOR_GCC) |
|
45 |
$(CC) $(CFLAGS) -c $(LOGGINGFILES) $(INCLUDE_DIRS_FOR_GCC) |
|
46 |
@echo "---finish creating object files---" |
|
47 |
@echo "" |
|
48 |
@echo "---link files---" |
|
49 |
$(CC) $(CFLAGS) $(VISIONFLAGS) $(COLONETOBJECTS) $(COLONETCPPOBJECTS) $(LOGGINGOBJECTS) $(VISIONOBJECTS) $(LIBRARY_DIRS_FOR_GCC) -lpthread -lwireless_colonet $(INCLUDE_DIRS_FOR_GCC) -o $@ |
|
50 |
@echo "---finish linking files---" |
|
51 |
@echo "" |
|
52 |
@echo "---finish compilation---" |
|
53 |
@echo "-----------------------" |
|
54 |
|
|
55 |
clean: |
|
56 |
rm -rf *.o ColonetServer *~ |
trunk/code/projects/colonet/ColonetServer/Command.cpp | ||
---|---|---|
1 |
/** |
|
2 |
* @file Command.cpp |
|
3 |
* |
|
4 |
* @author Jason Knichel |
|
5 |
* @date 10/9/07 |
|
6 |
* |
|
7 |
* @todo make it so command doesn't rely on connection pool. have methods |
|
8 |
* that may return a response return it by getting a response array passed in to them |
|
9 |
* that they fill in and then colonet server takes the contents of that response array |
|
10 |
* and sends it to the appropriate client |
|
11 |
*/ |
|
12 |
|
|
13 |
#include <ctype.h> |
|
14 |
#include <stdio.h> |
|
15 |
#include <stdlib.h> |
|
16 |
#include <string.h> |
|
17 |
|
|
18 |
#include <colonet_wireless.h> |
|
19 |
|
|
20 |
#include <Command.h> |
|
21 |
#include <ConnectionPool.h> |
|
22 |
#include <map> |
|
23 |
#include <vision.h> |
|
24 |
|
|
25 |
using namespace std; |
|
26 |
|
|
27 |
Command::Command(ConnectionPool * connection_pool_temp, ColonetServer* cs) { |
|
28 |
connection_pool = connection_pool_temp; |
|
29 |
colonet_server = cs; |
|
30 |
} |
|
31 |
|
|
32 |
Command::~Command() {} |
|
33 |
|
|
34 |
/** |
|
35 |
* Called by connection pool to parse command from client. |
|
36 |
*/ |
|
37 |
int Command::parse_command(char* command, int pool_index) { |
|
38 |
char tokens[MAX_TOKENS][MAX_TOKEN_SIZE]; |
|
39 |
int number_tokens = 0; |
|
40 |
char* end_pointer = NULL; |
|
41 |
int command_id; |
|
42 |
|
|
43 |
if (!connection_pool || !command || pool_index < 0) { |
|
44 |
return -1; |
|
45 |
} |
|
46 |
|
|
47 |
if ((number_tokens = tokenize_command(command, tokens)) < 0) { |
|
48 |
return -1; |
|
49 |
} |
|
50 |
|
|
51 |
//the 10 in the function call indicates number is base 10 |
|
52 |
command_id = strtol(tokens[0], &end_pointer, 10); |
|
53 |
|
|
54 |
if (!end_pointer || *end_pointer != '\0') { |
|
55 |
printf("There was an error converting first token into a number.\n"); |
|
56 |
return -1; |
|
57 |
} |
|
58 |
|
|
59 |
if (command_id == SEND_TO_ROBOT) { |
|
60 |
if (parse_send_to_robot(number_tokens, tokens, pool_index)) { |
|
61 |
fprintf(stderr, "parse_send_to_robot failed.\n"); |
|
62 |
return -1; |
|
63 |
} |
|
64 |
} else if (command_id == REQUEST_FROM_SERVER) { |
|
65 |
if (parse_request_from_server(number_tokens, tokens, pool_index)) { |
|
66 |
return -1; |
|
67 |
} |
|
68 |
} |
|
69 |
|
|
70 |
return 0; |
|
71 |
} |
|
72 |
|
|
73 |
/** |
|
74 |
* @brief Breaks a command up into tokens |
|
75 |
* |
|
76 |
* @param command The command to tokenize |
|
77 |
* @param tokens A two dimensional character array to store the tokens in |
|
78 |
* |
|
79 |
* @return 0 on success, negative error code on failure |
|
80 |
*/ |
|
81 |
int Command::tokenize_command(char* command, char tokens[MAX_TOKENS][MAX_TOKEN_SIZE]) { |
|
82 |
char* next_token = command; |
|
83 |
char* end_token = NULL; |
|
84 |
int number_tokens = 0; |
|
85 |
|
|
86 |
if (!command) { |
|
87 |
return -1; |
|
88 |
} |
|
89 |
|
|
90 |
while ((end_token = strstr(next_token, " "))) { |
|
91 |
*end_token = '\0'; |
|
92 |
|
|
93 |
if (strlen(next_token) > MAX_TOKEN_SIZE-1) { |
|
94 |
return -1; |
|
95 |
} |
|
96 |
|
|
97 |
strcpy(tokens[number_tokens], next_token); |
|
98 |
|
|
99 |
number_tokens++; |
|
100 |
next_token = end_token + 1; |
|
101 |
|
|
102 |
while (isspace(*next_token) && *next_token != '\0') { |
|
103 |
next_token++; |
|
104 |
} |
|
105 |
} |
|
106 |
|
|
107 |
if (end_token == NULL && *next_token != '\0') { |
|
108 |
if (strlen(next_token) > MAX_TOKEN_SIZE-1) { |
|
109 |
return -1; |
|
110 |
} |
|
111 |
|
|
112 |
strcpy(tokens[number_tokens], next_token); |
|
113 |
|
|
114 |
number_tokens++; |
|
115 |
} |
|
116 |
|
|
117 |
return number_tokens; |
|
118 |
} |
|
119 |
|
|
120 |
/** |
|
121 |
* @brief checks a list of tokens to see if it's valid |
|
122 |
* |
|
123 |
* @param tokens The tokens to check |
|
124 |
* @param number_tokens The number of tokens contained in the tokens parameter |
|
125 |
* |
|
126 |
* @return 0 if tokens is valid |
|
127 |
*/ |
|
128 |
int Command::check_tokens(unsigned char* tokens, int number_tokens) { |
|
129 |
if (number_tokens > 3 + PACKET_DATA_LEN) { |
|
130 |
/* Too many tokens */ |
|
131 |
return -1; |
|
132 |
} |
|
133 |
|
|
134 |
if (number_tokens < 3) { |
|
135 |
/* Not enough tokens */ |
|
136 |
return -1; |
|
137 |
} |
|
138 |
|
|
139 |
if (tokens[1] != COLONET_REQUEST && tokens[1] != COLONET_COMMAND) { |
|
140 |
/* Invalid message type */ |
|
141 |
return -1; |
|
142 |
} |
|
143 |
|
|
144 |
return 0; |
|
145 |
} |
|
146 |
|
|
147 |
/** |
|
148 |
* @brief Sends parsed command from server to robot(s). |
|
149 |
*/ |
|
150 |
int Command::parse_send_to_robot(int number_int_tokens, char tokens[MAX_TOKENS][MAX_TOKEN_SIZE], int pool_index) { |
|
151 |
unsigned char int_tokens[MAX_TOKENS], arguments[PACKET_DATA_LEN]; |
|
152 |
|
|
153 |
memset(arguments, 1, PACKET_DATA_LEN); |
|
154 |
|
|
155 |
// Convert tokens to ints |
|
156 |
for (int i = ROBOT_COMMAND_OFFSET; i < number_int_tokens; i++) { |
|
157 |
int_tokens[i-ROBOT_COMMAND_OFFSET] = atoi(tokens[i]); |
|
158 |
} |
|
159 |
|
|
160 |
// Fill arguments buffer with arguments |
|
161 |
for (int i = ROBOT_COMMAND_LEN; i < number_int_tokens-ROBOT_COMMAND_OFFSET; i++) { |
|
162 |
arguments[i-ROBOT_COMMAND_LEN] = int_tokens[i]; |
|
163 |
} |
|
164 |
|
|
165 |
// Check the tokens |
|
166 |
if (check_tokens(int_tokens, number_int_tokens) < 0) { |
|
167 |
fprintf(stderr, "%s: Error - Invalid robot command/request.\n", __FUNCTION__); |
|
168 |
return -1; |
|
169 |
} |
|
170 |
|
|
171 |
printf("parsed command from internet client: "); |
|
172 |
for (int i = 0; i < number_int_tokens - ROBOT_COMMAND_OFFSET; i++) { |
|
173 |
printf("%d ", int_tokens[i]); |
|
174 |
} |
|
175 |
printf("\n"); |
|
176 |
|
|
177 |
// Send packet to robot |
|
178 |
if (colonet_wl_send((short)pool_index, int_tokens[0], (ColonetRobotMessageType)int_tokens[1], int_tokens[2], |
|
179 |
arguments) != 0) { |
|
180 |
fprintf(stderr, "Error - Colonet_wl_send failed.\n"); |
|
181 |
exit(1); |
|
182 |
} |
|
183 |
|
|
184 |
return 0; |
|
185 |
} |
|
186 |
|
|
187 |
/** |
|
188 |
* |
|
189 |
*/ |
|
190 |
int Command::parse_request_from_server(int number_tokens, char tokens[MAX_TOKENS][MAX_TOKEN_SIZE], int pool_index) { |
|
191 |
char* end_pointer = NULL; |
|
192 |
|
|
193 |
if (!connection_pool) { |
|
194 |
return -1; |
|
195 |
} |
|
196 |
|
|
197 |
if (number_tokens < 2) { |
|
198 |
return -1; |
|
199 |
} |
|
200 |
|
|
201 |
end_pointer=NULL; |
|
202 |
int second_token = strtol(tokens[1], &end_pointer, 10); |
|
203 |
|
|
204 |
if (!end_pointer || *end_pointer != '\0') { |
|
205 |
printf("There was an error converting second token into a number.\n"); |
|
206 |
return -1; |
|
207 |
} |
|
208 |
|
|
209 |
switch (second_token) { |
|
210 |
case REQUEST_BOM_MATRIX: |
|
211 |
if (parse_request_bom_matrix(pool_index)) { |
|
212 |
return -1; |
|
213 |
} |
|
214 |
break; |
|
215 |
|
|
216 |
case REQUEST_XBEE_IDS: |
|
217 |
if (parse_request_xbee_ids(pool_index)) { |
|
218 |
return -1; |
|
219 |
} |
|
220 |
break; |
|
221 |
|
|
222 |
case CLIENT_REQUEST_ROBOT_POSITIONS: |
|
223 |
if (parse_request_robot_positions(pool_index)) { |
|
224 |
return -1; |
|
225 |
} |
|
226 |
break; |
|
227 |
|
|
228 |
case CLIENT_ASSIGN_ROBOT_ID: |
|
229 |
colonet_server->getPositionMonitor()->assignRealId(atoi(tokens[1]), atoi(tokens[2])); |
|
230 |
break; |
|
231 |
|
|
232 |
default: |
|
233 |
char * my_current_message = "Invalid request!\n"; |
|
234 |
printf("Sending %s\n", my_current_message); |
|
235 |
connection_pool->write_to_client(pool_index, my_current_message, strlen(my_current_message)); |
|
236 |
break; |
|
237 |
} |
|
238 |
|
|
239 |
return 0; |
|
240 |
} |
|
241 |
|
|
242 |
int Command::parse_request_bom_matrix(int pool_index) { |
|
243 |
char response_bom_matrix_buffer[MAX_RESPONSE_LEN]; |
|
244 |
char temp_bom_matrix_buffer[MAX_RESPONSE_LEN]; |
|
245 |
|
|
246 |
if (!connection_pool) { |
|
247 |
return -1; |
|
248 |
} |
|
249 |
|
|
250 |
int num_robots; |
|
251 |
int * xbee_ids; |
|
252 |
int** bom_matrix = colonet_get_sensor_matrix(&num_robots, &xbee_ids); |
|
253 |
|
|
254 |
printf("number of robots is %d\n", num_robots); |
|
255 |
|
|
256 |
//TODO: make this better |
|
257 |
//TODO: make sure I don't need to do MAX_RESPONSE_LENGTH-1 |
|
258 |
snprintf(response_bom_matrix_buffer, MAX_RESPONSE_LEN, "%d %d %d", RESPONSE_TO_CLIENT_REQUEST, REQUEST_BOM_MATRIX, |
|
259 |
num_robots); |
|
260 |
for (int i = 0; i < num_robots; i++) { |
|
261 |
for (int j = 0; j < num_robots; j++) { |
|
262 |
//TODO: don't use strcpy |
|
263 |
strcpy(temp_bom_matrix_buffer, response_bom_matrix_buffer); |
|
264 |
//TODO: put length checking in here so array doesn't go out of bounds |
|
265 |
//TODO: maybe use strncat? |
|
266 |
strcat(temp_bom_matrix_buffer," %d"); |
|
267 |
snprintf(response_bom_matrix_buffer, MAX_RESPONSE_LEN, temp_bom_matrix_buffer, bom_matrix[i][j]); |
|
268 |
} |
|
269 |
} |
|
270 |
strcat(response_bom_matrix_buffer,"\n"); |
|
271 |
connection_pool->write_to_client(pool_index, response_bom_matrix_buffer, strlen(response_bom_matrix_buffer)); |
|
272 |
printf("Sending %s", response_bom_matrix_buffer); |
|
273 |
|
|
274 |
for (int i = 0; i < num_robots; i++) { |
|
275 |
free(bom_matrix[i]); |
|
276 |
} |
|
277 |
free(bom_matrix); |
|
278 |
|
|
279 |
free(xbee_ids); |
|
280 |
|
|
281 |
return 0; |
|
282 |
} |
|
283 |
|
|
284 |
int Command::parse_request_robot_positions(int pool_index) { |
|
285 |
printf("*****parse_request_robot_positions\n"); |
|
286 |
|
|
287 |
map<int, VisionPosition> positions = colonet_server->getPositionMonitor()->getAllRobotPositions(); |
|
288 |
map<int, VisionPosition>::iterator iter; |
|
289 |
|
|
290 |
char position_buffer[256]; |
|
291 |
position_buffer[0] = 0; |
|
292 |
sprintf(position_buffer, "%d %d", RESPONSE_TO_CLIENT_REQUEST, CLIENT_REQUEST_ROBOT_POSITIONS); |
|
293 |
|
|
294 |
for (iter = positions.begin(); iter != positions.end(); iter++) { |
|
295 |
char tmpbuf[80]; |
|
296 |
sprintf(tmpbuf, " %d %d %d", iter->first, iter->second.x, iter->second.y); |
|
297 |
strcat(position_buffer, tmpbuf); |
|
298 |
} |
|
299 |
|
|
300 |
strcat(position_buffer, "\n"); |
|
301 |
|
|
302 |
printf("position buffer is: %s\n", position_buffer); |
|
303 |
|
|
304 |
connection_pool->write_to_client(pool_index, position_buffer, strlen(position_buffer)); |
|
305 |
|
|
306 |
return 0; |
|
307 |
} |
|
308 |
|
|
309 |
int Command::parse_request_xbee_ids(int pool_index) { |
|
310 |
char temp_xbee_id_buffer[MAX_RESPONSE_LEN]; |
|
311 |
char xbee_id_buffer[MAX_RESPONSE_LEN]; |
|
312 |
|
|
313 |
int num_robots; |
|
314 |
int* xbee_ids = colonet_get_xbee_ids(&num_robots); |
|
315 |
|
|
316 |
printf("num_robots: %d\n", num_robots); |
|
317 |
printf("xbee_ids: "); |
|
318 |
for (int i = 0; i < num_robots; i++) { |
|
319 |
printf("%d ", xbee_ids[i]); |
|
320 |
} |
|
321 |
printf("\n"); |
|
322 |
|
|
323 |
if (!connection_pool) { |
|
324 |
return -1; |
|
325 |
} |
|
326 |
|
|
327 |
snprintf(xbee_id_buffer, MAX_RESPONSE_LEN, "%d %d %d", RESPONSE_TO_CLIENT_REQUEST, REQUEST_XBEE_IDS, num_robots); |
|
328 |
|
|
329 |
for (int i = 0; i < num_robots; i++) { |
|
330 |
strcpy(temp_xbee_id_buffer, xbee_id_buffer); |
|
331 |
//TODO: put length checking in here so array doesn't go out of bounds |
|
332 |
//TODO: maybe use strncat? |
|
333 |
strcat(temp_xbee_id_buffer, " %d"); |
|
334 |
snprintf(xbee_id_buffer, MAX_RESPONSE_LEN, temp_xbee_id_buffer, xbee_ids[i]); |
|
335 |
} |
|
336 |
strcat(xbee_id_buffer, "\n"); |
|
337 |
connection_pool->write_to_client(pool_index, xbee_id_buffer, strlen(xbee_id_buffer)); |
|
338 |
printf("Sending %s", xbee_id_buffer); |
|
339 |
|
|
340 |
free(xbee_ids); |
|
341 |
|
|
342 |
return 0; |
|
343 |
} |
trunk/code/projects/colonet/ColonetServer/colonet_wireless.cpp | ||
---|---|---|
1 |
/** @file colonet_wireless.c |
|
2 |
* |
|
3 |
* @brief Implementation of server-side colonet wireless library |
|
4 |
* |
|
5 |
* @author Eugene Marinelli |
|
6 |
*/ |
|
7 |
|
|
8 |
/********************************* Includes **********************************/ |
|
9 |
#include <string> |
|
10 |
#include <pthread.h> |
|
11 |
#include <iostream> |
|
12 |
#include <fstream> |
|
13 |
#include <unistd.h> |
|
14 |
#include <fcntl.h> |
|
15 |
#include <signal.h> |
|
16 |
|
|
17 |
#include <ColonetServer.h> |
|
18 |
#include <wireless.h> // Colonet wireless library. |
|
19 |
#include <wl_token_ring.h> |
|
20 |
|
|
21 |
#include <colonet_defs.h> |
|
22 |
#include <colonet_wireless.h> |
|
23 |
|
|
24 |
/******************************* Definitions *********************************/ |
|
25 |
|
|
26 |
//Enable debug printouts |
|
27 |
#define DEBUG 1 |
|
28 |
|
|
29 |
#ifdef DEBUG |
|
30 |
#define dbg_printf(...) printf(__VA_ARGS__) |
|
31 |
#define dbg_fprintf(...) fprintf(__VA_ARGS__) |
|
32 |
#else |
|
33 |
#define dbg_printf(...) |
|
34 |
#define dbg_fprintf(...) |
|
35 |
#endif |
|
36 |
|
|
37 |
extern ColonetServer colonet_server; |
|
38 |
|
|
39 |
static bool logging_enabled; |
|
40 |
static char log_filename[80]; |
|
41 |
static pthread_t listener_thread; |
|
42 |
static char wl_port[40]; |
|
43 |
|
|
44 |
/************************* Internal prototypes *******************************/ |
|
45 |
static void* listen(void* args); |
|
46 |
static void timeout_handler(void); |
|
47 |
static void handle_response(int frame, int received); |
|
48 |
static void handle_receive(char type, int source, unsigned char* packet, int len); |
|
49 |
static void unregister(void); |
|
50 |
static int log_packet(unsigned char* packet, int len); |
|
51 |
|
|
52 |
/**************************** Public functions *******************************/ |
|
53 |
int colonet_wl_init(char* wl_port_, char* log_filename_) { |
|
54 |
if (log_filename_ != NULL) { |
|
55 |
logging_enabled = true; |
|
56 |
strcpy(log_filename, log_filename_); |
|
57 |
} |
|
58 |
|
|
59 |
strncpy(wl_port, wl_port_, 40); |
|
60 |
|
|
61 |
wl_set_com_port(wl_port); |
|
62 |
|
|
63 |
printf("Calling wl_init(%s)...\n", wl_port); |
|
64 |
if (wl_init() != 0) { |
|
65 |
fprintf(stderr, "wl_init failed.\n"); |
|
66 |
return -1; |
|
67 |
} |
|
68 |
|
|
69 |
wl_token_ring_register(); |
|
70 |
|
|
71 |
printf("Joining token ring...\n"); |
|
72 |
if (wl_token_ring_join() != 0) { |
|
73 |
fprintf(stderr, "Failed to join token ring.\n"); |
|
74 |
return -1; |
|
75 |
} |
|
76 |
printf("Joined token ring.\n"); |
|
77 |
|
|
78 |
return 0; |
|
79 |
} |
|
80 |
|
|
81 |
void colonet_wl_kill_listener_thread() { |
|
82 |
pthread_kill(listener_thread, 9); |
|
83 |
} |
|
84 |
|
|
85 |
int colonet_wl_run_listener_thread() { |
|
86 |
dbg_printf("Spawning listener thread...\n"); |
|
87 |
|
|
88 |
if (pthread_create(&listener_thread, NULL, listen, NULL)) { |
|
89 |
perror("pthread_create"); |
|
90 |
return -1; |
|
91 |
} |
|
92 |
|
|
93 |
return 0; |
|
94 |
} |
|
95 |
|
|
96 |
int colonet_wl_send(short client_source, short dest, ColonetRobotMessageType msg_type, unsigned char msg_code, |
|
97 |
unsigned char* args) { |
|
98 |
printf("colonet_wl_send: client_source:%d, dest:%d, msg_code:%d\n", client_source, dest, msg_code); |
|
99 |
|
|
100 |
ColonetRobotServerPacket pkt; |
|
101 |
pkt.client_id = client_source; |
|
102 |
pkt.msg_code = msg_code; |
|
103 |
|
|
104 |
for (int i = 0; i < PACKET_DATA_LEN; i++) { |
|
105 |
pkt.data[i] = args[i]; |
|
106 |
} |
|
107 |
|
|
108 |
if (dest == GLOBAL_DEST) { |
|
109 |
printf("sending to global dest\n"); |
|
110 |
if (wl_send_global_packet(COLONET_PACKET_GROUP_ID, (char)msg_type, (char*)(&pkt), |
|
111 |
sizeof(ColonetRobotServerPacket), 0) != 0) { |
|
112 |
return -1; |
|
113 |
} |
|
114 |
} else { |
|
115 |
printf("sending to specific robot: %d.\n", dest); |
|
116 |
if (wl_send_robot_to_robot_global_packet(COLONET_PACKET_GROUP_ID, (char)msg_type, (char*)(&pkt), |
|
117 |
sizeof(ColonetRobotServerPacket), dest, COLONET_RESPONSE_PACKET_FRAME_ID) != 0) { |
|
118 |
return -1; |
|
119 |
} |
|
120 |
} |
|
121 |
|
|
122 |
return 0; |
|
123 |
} |
|
124 |
|
|
125 |
int colonet_get_num_robots(void) { |
|
126 |
return wl_token_get_num_robots(); |
|
127 |
} |
|
128 |
|
|
129 |
int* colonet_get_xbee_ids(int* numrobots) { |
|
130 |
int num_robots = wl_token_get_num_robots(); |
|
131 |
int* ids = (int*)malloc(num_robots * sizeof(int)); |
|
132 |
|
|
133 |
wl_token_iterator_begin(); |
|
134 |
|
|
135 |
int i = 0; |
|
136 |
while (wl_token_iterator_has_next()) { |
|
137 |
ids[i] = wl_token_iterator_next(); |
|
138 |
i++; |
|
139 |
} |
|
140 |
|
|
141 |
*numrobots = num_robots; |
|
142 |
return ids; |
|
143 |
} |
|
144 |
|
|
145 |
// Returns int**; should be freed |
|
146 |
int** colonet_get_sensor_matrix(int* numrobots, int** ids_) { |
|
147 |
int num_robots; |
|
148 |
int* ids = colonet_get_xbee_ids(&num_robots); |
|
149 |
|
|
150 |
int** m = (int**)malloc(num_robots * sizeof(int*)); |
|
151 |
for (int i = 0; i < num_robots; i++) { |
|
152 |
m[i] = (int*)malloc(num_robots * sizeof(int*)); |
|
153 |
} |
|
154 |
|
|
155 |
for (int i = 0; i < num_robots; i++) { |
|
156 |
for (int j = 0; j < num_robots; j++) { |
|
157 |
m[i][j] = wl_token_get_sensor_reading(ids[i], ids[j]); |
|
158 |
} |
|
159 |
} |
|
160 |
|
|
161 |
*numrobots = num_robots; |
|
162 |
*ids_ = ids; |
|
163 |
return m; |
|
164 |
} |
|
165 |
|
|
166 |
/**************************** Private functions ******************************/ |
|
167 |
|
|
168 |
static void timeout_handler() { |
|
169 |
// printf("colonet wireless - timeout!\n"); |
|
170 |
} |
|
171 |
|
|
172 |
static void handle_response(int frame, int received) { |
|
173 |
//TODO: These are just here to get rid of compiler warnings |
|
174 |
frame = frame; |
|
175 |
received = received; |
|
176 |
//printf("got response.\n"); |
|
177 |
} |
|
178 |
|
|
179 |
static void handle_receive(char type, int source, unsigned char* data, int len) { |
|
180 |
//TODO: These are just here to get rid of compiler warnings |
|
181 |
type = type; |
|
182 |
|
|
183 |
printf("handle receive\n"); |
|
184 |
|
|
185 |
ColonetRobotServerPacket* pkt = (ColonetRobotServerPacket*)data; |
|
186 |
|
|
187 |
if (logging_enabled) { |
|
188 |
log_packet(data, len); |
|
189 |
} |
|
190 |
|
|
191 |
if (pkt->msg_code == ROBOT_REQUEST_POSITION_FROM_SERVER) { |
|
192 |
/* Robot has requested its position. */ |
|
193 |
int robot_x, robot_y; |
|
194 |
if (colonet_server.getPositionMonitor()->getRobotPosition(source, &robot_x, &robot_y) != 0) { |
|
195 |
fprintf(stderr, "Robot %d requested position, but its position is not known.\n", source); |
|
196 |
} else { |
|
197 |
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; |
|
202 |
|
|
203 |
if (colonet_wl_send(-1, source, COLONET_COMMAND, SERVER_REPORT_POSITION_TO_ROBOT, response) != 0) { |
|
204 |
fprintf(stderr, "colonet_wl_send failed!\n"); |
|
205 |
exit(1); |
|
206 |
} |
|
207 |
} |
|
208 |
} else { |
|
209 |
char processed_data[80]; |
|
210 |
sprintf(processed_data, "%d %d %d %s\n", RESPONSE_TO_CLIENT_REQUEST, pkt->msg_code, source, pkt->data); |
|
211 |
|
|
212 |
colonet_server.process_received_wireless_message(pkt->client_id, processed_data, strlen(processed_data)); |
|
213 |
} |
|
214 |
} |
|
215 |
|
|
216 |
static void unregister(void) { |
|
217 |
printf("unregister\n"); |
|
218 |
} |
|
219 |
|
|
220 |
/** @brief Analogous to the "SIGNAL" or "ISR" function on the robots. |
|
221 |
* Listens for bytes on the wireless port and constructs packets based on them. |
|
222 |
* Not part of the ColonetWireless class since a function pointer must be |
|
223 |
* passed in starting the thread that runs it (tricky or impossible if it's |
|
224 |
* a class function). |
|
225 |
* |
|
226 |
* @param args Pointer to arguments. Can be safely casted to ListenerArgs type |
|
227 |
* @return NULL |
|
228 |
*/ |
|
229 |
static void* listen(void* args) { |
|
230 |
//TODO: These are just here to get rid of compiler warnings |
|
231 |
args = args; |
|
232 |
|
|
233 |
printf("Called listen.\n"); |
|
234 |
|
|
235 |
PacketGroupHandler pgh = {COLONET_PACKET_GROUP_ID, |
|
236 |
timeout_handler, |
|
237 |
handle_response, |
|
238 |
handle_receive, |
|
239 |
unregister}; |
|
240 |
wl_register_packet_group(&pgh); |
|
241 |
|
|
242 |
while (1) { |
|
243 |
wl_do(); |
|
244 |
usleep(1000); |
|
245 |
} |
|
246 |
|
|
247 |
wl_terminate(); |
|
248 |
return NULL; |
|
249 |
} |
|
250 |
|
|
251 |
/** @brief |
|
252 |
* |
|
253 |
* @param pkt Packet to be logged |
|
254 |
* |
|
255 |
* @return 0 on success, -1 on failure |
|
256 |
*/ |
|
257 |
int log_packet(unsigned char* packet, int len) { |
|
258 |
FILE* logfile = fopen(log_filename, "a"); |
|
259 |
|
|
260 |
if (logfile == NULL) { |
|
261 |
dbg_printf("%s: Error - fopen %s failed.\n", log_filename, __FUNCTION__); |
|
262 |
return -1; |
|
263 |
} |
|
264 |
|
|
265 |
for (int i = 0; i < len; i++) { |
|
266 |
fprintf(logfile, "%d ", *((unsigned char*)packet + i)); |
|
267 |
} |
|
268 |
fprintf(logfile, "\n"); |
|
269 |
|
|
270 |
fclose(logfile); |
|
271 |
return 0; |
|
272 |
} |
trunk/code/projects/colonet/ColonetServer/Log.cpp | ||
---|---|---|
1 |
/** |
|
2 |
* @file Log.cpp |
|
3 |
* |
|
4 |
* @brief This file contains the code to do logging for Colonet |
|
5 |
* |
|
6 |
* @author Jason Knichel |
|
7 |
* |
|
8 |
*/ |
|
9 |
|
|
10 |
#include <time.h> |
|
11 |
#include <string.h> |
|
12 |
#include <stdio.h> |
|
13 |
|
|
14 |
#include <Log.h> |
|
15 |
|
|
16 |
/** |
|
17 |
* @brief Constructor for the Log class |
|
18 |
* |
|
19 |
* @param filename The name of the file to output log statements to |
|
20 |
*/ |
|
21 |
Log::Log(char * filename) { |
|
22 |
printf("Creating log object\n"); |
|
23 |
|
|
24 |
if (!filename) { |
|
25 |
fprintf(stderr, "Provided a null filename when trying to create a log.\n"); |
|
26 |
return; |
|
27 |
} |
|
28 |
|
|
29 |
log_file = fopen(filename, "a"); //open file for appending |
|
30 |
if (!log_file) { |
|
31 |
fprintf(stderr, "Error opening %s as log file.\n", filename); |
|
32 |
return; |
|
33 |
} |
|
34 |
|
|
35 |
printf("About to log start message.\n"); |
|
36 |
log_string(LOG_TYPE_START_LOG, "Starting server"); |
|
37 |
} |
|
38 |
|
|
39 |
/** |
|
40 |
* @brief Destructor for the Log class |
|
41 |
*/ |
|
42 |
Log::~Log() { |
|
43 |
if (!log_file) { |
|
44 |
fclose(log_file); |
|
45 |
} |
|
46 |
} |
|
47 |
|
|
48 |
/** |
|
49 |
* @brief A function to get the current time |
|
50 |
* |
|
51 |
* @return A struct tm that represents the current time |
|
52 |
*/ |
|
53 |
struct tm Log::get_current_time() { |
|
54 |
struct tm current_time; |
|
55 |
memset(¤t_time, 0, sizeof(struct tm)); |
|
56 |
|
|
57 |
time_t t = time(NULL); |
|
58 |
localtime_r(&t, ¤t_time); |
|
59 |
|
|
60 |
return current_time; |
|
61 |
} |
|
62 |
|
|
63 |
/** |
|
64 |
* @brief This method logs a message with the specified type |
|
65 |
* |
|
66 |
* @param type The type of the message to log |
|
67 |
* @param message The message to log |
|
68 |
* |
|
69 |
* @return 0 on success, negative error code on error |
|
70 |
*/ |
|
71 |
int Log::log_string(int type, char * message) { |
|
72 |
if (!log_file) { |
|
73 |
fprintf(stderr, "Tried to log a message but the file pointer was null.\n"); |
|
74 |
return -1; |
|
75 |
} |
|
76 |
|
|
77 |
if (!message) { |
|
78 |
fprintf(stderr, "Tried to log a null message.\n"); |
|
79 |
return -1; |
|
80 |
} |
|
81 |
|
|
82 |
static char * start_log = "Starting Log"; |
|
83 |
static char * connect = "Client Connecting"; |
|
84 |
static char * disconnect = "Client Disconnecting"; |
|
85 |
static char * error = "Error"; |
|
86 |
static char * log_generic_message = "Generic Message"; |
|
87 |
|
|
88 |
char * message_type; |
|
89 |
|
|
90 |
switch(type) { |
|
91 |
case LOG_TYPE_START_LOG: |
|
92 |
message_type = start_log; |
|
93 |
break; |
|
94 |
case LOG_TYPE_CONNECT: |
|
95 |
message_type = connect; |
|
96 |
break; |
|
97 |
case LOG_TYPE_DISCONNECT: |
|
98 |
message_type = disconnect; |
|
99 |
break; |
|
100 |
case LOG_TYPE_ERROR: |
|
101 |
message_type = error; |
|
102 |
break; |
|
103 |
case LOG_TYPE_MESSAGE: |
|
104 |
message_type = log_generic_message; |
|
105 |
break; |
|
106 |
default: |
|
107 |
fprintf(stderr, "Tried to log a message with an invalid type.\n"); |
|
108 |
return -1; |
|
109 |
} |
|
110 |
|
|
111 |
struct tm current_time = get_current_time(); |
|
112 |
|
|
113 |
char buffer[LOG_MAX_TIME_LENGTH]; |
|
114 |
asctime_r(¤t_time, buffer); |
|
115 |
int len = strlen(buffer); |
|
116 |
buffer[len-1] = '\0'; //remove the newline that is put at the end by asctime_r |
|
117 |
fprintf(log_file, "%s %*s %s\n", buffer, LOG_MAX_TYPE_LENGTH, message_type, message); |
|
118 |
fflush(log_file); |
|
119 |
|
|
120 |
return 0; |
|
121 |
} |
|
122 |
|
|
123 |
/** |
|
124 |
* @brief An easier way to log an error message |
|
125 |
* |
|
126 |
* @param message The message to log as an error |
|
127 |
* |
|
128 |
* @return 0 on success, negative error code on error |
|
129 |
*/ |
|
130 |
int Log::log_error(char * message) { |
|
131 |
return log_string(LOG_TYPE_ERROR, message); |
|
132 |
} |
|
133 |
|
|
134 |
/** |
|
135 |
* @brief An easier way to log a normal message |
|
136 |
* |
|
137 |
* @param message The message to log as a normal message |
|
138 |
* |
|
139 |
* @return 0 on success, negative error code on error |
|
140 |
*/ |
|
141 |
int Log::log_message(char * message) { |
|
142 |
return log_string(LOG_TYPE_MESSAGE, message); |
|
143 |
} |
trunk/code/projects/colonet/ColonetServer/Main.cpp | ||
---|---|---|
1 |
#include <stdio.h> |
|
2 |
#include <ColonetServer.h> |
|
3 |
|
|
4 |
ColonetServer colonet_server; |
|
5 |
|
|
6 |
/** |
|
7 |
* @brief The main function of the server |
|
8 |
* |
|
9 |
* @param argc The number of command line arguments passed to the program |
|
10 |
* @param argv The command line arguments passed to the program |
|
11 |
* |
|
12 |
* @return 0 on success, negative error code on error |
|
13 |
*/ |
|
14 |
int main(int argc, char** argv) { |
|
15 |
if (colonet_server.initialize_server(argc, argv) < 0) { |
|
16 |
fprintf(stderr, "\nThere was an error initializing the server.\n"); |
|
17 |
return -1; |
|
18 |
} |
|
19 |
|
|
20 |
if (colonet_server.start_listening() < 0) { |
|
21 |
fprintf(stderr, "ColonetServer.start_listening failed.\n"); |
|
22 |
return -1; |
|
23 |
} |
|
24 |
|
|
25 |
colonet_server.run_server(); |
|
26 |
|
|
27 |
return 0; |
|
28 |
} |
trunk/code/projects/colonet/ColonetServer/includes/ConnectionPool.h | ||
---|---|---|
1 |
/** |
|
2 |
* @author Jason Knichel |
|
3 |
* @date 7/22/07 |
|
4 |
*/ |
|
5 |
|
|
6 |
#ifndef CONNECTION_POOL_H |
|
7 |
#define CONNECTION_POOL_H |
|
8 |
|
|
9 |
#include <sys/select.h> |
|
10 |
|
|
11 |
#include <colonet_wireless.h> |
|
12 |
#include <ColonetServer.h> |
|
13 |
|
|
14 |
#define MAX_TOKENS 15 |
|
15 |
#define MAX_TOKEN_SIZE 30 |
|
16 |
|
|
17 |
#define ROBOT_COMMAND_OFFSET 1 |
|
18 |
#define ROBOT_COMMAND_LEN 3 |
|
19 |
|
|
20 |
#define ERROR_INVALID_CLIENT_DESCRIPTOR -1 |
|
21 |
#define ERROR_TOO_MANY_CLIENTS -2 |
|
22 |
#define ERROR_ALLOCATING_MEMORY -3 |
|
23 |
#define ERROR_NOT_ENOUGH_ROOM -4 |
|
24 |
#define ERROR_INVALID_COMMAND -5 |
|
25 |
#define ERROR_INVALID_CLIENT_ID -6 |
|
26 |
#define ERROR_INVALID_MESSAGE -7 |
|
27 |
#define ERROR_INVALID_MESSAGE_LENGTH -8 |
|
28 |
|
|
29 |
#define DECREMENT_INDEX_COUNTER 1 |
|
30 |
|
|
31 |
#define MAX_CONNECTIONS 250 |
|
32 |
#define READ_BUFFER_SIZE 1024 |
|
33 |
#define WRITE_BUFFER_SIZE 4096 |
|
34 |
|
|
35 |
class ColonetServer; |
|
36 |
|
|
37 |
class ConnectionPool { |
|
38 |
|
|
39 |
public: |
|
40 |
ConnectionPool(ColonetServer* cs); |
|
41 |
~ConnectionPool(); |
|
42 |
|
|
43 |
int add_client(int client_file_descriptor); |
|
44 |
int remove_client(int pool_index); |
|
45 |
int check_clients(); |
|
46 |
|
|
47 |
int write_to_client(int pool_index, char * message, int length); |
|
48 |
void add_new_socket_to_pool(int new_socket); |
|
49 |
int perform_select(int listen_socket); |
|
50 |
int is_socket_ready_to_read(int socket); |
|
51 |
int get_number_clients_ready(); |
|
52 |
|
|
53 |
private: |
|
54 |
int max_file_descriptor; |
|
55 |
int next_available_slot; |
|
56 |
int number_clients_ready; |
|
57 |
fd_set ready_set; |
|
58 |
fd_set read_set; |
|
59 |
fd_set write_set; |
|
60 |
int client_file_descriptor_array[MAX_CONNECTIONS]; |
|
61 |
char * read_buffer[MAX_CONNECTIONS]; |
|
62 |
int read_buffer_size[MAX_CONNECTIONS]; |
|
63 |
char * write_buffer[MAX_CONNECTIONS]; |
|
64 |
int write_buffer_size[MAX_CONNECTIONS]; |
|
65 |
ColonetServer* colonet_server; |
|
66 |
|
|
67 |
int read_data(int pool_index, int client_file_descriptor); |
|
68 |
int write_data(int pool_index, int client_file_descriptor); |
|
69 |
}; |
|
70 |
|
|
71 |
#endif |
trunk/code/projects/colonet/ColonetServer/includes/ColonetServer.h | ||
---|---|---|
1 |
/** |
|
2 |
* @author Jason Knichel |
|
3 |
* @date 9/10/07 |
|
4 |
*/ |
|
5 |
|
|
6 |
#ifndef COLONETSERVER_H |
|
7 |
#define COLONETSERVER_H |
|
8 |
|
|
9 |
class ConnectionPool; |
|
10 |
|
|
11 |
#include <ConnectionPool.h> |
|
12 |
#include <PositionMonitor.h> |
|
13 |
#include <Log.h> |
|
14 |
|
|
15 |
class ColonetServer { |
|
16 |
public: |
|
17 |
ColonetServer(); |
|
18 |
~ColonetServer(); |
|
19 |
|
|
20 |
int initialize_server(int argc, char * argv[]); |
|
21 |
int start_listening(void); |
|
22 |
int run_server(void); |
|
23 |
int process_received_wireless_message(int dest, char* data, int len); |
|
24 |
PositionMonitor* getPositionMonitor(void); |
|
25 |
|
|
26 |
private: |
|
27 |
ConnectionPool* connection_pool; |
|
28 |
Log* logger; |
|
29 |
int listen_socket; |
|
30 |
PositionMonitor position_monitor; |
|
31 |
|
|
32 |
int initialize_wireless(void); |
|
33 |
int initialize_connection(int port); |
|
34 |
}; |
|
35 |
|
|
36 |
#endif |
trunk/code/projects/colonet/ColonetServer/includes/options.h | ||
---|---|---|
1 |
/** |
|
2 |
* @file options.h |
|
3 |
* @brief Options for ColonetServer |
|
4 |
* @author Eugene Marinelli |
|
5 |
* @date 2/13/07 |
|
6 |
*/ |
|
7 |
|
|
8 |
#ifndef OPTIONS_H |
|
9 |
#define OPTIONS_H |
|
10 |
|
|
11 |
#define DEFAULTPORT 10123 |
|
12 |
#define SMALLEST_POSS_LISTEN_PORT 2000 |
|
13 |
|
|
14 |
typedef struct { |
|
15 |
int listen_port; |
|
16 |
char wireless_port[80]; |
|
17 |
char log_filename[80]; |
|
18 |
bool logging_enabled; |
|
19 |
bool listener_mode; |
|
20 |
} ColonetServerOptionsT; |
|
21 |
|
|
22 |
extern ColonetServerOptionsT optionsG; |
|
23 |
|
|
24 |
void options_parseCmdLine(int argc, char** argv); |
|
25 |
|
|
26 |
#endif |
trunk/code/projects/colonet/ColonetServer/includes/PositionMonitor.h | ||
---|---|---|
1 |
/** |
|
2 |
* @file PositionMonitor.h |
|
3 |
* |
|
4 |
* @author Jason Knichel |
|
5 |
* |
|
6 |
* @date 2/4/08 |
|
7 |
*/ |
|
8 |
|
|
9 |
#ifndef POSITIONMONITOR_H |
|
10 |
#define POSITIONMONITOR_H |
|
11 |
|
|
12 |
#include <map> |
|
13 |
#include <vision.h> |
|
14 |
using namespace std; |
|
15 |
|
|
16 |
#define MAX_POSITIONS 20 |
|
17 |
#define MAX_DISTANCE 5 |
|
18 |
|
|
19 |
using namespace std; |
|
20 |
|
|
21 |
class PositionMonitor { |
|
22 |
public: |
|
23 |
PositionMonitor(void); |
|
24 |
~PositionMonitor(void); |
|
25 |
|
|
26 |
int startMonitoring(void); |
|
27 |
int stopMonitoring(void); |
|
28 |
int updatePositions(void); |
|
29 |
|
|
30 |
int assignRealId(int old_id, int real_id); |
|
31 |
map<int, VisionPosition> getAllRobotPositions(void); |
|
32 |
int getRobotPosition(int robot_id, int* xbuf, int* ybuf); |
|
33 |
|
|
34 |
private: |
|
35 |
map<int, VisionPosition> positionMap; |
|
36 |
bool isProbablySameRobot(VisionPosition p1, VisionPosition p2); |
|
37 |
int newIdToAssign; |
|
38 |
}; |
|
39 |
|
|
40 |
#endif |
trunk/code/projects/colonet/ColonetServer/includes/Command.h | ||
---|---|---|
1 |
/** |
|
2 |
* @file Command.h |
|
3 |
* |
|
4 |
* @author Jason Knichel |
|
5 |
* @date 10/9/07 |
|
6 |
*/ |
|
7 |
|
|
8 |
#ifndef COMMAND_H |
|
9 |
#define COMMAND_H |
|
10 |
|
|
11 |
#include <ConnectionPool.h> |
|
12 |
#include <ColonetServer.h> |
|
13 |
#include <list> |
|
14 |
|
|
15 |
class Command { |
|
16 |
public: |
|
17 |
Command(ConnectionPool * connection_pool, ColonetServer* cs); |
|
18 |
~Command(); |
|
19 |
|
|
20 |
int parse_command(char* command, int pool_index); |
|
21 |
|
|
22 |
private: |
|
23 |
int tokenize_command(char* command, char tokens[MAX_TOKENS][MAX_TOKEN_SIZE]); |
|
24 |
int check_tokens(unsigned char* tokens, int number_tokens); |
|
25 |
|
|
26 |
int parse_send_to_robot(int number_tokens, char tokens[MAX_TOKENS][MAX_TOKEN_SIZE], int pool_index); |
|
27 |
int parse_request_from_server(int number_tokens, char tokens[MAX_TOKENS][MAX_TOKEN_SIZE], int pool_index); |
|
28 |
|
|
29 |
int parse_request_bom_matrix(int pool_index); |
|
30 |
int parse_request_xbee_ids(int pool_index); |
|
31 |
int parse_request_robot_positions(int pool_index); |
|
32 |
|
|
33 |
ConnectionPool * connection_pool; |
|
34 |
ColonetServer* colonet_server; |
|
35 |
}; |
|
36 |
|
|
37 |
|
|
38 |
#endif |
trunk/code/projects/colonet/ColonetServer/includes/colonet_wireless.h | ||
---|---|---|
1 |
/** @file colonet_wireless.h |
|
2 |
* |
|
3 |
* @brief Wireless library for communicating with colony robots |
|
4 |
* |
|
5 |
* @author Eugene Marinelli |
|
6 |
* @date 10/10/07 |
|
7 |
*/ |
|
8 |
|
|
9 |
#ifndef COLONET_WIRELESS_H_ |
|
10 |
#define COLONET_WIRELESS_H_ |
|
11 |
|
|
12 |
#include <colonet_defs.h> |
|
13 |
|
|
14 |
/** @brief Initializes colonet wireless library |
|
15 |
* |
|
16 |
* @param wl_port Either SERIAL_PORT or USB_PORT (as defined in COLONET_DEFS) |
|
17 |
* @param log_filename Path to log file - outputs all raw data on the network |
|
18 |
* If set to NULL, logging is disabled. |
|
19 |
* @param msg_handler Function to be called when a packet is received. |
|
20 |
* Must take a ColonetPacket as an argument (see defn of MsgHandlerFunction) |
|
21 |
* |
|
22 |
* @return new ColonetWireless object |
|
23 |
*/ |
|
24 |
int colonet_wl_init(char* wl_port, char* log_filename_); |
|
25 |
|
|
26 |
/** @brief Spawns a thread which reads data from the hardware interface |
|
27 |
* with the colony (either a dongle or a robot programmed to relay data) |
|
28 |
* and runs msg_handler when a full packet is received |
|
29 |
* |
|
30 |
* @return pointer to the thread |
|
31 |
*/ |
|
32 |
int colonet_wl_run_listener_thread(void); |
|
33 |
|
|
34 |
void colonet_wl_kill_listener_thread(void); |
|
35 |
|
|
36 |
/** |
|
37 |
* @brief ... |
|
38 |
* |
|
39 |
* @return void |
|
40 |
*/ |
|
41 |
int colonet_wl_send(short client_source, short dest, ColonetRobotMessageType msg_type, unsigned char msg_code, |
|
42 |
unsigned char* args); |
|
43 |
|
|
44 |
int colonet_get_num_robots(void); |
|
45 |
|
|
46 |
int** colonet_get_sensor_matrix(int* numrobots, int** ids); |
|
47 |
int* colonet_get_xbee_ids(int* numrobots); |
|
48 |
|
|
49 |
#endif |
trunk/code/projects/colonet/ColonetServer/includes/Log.h | ||
---|---|---|
1 |
/** |
|
2 |
* @file Log.h |
|
3 |
* |
|
4 |
* @author Jason Knichel |
|
5 |
* |
|
6 |
*/ |
|
7 |
|
|
8 |
#ifndef LOG_H |
|
9 |
#define LOG_H |
|
10 |
|
|
11 |
#include <time.h> |
|
12 |
#include <stdio.h> |
|
13 |
|
|
14 |
#define LOG_TYPE_START_LOG 0 |
|
15 |
#define LOG_TYPE_CONNECT 1 |
|
16 |
#define LOG_TYPE_DISCONNECT 2 |
|
17 |
#define LOG_TYPE_ERROR 3 |
|
18 |
#define LOG_TYPE_MESSAGE 4 |
|
19 |
|
|
20 |
#define LOG_MAX_TYPE_LENGTH 25 |
|
21 |
#define LOG_MAX_TIME_LENGTH 64 |
|
22 |
|
|
23 |
class Log { |
|
24 |
public: |
|
25 |
Log(char * fileName); |
|
26 |
~Log(void); |
|
27 |
|
|
28 |
int log_string(int type, char * message); |
|
29 |
int log_error(char * message); |
|
30 |
int log_message(char * message); |
|
31 |
|
|
32 |
private: |
|
33 |
struct tm get_current_time(); |
|
34 |
|
|
35 |
FILE * log_file; |
|
36 |
}; |
|
37 |
|
|
38 |
#endif |
trunk/code/projects/colonet/ColonetServer/ConnectionPool.cpp | ||
---|---|---|
1 |
/** |
|
2 |
* @file ConnectionPool.cpp |
|
3 |
* |
|
4 |
* @author Jason Knichel |
|
5 |
* @date 7/22/07 |
|
6 |
* |
|
7 |
*/ |
|
8 |
|
|
9 |
#include <sys/select.h> |
|
10 |
#include <ctype.h> |
|
11 |
#include <errno.h> |
|
12 |
#include <unistd.h> |
|
13 |
#include <string.h> |
|
14 |
#include <stdlib.h> |
|
15 |
#include <stdio.h> |
|
16 |
|
|
17 |
#include <ConnectionPool.h> |
|
18 |
#include <Command.h> |
|
19 |
#include <colonet_defs.h> |
|
20 |
|
|
21 |
#include <colonet_wireless.h> |
|
22 |
|
|
23 |
/** |
|
24 |
* @brief The default constructor for ConnectionPool |
|
25 |
*/ |
|
26 |
ConnectionPool::ConnectionPool(ColonetServer* cs) { |
|
27 |
colonet_server = cs; |
|
28 |
|
|
29 |
max_file_descriptor = 0; |
|
30 |
next_available_slot = 0; |
|
31 |
number_clients_ready = 0; |
|
32 |
|
|
33 |
FD_ZERO(&ready_set); |
|
34 |
FD_ZERO(&read_set); |
|
35 |
FD_ZERO(&write_set); |
|
36 |
|
|
37 |
memset(&client_file_descriptor_array, 0, sizeof(int)*MAX_CONNECTIONS); |
|
38 |
memset(&read_buffer, 0, sizeof(char *)*MAX_CONNECTIONS); |
|
39 |
memset(&read_buffer_size, 0, sizeof(int)*MAX_CONNECTIONS); |
|
40 |
memset(&write_buffer, 0, sizeof(char *)*MAX_CONNECTIONS); |
|
41 |
memset(&write_buffer_size, 0, sizeof(int)*MAX_CONNECTIONS); |
|
42 |
} |
|
43 |
|
|
44 |
/** |
|
45 |
* @brief The destructor for ConnectionPool |
|
46 |
*/ |
|
47 |
ConnectionPool::~ConnectionPool() { |
|
48 |
} |
|
49 |
|
|
50 |
/** |
|
51 |
* @brief Adds a client to the connection pool |
|
52 |
* |
|
53 |
* @param client_file_descriptor The file descriptor to add to the connection pool |
|
54 |
* |
|
55 |
* @return 0 on success, negative error code on failure |
|
56 |
*/ |
|
57 |
int ConnectionPool::add_client(int client_file_descriptor) { |
|
58 |
if (client_file_descriptor < 0) { |
|
59 |
return ERROR_INVALID_CLIENT_DESCRIPTOR; |
|
60 |
} |
|
61 |
|
|
62 |
if (next_available_slot == MAX_CONNECTIONS) { |
|
63 |
return ERROR_TOO_MANY_CLIENTS; |
|
64 |
} |
|
65 |
|
|
66 |
if (client_file_descriptor > max_file_descriptor) { |
|
67 |
max_file_descriptor = client_file_descriptor; |
|
68 |
} |
|
69 |
|
|
70 |
FD_SET(client_file_descriptor, &ready_set); |
|
71 |
|
|
72 |
int next_slot = next_available_slot; |
|
73 |
|
|
74 |
client_file_descriptor_array[next_slot] = client_file_descriptor; |
|
75 |
read_buffer[next_slot] = (char*) malloc(sizeof(char) * READ_BUFFER_SIZE); |
|
76 |
if (!(read_buffer[next_slot])) { |
|
77 |
return ERROR_ALLOCATING_MEMORY; |
|
78 |
} |
|
79 |
read_buffer_size[next_slot] = 0; |
|
80 |
write_buffer[next_slot] = (char *)malloc(sizeof(char) * WRITE_BUFFER_SIZE); |
|
81 |
|
|
82 |
if (!(write_buffer[next_slot])) { |
|
83 |
free(read_buffer[next_slot]); |
|
84 |
return ERROR_ALLOCATING_MEMORY; |
|
85 |
} |
|
86 |
|
|
87 |
write_buffer_size[next_slot] = 0; |
|
88 |
|
|
89 |
next_available_slot++; |
|
90 |
|
|
91 |
return 0; |
|
92 |
} |
|
93 |
|
|
94 |
/** |
|
95 |
* @brief Removes a client from the connection pool |
|
96 |
* |
|
97 |
* @param The index in the pool of the client to remove |
|
98 |
* |
|
99 |
* @return 0 on success, negative error code on failure |
|
100 |
*/ |
|
101 |
int ConnectionPool::remove_client(int pool_index) { |
|
102 |
if (pool_index < 0 || pool_index >= next_available_slot) { |
|
103 |
return ERROR_INVALID_CLIENT_DESCRIPTOR; |
|
104 |
} |
|
105 |
|
|
106 |
int client_file_descriptor = client_file_descriptor_array[pool_index]; |
|
107 |
|
|
108 |
if (FD_ISSET(client_file_descriptor, &ready_set)) { |
|
109 |
FD_CLR(client_file_descriptor, &ready_set); |
|
110 |
} |
|
111 |
if (FD_ISSET(client_file_descriptor, &read_set)) { |
|
112 |
FD_CLR(client_file_descriptor, &read_set); |
|
113 |
} |
|
114 |
if (FD_ISSET(client_file_descriptor, &write_set)) { |
|
115 |
FD_CLR(client_file_descriptor, &write_set); |
|
116 |
} |
|
117 |
|
|
118 |
free(read_buffer[pool_index]); |
|
119 |
free(write_buffer[pool_index]); |
|
120 |
for (int j = pool_index; j < next_available_slot - 1; j++) { |
|
121 |
client_file_descriptor_array[pool_index] = client_file_descriptor_array[pool_index+1]; |
|
122 |
read_buffer[pool_index] = read_buffer[pool_index+1]; |
|
123 |
read_buffer_size[pool_index] = read_buffer_size[pool_index+1]; |
|
124 |
write_buffer[pool_index] = write_buffer[pool_index+1]; |
|
125 |
write_buffer_size[pool_index] = write_buffer_size[pool_index+1]; |
|
126 |
} |
|
127 |
next_available_slot--; |
|
128 |
int temp_max_file_descriptor = 0; |
|
129 |
|
|
130 |
for (int j = 0; j < next_available_slot; j++) { |
|
131 |
if (client_file_descriptor_array[j] > temp_max_file_descriptor) |
|
132 |
temp_max_file_descriptor = client_file_descriptor_array[j]; |
|
133 |
} |
|
134 |
max_file_descriptor = temp_max_file_descriptor; |
|
135 |
|
|
136 |
printf("Removing client.\n"); |
|
137 |
|
|
138 |
return 0; |
|
139 |
} |
|
140 |
|
|
141 |
/** |
|
142 |
* @brief Checks the status of the clients |
|
143 |
* |
|
144 |
* Sees is any clients are ready to read from their file descriptor or are |
|
145 |
* ready to write to their file descriptor. |
|
146 |
* |
|
147 |
* @return 0 on success, negative error code on error |
|
148 |
*/ |
|
149 |
//TODO: test that it drops commands properly if it gets sent too much data |
|
150 |
// do we want it to drop the data or drop the connection? |
|
151 |
int ConnectionPool::check_clients() { |
|
152 |
int i; |
|
153 |
|
|
154 |
for (i = 0; i < next_available_slot; i++) { |
|
155 |
int client_file_descriptor = client_file_descriptor_array[i]; |
|
156 |
|
|
157 |
if (FD_ISSET(client_file_descriptor, &read_set)) { |
Also available in: Unified diff