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

///////////////////////////////////////////////////////////////////////////
//
// 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/sicklms200.cc,v $
//  $Author: gerkey $
//  $Revision: 1.2.2.1 $
//
// Usage:
//  (empty)
//
// Theory of operation:
//  (empty)
//
// Known bugs:
//  (empty)
//
// Possible enhancements:
//  (empty)
//
///////////////////////////////////////////////////////////////////////////

#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 <playertime.h>
extern PlayerTime* GlobalTime;

#define PLAYER_ENABLE_MSG 0
#define PLAYER_ENABLE_TRACE 0

#include "playercommon.h"
#include "drivertable.h"
#include "player.h"

#define DEFAULT_LASER_PORT "/dev/ttyS1"

// The laser device class.
class SickLMS200 : public CDevice
{
  public:
    
    // Constructor
    SickLMS200(char* interface, ConfigFile* cf, int section);

    int Setup();
    int Shutdown();

  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;           

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

    // Turn intensity data on/off
    bool intensity;
};

// a factory creation function
CDevice* SickLMS200_Init(char* interface, ConfigFile* cf, int section)
{
  if(strcmp(interface, PLAYER_LASER_STRING))
  {
    PLAYER_ERROR1("driver \"sicklms200\" does not support interface \"%s\"\n",
                  interface);
    return(NULL);
  }
  else
    return((CDevice*)(new SickLMS200(interface, cf, section)));
}

// a driver registration function
void SickLMS200_Register(DriverTable* table)
{
  table->AddDriver("sicklms200", PLAYER_READ_MODE, SickLMS200_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
SickLMS200::SickLMS200(char* interface, ConfigFile* cf, int section)
    : CDevice(sizeof(player_laser_data_t),0,10,10)
{
  // 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->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;

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


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

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

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

  // Display the laser type
  char type[64];
  if (GetLaserType(type, sizeof(type)))
    return 1;
  PLAYER_MSG1("SICK laser type [%s]", (char*) type);

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

  puts("laser ready");

  // Start the device thread
  StartThread();

  return 0;
}


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

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

  return(0);
}


////////////////////////////////////////////////////////////////////////////////
// Main function for device thread
void SickLMS200::Main() 
{
  // 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
      {
        if (SetLaserRes(this->scan_width, this->scan_res) != 0)
          PLAYER_ERROR("failed setting resolution");
        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;
    if (ReadLaserData(data.ranges, sizeof(data.ranges) / sizeof(data.ranges[0])) == 0)
    {
      // Prepare packet and byte swap
      data.min_angle = htons(this->scan_min_segment * this->scan_res - this->scan_width * 50);
      data.max_angle = htons(this->scan_max_segment * this->scan_res - this->scan_width * 50);
      data.resolution = htons(this->scan_res);
      data.range_count = htons(this->scan_max_segment - this->scan_min_segment + 1);
      for (int i = 0; i < this->scan_max_segment - this->scan_min_segment + 1; i++)
      {
        data.intensity[i] = ((data.ranges[i] >> 13) & 0x000E);
        data.ranges[i] = htons((data.ranges[i] & 0x1FFF));
      }

      // Make data available
      PutData((uint8_t*) &data, sizeof(data), time.tv_sec, time.tv_usec);
    }
  }
}


////////////////////////////////////////////////////////////////////////////////
// Process configuration requests.  Returns 1 if the configuration has changed.
int SickLMS200::UpdateConfig()
{
  int len;
  void *client;
  char buffer[PLAYER_MAX_REQREP_SIZE];
  player_laser_config_t config;
  player_laser_geom_t geom;
  
  while ((len = GetConfig(&client, &buffer, sizeof(buffer))) > 0)
  {
    switch (buffer[0])
    {
      case PLAYER_LASER_SET_CONFIG:
      {
        if (len != sizeof(player_laser_config_t))
        {
          PLAYER_ERROR2("config request len is invalid (%d != %d)", len, sizeof(config));
          if(PutReply(client, PLAYER_MSGTYPE_RESP_NACK) != 0)
            PLAYER_ERROR("PutReply() failed");
          continue;
        }

        memcpy(&config, buffer, sizeof(config));
        this->intensity = config.intensity;
        this->scan_res = ntohs(config.resolution);
        this->min_angle = (short) ntohs(config.min_angle);
        this->max_angle = (short) ntohs(config.max_angle);