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