52 #define CEREAL_EXCEPT(except, msg, ...) \
55 snprintf(buf, 1000, msg " (in cereal::CerealPort::%s)" , ##__VA_ARGS__, __FUNCTION__); \
66 if(portOpen()) close();
71 if(portOpen()) close();
77 fd_ = ::open(port_name, O_RDWR | O_NONBLOCK | O_NOCTTY);
81 const char *extra_msg =
"";
85 extra_msg =
"You probably don't have premission to open the port for reading and writing.";
89 extra_msg =
"The requested port does not exist. Is the hokuyo connected? Was the port name misspelled?";
92 CEREAL_EXCEPT(cereal::Exception,
"Failed to open port: %s. %s (errno = %d). %s", port_name, strerror(errno), errno, extra_msg);
99 fl.l_whence = SEEK_SET;
104 if(fcntl(fd_, F_SETLK, &fl) != 0)
105 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);
108 struct termios newtio;
109 tcgetattr(fd_, &newtio);
110 memset (&newtio.c_cc, 0, sizeof (newtio.c_cc));
111 newtio.c_cflag = CS8 | CLOCAL | CREAD;
112 newtio.c_iflag = IGNPAR;
115 cfsetspeed(&newtio, baud_rate);
119 tcflush(fd_, TCIFLUSH);
120 if(tcsetattr(fd_, TCSANOW, &newtio) < 0)
121 CEREAL_EXCEPT(cereal::Exception,
"Unable to set serial port attributes. The port you specified (%s) may not be a serial port.", port_name);
124 catch(cereal::Exception& e)
127 if(fd_ != -1) ::close(fd_);
137 retval = ::close(fd_);
142 CEREAL_EXCEPT(cereal::Exception,
"Failed to close port properly -- error = %d: %s\n", errno, strerror(errno));
147 int len = length==-1 ? strlen(data) : length;
150 int origflags = fcntl(fd_, F_GETFL, 0);
151 fcntl(fd_, F_SETFL, origflags & ~O_NONBLOCK);
152 int retval = ::write(fd_, data, len);
153 fcntl(fd_, F_SETFL, origflags | O_NONBLOCK);
155 if(retval == len)
return retval;
163 struct pollfd ufd[1];
166 ufd[0].events = POLLIN;
168 if(timeout == 0) timeout = -1;
170 if((retval = poll(ufd, 1, timeout)) < 0)
CEREAL_EXCEPT(cereal::Exception,
"poll failed -- error = %d: %s", errno, strerror(errno));
172 if(retval == 0)
CEREAL_EXCEPT(cereal::TimeoutException,
"timeout reached");
174 if(ufd[0].revents & POLLERR)
CEREAL_EXCEPT(cereal::Exception,
"error on socket, possibly unplugged");
176 ret = ::read(fd_, buffer, max_length);
178 if(ret == -1 && errno != EAGAIN && errno != EWOULDBLOCK)
CEREAL_EXCEPT(cereal::Exception,
"read failed");
188 struct pollfd ufd[1];
191 ufd[0].events = POLLIN;
193 if(timeout == 0) timeout = -1;
195 while(current < length)
197 if((retval = poll(ufd, 1, timeout)) < 0)
CEREAL_EXCEPT(cereal::Exception,
"poll failed -- error = %d: %s", errno, strerror(errno));
199 if(retval == 0)
CEREAL_EXCEPT(cereal::TimeoutException,
"timeout reached");
201 if(ufd[0].revents & POLLERR)
CEREAL_EXCEPT(cereal::Exception,
"error on socket, possibly unplugged");
203 ret = ::read(fd_, &buffer[current], length-current);
205 if(ret == -1 && errno != EAGAIN && errno != EWOULDBLOCK)
CEREAL_EXCEPT(cereal::Exception,
"read failed");
217 struct pollfd ufd[1];
220 ufd[0].events = POLLIN;
222 if(timeout == 0) timeout = -1;
224 while(current < length-1)
227 if(buffer[current-1] ==
'\n')
230 if((retval = poll(ufd, 1, timeout)) < 0)
CEREAL_EXCEPT(cereal::Exception,
"poll failed -- error = %d: %s", errno, strerror(errno));
232 if(retval == 0)
CEREAL_EXCEPT(cereal::TimeoutException,
"timeout reached");
234 if(ufd[0].revents & POLLERR)
CEREAL_EXCEPT(cereal::Exception,
"error on socket, possibly unplugged");
236 ret = ::read(fd_, &buffer[current], length-current);
238 if(ret == -1 && errno != EAGAIN && errno != EWOULDBLOCK)
CEREAL_EXCEPT(cereal::Exception,
"read failed");
242 CEREAL_EXCEPT(cereal::Exception,
"buffer filled without end of line being found");
249 struct pollfd ufd[1];
252 ufd[0].events = POLLIN;
254 if(timeout == 0) timeout = -1;
257 while(buffer->size() < buffer->max_size()/2)
260 ret = buffer->find_first_of(
'\n');
264 buffer->erase(ret+1, buffer->size()-ret-1);
268 if((retval = poll(ufd, 1, timeout)) < 0)
CEREAL_EXCEPT(cereal::Exception,
"poll failed -- error = %d: %s", errno, strerror(errno));
270 if(retval == 0)
CEREAL_EXCEPT(cereal::TimeoutException,
"timeout reached");
272 if(ufd[0].revents & POLLERR)
CEREAL_EXCEPT(cereal::Exception,
"error on socket, possibly unplugged");
274 char temp_buffer[128];
275 ret = ::read(fd_, temp_buffer, 128);
277 if(ret == -1 && errno != EAGAIN && errno != EWOULDBLOCK)
CEREAL_EXCEPT(cereal::Exception,
"read failed");
280 try{ buffer->append(temp_buffer, ret); }
281 catch(std::length_error& le)
283 CEREAL_EXCEPT(cereal::Exception,
"buffer filled without reaching end of data stream");
286 CEREAL_EXCEPT(cereal::Exception,
"buffer filled without end of line being found");
293 struct pollfd ufd[1];
296 ufd[0].events = POLLIN;
298 if(timeout == 0) timeout = -1;
302 while(buffer->size() < buffer->max_size()/2)
304 if((retval = poll(ufd, 1, timeout)) < 0)
CEREAL_EXCEPT(cereal::Exception,
"poll failed -- error = %d: %s", errno, strerror(errno));
306 if(retval == 0)
CEREAL_EXCEPT(cereal::TimeoutException,
"timeout reached");
308 if(ufd[0].revents & POLLERR)
CEREAL_EXCEPT(cereal::Exception,
"error on socket, possibly unplugged");
310 char temp_buffer[128];
311 ret = ::read(fd_, temp_buffer, 128);
313 if(ret == -1 && errno != EAGAIN && errno != EWOULDBLOCK)
CEREAL_EXCEPT(cereal::Exception,
"read failed");
316 try{ buffer->append(temp_buffer, ret); }
317 catch(std::length_error& le)
319 CEREAL_EXCEPT(cereal::Exception,
"buffer filled without reaching end of data stream");
323 ret = buffer->find_first_of(start);
325 if(ret == -1) buffer->clear();
327 else if(ret > 0) buffer->erase(0, ret);
330 ret = buffer->find_first_of(end);
334 buffer->erase(ret+1, buffer->size()-ret-1);
338 CEREAL_EXCEPT(cereal::Exception,
"buffer filled without reaching end of data stream");
343 int retval = tcflush(fd_, TCIOFLUSH);
344 if(retval != 0)
CEREAL_EXCEPT(cereal::Exception,
"tcflush failed");
351 if(stream_thread_ != NULL)
return false;
353 stream_stopped_ =
false;
354 stream_paused_ =
false;
367 struct pollfd ufd[1];
369 ufd[0].events = POLLIN;
371 while(!stream_stopped_)
375 if(poll(ufd, 1, 10) > 0)
377 if(!(ufd[0].revents & POLLERR))
382 readCallback(data, ret);
392 if(stream_thread_ != NULL)
return false;
394 stream_stopped_ =
false;
395 stream_paused_ =
false;
397 readLineCallback = f;
408 while(!stream_stopped_)
413 try{ readLine(&data, 100); }
414 catch(cereal::Exception& e)
419 if(!error && data.size()>0) readLineCallback(&data);
426 if(stream_thread_ != NULL)
return false;
428 stream_stopped_ =
false;
429 stream_paused_ =
false;
431 readBetweenCallback = f;
442 while(!stream_stopped_)
447 try{ readBetween(&data, start, end, 100); }
448 catch(cereal::Exception& e)
453 if(!error && data.size()>0) readBetweenCallback(&data);
460 stream_stopped_ =
true;
461 stream_thread_->join();
463 delete stream_thread_;
464 stream_thread_ = NULL;
469 stream_paused_ =
true;
474 stream_paused_ =
false;
bool startReadBetweenStream(boost::function< void(std::string *)> f, char start, char end)
Start a stream of readBetween()
void stopStream()
Stop streaming.
#define CEREAL_EXCEPT(except, msg,...)
Macro for throwing an exception with a message, passing args.
void readBetweenThread(char start, char end)
Thread for a stream of readBetween()
int readBytes(char *data, int length, int timeout=-1)
Read a fixed number of bytes from the serial port.
void open(const char *port_name, int baud_rate=115200)
Open the serial port.
int write(const char *data, int length=-1)
Write to the port.
bool readBetween(std::string *data, char start, char end, int timeout=-1)
Read from the serial port between a start char and an end char.
bool startReadStream(boost::function< void(char *, int)> f)
Start a stream of read()
void close()
Close the serial port.
int read(char *data, int max_length, int timeout=-1)
Read from the port.
void readThread()
Thread for a stream of read()
void resumeStream()
Resume streaming.
int flush()
Wrapper around tcflush.
bool startReadLineStream(boost::function< void(std::string *)> f)
Start a stream of readLine(std::string*, int)
void readLineThread()
Thread for a stream of readLine(std::string*, int)
boost::thread * stream_thread_
Stream thread.
int readLine(char *data, int length, int timeout=-1)
Read a line from the serial port.
void pauseStream()
Pause streaming.