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