SickLDBufferMonitor.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/SickLDBufferMonitor.hh>
00025 #include <sicktoolbox/SickLDMessage.hh>
00026 #include <sicktoolbox/SickException.hh>
00027 #include <sicktoolbox/SickLDUtility.hh>
00028 
00029 /* Associate the namespace */
00030 namespace SickToolbox {
00031 
00035   SickLDBufferMonitor::SickLDBufferMonitor( ) : SickBufferMonitor< SickLDBufferMonitor, SickLDMessage >(this) { }
00036 
00041   void SickLDBufferMonitor::GetNextMessageFromDataStream( SickLDMessage &sick_message ) throw( SickIOException ) {
00042 
00043     /* Flush the input buffer */
00044     uint8_t byte_buffer;  
00045 
00046     /* A buffer to hold the current byte out of the stream */
00047     const uint8_t sick_response_header[4] = {0x02,'U','S','P'};
00048     
00049     uint8_t checksum = 0;  
00050     uint8_t message_buffer[SickLDMessage::MESSAGE_MAX_LENGTH] = {0};
00051     uint32_t payload_length = 0;
00052 
00053     try {
00054 
00055       /* Search for the header in the byte stream */
00056       for (unsigned int i = 0; i < sizeof(sick_response_header);) {
00057         
00058         /* Acquire the next byte from the stream */
00059         _readBytes(&byte_buffer,1,DEFAULT_SICK_BYTE_TIMEOUT);
00060         
00061         /* Check if the current byte matches the expected header byte */
00062         if (byte_buffer == sick_response_header[i]) {
00063           i++;      
00064         }
00065         else {
00066           i = 0;
00067         }
00068         
00069       }  
00070       
00071       /* Populate message buffer w/ response header */
00072       memcpy(message_buffer,sick_response_header,4);
00073 
00074       /* Acquire the payload length! */
00075       _readBytes(&message_buffer[4],4,DEFAULT_SICK_BYTE_TIMEOUT);
00076       
00077       /* Extract the payload size and adjust the byte order */
00078       memcpy(&payload_length,&message_buffer[4],4);
00079       payload_length = sick_ld_to_host_byte_order(payload_length);
00080       
00081       /* Read the packet payload */
00082       _readBytes(&message_buffer[8],payload_length,DEFAULT_SICK_BYTE_TIMEOUT);
00083       
00084       /* Read the checksum */
00085       _readBytes(&checksum,1,DEFAULT_SICK_BYTE_TIMEOUT);
00086       
00087       /* Build the return message object based upon the received payload
00088        * and compute the associated checksum.
00089        *
00090        * NOTE: In constructing this message we ignore the header bytes
00091        *       buffered since the BuildMessage routine will insert the
00092        *       correct header automatically and compute the payload's
00093        *       checksum for us. We could probably get away with using
00094        *       just ParseMessage here and not computing the checksum as
00095        *       we are using TCP.  However, its safer this way.
00096        */
00097       sick_message.BuildMessage(&message_buffer[SickLDMessage::MESSAGE_HEADER_LENGTH],payload_length);
00098       
00099       /* Verify the checksum is correct (this is probably unnecessary since we are using TCP/IP) */
00100       if (sick_message.GetChecksum() != checksum) {
00101         throw SickBadChecksumException("SickLD::GetNextMessageFromDataStream: BAD CHECKSUM!!!");
00102       }
00103       
00104       /* Success */
00105 
00106     }
00107 
00108     catch(SickTimeoutException &sick_timeout) { /* This is ok! */ }
00109     
00110     /* Catch a bad checksum! */
00111     catch(SickBadChecksumException &sick_checksum_exception) {
00112       sick_message.Clear(); // Clear the message container
00113     }
00114     
00115     /* Catch any serious IO buffer exceptions */
00116     catch(SickIOException &sick_io_exception) {
00117       throw;
00118     }
00119 
00120     /* A sanity check */
00121     catch (...) {
00122       throw;
00123     }
00124     
00125   }
00126   
00130   SickLDBufferMonitor::~SickLDBufferMonitor( ) { }
00131     
00132 } /* namespace SickToolbox */


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