emcd.c

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

#include "config.h"

#include <stdio.h>
#include <math.h>
#include <assert.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <unistd.h>
#include <stdlib.h>
#include <signal.h>
#include <paths.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <errno.h>
#include <string.h>
#include <netdb.h>
#include <sys/socket.h>

#include "log.h"
#include "tbdefs.h"
#include "event.h"
#include <elvin/elvin.h>

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

#include "emcd.h"

#ifndef __XSTRING
#define __XSTRING(x) __STRING(x)
#endif

static event_handle_t handle;

static char *pideid;

static char *progname;

static int debug;

static int port = EMC_SERVER_PORT;
static int command_seq_no = 0;

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

static elvin_error_t elvin_error;

static struct mtp_packet config_rmc;
static struct mtp_packet config_vmc;

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


static struct rmc_client rmc_data;
static struct vmc_client vmc_data;
static mtp_handle_t emulab_handle = NULL;


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

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

static int update_callback(elvin_timeout_t timeout,
			   void *rock,
			   elvin_error_t eerror);

static void parse_config_file(char *filename);
static void parse_movement_file(char *filename);

static char *unix_path = NULL;

#if defined(SIGINFO)
/* SIGINFO-related stuff */

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

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

static void dump_info(void)
{
  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);
  }
}
#endif

static void emcd_atexit(void)
{
  if (unix_path != NULL)
    unlink(unix_path);
}

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

static void sigpanic(int sig)
{
  char buf[32];

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

static void usage(char *progname)
{
  fprintf(stderr,
	  "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"
	  "  -U path\tUnix socket path\n"
	  "  -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");
}

int main(int argc, char *argv[])
{
  char pid[MAXHOSTNAMELEN], eid[MAXHOSTNAMELEN];
  int c, emc_sock, retval = EXIT_SUCCESS;
  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;
  char buf[BUFSIZ], buf2[BUFSIZ];
  char *idx;
  FILE *fp;

  progname = argv[0];
  
  while ((c = getopt(argc, argv, "hde:k:p:c:f:s:P:U:l:i:")) != -1) {
    switch (c) {
    case 'h':
      usage(progname);
      exit(0);
      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;
    case 'U':
      unix_path = optarg;
      break;
    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;
    }
  }
  
  argc -= optind;
  argv += optind;
  
  // initialize the global lists
  hostname_list = robot_list_create();
  initial_position_list = robot_list_create();
  position_queue = robot_list_create();

  g_camera_config = NULL;
  g_obstacle_config = NULL;
  
  /* read config file into the above lists*/
  parse_config_file(config_file);
  /* read movement file into the position queue */
  parse_movement_file(movement_file);

  if (debug) 
    loginit(0, logfile);
  else {
    /* Become a daemon */
    daemon(0, 0);
    
    if (logfile)
      loginit(0, logfile);
    else
      loginit(1, "emcd");
  }

  debug = 1;

  atexit(emcd_atexit);

  signal(SIGINT, sigquit);
  signal(SIGTERM, sigquit);
  signal(SIGQUIT, sigquit);
  
  signal(SIGSEGV, sigpanic);
  signal(SIGBUS, sigpanic);
  
  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);
  }
  
  // first, EMC server sock:
  emc_sock = mtp_bind("0.0.0.0", port, unix_path);
  if (emc_sock == -1) {
    fprintf(stdout,"Could not open socket for EMC: %s\n",strerror(errno));
    exit(1);
  }

#if defined(SIGINFO)
  signal(SIGINFO, siginfo);
#endif
  
  /*
   * 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;
  }

  if (!keyfile && pideid) {
    snprintf(buf2, sizeof(buf2), "/proj/%s/exp/%s/tbdata/eventkey", pid, eid);
    keyfile = buf2;
  }
  
  /*
   * Register with the event system. 
   */
  handle = event_register_withkeyfile(server, 0, keyfile);
  if (handle == NULL) {
    fatal("could not register with event system");
  }

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

  }
  
  if ((elvin_error = elvin_error_alloc()) == NULL) {
    fatal("could not allocate elvin error");
  }
  
  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");
  }

  if (elvin_sync_add_timeout(NULL,
			     EMC_UPDATE_HZ,
			     update_callback,
			     NULL,
			     elvin_error) == NULL) {
    fatal("could not add timeout");
  }

  while (1) {
    event_main(handle);

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

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) {
	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)) {
	    retval = 1;
	    break;
	}
    }

    return retval;
}

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

void parse_config_file(char *config_file) {
  int oc_size = 0, cc_size = 0, line_no = 0;
  struct obstacle_config *oc = NULL;
  struct camera_config *cc = NULL;
  char line[BUFSIZ];
  struct stat st;
  FILE *fp;

  if (config_file == NULL) {
    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);
  }

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

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

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

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

    if (directive[0] == '#' || directive[0] == '\0') {
      // Comment or empty line.
    }
    else if (strcmp(directive, "robot") == 0) {
      char area[32], hostname[MAXHOSTNAMELEN], vname[TBDB_FLEN_EVOBJNAME];
      int id;
      float init_x;
      float init_y;
      float init_theta;

      if (sscanf(line,
		 "%16s %32s %d %" __XSTRING(MAXHOSTNAMELEN) "s %f %f %f "
		 "%" __XSTRING(TBDB_FLEN_EVOBJNAME) "s",
		 directive,
		 area,
		 &id,
		 hostname,
		 &init_x,
		 &init_y,
		 &init_theta,
		 vname) != 8) {
	fprintf(stderr,
		"error:%d: syntax error in config file - %s\n",
		line_no,
		line);
      }
      else {
	struct emc_robot_config *rc;
	struct hostent *he;
	
	// 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;
	rc->init_x = init_x;
	rc->init_y = init_y;
	rc->init_theta = init_theta;

	robot_list_append(hostname_list, id, (void *)rc);
      }
    }
    else if (strcmp(directive, "camera") == 0) {
      char area[32], hostname[MAXHOSTNAMELEN];
      float x, y, width, height;
      int port;

      if (sscanf(line,
		 "%16s %32s %" __XSTRING(MAXHOSTNAMELEN) "s %d %f %f %f %f",
		 directive,
		 area,
		 hostname,
		 &port,
		 &x,
		 &y,
		 &width,
		 &height) != 8) {
	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;
	cc[cc_size].x = x;
	cc[cc_size].y = y;
	cc[cc_size].width = width;
	cc[cc_size].height = height;
	cc_size += 1;
      }
    }
    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;
	oc[oc_size].xmin = x1;
	oc[oc_size].ymin = y1;
	oc[oc_size].xmax = x2;
	oc[oc_size].ymax = y2;
	oc_size += 1;
      }
    }
    else {
      fprintf(stderr,
	      "error:%d: unknown directive - %s\n",
	      line_no,
	      directive);
    }
    // next line!
  }

  {
      struct robot_config *robot_val, *rc;
      struct robot_list_enum *e;
      struct box *boxes_val;
      int boxes_len;
      int lpc = 0;

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

void parse_movement_file(char *movement_file) {
  char line[BUFSIZ];
  int line_no = 0;
  FILE *fp;

  if (movement_file == NULL) {
    return;
  }

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

  // read line by line
  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;
    struct robot_position *p;
    
    ++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:
    p = (struct robot_position *)malloc(sizeof(struct robot_position *));
    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)
{
  struct emc_robot_config *match = NULL;
  char objname[TBDB_FLEN_EVOBJNAME];
  robot_list_item_t *rli;
  int in_obstacle, in_camera;
  
  if (! event_notification_get_objname(handle, notification,
				       objname, sizeof(objname))) {
    error("Could not get objname from notification!\n");
    return;
  }
  
  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");
  }
  else {
    char *value, args[BUFSIZ];
    float x, y, orientation;
    struct mtp_packet mp;
    
    event_notification_get_arguments(handle, notification, args, sizeof(args));

    /* XXX copy the current X, Y, and orientation! */
    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);
	return;
      }
    }
    
    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);
	return;
      }
    }
    
    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);
	return;
      }
    }
    
    event_notification_get_int32(handle, notification,
				 "TOKEN", (int32_t *)&match->token);

    orientation = orientation * M_PI / 180.0;

    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);
    }
    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
    }
    
    // XXX What to do with the data?
    
    // construct a COMMAND_GOTO packet and send to rmc.
    
  }
}

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