int cereal::CerealPort::readLine(char * buffer, int length, int timeout)
{
int ret;
int current = 0;
struct pollfd ufd[1];
int retval;
ufd[0].fd = 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(current < length-1)
{
if(current > 0)
if(buffer[current-1] == '\n' || buffer[current-1] == '\r')
{
buffer[current] = 0;
return current;
}
if((retval = poll(ufd, 1, timeout)) < 0) CEREAL_EXCEPT(cereal::Exception, "poll failed -- error = %d: %s", errno, strerror(errno));
if(retval == 0) CEREAL_EXCEPT(cereal::TimeoutException, "timeout reached");
if(ufd[0].revents & POLLERR) CEREAL_EXCEPT(cereal::Exception, "error on socket, possibly unplugged");
ret = ::read(fd_, &buffer[current], length-current);
if(ret == -1 && errno != EAGAIN && errno != EWOULDBLOCK) CEREAL_EXCEPT(cereal::Exception, "read failed");
current += ret;
}
CEREAL_EXCEPT(cereal::Exception, "buffer filled without end of line being found");
}
int cereal::CerealPort::flush()
{
int retval = tcflush(fd_, TCIOFLUSH);
if(retval != 0) CEREAL_EXCEPT(cereal::Exception, "tcflush failed");
return retval;
}
bool cereal::CerealPort::readBetween(std::string * buffer, char start, char end, int timeout)
{
int ret;
struct pollfd ufd[1];
int retval;
ufd[0].fd = 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.
// Clear the buffer before we start
buffer->clear();
while(buffer->size() < buffer->max_size()/2)
{
if((retval = poll(ufd, 1, timeout)) < 0) CEREAL_EXCEPT(cereal::Exception, "poll failed -- error = %d: %s", errno, strerror(errno));
if(retval == 0) CEREAL_EXCEPT(cereal::TimeoutException, "timeout reached");
if(ufd[0].revents & POLLERR) CEREAL_EXCEPT(cereal::Exception, "error on socket, possibly unplugged");
char temp_buffer[128];
ret = ::read(fd_, temp_buffer, 128);
if(ret == -1 && errno != EAGAIN && errno != EWOULDBLOCK) CEREAL_EXCEPT(cereal::Exception, "read failed");
// Append the new data to the buffer
buffer->append(temp_buffer, ret);
// Look for the start char
ret = buffer->find_first_of(start);
// If it is not on the buffer, clear it
if(ret == -1) buffer->clear();
// If it is there, but not on the first position clear everything behind it
else if(ret > 0) buffer->erase(0, ret);
// Look for the end char
ret = buffer->find_first_of(end);
if(ret > 0)
{
// If it is there clear everything after it and return
buffer->erase(ret+1, buffer->size()-ret-1);
return true;
}
}
CEREAL_EXCEPT(cereal::Exception, "buffer filled without reaching end of data stream");
}
void cereal::CerealPort::close()
{
int retval = 0;
retval = ::close(fd_);
fd_ = -1;
if(retval != 0)
CEREAL_EXCEPT(cereal::Exception, "Failed to close port properly -- error = %d: %s\n", errno, strerror(errno));
}
int cereal::CerealPort::available()
{
if (!portOpen()) {
return 0;
}
int count = 0;
if (-1 == ioctl (fd_, TIOCINQ, &count)) {
CEREAL_EXCEPT(cereal::Exception, "Available error -- error = %d: %s\n", errno, strerror(errno));
} else {
return static_cast<size_t> (count);
}
}
int cereal::CerealPort::write(const char * data, int length)
{
int len = length==-1 ? strlen(data) : length;
// IO is currently non-blocking. This is what we want for the more cerealon read case.
int origflags = fcntl(fd_, F_GETFL, 0);
fcntl(fd_, F_SETFL, origflags & ~O_NONBLOCK); // TODO: @todo can we make this all work in non-blocking?
int retval = ::write(fd_, data, len);
fcntl(fd_, F_SETFL, origflags | O_NONBLOCK);
if(retval == len) return retval;
else CEREAL_EXCEPT(cereal::Exception, "write failed");
}
int cereal::CerealPort::read(char * buffer, int max_length, int timeout)
{
int ret;
struct pollfd ufd[1];
int retval;
ufd[0].fd = 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.
if((retval = poll(ufd, 1, timeout)) < 0) CEREAL_EXCEPT(cereal::Exception, "poll failed -- error = %d: %s", errno, strerror(errno));
if(retval == 0) CEREAL_EXCEPT(cereal::TimeoutException, "timeout reached");
if(ufd[0].revents & POLLERR) CEREAL_EXCEPT(cereal::Exception, "error on socket, possibly unplugged");
ret = ::read(fd_, buffer, max_length);
if(ret == -1 && errno != EAGAIN && errno != EWOULDBLOCK) CEREAL_EXCEPT(cereal::Exception, "read failed");
return ret;
}
bool cereal::cereal_port::readLine(std::string * buffer, int timeout)
{
int ret;
struct pollfd ufd[1];
int retval;
ufd[0].fd = 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.
buffer->clear();
while(buffer->size() < buffer->max_size()/2)
{
// Look for the end char
ret = buffer->find_first_of('\n');
if(ret > 0)
{
// If it is there clear everything after it and return
buffer->erase(ret+1, buffer->size()-ret-1);
return true;
}
if((retval = poll(ufd, 1, timeout)) < 0) CEREAL_EXCEPT(cereal::Exception, "poll failed -- error = %d: %s", errno, strerror(errno));
if(retval == 0) CEREAL_EXCEPT(cereal::TimeoutException, "timeout reached");
if(ufd[0].revents & POLLERR) CEREAL_EXCEPT(cereal::Exception, "error on socket, possibly unplugged");
char temp_buffer[128];
ret = ::read(fd_, temp_buffer, 128);
if(ret == -1 && errno != EAGAIN && errno != EWOULDBLOCK) CEREAL_EXCEPT(cereal::Exception, "read failed");
// Append the new data to the buffer
try{ buffer->append(temp_buffer, ret); }
catch(std::length_error& le)
{
CEREAL_EXCEPT(cereal::Exception, "buffer filled without reaching end of data stream");
}
}
CEREAL_EXCEPT(cereal::Exception, "buffer filled without end of line being found");
}
void cereal::CerealPort::open(const char * port_name, int baud_rate)
{
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.
fd_ = ::open(port_name, O_RDWR | O_NONBLOCK | O_NOCTTY);
if(fd_ == -1)
{
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;
}
CEREAL_EXCEPT(cereal::Exception, "Failed to open port: %s. %s (errno = %d). %s", port_name, strerror(errno), errno, extra_msg);
}
try
{
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(fd_, F_SETLK, &fl) != 0)
CEREAL_EXCEPT(cereal::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;
tcgetattr(fd_, &newtio);
memset (&newtio.c_cc, 0, sizeof (newtio.c_cc));
newtio.c_cflag = CS8 | CLOCAL | CREAD;
newtio.c_iflag = IGNPAR;
newtio.c_oflag = 0;
newtio.c_lflag = 0;
cfsetspeed(&newtio, baud_rate);
baud_ = baud_rate;
// Activate new settings
tcflush(fd_, TCIFLUSH);
if(tcsetattr(fd_, TCSANOW, &newtio) < 0)
CEREAL_EXCEPT(cereal::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);
}
catch(cereal::Exception& e)
{
// These exceptions mean something failed on open and we should close
if(fd_ != -1) ::close(fd_);
fd_ = -1;
throw e;
}
}
