amtecpowercube.cc

机器人仿真软件

/*
 *  Player - One Hell of a Robot Server
 *  Copyright (C) 2003  Brian Gerkey [e-mail]
 *
 *  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: amtecpowercube.cc,v 1.17.2.2 2006/09/25 15:54:58 gerkey Exp $
 */

/** @ingroup drivers */
/** @{ */
/** @defgroup driver_amtecpowercube amtecpowercube
 * @brief Amtec PowerCube pan-tilt unit

The amtecpowercube driver controls the Amtec PowerCube Wrist,
a powerful pan-tilt unit that can, for example, carry a SICK laser
(@ref driver_sicklms200).

This driver communicates with the PowerCube via RS232, and does NOT handle
the newer CAN-based units.  Please submit a patch to support the CAN
protocol.

The amtecpowercube driver supports both position and velocity control,
via the PLAYER_PTZ_CONTROL_MODE_REQ request.  For constant swiveling,
the PowerCube works better under velocity control.

Note that this driver is relatively new and not thoroughly tested.

@par Compile-time dependencies

- none

@par Provides

- @ref interface_ptz

@par Requires

- none

@par Configuration requests

- PLAYER_PTZ_CONTROL_MODE_REQ

@par Configuration file options

- port (string)
  - Default: "/dev/ttyS0"
  - Serial port where the unit is attached.
- home (integer)
  - Default: 0
  - Whether to home (i.e., reset to the zero position) the unit before
    commanding it
- speed (angle)
  - Default: 40 deg/sec
  - Maximum pan/tilt speed 

@par Example

@verbatim
driver
(
  name "amtecpowercube"
  port "/dev/ttyS0"
  home 1
)
@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 <termios.h>
#include <stdlib.h>
#include <unistd.h>
#include <netinet/in.h>  /* for struct sockaddr_in, htons(3) */
#include <math.h>

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


#define AMTEC_DEFAULT_PORT "/dev/ttyS0"

#define AMTEC_SLEEP_TIME_USEC 20000

/* angular velocity used when in position control mode */
#define AMTEC_DEFAULT_SPEED_DEG_PER_SEC DTOR(40)

// start, end, and escape chars
#define AMTEC_STX       0x02
#define AMTEC_ETX       0x03
#define AMTEC_DLE       0x10

// sizes
#define AMTEC_MAX_CMDSIZE     48

// module IDs
#define AMTEC_MODULE_TILT       11
#define AMTEC_MODULE_PAN        12

// command IDs
#define AMTEC_CMD_RESET        0x00
#define AMTEC_CMD_HOME         0x01
#define AMTEC_CMD_HALT         0x02
#define AMTEC_CMD_SET_EXT      0x08
#define AMTEC_CMD_GET_EXT      0x0a
#define AMTEC_CMD_SET_MOTION   0x0b
#define AMTEC_CMD_SET_ISTEP    0x0d

// parameter IDs
#define AMTEC_PARAM_ACT_POS   0x3c
#define AMTEC_PARAM_MIN_POS   0x45
#define AMTEC_PARAM_MAX_POS   0x46
#define AMTEC_PARAM_CUBESTATE 0x27
#define AMTEC_PARAM_MAXCURR   0x4c
#define AMTEC_PARAM_ACT_VEL   0x41

// motion IDs
#define AMTEC_MOTION_FRAMP       4
#define AMTEC_MOTION_FRAMP_ACK  14
#define AMTEC_MOTION_FSTEP_ACK  16
#define AMTEC_MOTION_FVEL_ACK   17

// module state bitmasks
#define AMTEC_STATE_ERROR     0x01
#define AMTEC_STATE_HOME_OK   0x02
#define AMTEC_STATE_HALTED    0x04

class AmtecPowerCube:public Driver 
{
  private:
    // this function will be run in a separate thread
    virtual void Main();

    // bookkeeping
    bool fd_blocking;
    int return_to_home;
    int minpan, maxpan;
    int mintilt, maxtilt;
    int speed;
    uint8_t controlmode;

    // low-level methods to interact with the device
    int SendCommand(int id, unsigned char* cmd, size_t len);
    int WriteData(unsigned char *buf, size_t len);
    int AwaitAnswer(unsigned char* buf, size_t len);
    int AwaitETX(unsigned char* buf, size_t len);
    int ReadAnswer(unsigned char* buf, size_t len);
    size_t ConvertBuffer(unsigned char* buf, size_t len);
    int GetFloatParam(int id, int param, float* val);
    int GetUint32Param(int id, int param, unsigned int* val);
    int SetFloatParam(int id, int param, float val);

    // data (de)marshalling helpers
    // NOTE: these currently assume little-endianness, which is NOT
    //       portable (but works fine on x86).
    float BytesToFloat(unsigned char* bytes);
    unsigned int BytesToUint32(unsigned char* bytes);
    void FloatToBytes(unsigned char *bytes, float f);
    void Uint16ToBytes(unsigned char *bytes, unsigned short s);

    // higher-level methods for common use
    int GetPanTiltPos(short* pan, short* tilt);
    int GetPanTiltVel(short* panspeed, short* tiltspeed);
    int SetPanPos(short oldpan, short pan);
    int SetTiltPos(short oldtilt, short tilt);
    int SetPanVel(short panspeed);
    int SetTiltVel(short tiltspeed);
    int Home();
    int Halt();
    int Reset();
    int SetLimits();

    // helper for dealing with config requests.
    //void HandleConfig(void *client, unsigned char *buf, size_t len);
      short lastpan, lasttilt;

      short lastpanspeed, lasttiltspeed;

  public:
    int fd; // amtec device file descriptor
    /* device used to communicate with the ptz */
    const char* serial_port;

    AmtecPowerCube( ConfigFile* cf, int section);

    // MessageHandler
    int ProcessMessage(MessageQueue* resp_queue, player_msghdr * hdr, void * data);

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

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

// a driver registration function
void 
AmtecPowerCube_Register(DriverTable* table)
{
  table->AddDriver("amtecpowercube",  AmtecPowerCube_Init);
}

AmtecPowerCube::AmtecPowerCube( ConfigFile* cf, int section) 
        : Driver(cf, section, true, PLAYER_MSGQUEUE_DEFAULT_MAXLEN, PLAYER_PTZ_CODE)
{
  fd = -1;
/*  player_ptz_data_t data;
  player_ptz_cmd_t cmd;

  data.pan = data.tilt = data.zoom = 0;
  cmd.pan = cmd.tilt = cmd.zoom = 0;

  PutData((unsigned char*)&data,sizeof(data),NULL);
  PutCommand(this->device_id,(unsigned char*)&cmd,sizeof(cmd),NULL);*/

  this->serial_port = cf->ReadString(section, "port", AMTEC_DEFAULT_PORT);
  this->return_to_home = cf->ReadInt(section, "home", 0);
  this->speed = (int)rint(RTOD(cf->ReadAngle(section, "speed", 
                                             AMTEC_DEFAULT_SPEED_DEG_PER_SEC)));
}

int 
AmtecPowerCube::Reset()
{
  unsigned char buf[AMTEC_MAX_CMDSIZE];
  unsigned char cmd[1];

  cmd[0] = AMTEC_CMD_RESET;
  if(SendCommand(AMTEC_MODULE_PAN,cmd,1) < 0)
  {
    PLAYER_ERROR("SendCommand() failed");
    return(-1);
  }
  if(ReadAnswer(buf,sizeof(buf)) < 0)
  {
    PLAYER_ERROR("ReadAnswer() failed");
    return(-1);
  }
  if(SendCommand(AMTEC_MODULE_TILT,cmd,1) < 0)
  {
    PLAYER_ERROR("SendCommand() failed");
    return(-1);
  }
  if(ReadAnswer(buf,sizeof(buf)) < 0)
  {
    PLAYER_ERROR("ReadAnswer() failed");
    return(-1);
  }
  return(0);
}

int 
AmtecPowerCube::Home()
{
  unsigned char buf[AMTEC_MAX_CMDSIZE];
  unsigned char cmd[1];
  unsigned int state;

  cmd[0] = AMTEC_CMD_HOME;
  if(SendCommand(AMTEC_MODULE_PAN,cmd,1) < 0)
  {
    PLAYER_ERROR("SendCommand() failed");
    return(-1);
  }
  if(ReadAnswer(buf,sizeof(buf)) < 0)
  {
    PLAYER_ERROR("ReadAnswer() failed");
    return(-1);
  }
  // poll the device state, wait for homing to finish
  for(;;)
  {
    usleep(AMTEC_SLEEP_TIME_USEC);
    if(GetUint32Param(AMTEC_MODULE_PAN, AMTEC_PARAM_CUBESTATE, &state) < 0)
    {
      PLAYER_ERROR("GetUint32Param() failed");
      return(-1);
    }
    if(state & AMTEC_STATE_HOME_OK)
      break;
  }
  if(SendCommand(AMTEC_MODULE_TILT,cmd,1) < 0)
  {
    PLAYER_ERROR("SendCommand() failed");
    return(-1);
  }
  if(ReadAnswer(buf,sizeof(buf)) < 0)
  {
    PLAYER_ERROR("ReadAnswer() failed");
    return(-1);
  }
  // poll the device state, wait for homing to finish
  for(;;)
  {
    usleep(AMTEC_SLEEP_TIME_USEC);
    if(GetUint32Param(AMTEC_MODULE_TILT, AMTEC_PARAM_CUBESTATE, &state) < 0)
    {
      PLAYER_ERROR("GetUint32Param() failed");
      return(-1);
    }
    if(state & AMTEC_STATE_HOME_OK)
      break;
  }
  return(0);
}

int 
AmtecPowerCube::Halt()
{
  unsigned char buf[AMTEC_MAX_CMDSIZE];
  unsigned char cmd[1];

  cmd[0] = AMTEC_CMD_HALT;
  if(SendCommand(AMTEC_MODULE_PAN,cmd,1) < 0)
  {
    PLAYER_ERROR("SendCommand() failed");
    return(-1);
  }
  if(ReadAnswer(buf,sizeof(buf)) < 0)
  {
    PLAYER_ERROR("ReadAnswer() failed");
    return(-1);
  }
  if(SendCommand(AMTEC_MODULE_TILT,cmd,1) < 0)
  {
    PLAYER_ERROR("SendCommand() failed");
    return(-1);
  }
  if(ReadAnswer(buf,sizeof(buf)) < 0)
  {
    PLAYER_ERROR("ReadAnswer() failed");
    return(-1);
  }
  return(0);
}

int 
AmtecPowerCube::Setup()
{
  struct termios term;
  short pan,tilt;
  int flags;

  // default to position control
  this->controlmode = PLAYER_PTZ_POSITION_CONTROL;

  player_ptz_cmd_t cmd;
  cmd.pan = cmd.tilt = cmd.zoom = 0;

  printf("Amtec PowerCube connection initializing (%s)...", serial_port);
  fflush(stdout);

  // open it.  non-blocking at first, in case there's no ptz unit.
  if((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(tcflush(fd, TCIFLUSH ) < 0 )
  {
    PLAYER_ERROR1("tcflush() failed: %s", strerror(errno));
    close(fd);
    fd = -1;
    return(-1);
  }
  if(tcgetattr(fd, &term) < 0 )
  {
    PLAYER_ERROR1("tcgetattr() failed: %s", strerror(errno));
    close(fd);
    fd = -1;
    return(-1);
  }
  
  cfmakeraw(&term);
  cfsetispeed(&term, B38400);
  cfsetospeed(&term, B38400);
  
  if(tcsetattr(fd, TCSAFLUSH, &term) < 0 )
  {
    PLAYER_ERROR1("tcsetattr() failed: %s", strerror(errno));
    close(fd);
    fd = -1;
    return(-1);
  }

  fd_blocking = false;
  /* try to get current state, just to make sure we actually have a camera */
  if(GetPanTiltPos(&pan,&tilt))
  {
    printf("Couldn't connect to Amtec PowerCube most likely because the unit\n"
                    "is not connected or is connected not to %s\n", 
                    serial_port);
    close(fd);
    fd = -1;
    return(-1);
  }

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

  // zero the command buffer
  //PutCommand(this->device_id,(unsigned char*)&cmd,sizeof(cmd),NULL);

  // reset and home the unit.
  if(Reset() < 0)
  {
    PLAYER_ERROR("Reset() failed; bailing.");
    close(fd);
    fd = -1;
    return(-1);
  }
  if(Home() < 0)
  {
    PLAYER_ERROR("Home() failed; bailing.");
    close(fd);
    fd = -1;
    return(-1);
  }

  // start the thread to talk with the camera
  StartThread();

  return(0);
}

int
AmtecPowerCube::Shutdown()
{
  if(fd == -1)
    return(0);

  StopThread();

  // stop the unit
  if(Halt())
    PLAYER_WARN("Halt() failed.");

  // maybe return it to home
  if(return_to_home && Home())
    PLAYER_WARN("Home() failed.");

  if(close(fd))
    PLAYER_ERROR1("close() failed:%s",strerror(errno));
  fd = -1;
  puts("Amtec PowerCube has been shutdown");
  return(0);
}

////////////////////////////////////////////////////////////////////////////
// The following methods are based on some found in CARMEN.  Thanks to the
// authors.

// NOTE: these conversion methods only work on little-endian machines
// (the Amtec protocol also uses little-endian).
float
AmtecPowerCube::BytesToFloat(unsigned char *bytes)
{
  float f;
  memcpy((void*)&f, bytes, 4);
  return(f);
}
unsigned int
AmtecPowerCube::BytesToUint32(unsigned char* bytes)
{
  unsigned int i;
  memcpy((void*)&i, bytes, 4);
  return(i);
}
void
AmtecPowerCube::FloatToBytes(unsigned char *bytes, float f)
{
  memcpy(bytes, (void*)&f, 4);
}
void
AmtecPowerCube::Uint16ToBytes(unsigned char *bytes, unsigned short s)
{
  memcpy(bytes, (void*)&s, 2);
}

int 
AmtecPowerCube::SendCommand(int id, unsigned char* cmd, size_t len)
{
  size_t i;
  int ctr, add;
  unsigned char rcmd[AMTEC_MAX_CMDSIZE];
  unsigned char bcc;
  unsigned char umnr;
  unsigned char lmnr;

  add  = 0;
  lmnr = id & 7;
  lmnr = lmnr << 5;
  umnr = id >> 3;
  umnr = umnr | 4;
  for (i=0;i<len;i++) {
    if ( (cmd[i]==0x02) ||
	 (cmd[i]==0x03) ||
	 (cmd[i]==0x10) ) {
      add++;
    }
  }
  lmnr = lmnr + len;
  rcmd[0] = AMTEC_STX;
  rcmd[1] = umnr;
  rcmd[2] = lmnr;
  ctr = 3;
  for (i=0;i<len;i++) {
    switch(cmd[i]) {
    case 0x02:
      rcmd[ctr] = 0x10;
      rcmd[++ctr] = 0x82;
      break;
    case 0x03:
      rcmd[ctr] = 0x10;
      rcmd[++ctr] = 0x83;
      break;
    case 0x10:
      rcmd[ctr] = 0x10;
      rcmd[++ctr] = 0x90;
      break;
    default:
      rcmd[ctr] = cmd[i];
    }
    ctr++;
  }
  bcc = id;
  for (i=0;i<len;i++) {
    bcc += cmd[i];
  }
  bcc = bcc + (bcc>>8);
  switch(bcc) {
  case 0x02:
    rcmd[ctr++] = 0x10;
    rcmd[ctr++] = 0x82;
    break;
  case 0x03:
    rcmd[ctr++] = 0x10;
    rcmd[ctr++] = 0x83;
    break;
  case 0x10: