自学教程：C++ HOKUYO_EXCEPT函数代码示例

uint64_thokuyo::Laser::readTime(int timeout){  char buf[100];  laserReadline(buf, 100, timeout);  if (!checkSum(buf, 6))    HOKUYO_EXCEPT(hokuyo::CorruptedDataException, "Checksum failed on time stamp.");  unsigned int laser_time = ((buf[0]-0x30) << 18) | ((buf[1]-0x30) << 12) | ((buf[2]-0x30) << 6) | (buf[3] - 0x30);  if (laser_time == last_time_)  {    if (++time_repeat_count_ > 2)    {      HOKUYO_EXCEPT(hokuyo::RepeatedTimeException, "The timestamp has not changed for %d reads", time_repeat_count_);    }    else if (time_repeat_count_ > 0)      ROS_DEBUG("The timestamp has not changed for %d reads. Ignoring for now.", time_repeat_count_);  }  else  {    time_repeat_count_ = 0;  }  if (laser_time < last_time_)    wrapped_++;    last_time_ = laser_time;    return (uint64_t)((wrapped_ << 24) | laser_time)*(uint64_t)(1000000);}

voidhokuyo::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_;}

inthokuyo::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);}

voidhokuyo::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;}

voidhokuyo::Laser::readData(hokuyo::LaserScan& scan, bool has_intensity, int timeout){  scan.ranges.clear();  scan.intensities.clear();  int data_size = 3;  if (has_intensity)    data_size = 6;  char buf[100];  int ind = 0;  scan.self_time_stamp = readTime(timeout);  int bytes;  float range;  float intensity;  for (;;)  {    bytes = laserReadline(&buf[ind], 100 - ind, timeout);        if (bytes == 1)          // This is /n/n so we should be done      return;        if (!checkSum(&buf[ind], bytes))      HOKUYO_EXCEPT(hokuyo::CorruptedDataException, "Checksum failed on data read.");        bytes += ind - 2;        // Read as many ranges as we can get    for (int j = 0; j < bytes - (bytes % data_size); j+=data_size)    {      if (scan.ranges.size() < MAX_READINGS)      {        range = (((buf[j]-0x30) << 12) | ((buf[j+1]-0x30) << 6) | (buf[j+2]-0x30)) / 1000.0;	scan.ranges.push_back(range);        if (has_intensity)        {	  intensity = (((buf[j+3]-0x30) << 12) | ((buf[j+4]-0x30) << 6) | (buf[j+5]-0x30));          scan.intensities.push_back(intensity);        }      }      else      {        HOKUYO_EXCEPT(hokuyo::CorruptedDataException, "Got more readings than expected");      }    }    // Shuffle remaining bytes to front of buffer to get them on the next loop    ind = 0;    for (int j = bytes - (bytes % data_size); j < bytes ; j++)      buf[ind++] = buf[j];  }}

inthokuyo::Laser::laserReadline(char *buf, int len, int timeout){    int current=0;    struct pollfd ufd[1];    int retval;    ufd[0].fd = laser_fd_;    ufd[0].events = POLLIN;    if (timeout == 0)        timeout = -1; // For compatibility with former behavior, 0 means no timeout. For poll, negative means no timeout.    while (true)    {        if (read_buf_start == read_buf_end) // Need to read?        {            if ((retval = poll(ufd, 1, timeout)) < 0)                HOKUYO_EXCEPT(hokuyo::Exception, "poll failed   --  error = %d: %s", errno, strerror(errno));            if (retval == 0)                HOKUYO_EXCEPT(hokuyo::TimeoutException, "timeout reached");            if (ufd[0].revents & POLLERR)                HOKUYO_EXCEPT(hokuyo::Exception, "error on socket, possibly unplugged");            int bytes = read(laser_fd_, read_buf, sizeof(read_buf));            if (bytes == -1 && errno != EAGAIN && errno != EWOULDBLOCK)                HOKUYO_EXCEPT(hokuyo::Exception, "read failed");            read_buf_start = 0;            read_buf_end = bytes;        }        while (read_buf_end != read_buf_start)        {            if (current == len - 1)            {                buf[current] = 0;                HOKUYO_EXCEPT(hokuyo::Exception, "buffer filled without end of line being found");            }            buf[current] = read_buf[read_buf_start++];            if (buf[current++] == '/n')            {                buf[current] = 0;                return current;            }        }#ifdef USE_LOG_FILE        long long outtime = timeHelper();        fprintf(logfile, "In: %lli.%09lli %s", outtime / 1000000000L, outtime % 1000000000L, buf);#endif    }}

boolhokuyo::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.")  } }

voidhokuyo::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));}

inthokuyo::Laser::laserOn() {  int res = sendCmd("BM",1000);  if (res == 1)    HOKUYO_EXCEPT(hokuyo::Exception, "Unable to control laser due to malfunction.");  return res;}

long long int hokuyo::Laser::getHokuyoScanStampToSystemStampOffset(bool intensity, double min_ang, double max_ang, int clustering, int skip, int reps, int timeout){    if (reps < 1)        reps = 1;    else if (reps > 99)        reps = 99;    std::vector<long long int> offset(reps);    if (requestScans(intensity, min_ang, max_ang, clustering, skip, reps, timeout) != 0)    {        HOKUYO_EXCEPT(hokuyo::Exception, "Error requesting scan while caliblating time.");        return 1;    }    hokuyo::LaserScan scan;    for (int i = 0; i < reps; i++)    {        serviceScan(scan, timeout);        //printf("%lli %lli/n", scan.self_time_stamp, scan.system_time_stamp);        offset[i] = scan.self_time_stamp - scan.system_time_stamp;    }    return median(offset);}

inthokuyo::Laser::laserWrite(const char* msg){    // IO is currently non-blocking. This is what we want for the more common read case.    int origflags = fcntl(laser_fd_,F_GETFL,0);    fcntl(laser_fd_, F_SETFL, origflags & ~O_NONBLOCK); // @todo can we make this all work in non-blocking?    ssize_t len = strlen(msg);    ssize_t retval = write(laser_fd_, msg, len);    int fputserrno = errno;    fcntl(laser_fd_, F_SETFL, origflags | O_NONBLOCK);    errno = fputserrno; // Don't want to see the fcntl errno below.    if (retval != -1)    {#ifdef USE_LOG_FILE        if (strlen(msg) > 1)        {            long long outtime = timeHelper();            fprintf(logfile, "Out: %lli.%09lli %s/n", outtime / 1000000000L, outtime % 1000000000L, msg);        }#endif        return retval;    }    else        HOKUYO_EXCEPT(hokuyo::Exception, "fputs failed -- Error = %d: %s", errno, strerror(errno));}

voidhokuyo::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;  }}

inthokuyo::Laser::laserFlush(){  int retval = tcflush(laser_fd_, TCIOFLUSH);  if (retval != 0)    HOKUYO_EXCEPT(hokuyo::Exception, "tcflush failed");    return retval;} 

int hokuyo::Laser::laserReadline(char *buf, int len, int timeout){  char* ret;  int current=0;  struct pollfd ufd[1];  int retval;  ufd[0].fd = laser_fd_;  ufd[0].events = POLLIN;  while (current < len - 1)  {    if (current > 0)      if (buf[current-1] == '/n')	return current;    if (timeout == 0)      timeout = -1; // For compatibility with former behavior, 0 means no timeout. For poll, negative means no timeout.    if ((retval = poll(ufd, 1, timeout)) < 0)      HOKUYO_EXCEPT(hokuyo::Exception, "poll failed   --  error = %d: %s", errno, strerror(errno));    if (retval == 0)      HOKUYO_EXCEPT(hokuyo::TimeoutException, "timeout reached");    // Non blocking call so we don't block if a misbehaved process is    // accessing the port.    ret = fgets(&buf[current], len-current, laser_port_);    if (ret != &buf[current])      HOKUYO_EXCEPT(hokuyo::Exception, "fgets failed");    current += strlen(&buf[current]);  }  HOKUYO_EXCEPT(hokuyo::Exception, "buffer filled without end of line being found");}

inthokuyo::Laser::laserWrite(const char* msg){  // IO is currently non-blocking. This is what we want for the more common read case.  int origflags = fcntl(laser_fd_,F_GETFL,0);  fcntl(laser_fd_, F_SETFL, origflags & ~O_NONBLOCK); // @todo can we make this all work in non-blocking?  int retval = fputs(msg, laser_port_);  int fputserrno = errno;  fcntl(laser_fd_, F_SETFL, origflags | O_NONBLOCK);  errno = fputserrno; // Don't want to see the fcntl errno below.    if (retval != EOF)    return retval;  else    HOKUYO_EXCEPT(hokuyo::Exception, "fputs failed -- Error = %d: %s", errno, strerror(errno));}

inthokuyo::Laser::laserFlush(){#if HOKUYO_INTERFACE_ETHERNET  ::tcpclient_buffer_flush(&tcpclient);  return 0;#else  int retval = tcflush(laser_fd_, TCIOFLUSH);  if (retval != 0)    HOKUYO_EXCEPT(hokuyo::Exception, "tcflush failed errno: %d, socfd: %d", errno,laser_fd_);  read_buf_start = 0;  read_buf_end = 0;  return retval; #endif} 

long longhokuyo::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_;}

voidhokuyo::Laser::open(const char * port_name){  if (portOpen())    close();    laser_port_ = fopen(port_name, "r+");  if (laser_port_ == NULL)  {    const char *extra_msg = "";    switch (errno)    {      case EACCES:        extra_msg = "You probably don't have premission to open the port for reading and writing.";        break;      case ENOENT:        extra_msg = "The requested port does not exist. Is the hokuyo connected? Was the port name misspelled?";        break;    }    HOKUYO_EXCEPT(hokuyo::Exception, "Failed to open port: %s. %s (errno = %d). %s", port_name, strerror(errno), errno, extra_msg);  }  try  {    laser_fd_ = fileno (laser_port_);    if (laser_fd_ == -1)      HOKUYO_EXCEPT(hokuyo::Exception, "Failed to get file descriptor --  error = %d: %s", errno, strerror(errno));    // 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.    // No error checking. This really shouldn't fail, and even if it does,    // we aren't so badly off.    fcntl(laser_fd_, F_SETFL, fcntl(laser_fd_,F_GETFL,0) | O_NONBLOCK);    struct flock fl;    fl.l_type   = F_WRLCK;    fl.l_whence = SEEK_SET;    fl.l_start = 0;    fl.l_len   = 0;    fl.l_pid   = getpid();    if (fcntl(laser_fd_, F_SETLK, &fl) != 0)      HOKUYO_EXCEPT(hokuyo::Exception, "Device %s is already locked. Try 'lsof | grep %s' to find other processes that currently have the port open.", port_name, port_name);    // Settings for USB?    struct termios newtio;    memset (&newtio, 0, sizeof (newtio));    newtio.c_cflag = CS8 | CLOCAL | CREAD;    newtio.c_iflag = IGNPAR;    newtio.c_oflag = 0;    newtio.c_lflag = ICANON;        // activate new settings    tcflush (laser_fd_, TCIFLUSH);    if (tcsetattr (laser_fd_, TCSANOW, &newtio) < 0)      HOKUYO_EXCEPT(hokuyo::Exception, "Unable to set serial port attributes. The port you specified (%s) may not be a serial port.", port_name); /// @todo tcsetattr returns true if at least one attribute was set. Hence, we might not have set everything on success.    usleep (200000);    // 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_port_ != NULL)      fclose(laser_port_);    laser_port_ = NULL;    laser_fd_ = -1;    throw e;  }}