md49_serialport.cpp
Go to the documentation of this file.
00001 #include <stdio.h>
00002 #include <string.h>
00003 #include <errno.h>
00004 #include <termios.h>
00005 #include <math.h>
00006 #include <poll.h>
00007 #include <signal.h>
00008 #include <fcntl.h>
00009 #include <iostream>
00010 #include <fstream>
00011 #include <stdexcept>
00012 
00013 #include "md49_serialport/md49_serialport.h"
00014 
00016 #define CEREAL_EXCEPT(except, msg, ...) \
00017 { \
00018     char buf[1000]; \
00019     snprintf(buf, 1000, msg " (in cereal::CerealPort::%s)" , ##__VA_ARGS__, __FUNCTION__); \
00020     throw except(buf); \
00021 }
00022 
00023 cereal::CerealPort::CerealPort() : fd_(-1)
00024 {
00025         stream_thread_ = NULL;
00026 }
00027 
00028 cereal::CerealPort::~CerealPort()
00029 {
00030         if(portOpen()) close();
00031 }
00032 
00033 void cereal::CerealPort::open(const char * port_name, int baud_rate)
00034 {
00035         if(portOpen()) close();
00036   
00037         // Make IO non blocking. This way there are no race conditions that
00038         // cause blocking when a badly behaving process does a read at the same
00039         // time as us. Will need to switch to blocking for writes or errors
00040         // occur just after a replug event.
00041         fd_ = ::open(port_name, O_RDWR | O_NONBLOCK | O_NOCTTY);
00042 
00043         if(fd_ == -1)
00044         {
00045                 const char *extra_msg = "";
00046                 switch(errno)
00047                 {
00048                         case EACCES:
00049                         extra_msg = "You probably don't have premission to open the port for reading and writing.";
00050                         break;
00051 
00052                         case ENOENT:
00053                         extra_msg = "The requested port does not exist. Is the hokuyo connected? Was the port name misspelled?";
00054                         break;
00055                 }
00056                 CEREAL_EXCEPT(cereal::Exception, "Failed to open port: %s. %s (errno = %d). %s", port_name, strerror(errno), errno, extra_msg);
00057         }
00058         
00059         try
00060         {       
00061                 struct flock fl;
00062                 fl.l_type = F_WRLCK;
00063                 fl.l_whence = SEEK_SET;
00064                 fl.l_start = 0;
00065                 fl.l_len = 0;
00066                 fl.l_pid = getpid();
00067 
00068                 if(fcntl(fd_, F_SETLK, &fl) != 0)
00069                         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);
00070 
00071                 // Settings for USB?
00072                 struct termios newtio;
00073                 tcgetattr(fd_, &newtio);
00074                 memset (&newtio.c_cc, 0, sizeof (newtio.c_cc));
00075                 newtio.c_cflag = CS8 | CLOCAL | CREAD;
00076                 newtio.c_iflag = IGNPAR;
00077                 newtio.c_oflag = 0;
00078                 newtio.c_lflag = 0;
00079                 cfsetspeed(&newtio, baud_rate);
00080                 baud_ = baud_rate;
00081 
00082                 // Activate new settings
00083                 tcflush(fd_, TCIFLUSH);
00084                 if(tcsetattr(fd_, TCSANOW, &newtio) < 0)
00085                         CEREAL_EXCEPT(cereal::Exception, "Unable to set serial port attributes. The port you specified (%s) may not be a serial port.", port_name); 
00086                 usleep (200000);
00087         }
00088         catch(cereal::Exception& e)
00089         {
00090                 // These exceptions mean something failed on open and we should close
00091                 if(fd_ != -1) ::close(fd_);
00092                 fd_ = -1;
00093                 throw e;
00094         }
00095 }
00096 
00097 void cereal::CerealPort::close()
00098 {
00099         int retval = 0;
00100         
00101         retval = ::close(fd_);
00102 
00103         fd_ = -1;
00104 
00105         if(retval != 0)
00106                 CEREAL_EXCEPT(cereal::Exception, "Failed to close port properly -- error = %d: %s\n", errno, strerror(errno));
00107 }
00108 
00109 int cereal::CerealPort::write(const char * data, int length)
00110 //int cereal::CerealPort::write(unsigned char data, int length)
00111 {
00112     int len = length==-1 ? strlen(data) : length;
00113 
00114         // IO is currently non-blocking. This is what we want for the more cerealon read case.
00115         int origflags = fcntl(fd_, F_GETFL, 0);
00116         fcntl(fd_, F_SETFL, origflags & ~O_NONBLOCK); // TODO: @todo can we make this all work in non-blocking?
00117         int retval = ::write(fd_, data, len);
00118         fcntl(fd_, F_SETFL, origflags | O_NONBLOCK);
00119 
00120         if(retval == len) return retval;
00121         else CEREAL_EXCEPT(cereal::Exception, "write failed");
00122 }
00123 
00124 int cereal::CerealPort::read(char * buffer, int max_length, int timeout)
00125 {
00126         int ret;
00127 
00128         struct pollfd ufd[1];
00129         int retval;
00130         ufd[0].fd = fd_;
00131         ufd[0].events = POLLIN;
00132 
00133         if(timeout == 0) timeout = -1; // For compatibility with former behavior, 0 means no timeout. For poll, negative means no timeout.
00134 
00135         if((retval = poll(ufd, 1, timeout)) < 0) CEREAL_EXCEPT(cereal::Exception, "poll failed -- error = %d: %s", errno, strerror(errno));
00136 
00137         if(retval == 0) CEREAL_EXCEPT(cereal::TimeoutException, "timeout reached");
00138                 
00139         if(ufd[0].revents & POLLERR) CEREAL_EXCEPT(cereal::Exception, "error on socket, possibly unplugged");
00140                 
00141     ret = ::read(fd_, buffer, max_length);
00142                 
00143         if(ret == -1 && errno != EAGAIN && errno != EWOULDBLOCK) CEREAL_EXCEPT(cereal::Exception, "read failed");
00144         
00145         return ret;
00146 }
00147 
00148 int cereal::CerealPort::readBytes(char * buffer, int length, int timeout)
00149 {
00150         int ret;
00151         int current = 0;
00152 
00153         struct pollfd ufd[1];
00154         int retval;
00155         ufd[0].fd = fd_;
00156         ufd[0].events = POLLIN;
00157 
00158         if(timeout == 0) timeout = -1; // For compatibility with former behavior, 0 means no timeout. For poll, negative means no timeout.
00159 
00160         while(current < length)
00161         {
00162                 if((retval = poll(ufd, 1, timeout)) < 0) CEREAL_EXCEPT(cereal::Exception, "poll failed -- error = %d: %s", errno, strerror(errno));
00163 
00164                 if(retval == 0) CEREAL_EXCEPT(cereal::TimeoutException, "timeout reached");
00165                 
00166                 if(ufd[0].revents & POLLERR) CEREAL_EXCEPT(cereal::Exception, "error on socket, possibly unplugged");
00167                 
00168         ret = ::read(fd_, &buffer[current], length-current);
00169                 
00170                 if(ret == -1 && errno != EAGAIN && errno != EWOULDBLOCK) CEREAL_EXCEPT(cereal::Exception, "read failed");
00171 
00172                 current += ret;
00173         }
00174         return current;
00175 }
00176 
00177 int cereal::CerealPort::readLine(char * buffer, int length, int timeout)
00178 {
00179         int ret;
00180         int current = 0;
00181 
00182         struct pollfd ufd[1];
00183         int retval;
00184         ufd[0].fd = fd_;
00185         ufd[0].events = POLLIN;
00186 
00187         if(timeout == 0) timeout = -1; // For compatibility with former behavior, 0 means no timeout. For poll, negative means no timeout.
00188 
00189         while(current < length-1)
00190         {
00191                 if(current > 0)
00192                         if(buffer[current-1] == '\n')
00193                                 return current;
00194 
00195                 if((retval = poll(ufd, 1, timeout)) < 0) CEREAL_EXCEPT(cereal::Exception, "poll failed -- error = %d: %s", errno, strerror(errno));
00196 
00197                 if(retval == 0) CEREAL_EXCEPT(cereal::TimeoutException, "timeout reached");
00198                 
00199                 if(ufd[0].revents & POLLERR) CEREAL_EXCEPT(cereal::Exception, "error on socket, possibly unplugged");
00200                 
00201         ret = ::read(fd_, &buffer[current], length-current);
00202                 
00203                 if(ret == -1 && errno != EAGAIN && errno != EWOULDBLOCK) CEREAL_EXCEPT(cereal::Exception, "read failed");
00204 
00205                 current += ret;
00206         }
00207         CEREAL_EXCEPT(cereal::Exception, "buffer filled without end of line being found");
00208 }
00209 
00210 bool cereal::CerealPort::readLine(std::string * buffer, int timeout)
00211 {
00212         int ret;
00213 
00214         struct pollfd ufd[1];
00215         int retval;
00216         ufd[0].fd = fd_;
00217         ufd[0].events = POLLIN;
00218 
00219         if(timeout == 0) timeout = -1; // For compatibility with former behavior, 0 means no timeout. For poll, negative means no timeout.
00220 
00221         buffer->clear();
00222         while(buffer->size() < buffer->max_size()/2)
00223         {
00224                 // Look for the end char
00225                 ret = buffer->find_first_of('\n');
00226                 if(ret > 0)
00227                 {
00228                         // If it is there clear everything after it and return
00229                         buffer->erase(ret+1, buffer->size()-ret-1);
00230                         return true;
00231                 }
00232 
00233                 if((retval = poll(ufd, 1, timeout)) < 0) CEREAL_EXCEPT(cereal::Exception, "poll failed -- error = %d: %s", errno, strerror(errno));
00234 
00235                 if(retval == 0) CEREAL_EXCEPT(cereal::TimeoutException, "timeout reached");
00236                 
00237                 if(ufd[0].revents & POLLERR) CEREAL_EXCEPT(cereal::Exception, "error on socket, possibly unplugged");
00238                 
00239         char temp_buffer[128];
00240         ret = ::read(fd_, temp_buffer, 128);
00241                 
00242                 if(ret == -1 && errno != EAGAIN && errno != EWOULDBLOCK) CEREAL_EXCEPT(cereal::Exception, "read failed");
00243 
00244                 // Append the new data to the buffer
00245         try{ buffer->append(temp_buffer, ret); }
00246         catch(std::length_error& le)
00247         {
00248             CEREAL_EXCEPT(cereal::Exception, "buffer filled without reaching end of data stream");
00249         }
00250         }
00251         CEREAL_EXCEPT(cereal::Exception, "buffer filled without end of line being found");
00252 }
00253 
00254 bool cereal::CerealPort::readBetween(std::string * buffer, char start, char end, int timeout)
00255 {
00256         int ret;
00257 
00258         struct pollfd ufd[1];
00259         static std::string erased;
00260         int retval;
00261         ufd[0].fd = fd_;
00262         ufd[0].events = POLLIN;
00263 
00264         if(timeout == 0) timeout = -1; // For compatibility with former behavior, 0 means no timeout. For poll, negative means no timeout.
00265 
00266         // Clear the buffer before we start
00267         buffer->clear();
00268         while(buffer->size() < buffer->max_size()/2)
00269         {
00270                 if((retval = poll(ufd, 1, timeout)) < 0) CEREAL_EXCEPT(cereal::Exception, "poll failed -- error = %d: %s", errno, strerror(errno));
00271                 
00272                 if(retval == 0) CEREAL_EXCEPT(cereal::TimeoutException, "timeout reached");
00273                 
00274                 if(ufd[0].revents & POLLERR) CEREAL_EXCEPT(cereal::Exception, "error on socket, possibly unplugged");
00275 
00276                 // Append erased characters in last iteration
00277                 if(!erased.empty())
00278                 {
00279                         try
00280                         { 
00281                                 buffer->append(erased);
00282                                 erased.clear();
00283                         }
00284                 catch(std::length_error& le)
00285                 {
00286                     CEREAL_EXCEPT(cereal::Exception, "failed to append erased to buffer");
00287                 }
00288             }
00289                 
00290                 char temp_buffer[3];
00291                 ret = ::read(fd_, temp_buffer, 3);
00292         
00293                 if(ret == -1 && errno != EAGAIN && errno != EWOULDBLOCK) CEREAL_EXCEPT(cereal::Exception, "read failed");
00294         
00295                 // Append the new data to the buffer
00296                 try{ buffer->append(temp_buffer, ret); }
00297         catch(std::length_error& le)
00298         {
00299             CEREAL_EXCEPT(cereal::Exception, "buffer filled without reaching end of data stream");
00300         }
00301         
00302         // Look for the start char
00303         ret = buffer->find_first_of(start);
00304         // If it is not on the buffer, clear it
00305         if(ret == -1) buffer->clear();
00306         // If it is there, but not on the first position clear everything behind it
00307         else if(ret > 0) buffer->erase(0, ret);
00308                 
00309                 // Look for the end char
00310                 ret = buffer->find_first_of(end);
00311                 if(ret > 0)
00312                 {
00313                         // If it is there clear everything after it and return
00314                         erased = buffer->substr(ret+1, buffer->size()-ret-1);
00315                         //std::cout << "sobra |" << erased << "|\n";
00316                         buffer->erase(ret+1, buffer->size()-ret-1);
00317                         return true;
00318                 }
00319         }
00320         CEREAL_EXCEPT(cereal::Exception, "buffer filled without reaching end of data stream");
00321 }
00322 
00323 int cereal::CerealPort::flush()
00324 {
00325           int retval = tcflush(fd_, TCIOFLUSH);
00326           if(retval != 0) CEREAL_EXCEPT(cereal::Exception, "tcflush failed");
00327           
00328           return retval;
00329 }
00330 
00331 bool cereal::CerealPort::startReadStream(boost::function<void(char*, int)> f)
00332 {
00333         if(stream_thread_ != NULL) return false;
00334 
00335         stream_stopped_ = false;
00336         stream_paused_ = false;
00337         
00338         readCallback = f;
00339         
00340         stream_thread_ = new boost::thread(boost::bind(&cereal::CerealPort::readThread, this));
00341         return true;
00342 }
00343 
00344 void cereal::CerealPort::readThread()
00345 {
00346         char data[MAX_LENGTH];
00347         int ret;
00348 
00349         struct pollfd ufd[1];
00350         ufd[0].fd = fd_;
00351         ufd[0].events = POLLIN;
00352         
00353         while(!stream_stopped_)
00354         {
00355                 if(!stream_paused_)
00356                 {
00357                         if(poll(ufd, 1, 10) > 0)
00358                         {
00359                                 if(!(ufd[0].revents & POLLERR))
00360                                 {
00361                                         ret = ::read(fd_, data, MAX_LENGTH);
00362                                         if(ret>0)
00363                                         {
00364                                                 readCallback(data, ret);
00365                                         }
00366                                 }
00367                         }
00368                 }
00369         }
00370 }
00371 
00372 bool cereal::CerealPort::startReadLineStream(boost::function<void(std::string*)> f)
00373 {
00374         if(stream_thread_ != NULL) return false;
00375 
00376         stream_stopped_ = false;
00377         stream_paused_ = false;
00378         
00379         readLineCallback = f;
00380         
00381         stream_thread_ = new boost::thread(boost::bind(&cereal::CerealPort::readLineThread, this));
00382         return true;
00383 }
00384 
00385 void cereal::CerealPort::readLineThread()
00386 {
00387         std::string data;
00388         bool error = false;
00389 
00390         while(!stream_stopped_)
00391         {
00392                 if(!stream_paused_)
00393                 {
00394                         error = false;
00395                         try{ readLine(&data, 100); }
00396                         catch(cereal::Exception& e)
00397                         { 
00398                                 error = true;
00399                         }
00400                         
00401                         if(!error && data.size()>0) readLineCallback(&data);
00402                 }
00403         }
00404 }
00405 
00406 bool cereal::CerealPort::startReadBetweenStream(boost::function<void(std::string*)> f, char start, char end)
00407 {
00408         if(stream_thread_ != NULL) return false;
00409 
00410         stream_stopped_ = false;
00411         stream_paused_ = false;
00412         
00413         readBetweenCallback = f;
00414         
00415         stream_thread_ = new boost::thread(boost::bind(&cereal::CerealPort::readBetweenThread, this, start, end));
00416         return true;
00417 }
00418 
00419 void cereal::CerealPort::readBetweenThread(char start, char end)
00420 {
00421         std::string data;
00422         bool error = false;
00423 
00424         while(!stream_stopped_)
00425         {
00426                 if(!stream_paused_)
00427                 {
00428                         error = false;
00429                         try{ readBetween(&data, start, end, 100); }
00430                         catch(cereal::Exception& e)
00431                         { 
00432                                 error = true;
00433                         }
00434                         
00435                         if(!error && data.size()>0) readBetweenCallback(&data);
00436                 }
00437         }
00438 }
00439 
00440 void cereal::CerealPort::stopStream()
00441 {
00442         stream_stopped_ = true;
00443         stream_thread_->join();
00444         
00445         delete stream_thread_;
00446         stream_thread_ = NULL;
00447 }
00448 
00449 void cereal::CerealPort::pauseStream()
00450 {
00451         stream_paused_ = true;
00452 }
00453 
00454 void cereal::CerealPort::resumeStream()
00455 {
00456         stream_paused_ = false;
00457 }


md49_serialport
Author(s): Fabian Prinzing
autogenerated on Thu Feb 11 2016 23:25:31