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);
}
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;
}
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];
}
}
int
hokuyo::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
}
}
bool
hokuyo::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.")
}
}
void
hokuyo::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));
}
int
hokuyo::Laser::laserOn() {
int res = sendCmd("BM",1000);
if (res == 1)
HOKUYO_EXCEPT(hokuyo::Exception, "Unable to control laser due to malfunction.");
return res;
}
int
hokuyo::Laser::requestScans(bool intensity, double min_ang, double max_ang, int cluster, int skip, int count, int timeout)
{
if (!portOpen())
HOKUYO_EXCEPT(hokuyo::Exception, "Port not open.");
int status;
if (cluster == 0)
cluster = 1;
int min_i = (int)(afrt_ + min_ang*ares_/(2.0*M_PI));
int max_i = (int)(afrt_ + max_ang*ares_/(2.0*M_PI));
char cmdbuf[MAX_CMD_LEN];
char intensity_char = 'D';
if (intensity)
intensity_char = 'E';
sprintf(cmdbuf,"M%c%.4d%.4d%.2d%.1d%.2d", intensity_char, min_i, max_i, cluster, skip, count);
status = sendCmd(cmdbuf, timeout);
return status;
}
long long
hokuyo::Laser::calcLatency(bool intensity, double min_ang, double max_ang, int clustering, int skip, int num, int timeout)
{
offset_ = 0;
if (!portOpen())
HOKUYO_EXCEPT(hokuyo::Exception, "Port not open.");
if (num <= 0)
num = 10;
int ckreps = 1;
int scanreps = 1;
long long int start = getHokuyoClockOffset(ckreps, timeout);
long long int pre = 0;
std::vector<long long int> samples(num);
for (int i = 0; i < num; i++)
{
long long int scan = getHokuyoScanStampToSystemStampOffset(intensity, min_ang, max_ang, clustering, skip, scanreps, timeout) - start;
long long int post = getHokuyoClockOffset(ckreps, timeout) - start;
samples[i] = scan - (post+pre)/2;
//printf("%lli %lli %lli %lli %lli\n", samples[i], post, pre, scan, pre - post);
//fflush(stdout);
pre = post;
}
offset_ = median(samples);
//printf("%lli\n", median(samples));
return offset_;
}
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);
}
int
hokuyo::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));
}
std::string
hokuyo::Laser::getProtocolVersion()
{
if (!portOpen())
HOKUYO_EXCEPT(hokuyo::Exception, "Port not open.");
return protocol_version_;
}
std::string
hokuyo::Laser::getVendorName()
{
if (!portOpen())
HOKUYO_EXCEPT(hokuyo::Exception, "Port not open.");
return vendor_name_;
}
std::string
hokuyo::Laser::getID()
{
if (!portOpen())
HOKUYO_EXCEPT(hokuyo::Exception, "Port not open.");
return serial_number_;
}
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;
}
std::string
hokuyo::Laser::getFirmwareVersion()
{
if (!portOpen())
HOKUYO_EXCEPT(hokuyo::Exception, "Port not open.");
return firmware_version_;
}
std::string
hokuyo::Laser::getProductName()
{
if (!portOpen())
HOKUYO_EXCEPT(hokuyo::Exception, "Port not open.");
return product_name_;
}
void
hokuyo::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;
}
}
int
hokuyo::Laser::laserFlush()
{
int retval = tcflush(laser_fd_, TCIOFLUSH);
if (retval != 0)
HOKUYO_EXCEPT(hokuyo::Exception, "tcflush failed");
return retval;
}
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);
}
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");
}
int
hokuyo::Laser::pollScan(hokuyo::LaserScan& scan, double min_ang, double max_ang, int cluster, int timeout)
{
if (!portOpen())
HOKUYO_EXCEPT(hokuyo::Exception, "Port not open.");
int status;
// Always clear ranges/intensities so we can return easily in case of erro
scan.ranges.clear();
scan.intensities.clear();
// clustering of 0 and 1 are actually the same
if (cluster == 0)
cluster = 1;
int min_i = (int)(afrt_ + min_ang*ares_/(2.0*M_PI));
int max_i = (int)(afrt_ + max_ang*ares_/(2.0*M_PI));
char cmdbuf[MAX_CMD_LEN];
sprintf(cmdbuf,"GD%.4d%.4d%.2d", min_i, max_i, cluster);
status = sendCmd(cmdbuf, timeout);
scan.system_time_stamp = timeHelper() + offset_;
if (status != 0)
return status;
// Populate configuration
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 = 0.0;
scan.config.min_range = dmin_ / 1000.0;
scan.config.max_range = dmax_ / 1000.0;
readData(scan, false, 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::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));
}
int
hokuyo::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
}
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);
}
void hokuyo::Laser::reset ()
{
if (!portOpen())
HOKUYO_EXCEPT(hokuyo::Exception, "Port not open.");
laserFlush();
try
{
sendCmd("TM2", 1000);
}
catch (hokuyo::Exception &e)
{} // Ignore. If the laser was scanning TM2 would fail
try
{
sendCmd("RS", 1000);
}
catch (hokuyo::Exception &e)
{} // Ignore. If the command coincided with a scan we might get garbage.
laserFlush();
sendCmd("RS", 1000); // This one should just work.
}
long long
hokuyo::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_;
}
void
hokuyo::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;
}
}
