emcd.c 43.8 KB
Newer Older
David Johnson's avatar
David Johnson committed
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36

/* emc sets up an interface for accepting commands over an internet socket;
 * listens on a well-known port for instances of vmc and rmc;
 * and maintains several key data structures.
 */

/* so emc reads config data, sets up data structures, listens on main port
 * every connecting client is configured or rejected, then is placed in
 * a working set to select() on.  Then commands are processed from either a 
 * file, or from an active user interface; requests/data are processed from
 * RMC and VMC, and appropriate responses are generated if necessary (whether
 * the response be to the requesting client, or to somebody else (i.e., if 
 * VMC pushes a position update, EMC does not need to respond to VMC -- but it
 * might need to push data to RMC!)
 */

// so we need to store a bunch of data for data from RMC, VMC, and initial
// all this data can reuse the same data structure, which includes a list of
//   robot id
//   position
//   orientation
//   status

// for the initial configuration of rmc, we need to send a list of id -> 
// hostname pairs (later we will also need to send a bounding box, but not
// until the vision system is a long ways along).

// for the initial configuration of vmc, we need to send a list of robot ids,
// nothing more (as the vision system starts picking up robots, it will query
// emc with known positions/orientations to get matching ids)

// so the emc config file is a bunch of lines with id number, hostname, initial
// position (x, y, and r floating point vals).  We read this file into the 
// initial position data structure, as well as a list of id to hostname
// mappings.

Timothy Stack's avatar
   
Timothy Stack committed
37
38
#include "config.h"

David Johnson's avatar
David Johnson committed
39
#include <stdio.h>
Timothy Stack's avatar
Timothy Stack committed
40
#include <math.h>
Timothy Stack's avatar
   
Timothy Stack committed
41
#include <assert.h>
David Johnson's avatar
David Johnson committed
42
#include <sys/types.h>
Timothy Stack's avatar
   
Timothy Stack committed
43
#include <sys/stat.h>
David Johnson's avatar
David Johnson committed
44
45
46
#include <unistd.h>
#include <stdlib.h>
#include <signal.h>
Timothy Stack's avatar
   
Timothy Stack committed
47
#include <paths.h>
David Johnson's avatar
David Johnson committed
48
#include <netinet/in.h>
Timothy Stack's avatar
   
Timothy Stack committed
49
#include <arpa/inet.h>
David Johnson's avatar
David Johnson committed
50
51
#include <errno.h>
#include <string.h>
Timothy Stack's avatar
   
Timothy Stack committed
52
#include <netdb.h>
David Johnson's avatar
David Johnson committed
53
54
#include <sys/socket.h>

Timothy Stack's avatar
   
Timothy Stack committed
55
56
57
58
#include "log.h"
#include "tbdefs.h"
#include "event.h"
#include <elvin/elvin.h>
David Johnson's avatar
David Johnson committed
59
60
61
62

#include "robot_list.h"
#include "mtp.h"

Timothy Stack's avatar
   
Timothy Stack committed
63
#include "emcd.h"
David Johnson's avatar
David Johnson committed
64

65
66
67
68
#ifndef __XSTRING
#define __XSTRING(x) __STRING(x)
#endif

Timothy Stack's avatar
   
Timothy Stack committed
69
static event_handle_t handle;
David Johnson's avatar
David Johnson committed
70

Timothy Stack's avatar
   
Timothy Stack committed
71
static char *pideid;
David Johnson's avatar
David Johnson committed
72

Timothy Stack's avatar
   
Timothy Stack committed
73
static char *progname;
David Johnson's avatar
David Johnson committed
74

Timothy Stack's avatar
   
Timothy Stack committed
75
static int debug;
David Johnson's avatar
David Johnson committed
76

Timothy Stack's avatar
   
Timothy Stack committed
77
78
static int port = EMC_SERVER_PORT;
static int command_seq_no = 0;
David Johnson's avatar
David Johnson committed
79
80
81
82
83
84
85
86
87
88
89
90
91

/* initial data pulled from the config file */
/* it's somewhat non-intuitive to use lists here -- since we're doing maps,
 * we could just as well use hashtables -- but speed is not a big deal here,
 * and linked lists might be easier for everybody to understand
 */
/* the robot_id/hostname pairings as given in the config file */
struct robot_list *hostname_list = NULL;
/* the robot_id/initial positions as given in the config file */
struct robot_list *initial_position_list = NULL;
/* a list of positions read in from the movement file */
struct robot_list *position_queue = NULL;

Timothy Stack's avatar
   
Timothy Stack committed
92
93
static elvin_error_t elvin_error;

Timothy Stack's avatar
   
Timothy Stack committed
94
95
96
static struct mtp_packet config_rmc;
static struct mtp_packet config_vmc;

97
98
99
100
101
102
103
104
105
106
/* 
 * these are for some global bounds checking on user-requested
 * positions
 */
static struct camera_config *g_camera_config;
int g_camera_config_size = 0;
static struct obstacle_config *g_obstacle_config;
int g_obstacle_config_size = 0;


Timothy Stack's avatar
   
Timothy Stack committed
107
108
109
static struct rmc_client rmc_data;
static struct vmc_client vmc_data;
static mtp_handle_t emulab_handle = NULL;
Timothy Stack's avatar
   
Timothy Stack committed
110

111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130

/* 
 * returns 1 if x,y are inside the union of the boxes defined
 * in the camera_config struct; 0 if not.
 */
static int position_in_camera(struct camera_config *cc,
			      int cc_size,
			      float x,
			      float y
			      );

/*
 * returns 0 if the position is not in a known obstacle; 1 if it is.
 */
static int position_in_obstacle(struct obstacle_config *oc,
				int oc_size,
				float x,
				float y
				);

Timothy Stack's avatar
   
Timothy Stack committed
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
static void ev_callback(event_handle_t handle,
			event_notification_t notification,
			void *data);

static int acceptor_callback(elvin_io_handler_t handler,
			     int fd,
			     void *rock,
			     elvin_error_t eerror);

static int unknown_client_callback(elvin_io_handler_t handler,
				   int fd,
				   void *rock,
				   elvin_error_t eerror);

static int rmc_callback(elvin_io_handler_t handler,
			int fd,
			void *rock,
			elvin_error_t eerror);

static int emulab_callback(elvin_io_handler_t handler,
			   int fd,
			   void *rock,
			   elvin_error_t eerror);

static int vmc_callback(elvin_io_handler_t handler,
			int fd,
			void *rock,
			elvin_error_t eerror);

Timothy Stack's avatar
   
Timothy Stack committed
160
161
162
163
static int update_callback(elvin_timeout_t timeout,
			   void *rock,
			   elvin_error_t eerror);

Timothy Stack's avatar
   
Timothy Stack committed
164
165
166
static void parse_config_file(char *filename);
static void parse_movement_file(char *filename);

167
168
static char *unix_path = NULL;

Timothy Stack's avatar
   
Timothy Stack committed
169
170
171
172
173
#if defined(SIGINFO)
/* SIGINFO-related stuff */

/**
 * Variable used to tell the main loop that we received a SIGINFO.
David Johnson's avatar
David Johnson committed
174
 */
Timothy Stack's avatar
   
Timothy Stack committed
175
static int got_siginfo = 0;
David Johnson's avatar
David Johnson committed
176

Timothy Stack's avatar
   
Timothy Stack committed
177
178
179
180
181
182
183
184
185
186
187
188
/**
 * Handler for SIGINFO that sets the got_siginfo variable and breaks the main
 * loop so we can really handle the signal.
 *
 * @param sig The actual signal number received.
 */
static void siginfo(int sig)
{
  got_siginfo = 1;
  if (handle->do_loop)
    event_stop_main(handle);
}
David Johnson's avatar
David Johnson committed
189

Timothy Stack's avatar
   
Timothy Stack committed
190
191
static void dump_info(void)
{
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
  if (vmc_data.position_list == NULL) {
    info("info: vision is not connected\n");
  }
  else {
    struct mtp_update_position *mup;
    struct robot_list_enum *e;

    info("info: vision position list\n");
    
    e = robot_list_enum(vmc_data.position_list);
    while ((mup = (struct mtp_update_position *)
	    robot_list_enum_next_element(e)) != NULL) {
      struct emc_robot_config *erc;
      struct mtp_packet mp;
      
      erc = robot_list_search(hostname_list, mup->robot_id);
      info("  %s: %.2f %.2f %.2f\n",
	   erc->hostname,
	   mup->position.x,
	   mup->position.y,
	   mup->position.theta);
    }
    robot_list_enum_destroy(e);
  }
Timothy Stack's avatar
   
Timothy Stack committed
216
217
218
}
#endif

Timothy Stack's avatar
   
Timothy Stack committed
219
220
221
222
223
224
225
226
227
228
229
static void emcd_atexit(void)
{
  if (unix_path != NULL)
    unlink(unix_path);
}

static void sigquit(int sig)
{
  exit(0);
}

Timothy Stack's avatar
   
Timothy Stack committed
230
231
232
233
234
235
236
237
238
239
static void sigpanic(int sig)
{
  char buf[32];

  sprintf(buf, "sigpanic %d\n", sig);
  write(1, buf, strlen(buf));
  sleep(120);
  exit(1);
}

Timothy Stack's avatar
   
Timothy Stack committed
240
241
242
static void usage(char *progname)
{
  fprintf(stderr,
Timothy Stack's avatar
   
Timothy Stack committed
243
244
245
246
247
248
249
	  "Usage: emcd [-hd] [-c config] [-e pid/eid] [-k keyfile] [...]\n"
	  "Options:\n"
	  "  -h\t\tPrint this message.\n"
	  "  -d\t\tTurn on debugging messages and do not daemonize\n"
	  "  -e pid/eid\tConnect to the experiment event server\n"
	  "  -k keyfile\tKeyfile for the experiment\n"
	  "  -p port\tPort to listen on for mtp messages\n"
Timothy Stack's avatar
   
Timothy Stack committed
250
	  "  -U path\tUnix socket path\n"
Timothy Stack's avatar
   
Timothy Stack committed
251
252
253
	  "  -c config\tThe config file to use\n"
	  "  -l logfile\tThe log file to use\n"
	  "  -i pidfile\tThe file to write the PID to\n");
Timothy Stack's avatar
   
Timothy Stack committed
254
255
256
257
258
}

int main(int argc, char *argv[])
{
  char pid[MAXHOSTNAMELEN], eid[MAXHOSTNAMELEN];
Timothy Stack's avatar
   
Timothy Stack committed
259
  int c, emc_sock, retval = EXIT_SUCCESS;
Timothy Stack's avatar
   
Timothy Stack committed
260
261
262
263
264
265
266
267
268
  char *server = "localhost";
  char *movement_file = NULL;
  char *config_file = NULL;
  elvin_io_handler_t eih;
  address_tuple_t tuple;
  char *keyfile = NULL;
  char *logfile = NULL;
  char *pidfile = NULL;
  char *eport = NULL;
Timothy Stack's avatar
   
Timothy Stack committed
269
  char buf[BUFSIZ], buf2[BUFSIZ];
Timothy Stack's avatar
   
Timothy Stack committed
270
271
  char *idx;
  FILE *fp;
Timothy Stack's avatar
   
Timothy Stack committed
272

Timothy Stack's avatar
   
Timothy Stack committed
273
274
  progname = argv[0];
  
Timothy Stack's avatar
   
Timothy Stack committed
275
  while ((c = getopt(argc, argv, "hde:k:p:c:f:s:P:U:l:i:")) != -1) {
Timothy Stack's avatar
   
Timothy Stack committed
276
277
    switch (c) {
    case 'h':
Timothy Stack's avatar
   
Timothy Stack committed
278
279
      usage(progname);
      exit(0);
Timothy Stack's avatar
   
Timothy Stack committed
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
      break;
    case 'd':
      debug += 1;
      break;
    case 'e':
      pideid = optarg;
      if ((idx = strchr(pideid, '/')) == NULL) {
	fprintf(stderr,
		"error: malformed pid/eid argument - %s\n",
		pideid);
	usage(progname);
      }
      else if ((idx - pideid) >= sizeof(pid)) {
	fprintf(stderr,
		"error: pid is too long - %s\n",
		pideid);
	usage(progname);
      }
      else if (strlen(idx + 1) >= sizeof(eid)) {
	fprintf(stderr,
		"error: eid is too long - %s\n",
		pideid);
	usage(progname);
      }
      else {
	strncpy(pid, pideid, (idx - pideid));
	pid[idx - pideid] = '\0';
	strcpy(eid, idx + 1);
      }
      break;
    case 'k':
      keyfile = optarg;
      break;
    case 'p':
      if (sscanf(optarg, "%d", &port) != 1) {
	fprintf(stderr, "error: illegible port: %s\n", optarg);
	exit(1);
      }
      break;
Timothy Stack's avatar
   
Timothy Stack committed
319
320
321
    case 'U':
      unix_path = optarg;
      break;
Timothy Stack's avatar
   
Timothy Stack committed
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
    case 'c':
      config_file = optarg;
      break;
    case 'f':
      movement_file = optarg;
      break;
    case 's':
      server = optarg;
      break;
    case 'P':
      eport = optarg;
      break;
    case 'l':
      logfile = optarg;
      break;
    case 'i':
      pidfile = optarg;
      break;
    default:
      break;
    }
David Johnson's avatar
David Johnson committed
343
344
  }
  
Timothy Stack's avatar
   
Timothy Stack committed
345
346
347
  argc -= optind;
  argv += optind;
  
David Johnson's avatar
David Johnson committed
348
349
350
351
  // initialize the global lists
  hostname_list = robot_list_create();
  initial_position_list = robot_list_create();
  position_queue = robot_list_create();
352
353
354

  g_camera_config = NULL;
  g_obstacle_config = NULL;
David Johnson's avatar
David Johnson committed
355
356
  
  /* read config file into the above lists*/
Timothy Stack's avatar
   
Timothy Stack committed
357
  parse_config_file(config_file);
David Johnson's avatar
David Johnson committed
358
  /* read movement file into the position queue */
Timothy Stack's avatar
   
Timothy Stack committed
359
  parse_movement_file(movement_file);
David Johnson's avatar
David Johnson committed
360

Timothy Stack's avatar
   
Timothy Stack committed
361
362
363
364
365
366
367
368
369
370
  if (debug) 
    loginit(0, logfile);
  else {
    /* Become a daemon */
    daemon(0, 0);
    
    if (logfile)
      loginit(0, logfile);
    else
      loginit(1, "emcd");
David Johnson's avatar
David Johnson committed
371
  }
Timothy Stack's avatar
   
Timothy Stack committed
372
373

  debug = 1;
Timothy Stack's avatar
   
Timothy Stack committed
374
375
376
377
378
379

  atexit(emcd_atexit);

  signal(SIGINT, sigquit);
  signal(SIGTERM, sigquit);
  signal(SIGQUIT, sigquit);
Timothy Stack's avatar
   
Timothy Stack committed
380
  
Timothy Stack's avatar
   
Timothy Stack committed
381
382
  signal(SIGSEGV, sigpanic);
  signal(SIGBUS, sigpanic);
David Johnson's avatar
David Johnson committed
383
  
Timothy Stack's avatar
   
Timothy Stack committed
384
385
386
387
388
389
390
391
392
  if (pidfile)
    strcpy(buf, pidfile);
  else
    sprintf(buf, "%s/progagent.pid", _PATH_VARRUN);
  fp = fopen(buf, "w");
  if (fp != NULL) {
    fprintf(fp, "%d\n", getpid());
    (void) fclose(fp);
  }
David Johnson's avatar
David Johnson committed
393
394
  
  // first, EMC server sock:
Timothy Stack's avatar
   
Timothy Stack committed
395
  emc_sock = mtp_bind("0.0.0.0", port, unix_path);
David Johnson's avatar
David Johnson committed
396
  if (emc_sock == -1) {
Timothy Stack's avatar
   
Timothy Stack committed
397
398
    fprintf(stdout,"Could not open socket for EMC: %s\n",strerror(errno));
    exit(1);
David Johnson's avatar
David Johnson committed
399
  }
Timothy Stack's avatar
   
Timothy Stack committed
400
401
402
403

#if defined(SIGINFO)
  signal(SIGINFO, siginfo);
#endif
David Johnson's avatar
David Johnson committed
404
  
Timothy Stack's avatar
   
Timothy Stack committed
405
406
407
408
409
410
411
412
413
414
415
  /*
   * Convert server/port to elvin thing.
   *
   * XXX This elvin string stuff should be moved down a layer. 
   */
  if (server) {
    snprintf(buf, sizeof(buf), "elvin://%s%s%s",
	     server,
	     (eport ? ":"  : ""),
	     (eport ? eport : ""));
    server = buf;
David Johnson's avatar
David Johnson committed
416
  }
Timothy Stack's avatar
   
Timothy Stack committed
417
418
419
420
421

  if (!keyfile && pideid) {
    snprintf(buf2, sizeof(buf2), "/proj/%s/exp/%s/tbdata/eventkey", pid, eid);
    keyfile = buf2;
  }
Timothy Stack's avatar
   
Timothy Stack committed
422
423
424
425
426
427
428
  
  /*
   * Register with the event system. 
   */
  handle = event_register_withkeyfile(server, 0, keyfile);
  if (handle == NULL) {
    fatal("could not register with event system");
David Johnson's avatar
David Johnson committed
429
430
  }

Timothy Stack's avatar
   
Timothy Stack committed
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
  if (pideid != NULL) {
    /*
     * Construct an address tuple for subscribing to events for this node.
     */
    tuple = address_tuple_alloc();
    if (tuple == NULL) {
      fatal("could not allocate an address tuple");
    }
    
    /*
     * Ask for just the SETDEST event for NODEs we care about. 
     */
    tuple->expt      = pideid;
    tuple->objtype   = TBDB_OBJECTTYPE_NODE;
    tuple->eventtype = TBDB_EVENTTYPE_SETDEST;
    
    if (!event_subscribe(handle, ev_callback, tuple, NULL)) {
      fatal("could not subscribe to event");
    }
Timothy Stack's avatar
   
Timothy Stack committed
450
451
452
453
454

  }
  
  if ((elvin_error = elvin_error_alloc()) == NULL) {
    fatal("could not allocate elvin error");
David Johnson's avatar
David Johnson committed
455
  }
Timothy Stack's avatar
   
Timothy Stack committed
456
457
458
459
460
461
462
463
  
  if ((eih = elvin_sync_add_io_handler(NULL,
				       emc_sock,
				       ELVIN_READ_MASK,
				       acceptor_callback,
				       NULL,
				       elvin_error)) == NULL) {
    fatal("could not register I/O callback");
David Johnson's avatar
David Johnson committed
464
  }
Timothy Stack's avatar
   
Timothy Stack committed
465
466
467
468
469
470
471
472

  if (elvin_sync_add_timeout(NULL,
			     EMC_UPDATE_HZ,
			     update_callback,
			     NULL,
			     elvin_error) == NULL) {
    fatal("could not add timeout");
  }
473
474
475
476
477
478
479
480
481
482
483

  while (1) {
    event_main(handle);

#if defined(SIGINFO)
    if (got_siginfo) {
      dump_info();
      got_siginfo = 0;
    }
#endif
  }
Timothy Stack's avatar
   
Timothy Stack committed
484
485
  
  return retval;
David Johnson's avatar
David Johnson committed
486
487
}

488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
static int position_in_camera(struct camera_config *cc,
                              int cc_size,
                              float x,
                              float y
                              ) {
    int i;
    int retval = 0;

    for (i = 0; i < cc_size; ++i) {
	if (x >= cc[i].x && y >= cc[i].y && 
	    x <= (cc[i].x + cc[i].width) && y <= (cc[i].y + cc[i].height)) {
	    retval = 1;
	    break;
	}
    }

    return retval;
}

static int position_in_obstacle(struct obstacle_config *oc,
                                int oc_size,
                                float x,
                                float y
                                ) {
    int i;
    int retval = 0;

    for (i = 0; i < oc_size; ++i) {
516
517
	if (x >= (oc[i].xmin - 0.25) && y >= (oc[i].ymin - 0.25) &&
	    x <= (oc[i].xmax + 0.25) && y <= (oc[i].ymax + 0.25)) {
518
519
520
521
522
523
524
525
	    retval = 1;
	    break;
	}
    }

    return retval;
}

Timothy Stack's avatar
   
Timothy Stack committed
526
527
528
529
530
531
532
533
534
535
536
537
538
539
int have_camera_config(char *hostname, int port,
		       struct camera_config *cc, int cc_size)
{
    int lpc, retval = 0;

    for (lpc = 0; lpc < cc_size; lpc++) {
	if (strcmp(hostname, cc[lpc].hostname) == 0 && port == cc[lpc].port) {
	    return 1;
	}
    }
    
    return retval;
}

Timothy Stack's avatar
   
Timothy Stack committed
540
void parse_config_file(char *config_file) {
541
542
  int oc_size = 0, cc_size = 0, line_no = 0;
  struct obstacle_config *oc = NULL;
Timothy Stack's avatar
   
Timothy Stack committed
543
  struct camera_config *cc = NULL;
Timothy Stack's avatar
   
Timothy Stack committed
544
  char line[BUFSIZ];
545
  struct stat st;
David Johnson's avatar
David Johnson committed
546
547
548
  FILE *fp;

  if (config_file == NULL) {
549
550
551
552
553
554
555
556
557
558
559
    fprintf(stderr, "error: no config file given\n");
    exit(1);
  }

  if (stat(config_file, &st) != 0) {
    fprintf(stderr, "error: cannot stat config file\n");
    exit(1);
  }
  else if (!S_ISREG(st.st_mode)) {
    fprintf(stderr, "error: config file is not a regular file\n");
    exit(1);
David Johnson's avatar
David Johnson committed
560
561
562
563
  }

  fp = fopen(config_file,"r");
  if (fp == NULL) {
Timothy Stack's avatar
   
Timothy Stack committed
564
565
    fprintf(stderr,"Could not open config file!\n");
    exit(-1);
David Johnson's avatar
David Johnson committed
566
567
568
  }

  // read line by line
Timothy Stack's avatar
   
Timothy Stack committed
569
  while (fgets(line, sizeof(line), fp) != NULL) {
570
    char directive[16] = "";
Timothy Stack's avatar
   
Timothy Stack committed
571
572
573

    // parse the line
    // sorry, i ain't using regex.h to do this simple crap
Timothy Stack's avatar
   
Timothy Stack committed
574
    // lines look like '5 garcia1.flux.utah.edu 6.5 4.234582 0.38 garcia1'
Timothy Stack's avatar
   
Timothy Stack committed
575
576
577
578
    // the regex would be '\s*\S+\s+\S+\s+\S+\s+\S+\s+\S+'
    // so basically, 5 non-whitespace groups separated by whitespace (tabs or
    // spaces)

Timothy Stack's avatar
   
Timothy Stack committed
579
580
    sscanf(line, "%16s", directive);

581
582
583
584
    if (directive[0] == '#' || directive[0] == '\0') {
      // Comment or empty line.
    }
    else if (strcmp(directive, "robot") == 0) {
585
      char area[32], hostname[MAXHOSTNAMELEN], vname[TBDB_FLEN_EVOBJNAME];
Timothy Stack's avatar
   
Timothy Stack committed
586
587
588
589
590
591
      int id;
      float init_x;
      float init_y;
      float init_theta;

      if (sscanf(line,
592
		 "%16s %32s %d %" __XSTRING(MAXHOSTNAMELEN) "s %f %f %f "
Timothy Stack's avatar
   
Timothy Stack committed
593
594
		 "%" __XSTRING(TBDB_FLEN_EVOBJNAME) "s",
		 directive,
595
		 area,
Timothy Stack's avatar
   
Timothy Stack committed
596
597
598
599
600
		 &id,
		 hostname,
		 &init_x,
		 &init_y,
		 &init_theta,
601
		 vname) != 8) {
Timothy Stack's avatar
   
Timothy Stack committed
602
603
604
605
606
607
608
609
	fprintf(stderr,
		"error:%d: syntax error in config file - %s\n",
		line_no,
		line);
      }
      else {
	struct emc_robot_config *rc;
	struct hostent *he;
David Johnson's avatar
David Johnson committed
610
	
Timothy Stack's avatar
   
Timothy Stack committed
611
612
613
614
615
616
617
618
619
620
621
622
623
624
	// now we save this data to the lists:
	rc = (struct emc_robot_config *)
	  malloc(sizeof(struct emc_robot_config));
	rc->id = id;
	rc->hostname = strdup(hostname);
	if ((he = gethostbyname(hostname)) == NULL) {
	  fprintf(stderr, "error: unknown robot: %s\n", hostname);
	  free(hostname);
	  free(rc);
	  continue;
	}
	memcpy(&rc->ia, he->h_addr, he->h_length);
	rc->vname = strdup(vname);
	rc->token = ~0;
Timothy Stack's avatar
   
Timothy Stack committed
625
626
627
628
629
	rc->init_x = init_x;
	rc->init_y = init_y;
	rc->init_theta = init_theta;

	robot_list_append(hostname_list, id, (void *)rc);
Timothy Stack's avatar
   
Timothy Stack committed
630
631
632
      }
    }
    else if (strcmp(directive, "camera") == 0) {
633
      char area[32], hostname[MAXHOSTNAMELEN];
Timothy Stack's avatar
   
Timothy Stack committed
634
      float x, y, width, height;
Timothy Stack's avatar
   
Timothy Stack committed
635
636
637
      int port;

      if (sscanf(line,
Timothy Stack's avatar
   
Timothy Stack committed
638
		 "%16s %32s %" __XSTRING(MAXHOSTNAMELEN) "s %d %f %f %f %f",
Timothy Stack's avatar
   
Timothy Stack committed
639
		 directive,
640
		 area,
Timothy Stack's avatar
   
Timothy Stack committed
641
		 hostname,
Timothy Stack's avatar
   
Timothy Stack committed
642
643
644
645
646
		 &port,
		 &x,
		 &y,
		 &width,
		 &height) != 8) {
Timothy Stack's avatar
   
Timothy Stack committed
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
	fprintf(stderr,
		"error:%d: syntax error in config file - %s\n",
		line_no,
		line);
      }
      else if (have_camera_config(hostname, port, cc, cc_size)) {
      }
      else if ((cc = realloc(cc,
			     (cc_size + 1) *
			     sizeof(struct camera_config))) == 0) {
	fatal("cannot allocate memory\n");
      }
      else {
	cc[cc_size].hostname = strdup(hostname);
	cc[cc_size].port = port;
Timothy Stack's avatar
   
Timothy Stack committed
662
663
664
665
	cc[cc_size].x = x;
	cc[cc_size].y = y;
	cc[cc_size].width = width;
	cc[cc_size].height = height;
Timothy Stack's avatar
   
Timothy Stack committed
666
667
	cc_size += 1;
      }
Timothy Stack's avatar
   
Timothy Stack committed
668
    }
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
694
    else if (strcmp(directive, "obstacle") == 0) {
      float x1, y1, x2, y2;
      char area[32];
      int id;

      if (sscanf(line,
		 "%16s %32s %d %f %f %f %f",
		 directive,
		 area,
		 &id,
		 &x1,
		 &y1,
		 &x2,
		 &y2) != 7) {
	fprintf(stderr,
		"error:%d: syntax error in config file - %s\n",
		line_no,
		line);
      }
      else if ((oc = realloc(oc,
			     (oc_size + 1) *
			     sizeof(struct camera_config))) == 0) {
	fatal("cannot allocate memory\n");
      }
      else {
	oc[oc_size].id = id;
Timothy Stack's avatar
   
Timothy Stack committed
695
696
697
698
	oc[oc_size].xmin = x1;
	oc[oc_size].ymin = y1;
	oc[oc_size].xmax = x2;
	oc[oc_size].ymax = y2;
699
700
701
	oc_size += 1;
      }
    }
702
703
704
705
706
707
    else {
      fprintf(stderr,
	      "error:%d: unknown directive - %s\n",
	      line_no,
	      directive);
    }
Timothy Stack's avatar
   
Timothy Stack committed
708
    // next line!
David Johnson's avatar
David Johnson committed
709
710
  }

Timothy Stack's avatar
   
Timothy Stack committed
711
712
713
  {
      struct robot_config *robot_val, *rc;
      struct robot_list_enum *e;
714
715
      struct box *boxes_val;
      int boxes_len;
Timothy Stack's avatar
   
Timothy Stack committed
716
      int lpc = 0;
717
718
719
720

      if (hostname_list->item_count == 0) {
	fatal("no robots have been specified!");
      }
Timothy Stack's avatar
   
Timothy Stack committed
721
722
723
724
725
726
727
728
729
      
      robot_val = malloc(sizeof(robot_config) * hostname_list->item_count);
      e = robot_list_enum(hostname_list);
      while ((rc = (struct robot_config *)
	      robot_list_enum_next_element(e)) != NULL) {
	  robot_val[lpc] = *rc;
	  lpc += 1;
      }
      robot_list_enum_destroy(e);
730

731
732
733
734
      if (cc_size == 0) {
	fatal("no cameras have been specified!");
      }
      
735
736
737
738
739
740
741
742
      boxes_len = cc_size;
      boxes_val = (struct box *)malloc(sizeof(struct box) * boxes_len);
      for (lpc = 0; lpc < boxes_len; ++lpc) {
	  boxes_val[lpc].x = cc[lpc].x;
	  boxes_val[lpc].y = cc[lpc].y;
	  boxes_val[lpc].width = cc[lpc].width;
	  boxes_val[lpc].height = cc[lpc].height;
      }
Timothy Stack's avatar
   
Timothy Stack committed
743
744
745
746
747
748
      
      mtp_init_packet(&config_rmc,
		      MA_Opcode, MTP_CONFIG_RMC,
		      MA_Role, MTP_ROLE_EMC,
		      MA_RobotLen, hostname_list->item_count,
		      MA_RobotVal, robot_val,
749
750
		      MA_BoundsLen, boxes_len,
		      MA_BoundsVal, boxes_val,
751
752
		      MA_ObstacleLen, oc_size,
		      MA_ObstacleVal, oc,
Timothy Stack's avatar
   
Timothy Stack committed
753
754
755
756
757
758
		      MA_TAG_DONE);
      mtp_init_packet(&config_vmc,
		      MA_Opcode, MTP_CONFIG_VMC,
		      MA_Role, MTP_ROLE_EMC,
		      MA_RobotLen, hostname_list->item_count,
		      MA_RobotVal, robot_val,
Timothy Stack's avatar
   
Timothy Stack committed
759
760
		      MA_CameraLen, cc_size,
		      MA_CameraVal, cc,
Timothy Stack's avatar
   
Timothy Stack committed
761
		      MA_TAG_DONE);
762
763
764
765
766
      /* don't free cc or oc! */
      g_camera_config = cc;
      g_camera_config_size = cc_size;
      g_obstacle_config = oc;
      g_obstacle_config_size = oc_size;
Timothy Stack's avatar
   
Timothy Stack committed
767
  }
David Johnson's avatar
David Johnson committed
768
769
}

Timothy Stack's avatar
   
Timothy Stack committed
770
771
772
void parse_movement_file(char *movement_file) {
  char line[BUFSIZ];
  int line_no = 0;
David Johnson's avatar
David Johnson committed
773
774
775
  FILE *fp;

  if (movement_file == NULL) {
Timothy Stack's avatar
   
Timothy Stack committed
776
    return;
David Johnson's avatar
David Johnson committed
777
778
779
780
  }

  fp = fopen(movement_file,"r");
  if (fp == NULL) {
Timothy Stack's avatar
   
Timothy Stack committed
781
782
    fprintf(stderr,"Could not open movement file!\n");
    exit(-1);
David Johnson's avatar
David Johnson committed
783
784
785
  }

  // read line by line
Timothy Stack's avatar
   
Timothy Stack committed
786
787
788
789
790
791
792
793
794
795
796
797
798
799
800
801
802
  while (fgets(line, sizeof(line), fp) != NULL) {
    int id;
    float init_x;
    float init_y;
    float init_theta;

    // parse the line
    // sorry, i ain't using regex.h to do this simple crap
    // lines look like '5  6.5 4.234582 0.38'
    // the regex would be '\s*\S+\s+\S+\s+\S+\s+\S+'
    // so basically, 4 non-whitespace groups separated by whitespace (tabs or
    // spaces)

    // we use strtok_r because i'm complete
    char *token = NULL;
    char *delim = " \t";
    char *state = NULL;
Timothy Stack's avatar
   
Timothy Stack committed
803
    struct robot_position *p;
Timothy Stack's avatar
   
Timothy Stack committed
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
    
    ++line_no;

    token = strtok_r(line,delim,&state);
    if (token == NULL) {
      fprintf(stdout,"Syntax error in movement file, line %d.\n",line_no);
      continue;
    }
    id = atoi(token);

    token = strtok_r(NULL,delim,&state);
    if (token == NULL) {
      fprintf(stdout,"Syntax error in movement file, line %d.\n",line_no);
      continue;
    }
    init_x = (float)atof(token);
	  
    token = strtok_r(NULL,delim,&state);
    if (token == NULL) {
      fprintf(stdout,"Syntax error in movement file, line %d.\n",line_no);
      continue;
    }
    init_y = (float)atof(token);

    token = strtok_r(NULL,delim,&state);
    if (token == NULL) {
      fprintf(stdout,"Syntax error in movement file, line %d.\n",line_no);
      continue;
    }
    init_theta = (float)atof(token);


    // now we save this data to the lists:
Timothy Stack's avatar
   
Timothy Stack committed
837
    p = (struct robot_position *)malloc(sizeof(struct robot_position *));
Timothy Stack's avatar
   
Timothy Stack committed
838
839
840
841
842
843
844
845
846
847
848
849
850
851
    p->x = init_x;
    p->y = init_y;
    p->theta = init_theta;
    robot_list_append(position_queue,id,(void*)p);

    // next line!
  }

}

void ev_callback(event_handle_t handle,
		 event_notification_t notification,
		 void *data)
{
852
  struct emc_robot_config *match = NULL;
Timothy Stack's avatar
   
Timothy Stack committed
853
  char objname[TBDB_FLEN_EVOBJNAME];
854
  robot_list_item_t *rli;
855
  int in_obstacle, in_camera;
Timothy Stack's avatar
   
Timothy Stack committed
856
  
857
858
859
  if (! event_notification_get_objname(handle, notification,
				       objname, sizeof(objname))) {
    error("Could not get objname from notification!\n");
Timothy Stack's avatar
   
Timothy Stack committed
860
    return;
861
  }
David Johnson's avatar
   
David Johnson committed
862
  
863
864
865
866
867
868
869
870
871
872
873
  for (rli = hostname_list->head; rli != NULL && !match; rli = rli->next) {
    struct emc_robot_config *erc = rli->data;
    
    if (strcmp(erc->vname, objname) == 0) {
      match = erc;
      break;
    }
  }
  
  if (match == NULL) {
    error("no match for host\n");
Timothy Stack's avatar
   
Timothy Stack committed
874
875
  }
  else {
876
877
    char *value, args[BUFSIZ];
    float x, y, orientation;
Timothy Stack's avatar
   
Timothy Stack committed
878
    struct mtp_packet mp;
Timothy Stack's avatar
   
Timothy Stack committed
879
    
880
    event_notification_get_arguments(handle, notification, args, sizeof(args));
Timothy Stack's avatar
   
Timothy Stack committed
881
882

    /* XXX copy the current X, Y, and orientation! */
883
884
885
    if (event_arg_get(args, "X", &value) > 0) {
      if (sscanf(value, "%f", &x) != 1) {
	error("X argument in event is not a float: %s\n", value);
Timothy Stack's avatar
   
Timothy Stack committed
886
	return;
Timothy Stack's avatar
   
Timothy Stack committed
887
888
      }
    }
David Johnson's avatar
   
David Johnson committed
889
    
890
891
892
    if (event_arg_get(args, "Y", &value) > 0) {
      if (sscanf(value, "%f", &y) != 1) {
	error("Y argument in event is not a float: %s\n", value);
Timothy Stack's avatar
   
Timothy Stack committed
893
	return;
Timothy Stack's avatar
   
Timothy Stack committed
894
      }
895
896
897
898
899
    }
    
    if (event_arg_get(args, "ORIENTATION", &value) > 0) {
      if (sscanf(value, "%f", &orientation) != 1) {
	error("ORIENTATION argument in event is not a float: %s\n", value);
Timothy Stack's avatar
   
Timothy Stack committed
900
	return;
Timothy Stack's avatar
   
Timothy Stack committed
901
      }
902
903
904
905
    }
    
    event_notification_get_int32(handle, notification,
				 "TOKEN", (int32_t *)&match->token);
David Johnson's avatar
   
David Johnson committed
906

907
    orientation = orientation * M_PI / 180.0;
David Johnson's avatar
   
David Johnson committed
908

909
910
911
912
913
914
915
916
917
918
919
920
921
922
923
924
925
926
927
928
929
930
931
932
933
934
935
936
937
938
939
940
941
942
943
944
945
    if ((in_camera = position_in_camera(g_camera_config,g_camera_config_size,
					x,y)) &&
	!(in_obstacle = position_in_obstacle(g_obstacle_config,
					     g_obstacle_config_size,
					     x,y))
	) {
	

	mtp_init_packet(&mp,
			MA_Opcode, MTP_COMMAND_GOTO,
			MA_Role, MTP_ROLE_EMC,
			MA_CommandID, 1,
			MA_RobotID, match->id,
			MA_X, x,
			MA_Y, y,
			MA_Theta, orientation,
			MA_TAG_DONE);

	if (rmc_data.handle != NULL) {
	    mtp_send_packet(rmc_data.handle, &mp);
	}
	else {
	    mtp_print_packet(stdout, &mp);
	    
#if defined(TBDB_EVENTTYPE_COMPLETE)
	    event_do(handle,
		     EA_Experiment, pideid,
		     EA_Type, TBDB_OBJECTTYPE_NODE,
		     EA_Name, match->vname,
		     EA_Event, TBDB_EVENTTYPE_COMPLETE,
		     EA_ArgInteger, "ERROR", 0,
		     EA_ArgInteger, "CTOKEN", match->token,
		     EA_TAG_DONE);
#endif
	}
	
	mtp_free_packet(&mp);
946
947
    }
    else {
948
	int which_err_num;
Timothy Stack's avatar
   
Timothy Stack committed
949

950
951
952
953
954
	which_err_num = (in_camera == 0)?1:2;

	info("requested position either outside camera bounds or inside"
	     " an obstacle!\n"
	     );
Timothy Stack's avatar
   
Timothy Stack committed
955
#if defined(TBDB_EVENTTYPE_COMPLETE)
956
957
958
959
960
961
962
963
964
	event_do(handle,
		 EA_Experiment, pideid,
		 EA_Type, TBDB_OBJECTTYPE_NODE,
		 EA_Name, match->vname,
		 EA_Event, TBDB_EVENTTYPE_COMPLETE,
		 EA_ArgInteger, "ERROR", which_err_num,
		 EA_ArgInteger, "CTOKEN", match->token,
		 EA_TAG_DONE
		 );
Timothy Stack's avatar
   
Timothy Stack committed
965
#endif
Timothy Stack's avatar
   
Timothy Stack committed
966
    }
967
968
969
970
971
    
    // XXX What to do with the data?
    
    // construct a COMMAND_GOTO packet and send to rmc.
    
Timothy Stack's avatar
   
Timothy Stack committed
972
  }
Timothy Stack's avatar
   
Timothy Stack committed
973
974
975
976
977
978
979
980
981
982
983
984
985
986
987
988
989
990
991
992
993
994
995
996
997
998
999
1000
}

int acceptor_callback(elvin_io_handler_t handler,
		      int fd,
		      void *rock,
		      elvin_error_t eerror)
{
  // we have a new client -- but who?
  int addrlen = sizeof(struct sockaddr_in);
  struct sockaddr_in client_sin;
  int client_sock;

  if ((client_sock = accept(fd,
			    (struct sockaddr *)(&client_sin),
			    &addrlen)) == -1) {
    perror("accept");
  }
  else if (elvin_sync_add_io_handler(NULL,
				     client_sock,
				     ELVIN_READ_MASK,
				     unknown_client_callback,
				     NULL,
				     eerror) == NULL) {
    error("could not add handler for %d\n", client_sock);
    
    close(client_sock);
    client_sock = -1;
  }
For faster browsing, not all history is shown. View entire blame