emcd.c 49.3 KB
Newer Older
Timothy Stack's avatar
Timothy Stack committed
1 2
/*
 * Copyright (c) 2005 University of Utah and the Flux Group.
3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21
 * 
 * {{{EMULAB-LICENSE
 * 
 * This file is part of the Emulab network testbed software.
 * 
 * This file is free software: you can redistribute it and/or modify it
 * under the terms of the GNU Affero General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or (at
 * your option) any later version.
 * 
 * This file is distributed in the hope that it will be useful, but WITHOUT
 * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Affero General Public
 * License for more details.
 * 
 * You should have received a copy of the GNU Affero General Public License
 * along with this file.  If not, see <http://www.gnu.org/licenses/>.
 * 
 * }}}
Timothy Stack's avatar
Timothy Stack committed
22
 */
23 24 25 26 27 28 29 30

/* 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
31
 * a working set to select() on.  Then commands are processed from either a
32 33
 * file, or from an active user interface; requests/data are processed from
 * RMC and VMC, and appropriate responses are generated if necessary (whether
34
 * the response be to the requesting client, or to somebody else (i.e., if
35 36 37 38 39 40 41 42 43 44 45
 * 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

46
// for the initial configuration of rmc, we need to send a list of id ->
47 48 49 50 51 52 53 54
// 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
55
// position (x, y, and r floating point vals).  We read this file into the
56 57 58
// initial position data structure, as well as a list of id to hostname
// mappings.

59 60
#include "config.h"

61
#include <stdio.h>
Timothy Stack's avatar
Timothy Stack committed
62
#include <math.h>
63
#include <assert.h>
64
#include <sys/types.h>
65
#include <sys/stat.h>
66 67 68
#include <unistd.h>
#include <stdlib.h>
#include <signal.h>
69
#include <paths.h>
70
#include <netinet/in.h>
71
#include <arpa/inet.h>
72 73
#include <errno.h>
#include <string.h>
74
#include <netdb.h>
75 76
#include <sys/socket.h>

77 78 79 80
#include "log.h"
#include "tbdefs.h"
#include "event.h"
#include <elvin/elvin.h>
81 82 83 84

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

85
#include "emcd.h"
86

87 88 89 90
#ifndef __XSTRING
#define __XSTRING(x) __STRING(x)
#endif

91
// #define USE_POSTURE_REG
92

93
static event_handle_t handle;
94

95
static char *pideid;
96

97
static char *progname;
98

99
static int debug;
100

101
static int port = EMC_SERVER_PORT;
102 103 104 105 106 107 108 109 110 111 112 113 114

/* 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;

115 116
static elvin_error_t elvin_error;

117 118 119
static struct mtp_packet config_rmc;
static struct mtp_packet config_vmc;

120 121
static char *topography_name;

122
/*
123 124 125 126 127 128 129 130 131
 * 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;


132 133 134
static struct rmc_client rmc_data;
static struct vmc_client vmc_data;
static mtp_handle_t emulab_handle = NULL;
135

136

137
/*
138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155
 * 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
				);

156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184
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);

185 186 187 188
static int update_callback(elvin_timeout_t timeout,
			   void *rock,
			   elvin_error_t eerror);

189 190 191
static void parse_config_file(char *filename);
static void parse_movement_file(char *filename);

192 193
static char *unix_path = NULL;

194 195 196 197 198
#if defined(SIGINFO)
/* SIGINFO-related stuff */

/**
 * Variable used to tell the main loop that we received a SIGINFO.
199
 */
200
static int got_siginfo = 0;
201

202 203 204 205 206 207 208 209 210 211 212 213
/**
 * 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);
}
214

215 216
static void dump_info(void)
{
217 218 219 220 221 222 223 224
  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");
225

226 227 228 229
    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;
230

231 232 233 234 235 236 237 238 239
      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);
  }
240 241 242
}
#endif

243 244 245 246 247 248 249 250 251 252 253
static void emcd_atexit(void)
{
  if (unix_path != NULL)
    unlink(unix_path);
}

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

254 255 256 257 258 259 260 261 262 263
static void sigpanic(int sig)
{
  char buf[32];

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

264 265 266
static void usage(char *progname)
{
  fprintf(stderr,
267 268 269 270 271 272 273
	  "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"
274
	  "  -U path\tUnix socket path\n"
275 276 277
	  "  -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");
278 279 280 281 282
}

int main(int argc, char *argv[])
{
  char pid[MAXHOSTNAMELEN], eid[MAXHOSTNAMELEN];
283
  int c, emc_sock, retval = EXIT_SUCCESS;
284 285 286 287 288 289 290 291 292
  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;
293
  char buf[BUFSIZ], buf2[BUFSIZ];
294 295
  char *idx;
  FILE *fp;
296

297
  progname = argv[0];
298

299
  while ((c = getopt(argc, argv, "hde:k:p:c:f:s:P:U:l:i:")) != -1) {
300 301
    switch (c) {
    case 'h':
302 303
      usage(progname);
      exit(0);
304 305 306 307 308 309 310 311 312 313 314 315 316 317 318 319 320 321 322 323 324 325 326 327 328 329 330 331 332 333 334 335 336 337 338 339 340 341 342
      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;
343 344 345
    case 'U':
      unix_path = optarg;
      break;
346 347 348 349 350 351 352 353 354 355 356 357 358 359 360 361 362 363 364 365 366
    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;
    }
367
  }
368 369

  if (!config_file) {
370 371
    fprintf(stderr, "error: no config file specified\n");
    exit(1);
372
  }
373

374 375
  argc -= optind;
  argv += optind;
376

377 378 379 380
  // initialize the global lists
  hostname_list = robot_list_create();
  initial_position_list = robot_list_create();
  position_queue = robot_list_create();
381 382 383

  g_camera_config = NULL;
  g_obstacle_config = NULL;
384

385
  /* read config file into the above lists*/
386
  parse_config_file(config_file);
387
  /* read movement file into the position queue */
388
  parse_movement_file(movement_file);
389

390
  if (debug)
391 392 393 394
    loginit(0, logfile);
  else {
    /* Become a daemon */
    daemon(0, 0);
395

396 397 398 399
    if (logfile)
      loginit(0, logfile);
    else
      loginit(1, "emcd");
400
  }
401 402

  debug = 1;
403 404 405 406 407 408

  atexit(emcd_atexit);

  signal(SIGINT, sigquit);
  signal(SIGTERM, sigquit);
  signal(SIGQUIT, sigquit);
409

410 411
  signal(SIGSEGV, sigpanic);
  signal(SIGBUS, sigpanic);
412

413 414 415
  if (pidfile)
    strcpy(buf, pidfile);
  else
416
    sprintf(buf, "%s/emcd.pid", _PATH_VARRUN);
417 418 419 420 421
  fp = fopen(buf, "w");
  if (fp != NULL) {
    fprintf(fp, "%d\n", getpid());
    (void) fclose(fp);
  }
422

423
  // first, EMC server sock:
424
  emc_sock = mtp_bind("0.0.0.0", port, unix_path);
425
  if (emc_sock == -1) {
426 427
    fprintf(stdout,"Could not open socket for EMC: %s\n",strerror(errno));
    exit(1);
428
  }
429 430 431 432

#if defined(SIGINFO)
  signal(SIGINFO, siginfo);
#endif
433

434 435 436
  /*
   * Convert server/port to elvin thing.
   *
437
   * XXX This elvin string stuff should be moved down a layer.
438 439 440 441 442 443 444
   */
  if (server) {
    snprintf(buf, sizeof(buf), "elvin://%s%s%s",
	     server,
	     (eport ? ":"  : ""),
	     (eport ? eport : ""));
    server = buf;
445
  }
446 447 448 449 450

  if (!keyfile && pideid) {
    snprintf(buf2, sizeof(buf2), "/proj/%s/exp/%s/tbdata/eventkey", pid, eid);
    keyfile = buf2;
  }
451

452
  /*
453
   * Register with the event system.
454 455 456 457
   */
  handle = event_register_withkeyfile(server, 0, keyfile);
  if (handle == NULL) {
    fatal("could not register with event system");
458 459
  }

460 461 462 463 464 465 466 467
  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");
    }
468

469
    /*
470
     * Ask for just the SETDEST event for NODEs we care about.
471 472 473 474
     */
    tuple->expt      = pideid;
    tuple->objtype   = TBDB_OBJECTTYPE_NODE;
    tuple->eventtype = TBDB_EVENTTYPE_SETDEST;
475

476 477 478
    if (!event_subscribe(handle, ev_callback, tuple, NULL)) {
      fatal("could not subscribe to event");
    }
479 480

  }
481

482 483
  if ((elvin_error = elvin_error_alloc()) == NULL) {
    fatal("could not allocate elvin error");
484
  }
485

486 487 488 489 490 491 492
  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");
493
  }
494 495 496 497 498 499 500 501

  if (elvin_sync_add_timeout(NULL,
			     EMC_UPDATE_HZ,
			     update_callback,
			     NULL,
			     elvin_error) == NULL) {
    fatal("could not add timeout");
  }
502 503 504 505 506 507 508 509 510 511 512

  while (1) {
    event_main(handle);

#if defined(SIGINFO)
    if (got_siginfo) {
      dump_info();
      got_siginfo = 0;
    }
#endif
  }
513

514
  return retval;
515 516
}

517 518 519 520 521
static int position_in_camera(struct camera_config *cc,
                              int cc_size,
                              float x,
                              float y
                              ) {
522 523
  int i;
  int retval = 0;
524

525 526 527 528 529
  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;
530
    }
531
  }
532

533
  return retval;
534 535 536 537 538 539 540
}

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

544
  for (i = 0; i < oc_size; ++i) {
545 546 547 548
    if (x >= (oc[i].xmin - OBSTACLE_BUFFER) &&
	y >= (oc[i].ymin - OBSTACLE_BUFFER) &&
	x <= (oc[i].xmax + OBSTACLE_BUFFER) &&
	y <= (oc[i].ymax + OBSTACLE_BUFFER)) {
549 550
      retval = 1;
      break;
551
    }
552
  }
553

554
  return retval;
555 556
}

557 558 559
int have_camera_config(char *hostname, int port,
		       struct camera_config *cc, int cc_size)
{
560
  int lpc, retval = 0;
561

562 563 564
  for (lpc = 0; lpc < cc_size; lpc++) {
    if (strcmp(hostname, cc[lpc].hostname) == 0 && port == cc[lpc].port) {
      return 1;
565
    }
566 567 568
  }

  return retval;
569 570
}

571
void parse_config_file(char *config_file) {
572 573
  int oc_size = 0, cc_size = 0, line_no = 0;
  struct obstacle_config *oc = NULL;
574
  struct camera_config *cc = NULL;
575
  char line[BUFSIZ];
576
  struct stat st;
577 578 579
  FILE *fp;

  if (config_file == NULL) {
580 581 582 583 584 585 586 587 588 589 590
    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);
591 592 593 594
  }

  fp = fopen(config_file,"r");
  if (fp == NULL) {
595 596
    fprintf(stderr,"Could not open config file!\n");
    exit(-1);
597 598 599
  }

  // read line by line
600
  while (fgets(line, sizeof(line), fp) != NULL) {
601
    char directive[16] = "";
602 603 604

    // parse the line
    // sorry, i ain't using regex.h to do this simple crap
605
    // lines look like '5 garcia1.flux.utah.edu 6.5 4.234582 0.38 garcia1'
606 607 608 609
    // 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)

610 611
    sscanf(line, "%16s", directive);

612 613 614
    if (directive[0] == '#' || directive[0] == '\0') {
      // Comment or empty line.
    }
615 616 617 618 619 620 621 622 623 624 625 626 627
    else if (strcmp(directive, "topography") == 0) {
      char area[32];

      if (sscanf(line, "%16s %32s", directive, area) != 2) {
	fprintf(stderr,
		"error:%d: syntax error in config file - %s\n",
		line_no,
		line);
      }
      else {
	topography_name = strdup(area);
      }
    }
628
    else if (strcmp(directive, "robot") == 0) {
629
      char area[32], hostname[MAXHOSTNAMELEN], vname[TBDB_FLEN_EVOBJNAME];
630 631 632 633 634 635
      int id;
      float init_x;
      float init_y;
      float init_theta;

      if (sscanf(line,
636
		 "%16s %32s %d %" __XSTRING(MAXHOSTNAMELEN) "s %f %f %f "
637 638
		 "%" __XSTRING(TBDB_FLEN_EVOBJNAME) "s",
		 directive,
639
		 area,
640 641 642 643 644
		 &id,
		 hostname,
		 &init_x,
		 &init_y,
		 &init_theta,
645
		 vname) != 8) {
646 647 648 649 650 651 652 653
	fprintf(stderr,
		"error:%d: syntax error in config file - %s\n",
		line_no,
		line);
      }
      else {
	struct emc_robot_config *rc;
	struct hostent *he;
654

655 656 657 658 659 660 661 662 663 664 665 666 667 668
	// 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;
669 670 671 672 673
	rc->init_x = init_x;
	rc->init_y = init_y;
	rc->init_theta = init_theta;

	robot_list_append(hostname_list, id, (void *)rc);
674 675 676
      }
    }
    else if (strcmp(directive, "camera") == 0) {
677
      char area[32], hostname[MAXHOSTNAMELEN];
678
      float x, y, width, height, fixed_x, fixed_y;
679 680 681
      int port;

      if (sscanf(line,
682 683 684
		 "%16s %32s %"
		 __XSTRING(MAXHOSTNAMELEN)
		 "s %d %f %f %f %f %f %f",
685
		 directive,
686
		 area,
687
		 hostname,
Timothy Stack's avatar
Timothy Stack committed
688 689 690 691
		 &port,
		 &x,
		 &y,
		 &width,
692 693 694
		 &height,
		 &fixed_x,
		 &fixed_y) != 10) {
695 696 697 698 699 700 701 702 703 704 705 706 707 708 709
	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
710 711 712 713
	cc[cc_size].x = x;
	cc[cc_size].y = y;
	cc[cc_size].width = width;
	cc[cc_size].height = height;
714 715
	cc[cc_size].fixed_x = fixed_x;
	cc[cc_size].fixed_y = fixed_y;
716 717
	cc_size += 1;
      }
718
    }
719 720 721 722 723 724 725 726 727 728 729 730 731 732 733 734 735 736 737 738 739 740 741 742 743 744
    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;
745 746
	oc[oc_size].xmin = x1;
	oc[oc_size].ymin = y1;
747
	oc[oc_size].zmin = 0.0;
748 749
	oc[oc_size].xmax = x2;
	oc[oc_size].ymax = y2;
750
	oc[oc_size].zmax = 0.0;
751 752 753
	oc_size += 1;
      }
    }
754 755 756 757 758 759
    else {
      fprintf(stderr,
	      "error:%d: unknown directive - %s\n",
	      line_no,
	      directive);
    }
760
    // next line!
761 762
  }

763
  {
764 765 766 767 768
    struct robot_config *robot_val, *rc;
    struct robot_list_enum *e;
    struct box *boxes_val;
    int boxes_len;
    int lpc = 0;
769

770 771 772 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
    if (hostname_list->item_count == 0) {
      fatal("no robots have been specified!");
    }

    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);

    if (cc_size == 0) {
      fatal("no cameras have been specified!");
    }

    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;
    }

    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,
		    MA_BoundsLen, boxes_len,
		    MA_BoundsVal, boxes_val,
		    MA_ObstacleLen, oc_size,
		    MA_ObstacleVal, oc,
		    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,
		    MA_CameraLen, cc_size,
		    MA_CameraVal, cc,
		    MA_TAG_DONE);
    /* 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;
819
  }
820 821
}

822 823 824
void parse_movement_file(char *movement_file) {
  char line[BUFSIZ];
  int line_no = 0;
825 826 827
  FILE *fp;

  if (movement_file == NULL) {
828
    return;
829 830 831 832
  }

  fp = fopen(movement_file,"r");
  if (fp == NULL) {
833 834
    fprintf(stderr,"Could not open movement file!\n");
    exit(-1);
835 836 837
  }

  // read line by line
838 839 840 841 842 843 844 845 846 847 848 849 850 851 852 853 854
  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;
855
    struct robot_position *p;
856

857 858 859 860 861 862 863 864 865 866 867 868 869 870 871
    ++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);
872

873 874 875 876 877 878 879 880 881 882 883 884 885 886 887 888
    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:
889
    p = (struct robot_position *)malloc(sizeof(struct robot_position *));
890 891 892 893 894 895 896 897 898 899 900 901 902 903
    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)
{
904
  struct emc_robot_config *match = NULL;
905
  char objname[TBDB_FLEN_EVOBJNAME];
906
  robot_list_item_t *rli;
907
  int in_obstacle, in_camera;
908

909 910 911
  if (! event_notification_get_objname(handle, notification,
				       objname, sizeof(objname))) {
    error("Could not get objname from notification!\n");
912
    return;
913
  }
914

915 916
  for (rli = hostname_list->head; rli != NULL && !match; rli = rli->next) {
    struct emc_robot_config *erc = rli->data;
917

918 919 920 921 922
    if (strcmp(erc->vname, objname) == 0) {
      match = erc;
      break;
    }
  }
923

924 925
  if (match == NULL) {
    error("no match for host\n");
926 927
  }
  else {
928
    float x, y, orientation = 0.0, speed = 0.2;
929
    char *value, args[BUFSIZ];
930
    struct mtp_packet mp;
931

932
    event_notification_get_arguments(handle, notification, args, sizeof(args));
933 934

    /* XXX copy the current X, Y, and orientation! */
935 936 937
    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);
938
	return;
939 940
      }
    }
941

942 943 944
    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);
945
	return;
946
      }
947
    }
948

949 950 951
    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);
952
	return;
953
      }
954
    }
955

956 957 958 959 960 961
    if (event_arg_get(args, "SPEED", &value) > 0) {
      if (sscanf(value, "%f", &speed) != 1) {
	error("SPEED argument in event is not a float: %s\n", value);
	return;
      }
    }
962

963 964
    event_notification_get_int32(handle, notification,
				 "TOKEN", (int32_t *)&match->token);
David Johnson's avatar
David Johnson committed
965

966
    orientation = orientation * M_PI / 180.0;
David Johnson's avatar
David Johnson committed
967

968 969 970 971 972 973
    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))
	) {
974

975

976 977 978 979 980 981 982 983 984 985 986
      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_Speed, speed,
		      MA_TAG_DONE);

987
#ifdef USE_POSTURE_REG
988
      match->flags |= ERF_HAS_GOAL;
989
#endif
990 991 992 993 994 995 996 997 998
      match->last_goal_pos = mp.data.mtp_payload_u.command_goto.position;

      if (rmc_data.handle != NULL) {
	mtp_send_packet(rmc_data.handle, &mp);
      }
#if 0
      else {
	mtp_print_packet(stdout, &mp);

999
#if defined(TBDB_EVENTTYPE_COMPLETE)
1000 1001 1002 1003 1004
	event_do(handle,
		 EA_Experiment, pideid,
		 EA_Type, TBDB_OBJECTTYPE_NODE,
		 EA_Name, match->vname,
		 EA_Event, TBDB_EVENTTYPE_COMPLETE,
1005
		 EA_ArgInteger, "ERROR", 0,
1006
		 EA_ArgInteger, "CTOKEN", match->token,
1007 1008 1009
		 EA_TAG_DONE);
#endif
      }
1010
#endif
1011 1012

      mtp_free_packet(&mp);
1013
    }
1014 1015 1016 1017 1018 1019 1020 1021 1022 1023 1024 1025 1026 1027 1028 1029 1030 1031 1032 1033 1034
    else {
      int which_err_num;

      which_err_num = (in_camera == 0)?1:2;

      info("requested position either outside camera bounds or inside"
	   " an obstacle!\n"
	   );
#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", which_err_num,
	       EA_ArgInteger, "CTOKEN", match->token,
	       EA_TAG_DONE
	       );
#endif
    }

1035
    // XXX What to do with the data?
1036

1037
    // construct a COMMAND_GOTO packet and send to rmc.
1038

1039
  }
1040 1041 1042 1043 1044 1045 1046 1047 1048 1049 1050 1051 1052 1053 1054 1055 1056 1057 1058 1059 1060 1061 1062 1063
}

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);
1064

1065 1066 1067
    close(client_sock);
    client_sock = -1;
  }
1068 1069 1070 1071 1072 1073 1074
  else {
    if (debug > 1) {
      info("debug: connect from %s:%d\n",
	   inet_ntoa(client_sin.sin_addr),
	   ntohs(client_sin.sin_port));
    }
  }
1075

1076 1077 1078 1079 1080 1081 1082 1083
  return 1;
}

int unknown_client_callback(elvin_io_handler_t handler,
			    int fd,
			    void *rock,
			    elvin_error_t eerror)
{
1084
  mtp_packet_t mp_data, *mp = &mp_data;
1085
  int rc, retval = 0;
1086
  mtp_handle_t mh;
1087

1088 1089 1090 1091 1092
  /*
   * NB: We remove the handler before adding the new one because elvin calls
   * FD_CLR(fd, ...) here and we do not want it clearing the bit after it was
   * set by the add.
   */
1093
  elvin_sync_remove_io_handler(handler, eerror);
1094

1095 1096 1097 1098 1099 1100 1101
  if ((mh = mtp_create_handle(fd)) == NULL) {
    error("mtp_create_handle\n");
  }
  else if (((rc = mtp_receive_packet(mh, mp)) != MTP_PP_SUCCESS) ||
	   (mp->vers != MTP_VERSION) ||
	   (mp->data.opcode != MTP_CONTROL_INIT)) {
    error("invalid client %d %p\n", rc, mp);
1102 1103 1104 1105
  }
  else {
    switch (mp->role) {
    case MTP_ROLE_RMC:
1106
      if (rmc_data.handle != NULL) {
1107 1108
	error("rmc client is already connected\n");
      }
1109 1110 1111 1112 1113 1114 1115 1116 1117 1118 1119
      else if ((retval = mtp_send_packet(mh, &config_rmc)) != MTP_PP_SUCCESS) {
	error("unable to send rmc_config packet");
      }
      else if (elvin_sync_add_io_handler(NULL,
					 fd,
					 ELVIN_READ_MASK,
					 rmc_callback,
					 &rmc_data,
					 eerror) == NULL) {
	error("unable to add rmc_callback handler");
      }
1120
      else {
1121
	robot_list_item_t *rli;
1122

1123 1124
	if (debug) {
	  info("established rmc connection\n");
1125 1126
	}

1127 1128 1129
	// add descriptor to list, etc:
	if (rmc_data.position_list == NULL)
	  rmc_data.position_list = robot_list_create();
1130

1131 1132 1133 1134
	for (rli = hostname_list->head; rli != NULL; rli = rli->next) {
	  struct emc_robot_config *erc = rli->data;
	  struct mtp_packet gmp;

1135 1136 1137 1138 1139 1140 1141 1142 1143 1144 1145 1146 1147 1148 1149 1150 1151 1152 1153 1154
	  if (erc->flags & ERF_HAS_GOAL) {
	    mtp_init_packet(&gmp,
			    MA_Opcode, MTP_COMMAND_GOTO,
			    MA_Role, MTP_ROLE_EMC,
			    MA_CommandID, 1,
			    MA_RobotID, erc->id,
			    MA_Position, &erc->last_goal_pos,
			    MA_TAG_DONE);
	  }
	  else {
	    mtp_init_packet(&gmp,
			    MA_Opcode, MTP_COMMAND_GOTO,
			    MA_Role, MTP_ROLE_EMC,
			    MA_CommandID, 1,
			    MA_RobotID, erc->id,
			    MA_X, (double)erc->init_x,
			    MA_Y, (double)erc->init_y,
			    MA_Theta, (double)erc->init_theta,
			    MA_TAG_DONE);
	  }
1155 1156
	  mtp_send_packet(mh, &gmp);
	}
1157

1158 1159
	rmc_data.handle = mh;
	mh = NULL;
1160

1161
	if (rmc_data.handle->mh_remaining > 0) {
1162
	  // XXX run the callbacks here to catch any queued data.
1163
	}
1164

1165
	retval = 1;
1166 1167 1168
      }
      break;
    case MTP_ROLE_EMULAB:
1169
      if (emulab_handle != NULL) {
1170 1171 1172 1173 1174 1175 1176 1177
	error("emulab client is already connected\n");
      }
      else if (elvin_sync_add_io_handler(NULL,
					 fd,
					 ELVIN_READ_MASK,
					 emulab_callback,
					 NULL,
					 eerror) == NULL) {
Timothy Stack's avatar
Timothy Stack committed
1178
	error("unable to add elvin io handler");
1179 1180
      }
      else {
1181 1182 1183
	if (debug) {
	  info("established emulab connection\n");
	}
1184 1185 1186

	emulab_handle = mh;
	mh = NULL;
1187

1188 1189 1190 1191
	retval = 1;
      }
      break;
    case MTP_ROLE_VMC:
1192
      if (vmc_data.handle != NULL) {
1193 1194
	error("vmc client is already connected\n");
      }
1195 1196 1197 1198 1199 1200 1201 1202 1203 1204 1205
      else if ((retval = mtp_send_packet(mh, &config_vmc)) != MTP_PP_SUCCESS) {
	error("unable to send rmc_config packet");
      }
      else if (elvin_sync_add_io_handler(NULL,
					 fd,
					 ELVIN_READ_MASK,
					 vmc_callback,
					 &vmc_data,
					 eerror) == NULL) {
	error("unable to add vmc_callback handler");
      }
1206
      else {
1207 1208
	if (debug) {
	  info("established vmc connection\n");
Timothy Stack's avatar
Timothy Stack committed
1209 1210
	}

1211 1212
	vmc_data.handle = mh;
	mh = NULL;
1213

1214 1215 1216
	// add descriptor to list, etc:
	if (vmc_data.position_list == NULL)
	  vmc_data.position_list = robot_list_create();
1217

1218
	retval = 1;
1219 1220 1221 1222 1223 1224 1225 1226 1227 1228
      }
      break;
    default:
      error("unknown role %d\n", mp->role);
      break;
    }
  }

  mtp_free_packet(mp);
  mp = NULL;
1229

1230 1231
  mtp_delete_handle(mh);
  mh = NULL;
1232

1233 1234
  if (!retval) {
    info("dropping bad connection %d\n", fd);
1235

1236 1237 1238
    close(fd);
    fd = -1;
  }
1239

1240 1241 1242 1243 1244 1245 1246 1247
  return retval;
}

int rmc_callback(elvin_io_handler_t handler,
		 int fd,
		 void *rock,
		 elvin_error_t eerror)
{
1248 1249


1250
  mtp_packet_t mp_data, *mp = &mp_data;
1251 1252 1253
  struct rmc_client *rmc = rock;
  int rc, retval = 0;

1254
  do {
1255 1256 1257
    if (((rc = mtp_receive_packet(rmc->handle, mp)) != MTP_PP_SUCCESS) ||
	(mp->vers != MTP_VERSION)) {
      error("invalid rmc client %d %p\n", rc, mp);
1258
    }
1259 1260 1261 1262 1263 1264 1265 1266 1267 1268 1269 1270 1271 1272 1273 1274 1275
    else {
      if (0 || debug) {
	fprintf(stderr, "rmc_callback: ");
	mtp_print_packet(stderr, mp);
      }

      switch (mp->data.opcode) {
      case MTP_REQUEST_POSITION:
	{
	  // find the latest position data for the robot:
	  //   supply init pos if no positions in rmc_data or vmc_data;
	  //   otherwise, take the position with the most recent timestamp
	  //     from rmc_data or vmc_data
	  int my_id = mp->data.mtp_payload_u.request_position.robot_id;
	  struct mtp_update_position *up;
	  struct emc_robot_config *erc;

1276

1277 1278 1279 1280 1281
	  erc = robot_list_search(hostname_list, my_id);

	  up = (struct mtp_update_position *)
	    robot_list_search(vmc_data.position_list, my_id);

1282 1283 1284 1285 1286 1287 1288 1289 1290 1291 1292 1293 1294 1295 1296 1297 1298 1299 1300 1301 1302 1303 1304
      if (0 || debug) {
          if (erc->flags & ERF_HAS_GOAL) {
              info("ERF_HAS_GOAL is set for this robot\n");
          }
          else {
              info("ERF_HAS_GOAL is not set for this robot\n");
          }
      }

	  if ((up != NULL && up->status != MTP_POSITION_STATUS_ERROR) &&
          !(erc->flags & ERF_HAS_GOAL)) {

          if (0 || debug) {
              info("Sending STATUS_UNKNOWN UPDATE_POSITION (ERF_HAS_GOAL not set)\n");
          }

          mtp_send_packet2(rmc->handle,
                           MA_Opcode, MTP_UPDATE_POSITION,
                           MA_Role, MTP_ROLE_EMC,
                           MA_RobotID, up->robot_id,
                           MA_Position, &up->position,
                           MA_Status, MTP_POSITION_STATUS_UNKNOWN,
                           MA_TAG_DONE);
1305 1306 1307 1308 1309 1310 1311 1312 1313 1314
	  }
	  else if (up == NULL) {
	    up = (struct mtp_update_position *)
	      calloc(1, sizeof(struct mtp_update_position));
	    up->robot_id = my_id;
	    up->status = MTP_POSITION_STATUS_ERROR;
	    robot_list_append(vmc_data.position_list, my_id, up);
	  }

	  retval = 1;
1315
	}
1316 1317 1318 1319 1320 1321 1322 1323 1324 1325 1326 1327 1328 1329
	break;

      case MTP_UPDATE_POSITION:
	{
	  // store the latest data in the robot position/status list
	  // in rmc_data
	  int my_id = mp->data.mtp_payload_u.update_position.robot_id;
	  struct mtp_update_position *up = (struct mtp_update_position *)
	    robot_list_remove_by_id(rmc->position_list, my_id);
	  struct emc_robot_config *erc;

	  free(up);
	  up = NULL;

1330
	  up = (struct mtp_update_position *)
1331 1332 1333
	    malloc(sizeof(struct mtp_update_position));
	  *up = mp->data.mtp_payload_u.update_position;
	  robot_list_append(rmc->position_list, my_id, up);
1334

1335 1336 1337 1338 1339 1340 1341 1342 1343 1344 1345
	  erc = robot_list_search(hostname_list, my_id);

	  info("update %d\n", my_id);

	  switch (mp->data.mtp_payload_u.update_position.status) {
	  case MTP_POSITION_STATUS_ERROR:
	  case MTP_POSITION_STATUS_COMPLETE:
	    if (emulab_handle != NULL) {
	      mtp_send_packet(emulab_handle, mp);
	    }
	    if (erc->token != ~0) {
1346
#if defined(TBDB_EVENTTYPE_COMPLETE)
1347 1348 1349 1350 1351 1352 1353 1354 1355 1356
	      event_do(handle,
		       EA_Experiment, pideid,
		       EA_Type, TBDB_OBJECTTYPE_NODE,
		       EA_Name, erc->vname,
		       EA_Event, TBDB_EVENTTYPE_COMPLETE,
		       EA_ArgInteger, "ERROR",
		       mp->data.mtp_payload_u.update_position.status ==
		       MTP_POSITION_STATUS_ERROR ? 1 : 0,
		       EA_ArgInteger, "CTOKEN", erc->token,
		       EA_TAG_DONE);
1357
#endif
1358 1359
	      erc->token = ~0;
	    }
1360

1361 1362
	    /* unset ERF_HAS_GOAL flag */
	    erc->flags &= ~ERF_HAS_GOAL;
1363

1364 1365 1366 1367 1368 1369 1370 1371 1372 1373 1374 1375
	    info("Received STATUS COMPLETE from %d\n", my_id);


	    break;
	  default:
	    assert(0);
	    break;
	  }

	  // also, if status is MTP_POSITION_STATUS_COMPLETE ||
	  // MTP_POSITION_STATUS_ERROR, notify emulab, or issue the next
	  // command to rmc from the movement list.
1376

1377 1378 1379 1380 1381 1382 1383 1384
	  // later ....

	  retval = 1;
	}
	break;

      case MTP_WIGGLE_STATUS:
	{
1385 1386 1387
	  /* simply forward this to vmc */
	  /* actually, we'll also ack it back to rmc */
	  mp->role = MTP_ROLE_EMC;
1388

1389
	  if (mtp_send_packet(vmc_data.handle,mp) != MTP_PP_SUCCESS) {
1390
	    error("vmc unavailable; cannot forward wiggle-status\n");
1391
	  }
1392

1393
	  retval = 1;
1394 1395
      }
      break;
1396

1397
    case MTP_CREATE_OBSTACLE:
1398
	{
1399 1400 1401 1402 1403 1404 1405 1406 1407 1408 1409 1410 1411 1412 1413 1414 1415 1416 1417 1418 1419 1420 1421 1422 1423 1424 1425 1426 1427 1428 1429 1430 1431 1432 1433 1434 1435
	    struct dyn_obstacle_config *doc;
	    struct emc_robot_config *erc;

	    doc = &mp->data.mtp_payload_u.create_obstacle;
	    erc = robot_list_search(hostname_list, doc->robot_id);

	    doc->config.xmin += OBSTACLE_BUFFER;
	    doc->config.ymin += OBSTACLE_BUFFER;
	    doc->config.xmax -= OBSTACLE_BUFFER;
	    doc->config.ymax -= OBSTACLE_BUFFER;
	    if (erc != NULL) {
		event_do(handle,
			 EA_Experiment, pideid,
			 EA_Type, TBDB_OBJECTTYPE_TOPOGRAPHY,
			 EA_Event, TBDB_EVENTTYPE_CREATE,
			 EA_Name, topography_name,
			 EA_ArgInteger, "ID", doc->config.id,
			 EA_ArgFloat, "XMIN", doc->config.xmin,
			 EA_ArgFloat, "YMIN", doc->config.ymin,
			 EA_ArgFloat, "XMAX", doc->config.xmax,
			 EA_ArgFloat, "YMAX", doc->config.ymax,
			 EA_ArgString, "ROBOT", erc->vname,
			 EA_TAG_DONE);
	    }
	    else {
		event_do(handle,
			 EA_Experiment, pideid,
			 EA_Type, TBDB_OBJECTTYPE_TOPOGRAPHY,
			 EA_Event, TBDB_EVENTTYPE_CREATE,
			 EA_Name, topography_name,
			 EA_ArgInteger, "ID", doc->config.id,
			 EA_ArgFloat, "XMIN", doc->config.xmin,
			 EA_ArgFloat, "YMIN", doc->config.ymin,
			 EA_ArgFloat, "XMAX", doc->config.xmax,
			 EA_ArgFloat, "YMAX", doc->config.ymax,
			 EA_TAG_DONE);
	    }
1436

1437
	    retval = 1;
1438 1439
	}
	break;
1440 1441 1442 1443 1444 1445 1446 1447 1448 1449 1450 1451 1452 1453 1454 1455 1456 1457 1458 1459 1460 1461 1462 1463

      case MTP_UPDATE_OBSTACLE:
	{
	  struct obstacle_config *oc;

	  oc = &mp->data.mtp_payload_u.update_obstacle;
	  /* XXX Hack */
	  event_do(handle,
		   EA_Experiment, pideid,
		   EA_Type, TBDB_OBJECTTYPE_TOPOGRAPHY,
		   EA_Event, TBDB_EVENTTYPE_MODIFY,
		   EA_Name, topography_name,
		   EA_ArgInteger, "ID", oc->id,
		   EA_ArgFloat, "XMIN", oc->xmin + 0.25,
		   EA_ArgFloat, "YMIN", oc->ymin + 0.25,
		   EA_ArgFloat, "XMAX", oc->xmax - 0.25,
		   EA_ArgFloat, "YMAX", oc->ymax - 0.25,
		   EA_TAG_DONE);

	  retval = 1;
	}
	break;

      case MTP_REMOVE_OBSTACLE:
1464 1465
	event_do(handle,
		 EA_Experiment, pideid,
1466
		 EA_Type, TBDB_OBJECTTYPE_TOPOGRAPHY,
1467
		 EA_Event, TBDB_EVENTTYPE_CLEAR,
1468
		 EA_Name, topography_name,
1469 1470
		 EA_ArgInteger, "ID", mp->data.mtp_payload_u.remove_obstacle,
		 EA_TAG_DONE);
1471

1472 1473
	retval = 1;
	break;
1474 1475 1476 1477 1478 1479 1480 1481 1482 1483 1484 1485 1486 1487 1488 1489 1490 1491 1492 1493

      case MTP_TELEMETRY:
	retval = 1;
	break;

      default:
	{
	  struct mtp_packet mp_reply;

	  error("received bogus opcode from RMC: %d\n", mp->data.opcode);

	  mtp_init_packet(&mp_reply,
			  MA_Opcode, MTP_CONTROL_ERROR,
			  MA_Role, MTP_ROLE_EMC,
			  MA_Message, "protocol error: bad opcode",
			  MA_TAG_DONE);

	  mtp_send_packet(rmc->handle, &mp_reply);
	}
	break;
1494 1495
      }
    }
1496

1497
    mtp_free_packet(mp);
1498
  } while (retval && (rmc->handle->mh_remaining > 0));
1499

1500
  if (!retval) {
Timothy Stack's avatar
Timothy Stack committed
1501
    info("dropping rmc connection %d\n", fd);
1502

1503
    elvin_sync_remove_io_handler(handler, eerror);
1504

1505 1506
    mtp_delete_handle(rmc->handle);
    rmc->handle = NULL;
1507

1508 1509
    close(fd);
    fd = -1;
1510
  }
1511

1512 1513 1514 1515 1516 1517 1518 1519
  return retval;
}

int emulab_callback(elvin_io_handler_t handler,
		    int fd,
		    void *rock,
		    elvin_error_t eerror)
{
1520
  mtp_packet_t mp_data, *mp = &mp_data;
Timothy Stack's avatar
Timothy Stack committed
1521
  int rc, retval = 0;
1522

1523 1524 1525
  struct emc_robot_config *match = NULL;
  robot_list_item_t *rli;

1526

1527
  do {
1528 1529 1530
    if (((rc = mtp_receive_packet(emulab_handle, mp)) != MTP_PP_SUCCESS) ||
	(mp->vers != MTP_VERSION)) {
      error("invalid client %p\n", mp);
1531
    }
1532 1533 1534 1535 1536 1537 1538 1539
    else {
      if (debug) {
	fprintf(stderr, "emulab_callback: ");
	mtp_print_packet(stderr, mp);
      }

      switch (mp->data.opcode) {
      case MTP_COMMAND_GOTO:
1540 1541 1542 1543