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

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