sick_generic_radar.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_generic_parser.h>
58 #include <sick_scan/sick_generic_radar.h>
59 #include <sick_scan/RadarScan.h> // generated by msg-generator
60 
61 #ifdef _MSC_VER
62 #include "sick_scan/rosconsole_simu.hpp"
63 #endif
64 #define _USE_MATH_DEFINES
65 
66 #include <math.h>
67 #include "string"
68 #include <stdio.h>
69 #include <stdlib.h>
70 
71 namespace sick_scan
72 {
73 
74 
75 /* Null, because instance will be initialized on demand. */
76  SickScanRadarSingleton *SickScanRadarSingleton::instance = 0;
77 
78  SickScanRadarSingleton *SickScanRadarSingleton::getInstance()
79  {
80  if (instance == 0)
81  {
82  instance = new SickScanRadarSingleton();
83  }
84 
85  return instance;
86  }
87 
88  SickScanRadarSingleton::SickScanRadarSingleton()
89  {
90  // just for debugging, but very helpful for the start
91  cloud_radar_rawtarget_pub_ = nh_.advertise<sensor_msgs::PointCloud2>("cloud_radar_rawtarget", 100);
92  cloud_radar_track_pub_ = nh_.advertise<sensor_msgs::PointCloud2>("cloud_radar_track", 100);
93 
94  radarScan_pub_ = nh_.advertise<sick_scan::RadarScan>("radar", 100);
95 
96  }
97 
98 
99  int16_t getShortValue(std::string str)
100  {
101  int val = 0;
102  if (1 == sscanf(str.c_str(), "%x", &val))
103  {
104 
105  }
106  else
107  {
108  ROS_WARN("Problems parsing %s\n", str.c_str());
109  }
110  return (val);
111 
112  }
113 
114  int getHexValue(std::string str)
115  {
116  int val = 0;
117  if (1 == sscanf(str.c_str(), "%x", &val))
118  {
119 
120  }
121  else
122  {
123  ROS_WARN("Problems parsing %s\n", str.c_str());
124  }
125  return (val);
126 
127  }
128 
129 
130  float convertScaledIntValue(int value, float scale, float offset)
131  {
132  float val = 0;
133  val = (float) (value * scale + offset);
134  return (val);
135  }
136 
137  float getFloatValue(std::string str)
138  {
139  float tmpVal = 0.0;
140  unsigned char *ptr;
141  ptr = (unsigned char *) (&tmpVal);
142  int strLen = str.length();
143  if (strLen < 8)
144  {
145  }
146  else
147  {
148  for (int i = 0; i < 4; i++)
149  {
150  std::string dummyStr = "";
151  dummyStr += str[i * 2];
152  dummyStr += str[i * 2 + 1];
153  int val = getHexValue(dummyStr);
154  unsigned char ch = (0xFF & val);
155  ptr[3 - i] = ch;
156  }
157  }
158  return (tmpVal);
159  }
160 
161  void SickScanRadarSingleton::setEmulation(bool _emul)
162  {
163  emul = _emul;
164  }
165 
166  bool SickScanRadarSingleton::getEmulation(void)
167  {
168  return (emul);
169  }
170 
181  int SickScanRadarSingleton::parseAsciiDatagram(char *datagram, size_t datagram_length,
182  sick_scan::RadarScan *msgPtr,
183  std::vector<SickScanRadarObject> &objectList,
184  std::vector<SickScanRadarRawTarget> &rawTargetList)
185  {
186  int exitCode = ExitSuccess;
187  ros::NodeHandle tmpParam("~");
188  bool dumpData = false;
189  int verboseLevel = 0;
190  tmpParam.getParam("verboseLevel", verboseLevel);
191 
192  // !!!!!
193  // verboseLevel = 1;
194  int HEADER_FIELDS = 32;
195  char *cur_field;
196  size_t count;
197 
198  // Reserve sufficient space
199  std::vector<char *> fields;
200  fields.reserve(datagram_length / 2);
201 
202  // ----- only for debug output
203  std::vector<char> datagram_copy_vec;
204  datagram_copy_vec.resize(datagram_length + 1); // to avoid using malloc. destructor frees allocated mem.
205  char *datagram_copy = &(datagram_copy_vec[0]);
206 
207  if (verboseLevel > 0)
208  {
209  ROS_WARN("Verbose LEVEL activated. Only for DEBUG.");
210  }
211 
212  if (verboseLevel > 0)
213  {
214  static int cnt = 0;
215  char szDumpFileName[255] = {0};
216  char szDir[255] = {0};
217 #ifdef _MSC_VER
218  strcpy(szDir, "C:\\temp\\");
219 #else
220  strcpy(szDir, "/tmp/");
221 #endif
222  sprintf(szDumpFileName, "%stmp%06d.bin", szDir, cnt);
223  FILE *ftmp;
224  ftmp = fopen(szDumpFileName, "wb");
225  if (ftmp != NULL)
226  {
227  fwrite(datagram, datagram_length, 1, ftmp);
228  fclose(ftmp);
229  }
230  }
231 
232  strncpy(datagram_copy, datagram, datagram_length); // datagram will be changed by strtok
233  datagram_copy[datagram_length] = 0;
234 
235  // ----- tokenize
236  count = 0;
237  cur_field = strtok(datagram, " ");
238 
239  while (cur_field != NULL)
240  {
241  fields.push_back(cur_field);
242  //std::cout << cur_field << std::endl;
243  cur_field = strtok(NULL, " ");
244  }
245 
246  //std::cout << fields[27] << std::endl;
247 
248  count = fields.size();
249 
250 
251  if (verboseLevel > 0)
252  {
253  static int cnt = 0;
254  char szDumpFileName[255] = {0};
255  char szDir[255] = {0};
256 #ifdef _MSC_VER
257  strcpy(szDir, "C:\\temp\\");
258 #else
259  strcpy(szDir, "/tmp/");
260 #endif
261  sprintf(szDumpFileName, "%stmp%06d.txt", szDir, cnt);
262  ROS_WARN("Verbose LEVEL activated. Only for DEBUG.");
263  FILE *ftmp;
264  ftmp = fopen(szDumpFileName, "w");
265  if (ftmp != NULL)
266  {
267  int i;
268  for (i = 0; i < count; i++)
269  {
270  fprintf(ftmp, "%3d: %s\n", i, fields[i]);
271  }
272  fclose(ftmp);
273  }
274  cnt++;
275  }
276 
277 
278  enum PREHEADER_TOKEN_SEQ
279  {
280  PREHEADER_TOKEN_SSN, // 0: sSN
281  PREHEADER_TOKEN_LMDRADARDATA, // 1: LMDradardata
282  PREHEADER_TOKEN_UI_VERSION_NO,
283  PREHEADER_TOKEN_UI_IDENT,
284  PREHEADER_TOKEN_UDI_SERIAL_NO,
285  PREHEADER_TOKEN_XB_STATE_0,
286  PREHEADER_TOKEN_XB_STATE_1,
287  PREHEADER_TOKEN_TELEGRAM_COUNT, // 7
288  PREHEADER_TOKEN_CYCLE_COUNT,
289  PREHEADER_TOKEN_SYSTEM_COUNT_SCAN,
290  PREHEADER_TOKEN_SYSTEM_COUNT_TRANSMIT,
291  PREHEADER_TOKEN_XB_INPUTS_0,
292  PREHEADER_TOKEN_XB_INPUTS_1,
293  PREHEADER_TOKEN_XB_OUTPUTS_0, // 13
294  PREHEADER_TOKEN_XB_OUTPUTS_1, // 14
295  PREHEADER_TOKEN_CYCLE_DURATION, // 15
296  PREHEADER_TOKEN_NOISE_LEVEL, // 16
297  PREHEADER_NUM_ENCODER_BLOCKS, // 17
298  PREHADERR_TOKEN_FIX_NUM
299  };
300 /*
301 
302  StatusBlock
303  ===========
304  7: BCC uiTelegramCount
305  8: DC0C uiCycleCount (or uiScanCount???)
306  9: 730E9D16 udiSystemCountScan
307  10: 730EA06D udiSystemCountTransmit
308  11: 0 xbInputs[0]
309  12: 0 xbInputs[1]
310  13: 0 xbOutputs[0]
311  14: 0 xbOutputs[1]
312 
313  MeasurementParam1Block
314  ======================
315  15: 0 MeasurementParam1Block.uiCycleDuration
316  16: 0 MeasurementParam1Block.uiNoiseLevel
317 
318  aEncoderBlock
319  =============
320  17: 1 Number of aEncoderBlocks
321 
322 
323  18: 0 aEncoderBlock[0].udiEncoderPos
324  19: 0 aEncoderBlock[0].iEncoderSpeed
325 
326 
327  PREHADERR_TOKEN_FIX_NUM}; // Number of fix token (excluding aEncoderBlock)
328 */
329  /*
330  * To read a single unsigned byte, use the %hhx modifier.
331  * %hx is for an unsigned short,
332  * %x is for an unsigned int,
333  * %lx is for an unsigned long,
334  * and %llx is for an `unsigned long long.
335  *
336  */
337  for (int i = 0; i < PREHADERR_TOKEN_FIX_NUM; i++)
338  {
339  UINT16 uiValue = 0x00;
340  UINT32 udiValue = 0x00;
341  unsigned long int uliDummy;
342  uliDummy = strtoul(fields[i], NULL, 16);
343  switch (i)
344  {
345  case PREHEADER_TOKEN_UI_VERSION_NO:
346  msgPtr->radarPreHeader.uiVersionNo = (UINT16) (uliDummy & 0xFFFF);
347  break;
348  case PREHEADER_TOKEN_UI_IDENT:
349  msgPtr->radarPreHeader.radarPreHeaderDeviceBlock.uiIdent = (UINT16) (uliDummy & 0xFFFF);
350  break;
351  case PREHEADER_TOKEN_UDI_SERIAL_NO:
352  msgPtr->radarPreHeader.radarPreHeaderDeviceBlock.udiSerialNo = (UINT32) (uliDummy & 0xFFFFFFFF);
353  break;
354  case PREHEADER_TOKEN_XB_STATE_0:
355  for (int j = 0; j < 3; j++)
356  {
357  bool flag = false;
358  if (0 != (uliDummy & (1 << j)))
359  {
360  flag = true;
361  }
362  switch (j)
363  {
364  case 0:
365  msgPtr->radarPreHeader.radarPreHeaderDeviceBlock.bDeviceError = flag;
366  break;
367  case 1:
368  msgPtr->radarPreHeader.radarPreHeaderDeviceBlock.bContaminationWarning = flag;
369  break;
370  case 2:
371  msgPtr->radarPreHeader.radarPreHeaderDeviceBlock.bContaminationError = flag;
372  break;
373  default:
374  ROS_WARN("Flag parsing for this PREHEADER-Flag not implemented");
375  }
376  }
377  break;
378  case PREHEADER_TOKEN_XB_STATE_1:
379  // reserved - do nothing
380  break;
381  case PREHEADER_TOKEN_TELEGRAM_COUNT: // 7
382  msgPtr->radarPreHeader.radarPreHeaderStatusBlock.uiTelegramCount = (UINT16) (uliDummy & 0xFFFF);
383  break;
384  case PREHEADER_TOKEN_CYCLE_COUNT:
385  sscanf(fields[i], "%hu", &uiValue);
386  msgPtr->radarPreHeader.radarPreHeaderStatusBlock.uiCycleCount = (UINT16) (uliDummy & 0xFFFF);
387  break;
388  case PREHEADER_TOKEN_SYSTEM_COUNT_SCAN:
389  msgPtr->radarPreHeader.radarPreHeaderStatusBlock.udiSystemCountScan = (UINT32) (uliDummy & 0xFFFFFFFF);
390  break;
391  case PREHEADER_TOKEN_SYSTEM_COUNT_TRANSMIT:
392  msgPtr->radarPreHeader.radarPreHeaderStatusBlock.udiSystemCountTransmit = (UINT32) (uliDummy & 0xFFFFFFFF);
393  break;
394  case PREHEADER_TOKEN_XB_INPUTS_0:
395  msgPtr->radarPreHeader.radarPreHeaderStatusBlock.uiInputs = (UINT8) (uliDummy & 0xFF);;
396  msgPtr->radarPreHeader.radarPreHeaderStatusBlock.uiInputs <<= 8;
397  break;
398  case PREHEADER_TOKEN_XB_INPUTS_1:
399  msgPtr->radarPreHeader.radarPreHeaderStatusBlock.uiInputs |= (UINT8) (uliDummy & 0xFF);;
400  break;
401  case PREHEADER_TOKEN_XB_OUTPUTS_0:
402  msgPtr->radarPreHeader.radarPreHeaderStatusBlock.uiOutputs = (UINT8) (uliDummy & 0xFF);;
403  msgPtr->radarPreHeader.radarPreHeaderStatusBlock.uiOutputs <<= 8;
404  break;
405  case PREHEADER_TOKEN_XB_OUTPUTS_1:
406  msgPtr->radarPreHeader.radarPreHeaderStatusBlock.uiOutputs |= (UINT8) (uliDummy & 0xFF);;
407  break;
408  case PREHEADER_TOKEN_CYCLE_DURATION:
409  msgPtr->radarPreHeader.radarPreHeaderMeasurementParam1Block.uiCycleDuration = (UINT16) (uliDummy & 0xFFFF);
410  break;
411  case PREHEADER_TOKEN_NOISE_LEVEL:
412  msgPtr->radarPreHeader.radarPreHeaderMeasurementParam1Block.uiNoiseLevel = (UINT16) (uliDummy & 0xFFFF);
413  break;
414  case PREHEADER_NUM_ENCODER_BLOCKS:
415  {
416  UINT16 u16NumberOfBlock = (UINT16) (uliDummy & 0xFFFF);
417 
418  if (u16NumberOfBlock > 0)
419  {
420  msgPtr->radarPreHeader.radarPreHeaderArrayEncoderBlock.resize(u16NumberOfBlock);
421 
422  for (int j = 0; j < u16NumberOfBlock; j++)
423  {
424  INT16 iEncoderSpeed;
425  int rowIndex = PREHEADER_NUM_ENCODER_BLOCKS + j * 2 + 1;
426  udiValue = strtoul(fields[rowIndex], NULL, 16);
427  msgPtr->radarPreHeader.radarPreHeaderArrayEncoderBlock[j].udiEncoderPos = udiValue;
428  udiValue = strtoul(fields[rowIndex + 1], NULL, 16);
429  iEncoderSpeed = (int) udiValue;
430  msgPtr->radarPreHeader.radarPreHeaderArrayEncoderBlock[j].iEncoderSpeed = iEncoderSpeed;
431 
432  }
433  }
434  }
435  break;
436  }
437  }
438 /*
439  MeasurementData
440  ===============
441  2: 1 MeasurementData.uiVersionNo : Version Information for this while structureValue
442  Value Range: 0 ... 65535
443  };
444 
445 */
446  std::vector<std::string> keyWordList;
447 #define DIST1_KEYWORD "DIST1"
448 #define AZMT1_KEYWORD "AZMT1"
449 #define VRAD1_KEYWORD "VRAD1"
450 #define AMPL1_KEYWORD "AMPL1"
451 #define MODE1_KEYWORD "MODE1"
452 
453 #define P3DX1_KEYWORD "P3DX1"
454 #define P3DY1_KEYWORD "P3DY1"
455 #define V3DX1_KEYWORD "V3DX1"
456 #define V3DY1_KEYWORD "V3DY1"
457 #define OBJLEN_KEYWORD "OBLE1"
458 #define OBJID_KEYWORD "OBID1"
459 
460  const int RAWTARGET_LOOP = 0;
461  const int OBJECT_LOOP = 1;
462 
463  // std::vector<SickScanRadarRawTarget> rawTargets;
464  // std::vector<SickScanRadarObject> objectList;
465 
466  for (int iLoop = 0; iLoop < 2; iLoop++)
467  {
468  keyWordList.clear();
469  switch (iLoop)
470  {
471  case RAWTARGET_LOOP:
472  keyWordList.push_back(DIST1_KEYWORD);
473  keyWordList.push_back(AZMT1_KEYWORD);
474  keyWordList.push_back(VRAD1_KEYWORD);
475  keyWordList.push_back(AMPL1_KEYWORD);
476  keyWordList.push_back(MODE1_KEYWORD);
477  break;
478  case OBJECT_LOOP:
479  keyWordList.push_back(P3DX1_KEYWORD);
480  keyWordList.push_back(P3DY1_KEYWORD);
481  keyWordList.push_back(V3DX1_KEYWORD);
482  keyWordList.push_back(V3DY1_KEYWORD);
483  keyWordList.push_back(OBJLEN_KEYWORD);
484  keyWordList.push_back(OBJID_KEYWORD);
485  break;
486  }
487  std::vector<int> keyWordPos;
488  std::vector<float> keyWordScale;
489  std::vector<float> keyWordScaleOffset;
490  keyWordPos.resize(keyWordList.size());
491  keyWordScale.resize(keyWordList.size());
492  keyWordScaleOffset.resize(keyWordList.size());
493  for (int i = 0; i < keyWordPos.size(); i++)
494  {
495  keyWordPos[i] = -1;
496  }
497  int numKeyWords = keyWordPos.size();
498  for (int i = 0; i < fields.size(); i++)
499  {
500  for (int j = 0; j < keyWordList.size(); j++)
501  {
502  if (strcmp(fields[i], keyWordList[j].c_str()) == 0)
503  {
504  keyWordPos[j] = i;
505  }
506  }
507  }
508 
509  bool entriesNumOk = true;
510  int entriesNum = 0;
511  if (keyWordPos[0] == -1)
512  {
513  entriesNumOk = false;
514  }
515  else
516  {
517 
518  entriesNum = getHexValue(fields[keyWordPos[0] + 3]);
519  for (int i = 0; i < numKeyWords; i++)
520  {
521  if (keyWordPos[i] == -1)
522  {
523  entriesNumOk = false;
524  ROS_WARN("Missing keyword %s but first keyword found.\n", keyWordList[i].c_str());
525  entriesNumOk = false;
526  }
527  else
528  {
529  int entriesNumTmp = getHexValue(fields[keyWordPos[i] + 3]);
530  if (entriesNumTmp != entriesNum)
531  {
532  ROS_WARN("Number of items for keyword %s differs from number of items for %s\n.",
533  keyWordList[i].c_str(), keyWordList[0].c_str());
534  entriesNumOk = false;
535  }
536  }
537  }
538  }
539 
540  if (true == entriesNumOk)
541  {
542 
543 
544  for (int i = 0; i < numKeyWords; i++)
545  {
546  int scaleLineIdx = keyWordPos[i] + 1;
547  int scaleOffsetLineIdx = keyWordPos[i] + 2;
548  std::string token = fields[scaleLineIdx];
549  keyWordScale[i] = getFloatValue(token);
550  token = fields[scaleOffsetLineIdx];
551  keyWordScaleOffset[i] = getFloatValue(token);
552  // printf("Keyword: %-6s %8.3lf %8.3lf\n", keyWordList[i].c_str(), keyWordScale[i], keyWordScaleOffset[i]);
553  }
554 
555  switch (iLoop)
556  {
557  case RAWTARGET_LOOP:
558  {
559  rawTargetList.resize(entriesNum);
560  break;
561  }
562  case OBJECT_LOOP:
563  {
564  objectList.resize(entriesNum);
565  break;
566  }
567  }
568  for (int i = 0; i < entriesNum; i++)
569  {
570  switch (iLoop)
571  {
572  case RAWTARGET_LOOP:
573  {
574  float dist = 0.0;
575  float azimuth = 0.0;
576  float ampl = 0.0;
577  float vrad = 0.0;
578  int mode = 0;
579  for (int j = 0; j < numKeyWords; j++)
580  {
581  int dataRowIdx = keyWordPos[j] + 4 + i;
582  std::string token = keyWordList[j];
583  if (token.compare(DIST1_KEYWORD) == 0)
584  {
585  int distRaw = getHexValue(fields[dataRowIdx]);
586  dist = convertScaledIntValue(distRaw, keyWordScale[j], keyWordScaleOffset[j]) * 0.001;
587  }
588  if (token.compare(AZMT1_KEYWORD) == 0)
589  {
590  int azimuthRaw = getShortValue(fields[dataRowIdx]);
591  azimuth = (float) convertScaledIntValue(azimuthRaw, keyWordScale[j], keyWordScaleOffset[j]);
592  }
593  if (token.compare(VRAD1_KEYWORD) == 0)
594  {
595  int vradRaw = getShortValue(fields[dataRowIdx]);
596  vrad = (float) convertScaledIntValue(vradRaw, keyWordScale[j], keyWordScaleOffset[j]);
597  }
598  if (token.compare(MODE1_KEYWORD) == 0)
599  {
600  int modeRaw = getHexValue(fields[dataRowIdx]);
601  mode = (int) (convertScaledIntValue(modeRaw, keyWordScale[j], keyWordScaleOffset[j]) + 0.5);
602  }
603  if (token.compare(AMPL1_KEYWORD) == 0)
604  {
605  int amplRaw = getShortValue(fields[dataRowIdx]);
606  ampl = (int) (convertScaledIntValue(amplRaw, keyWordScale[j], keyWordScaleOffset[j]) + 0.5);
607  }
608  }
609  rawTargetList[i].Dist(dist);
610  rawTargetList[i].Ampl(ampl);
611  rawTargetList[i].Azimuth(azimuth);
612  rawTargetList[i].Mode(mode);
613  rawTargetList[i].Vrad(vrad);
614 
615  }
616  break;
617  case OBJECT_LOOP:
618  {
619  float px = 0.0;
620  float py = 0.0;
621  float vx = 0.0;
622  float vy = 0.0;
623  float objLen = 0.0;
624  int objId = 0;
625 
626  for (int j = 0; j < numKeyWords; j++)
627  {
628  int dataRowIdx = keyWordPos[j] + 4 + i;
629  std::string token = keyWordList[j];
630  int intVal = getShortValue(fields[dataRowIdx]);
631  float val = convertScaledIntValue(intVal, keyWordScale[j], keyWordScaleOffset[j]);
632 
633  if (token.compare(P3DX1_KEYWORD) == 0)
634  {
635  px = val * 0.001f;
636  }
637  if (token.compare(P3DY1_KEYWORD) == 0)
638  {
639  py = val * 0.001f;
640  }
641  if (token.compare(V3DX1_KEYWORD) == 0)
642  {
643  vx = val;
644  }
645  if (token.compare(V3DY1_KEYWORD) == 0)
646  {
647  vy = val;
648 
649  }
650  if (token.compare(OBJLEN_KEYWORD) == 0)
651  {
652  objLen = val;
653  }
654  if (token.compare(OBJID_KEYWORD) == 0)
655  {
656  int objIdRaw = getHexValue(fields[dataRowIdx]);
657  objId = (int) (objIdRaw * keyWordScale[j] + 0.5);
658  }
659  }
660 
661  objectList[i].ObjId(objId);
662  objectList[i].ObjLength(objLen);
663  objectList[i].P3Dx(px);
664  objectList[i].P3Dy(py);
665  objectList[i].V3Dx(vx);
666  objectList[i].V3Dy(vy);
667  }
668  break;
669  }
670  }
671  }
672  // Now parsing the entries
673  }
674 
675  return (exitCode);
676  }
677 
678  void SickScanRadarSingleton::simulateAsciiDatagram(unsigned char *receiveBuffer, int *actual_length)
679  {
680  static int callCnt = 0;
681 
682  callCnt++;
683 
684  // end
685  std::string header = "\x2sSN LMDradardata 1 1 112F6E9 0 0 DFB6 B055 6E596002 6E5AE0E5 0 0 0 0 0 0 1 0 0 ";
686  // std::string header = "\x2sSN LMDradardata 10 20 30 5 6 40 50 60 70 90 A0 B0 C0 D0 E0 1 A000 FFFFFF00 ";
687  int channel16BitCnt = 4;
688  // Faktoren fuer Rohziele: 40.0 0.16 0.04 1.00 1.00
689  float rawTargetFactorList[] = {40.0f, 0.16f, 0.04f, 1.00f, 1.00f};
690  float objectFactorList[] = {64.0f, 64.0f, 0.1f, 0.1f, 0.75f, 1.0f};
691 
692  std::string dist1_intro = "DIST1 42200000 00000000";
693  std::string azmt1_intro = "AZMT1 3E23D70A 00000000";
694  std::string vrad1_intro = "VRAD1 3D23D70A 00000000";
695  std::string ampl1_intro = "AMPL1 3F800000 00000000";
696 
697  std::string pdx1_intro = "P3DX1 42800000 00000000";
698  std::string pdy1_intro = "P3DY1 42800000 00000000";
699  std::string v3dx_intro = "V3DX1 3DCCCCCD 00000000";
700  std::string v3dy_intro = "V3DY1 3DCCCCCD 00000000";
701  std::string oblen_intro = "OBLE1 3F400000 00000000";
702 
703  int rawTargetChannel16BitCnt = 4;
704  int rawTargetChannel8BitCnt = 1;
705  std::string mode1_intro = "MODE1 3F800000 00000000";
706 
707  std::string obid_intro = "OBID1 3F800000 00000000";
708 
709 
710  std::string trailer = "0 0 0 0 0 0\x3";
711 
712  std::vector<std::string> channel16BitID;
713  std::vector<std::string> channel8BitID;
714  channel16BitID.push_back(dist1_intro);
715  channel16BitID.push_back(azmt1_intro);
716  channel16BitID.push_back(vrad1_intro);
717  channel16BitID.push_back(ampl1_intro);
718 
719  channel16BitID.push_back(pdx1_intro);
720  channel16BitID.push_back(pdy1_intro);
721  channel16BitID.push_back(v3dx_intro);
722  channel16BitID.push_back(v3dy_intro);
723  channel16BitID.push_back(oblen_intro);
724 
725 
726  channel8BitID.push_back(mode1_intro);
727  channel8BitID.push_back(obid_intro);
728 
729  int channel8BitCnt = channel8BitID.size();
730  int objectChannel16BitCnt = 5;
731  channel16BitCnt = channel16BitID.size();
732  float x = 20.0;
733  float speed = 50.0; // [m/s]
734  std::vector<SickScanRadarRawTarget> rawTargetList;
735 
736 #if 0 // simulate railway crossing
737  for (float y = -20; y <= 20.0; y += 2.0)
738  {
739  SickScanRadarRawTarget rawTarget;
740  float azimuth = atan2(y, x);
741  float dist = sqrt(x*x + y*y);
742  float vrad = speed * sin(azimuth); // speed in y direction
743  float mode = 0;
744  float ampl = 50.0 + y; // between 30 and 70
745  rawTarget.Ampl(ampl);
746  rawTarget.Mode(mode);
747  rawTarget.Dist(dist);
748  rawTarget.Azimuth(azimuth);
749  rawTarget.Vrad(vrad);
750  rawTargetList.push_back(rawTarget);
751  }
752 #endif
753 
754  std::vector<SickScanRadarObject> objectList;
755 
756  int objId = 0;
757  for (float x = 20; x <= 100.0; x += 50.0)
758  {
759  SickScanRadarObject vehicle;
760  float y = 0.0;
761  for (int iY = -1; iY <= 1; iY += 2)
762  {
763  float xp[2] = {0}; // for raw target
764  float yp[2] = {0};
765  float vehicleWidth = 1.8;
766  y = iY * 2.0;
767  float speed = y * 10.0f;
768  vehicle.V3Dx(speed); // +/- 20 m/s
769  vehicle.V3Dy(0.1f); // just for testing
770 
771  float xOff = 20.0;
772  if (speed < 0.0)
773  {
774  xOff = 100.0;
775  }
776  vehicle.P3Dx((xOff + 0.1 * speed * (callCnt % 20)) * 1000.0);
777  vehicle.P3Dy(y * 1000.0);
778  float objLen = 6.0f + y;
779  vehicle.ObjLength(objLen);
780 
781  vehicle.ObjId(objId++);
782  objectList.push_back(vehicle);
783 
784  for (int i = 0; i < 2; i++)
785  {
786  SickScanRadarRawTarget rawTarget;
787 
788  xp[i] = vehicle.P3Dx() * 0.001;
789  yp[i] = vehicle.P3Dy() * 0.001;
790 
791  if (i == 0)
792  {
793  yp[i] -= vehicleWidth / 2.0;
794  }
795  else
796  {
797  yp[i] += vehicleWidth / 2.0;
798  }
799  if (speed < 0.0) // oncoming
800  {
801  xp[i] -= objLen * 0.5;
802  }
803  else
804  {
805  xp[i] += objLen * 0.5;
806  }
807 
808  float azimuth = atan2(yp[i], xp[i]);
809  float dist = sqrt(xp[i] * xp[i] + yp[i] * yp[i]);
810  float vrad = speed * cos(azimuth); // speed in y direction
811  float mode = 0;
812  float ampl = 50.0; // between 30 and 70
813 
814  rawTarget.Ampl(ampl);
815  rawTarget.Mode(mode);
816  rawTarget.Dist(dist);
817  rawTarget.Azimuth(azimuth);
818  rawTarget.Vrad(vrad);
819  rawTargetList.push_back(rawTarget);
820 
821  }
822 
823  }
824 
825 
826  }
827 
828  char szDummy[255] = {0};
829  std::string resultStr;
830  resultStr += header;
831  sprintf(szDummy, "%x ", channel16BitCnt);
832  resultStr += szDummy;
833  for (int i = 0; i < channel16BitCnt; i++)
834  {
835  resultStr += channel16BitID[i];
836  int valNum = rawTargetList.size();
837  bool processRawTarget = true;
838  if (i < rawTargetChannel16BitCnt)
839  {
840  valNum = rawTargetList.size();
841  }
842  else
843  {
844  processRawTarget = false;
845  valNum = objectList.size();
846  }
847 
848  sprintf(szDummy, " %x ", valNum);
849  resultStr += szDummy;
850  float val = 0.0;
851  for (int j = 0; j < valNum; j++)
852  {
853  switch (i)
854  {
855  case 0:
856  val = 1000.0 * rawTargetList[j].Dist();
857  break;
858  case 1:
859  val = 1.0 / deg2rad * rawTargetList[j].Azimuth();
860  break;
861  case 2:
862  val = rawTargetList[j].Vrad();
863  break;
864  case 3:
865  val = rawTargetList[j].Ampl();
866  break;
867  case 4:
868  val = objectList[j].P3Dx();
869  break;
870  case 5:
871  val = objectList[j].P3Dy();
872  break;
873  case 6:
874  val = objectList[j].V3Dx();
875  break;
876  case 7:
877  val = objectList[j].V3Dy();
878  break;
879  case 8:
880  val = objectList[j].ObjLength();
881  break;
882  }
883 
884 
885  if (processRawTarget)
886  {
887  val /= rawTargetFactorList[i];
888  }
889  else
890  {
891  int idx = i - rawTargetChannel16BitCnt;
892  val /= objectFactorList[idx];
893  }
894 
895  if (val > 0.0)
896  {
897  val += 0.5;
898  }
899  else
900  {
901  val -= 0.5;
902  }
903  int16_t shortVal = (int16_t) (val);
904 
905  sprintf(szDummy, "%08x", shortVal);
906  strcpy(szDummy, szDummy + 4); // remove first 4 digits due to integer / short
907  resultStr += szDummy;
908  resultStr += " ";
909  }
910  }
911 
912  sprintf(szDummy, "%x ", channel8BitCnt);
913  resultStr += szDummy;
914  int valNum = rawTargetList.size();
915  for (int i = 0; i < channel8BitCnt; i++)
916  {
917  resultStr += channel8BitID[i];
918  float val = 0.0;
919  bool processRawTarget = true;
920  if (i < rawTargetChannel8BitCnt)
921  {
922  valNum = rawTargetList.size();
923  }
924  else
925  {
926  processRawTarget = false;
927  valNum = objectList.size();
928  }
929  sprintf(szDummy, " %x ", valNum);
930  resultStr += szDummy;
931  for (int j = 0; j < valNum; j++)
932  {
933  switch (i)
934  {
935  case 0:
936  val = rawTargetList[j].Mode();
937  break;
938  case 1:
939  val = objectList[j].ObjId();
940  break;
941  }
942 
943  int offset = 0;
944  if (processRawTarget)
945  {
946  offset = rawTargetChannel16BitCnt;
947  val /= rawTargetFactorList[i + offset];
948  }
949  else
950  {
951  offset = objectChannel16BitCnt;
952  int idx = i - rawTargetChannel8BitCnt;
953  val /= objectFactorList[idx + offset];
954  }
955  if (val > 0.0)
956  {
957  val += 0.5;
958  }
959  else
960  {
961  val -= 0.5;
962  }
963  int8_t shortVal = (int16_t) (val);
964 
965  sprintf(szDummy, "%08x", shortVal);
966  strcpy(szDummy, szDummy + 6); // remove first 6 digits due to integer / short
967  resultStr += szDummy;
968  resultStr += " ";
969  }
970  }
971 
972  resultStr += trailer;
973 
974  *actual_length = resultStr.length();
975  strcpy((char *) receiveBuffer, resultStr.c_str());
976  }
977 
978  void SickScanRadarSingleton::simulateAsciiDatagramFromFile(unsigned char *receiveBuffer, int *pActual_length,
979  std::string filePattern)
980  {
981  static int callCnt = 0;
982  FILE *fin;
983  char szLine[1024] = {0};
984  char szDummyWord[1024] = {0};
985  int actual_length = 0;
986  int cnt = 0;
987  char szFileName[1024] = {0};
988 
989  receiveBuffer[0] = '\x02';
990  actual_length++;
991 
992  for (int iLoop = 0; iLoop < 2; iLoop++)
993  {
994  sprintf(szFileName, filePattern.c_str(), callCnt);
995  callCnt++;
996  fin = fopen(szFileName, "r");
997 
998  if ((fin == NULL) && (iLoop == 0))
999  {
1000  callCnt = 0; // reset to 0. This enables a loop over recorded raw data
1001  }
1002  else
1003  {
1004  break;
1005  }
1006 
1007  if ((iLoop == 1) && (fin == NULL))
1008  {
1009  ROS_ERROR("Can not find simulation file corresponding to pattern %s", filePattern.c_str());
1010  }
1011  }
1012 
1013  while (fgets(szLine, 1024, fin) != NULL)
1014  {
1015  char *ptr = strchr(szLine, '\n');;
1016  if (ptr != NULL)
1017  {
1018  *ptr = '\0';
1019  }
1020 
1021  ptr = strchr(szLine, ':');
1022  if (ptr != NULL)
1023  {
1024  if (1 == sscanf(ptr + 2, "%s", szDummyWord))
1025  {
1026  if (cnt > 0)
1027  {
1028  receiveBuffer[actual_length++] = ' ';
1029  }
1030  strcpy((char *) receiveBuffer + actual_length, szDummyWord);
1031  actual_length += strlen(szDummyWord);
1032  }
1033  cnt++;
1034  }
1035  }
1036  receiveBuffer[actual_length] = (char) '\x03';
1037  actual_length++;
1038  receiveBuffer[actual_length] = (char) '\x00';
1039  actual_length++;
1040  *pActual_length = actual_length;
1041  fclose(fin);
1042  }
1043 
1044  int SickScanRadarSingleton::parseDatagram(ros::Time timeStamp, unsigned char *receiveBuffer, int actual_length,
1045  bool useBinaryProtocol)
1046  {
1047  int exitCode = ExitSuccess;
1048 
1049  enum enumSimulationMode
1050  {
1051  EMULATE_OFF, EMULATE_SYN, EMULATE_FROM_FILE_TRAIN, EMULATE_FROM_FILE_CAR
1052  };
1053 
1054  int simulationMode = EMULATE_OFF;
1055 
1056  if (this->getEmulation())
1057  {
1058  simulationMode = EMULATE_SYN;
1059 
1060  }
1061  switch (simulationMode)
1062  {
1063  case EMULATE_OFF: // do nothing - use real data
1064  break;
1065  case EMULATE_SYN:
1066  simulateAsciiDatagram(receiveBuffer, &actual_length);
1067  break;
1068  case EMULATE_FROM_FILE_TRAIN:
1069  simulateAsciiDatagramFromFile(receiveBuffer, &actual_length,
1070  "/mnt/hgfs/development/ros/bags/raw/trainSeq/tmp%06d.txt");
1071  break;
1072  case EMULATE_FROM_FILE_CAR:
1073  simulateAsciiDatagramFromFile(receiveBuffer, &actual_length,
1074  "/mnt/hgfs/development/ros/bags/raw/carSeq/tmp%06d.txt");
1075  break;
1076 
1077  default:
1078  printf("Simulation Mode unknown\n");
1079 
1080  }
1081 
1082  sensor_msgs::PointCloud2 cloud_;
1083  sick_scan::RadarScan radarMsg_;
1084 
1085  std::vector<SickScanRadarObject> objectList;
1086  std::vector<SickScanRadarRawTarget> rawTargetList;
1087 
1088  if (useBinaryProtocol)
1089  {
1090  throw std::logic_error("Binary protocol currently not supported.");
1091  }
1092  else
1093  {
1094  bool dataToProcess = false;
1095  char *buffer_pos = (char *) receiveBuffer;
1096  char *dstart = NULL;
1097  char *dend = NULL;
1098  int dlength = 0;
1099  dstart = strchr(buffer_pos, 0x02);
1100  if (dstart != NULL)
1101  {
1102  dend = strchr(dstart + 1, 0x03);
1103  }
1104  if ((dstart != NULL) && (dend != NULL))
1105  {
1106  dataToProcess = true; // continue parsing
1107  dlength = dend - dstart;
1108  *dend = '\0';
1109  dstart++;
1110  parseAsciiDatagram(dstart, dlength, &radarMsg_, objectList, rawTargetList);
1111  }
1112  else
1113  {
1114  dataToProcess = false;
1115  }
1116 
1117 
1118  enum RADAR_PROC_LIST
1119  {
1120  RADAR_PROC_RAW_TARGET, RADAR_PROC_TRACK, RADAR_PROC_NUM
1121  };
1122  //
1123  // First loop: looking for raw targets
1124  // Second loop: looking for tracking objects
1125  for (int iLoop = 0; iLoop < RADAR_PROC_NUM; iLoop++)
1126  {
1127  int numTargets = 0;
1128  if (dataToProcess)
1129  {
1130  std::string channelRawTargetId[] = {"x", "y", "z", "vrad", "amplitude"};
1131  std::string channelObjectId[] = {"x", "y", "z", "vx", "vy", "vz", "objLen", "objId"};
1132  std::vector<std::string> channelList;
1133  std::string frameId = "radar"; //this->commonPtr->getConfigPtr()->frame_id;;
1134  switch (iLoop)
1135  {
1136  case RADAR_PROC_RAW_TARGET:
1137  numTargets = rawTargetList.size();
1138  for (int i = 0; i < sizeof(channelRawTargetId) / sizeof(channelRawTargetId[0]); i++)
1139  {
1140  channelList.push_back(channelRawTargetId[i]);
1141  }
1142  frameId = "radar"; // TODO - move to param list
1143  break;
1144  case RADAR_PROC_TRACK:
1145  numTargets = objectList.size();
1146  for (int i = 0; i < sizeof(channelObjectId) / sizeof(channelObjectId[0]); i++)
1147  {
1148  channelList.push_back(channelObjectId[i]);
1149  }
1150  frameId = "radar"; // TODO - move to param list
1151  break;
1152  }
1153  if (numTargets == 0)
1154  {
1155  continue;
1156  }
1157  int numChannels = channelList.size();
1158 
1159  std::vector<float> valSingle;
1160  valSingle.resize(numChannels);
1161  cloud_.header.stamp = timeStamp;
1162  cloud_.header.frame_id = frameId;
1163  cloud_.header.seq = 0;
1164  cloud_.height = 1; // due to multi echo multiplied by num. of layers
1165  cloud_.width = numTargets;
1166  cloud_.is_bigendian = false;
1167  cloud_.is_dense = true;
1168  cloud_.point_step = numChannels * sizeof(float);
1169  cloud_.row_step = cloud_.point_step * cloud_.width;
1170  cloud_.fields.resize(numChannels);
1171  for (int i = 0; i < numChannels; i++)
1172  {
1173  cloud_.fields[i].name = channelList[i];
1174  cloud_.fields[i].offset = i * sizeof(float);
1175  cloud_.fields[i].count = 1;
1176  cloud_.fields[i].datatype = sensor_msgs::PointField::FLOAT32;
1177  }
1178 
1179  cloud_.data.resize(cloud_.row_step * cloud_.height);
1180  float *valPtr = (float *) (&(cloud_.data[0]));
1181  int off = 0;
1182  for (int i = 0; i < numTargets; i++)
1183  {
1184  switch (iLoop)
1185  {
1186  case 0:
1187  {
1188  float angle = deg2rad * rawTargetList[i].Azimuth();
1189  valSingle[0] = rawTargetList[i].Dist() * cos(angle);
1190  valSingle[1] = rawTargetList[i].Dist() * sin(angle);
1191  valSingle[2] = 0.0;
1192  valSingle[3] = rawTargetList[i].Vrad();
1193  valSingle[4] = rawTargetList[i].Ampl();
1194  }
1195  break;
1196 
1197  case 1:
1198  valSingle[0] = objectList[i].P3Dx();
1199  valSingle[1] = objectList[i].P3Dy();
1200  valSingle[2] = 0.0;
1201  valSingle[3] = objectList[i].V3Dx();
1202  valSingle[4] = objectList[i].V3Dy();
1203  valSingle[5] = 0.0;
1204  valSingle[6] = objectList[i].ObjLength();
1205  valSingle[7] = objectList[i].ObjId();
1206  break;
1207  }
1208 
1209  for (int j = 0; j < numChannels; j++)
1210  {
1211  valPtr[off] = valSingle[j];
1212  off++;
1213  }
1214 #ifndef _MSC_VER
1215 #if 1 // just for debugging
1216  switch (iLoop)
1217  {
1218  case RADAR_PROC_RAW_TARGET:
1219  cloud_radar_rawtarget_pub_.publish(cloud_);
1220  break;
1221  case RADAR_PROC_TRACK:
1222  cloud_radar_track_pub_.publish(cloud_);
1223  break;
1224  }
1225 #endif
1226 #else
1227  printf("PUBLISH:\n");
1228 #endif
1229  if (iLoop == RADAR_PROC_RAW_TARGET)
1230  {
1231  // is this a deep copy ???
1232  radarMsg_.targets = cloud_;
1233  }
1234  }
1235  }
1236  }
1237  // Publishing radar messages
1238  // ...
1239  radarMsg_.header.stamp = timeStamp;
1240  radarMsg_.header.frame_id = "radar";
1241  radarMsg_.header.seq = 0;
1242 
1243  radarMsg_.objects.resize(objectList.size());
1244  for (int i = 0; i < radarMsg_.objects.size(); i++)
1245  {
1246  float vx = objectList[i].V3Dx();
1247  float vy = objectList[i].V3Dy();
1248  float v = sqrt(vx * vx + vy * vy);
1249  float heading = atan2(objectList[i].V3Dy(), objectList[i].V3Dx());
1250 
1251  radarMsg_.objects[i].velocity.twist.linear.x = objectList[i].V3Dx();
1252  radarMsg_.objects[i].velocity.twist.linear.y = objectList[i].V3Dy();
1253  radarMsg_.objects[i].velocity.twist.linear.z = 0.0;
1254 
1255  radarMsg_.objects[i].bounding_box_center.position.x = objectList[i].P3Dx();
1256  radarMsg_.objects[i].bounding_box_center.position.y = objectList[i].P3Dy();
1257  radarMsg_.objects[i].bounding_box_center.position.z = 0.0;
1258 
1259  float heading2 = heading / 2.0;
1260  // (n_x, n_y, n_z) = (0, 0, 1), so (x, y, z, w) = (0, 0, sin(theta/2), cos(theta/2))
1262  // see also this beautiful website: https://quaternions.online/
1263  radarMsg_.objects[i].bounding_box_center.orientation.x = 0.0;
1264  radarMsg_.objects[i].bounding_box_center.orientation.y = 0.0;
1265  radarMsg_.objects[i].bounding_box_center.orientation.z = sin(heading2);
1266  radarMsg_.objects[i].bounding_box_center.orientation.w = cos(heading2);
1267 
1268 
1269  radarMsg_.objects[i].bounding_box_size.x = objectList[i].ObjLength();
1270  radarMsg_.objects[i].bounding_box_size.y = 1.7;
1271  radarMsg_.objects[i].bounding_box_size.z = 1.7;
1272  for (int ii = 0; ii < 6; ii++)
1273  {
1274  int mainDiagOffset = ii * 6 + ii; // build eye-matrix
1275  radarMsg_.objects[i].object_box_center.covariance[mainDiagOffset] = 1.0; // it is a little bit hacky ...
1276  radarMsg_.objects[i].velocity.covariance[mainDiagOffset] = 1.0;
1277  }
1278  radarMsg_.objects[i].object_box_center.pose = radarMsg_.objects[i].bounding_box_center;
1279  radarMsg_.objects[i].object_box_size = radarMsg_.objects[i].bounding_box_size;
1280 
1281 
1282  }
1283 
1284  radarScan_pub_.publish(radarMsg_);
1285 
1286 
1287  }
1288  return (exitCode);
1289  }
1290 
1291 }
sick_scan::SickScanRadarSingleton::cloud_radar_rawtarget_pub_
ros::Publisher cloud_radar_rawtarget_pub_
Definition: sick_generic_radar.h:174
sick_scan::SickScanRadarSingleton::parseAsciiDatagram
int parseAsciiDatagram(char *datagram, size_t datagram_length, sick_scan::RadarScan *msgPtr, std::vector< SickScanRadarObject > &objectList, std::vector< SickScanRadarRawTarget > &rawTargetList)
Parsing Ascii datagram.
Definition: sick_generic_radar.cpp:181
sick_scan::SickScanRadarSingleton::SickScanRadarSingleton
SickScanRadarSingleton()
Definition: sick_generic_radar.cpp:88
ros::NodeHandle
sick_scan::SickScanRadarSingleton::emul
bool emul
Definition: sick_generic_radar.h:171
UINT16
uint16_t UINT16
Definition: BasicDatatypes.hpp:27
f
f
sick_scan::SickScanRadarRawTarget::Vrad
float Vrad() const
Definition: sick_generic_radar.h:85
sick_scan::SickScanRadarSingleton::instance
static SickScanRadarSingleton * instance
Definition: sick_generic_radar.h:164
sick_scan::ExitSuccess
Definition: abstract_parser.h:46
sick_scan::SickScanRadarSingleton::radarScan_pub_
ros::Publisher radarScan_pub_
Definition: sick_generic_radar.h:176
sick_scan::SickScanRadarObject::P3Dy
float P3Dy() const
Definition: sick_generic_radar.h:120
sick_scan::SickScanRadarObject::V3Dy
float V3Dy() const
Definition: sick_generic_radar.h:132
UINT32
uint32_t UINT32
Definition: BasicDatatypes.hpp:26
sick_scan::SickScanRadarSingleton::simulateAsciiDatagramFromFile
void simulateAsciiDatagramFromFile(unsigned char *receiveBuffer, int *actual_length, std::string filePattern)
Definition: sick_generic_radar.cpp:978
sick_scan::SickScanRadarRawTarget
Definition: sick_generic_radar.h:70
frameId
std::string * frameId(M &m)
sick_scan::convertScaledIntValue
float convertScaledIntValue(int value, float scale, float offset)
Definition: sick_generic_radar.cpp:130
sick_scan::SickScanRadarSingleton::cloud_radar_track_pub_
ros::Publisher cloud_radar_track_pub_
Definition: sick_generic_radar.h:175
ROS_WARN
#define ROS_WARN(...)
deg2rad
#define deg2rad
Definition: BasicDatatypes.hpp:37
sick_scan::getHexValue
int getHexValue(std::string str)
Definition: sick_generic_radar.cpp:114
angle
TFSIMD_FORCE_INLINE tfScalar angle(const Quaternion &q1, const Quaternion &q2)
sick_scan::SickScanRadarSingleton::getEmulation
bool getEmulation(void)
Definition: sick_generic_radar.cpp:166
sick_scan::SickScanRadarObject::V3Dx
float V3Dx() const
Definition: sick_generic_radar.h:126
VRAD1_KEYWORD
#define VRAD1_KEYWORD
OBJID_KEYWORD
#define OBJID_KEYWORD
V3DX1_KEYWORD
#define V3DX1_KEYWORD
sick_scan_common_tcp.h
sick_scan::SickScanRadarRawTarget::Azimuth
float Azimuth() const
Definition: sick_generic_radar.h:79
ros::Publisher::publish
void publish(const boost::shared_ptr< M > &message) const
sick_scan::SickScanRadarSingleton
Definition: sick_generic_radar.h:160
ros::NodeHandle::getParam
bool getParam(const std::string &key, std::string &s) const
sick_scan::SickScanRadarSingleton::parseDatagram
int parseDatagram(ros::Time timeStamp, unsigned char *receiveBuffer, int actual_length, bool useBinaryProtocol)
Definition: sick_generic_radar.cpp:1044
OBJLEN_KEYWORD
#define OBJLEN_KEYWORD
P3DY1_KEYWORD
#define P3DY1_KEYWORD
sick_scan::SickScanRadarRawTarget::Ampl
float Ampl() const
Definition: sick_generic_radar.h:91
ros::NodeHandle::advertise
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
sick_scan::SickScanRadarSingleton::getInstance
static SickScanRadarSingleton * getInstance()
Definition: sick_generic_radar.cpp:78
sick_scan::SickScanRadarRawTarget::Mode
int Mode() const
Definition: sick_generic_radar.h:97
P3DX1_KEYWORD
#define P3DX1_KEYWORD
V3DY1_KEYWORD
#define V3DY1_KEYWORD
sick_scan::SickScanRadarSingleton::setEmulation
void setEmulation(bool _emul)
Definition: sick_generic_radar.cpp:161
sick_scan::SickScanRadarSingleton::nh_
ros::NodeHandle nh_
Definition: sick_generic_radar.h:173
sick_generic_parser.h
sick_scan::SickScanRadarObject::ObjLength
float ObjLength() const
Definition: sick_generic_radar.h:138
sick_scan::SickScanRadarObject::ObjId
int ObjId() const
Definition: sick_generic_radar.h:144
sick_scan::SickScanRadarRawTarget::Dist
float Dist() const
Definition: sick_generic_radar.h:73
sick_scan
Definition: abstract_parser.cpp:50
DIST1_KEYWORD
#define DIST1_KEYWORD
sick_scan::SickScanRadarObject
Definition: sick_generic_radar.h:111
sick_scan::getFloatValue
float getFloatValue(std::string str)
Definition: sick_generic_radar.cpp:137
header
const std::string header
sick_generic_radar.h
INT16
int16_t INT16
Definition: BasicDatatypes.hpp:28
AMPL1_KEYWORD
#define AMPL1_KEYWORD
sick_scan::SickScanRadarObject::P3Dx
float P3Dx() const
Definition: sick_generic_radar.h:114
sick_scan::getShortValue
int16_t getShortValue(std::string str)
Definition: sick_generic_radar.cpp:99
MODE1_KEYWORD
#define MODE1_KEYWORD
AZMT1_KEYWORD
#define AZMT1_KEYWORD
ROS_ERROR
#define ROS_ERROR(...)
sick_scan::SickScanRadarSingleton::simulateAsciiDatagram
void simulateAsciiDatagram(unsigned char *receiveBuffer, int *actual_length)
Definition: sick_generic_radar.cpp:678
UINT8
uint8_t UINT8
Definition: BasicDatatypes.hpp:29

sick_scan
Author(s): Michael Lehning , Jochen Sprickerhof , Martin Günther
autogenerated on Wed Sep 7 2022 02:25:06