sicklms200.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
 *
 */
/*
 Desc: Driver for the SICK laser
 Author: Andrew Howard
 Date: 7 Nov 2000
 CVS: $Id: sicklms200.cc,v 1.58.2.2 2006/09/22 23:58:35 gerkey Exp $
*/

/** @ingroup drivers Drivers */
/** @{ */
/** @defgroup driver_sicklms200 sicklms200
 * @brief SICK LMS 200 laser range-finder


The sicklms200 driver controls the SICK LMS 200 scanning laser range-finder.

@par Compile-time dependencies

- none

@par Provides

- @ref interface_laser

@par Requires

- none

@par Configuration requests

- PLAYER_LASER_REQ_GET_GEOM
- PLAYER_LASER_REQ_GET_CONFIG
- PLAYER_LASER_REQ_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".

- connect_rate (integer)
  - Rate used when stablishing connection with the laser.
  - Default: 9600 
  - Baud rate.  Valid values are 9600, 38400 (RS232 or RS422) and
    500000 (RS422 only).

- transfer_rate (integer)
  - Rate desired for data transfers, negotiated after connection
  - Default: 38400
  - Baud rate.  Valid values are 9600, 38400 (RS232 or RS422) and
    500000 (RS422 only).

- retry (integer)
  - Default: 0
  - If the initial connection to the laser fails, retry this many times before
    giving up.
  
- delay (integer)
  - Default: 0
  - Delay (in seconds) before laser is initialized (set this to 32-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).

- range_res (integer)
  - Default: 1
  - Range resolution.  Valid values are:
    - range_res 1 : 1mm precision, 8.192m max range.
    - range_res 10 : 10mm precision, 81.92m max range.
    - range_res 100 : 100mm precision, 819.2m max range.

- 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).

- size (length tuple)
  - Default: [0.15 0.15]
  - Footprint (x,y) of the laser.
      
@par Example 

@verbatim
driver
(
  name "sicklms200"
  provides ["laser:0"]
  port "/dev/ttyS0"
  resolution 100   # Angular resolution 1 degree (181 readings @ 10Hz)
  range_res 10     # Range resolution 1 cm (maximum range 81.92m)
)
@endverbatim

@author Andrew Howard, Richard Vaughan, Kasper Stoy

*/
/** @} */
  

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

#include <assert.h>
#include <math.h>
#include <errno.h>
#include <fcntl.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <termios.h>
#include <unistd.h>
#include <sys/ioctl.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

#include <libplayercore/playercore.h>
#include <replace/replace.h>
extern PlayerTime* GlobalTime;

#define DEFAULT_LASER_PORT "/dev/ttyS1"
#define DEFAULT_LASER_CONNECT_RATE 9600
#define DEFAULT_LASER_TRANSFER_RATE 38400
#define DEFAULT_LASER_RETRIES 3


// The laser device class.
class SickLMS200 : public Driver
{
  public:
    
    // Constructor
    SickLMS200(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);

    // Change the resolution of the laser
    int SetLaserRes(int angle, int res);
    
    // 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;

    // Number of time to try connecting
    int retry_limit;
  
    // 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 connect_rate;  // Desired rate for first connection
    int transfer_rate; // Desired rate for operation
    int current_rate;  // Current rate

    int scan_id;

#ifdef HAVE_HI_SPEED_SERIAL
  struct serial_struct old_serial;
#endif
};

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

// a driver registration function
void SickLMS200_Register(DriverTable* table)
{
  table->AddDriver("sicklms200", SickLMS200_Init);
}

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

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


////////////////////////////////////////////////////////////////////////////////
// Error macros
#define RETURN_ERROR(erc, m) {PLAYER_ERROR(m); return erc;}
 
////////////////////////////////////////////////////////////////////////////////
// Constructor
SickLMS200::SickLMS200(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;

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

  // Serial rate
  this->connect_rate = cf->ReadInt(section, "connect_rate", DEFAULT_LASER_CONNECT_RATE);
  this->transfer_rate = cf->ReadInt(section, "transfer_rate", DEFAULT_LASER_TRANSFER_RATE);
  this->current_rate = 0;
  this->retry_limit = cf->ReadInt(section, "retry", 0) + 1;

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

  if (!this->can_do_hi_speed && this->connect_rate > 38400)
  {
    PLAYER_ERROR1("sicklms200: requested hi speed serial, but no support compiled in. Defaulting to %d bps.",
		    DEFAULT_LASER_CONNECT_RATE);
    this->connect_rate = DEFAULT_LASER_CONNECT_RATE;
  }

  if (!this->can_do_hi_speed && this->transfer_rate > 38400)
  {
    PLAYER_ERROR1("sicklms200: requested hi speed serial, but no support compiled in. Defaulting to %d bps.",
		    DEFAULT_LASER_TRANSFER_RATE);
    this->connect_rate = DEFAULT_LASER_TRANSFER_RATE;
  }

  // Set default configuration
  this->startup_delay = cf->ReadInt(section, "delay", 0);
  this->scan_width = 180;
  this->scan_res = cf->ReadInt(section, "resolution", 50);
  if((this->scan_res != 25) && 
     (this->scan_res != 50) && 
     (this->scan_res != 100))
  {
    PLAYER_ERROR1("Invalid angular resolution %d. Defaulting to 50 (0.5 degree)", 
                  this->scan_res);
    this->scan_res = 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 = cf->ReadInt(section, "range_res", 1);
  this->invert = cf->ReadInt(section, "invert", 0);

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

  
  return;
}

////////////////////////////////////////////////////////////////////////////////
// Set up the device
int SickLMS200::Setup()
{
  PLAYER_MSG1(2, "Laser initialising (%s)", 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);
  
  for(int i=0;i<this->retry_limit;i++)
  {
    // Try connecting at the given rate
    PLAYER_MSG1(2, "connecting at %d", this->connect_rate);
    if (ChangeTermSpeed(this->connect_rate))
      return 1;
    if (SetLaserMode() == 0)
      this->current_rate = this->connect_rate;
    else if (SetLaserMode() == 0)
      this->current_rate = this->connect_rate;

    if(this->current_rate != 0)
      break;
  }

  // Could not find the laser
  if (this->current_rate == 0)
  {
    PLAYER_ERROR("unable to connect to laser");
    return 1;
  }

  // Jump up to 38400 rate
  if (this->current_rate != this->transfer_rate && this->transfer_rate == 38400)
  {
    PLAYER_MSG2(2, "laser operating at %d; changing to %d", this->current_rate, this->transfer_rate);
    if (SetLaserSpeed(this->transfer_rate))
      return 1;
    sleep(1);
    if (ChangeTermSpeed(this->transfer_rate))
      return 1;
    sleep(1);
  }

  // Jump up to 500000
  else if (this->current_rate != 500000 && this->transfer_rate == 500000 && this->can_do_hi_speed)
  {
    PLAYER_MSG2(2, "laser operating at %d; changing to %d", this->current_rate, this->transfer_rate);
    if (SetLaserSpeed(this->transfer_rate))
      return 1;
    sleep(1);
    if (ChangeTermSpeed(this->transfer_rate))
      return 1;
    sleep(1);
  }

  // Dont know this rate
  else if (this->current_rate != this->transfer_rate)
  {
    PLAYER_ERROR1("unsupported transfer rate %d", this->transfer_rate);
    return 1;
  }

  // Display the laser type
  char type[64];
  memset(type,0,sizeof(type));
  if (GetLaserType(type, sizeof(type)))
    return 1;
  PLAYER_MSG3(2, "SICK laser type [%s] at [%s:%d]", type, this->device_name, this->transfer_rate);

  // Configure the laser
  if (SetLaserRes(this->scan_width, this->scan_res))
    return 1;
  if (SetLaserConfig(this->intensity))
    return 1;

  this->scan_id = 0;

  PLAYER_MSG0(2, "laser ready");

  // Start the device thread
  StartThread();

  return 0;
}


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

  // switch to connect rate just in case
  if (this->connect_rate != this->current_rate)
    if (SetLaserSpeed(this->connect_rate))
      PLAYER_WARN1("Cannot throttle back to %d bauds", this->connect_rate);
  
  CloseTerm();

  PLAYER_MSG0(2, "laser shutdown");
  
  return(0);
}


int 
SickLMS200::ProcessMessage(MessageQueue * resp_queue, 
                           player_msghdr * hdr,
                           void * data)
{
  if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, 
                           PLAYER_LASER_REQ_SET_CONFIG, 
                           this->device_addr))
  {
    if(hdr->size != sizeof(player_laser_config_t))
    {
      PLAYER_ERROR2("request is wrong length (%d != %d); ignoring",
                    hdr->size, sizeof(player_laser_config_t));
      return(-1);
    }

    player_laser_config_t * config = 
            reinterpret_cast<player_laser_config_t *> (data);
    this->intensity = config->intensity;
    this->scan_res = (int) rint(RTOD(config->resolution)*100);
    this->min_angle = (int)rint(RTOD(config->min_angle)*100);
    this->max_angle = (int)rint(RTOD(config->max_angle)*100);
    this->range_res = (int)config->range_res*1000;

    if(this->CheckScanConfig() != 0)
    {
      PLAYER_ERROR("invalid laser configuration requested");
      return(-1);
    }
    if (SetLaserMode() != 0)
      PLAYER_ERROR("request for config mode failed");
    else
    {
      if (SetLaserRes(this->scan_width, this->scan_res) != 0)
        PLAYER_ERROR("failed setting resolution");
      /* This call fails for me, but I've only tested with one laser - BPG
       * */
      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");

    // Configuration succeeded; send the new config back in the ACK
    this->Publish(this->device_addr,