byte_array.cpp
Go to the documentation of this file.
1 
7 #include "byte_array.h"
8 
9 #include <argos3/core/utility/math/general.h>
10 
11 #include <arpa/inet.h>
12 #include <cstdlib>
13 #include <cstring>
14 #include <cmath>
15 
16 namespace argos {
17 
18  /****************************************/
19  /****************************************/
20 
21  static SInt64 MAX_MANTISSA = 9223372036854775806LL; // 2 << 63 - 2;
22 
23  /****************************************/
24  /****************************************/
25 
26 #ifndef htonll
27  static UInt64 htonll(UInt64 un_value) {
28  /* Define a test variable for endianness - 42 is 'the' answer */
29  static const SInt32 nTest = 42;
30  /* Test for host endianness */
31  if(*reinterpret_cast<const UInt8*>(&nTest) == nTest) {
32  /* Host is little endian, network is big -> convert to big */
33  const UInt32 unHighWord = htonl(static_cast<UInt32>(un_value >> 32));
34  const UInt32 unLowWord = htonl(static_cast<UInt32>(un_value & 0xFFFFFFFFLL));
35  return static_cast<UInt64>(unLowWord) << 32 | unHighWord;
36  }
37  else {
38  /* Host is big endian - leave as is */
39  return un_value;
40  }
41  }
42 #endif
43 
44 #ifndef ntohll
45  static UInt64 ntohll(UInt64 un_value) {
46  /* Define a test variable for endianness - 42 is 'the' answer */
47  static const SInt32 nTest = 42;
48  /* Test for host endianness */
49  if(*reinterpret_cast<const UInt8*>(&nTest) == nTest) {
50  /* Host is little endian, network is big -> convert to big */
51  const UInt32 unHighWord = ntohl(static_cast<UInt32>(un_value >> 32));
52  const UInt32 unLowWord = ntohl(static_cast<UInt32>(un_value & 0xFFFFFFFFLL));
53  return static_cast<UInt64>(unLowWord) << 32 | unHighWord;
54  }
55  else {
56  /* Host is big endian - leave as is */
57  return un_value;
58  }
59  }
60 #endif
61 
62  /****************************************/
63  /****************************************/
64 
65  CByteArray::CByteArray(const UInt8* pun_buffer,
66  size_t un_size) {
67  AddBuffer(pun_buffer, un_size);
68  }
69 
70  /****************************************/
71  /****************************************/
72 
73  CByteArray::CByteArray(size_t un_size,
74  UInt8 un_value) {
75  m_vecBuffer.assign(un_size, un_value);
76  }
77 
78  /****************************************/
79  /****************************************/
80 
81  void CByteArray::Zero() {
82  ::memset(&m_vecBuffer[0], 0, sizeof(UInt8) * Size());
83  }
84 
85  /****************************************/
86  /****************************************/
87 
88  CByteArray& CByteArray::operator=(const CByteArray& c_byte_array) {
89  if(this != &c_byte_array) {
90  m_vecBuffer = c_byte_array.m_vecBuffer;
91  }
92  return *this;
93  }
94 
95  /****************************************/
96  /****************************************/
97 
98  bool CByteArray::operator==(const CByteArray& c_byte_array) const {
99  return m_vecBuffer == c_byte_array.m_vecBuffer;
100  }
101 
102  /****************************************/
103  /****************************************/
104 
105  CByteArray& CByteArray::AddBuffer(const UInt8* pun_buffer,
106  size_t un_size) {
107  for(size_t i = 0; i < un_size; ++i) {
108  m_vecBuffer.push_back(pun_buffer[i]);
109  }
110  return *this;
111  }
112 
113  /****************************************/
114  /****************************************/
115 
116  CByteArray& CByteArray::FetchBuffer(UInt8* pun_buffer,
117  size_t un_size) {
118  if(Size() < un_size) THROW_ARGOSEXCEPTION("Attempting to extract too many bytes from byte array (" << un_size << " requested, " << Size() << " available)");
119  for(size_t i = 0; i < un_size; ++i) {
120  *(pun_buffer+i) = m_vecBuffer[i];
121  }
122  m_vecBuffer.erase(m_vecBuffer.begin(), m_vecBuffer.begin() + un_size);
123  return *this;
124  }
125 
126  /****************************************/
127  /****************************************/
128 
129  CByteArray* CByteArray::operator()(size_t un_start,
130  ssize_t un_end) {
131  if(un_start >= Size()) THROW_ARGOSEXCEPTION("Attempting to extract from byte array beyond the limits (" << un_start << " requested, " << Size() << " size)");
132  un_end = un_end < 0 ? Size() : Min<ssize_t>(un_end, Size());
133  return new CByteArray(ToCArray() + un_start, un_end - un_start);
134  }
135 
136  /****************************************/
137  /****************************************/
138 
139  CByteArray& CByteArray::operator<<(UInt8 un_value) {
140  m_vecBuffer.push_back(un_value);
141  return *this;
142  }
143 
144  /****************************************/
145  /****************************************/
146 
147  CByteArray& CByteArray::operator>>(UInt8& un_value) {
148  if(Size() < 1) THROW_ARGOSEXCEPTION("Attempting to extract too many bytes from byte array (1 requested, " << Size() << " available)");
149  un_value = m_vecBuffer.front();
150  m_vecBuffer.erase(m_vecBuffer.begin());
151  return *this;
152  }
153 
154  /****************************************/
155  /****************************************/
156 
157  CByteArray& CByteArray::operator<<(SInt8 n_value) {
158  m_vecBuffer.push_back(n_value);
159  return *this;
160  }
161 
162  /****************************************/
163  /****************************************/
164 
165  CByteArray& CByteArray::operator>>(SInt8& n_value) {
166  if(Size() < 1) THROW_ARGOSEXCEPTION("Attempting to extract too many bytes from byte array (1 requested, " << Size() << " available)");
167  n_value = m_vecBuffer.front();
168  m_vecBuffer.erase(m_vecBuffer.begin());
169  return *this;
170  }
171 
172  /****************************************/
173  /****************************************/
174 
175  CByteArray& CByteArray::operator<<(UInt16 un_value) {
176  un_value = htons(un_value);
177  auto* punByte = reinterpret_cast<UInt8*>(&un_value);
178  m_vecBuffer.push_back(punByte[0]);
179  m_vecBuffer.push_back(punByte[1]);
180  return *this;
181  }
182 
183  /****************************************/
184  /****************************************/
185 
186  CByteArray& CByteArray::operator>>(UInt16& un_value) {
187  if(Size() < 2) THROW_ARGOSEXCEPTION("Attempting to extract too many bytes from byte array (2 requested, " << Size() << " available)");
188  auto* punByte = reinterpret_cast<UInt8*>(&un_value);
189  punByte[0] = m_vecBuffer[0];
190  punByte[1] = m_vecBuffer[1];
191  m_vecBuffer.erase(m_vecBuffer.begin(), m_vecBuffer.begin() + 2);
192  un_value = ntohs(un_value);
193  return *this;
194  }
195 
196  /****************************************/
197  /****************************************/
198 
199  CByteArray& CByteArray::operator<<(SInt16 n_value) {
200  n_value = htons(n_value);
201  auto* punByte = reinterpret_cast<UInt8*>(&n_value);
202  m_vecBuffer.push_back(punByte[0]);
203  m_vecBuffer.push_back(punByte[1]);
204  return *this;
205  }
206 
207  /****************************************/
208  /****************************************/
209 
210  CByteArray& CByteArray::operator>>(SInt16& n_value) {
211  if(Size() < 2) THROW_ARGOSEXCEPTION("Attempting to extract too many bytes from byte array (2 requested, " << Size() << " available)");
212  auto* punByte = reinterpret_cast<UInt8*>(&n_value);
213  punByte[0] = m_vecBuffer[0];
214  punByte[1] = m_vecBuffer[1];
215  m_vecBuffer.erase(m_vecBuffer.begin(), m_vecBuffer.begin() + 2);
216  n_value = ntohs(n_value);
217  return *this;
218  }
219 
220  /****************************************/
221  /****************************************/
222 
223  CByteArray& CByteArray::operator<<(UInt32 un_value) {
224  un_value = htonl(un_value);
225  auto* punByte = reinterpret_cast<UInt8*>(&un_value);
226  m_vecBuffer.push_back(punByte[0]);
227  m_vecBuffer.push_back(punByte[1]);
228  m_vecBuffer.push_back(punByte[2]);
229  m_vecBuffer.push_back(punByte[3]);
230  return *this;
231  }
232 
233  /****************************************/
234  /****************************************/
235 
236  CByteArray& CByteArray::operator>>(UInt32& un_value) {
237  if(Size() < 4) THROW_ARGOSEXCEPTION("Attempting to extract too many bytes from byte array (4 requested, " << Size() << " available)");
238  auto* punByte = reinterpret_cast<UInt8*>(&un_value);
239  punByte[0] = m_vecBuffer[0];
240  punByte[1] = m_vecBuffer[1];
241  punByte[2] = m_vecBuffer[2];
242  punByte[3] = m_vecBuffer[3];
243  m_vecBuffer.erase(m_vecBuffer.begin(), m_vecBuffer.begin() + 4);
244  un_value = ntohl(un_value);
245  return *this;
246  }
247 
248  /****************************************/
249  /****************************************/
250 
251  CByteArray& CByteArray::operator<<(SInt32 n_value) {
252  n_value = htonl(n_value);
253  auto* punByte = reinterpret_cast<UInt8*>(&n_value);
254  m_vecBuffer.push_back(punByte[0]);
255  m_vecBuffer.push_back(punByte[1]);
256  m_vecBuffer.push_back(punByte[2]);
257  m_vecBuffer.push_back(punByte[3]);
258  return *this;
259  }
260 
261  /****************************************/
262  /****************************************/
263 
264  CByteArray& CByteArray::operator>>(SInt32& n_value) {
265  if(Size() < 4) THROW_ARGOSEXCEPTION("Attempting to extract too many bytes from byte array (4 requested, " << Size() << " available)");
266  auto* punByte = reinterpret_cast<UInt8*>(&n_value);
267  punByte[0] = m_vecBuffer[0];
268  punByte[1] = m_vecBuffer[1];
269  punByte[2] = m_vecBuffer[2];
270  punByte[3] = m_vecBuffer[3];
271  m_vecBuffer.erase(m_vecBuffer.begin(), m_vecBuffer.begin() + 4);
272  n_value = ntohl(n_value);
273  return *this;
274  }
275 
276  /****************************************/
277  /****************************************/
278 
279  CByteArray& CByteArray::operator<<(UInt64 un_value) {
280  un_value = htonll(un_value);
281  auto* punByte = reinterpret_cast<UInt8*>(&un_value);
282  m_vecBuffer.push_back(punByte[0]);
283  m_vecBuffer.push_back(punByte[1]);
284  m_vecBuffer.push_back(punByte[2]);
285  m_vecBuffer.push_back(punByte[3]);
286  m_vecBuffer.push_back(punByte[4]);
287  m_vecBuffer.push_back(punByte[5]);
288  m_vecBuffer.push_back(punByte[6]);
289  m_vecBuffer.push_back(punByte[7]);
290  return *this;
291  }
292 
293  /****************************************/
294  /****************************************/
295 
296  CByteArray& CByteArray::operator>>(UInt64& un_value) {
297  if(Size() < 8) THROW_ARGOSEXCEPTION("Attempting to extract too many bytes from byte array (8 requested, " << Size() << " available)");
298  auto* punByte = reinterpret_cast<UInt8*>(&un_value);
299  punByte[0] = m_vecBuffer[0];
300  punByte[1] = m_vecBuffer[1];
301  punByte[2] = m_vecBuffer[2];
302  punByte[3] = m_vecBuffer[3];
303  punByte[4] = m_vecBuffer[4];
304  punByte[5] = m_vecBuffer[5];
305  punByte[6] = m_vecBuffer[6];
306  punByte[7] = m_vecBuffer[7];
307  m_vecBuffer.erase(m_vecBuffer.begin(), m_vecBuffer.begin() + 8);
308  un_value = ntohll(un_value);
309  return *this;
310  }
311 
312  /****************************************/
313  /****************************************/
314 
315  CByteArray& CByteArray::operator<<(SInt64 n_value) {
316  n_value = htonll(n_value);
317  auto* punByte = reinterpret_cast<UInt8*>(&n_value);
318  m_vecBuffer.push_back(punByte[0]);
319  m_vecBuffer.push_back(punByte[1]);
320  m_vecBuffer.push_back(punByte[2]);
321  m_vecBuffer.push_back(punByte[3]);
322  m_vecBuffer.push_back(punByte[4]);
323  m_vecBuffer.push_back(punByte[5]);
324  m_vecBuffer.push_back(punByte[6]);
325  m_vecBuffer.push_back(punByte[7]);
326  return *this;
327  }
328 
329  /****************************************/
330  /****************************************/
331 
332  CByteArray& CByteArray::operator>>(SInt64& n_value) {
333  if(Size() < 8) THROW_ARGOSEXCEPTION("Attempting to extract too many bytes from byte array (8 requested, " << Size() << " available)");
334  auto* punByte = reinterpret_cast<UInt8*>(&n_value);
335  punByte[0] = m_vecBuffer[0];
336  punByte[1] = m_vecBuffer[1];
337  punByte[2] = m_vecBuffer[2];
338  punByte[3] = m_vecBuffer[3];
339  punByte[4] = m_vecBuffer[4];
340  punByte[5] = m_vecBuffer[5];
341  punByte[6] = m_vecBuffer[6];
342  punByte[7] = m_vecBuffer[7];
343  m_vecBuffer.erase(m_vecBuffer.begin(), m_vecBuffer.begin() + 8);
344  n_value = ntohll(n_value);
345  return *this;
346  }
347 
348  /****************************************/
349  /****************************************/
350 
351  CByteArray& CByteArray::operator<<(unsigned long int un_value) {
352  if(sizeof(un_value) == sizeof(UInt32)) {
353  *this << static_cast<UInt32>(un_value);
354  }
355  else if(sizeof(un_value) == sizeof(UInt64)) {
356  *this << static_cast<UInt64>(un_value);
357  }
358  return *this;
359  }
360 
361  /****************************************/
362  /****************************************/
363 
364  CByteArray& CByteArray::operator>>(unsigned long int& un_value) {
365  if(sizeof(un_value) == sizeof(UInt32)) {
366  *this >> *reinterpret_cast<UInt32*>(&un_value);
367  }
368  else if(sizeof(un_value) == sizeof(UInt64)) {
369  *this >> *reinterpret_cast<UInt64*>(&un_value);
370  }
371  return *this;
372  }
373 
374  /****************************************/
375  /****************************************/
376 
377  CByteArray& CByteArray::operator<<(signed long int n_value) {
378  if(sizeof(n_value) == sizeof(SInt32)) {
379  *this << static_cast<SInt32>(n_value);
380  }
381  else if(sizeof(n_value) == sizeof(SInt64)) {
382  *this << static_cast<SInt64>(n_value);
383  }
384  return *this;
385  }
386 
387  /****************************************/
388  /****************************************/
389 
390  CByteArray& CByteArray::operator>>(signed long int& n_value) {
391  if(sizeof(n_value) == sizeof(SInt32)) {
392  *this >> *reinterpret_cast<SInt32*>(&n_value);
393  }
394  else if(sizeof(n_value) == sizeof(SInt64)) {
395  *this >> *reinterpret_cast<SInt64*>(&n_value);
396  }
397  return *this;
398  }
399 
400  /****************************************/
401  /****************************************/
402 
403  CByteArray& CByteArray::operator<<(double f_value) {
404  /* Buffer for the mantissa */
405  SInt64 nMantissa;
406  /* Buffer for the exponent */
407  SInt32 nExponent;
408  /* Calculate exponent and shifted significand */
409  /* The absolute value of the significand is either 0 (when f_value is 0) or
410  * in the range [0.5,1). The sign of the significand is the same as f_value.
411  * The shifted significand is then -0.5 when the significand is 0, and in the
412  * range [0,0.5) otherwise.
413  */
414  double fShiftedSignificand = Abs(frexp(f_value, &nExponent)) - 0.5;
415  /* Calculate mantissa */
416  if(fShiftedSignificand < 0.0) {
417  /* This means f_value was either +0 or -0 */
418  nMantissa = 0;
419  }
420  else {
421  /* Calculate the mantissa
422  * Idea:
423  * - take the shifted significand, which is in [0,0.5)
424  * - multiply it by 2, so it's in [0,1)
425  * - multiply this by the maximum value the mantissa can have
426  * - add 1 to the result to avoid having a zero mantissa
427  * (because it maps to a zero double when trasforming back)
428  */
429  nMantissa = static_cast<SInt64>(fShiftedSignificand * 2.0 * (MAX_MANTISSA)) + 1;
430  /* Take care of the sign */
431  if(f_value < 0.0) {
432  nMantissa = -nMantissa;
433  }
434  }
435  /* Store exponent and mantissa in the buffer */
436  *this << nMantissa;
437  *this << nExponent;
438  return *this;
439  }
440 
441  /****************************************/
442  /****************************************/
443 
444  CByteArray& CByteArray::operator>>(double& f_value) {
445  if(Size() < sizeof(SInt64) + sizeof(SInt32)) THROW_ARGOSEXCEPTION("Attempting to extract too many bytes from byte array (" << sizeof(SInt64) + sizeof(SInt32) << " requested, " << Size() << " available)");
446  /* Buffer for the mantissa */
447  SInt64 nMantissa;
448  /* Buffer for the exponent */
449  SInt32 nExponent;
450  /* Extract the values from the buffer */
451  *this >> nMantissa;
452  *this >> nExponent;
453  /* Calculate double value */
454  if(nMantissa == 0) {
455  /* Special case: zero mantissa */
456  f_value = 0.0;
457  }
458  else {
459  /* Calculate the significand, whose absolute value must be in the range [0.5,1).
460  * Idea:
461  * - take the absolute value of the mantissa, which is in the range [1,MANTISSA_MAX]
462  * - subtract 1, so it's in [0,MANTISSA_MAX-1]
463  * - divide by MANTISSA_MAX, so you get a value in [0,1)
464  * - divide by 2, so it's in [0,0.5)
465  * - add 0.5, so you end up in [0.5,1)
466  */
467  double fSignificand = (static_cast<double>(llabs(nMantissa) - 1) / MAX_MANTISSA) / 2.0 + 0.5;
468  /* Calculate f_value */
469  f_value = ldexp(fSignificand, nExponent);
470  /* Take care of the sign */
471  if(nMantissa < 0.0) {
472  f_value = -f_value;
473  }
474  }
475  return *this;
476  }
477 
478  /****************************************/
479  /****************************************/
480 
481  CByteArray& CByteArray::operator<<(float f_value) {
482  *this << static_cast<double>(f_value);
483  return *this;
484  }
485 
486  /****************************************/
487  /****************************************/
488 
489  CByteArray& CByteArray::operator>>(float& f_value) {
490  double fDoubleValue;
491  *this >> fDoubleValue;
492  f_value = static_cast<float>(fDoubleValue);
493  return *this;
494  }
495 
496  /****************************************/
497  /****************************************/
498 
499  CByteArray& CByteArray::operator<<(const std::string& str_value) {
500  /* Insert string contents */
501  for(size_t i = 0; i < str_value.size(); ++i) {
502  *this << static_cast<UInt8>(str_value[i]);
503  }
504  /* Terminate string with a \0 */
505  *this << static_cast<UInt8>(0);
506  return *this;
507  }
508 
509  /****************************************/
510  /****************************************/
511 
512  CByteArray& CByteArray::operator>>(std::string& str_value) {
513  if(Empty()) THROW_ARGOSEXCEPTION("Attempting to extract values from empty byte array");
514  str_value.clear();
515  size_t i = 0;
516  while(i < Size() && m_vecBuffer[i] != '\0') {
517  str_value += m_vecBuffer[i];
518  ++i;
519  }
520  if(m_vecBuffer[i] == '\0') {
521  ++i;
522  }
523  m_vecBuffer.erase(m_vecBuffer.begin(), m_vecBuffer.begin() + i);
524  return *this;
525  }
526 
527  /****************************************/
528  /****************************************/
529 
530  std::ostream& operator<<(std::ostream& c_os, const CByteArray& c_byte_array) {
531  c_os << "CByteArray [";
532  for(size_t i = 0; i < c_byte_array.Size(); ++i) {
533  c_os << " " << c_byte_array.m_vecBuffer[i];
534  }
535  c_os << " ]" << std::endl;
536  return c_os;
537  }
538 
539  /****************************************/
540  /****************************************/
541 
542 }
THROW_ARGOSEXCEPTION
#define THROW_ARGOSEXCEPTION(message)
This macro throws an ARGoS exception with the passed message.
Definition: argos_exception.h:111
SInt32
signed int SInt32
32-bit signed integer.
Definition: datatypes.h:93
UInt32
unsigned int UInt32
32-bit unsigned integer.
Definition: datatypes.h:97
SInt16
signed short SInt16
16-bit signed integer.
Definition: datatypes.h:74
UInt8
unsigned char UInt8
8-bit unsigned integer.
Definition: datatypes.h:60
SInt64
signed long long SInt64
64-bit signed integer.
Definition: datatypes.h:103
UInt64
unsigned long long UInt64
64-bit unsigned integer.
Definition: datatypes.h:107
UInt16
unsigned short UInt16
16-bit unsigned integer.
Definition: datatypes.h:78
SInt8
signed char SInt8
8-bit signed integer.
Definition: datatypes.h:45
argos
The namespace containing all the ARGoS related code.
Definition: ci_actuator.h:12
argos::operator<<
std::ostream & operator<<(std::ostream &c_os, const CByteArray &c_byte_array)
Definition: byte_array.cpp:530
argos::Abs
T Abs(const T &t_v)
Returns the absolute value of the passed argument.
Definition: general.h:25
argos::CByteArray
Byte array utility class.
Definition: byte_array.h:28
argos::CByteArray::Zero
void Zero()
Sets the contents of the byte array to all zeros.
Definition: byte_array.cpp:81
argos::CByteArray::operator=
CByteArray & operator=(const CByteArray &c_byte_array)
Assignment operator.
Definition: byte_array.cpp:88
argos::CByteArray::Size
size_t Size() const
Returns the current size of the byte array.
Definition: byte_array.h:66
argos::CByteArray::AddBuffer
CByteArray & AddBuffer(const UInt8 *pun_buffer, size_t un_size)
Appends bytes to the byte array.
Definition: byte_array.cpp:105
argos::CByteArray::FetchBuffer
CByteArray & FetchBuffer(UInt8 *pun_buffer, size_t un_size)
Moves elements from the byte array into the passed buffer.
Definition: byte_array.cpp:116
argos::CByteArray::ToCArray
const UInt8 * ToCArray() const
Returns the contents of the byte array as a const c-style array.
Definition: byte_array.h:112
argos::CByteArray::Empty
bool Empty() const
Returns true if the byte array is empty.
Definition: byte_array.h:100
argos::CByteArray::operator<<
CByteArray & operator<<(UInt8 un_value)
Appends a 8-bit unsigned integer to the byte array.
Definition: byte_array.cpp:139
argos::CByteArray::CByteArray
CByteArray()
Class constructor.
Definition: byte_array.h:35
argos::CByteArray::operator==
bool operator==(const CByteArray &c_byte_array) const
Equality comparison operator.
Definition: byte_array.cpp:98
argos::CByteArray::operator>>
CByteArray & operator>>(UInt8 &un_value)
Moves an 8-bit unsigned integer from the beginning of the byte array to the target variable.
Definition: byte_array.cpp:147
argos::CByteArray::operator()
CByteArray * operator()(size_t un_start, ssize_t un_end=-1)
Returns a new byte array that corresponds to a part of this byte array.
Definition: byte_array.cpp:129