packet_finder.cpp
Go to the documentation of this file.
1 
10 /*****************************************************************************
11 ** Includes
12 *****************************************************************************/
13 
14 #include <sstream>
15 #include "../../include/kobuki_driver/packet_handler/packet_finder.hpp"
16 
17 /*****************************************************************************
18 ** Namespaces
19 *****************************************************************************/
20 
21 namespace kobuki {
22 
23 /*****************************************************************************
24 ** Implementation
25 *****************************************************************************/
26 
28  state(waitingForStx), verbose(false)
29 {
30 }
31 
32 
33 /*****************************************************************************
34 ** Public
35 *****************************************************************************/
36 
37 void PacketFinderBase::configure(const std::string &sigslots_namespace,
38  const BufferType & putStx, const BufferType & putEtx, unsigned int sizeLengthField,
39  unsigned int sizeMaxPayload, unsigned int sizeChecksumField, bool variableSizePayload)
40 {
41  size_stx = putStx.size();
42  size_etx = putEtx.size();
43  size_length_field = sizeLengthField;
44  variable_size_payload = variableSizePayload;
45  size_max_payload = sizeMaxPayload;
46  size_payload = variable_size_payload ? 0 : sizeMaxPayload;
47  size_checksum_field = sizeChecksumField;
48  STX = putStx;
49  ETX = putEtx;
50  buffer = BufferType(size_stx + size_length_field + size_max_payload + size_checksum_field + size_etx);
51  state = waitingForStx;
52 
53  sig_warn.connect(sigslots_namespace + std::string("/ros_warn"));
54  sig_error.connect(sigslots_namespace + std::string("/ros_error"));
55 
56  //todo; exception
57  // Problem1: size_length_field = 1, vairable_size_payload = false
58 
59  clear();
60 }
61 
62 void PacketFinderBase::clear()
63 {
64  state = waitingForStx;
65  buffer.clear();
66 }
67 
68 void PacketFinderBase::enableVerbose()
69 {
70  verbose = true;
71 }
72 
73 bool PacketFinderBase::checkSum()
74 {
75  return true;
76 }
77 
78 unsigned int PacketFinderBase::numberOfDataToRead()
79 {
80  unsigned int num(0);
81 
82  switch (state)
83  {
84  case waitingForEtx:
85  num = 1;
86  break;
87 
88  case waitingForPayloadToEtx:
89  num = size_payload + size_etx + size_checksum_field;
90  break;
91 
92  case waitingForPayloadSize:
93  num = size_checksum_field;
94  break;
95 
96  case waitingForStx:
97  case clearBuffer:
98  default:
99  num = 1;
100  break;
101  }
102 
103  if (verbose)
104  {
105  printf("[state(%d):%02d]", state, num);
106  }
107  return num;
108 }
109 
110 void PacketFinderBase::getBuffer(BufferType & bufferRef)
111 {
112  bufferRef = buffer;
113 }
114 
115 void PacketFinderBase::getPayload(BufferType & bufferRef)
116 {
117  bufferRef.clear();
118  bufferRef.resize( buffer.size() - size_stx - size_etx - size_length_field - size_checksum_field );
119  for (unsigned int i = size_stx + size_length_field; i < buffer.size() - size_etx - size_checksum_field; i++) {
120  bufferRef.push_back(buffer[i]);
121  }
122 }
123 
131 bool PacketFinderBase::update(const unsigned char * incoming, unsigned int numberOfIncoming)
132 {
133  // clearBuffer = 0, waitingForStx, waitingForPayloadSize, waitingForPayloadToEtx, waitingForEtx,
134  // std::cout << "update [" << numberOfIncoming << "][" << state << "]" << std::endl;
135  if (!(numberOfIncoming > 0))
136  return false;
137 
138  bool found_packet(false);
139 
140  if ( state == clearBuffer ) {
141  buffer.clear();
142  state = waitingForStx;
143  }
144  switch (state)
145  {
146  case waitingForStx:
147  if (WaitForStx(incoming[0]))
148  {
149  if (size_length_field)
150  {
151  state = waitingForPayloadSize; // kobukibot
152  }
153  else
154  {
155  if (variable_size_payload)
156  {
157  // e.g. stargazer
158  state = waitingForEtx;
159  }
160  else
161  {
162  // e.g. iroboQ
163  //Todo; should put correct state
164  state = waitingForPayloadToEtx;
165  }
166  }
167  }
168  break;
169  case waitingForEtx:
170  if (waitForEtx(incoming[0], found_packet))
171  {
172  state = clearBuffer;
173  }
174  break;
175 
176  case waitingForPayloadSize:
177  if (waitForPayloadSize(incoming, numberOfIncoming))
178  {
179  state = waitingForPayloadToEtx;
180  }
181  break;
182 
183  case waitingForPayloadToEtx:
184  if (waitForPayloadAndEtx(incoming, numberOfIncoming, found_packet))
185  {
186  state = clearBuffer;
187  }
188  break;
189 
190  default:
191  state = waitingForStx;
192  break;
193  }
194  if ( found_packet ) {
195  return checkSum(); //what happen if checksum is equal to false(== -1)?
196  } else {
197  return false;
198  }
199 }
200 /*****************************************************************************
201 ** Protected
202 *****************************************************************************/
203 
204 bool PacketFinderBase::WaitForStx(const unsigned char datum)
205 {
206  bool found_stx(true);
207 
208  // add incoming datum
209  buffer.push_back(datum);
210 
211  // check whether we have STX
212  for (unsigned int i = 0; i < buffer.size() && i < STX.size(); i++)
213  {
214  if (buffer[i] != STX[i])
215  {
216  found_stx = false;
217  buffer.pop_front();
218  break;
219  }
220  }
221 
222  return (found_stx && buffer.size() == STX.size());
223 }
224 
225 bool PacketFinderBase::waitForPayloadSize(const unsigned char * incoming, unsigned int numberOfIncoming)
226 {
227  // push data
228  unsigned char first_byte;
229  for (unsigned int i = 0; i < numberOfIncoming; i++) {
230  first_byte = incoming[i];
231  buffer.push_back(incoming[i]);
232  }
233 
234  if (verbose)
235  {
236  for (unsigned int i = 0; i < buffer.size(); i++)
237  printf("%02x ", buffer[i]);
238  printf("\n");
239  }
240 
241  // check when we need to wait for etx
242  if (buffer.size() < size_stx + size_length_field)
243  {
244  return false;
245  }
246  else
247  {
248  switch (size_length_field)
249  {
250  case 1: // kobuki
251  size_payload = buffer[size_stx];
252  break;
253  case 2:
254  size_payload = buffer[size_stx];
255  size_payload |= buffer[size_stx + 1] << 8;
256  break;
257  case 4:
258  size_payload = buffer[size_stx];
259  size_payload |= buffer[size_stx + 1] << 8;
260  size_payload |= buffer[size_stx + 2] << 16;
261  size_payload |= buffer[size_stx + 3] << 24;
262  break;
263  default:
264  // put assertion failure
265  size_payload = 1;
266  break;
267  }
268 
269  if (verbose)
270  {
271  printf("[payloadSize: %d]\n", size_payload);
272  }
273 
274  return true;
275  }
276 }
277 
278 bool PacketFinderBase::waitForEtx(const unsigned char incoming, bool & foundPacket)
279 {
280  // push data
281  buffer.push_back(incoming);
282 
283  // check when we need to wait for etx
284  // if minimum payload size is 1
285  if (buffer.size() < size_stx + size_etx + 1)
286  {
287  return false;
288  }
289  else
290  {
291  unsigned int number_of_match(0);
292  for (unsigned int i = 0; i < ETX.size(); i++)
293  {
294  if (buffer[buffer.size() - ETX.size() + i] == ETX[i])
295  {
296  number_of_match++;
297  }
298  }
299 
300  if (number_of_match == ETX.size())
301  {
302  foundPacket = true;
303  return true;
304  }
305 
306  if (buffer.size() >= size_stx + size_max_payload + size_etx)
307  return true;
308  else
309  return false;
310  }
311 }
312 
313 bool PacketFinderBase::waitForPayloadAndEtx(const unsigned char * incoming, unsigned int numberOfIncoming, bool & foundPacket)
314 {
315  // push data
316  for (unsigned int i = 0; i < numberOfIncoming; i++)
317  {
318  buffer.push_back(incoming[i]);
319  }
320  /*********************
321  ** Error Handling
322  **********************/
323  if ( size_payload > size_max_payload ) {
324  state = clearBuffer;
325  std::ostringstream ostream;
326  ostream << "abnormally sized payload retrieved, clearing [" << size_max_payload << "][" << size_payload << "]";
327 
328  ostream << std::setfill('0') << std::uppercase; //declare once, use everytime
329 // ostream.fill('0') //call once, affects everytime
330 // ostream.width(2); //need to call everytime
331 
332 /*
333  ostream << "[";
334  for (unsigned int i = 0; i < numberOfIncoming; ++i ) {
335  ostream.width(2); // need to declare evertime
336  ostream << std::hex << static_cast<int>(*(incoming+i)) << " " << std::dec;
337  }
338  ostream << "\b]";
339 */
340  ostream << ", buffer: [" << std::setw(2) << buffer.size() << "][";
341  for (unsigned int i = 0; i < buffer.size(); ++i ) {
342  ostream << std::setw(2) << std::hex << static_cast<int>(buffer[i]) << " " << std::dec;
343  }
344  ostream << "\b]";
345 
346  sig_error.emit(ostream.str());
347  return false;
348  }
349  // check when we need to wait for etx
350  if (buffer.size() < size_stx + size_length_field + size_payload + size_checksum_field + size_etx)
351  {
352  return false;
353  }
354  else
355  {
356  if (verbose) {
357  std::cout << "Start check etx " << std::endl;
358  for (unsigned int i = 0; i < numberOfIncoming; ++i ) {
359  std::cout << std::hex << static_cast<int>(*(incoming+i)) << " ";
360  }
361  std::cout << std::dec << std::endl;
362  }
363  foundPacket = true;
364 
365  for (unsigned int i = (size_stx + size_length_field + size_payload + size_checksum_field);
366  i < (size_stx + size_length_field + size_payload + size_checksum_field + size_etx); i++)
367  {
368  if (buffer[i] != ETX[i])
369  {
370  foundPacket = false;
371  }
372  }
373  if (verbose)
374  std::cout << "End of checking etx " << std::endl;
375  return true;
376  }
377 }
378 
379 
380 } // namespace kobuki
kobuki
Definition: command.hpp:31
ecl::PushAndPop::push_back
void push_back(const Type &datum)
kobuki::PacketFinderBase::PacketFinderBase
PacketFinderBase()
Definition: packet_finder.cpp:31
ecl::PushAndPop< unsigned char >
ecl::PushAndPop::size
unsigned int size() const
ecl::PushAndPop::clear
void clear()


kobuki_driver
Author(s): Daniel Stonier , Younghun Ju , Jorge Santos Simon
autogenerated on Wed Mar 2 2022 00:26:14