Project

General

Profile

Statistics
| Revision:

root / trunk / code / projects / colonet / ColonetServer / Command.cpp @ 408

History | View | Annotate | Download (7.39 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

    
20
#include <Command.h>
21
#include <ConnectionPool.h>
22

    
23
Command::Command(ConnectionPool * connection_pool_temp) {
24
  connection_pool = connection_pool_temp;
25
}
26

    
27
Command::~Command() {
28
}
29

    
30
int Command::parse_command(char* command, int pool_index) {
31
  char tokens[MAX_TOKENS][MAX_TOKEN_SIZE];
32
  int number_tokens = 0;
33
  char* end_pointer = NULL;
34
  int command_id;
35

    
36
  if (!connection_pool) {
37
    return -1;
38
  }
39

    
40
  if (!command) {
41
    return -1;
42
  }
43

    
44
  if (pool_index < 0) {
45
    return -1;
46
  }
47

    
48
  if ((number_tokens = tokenize_command(command, tokens)) < 0) {
49
    return -1;
50
  }
51

    
52
  //the 10 in the function call indicates number is base 10
53
  command_id = strtol(tokens[0], &end_pointer, 10);
54

    
55
  if (!end_pointer || *end_pointer != '\0') {
56
    printf("There was an error converting first token into a number.\n");
57
    return -1;
58
  }
59

    
60
  if (command_id == SEND_TO_ROBOT) {
61
    if (parse_send_to_robot(number_tokens, tokens, pool_index)) {
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
/**
122
 * @brief checks a list of tokens to see if it's valid
123
 *
124
 * @param tokens The tokens to check
125
 * @param number_tokens The number of tokens contained in the tokens parameter
126
 *
127
 * @return 0 if tokens is valid
128
 */
129
int Command::check_tokens(unsigned char* tokens, int number_tokens) {
130
  if (number_tokens > 3 + PACKET_DATA_LEN) {
131
    /* Too many tokens */
132
    return -1;
133
  }
134

    
135
  if (number_tokens < 3) {
136
    /* Not enough tokens */
137
    return -1;
138
  }
139

    
140
  if (tokens[1] != COLONET_REQUEST && tokens[1] != COLONET_COMMAND) {
141
    /* Invalid message type */
142
    return -1;
143
  }
144

    
145
  return 0;
146
}
147

    
148
int Command::parse_send_to_robot(int number_tokens, char tokens[MAX_TOKENS][MAX_TOKEN_SIZE], int pool_index) {
149
  int i;
150
  unsigned char int_tokens[MAX_TOKENS];
151
  int number_int_tokens = number_tokens;
152
  unsigned char arguments[PACKET_DATA_LEN];
153

    
154
  memset(arguments, 1, PACKET_DATA_LEN);
155

    
156
  // Convert tokens to ints
157
  for (i = ROBOT_COMMAND_OFFSET; i < number_int_tokens; i++) {
158
    int_tokens[i-ROBOT_COMMAND_OFFSET] = atoi(tokens[i]);
159
  }
160

    
161
  // Fill arguments buffer with arguments
162
  for (i = ROBOT_COMMAND_LEN; i < number_int_tokens-ROBOT_COMMAND_OFFSET; i++) {
163
    arguments[i-ROBOT_COMMAND_LEN] = int_tokens[i];
164
  }
165

    
166
  // Check the tokens
167
  if (check_tokens(int_tokens, number_int_tokens) < 0) {
168
    fprintf(stderr, "%s: Error - Invalid robot command/request.\n", __FUNCTION__);
169
    return -1;
170
  }
171

    
172
  // Send packet to robot
173
  fprintf(stderr, "Calling colonet_wl_send(%d, %d, %d, arguments)\n",
174
          int_tokens[0], int_tokens[1], int_tokens[2]);
175
  if (colonet_wl_send((short)pool_index, int_tokens[0], (ColonetMessageType)int_tokens[1], int_tokens[2], arguments)
176
      != 0) {
177
    fprintf(stderr, "Error - Colonet_wl_send failed.");
178
    return -1;
179
  }
180

    
181
  return 0;
182
}
183

    
184
int Command::parse_request_from_server(int number_tokens, char tokens[MAX_TOKENS][MAX_TOKEN_SIZE], int pool_index) {
185
  char* end_pointer = NULL;
186

    
187
  if (!connection_pool) {
188
    return -1;
189
  }
190

    
191
  if (number_tokens < 2) {
192
    return -1;
193
  }
194

    
195
  end_pointer=NULL;
196
  int second_token = strtol(tokens[1], &end_pointer, 10);
197

    
198
  if (!end_pointer || *end_pointer != '\0') {
199
    printf("There was an error converting second token into a number.\n");
200
    return -1;
201
  }
202

    
203
  if (second_token == REQUEST_BOM_MATRIX) {
204
    if (parse_request_bom_matrix(pool_index)) {
205
      return -1;
206
    }
207
  } else if (second_token == REQUEST_XBEE_IDS) {
208
    if (parse_request_xbee_ids(pool_index)) {
209
      return -1;
210
    }
211
  } else {
212
    char * my_current_message = "Hi, how are you?\n";
213
    printf("Sending %s\n", my_current_message);
214
    connection_pool->write_to_client(pool_index, my_current_message, strlen(my_current_message));
215
  }
216

    
217
  return 0;
218
}
219

    
220
int Command::parse_request_bom_matrix(int pool_index) {
221
  char response_bom_matrix_buffer[MAX_RESPONSE_LEN];
222
  char temp_bom_matrix_buffer[MAX_RESPONSE_LEN];
223

    
224
  if (!connection_pool) {
225
    return -1;
226
  }
227

    
228
  int num_robots;
229
  int * xbee_ids;
230
  int** bom_matrix = colonet_get_sensor_matrix(&num_robots, &xbee_ids);
231

    
232
  printf("number of robots is %d\n", num_robots);
233

    
234
  //TODO: make this better
235
  //TODO: make sure I don't need to do MAX_RESPONSE_LENGTH-1
236
  snprintf(response_bom_matrix_buffer, MAX_RESPONSE_LEN, "%d %d %d", RESPONSE_TO_CLIENT_REQUEST, REQUEST_BOM_MATRIX,
237
           num_robots);
238
  for (int i = 0; i < num_robots; i++) {
239
    for (int j = 0; j < num_robots; j++) {
240
      //TODO: don't use strcpy
241
      strcpy(temp_bom_matrix_buffer, response_bom_matrix_buffer);
242
      //TODO: put length checking in here so array doesn't go out of bounds
243
      //TODO: maybe use strncat?
244
      strcat(temp_bom_matrix_buffer," %d");
245
      snprintf(response_bom_matrix_buffer, MAX_RESPONSE_LEN, temp_bom_matrix_buffer, bom_matrix[i][j]);
246
    }
247
  }
248
  strcat(response_bom_matrix_buffer,"\n");
249
  connection_pool->write_to_client(pool_index, response_bom_matrix_buffer, strlen(response_bom_matrix_buffer));
250
  printf("Sending %s", response_bom_matrix_buffer);
251
  
252
  for (int i = 0; i < num_robots; i++) {
253
    free(bom_matrix[i]);
254
  }
255
  free(bom_matrix);
256

    
257
  free(xbee_ids);
258

    
259
  return 0;
260
}
261

    
262
int Command::parse_request_xbee_ids(int pool_index) {
263
  char temp_xbee_id_buffer[MAX_RESPONSE_LEN];
264
  char xbee_id_buffer[MAX_RESPONSE_LEN];
265

    
266
  int num_robots;
267
  int* xbee_ids = colonet_get_xbee_ids(&num_robots);
268

    
269
  if (!connection_pool) {
270
    return -1;
271
  }
272

    
273
  snprintf(xbee_id_buffer, MAX_RESPONSE_LEN, "%d %d %d", RESPONSE_TO_CLIENT_REQUEST, REQUEST_XBEE_IDS, num_robots);
274

    
275
  for (int i = 0; i < num_robots; i++) {
276
    strcpy(temp_xbee_id_buffer, xbee_id_buffer);
277
    //TODO: put length checking in here so array doesn't go out of bounds
278
    //TODO: maybe use strncat?
279
    strcat(temp_xbee_id_buffer, " %d");
280
    snprintf(xbee_id_buffer, MAX_RESPONSE_LEN, temp_xbee_id_buffer, xbee_ids[i]);
281
  }
282
  strcat(xbee_id_buffer, "\n");
283
  connection_pool->write_to_client(pool_index, xbee_id_buffer, strlen(xbee_id_buffer));
284
  printf("Sending %s", xbee_id_buffer);
285

    
286
  free(xbee_ids);
287

    
288
  return 0;
289
}