obot.cc

机器人仿真软件

/*
 *  Player - One Hell of a Robot Server
 *  Copyright (C) 2000-2003
 *     Brian Gerkey
 *                      
 *
 *  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
 *
 */

/*
 * $Id: obot.cc,v 1.22.2.1 2006/09/21 03:51:36 gerkey Exp $
 *
 *
 * Some of this code is borrowed and/or adapted from the 'cerebellum'
 * module of CARMEN; thanks to the authors of that module.
 */

/** @ingroup drivers */
/** @{ */
/** @defgroup driver_obot obot
 * @brief Botrics Obot mobile robot

The obot driver controls the Obot robot, made by Botrics.  It's a
small, very fast robot that can carry a SICK laser (talk to the laser
over a normal serial port using the @ref driver_sicklms200 driver).

@par Compile-time dependencies

- none

@par Provides

- @ref interface_position2d
- @ref interface_power

@par Requires

- none

@par Supported commands

- PLAYER_POSITION2D_CMD_VEL
- PLAYER_POSITION2D_CMD_CAR

@par Supported configuration requests

- PLAYER_POSITION2D_REQ_GET_GEOM
- PLAYER_POSITION2D_REQ_SET_ODOM
- PLAYER_POSITION2D_REQ_RESET_ODOM

@par Configuration file options

- offset (length tuple)
  - Default: [0.0 0.0 0.0]
  - Offset of the robot's center of rotation

- size (length tuple)
  - Default: [0.45 0.45]
  - Bounding box (length, width) of the robot

- port (string)
  - Default: "/dev/usb/ttyUSB1"
  - Serial port used to communicate with the robot.

- max_speed (length, angle tuple)
  - Default: [0.5 40.0]
  - Maximum (translational, rotational) velocities

- max_accel (integer)
  - Default: 5
  - Maximum acceleration/deceleration (units?)

- motors_swapped (integer)
  - Default: 0
  - If non-zero, then assume that the motors and encoders connections
    are swapped.

- car_angle_deadzone (angle)
  - Default: 5.0 degrees
  - Minimum angular error required to induce servoing when in car-like
    command mode.

- car_angle_p (float)
  - Default: 1.0
  - Value to be multiplied by angular error (in rad) to produce angular 
    velocity command (in rad/sec) when in car-like command mode
  
@par Example 

@verbatim
driver
(
  name "obot"
  provides ["position2d:0"]
)
@endverbatim

@author Brian Gerkey
*/
/** @} */

#ifdef HAVE_CONFIG_H
  #include "config.h"
#endif

#include <errno.h>
#include <fcntl.h>
#include <stdio.h>
#include <string.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <sys/time.h>
#include <termios.h>
#include <stdlib.h>
#include <unistd.h>
#include <math.h>

#include <replace/replace.h>
#include <libplayercore/playercore.h>
#include "obot_constants.h"

static void StopRobot(void* obotdev);

class Obot : public Driver 
{
  private:
    // this function will be run in a separate thread
    virtual void Main();
    
    // bookkeeping
    bool fd_blocking;
    double px, py, pa;  // integrated odometric pose (m,m,rad)
    int last_ltics, last_rtics;
    bool odom_initialized;
    player_devaddr_t position_addr;
    player_devaddr_t power_addr;
    double max_xspeed, max_yawspeed;
    bool motors_swapped;
    int max_accel;

    // Minimum angular error required to induce servoing when in car-like
    // command mode.
    double car_angle_deadzone;
    // Value to be multiplied by angular error (in rad) to produce angular 
    // velocity command (in rad/sec) when in car-like command mode
    double car_angle_p;

    // Robot geometry (size and rotational offset)
    player_bbox_t robot_size;
    player_pose_t robot_pose;

    // methods for internal use
    int WriteBuf(unsigned char* s, size_t len);
    int ReadBuf(unsigned char* s, size_t len);
    int BytesToInt32(unsigned char *ptr);
    void Int32ToBytes(unsigned char* buf, int i);
    int ValidateChecksum(unsigned char *ptr, size_t len);
    int GetOdom(int *ltics, int *rtics, int *lvel, int *rvel);
    void UpdateOdom(int ltics, int rtics);
    unsigned char ComputeChecksum(unsigned char *ptr, size_t len);
    int SendCommand(unsigned char cmd, int val1, int val2);
    int ComputeTickDiff(int from, int to);
    int ChangeMotorState(int state);
    int OpenTerm();
    int InitRobot();
    int GetBatteryVoltage(int* voltage);
    double angle_diff(double a, double b);

    player_position2d_cmd_car_t last_car_cmd;
    int last_final_lvel, last_final_rvel;
    bool sent_new_command;
    bool car_command_mode;

  public:
    int fd; // device file descriptor
    const char* serial_port; // name of dev file

    // public, so that it can be called from pthread cleanup function
    int SetVelocity(int lvel, int rvel);

    Obot(ConfigFile* cf, int section);

    void ProcessCommand(player_position2d_cmd_vel_t * cmd);
    void ProcessCarCommand(player_position2d_cmd_car_t * cmd);

    // Process incoming messages from clients 
    int ProcessMessage(MessageQueue * resp_queue, 
                       player_msghdr * hdr, 
                       void * data);

    virtual int Setup();
    virtual int Shutdown();
};


// initialization function
Driver* Obot_Init( ConfigFile* cf, int section)
{
  return((Driver*)(new Obot( cf, section)));
}

// a driver registration function
void 
Obot_Register(DriverTable* table)
{
  table->AddDriver("obot",  Obot_Init);
}

Obot::Obot( ConfigFile* cf, int section) 
  : Driver(cf,section,true,PLAYER_MSGQUEUE_DEFAULT_MAXLEN)
{
  memset(&this->position_addr,0,sizeof(player_devaddr_t));
  memset(&this->power_addr,0,sizeof(player_devaddr_t));

  // Do we create a robot position interface?
  if(cf->ReadDeviceAddr(&(this->position_addr), section, "provides",
                        PLAYER_POSITION2D_CODE, -1, NULL) == 0)
  {
    if(this->AddInterface(this->position_addr) != 0)
    {
      this->SetError(-1);    
      return;
    }

    this->robot_size.sl = cf->ReadTupleLength(section, "size", 
                                              0, OBOT_LENGTH);
    this->robot_size.sw = cf->ReadTupleLength(section, "size", 
                                              1, OBOT_WIDTH);
    this->robot_pose.px = cf->ReadTupleLength(section, "offset",
                                              0, OBOT_POSE_X);
    this->robot_pose.py = cf->ReadTupleLength(section, "offset",
                                              1, OBOT_POSE_Y);
    this->robot_pose.pa = cf->ReadTupleAngle(section, "offset",
                                             2, OBOT_POSE_A);

    this->max_xspeed = cf->ReadTupleLength(section, "max_speed",
                                           0, 0.5);
    this->max_yawspeed = cf->ReadTupleAngle(section, "max_speed",
                                            1, DTOR(40.0));
    this->motors_swapped = cf->ReadInt(section, "motors_swapped", 0);
    this->max_accel = cf->ReadInt(section, "max_accel", 5);

    this->car_angle_deadzone = cf->ReadAngle(section, "car_angle_deadzone",
                                             DTOR(5.0));
    this->car_angle_p = cf->ReadFloat(section, "car_angle_p", 1.0);
  }

  // Do we create a power interface?
  if(cf->ReadDeviceAddr(&(this->power_addr), section, "provides", 
                      PLAYER_POWER_CODE, -1, NULL) == 0)
  {
    if(this->AddInterface(this->power_addr) != 0)
    {
      this->SetError(-1);
      return;
    }
  }

  this->fd = -1;
  this->serial_port = cf->ReadString(section, "port", OBOT_DEFAULT_PORT);
}

int
Obot::InitRobot()
{
  // initialize the robot
  unsigned char initstr[3];
  initstr[0] = OBOT_INIT1;
  initstr[1] = OBOT_INIT2;
  initstr[2] = OBOT_INIT3;
  unsigned char deinitstr[1];
  deinitstr[0] = OBOT_DEINIT;

  if(tcflush(this->fd, TCIOFLUSH) < 0 )
  {
    PLAYER_ERROR1("tcflush() failed: %s", strerror(errno));
    close(this->fd);
    this->fd = -1;
    return(-1);
  }

  if(WriteBuf(initstr,sizeof(initstr)) < 0)
  {
    PLAYER_WARN("failed to initialize robot; i'll try to de-initializate it");
    if(WriteBuf(deinitstr,sizeof(deinitstr)) < 0)
    {
      PLAYER_ERROR("failed on write of de-initialization string");
      return(-1);
    }
    if(WriteBuf(initstr,sizeof(initstr)) < 0)
    {
      PLAYER_ERROR("failed on 2nd write of initialization string; giving up");
      return(-1);
    }
  }
  return(0);
}

int
Obot::OpenTerm()
{
  struct termios term;
  
  // open it.  non-blocking at first, in case there's no robot
  if((this->fd = open(serial_port, O_RDWR | O_SYNC | O_NONBLOCK, S_IRUSR | S_IWUSR )) < 0 )
  {
    PLAYER_ERROR1("open() failed: %s", strerror(errno));
    return(-1);
  }  
 
  if(tcgetattr(this->fd, &term) < 0 )
  {
    PLAYER_ERROR1("tcgetattr() failed: %s", strerror(errno));
    close(this->fd);
    this->fd = -1;
    return(-1);
  }
  
  cfmakeraw(&term);
  cfsetispeed(&term, B57600);
  cfsetospeed(&term, B57600);
  
  if(tcsetattr(this->fd, TCSAFLUSH, &term) < 0 )
  {
    PLAYER_ERROR1("tcsetattr() failed: %s", strerror(errno));
    close(this->fd);
    this->fd = -1;
    return(-1);
  }

  fd_blocking = false;
  return(0);
}

int 
Obot::Setup()
{
  int flags;
  int ltics,rtics,lvel,rvel;

  this->px = this->py = this->pa = 0.0;
  this->odom_initialized = false;
  this->last_final_rvel = this->last_final_lvel = 0;
  this->sent_new_command = false;
  this->car_command_mode = false;

  printf("Botrics Obot connection initializing (%s)...", serial_port);
  fflush(stdout);

  if(OpenTerm() < 0)
  {
    PLAYER_ERROR("failed to initialize robot");
    return(-1);
  }

  if(InitRobot() < 0)
  {
    PLAYER_ERROR("failed to initialize robot");
    close(this->fd);
    this->fd = -1;
    return(-1);
  }

  /* try to get current odometry, just to make sure we actually have a robot */
  if(GetOdom(&ltics,&rtics,&lvel,&rvel) < 0)
  {
    PLAYER_ERROR("failed to get odometry");
    close(this->fd);
    this->fd = -1;
    return(-1);
  }

  UpdateOdom(ltics,rtics);

  /* ok, we got data, so now set NONBLOCK, and continue */
  if((flags = fcntl(this->fd, F_GETFL)) < 0)
  {
    PLAYER_ERROR1("fcntl() failed: %s", strerror(errno));
    close(this->fd);
    this->fd = -1;
    return(-1);
  }
  if(fcntl(this->fd, F_SETFL, flags ^ O_NONBLOCK) < 0)
  {
    PLAYER_ERROR1("fcntl() failed: %s", strerror(errno));
    close(this->fd);
    this->fd = -1;
    return(-1);
  }
  fd_blocking = true;
  puts("Done.");

  // TODO: what are reasoanable numbers here?