nclient.c

机器人仿真软件

 * delete_obstacle - deletes an obstacle specified by obs from the robot 
 *                   environment 
 * parameters:
 *    long obs[2*MAX_VERTICES+1] -- 
 *                The first element of obs specifies the number of 
 *                vertices of the polygonal obstacle (must be no greater 
 *                than MAX_VERTICES). The subsequent elements of obs 
 *                specifies the x and y coordinates of the vertices, 
 *                in counter-clockwise direction.
 */
int delete_obstacle(long obs[2*MAX_VERTICES+1])
{
  int i;
  
  the_request.type = DELETEOBS_MSG;
  the_request.size = obs[0]*2+1;
  for (i=0; i<obs[0]*2+1; i++)
  {
    the_request.mesg[i] = obs[i];
  }
  
  if (ipc_comm(&the_request, &the_reply))
  {
    return(process_simple_reply(&the_reply));
  }
  else
  {
    State[ STATE_ERROR ] = IPC_ERROR;  /* indicate IPC_ERROR */
    return(FALSE);
  }
}

/*
 * delete_Obs - is the same as delete_obstacle, for backward compatibility
 */
int delete_Obs(long obs[2*MAX_VERTICES+1])
{
  return(delete_obstacle(obs));
}

/*
 * move_obstacle - moves the obstacle obs by dx along x direction and 
 *                 dy along y direction. obs is modified.
 *
 * parameters:
 *    long obs[2*MAX_VERTICES+1] -- 
 *                The first element of obs specifies the number of 
 *                vertices of the polygonal obstacle (must be no greater 
 *                than MAX_VERTICES). The subsequent elements of obs 
 *                specifies the x and y coordinates of the vertices, 
 *                in counter-clockwise direction.
 *    long dx, dy -- the x and y distances to translate the obstacle
 */
int move_obstacle(long obs[2*MAX_VERTICES+1], long dx, long dy)
{
  int i;
  
  the_request.type = MOVEOBS_MSG;
  the_request.size = obs[0]*2+3;
  for (i=0; i<obs[0]*2+1; i++)
  {
    the_request.mesg[i] = obs[i];
  }
  the_request.mesg[2*obs[0]+1] = dx;
  the_request.mesg[2*obs[0]+2] = dy;
  
  if (ipc_comm(&the_request, &the_reply))
  {
    return(process_obstacle_reply(&the_reply, obs));
  }
  else
  {
    State[ STATE_ERROR ] = IPC_ERROR;  /* indicate IPC_ERROR */
    return(FALSE);
  }
}

/*
 * move_Obs - is the same as move_obstacle, for backward compatibility
 */
int move_Obs(long obs[2*MAX_VERTICES+1], long dx, long dy)
{
  return(move_obstacle(obs, dx, dy));
}

/*
 * new_world - deletes all obstacles in the current robot world
 */
int new_world(void)
{
  the_request.type = NEWWORLD_MSG;
  the_request.size = 0;
  
  if (ipc_comm(&the_request, &the_reply))
  {
    return(process_simple_reply(&the_reply));
  }
  else
  {
    State[ STATE_ERROR ] = IPC_ERROR;  /* indicate IPC_ERROR */
    return(FALSE);
  }
}

/****************************
 *                          *
 * Graphics refresh control *
 *                          *
 ****************************/

/*
 * refresh_all - causes all temporary drawing in graphics window, including
 *               traces, sensors, and client graphics to be erased
 */
int refresh_all(void)
{
  the_request.type = REFRESHALL_MSG;
  the_request.size = 0;
  
  if (ipc_comm(&the_request, &the_reply))
  {
    return(process_simple_reply(&the_reply));
  }
  else
  {
    State[ STATE_ERROR ] = IPC_ERROR;  /* indicate IPC_ERROR */
    return(FALSE);
  }
}

/*
 * refresh_all_traces - causes all robot traces in graphics to be erased
 */
int refresh_all_traces(void)
{
  the_request.type = REFRESHALLTRACES_MSG;
  the_request.size = 0;
  
  if (ipc_comm(&the_request, &the_reply))
  {
    return(process_simple_reply(&the_reply));
  }
  else
  {
    State[ STATE_ERROR ] = IPC_ERROR;  /* indicate IPC_ERROR */
    return(FALSE);
  }
}

/*
 * refresh_actual_trace - causes actual robot trace in graphics to be erased
 */
int refresh_actual_trace(void)
{
  the_request.type = REFRESHACTTRACE_MSG;
  the_request.size = 0;
  
  if (ipc_comm(&the_request, &the_reply))
  {
    return(process_simple_reply(&the_reply));
  }
  else
  {
    State[ STATE_ERROR ] = IPC_ERROR;  /* indicate IPC_ERROR */
    return(FALSE);
  }
}

/*
 * refresh_encoder_trace - causes encoder robot trace in graphics to be erased
 */
int refresh_encoder_trace(void)
{
  the_request.type = REFRESHENCTRACE_MSG;
  the_request.size = 0;
  
  if (ipc_comm(&the_request, &the_reply))
  {
    return(process_simple_reply(&the_reply));
  }
  else
  {
    State[ STATE_ERROR ] = IPC_ERROR;  /* indicate IPC_ERROR */
    return(FALSE);
  }
}

/*
 * refresh_all_sensors - causes all sensor drawings in graphics to be erased
 */
int refresh_all_sensors(void)
{
  the_request.type = REFRESHALLSENSORS_MSG;
  the_request.size = 0;
  
  if (ipc_comm(&the_request, &the_reply))
  {
    return(process_simple_reply(&the_reply));
  }
  else
  {
    State[ STATE_ERROR ] = IPC_ERROR;  /* indicate IPC_ERROR */
    return(FALSE);
  }
}

/*
 * refresh_bumper_sensor - causes bumper drawings in graphics to be erased
 */
int refresh_bumper_sensor(void)
{
  the_request.type = REFRESHBPSENSOR_MSG;
  the_request.size = 0;
  
  if (ipc_comm(&the_request, &the_reply))
  {
    return(process_simple_reply(&the_reply));
  }
  else
  {
    State[ STATE_ERROR ] = IPC_ERROR;  /* indicate IPC_ERROR */
    return(FALSE);
  }
}

/*
 * refresh_infrared_sensor - causes infrared drawings in graphics to be erased
 */
int refresh_infrared_sensor(void)
{
  the_request.type = REFRESHIRSENSOR_MSG;
  the_request.size = 0;
  
  if (ipc_comm(&the_request, &the_reply))
  {
    return(process_simple_reply(&the_reply));
  }
  else
  {
    State[ STATE_ERROR ] = IPC_ERROR;  /* indicate IPC_ERROR */
    return(FALSE);
  }
}

/*
 * refresh_sonar_sensor - causes sonar drawings in graphics to be erased
 */
int refresh_sonar_sensor(void)
{
  the_request.type = REFRESHSNSENSOR_MSG;
  the_request.size = 0;
  
  if (ipc_comm(&the_request, &the_reply))
  {
    return(process_simple_reply(&the_reply));
  }
  else
  {
    State[ STATE_ERROR ] = IPC_ERROR;  /* indicate IPC_ERROR */
    return(FALSE);
  }
}

/*
 * refresh_laser_sensor - causes laser drawings in graphics to be erased
 */
int refresh_laser_sensor(void)
{
  the_request.type = REFRESHLSSENSOR_MSG;
  the_request.size = 0;
  
  if (ipc_comm(&the_request, &the_reply))
  {
    return(process_simple_reply(&the_reply));
  }
  else
  {
    State[ STATE_ERROR ] = IPC_ERROR;  /* indicate IPC_ERROR */
    return(FALSE);
  }
}

/*
 * refresh_client_graphics - causes drawings performed by any clients into
 *                           graphics window to be erased
 */
int refresh_client_graphics(void)
{
  the_request.type = REFRESHGRAPHICS_MSG;
  the_request.size = 0;
  
  if (ipc_comm(&the_request, &the_reply))
  {
    return(process_simple_reply(&the_reply));
  }
  else
  {
    State[ STATE_ERROR ] = IPC_ERROR;  /* indicate IPC_ERROR */
    return(FALSE);
  }
}

/*******************************
 *                             *
 * Miscellaneous robot control *
 *                             *
 *******************************/

/*
 * init_mask - initialize the sensor mask, Smask.
 */
void init_mask(void)
{
  int i;
  
  Smask[ SMASK_POS_DATA ] = 0;
  for (i=1; i<44; i++)
    Smask[i] = 1;
}

/*
 * init_sensors - initialize the sensor mask, Smask, and send it to the
 *                robot. It has no effect on the sensors 
 */
int init_sensors(void)
{
  int i;
  
  Smask[ SMASK_POS_DATA ] = 0;
  for (i=1; i<44; i++)
    Smask[i] = 1;
  return ct();
}


/*
 * place_robot - places the robot at configuration (x, y, th, tu). 
 *               In simulation mode, it will place both the Encoder-robot
 *               and the Actual-robot at this configuration. In real robot
 *               mode, it will call dp(x, y) and da(th, tu).
 * 
 * parameters:
 *    int x, y -- x-y position of the desired robot configuration
 *    int th, tu -- the steering and turret orientation of the robot
 *                  desired configuration
 */
int place_robot(int x, int y, int th, int tu)
{
  the_request.type = RPLACE_MSG;
  the_request.size = 4;
  the_request.mesg[0] = x;
  the_request.mesg[1] = y;
  the_request.mesg[2] = th;
  the_request.mesg[3] = tu;
  
  if (ipc_comm(&the_request, &the_reply))
  {
    return(process_simple_reply(&the_reply));
  }
  else
  {
    State[ STATE_ERROR ] = IPC_ERROR;  /* indicate IPC_ERROR */
    return(FALSE);
  }
}

/*
 * special_request - sends a special request (stored in user_send_buffer) 
 *                   to the robot and waits for the robot's response (which
 *                   will be stored in user_receive_buffer). 
 * 
 * parameters:
 *    unsigned char *user_send_buffer -- stores data to be sent to the robot
 *                                       Should be a pointer to an array of
 *                                       MAX_MSG_LENGTH elements
 *    unsigned char *user_receive_buffer -- stores data received from the robot
 *                                          Should be a pointer to an array of 
 *                                       MAX_MSG_LENGTH elements
 */
int special_request(unsigned char *user_send_buffer,
		    unsigned char *user_receive_buffer)
{
  unsigned short num_data;
  int i;
  
  the_request.type = SPECIAL_MSG;
  num_data = user_send_buffer[0]+256*user_send_buffer[1];
  if (num_data>USER_BUFFER_LENGTH-5)
  {
    printf("Data + protocol bytes exceeding %d, truncating\n",USER_BUFFER_LENGTH);
    num_data  = USER_BUFFER_LENGTH-5; /* num_data already includes the 4 bytes of user packets protocol */
  }
  the_request.size = num_data;
  for (i=0; i<num_data; i++)
  {
    the_request.mesg[i] = user_send_buffer[i];
  }
  if (ipc_comm(&the_request, &the_reply))
  {
    return(process_special_reply(&the_reply, user_receive_buffer));
  }
  else
  {
    State[ STATE_ERROR ] = IPC_ERROR;  /* indicate IPC_ERROR */
    return(FALSE);
  }
}

/*******************************
 *                             *
 * Graphic Interface Functions *
 *                             *
 *******************************/

/*
 * draw_robot - this function allows the client to draw a robot at
 *              configuration x, y, th, tu (using the robot world 
 *              coordinates). 
 * 
 * parameters:
 *    long x, y -- the x-y position of the robot.
 *    int th, tu -- the steering and turret orientation of the robot
 *    int mode - the drawing mode. If mode = 1, the robot is drawn in 
 *              BlackPixel using GXxor (using GXxor you can erase the trace 
 *              of robotby drawing over it). If mode = 2, the robot is 
 *              drawn in BlackPixel using GXxor and in addition, a small arrow
 *              is drawn at the center of the robot using GXcopy (using this 
 *              mode you can leave a trace of small arrow). If mode = 3, 
 *              the robot is drawn in BlackPixel using GXcopy. When mode > 3,
 *              the robot is drawn in color using GXxor.
 */
int draw_robot(long x, long y, int th, int tu, int mode)
{
  the_request.type = DRAWROBOT_MSG;
  the_request.size = 5;
  the_request.mesg[0] = x;
  the_request.mesg[1] = y;
  the_request.mesg[2] = th;
  the_request.mesg[3] = tu;
  the_request.mesg[4] = (long)(mode);
  
  if (ipc_comm(&the_request, &the_reply))
  {
    return(process_simple_reply(&the_reply));
  }
  else
  {
    State[ STATE_ERROR ] = IPC_ERROR;  /* indicate IPC_ERROR */
    return(FALSE);
  }
}

/*
 * draw_line - this function allows the client to draw a line from
 *             (x_1, y_1) to (x_2, y_2) (using the robot world coordinates). 
 *
 * parameters:
 *    long x_1, y_1, x_2, y_2 -- the two end-points of the line
 *    int mode -- the mode of drawing: when mode is 1, the drawing is 
 *                done in BlackPixel using GXcopy; when mode is 2, the drawing
 *                is done in BlackPixel using GXxor, when mode > 2, the drawing
 *                is done in color using GXxor.
 */
int draw_line(long x_1, long y_1, long x_2, long y_2, int mode)
{
  the_request.type = DRAWLINE_MSG;
  the_request.size = 5;
  the_request.mesg[0] = x_1;
  the_request.mesg[1] = y_1;
  the_request.mesg[2] = x_2;
  the_request.mesg[3] = y_2;
  the_request.mesg[4] = (long)(mode);
  
  if (ipc_comm(&the_request, &the_reply))
  {
    return(process_simple_reply(&the_reply));
  }
  else
  {
    State[ STATE_ERROR ] = IPC_ERROR;  /* indicate IPC_ERROR */
    return(FALSE);
  }
}

/*
 * draw_arc - this function allows the client to draw arc which is part
 *            of an ellipse (using the robot world coordinates). 
 *
 * parameters:
 *    long x_0, y_0, w, h -- (x_0, y_0) specifies the upper left corner of the 
 *                          rectangle bounding the ellipse while w and h
 *                          specifies the width and height of the bounding 
 *                          rectangle, respectively.
 *    int th1, th2 -- th1 and th2 specifies the angular range of the arc.
 *    int mode -- the mode of drawing: when mode is 1, the drawing is 
 *                done in BlackPixel using GXcopy; when mode is 2, the drawing
 *                is done in BlackPixel using GXxor, when mode > 2, the drawing
 *                is done in color using GXxor.
 */
int draw_arc(long x_0, long y_0, long w, long h, int th1, int th2, int mode)
{
  the_request.type = DRAWARC_MSG;
  the_request.size = 7;
  the_request.mesg[0] = x_0;
  the_request.mesg[