sickpls.cc

机器人仿真软件

      data.ranges_count = (this->scan_max_segment - this->scan_min_segment + 1);
//      data.range_res = (this->range_res);
      for (int i = 0; i < this->scan_max_segment - this->scan_min_segment + 1; i++)
      {
        data.intensity[i] = ((TempData[i] >> 13) & 0x000E);
        data.ranges[i] = ((TempData[i] & 0x1FFF));
      }

      // if the laser is upside-down, reverse the data and intensity
      // arrays, in place.  this could probably be made more efficient by
      // burying it in a lower-level loop where the data is being read, but
      // i can't be bothered to figure out where.
      if(this->invert)
      {
        for (int i = 0; 
             i < (this->scan_max_segment - this->scan_min_segment + 1)/2; 
             i++)
        {
          tmp=data.ranges[i];
          data.ranges[i]=data.ranges[this->scan_max_segment-this->scan_min_segment-i];
          data.ranges[this->scan_max_segment-this->scan_min_segment-i] = tmp;
          uint8_t tmp_intensity=data.intensity[i];
          data.intensity[i]=data.intensity[this->scan_max_segment-this->scan_min_segment-i];
          data.intensity[this->scan_max_segment-this->scan_min_segment-i] = tmp_intensity;
        }
      }

      // Make data available
      this->Publish(this->device_addr, NULL, 
                    PLAYER_MSGTYPE_DATA, PLAYER_LASER_DATA_SCAN,
                    (void*)&data, sizeof(data), NULL);
    }
    delete TempData;
  }
}


////////////////////////////////////////////////////////////////////////////////
// Process an incoming message
int 
SickPLS::ProcessMessage(MessageQueue * resp_queue, 
                           player_msghdr * hdr,
                           void * data)
{
  assert(hdr);
  assert(data);
  
  if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, 
                           PLAYER_LASER_REQ_SET_CONFIG, 
                           this->device_addr))
  {
  	assert(hdr->size == sizeof(player_laser_config_t));
    player_laser_config_t * l_cfg = reinterpret_cast<player_laser_config_t *> (data);
    Lock();
    this->intensity = l_cfg->intensity;
    this->scan_res = (int)rint(RTOD(l_cfg->resolution)*100);
    this->min_angle = (int)rint(RTOD(l_cfg->min_angle)*100);
    this->max_angle = (int)rint(RTOD(l_cfg->max_angle)*100);
    this->range_res = (int)l_cfg->range_res*1000;
    Unlock();
    if (this->CheckScanConfig() == 0)
    {
      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");
      return PLAYER_MSGTYPE_RESP_ACK;
    }
    else
      return PLAYER_MSGTYPE_RESP_NACK;
  }

  if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, 
                           PLAYER_LASER_REQ_GET_CONFIG, 
                           this->device_addr))
  {
  	assert(hdr->size == 0);
    player_laser_config_t lcfg;
    
    lcfg.intensity = this->intensity;
    lcfg.resolution = (this->scan_res);
    lcfg.min_angle = ((short) this->min_angle);
    lcfg.max_angle = ((short) this->max_angle);
    lcfg.range_res = (this->range_res);
    this->Publish(this->device_addr,
                  resp_queue,
                  PLAYER_MSGTYPE_RESP_ACK,
                  PLAYER_LASER_REQ_GET_CONFIG,
                  (void*)&lcfg, sizeof(lcfg), NULL);
    return 0;
  }

  if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, 
                           PLAYER_LASER_REQ_GET_GEOM, 
                           this->device_addr))
  {
    player_laser_geom_t geom;
    geom.pose.px = this->pose[0];
    geom.pose.py = this->pose[1];
    geom.pose.pa = this->pose[2];
    geom.size.sl = this->size[0];
    geom.size.sw = this->size[1];

    this->Publish(this->device_addr,
                  resp_queue,
                  PLAYER_MSGTYPE_RESP_ACK,
                  PLAYER_LASER_REQ_GET_GEOM,
                  (void*)&geom, sizeof(geom), NULL);
    return(0);
  }
  return -1;
}


////////////////////////////////////////////////////////////////////////////////
// Process configuration requests.  Returns 1 if the configuration has changed.
/*int SickPLS::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), NULL)) > 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,NULL) != 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);
	this->range_res = ntohs(config.range_res);

        if (this->CheckScanConfig() == 0)
        {
          if(PutReply(client, PLAYER_MSGTYPE_RESP_ACK, 
                      &config, sizeof(config),NULL) != 0)
            PLAYER_ERROR("PutReply() failed");
          return 1;
        }
        else
        {
          if(PutReply(client, PLAYER_MSGTYPE_RESP_NACK,NULL) != 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,NULL) != 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);
        config.range_res = htons(this->range_res);

        if(PutReply(client, PLAYER_MSGTYPE_RESP_ACK, 
                    &config, sizeof(config), NULL) != 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,NULL) != 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, 
                    &geom, sizeof(geom),NULL) != 0)
          PLAYER_ERROR("PutReply() failed");
        break;
      }

      default:
      {
        if(PutReply(client, PLAYER_MSGTYPE_RESP_NACK,NULL) != 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 SickPLS::CheckScanConfig()
{
  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 0;
}


////////////////////////////////////////////////////////////////////////////////
// Open the terminal
// Returns 0 on success
int SickPLS::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");
  
  cfmakeraw( &term );

  // Set to even parity
  term.c_iflag |= INPCK; 
  term.c_iflag &= ~IXOFF;
  term.c_cflag |= PARENB;

     
  cfsetispeed( &term, B9600 );
  cfsetospeed( &term, B9600 );
     
  tcflush( this->laser_fd, TCIFLUSH);    
  if( tcsetattr( this->laser_fd, TCSANOW, &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 SickPLS::CloseTerm()
{
#ifdef HAVE_HI_SPEED_SERIAL
  if (ioctl(this->laser_fd, TIOCSSERIAL, &this->old_serial) < 0) {
    RETURN_ERROR(1, "error trying to reset serial to old state");
  }
#endif

  ::close(this->laser_fd);
  return 0;
}


////////////////////////////////////////////////////////////////////////////////
// Set the terminal speed
// Valid values are 9600 and 38400
// Returns 0 on success
//
int SickPLS::ChangeTermSpeed(int speed)
{
  struct termios term;

#ifdef HAVE_HI_SPEED_SERIAL
  struct serial_struct serial;

  // we should check and reset the AYSNC_SPD_CUST flag
  // since if it's set and we request 38400, we're likely
  // to get another baud rate instead (based on custom_divisor)
  // this way even if the previous player doesn't reset the
  // port correctly, we'll end up with the right speed we want
  if (ioctl(this->laser_fd, TIOCGSERIAL, &serial) < 0) {
    RETURN_ERROR(1, "error on TIOCGSERIAL in beginning");
  }

  serial.flags &= ~ASYNC_SPD_CUST;
  serial.custom_divisor = 0;
  if (ioctl(this->laser_fd, TIOCSSERIAL, &serial) < 0) {
    RETURN_ERROR(1, "error on TIOCSSERIAL in beginning");
  }
#endif  

  printf("LASER: change TERM speed: %d\n", speed);

  switch(speed) {
  case 9600:
    PLAYER_MSG0(2, "terminal speed to 9600");
    if( tcgetattr( this->laser_fd, &term ) < 0 )
      RETURN_ERROR(1, "unable to get device attributes");
        
    cfmakeraw( &term );

    term.c_iflag |= INPCK; 
    term.c_iflag &= ~IXOFF;
    term.c_cflag |= PARENB;

    cfsetispeed( &term, B9600 );
    cfsetospeed( &term, B9600 );

    tcflush( this->laser_fd, TCIFLUSH);    
    if( tcsetattr( this->laser_fd, TCSANOW, &term ) < 0 )
      RETURN_ERROR(1, "unable to set device attributes");
    break;

  case 38400:
    PLAYER_MSG0(2, "terminal speed to 38400");
    if( tcgetattr( this->laser_fd, &term ) < 0 )
      RETURN_ERROR(1, "unable to get device attributes");
        
    cfmakeraw( &term );
    term.c_iflag |= INPCK; 
    term.c_iflag &= ~IXOFF;
    term.c_cflag |= PARENB;

    cfsetispeed( &term, B38400 );
    cfsetospeed( &term, B38400 );

    tcflush( this->laser_fd, TCIFLUSH);    
    if( tcsetattr( this->laser_fd, TCSANOW, &term ) < 0 )        
      // if( tcsetattr( this->laser_fd, TCSAFLUSH, &term ) < 0 )
      RETURN_ERROR(1, "unable to set device attributes");
    break;

  case 500000:
    PLAYER_MSG0(2, "terminal speed to 500000");

#ifdef HAVE_HI_SPEED_SERIAL    
    if (ioctl(this->laser_fd, TIOCGSERIAL, &this->old_serial) < 0) {
      RETURN_ERROR(1, "error on TIOCGSERIAL ioctl");
    }
    
    serial = this->old_serial;
    
    serial.flags |= ASYNC_SPD_CUST;
    serial.custom_divisor = 48; // for FTDI USB/serial converter divisor is 240/5
    
    if (ioctl(this->laser_fd, TIOCSSERIAL, &serial) < 0) {
      RETURN_ERROR(1, "error on TIOCSSERIAL ioctl");
    }
    
#else
    fprintf(stderr, "sicklms200: Trying to change to 500kbps, but no support compiled in, defaulting to 38.4kbps.\n");
#endif

    // even if we are doing 500kbps, we have to set the speed to 38400...
    // the driver will know we want 500000 instead.

    if( tcgetattr( this->laser_fd, &term ) < 0 )
      RETURN_ERROR(1, "unable to get device attributes");    

    cfmakeraw( &term );
    
    term.c_iflag |= INPCK; 
    term.c_iflag &= ~IXOFF;
    term.c_cflag |= PARENB;

    cfsetispeed( &term, B38400 );
    cfsetospeed( &term, B38400 );
    
    if( tcsetattr( this->laser_fd, TCSAFLUSH, &term ) < 0 )
      RETURN_ERROR(1, "unable to set device attributes");
    
    break;
  default:
    fprintf(stderr, "sicklms200: unknown speed %d\n", speed);
  }

    
  return 0;
}


////////////////////////////////////////////////////////////////////////////////
// Put the laser into configuration mode
//
int SickPLS::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] = 'P'; // P
  packet[8] = 'L'; // L
  packet[9] = 'S'; // 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, -1);
  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 SickPLS::SetLaserSpeed(int speed)
{
  ssize_t len;
  uint8_t packet[20];

  if (SetLaserMode() != 0)
    RETURN_ERROR(1,"request for config mode failed");

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

  printf("LASER: SLS: sending bps rate request\n");
  // 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");
  printf("LASER: SLS: waiting for ACK\n");
  len = ReadFromLaser(packet, sizeof(packet), true, 2000);
  if (len < 0)
    return 1;
  else if (len < 1)
    RETURN_ERROR(1, "no reply from laser")