ndirect.c

TeamBots 是一个可移植的多代理机器人仿真器

 *****************************/

/*
 * dummy function to maintain compatibility with Nclient
 *
 * create_robot - requests the server to create a robot with
 *                id = robot_id and establishes a connection with
 *                the robot. This function is disabled in this
 *                version of the software.
 * 
 * parameters:
 *    long robot_id -- id of the robot to be created. The robot
 *                     will be referred to by this id. If a process
 *                     wants to talk (connect) to a robot, it must
 *                     know its id.
 */
int create_robot ( long robot_id )
{
  Robot_id = robot_id;
  return ( TRUE );
}

/* Helper function for connect_robot */
static char *convertAddr ( char *name, char *addr )
{
  int addrInt[10];

  sscanf(name, "%d.%d.%d.%d", 
	 &(addrInt[0]), &(addrInt[1]), &(addrInt[2]), &(addrInt[3]));
  addr[0] = addrInt[0];
  addr[1] = addrInt[1];
  addr[2] = addrInt[2];
  addr[3] = addrInt[3];
  return ( addr );
}

int open_serial(unsigned char port, unsigned short baud)
{
  char *dev_name;
  struct termios info;  

  switch (port)
    {
    case 1:
      dev_name="/dev/ttyS0";
      break;
    case 2:
      dev_name="/dev/ttyS1";
      break;
    case 3:
      dev_name="/dev/ttyS2";
      break;
    case 4:
      dev_name="/dev/ttyS3";
      break;
    case 5:
      dev_name="/dev/ttyS4";
      break;
    case 6:
      dev_name="/dev/ttyS5";
      break;
    }
    
  if (Fd == 0)
    close(Fd);
  if ((Fd=open(dev_name, O_RDWR|O_NONBLOCK)) < 0)
    {
      perror("Error opening serial port");
      return 0;
    }
  
  if (tcgetattr(Fd, &info) < 0) 
    {
      perror("Error using TCGETS in ioctl.");
      close(Fd);
      return 0;
    }
  
  /* restore old values to unhang the bastard, if hung */
  info.c_iflag=1280;
  info.c_oflag=5;
  info.c_cflag=3261;
  info.c_lflag=35387;
  
  if (tcsetattr(Fd, TCSANOW, &info) < 0) 
    { 
      perror("Error using TCSETS in ioctl.");
      close(Fd);
      return 0;
    } 
  close(Fd);
  
  if ((Fd = open(dev_name, O_RDWR)) == -1) {
    perror("Error opening serial port");
    errorp = SERIAL_OPEN_ERROR;
    close(Fd);
    return(FALSE);
  }
  
  if (tcgetattr(Fd,&info) < 0) {
    perror("Error using TCGETS in ioctl.");
    errorp = SERIAL_OPEN_ERROR;
    close(Fd);
    return(FALSE);
  }
  
  info.c_iflag = 0;
  info.c_oflag = 0;
  info.c_lflag = 0;
  switch (baud) { /* serial port rate */
  case 4800:
    info.c_cflag = B4800 | CS8 | CREAD | CLOCAL;
    break;
  case 9600:
    info.c_cflag = B9600 | CS8 | CREAD | CLOCAL;
    break;
  case 19200:
    info.c_cflag = B19200 | CS8 | CREAD | CLOCAL;
    break;
  case 38400:
    info.c_cflag = B38400 | CS8 | CREAD | CLOCAL;
    break;
  default:
    fprintf(stderr, "Invalid baud rate %d; using %d.\n",
	    baud, SERIAL_BAUD);
    baud=SERIAL_BAUD;
    info.c_cflag = B9600 | CS8 | CREAD | CLOCAL;
    break;
  }
  /* set time out on serial read */
  info.c_cc[VMIN] = 0;
  info.c_cc[VTIME] = 10; 
  wait_time = NORMAL_TIMEOUT;
  
  if (tcsetattr(Fd,TCSANOW,&info) < 0) { 
    perror("Error using TCSETS in ioctl.");
    errorp = SERIAL_OPEN_ERROR;
    close(Fd);
    return(FALSE);
  }
  
  printf("Robot <-> Host serial communication setup\n");
  printf("(%d baud using %s)\n", baud, dev_name);
}

/*
 * connect_robot - requests the server to connect to the robot
 *                 with id = robot_id. In order to talk to the server,
 *                 the SERVER_MACHINE_NAME and SERV_TCP_PORT must be
 *                 set properly. If a robot with robot_id exists,
 *                 a connection is established with that robot. If
 *                 no robot exists with robot_id, no connection is
 *                 established. In this single robot version, the robot_id
 *                 is unimportant. You can call connect_robot with any
 *                 robot_id, it will connect to the robot.
 *
 * parameters:
 *    long robot_id -- robot's id. In this multiple robot version, in order
 *                     to connect to a robot, you must know it's id.
 */
int connect_robot(long robot_id)
{
  struct hostent *hp;
  struct sockaddr_in serv_addr;
  int ret, retlen, i;
  unsigned char ir_mask[16],sn_mask[16],cf_mask[4],vl_mask[3];
  unsigned char cp_mask,bp_mask,ls_mask,pos_mask, byte;
  char addr[10];

  if (CONN_TYPE == 0 || CONN_TYPE > 2) {
    fprintf(stderr, "Communication type selected for robot ID %ld is invalid.\n"
            "Known types are serial (1) and TCP/IP (2).\n", robot_id);
    return FALSE;
  }
  
  if (CONN_TYPE == 1) 
  {
    open_serial(DEFAULT_COMPORT, SERIAL_BAUD);
    /* Send init_sensors to make sure that server and robot are synchronized */
    init_sensors();
  } else {
    if (ROBOT_MACHINE_NAME[0] == 0)
      strcpy(ROBOT_MACHINE_NAME, "localhost");
    if ( ((hp = gethostbyname(ROBOT_MACHINE_NAME)) == NULL) &&
	 ((hp = gethostbyaddr(convertAddr(ROBOT_MACHINE_NAME,addr),4,AF_INET)) 
	  == NULL) ){
      fprintf(stderr, "Machine %s not valid.\n", ROBOT_MACHINE_NAME);
      return FALSE;
    }
    memset((char *) &serv_addr, 0, sizeof(serv_addr));
    memcpy((char *) &(serv_addr.sin_addr), hp->h_addr, hp->h_length);
    serv_addr.sin_family = AF_INET;            /* address family */
    /* TCP port number */
    serv_addr.sin_port = htons(ROBOT_TCP_PORT);
    
    if ((Fd = socket(AF_INET, SOCK_STREAM, 0)) < 0) {
      fprintf(stderr, "Error: in open_socket_to_send_data, socket failed.\n");
      return FALSE;
    }
    fcntl(Fd, F_SETFL, O_NDELAY);
    if (connect(Fd, (struct sockaddr *) &serv_addr, sizeof(serv_addr)) < 0) {
      if (errno == EINPROGRESS) {
        fd_set          lfdvar;
        struct timeval  timeout;
        
        FD_ZERO(&lfdvar);
        FD_SET(Fd, &lfdvar);
        
        timeout.tv_sec = (long)(NORMAL_TIMEOUT/100);
        timeout.tv_usec = (long)((NORMAL_TIMEOUT%100)*10000);
        
        if (select(Fd+1, NULL, &lfdvar, NULL, &timeout) == 0) {
          fprintf(stderr, "Error: connect timed out.\n");
          close(Fd);
          return FALSE;
        } else {
	  errno = 0;
	  retlen = 4;
	  if (getsockopt(Fd, SOL_SOCKET, SO_ERROR, (char *)&ret, &retlen) == 0)
	  {
	    if (ret != 0)
	      errno = ret;
	    if (errno != 0)
	    {
	      perror("Error: connect failed");
	      close(Fd);
	      return FALSE;
	    }
	  }
	}
      } else {
        perror("Error: connect failed");
        close(Fd);
        return FALSE;
      }
    }
    
    wait_time = NORMAL_TIMEOUT;
    
    printf("Robot <-> Host TCP/IP communication setup\n");
    printf("(machine %s on port %d)\n", ROBOT_MACHINE_NAME, ROBOT_TCP_PORT);
    
    /* Read configuration data */
    byte = GETC(Fd, CONN_TYPE);
    ir_mask[ 0] = byte & (1 << 0) ? 1 : 0;
    ir_mask[ 1] = byte & (1 << 1) ? 1 : 0;
    ir_mask[ 2] = byte & (1 << 2) ? 1 : 0;
    ir_mask[ 3] = byte & (1 << 3) ? 1 : 0;
    ir_mask[ 4] = byte & (1 << 4) ? 1 : 0;
    ir_mask[ 5] = byte & (1 << 5) ? 1 : 0;
    ir_mask[ 6] = byte & (1 << 6) ? 1 : 0;
    ir_mask[ 7] = byte & (1 << 7) ? 1 : 0;
    byte = GETC(Fd, CONN_TYPE);
    ir_mask[ 8] = byte & (1 << 0) ? 1 : 0;
    ir_mask[ 9] = byte & (1 << 1) ? 1 : 0;
    ir_mask[10] = byte & (1 << 2) ? 1 : 0;
    ir_mask[11] = byte & (1 << 3) ? 1 : 0;
    ir_mask[12] = byte & (1 << 4) ? 1 : 0;
    ir_mask[13] = byte & (1 << 5) ? 1 : 0;
    ir_mask[14] = byte & (1 << 6) ? 1 : 0;
    ir_mask[15] = byte & (1 << 7) ? 1 : 0;
    byte = GETC(Fd, CONN_TYPE);
    sn_mask[ 0] = byte & (1 << 0) ? 1 : 0;
    sn_mask[ 1] = byte & (1 << 1) ? 1 : 0;
    sn_mask[ 2] = byte & (1 << 2) ? 1 : 0;
    sn_mask[ 3] = byte & (1 << 3) ? 1 : 0;
    sn_mask[ 4] = byte & (1 << 4) ? 1 : 0;
    sn_mask[ 5] = byte & (1 << 5) ? 1 : 0;
    sn_mask[ 6] = byte & (1 << 6) ? 1 : 0;
    sn_mask[ 7] = byte & (1 << 7) ? 1 : 0;
    byte = GETC(Fd, CONN_TYPE);
    sn_mask[ 8] = byte & (1 << 0) ? 1 : 0;
    sn_mask[ 9] = byte & (1 << 1) ? 1 : 0;
    sn_mask[10] = byte & (1 << 2) ? 1 : 0;
    sn_mask[11] = byte & (1 << 3) ? 1 : 0;
    sn_mask[12] = byte & (1 << 4) ? 1 : 0;
    sn_mask[13] = byte & (1 << 5) ? 1 : 0;
    sn_mask[14] = byte & (1 << 6) ? 1 : 0;
    sn_mask[15] = byte & (1 << 7) ? 1 : 0;
    byte = GETC(Fd, CONN_TYPE);
    bp_mask    = byte & (1 << 0) ? 1 : 0;
    cf_mask[0] = byte & (1 << 1) ? 1 : 0;
    cf_mask[1] = byte & (1 << 2) ? 1 : 0;
    cf_mask[2] = byte & (1 << 3) ? 1 : 0;
    cf_mask[3] = byte & (1 << 4) ? 1 : 0;
    vl_mask[0] = byte & (1 << 5) ? 1 : 0;
    vl_mask[1] = byte & (1 << 6) ? 1 : 0;
    vl_mask[2] = byte & (1 << 7) ? 1 : 0;
    byte = GETC(Fd, CONN_TYPE);
    cp_mask = byte & 1;
    byte = GETC(Fd, CONN_TYPE);
    ls_mask = byte & 1;
    pos_mask = byte >> 1;
    
    usedSmask[0] = pos_mask;
    for (i = 0; i < 16; i++)
      usedSmask[i+1] = ir_mask[i];
    for (i = 0; i < 16; i++)
      usedSmask[i+17] = sn_mask[i];
    usedSmask[33] = bp_mask;
    for (i = 0; i < 4; i++)
      usedSmask[i+34] = cf_mask[i];
    for (i = 0; i < 3; i++)
      usedSmask[i+38] = vl_mask[i];
    usedSmask[42] = ls_mask;
    usedSmask[43] = cp_mask;
    
    /* get laser mode, num_points, processing */
    byte = GETC(Fd, CONN_TYPE);
    laser_mode = byte;
    byte = GETC(Fd, CONN_TYPE);
    byte = GETC(Fd, CONN_TYPE);
    byte = GETC(Fd, CONN_TYPE);
  }
  
  return TRUE;
}

/*
 * dummy function to maintain compatibility with Nclient
 *
 * disconnect_robot - requests the server to close the connect with robot
 *                    with id = robot_id. 
 *
 * parameters:
 *    long robot_id -- robot's id. In order to disconnect a robot, you
 *                     must know it's id.
 */
int disconnect_robot(long robot_id)
{
  Robot_id = -1;
  return ( TRUE );
}

/* 
 * ac - sets accelerations of the robot. Currently it has no effect in 
 *      simulation mode.
 *
 * parameters:
 *    int t_ac, s_ac, r_ac -- the translation, steering, and turret
 *                            accelerations. t_ac is in 1/10 inch/sec^2
 *                            s_ac and r_ac are in 1/10 degree/sec^2.
 */
int ac(int t_ac, int s_ac, int r_ac)
{
  unsigned char outbuf[BUFSIZE];
  
  stuff_three_unsigned_int(8, AC, t_ac, s_ac, r_ac, outbuf);
  return(Comm_Robot(Fd, outbuf));  
}

/*
 * sp - sets speeds of the robot, this function will not cause the robot to
 *      move. However, the set speed will be used when executing a pr()
 *      or a pa().
 *
 * parameters:
 *    int t_sp, s_sp, r_sp -- the translation, steering, and turret
 *                            speeds. t_sp is in 1/10 inch/sec and
 *                            s_sp and r_sp are in 1/10 degree/sec.
 */
int sp(int t_sp, int s_sp, int r_sp)
{
  unsigned char outbuf[BUFSIZE];
  
  stuff_three_unsigned_int(8, SP, t_sp, s_sp, r_sp, outbuf);
  return(Comm_Robot(Fd, outbuf));
}

/*
 * pr - moves the motors of the robot by a relative distance, using the speeds
 *      set by sp(). The three parameters specify the relative distances for
 *      the three motors: translation, steering, and turret. All the three
 *      motors move concurrently if the speeds are not set to zero and the 
 *      distances to be traveled are non-zero. Depending on the timeout 
 *      period set (by function conf_tm(timeout)), the motion may 
 *      terminate before the robot has moved the specified distances
 *
 * parameters:
 *    int t_pr, s_pr, r_pr -- the specified relative distances of the
 *                            translation, steering, and turret motors.
 *                            t_pr is in 1/10 inch and s_pr and r_pr are
 *                            in 1/10 degrees.
 */
int pr(int t_pr, int s_pr, int r_pr)
{
  unsigned char outbuf[BUFSIZE];
  
  stuff_three_signed_int(8, PR, t_pr, s_pr, r_pr, outbuf);
  return(Comm_Robot(Fd, outbuf));
}

/*
 * pa - moves the motors of the robot to the specified absolute positions 
 *      using the speeds set by sp().  Depending on the timeout period set 
 *      (by conf_tm()), the motion may terminate before the robot has 
 *      moved to the specified positions.
 *
 * parameters:
 *    int t_pa, s_pa, r_pa -- the specified absolute positions of the
 *                            translation, steering, and turret motors.
 *                            t_pa is in 1/10 inch and s_pa and r_pa are
 *                            in 1/10 degrees.
 */
int pa(int t_pa, int s_pa, int r_pa);
int pa(int t_pa, int s_pa, int r_pa)
{
  unsigned char outbuf[BUFSIZE];
  
  stuff_three_signed_int(8, PA, t_pa, s_pa, r_pa, outbuf);
  return(Comm_Robot(Fd, outbuf));
}

/*
 * vm - velocity mode, command the robot to move at translational
 *      velocity = tv, steering velocity = sv, and rotational velocity =
 *      rv. The robot will continue to move at these velocities until
 *      either it receives another command or this command has been
 *      timeout (in which case it will stop its motion).
 *
 * parameters: 
 *    int t_vm, s_vm, r_vm -- the desired translation, steering, and turret
 *                            velocities. tv is in 1/10 inch/sec and
 *                            sv and rv are in 1/10 degree/sec.
 */
int vm(t_v, s_v, r_v)
    int t_v, s_v, r_v;
{
  unsigned char outbuf[BUFSIZE];
  
  stuff_three_signed_int(8, VM, t_v, s_v, r_v, outbuf);
  return(Comm_Robot(Fd, outbuf));
}

/*
 * mv - move, send a generalized motion command to the robot.
 *      For each of the three axis (translation, steering, and
 *      turret) a motion mode (t_mode, s_mode, r_mode) can be 
 *      specified (using the values MV_IGNORE, MV_AC, MV_SP,
 *      MV_LP, MV_VM, and MV_PR defined above):
 *
 *         MV_IGNORE : the argument for this axis is ignored
 *                     and the axis's motion will remain 
 *                     unchanged.
 *         MV_AC :     the argument for this axis specifies
 *                     an acceleration value that will be used
 *                     during motion commands.
 *         MV_SP :     the argument for this axis specifies
 *                     a speed value that will be used during
 *                     position relative (PR) commands.
 *         MV_LP :     the arguemnt for this axis is ignored
 *                     but the motor is turned off.
 *         MV_VM :     the argument for this axis specifies
 *                     a velocity and the axis will be moved
 *                     with this velocity until a new motion
 *                     command is issued (vm,pr,mv) or 
 *                     recieves a timeout.
 *         MV_PR :     the argument for this axis specifies
 *                     a position and the axis will be moved
 *                     to this position, unless this command
 *                     is overwritten by another (vm,pr,mv).
 *
 * parameters: 
 *    int t_mode - the desired mode for the tranlation axis
 *    int t_mv   - the value for that axis, velocity or position,
 *                 depending on t_mode
 *    int s_mode - the desired mode for the steering axis
 *    int s_mv   - the value for that axis, velocity or position,
 *                 depending on t_mode
 *    int r_mode - the desired mode for the turret axis
 *    int r_mv   - the value for that axis, velocity or position,
 *                 depending on t_mode
 */
int mv(int t_mode, int t_mv, int s_mode, 
       int s_mv, int r_mode, int r_mv) 
{
  unsigned char outbuf[BUFSIZE];
  unsigned char *ptr;

  ptr = outbuf;
  
  /* skip the first byte for begin package */
  ptr++;