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

sick_scan
Author(s): Michael Lehning , Jochen Sprickerhof , Martin Günther
autogenerated on Tue Jul 9 2019 04:55:32