mtp.c 24.6 KB
Newer Older
Timothy Stack's avatar
   
Timothy Stack committed
1

Timothy Stack's avatar
   
Timothy Stack committed
2
#include <errno.h>
David Johnson's avatar
David Johnson committed
3
#include <stdio.h>
Timothy Stack's avatar
Timothy Stack committed
4
#include <inttypes.h>
Timothy Stack's avatar
   
Timothy Stack committed
5
#include <string.h>
Timothy Stack's avatar
   
Timothy Stack committed
6
7
#include <stdlib.h>
#include <assert.h>
David Johnson's avatar
David Johnson committed
8
9
10
#include <unistd.h>
#include <netinet/in.h>

Timothy Stack's avatar
   
Timothy Stack committed
11
12
13
14
15
16
17
18
19
#include "mtp.h"

static int readall(int fd,char *buffer,unsigned int len)
{
  int rc, off = 0, retval;

  assert(fd >= 0);
  assert(buffer != NULL);

Timothy Stack's avatar
   
Timothy Stack committed
20
21
22
23
24
  while (((rc = read(fd,&buffer[off],len - off)) > 0) ||
	 ((rc == -1) && (errno == EINTR))) {
    if (rc > 0) {
      off += rc;
    }
Timothy Stack's avatar
   
Timothy Stack committed
25
26
27
28
29
  }

  if (rc < 0) {
    retval = rc;
  }
Timothy Stack's avatar
   
Timothy Stack committed
30
31
32
33
  else if (off == 0) {
    errno = ENOTCONN;
    retval = -1;
  }
Timothy Stack's avatar
   
Timothy Stack committed
34
35
36
37
  else if (off != len) {
    errno = EIO;
    retval = -1;
  }
Timothy Stack's avatar
   
Timothy Stack committed
38
39
40
41
42
43
  else {
    retval = off;
  }
  
  return retval;
}
David Johnson's avatar
David Johnson committed
44
45
46

int mtp_receive_packet(int fd,struct mtp_packet **packet) {
  unsigned int length;
Timothy Stack's avatar
   
Timothy Stack committed
47
  uint32_t clength;
David Johnson's avatar
David Johnson committed
48
49
50
51
  char *buf = NULL;
  int retval;

  if (packet == NULL) {
52
    return MTP_PP_ERROR_ARGS;
David Johnson's avatar
David Johnson committed
53
54
  }

Timothy Stack's avatar
   
Timothy Stack committed
55
  retval = readall(fd,(char *)&clength,4);
David Johnson's avatar
David Johnson committed
56
57

  if (retval == -1) {
58
    return MTP_PP_ERROR_READ;
David Johnson's avatar
David Johnson committed
59
60
  }
  else {
Timothy Stack's avatar
   
Timothy Stack committed
61
62
    int remaining_length;
    
63
    /* length is currently in bytes 0,1,2,and 3, in network byte order */
Timothy Stack's avatar
   
Timothy Stack committed
64
    length = ntohl(clength);
65
66
67
68
69
70

    if (length > MTP_PACKET_MAXLEN) {
      return MTP_PP_ERROR_LENGTH;
    }

    /* read the rest of the packet into buf */
Timothy Stack's avatar
   
Timothy Stack committed
71
    remaining_length = length - 4;
72
73
74
75
76
77
    buf = (char*)malloc(sizeof(char)*(length+1));
    if (buf == NULL) {
      return MTP_PP_ERROR_MALLOC;
    }

    // copy the header bytes to the new buf
Timothy Stack's avatar
   
Timothy Stack committed
78
    memcpy(buf,&clength,4);
79
    
Timothy Stack's avatar
   
Timothy Stack committed
80
    retval = readall(fd,&buf[MTP_PACKET_HEADER_OFFSET_OPCODE],remaining_length);
81
82
83
84
85
86
87
88
89
90
91
    if (retval == -1) {
      free(buf);
      return MTP_PP_ERROR_READ;
    }

    retval = mtp_decode_packet(buf,packet);
    free(buf);
    return retval;
  }

}
David Johnson's avatar
David Johnson committed
92

Timothy Stack's avatar
   
Timothy Stack committed
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
#define encode_float32(buf, idx, val) { \
  union { \
    float f; \
    unsigned int i; \
  } __z; \
\
  __z.f = val; \
  *((int *)((buf)+(idx))) = htonl(__z.i); \
  idx += 4; \
}

#define decode_float32(buf, idx, val) { \
  union { \
    float f; \
    unsigned int i; \
  } __z; \
\
  __z.i = ntohl(*((int *)((buf)+(idx)))); \
  val = __z.f; \
  idx += 4; \
}

115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
/* returns 0 if little, 1 if big */
int endian() {
  union {
	short x;
	char y[sizeof(short)];
  } u;
  u.x = 0x0102;
  if (u.y[0] == 2) {
	return 0;
  }
  else {
	return 1;
  }
}
  
#define encode_float64(buf, idx, val) { \
  union { \
    double d; \
    char c[sizeof(double)]; \
  } __z; \
  __z.d = val; \
  if (endian() == 1) { \
    memcpy((char *)((buf)+(idx)),__z.c,sizeof(double)); \
Timothy Stack's avatar
   
Timothy Stack committed
138
    idx += 8; \
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
  } \
  else { \
    *((int *)((buf)+(idx))) = htonl(*((int *)(__z.c + 4))); \
    idx += 4; \
    *((int *)((buf)+(idx))) = htonl(*((int *)(__z.c))); \
    idx += 4; \
  } \
}

#define decode_float64(buf, idx, val) { \
  union { \
    double d; \
    char c[sizeof(double)]; \
  } __z; \
\
  if (endian() == 1) { \
	memcpy((char *)(__z.c),(char *)((buf)+(idx)),sizeof(double)); \
	val = __z.d; \
Timothy Stack's avatar
   
Timothy Stack committed
157
    idx += 8; \
158
159
160
161
162
163
164
165
166
167
  } \
  else { \
    *((int *)(__z.c + 4)) = htonl(*((int *)((buf) + (idx)))); \
    idx += 4; \
    *((int *)(__z.c)) = htonl(*((int *)((buf) + (idx)))); \
    idx += 4; \
	val = __z.d; \
  } \
}

David Johnson's avatar
David Johnson committed
168
int mtp_encode_packet(char **buf_ptr,struct mtp_packet *packet) {
Timothy Stack's avatar
   
Timothy Stack committed
169
  char *buf = *buf_ptr;
Timothy Stack's avatar
   
Timothy Stack committed
170
  int i,j;
David Johnson's avatar
David Johnson committed
171
172
  int buf_size;

Timothy Stack's avatar
   
Timothy Stack committed
173
  if (packet == NULL || buf_ptr == NULL) {
174
    return MTP_PP_ERROR_ARGS;
David Johnson's avatar
David Johnson committed
175
176
177
178
179
180
181
  }

  // first we have to caculate how many bytes we need for this
  // specific data -- we could've defined a static size for things,
  // but we're encoding variable-length arrays and strings in addition
  // to ints and floats -- but we didn't -- so we have to skim through.

Timothy Stack's avatar
   
Timothy Stack committed
182
183
184
185
186
187
  if ((buf_size = mtp_calc_size(packet->opcode,
				(void *)packet->data.control)) == -1) {
    return MTP_PP_ERROR;
  }
  
  if (buf == NULL) {
188
189
190
191
192
    buf = (char *)malloc(sizeof(char)*buf_size);
    *buf_ptr = buf;
    if (buf == NULL) {
      return MTP_PP_ERROR_MALLOC;
    }
Timothy Stack's avatar
   
Timothy Stack committed
193
194
195
196
197
198
199
  }
  
  if (packet->opcode == MTP_CONTROL_ERROR ||
      packet->opcode == MTP_CONTROL_NOTIFY ||
      packet->opcode == MTP_CONTROL_INIT ||
      packet->opcode == MTP_CONTROL_CLOSE) {
    struct mtp_control *data = packet->data.control;
Timothy Stack's avatar
   
Timothy Stack committed
200
201
    int len;
    
202
203
204
205
206
207
208
209
210
211
212
213
    *((int *)buf) = htonl(buf_size);
    buf[MTP_PACKET_HEADER_OFFSET_OPCODE] = (char)(packet->opcode);
    buf[MTP_PACKET_HEADER_OFFSET_VERSION] = (char)(packet->version);
    buf[MTP_PACKET_HEADER_OFFSET_ROLE] = (char)(packet->role);

    i = MTP_PACKET_HEADER_LEN;
    // write id field
    *((int *)(buf+i)) = htonl(data->id);
    i += 4;
    // write code field
    *((int *)(buf+i)) = htonl(data->code);
    i += 4;
Timothy Stack's avatar
   
Timothy Stack committed
214
215
    // write msg field
    strcpy(&buf[i],data->msg);
Timothy Stack's avatar
   
Timothy Stack committed
216
217
    len = strlen(&buf[i]) + 1;
    i += (len + 3) & ~3;
David Johnson's avatar
David Johnson committed
218
219
  }
  else if (packet->opcode == MTP_CONFIG_RMC) {
220
221
222
223
224
225
    struct mtp_config_rmc *data = packet->data.config_rmc;
    
    *((int *)buf) = htonl(buf_size);
    buf[MTP_PACKET_HEADER_OFFSET_OPCODE] = (char)(packet->opcode);
    buf[MTP_PACKET_HEADER_OFFSET_VERSION] = (char)(packet->version);
    buf[MTP_PACKET_HEADER_OFFSET_ROLE] = (char)(packet->role);
David Johnson's avatar
David Johnson committed
226
227

    // now write the specific data:
228
229
230
231
232
    i = MTP_PACKET_HEADER_LEN;
    *((int *)(buf+i)) = htonl(data->num_robots);
    i += 4;
    // now write the list
    for (j = 0; j < data->num_robots; ++j) {
Timothy Stack's avatar
   
Timothy Stack committed
233
234
      int len;
      
235
236
237
238
      // write the id field
      *((int *)(buf+i)) = htonl(data->robots[j].id);
      i += 4;
      // write the hostname field
Timothy Stack's avatar
   
Timothy Stack committed
239
      strcpy(&buf[i],data->robots[j].hostname);
Timothy Stack's avatar
   
Timothy Stack committed
240
241
      len = strlen(&buf[i]) + 1;
      i += (len + 3) & ~3;
242
    }
David Johnson's avatar
David Johnson committed
243
    // now write the global_bound data
Timothy Stack's avatar
   
Timothy Stack committed
244
245
    encode_float32(buf, i, data->box.horizontal);
    encode_float32(buf, i, data->box.vertical);
David Johnson's avatar
David Johnson committed
246
247
  }
  else if (packet->opcode == MTP_CONFIG_VMC) {
248
249
250
251
252
253
    struct mtp_config_rmc *data = packet->data.config_rmc;
    
    *((int *)buf) = htonl(buf_size);
    buf[MTP_PACKET_HEADER_OFFSET_OPCODE] = (char)(packet->opcode);
    buf[MTP_PACKET_HEADER_OFFSET_VERSION] = (char)(packet->version);
    buf[MTP_PACKET_HEADER_OFFSET_ROLE] = (char)(packet->role);
David Johnson's avatar
David Johnson committed
254
255

    // now write the specific data:
256
257
258
259
260
    i = MTP_PACKET_HEADER_LEN;
    *((int *)(buf+i)) = htonl(data->num_robots);
    i += 4;
    // now write the list
    for (j = 0; j < data->num_robots; ++j) {
Timothy Stack's avatar
   
Timothy Stack committed
261
262
      int len;
      
263
264
265
266
      // write the id field
      *((int *)(buf+i)) = htonl(data->robots[j].id);
      i += 4;
      // write the hostname field
Timothy Stack's avatar
   
Timothy Stack committed
267
      strcpy(&buf[i],data->robots[j].hostname);
Timothy Stack's avatar
   
Timothy Stack committed
268
269
      len = strlen(&buf[i]) + 1;
      i += (len + 3) & ~3;
270
271
    }
    
David Johnson's avatar
David Johnson committed
272
273
  }
  else if (packet->opcode == MTP_REQUEST_POSITION) {
274
275
276
277
278
279
    struct mtp_request_position *data = packet->data.request_position;
    
    *((int *)buf) = htonl(buf_size);
    buf[MTP_PACKET_HEADER_OFFSET_OPCODE] = (char)(packet->opcode);
    buf[MTP_PACKET_HEADER_OFFSET_VERSION] = (char)(packet->version);
    buf[MTP_PACKET_HEADER_OFFSET_ROLE] = (char)(packet->role);
David Johnson's avatar
David Johnson committed
280
281

    // now write the specific data:
282
283
284
    i = MTP_PACKET_HEADER_LEN;
    *((int *)(buf+i)) = htonl(data->robot_id);
    i += 4;
David Johnson's avatar
David Johnson committed
285
286
287

  }
  else if (packet->opcode == MTP_REQUEST_ID) {
288
289
290
291
292
293
    struct mtp_request_id *data = packet->data.request_id;
    
    *((int *)buf) = htonl(buf_size);
    buf[MTP_PACKET_HEADER_OFFSET_OPCODE] = (char)(packet->opcode);
    buf[MTP_PACKET_HEADER_OFFSET_VERSION] = (char)(packet->version);
    buf[MTP_PACKET_HEADER_OFFSET_ROLE] = (char)(packet->role);
David Johnson's avatar
David Johnson committed
294
295

    // now write the specific data:
296
    i = MTP_PACKET_HEADER_LEN;
David Johnson's avatar
   
David Johnson committed
297
298
	*((int *)(buf+i)) = htonl(data->request_id);
    i += 4;
Timothy Stack's avatar
   
Timothy Stack committed
299
300
301
    encode_float32(buf, i, data->position.x);
    encode_float32(buf, i, data->position.y);
    encode_float32(buf, i, data->position.theta);
302
	encode_float64(buf, i, data->position.timestamp);
David Johnson's avatar
David Johnson committed
303
304
305

  }
  else if (packet->opcode == MTP_UPDATE_POSITION) {
306
307
308
309
310
311
    struct mtp_update_position *data = packet->data.update_position;
    
    *((int *)buf) = htonl(buf_size);
    buf[MTP_PACKET_HEADER_OFFSET_OPCODE] = (char)(packet->opcode);
    buf[MTP_PACKET_HEADER_OFFSET_VERSION] = (char)(packet->version);
    buf[MTP_PACKET_HEADER_OFFSET_ROLE] = (char)(packet->role);
David Johnson's avatar
David Johnson committed
312
313

    // now write the specific data:
314
315
316
    i = MTP_PACKET_HEADER_LEN;
    *((int *)(buf+i)) = htonl(data->robot_id);
    i += 4;
Timothy Stack's avatar
   
Timothy Stack committed
317
318
319
    encode_float32(buf, i, data->position.x);
    encode_float32(buf, i, data->position.y);
    encode_float32(buf, i, data->position.theta);
320
321
	encode_float64(buf, i, data->position.timestamp);

322
323
    *((int *)(buf+i)) = htonl(data->status);
    i += 4;
David Johnson's avatar
David Johnson committed
324
325
326

  }
  else if (packet->opcode == MTP_UPDATE_ID) {
327
328
329
330
331
332
    struct mtp_update_id *data = packet->data.update_id;
    
    *((int *)buf) = htonl(buf_size);
    buf[MTP_PACKET_HEADER_OFFSET_OPCODE] = (char)(packet->opcode);
    buf[MTP_PACKET_HEADER_OFFSET_VERSION] = (char)(packet->version);
    buf[MTP_PACKET_HEADER_OFFSET_ROLE] = (char)(packet->role);
David Johnson's avatar
David Johnson committed
333
334

    // now write the specific data:
335
    i = MTP_PACKET_HEADER_LEN;
David Johnson's avatar
   
David Johnson committed
336
337
	*((int *)(buf+i)) = htonl(data->request_id);
    i += 4;
338
339
    *((int *)(buf+i)) = htonl(data->robot_id);
    i += 4;
David Johnson's avatar
David Johnson committed
340
341
342

  }
  else if (packet->opcode == MTP_COMMAND_GOTO) {
343
344
345
346
347
348
    struct mtp_command_goto *data = packet->data.command_goto;
    
    *((int *)buf) = htonl(buf_size);
    buf[MTP_PACKET_HEADER_OFFSET_OPCODE] = (char)(packet->opcode);
    buf[MTP_PACKET_HEADER_OFFSET_VERSION] = (char)(packet->version);
    buf[MTP_PACKET_HEADER_OFFSET_ROLE] = (char)(packet->role);
David Johnson's avatar
David Johnson committed
349
350

    // now write the specific data:
351
352
353
354
355
    i = MTP_PACKET_HEADER_LEN;
    *((int *)(buf+i)) = htonl(data->command_id);
    i += 4;
    *((int *)(buf+i)) = htonl(data->robot_id);
    i += 4;
Timothy Stack's avatar
   
Timothy Stack committed
356
    
Timothy Stack's avatar
   
Timothy Stack committed
357
358
359
    encode_float32(buf, i, data->position.x);
    encode_float32(buf, i, data->position.y);
    encode_float32(buf, i, data->position.theta);
Timothy Stack's avatar
   
Timothy Stack committed
360
    encode_float64(buf, i, data->position.timestamp);
David Johnson's avatar
David Johnson committed
361
362
  }
  else if (packet->opcode == MTP_COMMAND_STOP) {
363
364
365
366
367
368
    struct mtp_command_stop *data = packet->data.command_stop;
    
    *((int *)buf) = htonl(buf_size);
    buf[MTP_PACKET_HEADER_OFFSET_OPCODE] = (char)(packet->opcode);
    buf[MTP_PACKET_HEADER_OFFSET_VERSION] = (char)(packet->version);
    buf[MTP_PACKET_HEADER_OFFSET_ROLE] = (char)(packet->role);
David Johnson's avatar
David Johnson committed
369
370

    // now write the specific data:
371
372
373
374
375
    i = MTP_PACKET_HEADER_LEN;
    *((int *)(buf+i)) = htonl(data->command_id);
    i += 4;
    *((int *)(buf+i)) = htonl(data->robot_id);
    i += 4;
David Johnson's avatar
David Johnson committed
376
377
378

  }
  else {
379
    return MTP_PP_ERROR;
David Johnson's avatar
David Johnson committed
380
381
382
383
384
385
386
387
  }
  
  // if we get here, the packet encoded successfully into buf:
  return buf_size;
}

int mtp_decode_packet(char *buf,struct mtp_packet **packet_ptr) {
  struct mtp_packet *packet;
Timothy Stack's avatar
   
Timothy Stack committed
388
  int i,j;
David Johnson's avatar
David Johnson committed
389
390
391
  int len;
  
  if (buf == NULL) {
392
    return MTP_PP_ERROR_ARGS;
David Johnson's avatar
David Johnson committed
393
394
395
396
397
  }

  packet = (struct mtp_packet *)malloc(sizeof(struct mtp_packet));
  *packet_ptr = packet;
  if (packet == NULL) {
398
    return MTP_PP_ERROR_MALLOC;
David Johnson's avatar
David Johnson committed
399
400
401
402
403
404
405
406
407
408
  }

  packet->length = ntohl(*((int*)(buf+MTP_PACKET_HEADER_OFFSET_LENGTH)));
  packet->opcode = (char)(buf[MTP_PACKET_HEADER_OFFSET_OPCODE]);
  packet->version = (char)(buf[MTP_PACKET_HEADER_OFFSET_VERSION]);
  packet->role = (char)(buf[MTP_PACKET_HEADER_OFFSET_ROLE]);

  i = MTP_PACKET_HEADER_LEN;
  // process data field
  if (packet->opcode == MTP_CONTROL_ERROR ||
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
      packet->opcode == MTP_CONTROL_NOTIFY ||
      packet->opcode == MTP_CONTROL_INIT ||
      packet->opcode == MTP_CONTROL_CLOSE) {
    struct mtp_control *data = (struct mtp_control *)malloc(sizeof(struct mtp_control));
    if (data == NULL) {
      return MTP_PP_ERROR_MALLOC;
    }
    packet->data.control = data;
    data->id = ntohl(*((int *)(buf+i)));
    i += 4;
    data->code = ntohl(*((int *)(buf+i)));
    i += 4;
    len = strlen(buf+i);
    data->msg = (char *)malloc(sizeof(char)*(len + 1));
    if (data->msg == NULL) {
      return MTP_PP_ERROR_MALLOC;
    }

    memcpy(data->msg,buf+i,len+1);
David Johnson's avatar
David Johnson committed
428
429
  }
  else if (packet->opcode == MTP_CONFIG_RMC) {
430
431
432
433
434
435
436
437
438
439
440
441
    struct mtp_config_rmc *data = (struct mtp_config_rmc *)malloc(sizeof(struct mtp_config_rmc));
    if (data == NULL) {
      return MTP_PP_ERROR_MALLOC;
    }
    packet->data.config_rmc = data;
    data->num_robots = ntohl(*((int *)(buf+i)));
    i += 4;
    data->robots = (struct robot_config *)malloc(sizeof(struct robot_config)*(data->num_robots));
    if (data->robots == NULL) {
      return MTP_PP_ERROR_MALLOC;
    }
    for (j = 0; j < data->num_robots; ++j) {
Timothy Stack's avatar
   
Timothy Stack committed
442
443
      int len;
      
444
445
      data->robots[j].id = ntohl(*((int *)(buf+i)));
      i += 4;
Timothy Stack's avatar
   
Timothy Stack committed
446
      len = strlen(buf+i) + 1;
447
448
      data->robots[j].hostname = (char *)malloc(sizeof(char)*len);
      memcpy(data->robots[j].hostname,buf+i,len);
Timothy Stack's avatar
   
Timothy Stack committed
449
      i += (len + 3) & ~3;
450
451
    }
    // write the global_coord box
Timothy Stack's avatar
   
Timothy Stack committed
452
453
    decode_float32(buf, i, data->box.horizontal);
    decode_float32(buf, i, data->box.vertical);
David Johnson's avatar
David Johnson committed
454
455
  }
  else if (packet->opcode == MTP_CONFIG_VMC) {
456
457
458
459
460
461
462
463
464
465
466
467
    struct mtp_config_vmc *data = (struct mtp_config_vmc *)malloc(sizeof(struct mtp_config_vmc));
    if (data == NULL) {
      return MTP_PP_ERROR_MALLOC;
    }
    packet->data.config_vmc = data;
    data->num_robots = ntohl(*((int *)(buf+i)));
    i += 4;
    data->robots = (struct robot_config *)malloc(sizeof(struct robot_config)*(data->num_robots));
    if (data->robots == NULL) {
      return MTP_PP_ERROR_MALLOC;
    }
    for (j = 0; j < data->num_robots; ++j) {
Timothy Stack's avatar
   
Timothy Stack committed
468
469
      int len;
      
470
471
      data->robots[j].id = ntohl(*((int *)(buf+i)));
      i += 4;
Timothy Stack's avatar
   
Timothy Stack committed
472
      len = strlen(buf+i) + 1;
473
474
      data->robots[j].hostname = (char *)malloc(sizeof(char)*len);
      memcpy(data->robots[j].hostname,buf+i,len);
Timothy Stack's avatar
   
Timothy Stack committed
475
      i += (len + 3) & ~3;
476
    }
David Johnson's avatar
David Johnson committed
477
478
479

  }
  else if (packet->opcode == MTP_REQUEST_POSITION) {
480
481
482
483
484
485
486
    struct mtp_request_position *data = (struct mtp_request_position *)malloc(sizeof(struct mtp_request_position));
    if (data == NULL) {
      return MTP_PP_ERROR_MALLOC;
    }
    packet->data.request_position = data;
    data->robot_id = ntohl(*((int *)(buf+i)));
    i += 4;
David Johnson's avatar
David Johnson committed
487
488
489

  }
  else if (packet->opcode == MTP_REQUEST_ID) {
490
491
492
493
494
    struct mtp_request_id *data = (struct mtp_request_id *)malloc(sizeof(struct mtp_request_id));
    if (data == NULL) {
      return MTP_PP_ERROR_MALLOC;
    }
    packet->data.request_id = data;
David Johnson's avatar
   
David Johnson committed
495

Timothy Stack's avatar
   
Timothy Stack committed
496
    data->request_id = ntohl(*((int *)(buf+i)));
David Johnson's avatar
   
David Johnson committed
497
    i += 4;
Timothy Stack's avatar
   
Timothy Stack committed
498
499
500
    decode_float32(buf, i, data->position.x);
    decode_float32(buf, i, data->position.y);
    decode_float32(buf, i, data->position.theta);
501
    decode_float64(buf, i, data->position.timestamp);
David Johnson's avatar
David Johnson committed
502
503
504

  }
  else if (packet->opcode == MTP_UPDATE_POSITION) {
505
506
507
508
509
510
511
    struct mtp_update_position *data = (struct mtp_update_position *)malloc(sizeof(struct mtp_update_position));
    if (data == NULL) {
      return MTP_PP_ERROR_MALLOC;
    }
    packet->data.update_position = data;
    data->robot_id = ntohl(*((int *)(buf+i)));
    i += 4;
Timothy Stack's avatar
   
Timothy Stack committed
512
513
514
    decode_float32(buf, i, data->position.x);
    decode_float32(buf, i, data->position.y);
    decode_float32(buf, i, data->position.theta);
515
	decode_float64(buf, i, data->position.timestamp);
516
517
    data->status = ntohl(*((int *)(buf+i)));
    i += 4;
David Johnson's avatar
David Johnson committed
518
519
520

  }
  else if (packet->opcode == MTP_UPDATE_ID) {
521
522
523
524
525
    struct mtp_update_id *data = (struct mtp_update_id *)malloc(sizeof(struct mtp_update_id));
    if (data == NULL) {
      return MTP_PP_ERROR_MALLOC;
    }
    packet->data.update_id = data;
David Johnson's avatar
   
David Johnson committed
526
527
528

	data->request_id = ntohl(*((int *)(buf+i)));
    i += 4;
529
530
    data->robot_id = ntohl(*((int *)(buf+i)));
    i += 4;
David Johnson's avatar
David Johnson committed
531
532
533

  }
  else if (packet->opcode == MTP_COMMAND_GOTO) {
534
535
536
537
538
539
540
541
542
    struct mtp_command_goto *data = (struct mtp_command_goto *)malloc(sizeof(struct mtp_command_goto));
    if (data == NULL) {
      return MTP_PP_ERROR_MALLOC;
    }
    packet->data.command_goto = data;
    data->command_id = ntohl(*((int *)(buf+i)));
    i += 4;
    data->robot_id = ntohl(*((int *)(buf+i)));
    i += 4;
Timothy Stack's avatar
   
Timothy Stack committed
543
544
545
    decode_float32(buf, i, data->position.x);
    decode_float32(buf, i, data->position.y);
    decode_float32(buf, i, data->position.theta);
546
	decode_float64(buf, i, data->position.timestamp);
David Johnson's avatar
David Johnson committed
547
548
549

  }
  else if (packet->opcode == MTP_COMMAND_STOP) {
550
551
552
553
554
555
556
557
558
    struct mtp_command_stop *data = (struct mtp_command_stop *)malloc(sizeof(struct mtp_command_stop));
    if (data == NULL) {
      return MTP_PP_ERROR_MALLOC;
    }
    packet->data.command_stop = data;
    data->command_id = ntohl(*((int *)(buf+i)));
    i += 4;
    data->robot_id = ntohl(*((int *)(buf+i)));
    i += 4;
David Johnson's avatar
David Johnson committed
559
560
561

  }
  else {
562
    return MTP_PP_ERROR;
David Johnson's avatar
David Johnson committed
563
564
565
566
567
568
569
  }

  return MTP_PP_SUCCESS;
  
}

int mtp_send_packet(int fd,struct mtp_packet *packet) {
Timothy Stack's avatar
   
Timothy Stack committed
570
  char *buf = NULL;
David Johnson's avatar
David Johnson committed
571
572
573
  int retval;

  if (packet == NULL) {
574
    return MTP_PP_ERROR;
David Johnson's avatar
David Johnson committed
575
576
  }

Timothy Stack's avatar
   
Timothy Stack committed
577
  retval = mtp_encode_packet(&buf,packet);
David Johnson's avatar
David Johnson committed
578
  if (retval < MTP_PP_SUCCESS) {
579
    return retval;
David Johnson's avatar
David Johnson committed
580
581
582
  }

  // now we can write the buffer out the socket.
Timothy Stack's avatar
   
Timothy Stack committed
583
  retval = write(fd,buf,retval);
Timothy Stack's avatar
   
Timothy Stack committed
584
585
586
587

  free(buf);
  buf = NULL;
  
David Johnson's avatar
David Johnson committed
588
  if (retval != -1) {
589
    return MTP_PP_SUCCESS;
David Johnson's avatar
David Johnson committed
590
591
  }
  else {
592
    return MTP_PP_ERROR_WRITE;
David Johnson's avatar
David Johnson committed
593
594
595
596
597
  }

}

struct mtp_packet *mtp_make_packet( unsigned char opcode,
598
599
600
601
				    unsigned char role,
				    /* data's type is only known by opcode */
				    void *data
				    ) {
David Johnson's avatar
David Johnson committed
602
603
604
605
  struct mtp_packet *retval = NULL;

  retval = (struct mtp_packet *)malloc(sizeof(struct mtp_packet));
  if (retval == NULL) {
606
    return NULL;
David Johnson's avatar
David Johnson committed
607
608
609
610
611
612
613
614
  }

  retval->opcode = opcode;
  retval->role = role;
  retval->length = 0;
  retval->version = MTP_VERSION;

  if (opcode == MTP_CONTROL_ERROR ||
615
616
617
618
619
      opcode == MTP_CONTROL_NOTIFY ||
      opcode == MTP_CONTROL_INIT ||
      opcode == MTP_CONTROL_CLOSE) {
    retval->data.control = (struct mtp_control *)data;
    retval->length = mtp_calc_size(opcode,data);
David Johnson's avatar
David Johnson committed
620
621
  }
  else if (opcode == MTP_CONFIG_RMC) {
622
623
    retval->data.config_rmc = (struct mtp_config_rmc *)data;
    retval->length = mtp_calc_size(opcode,data);
David Johnson's avatar
David Johnson committed
624
625
  }
  else if (opcode == MTP_CONFIG_VMC) {
626
627
    retval->data.config_vmc = (struct mtp_config_vmc *)data;
    retval->length = mtp_calc_size(opcode,data);
David Johnson's avatar
David Johnson committed
628
629
  }
  else if (opcode == MTP_REQUEST_POSITION) {
630
631
    retval->data.request_position = (struct mtp_request_position *)data;
    retval->length = mtp_calc_size(opcode,data);
David Johnson's avatar
David Johnson committed
632
633
  }
  else if (opcode == MTP_REQUEST_ID) {
634
635
    retval->data.request_id = (struct mtp_request_id *)data;
    retval->length = mtp_calc_size(opcode,data);
David Johnson's avatar
David Johnson committed
636
637
  }
  else if (opcode == MTP_UPDATE_POSITION) {
638
639
    retval->data.update_position = (struct mtp_update_position *)data;
    retval->length = mtp_calc_size(opcode,data);
David Johnson's avatar
David Johnson committed
640
641
  }
  else if (opcode == MTP_UPDATE_ID) {
642
643
    retval->data.update_id = (struct mtp_update_id *)data;
    retval->length = mtp_calc_size(opcode,data);
David Johnson's avatar
David Johnson committed
644
645
  }
  else if (opcode == MTP_COMMAND_GOTO) {
646
647
    retval->data.command_goto = (struct mtp_command_goto *)data;
    retval->length = mtp_calc_size(opcode,data);
David Johnson's avatar
David Johnson committed
648
649
  }
  else if (opcode == MTP_COMMAND_STOP) {
650
651
    retval->data.command_stop = (struct mtp_command_stop *)data;
    retval->length = mtp_calc_size(opcode,data);
David Johnson's avatar
David Johnson committed
652
653
  }
  else {
654
    return NULL;
David Johnson's avatar
David Johnson committed
655
656
657
658
659
  }

  return retval;
}

Timothy Stack's avatar
   
Timothy Stack committed
660
661
662
663
664
665
666
667
668
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
void mtp_free_packet(struct mtp_packet *mp) {
  if (mp != NULL) {
    int lpc;
    
    switch (mp->opcode) {
    case MTP_CONTROL_ERROR:
    case MTP_CONTROL_NOTIFY:
    case MTP_CONTROL_INIT:
    case MTP_CONTROL_CLOSE:
      free(mp->data.control->msg);
      break;

    case MTP_CONFIG_VMC:
      for (lpc = 0; lpc < mp->data.config_vmc->num_robots; lpc++) {
	free(mp->data.config_vmc->robots[lpc].hostname);
      }
      free(mp->data.config_vmc->robots);
      break;
      
    case MTP_CONFIG_RMC:
      for (lpc = 0; lpc < mp->data.config_rmc->num_robots; lpc++) {
	free(mp->data.config_rmc->robots[lpc].hostname);
      }
      free(mp->data.config_rmc->robots);
      break;
    }
    
    free(mp);
    mp = NULL;
  }
}
David Johnson's avatar
David Johnson committed
691
692
693

int mtp_calc_size(int opcode,void *data) {
  int retval = -1;
Timothy Stack's avatar
   
Timothy Stack committed
694
  int i;
David Johnson's avatar
David Johnson committed
695
696

  if (data == NULL) {
697
    return retval;
David Johnson's avatar
David Johnson committed
698
699
700
701
702
703
  }

  // standard header of one int and three chars
  retval = MTP_PACKET_HEADER_LEN;

  if (opcode == MTP_CONTROL_ERROR ||
704
705
706
707
      opcode == MTP_CONTROL_NOTIFY ||
      opcode == MTP_CONTROL_INIT ||
      opcode == MTP_CONTROL_CLOSE) {
    struct mtp_control *c = (struct mtp_control *)data;
Timothy Stack's avatar
   
Timothy Stack committed
708
709
    int len;
    
Timothy Stack's avatar
   
Timothy Stack committed
710
    assert(c->msg);
711
    retval += 8;
Timothy Stack's avatar
   
Timothy Stack committed
712
713
    len = strlen(c->msg) + 1;
    retval += (len + 3) & ~3;
David Johnson's avatar
David Johnson committed
714
715
  }
  else if (opcode == MTP_CONFIG_RMC) {
716
717
718
    struct mtp_config_rmc *c = (struct mtp_config_rmc *)data;
    retval += 4;
    for (i = 0; i < c->num_robots; ++i) {
Timothy Stack's avatar
   
Timothy Stack committed
719
720
      int len;
      
Timothy Stack's avatar
   
Timothy Stack committed
721
722
      assert(c->robots[i].hostname);
      
723
      retval += 4;
Timothy Stack's avatar
   
Timothy Stack committed
724
725
      len = strlen(c->robots[i].hostname) + 1;
      retval += (len + 3) & ~3;
726
727
    }
    retval += 8;
David Johnson's avatar
David Johnson committed
728
729
  }
  else if (opcode == MTP_CONFIG_VMC) {
730
731
732
    struct mtp_config_vmc *c = (struct mtp_config_vmc *)data;
    retval += 4;
    for (i = 0; i < c->num_robots; ++i) {
Timothy Stack's avatar
   
Timothy Stack committed
733
734
      int len;
      
Timothy Stack's avatar
   
Timothy Stack committed
735
736
      assert(c->robots[i].hostname);
      
737
      retval += 4;
Timothy Stack's avatar
   
Timothy Stack committed
738
739
      len = strlen(c->robots[i].hostname) + 1;
      retval += (len + 3) & ~3;
740
    }
David Johnson's avatar
David Johnson committed
741
742
  }
  else if (opcode == MTP_REQUEST_POSITION) {
743
744
    struct mtp_request_position *c = (struct mtp_request_position *)data;
    retval += 4;
David Johnson's avatar
David Johnson committed
745
746
  }
  else if (opcode == MTP_REQUEST_ID) {
747
    struct mtp_request_id *c = (struct mtp_request_id *)data;
David Johnson's avatar
   
David Johnson committed
748
    retval += 4 + 20;
David Johnson's avatar
David Johnson committed
749
750
  }
  else if (opcode == MTP_UPDATE_POSITION) {
751
    struct mtp_update_position *c = (struct mtp_update_position *)data;
752
    retval += 4 + 20 + 4;
David Johnson's avatar
David Johnson committed
753
754
  }
  else if (opcode == MTP_UPDATE_ID) {
755
    struct mtp_update_id *c = (struct mtp_update_id *)data;
David Johnson's avatar
   
David Johnson committed
756
    retval += 4 + 4;
David Johnson's avatar
David Johnson committed
757
758
  }
  else if (opcode == MTP_COMMAND_GOTO) {
759
    struct mtp_command_goto *c = (struct mtp_command_goto *)data;
760
    retval += 4 + 4 + 20;
David Johnson's avatar
David Johnson committed
761
762
  }
  else if (opcode == MTP_COMMAND_STOP) {
763
764
    struct mtp_command_stop *c = (struct mtp_command_stop *)data;
    retval += 4 + 4;
David Johnson's avatar
David Johnson committed
765
766
  }
  else {
767
    retval = -1;
David Johnson's avatar
David Johnson committed
768
769
770
771
772
  }
  
  return retval;

}
Timothy Stack's avatar
   
Timothy Stack committed
773
774
775
776
777
778
779
780
781
782
783
784
785
786
787
788
789
790
791
792
793
794
795
796
797
798
799
800
801
802
803
804
805
806
807
808
809
810
811
812
813
814
815
816
817
818
819
820
821
822
823
824
825
826
827
828
829
830
831
832
833
834
835
836
837
838
839
840
841
842
843
844
845
846
847
848
849
850
851
852
853
854
855
856
857

void mtp_print_packet(FILE *file, struct mtp_packet *mp)
{
  int lpc;
  
  fprintf(file,
	  "Packet: length %d; version %d; role %d\n",
	  mp->length,
	  mp->version,
	  mp->role);
  
  switch (mp->opcode) {
  case MTP_CONTROL_ERROR:
  case MTP_CONTROL_NOTIFY:
  case MTP_CONTROL_INIT:
  case MTP_CONTROL_CLOSE:
    switch (mp->opcode) {
    case MTP_CONTROL_ERROR:
      fprintf(file, " opcode:\terror\n");
      break;
    case MTP_CONTROL_NOTIFY:
      fprintf(file, " opcode:\tnotify\n");
      break;
    case MTP_CONTROL_INIT:
      fprintf(file, " opcode:\tinit\n");
      break;
    case MTP_CONTROL_CLOSE:
      fprintf(file, " opcode:\tclose\n");
      break;
    }
    fprintf(file,
	    "  id:\t\t%d\n"
	    "  code:\t\t%d\n"
	    "  msg:\t\t%s\n",
	    mp->data.control->id,
	    mp->data.control->code,
	    mp->data.control->msg);
    break;

  case MTP_CONFIG_VMC:
    fprintf(file,
	    " opcode:\tconfig-vmc\n"
	    "  num:\t\t%d\n",
	    mp->data.config_vmc->num_robots);
    for (lpc = 0;
	 lpc < mp->data.config_vmc->num_robots;
	 lpc++) {
      fprintf(file,
	      "  robot[%d]:\t%d, %s\n",
	      lpc,
	      mp->data.config_vmc->robots[lpc].id,
	      mp->data.config_vmc->robots[lpc].hostname);
    }
    break;
    
  case MTP_CONFIG_RMC:
    fprintf(file,
	    " opcode:\tconfig-rmc\n"
	    "  num:\t\t%d\n"
	    "  horiz:\t%f\n"
	    "  vert:\t\t%f\n",
	    mp->data.config_rmc->num_robots,
	    mp->data.config_rmc->box.horizontal,
	    mp->data.config_rmc->box.vertical);
    for (lpc = 0;
	 lpc < mp->data.config_rmc->num_robots;
	 lpc++) {
      fprintf(file,
	      "  robot[%d]:\t%d, %s\n",
	      lpc,
	      mp->data.config_rmc->robots[lpc].id,
	      mp->data.config_rmc->robots[lpc].hostname);
    }
    break;
    
  case MTP_REQUEST_POSITION:
    fprintf(file,
	    " opcode:\trequest-position\n"
	    "  id:\t\t%d\n",
	    mp->data.request_position->robot_id);
    break;
    
  case MTP_REQUEST_ID:
    fprintf(file,
	    " opcode:\trequest-id\n"
David Johnson's avatar
   
David Johnson committed
858
		"  request_id:\t\t%d\n"
Timothy Stack's avatar
   
Timothy Stack committed
859
860
861
	    "  x:\t\t%f\n"
	    "  y:\t\t%f\n"
	    "  theta:\t%f\n"
David Johnson's avatar
   
David Johnson committed
862
863
	    "  timestamp:\t\t%f\n",
		mp->data.request_id->request_id,
Timothy Stack's avatar
   
Timothy Stack committed
864
865
866
	    mp->data.request_id->position.x,
	    mp->data.request_id->position.y,
	    mp->data.request_id->position.theta,
867
	    mp->data.request_id->position.timestamp);
Timothy Stack's avatar
   
Timothy Stack committed
868
869
870
871
872
873
874
875
876
877
    break;
    
  case MTP_UPDATE_POSITION:
    fprintf(file,
	    " opcode:\tupdate-position\n"
	    "  id:\t\t%d\n"
	    "  x:\t\t%f\n"
	    "  y:\t\t%f\n"
	    "  theta:\t%f\n"
	    "  status:\t%d\n"
878
	    "  timestamp:\t%f\n",
Timothy Stack's avatar
   
Timothy Stack committed
879
880
881
882
883
	    mp->data.update_position->robot_id,
	    mp->data.update_position->position.x,
	    mp->data.update_position->position.y,
	    mp->data.update_position->position.theta,
	    mp->data.update_position->status,
884
	    mp->data.update_position->position.timestamp);
Timothy Stack's avatar
   
Timothy Stack committed
885
886
887
888
889
    break;
    
  case MTP_UPDATE_ID:
    fprintf(file,
	    " opcode:\tupdate-id\n"
Timothy Stack's avatar
   
Timothy Stack committed
890
	    "  request_id:\t%d\n"
Timothy Stack's avatar
   
Timothy Stack committed
891
	    "  id:\t%d\n",
Timothy Stack's avatar
   
Timothy Stack committed
892
893
	    mp->data.update_id->request_id,
	    mp->data.update_id->robot_id);
Timothy Stack's avatar
   
Timothy Stack committed
894
895
896
897
898
899
900
901
902
    break;
    
  case MTP_COMMAND_GOTO:
    fprintf(file,
	    " opcode:\tcommand-goto\n"
	    "  commid:\t%d\n"
	    "  id:\t\t%d\n"
	    "  x:\t\t%f\n"
	    "  y:\t\t%f\n"
903
904
	    "  theta:\t%f\n"
        "  timestamp:\t%f\n",
Timothy Stack's avatar
   
Timothy Stack committed
905
906
907
908
	    mp->data.command_goto->command_id,
	    mp->data.command_goto->robot_id,
	    mp->data.command_goto->position.x,
	    mp->data.command_goto->position.y,
909
	    mp->data.command_goto->position.theta,
Timothy Stack's avatar
   
Timothy Stack committed
910
	    mp->data.command_goto->position.timestamp);
Timothy Stack's avatar
   
Timothy Stack committed
911
912
913
914
915
916
917
918
919
920
921
922
923
924
925
926
    break;
    
  case MTP_COMMAND_STOP:
    fprintf(file,
	    " opcode:\tupdate-id\n"
	    "  commid:\t%d\n"
	    "  id:\t%d\n",
	    mp->data.command_stop->command_id,
	    mp->data.command_stop->robot_id);
    break;

  default:
    assert(0);
    break;
  }
}