reb.cc

机器人仿真平台,和stage配合运行

/*
 *  Player - One Hell of a Robot Server
 *  Copyright (C) 2000  
 *     Brian Gerkey, Kasper Stoy, Richard Vaughan, & Andrew Howard
 *
 *
 *  This program 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.
 *
 *  This program 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 this program; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 *
 */

/* Copyright (C) 2002
 *   John Sweeney, UMASS, Amherst, Laboratory for Perceptual Robotics
 *
 * $Id: reb.cc,v 1.1.2.1 2003/04/17 01:22:11 gerkey Exp $
 *
 *   The REB device.  This controls the K-Team Kameleon 376SBC with
 * the Robotics Extension Board (REB).  (Technically the REB doesn't control
 * anything, it just provides the analog I/Os, H-bridges, etc but we
 * thought REB was a good acronym...)  The REB/Kameleon board has the motor
 * drivers and sensor I/O, and we communicate with it via a serial port.
 * So the overall architecture is similar to the p2osdevice, where this class
 * handles the data gathering tasks for the Position, IR and power devices.
 *
 * Note that we have actually made our own version of the SerCom program that
 * runs on the Kameleon.  Our version runs faster than K-Teams, so we
 * can reliably get new data at around 10 Hz.  (K-Team SerCom barfed for us
 * faster than about 2 Hz!)  Our SerCom, called LPRSerCom, also handles turning
 * the IRs on and off, so we don't have to worry about that in the player server.
 * If you would like a copy of LPRSerCom, then send me email: sweeney (at) cs.umass.edu
 *
 * Our robots use a StrongARM SA110 for the compute power, so we have
 * to minimize the use of floating point, since the ARM can only emulate
 * it.
 * 
 * Note that this device came from copying p2osdevice.cc and then editing
 * to taste, so thanks to the writers of p2osdevice.cc!
 */

#include <fcntl.h>
#include <signal.h>
#include <sys/stat.h>
#include <sys/types.h>
#include <stdio.h>
#include <string.h>
#include <termios.h>
#include <unistd.h>
#include <math.h>
#include <stdlib.h>  /* for abs() */
#include <netinet/in.h>
#include <ctype.h>

#include <reb.h>

#include <playertime.h>
extern PlayerTime* GlobalTime;

// so we can access the deviceTable and extract pointers to the sonar
// and position objects
#include <devicetable.h>
extern CDeviceTable* deviceTable;
extern int global_playerport; // used to get at devices

// we need to debug different things at different times
//#define DEBUG_POS
#define DEBUG_SERIAL
#define DEBUG_CONFIG

// useful macros
#define DEG2RAD(x) (((double)(x))*0.01745329251994)
#define RAD2DEG(x) (((double)(x))*57.29577951308232)

#define DEG2RAD_FIX(x) ((x) * 17453)
#define RAD2DEG_FIX(x) ((x) * 57295780)

/* these are necessary to make the static fields visible to the linker */
extern pthread_t		REB::thread;
extern struct timeval		REB::timeBegan_tv;
extern int			REB::reb_fd; 
extern char			REB::reb_serial_port[];
extern bool			REB::initdone;
extern int			REB::param_index;
extern pthread_mutex_t		REB::reb_accessMutex;
extern pthread_mutex_t		REB::reb_setupMutex;
extern int			REB::reb_subscriptions;
extern int			REB::ir_subscriptions;
extern int			REB::pos_subscriptions;
extern int			REB::power_subscriptions;
extern player_reb_data_t*	REB::data;
extern player_reb_cmd_t*	REB::command;
extern unsigned char*		REB::reqqueue;
extern unsigned char*		REB::repqueue;
extern struct timeval		REB::last_position;
extern bool			REB::refresh_last_position;
extern bool			REB::motors_enabled;
extern bool			REB::velocity_mode;
extern bool			REB::direct_velocity_control;
extern int			REB::ir_sequence;
extern struct timeval		REB::last_ir;
extern short			REB::desired_heading;
extern struct timeval		REB::last_pos_update;
extern struct timeval		REB::last_ir_update;
extern struct timeval		REB::last_power_update;
extern int			REB::pos_update_period;
extern int			REB::locks;
extern int			REB::slocks;
extern struct pollfd		REB::write_pfd;
extern struct pollfd		REB::read_pfd;

REB::REB(char *interface, ConfigFile *cf, int section)
{
  int reqqueuelen = 1;
  int repqueuelen = 1;

  if(!initdone)
  {
    locks = slocks =0;
    // build the table of robot parameters.
    initialize_reb_params();
  
    // also, install default parameter values.
    strncpy(this->reb_serial_port,REB_DEFAULT_SERIAL_PORT,sizeof(this->reb_serial_port));
    reb_fd = -1;
  
    data = new player_reb_data_t;
    command = new player_reb_cmd_t;

    reqqueue = (unsigned char*)(new playerqueue_elt_t[reqqueuelen]);
    repqueue = (unsigned char*)(new playerqueue_elt_t[repqueuelen]);

    SetupBuffers((unsigned char*)data, sizeof(player_reb_data_t), 
                 (unsigned char*)command, sizeof(player_reb_cmd_t),
                 reqqueue, reqqueuelen,
                 repqueue, repqueuelen);

    ((player_reb_cmd_t*)device_command)->position.xspeed = 0;
    ((player_reb_cmd_t*)device_command)->position.yawspeed = 0;

    this->reb_subscriptions = 0;
    this->ir_subscriptions = 0;
    this->pos_subscriptions = 0;
    this->power_subscriptions = 0;

    //set up the poll parameters... used for the comms
    // over the serial port to the Kam
    write_pfd.events = POLLOUT;
    read_pfd.events = POLLIN;

    // we want to stagger our writes to the serial port
    // so we are doing some rudimentary scheduling
    GlobalTime->GetTime(&this->last_pos_update);
    this->last_ir_update = this->last_pos_update;
    if (this->last_ir_update.tv_usec < 
	this->last_ir_update.tv_usec+REB_IR_UPDATE_PERIOD*1000) {
      this->last_ir_update.tv_usec += REB_IR_UPDATE_PERIOD*1000;
    } else {
      this->last_ir_update.tv_sec++;
      this->last_ir_update.tv_usec += REB_IR_UPDATE_PERIOD*1000;
    }
    this->last_power_update = this->last_pos_update;

    pthread_mutex_init(&reb_accessMutex,NULL);
    pthread_mutex_init(&reb_setupMutex,NULL);

    initdone = true; 
  }
  else
  {
    // every sub-device gets its own queue object (but they all point to the
    // same chunk of memory)
    
    // every sub-device needs to get its various pointers set up
    SetupBuffers((unsigned char*)data, sizeof(player_reb_data_t),
                 (unsigned char*)command, sizeof(player_reb_cmd_t),
                 reqqueue, reqqueuelen,
                 repqueue, repqueuelen);
  }

  this->param_index = 0;   
  this->refresh_last_position = true;

  strncpy(reb_serial_port, cf->ReadString(section, "port", reb_serial_port),
	  sizeof(reb_serial_port));
  
  // zero the subscription counter.
  subscriptions = 0;
}

void 
REB::Lock()
{
  // keep track of our locks cuz we seem to lose one
  // somewhere somehow
  locks++;
  pthread_mutex_lock(&reb_accessMutex);
}

void 
REB::Unlock()
{
  locks--;
  pthread_mutex_unlock(&reb_accessMutex);
}

void 
REB::SetupLock()
{
  slocks++;
  pthread_mutex_lock(&reb_setupMutex);
}

void 
REB::SetupUnlock()
{
  slocks--;
  pthread_mutex_unlock(&reb_setupMutex);
}

/* called the first time a client connects
 *
 * returns: 0 on success
 */
int 
REB::Setup()
{
  struct termios oldtio;
  struct termios params;

  // open and initialize the serial port from the ARM -> REB  
  printf("REB: connection initializing (%s)...\n", this->reb_serial_port);
  fflush(stdout);

  if ((this->reb_fd = open(this->reb_serial_port, O_RDWR | O_NOCTTY)) == -1) {
    perror("REB::Setup():open()");
    return(1);
  }	

  // set the poll params
  write_pfd.fd = reb_fd;
  read_pfd.fd = reb_fd;
  
  bzero(&params, sizeof(params));  
  tcgetattr(this->reb_fd, &oldtio); /* save current serial port settings */
  params.c_cflag = REB_BAUDRATE | CS8 | CLOCAL | CREAD | CSTOPB;
  params.c_iflag = 0; 
  params.c_oflag = 0;
  params.c_lflag = ICANON; 
   
  params.c_cc[VMIN] = 0;
  params.c_cc[VTIME] = 0;
   
  tcflush(this->reb_fd, TCIFLUSH);
  tcsetattr(this->reb_fd, TCSANOW, &params);

  //  Restart();

  // so no IRs firing
  SetIRState(REB_IR_STOP);

  this->param_index =0;
  this->motors_enabled = false;
  this->velocity_mode = true;
  this->direct_velocity_control = false;
  this->refresh_last_position = false;

  this->pos_update_period = REB_POS_UPDATE_PERIOD_VEL;

  this->desired_heading = 0;

  /* now spawn reading thread */
  StartThread();
  return(0);
}


int REB::Shutdown()
{
  printf("REB: SHUTDOWN\n");

  StopThread();

  SetSpeed(REB_MOTOR_LEFT, 0);
  SetSpeed(REB_MOTOR_RIGHT, 0);

  SetIRState(REB_IR_STOP);
  // zero these out or we may have problems
  // next time we connect
  player_reb_cmd_t cmd;

  cmd.position.xspeed = 0;
  cmd.position.yawspeed = 0;
  cmd.position.yaw = 0;

  //  PutCommand((uint8_t *)&cmd, sizeof(cmd));


  if (locks > 0) {
    printf("REB: %d LOCKS STILL EXIST\n", locks);
    while (locks) {
      Unlock();
    }
  }
  
  close(this->reb_fd);
  this->reb_fd = -1;
  return(0);
}

int REB::Subscribe(void *client)
{
  int setupResult;

  SetupLock();

  if(reb_subscriptions == 0) 
  {
    setupResult = Setup();
    if (setupResult == 0 ) 
    {
      reb_subscriptions++;  // increment the static reb-wide subscr counter
      subscriptions++;       // increment the per-device subscr counter
    }
  }
  else 
  {
    reb_subscriptions++;  // increment the static reb-wide subscr counter
    subscriptions++;       // increment the per-device subscr counter
    setupResult = 0;
  }
  
  SetupUnlock();
  return( setupResult );
}

int REB::Unsubscribe(void *client)
{
  int shutdownResult;

  SetupLock();
  
  if(reb_subscriptions == 0) 
  {
    shutdownResult = -1;
  }
  else if(reb_subscriptions == 1) 
  {
    shutdownResult = Shutdown();
    if (shutdownResult == 0 ) 
    { 
      reb_subscriptions--;  // decrement the static reb-wide subscr counter
      subscriptions--;       // decrement the per-device subscr counter
    }
    /* do we want to unsubscribe even though the shutdown went bad? */
  }
  else 
  {
    reb_subscriptions--;  // decrement the static reb-wide subscr counter
    subscriptions--;       // decrement the per-device subscr counter
    shutdownResult = 0;
  }
  
  SetupUnlock();

  return( shutdownResult );
}


void REB::PutData( unsigned char* src, size_t maxsize,
                         uint32_t timestamp_sec, uint32_t timestamp_usec)
{
  Lock();

  *((player_reb_data_t*)device_data) = *((player_reb_data_t*)src);


  if(timestamp_sec == 0)
  {
    struct timeval curr;
    GlobalTime->GetTime(&curr);
    timestamp_sec = curr.tv_sec;
    timestamp_usec = curr.tv_usec;
  }

  data_timestamp_sec = timestamp_sec;
  data_timestamp_usec = timestamp_usec;
  
  // need to fill in the timestamps on all REB devices, both so that they
  // can read it, but also because other devices may want to read it
  player_device_id_t id = device_id;

  id.code = PLAYER_IR_CODE;
  CDevice* ir = deviceTable->GetDevice(id);
  if(ir)
  {
    ir->data_timestamp_sec = this->data_timestamp_sec;
    ir->data_timestamp_usec = this->data_timestamp_usec;
  }

  id.code = PLAYER_POWER_CODE;
  CDevice* power = deviceTable->GetDevice(id);
  if(power)
  {
    power->data_timestamp_sec = this->data_timestamp_sec;
    power->data_timestamp_usec = this->data_timestamp_usec;
  }

  id.code = PLAYER_POSITION_CODE;
  CDevice* position = deviceTable->GetDevice(id);
  if(position)
  {
    position->data_timestamp_sec = this->data_timestamp_sec;
    position->data_timestamp_usec = this->data_timestamp_usec;
  }

  Unlock();
}

void 
REB::Main()
{
  player_reb_cmd_t cmd;

  short last_trans_command=0, last_rot_command=0;
  int leftvel=0, rightvel=0;
  int leftpos=0, rightpos=0;

  //  static struct timeval pmot={0,0};
  //  struct timeval ds, de, da={0,0};
  // first get pointers to all the devices we control
  player_device_id_t id = device_id;

  id.code = PLAYER_IR_CODE;
  CDevice *ir = deviceTable->GetDevice(id);

  id.code = PLAYER_POSITION_CODE;
  CDevice *pos = deviceTable->GetDevice(id);

  id.code = PLAYER_POWER_CODE;
  CDevice *power = deviceTable->GetDevice(id);

  this->pos_subscriptions = 0;
  this->ir_subscriptions = 0;
  this->power_subscriptions = 0;

  GlobalTime->GetTime(&timeBegan_tv);

  pthread_setcanceltype(PTHREAD_CANCEL_DEFERRED,NULL);

  while (1) {
    // we want to turn on the IR if someone just subscribed, and turn
    // them off if the last subscriber just unsubscribed.
    if(ir) {
      if(!this->ir_subscriptions && ir->subscriptions) {
	// then someone just subbed to IR
	SetIRState(REB_IR_START);
	
      } else if(this->ir_subscriptions && !(ir->subscriptions)) {
	// then last person stopped sub from IR..
	SetIRState(REB_IR_STOP);
	
      }
      
      this->ir_subscriptions = ir->subscriptions;
    }
    
    // we want to reset the odometry and enable the motors if the first 
    // client just subscribed to the position device, and we want to stop 
    // and disable the motors if the last client unsubscribed.
    if(pos) {
      if(!this->pos_subscriptions && pos->subscriptions) {
	printf("REB: first pos sub. turn off and reset\n");
	// then first sub for pos, so turn off motors and reset odom
	SetSpeed(REB_MOTOR_LEFT, 0);