pioneer_lib.c

卡耐基.梅隆大学的机器人仿真软件（Redhat linux 9下安装）

/*********************************************************
 *
 * This source code is part of the Carnegie Mellon Robot
 * Navigation Toolkit (CARMEN)
 *
 * CARMEN Copyright (c) 2002 Michael Montemerlo, Nicholas
 * Roy, Sebastian Thrun, Dirk Haehnel, Cyrill Stachniss,
 * and Jared Glover
 *
 * CARMEN is free software; you can redistribute it and/or 
 * modify it under the terms of the GNU General Public 
 * License as published by the Free Software Foundation; 
 * either version 2 of the License, or (at your option)
 * any later version.
 *
 * CARMEN 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 General Public License for more 
 * details.
 *
 * You should have received a copy of the GNU General 
 * Public License along with CARMEN; if not, write to the
 * Free Software Foundation, Inc., 59 Temple Place, 
 * Suite 330, Boston, MA  02111-1307 USA
 *
 ********************************************************/

#include <carmen/carmen.h>
#include <carmen/drive_low_level.h>
#include <limits.h>

#include "pioneer_params.h"

#define        PIONEER_SERIAL_TIMEOUT            5

static int dev_fd;
static int sonar_is_on = 0;
static int pioneer_version = 0;

static double angle_conv_factor, dist_conv_factor, vel_conv_factor;
static double range_conv_factor, diff_conv_factor, vel2_divisor;

/*** Some pioneer internal constants.  I have them defined here since the ***/
/*** outside world doesn't need to know about them.                       ***/

#define PIONEER_PACKET_HEADER  "\xFA\xFB"

#define PIONEER_SYNC0           0
#define PIONEER_SYNC1           1
#define PIONEER_SYNC2           2

#define PIONEER_PULSE           0
#define PIONEER_OPEN            1
#define PIONEER_CLOSE           2
#define PIONEER_POLLING         3
#define PIONEER_ENABLE          4
#define PIONEER_SETA            5
#define PIONEER_VEL             11
#define PIONEER_RVEL            21
#define PIONEER_SETRA           23
#define PIONEER_SONAR           28
#define PIONEER_STOP            29
#define PIONEER_VEL2            32

#define PIONEER_NEGATIVE_INT    0x1B
#define PIONEER_STRING          0x2B
#define PIONEER_POSITIVE_INT    0x3B

#define PIONEER_ON              1
#define PIONEER_OFF             0
#define PIONEER_MOTOR_NO_POWER  0x31
#define PIONEER_MOTOR_STOPPED   0x32
#define PIONEER_MOTOR_MOVING    0x33

#define PIONEER_BATTERY_CONVERSION_FACTOR           0.1
#define PIONEER_COMPASS_CONVERSION_FACTOR           2.0

#define PIONEER_MAX_NUM_SONARS  16


/* Cyrill, 14.01.2004
   according to the ActivMedia manual the discretization
   is 4mm/sec, not 5mm/sec. So the factor should be 250.0
   and not 200.0 */
//#define PIONEER1_VEL2_CONVERSION_FACTOR             200.0             // m/s => 5mm/sec
#define PIONEER1_VEL2_CONVERSION_FACTOR             250.0             // m/s => 4mm/sec
#define PIONEER2_VEL2_CONVERSION_FACTOR             50.0              // m/s => 2cm/sec


/*** When we receive an information packet back from the robot, it is     ***/
/*** initially read into a raw information packet as if to a big char     ***/
/*** buffer.  It is then converted to a regular information packet, which ***/
/*** contains more useful data types.                                     ***/
struct pioneer_raw_information_packet 
{
  unsigned char motor_status;
  unsigned char x[2];
  unsigned char y[2];
  unsigned char orientation[2];
  unsigned char vl[2];
  unsigned char vr[2];
  unsigned char battery;
  unsigned char bumper[2];
  unsigned char servo_position[2];
  unsigned char pulse_width[2];
  unsigned char compass_heading;
  unsigned char num_sonar_readings;
  struct 
  {
    unsigned char n;
    unsigned char range[2];
  } sonar[3*PIONEER_MAX_NUM_SONARS];
};

typedef enum {PIONEER_NO_POWER, PIONEER_STOPPED, PIONEER_MOVING} 
motor_status_t;

static struct pioneer_raw_information_packet raw_state;

int pioneer_vel2(int vl, int vr) ;


/*** TABLE OF CONTENTS ***/

/*** Higher-level robot functions ***/

static int 
pioneer_buf_to_checksum(unsigned char *buf) 
{
  return 256*buf[0] + buf[1];
}


static int 
pioneer_buf_to_int(unsigned char *buf) 
{
  int val = buf[0] + 256*buf[1];
  if (val > SHRT_MAX)
    val -= USHRT_MAX;
  return val;
}


static int 
pioneer_buf_to_unsigned_int(unsigned char *buf) 
{
  int val = buf[0] + 256 * (0x7F & buf[1]);  
  return val;
}

static void 
pioneer_int_to_buf(unsigned char *buf, int n) 
{
  if (n >= 0)
    buf[0] = PIONEER_POSITIVE_INT;
  else
    buf[0] = PIONEER_NEGATIVE_INT;

  n = abs(n);

  buf[1] = n & 0x00FF;
  buf[2] = (n & 0xFF00) >> 8;
}
 
static int 
pioneer_calculate_checksum(unsigned char *ptr, int length) 
{
  unsigned int c = 0;
  
  while(length > 1) 
    {
      c += (*(ptr)<<8) | *(ptr+1);
      c = c & 0xffff;
      length -= 2;
      ptr += 2;
    }

  if(length > 0) 
    c = c ^ (int)*(ptr++);

  return(c);
}

/*** Return value:  the length of the string read                       ***/
static int 
pioneer_read_string(unsigned char *buf, double read_timeout, double * packet_timestamp) 
{
  int c = 0;
  int header_length = strlen(PIONEER_PACKET_HEADER);
  unsigned char header[header_length+1];
  unsigned char length_char[1];
  int i;
  double start_time, current_time;
  double header_time;

  for (i = 0; i <= header_length; ++i)
    header[i] = '\0';

  start_time = carmen_get_time();
  header_time = start_time;
  do 
    {                                  /* busy waits until we read the   */
      for (i = 1; i < header_length; ++i) /* command header                 */
	header[i-1] = header[i];
      carmen_serial_readn(dev_fd, header + header_length - 1, 1);
      header_time=carmen_get_time();
      if (read_timeout > 0) {
	current_time = carmen_get_time();
	if (current_time - start_time > read_timeout)
	  // timeout
	  return -1;
      }
    } 
  while (strcmp((char *)header, PIONEER_PACKET_HEADER) != 0);
 
  carmen_serial_readn(dev_fd, length_char, 1);
  c = (int) length_char[0];             /* the length, including chksum   */

  carmen_serial_readn(dev_fd, buf, c);
  c -= 2;                               /* removes checksum from length,  */
                                        /* checksum is now at buf+c       */
  if (pioneer_calculate_checksum(buf, c) != pioneer_buf_to_checksum(buf+c)) 
    {
      fprintf(stderr, "Checksum error on packet ");
      for (i = 0; i < c; ++i)
	fprintf(stderr, "%c", buf[i]);
      fprintf(stderr, "\n");
      return pioneer_read_string(buf, read_timeout, NULL);
    }
  if (packet_timestamp)
	*packet_timestamp=header_time;
  return c;
}


static int 
pioneer_send_string(unsigned char *ptr, int length) 
{
  int send_length = length + 5;         /* adds a 2-byte packet header,   */
                                        /* a 1-byte length (inc. chksum), */
                                        /* and a 2-byte checksum          */
  unsigned char cmd[256] = PIONEER_PACKET_HEADER;
  int checksum;
  int i;

  if (length <= 0)
    return 0;

  cmd[2] = (unsigned char) length+2;

  for (i = 0; i < length; ++i)
    cmd[i+3] = ptr[i];

  checksum = pioneer_calculate_checksum(ptr, length);
  cmd[send_length-2] = (unsigned char) ((checksum & 0xFF00) >> 8);
  cmd[send_length-1] = (unsigned char) (checksum & 0x00FF);

  return carmen_serial_writen(dev_fd, cmd, send_length);
}

static int 
pioneer_send_command0(char cmd) 
{
  char buf[256];

  buf[0] = cmd;
  return pioneer_send_string((unsigned char *)buf, 1);
}


static int 
pioneer_send_command1(char cmd, int arg1) 
{
  char buf[256];
  
  buf[0] = cmd;
  pioneer_int_to_buf((unsigned char *)(buf+1), arg1);
  return pioneer_send_string((unsigned char *)buf, 4);
}

static int
pioneer_sonar(int b) 
{
  if (pioneer_version == 1 && b == 0)
    return pioneer_send_command1(PIONEER_POLLING, 0);
  else 
    return pioneer_send_command1(PIONEER_SONAR, b);
}


static int 
pioneer_sync0(void) 
{
  unsigned char buf[256];
  buf[0] = !PIONEER_SYNC0;

  pioneer_send_command0(PIONEER_SYNC0);
  if ((pioneer_read_string(buf, 0.2, NULL) == -1) && (pioneer_version == 1)) 
    {
      /* sometimes the Pioneer I's want two syncs */
      pioneer_send_command0(PIONEER_SYNC0);
      if (pioneer_read_string(buf, 0.2, NULL) == -1) 
	{
	  carmen_warn("Could not SYNC0\n");
	  return -1;
	}
    }

  if (buf[0] != PIONEER_SYNC0) 
    {
      carmen_warn("Could not SYNC0\n");
      return -1;
    }
  
  return 0;
}

static int 
pioneer_sync1(void) 
{
  unsigned char buf[256];

  pioneer_send_command0(PIONEER_SYNC1);
  pioneer_read_string(buf, PIONEER_SERIAL_TIMEOUT, NULL);
  if (buf[0] != PIONEER_SYNC1) 
    {
      carmen_warn("Could not SYNC1\n");
      return -1;
    }

  return 0;
}

static int 
pioneer_sync2(void) 
{
  unsigned char buf[256];

  pioneer_send_command0(PIONEER_SYNC2);
  pioneer_read_string(buf, PIONEER_SERIAL_TIMEOUT, NULL);

  return 0;
}

int abs_max(int a, int b) {
  a = abs(a);
  b = abs(b);
  return (a>b)?a:b;
}

int
pioneer_vel2(int vl, int vr) 
{
  int arg;
  int absmax;

  absmax = abs_max(vl, vr);
  if (absmax > 127) {
    float scale_factor;
    scale_factor = 127.0 / ((float)absmax);
    vl = (int) ( scale_factor * ((float)vl));
    vr = (int) ( scale_factor * ((float)vr));
  }

  if (1)
    fprintf(stderr,"(%d %d)\n", vl, vr);

  arg = 256*vl + vr;

  return pioneer_send_command1(PIONEER_VEL2, arg);
}

int
carmen_base_direct_sonar_on(void)
{
  int err = 0;

  if (!sonar_is_on) 
    err = pioneer_sonar(1);

  if (err < 0)
    return -1;

  return PIONEER_MAX_NUM_SONARS;
}

int
carmen_base_direct_sonar_off(void)
{
  if (sonar_is_on) 
    return pioneer_sonar(0);

  return 0;
}

int 
carmen_base_direct_reset(void)
{
  return 0;
}

int