SickLDBufferMonitor.cc
Go to the documentation of this file.
1 
17 /* Auto-generated header */
19 
20 /* Implementation dependencies */
21 #include <iostream>
22 #include <sys/ioctl.h>
23 
28 
29 /* Associate the namespace */
30 namespace SickToolbox {
31 
36 
42 
43  /* Flush the input buffer */
44  uint8_t byte_buffer;
45 
46  /* A buffer to hold the current byte out of the stream */
47  const uint8_t sick_response_header[4] = {0x02,'U','S','P'};
48 
49  uint8_t checksum = 0;
50  uint8_t message_buffer[SickLDMessage::MESSAGE_MAX_LENGTH] = {0};
51  uint32_t payload_length = 0;
52 
53  try {
54 
55  /* Search for the header in the byte stream */
56  for (unsigned int i = 0; i < sizeof(sick_response_header);) {
57 
58  /* Acquire the next byte from the stream */
59  _readBytes(&byte_buffer,1,DEFAULT_SICK_BYTE_TIMEOUT);
60 
61  /* Check if the current byte matches the expected header byte */
62  if (byte_buffer == sick_response_header[i]) {
63  i++;
64  }
65  else {
66  i = 0;
67  }
68 
69  }
70 
71  /* Populate message buffer w/ response header */
72  memcpy(message_buffer,sick_response_header,4);
73 
74  /* Acquire the payload length! */
75  _readBytes(&message_buffer[4],4,DEFAULT_SICK_BYTE_TIMEOUT);
76 
77  /* Extract the payload size and adjust the byte order */
78  memcpy(&payload_length,&message_buffer[4],4);
79  payload_length = sick_ld_to_host_byte_order(payload_length);
80 
81  /* Read the packet payload */
82  _readBytes(&message_buffer[8],payload_length,DEFAULT_SICK_BYTE_TIMEOUT);
83 
84  /* Read the checksum */
86 
87  /* Build the return message object based upon the received payload
88  * and compute the associated checksum.
89  *
90  * NOTE: In constructing this message we ignore the header bytes
91  * buffered since the BuildMessage routine will insert the
92  * correct header automatically and compute the payload's
93  * checksum for us. We could probably get away with using
94  * just ParseMessage here and not computing the checksum as
95  * we are using TCP. However, its safer this way.
96  */
97  sick_message.BuildMessage(&message_buffer[SickLDMessage::MESSAGE_HEADER_LENGTH],payload_length);
98 
99  /* Verify the checksum is correct (this is probably unnecessary since we are using TCP/IP) */
100  if (sick_message.GetChecksum() != checksum) {
101  throw SickBadChecksumException("SickLD::GetNextMessageFromDataStream: BAD CHECKSUM!!!");
102  }
103 
104  /* Success */
105 
106  }
107 
108  catch(SickTimeoutException &sick_timeout) { /* This is ok! */ }
109 
110  /* Catch a bad checksum! */
111  catch(SickBadChecksumException &sick_checksum_exception) {
112  sick_message.Clear(); // Clear the message container
113  }
114 
115  /* Catch any serious IO buffer exceptions */
116  catch(SickIOException &sick_io_exception) {
117  throw;
118  }
119 
120  /* A sanity check */
121  catch (...) {
122  throw;
123  }
124 
125  }
126 
131 
132 } /* namespace SickToolbox */
#define DEFAULT_SICK_BYTE_TIMEOUT
Max allowable time between consecutive bytes.
A class for monitoring the receive buffer when interfacing with a Sick LD LIDAR.
Contains some simple exception classes.
void _readBytes(uint8_t *const dest_buffer, const int num_bytes_to_read, const unsigned int timeout_value=0) const
A class to represent all messages sent to and from the Sick LD unit.
uint16_t sick_ld_to_host_byte_order(uint16_t value)
Converts Sick LD byte order (big-endian) to host byte order (little-endian)
void GetNextMessageFromDataStream(SickLDMessage &sick_message)
Acquires the next message from the SickLD byte stream.
SickLDBufferMonitor()
A standard constructor.
Defines a class for monitoring the receive buffer when interfacing w/ a Sick LMS LIDAR.
Encapsulates the Sick LIDAR Matlab/C++ toolbox.
Definition: SickLD.cc:44
Defines simple utility functions for working with the Sick LD.
Defines the class SickLDMessage.
~SickLDBufferMonitor()
A standard destructor.
Thrown instance where the driver can&#39;t read,write,drain,flush,... the buffers.
Makes handling timeouts much easier.
Thrown when a received message has an invalid checksum.


sicktoolbox
Author(s): Jason Derenick , Thomas Miller
autogenerated on Tue Sep 10 2019 03:37:34