SickLMS2xxBufferMonitor.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 <termios.h>
00023 
00024 #include <sicktoolbox/SickLMS2xx.hh>
00025 #include <sicktoolbox/SickLMS2xxBufferMonitor.hh>
00026 #include <sicktoolbox/SickLMS2xxMessage.hh>
00027 #include <sicktoolbox/SickLMS2xxUtility.hh>
00028 #include <sicktoolbox/SickException.hh>
00029 
00030 /* Associate the namespace */
00031 namespace SickToolbox {
00032 
00036   SickLMS2xxBufferMonitor::SickLMS2xxBufferMonitor( ) : SickBufferMonitor<SickLMS2xxBufferMonitor,SickLMS2xxMessage>(this) { }
00037 
00042   void SickLMS2xxBufferMonitor::GetNextMessageFromDataStream( SickLMS2xxMessage &sick_message ) throw( SickIOException ) {
00043     
00044     uint8_t search_buffer[2] = {0};
00045     uint8_t payload_length_buffer[2] = {0};
00046     uint8_t payload_buffer[SickLMS2xxMessage::MESSAGE_PAYLOAD_MAX_LENGTH] = {0};
00047     uint8_t checksum_buffer[2] = {0};  
00048     uint16_t payload_length, checksum;
00049     
00050     try {
00051 
00052       /* Drain the I/O buffers! */
00053       if (tcdrain(_sick_fd) != 0) {
00054         throw SickIOException("SickLMS2xxBufferMonitor::GetNextMessageFromDataStream: tcdrain failed!");
00055       }
00056 
00057       /* Read until we get a valid message header */
00058       unsigned int bytes_searched = 0;
00059       while(search_buffer[0] != 0x02 || search_buffer[1] != DEFAULT_SICK_LMS_2XX_HOST_ADDRESS) {
00060         
00061         /* Slide the search window */
00062         search_buffer[0] = search_buffer[1];
00063         
00064         /* Attempt to read in another byte */
00065         _readBytes(&search_buffer[1],1,DEFAULT_SICK_LMS_2XX_SICK_BYTE_TIMEOUT);
00066 
00067         /* Header should be no more than max message length + header length bytes away */
00068         if (bytes_searched > SickLMS2xxMessage::MESSAGE_MAX_LENGTH + SickLMS2xxMessage::MESSAGE_HEADER_LENGTH) {
00069           throw SickTimeoutException("SickLMS2xxBufferMonitor::GetNextMessageFromDataStream: header timeout!");
00070         }
00071         
00072         /* Increment the number of bytes searched */
00073         bytes_searched++;
00074         
00075       }
00076       
00077       /* Read until we receive the payload length or we timeout */
00078       _readBytes(payload_length_buffer,2,DEFAULT_SICK_LMS_2XX_SICK_BYTE_TIMEOUT);
00079 
00080       /* Extract the payload length */
00081       memcpy(&payload_length,payload_length_buffer,2);
00082       payload_length = sick_lms_2xx_to_host_byte_order(payload_length);
00083 
00084       /* Make sure the payload length is legitimate, otherwise disregard */
00085       if (payload_length <= SickLMS2xxMessage::MESSAGE_MAX_LENGTH) {
00086 
00087         /* Read until we receive the payload or we timeout */
00088         _readBytes(payload_buffer,payload_length,DEFAULT_SICK_LMS_2XX_SICK_BYTE_TIMEOUT);
00089         
00090         /* Read until we receive the checksum or we timeout */
00091         _readBytes(checksum_buffer,2,DEFAULT_SICK_LMS_2XX_SICK_BYTE_TIMEOUT);
00092         
00093         /* Copy into uint16_t so it can be used */
00094         memcpy(&checksum,checksum_buffer,2);
00095         checksum = sick_lms_2xx_to_host_byte_order(checksum);
00096         
00097         /* Build a frame and compute the crc */
00098         sick_message.BuildMessage(DEFAULT_SICK_LMS_2XX_HOST_ADDRESS,payload_buffer,payload_length);
00099         
00100         /* See if the checksums match */
00101         if(sick_message.GetChecksum() != checksum) {
00102           throw SickBadChecksumException("SickLMS2xx::GetNextMessageFromDataStream: CRC16 failed!");
00103         }
00104 
00105       }
00106       
00107     }
00108     
00109     catch(SickTimeoutException &sick_timeout_exception) { /* This is ok! */ }
00110     
00111     /* Handle a bad checksum! */
00112     catch(SickBadChecksumException &sick_checksum_exception) {
00113       sick_message.Clear(); // Clear the message container
00114     }
00115     
00116     /* Handle any serious IO exceptions */
00117     catch(SickIOException &sick_io_exception) {
00118       throw; 
00119     }
00120 
00121     /* A sanity check */
00122     catch (...) { 
00123       throw; 
00124     }
00125     
00126   }
00127   
00131   SickLMS2xxBufferMonitor::~SickLMS2xxBufferMonitor( ) { }
00132     
00133 } /* namespace SickToolbox */


sicktoolbox
Author(s): Jason Derenick , Thomas Miller
autogenerated on Thu Aug 27 2015 15:17:16