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