uint64_t hokuyo::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); }
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::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]; } }
void hokuyo::Laser::setToSCIP2() { if (!portOpen()) HOKUYO_EXCEPT(hokuyo::Exception, "Port not open."); const char * cmd = "SCIP2.0"; char buf[100]; laserWrite(cmd); laserWrite("\n"); laserReadline(buf, 100, 1000); ROS_DEBUG("Laser comm protocol changed to %s \n", buf); //printf ("Laser comm protocol changed to %s \n", buf); }
char* hokuyo::Laser::laserReadlineAfter(char* buf, int len, const char* str, int timeout) { buf[0] = 0; char* ind = &buf[0]; int bytes_read = 0; int skipped = 0; while ((strncmp(buf, str, strlen(str))) != 0) { bytes_read = laserReadline(buf,len,timeout); if ((skipped += bytes_read) > MAX_SKIPPED) HOKUYO_EXCEPT(hokuyo::Exception, "too many bytes skipped while searching for match"); } return ind += strlen(str); }
int hokuyo::Laser::serviceScan(hokuyo::LaserScan& scan, int timeout) { if (!portOpen()) HOKUYO_EXCEPT(hokuyo::Exception, "Port not open."); // Always clear ranges/intensities so we can return easily in case of erro scan.ranges.clear(); scan.intensities.clear(); char buf[100]; bool intensity = false; int min_i; int max_i; int cluster; int skip; int left; char* ind; int status = -1; do { ind = laserReadlineAfter(buf, 100, "M",timeout); scan.system_time_stamp = timeHelper() + offset_; if (ind[0] == 'D') intensity = false; else if (ind[0] == 'E') intensity = true; else continue; ind++; sscanf(ind, "%4d%4d%2d%1d%2d", &min_i, &max_i, &cluster, &skip, &left); laserReadline(buf,100,timeout); buf[4] = 0; if (!checkSum(buf, 4)) HOKUYO_EXCEPT(hokuyo::CorruptedDataException, "Checksum failed on status code: %s", buf); sscanf(buf, "%2d", &status); if (status != 99) return status; } while(status != 99); scan.config.min_angle = (min_i - afrt_) * (2.0*M_PI)/(ares_); scan.config.max_angle = (max_i - afrt_) * (2.0*M_PI)/(ares_); scan.config.ang_increment = cluster*(2.0*M_PI)/(ares_); scan.config.time_increment = (60.0)/(double)(rate_ * ares_); scan.config.scan_time = (60.0 * (skip + 1))/((double)(rate_)); scan.config.min_range = dmin_ / 1000.0; scan.config.max_range = dmax_ / 1000.0; readData(scan, intensity, timeout); long long inc = (long long)(min_i * scan.config.time_increment * 1000000000); scan.system_time_stamp += inc; scan.self_time_stamp += inc; return 0; }
int hokuyo::Laser::serviceScan(hokuyo::LaserScan& scan, int timeout) { if (!portOpen()) HOKUYO_EXCEPT(hokuyo::Exception, "Port not open."); // Always clear ranges/intensities so we can return easily in case of erro scan.ranges.clear(); scan.intensities.clear(); char buf[100]; bool intensity = false; int min_i; int max_i; int cluster; int skip; int left; char* ind; int status = -1; //printf("in serviceReques!\n"); do { //printf("in serviceReques1!\n"); ind = laserReadlineAfter(buf, 100, "M",timeout); //printf("in serviceReques2!\n"); scan.system_time_stamp = timeHelper() + offset_; // //printf("buf: %s\n",buf); // //printf("in serviceReques3!\n"); if (ind[0] == 'D') intensity = false; else if (ind[0] == 'E') intensity = true; else { //printf("in serviceReques4!\n"); continue; } ind++; sscanf(ind, "%4d%4d%2d%1d%2d", &min_i, &max_i, &cluster, &skip, &left); //99b //02]I8 //0^k0ij0Hm0>W1851:C1DN08018m1E11:^1Ao0GV0c?0Kg13j0E@0YF0h81C\1A_0X laserReadline(buf,100,timeout); buf[4] = 0; if (!checkSum(buf, 4)) HOKUYO_EXCEPT(hokuyo::CorruptedDataException, "Checksum failed on status code: %s", buf); sscanf(buf, "%2d", &status); if (status != 99) return status; } while(status != 99); //printf("in serviceReques5!\n"); scan.config.min_angle = (min_i - afrt_) * (2.0*M_PI)/(ares_); scan.config.max_angle = (max_i - afrt_) * (2.0*M_PI)/(ares_); scan.config.ang_increment = cluster*(2.0*M_PI)/(ares_); scan.config.time_increment = (60.0)/(double)(rate_ * ares_); scan.config.scan_time = (60.0 * (skip + 1))/((double)(rate_)); scan.config.min_range = dmin_ / 1000.0; scan.config.max_range = dmax_ / 1000.0; readData(scan, intensity, timeout); long long inc = (long long)(min_i * scan.config.time_increment * 1000000000); scan.system_time_stamp += inc; scan.self_time_stamp += inc; // printf("Scan took %lli.\n", -scan.system_time_stamp + timeHelper() + offset_); return 0; }
void hokuyo::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); //printf("readData 1\n"); int bytes; int range; float intensity; for (;;) { bytes = laserReadline(&buf[ind], 100 - ind, timeout); if (bytes == 1) // This is \n\n so we should be done return; //printf("readData: %s", buf+ind); if (!checkSum(&buf[ind], bytes)) { //printf("Checksum error!\n"); HOKUYO_EXCEPT(hokuyo::CorruptedDataException, "Checksum failed on data read."); } bytes += ind - 2; // Read as many ranges as we can get if(dmax_ > 20){ // Check error codes for the UTM 30LX (it is the only one with the long max range and has different error codes) 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)); switch (range) // See the SCIP2.0 reference on page 12, Table 4 { case 1: // No Object in Range scan.ranges.push_back(std::numeric_limits<float>::infinity()); break; case 2: // Object is too near (Internal Error) scan.ranges.push_back(-std::numeric_limits<float>::infinity()); break; case 3: // Measurement Error (May be due to interference) scan.ranges.push_back(std::numeric_limits<float>::quiet_NaN()); break; case 4: // Object out of range (at the near end) ///< @todo, Should this be an Infinity Instead? scan.ranges.push_back(std::numeric_limits<float>::quiet_NaN()); break; case 5: // Other errors scan.ranges.push_back(std::numeric_limits<float>::quiet_NaN()); break; default: scan.ranges.push_back(((float)range)/1000.0); } 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"); } } } else { // Check error codes for all other lasers (URG-04LX UBG-04LX-F01 UHG-08LX) 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)); switch (range) // See the SCIP2.0 reference on page 12, Table 3 { case 0: // Detected object is possibly at 22m scan.ranges.push_back(std::numeric_limits<float>::quiet_NaN()); break; case 1: // Reflected light has low intensity scan.ranges.push_back(std::numeric_limits<float>::quiet_NaN()); break; case 2: // Reflected light has low intensity scan.ranges.push_back(std::numeric_limits<float>::quiet_NaN()); break; case 6: // Others scan.ranges.push_back(std::numeric_limits<float>::quiet_NaN()); break; case 7: // Distance data on the preceding and succeeding steps have errors scan.ranges.push_back(std::numeric_limits<float>::quiet_NaN()); break; case 8: // Intensity difference of two waves scan.ranges.push_back(std::numeric_limits<float>::quiet_NaN()); break; case 9: // The same step had error in the last two scan scan.ranges.push_back(std::numeric_limits<float>::quiet_NaN()); break; case 10: // Others scan.ranges.push_back(std::numeric_limits<float>::quiet_NaN()); break; case 11: // Others scan.ranges.push_back(std::numeric_limits<float>::quiet_NaN()); break; case 12: // Others scan.ranges.push_back(std::numeric_limits<float>::quiet_NaN()); break; case 13: // Others scan.ranges.push_back(std::numeric_limits<float>::quiet_NaN()); break; case 14: // Others scan.ranges.push_back(std::numeric_limits<float>::quiet_NaN()); break; case 15: // Others scan.ranges.push_back(std::numeric_limits<float>::quiet_NaN()); break; case 16: // Others scan.ranges.push_back(std::numeric_limits<float>::quiet_NaN()); break; case 17: // Others scan.ranges.push_back(std::numeric_limits<float>::quiet_NaN()); break; case 18: // Error reading due to strong reflective object scan.ranges.push_back(std::numeric_limits<float>::quiet_NaN()); break; case 19: // Non-Measurable step scan.ranges.push_back(std::numeric_limits<float>::quiet_NaN()); break; default: scan.ranges.push_back(((float)range)/1000.0); } 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]; } }