C++ portOpen函数代码示例

void
hokuyo::Laser::queryVersionInformation()
{
  if (!portOpen())
    HOKUYO_EXCEPT(hokuyo::Exception, "Port not open.");

  if (sendCmd("VV",1000) != 0)
    HOKUYO_EXCEPT(hokuyo::Exception, "Error requesting version information");
  
  char buf[100];
  vendor_name_ = laserReadlineAfter(buf, 100, "VEND:");
  vendor_name_ = vendor_name_.substr(0,vendor_name_.length() - 3);
  
  product_name_ = laserReadlineAfter(buf, 100, "PROD:");
  product_name_ = product_name_.substr(0,product_name_.length() - 3);
  
  firmware_version_ = laserReadlineAfter(buf, 100, "FIRM:");
  firmware_version_ = firmware_version_.substr(0,firmware_version_.length() - 3);

  protocol_version_ = laserReadlineAfter(buf, 100, "PROT:");
  protocol_version_ = protocol_version_.substr(0,protocol_version_.length() - 3);
  
  // This crazy naming scheme is for backward compatibility. Initially
  // the serial number always started with an H. Then it got changed to a
  // zero. For a while the driver was removing the leading zero in the
  // serial number. This is fine as long as it is indeed a zero in front.
  // The current behavior is backward compatible but will accomodate full
  // length serial numbers.
  serial_number_ = laserReadlineAfter(buf, 100, "SERI:");
  serial_number_ = serial_number_.substr(0,serial_number_.length() - 3);
  if (serial_number_[0] == '0')
    serial_number_[0] = 'H';
  else if (serial_number_[0] != 'H')
    serial_number_ = 'H' + serial_number_;
}

int
hokuyo::Laser::requestScans(bool intensity, double min_ang, double max_ang, int cluster, int skip, int count, int timeout)
{
  if (!portOpen())
    HOKUYO_EXCEPT(hokuyo::Exception, "Port not open.");

  int status;

  if (cluster == 0)
    cluster = 1;
  
  int min_i = (int)(afrt_ + min_ang*ares_/(2.0*M_PI));
  int max_i = (int)(afrt_ + max_ang*ares_/(2.0*M_PI));
  
  char cmdbuf[MAX_CMD_LEN];
  
  char intensity_char = 'D';
  if (intensity)
    intensity_char = 'E';
  
  sprintf(cmdbuf,"M%c%.4d%.4d%.2d%.1d%.2d", intensity_char, min_i, max_i, cluster, skip, count);
  
  status = sendCmd(cmdbuf, timeout);
  
  return status;
}

long long
hokuyo::Laser::calcLatency(bool intensity, double min_ang, double max_ang, int clustering, int skip, int num, int timeout)
{
    offset_ = 0;
    if (!portOpen())
        HOKUYO_EXCEPT(hokuyo::Exception, "Port not open.");

    if (num <= 0)
        num = 10;

    int ckreps = 1;
    int scanreps = 1;
    long long int start = getHokuyoClockOffset(ckreps, timeout);
    long long int pre = 0;
    std::vector<long long int> samples(num);
    for (int i = 0; i < num; i++)
    {
        long long int scan = getHokuyoScanStampToSystemStampOffset(intensity, min_ang, max_ang, clustering, skip, scanreps, timeout) - start;
        long long int post = getHokuyoClockOffset(ckreps, timeout) - start;
        samples[i] = scan - (post+pre)/2;
        //printf("%lli %lli %lli %lli %lli\n", samples[i], post, pre, scan, pre - post);
        //fflush(stdout);
        pre = post;
    }

    offset_ = median(samples);
    //printf("%lli\n", median(samples));
    return offset_;
}

void
hokuyo::Laser::querySensorConfig()
{
  if (!portOpen())
    HOKUYO_EXCEPT(hokuyo::Exception, "Port not open.");

  if (sendCmd("PP",1000) != 0)
    HOKUYO_EXCEPT(hokuyo::Exception, "Error requesting configuration information");

  char buf[100];
  char* ind;
    
  ind = laserReadlineAfter(buf,100,"DMIN:",-1);
  sscanf(ind, "%d", &dmin_);
    
  ind = laserReadlineAfter(buf,100,"DMAX:",-1);
  sscanf(ind, "%d", &dmax_);
    
  ind = laserReadlineAfter(buf,100,"ARES:",-1);
  sscanf(ind, "%d", &ares_);
    
  ind = laserReadlineAfter(buf,100,"AMIN:",-1);
  sscanf(ind, "%d", &amin_);
    
  ind = laserReadlineAfter(buf,100,"AMAX:",-1);
  sscanf(ind, "%d", &amax_);
    
  ind = laserReadlineAfter(buf,100,"AFRT:",-1);
  sscanf(ind, "%d", &afrt_);
    
  ind = laserReadlineAfter(buf,100,"SCAN:",-1);
  sscanf(ind, "%d", &rate_);
    
  return;
}

int
hokuyo::Laser::sendCmd(const char* cmd, int timeout)
{
if (!portOpen())
    HOKUYO_EXCEPT(hokuyo::Exception, "Port not open.");

  char buf[100]; 
  //printf("sendreq: %s\n", cmd);
  laserWrite(cmd);
  laserWrite("\n");;
  laserReadlineAfter(buf, 100, cmd, timeout);
  laserReadline(buf,100,timeout);
  //printf("chksum: %s",buf);
  if (!checkSum(buf,4))
  {
    //printf("chksum error\n");
    HOKUYO_EXCEPT(hokuyo::CorruptedDataException, "Checksum failed on status code.");
  }
  buf[2] = 0;
  //printf("sendreq_end: %s\n", cmd);

  if (buf[0] - '0' >= 0 && buf[0] - '0' <= 9 && buf[1] - '0' >= 0 && buf[1] - '0' <= 9)
    return (buf[0] - '0')*10 + (buf[1] - '0');
  else
    HOKUYO_EXCEPT(hokuyo::Exception, "Hokuyo error code returned. Cmd: %s --  Error: %s", cmd, buf);
}

void
hokuyo::Laser::close ()
{
  int retval = 0;

  if (portOpen()) {
    //Try to be a good citizen and completely shut down the laser before we shutdown communication
    try
    {
      reset();
    }
    catch (hokuyo::Exception& e) {
      //Exceptions here can be safely ignored since we are closing the port anyways
    }
#if HOKUYO_INTERFACE_ETHERNET
    tcpclient_close(&tcpclient);
#else
    retval = ::close(laser_fd_); // Automatically releases the lock.
#endif

  }

  laser_fd_ = -1;

  if (retval != 0)
    HOKUYO_EXCEPT(hokuyo::Exception, "Failed to close port properly -- error = %d: %s\n", errno, strerror(errno));
}

bool
hokuyo::Laser::isIntensitySupported()
{
  hokuyo::LaserScan  scan;
  
  if (!portOpen())
    HOKUYO_EXCEPT(hokuyo::Exception, "Port not open.");

  // Try an intensity command.
  try
  {
    requestScans(1, 0, 0, 0, 0, 1);
    serviceScan(scan, 1000);
    return true;
  }
  catch (hokuyo::Exception &e)
  {} 
  
  // Try an intensity command.
  try
  {
    requestScans(0, 0, 0, 0, 0, 1);
    serviceScan(scan, 1000);
    return false;
  }
  catch (hokuyo::Exception &e)
  {
    HOKUYO_EXCEPT(hokuyo::Exception, "Exception whil trying to determine if intensity scans are supported.")
  } 
}

std::string 
hokuyo::Laser::getID()
{
  if (!portOpen())
    HOKUYO_EXCEPT(hokuyo::Exception, "Port not open.");

  return serial_number_;
}

bool SerialPortControl::openPort (void)
{
    this->portInit();
    bool result = 0;
    result = serialPort->open(open_mode);
    if (result)
        emit portOpen();
    return(result);
}

void
hokuyo::Laser::open(const char * tcphost, const int tcpport)
{
  if (portOpen())
    close();

  // Make IO non blocking. This way there are no race conditions that
  // cause blocking when a badly behaving process does a read at the same
  // time as us. Will need to switch to blocking for writes or errors
  // occur just after a replug event.

  laser_fd_ = tcpclient_open(&tcpclient,tcphost, tcpport);//::open(port_name, O_RDWR | O_NONBLOCK | O_NOCTTY);
  //laser_fd_ = tcpclient_open(&tcpclient,"127.0.0.1", 10001);//::open(port_name, O_RDWR | O_NONBLOCK | O_NOCTTY);

  read_buf_start = read_buf_end = 0;


  if (laser_fd_ < 0)
  {
    HOKUYO_EXCEPT(hokuyo::Exception, "Failed to open tcp_client: %s:%d. %s (errno = %d)", tcphost,tcpport, strerror(errno), errno);
  }

  laser_fd_ = tcpclient.sock_desc;

  try
  {
    // Some models (04LX) need to be told to go into SCIP2 mode...
    laserFlush();
    // Just in case a previous failure mode has left our Hokuyo
    // spewing data, we send reset the laser to be safe.
    try {
      reset();
    }
    catch (hokuyo::Exception &e)
    {
      // This might be a device that needs to be explicitely placed in
      // SCIP2 mode.
      // Note: Not tested: a device that is currently scanning in SCIP1.1
      // mode might not manage to switch to SCIP2.0.
      setToSCIP2(); // If this fails then it wasn't a device that could be switched to SCIP2.
      reset(); // If this one fails, it really is an error.
    }

    querySensorConfig();

    queryVersionInformation(); // In preparation for calls to get various parts of the version info.
  }
  catch (hokuyo::Exception& e)
  {
    // These exceptions mean something failed on open and we should close
    if (laser_fd_ != -1)
      tcpclient_close(&tcpclient);
    laser_fd_ = -1;
    throw e;
  }

}

int cereal::CerealPort::available()
{
    if (!portOpen()) {
        return 0;
    }
    int count = 0;
    if (-1 == ioctl (fd_, TIOCINQ, &count)) {
        CEREAL_EXCEPT(cereal::Exception, "Available error -- error = %d: %s\n",  errno, strerror(errno));
    } else {
        return static_cast<size_t> (count);
    }
}

std::string
hokuyo::Laser::getStatus()
{
  if (!portOpen())
    HOKUYO_EXCEPT(hokuyo::Exception, "Port not open.");

  if (sendCmd("II",1000) != 0)
    HOKUYO_EXCEPT(hokuyo::Exception, "Error requesting device information information");
  
  char buf[100];
  char* stat = laserReadlineAfter(buf, 100, "STAT:");

  std::string statstr(stat);
  statstr = statstr.substr(0,statstr.length() - 3);

  return statstr;
}

void hokuyo::Laser::reset ()
{
	if (!portOpen())
	    HOKUYO_EXCEPT(hokuyo::Exception, "Port not open.");
        laserFlush();
        try
        {
          sendCmd("TM2", 1000);
        }
        catch (hokuyo::Exception &e)
        {} // Ignore. If the laser was scanning TM2 would fail
        try
        {
          sendCmd("RS", 1000);
        }
        catch (hokuyo::Exception &e)
        {} // Ignore. If the command coincided with a scan we might get garbage.
        laserFlush();
        sendCmd("RS", 1000); // This one should just work.
}

SerialPort::~SerialPort()
{
  if (portOpen())
    close();
}

long long
hokuyo::Laser::calcLatency(bool intensity, double min_ang, double max_ang, int clustering, int skip, int num, int timeout)
{
  ROS_DEBUG("Entering calcLatency.");

  if (!portOpen())
    HOKUYO_EXCEPT(hokuyo::Exception, "Port not open.");

  static const std::string buggy_version = "1.16.02(19/Jan./2010)";
  if (firmware_version_ == buggy_version)
  {
    ROS_INFO("Hokuyo firmware version %s detected. Using hard-coded time offset of -23 ms.", 
        buggy_version.c_str());
    offset_ = -23000000;
  }
  else
  {
    offset_ = 0;

    uint64_t comp_time = 0;
    uint64_t laser_time = 0;
    long long diff_time = 0;
    long long drift_time = 0;
    long long tmp_offset1 = 0;
    long long tmp_offset2 = 0;

    int count = 0;

    sendCmd("TM0",timeout);
    count = 100;

    for (int i = 0; i < count;i++)
    {
      usleep(1000);
      sendCmd("TM1",timeout);
      comp_time = timeHelper();
      try 
      {
        laser_time = readTime();

        diff_time = comp_time - laser_time;

        tmp_offset1 += diff_time / count;
      } catch (hokuyo::RepeatedTimeException &e)
      {
        // We expect to get Repeated Time's when hammering on the time server
        continue;
      }
    }

    uint64_t start_time = timeHelper();
    usleep(5000000);
    sendCmd("TM1;a",timeout);
    sendCmd("TM1;b",timeout);
    comp_time = timeHelper();
    drift_time = comp_time - start_time;
    laser_time = readTime() + tmp_offset1;
    diff_time = comp_time - laser_time;
    double drift_rate = double(diff_time) / double(drift_time);

    sendCmd("TM2",timeout);

    if (requestScans(intensity, min_ang, max_ang, clustering, skip, num, timeout) != 0)
      HOKUYO_EXCEPT(hokuyo::Exception, "Error requesting scans during latency calculation");

    hokuyo::LaserScan scan;

    count = 200;
    for (int i = 0; i < count;i++)
    {
      try
      {
        serviceScan(scan, 1000);
      } catch (hokuyo::CorruptedDataException &e) {
        continue;
      }

      comp_time = scan.system_time_stamp;
      drift_time = comp_time - start_time;
      laser_time = scan.self_time_stamp + tmp_offset1 + (long long)(drift_time*drift_rate);
      diff_time = laser_time - comp_time;

      tmp_offset2 += diff_time / count;
    }

    offset_ = tmp_offset2;

    stopScanning();
  }

  ROS_DEBUG("Leaving calcLatency.");

  return offset_;
}