toolbox.cpp
Go to the documentation of this file.
1 
5 #include <stdio.h> /* for printf() and fprintf() */
6 #include <sys/socket.h> /* for socket(), bind(), and connect() */
7 #include <arpa/inet.h> /* for sockaddr_in and inet_ntoa() */
8 #include <stdlib.h> /* for atoi() and exit() */
9 #include <string.h> /* for memset() */
10 #include "toolbox.hpp"
11 #include <iostream>
12 #include <iomanip> // for std::setprecision
13 #include <sstream> // for std::stringstream
14 #include "errorhandler.hpp"
15 
16 //
17 // Write a binary trace output, e.g. of buffer contents.
18 // Can be used for debugging.
19 //
20 void traceBuffer(std::string headerText, BYTE* buffer, UINT32 len)
21 {
22  // Table header
23  printInfoMessage(headerText, true);
24 
25  // Length
26  std::string line;
27  line = "Length= " + toString(len) + " bytes.";
28  printInfoMessage(line, true);
29 
30  // Contents
31  UINT32 pos = 0;
32  while (pos < len)
33  {
34  line = toHexString(pos) + ": ";
35  for (UINT16 i=0; i< 16; i++)
36  {
37  line += toHexString(buffer[pos]) + " ";
38  pos++;
39  if (pos >= len)
40  {
41  break;
42  }
43  }
44  printInfoMessage(line, true);
45  }
46 }
47 
48 //
49 // String conversion: Values to a hex string.
50 // 0..16 --> 0..F
51 //
52 std::string toHexStringNibble(UINT8 val)
53 {
54  std::string s = "0123456789ABCDEF";
55  std::string c;
56  if (val < 16)
57  {
58  c = s.substr(val, 1);
59  }
60  else
61  {
62  c = "x";
63  }
64  return c;
65 }
66 
67 //
68 // UINT32-value to Hex-String
69 // Result: "xxxxxxxx"
70 //
71 std::string toHexString(UINT32 val)
72 {
73  std::string s = toHexString((UINT16)(val >> 16));
74  s += toHexString((UINT16)(val & 0xFFFF));
75  return s;
76 }
77 
78 // Ergebnis: "xxxx"
79 std::string toHexString(UINT16 val)
80 {
81  std::string s = toHexStringNibble((UINT8)(val >> 12));
82  s += toHexStringNibble((UINT8)((val >> 8) & 0xF));
83  s += toHexStringNibble((UINT8)((val >> 4) & 0xF));
84  s += toHexStringNibble((UINT8)(val & 0xF));
85  return s;
86 }
87 
88 // Ergebnis: "xx"
89 std::string toHexString(UINT8 val)
90 {
91  std::string s1 = toHexStringNibble((UINT8)(val >> 4));
92  std::string s2 = toHexStringNibble((UINT8)(val & 0x0F));
93  std::string s = s1 + s2;
94  return s;
95 }
96 
97 
98 //
99 // Konvertiert einen String in Kleinbuchstaben
100 //
101 std::string toLower(const std::string& text)
102 {
103  std::string low;
104  UINT32 i;
105  unsigned char c;
106  for (i=0; i < text.length(); i++)
107  {
108  c = text.at(i);
109  if ((c >= 'A') || (c <= 'Z'))
110  {
111  // Grossbuchstabe umwandeln
112  c += ('a' - 'A');
113  }
114  low += c;
115  }
116 
117  return low;
118 }
119 
120 
121 //
122 // Konvertiere eine Angabe in [m] in Fuesse und Inches, als Text.
123 // Ausgabeformat ist <<ff' ii">>
124 //
125 std::string convertMeterToFeetAndInch(double m)
126 {
127  std::ostringstream os;
128  std::string text;
129 
130  // Vorzeichen verarbeiten
131  if (m < 0.0)
132  {
133  os << "-";
134  m = -m;
135  }
136 
137  INT32 feet = (INT32)(m / 0.3048);
138  INT32 inch = (INT32)((m - ((double)feet * 0.3048)) / 0.0254);
139  if (feet > 0)
140  {
141  os << feet << "'";
142  }
143  if ((inch > 0) || (feet == 0))
144  {
145  os << inch << "\"";
146  }
147  text = os.str();
148 
149  // Ausgabe
150  return text;
151 }
152 
153 
154 
155 //
156 // String --> UINT16
157 //
158 UINT16 fromString(const std::string& text)
159 {
160  int value;
161  int conversions = sscanf(text.c_str(), "%d", &value);
162  if (conversions == 1)
163  {
164  return (UINT16)value;
165  }
166 
167  return 0;
168 }
169 
173 int hexCharToValue(char c)
174 {
175  int value = 0;
176 
177  if ((c >= '0') && (c <= '9'))
178  {
179  value = c - '0';
180  }
181  else if ((c >= 'A') && (c <= 'F'))
182  {
183  value = c - 'A' + 10;
184  }
185  else if ((c >= 'a') && (c <= 'f'))
186  {
187  value = c - 'a' + 10;
188  }
189 
190  return value;
191 }
192 
193 
197 char convertNibbleToHexChar(int value, bool useLowerCaseLetters)
198 {
199  char c;
200 
201  if (value < 10)
202  {
203  c = '0' + value;
204  }
205  else
206  {
207  if (useLowerCaseLetters == false)
208  {
209  // Grossbuchstaben
210  c = 'A' + value - 10;
211  }
212  else
213  {
214  // Kleinbuchstaben
215  c = 'a' + value - 10;
216  }
217  }
218 
219  return c;
220 }
221 
227 void convertUINT8toHexString(UINT8 byte, char* buffer)
228 {
229  UINT8 value = (byte >> 4);
230  buffer[0] = convertNibbleToHexChar(value);
231  value = byte & 0x0F;
232  buffer[1] = convertNibbleToHexChar(value);
233 }
234 
240 void convertRGBtoHexString(UINT8 r, UINT8 g, UINT8 b, char* buffer)
241 {
242  convertUINT8toHexString(r, buffer);
243  convertUINT8toHexString(g, &buffer[2]);
244  convertUINT8toHexString(b, &buffer[4]);
245 }
246 
247 
248 
252 double makeAngleValid(double angle)
253 {
254  const double twoPi = (2.0 * PI);
255 
256  while (angle >= PI)
257  {
258  angle -= twoPi;
259  }
260  while (angle < -PI)
261  {
262  angle += twoPi;
263  }
264 
265  return angle;
266 }
267 
271 std::string toString(INT32 value)
272 {
273  char c[16];
274  sprintf(c, "%i", value);
275  return (std::string(c));
276 }
277 
281 std::string toString(UINT32 value)
282 {
283  char c[16];
284  sprintf(c, "%i", value);
285  return (std::string(c));
286 }
287 
288 #if INTPTR_MAX != INT32_MAX
289 std::string toString(size_t value)
290 {
291  char c[16];
292  sprintf(c, "%zu", value);
293  return (std::string(c));
294 }
295 #endif
296 
297 /*
298  * Konvertiere Zahl in formatierten String.
299  * digits_before_decimal_point = 0..20
300  * digits_after_decimal_point = 0..20
301  *
302  * Der Ergebnisstring ist formatiert gem. den Vorgaben, d.h. er hat
303  * exakt die Laenge digits_before_decimal_point + digits_after_decimal_point + 1,
304  * ausser, die Vorkommazahl passte nicht in die Laengenvorgabe
305  * "digits_before_decimal_point", dann ist er entsprechend laenger.
306  */
307 std::string doubleToString(double val,
308  std::string::size_type digits_before_decimal_point,
309  std::string::size_type digits_after_decimal_point)
310 {
311  // Konvertierung in String
312  std::string text = doubleToString(val, digits_after_decimal_point);
313 
314  // Laengen festlegen: Zuerst vor dem Dezimalpunkt
315  const std::string::size_type dotPosition = text.find_first_of('.', 0);
316  if (dotPosition != std::string::npos)
317  {
318  // Punkt gefunden
319  if (dotPosition < digits_before_decimal_point)
320  {
321  // Zu kurz, also vorne auffuellen
322  std::string::size_type numExtraSpaces = digits_before_decimal_point - dotPosition;
323  text = std::string(numExtraSpaces, ' ') + text;
324  }
325  }
326 
327  // Gesamtlaenge pruefen und ggf. verlaengern. NOTE: This will
328  // never happen because the first doubleToString() will always fill
329  // up with zero at the end, doesn't it?
330  if (text.length() < (digits_before_decimal_point + digits_after_decimal_point + 1))
331  {
332  // Spaces hinten anfuegen
333  std::string::size_type numExtraSpaces =
334  (digits_before_decimal_point + digits_after_decimal_point + 1) - text.length();
335  text += std::string(numExtraSpaces, ' ');
336  }
337 
338  return text;
339 }
340 
344 std::string doubleToString(double val,
345  int digits_after_decimal_point)
346 {
347  // Konvertierung in String
348  std::stringstream sstr;
349  sstr << std::fixed << std::setprecision(digits_after_decimal_point) << val;
350 
351  return sstr.str();
352 }
353 
354 std::string toString(double val, int digits_after_decimal_point)
355 {
356  return doubleToString(val, digits_after_decimal_point);
357 }
358 
359 
360 //
361 // Konvertiere einen String in seine Adresse und seinen Port
362 //
363 // Beispiel: "192.168.0.1:1234" -> 0x0100A8C0
364 //
365 void stringToIpTarget(std::string ipAdrStr, UINT32& ipAddress, UINT16& port)
366 {
367  std::string addrStr;
368  std::string portStr;
369 
370  if (ipAdrStr.length() < 3)
371  {
372  // Ungueltig
373  return;
374  }
375 
376  UINT32 adrVal = INADDR_NONE;
377  UINT16 portVal = 0;
378 
379  // Port extrahieren
380  size_t pos = ipAdrStr.find_first_of(':');
381  if ((pos > 0) && (pos < (ipAdrStr.length() - 1)))
382  {
383  addrStr = ipAdrStr.substr(0, pos);
384  portStr = ipAdrStr.substr(pos+1);
385  }
386  else
387  {
388  addrStr = ipAdrStr;
389  }
390 
391  // Adresse
392  adrVal = (UINT32)inet_addr(addrStr.c_str()); // inet_addr("127.0.0.1");
393  ipAddress = adrVal;
394 
395  // Port
396  if (portStr.length() > 0)
397  {
398  portVal = fromString(portStr);
399  port = portVal;
400  }
401 }
402 
403 
404 //
405 // Konvertiere die IP-Adresse (IPv4) in einen String der Form a.b.c.d:port
406 //
407 std::string ipTargetToString(UINT32 ipAddress, UINT16 port)
408 {
409  std::string addr;
410  addr = ipAdrToString(ipAddress);
411 
412  // Port
413  addr += ":";
414  addr += toString((UINT16)port);
415 
416  return addr;
417 }
418 
419 
420 //
421 // Konvertiere die IP-Adresse (IPv4) in einen String der Form a.b.c.d.
422 // OHNE PORT!
423 //
424 std::string ipAdrToString(UINT32 ipAddress)
425 {
426  std::string addr;
427  addr = toString((UINT16)((ipAddress >> 0 ) & 0xFF)) + "." +
428  toString((UINT16)((ipAddress >> 8 ) & 0xFF)) + "." +
429  toString((UINT16)((ipAddress >> 16) & 0xFF)) + "." +
430  toString((UINT16)((ipAddress >> 24) & 0xFF));
431 
432  return addr;
433 }
434 
435 
436 //
437 // Read an UINT32 from a buffer. The buffer has the value in Big Endian format.
438 //
440 {
441  UINT32 value = (((UINT32)buffer[0]) << 24) +
442  (((UINT32)buffer[1]) << 16) +
443  (((UINT32)buffer[2]) << 8 ) +
444  (((UINT32)buffer[3]) );
445  buffer += 4;
446  return value;
447 }
448 
449 
450 //
451 // Read an UINT16 from a buffer. The buffer has the value in Big Endian format.
452 //
454 {
455  UINT16 value = (((UINT16)buffer[0]) << 8) +
456  ((UINT16)buffer[1]);
457  buffer += 2;
458  return value;
459 }
460 
461 //
462 // Read an UINT8 from a buffer.
463 //
465 {
466  UINT8 value = buffer[0];
467  buffer++;
468  return value;
469 }
470 
471 //
472 // Read an INT16 from a buffer. The buffer has the value in Big Endian format.
473 //
475 {
476  UINT16 value = (((UINT16)buffer[0]) << 8) +
477  ((UINT16)buffer[1]);
478  buffer += 2;
479  return (INT16)value;
480 }
481 
482 //
483 // Read an INT32 from a buffer. The buffer has the value in Big Endian format.
484 //
486 {
487  UINT32 value = (((UINT32)buffer[0]) << 24) +
488  (((UINT32)buffer[1]) << 16) +
489  (((UINT32)buffer[2]) << 8 ) +
490  (((UINT32)buffer[3]) );
491  buffer += 4;
492  return (INT32)value;
493 }
494 
495 //
496 // Read a string (with fixed length) from a buffer.
497 //
498 std::string memread_string(BYTE*& buffer, UINT16 length)
499 {
500  std::string text;
501 
502  for (UINT16 i = 0; i<length; i++)
503  {
504  text += buffer[i];
505  }
506  buffer += length;
507 
508  return text;
509 }
510 
511 
513 union FloatInt
514 {
515  float value_float;
517 };
518 
519 //
520 // Read a float value from a buffer. The buffer has the value in Big Endian format, so it
521 // cannot be read directly on x86 machines.
522 //
523 float memread_float(BYTE*& buffer)
524 {
525  FloatInt floatint;
526  floatint.value_int = memread_UINT32(buffer);
527  return floatint.value_float;
528 }
529 
530 //
531 // Write a float value to a buffer, and advance the buffer pointer.
532 // After writing, the buffer has the value in Big Endian format.
533 //
534 void memwrite_float(BYTE*& buffer, float value)
535 {
536  FloatInt floatint;
537  floatint.value_float = value;
538  memwrite_UINT32(buffer, floatint.value_int);
539 }
540 
541 //
542 // Write an INT32 to a buffer, and advance the buffer pointer.
543 // The buffer has the value in Big Endian format.
544 //
545 void memwrite_INT32(BYTE*& buffer, INT32 value)
546 {
547  memwrite_UINT32(buffer, (UINT32)value);
548 }
549 
550 //
551 // Write an UINT32 to a buffer, and advance the buffer pointer.
552 // The buffer has the value in Big Endian format.
553 //
554 void memwrite_UINT32(BYTE*& buffer, UINT32 value)
555 {
556  buffer[0] = ((value >> 24) & 0xFF);
557  buffer[1] = ((value >> 16) & 0xFF);
558  buffer[2] = ((value >> 8 ) & 0xFF);
559  buffer[3] = ((value ) & 0xFF);
560  buffer += 4;
561 }
562 
563 //
564 // Write an INT16 to a buffer, and advance the buffer pointer.
565 // The buffer has the value in Big Endian format.
566 //
567 void memwrite_INT16(BYTE*& buffer, INT16 value)
568 {
569  memwrite_UINT16(buffer, (UINT16)value);
570 }
571 
572 //
573 // Write an UINT16 to a buffer, and advance the buffer pointer.
574 // The buffer has the value in Big Endian format.
575 //
576 void memwrite_UINT16(BYTE*& buffer, UINT16 value)
577 {
578  buffer[0] = ((value >> 8 ) & 0xFF);
579  buffer[1] = ((value ) & 0xFF);
580  buffer += 2;
581 }
582 
583 //
584 // Write an UINT8 to a buffer, and advance the buffer pointer.
585 //
586 void memwrite_UINT8(BYTE*& buffer, UINT8 value)
587 {
588  buffer[0] = value;
589  buffer++;
590 }
591 
592 //
593 // Write an INT8 to a buffer, and advance the buffer pointer.
594 //
595 void memwrite_INT8(BYTE*& buffer, INT8 value)
596 {
597  buffer[0] = value;
598  buffer++;
599 }
600 
601 //
602 // Write a string to a buffer, and advance the buffer pointer.
603 //
604 void memwrite_string(BYTE*& buffer, std::string text)
605 {
606  strncpy((char*)buffer, text.c_str(), text.length());
607  buffer += text.length();
608 }
unsigned char BYTE
std::string convertMeterToFeetAndInch(double m)
Definition: toolbox.cpp:125
std::string toHexStringNibble(UINT8 val)
Definition: toolbox.cpp:52
std::string toHexString(UINT32 val)
Definition: toolbox.cpp:71
uint16_t UINT16
void memwrite_INT16(BYTE *&buffer, INT16 value)
Definition: toolbox.cpp:567
void traceBuffer(std::string headerText, BYTE *buffer, UINT32 len)
Definition: toolbox.cpp:20
void memwrite_UINT32(BYTE *&buffer, UINT32 value)
Definition: toolbox.cpp:554
Internal: Structure for reading/writing a float as an UINT32.
Definition: toolbox.cpp:513
std::string doubleToString(double val, std::string::size_type digits_before_decimal_point, std::string::size_type digits_after_decimal_point)
Definition: toolbox.cpp:307
#define printInfoMessage(a, b)
uint32_t UINT32
void convertUINT8toHexString(UINT8 byte, char *buffer)
Definition: toolbox.cpp:227
std::string ipTargetToString(UINT32 ipAddress, UINT16 port)
Definition: toolbox.cpp:407
int hexCharToValue(char c)
Definition: toolbox.cpp:173
float value_float
Definition: toolbox.cpp:515
void memwrite_float(BYTE *&buffer, float value)
Definition: toolbox.cpp:534
float memread_float(BYTE *&buffer)
Definition: toolbox.cpp:523
void memwrite_INT8(BYTE *&buffer, INT8 value)
Definition: toolbox.cpp:595
UINT8 memread_UINT8(BYTE *&buffer)
Definition: toolbox.cpp:464
std::string toLower(const std::string &text)
Definition: toolbox.cpp:101
double makeAngleValid(double angle)
Definition: toolbox.cpp:252
std::string ipAdrToString(UINT32 ipAddress)
Definition: toolbox.cpp:424
INT32 memread_INT32(BYTE *&buffer)
Definition: toolbox.cpp:485
void memwrite_string(BYTE *&buffer, std::string text)
Definition: toolbox.cpp:604
int32_t INT32
void memwrite_UINT16(BYTE *&buffer, UINT16 value)
Definition: toolbox.cpp:576
UINT16 fromString(const std::string &text)
Definition: toolbox.cpp:158
void convertRGBtoHexString(UINT8 r, UINT8 g, UINT8 b, char *buffer)
Definition: toolbox.cpp:240
std::string toString(INT32 value)
Definition: toolbox.cpp:271
void memwrite_UINT8(BYTE *&buffer, UINT8 value)
Definition: toolbox.cpp:586
int8_t INT8
UINT32 memread_UINT32(BYTE *&buffer)
Definition: toolbox.cpp:439
int16_t INT16
void stringToIpTarget(std::string ipAdrStr, UINT32 &ipAddress, UINT16 &port)
Definition: toolbox.cpp:365
UINT16 memread_UINT16(BYTE *&buffer)
Definition: toolbox.cpp:453
UINT32 value_int
Definition: toolbox.cpp:516
#define PI
void memwrite_INT32(BYTE *&buffer, INT32 value)
Definition: toolbox.cpp:545
std::string memread_string(BYTE *&buffer, UINT16 length)
Definition: toolbox.cpp:498
uint8_t UINT8
INT16 memread_INT16(BYTE *&buffer)
Definition: toolbox.cpp:474
char convertNibbleToHexChar(int value, bool useLowerCaseLetters)
Definition: toolbox.cpp:197


libsick_ldmrs
Author(s): SICK AG , Martin Günther , Jochen Sprickerhof
autogenerated on Sat Jun 8 2019 17:57:33