sick_generic_imu.cpp
Go to the documentation of this file.
1 
45 #ifdef _MSC_VER
46 #define _WIN32_WINNT 0x0501
47 #pragma warning(disable: 4996)
48 #pragma warning(disable: 4267)
49 #endif
50 
51 #ifndef _MSC_VER
52 
53 
54 #endif
55 
60 
61 #ifdef _MSC_VER
62 #include "sick_scan/rosconsole_simu.hpp"
63 #endif
64 
65 // #include <tf/tf.h>
66 #include <geometry_msgs/Pose2D.h>
67 
68 #include "sensor_msgs/Imu.h"
69 
70 #define _USE_MATH_DEFINES
71 
72 #include <math.h>
73 #include "string"
74 #include <stdio.h>
75 #include <stdlib.h>
76 
77 #ifdef DEBUG_DUMP_ENABLED
78 
79 #include "sick_scan/dataDumper.h"
80 
81 #endif
82 namespace sick_scan
83 {
84 
85  bool SickScanImu::isImuDatagram(char *datagram, size_t datagram_length)
86  {
87  bool ret = false;
88  if (this->isImuBinaryDatagram(datagram, datagram_length))
89  {
90  ret = true;
91  }
92  else
93  {
94  if (this->isImuAsciiDatagram(datagram, datagram_length))
95  {
96  ret = true;
97  }
98  else
99  {
100  if (this->isImuAckDatagram(datagram, datagram_length))
101  {
102  ret = true;
103  }
104  }
105  }
106 
107  return (ret);
108  }
109 
110 
117  double SickScanImu::simpleFmodTwoPi(double angle)
118  {
119  while (angle < M_PI)
120  {
121  angle += 2 * M_PI;
122  }
123  while (angle > M_PI)
124  {
125  angle -= 2 * M_PI;
126  }
127  return (angle);
128  }
129 
130  bool SickScanImu::isImuAckDatagram(char *datagram, size_t datagram_length)
131  {
132  bool isImuMsg = false;
133  std::string szKeyWord = "sEA InertialMeasurementUnit";
134  std::string cmpKeyWord = "";
135  int keyWordLen = szKeyWord.length();
136  int posTrial[] = {0, 1, 8};
137  for (int iPos = 0; iPos < sizeof(posTrial) / sizeof(posTrial[0]); iPos++)
138 
139  if (datagram_length >= (keyWordLen + posTrial[iPos])) // 8 Bytes preheader
140  {
141  cmpKeyWord = "";
142  for (int i = 0; i < keyWordLen; i++)
143  {
144  cmpKeyWord += datagram[i + posTrial[iPos]];
145  }
146  }
147  if (szKeyWord.compare(cmpKeyWord) == 0)
148  {
149  isImuMsg = true;
150  }
151  return (isImuMsg);
152  }
153 
160  bool SickScanImu::isImuBinaryDatagram(char *datagram, size_t datagram_length)
161  {
162  /*
163  * sSN InertialM
164 06b0 65 61 73 75 72 65 6d 65 6e 74 55 6e 69 74 20 be easurementUnit .
165 06c0 9d 86 2d bb 9c e9 44 41 1c 33 d6 bb 0b a1 6f 00 ..-...DA.3....o.
166 06d0 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 ................
167 06e0 00 00 00 3f 7f ec 00 3a 60 00 00 3c cd 00 00 39 ...?...:`..<...9
168 06f0 a0 00 00 00 00 00 02 1c 7e 93 a8 23
169  */
170  bool isImuMsg = false;
171  std::string szKeyWord = "sSN InertialMeasurementUnit";
172  std::string cmpKeyWord = "";
173  int keyWordLen = szKeyWord.length();
174  if (datagram_length >= (keyWordLen + 8)) // 8 Bytes preheader
175  {
176  for (int i = 0; i < keyWordLen; i++)
177  {
178  cmpKeyWord += datagram[i + 8];
179  }
180  }
181  if (szKeyWord.compare(cmpKeyWord) == 0)
182  {
183  isImuMsg = true;
184  }
185  else
186  {
187  isImuMsg = false;
188  }
189  return (isImuMsg);
190  }
191 
197  int SickScanImu::parseBinaryDatagram(char *datagram, size_t datagram_length, SickScanImuValue *imuValue)
198  {
199  static int cnt = 0;
200  cnt++;
201  int iRet = 0;
202  float tmpArr[13] = {0};
203  uint64_t timeStamp;
204  unsigned char *receiveBuffer = (unsigned char *) datagram;
205  if (false == isImuBinaryDatagram(datagram, datagram_length))
206  {
207  return (-1);
208  }
209 
210  unsigned char *bufPtr = (unsigned char *) datagram;
211 #ifdef DEBUG_DUMP_TO_CONSOLE_ENABLED
212  DataDumper::instance().dumpUcharBufferToConsole((unsigned char*)datagram, datagram_length);
213 #endif
214  memcpy(&timeStamp, receiveBuffer + 36 + 13 * 4, 4);
215  swap_endian((unsigned char *) &timeStamp, 4);
216  int adrOffset = 36;
217  for (int i = 0; i < 13; i++)
218  {
219  memcpy(&(tmpArr[i]), receiveBuffer + adrOffset, 4);
220  swap_endian((unsigned char *) (&(tmpArr[i])), 4);
221  adrOffset += 4;
222 
223 // if ((cnt % 10) == 0)
224 // {
225 // if (i == 0)
226 // {
227 // printf("===================\n");
228 // }
229 // printf("%2d: %8.6f\n", i, tmpArr[i]);
230 // }
231  }
232 
233  imuValue->LinearAccelerationX(tmpArr[0]);
234  imuValue->LinearAccelerationY(tmpArr[1]);
235  imuValue->LinearAccelerationZ(tmpArr[2]);
236 
237 
238  double angleVelMultiplier = 1.0;
239  imuValue->AngularVelocityX(angleVelMultiplier * tmpArr[3]);
240  imuValue->AngularVelocityY(angleVelMultiplier * tmpArr[4]);
241  imuValue->AngularVelocityZ(angleVelMultiplier * tmpArr[5]);
242 
243  imuValue->TimeStamp(timeStamp);
244 
245 
246  imuValue->QuaternionW(tmpArr[9]); // w is first element
247  imuValue->QuaternionX(tmpArr[10]);
248  imuValue->QuaternionY(tmpArr[11]);
249  imuValue->QuaternionZ(tmpArr[12]);
250 
251  imuValue->QuaternionAccuracy(0.0); // not used tmpArr[4]);
252 
253 
254  uint8_t val = 0x00;
255  imuValue->AngularVelocityReliability((UINT16) val);
256  imuValue->LinearAccelerationReliability((UINT16) val);
257  return (iRet);
258  }
259 
266  bool SickScanImu::isImuAsciiDatagram(char *datagram, size_t datagram_length)
267  {
268  bool isImuMsg = false;
269  std::string szKeyWord = "sSN InertialMeasurementUnit";
270  int keyWordLen = szKeyWord.length();
271  if (datagram_length >= keyWordLen)
272  {
273 
274  char *ptr = NULL;
275  ptr = strstr(datagram, szKeyWord.c_str());
276  if (ptr != NULL)
277  {
278  int offset = ptr - datagram;
279  if ((offset == 0) || (offset == 1)) // should work with 0x02 and without 0x02
280  {
281  isImuMsg = true;
282  }
283  }
284  }
285  return (isImuMsg);
286  }
287 
293  int SickScanImu::parseAsciiDatagram(char *datagram, size_t datagram_length, SickScanImuValue *imuValue)
294  {
295  int exitCode = ExitSuccess;
296  bool dumpData = false;
297  int verboseLevel = 0;
298 
299  // !!!!!
300  // verboseLevel = 1;
301  int HEADER_FIELDS = 32;
302  char *cur_field;
303  size_t count;
304 
305  // Reserve sufficient space
306  std::vector<char *> fields;
307  fields.reserve(datagram_length / 2);
308 
309  // ----- only for debug output
310  std::vector<char> datagram_copy_vec;
311  datagram_copy_vec.resize(datagram_length + 1); // to avoid using malloc. destructor frees allocated mem.
312  char *datagram_copy = &(datagram_copy_vec[0]);
313 
314  if (verboseLevel > 0)
315  {
316  ROS_WARN("Verbose LEVEL activated. Only for DEBUG.");
317  }
318 
319 
320  strncpy(datagram_copy, datagram, datagram_length); // datagram will be changed by strtok
321  datagram_copy[datagram_length] = 0;
322 
323  // ----- tokenize
324  count = 0;
325  cur_field = strtok(datagram, " ");
326 
327  while (cur_field != NULL)
328  {
329  fields.push_back(cur_field);
330  //std::cout << cur_field << std::endl;
331  cur_field = strtok(NULL, " ");
332  }
333 
334  //std::cout << fields[27] << std::endl;
335 
336  count = fields.size();
337 
338  enum IMU_TOKEN_SEQ // see specification
339  {
340  IMU_TOKEN_SSN, // 0: sSN
341  IMU_TOKEN_IMUDATA, // 1: LMDradardata
342  IMU_TOKEN_TIMESTAMP, // unsigned long value timestamp
343  IMU_TOKEN_QUATERNION_W,
344  IMU_TOKEN_QUATERNION_X,
345  IMU_TOKEN_QUATERNION_Y,
346  IMU_TOKEN_QUATERNION_Z,
347  IMU_TOKEN_QUATERNION_ACCURACY, // float value
348  IMU_TOKEN_ANGULAR_VELOCITY_X,
349  IMU_TOKEN_ANGULAR_VELOCITY_Y,
350  IMU_TOKEN_ANGULAR_VELOCITY_Z,
351  IMU_TOKEN_ANGULAR_VELOCITY_RELIABILITY, // int value
352  IMU_TOKEN_LINEAR_ACCELERATION_X,
353  IMU_TOKEN_LINEAR_ACCELERATION_Y,
354  IMU_TOKEN_LINEAR_ACCELERATION_Z,
355  IMU_TOKEN_LINEAR_ACCELERATION_RELIABILITY, // int value
356  IMU_TOKEN_NUM
357  };
358  for (int i = 0; i < IMU_TOKEN_NUM; i++)
359  {
360  UINT16 uiValue = 0x00;
361  UINT32 udiValue = 0x00;
362  unsigned long int uliDummy;
363  uliDummy = strtoul(fields[i], NULL, 16);
364  float floatDummy;
365  switch (i)
366  {
367  case IMU_TOKEN_TIMESTAMP:
368  imuValue->TimeStamp(uliDummy);
369  break;
370  case IMU_TOKEN_QUATERNION_X:
371  memcpy(&floatDummy, &uliDummy, sizeof(float));
372  imuValue->QuaternionX(floatDummy); // following IEEE 754 float convention
373  break;
374  case IMU_TOKEN_QUATERNION_Y:
375  memcpy(&floatDummy, &uliDummy, sizeof(float));
376  imuValue->QuaternionY(floatDummy);
377  break;
378  case IMU_TOKEN_QUATERNION_Z:
379  memcpy(&floatDummy, &uliDummy, sizeof(float));
380  imuValue->QuaternionZ(floatDummy);
381  break;
382  case IMU_TOKEN_QUATERNION_W:
383  memcpy(&floatDummy, &uliDummy, sizeof(float));
384  imuValue->QuaternionW(floatDummy);
385  break;
386 
387  case IMU_TOKEN_QUATERNION_ACCURACY: // float value
388  memcpy(&floatDummy, &uliDummy, sizeof(float));
389  imuValue->QuaternionAccuracy(floatDummy);
390  break;
391  case IMU_TOKEN_ANGULAR_VELOCITY_X:
392  memcpy(&floatDummy, &uliDummy, sizeof(float));
393  imuValue->AngularVelocityX(floatDummy);
394  break;
395  case IMU_TOKEN_ANGULAR_VELOCITY_Y:
396  memcpy(&floatDummy, &uliDummy, sizeof(float));
397  imuValue->AngularVelocityY(floatDummy);
398  break;
399  case IMU_TOKEN_ANGULAR_VELOCITY_Z:
400  memcpy(&floatDummy, &uliDummy, sizeof(float));
401  imuValue->AngularVelocityZ(floatDummy);
402  break;
403  case IMU_TOKEN_ANGULAR_VELOCITY_RELIABILITY:
404  uiValue = (UINT16) (0xFFFF & uliDummy);
405  imuValue->AngularVelocityReliability(
406  uiValue); // per definition is a 8 bit value, but we use a 16 bit value
407  break;
408  case IMU_TOKEN_LINEAR_ACCELERATION_X:
409  memcpy(&floatDummy, &uliDummy, sizeof(float));
410  imuValue->LinearAccelerationX(floatDummy);
411  break;
412  case IMU_TOKEN_LINEAR_ACCELERATION_Y:
413  memcpy(&floatDummy, &uliDummy, sizeof(float));
414  imuValue->LinearAccelerationY(floatDummy);
415  break;
416  case IMU_TOKEN_LINEAR_ACCELERATION_Z:
417  memcpy(&floatDummy, &uliDummy, sizeof(float));
418  imuValue->LinearAccelerationZ(floatDummy);
419  break;
420  case IMU_TOKEN_LINEAR_ACCELERATION_RELIABILITY:
421  uiValue = (UINT16) (0xFFFF & uliDummy);
423  uiValue); // per definition is a 8 bit value, but we use a 16 bit value
424  break;
425 
426  }
427  }
428  return (0);
429  }
430 
438  {
439  // Quaterions derived from https://quaternions.online/
440  double wxyz_test_vec[] = { 0.723, 0.440, 0.022, -0.532}; // corresponds to rpy 45,30,-60 in zyx-Order
441  double rad2deg = 180.0/M_PI;
442 
443 #if 0
444  tf::Quaternion qOrientation(
445  wxyz_test_vec[1], // x
446  wxyz_test_vec[2], // y
447  wxyz_test_vec[3], // z
448  wxyz_test_vec[0]); // w
449 
450  tf::Matrix3x3 m(qOrientation);
451  double roll, pitch, yaw;
452  m.getRPY(roll, pitch, yaw); // convert to roll pitch yaw and try to derive Angular Velocity from these values
453 
454  printf("Test result\n");
455  printf("Roll 45.054 [deg]: %8.3lf [deg]\n", roll * rad2deg);
456  printf("Pitch: 30.004 [deg]: %8.3lf [deg]\n", pitch * rad2deg);
457  printf("Yaw: -60.008 [deg]: %8.3lf [deg]\n", yaw * rad2deg);
458 #endif
459 
460  struct Quaternion {
461  double w, x, y, z;
462  };
463 
464  struct EulerAngles {
465  double roll, pitch, yaw;
466  };
467 
468  Quaternion q;
469  q.x = wxyz_test_vec[1];
470  q.y = wxyz_test_vec[2];
471  q.z = wxyz_test_vec[3];
472  q.w = wxyz_test_vec[0];
473 
474  EulerAngles angles;
475 
476 
477  // roll (x-axis rotation)
478  double sinr_cosp = 2 * (q.w * q.x + q.y * q.z);
479  double cosr_cosp = 1 - 2 * (q.x * q.x + q.y * q.y);
480  angles.roll = std::atan2(sinr_cosp, cosr_cosp);
481 
482  // pitch (y-axis rotation)
483  double sinp = 2 * (q.w * q.y - q.z * q.x);
484  if (std::abs(sinp) >= 1)
485  angles.pitch = std::copysign(M_PI / 2, sinp); // use 90 degrees if out of range
486  else
487  angles.pitch = std::asin(sinp);
488 
489  // yaw (z-axis rotation)
490  double siny_cosp = 2 * (q.w * q.z + q.x * q.y);
491  double cosy_cosp = 1 - 2 * (q.y * q.y + q.z * q.z);
492  angles.yaw = std::atan2(siny_cosp, cosy_cosp);
493 
494  printf("Test result (should be similiar to the result above)\n");
495  printf("Roll 45 [deg]: %8.3lf [deg]\n", angles.roll * rad2deg);
496  printf("Pitch: 30 [deg]: %8.3lf [deg]\n", angles.pitch * rad2deg);
497  printf("Yaw: -60 [deg]: %8.3lf [deg]\n", angles.yaw * rad2deg);
498 
499  }
501  {
502  sick_scan::SickScanImu scanImu(NULL);
504  // checked with online converter
505  // https://www.h-schmidt.net/FloatConverter/IEEE754de.html
506  // 55570143 0.9998779 -0.0057373047 0.016174316 0.0 0.0 0.002130192 -0.31136206 -0.10777917 9.823472
507  std::string imuTestStr = "sSN IMUData 34FEEDF 3F7FF800 BBBC0000 3C848000 00000000 00000000 00000000 3B0B9AB1 00000000 3 BE9F6AD9 BDDCBB53 411D2CF1 0";
508  const char imuTestBinStr[] =
509 
510 /*
511  * 02 02 02 02 00 00 ................
512  0640 00 58
513  73 53 4e 20 49 6e 65 72 74 69 61 6c 4d 65 .XsSN InertialMe
514  0650 61 73 75 72 65 6d 65 6e 74 55 6e 69 74 20 be a4 asurementUnit ..
515  0660 e1 1c 3b 6b 5d e5 41 1c 6e ad bb 0b a1 6f bb 0b ..;k].A.n....o..
516  0670 a1 6f 00 00 00 00 00 00 00 00 00 00 00 00 00 00 .o..............
517  0680 00 00 3f 7f ec 00 3a 60 00 00 3c cd 00 00 39 a0 ..?...:`..<...9.
518  0690 00 00 00 00 00 02 1c 7e 6c 01 20
519 
520  56 Data Bytes + 2 Byte Timestamp + CRC
521  14 Float + 4 Byte CRC
522 
523  3 Acceleration = 12 Bytes
524  3 AngularVelocity = 12
525  Magnetic Field = 12 Bytes
526  Orientatoin = 16 Bytes
527  TimeStamp = 4
528  Sum: 12 + 12 + 12 + 16 + 4 = 56 Bytes
529 
530 */
531  "\x02\x02\x02\x02\x00\x00\x00\x58" // 8 Byte Header
532  "\x73\x53\x4e\x20\x49\x6e\x65\x72\x74\x69\x61\x6c\x4d\x65" // Keyword
533  "\x61\x73\x75\x72\x65\x6d\x65\x6e\x74\x55\x6e\x69\x74\x20\xbe\xa4"
534  "\xe1\x1c\x3b\x6b\x5d\xe5\x41\x1c\x6e\xad\xbb\x0b\xa1\x6f\xbb\x0b"
535  "\xa1\x6f\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00"
536  "\x00\x00\x3f\x7f\xec\x00\x3a\x60\x00\x00\x3c\xcd\x00\x00\x39\xa0"
537  "\x00\x00\x00\x00\x00\x02\x1c\x7e\x6c\x01\x20";
538 
539  char *datagramPtr = (char *) imuTestStr.c_str();
540  int datagramLen = imuTestStr.size();
541 
542  if (scanImu.isImuAsciiDatagram(datagramPtr, datagramLen))
543  {
544  scanImu.parseAsciiDatagram(datagramPtr, datagramLen, &imuValue);
545  }
546 
547  datagramPtr = (char *) imuTestBinStr;
548  datagramLen = sizeof(imuTestBinStr) / sizeof(imuTestBinStr[0]);
549 
550  if (scanImu.isImuBinaryDatagram(datagramPtr, datagramLen))
551  {
552  scanImu.parseBinaryDatagram(datagramPtr, datagramLen, &imuValue);
553  }
554 
555  }
556 
557  int SickScanImu::parseDatagram(ros::Time timeStamp, unsigned char *receiveBuffer, int actual_length,
558  bool useBinaryProtocol)
559  {
560  int exitCode = ExitSuccess;
561 
562  SickScanImuValue imuValue;
563  sensor_msgs::Imu imuMsg_;
564  static ros::Time lastTimeStamp;
565 
566  static double lastRoll = 0.0;
567  static double lastPitch = 0.0;
568  static double lastYaw = 0.0;
569 
570  static bool firstTime = true;
571  /*
572  static int cnt = 0;
573  static u_int32_t timeStampSecBuffer[1000];
574  static u_int32_t timeStampNanoSecBuffer[1000];
575  static u_int32_t timeStampSecCorBuffer[1000];
576  static u_int32_t timeStampNanoSecCorBuffer[1000];
577  static u_int32_t imuTimeStamp[1000];
578  static int timeStampValid[1000];
579 
580  int idx = cnt % 1000;
581  cnt++;
582 
583  int dumpIdx = 500;
584  if (cnt == dumpIdx)
585  {
586  printf("TICK;TS_SEC;TS_NANO;TS;TS_CORR_SEC;TS_CORR_NANO;TS_CORR;TS_DIFF;TS_VALID\n");
587 
588  for (int i = 0; i < dumpIdx; i++)
589  {
590  double tsDouble = timeStampSecBuffer[i] + 1E-9 * timeStampNanoSecBuffer[i];
591  double tsDoubleCorr = timeStampSecCorBuffer[i] + 1E-9 * timeStampNanoSecCorBuffer[i];
592  double tsDiff = tsDouble - tsDoubleCorr;
593  printf("%10u;%10u;%10u;%16.8lf;%10u;%10u;%16.8lf;%16.8lf;%d\n",
594  imuTimeStamp[i],
595  timeStampSecBuffer[i], timeStampNanoSecBuffer[i], tsDouble,
596  timeStampSecCorBuffer[i], timeStampNanoSecCorBuffer[i], tsDoubleCorr, tsDiff, timeStampValid[i]);
597  }
598  }
599  */
600  if (useBinaryProtocol)
601  {
602  this->parseBinaryDatagram((char *) receiveBuffer, actual_length, &imuValue);
603 
604  }
605  else
606  {
607  this->parseAsciiDatagram((char *) receiveBuffer, actual_length, &imuValue);
608  }
609  /*
610  timeStampSecBuffer[idx] = timeStamp.sec;
611  timeStampNanoSecBuffer[idx] = timeStamp.nsec;
612  imuTimeStamp[idx] = imuValue.TimeStamp();
613 */
615  (uint32_t) (imuValue.TimeStamp() & 0xFFFFFFFF));
616  /*
617  timeStampSecCorBuffer[idx] = timeStamp.sec;
618  timeStampNanoSecCorBuffer[idx] = timeStamp.nsec;
619  timeStampValid[idx] = bRet ? 1 : 0;
620  */
621  imuMsg_.header.stamp = timeStamp;
622  imuMsg_.header.seq = 0;
623  imuMsg_.header.frame_id = commonPtr->config_.imu_frame_id; //
624 
625 
626 
627  imuMsg_.orientation.x = imuValue.QuaternionX();
628  imuMsg_.orientation.y = imuValue.QuaternionY();
629  imuMsg_.orientation.z = imuValue.QuaternionZ();
630  imuMsg_.orientation.w = imuValue.QuaternionW();
631  imuMsg_.orientation_covariance[0] = 1.0;
632 
633  double roll, pitch, yaw;
634 
635 #if 0
636  // due to problems compile tf.h against the bloom server we implent a bare bone conversion from
637  // quaternion to euler following https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
638  // include tf.h for testing it.
639  tf::Quaternion qOrientation(
640  imuMsg_.orientation.x,
641  imuMsg_.orientation.y,
642  imuMsg_.orientation.z,
643  imuMsg_.orientation.w);
644  tf::Matrix3x3 m(qOrientation);
645  m.getRPY(roll, pitch, yaw); // convert to roll pitch yaw and try to derive Angular Velocity from these values
646 #else // barebone implementation without include tf.h
647  {
648  struct Quaternion {
649  double w, x, y, z;
650  };
651 
652  struct EulerAngles {
653  double roll, pitch, yaw;
654  };
655 
656  Quaternion q;
657  q.x = imuMsg_.orientation.x;
658  q.y = imuMsg_.orientation.y;
659  q.z = imuMsg_.orientation.z;
660  q.w = imuMsg_.orientation.w;
661 
662  EulerAngles angles;
663 
664 
665  // roll (x-axis rotation)
666  double sinr_cosp = 2 * (q.w * q.x + q.y * q.z);
667  double cosr_cosp = 1 - 2 * (q.x * q.x + q.y * q.y);
668  angles.roll = std::atan2(sinr_cosp, cosr_cosp);
669 
670  // pitch (y-axis rotation)
671  double sinp = 2 * (q.w * q.y - q.z * q.x);
672  if (std::abs(sinp) >= 1)
673  angles.pitch = std::copysign(M_PI / 2, sinp); // use 90 degrees if out of range
674  else
675  angles.pitch = std::asin(sinp);
676 
677  // yaw (z-axis rotation)
678  double siny_cosp = 2 * (q.w * q.z + q.x * q.y);
679  double cosy_cosp = 1 - 2 * (q.y * q.y + q.z * q.z);
680  angles.yaw = std::atan2(siny_cosp, cosy_cosp);
681 
682  roll = angles.roll;
683  pitch = angles.pitch;
684  yaw = angles.yaw;
685  }
686 #endif
687  imuMsg_.angular_velocity.x = imuValue.AngularVelocityX();
688  imuMsg_.angular_velocity.y = imuValue.AngularVelocityY();
689  imuMsg_.angular_velocity.z = imuValue.AngularVelocityZ();
690 #if 0
691  if (firstTime)
692  {
693  lastTimeStamp = timeStamp;
694  firstTime = false;
695  }
696  else
697  {
698 
699 #ifdef DEBUG_DUMP_ENABLED
700  /*
701  float LinearAccelerationX() const { return linearAccelerationX; }
702  void LinearAccelerationX(float val) { linearAccelerationX = val; }
703  float LinearAccelerationY() const { return linearAccelerationY; }
704  void LinearAccelerationY(float val) { linearAccelerationY = val; }
705  float LinearAccelerationZ() const { return linearAccelerationZ; }
706  void LinearAccelerationZ(float val) { linearAccelerationZ = val; }
707  */
708  DataDumper::instance().pushData((double)imuValue.TimeStamp(), "ACCX", imuValue.LinearAccelerationX());
709  DataDumper::instance().pushData((double)imuValue.TimeStamp(), "ACCY", imuValue.LinearAccelerationY());
710  DataDumper::instance().pushData((double)imuValue.TimeStamp(), "ACCZ", imuValue.LinearAccelerationZ());
711 #endif
712  /*
713  * The built-in IMU unit provides three parameter sets:
714  * quaternions, angular velocity and linear accelerations.
715  * The angular velocities give noisy data without offset compensation (i.e. with drift).
716  * For this reason, the angular velocities are derived from the quaternions. The quaternions are converted
717  * into Euler angles for this purpose. The angular velocity is then calculated from these
718  * sequential Euler angles.
719  */
720  ros::Duration timeDiffRos = timeStamp - lastTimeStamp;
721  double timeDiff = timeDiffRos.toSec();
722  if (timeDiff > 1E-6) // Epsilon-Check
723  {
724  double angleDiffX = roll - lastRoll;
725  double angleDiffY = pitch - lastPitch;
726  double angleDiffZ = yaw - lastYaw;
727  angleDiffX = simpleFmodTwoPi(angleDiffX);
728  angleDiffY = simpleFmodTwoPi(angleDiffY);
729  angleDiffZ = simpleFmodTwoPi(angleDiffZ);
730  double angleAngleApproxX = angleDiffX / timeDiff;
731  double angleAngleApproxY = angleDiffY / timeDiff;
732  double angleAngleApproxZ = angleDiffZ / timeDiff;
733 
734  imuMsg_.angular_velocity.x = angleAngleApproxX;
735  imuMsg_.angular_velocity.y = angleAngleApproxY;
736  imuMsg_.angular_velocity.z = angleAngleApproxZ;
737 
738  }
739  }
740  lastTimeStamp = timeStamp;
741  lastRoll = roll;
742  lastPitch = pitch;
743  lastYaw = yaw;
744 #endif
745  imuMsg_.linear_acceleration.x = imuValue.LinearAccelerationX();
746  imuMsg_.linear_acceleration.y = imuValue.LinearAccelerationY();
747  imuMsg_.linear_acceleration.z = imuValue.LinearAccelerationZ();
748  // setting main diagonal elements of covariance matrix
749  // to some meaningful values.
750  // see https://github.com/ROBOTIS-GIT/OpenCR/blob/master/arduino/opencr_arduino/opencr/libraries/ROS/examples/01.%20Basics/d_IMU/d_IMU.ino
751  // as a reference.
752 
753 
754 
755  for (int i = 0; i < 9; i++)
756  {
757  imuMsg_.angular_velocity_covariance[i] = 0.00;
758  imuMsg_.linear_acceleration_covariance[i] = 0.00;
759  imuMsg_.orientation_covariance[i] = 0.00;
760 
761  }
762 
763  if (commonPtr->config_.cloud_output_mode == 2)
764  {
765  imuMsg_.angular_velocity_covariance[0] = 0.02;
766  imuMsg_.angular_velocity_covariance[1] = 0;
767  imuMsg_.angular_velocity_covariance[2] = 0;
768  imuMsg_.angular_velocity_covariance[3] = 0;
769  imuMsg_.angular_velocity_covariance[4] = 0.02;
770  imuMsg_.angular_velocity_covariance[5] = 0;
771  imuMsg_.angular_velocity_covariance[6] = 0;
772  imuMsg_.angular_velocity_covariance[7] = 0;
773  imuMsg_.angular_velocity_covariance[8] = 0.02;
774 
775  imuMsg_.linear_acceleration_covariance[0] = 0.04;
776  imuMsg_.linear_acceleration_covariance[1] = 0;
777  imuMsg_.linear_acceleration_covariance[2] = 0;
778  imuMsg_.linear_acceleration_covariance[3] = 0;
779  imuMsg_.linear_acceleration_covariance[4] = 0.04;
780  imuMsg_.linear_acceleration_covariance[5] = 0;
781  imuMsg_.linear_acceleration_covariance[6] = 0;
782  imuMsg_.linear_acceleration_covariance[7] = 0;
783  imuMsg_.linear_acceleration_covariance[8] = 0.04;
784 
785  imuMsg_.orientation_covariance[0] = 0.0025;
786  imuMsg_.orientation_covariance[1] = 0;
787  imuMsg_.orientation_covariance[2] = 0;
788  imuMsg_.orientation_covariance[3] = 0;
789  imuMsg_.orientation_covariance[4] = 0.0025;
790  imuMsg_.orientation_covariance[5] = 0;
791  imuMsg_.orientation_covariance[6] = 0;
792  imuMsg_.orientation_covariance[7] = 0;
793  imuMsg_.orientation_covariance[8] = 0.0025;
794  }
795  else
796  {
797  imuMsg_.angular_velocity_covariance[0] = 0;
798  imuMsg_.angular_velocity_covariance[1] = 0;
799  imuMsg_.angular_velocity_covariance[2] = 0;
800  imuMsg_.angular_velocity_covariance[3] = 0;
801  imuMsg_.angular_velocity_covariance[4] = 0;
802  imuMsg_.angular_velocity_covariance[5] = 0;
803  imuMsg_.angular_velocity_covariance[6] = 0;
804  imuMsg_.angular_velocity_covariance[7] = 0;
805  imuMsg_.angular_velocity_covariance[8] = 0;
806 
807  imuMsg_.linear_acceleration_covariance[0] = 0;
808  imuMsg_.linear_acceleration_covariance[1] = 0;
809  imuMsg_.linear_acceleration_covariance[2] = 0;
810  imuMsg_.linear_acceleration_covariance[3] = 0;
811  imuMsg_.linear_acceleration_covariance[4] = 0;
812  imuMsg_.linear_acceleration_covariance[5] = 0;
813  imuMsg_.linear_acceleration_covariance[6] = 0;
814  imuMsg_.linear_acceleration_covariance[7] = 0;
815  imuMsg_.linear_acceleration_covariance[8] = 0;
816 
817  imuMsg_.orientation_covariance[0] = 0;
818  imuMsg_.orientation_covariance[1] = 0;
819  imuMsg_.orientation_covariance[2] = 0;
820  imuMsg_.orientation_covariance[3] = 0;
821  imuMsg_.orientation_covariance[4] = 0;
822  imuMsg_.orientation_covariance[5] = 0;
823  imuMsg_.orientation_covariance[6] = 0;
824  imuMsg_.orientation_covariance[7] = 0;
825  imuMsg_.orientation_covariance[8] = 0;
826 
827  }
828  if (true == bRet)
829  {
830  this->commonPtr->imuScan_pub_.publish(imuMsg_);
831  }
832  return (exitCode);
833 
834  }
835 }
836 
tf::Matrix3x3
UINT16
uint16_t UINT16
Definition: BasicDatatypes.hpp:27
sick_scan::SickScanImu::imuParserTest
static void imuParserTest()
Definition: sick_generic_imu.cpp:500
sick_scan::SickScanImuValue::QuaternionAccuracy
float QuaternionAccuracy() const
Definition: sick_generic_imu.h:111
swap_endian
void swap_endian(unsigned char *ptr, int numBytes)
Universal swapping function.
Definition: sick_scan_common.cpp:102
angles
angle
TFSIMD_FORCE_INLINE tfScalar angle(const Quaternion &q1, const Quaternion &q2)
sick_scan::SickScanImu::isImuDatagram
bool isImuDatagram(char *datagram, size_t datagram_length)
Definition: sick_generic_imu.cpp:85
tf::Matrix3x3::getRPY
void getRPY(tfScalar &roll, tfScalar &pitch, tfScalar &yaw, unsigned int solution_number=1) const
rad2deg
#define rad2deg(x)
Definition: sick_scan_common.cpp:84
DataDumper::dumpUcharBufferToConsole
int dumpUcharBufferToConsole(unsigned char *buffer, int bufLen)
Definition: dataDumper.cpp:67
DataDumper::instance
static DataDumper & instance()
Definition: dataDumper.h:13
sick_scan::ExitSuccess
@ ExitSuccess
Definition: abstract_parser.h:46
sick_scan::SickScanImuValue
Definition: sick_generic_imu.h:78
SoftwarePLL::instance
static SoftwarePLL & instance()
Definition: softwarePLL.h:23
sick_scan::SickScanImuValue::LinearAccelerationY
float LinearAccelerationY() const
Definition: sick_generic_imu.h:148
sick_scan::SickScanImuValue::QuaternionZ
float QuaternionZ() const
Definition: sick_generic_imu.h:99
sick_scan::SickScanImuValue::QuaternionW
float QuaternionW() const
Definition: sick_generic_imu.h:105
sick_scan::SickScanImuValue::LinearAccelerationX
float LinearAccelerationX() const
Definition: sick_generic_imu.h:142
SoftwarePLL::getCorrectedTimeStamp
bool getCorrectedTimeStamp(uint32_t &sec, uint32_t &nanoSec, uint32_t tick)
Definition: softwarePLL.cpp:183
ros::Publisher::publish
void publish(const boost::shared_ptr< M > &message) const
sick_scan_common.h
sick_scan::SickScanCommon::config_
SickScanConfig config_
Definition: sick_scan_common.h:282
sick_generic_parser.h
sick_scan::SickScanImu::quaternion2rpyTest
static void quaternion2rpyTest()
Test barebone implemetation from quaternio to euler (Roll-Pitch-Yaw-Sequence) to avoid problems using...
Definition: sick_generic_imu.cpp:437
sick_scan_common_tcp.h
sick_generic_imu.h
sick_scan::SickScanImu::parseAsciiDatagram
int parseAsciiDatagram(char *datagram, size_t datagram_length, SickScanImuValue *imValuePtr)
Parsing Ascii datagram.
Definition: sick_generic_imu.cpp:293
sick_scan::SickScanImuValue::TimeStamp
UINT64 TimeStamp() const
Definition: sick_generic_imu.h:81
sick_scan::SickScanImuValue::AngularVelocityX
float AngularVelocityX() const
Definition: sick_generic_imu.h:118
sick_scan::SickScanImu::isImuBinaryDatagram
bool isImuBinaryDatagram(char *datagram, size_t datagram_length)
Checking ASCII diagram for imu message type.
Definition: sick_generic_imu.cpp:160
sick_scan::SickScanImu::parseDatagram
int parseDatagram(ros::Time timeStamp, unsigned char *receiveBuffer, int actual_length, bool useBinaryProtocol)
Definition: sick_generic_imu.cpp:557
ROS_WARN
#define ROS_WARN(...)
sick_scan::SickScanCommon::imuScan_pub_
ros::Publisher imuScan_pub_
Definition: sick_scan_common.h:276
sick_scan
Definition: abstract_parser.cpp:50
sick_scan::SickScanImu::simpleFmodTwoPi
double simpleFmodTwoPi(double angle)
Checking angle to be in the interval [-M_PI,M_PI] Of course you can also use fmod,...
Definition: sick_generic_imu.cpp:117
TimeBase< Time, Duration >::sec
uint32_t sec
sick_scan::SickScanImu::parseBinaryDatagram
int parseBinaryDatagram(char *datagram, size_t datagram_length, SickScanImuValue *imValuePtr)
Parsing Ascii datagram.
Definition: sick_generic_imu.cpp:197
TimeBase< Time, Duration >::nsec
uint32_t nsec
sick_scan::SickScanImu::isImuAckDatagram
bool isImuAckDatagram(char *datagram, size_t datagram_length)
Definition: sick_generic_imu.cpp:130
sick_scan::SickScanImuValue::QuaternionY
float QuaternionY() const
Definition: sick_generic_imu.h:93
sick_scan::SickScanImuValue::LinearAccelerationZ
float LinearAccelerationZ() const
Definition: sick_generic_imu.h:154
sick_scan::SickScanImuValue::AngularVelocityZ
float AngularVelocityZ() const
Definition: sick_generic_imu.h:130
sick_scan::SickScanImuValue::AngularVelocityReliability
UINT16 AngularVelocityReliability() const
Definition: sick_generic_imu.h:136
sick_scan::SickScanImu::commonPtr
SickScanCommon * commonPtr
Definition: sick_generic_imu.h:213
sick_scan::SickScanImuValue::AngularVelocityY
float AngularVelocityY() const
Definition: sick_generic_imu.h:124
ros::Time
sick_scan::SickScanImuValue::LinearAccelerationReliability
UINT16 LinearAccelerationReliability() const
Definition: sick_generic_imu.h:160
timeStamp
ros::Time const * timeStamp(const M &m)
dataDumper.h
sick_scan::SickScanImuValue::QuaternionX
float QuaternionX() const
Definition: sick_generic_imu.h:87
sick_scan::SickScanImu::isImuAsciiDatagram
bool isImuAsciiDatagram(char *datagram, size_t datagram_length)
Checking ASCII diagram for imu message type.
Definition: sick_generic_imu.cpp:266
UINT32
uint32_t UINT32
Definition: BasicDatatypes.hpp:26
DurationBase< Duration >::toSec
double toSec() const
ros::Duration
tf::Quaternion
DataDumper::pushData
int pushData(double timeStamp, std::string info, double val)
Definition: dataDumper.cpp:13
sick_scan::SickScanImu
Definition: sick_generic_imu.h:184


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