sickpls.cc

机器人仿真软件

/*
 *  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
 *
 */

///////////////////////////////////////////////////////////////////////////
//
// File: laserdevice.cc
// Author: Andrew Howard
// Date: 7 Nov 2000
// Desc: Driver for the SICK laser
//
// CVS info:
//  $Source: /cvsroot/playerstage/code/player/server/drivers/laser/sickpls.cc,v $
//  $Author: gerkey $
//  $Revision: 1.10 $
//
// Usage:
//  (empty)
//
// Theory of operation:
//  (empty)
//
// Known bugs:
//  (empty)
//
// Possible enhancements:
//  (empty)
//
///////////////////////////////////////////////////////////////////////////

/** @ingroup drivers */
/** @{ */
/** @defgroup driver_sickpls sickpls
 * @brief SICK PLS laser range-finder

The sickpls driver controls the SICK PLS scanning laser range-finder.
This driver will likely be merged into the @ref driver_sicklms200
driver (eventually).

@par Compile-time dependencies

- none

@par Provides

- @ref interface_laser

@par Requires

- none

@par Configuration requests

- PLAYER_LASER_GET_GEOM
- PLAYER_LASER_GET_CONFIG
- PLAYER_LASER_SET_CONFIG
  
@par Configuration file options

- port (string)
  - Default: "/dev/ttyS1"
  - Serial port to which laser is attached.  If you are using a
    USB/232 or USB/422 converter, this will be "/dev/ttyUSBx".

- rate (integer)
  - Default: 9600
  - Baud rate.  Valid values are 9600, 38400 (RS232 or RS422) and
    500000 (RS422 only).
  
- delay (integer)
  - Default: 0
  - Delay (in seconds) before laser is initialized (set this to 35 if
    you have a newer generation Pioneer whose laser is switched on
    when the serial port is open).

- resolution (integer)
  - Default: 50
  - Angular resolution.  Valid values are:
    - resolution 50 : 0.5 degree increments, 361 readings @ 5Hz (38400) or 32Hz (500000).
    - resolution 100 : 1 degree increments, 181 readings @ 10Hz (38400) or 75Hz (500000).

- invert (integer)
  - Default: 0
  - Is the laser physically inverted (i.e., upside-down)?  Is so, scan data 
    will be reversed accordingly.

- pose (length tuple)
  - Default: [0.0 0.0 0.0]
  - Pose (x,y,theta) of the laser, relative to its parent object (e.g.,
    the robot to which the laser is attached).

- autodetect_rate (integer)
  - Default: 1
  - Set to 0 to avoid baud rate autodetection, which fails on some lasers.

- ignore_errors (integer)
  - Default: 0
  - Ignore errors during initialization of the laser.
      
@par Example 

@verbatim
driver
(
  name "sickpls"
  provides ["laser:0"]
  port "/dev/ttyS0"
)
@endverbatim

@author Yannick Brosseau, Andrew Howard

*/
/** @} */

#if HAVE_CONFIG_H
  #include <config.h>
#endif

#include <assert.h>
#include <errno.h>
#include <fcntl.h>
#include <stdio.h>
#include <string.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <termios.h>
#include <unistd.h>
//#include <netinet/in.h>  /* for struct sockaddr_in, htons(3) */
#include <sys/ioctl.h>
#include <math.h>

#undef HAVE_HI_SPEED_SERIAL
#ifdef HAVE_LINUX_SERIAL_H
  #ifndef DISABLE_HIGHSPEEDSICK
    #include <linux/serial.h>
    #define HAVE_HI_SPEED_SERIAL
  #endif
#endif

#define PLAYER_ENABLE_MSG 0
#define PLAYER_ENABLE_TRACE 0

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

#define DEFAULT_LASER_PORT "/dev/ttyS1"
#define DEFAULT_LASER_PORT_RATE 9600

// The laser device class.
class SickPLS : public Driver
{
  public:
    
    // Constructor
    SickPLS( ConfigFile* cf, int section);

    int Setup();
    int Shutdown();

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

    // Main function for device thread.
    virtual void Main();

    // Process configuration requests.  Returns 1 if the configuration
    // has changed.
    //int UpdateConfig();

    // Compute the start and end scan segments based on the current resolution and
    // scan angles.  Returns 0 if the configuration is valid.
    int CheckScanConfig();
    
    // Open the terminal
    // Returns 0 on success
    int OpenTerm();

    // Close the terminal
    // Returns 0 on success
    int CloseTerm();
    
    // Set the terminal speed
    // Valid values are 9600 and 38400
    // Returns 0 on success
    int ChangeTermSpeed(int speed);

    // Get the laser type
    int GetLaserType(char *buffer, size_t bufflen);

    // Put the laser into configuration mode
    int SetLaserMode();

    // Set the laser data rate
    // Valid values are 9600 and 38400
    // Returns 0 on success
    int SetLaserSpeed(int speed);

    // Set the laser configuration
    // Returns 0 on success
    int SetLaserConfig(bool intensity);
    
    // Request data from the laser
    // Returns 0 on success
    int RequestLaserData(int min_segment, int max_segment);

    // Read range data from laser
    int ReadLaserData(uint16_t *data, size_t datalen);

    // Write a packet to the laser
    ssize_t WriteToLaser(uint8_t *data, ssize_t len); 
    
    // Read a packet from the laser
    ssize_t ReadFromLaser(uint8_t *data, ssize_t maxlen, bool ack = false, int timeout = -1);

    // Calculates CRC for a telegram
    unsigned short CreateCRC(uint8_t *data, ssize_t len);

    // Get the time (in ms)
    int64_t GetTime();
    
  protected:

    // Laser pose in robot cs.
    double pose[3];
    double size[2];
    
    // Name of device used to communicate with the laser
    const char *device_name;
    
    // laser device file descriptor
    int laser_fd;           

    // Starup delay
    int startup_delay;
  
    // Scan width and resolution.
    int scan_width, scan_res;

    // Start and end scan angles (for restricted scan).  These are in
    // units of 0.01 degrees.
    int min_angle, max_angle;
    
    // Start and end scan segments (for restricted scan).  These are
    // the values used by the laser.
    int scan_min_segment, scan_max_segment;

    // Range resolution (1 = 1mm, 10 = 1cm, 100 = 10cm).
    int range_res;

    // Turn intensity data on/off
    bool intensity;

    // Is the laser upside-down? (if so, we'll reverse the ordering of the
    // readings)
    int invert;

    bool can_do_hi_speed;
    int port_rate;
  
  // Allow the autodetect mechanism for the rate - MB
  int autodetect_rate;

  // Ignore errors in initialization
  int ignore_errors;

  int type;
  
#ifdef HAVE_HI_SPEED_SERIAL
  struct serial_struct old_serial;
#endif
};

// a factory creation function
Driver* SickPLS_Init( ConfigFile* cf, int section)
{
  return((Driver*)(new SickPLS( cf, section)));
}

// a driver registration function
void SickPLS_Register(DriverTable* table)
{
  table->AddDriver("sickpls", SickPLS_Init);
}

////////////////////////////////////////////////////////////////////////////////
// Device codes

#define STX     0x02
#define ACK     0xA0
#define NACK    0x92
#define CRC16_GEN_POL 0x8005
#define MAX_RETRIES 5


////////////////////////////////////////////////////////////////////////////////
// Error macros
#define RETURN_ERROR(erc, m) {PLAYER_ERROR(m); return erc;}


////////////////////////////////////////////////////////////////////////////////
// Constructor
SickPLS::SickPLS( ConfigFile* cf, int section)
    : Driver(cf, section, true, PLAYER_MSGQUEUE_DEFAULT_MAXLEN, PLAYER_LASER_CODE)
{
  // Laser geometry.
  this->pose[0] = cf->ReadTupleLength(section, "pose", 0, 0.0);
  this->pose[1] = cf->ReadTupleLength(section, "pose", 1, 0.0);;
  this->pose[2] = cf->ReadTupleLength(section, "pose", 2, 0.0);;
  this->size[0] = 0.15;
  this->size[1] = 0.15;

  // Default serial port
  this->device_name = cf->ReadString(section, "port", DEFAULT_LASER_PORT);

  // Set default configuration
  this->startup_delay = cf->ReadInt(section, "delay", 0);
  this->scan_width = 180;
  this->scan_res = cf->ReadInt(section, "resolution", 50);
  this->min_angle = -9000;
  this->max_angle = +9000;
  this->scan_min_segment = 0;
  this->scan_max_segment = 360;
  this->intensity = true;
  this->range_res = 10;
  this->invert = cf->ReadInt(section, "invert", 0);

  this->port_rate = cf->ReadInt(section, "rate", DEFAULT_LASER_PORT_RATE);
  this->autodetect_rate = cf->ReadInt(section, "autodetect_rate", 1);
  this->ignore_errors = cf->ReadInt(section, "ignore_errors", 0);

#ifdef HAVE_HI_SPEED_SERIAL
  this->can_do_hi_speed = true;
#else
  this->can_do_hi_speed = false;
#endif

  //TBM: need to validation the speed capacity of the PLS
  if (!this->can_do_hi_speed && this->port_rate > 38400) {
    fprintf(stderr, "sickpls: requested hi speed serial, but no support compiled in.  Defaulting to 38400 bps.\n");
    this->port_rate = 9600;
  }

  if (this->CheckScanConfig() != 0)
    PLAYER_ERROR("invalid scan configuration");

  return;
}


////////////////////////////////////////////////////////////////////////////////
// Set up the device
int SickPLS::Setup()
{   
  printf("Laser initialising (%s)\n", this->device_name);
    
  // Open the terminal
  if (OpenTerm())
    return 1;

  // Some Pioneers only power laser after the terminal is opened; wait
  // for the laser to initialized
  sleep(this->startup_delay);

  //////// MB
  // The autodetect mechanism for speed might not work for all (older) PLS lasers
  // we are then forced to use the rate given in the configuration file and *not* change it. 
  if(!autodetect_rate) {
    if (ChangeTermSpeed(port_rate))
      return 1;
    if (RequestLaserData(0,360) != 0)
      {
	PLAYER_ERROR("connection failed");
	return 1;
      }
    puts("laser ready");
    
    // Start the device thread
    StartThread();
    
    return 0;
  }
  ////////// 


  // Start out at 38400 with non-blocking io
  if (ChangeTermSpeed(38400))
    return 1;

  PLAYER_MSG0(2, "connecting at 38400");
  if (RequestLaserData(0,360) != 0)
  {
  
    PLAYER_MSG0(2, "connect at 38400 failed, trying 9600");
    if (ChangeTermSpeed(9600))
      return 1;
    if (RequestLaserData(0,360) != 0)
    {
      PLAYER_ERROR("connection failed");
      return 1;
    }
    PLAYER_MSG0(2, "laser operating at 9600; changing to 38400");

    if (SetLaserSpeed(38400))
      return 1;
    if (ChangeTermSpeed(38400))
      return 1;
  }

  puts("laser ready");

  // Start the device thread
  StartThread();

  return 0;
}


////////////////////////////////////////////////////////////////////////////////
// Shutdown the device
int SickPLS::Shutdown()
{
  // shutdown laser device
  StopThread();

  if (port_rate > 38400) {
    SetLaserSpeed(9600);
  }

  CloseTerm();
  puts("Laser has been shutdown");

  return(0);
}


////////////////////////////////////////////////////////////////////////////////
// Main function for device thread
void SickPLS::Main() 
{
  float tmp;
  // Ask the laser to send data

  for (int retry = 0; retry < MAX_RETRIES; retry++)
  {
    if (RequestLaserData(this->scan_min_segment, this->scan_max_segment) == 0)
      break;
    else if (retry >= MAX_RETRIES)
    {
      PLAYER_ERROR("laser not responding; exiting laser thread");
      return;
    }
  }

  while (true)
  {
    // test if we are supposed to cancel
    pthread_testcancel();

    // Update the configuration.
/*    if (UpdateConfig())
    {
      if (SetLaserMode() != 0)
        PLAYER_ERROR("request for config mode failed");
      else
      {
        //TBM: Check the configurability of the PLS before enabling this
        //if (SetLaserConfig(this->intensity) != 0)
        //  PLAYER_ERROR("failed setting intensity");          
      }

      // Issue a new request for data
      if (RequestLaserData(this->scan_min_segment, this->scan_max_segment))
        PLAYER_ERROR("request for laser data failed");
    }
*/
    // Get the time at which we started reading
    // This will be a pretty good estimate of when the phenomena occured
    struct timeval time;
    GlobalTime->GetTime(&time);
    
    // Process incoming data
    player_laser_data_t data;
    uint16_t * TempData = new uint16_t[sizeof(data.ranges) / sizeof(data.ranges[0])];
    if (ReadLaserData(TempData, sizeof(data.ranges) / sizeof(data.ranges[0])) == 0)
    {
      // Prepare packet 
      data.min_angle = (this->scan_min_segment * this->scan_res - this->scan_width * 50);
      data.max_angle = (this->scan_max_segment * this->scan_res - this->scan_width * 50);
      data.resolution = (this->scan_res);