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