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