nclient.c

机器人仿真软件

}

/* 
 * process_???_reply - processes reply from the server to extract the
 *                     appropriate information 
 */
static int process_state_reply(struct reply_struct *this_reply)
{
  int i,j, num_points;
  
  if (this_reply->type == ERROR_MSG)
  {
    State[ STATE_ERROR ] = this_reply->mesg[0];
    return(FALSE);
  }
  else
  {
    /* the state vector */

    for (i=0; i<=44; i++)
      State[i] = this_reply->mesg[i];
    num_points = this_reply->mesg[45];

    /* the laser data */

    if ((laser_mode == 1) || (laser_mode == 33))
    {
      for (i=45; i<=45+4*num_points; i++)
	Laser[i-45] = this_reply->mesg[i];
    }
    else
    {
      if ((laser_mode == 51) || (laser_mode == 50) || (laser_mode == 19))
	for (i=45; i<=45+2*num_points; i++)
	  Laser[i-45] = this_reply->mesg[i];
      else
	for (i=45; i<=45+num_points; i++)
	  Laser[i-45] = this_reply->mesg[i];
    }
    State[ STATE_ERROR ] = 0;

    /* 
     * depending on what PosData attachments were required,
     * the data have to be extracted from the buffer.
     * check for each of the sensors consecutively.
     */

    /* infrared */
    if ( POS_INFRARED_P )
      for ( j = 0; j < INFRAREDS; j++ )
	i = posDataProcess ( this_reply->mesg, i, &(posDataAll.infrared[j]) );
    
    /* sonar */
    if ( POS_SONAR_P )
      for ( j = 0; j < SONARS; j++ )
	i = posDataProcess ( this_reply->mesg, i, &(posDataAll.sonar[j]) );
    
    /* bumper */
    if ( POS_BUMPER_P )
      i = posDataProcess ( this_reply->mesg, i, &(posDataAll.bumper) );
    
    /* laser */
    if ( POS_LASER_P )
      i = posDataProcess ( this_reply->mesg, i, &(posDataAll.laser) );
    
    /* compass */
    if ( POS_COMPASS_P )
      i = posDataProcess ( this_reply->mesg, i, &(posDataAll.compass) );

    /* the Intellisys 100 time */
    i = timeDataProcess ( this_reply->mesg, i, &posDataTime );

    /* the voltages for CPU and motors */
    i = voltDataProcess ( this_reply->mesg, i, &voltageCPU, &voltageMotor );

  }
  return(TRUE);
}

static int process_infrared_reply(struct reply_struct *this_reply)
{
  int i,j;
  
  if (this_reply->type == ERROR_MSG)
  {
    State[ STATE_ERROR ] = this_reply->mesg[0];
    return(FALSE);
  }
  else
  {
    for (i= STATE_IR_0 ; i <= STATE_IR_15 ; i++)
      State[i] = this_reply->mesg[i-STATE_IR_0];

    /* 
     * if position attachment was requested for infrared....
     */
    
    i -= STATE_IR_0;
    if ( POS_INFRARED_P )
      for ( j = 0; j < INFRAREDS; j++ )
	i = posDataProcess ( this_reply->mesg, i, &(posDataAll.infrared[j]) );
  }

  /* the Intellisys 100 time */
  i = timeDataProcess ( this_reply->mesg, i, &posDataTime );

  return(TRUE);
}

static int process_sonar_reply(struct reply_struct *this_reply)
{
  int i,j;
  
  if (this_reply->type == ERROR_MSG)
  {
    State[ STATE_ERROR ] = this_reply->mesg[0];
    return(FALSE);
  }
  else
  {
    for (i = STATE_SONAR_0 ; i <= STATE_SONAR_15; i++)
      State[i] = this_reply->mesg[i - STATE_SONAR_0];

    /* 
     * if position attachment was requested for sonar....
     */
    
    i -= STATE_SONAR_0;
    if ( POS_SONAR_P )
      for ( j = 0; j < SONARS; j++ )
	i = posDataProcess ( this_reply->mesg, i, &(posDataAll.sonar[j]) );
  }

  /* the Intellisys 100 time */
  i = timeDataProcess ( this_reply->mesg, i, &posDataTime );

  return(TRUE);
}

static int process_configuration_reply(struct reply_struct *this_reply)
{
  int i;
  
  if (this_reply->type == ERROR_MSG)
  {
    State[ STATE_ERROR ] = this_reply->mesg[0];
    return(FALSE);
  }
  else
  {
    for (i = STATE_CONF_X; i <= STATE_CONF_TURRET; i++)
      State[i] = this_reply->mesg[i - STATE_CONF_X];
  }
  return(TRUE);
}

static int process_conf_reply(struct reply_struct *this_reply, long *conf)
{
  int i;
  
  if (this_reply->type == ERROR_MSG)
  {
    State[ STATE_ERROR ] = this_reply->mesg[0];
    return(FALSE);
  }
  else
  {
    for (i= 0; i<4; i++)
      conf[i] = this_reply->mesg[i];
  }
  return(TRUE);
}

static int process_velocity_reply(struct reply_struct *this_reply)
{
  int i;
  
  if (this_reply->type == ERROR_MSG)
  {
    State[ STATE_ERROR ] = this_reply->mesg[0];
    return(FALSE);
  }
  else
  {
    for (i = STATE_VEL_TRANS ; i <= STATE_VEL_TURRET; i++)
      State[i] = this_reply->mesg[i - STATE_VEL_TRANS];
  }
  return(TRUE);
}


static int process_compass_reply(struct reply_struct *this_reply)
{
  int i=0;

  if (this_reply->type == ERROR_MSG)
  {
    State[ STATE_ERROR ] = this_reply->mesg[0];
    return(FALSE);
  }
  else
  {
    State[ STATE_COMPASS ] = this_reply->mesg[0];

    /* 
     * if position attachment was requested for compass....
     */
    
    if ( POS_COMPASS_P )
      i = posDataProcess ( this_reply->mesg, 1, &(posDataAll.compass) );
  }

  /* the Intellisys 100 time */
  i = timeDataProcess ( this_reply->mesg, i, &posDataTime );

  return(TRUE);
}

static int process_bumper_reply(struct reply_struct *this_reply)
{
  int i;

  if (this_reply->type == ERROR_MSG)
  {
    State[ STATE_ERROR ] = this_reply->mesg[0];
    return(FALSE);
  }
  else
  {
    State[ STATE_BUMPER ] = this_reply->mesg[0];

    /* 
     * if position attachment was requested for bumper....
     */
    
    if ( POS_BUMPER_P )
      i = posDataProcess ( this_reply->mesg, 1, &(posDataAll.bumper) );
  }

  /* the Intellisys 100 time */
  i = timeDataProcess ( this_reply->mesg, i, &posDataTime );

  return(TRUE);
}

static int process_laser_reply(struct reply_struct *this_reply)
{
  int i, num_points;
  
  if (this_reply->type == ERROR_MSG)
  {
    State[ STATE_ERROR ] = this_reply->mesg[0];
    return(FALSE);
  }
  else
  {
    num_points = this_reply->mesg[0];
    
    if ((laser_mode == 1) || (laser_mode == 33))
    {
      for (i=0; i<=4*num_points; i++)
	Laser[i] = this_reply->mesg[i];
    }
    else
    {
      if ((laser_mode == 51) || (laser_mode == 50) || (laser_mode == 19))
	for (i=0; i<=2*num_points; i++)
	  Laser[i] = this_reply->mesg[i];
      else
	for (i=0; i<=num_points; i++)
	  Laser[i] = this_reply->mesg[i];
    }

    /* 
     * if position attachment was requested for laser....
     */
    
    if ( POS_LASER_P )
      i = posDataProcess ( this_reply->mesg, i, &(posDataAll.laser) );
  }

  /* the Intellisys 100 time */
  i = timeDataProcess ( this_reply->mesg, i, &posDataTime );

  return(TRUE);
}

static int process_predict_reply(struct reply_struct *this_reply, long *state,
				 int *laser)
{
  int i, num_points;
  
  for (i=1; i<33; i++)
    state[i] = this_reply->mesg[i];
  for (i=33; i<=44; i++)
    state[i] = 0;
  num_points = this_reply->mesg[44];
  if ((laser_mode == 1) || (laser_mode == 33))
  {
    for (i=44; i<=44+4*num_points; i++)
      laser[i-44] = this_reply->mesg[i];
  }
  else
  {
    if ((laser_mode == 51) || (laser_mode == 50) || (laser_mode == 19))
      for (i=44; i<=44+2*num_points; i++)
	laser[i-44] = this_reply->mesg[i];
    else
      for (i=44; i<=44+num_points; i++)
	laser[i-44] = this_reply->mesg[i];
  }
  return(TRUE);
}

static int process_simple_reply(struct reply_struct *this_reply)
{
  return(TRUE);
}

static int process_obstacle_reply(struct reply_struct *this_reply, long *obs)
{
  int i;
  
  for (i=0; i<this_reply->size; i++)
  {
    obs[i] = this_reply->mesg[i];
  }
  return(TRUE);
}

static int process_rpx_reply(struct reply_struct *this_reply, long *robot_pos)
{
  int i, num_robots;
  
  if (this_reply->type == ERROR_MSG)
  {
    State[ STATE_ERROR ] = this_reply->mesg[0];
    return(FALSE);
  }
  else
  {
    num_robots = this_reply->mesg[0];
    robot_pos[0] = num_robots;

    for (i=1; i<=3*num_robots; i++)
    {
      robot_pos[i] = this_reply->mesg[i];
    }
  }
  return(TRUE);
}


static int process_socket_reply(struct reply_struct *this_reply)
{
  own_tcp_port = this_reply->mesg[0];
  
  return(own_tcp_port);
}

/* this is something special for Greg at Yale */
static int process_mcheck_reply (struct reply_struct *this_reply,
				 double collide[3])
{
  if (this_reply->type == ERROR_MSG) {
    State[ STATE_ERROR ] = this_reply->mesg[0];
  }
  else {
    if (this_reply->mesg[0]) {
      collide[0] = (double)this_reply->mesg[1]/100.0;
      collide[1] = (double)this_reply->mesg[2]/100.0;
      collide[2] = (double)this_reply->mesg[3]/1000000.0;
      return(1);
    }
    else {
      return(0);
    }
  }
  return(TRUE);
}
static int process_special_reply(struct reply_struct *this_reply, unsigned char *data)
{
  int i;
  
  if (this_reply->type == ERROR_MSG)
  {
    State[ STATE_ERROR ] = this_reply->mesg[0];
    return(FALSE);
  }
  else
  {
    for (i=0; i<this_reply->size; i++)
    {
      data[i] = (unsigned char)this_reply->mesg[i];
    }
  }
  return(TRUE);
}

/*
 * bits_to_byte - converts 8 bits into one byte. Helper function for ct()
 */
static unsigned char bits_to_byte(char bt0, char bt1, char bt2, char bt3,
				  char bt4, char bt5, char bt6, char bt7)
{
  unsigned char  rbyte;
  
  if (bt0 > 0) bt0 = 1; else bt0 = 0;
  if (bt1 > 0) bt1 = 1; else bt1 = 0;
  if (bt2 > 0) bt2 = 1; else bt2 = 0;
  if (bt3 > 0) bt3 = 1; else bt3 = 0;
  if (bt4 > 0) bt4 = 1; else bt4 = 0;
  if (bt5 > 0) bt5 = 1; else bt5 = 0;
  if (bt6 > 0) bt6 = 1; else bt6 = 0;
  if (bt7 > 0) bt7 = 1; else bt7 = 0;
  
  rbyte = (unsigned char)(bt0 + (2*bt1) + (4*bt2) + (8*bt3) + (16*bt4) +
			  (32*bt5) + (64*bt6) + (128*bt7));
  return (rbyte);
}

/*****************************
 *                           *
 * Robot Interface Functions *
 *                           *
 *****************************/

/*
 * 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)
{
  pid_t process_id;
  
  process_id = getpid();
  the_request.type = CREATE_ROBOT_MSG;
  the_request.size = 2;
  the_request.mesg[0] = robot_id;
  the_request.mesg[1] = (long)process_id;
  
  if (dest_socket == -1) 
    dest_socket = 0; 
  own_tcp_port = 0; /* make sure the ipc_comm uses SERV_TCP_PORT */
  
  connectedp = 1;
  if (ipc_comm(&the_request, &the_reply))
  {
    return(process_socket_reply(&the_reply));
  }
  else
  {
    State[ STATE_ERROR ] = IPC_ERROR;  /* indicate IPC_ERROR */
    connectedp = 0;
    return(FALSE);
  }
}


/*
 * 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.
 *
 * 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, ...)
{
  static char first = 1;
  pid_t process_id;
  int error;

  if (first)
  {
    fprintf(stderr, "Nclient version 2.7\n");
    fprintf(stderr, "Copyright 1991-1998, Nomadic Technologies, Inc.\n");
    first = 0;
  }
  
  process_id = getpid();
  the_request.type = CONNECT_ROBOT_MSG;
  the_request.size = 2;
  the_request.mesg[0] = robot_id;
  the_request.mesg[1] = process_id;
  
  if (dest_socket == -1) 
    dest_socket = 0; 
  own_tcp_port = 0; /* make sure the ipc_comm uses SERV_TCP_PORT */
  
  connectedp = 1;
  if (ipc_comm(&the_request, &the_reply))
  {
    error = process_socket_reply(&the_reply);