sickpls.cc

机器人仿真软件

      else if (packet[0] == NACK)
        RETURN_ERROR(1, "request denied by laser")
          else if (packet[0] != ACK)
            RETURN_ERROR(1, "unexpected packet type");

  // PLAYER_TRACE0("baud rate request ok");
  printf("LASER: SLS: request OK\n");
  return 0;
}


////////////////////////////////////////////////////////////////////////////////
// Get the laser type
//
int SickPLS::GetLaserType(char *buffer, size_t bufflen)
{
  ssize_t len;
  uint8_t packet[512];

  packet[0] = 0x3A;
  len = 1;

  // PLAYER_TRACE0("sending get type request to laser");
  printf("LASER: GLT: sending get type request\n");
  if (WriteToLaser(packet, len) < 0)
    return 1;

  // Wait for laser to return data
  // This could take a while...
  //
  // PLAYER_TRACE0("waiting for reply");
  printf("LASER: GLT: waiting for ACK\n");
  len = ReadFromLaser(packet, sizeof(packet), false, -1);
  if (len < 0)
    return 1;
  else if (len < 1)
    RETURN_ERROR(1, "no reply from laser")
  else if (packet[0] == NACK)
    RETURN_ERROR(1, "request denied by laser")
  else if (packet[0] != 0xBA)
    RETURN_ERROR(1, "unexpected packet type");

  printf("LASER: GLT: reply OK\n");
  // NULL terminate the return string
  //
  assert((size_t) len + 1 < sizeof(packet));
  packet[len + 1] = 0;

  // Copy to buffer
  //
  assert(bufflen >= (size_t) len - 1);
  strcpy(buffer, (char*) (packet + 1));

  return 0;
}


////////////////////////////////////////////////////////////////////////////////
// Set the laser configuration
// Returns 0 on success
//
int SickPLS::SetLaserConfig(bool intensity)
{
  ssize_t len;
  uint8_t packet[512];

  packet[0] = 0x74;
  len = 1;

  // PLAYER_TRACE0("sending get configuration request to laser");
  printf("LASER: SLC: getting config info\n");
  if (WriteToLaser(packet, len) < 0)
    return 1;

  // Wait for laser to return data
  // This could take a while...
  //
  // PLAYER_TRACE0("waiting for reply");
  printf("LASER: SLC: waiting for reply\n");
  len = ReadFromLaser(packet, sizeof(packet), false, -1);
  if (len < 0)
    return 1;
  else if (len < 1)
    RETURN_ERROR(1, "no reply from laser")
  else if (packet[0] == NACK)
    RETURN_ERROR(1, "request denied by laser")
  else if (packet[0] != 0xF4)
    RETURN_ERROR(1, "unexpected packet type");

  // PLAYER_TRACE0("get configuration request ok");
  printf("LASER: SLC: got config OK units %d\n", (int)packet[7]);

  // PLAYER_TRACE1("laser units [%d]", (int) packet[7]);

  // Modify the configuration and send it back
  packet[0] = 0x77;

  // Return intensity in top 3 data bits
  packet[6] = (intensity ? 0x01 : 0x00); 

  // Set the units for the range reading
  if (this->range_res == 1)
    packet[7] = 0x01;
  else if (this->range_res == 10)
    packet[7] = 0x00;
  else if (this->range_res == 100)
    packet[7] = 0x02;
  else
    packet[7] = 0x01;

  // PLAYER_TRACE0("sending set configuration request to laser");
  printf("LASER: SLC: sending set config request range_res: %d\n", this->range_res);
  if (WriteToLaser(packet, len) < 0)
    return 1;

  // Wait for the change to "take"
  //
  // PLAYER_TRACE0("waiting for acknowledge");
  printf("LASER: SLC: waiting for ACK\n");
  len = ReadFromLaser(packet, sizeof(packet), false, -1);
  if (len < 0)
    return 1;
  else if (len < 1)
    RETURN_ERROR(1, "no reply from laser")
  else if (packet[0] == NACK)
    RETURN_ERROR(1, "request denied by laser")
  else if (packet[0] != 0xF7)
    RETURN_ERROR(1, "unexpected packet type");

  printf("LASER: SLC: set config request OK\n");
  // PLAYER_TRACE0("set configuration request ok");

  return 0;
}


////////////////////////////////////////////////////////////////////////////////
// Request data from the laser
// Returns 0 on success
//
int SickPLS::RequestLaserData(int min_segment, int max_segment)
{
  ssize_t len = 0;
  uint8_t packet[20];
  
  packet[len++] = 0x20; /* mode change command */
    
  if (min_segment == 0 && max_segment == 360)
  {
    // Use this for raw scan data...
    //
    packet[len++] = 0x24;
  }
  else
  {        
    // Or use this for selected scan data...
    //
    int first = min_segment + 1;
    int last = max_segment + 1;
    packet[len++] = 0x27;
    packet[len++] = (first & 0xFF);
    packet[len++] = (first >> 8);
    packet[len++] = (last & 0xFF);
    packet[len++] = (last >> 8);
  }

  // PLAYER_TRACE0("sending scan data request to laser");
  printf("LASER: RLD: writing scan data\n");
  if (WriteToLaser(packet, len) < 0)
    return 1;

  // Wait for laser to return ack
  // This should be fairly prompt
  //
  // PLAYER_TRACE0("waiting for acknowledge");
  printf("LASER: RLD: waiting for ACK\n");
  len = ReadFromLaser(packet, sizeof(packet), true, 1000);

  if (len < 0)
    return 1;
  else if (len < 1)
    RETURN_ERROR(1, "no reply from laser")
  else if (packet[0] == NACK)
    RETURN_ERROR(1, "request denied by laser")
  else if (packet[0] != ACK)
    RETURN_ERROR(1, "unexpected packet type");

  // PLAYER_TRACE0("scan data request ok");
  printf("LASER: RLD: scan data OK\n");
   
  return 0;
}


////////////////////////////////////////////////////////////////////////////////
// Read range data from laser
//
int SickPLS::ReadLaserData(uint16_t *data, size_t datalen)
{
  uint8_t raw_data[1024];

  // Read a packet from the laser
  //
  int len = ReadFromLaser(raw_data, sizeof(raw_data));
  if (len == 0)
  {
    // PLAYER_TRACE0("empty packet");
    return 1;
  }

  // Process raw packets
  //
  if (raw_data[0] == 0xB0)
  {
    // Determine the number of values returned
    //
    //int units = raw_data[2] >> 6;
    int count = (int) raw_data[1] | ((int) (raw_data[2] & 0x3F) << 8);
    assert((size_t) count <= datalen);

    // Strip the status info and shift everything down a few bytes
    // to remove packet header.
    //
    for (int i = 0; i < count; i++)
    {
      int src = 2 * i + 3;
      data[i] = raw_data[src + 0] | (raw_data[src + 1] << 8);
    }
  }
  else if (raw_data[0] == 0xB7)
  {
    // Determine which values were returned
    //
    //int first = ((int) raw_data[1] | ((int) raw_data[2] << 8)) - 1;
    //int last =  ((int) raw_data[3] | ((int) raw_data[4] << 8)) - 1;
        
    // Determine the number of values returned
    //
    //int units = raw_data[6] >> 6;
    int count = (int) raw_data[5] | ((int) (raw_data[6] & 0x3F) << 8);
    assert((size_t) count <= datalen);

    // Strip the status info and shift everything down a few bytes
    // to remove packet header.
    //
    for (int i = 0; i < count; i++)
    {
      int src = 2 * i + 7;
      data[i] = raw_data[src + 0] | (raw_data[src + 1] << 8);
    }
  }
  else
    RETURN_ERROR(1, "unexpected packet type");
  
  return 0;
}


////////////////////////////////////////////////////////////////////////////////
// Write a packet to the laser
//
ssize_t SickPLS::WriteToLaser(uint8_t *data, ssize_t len)
{
  uint8_t buffer[4 + 1024 + 2];
  assert(4 + len + 2 < (ssize_t) sizeof(buffer));

  // Create header
  //
  buffer[0] = STX;
  buffer[1] = 0;
  buffer[2] = LOBYTE(len);
  buffer[3] = HIBYTE(len);

  // Copy body
  //
  memcpy(buffer + 4, data, len);

  // Create footer (CRC)
  //
  uint16_t crc = CreateCRC(buffer, 4 + len);
  buffer[4 + len + 0] = LOBYTE(crc);
  buffer[4 + len + 1] = HIBYTE(crc);

  // Make sure both input and output queues are empty
  //
  tcflush(this->laser_fd, TCIOFLUSH);
    
  // Write the data to the port
  //
  ssize_t bytes = ::write( this->laser_fd, buffer, 4 + len + 2);

  // Make sure the queue is drained
  // Synchronous IO doesnt always work
  //
  ::tcdrain(this->laser_fd);
    
  // Return the actual number of bytes sent, including header and footer
  //
  return bytes;
}


////////////////////////////////////////////////////////////////////////////////
// Read a packet from the laser
// Set ack to true to ignore all packets except ack and nack
// Set timeout to -1 to make this blocking, otherwise it will return in timeout ms.
// Returns the packet length (0 if timeout occurs)
//
ssize_t SickPLS::ReadFromLaser(uint8_t *data, ssize_t maxlen, bool ack, int timeout)
{
  // If the timeout is infinite,
  // go to blocking io
  //
  if (timeout < 0)
  {
    // PLAYER_TRACE0("using blocking io");
    int flags = ::fcntl(this->laser_fd, F_GETFL);
    if (flags < 0)
    {
      PLAYER_ERROR("unable to get device flags");
      return 0;
    }
    if (::fcntl(this->laser_fd, F_SETFL, flags & (~O_NONBLOCK)) < 0)
    {
      PLAYER_ERROR("unable to set device flags");
      return 0;
    }
  }
  //
  // Otherwise, use non-blocking io
  //
  else
  {
    // PLAYER_TRACE0("using non-blocking io");
    int flags = ::fcntl(this->laser_fd, F_GETFL);
    if (flags < 0)
    {
      PLAYER_ERROR("unable to get device flags");
      return 0;
    }
    if (::fcntl(this->laser_fd, F_SETFL, flags | O_NONBLOCK) < 0)
    {
      PLAYER_ERROR("unable to set device flags");
      return 0;
    }
  }

  int64_t start_time = GetTime();
  int64_t stop_time = start_time + timeout;

  // PLAYER_TRACE2("%Ld %Ld", start_time, stop_time);

  int bytes = 0;
  uint8_t header[5] = {0};
  uint8_t footer[3];
    
  // Read until we get a valid header
  // or we timeout
  //
  while (true)
  {
    if (timeout >= 0)
      usleep(1000);
    bytes = ::read(this->laser_fd, header + sizeof(header) - 1, 1);
    if (header[0] == STX && header[1] == 0x80)
    {
      if (!ack)
        break;
      if (header[4] == ACK || header[4] == NACK)
        break;
    }
    memmove(header, header + 1, sizeof(header) - 1);
    if (timeout >= 0 && GetTime() >= stop_time)
    {
      // PLAYER_TRACE2("%Ld %Ld", GetTime(), stop_time);
      // PLAYER_TRACE0("timeout on read (1)");
      return 0;
    }
  }

  // Determine data length
  // Includes status, but not CRC, so subtract status to get data packet length.
  //
  ssize_t len = ((int) header[2] | ((int) header[3] << 8)) - 1;
    
  // Check for buffer overflows
  //
  if (len > maxlen)
    RETURN_ERROR(0, "buffer overflow (len > max_len)");

  // Read in the data
  // Note that we smooge the packet type from the header
  // onto the front of the data buffer.
  //
  bytes = 0;
  data[bytes++] = header[4];
  while (bytes < len)
  {
    if (timeout >= 0)
      usleep(1000);
    bytes += ::read(this->laser_fd, data + bytes, len - bytes);
    if (timeout >= 0 && GetTime() >= stop_time)
    {
      // PLAYER_TRACE2("%Ld %Ld", GetTime(), stop_time);
      RETURN_ERROR(0, "timeout on read (3)");
    }
  }

  // Read in footer
  //
  bytes = 0;
  while (bytes < 3)
  {
    if (timeout >= 0)
      usleep(1000);
    bytes += ::read(this->laser_fd, footer + bytes, 3 - bytes);
    if (timeout >= 0 && GetTime() >= stop_time)
    {
      // PLAYER_TRACE2("%Ld %Ld", GetTime(), stop_time);
      RETURN_ERROR(0, "timeout on read (4)");
    }
  }
    
  // Construct entire packet
  // And check the CRC
  //
  uint8_t buffer[4 + 1024 + 1];
  assert(4 + len + 1 < (ssize_t) sizeof(buffer));
  memcpy(buffer, header, 4);
  memcpy(buffer + 4, data, len);
  memcpy(buffer + 4 + len, footer, 1);
  uint16_t crc = CreateCRC(buffer, 4 + len + 1);
  if (crc != MAKEUINT16(footer[1], footer[2]))
    RETURN_ERROR(0, "CRC error, ignoring packet");
    
  return len;
}

           
////////////////////////////////////////////////////////////////////////////////
// Create a CRC for the given packet
//
unsigned short SickPLS::CreateCRC(uint8_t* data, ssize_t len)
{
  uint16_t uCrc16;
  uint8_t abData[2];
  
  uCrc16 = 0;
  abData[0] = 0;
  
  while(len-- )
  {
    abData[1] = abData[0];
    abData[0] = *data++;
    
    if( uCrc16 & 0x8000 )
    {
      uCrc16 = (uCrc16 & 0x7fff) << 1;
      uCrc16 ^= CRC16_GEN_POL;
    }
    else
    {    
      uCrc16 <<= 1;
    }
    uCrc16 ^= MAKEUINT16(abData[0],abData[1]);
  }
  return (uCrc16); 
}


////////////////////////////////////////////////////////////////////////////////
// Get the time (in ms)
//
int64_t SickPLS::GetTime()
{
  struct timeval tv;
  //gettimeofday(&tv, NULL);
  GlobalTime->GetTime(&tv);
  return (int64_t) tv.tv_sec * 1000 + (int64_t) tv.tv_usec / 1000;
}


/* demo of how to make a shared object for Player to load at runtime */
#if 0
#include <drivertable.h>
extern DriverTable* driverTable;

/* need the extern to avoid C++ name-mangling  */
extern "C" {
  void _init(void)
  {
    puts("Laser device initializing");
    SickPLS_Register(driverTable);
    puts("Laser device done");
  }

  void _fini(void)
  {
    puts("Laser device closing");

    /* probably don't need any code here; the destructor for the device
     * will be called when Player shuts down.  this function is only useful
     * if you want to dlclose() the shared object before Player exits
     */

    puts("Laser device closed");
  }
}
#endif