SickLMS1xxBufferMonitor.cc
Go to the documentation of this file.
00001 
00017 /* Auto-generated header */
00018 #include <sicktoolbox/SickConfig.hh>
00019 
00020 /* Implementation dependencies */
00021 #include <iostream>
00022 #include <sys/ioctl.h>
00023 
00024 #include <sicktoolbox/SickLMS1xxBufferMonitor.hh>
00025 #include <sicktoolbox/SickLMS1xxMessage.hh>
00026 #include <sicktoolbox/SickLMS1xxUtility.hh>
00027 #include <sicktoolbox/SickException.hh>
00028 
00029 /* Associate the namespace */
00030 namespace SickToolbox {
00031 
00035   SickLMS1xxBufferMonitor::SickLMS1xxBufferMonitor( ) : SickBufferMonitor< SickLMS1xxBufferMonitor, SickLMS1xxMessage >(this) { }
00036 
00041   void SickLMS1xxBufferMonitor::GetNextMessageFromDataStream( SickLMS1xxMessage &sick_message ) throw( SickIOException ) {
00042 
00043     /* Flush the input buffer */
00044     uint8_t byte_buffer = 0;
00045     uint8_t payload_buffer[SickLMS1xxMessage::MESSAGE_PAYLOAD_MAX_LENGTH] = {0};
00046     
00047     try {
00048 
00049       /* Flush the TCP receive buffer */
00050       _flushTCPRecvBuffer();
00051 
00052       /* Search for STX in the byte stream */
00053       do {
00054         
00055         /* Grab the next byte from the stream */
00056         _readBytes(&byte_buffer,1,DEFAULT_SICK_LMS_1XX_BYTE_TIMEOUT);
00057         
00058       }
00059       while (byte_buffer != 0x02);
00060       
00061       /* Ok, now acquire the payload! (until ETX) */
00062       int payload_length = 0;
00063       do {
00064         
00065         payload_length++;
00066         _readBytes(&payload_buffer[payload_length-1],1,DEFAULT_SICK_LMS_1XX_BYTE_TIMEOUT);
00067         
00068       }
00069       while (payload_buffer[payload_length-1] != 0x03);
00070       payload_length--;
00071       
00072       /* Build the return message object based upon the received payload
00073        * NOTE: In constructing this message we ignore the header bytes
00074        *       buffered since the BuildMessage routine will insert the
00075        *       correct header automatically and verify the message size
00076        */
00077       sick_message.BuildMessage(payload_buffer,payload_length);
00078 
00079       /* Success */
00080       
00081     }
00082     
00083     catch(SickTimeoutException &sick_timeout) { /* This is ok! */ }
00084     
00085     /* Catch any serious IO buffer exceptions */
00086     catch(SickIOException &sick_io_exception) {
00087       throw;
00088     }
00089     
00090     /* A sanity check */
00091     catch (...) {
00092       throw;
00093     }
00094     
00095   }
00096 
00100   void SickLMS1xxBufferMonitor::_flushTCPRecvBuffer( ) const throw (SickIOException) {
00101     
00102     char null_byte;
00103     int num_bytes_waiting = 0;    
00104 
00105     /* Acquire number of awaiting bytes */
00106     if (ioctl(_sick_fd,FIONREAD,&num_bytes_waiting)) {
00107       throw SickIOException("SickLMS1xxBufferMonitor::_flushTCPRecvBuffer: ioctl() failed!");
00108     }
00109     
00110     /* Flush awaiting bytes */
00111     for (int i = 0; i < num_bytes_waiting; i++) {
00112       
00113       /* Capture a single byte from the stream! */
00114       if (read(_sick_fd,&null_byte,1) != 1) {
00115         throw SickIOException("SickLMS1xxBufferMonitor::_flushTCPRecvBuffer: ioctl() failed!");
00116       }   
00117       
00118     }
00119     
00120   }
00121   
00125   SickLMS1xxBufferMonitor::~SickLMS1xxBufferMonitor( ) { }
00126     
00127 } /* namespace SickToolbox */


sicktoolbox
Author(s): Jason Derenick , Thomas Miller
autogenerated on Sun May 5 2019 02:28:23