Fawkes API  Fawkes Development Version
direct_com_message.cpp
1 /***************************************************************************
2  * direct_com_message.cpp - Message for RobotinoDirectThread
3  *
4  * Created: Mon Apr 04 15:48:52 2016
5  * Copyright 2011-2016 Tim Niemueller [www.niemueller.de]
6  ****************************************************************************/
7 
8 /* This program is free software; you can redistribute it and/or modify
9  * it under the terms of the GNU General Public License as published by
10  * the Free Software Foundation; either version 2 of the License, or
11  * (at your option) any later version.
12  *
13  * This program is distributed in the hope that it will be useful,
14  * but WITHOUT ANY WARRANTY; without even the implied warranty of
15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
16  * GNU Library General Public License for more details.
17  *
18  * Read the full text in the LICENSE.GPL file in the doc directory.
19  */
20 
21 #include "direct_com_message.h"
22 
23 #include <core/exception.h>
24 #include <iomanip>
25 #include <cstdlib>
26 
27 using namespace fawkes;
28 
29 /// @cond INTERNAL
30 const unsigned char DirectRobotinoComMessage::MSG_HEAD = 0xAA;
31 const unsigned char DirectRobotinoComMessage::MSG_DATA_ESCAPE = 0x55;
32 const unsigned char DirectRobotinoComMessage::MSG_DATA_MANGLE = 0x20;
33 
34 // 5: 0xAA + payload_size/2 ... + checksum/2
35 const unsigned int DirectRobotinoComMessage::MSG_METADATA_SIZE = 5;
36 /// @endcond INTERNAL
37 
38 /** @class DirectRobotinoComMessage::ChecksumError "direct_com_message.h"
39  * Excpetion thrown on checksum errors.
40  */
41 
42 /** Constructor.
43  * @param expected expected checksum
44  * @param received actually received checksm
45  * @param byte1 First byte of actually received checksum
46  * @param byte2 Second byte of actually received checksum
47  */
48 DirectRobotinoComMessage::ChecksumError::ChecksumError(unsigned int expected, unsigned int received,
49  unsigned char byte1, unsigned char byte2)
50  : Exception("Checksum verification error for Robotino message, "
51  "expected %u, got %u (%02x %02x)", expected, received, byte1, byte2)
52 {
53 }
54 
55 /** @class DirectRobotinoComMessage "direct_com_message.h"
56  * Robotino communication message.
57  *
58  * This object is used to create messages to send and parse messages
59  * to read. It is designed to be generic, i.e., it provides methods to
60  * add messages and its fields and to iterate over commands and read
61  * fields. These methods must be called in proper sequence, no command
62  * type specific processing is performed within the class. This
63  * approach was chosen since we do not strive for a user-facing
64  * generic transport, but rather for a minimal method to interact with
65  * Robotino's microcontroller.
66  *
67  * A message strictly differentiates between a reading and a writing
68  * mode, depending on the constructor with which it was created. The
69  * reading mode is used to parse received messages and provide access
70  * to its commands, the writing mode to construct messages to send.
71  * Furthermore, the message assumes quick run-throughs, i.e., after
72  * requesting the buffer of a writing message once, it is fixed and
73  * will not be re-generated after further additions.
74  *
75  * Terminology is a mix in part due to the naming in OpenRobotino:
76  * - Message: our representation of a message to send
77  * - Command: a command field within a message, this is called tag
78  * and also command in OpenRobotino. We chose the latter.
79  *
80  * @author Tim Niemueller
81  */
82 
83 /** Constructor.
84  * Create empty message for writing.
85  */
87 {
88  ctor();
89 }
90 
91 
92 /** Constructor for initial command.
93  * Create message for writing and add command for given message ID.
94  * @param cmdid message ID of command to add
95  */
97 {
98  ctor();
99 
100  add_command(cmdid);
101 }
102 
103 /** Constructor for incoming message.
104  * Create message for reading from incoming buffer.
105  * @param msg the message of \p msg_size is expected to be escaped and to range from
106  * the including 0xAA head byte to the checksum.
107  * @param msg_size size of \p msg buffer
108  */
109 DirectRobotinoComMessage::DirectRobotinoComMessage(const unsigned char *msg, size_t msg_size)
110 {
111  ctor();
112 
113  mode_ = READ;
114 
115  escaped_data_ = (unsigned char *)malloc(msg_size);
116  memcpy(escaped_data_, msg, msg_size);
117  escaped_data_size_ = msg_size;
118  size_t escaped_consumed = unescape_data();
119 
120  if (escaped_consumed < msg_size) {
121  escaped_data_ = (unsigned char *)realloc(escaped_data_, escaped_consumed);
122  escaped_data_size_ = escaped_consumed;
123  }
124 
125  check_checksum();
126 }
127 
128 void
129 DirectRobotinoComMessage::ctor()
130 {
131  payload_size_ = 0;
132  // always allocate 128 bytes, increase if necessary
133  data_size_ = 128;
134  data_ = (unsigned char *)malloc(data_size_);
135  memset(data_, 0, data_size_);
136  data_[0] = MSG_HEAD;
137  cur_data_ = data_ + 3;
138  cur_cmd_ = NULL;
139 
140  escaped_data_ = NULL;
141 
142  mode_ = WRITE;
143 }
144 
145 /** Destructor. */
147 {
148  ::free(data_);
149  data_size_ = payload_size_ = 0;
150  if (escaped_data_) ::free(escaped_data_);
151  cur_data_ = NULL;
152 }
153 
154 
155 /** Assert a given message mode.
156  * @param mode mode
157  * @throw Exception on mode mismatch
158  */
159 void
160 DirectRobotinoComMessage::assert_mode(mode_t mode) const
161 {
162  if (mode_ == WRITE && mode == READ) {
163  throw Exception("Message mode is writing, but requested reading operation");
164  } else if (mode_ == READ && mode == WRITE) {
165  throw Exception("Message mode is reading, but requested writing operation");
166  }
167 }
168 
169 
170 /** Assert that a command has been selected.
171  * @throw Exception if no command opened
172  */
173 void
174 DirectRobotinoComMessage::assert_command() const
175 {
176  if (! cur_cmd_) {
177  throw Exception("No command has been opened for reading (call next_command)");
178  }
179 }
180 
181 /** Assert minimum available data size.
182  * @param size number of bytes yet to be read
183  * @throw Exception if less than \p size bytes are available
184  */
185 void
186 DirectRobotinoComMessage::assert_command_data(uint8_t size) const
187 {
188  if (payload_size_ < size || cur_data_ + size > cur_cmd_ + cur_cmd_[1] + 2) {
189  throw Exception("Cannot read beyond command length %x %x (%x + %u >= %x + %u + 2)",
190  cur_data_ + size, cur_cmd_ + cur_cmd_[1] + 2, cur_data_, size, cur_cmd_, cur_cmd_[1]);
191  }
192 }
193 
194 /** Increase payload by a number of bytes.
195  * This may reallocate the memory to hold the data if it exceeds the current size.
196  * @param count number of bytes to extend payload by
197  */
198 void
199 DirectRobotinoComMessage::inc_payload_by(uint16_t count)
200 {
201  assert_mode(WRITE);
202  if (! cur_cmd_) {
203  throw Exception("Must add command before values");
204  }
205 
206  if (payload_size_ + count >= data_size_ - MSG_METADATA_SIZE) {
207  // need to realloc for more data
208  data_ = (unsigned char *)realloc(data_, data_size_ + 128);
209  }
210  payload_size_ += count;
211  cur_cmd_[1] += count;
212 }
213 
214 
215 /** Add a command header.
216  * This only allocates the header. You must call the appropriate methods to
217  * add the required data fields afterwards or the message will be
218  * rejected/ignored by the Robotino.
219  * @param cmdid command ID to add.
220  */
221 void
223 {
224  cur_cmd_ = cur_data_;
225  cur_data_ += 2;
226  inc_payload_by(2);
227 
228  cur_cmd_[0] = 0xff & cmdid;
229  cur_cmd_[1] = 0;
230 }
231 
232 /** Add 8-bit signed integer to current command.
233  * @param value value to add
234  * @throw Exception thrown if no command has been added, yet
235  */
236 void
238 {
239  inc_payload_by(1);
240  *(cur_data_++) = 0xFF & value;
241 }
242 
243 /** Add 8-bit unsigned integer to current command.
244  * @param value value to add
245  * @throw Exception thrown if no command has been added, yet
246  */
247 void
249 {
250  inc_payload_by(1);
251  *(cur_data_++) = 0xFF & value;
252 }
253 
254 /** Add 16-bit signed integer to current command.
255  * @param value value to add
256  * @throw Exception thrown if no command has been added, yet
257  */
258 void
260 {
261  inc_payload_by(2);
262  *(cur_data_++) = 0xFF & value;
263  *(cur_data_++) = value >> 8;
264 }
265 
266 /** Add 16-bit unsigned integer to current command.
267  * @param value value to add
268  * @throw Exception thrown if no command has been added, yet
269  */
270 void
272 {
273  inc_payload_by(2);
274  *(cur_data_++) = 0xFF & value;
275  *(cur_data_++) = value >> 8;
276 }
277 
278 /** Add 32-bit signed integer to current command.
279  * @param value value to add
280  * @throw Exception thrown if no command has been added, yet
281  */
282 void
284 {
285  inc_payload_by(4);
286  *(cur_data_++) = 0xFF & value;
287  *(cur_data_++) = 0xFF & ( value >> 8 );
288  *(cur_data_++) = 0xFF & ( value >> 16 );
289  *(cur_data_++) = 0xFF & ( value >> 24 );
290 }
291 
292 /** Add 32-bit unsigned integer to current command.
293  * @param value value to add
294  * @throw Exception thrown if no command has been added, yet
295  */
296 void
298 {
299  inc_payload_by(4);
300  *(cur_data_++) = 0xFF & value;
301  *(cur_data_++) = 0xFF & ( value >> 8 );
302  *(cur_data_++) = 0xFF & ( value >> 16 );
303  *(cur_data_++) = 0xFF & ( value >> 24 );
304 }
305 
306 /** Add float to current command.
307  * @param value value to add
308  * @throw Exception thrown if no command has been added, yet
309  */
310 void
312 {
313  inc_payload_by(4);
314  const char* p = reinterpret_cast<const char*>( &value );
315 
316  for(int i = 0; i < 4; ++i) {
317  *(cur_data_++) = *(p++);
318  }
319 }
320 
321 /** Rewind to read again from start.
322  * @throw Exception thrown if no not a reading message
323  */
324 void
326 {
327  assert_mode(READ);
328  cur_data_ = data_ + 3;
329  cur_cmd_ = NULL;
330 }
331 
332 /** Get next available command.
333  * @return ID of next command, or CMDID_NONE if no more commands available
334  */
335 DirectRobotinoComMessage::command_id_t
337 {
338  assert_mode(READ);
339  if (cur_cmd_ == NULL && payload_size_ >= 2) {
340  // no command set but payload that could hold one
341  cur_cmd_ = &data_[3];
342  cur_data_ = cur_cmd_ + 2;
343  return (command_id_t)cur_cmd_[0];
344  } else if (cur_cmd_ && ((data_ + payload_size_ + MSG_METADATA_SIZE - 2) - (cur_cmd_ + cur_cmd_[1] + 2)) >= 2) {
345  // we have a command and it does not extend beyond the payload, -2: subtract length of checksum
346  cur_cmd_ += cur_cmd_[1] + 2;
347  cur_data_ = cur_cmd_ + 2;
348  return (command_id_t)cur_cmd_[0];
349  } else {
350  return CMDID_NONE;
351  }
352 }
353 
354 
355 /** Get length of current command.
356  * @return length in bytes
357  */
358 uint8_t
360 {
361  assert_mode(READ);
362  assert_command();
363  return cur_cmd_[1];
364 }
365 
366 
367 /** Get ID of current command.
368  * @return command ID
369  */
370 DirectRobotinoComMessage::command_id_t
372 {
373  assert_mode(READ);
374  assert_command();
375  return (command_id_t)cur_cmd_[0];
376 }
377 
378 /** Get 8-bit signed integer from current command.
379  * This also forwards the command-internal pointer appropriately.
380  * @return value
381  * @throw Exception thrown if not enough data remains to be read
382  */
383 int8_t
385 {
386  assert_mode(READ);
387  assert_command();
388  assert_command_data(1);
389 
390  int8_t value = (int8_t)cur_data_[0];
391  cur_data_ += 1;
392  return value;
393 }
394 
395 /** Get 8-bit unsigned integer from current command.
396  * This also forwards the command-internal pointer appropriately.
397  * @return value
398  * @throw Exception thrown if not enough data remains to be read
399  */
400 uint8_t
402 {
403  assert_mode(READ);
404  assert_command();
405  assert_command_data(1);
406 
407  uint8_t value = (uint8_t)cur_data_[0];
408  cur_data_ += 1;
409  return value;
410 }
411 
412 /** Get 16-bit signed integer from current command.
413  * This also forwards the command-internal pointer appropriately.
414  * @return value
415  * @throw Exception thrown if not enough data remains to be read
416  */
417 int16_t
419 {
420  assert_mode(READ);
421  assert_command();
422  assert_command_data(2);
423 
424  int16_t value = (uint8_t)cur_data_[0];
425  value |= ( (int16_t)cur_data_[1] << 8 );
426  cur_data_ += 2;
427  return value;
428 }
429 
430 /** Get 16-bit unsigned integer from current command.
431  * This also forwards the command-internal pointer appropriately.
432  * @return value
433  * @throw Exception thrown if not enough data remains to be read
434  */
435 uint16_t
437 {
438  assert_mode(READ);
439  assert_command();
440  assert_command_data(2);
441 
442  uint16_t value = (uint8_t)cur_data_[0];
443  uint16_t h = (uint8_t)cur_data_[1];
444  value |= (h << 8);
445  cur_data_ += 2;
446  return value;
447 }
448 
449 /** Parse 16-bit unsigned integer from given buffer.
450  * @param buf buffer at least of size 2 bytes
451  * @return value
452  * @throw Exception thrown if not enough data remains to be read
453  */
454 uint16_t
455 DirectRobotinoComMessage::parse_uint16(const unsigned char *buf)
456 {
457  uint16_t value = (uint8_t)buf[0];
458  uint16_t h = (uint8_t)buf[1];
459  value |= (h << 8);
460  return value;
461 }
462 
463 /** Get 32-bit signed integer from current command.
464  * This also forwards the command-internal pointer appropriately.
465  * @return value
466  * @throw Exception thrown if not enough data remains to be read
467  */
468 int32_t
470 {
471  assert_mode(READ);
472  assert_command();
473  assert_command_data(4);
474 
475  int32_t value = (uint8_t)cur_data_[0];
476  int32_t h1 = (uint8_t)cur_data_[1];
477  int32_t h2 = (uint8_t)cur_data_[2];
478  int32_t h3 = (uint8_t)cur_data_[3];
479  value |= (h1 << 8);
480  value |= (h2 << 16);
481  value |= (h3 << 24);
482  cur_data_ += 4;
483  return value;
484 }
485 
486 /** Get 32-bit unsigned integer from current command.
487  * This also forwards the command-internal pointer appropriately.
488  * @return value
489  * @throw Exception thrown if not enough data remains to be read
490  */
491 uint32_t
493 {
494  assert_mode(READ);
495  assert_command();
496  assert_command_data(4);
497 
498  uint32_t value = (uint8_t)cur_data_[0];
499  uint32_t h1 = (uint8_t)cur_data_[1];
500  uint32_t h2 = (uint8_t)cur_data_[2];
501  uint32_t h3 = (uint8_t)cur_data_[3];
502  value |= (h1 << 8);
503  value |= (h2 << 16);
504  value |= (h3 << 24);
505  cur_data_ += 4;
506  return value;
507 }
508 
509 /** Get float from current command.
510  * This also forwards the command-internal pointer appropriately.
511  * @return value
512  * @throw Exception thrown if not enough data remains to be read
513  */
514 float
516 {
517  assert_mode(READ);
518  assert_command();
519  assert_command_data(4);
520 
521  float value;
522  char* p = reinterpret_cast<char*>( &value );
523  *(p++) = cur_data_[0];
524  *(p++) = cur_data_[1];
525  *(p++) = cur_data_[2];
526  *(p++) = cur_data_[3];
527  cur_data_ += 4;
528  return value;
529 }
530 
531 /** Get string from current command.
532  * This consumes all remaining data in the current command.
533  * @return value
534  * @throw Exception thrown if no data remains to be read
535  */
536 std::string
538 {
539  assert_mode(READ);
540  assert_command();
541  assert_command_data(1);
542 
543  size_t remaining = (cur_cmd_ + cur_cmd_[1] + 2) - cur_data_;
544  std::string value((const char *)cur_data_, remaining);
545  cur_data_ += remaining;
546  return value;
547 }
548 
549 
550 /** Skip 8-bit signed integer from current command.
551  * This also forwards the command-internal pointer appropriately.
552  * @throw Exception thrown if not enough data remains to be read
553  */
554 void
556 {
557  assert_mode(READ);
558  assert_command();
559  assert_command_data(1);
560 
561  cur_data_ += 1;
562 }
563 
564 /** Skip 8-bit unsigned integer from current command.
565  * This also forwards the command-internal pointer appropriately.
566  * @throw Exception thrown if not enough data remains to be read
567  */
568 void
570 {
571  assert_mode(READ);
572  assert_command();
573  assert_command_data(1);
574 
575  cur_data_ += 1;
576 }
577 
578 /** Skip 16-bit signed integer from current command.
579  * This also forwards the command-internal pointer appropriately.
580  * @throw Exception thrown if not enough data remains to be read
581  */
582 void
584 {
585  assert_mode(READ);
586  assert_command();
587  assert_command_data(2);
588 
589  cur_data_ += 2;
590 }
591 
592 /** Skip 16-bit unsigned integer from current command.
593  * This also forwards the command-internal pointer appropriately.
594  * @throw Exception thrown if not enough data remains to be read
595  */
596 void
598 {
599  assert_mode(READ);
600  assert_command();
601  assert_command_data(2);
602 
603  cur_data_ += 2;
604 }
605 
606 /** Skip 32-bit signed integer from current command.
607  * This also forwards the command-internal pointer appropriately.
608  * @throw Exception thrown if not enough data remains to be read
609  */
610 void
612 {
613  assert_mode(READ);
614  assert_command();
615  assert_command_data(4);
616 
617  cur_data_ += 4;
618 }
619 
620 /** Skip 32-bit unsigned integer from current command.
621  * This also forwards the command-internal pointer appropriately.
622  * @throw Exception thrown if not enough data remains to be read
623  */
624 void
626 {
627  assert_mode(READ);
628  assert_command();
629  assert_command_data(4);
630 
631  cur_data_ += 4;
632 }
633 
634 /** Skip float from current command.
635  * This also forwards the command-internal pointer appropriately.
636  * @throw Exception thrown if not enough data remains to be read
637  */
638 void
640 {
641  assert_mode(READ);
642  assert_command();
643  assert_command_data(4);
644 
645  cur_data_ += 4;
646 }
647 
648 
649 /** Size of escaped buffer.
650  * In particular, after calling the parsing ctor this denotes how many
651  * bytes of the input buffer have been consumed.
652  * On message creation, only provides meaningful values after calling
653  * pack() or buffer().
654  * @return size of escaped buffer
655  */
656 size_t
658 {
659  return escaped_data_size_;
660 }
661 
662 
663 /** Get payload size.
664  * @return payload size
665  */
666 size_t
668 {
669  return payload_size_;
670 }
671 
672 /** Get internal data buffer size.
673  * @return data buffer size
674  */
675 size_t
677 {
678  return data_size_;
679 }
680 
681 /** Perform message escaping. */
682 void
683 DirectRobotinoComMessage::escape()
684 {
685  unsigned short to_escape = 0;
686  for (int i = 1; i < payload_size_ + 4; ++i) {
687  if (data_[i] == MSG_HEAD || data_[i] == MSG_DATA_ESCAPE) {
688  ++to_escape;
689  }
690  }
691  if (escaped_data_) ::free(escaped_data_);
692  escaped_data_size_ = payload_size_ + MSG_METADATA_SIZE + to_escape;
693  escaped_data_ = (unsigned char *)malloc(escaped_data_size_);
694 
695  if (to_escape > 0) {
696  escaped_data_[0] = MSG_HEAD;
697  unsigned char *p = escaped_data_;
698  *p++ = MSG_HEAD;
699  for (unsigned int i = 1; i < payload_size_ + (MSG_METADATA_SIZE-1); ++i) {
700  if (data_[i] == MSG_HEAD || data_[i] == MSG_DATA_ESCAPE) {
701  *p++ = MSG_DATA_ESCAPE;
702  *p++ = data_[i] ^ MSG_DATA_MANGLE;
703  } else {
704  *p++ = data_[i];
705  }
706  }
707  } else {
708  memcpy(escaped_data_, data_, escaped_data_size_);
709  }
710 }
711 
712 /** Unescape a number of unescaped bytes.
713  * @param unescaped buffer to contain the unescaped data on return,
714  * must be at least of \p unescaped_size number of bytes
715  * @param unescaped_size expected number of bytes to unescape
716  * from input buffer
717  * @param escaped escaped buffer to process
718  * @param escaped_size size of escaped_buffer
719  * @return number of bytes consumed from escaped buffer
720  * @throw Exception if not enough bytes could be unescaped
721  */
722 size_t
723 DirectRobotinoComMessage::unescape(unsigned char *unescaped, size_t unescaped_size,
724  const unsigned char *escaped, size_t escaped_size)
725 {
726  if (unescaped_size == 0) return 0;
727 
728  unsigned int j = 0;
729  for (unsigned int i = 0; i < escaped_size; ++i) {
730  if (escaped[i] == MSG_DATA_ESCAPE) {
731  if (i >= escaped_size - 1) {
732  throw Exception("Read escaped byte last in message");
733  }
734  unescaped[j++] = escaped[i+1] ^ MSG_DATA_MANGLE;
735  i += 1;
736  } else {
737  unescaped[j++] = escaped[i];
738  }
739  if (j == unescaped_size) return i+1;
740  }
741 
742  throw Exception("Not enough escaped bytes for unescaping");
743 }
744 
745 /** Unescape all data.
746  * @return number of bytes consumed from escaped buffer
747  */
748 size_t
749 DirectRobotinoComMessage::unescape_data()
750 {
751  if (! escaped_data_ || escaped_data_size_ == 0) {
752  throw Exception("No escaped data to unescape");
753  }
754 
755  if (data_size_ < 3) {
756  data_ = (unsigned char *)realloc(data_, 3);
757  data_[0] = MSG_HEAD;
758  }
759  // +1: HEAD
760  size_t consumed_bytes = unescape(&data_[1], 2, &escaped_data_[1], escaped_data_size_-1) + 1;
761  size_t unescaped_size = parse_uint16(&data_[1]) + 2; // +2: checksum
762 
763  if (data_size_ < unescaped_size + 3) {
764  data_ = (unsigned char *)realloc(data_, unescaped_size + 3); // +3: HEAD, LENGTH
765  data_size_ = unescaped_size + 3;
766  }
767  payload_size_ = unescaped_size - 2; // -2: no checksum
768 
769  consumed_bytes +=
770  unescape(&data_[3], unescaped_size,
771  &escaped_data_[consumed_bytes], escaped_data_size_ - consumed_bytes);
772 
773  return consumed_bytes;
774 }
775 
776 
777 /** Get access to buffer for sending.
778  * This implies packing. Note that after calling this methods later
779  * modifications to the message will be ignored. The buffer is invalidated
780  * on the destruction of the message.
781  * @return buffer of escaped data.
782  */
783 boost::asio::const_buffer
785 {
786  pack();
787  return boost::asio::buffer(escaped_data_, escaped_data_size_);
788 }
789 
790 /** Pack message.
791  * This escapes the data to be sent.
792  */
793 void
795 {
796  if (! escaped_data_) {
797  data_[1] = 0xff & payload_size_;
798  data_[2] = payload_size_ >> 8;
799  unsigned short checksum_value = checksum();
800  data_[payload_size_ + 3] = 0xff & checksum_value;
801  data_[payload_size_ + 4] = checksum_value >> 8;
802  escape();
803  }
804 }
805 
806 /** Get checksum of message.
807  * @return checksum
808  */
809 uint16_t
811 {
812  uint16_t rv = 0;
813  const unsigned char* p = &data_[1];
814  // -3: do not include the 0xAA header and checksum
815  for (unsigned int i = 0; i < payload_size_ + (MSG_METADATA_SIZE-3); ++i) {
816  rv += (uint8_t)*p;
817  ++p;
818  }
819  return 0xffff & ( (1<<16) - rv );
820 }
821 
822 /** Check the checksum.
823  * This is for a parsed message to verify that the checksum in the received
824  * messages matches the self-calculated one.
825  */
826 void
827 DirectRobotinoComMessage::check_checksum() const
828 {
829  uint16_t checksum_v = checksum();
830  uint16_t packet_checksum = parse_uint16(&data_[payload_size_ + 3]);
831  if (checksum_v != packet_checksum) {
832  throw ChecksumError(checksum_v, packet_checksum,
833  data_[payload_size_ + 3], data_[payload_size_ + 4]);
834  }
835 }
836 
837 /** Generate string representation of message.
838  * @param escaped true to serialize escaped buffer, false to use unescaped buffer
839  * @return string representation of message in hexadecimal view with parantheses
840  * to indicate command boundaries.
841  */
842 std::string
844 {
845  boost::asio::const_buffer b;
846  const unsigned char *bp;
847  size_t bsize;
848  if (escaped) {
849  b = buffer();
850  bp = boost::asio::buffer_cast<const unsigned char*>(b);
851  bsize = boost::asio::buffer_size(b);
852  } else {
853  bp = data_;
854  bsize = payload_size_ + MSG_METADATA_SIZE;
855  }
856 
857  std::string rv;
858  // this buffer must hold and hex representation of a byte with whitespace
859  char tmp[8];
860 
861  // used for adding braces to indicate commands
862  int16_t cmd_l = -1;
863  int16_t cmd_i = 0;
864 
865  for (unsigned int i = 0; i < bsize; ++i) {
866  bool cmd_opened = false;
867  bool cmd_closed = false;
868  if (! escaped) {
869  if (i == 3 && bsize > MSG_METADATA_SIZE) {
870  cmd_l = bp[i+1];
871  cmd_i = 0;
872  cmd_opened = true;
873  } else if (i > 3 && cmd_l >= 0 && cmd_i == cmd_l) {
874  cmd_closed = true;
875  cmd_l = -1;
876  cmd_i = 0;
877  } else if (i > 3 && cmd_l < 0 && bsize - i > 2) {
878  cmd_opened = true;
879  cmd_l = bp[i+1];
880  cmd_i = 0;
881  } else {
882  ++cmd_i;
883  }
884  }
885 
886  if (i > 0 && (i+1) % 16 == 0) {
887  snprintf(tmp, 8, "%s%02x%s\n", cmd_opened ? "(" : " ", bp[i], cmd_closed ? ")" : " ");
888  } else if (i > 0 && (i+1) % 8 == 0) {
889  snprintf(tmp, 8, "%s%02x%s ", cmd_opened ? "(" : " ", bp[i], cmd_closed ? ")" : " ");
890  } else {
891  snprintf(tmp, 8, "%s%02x%s", cmd_opened ? "(" : " ", bp[i], cmd_closed ? ")" : " ");
892  }
893  rv += tmp;
894  }
895  if ((bsize-1) % 16 != 0) {
896  rv += "\n";
897  }
898 
899  return rv;
900 }
void add_uint32(uint32_t value)
Add 32-bit unsigned integer to current command.
int8_t get_int8()
Get 8-bit signed integer from current command.
void add_uint16(uint16_t value)
Add 16-bit unsigned integer to current command.
void add_command(command_id_t cmdid)
Add a command header.
ChecksumError(unsigned int expected, unsigned int received, unsigned char byte1, unsigned char byte2)
Constructor.
DirectRobotinoComMessage()
Constructor.
void add_uint8(uint8_t value)
Add 8-bit unsigned integer to current command.
void skip_uint16()
Skip 16-bit unsigned integer from current command.
void skip_int8()
Skip 8-bit signed integer from current command.
Fawkes library namespace.
std::string to_string(bool escaped=false)
Generate string representation of message.
Exception()
Constructor for subclasses.
Definition: exception.cpp:257
void rewind()
Rewind to read again from start.
std::string get_string()
Get string from current command.
void skip_float()
Skip float from current command.
virtual ~DirectRobotinoComMessage()
Destructor.
float get_float()
Get float from current command.
boost::asio::const_buffer buffer()
Get access to buffer for sending.
size_t data_size()
Get internal data buffer size.
size_t payload_size()
Get payload size.
void add_int16(int16_t value)
Add 16-bit signed integer to current command.
Base class for exceptions in Fawkes.
Definition: exception.h:36
void add_int32(int32_t value)
Add 32-bit signed integer to current command.
void skip_int32()
Skip 32-bit signed integer from current command.
void skip_int16()
Skip 16-bit signed integer from current command.
uint32_t get_uint32()
Get 32-bit unsigned integer from current command.
uint16_t get_uint16()
Get 16-bit unsigned integer from current command.
int32_t get_int32()
Get 32-bit signed integer from current command.
command_id_t next_command()
Get next available command.
static uint16_t parse_uint16(const unsigned char *buf)
Parse 16-bit unsigned integer from given buffer.
static size_t unescape(unsigned char *unescaped, size_t unescaped_size, const unsigned char *escaped, size_t escaped_size)
Unescape a number of unescaped bytes.
void skip_uint8()
Skip 8-bit unsigned integer from current command.
uint16_t checksum() const
Get checksum of message.
command_id_t command_id() const
Get ID of current command.
void skip_uint32()
Skip 32-bit unsigned integer from current command.
void add_int8(int8_t value)
Add 8-bit signed integer to current command.
int16_t get_int16()
Get 16-bit signed integer from current command.
uint8_t get_uint8()
Get 8-bit unsigned integer from current command.
size_t escaped_data_size()
Size of escaped buffer.
void add_float(float value)
Add float to current command.
uint8_t command_length() const
Get length of current command.