sicklms200.cc

机器人仿真平台,和stage配合运行

        if (this->CheckScanConfig() == 0)
        {
          if(PutReply(client, PLAYER_MSGTYPE_RESP_ACK, NULL, &config, sizeof(config)) != 0)
            PLAYER_ERROR("PutReply() failed");
          return 1;
        }
        else
        {
          if(PutReply(client, PLAYER_MSGTYPE_RESP_NACK) != 0)
            PLAYER_ERROR("PutReply() failed");
        }
        break;
      }

      case PLAYER_LASER_GET_CONFIG:
      {
        if (len != 1)
        {
          PLAYER_ERROR2("config request len is invalid (%d != %d)", len, 1);
          if(PutReply(client, PLAYER_MSGTYPE_RESP_NACK) != 0)
            PLAYER_ERROR("PutReply() failed");
          continue;
        }

        config.intensity = this->intensity;
        config.resolution = htons(this->scan_res);
        config.min_angle = htons((short) this->min_angle);
        config.max_angle = htons((short) this->max_angle);
        
        if(PutReply(client, PLAYER_MSGTYPE_RESP_ACK, NULL, &config, 
                    sizeof(config)) != 0)
          PLAYER_ERROR("PutReply() failed");
        break;
      }

      case PLAYER_LASER_GET_GEOM:
      {
        if (len != 1)
        {
          PLAYER_ERROR2("config request len is invalid (%d != %d)", len, 1);
          if(PutReply(client, PLAYER_MSGTYPE_RESP_NACK) != 0)
            PLAYER_ERROR("PutReply() failed");
          continue;
        }

        geom.pose[0] = htons((short) (this->pose[0] * 1000));
        geom.pose[1] = htons((short) (this->pose[1] * 1000));
        geom.pose[2] = htons((short) (this->pose[2] * 180/M_PI));
        geom.size[0] = htons((short) (this->size[0] * 1000));
        geom.size[1] = htons((short) (this->size[1] * 1000));
        
        if(PutReply(client, PLAYER_MSGTYPE_RESP_ACK, NULL, &geom, 
                    sizeof(geom)) != 0)
          PLAYER_ERROR("PutReply() failed");
        break;
      }

      default:
      {
        if(PutReply(client, PLAYER_MSGTYPE_RESP_NACK) != 0)
          PLAYER_ERROR("PutReply() failed");
        break;
      }
    }
  }
  return 0;
}


////////////////////////////////////////////////////////////////////////////////
// Compute the start and end scan segments based on the current resolution and
// scan angles.  Returns 0 if the configuration is valid.
int SickLMS200::CheckScanConfig()
{
  if (this->scan_res == 25)
  {
    // For high res, drop the scan range down to 100 degrees.
    // The angles must be interpreted differently too.
    this->scan_width = 100;
    this->scan_min_segment = (this->min_angle + 5000) / this->scan_res;
    this->scan_max_segment = (this->max_angle + 5000) / this->scan_res;

    if (this->scan_min_segment < 0)
      this->scan_min_segment = 0;
    if (this->scan_min_segment > 400)
      this->scan_min_segment = 400;
    
    if (this->scan_max_segment < 0)
      this->scan_max_segment = 0;
    if (this->scan_max_segment > 400)
      this->scan_max_segment = 400;

    return 0;
  }
  else if (this->scan_res == 50 || this->scan_res == 100)
  {
    this->scan_width = 180;
    this->scan_min_segment = (this->min_angle + 9000) / this->scan_res;
    this->scan_max_segment = (this->max_angle + 9000) / this->scan_res;
    
    if (this->scan_min_segment < 0)
      this->scan_min_segment = 0;
    if (this->scan_min_segment > 360)
      this->scan_min_segment = 360;

    if (this->scan_max_segment < 0)
      this->scan_max_segment = 0;
    if (this->scan_max_segment > 360)
      this->scan_max_segment = 360;

    return 0;
  }
  return -1;
}


////////////////////////////////////////////////////////////////////////////////
// Open the terminal
// Returns 0 on success
int SickLMS200::OpenTerm()
{
  this->laser_fd = ::open(this->device_name, O_RDWR | O_SYNC , S_IRUSR | S_IWUSR );
  if (this->laser_fd < 0)
  {
    PLAYER_ERROR2("unable to open serial port [%s]; [%s]",
                  (char*) this->device_name, strerror(errno));
    return 1;
  }

  // set the serial port speed to 9600 to match the laser
  // later we can ramp the speed up to the SICK's 38K
  //
  struct termios term;
  if( tcgetattr( this->laser_fd, &term ) < 0 )
    RETURN_ERROR(1, "Unable to get serial port attributes");
  
#if HAVE_CFMAKERAW
  cfmakeraw( &term );
#endif
  cfsetispeed( &term, B9600 );
  cfsetospeed( &term, B9600 );
  
  if( tcsetattr( this->laser_fd, TCSAFLUSH, &term ) < 0 )
    RETURN_ERROR(1, "Unable to set serial port attributes");

  // Make sure queue is empty
  //
  tcflush(this->laser_fd, TCIOFLUSH);
    
  return 0;
}


////////////////////////////////////////////////////////////////////////////////
// Close the terminal
// Returns 0 on success
//
int SickLMS200::CloseTerm()
{
  ::close(this->laser_fd);
  return 0;
}


////////////////////////////////////////////////////////////////////////////////
// Set the terminal speed
// Valid values are 9600 and 38400
// Returns 0 on success
//
int SickLMS200::ChangeTermSpeed(int speed)
{
  struct termios term;
    
  if (speed == 9600)
  {
    PLAYER_MSG0("terminal speed to 9600");
    if( tcgetattr( this->laser_fd, &term ) < 0 )
      RETURN_ERROR(1, "unable to get device attributes");
        
#if HAVE_CFMAKERAW
    cfmakeraw( &term );
#endif
    cfsetispeed( &term, B9600 );
    cfsetospeed( &term, B9600 );
        
    if( tcsetattr( this->laser_fd, TCSAFLUSH, &term ) < 0 )
      RETURN_ERROR(1, "unable to set device attributes");
  }
  else if (speed == 38400)
  {
    PLAYER_MSG0("terminal speed to 38400");
    if( tcgetattr( this->laser_fd, &term ) < 0 )
      RETURN_ERROR(1, "unable to get device attributes");
        
#if HAVE_CFMAKERAW
    cfmakeraw( &term );
#endif
    cfsetispeed( &term, B38400 );
    cfsetospeed( &term, B38400 );
        
    if( tcsetattr( this->laser_fd, TCSAFLUSH, &term ) < 0 )
      RETURN_ERROR(1, "unable to set device attributes");
  }
    
  return 0;
}


////////////////////////////////////////////////////////////////////////////////
// Put the laser into configuration mode
//
int SickLMS200::SetLaserMode()
{
  ssize_t len;
  uint8_t packet[20];

  packet[0] = 0x20; /* mode change command */
  packet[1] = 0x00; /* configuration mode */
  packet[2] = 0x53; // S - the password 
  packet[3] = 0x49; // I
  packet[4] = 0x43; // C
  packet[5] = 0x4B; // K
  packet[6] = 0x5F; // _
  packet[7] = 0x4C; // L
  packet[8] = 0x4D; // M
  packet[9] = 0x53; // S
  len = 10;
  
  PLAYER_TRACE0("sending configuration mode request to laser");
  if (WriteToLaser(packet, len) < 0)
    return 1;

  // Wait for laser to return ack
  // This could take a while...
  //
  PLAYER_TRACE0("waiting for acknowledge");
  len = ReadFromLaser(packet, sizeof(packet), true, 1000);
  if (len < 0)
    RETURN_ERROR(1, "error reading from laser")
      else if (len < 1)
      {
        PLAYER_TRACE0("no reply from laser");
        return 1;
      }
  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("configuration mode request ok");

  return 0;
}


////////////////////////////////////////////////////////////////////////////////
// Set the laser data rate
// Valid values are 9600 and 38400
// Returns 0 on success
//
int SickLMS200::SetLaserSpeed(int speed)
{
  ssize_t len;
  uint8_t packet[20];

  packet[0] = 0x20;
  packet[1] = (speed == 9600 ? 0x42 : 0x40);
  len = 2;

  PLAYER_TRACE0("sending baud rate request to laser");
  if (WriteToLaser(packet, len) < 0)
    return 1;
            
  // Wait for laser to return ack
  // This could take a while...
  //
  PLAYER_TRACE0("waiting for acknowledge");
  len = ReadFromLaser(packet, sizeof(packet), true, 2000);
  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("baud rate request ok");
            
  return 0;
}


////////////////////////////////////////////////////////////////////////////////
// Get the laser type
//
int SickLMS200::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");
  if (WriteToLaser(packet, len) < 0)
    return 1;

  // Wait for laser to return data
  // This could take a while...
  //
  PLAYER_TRACE0("waiting for reply");
  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");

  // 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 SickLMS200::SetLaserConfig(bool intensity)
{
  ssize_t len;
  uint8_t packet[512];

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

  PLAYER_TRACE0("sending get configuration request to laser");
  if (WriteToLaser(packet, len) < 0)
    return 1;

  // Wait for laser to return data
  // This could take a while...
  //
  PLAYER_TRACE0("waiting for reply");
  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");

  // Modify the configuration and send it back
  //
  packet[0] = 0x77;
  packet[6] = (intensity ? 0x01 : 0x00); // Return intensity in top 3 data bits

  PLAYER_TRACE0("sending set configuration request to laser");
  if (WriteToLaser(packet, len) < 0)
    return 1;

  // Wait for the change to "take"
  //
  PLAYER_TRACE0("waiting for acknowledge");
  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");

  PLAYER_TRACE0("set configuration request ok");

  return 0;
}


////////////////////////////////////////////////////////////////////////////////
// Change the resolution of the laser
// Valid widths are: 100, 180 (degrees)
// Valid resolitions are: 25, 50, 100 (1/100 degree)
//
int SickLMS200::SetLaserRes(int width, int res)
{
  ssize_t len;
  uint8_t packet[512];