Statistics
| Revision:

root / trunk / code / projects / colonet / server / Command.cpp @ 768

History | View | Annotate | Download (12.7 KB)

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
#include <wireless.h>
20

    
21
#include <Command.h>
22
#include <ConnectionPool.h>
23
#include <map>
24
#include <vision.h>
25

    
26
using namespace std;
27

    
28
/**
29
 * @brief Constructor for the Command class
30
 * 
31
 * @param connection_pool_temp The connection pool this instance of the Command class is supposed to use
32
 * @param cs The ColonetServer this instance of the Command class is supposed to use
33
 */
34
Command::Command(ConnectionPool * connection_pool_temp, ColonetServer* cs) {
35
  connection_pool = connection_pool_temp;
36
  colonet_server = cs;
37
}
38

    
39
/**
40
 * @brief Destructor for the Command class
41
 */
42
Command::~Command() {}
43

    
44
/**
45
 * @brief Called by connection pool to parse command from client.
46
 *
47
 * @param command The command to parse
48
 * @param pool_index The index in the connection pool of the client who generated this command
49
 *
50
 * @return 0 on success, negative error code on failure
51
 */
52
int Command::parse_command(char* command, int pool_index) {
53
  char tokens[MAX_TOKENS][MAX_TOKEN_SIZE];
54
  int number_tokens = 0;
55
  char* end_pointer = NULL;
56
  int command_id;
57

    
58
  if (!connection_pool || !command || pool_index < 0) {
59
    return -1;
60
  }
61

    
62
  if ((number_tokens = tokenize_command(command, tokens)) < 0) {
63
    return -1;
64
  }
65

    
66
  //the 10 in the function call indicates number is base 10
67
  command_id = strtol(tokens[0], &end_pointer, 10);
68

    
69
  if (!end_pointer || *end_pointer != '\0') {
70
    printf("There was an error converting first token into a number.\n");
71
    return -1;
72
  }
73

    
74
  if (command_id == SEND_TO_ROBOT) {
75
    if (parse_send_to_robot(number_tokens, tokens, pool_index)) {
76
      fprintf(stderr, "parse_send_to_robot failed.\n");
77
      return -1;
78
    }
79
  } else if (command_id == REQUEST_FROM_SERVER) {
80
    if (parse_request_from_server(number_tokens, tokens, pool_index)) {
81
      return -1;
82
    }
83
  }
84

    
85
  return 0;
86
}
87

    
88
/**
89
 * @brief Breaks a command up into tokens
90
 *
91
 * @param command The command to tokenize
92
 * @param tokens A two dimensional character array to store the tokens in
93
 *
94
 * @return 0 on success, negative error code on failure
95
 */
96
int Command::tokenize_command(char* command, char tokens[MAX_TOKENS][MAX_TOKEN_SIZE]) {
97
  char* next_token = command;
98
  char* end_token = NULL;
99
  int number_tokens = 0;
100

    
101
  if (!command) {
102
    return -1;
103
  }
104

    
105
  //look for the end of the next token (tokens are separated by spaces)
106
  while ((end_token = strstr(next_token, " "))) {
107
    //set the location of the space to a null terminator
108
    *end_token = '\0';
109

    
110
    //make sure the token isn't too long
111
    if (strlen(next_token) > MAX_TOKEN_SIZE-1) {
112
      return -1;
113
    }
114

    
115
    //copy the next token into the token array
116
    strcpy(tokens[number_tokens], next_token);
117

    
118
    number_tokens++;
119

    
120
    next_token = end_token + 1;
121

    
122
    //skip over extra whitespace if there is any between tokens
123
    while (isspace(*next_token) && *next_token != '\0') {
124
      next_token++;
125
    }
126
  }
127

    
128
  //get the last token if there were no spaces at the end of the string to indicate an end of token
129
  if (end_token == NULL && *next_token != '\0') {
130
    if (strlen(next_token) > MAX_TOKEN_SIZE-1) {
131
      return -1;
132
    }
133

    
134
    strcpy(tokens[number_tokens], next_token);
135

    
136
    number_tokens++;
137
  }
138

    
139
  return number_tokens;
140
}
141

    
142
/**
143
 * @brief checks a list of tokens to see if it's valid
144
 *
145
 * @param tokens The tokens to check
146
 * @param number_tokens The number of tokens contained in the tokens parameter
147
 *
148
 * @return 0 if tokens is valid
149
 */
150
int Command::check_tokens(unsigned char* tokens, int number_tokens) {
151
  if (number_tokens > 3 + PACKET_DATA_LEN) {
152
    /* Too many tokens */
153
    return -1;
154
  }
155

    
156
  if (number_tokens < 3) {
157
    /* Not enough tokens */
158
    return -1;
159
  }
160

    
161
  if (tokens[1] != COLONET_REQUEST && tokens[1] != COLONET_COMMAND) {
162
    /* Invalid message type */
163
    return -1;
164
  }
165

    
166
  return 0;
167
}
168

    
169
//TODO: fill in the doxygen comments for this
170
/**
171
* @brief Sends parsed command from server to robot(s).
172
*
173
* @param number_int_tokens 
174
* @param tokens
175
* @param pool_index
176
*
177
* @return 0 on success, negative error code on failure
178
*/
179
int Command::parse_send_to_robot(int number_int_tokens, char tokens[MAX_TOKENS][MAX_TOKEN_SIZE], int pool_index) {
180
  unsigned char int_tokens[MAX_TOKENS], arguments[PACKET_DATA_LEN];
181

    
182
  /* GREG'S FILTHY HACK */
183
  /*   PLEASE REMOVE    */
184
  int cmd = atoi(tokens[3]);
185
  int bot = atoi(tokens[1]);
186
  if (cmd == 96) {
187
    wl_set_channel(0xE);
188
    wl_send_robot_to_robot_global_packet(3, 9, NULL, 0, bot, 0);
189
    printf("Sent recharge request to %d on channel 0xE\n", bot);
190
    wl_set_channel(0xC);
191
    return 0;
192
  }
193
  if (cmd == 97) {
194
    wl_set_channel(0xE);
195
    wl_send_robot_to_robot_global_packet(3, 10, NULL, 0, bot, 0);
196
    printf("Sent recharge stop request to %d\n", bot);
197
    wl_set_channel(0xC);
198
    return 0;
199
  }
200
  /* END OF HACK */
201

    
202
  memset(arguments, 1, PACKET_DATA_LEN);
203

    
204
  // Convert tokens to ints
205
  for (int i = ROBOT_COMMAND_OFFSET; i < number_int_tokens; i++) {
206
    int_tokens[i-ROBOT_COMMAND_OFFSET] = atoi(tokens[i]);
207
  }
208

    
209
  // Fill arguments buffer with arguments
210
  for (int i = ROBOT_COMMAND_LEN; i < number_int_tokens-ROBOT_COMMAND_OFFSET; i++) {
211
    arguments[i-ROBOT_COMMAND_LEN] = int_tokens[i];
212
  }
213

    
214
  // Check the tokens
215
  if (check_tokens(int_tokens, number_int_tokens) < 0) {
216
    fprintf(stderr, "%s: Error - Invalid robot command/request.\n", __FUNCTION__);
217
    return -1;
218
  }
219

    
220
  //printf("parsed command from internet client: ");
221
  for (int i = 0; i < number_int_tokens - ROBOT_COMMAND_OFFSET; i++) {
222
    printf("%d ", int_tokens[i]);
223
  }
224
  printf("\n");
225

    
226
  // Send packet to robot
227
  if (colonet_wl_send((short)pool_index, int_tokens[0], (ColonetRobotMessageType)int_tokens[1], int_tokens[2],
228
                      arguments) != 0) {
229
    fprintf(stderr, "Error - Colonet_wl_send failed.\n");
230
    exit(1);
231
  }
232

    
233
  return 0;
234
}
235

    
236
/**
237
 * @brief This function parses a request where a client is asking for something from the server
238
 *
239
 * @param number_tokens Number of tokens in the tokens array
240
 * @param tokens The parsed form of the command
241
 * @param pool_index The index in the connection pool of the client who sent the command
242
 *
243
 * @return 0 on success, negative error code on failure
244
 */
245
int Command::parse_request_from_server(int number_tokens, char tokens[MAX_TOKENS][MAX_TOKEN_SIZE], int pool_index) {
246
  char* end_pointer = NULL;
247

    
248
        //make sure a connection pool is set
249
  if (!connection_pool) {
250
    return -1;
251
  }
252

    
253
        //make sure you enough enough tokens to be a valid command
254
  if (number_tokens < 2) {
255
    return -1;
256
  }
257

    
258
        //convert the second token to an integer
259
  end_pointer=NULL;
260
  int second_token = strtol(tokens[1], &end_pointer, 10);
261
  if (!end_pointer || *end_pointer != '\0') {
262
    printf("There was an error converting second token into a number.\n");
263
    return -1;
264
  }
265

    
266
  int up_x, up_y, low_x, low_y;
267

    
268
        //figure out which command was sent
269
  switch (second_token) {
270
        case REQUEST_BOM_MATRIX:
271
                if (parse_request_bom_matrix(pool_index)) {
272
      return -1;
273
    }
274
    break;
275

    
276
  case REQUEST_XBEE_IDS:
277
    if (parse_request_xbee_ids(pool_index)) {
278
      return -1;
279
    }
280
    break;
281

    
282
  case CLIENT_REQUEST_ROBOT_POSITIONS:
283
    if (parse_request_robot_positions(pool_index)) {
284
      return -1;
285
    }
286
    break;
287

    
288
  case CLIENT_ASSIGN_ROBOT_ID:
289
    colonet_server->getPositionMonitor()->assignRealId(atoi(tokens[2]), atoi(tokens[3]));
290
    break;
291

    
292
  case CLIENT_SET_VIRTUAL_WALL:
293
    up_x = atoi(tokens[2]);
294
    up_y = atoi(tokens[3]);
295
    low_x = atoi(tokens[4]);
296
    low_y = atoi(tokens[5]);
297

    
298
    if (colonet_server->getVirtualWall()->set_coordinates(up_x, up_y, low_x, low_y) == 0) {
299
      fprintf(stderr, "Wall set to ((%d, %d), (%d, %d)).\n", up_x, up_y, low_x, low_y);
300
    } else {
301
      fprintf(stderr, "Set wall failed ((%d, %d), (%d, %d)).\n", up_x, up_y, low_x, low_y);
302
    }
303
    break;
304

    
305
  default:
306
    char* my_current_message = "Invalid request!\n";
307
    //printf("Sending %s\n", my_current_message);
308
    connection_pool->write_to_client(pool_index, my_current_message, strlen(my_current_message));
309
    break;
310
  }
311

    
312
  return 0;
313
}
314

    
315
/**
316
 * @brief This function parses a client's request for the bom matrix as far as the wireless library knows it.
317
 *
318
 * @param pool_index The index in the connection pool of the client who sent the command
319
 *
320
 * @return 0 on success, negative error code on failure
321
 */
322
int Command::parse_request_bom_matrix(int pool_index) {
323
  char response_bom_matrix_buffer[MAX_RESPONSE_LEN];
324
  char temp_bom_matrix_buffer[MAX_RESPONSE_LEN];
325

    
326
  if (!connection_pool) {
327
    return -1;
328
  }
329

    
330
  int num_robots;
331
  int * xbee_ids;
332
        //get the sensor matrix from the wireless library
333
  int** bom_matrix = colonet_get_sensor_matrix(&num_robots, &xbee_ids);
334

    
335
  printf("number of robots is %d\n", num_robots);
336

    
337
        //copy the information over into a linear array to send to the client
338
  //TODO: make this better
339
  //TODO: make sure I don't need to do MAX_RESPONSE_LENGTH-1
340
  snprintf(response_bom_matrix_buffer, MAX_RESPONSE_LEN, "%d %d %d", RESPONSE_TO_CLIENT_REQUEST, REQUEST_BOM_MATRIX,
341
           num_robots);
342
  for (int i = 0; i < num_robots; i++) {
343
    for (int j = 0; j < num_robots; j++) {
344
      //TODO: don't use strcpy
345
      strcpy(temp_bom_matrix_buffer, response_bom_matrix_buffer);
346
      //TODO: put length checking in here so array doesn't go out of bounds
347
      //TODO: maybe use strncat?
348
      strcat(temp_bom_matrix_buffer," %d");
349
      snprintf(response_bom_matrix_buffer, MAX_RESPONSE_LEN, temp_bom_matrix_buffer, bom_matrix[i][j]);
350
    }
351
  }
352
  strcat(response_bom_matrix_buffer,"\n");
353

    
354
        //send the bom matrix to the client that requested it
355
  connection_pool->write_to_client(pool_index, response_bom_matrix_buffer, strlen(response_bom_matrix_buffer));
356
  //printf("Sending %s", response_bom_matrix_buffer);
357
  
358
  for (int i = 0; i < num_robots; i++) {
359
    free(bom_matrix[i]);
360
  }
361
  free(bom_matrix);
362

    
363
  free(xbee_ids);
364

    
365
  return 0;
366
}
367

    
368
/**
369
 * @brief This functions parses a client's request for the positions of robots
370
 * 
371
 * @param pool_index The index in the connection pool of the client that sent this command
372
 *
373
 * @return 0 on success, negative error code on failure
374
 */
375
int Command::parse_request_robot_positions(int pool_index) {
376
  //printf("*****parse_request_robot_positions\n");
377

    
378
        //ask the position monitor for where it sees robots
379
  map<int, VisionPosition> positions = colonet_server->getPositionMonitor()->getAllRobotPositions();
380

    
381
  map<int, VisionPosition>::iterator iter;
382

    
383
  char position_buffer[256];
384
  position_buffer[0] = 0;
385
  sprintf(position_buffer, "%d %d", RESPONSE_TO_CLIENT_REQUEST, CLIENT_REQUEST_ROBOT_POSITIONS);
386
 
387
  for (iter = positions.begin(); iter != positions.end(); iter++) {
388
    char tmpbuf[80];
389
    sprintf(tmpbuf, " %d %d %d", iter->first, iter->second.x, iter->second.y);
390
    strcat(position_buffer, tmpbuf);
391
  }
392

    
393
  strcat(position_buffer, "\n");
394

    
395
  //printf("position buffer is: %s\n", position_buffer);
396

    
397
        //send the positions of the robots to the client that requested them
398
  connection_pool->write_to_client(pool_index, position_buffer, strlen(position_buffer));
399

    
400
  return 0;
401
}
402

    
403
/**
404
 * @brief This functions parses a client's request for the ids of the xbees in the token ring
405
 *
406
 * @param pool_index The index in the connection pool of the client that requested the xbee ids
407
 *
408
 * @return 0 on success, negative error code on failure
409
 */
410
int Command::parse_request_xbee_ids(int pool_index) {
411
  char temp_xbee_id_buffer[MAX_RESPONSE_LEN];
412
  char xbee_id_buffer[MAX_RESPONSE_LEN];
413

    
414
  int num_robots;
415

    
416
        //get a list of the xbee ids
417
  int* xbee_ids = colonet_get_xbee_ids(&num_robots);
418

    
419
  //printf("num_robots: %d\n", num_robots);
420
  //printf("xbee_ids: ");
421
  //for (int i = 0; i < num_robots; i++) {
422
  //  printf("%d ", xbee_ids[i]);
423
  //}
424
  //printf("\n");
425

    
426
  if (!connection_pool) {
427
    return -1;
428
  }
429

    
430
  snprintf(xbee_id_buffer, MAX_RESPONSE_LEN, "%d %d %d", RESPONSE_TO_CLIENT_REQUEST, REQUEST_XBEE_IDS, num_robots);
431

    
432
        //copy over the ids into a response to send to the client
433
  for (int i = 0; i < num_robots; i++) {
434
    strcpy(temp_xbee_id_buffer, xbee_id_buffer);
435
    //TODO: put length checking in here so array doesn't go out of bounds
436
    //TODO: maybe use strncat?
437
    strcat(temp_xbee_id_buffer, " %d");
438
    snprintf(xbee_id_buffer, MAX_RESPONSE_LEN, temp_xbee_id_buffer, xbee_ids[i]);
439
  }
440
  strcat(xbee_id_buffer, "\n");
441

    
442
        //send the list of xbee ides to the client that requested them
443
  connection_pool->write_to_client(pool_index, xbee_id_buffer, strlen(xbee_id_buffer));
444
  //printf("Sending %s", xbee_id_buffer);
445

    
446
  free(xbee_ids);
447

    
448
  return 0;
449
}