root / trunk / code / projects / colonet / ColonetServer / Command.cpp @ 450
History | View | Annotate | Download (8.16 KB)
1 | 128 | jknichel | /**
|
---|---|---|---|
2 | * @file Command.cpp
|
||
3 | *
|
||
4 | * @author Jason Knichel
|
||
5 | * @date 10/9/07
|
||
6 | 144 | jknichel | *
|
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 | 128 | jknichel | */
|
12 | |||
13 | 156 | emarinel | #include <ctype.h> |
14 | #include <stdio.h> |
||
15 | 128 | jknichel | #include <stdlib.h> |
16 | #include <string.h> |
||
17 | 156 | emarinel | |
18 | 161 | emarinel | #include <colonet_wireless.h> |
19 | |||
20 | 391 | emarinel | #include <Command.h> |
21 | #include <ConnectionPool.h> |
||
22 | 447 | emarinel | #include <map> |
23 | #include <vision.h> |
||
24 | 128 | jknichel | |
25 | 447 | emarinel | using namespace std; |
26 | |||
27 | 139 | jknichel | Command::Command(ConnectionPool * connection_pool_temp) { |
28 | connection_pool = connection_pool_temp; |
||
29 | 128 | jknichel | } |
30 | |||
31 | 424 | emarinel | Command::~Command() {} |
32 | 128 | jknichel | |
33 | 424 | emarinel | /**
|
34 | * Called by connection pool to parse command from client.
|
||
35 | */
|
||
36 | 139 | jknichel | int Command::parse_command(char* command, int pool_index) { |
37 | 128 | jknichel | char tokens[MAX_TOKENS][MAX_TOKEN_SIZE];
|
38 | int number_tokens = 0; |
||
39 | char* end_pointer = NULL; |
||
40 | int command_id;
|
||
41 | |||
42 | 424 | emarinel | if (!connection_pool || !command || pool_index < 0) { |
43 | 139 | jknichel | return -1; |
44 | } |
||
45 | |||
46 | 128 | jknichel | if ((number_tokens = tokenize_command(command, tokens)) < 0) { |
47 | return -1; |
||
48 | } |
||
49 | |||
50 | //the 10 in the function call indicates number is base 10
|
||
51 | command_id = strtol(tokens[0], &end_pointer, 10); |
||
52 | |||
53 | if (!end_pointer || *end_pointer != '\0') { |
||
54 | printf("There was an error converting first token into a number.\n");
|
||
55 | return -1; |
||
56 | } |
||
57 | |||
58 | if (command_id == SEND_TO_ROBOT) {
|
||
59 | 156 | emarinel | if (parse_send_to_robot(number_tokens, tokens, pool_index)) {
|
60 | 424 | emarinel | fprintf(stderr, "parse_send_to_robot failed.\n");
|
61 | 129 | jknichel | return -1; |
62 | 128 | jknichel | } |
63 | } else if (command_id == REQUEST_FROM_SERVER) { |
||
64 | 139 | jknichel | if (parse_request_from_server(number_tokens, tokens, pool_index)) {
|
65 | 128 | jknichel | return -1; |
66 | } |
||
67 | } |
||
68 | |||
69 | return 0; |
||
70 | } |
||
71 | |||
72 | /**
|
||
73 | * @brief Breaks a command up into tokens
|
||
74 | *
|
||
75 | * @param command The command to tokenize
|
||
76 | * @param tokens A two dimensional character array to store the tokens in
|
||
77 | *
|
||
78 | * @return 0 on success, negative error code on failure
|
||
79 | */
|
||
80 | int Command::tokenize_command(char* command, char tokens[MAX_TOKENS][MAX_TOKEN_SIZE]) { |
||
81 | char* next_token = command;
|
||
82 | char* end_token = NULL; |
||
83 | int number_tokens = 0; |
||
84 | |||
85 | if (!command) {
|
||
86 | return -1; |
||
87 | } |
||
88 | |||
89 | while ((end_token = strstr(next_token, " "))) { |
||
90 | *end_token = '\0';
|
||
91 | |||
92 | if (strlen(next_token) > MAX_TOKEN_SIZE-1) { |
||
93 | return -1; |
||
94 | } |
||
95 | |||
96 | strcpy(tokens[number_tokens], next_token); |
||
97 | |||
98 | number_tokens++; |
||
99 | next_token = end_token + 1;
|
||
100 | |||
101 | while (isspace(*next_token) && *next_token != '\0') { |
||
102 | next_token++; |
||
103 | } |
||
104 | } |
||
105 | |||
106 | if (end_token == NULL && *next_token != '\0') { |
||
107 | if (strlen(next_token) > MAX_TOKEN_SIZE-1) { |
||
108 | return -1; |
||
109 | } |
||
110 | |||
111 | strcpy(tokens[number_tokens], next_token); |
||
112 | |||
113 | number_tokens++; |
||
114 | } |
||
115 | |||
116 | return number_tokens;
|
||
117 | } |
||
118 | |||
119 | 161 | emarinel | /**
|
120 | 128 | jknichel | * @brief checks a list of tokens to see if it's valid
|
121 | *
|
||
122 | * @param tokens The tokens to check
|
||
123 | * @param number_tokens The number of tokens contained in the tokens parameter
|
||
124 | *
|
||
125 | * @return 0 if tokens is valid
|
||
126 | */
|
||
127 | int Command::check_tokens(unsigned char* tokens, int number_tokens) { |
||
128 | if (number_tokens > 3 + PACKET_DATA_LEN) { |
||
129 | /* Too many tokens */
|
||
130 | return -1; |
||
131 | } |
||
132 | |||
133 | if (number_tokens < 3) { |
||
134 | /* Not enough tokens */
|
||
135 | return -1; |
||
136 | } |
||
137 | 161 | emarinel | |
138 | 128 | jknichel | if (tokens[1] != COLONET_REQUEST && tokens[1] != COLONET_COMMAND) { |
139 | /* Invalid message type */
|
||
140 | return -1; |
||
141 | } |
||
142 | |||
143 | return 0; |
||
144 | } |
||
145 | 129 | jknichel | |
146 | 424 | emarinel | /**
|
147 | * @brief Sends parsed command from server to robot(s).
|
||
148 | */
|
||
149 | int Command::parse_send_to_robot(int number_int_tokens, char tokens[MAX_TOKENS][MAX_TOKEN_SIZE], int pool_index) { |
||
150 | unsigned char int_tokens[MAX_TOKENS], arguments[PACKET_DATA_LEN]; |
||
151 | 129 | jknichel | |
152 | memset(arguments, 1, PACKET_DATA_LEN);
|
||
153 | |||
154 | 161 | emarinel | // Convert tokens to ints
|
155 | 424 | emarinel | for (int i = ROBOT_COMMAND_OFFSET; i < number_int_tokens; i++) { |
156 | 129 | jknichel | int_tokens[i-ROBOT_COMMAND_OFFSET] = atoi(tokens[i]); |
157 | } |
||
158 | |||
159 | 161 | emarinel | // Fill arguments buffer with arguments
|
160 | 424 | emarinel | for (int i = ROBOT_COMMAND_LEN; i < number_int_tokens-ROBOT_COMMAND_OFFSET; i++) { |
161 | 129 | jknichel | arguments[i-ROBOT_COMMAND_LEN] = int_tokens[i]; |
162 | } |
||
163 | |||
164 | 161 | emarinel | // Check the tokens
|
165 | 129 | jknichel | if (check_tokens(int_tokens, number_int_tokens) < 0) { |
166 | 151 | gtress | fprintf(stderr, "%s: Error - Invalid robot command/request.\n", __FUNCTION__);
|
167 | 129 | jknichel | return -1; |
168 | } |
||
169 | |||
170 | 424 | emarinel | printf("parsed command from internet client: ");
|
171 | for (int i = 0; i < number_int_tokens - ROBOT_COMMAND_OFFSET; i++) { |
||
172 | printf("%d ", int_tokens[i]);
|
||
173 | } |
||
174 | printf("\n");
|
||
175 | |||
176 | 161 | emarinel | // Send packet to robot
|
177 | 424 | emarinel | if (colonet_wl_send((short)pool_index, int_tokens[0], (ColonetRobotMessageType)int_tokens[1], int_tokens[2], |
178 | arguments) != 0) {
|
||
179 | 418 | emarinel | fprintf(stderr, "Error - Colonet_wl_send failed.\n");
|
180 | exit(1);
|
||
181 | 408 | emarinel | } |
182 | 156 | emarinel | |
183 | 129 | jknichel | return 0; |
184 | } |
||
185 | 130 | jknichel | |
186 | 424 | emarinel | /**
|
187 | *
|
||
188 | */
|
||
189 | 139 | jknichel | int Command::parse_request_from_server(int number_tokens, char tokens[MAX_TOKENS][MAX_TOKEN_SIZE], int pool_index) { |
190 | 130 | jknichel | char* end_pointer = NULL; |
191 | |||
192 | 139 | jknichel | if (!connection_pool) {
|
193 | return -1; |
||
194 | } |
||
195 | |||
196 | 156 | emarinel | if (number_tokens < 2) { |
197 | 130 | jknichel | return -1; |
198 | 156 | emarinel | } |
199 | 130 | jknichel | |
200 | end_pointer=NULL;
|
||
201 | int second_token = strtol(tokens[1], &end_pointer, 10); |
||
202 | |||
203 | if (!end_pointer || *end_pointer != '\0') { |
||
204 | printf("There was an error converting second token into a number.\n");
|
||
205 | return -1; |
||
206 | } |
||
207 | 161 | emarinel | |
208 | 445 | emarinel | switch (second_token) {
|
209 | case REQUEST_BOM_MATRIX:
|
||
210 | 139 | jknichel | if (parse_request_bom_matrix(pool_index)) {
|
211 | 131 | jknichel | return -1; |
212 | 130 | jknichel | } |
213 | 445 | emarinel | break;
|
214 | |||
215 | case REQUEST_XBEE_IDS:
|
||
216 | 139 | jknichel | if (parse_request_xbee_ids(pool_index)) {
|
217 | 131 | jknichel | return -1; |
218 | } |
||
219 | 445 | emarinel | break;
|
220 | |||
221 | case CLIENT_REQUEST_ROBOT_POSITIONS:
|
||
222 | if (parse_request_robot_positions(pool_index)) {
|
||
223 | return -1; |
||
224 | } |
||
225 | break;
|
||
226 | |||
227 | 450 | emarinel | case CLIENT_ASSIGN_POSITION_TO_ROBOT_ID:
|
228 | |||
229 | break;
|
||
230 | |||
231 | 445 | emarinel | default:
|
232 | 131 | jknichel | char * my_current_message = "Hi, how are you?\n"; |
233 | printf("Sending %s\n", my_current_message);
|
||
234 | connection_pool->write_to_client(pool_index, my_current_message, strlen(my_current_message)); |
||
235 | 445 | emarinel | break;
|
236 | 131 | jknichel | } |
237 | 130 | jknichel | |
238 | 131 | jknichel | return 0; |
239 | } |
||
240 | 130 | jknichel | |
241 | 139 | jknichel | int Command::parse_request_bom_matrix(int pool_index) { |
242 | 135 | jknichel | char response_bom_matrix_buffer[MAX_RESPONSE_LEN];
|
243 | 131 | jknichel | char temp_bom_matrix_buffer[MAX_RESPONSE_LEN];
|
244 | 161 | emarinel | |
245 | 139 | jknichel | if (!connection_pool) {
|
246 | return -1; |
||
247 | } |
||
248 | |||
249 | 297 | emarinel | int num_robots;
|
250 | 299 | jknichel | int * xbee_ids;
|
251 | int** bom_matrix = colonet_get_sensor_matrix(&num_robots, &xbee_ids);
|
||
252 | 130 | jknichel | |
253 | 297 | emarinel | printf("number of robots is %d\n", num_robots);
|
254 | 161 | emarinel | |
255 | 131 | jknichel | //TODO: make this better
|
256 | //TODO: make sure I don't need to do MAX_RESPONSE_LENGTH-1
|
||
257 | 297 | emarinel | snprintf(response_bom_matrix_buffer, MAX_RESPONSE_LEN, "%d %d %d", RESPONSE_TO_CLIENT_REQUEST, REQUEST_BOM_MATRIX,
|
258 | num_robots); |
||
259 | for (int i = 0; i < num_robots; i++) { |
||
260 | for (int j = 0; j < num_robots; j++) { |
||
261 | 131 | jknichel | //TODO: don't use strcpy
|
262 | 135 | jknichel | strcpy(temp_bom_matrix_buffer, response_bom_matrix_buffer); |
263 | 130 | jknichel | //TODO: put length checking in here so array doesn't go out of bounds
|
264 | //TODO: maybe use strncat?
|
||
265 | 131 | jknichel | strcat(temp_bom_matrix_buffer," %d");
|
266 | 297 | emarinel | snprintf(response_bom_matrix_buffer, MAX_RESPONSE_LEN, temp_bom_matrix_buffer, bom_matrix[i][j]); |
267 | 130 | jknichel | } |
268 | } |
||
269 | 135 | jknichel | strcat(response_bom_matrix_buffer,"\n");
|
270 | connection_pool->write_to_client(pool_index, response_bom_matrix_buffer, strlen(response_bom_matrix_buffer)); |
||
271 | printf("Sending %s", response_bom_matrix_buffer);
|
||
272 | 297 | emarinel | |
273 | for (int i = 0; i < num_robots; i++) { |
||
274 | free(bom_matrix[i]); |
||
275 | } |
||
276 | free(bom_matrix); |
||
277 | 130 | jknichel | |
278 | 299 | jknichel | free(xbee_ids); |
279 | |||
280 | 130 | jknichel | return 0; |
281 | } |
||
282 | 131 | jknichel | |
283 | 445 | emarinel | int Command::parse_request_robot_positions(int pool_index) { |
284 | printf("TODO - parse_request_robot_positions\n");
|
||
285 | 447 | emarinel | |
286 | map<int, VisionPosition> positions = colonet_server.getPositionMonitor()->getAllRobotPositions();
|
||
287 | |||
288 | 445 | emarinel | return 0; |
289 | } |
||
290 | |||
291 | 139 | jknichel | int Command::parse_request_xbee_ids(int pool_index) { |
292 | 297 | emarinel | char temp_xbee_id_buffer[MAX_RESPONSE_LEN];
|
293 | 131 | jknichel | char xbee_id_buffer[MAX_RESPONSE_LEN];
|
294 | 161 | emarinel | |
295 | 297 | emarinel | int num_robots;
|
296 | int* xbee_ids = colonet_get_xbee_ids(&num_robots);
|
||
297 | |||
298 | 418 | emarinel | printf("num_robots: %d\n", num_robots);
|
299 | printf("xbee_ids: ");
|
||
300 | for (int i = 0; i < num_robots; i++) { |
||
301 | printf("%d ", xbee_ids[i]);
|
||
302 | } |
||
303 | printf("\n");
|
||
304 | |||
305 | 139 | jknichel | if (!connection_pool) {
|
306 | return -1; |
||
307 | } |
||
308 | |||
309 | 297 | emarinel | snprintf(xbee_id_buffer, MAX_RESPONSE_LEN, "%d %d %d", RESPONSE_TO_CLIENT_REQUEST, REQUEST_XBEE_IDS, num_robots);
|
310 | 161 | emarinel | |
311 | 297 | emarinel | for (int i = 0; i < num_robots; i++) { |
312 | 131 | jknichel | strcpy(temp_xbee_id_buffer, xbee_id_buffer); |
313 | //TODO: put length checking in here so array doesn't go out of bounds
|
||
314 | //TODO: maybe use strncat?
|
||
315 | strcat(temp_xbee_id_buffer, " %d");
|
||
316 | 297 | emarinel | snprintf(xbee_id_buffer, MAX_RESPONSE_LEN, temp_xbee_id_buffer, xbee_ids[i]); |
317 | 131 | jknichel | } |
318 | strcat(xbee_id_buffer, "\n");
|
||
319 | connection_pool->write_to_client(pool_index, xbee_id_buffer, strlen(xbee_id_buffer)); |
||
320 | printf("Sending %s", xbee_id_buffer);
|
||
321 | |||
322 | 297 | emarinel | free(xbee_ids); |
323 | |||
324 | 131 | jknichel | return 0; |
325 | } |