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