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


sick_scan
Author(s): Michael Lehning , Jochen Sprickerhof , Martin Günther
autogenerated on Thu Sep 8 2022 02:30:19