sick_scan_common.cpp
Go to the documentation of this file.
1 
52 #ifdef _MSC_VER // compiling simulation for MS-Visual C++ - not defined for linux system
53 #pragma warning(disable: 4996)
54 #pragma warning(disable: 4267) // due to conversion warning size_t in the ros header file
55 #define _WIN32_WINNT 0x0501
56 
57 #endif
58 
65 
66 #ifdef _MSC_VER
67 #include "sick_scan/rosconsole_simu.hpp"
68 #endif
69 
70 #include "sick_scan/binScanf.hpp"
71 // if there is a missing RadarScan.h, try to run catkin_make in der workspace-root
72 #include <sick_scan/RadarScan.h>
73 
74 #include "sick_scan/Encoder.h"
75 
76 #include <cstdio>
77 #include <cstring>
78 
79 #define _USE_MATH_DEFINES
80 
81 #include <math.h>
82 
83 #ifndef rad2deg
84 #define rad2deg(x) ((x) / M_PI * 180.0)
85 #endif
86 
87 #define deg2rad_const (0.017453292519943295769236907684886f)
88 
89 #include "sick_scan/tcp/colaa.hpp"
90 #include "sick_scan/tcp/colab.hpp"
91 
92 #include <map>
93 #include <climits>
96 
102 void swap_endian(unsigned char *ptr, int numBytes)
103 {
104  unsigned char *buf = (ptr);
105  unsigned char tmpChar;
106  for (int i = 0; i < numBytes / 2; i++)
107  {
108  tmpChar = buf[numBytes - 1 - i];
109  buf[numBytes - 1 - i] = buf[i];
110  buf[i] = tmpChar;
111  }
112 }
113 
114 
122 std::vector<unsigned char> stringToVector(std::string s)
123 {
124  std::vector<unsigned char> result;
125  for (size_t j = 0; j < s.length(); j++)
126  {
127  result.push_back(s[j]);
128  }
129  return result;
130 
131 }
132 
133 
139 static int getDiagnosticErrorCode() // workaround due to compiling error under Visual C++
140 {
141 #ifdef _MSC_VER
142 #undef ERROR
143  return(2);
144 #else
145  return (diagnostic_msgs::DiagnosticStatus::ERROR);
146 #endif
147 }
148 
156 const std::string binScanfGetStringFromVec(std::vector<unsigned char> *replyDummy, int off, long len)
157 {
158  std::string s;
159  s = "";
160  for (int i = 0; i < len; i++)
161  {
162  char ch = (char) ((*replyDummy)[i + off]);
163  s += ch;
164  }
165  return (s);
166 }
167 
168 namespace sick_scan
169 {
177  unsigned char sick_crc8(unsigned char *msgBlock, int len)
178  {
179  unsigned char xorVal = 0x00;
180  int off = 0;
181  for (int i = off; i < len; i++)
182  {
183 
184  unsigned char val = msgBlock[i];
185  xorVal ^= val;
186  }
187  return (xorVal);
188  }
189 
195  std::string stripControl(std::vector<unsigned char> s)
196  {
197  bool isParamBinary = false;
198  int spaceCnt = 0x00;
199  int cnt0x02 = 0;
200 
201  for (size_t i = 0; i < s.size(); i++)
202  {
203  if (s[i] != 0x02)
204  {
205  isParamBinary = false;
206 
207  }
208  else
209  {
210  cnt0x02++;
211  }
212  if (i > 4)
213  {
214  break;
215  }
216  }
217  if (4 == cnt0x02)
218  {
219  isParamBinary = true;
220  }
221  std::string dest;
222  if (isParamBinary == true)
223  {
224  int parseState = 0;
225 
226  unsigned long lenId = 0x00;
227  char szDummy[255] = {0};
228  for (size_t i = 0; i < s.size(); i++)
229  {
230  switch (parseState)
231  {
232  case 0:
233  if (s[i] == 0x02)
234  {
235  dest += "<STX>";
236  }
237  else
238  {
239  dest += "?????";
240  }
241  if (i == 3)
242  {
243  parseState = 1;
244  }
245  break;
246  case 1:
247  lenId |= s[i] << (8 * (7 - i));
248  if (i == 7)
249  {
250  sprintf(szDummy, "<Len=%04lu>", lenId);
251  dest += szDummy;
252  parseState = 2;
253  }
254  break;
255  case 2:
256  {
257  unsigned long dataProcessed = i - 8;
258  if (s[i] == ' ')
259  {
260  spaceCnt++;
261  }
262  if (spaceCnt == 2)
263  {
264  parseState = 3;
265  }
266  dest += s[i];
267  if (dataProcessed >= (lenId - 1))
268  {
269  parseState = 4;
270  }
271 
272  break;
273  }
274 
275  case 3:
276  {
277  char ch = dest[dest.length() - 1];
278  if (ch != ' ')
279  {
280  dest += ' ';
281  }
282  sprintf(szDummy, "0x%02x", s[i]);
283  dest += szDummy;
284 
285  unsigned long dataProcessed = i - 8;
286  if (dataProcessed >= (lenId - 1))
287  {
288  parseState = 4;
289  }
290  break;
291  }
292  case 4:
293  {
294  sprintf(szDummy, " CRC:<0x%02x>", s[i]);
295  dest += szDummy;
296  break;
297  }
298  default:
299  break;
300  }
301  }
302  }
303  else
304  {
305  for (size_t i = 0; i < s.size(); i++)
306  {
307 
308  if (s[i] >= ' ')
309  {
310  // <todo> >= 0x80
311  dest += s[i];
312  }
313  else
314  {
315  switch (s[i])
316  {
317  case 0x02:
318  dest += "<STX>";
319  break;
320  case 0x03:
321  dest += "<ETX>";
322  break;
323  }
324  }
325  }
326  }
327 
328  return (dest);
329  }
330 
336  diagnosticPub_(NULL), parser_(parser)
337  // FIXME All Tims have 15Hz
338  {
340  m_min_intensity = 0.0; // Set range of LaserScan messages to infinity, if intensity < min_intensity (default: 0)
341 
342  setSensorIsRadar(false);
343  init_cmdTables();
344 #ifndef _MSC_VER
345  dynamic_reconfigure::Server<sick_scan::SickScanConfig>::CallbackType f;
346  f = boost::bind(&sick_scan::SickScanCommon::update_config, this, _1, _2);
347  dynamic_reconfigure_server_.setCallback(f);
348 #else
349  // For simulation under MS Visual c++ the update config is switched off
350  {
351  SickScanConfig cfg;
352  ros::NodeHandle tmp("~");
353  double min_angle, max_angle, res_angle;
354  tmp.getParam(PARAM_MIN_ANG, min_angle);
355  tmp.getParam(PARAM_MAX_ANG, max_angle);
356  tmp.getParam(PARAM_RES_ANG, res_angle);
357  cfg.min_ang = min_angle;
358  cfg.max_ang = max_angle;
359  cfg.skip = 0;
360  update_config(cfg);
361  }
362 #endif
363  // datagram publisher (only for debug)
364  ros::NodeHandle pn("~");
365  pn.param<bool>("publish_datagram", publish_datagram_, false);
366  if (publish_datagram_)
367  {
368  datagram_pub_ = nh_.advertise<std_msgs::String>("datagram", 1000);
369  }
370  std::string cloud_topic_val = "cloud";
371  pn.getParam("cloud_topic", cloud_topic_val);
372  std::string frame_id_val = cloud_topic_val;
373  pn.getParam("frame_id", frame_id_val);
374 
375 
376  cloud_marker_ = 0;
377  publish_lferec_ = false;
378  publish_lidoutputstate_ = false;
379  const std::string scannername = parser_->getCurrentParamPtr()->getScannerName();
381  {
382  lferec_pub_ = nh_.advertise<sick_scan::LFErecMsg>(scannername + "/lferec", 100);
383  lidoutputstate_pub_ = nh_.advertise<sick_scan::LIDoutputstateMsg>(scannername + "/lidoutputstate", 100);
384  publish_lferec_ = true;
386  cloud_marker_ = new sick_scan::SickScanMarker(&nh_, scannername + "/marker", frame_id_val); // "cloud");
387  }
388 
389  // Pointcloud2 publisher
390  //
391 
392 
393  ROS_INFO("Publishing laserscan-pointcloud2 to %s", cloud_topic_val.c_str());
394  cloud_pub_ = nh_.advertise<sensor_msgs::PointCloud2>(cloud_topic_val, 100);
395 
396  imuScan_pub_ = nh_.advertise<sensor_msgs::Imu>("imu", 100);
397 
398 
399  Encoder_pub = nh_.advertise<sick_scan::Encoder>("encoder", 100);
400  // scan publisher
401  pub_ = nh_.advertise<sensor_msgs::LaserScan>("scan", 1000);
402 
403 #ifndef _MSC_VER
404  diagnostics_.setHardwareID("none"); // set from device after connection
406  // frequency should be target +- 10%.
409  &expectedFrequency_, 0.1,
410  10),
411  // timestamp delta can be from 0.0 to 1.3x what it ideally is.
413  -1, 1.3 * 1.0 /
415  config_.time_offset));
416  ROS_ASSERT(diagnosticPub_ != NULL);
417 #endif
418  }
419 
425  {
426  /*
427  * Stop streaming measurements
428  */
429  const char requestScanData0[] = {"\x02sEN LMDscandata 0\x03\0"};
430  int result = sendSOPASCommand(requestScanData0, NULL);
431  if (result != 0)
432  {
433  // use printf because we cannot use ROS_ERROR from the destructor
434  printf("\nSOPAS - Error stopping streaming scan data!\n");
435  }
436  else
437  {
438  printf("\nSOPAS - Stopped streaming scan data.\n");
439  }
440 
442  {
443  if(sendSOPASCommand("\x02sEN LFErec 0\x03", NULL) != 0 // TiM781S: deactivate LFErec messages, send "sEN LFErec 0"
444  || sendSOPASCommand("\x02sEN LIDoutputstate 0\x03", NULL) != 0 // TiM781S: deactivate LIDoutputstate messages, send "sEN LIDoutputstate 0"
445  || sendSOPASCommand("\x02sEN LIDinputstate 0\x03", NULL) != 0) // TiM781S: deactivate LIDinputstate messages, send "sEN LIDinputstate 0"
446  {
447  // use printf because we cannot use ROS_ERROR from the destructor
448  printf("\nSOPAS - Error stopping streaming LFErec, LIDoutputstate and LIDinputstate messages!\n");
449  }
450  else
451  {
452  printf("\nSOPAS - Stopped streaming LFErec, LIDoutputstate and LIDinputstate messages\n");
453  }
454  }
455 
456  return result;
457  }
458 
464  unsigned long SickScanCommon::convertBigEndianCharArrayToUnsignedLong(const unsigned char *vecArr)
465  {
466  unsigned long val = 0;
467  for (int i = 0; i < 4; i++)
468  {
469  val = val << 8;
470  val |= vecArr[i];
471  }
472  return (val);
473  }
474 
475 
481  int sick_scan::SickScanCommon::checkForBinaryAnswer(const std::vector<unsigned char> *reply)
482  {
483  int retVal = -1;
484 
485  if (reply == NULL)
486  {
487  }
488  else
489  {
490  if (reply->size() < 8)
491  {
492  retVal = -1;
493  }
494  else
495  {
496  const unsigned char *ptr = &((*reply)[0]);
497  unsigned binId = convertBigEndianCharArrayToUnsignedLong(ptr);
498  unsigned cmdLen = convertBigEndianCharArrayToUnsignedLong(ptr + 4);
499  if (binId == 0x02020202)
500  {
501  int replyLen = reply->size();
502  if (replyLen == 8 + cmdLen + 1)
503  {
504  retVal = cmdLen;
505  }
506  }
507  }
508  }
509  return (retVal);
510 
511  }
512 
513 
519  {
520  /*
521  * Set Maintenance access mode to allow reboot to be sent
522  */
523  std::vector<unsigned char> access_reply;
524  // changed from "03" to "3"
525  int result = sendSOPASCommand("\x02sMN SetAccessMode 3 F4724744\x03\0", &access_reply);
526  if (result != 0)
527  {
528  ROS_ERROR("SOPAS - Error setting access mode");
529  diagnostics_.broadcast(getDiagnosticErrorCode(), "SOPAS - Error setting access mode.");
530  return false;
531  }
532  std::string access_reply_str = replyToString(access_reply);
533  if (access_reply_str != "sAN SetAccessMode 1")
534  {
535  ROS_ERROR_STREAM("SOPAS - Error setting access mode, unexpected response : " << access_reply_str);
536  diagnostics_.broadcast(getDiagnosticErrorCode(), "SOPAS - Error setting access mode.");
537  return false;
538  }
539 
540  /*
541  * Send reboot command
542  */
543  std::vector<unsigned char> reboot_reply;
544  result = sendSOPASCommand("\x02sMN mSCreboot\x03\0", &reboot_reply);
545  if (result != 0)
546  {
547  ROS_ERROR("SOPAS - Error rebooting scanner");
548  diagnostics_.broadcast(getDiagnosticErrorCode(), "SOPAS - Error rebooting device.");
549  return false;
550  }
551  std::string reboot_reply_str = replyToString(reboot_reply);
552  if (reboot_reply_str != "sAN mSCreboot")
553  {
554  ROS_ERROR_STREAM("SOPAS - Error rebooting scanner, unexpected response : " << reboot_reply_str);
555  diagnostics_.broadcast(getDiagnosticErrorCode(), "SOPAS - Error setting access mode.");
556  return false;
557  }
558 
559  ROS_INFO("SOPAS - Rebooted scanner");
560 
561  // Wait a few seconds after rebooting
562  ros::Duration(15.0).sleep();
563 
564  return true;
565  }
566 
571  {
572  delete cloud_marker_;
573  delete diagnosticPub_;
574 
575  printf("sick_scan driver exiting.\n");
576  }
577 
578 
584  std::string SickScanCommon::generateExpectedAnswerString(const std::vector<unsigned char> requestStr)
585  {
586  std::string expectedAnswer = "";
587  int i = 0;
588  char cntWhiteCharacter = 0;
589  int initalTokenCnt = 2; // number of initial token to identify command
590  std::map<std::string, int> specialTokenLen;
591  char firstToken[1024] = {0};
592  specialTokenLen["sRI"] = 1; // for SRi-Command only the first token identify the command
593  std::string tmpStr = "";
594  int cnt0x02 = 0;
595  bool isBinary = false;
596  for (size_t i = 0; i < 4; i++)
597  {
598  if (i < requestStr.size())
599  {
600  if (requestStr[i] == 0x02)
601  {
602  cnt0x02++;
603  }
604 
605  }
606  }
607 
608  int iStop = requestStr.size(); // copy string until end of string
609  if (cnt0x02 == 4)
610  {
611 
612  int cmdLen = 0;
613  for (int i = 0; i < 4; i++)
614  {
615  cmdLen |= cmdLen << 8;
616  cmdLen |= requestStr[i + 4];
617  }
618  iStop = cmdLen + 8;
619  isBinary = true;
620 
621  }
622  int iStart = (isBinary == true) ? 8 : 0;
623  for (int i = iStart; i < iStop; i++)
624  {
625  tmpStr += (char) requestStr[i];
626  }
627  if (isBinary)
628  {
629  tmpStr = "\x2" + tmpStr;
630  }
631  if (sscanf(tmpStr.c_str(), "\x2%s", firstToken) == 1)
632  {
633  if (specialTokenLen.find(firstToken) != specialTokenLen.end())
634  {
635  initalTokenCnt = specialTokenLen[firstToken];
636 
637  }
638  }
639 
640  for (int i = iStart; i < iStop; i++)
641  {
642  if ((requestStr[i] == ' ') || (requestStr[i] == '\x3'))
643  {
644  cntWhiteCharacter++;
645  }
646  if (cntWhiteCharacter >= initalTokenCnt)
647  {
648  break;
649  }
650  if (requestStr[i] == '\x2')
651  {
652  }
653  else
654  {
655  expectedAnswer += requestStr[i];
656  }
657  }
658 
662  std::map<std::string, std::string> keyWordMap;
663  keyWordMap["sWN"] = "sWA";
664  keyWordMap["sRN"] = "sRA";
665  keyWordMap["sRI"] = "sRA";
666  keyWordMap["sMN"] = "sAN";
667  keyWordMap["sEN"] = "sEA";
668 
669  for (std::map<std::string, std::string>::iterator it = keyWordMap.begin(); it != keyWordMap.end(); it++)
670  {
671 
672  std::string keyWord = it->first;
673  std::string newKeyWord = it->second;
674 
675  size_t pos = expectedAnswer.find(keyWord);
676  if (pos == std::string::npos)
677  {
678 
679  }
680  else
681  {
682  if (pos == 0) // must be 0, if keyword has been found
683  {
684  expectedAnswer.replace(pos, keyWord.length(), newKeyWord);
685  }
686  else
687  {
688  ROS_WARN("Unexpected position of key identifier.\n");
689  }
690  }
691 
692  }
693  return (expectedAnswer);
694 
695  }
696 
704  int SickScanCommon::sendSopasAndCheckAnswer(std::string requestStr, std::vector<unsigned char> *reply, int cmdId = -1)
705  {
706  std::vector<unsigned char> requestStringVec;
707  for (size_t i = 0; i < requestStr.length(); i++)
708  {
709  requestStringVec.push_back(requestStr[i]);
710  }
711  int retCode = sendSopasAndCheckAnswer(requestStringVec, reply, cmdId);
712  return (retCode);
713  }
714 
722  int SickScanCommon::sendSopasAndCheckAnswer(std::vector<unsigned char> requestStr, std::vector<unsigned char> *reply,
723  int cmdId = -1)
724  {
725  std::lock_guard<std::mutex> send_lock_guard(sopasSendMutex); // lock send mutex in case of asynchronous service calls
726 
727  std::string cmdStr = "";
728  int cmdLen = 0;
729  for (size_t i = 0; i < requestStr.size(); i++)
730  {
731  cmdLen++;
732  cmdStr += (char) requestStr[i];
733  }
734  int result = -1;
735 
736  std::string errString;
737  if (cmdId == -1)
738  {
739  errString = "Error unexpected Sopas Answer for request " + stripControl(requestStr);
740  }
741  else
742  {
743  errString = this->sopasCmdErrMsg[cmdId];
744  }
745 
746  std::string expectedAnswer = generateExpectedAnswerString(requestStr);
747 
748  // send sopas cmd
749 
750  std::string reqStr = replyToString(requestStr);
751  ROS_INFO("Sending : %s", stripControl(requestStr).c_str());
752  result = sendSOPASCommand(cmdStr.c_str(), reply, cmdLen);
753  std::string replyStr = replyToString(*reply);
754  std::vector<unsigned char> replyVec;
755  replyStr = "<STX>" + replyStr + "<ETX>";
756  replyVec = stringToVector(replyStr);
757  ROS_INFO("Receiving: %s", stripControl(replyVec).c_str());
758 
759  if (result != 0)
760  {
761  std::string tmpStr = "SOPAS Communication -" + errString;
762  ROS_INFO("%s\n", tmpStr.c_str());
764  }
765  else
766  {
767  std::string answerStr = replyToString(*reply);
768  std::string searchPattern = generateExpectedAnswerString(requestStr);
769 
770  if (answerStr.find(searchPattern) != std::string::npos)
771  {
772  result = 0;
773  }
774  else
775  {
776  if (cmdId == CMD_START_IMU_DATA)
777  {
778  ROS_INFO("IMU-Data transfer started. No checking of reply to avoid confusing with LMD Scandata\n");
779  result = 0;
780  }
781  else
782  {
783  std::string tmpMsg = "Error Sopas answer mismatch " + errString + "Answer= >>>" + answerStr + "<<<";
784  ROS_ERROR("%s\n", tmpMsg.c_str());
786  result = -1;
787  }
788  }
789  }
790  return result;
791 
792  }
793 
800  {
801  readTimeOutInMs = timeOutInMs;
802  }
803 
810  {
811  return (readTimeOutInMs);
812  }
813 
820  {
821  return m_protocolId;
822  }
823 
829  {
830  m_protocolId = cola_dialect_id;
831  }
832 
838  {
839  int result = init_device();
840  if (result != 0)
841  {
842  ROS_FATAL("Failed to init device: %d", result);
843  return result;
844  }
845 
846  result = init_scanner();
847  if (result != 0)
848  {
849  ROS_INFO("Failed to init scanner Error Code: %d\nWaiting for timeout...\n"
850  "If the communication mode set in the scanner memory is different from that used by the driver, the scanner's communication mode is changed.\n"
851  "This requires a restart of the TCP-IP connection, which can extend the start time by up to 30 seconds. There are two ways to prevent this:\n"
852  "1. [Recommended] Set the communication mode with the SOPAS ET software to binary and save this setting in the scanner's EEPROM.\n"
853  "2. Use the parameter \"use_binary_protocol\" to overwrite the default settings of the driver.",
854  result);
855  }
856 
857  return result;
858  }
859 
860 
866  {
868  sopasCmdMaskVec.resize(
869  SickScanCommon::CMD_END); // you for cmd with variable content. sprintf should print into corresponding sopasCmdVec
870  sopasCmdErrMsg.resize(
871  SickScanCommon::CMD_END); // you for cmd with variable content. sprintf should print into corresponding sopasCmdVec
875 
876  std::string unknownStr = "Command or Error message not defined";
877  for (int i = 0; i < SickScanCommon::CMD_END; i++)
878  {
879  sopasCmdVec[i] = unknownStr;
880  sopasCmdMaskVec[i] = unknownStr; // for cmd with variable content. sprintf should print into corresponding sopasCmdVec
881  sopasCmdErrMsg[i] = unknownStr;
882  sopasReplyVec[i] = unknownStr;
883  sopasReplyStrVec[i] = unknownStr;
884  }
885 
886  sopasCmdVec[CMD_DEVICE_IDENT_LEGACY] = "\x02sRI 0\x03\0";
887  sopasCmdVec[CMD_DEVICE_IDENT] = "\x02sRN DeviceIdent\x03\0";
888  sopasCmdVec[CMD_REBOOT] = "\x02sMN mSCreboot\x03";
889  sopasCmdVec[CMD_WRITE_EEPROM] = "\x02sMN mEEwriteall\x03";
890  sopasCmdVec[CMD_SERIAL_NUMBER] = "\x02sRN SerialNumber\x03\0";
891  sopasCmdVec[CMD_FIRMWARE_VERSION] = "\x02sRN FirmwareVersion\x03\0";
892  sopasCmdVec[CMD_DEVICE_STATE] = "\x02sRN SCdevicestate\x03\0";
893  sopasCmdVec[CMD_OPERATION_HOURS] = "\x02sRN ODoprh\x03\0";
894  sopasCmdVec[CMD_POWER_ON_COUNT] = "\x02sRN ODpwrc\x03\0";
895  sopasCmdVec[CMD_LOCATION_NAME] = "\x02sRN LocationName\x03\0";
896  sopasCmdVec[CMD_ACTIVATE_STANDBY] = "\x02sMN LMCstandby\x03";
897  sopasCmdVec[CMD_SET_ACCESS_MODE_3] = "\x02sMN SetAccessMode 3 F4724744\x03\0";
898  sopasCmdVec[CMD_SET_ACCESS_MODE_3_SAFETY_SCANNER] = "\x02sMN SetAccessMode 3 6FD62C05\x03\0";
899  sopasCmdVec[CMD_GET_OUTPUT_RANGES] = "\x02sRN LMPoutputRange\x03";
900  sopasCmdVec[CMD_RUN] = "\x02sMN Run\x03\0";
901  sopasCmdVec[CMD_STOP_SCANDATA] = "\x02sEN LMDscandata 0\x03";
902  sopasCmdVec[CMD_START_SCANDATA] = "\x02sEN LMDscandata 1\x03";
903  sopasCmdVec[CMD_START_RADARDATA] = "\x02sEN LMDradardata 1\x03";
904  sopasCmdVec[CMD_ACTIVATE_NTP_CLIENT] = "\x02sWN TSCRole 1\x03";
905  sopasCmdVec[CMD_SET_NTP_INTERFACE_ETH] = "\x02sWN TSCTCInterface 0\x03";
906 
907  /*
908  * Radar specific commands
909  */
910  sopasCmdVec[CMD_SET_TRANSMIT_RAWTARGETS_ON] = "\x02sWN TransmitTargets 1\x03"; // transmit raw target for radar
911  sopasCmdVec[CMD_SET_TRANSMIT_RAWTARGETS_OFF] = "\x02sWN TransmitTargets 0\x03"; // do not transmit raw target for radar
912  sopasCmdVec[CMD_SET_TRANSMIT_OBJECTS_ON] = "\x02sWN TransmitObjects 1\x03"; // transmit objects from radar tracking
913  sopasCmdVec[CMD_SET_TRANSMIT_OBJECTS_OFF] = "\x02sWN TransmitObjects 0\x03"; // do not transmit objects from radar tracking
914  sopasCmdVec[CMD_SET_TRACKING_MODE_0] = "\x02sWN TCTrackingMode 0\x03"; // set object tracking mode to BASIC
915  sopasCmdVec[CMD_SET_TRACKING_MODE_1] = "\x02sWN TCTrackingMode 1\x03"; // set object tracking mode to TRAFFIC
916 
917 
918  sopasCmdVec[CMD_LOAD_APPLICATION_DEFAULT] = "\x02sMN mSCloadappdef\x03"; // load application default
919  sopasCmdVec[CMD_DEVICE_TYPE] = "\x02sRN DItype\x03"; // ask for radar device type
920  sopasCmdVec[CMD_ORDER_NUMBER] = "\x02sRN OrdNum\x03"; // ask for radar order number
921 
922 
923 
924  sopasCmdVec[CMD_START_MEASUREMENT] = "\x02sMN LMCstartmeas\x03";
925  sopasCmdVec[CMD_STOP_MEASUREMENT] = "\x02sMN LMCstopmeas\x03";
926  sopasCmdVec[CMD_APPLICATION_MODE_FIELD_ON] = "\x02sWN SetActiveApplications 1 FEVL 1\x03"; // <STX>sWN{SPC}SetActiveApplications{SPC}1{SPC}FEVL{SPC}1<ETX>
927  sopasCmdVec[CMD_APPLICATION_MODE_FIELD_OFF] = "\x02sWN SetActiveApplications 1 FEVL 0\x03"; // <STX>sWN{SPC}SetActiveApplications{SPC}1{SPC}FEVL{SPC}1<ETX>
928  sopasCmdVec[CMD_APPLICATION_MODE_RANGING_ON] = "\x02sWN SetActiveApplications 1 RANG 1\x03";
929  sopasCmdVec[CMD_SET_TO_COLA_A_PROTOCOL] = "\x02sWN EIHstCola 0\x03";
930  sopasCmdVec[CMD_GET_PARTIAL_SCANDATA_CFG] = "\x02sRN LMDscandatacfg\x03";//<STX>sMN{SPC}mLMPsetscancfg{SPC } +5000{SPC}+1{SPC}+5000{SPC}-450000{SPC}+2250000<ETX>
931  sopasCmdVec[CMD_GET_PARTIAL_SCAN_CFG] = "\x02sRN LMPscancfg\x03";
932  sopasCmdVec[CMD_SET_TO_COLA_B_PROTOCOL] = "\x02sWN EIHstCola 1\x03";
933 
934  sopasCmdVec[CMD_STOP_IMU_DATA] = "\x02sEN InertialMeasurementUnit 0\x03";
935  sopasCmdVec[CMD_START_IMU_DATA] = "\x02sEN InertialMeasurementUnit 1\x03";
936  sopasCmdVec[CMD_SET_ENCODER_MODE_NO] = "\x02sWN LICencset 0\x03";
937  sopasCmdVec[CMD_SET_ENCODER_MODE_SI] = "\x02sWN LICencset 1\x03";
938  sopasCmdVec[CMD_SET_ENCODER_MODE_DP] = "\x02sWN LICencset 2\x03";
939  sopasCmdVec[CMD_SET_ENCODER_MODE_DL] = "\x02sWN LICencset 3\x03";
940  sopasCmdVec[CMD_SET_INCREMENTSOURCE_ENC] = "\x02sWN LICsrc 1\x03";
941  sopasCmdVec[CMD_SET_3_4_TO_ENCODER] = "\x02sWN DO3And4Fnc 1\x03";
942  //TODO remove this and add param
943  sopasCmdVec[CMD_SET_ENOCDER_RES_1] = "\x02sWN LICencres 1\x03";
944 
945  sopasCmdVec[CMD_SET_SCANDATACONFIGNAV] = "\x02sMN mLMPsetscancfg +2000 +1 +7500 +3600000 0 +2500 0 0 +2500 0 0 +2500 0 0\x03";
946  /*
947  * Special configuration for NAV Scanner
948  * in hex
949  * sMN mLMPsetscancfg 0320 01 09C4 0 0036EE80 09C4 0 0 09C4 0 0 09C4 0 0
950  * | | | | | | | | | | | | | |
951  * | | | | | | | | | | | | | +->
952  * | | | | | | | | | | | | +---> 0x0 --> 0 -> 0° start ang for sector 4
953  * | | | | | | | | | | | +-------> 0x09c4 --> 2500 -> 0.25° deg ang res for Sector 4
954  * | | | | | | | | | | +----------> 0x0 --> 0 -> 0° start ang for sector 4
955  * | | | | | | | | | +------------> 0x0 --> 0 -> 0° start ang for sector 3
956  * | | | | | | | | +----------------> 0x09c4 --> 2500 -> 0.25° deg ang res for Sector 3
957  * | | | | | | | +-------------------> 0x0 --> 0 -> 0° start ang for sector 2
958  * | | | | | | +---------------------> 0x0 --> 0 -> 0° start ang for sector 2
959  * | | | | | +------------------------> 0x09c4 --> 2500 -> 0.25° Deg ang res for Sector 2
960  * | | | | +-------------------------------> 0x36EE80h--> 3600000-> 360° stop ang for sector 1
961  * | | | +-------------------------------------> 0x0 --> 0 -> 0° Start ang for sector 1
962  * | | +----------------------------------------> 0x09c4 --> 2500 -> 0.25° Deg ang res for Sector 1
963  * | +---------------------------------------------> 0x01 --> 01 -> 1 Number of active sectors
964  * +------------------------------------------------> 0x0320 --> 0800 -> 8 Hz scanfreq
965  */
966  // 0320 01 09C4 0 0036EE80 09C4 0 0 09C4 0 0 09C4 0 0
967 
968  sopasCmdVec[CMD_SET_LFEREC_ACTIVE] = "\x02sEN LFErec 1\x03"; // TiM781S: activate LFErec messages, send "sEN LFErec 1"
969  sopasCmdVec[CMD_SET_LID_OUTPUTSTATE_ACTIVE] = "\x02sEN LIDoutputstate 1\x03"; // TiM781S: activate LIDoutputstate messages, send "sEN LIDoutputstate 1"
970  sopasCmdVec[CMD_SET_LID_INPUTSTATE_ACTIVE] = "\x02sEN LIDinputstate 1\x03"; // TiM781S: activate LIDinputstate messages, send "sEN LIDinputstate 1"
971 
972  /*
973  * Angle Compensation Command
974  *
975  */
976  sopasCmdVec[CMD_GET_ANGLE_COMPENSATION_PARAM] = "\x02sRN MCAngleCompSin\x03";
977  // defining cmd mask for cmds with variable input
978  sopasCmdMaskVec[CMD_SET_PARTIAL_SCAN_CFG] = "\x02sMN mLMPsetscancfg %+d 1 %+d 0 0\x03";//scanfreq [1/100 Hz],angres [1/10000°],
979  sopasCmdMaskVec[CMD_SET_PARTICLE_FILTER] = "\x02sWN LFPparticle %d %d\x03";
980  sopasCmdMaskVec[CMD_SET_MEAN_FILTER] = "\x02sWN LFPmeanfilter %d %d 0\x03";
981  sopasCmdMaskVec[CMD_ALIGNMENT_MODE] = "\x02sWN MMAlignmentMode %d\x03";
982  sopasCmdMaskVec[CMD_APPLICATION_MODE] = "\x02sWN SetActiveApplications 1 %s %d\x03";
983  sopasCmdMaskVec[CMD_SET_OUTPUT_RANGES] = "\x02sWN LMPoutputRange 1 %X %X %X\x03";
984  sopasCmdMaskVec[CMD_SET_OUTPUT_RANGES_NAV3] = "\x02sWN LMPoutputRange 1 %X %X %X %X %X %X %X %X %X %X %X %X\x03";
985  //sopasCmdMaskVec[CMD_SET_PARTIAL_SCANDATA_CFG]= "\x02sWN LMDscandatacfg %02d 00 %d 00 %d 0 %d 0 0 0 1 +1\x03"; //outputChannelFlagId,rssiFlag, rssiResolutionIs16Bit ,EncoderSetings
986  sopasCmdMaskVec[CMD_SET_PARTIAL_SCANDATA_CFG] = "\x02sWN LMDscandatacfg %02d 00 %d %d 0 0 %02d 0 0 0 1 1\x03";//outputChannelFlagId,rssiFlag, rssiResolutionIs16Bit ,EncoderSetings
987  sopasCmdMaskVec[CMD_GET_SAFTY_FIELD_CFG] = "\x02sRN field%03d\x03";
988  /*
989  configuration
990  * in ASCII
991  * sWN LMDscandatacfg %02d 00 %d %d 0 %02d 0 0 0 1 1
992  * | | | | | | | | | | +----------> Output rate -> All scans: 1--> every 1 scan
993  * | | | | | | | | +----------------> Time ->True (unused in Data Processing)
994  * | | | | | | | +-------------------> Comment ->False
995  * | | | | | | +------------------------> Device Name ->False
996  * | | | | | +---------------------------> Position ->False
997  * | | | | +----------------------------------> Encoder ->Param set by Mask
998  * | | | +----------------------------------------> Unit of Remission->Always 0
999  * | | +--------------------------------------------> RSSi Resolution ->0 8Bit 1 16 Bit
1000  * | +-------------------------------------------------> Remission data ->Param set by Mask 0 False 1 True
1001  * +--------------------------------------------------------> Data channel ->Param set by Mask
1002 */
1003 
1004  sopasCmdMaskVec[CMD_SET_ECHO_FILTER] = "\x02sWN FREchoFilter %d\x03";
1005  sopasCmdMaskVec[CMD_SET_NTP_UPDATETIME] = "\x02sWN TSCTCupdatetime %d\x03";
1006  sopasCmdMaskVec[CMD_SET_NTP_TIMEZONE] = "sWN TSCTCtimezone %d";
1007  sopasCmdMaskVec[CMD_SET_IP_ADDR] = "\x02sWN EIIpAddr %02X %02X %02X %02X\x03";
1008  sopasCmdMaskVec[CMD_SET_NTP_SERVER_IP_ADDR] = "\x02sWN TSCTCSrvAddr %02X %02X %02X %02X\x03";
1009  sopasCmdMaskVec[CMD_SET_GATEWAY] = "\x02sWN EIgate %02X %02X %02X %02X\x03";
1010  sopasCmdMaskVec[CMD_SET_ENCODER_RES] = "\x02sWN LICencres %f\x03";
1011 
1012  //error Messages
1013  sopasCmdErrMsg[CMD_DEVICE_IDENT_LEGACY] = "Error reading device ident";
1014  sopasCmdErrMsg[CMD_DEVICE_IDENT] = "Error reading device ident for MRS-family";
1015  sopasCmdErrMsg[CMD_SERIAL_NUMBER] = "Error reading SerialNumber";
1016  sopasCmdErrMsg[CMD_FIRMWARE_VERSION] = "Error reading FirmwareVersion";
1017  sopasCmdErrMsg[CMD_DEVICE_STATE] = "Error reading SCdevicestate";
1018  sopasCmdErrMsg[CMD_OPERATION_HOURS] = "Error reading operation hours";
1019  sopasCmdErrMsg[CMD_POWER_ON_COUNT] = "Error reading operation power on counter";
1020  sopasCmdErrMsg[CMD_LOCATION_NAME] = "Error reading Locationname";
1021  sopasCmdErrMsg[CMD_ACTIVATE_STANDBY] = "Error acticvating Standby";
1022  sopasCmdErrMsg[CMD_SET_PARTICLE_FILTER] = "Error setting Particelefilter";
1023  sopasCmdErrMsg[CMD_SET_MEAN_FILTER] = "Error setting Meanfilter";
1024  sopasCmdErrMsg[CMD_ALIGNMENT_MODE] = "Error setting Alignmentmode";
1025  sopasCmdErrMsg[CMD_APPLICATION_MODE] = "Error setting Meanfilter";
1026  sopasCmdErrMsg[CMD_SET_ACCESS_MODE_3] = "Error Access Mode";
1028  sopasCmdErrMsg[CMD_SET_OUTPUT_RANGES] = "Error setting angular ranges";
1029  sopasCmdErrMsg[CMD_GET_OUTPUT_RANGES] = "Error reading angle range";
1030  sopasCmdErrMsg[CMD_RUN] = "FATAL ERROR unable to start RUN mode!";
1031  sopasCmdErrMsg[CMD_SET_PARTIAL_SCANDATA_CFG] = "Error setting Scandataconfig";
1032  sopasCmdErrMsg[CMD_STOP_SCANDATA] = "Error stopping scandata output";
1033  sopasCmdErrMsg[CMD_START_SCANDATA] = "Error starting Scandata output";
1034  sopasCmdErrMsg[CMD_SET_IP_ADDR] = "Error setting IP address";
1035  sopasCmdErrMsg[CMD_SET_GATEWAY] = "Error setting gateway";
1036  sopasCmdErrMsg[CMD_REBOOT] = "Error rebooting the device";
1037  sopasCmdErrMsg[CMD_WRITE_EEPROM] = "Error writing data to EEPRom";
1038  sopasCmdErrMsg[CMD_ACTIVATE_NTP_CLIENT] = "Error activating NTP client";
1039  sopasCmdErrMsg[CMD_SET_NTP_INTERFACE_ETH] = "Error setting NTP interface to ETH";
1040  sopasCmdErrMsg[CMD_SET_NTP_SERVER_IP_ADDR] = "Error setting NTP server Adress";
1041  sopasCmdErrMsg[CMD_SET_NTP_UPDATETIME] = "Error setting NTP update time";
1042  sopasCmdErrMsg[CMD_SET_NTP_TIMEZONE] = "Error setting NTP timezone";
1043  sopasCmdErrMsg[CMD_SET_ENCODER_MODE] = "Error activating encoder in single incremnt mode";
1044  sopasCmdErrMsg[CMD_SET_INCREMENTSOURCE_ENC] = "Error seting encoder increment source to Encoder";
1045  sopasCmdErrMsg[CMD_SET_SCANDATACONFIGNAV] = "Error setting scandata config";
1046  sopasCmdErrMsg[CMD_SET_LFEREC_ACTIVE] = "Error activating LFErec messages";
1047  sopasCmdErrMsg[CMD_SET_LID_OUTPUTSTATE_ACTIVE] = "Error activating LIDoutputstate messages";
1048  sopasCmdErrMsg[CMD_SET_LID_INPUTSTATE_ACTIVE] = "Error activating LIDinputstate messages";
1049  // ML: Add here more useful cmd and mask entries
1050 
1051  // After definition of command, we specify the command sequence for scanner initalisation
1052 
1053 
1054 
1055 
1056 
1058  {
1060  }
1061  else
1062  {
1064  }
1065 
1067  {
1069  }
1070  else
1071  {
1072  //for binary Mode Testing
1074  }
1075 
1076 
1078  {
1080  }
1081 
1082  /*
1083  * NAV2xx supports angle compensation
1084  */
1085  bool isNav2xxOr3xx = false;
1087  {
1088  isNav2xxOr3xx = true;
1089  }
1091  {
1092  isNav2xxOr3xx = true;
1093  }
1094  if (isNav2xxOr3xx)
1095  {
1097  }
1098 
1099 
1100  bool tryToStopMeasurement = true;
1102  {
1103  tryToStopMeasurement = false;
1104  // do not stop measurement for TiM571 otherwise the scanner would not start after start measurement
1105  // do not change the application - not supported for TiM5xx
1106  }
1107  if (parser_->getCurrentParamPtr()->getDeviceIsRadar() == true)
1108  {
1109  sopasCmdChain.push_back(CMD_LOAD_APPLICATION_DEFAULT); // load application default for radar
1110 
1111  tryToStopMeasurement = false;
1112  // do not stop measurement for RMS320 - the RMS320 does not support the stop command
1113  }
1114  if (tryToStopMeasurement)
1115  {
1117  int numberOfLayers = parser_->getCurrentParamPtr()->getNumberOfLayers();
1118 
1119  switch (numberOfLayers)
1120  {
1121  case 4:
1124  sopasCmdChain.push_back(CMD_DEVICE_IDENT);
1125  sopasCmdChain.push_back(CMD_SERIAL_NUMBER);
1126 
1127  break;
1128  case 24:
1129  // just measuring - Application setting not supported
1130  // "Old" device ident command "SRi 0" not supported
1131  sopasCmdChain.push_back(CMD_DEVICE_IDENT);
1132  break;
1133 
1134  default:
1138 
1139  sopasCmdChain.push_back(CMD_SERIAL_NUMBER);
1140  break;
1141  }
1142 
1143  }
1144  sopasCmdChain.push_back(CMD_FIRMWARE_VERSION); // read firmware
1145  sopasCmdChain.push_back(CMD_DEVICE_STATE); // read device state
1146  sopasCmdChain.push_back(CMD_OPERATION_HOURS); // read operation hours
1147  sopasCmdChain.push_back(CMD_POWER_ON_COUNT); // read power on count
1148  sopasCmdChain.push_back(CMD_LOCATION_NAME); // read location name
1150  {
1152  }
1153 
1154  return (0);
1155 
1156  }
1157 
1158 
1164  {
1165 
1166  const int MAX_STR_LEN = 1024;
1167 
1168  int maxNumberOfEchos = 1;
1169 
1170 
1171  maxNumberOfEchos = this->parser_->getCurrentParamPtr()->getNumberOfMaximumEchos(); // 1 for TIM 571, 3 for MRS1104, 5 for 6000
1172 
1173 
1174  bool rssiFlag = false;
1175  bool rssiResolutionIs16Bit = true; //True=16 bit Flase=8bit
1176  int activeEchos = 0;
1177  ros::NodeHandle pn("~");
1178 
1179  pn.getParam("intensity", rssiFlag);
1180  pn.getParam("intensity_resolution_16bit", rssiResolutionIs16Bit);
1181  pn.getParam("min_intensity", m_min_intensity); // Set range of LaserScan messages to infinity, if intensity < min_intensity (default: 0)
1182 
1183  //check new ip adress and add cmds to write ip to comand chain
1184  std::string sNewIPAddr = "";
1185  boost::asio::ip::address_v4 ipNewIPAddr;
1186  bool setNewIPAddr = false;
1187  setNewIPAddr = pn.getParam("new_IP_address", sNewIPAddr);
1188  if (setNewIPAddr)
1189  {
1190  boost::system::error_code ec;
1191  ipNewIPAddr = boost::asio::ip::address_v4::from_string(sNewIPAddr, ec);
1192  if (ec == boost::system::errc::success)
1193  {
1194  sopasCmdChain.clear();
1196  }
1197  else
1198  {
1199  setNewIPAddr = false;
1200  ROS_ERROR("ERROR: IP ADDRESS could not be parsed Boost Error %s:%d", ec.category().name(), ec.value());;
1201  }
1202 
1203  }
1204  std::string sNTPIpAdress = "";
1205  boost::asio::ip::address_v4 NTPIpAdress;
1206  bool setUseNTP = false;
1207  setUseNTP = pn.getParam("ntp_server_address", sNTPIpAdress);
1208  if (setUseNTP)
1209  {
1210  boost::system::error_code ec;
1211  NTPIpAdress = boost::asio::ip::address_v4::from_string(sNTPIpAdress, ec);
1212  if (ec != boost::system::errc::success)
1213  {
1214  setUseNTP = false;
1215  ROS_ERROR("ERROR: NTP Server IP ADDRESS could not be parsed Boost Error %s:%d", ec.category().name(),
1216  ec.value());;
1217  }
1218  }
1219 
1220  this->parser_->getCurrentParamPtr()->setIntensityResolutionIs16Bit(rssiResolutionIs16Bit);
1221  // parse active_echos entry and set flag array
1222  pn.getParam("active_echos", activeEchos);
1223 
1224  ROS_INFO("Parameter setting for <active_echo: %d>", activeEchos);
1225  std::vector<bool> outputChannelFlag;
1226  outputChannelFlag.resize(maxNumberOfEchos);
1227  int i;
1228  int numOfFlags = 0;
1229  for (i = 0; i < outputChannelFlag.size(); i++)
1230  {
1231  /*
1232  After consultation with the company SICK,
1233  all flags are set to true because the firmware currently does not support single selection of targets.
1234  The selection of the echoes takes place via FREchoFilter.
1235  */
1236  /* former implementation
1237  if (activeEchos & (1 << i))
1238  {
1239  outputChannelFlag[i] = true;
1240  numOfFlags++;
1241  }
1242  else
1243  {
1244  outputChannelFlag[i] = false;
1245  }
1246  */
1247  outputChannelFlag[i] = true; // always true (see comment above)
1248  numOfFlags++;
1249  }
1250 
1251  if (numOfFlags == 0) // Fallback-Solution
1252  {
1253  outputChannelFlag[0] = true;
1254  numOfFlags = 1;
1255  ROS_WARN("Activate at least one echo.");
1256  }
1257 
1258  //================== DEFINE ENCODER SETTING ==========================
1259  int EncoderSetings = -1; //Do not use encoder commands as default
1260  pn.getParam("encoder_mode", EncoderSetings);
1261  this->parser_->getCurrentParamPtr()->setEncoderMode((int8_t) EncoderSetings);
1262  if (parser_->getCurrentParamPtr()->getEncoderMode() >= 0)
1263  {
1265  {
1266  case 0:
1271  break;
1272  case 1:
1277  break;
1278  case 2:
1283  break;
1284  case 3:
1289  break;
1290  }
1291  }
1292  int result;
1293  //================== DEFINE ANGULAR SETTING ==========================
1294  int angleRes10000th = 0;
1295  int angleStart10000th = 0;
1296  int angleEnd10000th = 0;
1297 
1298 
1299  // Mainloop for initial SOPAS cmd-Handling
1300  //
1301  // To add new commands do the followings:
1302  // 1. Define new enum-entry in the enumumation "enum SOPAS_CMD" in the file sick_scan_common.h
1303  // 2. Define new command sequence in the member function init_cmdTables()
1304  // 3. Add cmd-id in the sopasCmdChain to trigger sending of command.
1305  // 4. Add handling of reply by using for the pattern "REPLY_HANDLING" in this code and adding a new case instruction.
1306  // That's all!
1307 
1308 
1309  volatile bool useBinaryCmd = false;
1310  if (this->parser_->getCurrentParamPtr()->getUseBinaryProtocol()) // hard coded for every scanner type
1311  {
1312  useBinaryCmd = true; // shall we talk ascii or binary with the scanner type??
1313  }
1314 
1315  bool useBinaryCmdNow = false;
1316  int maxCmdLoop = 2; // try binary and ascii during startup
1317 
1318  const int shortTimeOutInMs = 5000; // during startup phase to check binary or ascii
1319  const int defaultTimeOutInMs = 120000; // standard time out 120 sec.
1320 
1321  setReadTimeOutInMs(shortTimeOutInMs);
1322 
1323  bool restartDueToProcolChange = false;
1324 
1325  /* NAV310 needs special handling */
1326  /* The NAV310 does not support LMDscandatacfg and rotates clockwise. */
1327  /* The X-axis shows backwards */
1328  bool NAV3xxOutputRangeSpecialHandling=false;
1330  {
1331  NAV3xxOutputRangeSpecialHandling = true;
1332  }
1333 
1334  for (size_t i = 0; i < this->sopasCmdChain.size(); i++)
1335  {
1336  ros::Duration(0.2).sleep(); // could maybe removed
1337 
1338  int cmdId = sopasCmdChain[i]; // get next command
1339  std::string sopasCmd = sopasCmdVec[cmdId];
1340  std::vector<unsigned char> replyDummy;
1341  std::vector<unsigned char> reqBinary;
1342 
1343  std::vector<unsigned char> sopasCmdVec;
1344  for (size_t j = 0; j < sopasCmd.length(); j++)
1345  {
1346  sopasCmdVec.push_back(sopasCmd[j]);
1347  }
1348  ROS_DEBUG("Command: %s", stripControl(sopasCmdVec).c_str());
1349 
1350  // switch to either binary or ascii after switching the command mode
1351  // via ... command
1352 
1353 
1354  for (int iLoop = 0; iLoop < maxCmdLoop; iLoop++)
1355  {
1356  if (iLoop == 0)
1357  {
1358  useBinaryCmdNow = useBinaryCmd; // start with expected value
1359 
1360  }
1361  else
1362  {
1363  useBinaryCmdNow = !useBinaryCmdNow;// try the other option
1364  useBinaryCmd = useBinaryCmdNow; // and use it from now on as default
1365 
1366  }
1367 
1368  this->setProtocolType(useBinaryCmdNow ? CoLa_B : CoLa_A);
1369 
1370 
1371  if (useBinaryCmdNow)
1372  {
1373  this->convertAscii2BinaryCmd(sopasCmd.c_str(), &reqBinary);
1374  result = sendSopasAndCheckAnswer(reqBinary, &replyDummy);
1375  sopasReplyBinVec[cmdId] = replyDummy;
1376  }
1377  else
1378  {
1379  result = sendSopasAndCheckAnswer(sopasCmd.c_str(), &replyDummy);
1380  }
1381  if (result == 0) // command sent successfully
1382  {
1383  // useBinaryCmd holds information about last successful command mode
1384  break;
1385  }
1386  }
1387  if (result != 0)
1388  {
1389  ROS_ERROR("%s", sopasCmdErrMsg[cmdId].c_str());
1391  }
1392  else
1393  {
1394  sopasReplyStrVec[cmdId] = replyToString(replyDummy);
1395  }
1396 
1397  //============================================
1398  // Handle reply of specific commands here by inserting new case instruction
1399  // REPLY_HANDLING
1400  //============================================
1401  maxCmdLoop = 1;
1402 
1403 
1404  // handle special configuration commands ...
1405 
1406  switch (cmdId)
1407  {
1408 
1410  {
1411  bool protocolCheck = checkForProtocolChangeAndMaybeReconnect(useBinaryCmdNow);
1412  if (false == protocolCheck)
1413  {
1414  restartDueToProcolChange = true;
1415  }
1416  useBinaryCmd = useBinaryCmdNow;
1417  setReadTimeOutInMs(defaultTimeOutInMs);
1418  }
1419  break;
1421  {
1422  bool protocolCheck = checkForProtocolChangeAndMaybeReconnect(useBinaryCmdNow);
1423  if (false == protocolCheck)
1424  {
1425  restartDueToProcolChange = true;
1426  }
1427 
1428  useBinaryCmd = useBinaryCmdNow;
1429  setReadTimeOutInMs(defaultTimeOutInMs);
1430  }
1431  break;
1432 
1433  /*
1434  SERIAL_NUMBER: Device ident must be read before!
1435  */
1436 
1437  case CMD_DEVICE_IDENT: // FOR MRS6xxx the Device Ident holds all specific information (used instead of CMD_SERIAL_NUMBER)
1438  {
1439  std::string deviceIdent = "";
1440  int cmdLen = this->checkForBinaryAnswer(&replyDummy);
1441  if (cmdLen == -1)
1442  {
1443  int idLen = 0;
1444  int versionLen = 0;
1445  // ASCII-Return
1446  std::string deviceIdentKeyWord = "sRA DeviceIdent";
1447  char *ptr = (char *) (&(replyDummy[0]));
1448  ptr++; // skip 0x02
1449  ptr += deviceIdentKeyWord.length();
1450  ptr++; //skip blank
1451  sscanf(ptr, "%d", &idLen);
1452  char *ptr2 = strchr(ptr, ' ');
1453  if (ptr2 != NULL)
1454  {
1455  ptr2++;
1456  for (int i = 0; i < idLen; i++)
1457  {
1458  deviceIdent += *ptr2;
1459  ptr2++;
1460  }
1461 
1462  }
1463  ptr = ptr2;
1464  ptr++; //skip blank
1465  sscanf(ptr, "%d", &versionLen);
1466  ptr2 = strchr(ptr, ' ');
1467  if (ptr2 != NULL)
1468  {
1469  ptr2++;
1470  deviceIdent += " V";
1471  for (int i = 0; i < versionLen; i++)
1472  {
1473  deviceIdent += *ptr2;
1474  ptr2++;
1475  }
1476  }
1477  diagnostics_.setHardwareID(deviceIdent);
1478  if (!isCompatibleDevice(deviceIdent))
1479  {
1480  return ExitFatal;
1481  }
1482 // ROS_ERROR("BINARY REPLY REQUIRED");
1483  }
1484  else
1485  {
1486  long dummy0, dummy1, identLen, versionLen;
1487  dummy0 = 0;
1488  dummy1 = 0;
1489  identLen = 0;
1490  versionLen = 0;
1491 
1492  const char *scanMask0 = "%04y%04ysRA DeviceIdent %02y";
1493  const char *scanMask1 = "%02y";
1494  unsigned char *replyPtr = &(replyDummy[0]);
1495  int scanDataLen0 = binScanfGuessDataLenFromMask(scanMask0);
1496  int scanDataLen1 = binScanfGuessDataLenFromMask(scanMask1); // should be: 2
1497  binScanfVec(&replyDummy, scanMask0, &dummy0, &dummy1, &identLen);
1498 
1499  std::string identStr = binScanfGetStringFromVec(&replyDummy, scanDataLen0, identLen);
1500  int off = scanDataLen0 + identLen; // consuming header + fixed part + ident
1501 
1502  std::vector<unsigned char> restOfReplyDummy = std::vector<unsigned char>(replyDummy.begin() + off,
1503  replyDummy.end());
1504 
1505  versionLen = 0;
1506  binScanfVec(&restOfReplyDummy, "%02y", &versionLen);
1507  std::string versionStr = binScanfGetStringFromVec(&restOfReplyDummy, scanDataLen1, versionLen);
1508  std::string fullIdentVersionInfo = identStr + " V" + versionStr;
1509  diagnostics_.setHardwareID(fullIdentVersionInfo);
1510  if (!isCompatibleDevice(fullIdentVersionInfo))
1511  {
1512  return ExitFatal;
1513  }
1514 
1515  }
1516  break;
1517  }
1518 
1519 
1520  case CMD_SERIAL_NUMBER:
1521  if (this->parser_->getCurrentParamPtr()->getNumberOfLayers() == 4)
1522  {
1523  // do nothing for MRS1104 here
1524  }
1525  else
1526  {
1529 
1531  {
1532  return ExitFatal;
1533  }
1534  }
1535  break;
1536  /*
1537  DEVICE_STATE
1538  */
1539  case CMD_DEVICE_STATE:
1540  {
1541  int deviceState = -1;
1542  /*
1543  * Process device state, 0=Busy, 1=Ready, 2=Error
1544  * If configuration parameter is set, try resetting device in error state
1545  */
1546 
1547  int iRetVal = 0;
1548  if (useBinaryCmd)
1549  {
1550  long dummy0 = 0x00;
1551  long dummy1 = 0x00;
1552  deviceState = 0x00; // must be set to zero (because only one byte will be copied)
1553  iRetVal = binScanfVec(&(sopasReplyBinVec[CMD_DEVICE_STATE]), "%4y%4ysRA SCdevicestate %1y", &dummy0,
1554  &dummy1, &deviceState);
1555  }
1556  else
1557  {
1558  iRetVal = sscanf(sopasReplyStrVec[CMD_DEVICE_STATE].c_str(), "sRA SCdevicestate %d", &deviceState);
1559  }
1560  if (iRetVal > 0) // 1 or 3 (depending of ASCII or Binary)
1561  {
1562  switch (deviceState)
1563  {
1564  case 0:
1565  ROS_DEBUG("Laser is busy");
1566  break;
1567  case 1:
1568  ROS_DEBUG("Laser is ready");
1569  break;
1570  case 2:
1571  ROS_ERROR_STREAM("Laser reports error state : " << sopasReplyStrVec[CMD_DEVICE_STATE]);
1572  if (config_.auto_reboot)
1573  {
1574  rebootScanner();
1575  };
1576  break;
1577  default:
1578  ROS_WARN_STREAM("Laser reports unknown devicestate : " << sopasReplyStrVec[CMD_DEVICE_STATE]);
1579  break;
1580  }
1581  }
1582  }
1583  break;
1584 
1585  case CMD_OPERATION_HOURS:
1586  {
1587  int operationHours = -1;
1588  int iRetVal = 0;
1589  /*
1590  * Process device state, 0=Busy, 1=Ready, 2=Error
1591  * If configuration parameter is set, try resetting device in error state
1592  */
1593  if (useBinaryCmd)
1594  {
1595  long dummy0, dummy1;
1596  dummy0 = 0;
1597  dummy1 = 0;
1598  operationHours = 0;
1599  iRetVal = binScanfVec(&(sopasReplyBinVec[CMD_OPERATION_HOURS]), "%4y%4ysRA ODoprh %4y", &dummy0, &dummy1,
1600  &operationHours);
1601  }
1602  else
1603  {
1604  operationHours = 0;
1605  iRetVal = sscanf(sopasReplyStrVec[CMD_OPERATION_HOURS].c_str(), "sRA ODoprh %x", &operationHours);
1606  }
1607  if (iRetVal > 0)
1608  {
1609  double hours = 0.1 * operationHours;
1610  pn.setParam("operationHours", hours);
1611  }
1612  }
1613  break;
1614 
1615  case CMD_POWER_ON_COUNT:
1616  {
1617  int powerOnCount = -1;
1618  int iRetVal = -1;
1619  if (useBinaryCmd)
1620  {
1621  long dummy0, dummy1;
1622  powerOnCount = 0;
1623  iRetVal = binScanfVec(&(sopasReplyBinVec[CMD_POWER_ON_COUNT]), "%4y%4ysRA ODpwrc %4y", &dummy0, &dummy1,
1624  &powerOnCount);
1625  }
1626  else
1627  {
1628  iRetVal = sscanf(sopasReplyStrVec[CMD_POWER_ON_COUNT].c_str(), "sRA ODpwrc %x", &powerOnCount);
1629  }
1630  if (iRetVal > 0)
1631  {
1632  pn.setParam("powerOnCount", powerOnCount);
1633  }
1634 
1635  }
1636  break;
1637 
1638  case CMD_LOCATION_NAME:
1639  {
1640  char szLocationName[MAX_STR_LEN] = {0};
1641  const char *strPtr = sopasReplyStrVec[CMD_LOCATION_NAME].c_str();
1642  const char *searchPattern = "sRA LocationName "; // Bug fix (leading space) Jan 2018
1643  strcpy(szLocationName, "unknown location");
1644  if (useBinaryCmd)
1645  {
1646  int iRetVal = 0;
1647  long dummy0, dummy1, locationNameLen;
1648  const char *binLocationNameMask = "%4y%4ysRA LocationName %2y";
1649  int prefixLen = binScanfGuessDataLenFromMask(binLocationNameMask);
1650  dummy0 = 0;
1651  dummy1 = 0;
1652  locationNameLen = 0;
1653 
1654  iRetVal = binScanfVec(&(sopasReplyBinVec[CMD_LOCATION_NAME]), binLocationNameMask, &dummy0, &dummy1,
1655  &locationNameLen);
1656  if (iRetVal > 0)
1657  {
1658  std::string s;
1659  std::string locationNameStr = binScanfGetStringFromVec(&(sopasReplyBinVec[CMD_LOCATION_NAME]), prefixLen,
1660  locationNameLen);
1661  strcpy(szLocationName, locationNameStr.c_str());
1662  }
1663  }
1664  else
1665  {
1666  if (strstr(strPtr, searchPattern) == strPtr)
1667  {
1668  const char *ptr = strPtr + strlen(searchPattern);
1669  strcpy(szLocationName, ptr);
1670  }
1671  else
1672  {
1673  ROS_WARN("Location command not supported.\n");
1674  }
1675  }
1676  pn.setParam("locationName", std::string(szLocationName));
1677  }
1678  break;
1679 
1680 
1682  {
1683 
1684  const char *strPtr = sopasReplyStrVec[CMD_GET_PARTIAL_SCANDATA_CFG].c_str();
1685  ROS_INFO("Config: %s\n", strPtr);
1686  }
1687  break;
1688 
1690  {
1691  bool useNegSign = false;
1692  if (NAV3xxOutputRangeSpecialHandling == 0)
1693  {
1694  useNegSign = true; // use negative phase compensation for NAV3xx
1695  }
1696 
1697  this->angleCompensator = new AngleCompensator(useNegSign);
1699  std::vector<unsigned char> tmpVec;
1700 
1701  if (useBinaryCmd == false)
1702  {
1703  for (int i = 0; i < s.length(); i++)
1704  {
1705  tmpVec.push_back((unsigned char)s[i]);
1706  }
1707  }
1708  else
1709  {
1711  }
1712  angleCompensator->parseReply(useBinaryCmd, tmpVec);
1713 
1714  ROS_INFO("Angle Comp. Formula used: %s\n", angleCompensator->getHumanReadableFormula().c_str());
1715  }
1716  break;
1717  // if (parser_->getCurrentParamPtr()->getScannerName().compare(SICK_SCANNER_NAV_2XX_NAME) == 0)
1718 
1719  // ML: add here reply handling
1720  }
1721 
1722 
1723  if (restartDueToProcolChange)
1724  {
1725  return ExitError;
1726 
1727  }
1728 
1729  }
1730 
1731  if (setNewIPAddr)
1732  {
1733 
1734  setNewIpAddress(ipNewIPAddr, useBinaryCmd);
1735  ROS_INFO("IP address changed. Node restart required");
1736  ROS_INFO("Exiting node NOW.");
1737  exit(0);//stopping node hard to avoide further IP-Communication
1738  }
1739 
1740 
1741 
1742 
1743 
1744 
1745  if (setUseNTP)
1746  {
1747 
1748  setNTPServerAndStart(NTPIpAdress, useBinaryCmd);
1749  }
1750 
1751  /*
1752  * The NAV310 and of course the radar does not support angular resolution handling
1753  */
1754  if (this->parser_->getCurrentParamPtr()->getDeviceIsRadar() )
1755  {
1756  //=====================================================
1757  // Radar specific commands
1758  //=====================================================
1759  }
1760  else
1761  {
1762  //-----------------------------------------------------------------
1763  //
1764  // This is recommended to decide between TiM551 and TiM561/TiM571 capabilities
1765  // The TiM551 has an angular resolution of 1.000 [deg]
1766  // The TiM561 and TiM571 have an angular resolution of 0.333 [deg]
1767  //-----------------------------------------------------------------
1768 
1769  angleRes10000th = (int) (boost::math::round(
1770  10000.0 * this->parser_->getCurrentParamPtr()->getAngularDegreeResolution()));
1771  std::vector<unsigned char> askOutputAngularRangeReply;
1772 
1773  // LMDscandatacfg corresponds to CMD_GET_OUTPUT_RANGES
1774  // LMDscandatacfg is not supported by NAV310
1775 
1776  if (NAV3xxOutputRangeSpecialHandling)
1777  {
1778 
1779  }
1780  else // CMD_GET_OUTPUT_RANGE (i.e. handling of LMDscandatacfg
1781  {
1782  if (useBinaryCmd)
1783  {
1784  std::vector<unsigned char> reqBinary;
1785  this->convertAscii2BinaryCmd(sopasCmdVec[CMD_GET_OUTPUT_RANGES].c_str(), &reqBinary);
1787  }
1788  else
1789  {
1790  result = sendSopasAndCheckAnswer(sopasCmdVec[CMD_GET_OUTPUT_RANGES].c_str(), &askOutputAngularRangeReply);
1791  }
1792 
1793 
1794  if (0 == result)
1795  {
1796  int askTmpAngleRes10000th = 0;
1797  int askTmpAngleStart10000th = 0;
1798  int askTmpAngleEnd10000th = 0;
1799  char dummy0[MAX_STR_LEN] = {0};
1800  char dummy1[MAX_STR_LEN] = {0};
1801  int dummyInt = 0;
1802 
1803  int iDummy0, iDummy1;
1804  std::string askOutputAngularRangeStr = replyToString(askOutputAngularRangeReply);
1805  // Binary-Reply Tab. 63
1806  // 0x20 Space
1807  // 0x00 0x01 -
1808  // 0x00 0x00 0x05 0x14 // Resolution in 1/10000th degree --> 0.13°
1809  // 0x00 0x04 0x93 0xE0 // Start Angle 300000 -> 30°
1810  // 0x00 0x16 0xE3 0x60 // End Angle 1.500.000 -> 150° // in ROS +/-60°
1811  // 0x83 // Checksum
1812 
1813  int numArgs;
1814 
1815  if (useBinaryCmd)
1816  {
1817  iDummy0 = 0;
1818  iDummy1 = 0;
1819  dummyInt = 0;
1820  askTmpAngleRes10000th = 0;
1821  askTmpAngleStart10000th = 0;
1822  askTmpAngleEnd10000th = 0;
1823 
1824  const char *askOutputAngularRangeBinMask = "%4y%4ysRA LMPoutputRange %2y%4y%4y%4y";
1825  numArgs = binScanfVec(&sopasReplyBinVec[CMD_GET_OUTPUT_RANGES], askOutputAngularRangeBinMask, &iDummy0,
1826  &iDummy1,
1827  &dummyInt,
1828  &askTmpAngleRes10000th,
1829  &askTmpAngleStart10000th,
1830  &askTmpAngleEnd10000th);
1831  //
1832  }
1833  else
1834  {
1835  numArgs = sscanf(askOutputAngularRangeStr.c_str(), "%s %s %d %X %X %X", dummy0, dummy1,
1836  &dummyInt,
1837  &askTmpAngleRes10000th,
1838  &askTmpAngleStart10000th,
1839  &askTmpAngleEnd10000th);
1840  }
1841  if (numArgs >= 6)
1842  {
1843  double askTmpAngleRes = askTmpAngleRes10000th / 10000.0;
1844  double askTmpAngleStart = askTmpAngleStart10000th / 10000.0;
1845  double askTmpAngleEnd = askTmpAngleEnd10000th / 10000.0;
1846 
1847  angleRes10000th = askTmpAngleRes10000th;
1848  ROS_INFO("Angle resolution of scanner is %0.5lf [deg] (in 1/10000th deg: 0x%X)", askTmpAngleRes,
1849  askTmpAngleRes10000th);
1850  ROS_INFO("[From:To] %0.5lf [deg] to %0.5lf [deg] (in 1/10000th deg: from 0x%X to 0x%X)",
1851  askTmpAngleStart, askTmpAngleEnd, askTmpAngleStart10000th, askTmpAngleEnd10000th);
1852  }
1853  }
1854  }
1855  //-----------------------------------------------------------------
1856  //
1857  // Set Min- und Max scanning angle given by config
1858  //
1859  //-----------------------------------------------------------------
1860 
1861  ROS_INFO("MIN_ANG: %8.3f [rad] %8.3f [deg]", config_.min_ang, rad2deg(this->config_.min_ang));
1862  ROS_INFO("MAX_ANG: %8.3f [rad] %8.3f [deg]", config_.max_ang, rad2deg(this->config_.max_ang));
1863 
1864 
1865  // convert to 10000th degree
1866  double minAngSopas = rad2deg(this->config_.min_ang);
1867  double maxAngSopas = rad2deg(this->config_.max_ang);
1868 
1869  minAngSopas -= rad2deg(this->parser_->getCurrentParamPtr()->getScanAngleShift());
1870  maxAngSopas -= rad2deg(this->parser_->getCurrentParamPtr()->getScanAngleShift());
1871 
1872  angleStart10000th = (int) (boost::math::round(10000.0 * minAngSopas));
1873  angleEnd10000th = (int) (boost::math::round(10000.0 * maxAngSopas));
1874 
1875  char requestOutputAngularRange[MAX_STR_LEN];
1876 
1877  std::vector<unsigned char> outputAngularRangeReply;
1878 
1879 
1880  if (NAV3xxOutputRangeSpecialHandling == 0)
1881  {
1882  const char *pcCmdMask = sopasCmdMaskVec[CMD_SET_OUTPUT_RANGES_NAV3].c_str();
1883  sprintf(requestOutputAngularRange, pcCmdMask,
1884  angleRes10000th, angleStart10000th, angleEnd10000th,
1885  angleRes10000th, angleStart10000th, angleEnd10000th,
1886  angleRes10000th, angleStart10000th, angleEnd10000th,
1887  angleRes10000th, angleStart10000th, angleEnd10000th);
1888  }
1889  else
1890  {
1891  const char *pcCmdMask = sopasCmdMaskVec[CMD_SET_OUTPUT_RANGES].c_str();
1892  sprintf(requestOutputAngularRange, pcCmdMask, angleRes10000th, angleStart10000th, angleEnd10000th);
1893  }
1894  if (useBinaryCmd)
1895  {
1896  unsigned char tmpBuffer[255] = {0};
1897  unsigned char sendBuffer[255] = {0};
1898  UINT16 sendLen;
1899  std::vector<unsigned char> reqBinary;
1900  int iStatus = 1;
1901  // const char *askOutputAngularRangeBinMask = "%4y%4ysWN LMPoutputRange %2y%4y%4y%4y";
1902  // int askOutputAngularRangeBinLen = binScanfGuessDataLenFromMask(askOutputAngularRangeBinMask);
1903  // askOutputAngularRangeBinLen -= 8; // due to header and length identifier
1904 
1905  strcpy((char *) tmpBuffer, "WN LMPoutputRange ");
1906  unsigned short orgLen = strlen((char *) tmpBuffer);
1907  if (NAV3xxOutputRangeSpecialHandling){
1908  colab::addIntegerToBuffer<UINT16>(tmpBuffer, orgLen, iStatus);
1909  colab::addIntegerToBuffer<UINT32>(tmpBuffer, orgLen, angleRes10000th);
1910  colab::addIntegerToBuffer<UINT32>(tmpBuffer, orgLen, angleStart10000th);
1911  colab::addIntegerToBuffer<UINT32>(tmpBuffer, orgLen, angleEnd10000th);
1912  colab::addIntegerToBuffer<UINT32>(tmpBuffer, orgLen, angleRes10000th);
1913  colab::addIntegerToBuffer<UINT32>(tmpBuffer, orgLen, angleStart10000th);
1914  colab::addIntegerToBuffer<UINT32>(tmpBuffer, orgLen, angleEnd10000th);
1915  colab::addIntegerToBuffer<UINT32>(tmpBuffer, orgLen, angleRes10000th);
1916  colab::addIntegerToBuffer<UINT32>(tmpBuffer, orgLen, angleStart10000th);
1917  colab::addIntegerToBuffer<UINT32>(tmpBuffer, orgLen, angleEnd10000th);
1918  colab::addIntegerToBuffer<UINT32>(tmpBuffer, orgLen, angleRes10000th);
1919  colab::addIntegerToBuffer<UINT32>(tmpBuffer, orgLen, angleStart10000th);
1920  colab::addIntegerToBuffer<UINT32>(tmpBuffer, orgLen, angleEnd10000th);
1921  }
1922  else
1923  {
1924  colab::addIntegerToBuffer<UINT16>(tmpBuffer, orgLen, iStatus);
1925  colab::addIntegerToBuffer<UINT32>(tmpBuffer, orgLen, angleRes10000th);
1926  colab::addIntegerToBuffer<UINT32>(tmpBuffer, orgLen, angleStart10000th);
1927  colab::addIntegerToBuffer<UINT32>(tmpBuffer, orgLen, angleEnd10000th);
1928  }
1929  sendLen = orgLen;
1930  colab::addFrameToBuffer(sendBuffer, tmpBuffer, &sendLen);
1931 
1932  // binSprintfVec(&reqBinary, askOutputAngularRangeBinMask, 0x02020202, askOutputAngularRangeBinLen, iStatus, angleRes10000th, angleStart10000th, angleEnd10000th);
1933 
1934  // unsigned char sickCrc = sick_crc8((unsigned char *)(&(reqBinary)[8]), reqBinary.size() - 8);
1935  // reqBinary.push_back(sickCrc);
1936  reqBinary = std::vector<unsigned char>(sendBuffer, sendBuffer + sendLen);
1937  // Here we must build a more complex binaryRequest
1938 
1939  // this->convertAscii2BinaryCmd(requestOutputAngularRange, &reqBinary);
1940  result = sendSopasAndCheckAnswer(reqBinary, &outputAngularRangeReply);
1941  }
1942  else
1943  {
1944  result = sendSopasAndCheckAnswer(requestOutputAngularRange, &outputAngularRangeReply);
1945  }
1946 
1947  //-----------------------------------------------------------------
1948  //
1949  // Get Min- und Max scanning angle from the scanner to verify angle setting and correct the config, if something went wrong.
1950  //
1951  // IMPORTANT:
1952  // Axis Orientation in ROS differs from SICK AXIS ORIENTATION!!!
1953  // In relation to a body the standard is:
1954  // x forward
1955  // y left
1956  // z up
1957  // see http://www.ros.org/reps/rep-0103.html#coordinate-frame-conventions for more details
1958  //-----------------------------------------------------------------
1959 
1960  askOutputAngularRangeReply.clear();
1961 
1962  if (useBinaryCmd)
1963  {
1964  std::vector<unsigned char> reqBinary;
1965  this->convertAscii2BinaryCmd(sopasCmdVec[CMD_GET_OUTPUT_RANGES].c_str(), &reqBinary);
1967  }
1968  else
1969  {
1970  result = sendSopasAndCheckAnswer(sopasCmdVec[CMD_GET_OUTPUT_RANGES].c_str(), &askOutputAngularRangeReply);
1971  }
1972 
1973  if (result == 0)
1974  {
1975  char dummy0[MAX_STR_LEN] = {0};
1976  char dummy1[MAX_STR_LEN] = {0};
1977  int dummyInt = 0;
1978  int askAngleRes10000th = 0;
1979  int askAngleStart10000th = 0;
1980  int askAngleEnd10000th = 0;
1981  int iDummy0, iDummy1;
1982  iDummy0 = 0;
1983  iDummy1 = 0;
1984  std::string askOutputAngularRangeStr = replyToString(askOutputAngularRangeReply);
1985  // Binary-Reply Tab. 63
1986  // 0x20 Space
1987  // 0x00 0x01 -
1988  // 0x00 0x00 0x05 0x14 // Resolution in 1/10000th degree --> 0.13°
1989  // 0x00 0x04 0x93 0xE0 // Start Angle 300000 -> 30°
1990  // 0x00 0x16 0xE3 0x60 // End Angle 1.500.000 -> 150° // in ROS +/-60°
1991  // 0x83 // Checksum
1992 
1993  int numArgs;
1994 
1995  /*
1996  *
1997  * Initialize variables
1998  */
1999 
2000  iDummy0 = 0;
2001  iDummy1 = 0;
2002  dummyInt = 0;
2003  askAngleRes10000th = 0;
2004  askAngleStart10000th = 0;
2005  askAngleEnd10000th = 0;
2006 
2007  /*
2008  * scan values
2009  *
2010  */
2011 
2012  if (useBinaryCmd)
2013  {
2014  const char *askOutputAngularRangeBinMask = "%4y%4ysRA LMPoutputRange %2y%4y%4y%4y";
2015  numArgs = binScanfVec(&sopasReplyBinVec[CMD_GET_OUTPUT_RANGES], askOutputAngularRangeBinMask, &iDummy0,
2016  &iDummy1,
2017  &dummyInt,
2018  &askAngleRes10000th,
2019  &askAngleStart10000th,
2020  &askAngleEnd10000th);
2021  //
2022  }
2023  else
2024  {
2025  numArgs = sscanf(askOutputAngularRangeStr.c_str(), "%s %s %d %X %X %X", dummy0, dummy1,
2026  &dummyInt,
2027  &askAngleRes10000th,
2028  &askAngleStart10000th,
2029  &askAngleEnd10000th);
2030  }
2031  if (numArgs >= 6)
2032  {
2033  double askTmpAngleRes = askAngleRes10000th / 10000.0;
2034  double askTmpAngleStart = askAngleStart10000th / 10000.0;
2035  double askTmpAngleEnd = askAngleEnd10000th / 10000.0;
2036 
2037  angleRes10000th = askAngleRes10000th;
2038  ROS_INFO("Angle resolution of scanner is %0.5lf [deg] (in 1/10000th deg: 0x%X)", askTmpAngleRes,
2039  askAngleRes10000th);
2040 
2041  }
2042  double askAngleRes = askAngleRes10000th / 10000.0;
2043  double askAngleStart = askAngleStart10000th / 10000.0;
2044  double askAngleEnd = askAngleEnd10000th / 10000.0;
2045 
2046 
2047  askAngleStart += rad2deg(this->parser_->getCurrentParamPtr()->getScanAngleShift());
2048  askAngleEnd += rad2deg(this->parser_->getCurrentParamPtr()->getScanAngleShift());
2049 
2050  ros::NodeHandle nhPriv("~");
2051  nhPriv.setParam("min_ang",
2052  this->config_.min_ang); // update parameter setting with "true" values read from scanner
2053  nhPriv.setParam("max_ang",
2054  this->config_.max_ang); // update parameter setting with "true" values read from scanner
2055  ROS_INFO("MIN_ANG (after command verification): %8.3f [rad] %8.3f [deg]", config_.min_ang,
2056  rad2deg(this->config_.min_ang));
2057  ROS_INFO("MAX_ANG (after command verification): %8.3f [rad] %8.3f [deg]", config_.max_ang,
2058  rad2deg(this->config_.max_ang));
2059  }
2060  //-----------------------------------------------------------------
2061  //
2062  // Configure the data content of scan messing regarding to config.
2063  //
2064  //-----------------------------------------------------------------
2065  /*
2066  see 4.3.1 Configure the data content for the scan in the
2067  */
2068  // 1 2 3
2069  // Prepare flag array for echos
2070  // Except for the LMS5xx scanner here the mask is hard 00 see SICK Telegram listing "Telegram structure: sWN LMDscandatacfg" for details
2071 
2072  outputChannelFlagId = 0x00;
2073  for (size_t i = 0; i < outputChannelFlag.size(); i++)
2074  {
2075  outputChannelFlagId |= ((outputChannelFlag[i] == true) << i);
2076  }
2077  if (outputChannelFlagId < 1)
2078  {
2079  outputChannelFlagId = 1; // at least one channel must be set
2080  }
2082  {
2083  outputChannelFlagId = 1;
2084  ROS_INFO("LMS 5xx detected overwriting output channel flag ID");
2085 
2086  ROS_INFO("LMS 5xx detected overwriting resolution flag (only 8 bit supported)");
2088  rssiResolutionIs16Bit = this->parser_->getCurrentParamPtr()->getIntensityResolutionIs16Bit();
2089  }
2091  {
2092  ROS_INFO("MRS 1xxx detected overwriting resolution flag (only 8 bit supported)");
2094  rssiResolutionIs16Bit = this->parser_->getCurrentParamPtr()->getIntensityResolutionIs16Bit();
2095 
2096  }
2097  //SAFTY FIELD PARSING
2098  if (this->parser_->getCurrentParamPtr()->getUseEvalFields() == USE_EVAL_FIELD_TIM7XX_LOGIC || this->parser_->getCurrentParamPtr()->getUseEvalFields() == USE_EVAL_FIELD_LMS5XX_LOGIC)
2099  {
2100  ROS_INFO("Reading safety fields");
2102  int maxFieldnum = this->parser_->getCurrentParamPtr()->getMaxEvalFields();
2103  for(int fieldnum=0;fieldnum<maxFieldnum;fieldnum++)
2104  {
2105  char requestFieldcfg[MAX_STR_LEN];
2106  const char *pcCmdMask = sopasCmdMaskVec[CMD_GET_SAFTY_FIELD_CFG].c_str();
2107  sprintf(requestFieldcfg, pcCmdMask, fieldnum);
2108  if (useBinaryCmd) {
2109  std::vector<unsigned char> reqBinary;
2110  std::vector<unsigned char> fieldcfgReply;
2111  this->convertAscii2BinaryCmd(requestFieldcfg, &reqBinary);
2112  result = sendSopasAndCheckAnswer(reqBinary, &fieldcfgReply);
2113  fieldMon->parseBinaryDatagram(fieldcfgReply);
2114  } else {
2115  std::vector<unsigned char> fieldcfgReply;
2116  result = sendSopasAndCheckAnswer(requestFieldcfg, &fieldcfgReply);
2117  fieldMon->parseAsciiDatagram(fieldcfgReply);
2118  }
2119  }
2120  if(cloud_marker_)
2121  {
2122  int fieldset = 0;
2123  // pn.getParam("fieldset", fieldset);
2124  std::string LIDinputstateRequest = "\x02sRN LIDinputstate\x03";
2125  std::vector<unsigned char> LIDinputstateResponse;
2126  if (useBinaryCmd)
2127  {
2128  std::vector<unsigned char> reqBinary;
2129  this->convertAscii2BinaryCmd(LIDinputstateRequest.c_str(), &reqBinary);
2130  result = sendSopasAndCheckAnswer(reqBinary, &LIDinputstateResponse);
2131  if(result == 0)
2132  {
2133  //fieldset = (LIDinputstateResponse[32] & 0xFF);
2134  //fieldMon->setActiveFieldset(fieldset);
2135  fieldMon->parseBinaryLIDinputstateMsg(LIDinputstateResponse.data(), LIDinputstateResponse.size());
2136  ROS_INFO_STREAM("Safety fieldset response to \"sRN LIDinputstate\": " << DataDumper::binDataToAsciiString(LIDinputstateResponse.data(), LIDinputstateResponse.size())
2137  << ", active fieldset = " << fieldMon->getActiveFieldset());
2138  }
2139  }
2140  else
2141  {
2142  result = sendSopasAndCheckAnswer(LIDinputstateRequest.c_str(), &LIDinputstateResponse);
2143  }
2144 
2145  std::string scanner_name = parser_->getCurrentParamPtr()->getScannerName();
2146  EVAL_FIELD_SUPPORT eval_field_logic = this->parser_->getCurrentParamPtr()->getUseEvalFields();
2147  cloud_marker_->updateMarker(fieldMon->getMonFields(), fieldset, eval_field_logic);
2148  std::stringstream field_info;
2149  field_info << "Safety fieldset " << fieldset << ", pointcounter = [ ";
2150  for(int n = 0; n < fieldMon->getMonFields().size(); n++)
2151  field_info << (n > 0 ? ", " : " ") << fieldMon->getMonFields()[n].getPointCount();
2152  field_info << " ]";
2153  ROS_INFO_STREAM(field_info.str());
2154  for(int n = 0; n < fieldMon->getMonFields().size(); n++)
2155  {
2156  if(fieldMon->getMonFields()[n].getPointCount() > 0)
2157  {
2158  std::stringstream field_info2;
2159  field_info2 << "Safety field " << n << ", type " << (int)(fieldMon->getMonFields()[n].fieldType()) << " : ";
2160  for(int m = 0; m < fieldMon->getMonFields()[n].getPointCount(); m++)
2161  {
2162  if(m > 0)
2163  field_info2 << ", ";
2164  field_info2 << "(" << fieldMon->getMonFields()[n].getFieldPointsX()[m] << "," << fieldMon->getMonFields()[n].getFieldPointsY()[m] << ")";
2165  }
2166  ROS_INFO_STREAM(field_info2.str());
2167  }
2168  }
2169  }
2170  }
2171 
2172  // set scanning angle for tim5xx and for mrs1104
2173  if ((this->parser_->getCurrentParamPtr()->getNumberOfLayers() == 1)
2174  || (this->parser_->getCurrentParamPtr()->getNumberOfLayers() == 4)
2175  || (this->parser_->getCurrentParamPtr()->getNumberOfLayers() == 24)
2176  )
2177  {
2178  char requestLMDscandatacfg[MAX_STR_LEN];
2179  // Uses sprintf-Mask to set bitencoded echos and rssi enable flag
2180  // sopasCmdMaskVec[CMD_SET_PARTIAL_SCANDATA_CFG] = "\x02sWN LMDscandatacfg %02d 00 %d %d 00 %d 00 0 0 0 1 1\x03";
2181  const char *pcCmdMask = sopasCmdMaskVec[CMD_SET_PARTIAL_SCANDATA_CFG].c_str();
2182  sprintf(requestLMDscandatacfg, pcCmdMask, outputChannelFlagId, rssiFlag ? 1 : 0, rssiResolutionIs16Bit ? 1 : 0,
2183  EncoderSetings != -1 ? EncoderSetings : 0);
2184  if (useBinaryCmd)
2185  {
2186  std::vector<unsigned char> reqBinary;
2187  this->convertAscii2BinaryCmd(requestLMDscandatacfg, &reqBinary);
2188  // FOR MRS6124 this should be
2189  // like this:
2190  // 0000 02 02 02 02 00 00 00 20 73 57 4e 20 4c 4d 44 73 .......sWN LMDs
2191  // 0010 63 61 6e 64 61 74 61 63 66 67 20 1f 00 01 01 00 candatacfg .....
2192  // 0020 00 00 00 00 00 00 00 01 5c
2194  }
2195  else
2196  {
2197  std::vector<unsigned char> lmdScanDataCfgReply;
2198  result = sendSopasAndCheckAnswer(requestLMDscandatacfg, &lmdScanDataCfgReply);
2199  }
2200 
2201 
2202  // check setting
2203  char requestLMDscandatacfgRead[MAX_STR_LEN];
2204  // Uses sprintf-Mask to set bitencoded echos and rssi enable flag
2205 
2206  strcpy(requestLMDscandatacfgRead, sopasCmdVec[CMD_GET_PARTIAL_SCANDATA_CFG].c_str());
2207  if (useBinaryCmd)
2208  {
2209  std::vector<unsigned char> reqBinary;
2210  this->convertAscii2BinaryCmd(requestLMDscandatacfgRead, &reqBinary);
2212  }
2213  else
2214  {
2215  std::vector<unsigned char> lmdScanDataCfgReadReply;
2216  result = sendSopasAndCheckAnswer(requestLMDscandatacfgRead, &lmdScanDataCfgReadReply);
2217  }
2218 
2219 
2220  }
2221  //BBB
2222  // set scanning angle for tim5xx and for mrs1104
2223  double scan_freq = 0;
2224  double ang_res = 0;
2225  pn.getParam("scan_freq", scan_freq); // filter_echos
2226  pn.getParam("ang_res", ang_res); // filter_echos
2227  if (scan_freq != 0 || ang_res != 0)
2228  {
2229  if (scan_freq != 0 && ang_res != 0)
2230  {
2232  {
2233  ROS_INFO("variable ang_res and scan_freq setings for NAV 3xx has not been implemented yet using 20 Hz 0.75 deg");
2234  }
2235  else
2236  {
2237  char requestLMDscancfg[MAX_STR_LEN];
2238  // sopasCmdMaskVec[CMD_SET_PARTIAL_SCAN_CFG] = "\x02sMN mLMPsetscancfg %d 1 %d 0 0\x03";//scanfreq [1/100 Hz],angres [1/10000°],
2239  const char *pcCmdMask = sopasCmdMaskVec[CMD_SET_PARTIAL_SCAN_CFG].c_str();
2240  sprintf(requestLMDscancfg, pcCmdMask, (long) (scan_freq * 100 + 1e-9), (long) (ang_res * 10000 + 1e-9));
2241  if (useBinaryCmd)
2242  {
2243  std::vector<unsigned char> reqBinary;
2244  this->convertAscii2BinaryCmd(requestLMDscancfg, &reqBinary);
2246  }
2247  else
2248  {
2249  std::vector<unsigned char> lmdScanCfgReply;
2250  result = sendSopasAndCheckAnswer(requestLMDscancfg, &lmdScanCfgReply);
2251  }
2252 
2253 
2254  // check setting
2255  char requestLMDscancfgRead[MAX_STR_LEN];
2256  // Uses sprintf-Mask to set bitencoded echos and rssi enable flag
2257 
2258  strcpy(requestLMDscancfgRead, sopasCmdVec[CMD_GET_PARTIAL_SCAN_CFG].c_str());
2259  if (useBinaryCmd)
2260  {
2261  std::vector<unsigned char> reqBinary;
2262  this->convertAscii2BinaryCmd(requestLMDscancfgRead, &reqBinary);
2264  }
2265  else
2266  {
2267  std::vector<unsigned char> lmdScanDataCfgReadReply;
2268  result = sendSopasAndCheckAnswer(requestLMDscancfgRead, &lmdScanDataCfgReadReply);
2269  }
2270 
2271  }
2272  }
2273  else
2274  {
2275  ROS_ERROR("ang_res and scan_freq have to be set, only one param is set skiping scan_fre/ang_res config");
2276  }
2277  }
2278  // Config Mean filter
2279  /*
2280  char requestMeanSetting[MAX_STR_LEN];
2281  int meanFilterSetting = 0;
2282  int MeanFilterActive=0;
2283  pn.getParam("mean_filter", meanFilterSetting); // filter_echos
2284  if(meanFilterSetting>2)
2285  {
2286  MeanFilterActive=1;
2287  }
2288  else{
2289  //needs to be at leas 2 even if filter is disabled
2290  meanFilterSetting = 2;
2291  }
2292  // Uses sprintf-Mask to set bitencoded echos and rssi enable flag
2293  const char *pcCmdMask = sopasCmdMaskVec[CMD_SET_MEAN_FILTER].c_str();
2294 
2295  //First echo : 0
2296  //All echos : 1
2297  //Last echo : 2
2298 
2299  sprintf(requestMeanSetting, pcCmdMask, MeanFilterActive, meanFilterSetting);
2300  std::vector<unsigned char> outputFilterMeanReply;
2301 
2302 
2303  if (useBinaryCmd)
2304  {
2305  std::vector<unsigned char> reqBinary;
2306  this->convertAscii2BinaryCmd(requestMeanSetting, &reqBinary);
2307  result = sendSopasAndCheckAnswer(reqBinary, &sopasReplyBinVec[CMD_SET_ECHO_FILTER]);
2308  }
2309  else
2310  {
2311  result = sendSopasAndCheckAnswer(requestMeanSetting, &outputFilterMeanReply);
2312  }
2313 
2314 */
2315  // CONFIG ECHO-Filter (only for MRS1000 not available for TiM5xx
2316  if (this->parser_->getCurrentParamPtr()->getNumberOfLayers() >= 4)
2317  {
2318  char requestEchoSetting[MAX_STR_LEN];
2319  int filterEchoSetting = 0;
2320  pn.getParam("filter_echos", filterEchoSetting); // filter_echos
2321  if (filterEchoSetting < 0)
2322  { filterEchoSetting = 0; }
2323  if (filterEchoSetting > 2)
2324  { filterEchoSetting = 2; }
2325  // Uses sprintf-Mask to set bitencoded echos and rssi enable flag
2326  const char *pcCmdMask = sopasCmdMaskVec[CMD_SET_ECHO_FILTER].c_str();
2327  /*
2328  First echo : 0
2329  All echos : 1
2330  Last echo : 2
2331  */
2332  sprintf(requestEchoSetting, pcCmdMask, filterEchoSetting);
2333  std::vector<unsigned char> outputFilterEchoRangeReply;
2334 
2335 
2336  if (useBinaryCmd)
2337  {
2338  std::vector<unsigned char> reqBinary;
2339  this->convertAscii2BinaryCmd(requestEchoSetting, &reqBinary);
2341  }
2342  else
2343  {
2344  result = sendSopasAndCheckAnswer(requestEchoSetting, &outputFilterEchoRangeReply);
2345  }
2346 
2347  }
2348 
2349 
2350  }
2352 
2353 
2354  //-----------------------------------------------------------------
2355  //
2356  // Start sending LMD-Scandata messages
2357  //
2358  //-----------------------------------------------------------------
2359  std::vector<int> startProtocolSequence;
2360  bool deviceIsRadar = false;
2362  {
2363  ros::NodeHandle tmpParam("~");
2364  bool transmitRawTargets = true;
2365  bool transmitObjects = true;
2366  int trackingMode = 0;
2367  std::string trackingModeDescription[] = {"BASIC", "VEHICLE"};
2368 
2369  int numTrackingModes = sizeof(trackingModeDescription) / sizeof(trackingModeDescription[0]);
2370 
2371  tmpParam.getParam("transmit_raw_targets", transmitRawTargets);
2372  tmpParam.getParam("transmit_objects", transmitObjects);
2373  tmpParam.getParam("tracking_mode", trackingMode);
2374 
2375 
2376  if ((trackingMode < 0) || (trackingMode >= numTrackingModes))
2377  {
2378  ROS_WARN("tracking mode id invalid. Switch to tracking mode 0");
2379  trackingMode = 0;
2380  }
2381  ROS_INFO("Raw target transmission is switched [%s]", transmitRawTargets ? "ON" : "OFF");
2382  ROS_INFO("Object transmission is switched [%s]", transmitObjects ? "ON" : "OFF");
2383  ROS_INFO("Tracking mode is set to id [%d] [%s]", trackingMode, trackingModeDescription[trackingMode].c_str());
2384 
2385  deviceIsRadar = true;
2386 
2387  // Asking some informational from the radar
2388  startProtocolSequence.push_back(CMD_DEVICE_TYPE);
2389  startProtocolSequence.push_back(CMD_SERIAL_NUMBER);
2390  startProtocolSequence.push_back(CMD_ORDER_NUMBER);
2391 
2392  /*
2393  * With "sWN TCTrackingMode 0" BASIC-Tracking activated
2394  * With "sWN TCTrackingMode 1" TRAFFIC-Tracking activated
2395  *
2396  */
2397  if (transmitRawTargets)
2398  {
2399  startProtocolSequence.push_back(CMD_SET_TRANSMIT_RAWTARGETS_ON); // raw targets will be transmitted
2400  }
2401  else
2402  {
2403  startProtocolSequence.push_back(CMD_SET_TRANSMIT_RAWTARGETS_OFF); // NO raw targets will be transmitted
2404  }
2405 
2406  if (transmitObjects)
2407  {
2408  startProtocolSequence.push_back(CMD_SET_TRANSMIT_OBJECTS_ON); // tracking objects will be transmitted
2409  }
2410  else
2411  {
2412  startProtocolSequence.push_back(CMD_SET_TRANSMIT_OBJECTS_OFF); // NO tracking objects will be transmitted
2413  }
2414 
2415  switch (trackingMode)
2416  {
2417  case 0:
2418  startProtocolSequence.push_back(CMD_SET_TRACKING_MODE_0);
2419  break;
2420  case 1:
2421  startProtocolSequence.push_back(CMD_SET_TRACKING_MODE_1);
2422  break;
2423  default:
2424  ROS_DEBUG("Tracking mode switching sequence unknown\n");
2425  break;
2426 
2427  }
2428  // leave user level
2429 
2430  // sWN TransmitTargets 1
2431  // initializing sequence for radar based devices
2432  startProtocolSequence.push_back(CMD_RUN); // leave user level
2433  startProtocolSequence.push_back(CMD_START_RADARDATA);
2434  }
2435  else
2436  {
2437 
2439  {
2440 
2441  // Activate LFErec, LIDoutputstate and LIDinputstate messages
2442  ros::NodeHandle tmpParam("~");
2443  bool activate_lferec = true, activate_lidoutputstate = true, activate_lidinputstate = true;
2444  if (true == tmpParam.getParam("activate_lferec", activate_lferec) && true == activate_lferec)
2445  {
2446  startProtocolSequence.push_back(CMD_SET_LFEREC_ACTIVE); // TiM781S: activate LFErec messages, send "sEN LFErec 1"
2447  ROS_INFO_STREAM(parser_->getCurrentParamPtr()->getScannerName() << ": activating field monitoring by lferec messages");
2448  }
2449  if (true == tmpParam.getParam("activate_lidoutputstate", activate_lidoutputstate) && true == activate_lidoutputstate)
2450  {
2451  startProtocolSequence.push_back(CMD_SET_LID_OUTPUTSTATE_ACTIVE); // TiM781S: activate LIDoutputstate messages, send "sEN LIDoutputstate 1"
2452  ROS_INFO_STREAM(parser_->getCurrentParamPtr()->getScannerName() << ": activating field monitoring by lidoutputstate messages");
2453  }
2454  if (true == tmpParam.getParam("activate_lidinputstate", activate_lidinputstate) && true == activate_lidinputstate)
2455  {
2456  startProtocolSequence.push_back(CMD_SET_LID_INPUTSTATE_ACTIVE); // TiM781S: activate LIDinputstate messages, send "sEN LIDinputstate 1"
2457  ROS_INFO_STREAM(parser_->getCurrentParamPtr()->getScannerName() << ": activating field monitoring by lidinputstate messages");
2458  }
2459  }
2460 
2461  // initializing sequence for laserscanner
2462  // is this device a TiM240????
2463  // The TiM240 can not interpret CMD_START_MEASUREMENT
2465  {
2466  // the TiM240 operates directly in the ros coordinate system
2467  // do nothing for a TiM240
2468  }
2469  else
2470  {
2471  startProtocolSequence.push_back(CMD_START_MEASUREMENT);
2472  }
2473  startProtocolSequence.push_back(CMD_RUN); // leave user level
2474  startProtocolSequence.push_back(CMD_START_SCANDATA);
2475 
2476  if (this->parser_->getCurrentParamPtr()->getNumberOfLayers() == 4) // MRS1104 - start IMU-Transfer
2477  {
2478  ros::NodeHandle tmp("~");
2479  bool imu_enable = false;
2480  tmp.getParam("imu_enable", imu_enable);
2481  if (imu_enable)
2482  {
2483  if (useBinaryCmdNow == true)
2484  {
2485  ROS_INFO("Enable IMU data transfer");
2486  // TODO Flag to decide between IMU on or off
2487  startProtocolSequence.push_back(CMD_START_IMU_DATA);
2488  }
2489  else
2490  {
2491  ROS_FATAL(
2492  "IMU USAGE NOT POSSIBLE IN ASCII COMMUNICATION MODE.\nTo use the IMU the communication with the scanner must be set to binary mode.\n This can be done by inserting the line:\n<param name=\"use_binary_protocol\" type=\"bool\" value=\"True\" />\n into the launchfile.\n See also https://github.com/SICKAG/sick_scan/blob/master/doc/IMU.md");
2493  exit(0);
2494  }
2495 
2496  }
2497  }
2498  }
2499 
2500  std::vector<int>::iterator it;
2501  for (it = startProtocolSequence.begin(); it != startProtocolSequence.end(); it++)
2502  {
2503  int cmdId = *it;
2504  std::vector<unsigned char> tmpReply;
2505  // sendSopasAndCheckAnswer(sopasCmdVec[cmdId].c_str(), &tmpReply);
2506 
2507  std::string sopasCmd = sopasCmdVec[cmdId];
2508  std::vector<unsigned char> replyDummy;
2509  std::vector<unsigned char> reqBinary;
2510  std::vector<unsigned char> sopasVec;
2511  sopasVec = stringToVector(sopasCmd);
2512  ROS_DEBUG("Command: %s", stripControl(sopasVec).c_str());
2513  if (useBinaryCmd)
2514  {
2515  this->convertAscii2BinaryCmd(sopasCmd.c_str(), &reqBinary);
2516  result = sendSopasAndCheckAnswer(reqBinary, &replyDummy, cmdId);
2517  sopasReplyBinVec[cmdId] = replyDummy;
2518 
2519  switch (cmdId)
2520  {
2521  case CMD_START_SCANDATA:
2522  // ROS_DEBUG("Sleeping for a couple of seconds of start measurement\n");
2523  // ros::Duration(10.0).sleep();
2524  break;
2525  }
2526  }
2527  else
2528  {
2529  result = sendSopasAndCheckAnswer(sopasCmd.c_str(), &replyDummy, cmdId);
2530  }
2531 
2532  if (result != 0)
2533  {
2534  ROS_ERROR("%s", sopasCmdErrMsg[cmdId].c_str());
2536  }
2537  else
2538  {
2539  sopasReplyStrVec[cmdId] = replyToString(replyDummy);
2540  }
2541 
2542 
2543  if (cmdId == CMD_START_RADARDATA)
2544  {
2545  ROS_DEBUG("Starting radar data ....\n");
2546  }
2547 
2548 
2549  if (cmdId == CMD_START_SCANDATA)
2550  {
2551  ROS_DEBUG("Starting scan data ....\n");
2552  }
2553 
2554  if (cmdId == CMD_RUN)
2555  {
2556  bool waitForDeviceState = true;
2557  if (this->parser_->getCurrentParamPtr()->getNumberOfLayers() == 1)
2558  {
2559  waitForDeviceState = false; // do nothing for tim5xx
2560  }
2561  if (this->parser_->getCurrentParamPtr()->getNumberOfLayers() == 24)
2562  {
2563  waitForDeviceState = false; // do nothing for MRS6xxx
2564  }
2565 
2566  if (waitForDeviceState)
2567  {
2568  int maxWaitForDeviceStateReady = 45; // max. 30 sec. (see manual)
2569  bool scannerReady = false;
2570  for (int i = 0; i < maxWaitForDeviceStateReady; i++)
2571  {
2572  double shortSleepTime = 1.0;
2573  std::string sopasCmd = sopasCmdVec[CMD_DEVICE_STATE];
2574  std::vector<unsigned char> replyDummy;
2575 
2576  int iRetVal = 0;
2577  int deviceState = 0;
2578 
2579  std::vector<unsigned char> reqBinary;
2580  std::vector<unsigned char> sopasVec;
2581  sopasVec = stringToVector(sopasCmd);
2582  ROS_DEBUG("Command: %s", stripControl(sopasVec).c_str());
2583  if (useBinaryCmd)
2584  {
2585  this->convertAscii2BinaryCmd(sopasCmd.c_str(), &reqBinary);
2586  result = sendSopasAndCheckAnswer(reqBinary, &replyDummy);
2587  sopasReplyBinVec[CMD_DEVICE_STATE] = replyDummy;
2588  }
2589  else
2590  {
2591  result = sendSopasAndCheckAnswer(sopasCmd.c_str(), &replyDummy);
2593  }
2594 
2595 
2596  if (useBinaryCmd)
2597  {
2598  long dummy0, dummy1;
2599  dummy0 = 0;
2600  dummy1 = 0;
2601  deviceState = 0;
2602  iRetVal = binScanfVec(&(sopasReplyBinVec[CMD_DEVICE_STATE]), "%4y%4ysRA SCdevicestate %1y", &dummy0,
2603  &dummy1, &deviceState);
2604  }
2605  else
2606  {
2607  iRetVal = sscanf(sopasReplyStrVec[CMD_DEVICE_STATE].c_str(), "sRA SCdevicestate %d", &deviceState);
2608  }
2609  if (iRetVal > 0) // 1 or 3 (depending of ASCII or Binary)
2610  {
2611  if (deviceState == 1) // scanner is ready
2612  {
2613  scannerReady = true; // interrupt waiting for scanner ready
2614  ROS_INFO("Scanner ready for measurement after %d [sec]", i);
2615  break;
2616  }
2617  }
2618  ROS_INFO("Waiting for scanner ready state since %d secs", i);
2619  ros::Duration(shortSleepTime).sleep();
2620 
2621  if (scannerReady)
2622  {
2623  break;
2624  }
2625  }
2626  }
2627  }
2628  tmpReply.clear();
2629 
2630  }
2631  return ExitSuccess;
2632  }
2633 
2634 
2640  std::string sick_scan::SickScanCommon::replyToString(const std::vector<unsigned char> &reply)
2641  {
2642  std::string reply_str;
2643  std::vector<unsigned char>::const_iterator it_start, it_end;
2644  int binLen = this->checkForBinaryAnswer(&reply);
2645  if (binLen == -1) // ASCII-Cmd
2646  {
2647  it_start = reply.begin();
2648  it_end = reply.end();
2649  }
2650  else
2651  {
2652  it_start = reply.begin() + 8; // skip header and length id
2653  it_end = reply.end() - 1; // skip CRC
2654  }
2655  bool inHexPrintMode = false;
2656  for (std::vector<unsigned char>::const_iterator it = it_start; it != it_end; it++)
2657  {
2658  // inHexPrintMode means that we should continue printing hex value after we started with hex-Printing
2659  // That is easier to debug for a human instead of switching between ascii binary and then back to ascii
2660  if (*it >= 0x20 && (inHexPrintMode == false)) // filter control characters for display
2661  {
2662  reply_str.push_back(*it);
2663  }
2664  else
2665  {
2666  if (binLen != -1) // binary
2667  {
2668  char szTmp[255] = {0};
2669  unsigned char val = *it;
2670  inHexPrintMode = true;
2671  sprintf(szTmp, "\\x%02x", val);
2672  for (size_t ii = 0; ii < strlen(szTmp); ii++)
2673  {
2674  reply_str.push_back(szTmp[ii]);
2675  }
2676  }
2677  }
2678 
2679  }
2680  return reply_str;
2681  }
2682 
2683  bool sick_scan::SickScanCommon::dumpDatagramForDebugging(unsigned char *buffer, int bufLen)
2684  {
2685  bool ret = true;
2686  static int cnt = 0;
2687  char szDumpFileName[511] = {0};
2688  char szDir[255] = {0};
2689  if (cnt == 0)
2690  {
2691  ROS_INFO("Attention: verboseLevel is set to 1. Datagrams are stored in the /tmp folder.");
2692  }
2693 #ifdef _MSC_VER
2694  strcpy(szDir, "C:\\temp\\");
2695 #else
2696  strcpy(szDir, "/tmp/");
2697 #endif
2698  sprintf(szDumpFileName, "%ssick_datagram_%06d.bin", szDir, cnt);
2699  bool isBinary = this->parser_->getCurrentParamPtr()->getUseBinaryProtocol();
2700  if (isBinary)
2701  {
2702  FILE *ftmp;
2703  ftmp = fopen(szDumpFileName, "wb");
2704  if (ftmp != NULL)
2705  {
2706  fwrite(buffer, bufLen, 1, ftmp);
2707  fclose(ftmp);
2708  }
2709  }
2710  cnt++;
2711 
2712  return (true);
2713 
2714  }
2715 
2716 
2722  bool sick_scan::SickScanCommon::isCompatibleDevice(const std::string identStr) const
2723  {
2724  char device_string[7];
2725  int version_major = -1;
2726  int version_minor = -1;
2727 
2728  strcpy(device_string, "???");
2729  // special for TiM3-Firmware
2730  if (sscanf(identStr.c_str(), "sRA 0 6 %6s E V%d.%d", device_string,
2731  &version_major, &version_minor) == 3
2732  && strncmp("TiM3", device_string, 4) == 0
2733  && version_major >= 2 && version_minor >= 50)
2734  {
2735  ROS_ERROR("This scanner model/firmware combination does not support ranging output!");
2736  ROS_ERROR("Supported scanners: TiM5xx: all firmware versions; TiM3xx: firmware versions < V2.50.");
2737  ROS_ERROR("This is a %s, firmware version %d.%d", device_string, version_major, version_minor);
2738 
2739  return false;
2740  }
2741 
2742  bool supported = false;
2743 
2744  // DeviceIdent 8 MRS1xxxx 8 1.3.0.0R.
2745  if (sscanf(identStr.c_str(), "sRA 0 6 %6s E V%d.%d", device_string, &version_major, &version_minor) == 3)
2746  {
2747  std::string devStr = device_string;
2748 
2749 
2750  if (devStr.compare(0, 4, "TiM5") == 0)
2751  {
2752  supported = true;
2753  }
2754 
2755  if (supported == true)
2756  {
2757  ROS_INFO("Device %s V%d.%d found and supported by this driver.", identStr.c_str(), version_major,
2758  version_minor);
2759  }
2760 
2761  }
2762 
2763  if ((identStr.find("MRS1xxx") !=
2764  std::string::npos) // received pattern contains 4 'x' but we check only for 3 'x' (MRS1104 should be MRS1xxx)
2765  || (identStr.find("LMS1xxx") != std::string::npos)
2766  )
2767  {
2768  ROS_INFO("Deviceinfo %s found and supported by this driver.", identStr.c_str());
2769  supported = true;
2770  }
2771 
2772 
2773  if (identStr.find("MRS6") !=
2774  std::string::npos) // received pattern contains 4 'x' but we check only for 3 'x' (MRS1104 should be MRS1xxx)
2775  {
2776  ROS_INFO("Deviceinfo %s found and supported by this driver.", identStr.c_str());
2777  supported = true;
2778  }
2779 
2780  if (identStr.find("RMS3xx") !=
2781  std::string::npos) // received pattern contains 4 'x' but we check only for 3 'x' (MRS1104 should be MRS1xxx)
2782  {
2783  ROS_INFO("Deviceinfo %s found and supported by this driver.", identStr.c_str());
2784  supported = true;
2785  }
2786 
2787 
2788  if (supported == false)
2789  {
2790  ROS_WARN("Device %s V%d.%d found and maybe unsupported by this driver.", device_string, version_major,
2791  version_minor);
2792  ROS_WARN("Full SOPAS answer: %s", identStr.c_str());
2793  }
2794  return true;
2795  }
2796 
2797 
2803  {
2804  static int cnt = 0;
2805  diagnostics_.update();
2806 
2807  unsigned char receiveBuffer[65536];
2808  int actual_length = 0;
2809  static unsigned int iteration_count = 0;
2810  bool useBinaryProtocol = this->parser_->getCurrentParamPtr()->getUseBinaryProtocol();
2811 
2812  ros::Time recvTimeStamp = ros::Time::now(); // timestamp incoming package, will be overwritten by get_datagram
2813  // tcp packet
2814  ros::Time recvTimeStampPush = ros::Time::now(); // timestamp
2815 
2816  /*
2817  * Try to get datagram
2818  *
2819  *
2820  */
2821 
2822 
2823  int packetsInLoop = 0;
2824 
2825  const int maxNumAllowedPacketsToProcess = 25; // maximum number of packets, which will be processed in this loop.
2826 
2827  int numPacketsProcessed = 0; // count number of processed datagrams
2828 
2829  static bool firstTimeCalled = true;
2830  static bool dumpData = false;
2831  static int verboseLevel = 0;
2832  static bool slamBundle = false;
2833  float timeIncrement;
2834  static std::string echoForSlam = "";
2835  if (firstTimeCalled == true)
2836  {
2837 
2838  /* Dump Binary Protocol */
2839  ros::NodeHandle tmpParam("~");
2840  tmpParam.getParam("slam_echo", echoForSlam);
2841  tmpParam.getParam("slam_bundle", slamBundle);
2842  tmpParam.getParam("verboseLevel", verboseLevel);
2843  firstTimeCalled = false;
2844  }
2845  do
2846  {
2847 
2848  int result = get_datagram(recvTimeStamp, receiveBuffer, 65536, &actual_length, useBinaryProtocol, &packetsInLoop);
2849  numPacketsProcessed++;
2850 
2851  ros::Duration dur = recvTimeStampPush - recvTimeStamp;
2852 
2853  if (result != 0)
2854  {
2855  ROS_ERROR("Read Error when getting datagram: %i.", result);
2856  diagnostics_.broadcast(getDiagnosticErrorCode(), "Read Error when getting datagram.");
2857  return ExitError; // return failure to exit node
2858  }
2859  if (actual_length <= 0)
2860  {
2861  return ExitSuccess;
2862  } // return success to continue looping
2863 
2864  // ----- if requested, skip frames
2865  if (iteration_count++ % (config_.skip + 1) != 0)
2866  {
2867  return ExitSuccess;
2868  }
2869 
2870  ROS_DEBUG_STREAM("SickScanCommon::loopOnce: received " << actual_length << " byte data " << DataDumper::binDataToAsciiString(&receiveBuffer[0], std::min(32, actual_length)) << " ... ");
2871 
2872 
2873  if (publish_datagram_)
2874  {
2875  std_msgs::String datagram_msg;
2876  datagram_msg.data = std::string(reinterpret_cast<char *>(receiveBuffer));
2877  datagram_pub_.publish(datagram_msg);
2878  }
2879 
2880 
2881  if (verboseLevel > 0)
2882  {
2883  dumpDatagramForDebugging(receiveBuffer, actual_length);
2884  }
2885 
2886 
2887  bool deviceIsRadar = false;
2888 
2889  if (this->parser_->getCurrentParamPtr()->getDeviceIsRadar() == true)
2890  {
2891  deviceIsRadar = true;
2892  }
2893 
2894  if (true == deviceIsRadar)
2895  {
2897  int errorCode = ExitSuccess;
2898  // parse radar telegram and send pointcloud2-debug messages
2899  errorCode = radar->parseDatagram(recvTimeStamp, (unsigned char *) receiveBuffer, actual_length,
2900  useBinaryProtocol);
2901  return errorCode; // return success to continue looping
2902  }
2903 
2904  static SickScanImu scanImu(this); // todo remove static
2905  if (scanImu.isImuDatagram((char *) receiveBuffer, actual_length))
2906  {
2907  int errorCode = ExitSuccess;
2908  if (scanImu.isImuAckDatagram((char *) receiveBuffer, actual_length))
2909  {
2910 
2911  }
2912  else
2913  {
2914  // parse radar telegram and send pointcloud2-debug messages
2915  errorCode = scanImu.parseDatagram(recvTimeStamp, (unsigned char *) receiveBuffer, actual_length,
2916  useBinaryProtocol);
2917 
2918  }
2919  return errorCode; // return success to continue looping
2920  }
2921  else if(memcmp(&receiveBuffer[8], "sSN LIDoutputstate", strlen("sSN LIDoutputstate")) == 0)
2922  {
2923  int errorCode = ExitSuccess;
2924  ROS_DEBUG_STREAM("SickScanCommon: received " << actual_length << " byte LIDoutputstate " << DataDumper::binDataToAsciiString(&receiveBuffer[0], actual_length));
2925  // Parse and convert LIDoutputstate message
2926  std::string scanner_name = parser_->getCurrentParamPtr()->getScannerName();
2927  EVAL_FIELD_SUPPORT eval_field_logic = this->parser_->getCurrentParamPtr()->getUseEvalFields();
2928  sick_scan::LIDoutputstateMsg outputstate_msg;
2929  if (sick_scan::SickScanMessages::parseLIDoutputstateMsg(recvTimeStamp, receiveBuffer, actual_length, useBinaryProtocol, scanner_name, outputstate_msg))
2930  {
2931  // Publish LIDoutputstate message
2933  {
2934  lidoutputstate_pub_.publish(outputstate_msg);
2935  }
2936  if(cloud_marker_)
2937  {
2938  cloud_marker_->updateMarker(outputstate_msg, eval_field_logic);
2939  }
2940  }
2941  else
2942  {
2943  ROS_WARN_STREAM("## ERROR SickScanCommon: parseLIDoutputstateMsg failed, received " << actual_length << " byte LIDoutputstate " << DataDumper::binDataToAsciiString(&receiveBuffer[0], actual_length));
2944  }
2945  return errorCode; // return success to continue looping
2946  }
2947  else if(memcmp(&receiveBuffer[8], "sSN LIDinputstate", strlen("sSN LIDinputstate")) == 0)
2948  {
2949  int errorCode = ExitSuccess;
2950  // Parse active_fieldsetfrom LIDinputstate message
2952  if(fieldMon && useBinaryProtocol && actual_length > 32)
2953  {
2954  // int fieldset = (receiveBuffer[32] & 0xFF);
2955  // fieldMon->setActiveFieldset(fieldset);
2956  fieldMon->parseBinaryLIDinputstateMsg(receiveBuffer, actual_length);
2957  ROS_DEBUG_STREAM("SickScanCommon: received " << actual_length << " byte LIDinputstate " << DataDumper::binDataToAsciiString(&receiveBuffer[0], actual_length)
2958  << ", active fieldset = " << fieldMon->getActiveFieldset());
2959  }
2960  return errorCode; // return success to continue looping
2961  }
2962  else if(memcmp(&receiveBuffer[8], "sSN LFErec", strlen("sSN LFErec")) == 0)
2963  {
2964  int errorCode = ExitSuccess;
2965  ROS_DEBUG_STREAM("SickScanCommon: received " << actual_length << " byte LFErec " << DataDumper::binDataToAsciiString(&receiveBuffer[0], actual_length));
2966  // Parse and convert LFErec message
2967  sick_scan::LFErecMsg lferec_msg;
2968  std::string scanner_name = parser_->getCurrentParamPtr()->getScannerName();
2969  EVAL_FIELD_SUPPORT eval_field_logic = parser_->getCurrentParamPtr()->getUseEvalFields(); // == USE_EVAL_FIELD_LMS5XX_LOGIC
2970  if (sick_scan::SickScanMessages::parseLFErecMsg(recvTimeStamp, receiveBuffer, actual_length, useBinaryProtocol, eval_field_logic, scanner_name, lferec_msg))
2971  {
2972  // Publish LFErec message
2973  if(publish_lferec_)
2974  {
2975  lferec_pub_.publish(lferec_msg);
2976  }
2977  if(cloud_marker_)
2978  {
2979  cloud_marker_->updateMarker(lferec_msg, eval_field_logic);
2980  }
2981  }
2982  else
2983  {
2984  ROS_WARN_STREAM("## ERROR SickScanCommon: parseLFErecMsg failed, received " << actual_length << " byte LFErec " << DataDumper::binDataToAsciiString(&receiveBuffer[0], actual_length));
2985  }
2986  return errorCode; // return success to continue looping
2987  }
2988  else if(memcmp(&receiveBuffer[8], "sSN LMDscandatamon", strlen("sSN LMDscandatamon")) == 0)
2989  {
2990  int errorCode = ExitSuccess;
2991  ROS_DEBUG_STREAM("SickScanCommon: received " << actual_length << " byte LMDscandatamon (ignored) ..."); // << DataDumper::binDataToAsciiString(&receiveBuffer[0], actual_length));
2992  return errorCode; // return success to continue looping
2993  }
2994  else
2995  {
2996 
2997  sensor_msgs::LaserScan msg;
2998  sick_scan::Encoder EncoderMsg;
2999  EncoderMsg.header.stamp = recvTimeStamp + ros::Duration(config_.time_offset);
3000  //TODO remove this hardcoded variable
3001  bool FireEncoder = false;
3002  EncoderMsg.header.frame_id = "Encoder";
3003  EncoderMsg.header.seq = numPacketsProcessed;
3004  msg.header.stamp = recvTimeStamp + ros::Duration(config_.time_offset);
3005  double elevationAngleInRad = 0.0;
3006  /*
3007  * datagrams are enclosed in <STX> (0x02), <ETX> (0x03) pairs
3008  */
3009  char *buffer_pos = (char *) receiveBuffer;
3010  char *dstart, *dend;
3011  bool dumpDbg = false;
3012  bool dataToProcess = true;
3013  std::vector<float> vang_vec;
3014  vang_vec.clear();
3015  dstart = NULL;
3016  dend = NULL;
3017 
3018  while (dataToProcess)
3019  {
3020  const int maxAllowedEchos = 5;
3021 
3022  int numValidEchos = 0;
3023  int aiValidEchoIdx[maxAllowedEchos] = {0};
3024  size_t dlength;
3025  int success = -1;
3026  int numEchos = 0;
3027  int echoMask = 0;
3028  bool publishPointCloud = true;
3029 
3030  if (useBinaryProtocol)
3031  {
3032  // if binary protocol used then parse binary message
3033  std::vector<unsigned char> receiveBufferVec = std::vector<unsigned char>(receiveBuffer,
3034  receiveBuffer + actual_length);
3035 #ifdef DEBUG_DUMP_TO_CONSOLE_ENABLED
3036  if (actual_length > 1000)
3037  {
3038  DataDumper::instance().dumpUcharBufferToConsole(receiveBuffer, actual_length);
3039 
3040  }
3041 
3042  DataDumper::instance().dumpUcharBufferToConsole(receiveBuffer, actual_length);
3043 #endif
3044  if (receiveBufferVec.size() > 8)
3045  {
3046  long idVal = 0;
3047  long lenVal = 0;
3048  memcpy(&idVal, receiveBuffer + 0, 4); // read identifier
3049  memcpy(&lenVal, receiveBuffer + 4, 4); // read length indicator
3050  swap_endian((unsigned char *) &lenVal, 4);
3051 
3052  if (idVal == 0x02020202) // id for binary message
3053  {
3054 #if 0
3055  {
3056  static int cnt = 0;
3057  char szFileName[255];
3058 
3059 #ifdef _MSC_VER
3060  sprintf(szFileName, "c:\\temp\\dump%05d.bin", cnt);
3061 #else
3062  sprintf(szFileName, "/tmp/dump%05d.txt", cnt);
3063 #endif
3064  FILE *fout;
3065  fout = fopen(szFileName, "wb");
3066  fwrite(receiveBuffer, actual_length, 1, fout);
3067  fclose(fout);
3068  cnt++;
3069  }
3070 #endif
3071  // binary message
3072  if (lenVal < actual_length)
3073  {
3074  short elevAngleX200 = 0; // signed short (F5 B2 -> Layer 24
3075  // F5B2h -> -2638/200= -13.19°
3076  int scanFrequencyX100 = 0;
3077  double elevAngle = 0.00;
3078  double scanFrequency = 0.0;
3079  long measurementFrequencyDiv100 = 0; // multiply with 100
3080  int numOfEncoders = 0;
3081  int numberOf16BitChannels = 0;
3082  int numberOf8BitChannels = 0;
3083  uint32_t SystemCountScan = 0;
3084  static uint32_t lastSystemCountScan = 0;// this variable is used to ensure that only the first time stamp of an multi layer scann is used for PLL updating
3085  uint32_t SystemCountTransmit = 0;
3086 
3087  memcpy(&elevAngleX200, receiveBuffer + 50, 2);
3088  swap_endian((unsigned char *) &elevAngleX200, 2);
3089 
3090  elevationAngleInRad = -elevAngleX200 / 200.0 * deg2rad_const;
3091  //TODO check this ??
3092  msg.header.seq = elevAngleX200; // should be multiple of 0.625° starting with -2638 (corresponding to 13.19°)
3093 
3094  memcpy(&SystemCountScan, receiveBuffer + 0x26, 4);
3095  swap_endian((unsigned char *) &SystemCountScan, 4);
3096 
3097  memcpy(&SystemCountTransmit, receiveBuffer + 0x2A, 4);
3098  swap_endian((unsigned char *) &SystemCountTransmit, 4);
3099  double timestampfloat = recvTimeStamp.sec + recvTimeStamp.nsec * 1e-9;
3100  bool bRet;
3101  if (SystemCountScan !=
3102  lastSystemCountScan)//MRS 6000 sends 6 packets with same SystemCountScan we should only update the pll once with this time stamp since the SystemCountTransmit are different and this will only increase jitter of the pll
3103  {
3104  bRet = SoftwarePLL::instance().updatePLL(recvTimeStamp.sec, recvTimeStamp.nsec,
3105  SystemCountTransmit);
3106  lastSystemCountScan = SystemCountScan;
3107  }
3108  ros::Time tmp_time = recvTimeStamp;
3109  bRet = SoftwarePLL::instance().getCorrectedTimeStamp(recvTimeStamp.sec, recvTimeStamp.nsec,
3110  SystemCountScan);
3111  double timestampfloat_coor = recvTimeStamp.sec + recvTimeStamp.nsec * 1e-9;
3112  double DeltaTime = timestampfloat - timestampfloat_coor;
3113  //ROS_INFO("%F,%F,%u,%u,%F",timestampfloat,timestampfloat_coor,SystemCountTransmit,SystemCountScan,DeltaTime);
3114  //TODO Handle return values
3115  if (config_.sw_pll_only_publish == true && bRet == false)
3116  {
3117  int packets_expected_to_drop = SoftwarePLL::instance().fifoSize - 1;
3119  ROS_INFO("%i / %i Packet dropped Software PLL not yet locked.",
3120  SoftwarePLL::instance().packeds_droped, packets_expected_to_drop);
3121  if (SoftwarePLL::instance().packeds_droped == packets_expected_to_drop)
3122  {
3123  ROS_INFO("Software PLL is expected to be ready now!");
3124  }
3125  if (SoftwarePLL::instance().packeds_droped > packets_expected_to_drop)
3126  {
3127  ROS_WARN("More packages than expected were dropped!!\n"
3128  "Check the network connection.\n"
3129  "Check if the system time has been changed in a leap.\n"
3130  "If the problems can persist, disable the software PLL with the option sw_pll_only_publish=False !");
3131  }
3132  dataToProcess = false;
3133  break;
3134  }
3135 
3136 #ifdef DEBUG_DUMP_ENABLED
3137  double elevationAngleInDeg = elevationAngleInRad = -elevAngleX200 / 200.0;
3138  // DataDumper::instance().pushData((double)SystemCountScan, "LAYER", elevationAngleInDeg);
3139  //DataDumper::instance().pushData((double)SystemCountScan, "LASESCANTIME", SystemCountScan);
3140  //DataDumper::instance().pushData((double)SystemCountTransmit, "LASERTRANSMITTIME", SystemCountTransmit);
3141  //DataDumper::instance().pushData((double)SystemCountScan, "LASERTRANSMITDELAY", debug_duration.toSec());
3142 #endif
3143 
3144  /*
3145  uint16_t u16_active_fieldset = 0;
3146  memcpy(&u16_active_fieldset, receiveBuffer + 46, 2); // byte 46 + 47: input status (0 0), active fieldset
3147  swap_endian((unsigned char *) &u16_active_fieldset, 2);
3148  SickScanFieldMonSingleton *fieldMon = SickScanFieldMonSingleton::getInstance();
3149  if(fieldMon)
3150  {
3151  fieldMon->setActiveFieldset(u16_active_fieldset & 0xFF);
3152  ROS_INFO("Binary scandata: active_fieldset = %d", fieldMon->getActiveFieldset());
3153  }
3154  */
3155  // byte 48 + 49: output status (0 0)
3156  // byte 50 + 51: reserved
3157 
3158  memcpy(&scanFrequencyX100, receiveBuffer + 52, 4);
3159  swap_endian((unsigned char *) &scanFrequencyX100, 4);
3160 
3161  memcpy(&measurementFrequencyDiv100, receiveBuffer + 56, 4);
3162  swap_endian((unsigned char *) &measurementFrequencyDiv100, 4);
3163 
3164 
3165  msg.scan_time = 1.0 / (scanFrequencyX100 / 100.0);
3166 
3167  //due firmware inconsistency
3168  if (measurementFrequencyDiv100 > 10000)
3169  {
3170  measurementFrequencyDiv100 /= 100;
3171  }
3172  msg.time_increment = 1.0 / (measurementFrequencyDiv100 * 100.0);
3173  timeIncrement = msg.time_increment;
3174  msg.range_min = parser_->get_range_min();
3175  msg.range_max = parser_->get_range_max();
3176 
3177  memcpy(&numOfEncoders, receiveBuffer + 60, 2);
3178  swap_endian((unsigned char *) &numOfEncoders, 2);
3179  int encoderDataOffset = 6 * numOfEncoders;
3180  int32_t EncoderPosTicks[4] = {0};
3181  int16_t EncoderSpeed[4] = {0};
3182 
3183  if (numOfEncoders > 0 && numOfEncoders < 5)
3184  {
3185  FireEncoder = true;
3186  for (int EncoderNum = 0; EncoderNum < numOfEncoders; EncoderNum++)
3187  {
3188  memcpy(&EncoderPosTicks[EncoderNum], receiveBuffer + 62 + EncoderNum * 6, 4);
3189  swap_endian((unsigned char *) &EncoderPosTicks[EncoderNum], 4);
3190  memcpy(&EncoderSpeed[EncoderNum], receiveBuffer + 66 + EncoderNum * 6, 2);
3191  swap_endian((unsigned char *) &EncoderSpeed[EncoderNum], 2);
3192  }
3193  }
3194  //TODO handle multi encoder with multiple encode msg or different encoder msg definition now using only first encoder
3195  EncoderMsg.enc_position = EncoderPosTicks[0];
3196  EncoderMsg.enc_speed = EncoderSpeed[0];
3197  memcpy(&numberOf16BitChannels, receiveBuffer + 62 + encoderDataOffset, 2);
3198  swap_endian((unsigned char *) &numberOf16BitChannels, 2);
3199 
3200  int parseOff = 64 + encoderDataOffset;
3201 
3202 
3203  char szChannel[255] = {0};
3204  float scaleFactor = 1.0;
3205  float scaleFactorOffset = 0.0;
3206  int32_t startAngleDiv10000 = 1;
3207  int32_t sizeOfSingleAngularStepDiv10000 = 1;
3208  double startAngle = 0.0;
3209  double sizeOfSingleAngularStep = 0.0;
3210  short numberOfItems = 0;
3211 
3212  static int cnt = 0;
3213  cnt++;
3214  // get number of 8 bit channels
3215  // we must jump of the 16 bit data blocks including header ...
3216  for (int i = 0; i < numberOf16BitChannels; i++)
3217  {
3218  int numberOfItems = 0x00;
3219  memcpy(&numberOfItems, receiveBuffer + parseOff + 19, 2);
3220  swap_endian((unsigned char *) &numberOfItems, 2);
3221  parseOff += 21; // 21 Byte header followed by data entries
3222  parseOff += numberOfItems * 2;
3223  }
3224 
3225  // now we can read the number of 8-Bit-Channels
3226  memcpy(&numberOf8BitChannels, receiveBuffer + parseOff, 2);
3227  swap_endian((unsigned char *) &numberOf8BitChannels, 2);
3228 
3229  parseOff = 64 + encoderDataOffset;
3230  enum datagram_parse_task
3231  {
3232  process_dist,
3233  process_vang,
3234  process_rssi,
3235  process_idle
3236  };
3237  int rssiCnt = 0;
3238  int vangleCnt = 0;
3239  int distChannelCnt = 0;
3240 
3241  for (int processLoop = 0; processLoop < 2; processLoop++)
3242  {
3243  int totalChannelCnt = 0;
3244 
3245 
3246  bool bCont = true;
3247 
3248  datagram_parse_task task = process_idle;
3249  bool parsePacket = true;
3250  parseOff = 64 + encoderDataOffset;
3251  bool processData = false;
3252 
3253  if (processLoop == 0)
3254  {
3255  distChannelCnt = 0;
3256  rssiCnt = 0;
3257  vangleCnt = 0;
3258  }
3259 
3260  if (processLoop == 1)
3261  {
3262  processData = true;
3263  numEchos = distChannelCnt;
3264  msg.ranges.resize(numberOfItems * numEchos);
3265  if (rssiCnt > 0)
3266  {
3267  msg.intensities.resize(numberOfItems * rssiCnt);
3268  }
3269  else
3270  {
3271  }
3272  if (vangleCnt > 0) // should be 0 or 1
3273  {
3274  vang_vec.resize(numberOfItems * vangleCnt);
3275  }
3276  else
3277  {
3278  vang_vec.clear();
3279  }
3280  echoMask = (1 << numEchos) - 1;
3281 
3282  // reset count. We will use the counter for index calculation now.
3283  distChannelCnt = 0;
3284  rssiCnt = 0;
3285  vangleCnt = 0;
3286 
3287  }
3288 
3289  szChannel[6] = '\0';
3290  scaleFactor = 1.0;
3291  scaleFactorOffset = 0.0;
3292  startAngleDiv10000 = 1;
3293  sizeOfSingleAngularStepDiv10000 = 1;
3294  startAngle = 0.0;
3295  sizeOfSingleAngularStep = 0.0;
3296  numberOfItems = 0;
3297 
3298 
3299 #if 1 // prepared for multiecho parsing
3300 
3301  bCont = true;
3302  bool doVangVecProc = false;
3303  // try to get number of DIST and RSSI from binary data
3304  task = process_idle;
3305  do
3306  {
3307  task = process_idle;
3308  doVangVecProc = false;
3309  int processDataLenValuesInBytes = 2;
3310 
3311  if (totalChannelCnt == numberOf16BitChannels)
3312  {
3313  parseOff += 2; // jump of number of 8 bit channels- already parsed above
3314  }
3315 
3316  if (totalChannelCnt >= numberOf16BitChannels)
3317  {
3318  processDataLenValuesInBytes = 1; // then process 8 bit values ...
3319  }
3320  bCont = false;
3321  strcpy(szChannel, "");
3322 
3323  if (totalChannelCnt < (numberOf16BitChannels + numberOf8BitChannels))
3324  {
3325  szChannel[5] = '\0';
3326  strncpy(szChannel, (const char *) receiveBuffer + parseOff, 5);
3327  }
3328  else
3329  {
3330  // all channels processed (16 bit and 8 bit channels)
3331  }
3332 
3333  if (strstr(szChannel, "DIST") == szChannel)
3334  {
3335  task = process_dist;
3336  distChannelCnt++;
3337  bCont = true;
3338  numberOfItems = 0;
3339  memcpy(&numberOfItems, receiveBuffer + parseOff + 19, 2);
3340  swap_endian((unsigned char *) &numberOfItems, 2);
3341 
3342  }
3343  if (strstr(szChannel, "VANG") == szChannel)
3344  {
3345  vangleCnt++;
3346  task = process_vang;
3347  bCont = true;
3348  numberOfItems = 0;
3349  memcpy(&numberOfItems, receiveBuffer + parseOff + 19, 2);
3350  swap_endian((unsigned char *) &numberOfItems, 2);
3351 
3352  vang_vec.resize(numberOfItems);
3353 
3354  }
3355  if (strstr(szChannel, "RSSI") == szChannel)
3356  {
3357  task = process_rssi;
3358  rssiCnt++;
3359  bCont = true;
3360  numberOfItems = 0;
3361  // copy two byte value (unsigned short to numberOfItems
3362  memcpy(&numberOfItems, receiveBuffer + parseOff + 19, 2);
3363  swap_endian((unsigned char *) &numberOfItems, 2); // swap
3364 
3365  }
3366  if (bCont)
3367  {
3368  scaleFactor = 0.0;
3369  scaleFactorOffset = 0.0;
3370  startAngleDiv10000 = 0;
3371  sizeOfSingleAngularStepDiv10000 = 0;
3372  numberOfItems = 0;
3373 
3374  memcpy(&scaleFactor, receiveBuffer + parseOff + 5, 4);
3375  memcpy(&scaleFactorOffset, receiveBuffer + parseOff + 9, 4);
3376  memcpy(&startAngleDiv10000, receiveBuffer + parseOff + 13, 4);
3377  memcpy(&sizeOfSingleAngularStepDiv10000, receiveBuffer + parseOff + 17, 2);
3378  memcpy(&numberOfItems, receiveBuffer + parseOff + 19, 2);
3379 
3380 
3381  swap_endian((unsigned char *) &scaleFactor, 4);
3382  swap_endian((unsigned char *) &scaleFactorOffset, 4);
3383  swap_endian((unsigned char *) &startAngleDiv10000, 4);
3384  swap_endian((unsigned char *) &sizeOfSingleAngularStepDiv10000, 2);
3385  swap_endian((unsigned char *) &numberOfItems, 2);
3386 
3387  if (processData)
3388  {
3389  unsigned short *data = (unsigned short *) (receiveBuffer + parseOff + 21);
3390 
3391  unsigned char *swapPtr = (unsigned char *) data;
3392  // copy RSSI-Values +2 for 16-bit values +1 for 8-bit value
3393  for (int i = 0;
3394  i < numberOfItems * processDataLenValuesInBytes; i += processDataLenValuesInBytes)
3395  {
3396  if (processDataLenValuesInBytes == 1)
3397  {
3398  }
3399  else
3400  {
3401  unsigned char tmp;
3402  tmp = swapPtr[i + 1];
3403  swapPtr[i + 1] = swapPtr[i];
3404  swapPtr[i] = tmp;
3405  }
3406  }
3407  int idx = 0;
3408 
3409  switch (task)
3410  {
3411 
3412  case process_dist:
3413  {
3414  startAngle = startAngleDiv10000 / 10000.00;
3415  sizeOfSingleAngularStep = sizeOfSingleAngularStepDiv10000 / 10000.0;
3416  sizeOfSingleAngularStep *= (M_PI / 180.0);
3417 
3418  msg.angle_min = startAngle / 180.0 * M_PI - M_PI / 2;
3419  msg.angle_increment = sizeOfSingleAngularStep;
3420  msg.angle_max = msg.angle_min + (numberOfItems - 1) * msg.angle_increment;
3421 
3423  {
3424  msg.angle_min -= M_PI/2;
3425  msg.angle_max -= M_PI/2;
3426 
3427  msg.angle_min *= -1.0;
3428  msg.angle_increment *= -1.0;
3429  msg.angle_max *= -1.0;
3430 
3431  }
3432  float *rangePtr = NULL;
3433 
3434  if (numberOfItems > 0)
3435  {
3436  rangePtr = &msg.ranges[0];
3437  }
3438  float scaleFactor_001 = 0.001F * scaleFactor;// to avoid repeated multiplication
3439  for (int i = 0; i < numberOfItems; i++)
3440  {
3441  idx = i + numberOfItems * (distChannelCnt - 1);
3442  rangePtr[idx] = (float) data[i] * scaleFactor_001 + scaleFactorOffset;
3443 #ifdef DEBUG_DUMP_ENABLED
3444  if (distChannelCnt == 1)
3445  {
3446  if (i == floor(numberOfItems / 2))
3447  {
3448  double curTimeStamp = SystemCountScan + i * msg.time_increment * 1E6;
3449  //DataDumper::instance().pushData(curTimeStamp, "DIST", rangePtr[idx]);
3450  }
3451  }
3452 #endif
3453  //XXX
3454  }
3455 
3456  }
3457  break;
3458  case process_rssi:
3459  {
3460  // Das muss vom Protokoll abgeleitet werden. !!!
3461 
3462  float *intensityPtr = NULL;
3463 
3464  if (numberOfItems > 0)
3465  {
3466  intensityPtr = &msg.intensities[0];
3467 
3468  }
3469  for (int i = 0; i < numberOfItems; i++)
3470  {
3471  idx = i + numberOfItems * (rssiCnt - 1);
3472  // we must select between 16 bit and 8 bit values
3473  float rssiVal = 0.0;
3474  if (processDataLenValuesInBytes == 2)
3475  {
3476  rssiVal = (float) data[i];
3477  }
3478  else
3479  {
3480  unsigned char *data8Ptr = (unsigned char *) data;
3481  rssiVal = (float) data8Ptr[i];
3482  }
3483  intensityPtr[idx] = rssiVal * scaleFactor + scaleFactorOffset;
3484  }
3485  }
3486  break;
3487 
3488  case process_vang:
3489  float *vangPtr = NULL;
3490  if (numberOfItems > 0)
3491  {
3492  vangPtr = &vang_vec[0]; // much faster, with vang_vec[i] each time the size will be checked
3493  }
3494  for (int i = 0; i < numberOfItems; i++)
3495  {
3496  vangPtr[i] = (float) data[i] * scaleFactor + scaleFactorOffset;
3497  }
3498  break;
3499  }
3500  }
3501  parseOff += 21 + processDataLenValuesInBytes * numberOfItems;
3502 
3503 
3504  }
3505  totalChannelCnt++;
3506  } while (bCont);
3507  }
3508 #endif
3509 
3510  elevAngle = elevAngleX200 / 200.0;
3511  scanFrequency = scanFrequencyX100 / 100.0;
3512 
3513 
3514  }
3515  }
3516  }
3517 
3518  //TODO timing issue posible here
3519  parser_->checkScanTiming(msg.time_increment, msg.scan_time, msg.angle_increment, 0.00001f);
3520 
3521  success = ExitSuccess;
3522  // change Parsing Mode
3523  dataToProcess = false; // only one package allowed - no chaining
3524  }
3525  else // Parsing of Ascii-Encoding of datagram, xxx
3526  {
3527  dstart = strchr(buffer_pos, 0x02);
3528  if (dstart != NULL)
3529  {
3530  dend = strchr(dstart + 1, 0x03);
3531  }
3532  if ((dstart != NULL) && (dend != NULL))
3533  {
3534  dataToProcess = true; // continue parasing
3535  dlength = dend - dstart;
3536  *dend = '\0';
3537  dstart++;
3538  }
3539  else
3540  {
3541  dataToProcess = false;
3542  break; //
3543  }
3544 
3545  if (dumpDbg)
3546  {
3547  {
3548  static int cnt = 0;
3549  char szFileName[255];
3550 
3551 #ifdef _MSC_VER
3552  sprintf(szFileName, "c:\\temp\\dump%05d.txt", cnt);
3553 #else
3554  sprintf(szFileName, "/tmp/dump%05d.txt", cnt);
3555 #endif
3556 #if 0
3557  FILE *fout;
3558  fout = fopen(szFileName, "wb");
3559  fwrite(dstart, dlength, 1, fout);
3560  fclose(fout);
3561 #endif
3562  cnt++;
3563  }
3564  }
3565 
3566  // HEADER of data followed by DIST1 ... DIST2 ... DIST3 .... RSSI1 ... RSSI2.... RSSI3...
3567 
3568  // <frameid>_<sign>00500_DIST[1|2|3]
3569  numEchos = 1;
3570  // numEchos contains number of echos (1..5)
3571  // _msg holds ALL data of all echos
3572  success = parser_->parse_datagram(dstart, dlength, config_, msg, numEchos, echoMask);
3573  publishPointCloud = true; // for MRS1000
3574 
3575  numValidEchos = 0;
3576  for (int i = 0; i < maxAllowedEchos; i++)
3577  {
3578  aiValidEchoIdx[i] = 0;
3579  }
3580  }
3581 
3582 
3583  int numOfLayers = parser_->getCurrentParamPtr()->getNumberOfLayers();
3584 
3585  if (success == ExitSuccess)
3586  {
3587  bool elevationPreCalculated = false;
3588  double elevationAngleDegree = 0.0;
3589 
3590 
3591  std::vector<float> rangeTmp = msg.ranges; // copy all range value
3592  std::vector<float> intensityTmp = msg.intensities; // copy all intensity value
3593 
3594  int intensityTmpNum = intensityTmp.size();
3595  float *intensityTmpPtr = NULL;
3596  if (intensityTmpNum > 0)
3597  {
3598  intensityTmpPtr = &intensityTmp[0];
3599  }
3600 
3601  // Helpful: https://answers.ros.org/question/191265/pointcloud2-access-data/
3602  // https://gist.github.com/yuma-m/b5dcce1b515335c93ce8
3603  // Copy to pointcloud
3604  int layer = 0;
3605  int baseLayer = 0;
3606  bool useGivenElevationAngle = false;
3607  switch (numOfLayers)
3608  {
3609  case 1: // TIM571 etc.
3610  baseLayer = 0;
3611  break;
3612  case 4:
3613 
3614  baseLayer = -1;
3615  if (msg.header.seq == 250)
3616  { layer = -1; }
3617  else if (msg.header.seq == 0)
3618  { layer = 0; }
3619  else if (msg.header.seq == -250)
3620  { layer = 1; }
3621  else if (msg.header.seq == -500)
3622  { layer = 2; }
3623  elevationAngleDegree = this->parser_->getCurrentParamPtr()->getElevationDegreeResolution();
3624  elevationAngleDegree = elevationAngleDegree / 180.0 * M_PI;
3625  // 0.0436332 /*2.5 degrees*/;
3626  break;
3627  case 24: // Preparation for MRS6000
3628  baseLayer = -1;
3629  layer = (msg.header.seq - (-2638)) / 125;
3630  layer = (23 - layer) - 1;
3631 #if 0
3632  elevationAngleDegree = this->parser_->getCurrentParamPtr()->getElevationDegreeResolution();
3633  elevationAngleDegree = elevationAngleDegree / 180.0 * M_PI;
3634 #endif
3635 
3636  elevationPreCalculated = true;
3637  if (vang_vec.size() > 0)
3638  {
3639  useGivenElevationAngle = true;
3640  }
3641  break;
3642  default:
3643  assert(0);
3644  break; // unsupported
3645 
3646  }
3647 
3648 
3649 
3650 
3651 
3652  // XXX - HIER MEHRERE SCANS publish, falls Mehrzielszenario läuft
3653  if (numEchos > 5)
3654  {
3655  ROS_WARN("Too much echos");
3656  }
3657  else
3658  {
3659 
3660  size_t startOffset = 0;
3661  size_t endOffset = 0;
3662  int echoPartNum = msg.ranges.size() / numEchos;
3663  for (int i = 0; i < numEchos; i++)
3664  {
3665 
3666  bool sendMsg = false;
3667  if ((echoMask & (1 << i)) & outputChannelFlagId)
3668  {
3669  aiValidEchoIdx[numValidEchos] = i; // save index
3670  numValidEchos++;
3671  sendMsg = true;
3672  }
3673  startOffset = i * echoPartNum;
3674  endOffset = (i + 1) * echoPartNum;
3675 
3676  msg.ranges.clear();
3677  msg.intensities.clear();
3678  msg.ranges = std::vector<float>(
3679  rangeTmp.begin() + startOffset,
3680  rangeTmp.begin() + endOffset);
3681  // check also for MRS1104
3682  if (endOffset <= intensityTmp.size() && (intensityTmp.size() > 0))
3683  {
3684  msg.intensities = std::vector<float>(
3685  intensityTmp.begin() + startOffset,
3686  intensityTmp.begin() + endOffset);
3687  }
3688  else
3689  {
3690  msg.intensities.resize(echoPartNum); // fill with zeros
3691  }
3692  {
3693  // numEchos
3694  char szTmp[255] = {0};
3695  if (this->parser_->getCurrentParamPtr()->getNumberOfLayers() > 1)
3696  {
3697  const char *cpFrameId = config_.frame_id.c_str();
3698 #if 0
3699  sprintf(szTmp, "%s_%+04d_DIST%d", cpFrameId, msg.header.seq, i + 1);
3700 #else // experimental
3701  char szSignName[10] = {0};
3702  if ((int) msg.header.seq < 0)
3703  {
3704  strcpy(szSignName, "NEG");
3705  }
3706  else
3707  {
3708  strcpy(szSignName, "POS");
3709 
3710  }
3711 
3712  sprintf(szTmp, "%s_%s_%03d_DIST%d", cpFrameId, szSignName, abs((int) msg.header.seq), i + 1);
3713 #endif
3714  }
3715  else
3716  {
3717  strcpy(szTmp, config_.frame_id.c_str());
3718  }
3719 
3720  msg.header.frame_id = std::string(szTmp);
3721  // Hector slam can only process ONE valid frame id.
3722  if (echoForSlam.length() > 0)
3723  {
3724  if (slamBundle)
3725  {
3726  // try to map first echos to horizontal layers.
3727  if (i == 0)
3728  {
3729  // first echo
3730  msg.header.frame_id = echoForSlam;
3731  strcpy(szTmp, echoForSlam.c_str()); //
3732  if (elevationAngleInRad != 0.0)
3733  {
3734  float cosVal = cos(elevationAngleInRad);
3735  int rangeNum = msg.ranges.size();
3736  for (int j = 0; j < rangeNum; j++)
3737  {
3738  msg.ranges[j] *= cosVal;
3739  }
3740  }
3741  }
3742  }
3743 
3744  if (echoForSlam.compare(szTmp) == 0)
3745  {
3746  sendMsg = true;
3747  }
3748  else
3749  {
3750  sendMsg = false;
3751  }
3752  }
3753  // If msg.intensities[j] < min_intensity, then set msg.ranges[j] to inf according to https://github.com/SICKAG/sick_scan/issues/131
3754  if(m_min_intensity > 0) // Set range of LaserScan messages to infinity, if intensity < min_intensity (default: 0)
3755  {
3756  for (int j = 0, j_max = (int)std::min(msg.ranges.size(), msg.intensities.size()); j < j_max; j++)
3757  {
3758  if(msg.intensities[j] < m_min_intensity)
3759  {
3760  msg.ranges[j] = std::numeric_limits<float>::infinity();
3761  // ROS_DEBUG_STREAM("msg.intensities[" << j << "]=" << msg.intensities[j] << " < " << m_min_intensity << ", set msg.ranges[" << j << "]=" << msg.ranges[j] << " to infinity.");
3762  }
3763  }
3764  }
3765 
3766  }
3767 #ifndef _MSC_VER
3768  if (parser_->getCurrentParamPtr()->getEncoderMode() >= 0 && FireEncoder == true)//
3769  {
3770  Encoder_pub.publish(EncoderMsg);
3771  }
3772  if (numOfLayers > 4)
3773  {
3774  sendMsg = false; // too many layers for publish as scan message. Only pointcloud messages will be pub.
3775  }
3776  if (sendMsg &
3777  outputChannelFlagId) // publish only configured channels - workaround for cfg-bug MRS1104
3778  {
3779 
3781 
3782  }
3783 #else
3784  printf("MSG received...");
3785 #endif
3786  }
3787  }
3788 
3789 
3790  if (publishPointCloud == true)
3791  {
3792 
3793 
3794  const int numChannels = 4; // x y z i (for intensity)
3795 
3796  int numTmpLayer = numOfLayers;
3797 
3798 
3799  cloud_.header.stamp = recvTimeStamp + ros::Duration(config_.time_offset);
3800 
3801 
3802  cloud_.header.frame_id = config_.frame_id;
3803  cloud_.header.seq = 0;
3804  cloud_.height = numTmpLayer * numValidEchos; // due to multi echo multiplied by num. of layers
3805  cloud_.width = msg.ranges.size();
3806  cloud_.is_bigendian = false;
3807  cloud_.is_dense = true;
3808  cloud_.point_step = numChannels * sizeof(float);
3809  cloud_.row_step = cloud_.point_step * cloud_.width;
3810  cloud_.fields.resize(numChannels);
3811  for (int i = 0; i < numChannels; i++)
3812  {
3813  std::string channelId[] = {"x", "y", "z", "intensity"};
3814  cloud_.fields[i].name = channelId[i];
3815  cloud_.fields[i].offset = i * sizeof(float);
3816  cloud_.fields[i].count = 1;
3817  cloud_.fields[i].datatype = sensor_msgs::PointField::FLOAT32;
3818  }
3819 
3820  cloud_.data.resize(cloud_.row_step * cloud_.height);
3821 
3822  unsigned char *cloudDataPtr = &(cloud_.data[0]);
3823 
3824 
3825  // prepare lookup for elevation angle table
3826 
3827  std::vector<float> cosAlphaTable; // Lookup table for cos
3828  std::vector<float> sinAlphaTable; // Lookup table for sin
3829  size_t rangeNum = rangeTmp.size() / numValidEchos;
3830  cosAlphaTable.resize(rangeNum);
3831  sinAlphaTable.resize(rangeNum);
3832  float mirror_factor = 1.0;
3833  float angleShift=0;
3835  {
3836 // mirror_factor = -1.0;
3837 // angleShift = +M_PI/2.0; // add 90 deg for NAV3xx-series
3838  }
3839 
3840  for (size_t iEcho = 0; iEcho < numValidEchos; iEcho++)
3841  {
3842 
3843  float angle = config_.min_ang;
3844 
3845 
3846  float *cosAlphaTablePtr = &cosAlphaTable[0];
3847  float *sinAlphaTablePtr = &sinAlphaTable[0];
3848 
3849  float *vangPtr = NULL;
3850  float *rangeTmpPtr = &rangeTmp[0];
3851  if (vang_vec.size() > 0)
3852  {
3853  vangPtr = &vang_vec[0];
3854  }
3855  for (size_t i = 0; i < rangeNum; i++)
3856  {
3857  enum enum_index_descr
3858  {
3859  idx_x,
3860  idx_y,
3861  idx_z,
3862  idx_intensity,
3863  idx_num
3864  };
3865  long adroff = i * (numChannels * (int) sizeof(float));
3866 
3867  adroff += (layer - baseLayer) * cloud_.row_step;
3868 
3869  adroff += iEcho * cloud_.row_step * numTmpLayer;
3870 
3871  unsigned char *ptr = cloudDataPtr + adroff;
3872  float *fptr = (float *) (cloudDataPtr + adroff);
3873 
3874  geometry_msgs::Point32 point;
3875  float range_meter = rangeTmpPtr[iEcho * rangeNum + i];
3876  float phi = angle; // azimuth angle
3877  float alpha = 0.0; // elevation angle
3878 
3879  if (useGivenElevationAngle) // FOR MRS6124
3880  {
3881  alpha = -vangPtr[i] * deg2rad_const;
3882  }
3883  else
3884  {
3885  if (elevationPreCalculated) // FOR MRS6124 without VANGL
3886  {
3887  alpha = elevationAngleInRad;
3888  }
3889  else
3890  {
3891  alpha = layer * elevationAngleDegree; // for MRS1104
3892  }
3893  }
3894 
3895  if (iEcho == 0)
3896  {
3897  cosAlphaTablePtr[i] = cos(alpha); // for z-value (elevation)
3898  sinAlphaTablePtr[i] = sin(alpha);
3899  }
3900  else
3901  {
3902  // Just for Debugging: printf("%3d %8.3lf %8.3lf\n", (int)i, cosAlphaTablePtr[i], sinAlphaTablePtr[i]);
3903  }
3904  // Thanks to Sebastian Pütz <spuetz@uos.de> for his hint
3905  float rangeCos = range_meter * cosAlphaTablePtr[i];
3906 
3907  double phi_used = phi + angleShift;
3908  if (this->angleCompensator != NULL)
3909  {
3910  phi_used = angleCompensator->compensateAngleInRadFromRos(phi_used);
3911  }
3912  fptr[idx_x] = rangeCos * cos(phi_used); // copy x value in pointcloud
3913  fptr[idx_y] = rangeCos * sin(phi_used); // copy y value in pointcloud
3914  fptr[idx_z] = range_meter * sinAlphaTablePtr[i] * mirror_factor;// copy z value in pointcloud
3915 
3916  fptr[idx_intensity] = 0.0;
3917  if (config_.intensity)
3918  {
3919  int intensityIndex = aiValidEchoIdx[iEcho] * rangeNum + i;
3920  // intensity values available??
3921  if (intensityIndex < intensityTmpNum)
3922  {
3923  fptr[idx_intensity] = intensityTmpPtr[intensityIndex]; // copy intensity value in pointcloud
3924  }
3925  }
3926  angle += msg.angle_increment;
3927  }
3928  // Publish
3929  static int cnt = 0;
3930  int layerOff = (layer - baseLayer);
3931 
3932  }
3933  // if ( (msg.header.seq == 0) || (layerOff == 0)) // FIXEN!!!!
3934  bool shallIFire = false;
3935  if ((msg.header.seq == 0) || (msg.header.seq == 237))
3936  {
3937  shallIFire = true;
3938  }
3939 
3940 
3941  static int layerCnt = 0;
3942  static int layerSeq[4];
3943 
3944  if (config_.cloud_output_mode > 0)
3945  {
3946 
3947  layerSeq[layerCnt % 4] = layer;
3948  if (layerCnt >= 4) // mind. erst einmal vier Layer zusammensuchen
3949  {
3950  shallIFire = true; // here are at least 4 layers available
3951  }
3952  else
3953  {
3954  shallIFire = false;
3955  }
3956 
3957  layerCnt++;
3958  }
3959 
3960  if (shallIFire) // shall i fire the signal???
3961  {
3962  if (config_.cloud_output_mode == 0)
3963  {
3964  // standard handling of scans
3966 
3967  }
3968  else if (config_.cloud_output_mode == 2)
3969  {
3970  // Following cases are interesting:
3971  // LMS5xx: seq is always 0 -> publish every scan
3972  // MRS1104: Every 4th-Layer is 0 -> publish pointcloud every 4th layer message
3973  // LMS1104: Publish every layer. The timing for the LMS1104 seems to be:
3974  // Every 67 ms receiving of a new scan message
3975  // Scan message contains 367 measurements
3976  // angle increment is 0.75° (yields 274,5° covery -> OK)
3977  // MRS6124: Publish very 24th layer at the layer = 237 , MRS6124 contains no sequence with seq 0
3978  //BBB
3979 #ifndef _MSC_VER
3980  int numTotalShots = 360; //TODO where is this from ?
3981  int numPartialShots = 40; //
3982 
3983  for (int i = 0; i < numTotalShots; i += numPartialShots)
3984  {
3985  sensor_msgs::PointCloud2 partialCloud;
3986  partialCloud = cloud_;
3987  ros::Time partialTimeStamp = cloud_.header.stamp;
3988 
3989  partialTimeStamp += ros::Duration((i + 0.5 * (numPartialShots - 1)) * timeIncrement);
3990  partialTimeStamp += ros::Duration((3 * numTotalShots) * timeIncrement);
3991  partialCloud.header.stamp = partialTimeStamp;
3992  partialCloud.width = numPartialShots * 3; // die sind sicher in diesem Bereich
3993 
3994  int diffTo1100 = cloud_.width - 3 * (numTotalShots + i);
3995  if (diffTo1100 > numPartialShots)
3996  {
3997  diffTo1100 = numPartialShots;
3998  }
3999  if (diffTo1100 < 0)
4000  {
4001  diffTo1100 = 0;
4002  }
4003  partialCloud.width += diffTo1100;
4004  // printf("Offset: %4d Breite: %4d\n", i, partialCloud.width);
4005  partialCloud.height = 1;
4006 
4007 
4008  partialCloud.is_bigendian = false;
4009  partialCloud.is_dense = true;
4010  partialCloud.point_step = numChannels * sizeof(float);
4011  partialCloud.row_step = partialCloud.point_step * partialCloud.width;
4012  partialCloud.fields.resize(numChannels);
4013  for (int ii = 0; ii < numChannels; ii++)
4014  {
4015  std::string channelId[] = {"x", "y", "z", "intensity"};
4016  partialCloud.fields[ii].name = channelId[ii];
4017  partialCloud.fields[ii].offset = ii * sizeof(float);
4018  partialCloud.fields[ii].count = 1;
4019  partialCloud.fields[ii].datatype = sensor_msgs::PointField::FLOAT32;
4020  }
4021 
4022  partialCloud.data.resize(partialCloud.row_step);
4023 
4024  int partOff = 0;
4025  for (int j = 0; j < 4; j++)
4026  {
4027  int layerIdx = (j + (layerCnt)) % 4; // j = 0 -> oldest
4028  int rowIdx = 1 + layerSeq[layerIdx % 4]; // +1, da es bei -1 beginnt
4029  int colIdx = j * numTotalShots + i;
4030  int maxAvail = cloud_.width - colIdx; //
4031  if (maxAvail < 0)
4032  {
4033  maxAvail = 0;
4034  }
4035 
4036  if (maxAvail > numPartialShots)
4037  {
4038  maxAvail = numPartialShots;
4039  }
4040 
4041  // printf("Most recent LayerIdx: %2d RowIdx: %4d ColIdx: %4d\n", layer, rowIdx, colIdx);
4042  if (maxAvail > 0)
4043  {
4044  memcpy(&(partialCloud.data[partOff]),
4045  &(cloud_.data[(rowIdx * cloud_.width + colIdx + i) * cloud_.point_step]),
4046  cloud_.point_step * maxAvail);
4047 
4048  }
4049 
4050  partOff += maxAvail * partialCloud.point_step;
4051  }
4052  assert(partialCloud.data.size() == partialCloud.width * partialCloud.point_step);
4053 
4054 
4055  cloud_pub_.publish(partialCloud);
4056 #if 0
4057  memcpy(&(partialCloud.data[0]), &(cloud_.data[0]) + i * cloud_.point_step, cloud_.point_step * numPartialShots);
4058  cloud_pub_.publish(partialCloud);
4059 #endif
4060  }
4061  }
4062  // cloud_pub_.publish(cloud_);
4063 
4064 #else
4065  printf("PUBLISH:\n");
4066 #endif
4067  }
4068  }
4069  }
4070  // Start Point
4071  if (dend != NULL)
4072  {
4073  buffer_pos = dend + 1;
4074  }
4075  } // end of while loop
4076  }
4077 
4078  // shall we process more data? I.e. are there more packets to process in the input queue???
4079 
4080  } while ((packetsInLoop > 0) && (numPacketsProcessed < maxNumAllowedPacketsToProcess));
4081  return ExitSuccess; // return success to continue looping
4082  }
4083 
4084 
4088  void SickScanCommon::check_angle_range(SickScanConfig &conf)
4089  {
4090  if (conf.min_ang > conf.max_ang)
4091  {
4092  ROS_WARN("Maximum angle must be greater than minimum angle. Adjusting >min_ang<.");
4093  conf.min_ang = conf.max_ang;
4094  }
4095  }
4096 
4102  void SickScanCommon::update_config(sick_scan::SickScanConfig &new_config, uint32_t level)
4103  {
4104  check_angle_range(new_config);
4105  config_ = new_config;
4106  }
4107 
4114  int SickScanCommon::convertAscii2BinaryCmd(const char *requestAscii, std::vector<unsigned char> *requestBinary)
4115  {
4116  requestBinary->clear();
4117  if (requestAscii == NULL)
4118  {
4119  return (-1);
4120  }
4121  int cmdLen = strlen(requestAscii);
4122  if (cmdLen == 0)
4123  {
4124  return (-1);
4125  }
4126  if (requestAscii[0] != 0x02)
4127  {
4128  return (-1);
4129  }
4130  if (requestAscii[cmdLen - 1] != 0x03)
4131  {
4132  return (-1);
4133  }
4134  // Here we know, that the ascii format is correctly framed with <stx> .. <etx>
4135  for (int i = 0; i < 4; i++)
4136  {
4137  requestBinary->push_back(0x02);
4138  }
4139 
4140  for (int i = 0; i < 4; i++) // Puffer for Length identifier
4141  {
4142  requestBinary->push_back(0x00);
4143  }
4144 
4145  unsigned long msgLen = cmdLen - 2; // without stx and etx
4146 
4147  // special handling for the following commands
4148  // due to higher number of command arguments
4149  std::string keyWord0 = "sMN SetAccessMode";
4150  std::string keyWord1 = "sWN FREchoFilter";
4151  std::string keyWord2 = "sEN LMDscandata";
4152  std::string keyWord3 = "sWN LMDscandatacfg";
4153  std::string keyWord4 = "sWN SetActiveApplications";
4154  std::string keyWord5 = "sEN IMUData";
4155  std::string keyWord6 = "sWN EIIpAddr";
4156  std::string keyWord7 = "sMN mLMPsetscancfg";
4157  std::string keyWord8 = "sWN TSCTCupdatetime";
4158  std::string keyWord9 = "sWN TSCTCSrvAddr";
4159  std::string keyWord10 = "sWN LICencres";
4160  std::string keyWord11 = "sWN LFPmeanfilter";
4161  std::string KeyWord12 = "sRN field";
4162 
4163  //BBB
4164 
4165  std::string cmdAscii = requestAscii;
4166 
4167 
4168  int copyUntilSpaceCnt = 2;
4169  int spaceCnt = 0;
4170  char hexStr[255] = {0};
4171  int level = 0;
4172  unsigned char buffer[255];
4173  int bufferLen = 0;
4174  if (cmdAscii.find(keyWord0) != std::string::npos) // SetAccessMode
4175  {
4176  copyUntilSpaceCnt = 2;
4177  int keyWord0Len = keyWord0.length();
4178  sscanf(requestAscii + keyWord0Len + 1, " %d %s", &level, hexStr);
4179  buffer[0] = (unsigned char) (0xFF & level);
4180  bufferLen = 1;
4181  char hexTmp[3] = {0};
4182  for (int i = 0; i < 4; i++)
4183  {
4184  int val;
4185  hexTmp[0] = hexStr[i * 2];
4186  hexTmp[1] = hexStr[i * 2 + 1];
4187  hexTmp[2] = 0x00;
4188  sscanf(hexTmp, "%x", &val);
4189  buffer[i + 1] = (unsigned char) (0xFF & val);
4190  bufferLen++;
4191  }
4192  }
4193  if (cmdAscii.find(keyWord1) != std::string::npos)
4194  {
4195  int echoCodeNumber = 0;
4196  int keyWord1Len = keyWord1.length();
4197  sscanf(requestAscii + keyWord1Len + 1, " %d", &echoCodeNumber);
4198  buffer[0] = (unsigned char) (0xFF & echoCodeNumber);
4199  bufferLen = 1;
4200  }
4201  if (cmdAscii.find(keyWord2) != std::string::npos)
4202  {
4203  int scanDataStatus = 0;
4204  int keyWord2Len = keyWord2.length();
4205  sscanf(requestAscii + keyWord2Len + 1, " %d", &scanDataStatus);
4206  buffer[0] = (unsigned char) (0xFF & scanDataStatus);
4207  bufferLen = 1;
4208  }
4209 
4210  if (cmdAscii.find(keyWord3) != std::string::npos)
4211  {
4212  int scanDataStatus = 0;
4213  int keyWord3Len = keyWord3.length();
4214  int dummyArr[12] = {0};
4215  //sWN LMDscandatacfg %02d 00 %d %d 0 0 %02d 0 0 0 1 1\x03"
4216  int sscanfresult = sscanf(requestAscii + keyWord3Len + 1, " %d %d %d %d %d %d %d %d %d %d %d %d",
4217  &dummyArr[0], // Data Channel Idx LSB
4218  &dummyArr[1], // Data Channel Idx MSB
4219  &dummyArr[2], // Remission
4220  &dummyArr[3], // Remission data format
4221  &dummyArr[4], // Unit
4222  &dummyArr[5], // Encoder Setting LSB
4223  &dummyArr[6], // Encoder Setting MSB
4224  &dummyArr[7], // Position
4225  &dummyArr[8], // Send Name
4226  &dummyArr[9], // Send Comment
4227  &dummyArr[10], // Time information
4228  &dummyArr[11]); // n-th Scan (packed - not sent as single byte sequence) !!!
4229  if (1 < sscanfresult)
4230  {
4231 
4232  for (int i = 0; i < 13; i++)
4233  {
4234  buffer[i] = 0x00;
4235  }
4236  buffer[0] = (unsigned char) (0xFF & dummyArr[0]); //Data Channel 2 Bytes
4237  buffer[1] = (unsigned char) (0xFF & dummyArr[1]);; // MSB of Data Channel (here Little Endian!!)
4238  buffer[2] = (unsigned char) (0xFF & dummyArr[2]); // Remission
4239  buffer[3] = (unsigned char) (0xFF & dummyArr[3]); // Remission data format 0=8 bit 1= 16 bit
4240  buffer[4] = (unsigned char) (0xFF & dummyArr[4]); //Unit of remission data
4241  buffer[5] = (unsigned char) (0xFF & dummyArr[5]); //encoder Data LSB
4242  buffer[6] = (unsigned char) (0xFF & dummyArr[6]); //encoder Data MSB
4243  buffer[7] = (unsigned char) (0xFF & dummyArr[7]); // Position
4244  buffer[8] = (unsigned char) (0xFF & dummyArr[8]); // Send Scanner Name
4245  buffer[9] = (unsigned char) (0xFF & dummyArr[9]); // Comment
4246  buffer[10] = (unsigned char) (0xFF & dummyArr[10]); // Time information
4247  buffer[11] = (unsigned char) (0xFF & (dummyArr[11] >> 8)); // BIG Endian High Byte nth-Scan
4248  buffer[12] = (unsigned char) (0xFF & (dummyArr[11] >> 0)); // BIG Endian Low Byte nth-Scan
4249  bufferLen = 13;
4250 
4251  }
4252 
4253  }
4254 
4255  if (cmdAscii.find(keyWord4) != std::string::npos)
4256  {
4257  char tmpStr[1024] = {0};
4258  char szApplStr[255] = {0};
4259  int keyWord4Len = keyWord4.length();
4260  int scanDataStatus = 0;
4261  int dummy0, dummy1;
4262  strcpy(tmpStr, requestAscii + keyWord4Len + 2);
4263  sscanf(tmpStr, "%d %s %d", &dummy0, szApplStr, &dummy1);
4264  // rebuild string
4265  buffer[0] = 0x00;
4266  buffer[1] = dummy0 ? 0x01 : 0x00;
4267  for (int ii = 0; ii < 4; ii++)
4268  {
4269  buffer[2 + ii] = szApplStr[ii]; // idx: 1,2,3,4
4270  }
4271  buffer[6] = dummy1 ? 0x01 : 0x00;
4272  bufferLen = 7;
4273  }
4274 
4275  if (cmdAscii.find(keyWord5) != std::string::npos)
4276  {
4277  int imuSetStatus = 0;
4278  int keyWord5Len = keyWord5.length();
4279  sscanf(requestAscii + keyWord5Len + 1, " %d", &imuSetStatus);
4280  buffer[0] = (unsigned char) (0xFF & imuSetStatus);
4281  bufferLen = 1;
4282  }
4283 
4284  if (cmdAscii.find(keyWord6) != std::string::npos)
4285  {
4286  int adrPartArr[4];
4287  int imuSetStatus = 0;
4288  int keyWord6Len = keyWord6.length();
4289  sscanf(requestAscii + keyWord6Len + 1, " %x %x %x %x", &(adrPartArr[0]), &(adrPartArr[1]), &(adrPartArr[2]),
4290  &(adrPartArr[3]));
4291  buffer[0] = (unsigned char) (0xFF & adrPartArr[0]);
4292  buffer[1] = (unsigned char) (0xFF & adrPartArr[1]);
4293  buffer[2] = (unsigned char) (0xFF & adrPartArr[2]);
4294  buffer[3] = (unsigned char) (0xFF & adrPartArr[3]);
4295  bufferLen = 4;
4296  }
4297  //\x02sMN mLMPsetscancfg %d 1 %d 0 0\x03";
4298  //02 02 02 02 00 00 00 25 73 4D 4E 20 6D 4C 4D 50 73 65 74 73 63 61 6E 63 66 67 20
4299  // 00 00 13 88 4byte freq
4300  // 00 01 2 byte sectors always 1
4301  // 00 00 13 88 ang_res
4302  // FF F9 22 30 sector start always 0
4303  // 00 22 55 10 sector stop always 0
4304  // 21
4305  if (cmdAscii.find(keyWord7) != std::string::npos)
4306  {
4308  {
4309  {
4310  bufferLen = 18;
4311  for (int i = 0; i < bufferLen; i++)
4312  {
4313  unsigned char uch = 0x00;
4314  switch (i)
4315  {
4316  case 5:
4317  uch = 0x01;
4318  break;
4319  }
4320  buffer[i] = uch;
4321  }
4322  char tmpStr[1024] = {0};
4323  char szApplStr[255] = {0};
4324  int keyWord7Len = keyWord7.length();
4325  int scanDataStatus = 0;
4326  int dummy0, dummy1;
4327  strcpy(tmpStr, requestAscii + keyWord7Len + 2);
4328  sscanf(tmpStr, "%d 1 %d", &dummy0, &dummy1);
4329 
4330  buffer[0] = (unsigned char) (0xFF & (dummy0 >> 24));
4331  buffer[1] = (unsigned char) (0xFF & (dummy0 >> 16));
4332  buffer[2] = (unsigned char) (0xFF & (dummy0 >> 8));
4333  buffer[3] = (unsigned char) (0xFF & (dummy0 >> 0));
4334 
4335 
4336  buffer[6] = (unsigned char) (0xFF & (dummy1 >> 24));
4337  buffer[7] = (unsigned char) (0xFF & (dummy1 >> 16));
4338  buffer[8] = (unsigned char) (0xFF & (dummy1 >> 8));
4339  buffer[9] = (unsigned char) (0xFF & (dummy1 >> 0));
4340  }
4341  }
4342  else
4343  {
4344  int keyWord7Len = keyWord7.length();
4345  int dummyArr[14] = {0};
4346  if (14 == sscanf(requestAscii + keyWord7Len + 1, " %d %d %d %d %d %d %d %d %d %d %d %d %d %d",
4347  &dummyArr[0], &dummyArr[1], &dummyArr[2],
4348  &dummyArr[3], &dummyArr[4], &dummyArr[5],
4349  &dummyArr[6], &dummyArr[7], &dummyArr[8],
4350  &dummyArr[9], &dummyArr[10], &dummyArr[11], &dummyArr[12], &dummyArr[13]))
4351  {
4352  for (int i = 0; i < 54; i++)
4353  {
4354  buffer[i] = 0x00;
4355  }
4356  int targetPosArr[] = {0, 4, 6, 10, 14, 18, 22, 26, 30, 34, 38, 42, 46, 50, 54};
4357  int numElem = (sizeof(targetPosArr) / sizeof(targetPosArr[0])) - 1;
4358  for (int i = 0; i < numElem; i++)
4359  {
4360  int lenOfBytesToRead = targetPosArr[i + 1] - targetPosArr[i];
4361  int adrPos = targetPosArr[i];
4362  unsigned char *destPtr = buffer + adrPos;
4363  memcpy(destPtr, &(dummyArr[i]), lenOfBytesToRead);
4364  swap_endian(destPtr, lenOfBytesToRead);
4365  }
4366  bufferLen = targetPosArr[numElem];
4367  /*
4368  * 00 00 03 20 00 01
4369  00 00 09 C4 00 00 00 00 00 36 EE 80 00 00 09 C4 00 00 00 00 00 00 00 00 00 00 09 C4 00 00 00 00 00
4370  00 00 00 00 00 09 C4 00 00 00 00 00 00 00 00 00 00 09 C4 00 00 00 00 00 00 00 00 E4
4371  */
4372 
4373  }
4374  }
4375 
4376  }
4377  if (cmdAscii.find(keyWord8) != std::string::npos)
4378  {
4379  uint32_t updatetime = 0;
4380  int keyWord8Len = keyWord8.length();
4381  sscanf(requestAscii + keyWord8Len + 1, " %d", &updatetime);
4382  buffer[0] = (unsigned char) (0xFF & (updatetime >> 24));
4383  buffer[1] = (unsigned char) (0xFF & (updatetime >> 16));
4384  buffer[2] = (unsigned char) (0xFF & (updatetime >> 8));
4385  buffer[3] = (unsigned char) (0xFF & (updatetime >> 0));
4386  bufferLen = 4;
4387  }
4388  if (cmdAscii.find(keyWord9) != std::string::npos)
4389  {
4390  int adrPartArr[4];
4391  int imuSetStatus = 0;
4392  int keyWord9Len = keyWord9.length();
4393  sscanf(requestAscii + keyWord9Len + 1, " %x %x %x %x", &(adrPartArr[0]), &(adrPartArr[1]), &(adrPartArr[2]),
4394  &(adrPartArr[3]));
4395  buffer[0] = (unsigned char) (0xFF & adrPartArr[0]);
4396  buffer[1] = (unsigned char) (0xFF & adrPartArr[1]);
4397  buffer[2] = (unsigned char) (0xFF & adrPartArr[2]);
4398  buffer[3] = (unsigned char) (0xFF & adrPartArr[3]);
4399  bufferLen = 4;
4400  }
4401  if (cmdAscii.find(keyWord10) != std::string::npos)
4402  {
4403  float EncResolution = 0;
4404  bufferLen = 4;
4405  int keyWord10Len = keyWord10.length();
4406  sscanf(requestAscii + keyWord10Len + 1, " %f", &EncResolution);
4407  memcpy(buffer, &EncResolution, bufferLen);
4408  swap_endian(buffer, bufferLen);
4409 
4410  }
4411  if (cmdAscii.find(keyWord11) != std::string::npos)
4412  {
4413  char tmpStr[1024] = {0};
4414  char szApplStr[255] = {0};
4415  int keyWord11Len = keyWord11.length();
4416  int dummy0, dummy1,dummy2;
4417  strcpy(tmpStr, requestAscii + keyWord11Len + 2);
4418  sscanf(tmpStr, "%d %d %d", &dummy0, &dummy1, &dummy2);
4419  // rebuild string
4420  buffer[0] = dummy0 ? 0x01 : 0x00;
4421  buffer[1] =dummy1/256;//
4422  buffer[2] =dummy1%256;//
4423  buffer[3] =dummy2;
4424  bufferLen = 4;
4425  }
4426  if (cmdAscii.find(KeyWord12) != std::string::npos)
4427  {
4428  uint32_t fieldID = 0;
4429  int keyWord12Len = KeyWord12.length();
4430  sscanf(requestAscii + keyWord12Len + 1, "%d", &fieldID);
4431  bufferLen = 0;
4432  }
4433  // copy base command string to buffer
4434  bool switchDoBinaryData = false;
4435  for (int i = 1; i <= (int) (msgLen); i++) // STX DATA ETX --> 0 1 2
4436  {
4437  char c = requestAscii[i];
4438  if (switchDoBinaryData == true)
4439  {
4440  if (0 == bufferLen) // no keyword handling before this point
4441  {
4442  if (c >= '0' && c <= '9') // standard data format expected - only one digit
4443  {
4444  c -= '0'; // convert ASCII-digit to hex-digit
4445  } // 48dez to 00dez and so on.
4446  }
4447  else
4448  {
4449  break;
4450  }
4451  }
4452  requestBinary->push_back(c);
4453  if (requestAscii[i] == 0x20) // space
4454  {
4455  spaceCnt++;
4456  if (spaceCnt >= copyUntilSpaceCnt)
4457  {
4458  switchDoBinaryData = true;
4459  }
4460  }
4461 
4462  }
4463  // if there are already process bytes (due to special key word handling)
4464  // copy these data bytes to the buffer
4465  for (int i = 0; i < bufferLen; i++) // append variable data
4466  {
4467  requestBinary->push_back(buffer[i]);
4468  }
4469 
4470  msgLen = requestBinary->size();
4471  msgLen -= 8;
4472  for (int i = 0; i < 4; i++)
4473  {
4474  unsigned char bigEndianLen = msgLen & (0xFF << (3 - i) * 8);
4475  (*requestBinary)[i + 4] = ((unsigned char) (bigEndianLen)); // HIER WEITERMACHEN!!!!
4476  }
4477  unsigned char xorVal = 0x00;
4478  xorVal = sick_crc8((unsigned char *) (&((*requestBinary)[8])), requestBinary->size() - 8);
4479  requestBinary->push_back(xorVal);
4480 #if 0
4481  for (int i = 0; i < requestBinary->size(); i++)
4482  {
4483  unsigned char c = (*requestBinary)[i];
4484  printf("[%c]%02x ", (c < ' ') ? '.' : c, c) ;
4485  }
4486  printf("\n");
4487 #endif
4488  return (0);
4489 
4490  };
4491 
4492 
4500  {
4501  bool retValue = true;
4502  bool shouldUseBinary = this->parser_->getCurrentParamPtr()->getUseBinaryProtocol();
4503  if (shouldUseBinary == useBinaryCmdNow)
4504  {
4505  retValue = true; // !!!! CHANGE ONLY FOR TEST!!!!!
4506  }
4507  else
4508  {
4509  /*
4510  // we must reconnect and set the new protocoltype
4511  int iRet = this->close_device();
4512  ROS_INFO("SOPAS - Close and reconnected to scanner due to protocol change and wait 15 sec. ");
4513  ROS_INFO("SOPAS - Changing from %s to %s\n", shouldUseBinary ? "ASCII" : "BINARY", shouldUseBinary ? "BINARY" : "ASCII");
4514  // Wait a few seconds after rebooting
4515  ros::Duration(15.0).sleep();
4516 
4517  iRet = this->init_device();
4518  */
4519  if (shouldUseBinary == true)
4520  {
4521  this->setProtocolType(CoLa_B);
4522  }
4523  else
4524  {
4525  this->setProtocolType(CoLa_A);
4526  }
4527 
4528  useBinaryCmdNow = shouldUseBinary;
4529  retValue = false;
4530  }
4531  return (retValue);
4532  }
4533 
4534 
4535  bool SickScanCommon::setNewIpAddress(boost::asio::ip::address_v4 ipNewIPAddr, bool useBinaryCmd)
4536  {
4537  int eepwritetTimeOut = 1;
4538  bool result = false;
4539 
4540 
4541  unsigned long adrBytesLong[4];
4542  std::string s = ipNewIPAddr.to_string(); // convert to string, to_bytes not applicable for older linux version
4543  const char *ptr = s.c_str(); // char * to address
4544  // decompose pattern like aaa.bbb.ccc.ddd
4545  sscanf(ptr, "%lu.%lu.%lu.%lu", &(adrBytesLong[0]), &(adrBytesLong[1]), &(adrBytesLong[2]), &(adrBytesLong[3]));
4546 
4547  // convert into byte array
4548  unsigned char ipbytearray[4];
4549  for (int i = 0; i < 4; i++)
4550  {
4551  ipbytearray[i] = adrBytesLong[i] & 0xFF;
4552  }
4553 
4554 
4555  char ipcommand[255];
4556  const char *pcCmdMask = sopasCmdMaskVec[CMD_SET_IP_ADDR].c_str();
4557  sprintf(ipcommand, pcCmdMask, ipbytearray[0], ipbytearray[1], ipbytearray[2], ipbytearray[3]);
4558  if (useBinaryCmd)
4559  {
4560  std::vector<unsigned char> reqBinary;
4561  this->convertAscii2BinaryCmd(ipcommand, &reqBinary);
4562  result = (0 == sendSopasAndCheckAnswer(reqBinary, &sopasReplyBinVec[CMD_SET_IP_ADDR]));
4563  reqBinary.clear();
4564  this->convertAscii2BinaryCmd(sopasCmdVec[CMD_WRITE_EEPROM].c_str(), &reqBinary);
4565  result &= (0 == sendSopasAndCheckAnswer(reqBinary, &sopasReplyBinVec[CMD_WRITE_EEPROM]));
4566  reqBinary.clear();
4567  this->convertAscii2BinaryCmd(sopasCmdVec[CMD_RUN].c_str(), &reqBinary);
4568  result &= (0 == sendSopasAndCheckAnswer(reqBinary, &sopasReplyBinVec[CMD_RUN]));
4569  reqBinary.clear();
4570  this->convertAscii2BinaryCmd(sopasCmdVec[CMD_SET_ACCESS_MODE_3].c_str(), &reqBinary);
4571  result &= (0 == sendSopasAndCheckAnswer(reqBinary, &sopasReplyBinVec[CMD_SET_ACCESS_MODE_3]));
4572  reqBinary.clear();
4573  this->convertAscii2BinaryCmd(sopasCmdVec[CMD_REBOOT].c_str(), &reqBinary);
4574  result &= (0 == sendSopasAndCheckAnswer(reqBinary, &sopasReplyBinVec[CMD_REBOOT]));
4575  }
4576  else
4577  {
4578  std::vector<unsigned char> ipcomandReply;
4579  std::vector<unsigned char> resetReply;
4580  std::string runCmd = sopasCmdVec[CMD_RUN];
4581  std::string restartCmd = sopasCmdVec[CMD_REBOOT];
4582  std::string EEPCmd = sopasCmdVec[CMD_WRITE_EEPROM];
4583  std::string UserLvlCmd = sopasCmdVec[CMD_SET_ACCESS_MODE_3];
4584  result = (0 == sendSopasAndCheckAnswer(ipcommand, &ipcomandReply));
4585  result &= (0 == sendSopasAndCheckAnswer(EEPCmd, &resetReply));
4586  result &= (0 == sendSopasAndCheckAnswer(runCmd, &resetReply));
4587  result &= (0 == sendSopasAndCheckAnswer(UserLvlCmd, &resetReply));
4588  result &= (0 == sendSopasAndCheckAnswer(restartCmd, &resetReply));
4589  }
4590  return (result);
4591  }
4592 
4593  bool SickScanCommon::setNTPServerAndStart(boost::asio::ip::address_v4 ipNewIPAddr, bool useBinaryCmd)
4594  {
4595  bool result = false;
4596 
4597 
4598  unsigned long adrBytesLong[4];
4599  std::string s = ipNewIPAddr.to_string(); // convert to string, to_bytes not applicable for older linux version
4600  const char *ptr = s.c_str(); // char * to address
4601  // decompose pattern like aaa.bbb.ccc.ddd
4602  sscanf(ptr, "%lu.%lu.%lu.%lu", &(adrBytesLong[0]), &(adrBytesLong[1]), &(adrBytesLong[2]), &(adrBytesLong[3]));
4603 
4604  // convert into byte array
4605  unsigned char ipbytearray[4];
4606  for (int i = 0; i < 4; i++)
4607  {
4608  ipbytearray[i] = adrBytesLong[i] & 0xFF;
4609  }
4610 
4611 
4612  char ntpipcommand[255];
4613  char ntpupdatetimecommand[255];
4614  const char *pcCmdMask = sopasCmdMaskVec[CMD_SET_NTP_SERVER_IP_ADDR].c_str();
4615  sprintf(ntpipcommand, pcCmdMask, ipbytearray[0], ipbytearray[1], ipbytearray[2], ipbytearray[3]);
4616 
4617  const char *pcCmdMaskUpdatetime = sopasCmdMaskVec[CMD_SET_NTP_UPDATETIME].c_str();
4618  sprintf(ntpupdatetimecommand, pcCmdMaskUpdatetime, 5);
4619  std::vector<unsigned char> outputFilterntpupdatetimecommand;
4620  //
4621  if (useBinaryCmd)
4622  {
4623  std::vector<unsigned char> reqBinary;
4624  this->convertAscii2BinaryCmd(sopasCmdVec[CMD_SET_NTP_INTERFACE_ETH].c_str(), &reqBinary);
4626  reqBinary.clear();
4627  this->convertAscii2BinaryCmd(ntpipcommand, &reqBinary);
4629  reqBinary.clear();
4630  this->convertAscii2BinaryCmd(ntpupdatetimecommand, &reqBinary);
4631  result &= (0 == sendSopasAndCheckAnswer(reqBinary, &sopasReplyBinVec[CMD_SET_NTP_UPDATETIME]));
4632  reqBinary.clear();
4633  this->convertAscii2BinaryCmd(sopasCmdVec[CMD_ACTIVATE_NTP_CLIENT].c_str(), &reqBinary);
4634  result &= (0 == sendSopasAndCheckAnswer(reqBinary, &sopasReplyBinVec[CMD_ACTIVATE_NTP_CLIENT]));
4635  reqBinary.clear();
4636 
4637  }
4638  else
4639  {
4640  std::vector<unsigned char> ipcomandReply;
4641  std::vector<unsigned char> resetReply;
4642  std::string ntpInterFaceETHCmd = sopasCmdVec[CMD_SET_NTP_INTERFACE_ETH];
4643  std::string activateNTPCmd = sopasCmdVec[CMD_ACTIVATE_NTP_CLIENT];
4644  result &= (0 == sendSopasAndCheckAnswer(ntpInterFaceETHCmd, &resetReply));
4645  result &= (0 == sendSopasAndCheckAnswer(ntpipcommand, &ipcomandReply));
4646  result &= (0 == sendSopasAndCheckAnswer(activateNTPCmd, &resetReply));
4647  result &= (0 == sendSopasAndCheckAnswer(ntpupdatetimecommand, &outputFilterntpupdatetimecommand));
4648  }
4649  return (result);
4650  }
4651 
4653  {
4654  sensorIsRadar = _isRadar;
4655  }
4656 
4658  {
4659  return (sensorIsRadar);
4660  }
4661 
4662  // SopasProtocol m_protocolId;
4663 
4664 } /* namespace sick_scan */
4665 
4666 
4667 
sick_scan::SickScanCommon::isCompatibleDevice
bool isCompatibleDevice(const std::string identStr) const
check the identification string
Definition: sick_scan_common.cpp:2722
sick_scan_messages.h
UINT16
uint16_t UINT16
Definition: BasicDatatypes.hpp:27
sick_scan::SickScanCommon::CMD_SET_ENCODER_MODE_DL
@ CMD_SET_ENCODER_MODE_DL
Definition: sick_scan_common.h:128
sick_scan::SickScanCommon::CMD_SERIAL_NUMBER
@ CMD_SERIAL_NUMBER
Definition: sick_scan_common.h:93
sick_scan::SickScanCommon::sopasSendMutex
std::mutex sopasSendMutex
Definition: sick_scan_common.h:389
sick_scan::SickScanFieldMonSingleton::getInstance
static SickScanFieldMonSingleton * getInstance()
Definition: sick_generic_field_mon.cpp:176
sick_scan::ScannerBasicParam::getScannerName
std::string getScannerName(void)
Getting name (type) of scanner.
Definition: sick_generic_parser.cpp:86
sick_scan::SickScanCommon::CMD_SET_NTP_UPDATETIME
@ CMD_SET_NTP_UPDATETIME
Definition: sick_scan_common.h:155
sick_scan::SickScanCommon::CMD_SET_ACCESS_MODE_3
@ CMD_SET_ACCESS_MODE_3
Definition: sick_scan_common.h:109
binScanfGetStringFromVec
const std::string binScanfGetStringFromVec(std::vector< unsigned char > *replyDummy, int off, long len)
Convert part of unsigned char vector into a std::string.
Definition: sick_scan_common.cpp:156
sick_scan::SickScanMessages::parseLIDoutputstateMsg
static bool parseLIDoutputstateMsg(const ros::Time &timeStamp, uint8_t *receiveBuffer, int receiveLength, bool useBinaryProtocol, const std::string &frame_id, sick_scan::LIDoutputstateMsg &output_msg)
Definition: sick_scan_messages.cpp:83
ros::NodeHandle::setParam
void setParam(const std::string &key, bool b) const
sick_scan::SickScanCommon::CMD_APPLICATION_MODE_FIELD_OFF
@ CMD_APPLICATION_MODE_FIELD_OFF
Definition: sick_scan_common.h:107
sick_scan::ScannerBasicParam::getScanMirroredAndShifted
bool getScanMirroredAndShifted()
flag to mark mirroring of rotation direction
Definition: sick_generic_parser.cpp:280
sick_scan::SickScanCommon::CMD_WRITE_EEPROM
@ CMD_WRITE_EEPROM
Definition: sick_scan_common.h:95
msg
msg
sick_scan::ScannerBasicParam::getAngularDegreeResolution
double getAngularDegreeResolution(void)
Get angular resolution in degress.
Definition: sick_generic_parser.cpp:187
sick_scan::SickScanCommon::init_scanner
virtual int init_scanner()
initialize scanner
Definition: sick_scan_common.cpp:1163
sick_scan::USE_EVAL_FIELD_LMS5XX_LOGIC
@ USE_EVAL_FIELD_LMS5XX_LOGIC
Definition: sick_generic_parser.h:65
sick_scan::stripControl
std::string stripControl(std::vector< unsigned char > s)
Converts a SOPAS command to a human readable string.
Definition: sick_scan_common.cpp:195
SoftwarePLL::updatePLL
bool updatePLL(uint32_t sec, uint32_t nanoSec, uint32_t curtick)
Updates PLL internale State should be called only with network send timestamps.
Definition: softwarePLL.cpp:125
sick_scan::EVAL_FIELD_SUPPORT
EVAL_FIELD_SUPPORT
Definition: sick_generic_parser.h:61
swap_endian
void swap_endian(unsigned char *ptr, int numBytes)
Universal swapping function.
Definition: sick_scan_common.cpp:102
ROS_ERROR_STREAM
#define ROS_ERROR_STREAM(args)
sick_scan::ExitError
@ ExitError
Definition: abstract_parser.h:47
sick_scan::SickScanCommon::CMD_OPERATION_HOURS
@ CMD_OPERATION_HOURS
Definition: sick_scan_common.h:98
diagnostic_updater::TimeStampStatusParam
angle
TFSIMD_FORCE_INLINE tfScalar angle(const Quaternion &q1, const Quaternion &q2)
sick_scan::SickScanImu::isImuDatagram
bool isImuDatagram(char *datagram, size_t datagram_length)
Definition: sick_generic_imu.cpp:85
sick_scan::SickScanCommon::CMD_ACTIVATE_STANDBY
@ CMD_ACTIVATE_STANDBY
Definition: sick_scan_common.h:101
sick_scan::SickScanCommon::CMD_STOP_MEASUREMENT
@ CMD_STOP_MEASUREMENT
Definition: sick_scan_common.h:153
sick_scan::SickScanCommon::CMD_ACTIVATE_NTP_CLIENT
@ CMD_ACTIVATE_NTP_CLIENT
Definition: sick_scan_common.h:122
sick_scan::ScannerBasicParam::getEncoderMode
int8_t getEncoderMode()
Getter-Method for encoder mode.
Definition: sick_generic_parser.cpp:395
sick_scan::USE_EVAL_FIELD_TIM7XX_LOGIC
@ USE_EVAL_FIELD_TIM7XX_LOGIC
Definition: sick_generic_parser.h:64
sick_scan::SickScanCommon::CMD_DEVICE_IDENT
@ CMD_DEVICE_IDENT
Definition: sick_scan_common.h:92
sick_scan::SickGenericParser
Definition: sick_generic_parser.h:159
sick_scan::SickScanCommon::setProtocolType
void setProtocolType(SopasProtocol cola_dialect_id)
set protocol type as enum
Definition: sick_scan_common.cpp:828
sick_scan::SickScanMarker
Definition: sick_scan_marker.h:53
sick_scan::SickScanCommon::lferec_pub_
ros::Publisher lferec_pub_
Definition: sick_scan_common.h:351
ros::NodeHandle::getParam
bool getParam(const std::string &key, bool &b) const
rad2deg
#define rad2deg(x)
Definition: sick_scan_common.cpp:84
sick_scan::SickScanCommon::getReadTimeOutInMs
int getReadTimeOutInMs()
get timeout in milliseconds
Definition: sick_scan_common.cpp:809
sick_scan::SickScanCommon::readTimeOutInMs
int readTimeOutInMs
Definition: sick_scan_common.h:387
binScanfVec
int binScanfVec(const std::vector< unsigned char > *vec, const char *fmt,...)
Definition: binScanf.cpp:417
DataDumper::dumpUcharBufferToConsole
int dumpUcharBufferToConsole(unsigned char *buffer, int bufLen)
Definition: dataDumper.cpp:67
sick_generic_radar.h
sick_scan::SickScanCommon::CMD_SET_ENCODER_MODE_SI
@ CMD_SET_ENCODER_MODE_SI
Definition: sick_scan_common.h:126
sick_scan::ScannerBasicParam::getMaxEvalFields
int getMaxEvalFields(void)
Definition: sick_generic_parser.cpp:345
deg2rad_const
#define deg2rad_const
Definition: sick_scan_common.cpp:87
sick_scan::SickScanCommon::Encoder_pub
ros::Publisher Encoder_pub
Definition: sick_scan_common.h:277
sick_scan::SickScanCommon::CMD_LOAD_APPLICATION_DEFAULT
@ CMD_LOAD_APPLICATION_DEFAULT
Definition: sick_scan_common.h:148
sick_scan::SickScanCommon::dumpDatagramForDebugging
bool dumpDatagramForDebugging(unsigned char *buffer, int bufLen)
Definition: sick_scan_common.cpp:2683
sick_scan::ScannerBasicParam::setEncoderMode
void setEncoderMode(int8_t _EncoderMode)
Prama for encoder mode.
Definition: sick_generic_parser.cpp:384
DataDumper::instance
static DataDumper & instance()
Definition: dataDumper.h:13
sick_scan::ScannerBasicParam::getDeviceIsRadar
bool getDeviceIsRadar(void)
flag to mark the device as radar (instead of laser scanner)
Definition: sick_generic_parser.cpp:260
sick_scan::ExitSuccess
@ ExitSuccess
Definition: abstract_parser.h:46
sick_scan::SickScanCommon::CMD_APPLICATION_MODE_FIELD_ON
@ CMD_APPLICATION_MODE_FIELD_ON
Definition: sick_scan_common.h:106
sick_scan::SickScanCommon::CMD_GET_SAFTY_FIELD_CFG
@ CMD_GET_SAFTY_FIELD_CFG
Definition: sick_scan_common.h:164
sick_scan::SickScanCommon::sopasReplyBinVec
std::vector< std::vector< unsigned char > > sopasReplyBinVec
Definition: sick_scan_common.h:370
sick_scan::SickScanCommon::CMD_SET_NTP_TIMEZONE
@ CMD_SET_NTP_TIMEZONE
Definition: sick_scan_common.h:156
SoftwarePLL::instance
static SoftwarePLL & instance()
Definition: softwarePLL.h:23
sick_scan::SickScanCommon::CMD_DEVICE_STATE
@ CMD_DEVICE_STATE
Definition: sick_scan_common.h:97
sick_scan::SickScanCommon::CMD_START_RADARDATA
@ CMD_START_RADARDATA
Definition: sick_scan_common.h:121
sick_scan::SickScanCommon::CMD_SET_TRANSMIT_OBJECTS_OFF
@ CMD_SET_TRANSMIT_OBJECTS_OFF
Definition: sick_scan_common.h:143
sick_scan::SickScanCommon::CMD_SET_ECHO_FILTER
@ CMD_SET_ECHO_FILTER
Definition: sick_scan_common.h:154
colab::addFrameToBuffer
void addFrameToBuffer(UINT8 *sendBuffer, UINT8 *cmdBuffer, UINT16 *len)
Definition: colab.cpp:99
sick_scan::SickScanCommon::init_device
virtual int init_device()=0
sick_scan::SickScanCommon::CMD_START_SCANDATA
@ CMD_START_SCANDATA
Definition: sick_scan_common.h:120
sick_scan::SickScanCommon::sendSOPASCommand
virtual int sendSOPASCommand(const char *request, std::vector< unsigned char > *reply, int cmdLen=-1)=0
Send a SOPAS command to the device and print out the response to the console.
sick_scan::SickScanCommon::CMD_START_MEASUREMENT
@ CMD_START_MEASUREMENT
Definition: sick_scan_common.h:152
sick_scan::ScannerBasicParam::setIntensityResolutionIs16Bit
void setIntensityResolutionIs16Bit(bool _IntensityResolutionIs16Bit)
Set the RSSI Value length.
Definition: sick_generic_parser.cpp:300
sick_scan::SickScanCommon::CMD_SET_ENCODER_MODE
@ CMD_SET_ENCODER_MODE
Definition: sick_scan_common.h:124
sick_scan::SickScanCommon::check_angle_range
void check_angle_range(SickScanConfig &conf)
check angle setting in the config and adjust the min_ang to the max_ang if min_ang greater than max_a...
Definition: sick_scan_common.cpp:4088
sick_scan::SickScanFieldMonSingleton::parseBinaryLIDinputstateMsg
int parseBinaryLIDinputstateMsg(unsigned char *datagram, int datagram_length)
Parse binary LIDinputstate message and set active field set.
Definition: sick_generic_field_mon.cpp:210
sick_scan::SickScanCommon::SickScanCommon
SickScanCommon(SickGenericParser *parser)
Construction of SickScanCommon.
Definition: sick_scan_common.cpp:335
sick_scan::SickScanRadarSingleton
Definition: sick_generic_radar.h:160
SoftwarePLL::getCorrectedTimeStamp
bool getCorrectedTimeStamp(uint32_t &sec, uint32_t &nanoSec, uint32_t tick)
Definition: softwarePLL.cpp:183
angle_compensator.h
sick_scan::SickScanCommon::angleCompensator
AngleCompensator * angleCompensator
Definition: sick_scan_common.h:394
ROS_DEBUG_STREAM
#define ROS_DEBUG_STREAM(args)
sick_scan::SickScanCommon::CMD_GET_PARTIAL_SCAN_CFG
@ CMD_GET_PARTIAL_SCAN_CFG
Definition: sick_scan_common.h:116
sick_scan::ExitFatal
@ ExitFatal
Definition: abstract_parser.h:48
ros::Publisher::publish
void publish(const boost::shared_ptr< M > &message) const
diagnostic_updater::Updater::broadcast
void broadcast(int lvl, const std::string msg)
ros::NodeHandle::advertise
Publisher advertise(AdvertiseOptions &ops)
sick_scan::SickScanCommon::publish_lferec_
bool publish_lferec_
Definition: sick_scan_common.h:352
sick_scan::SickScanFieldMonSingleton::getMonFields
const std::vector< SickScanMonField > & getMonFields(void) const
Definition: sick_generic_field_mon.h:169
data
data
sick_scan_common.h
f
f
getDiagnosticErrorCode
static int getDiagnosticErrorCode()
return diagnostic error code (small helper function) This small helper function was introduced to all...
Definition: sick_scan_common.cpp:139
sick_scan::SickScanCommon::CMD_SET_ACCESS_MODE_3_SAFETY_SCANNER
@ CMD_SET_ACCESS_MODE_3_SAFETY_SCANNER
Definition: sick_scan_common.h:110
sick_scan::SickScanCommon::CMD_DEVICE_TYPE
@ CMD_DEVICE_TYPE
Definition: sick_scan_common.h:149
DataDumper::binDataToAsciiString
static std::string binDataToAsciiString(const uint8_t *binary_data, int length)
Definition: dataDumper.cpp:108
sick_scan::SickScanCommon::publish_datagram_
bool publish_datagram_
Definition: sick_scan_common.h:349
sick_scan::SickScanCommon::CMD_GET_OUTPUT_RANGES
@ CMD_GET_OUTPUT_RANGES
Definition: sick_scan_common.h:113
sick_scan::ScannerBasicParam::getScanAngleShift
double getScanAngleShift()
Definition: sick_generic_parser.cpp:360
sick_scan::SickScanCommon::CMD_SET_NTP_SERVER_IP_ADDR
@ CMD_SET_NTP_SERVER_IP_ADDR
Definition: sick_scan_common.h:159
sick_scan::SickScanCommon::dynamic_reconfigure_server_
dynamic_reconfigure::Server< sick_scan::SickScanConfig > dynamic_reconfigure_server_
Definition: sick_scan_common.h:363
sick_scan::SickScanCommon::CMD_SET_TRANSMIT_OBJECTS_ON
@ CMD_SET_TRANSMIT_OBJECTS_ON
Definition: sick_scan_common.h:142
sick_scan::SickScanCommon::CMD_SET_OUTPUT_RANGES
@ CMD_SET_OUTPUT_RANGES
Definition: sick_scan_common.h:111
binScanf.hpp
CoLa_A
@ CoLa_A
Command Language ASCI.
Definition: sick_scan_common_nw.h:32
sick_scan::SickScanCommon::config_
SickScanConfig config_
Definition: sick_scan_common.h:282
ROS_WARN_STREAM
#define ROS_WARN_STREAM(args)
sick_scan::SickScanCommon::CMD_SET_PARTICLE_FILTER
@ CMD_SET_PARTICLE_FILTER
Definition: sick_scan_common.h:102
sick_scan::SickScanCommon::expectedFrequency_
double expectedFrequency_
Definition: sick_scan_common.h:359
SoftwarePLL::packeds_droped
int packeds_droped
Definition: softwarePLL.h:83
sick_scan::SickScanCommon::CMD_SET_PARTIAL_SCANDATA_CFG
@ CMD_SET_PARTIAL_SCANDATA_CFG
Definition: sick_scan_common.h:118
AngleCompensator::parseReply
int parseReply(bool isBinary, std::vector< unsigned char > &replyVec)
Parse reply of angle compensation values given the command MCAngleCompSin (see testbed)
Definition: angle_compensator.cpp:268
sick_scan::SickScanCommon::checkForProtocolChangeAndMaybeReconnect
bool checkForProtocolChangeAndMaybeReconnect(bool &useBinaryCmdNow)
checks the current protocol type and gives information about necessary change
Definition: sick_scan_common.cpp:4499
sick_scan::SickScanCommon::rebootScanner
virtual bool rebootScanner()
Send a SOPAS command to the scanner that should cause a soft reset.
Definition: sick_scan_common.cpp:518
diagnostic_updater::DiagnosedPublisher::publish
virtual void publish(const boost::shared_ptr< T > &message)
sick_scan::SickScanCommon::CMD_START_IMU_DATA
@ CMD_START_IMU_DATA
Definition: sick_scan_common.h:135
sick_scan::SickScanCommon::CMD_ALIGNMENT_MODE
@ CMD_ALIGNMENT_MODE
Definition: sick_scan_common.h:104
colab.hpp
sick_scan::SickScanCommon::CMD_REBOOT
@ CMD_REBOOT
Definition: sick_scan_common.h:94
sick_scan::SickScanCommon::getProtocolType
int getProtocolType(void)
get protocol type as enum
Definition: sick_scan_common.cpp:819
sick_scan::SickScanCommon::CMD_END
@ CMD_END
Definition: sick_scan_common.h:173
sick_scan::SickScanCommon::CMD_SET_PARTIAL_SCAN_CFG
@ CMD_SET_PARTIAL_SCAN_CFG
Definition: sick_scan_common.h:115
sick_scan::SickScanCommon::setNTPServerAndStart
bool setNTPServerAndStart(boost::asio::ip::address_v4 ipNewIPAddr, bool useBinaryCmd)
Definition: sick_scan_common.cpp:4593
sick_scan::SickScanCommon::sensorIsRadar
bool sensorIsRadar
Definition: sick_scan_common.h:392
sick_scan::SickScanCommon::cloud_marker_
SickScanMarker * cloud_marker_
Definition: sick_scan_common.h:355
ROS_DEBUG
#define ROS_DEBUG(...)
sick_scan::SickScanRadarSingleton::parseDatagram
int parseDatagram(ros::Time timeStamp, unsigned char *receiveBuffer, int actual_length, bool useBinaryProtocol)
Definition: sick_generic_radar.cpp:1044
sick_scan::SickScanCommon::CMD_SET_ENCODER_MODE_NO
@ CMD_SET_ENCODER_MODE_NO
Definition: sick_scan_common.h:125
sick_scan::SickScanCommon::setNewIpAddress
bool setNewIpAddress(boost::asio::ip::address_v4 ipNewIPAddr, bool useBinaryCmd)
Definition: sick_scan_common.cpp:4535
sick_scan::SickScanCommon::m_protocolId
SopasProtocol m_protocolId
Definition: sick_scan_common.h:344
sick_generic_imu.h
sick_scan::SickScanCommon::CMD_APPLICATION_MODE
@ CMD_APPLICATION_MODE
Definition: sick_scan_common.h:105
sick_scan::SickScanCommon::m_min_intensity
double m_min_intensity
Definition: sick_scan_common.h:396
diagnostic_updater::DiagnosedPublisher< sensor_msgs::LaserScan >
sick_scan::SickScanCommon::CMD_GET_PARTIAL_SCANDATA_CFG
@ CMD_GET_PARTIAL_SCANDATA_CFG
Definition: sick_scan_common.h:117
sick_scan::SickScanCommon::CMD_SET_OUTPUT_RANGES_NAV3
@ CMD_SET_OUTPUT_RANGES_NAV3
Definition: sick_scan_common.h:112
sick_scan::SickGenericParser::checkScanTiming
void checkScanTiming(float time_increment, float scan_time, float angle_increment, float tol)
Definition: sick_generic_parser.cpp:817
sick_scan::SickScanCommon::sopasCmdChain
std::vector< int > sopasCmdChain
Definition: sick_scan_common.h:373
sick_scan::SickScanCommon::nh_
ros::NodeHandle nh_
Definition: sick_scan_common.h:346
sick_scan::SickScanCommon::convertAscii2BinaryCmd
int convertAscii2BinaryCmd(const char *requestAscii, std::vector< unsigned char > *requestBinary)
Convert ASCII-message to Binary-message.
Definition: sick_scan_common.cpp:4114
sick_scan::SickScanImu::parseDatagram
int parseDatagram(ros::Time timeStamp, unsigned char *receiveBuffer, int actual_length, bool useBinaryProtocol)
Definition: sick_generic_imu.cpp:557
sick_scan::SickScanCommon::CMD_SET_IP_ADDR
@ CMD_SET_IP_ADDR
Definition: sick_scan_common.h:157
sick_scan::SickScanRadarSingleton::getInstance
static SickScanRadarSingleton * getInstance()
Definition: sick_generic_radar.cpp:78
ROS_WARN
#define ROS_WARN(...)
sick_scan::SickScanCommon::imuScan_pub_
ros::Publisher imuScan_pub_
Definition: sick_scan_common.h:276
sick_scan
Definition: abstract_parser.cpp:50
sick_scan::SickScanCommon::CMD_SET_MEAN_FILTER
@ CMD_SET_MEAN_FILTER
Definition: sick_scan_common.h:103
sick_scan::SickScanCommon::stop_scanner
virtual int stop_scanner()
Stops sending scan data.
Definition: sick_scan_common.cpp:424
sick_scan::SickScanCommon::checkForBinaryAnswer
int checkForBinaryAnswer(const std::vector< unsigned char > *reply)
Check block for correct framing and starting sequence of a binary message.
Definition: sick_scan_common.cpp:481
sick_scan::SickScanCommon::CMD_SET_ENCODER_MODE_DP
@ CMD_SET_ENCODER_MODE_DP
Definition: sick_scan_common.h:127
sick_scan::SickScanFieldMonSingleton::getActiveFieldset
int getActiveFieldset(void)
Definition: sick_generic_field_mon.h:172
sick_scan::ScannerBasicParam::getUseEvalFields
EVAL_FIELD_SUPPORT getUseEvalFields()
Definition: sick_generic_parser.cpp:335
sick_scan::SickScanCommon::CMD_SET_TO_COLA_A_PROTOCOL
@ CMD_SET_TO_COLA_A_PROTOCOL
Definition: sick_scan_common.h:162
sick_scan::SickScanCommon::getSensorIsRadar
bool getSensorIsRadar(void)
Definition: sick_scan_common.cpp:4657
sick_scan::SickScanCommon::CMD_SET_GATEWAY
@ CMD_SET_GATEWAY
Definition: sick_scan_common.h:158
sick_scan::SickScanCommon::CMD_SET_NTP_INTERFACE_ETH
@ CMD_SET_NTP_INTERFACE_ETH
Definition: sick_scan_common.h:123
ROS_INFO_STREAM
#define ROS_INFO_STREAM(args)
sick_scan::SickScanCommon::CMD_SET_SCANDATACONFIGNAV
@ CMD_SET_SCANDATACONFIGNAV
Definition: sick_scan_common.h:160
sick_scan::SickScanFieldMonSingleton
Definition: sick_generic_field_mon.h:151
sick_scan::SickScanCommon::CMD_STOP_IMU_DATA
@ CMD_STOP_IMU_DATA
Definition: sick_scan_common.h:136
sick_scan::SickScanCommon::sendSopasAndCheckAnswer
int sendSopasAndCheckAnswer(std::string request, std::vector< unsigned char > *reply, int cmdId)
send command and check answer
Definition: sick_scan_common.cpp:704
diagnostic_updater::Updater::setHardwareID
void setHardwareID(const std::string &hwid)
ROS_FATAL
#define ROS_FATAL(...)
sick_scan::SickScanCommon::CMD_FIRMWARE_VERSION
@ CMD_FIRMWARE_VERSION
Definition: sick_scan_common.h:96
sick_scan::SickScanImu::isImuAckDatagram
bool isImuAckDatagram(char *datagram, size_t datagram_length)
Definition: sick_generic_imu.cpp:130
sick_scan::ScannerBasicParam::getNumberOfMaximumEchos
int getNumberOfMaximumEchos(void)
Get number of maximum echoes for this laser scanner type.
Definition: sick_generic_parser.cpp:155
sick_scan::SickGenericParser::get_range_max
float get_range_max(void)
Getting maximum range.
Definition: sick_generic_parser.cpp:1203
PARAM_MIN_ANG
#define PARAM_MIN_ANG
Definition: sick_scan_common.h:176
sick_scan::SickScanCommon::cloud_
sensor_msgs::PointCloud2 cloud_
Definition: sick_scan_common.h:279
AngleCompensator::getHumanReadableFormula
std::string getHumanReadableFormula(void)
Definition: angle_compensator.cpp:342
sick_scan::SickGenericParser::get_range_min
float get_range_min(void)
Getting minimum range.
Definition: sick_generic_parser.cpp:1213
sick_scan::SickScanMessages::parseLFErecMsg
static bool parseLFErecMsg(const ros::Time &timeStamp, uint8_t *receiveBuffer, int receiveLength, bool useBinaryProtocol, EVAL_FIELD_SUPPORT eval_field_logic, const std::string &frame_id, sick_scan::LFErecMsg &output_msg)
Definition: sick_scan_messages.cpp:231
sick_scan::SickScanCommon::CMD_STOP_SCANDATA
@ CMD_STOP_SCANDATA
Definition: sick_scan_common.h:119
sick_generic_field_mon.h
SICK_SCANNER_MRS_1XXX_NAME
#define SICK_SCANNER_MRS_1XXX_NAME
Definition: sick_generic_parser.h:40
sick_scan::SickScanCommon::CMD_APPLICATION_MODE_RANGING_ON
@ CMD_APPLICATION_MODE_RANGING_ON
Definition: sick_scan_common.h:108
sick_scan::SickScanCommon::CMD_RUN
@ CMD_RUN
Definition: sick_scan_common.h:114
sick_scan::SickGenericParser::parse_datagram
virtual int parse_datagram(char *datagram, size_t datagram_length, SickScanConfig &config, sensor_msgs::LaserScan &msg, int &numEchos, int &echoMask)
Parsing Ascii datagram.
Definition: sick_generic_parser.cpp:847
diagnostic_updater::Updater::update
void update()
sick_scan::SickScanCommon::CMD_SET_ENCODER_RES
@ CMD_SET_ENCODER_RES
Definition: sick_scan_common.h:133
AngleCompensator
Definition: angle_compensator.h:11
sick_scan::SickScanCommon::CMD_SET_LFEREC_ACTIVE
@ CMD_SET_LFEREC_ACTIVE
Definition: sick_scan_common.h:166
sick_scan_config_internal.h
sick_scan::ScannerBasicParam::getElevationDegreeResolution
double getElevationDegreeResolution(void)
get angular resolution in VERTICAL direction for multilayer scanner
Definition: sick_generic_parser.cpp:230
ros::Time
sick_scan::SickScanCommon::CMD_SET_TRANSMIT_RAWTARGETS_OFF
@ CMD_SET_TRANSMIT_RAWTARGETS_OFF
Definition: sick_scan_common.h:140
sick_scan::SickScanCommon::datagram_pub_
ros::Publisher datagram_pub_
Definition: sick_scan_common.h:348
sick_scan::SickScanCommon::CMD_SET_TO_COLA_B_PROTOCOL
@ CMD_SET_TO_COLA_B_PROTOCOL
Definition: sick_scan_common.h:163
sick_scan::SickScanCommon::CMD_ORDER_NUMBER
@ CMD_ORDER_NUMBER
Definition: sick_scan_common.h:150
SICK_SCANNER_NAV_3XX_NAME
#define SICK_SCANNER_NAV_3XX_NAME
Definition: sick_generic_parser.h:50
sick_scan::ScannerBasicParam::getIntensityResolutionIs16Bit
bool getIntensityResolutionIs16Bit(void)
Get the RSSI Value length.
Definition: sick_generic_parser.cpp:310
sick_scan::SickScanCommon::sopasCmdVec
std::vector< std::string > sopasCmdVec
Definition: sick_scan_common.h:367
sick_scan::SickScanMarker::updateMarker
void updateMarker(const std::vector< SickScanMonField > &fields, int fieldset, int eval_field_logic)
Definition: sick_scan_marker.cpp:93
sick_scan::ScannerBasicParam::getUseSafetyPasWD
bool getUseSafetyPasWD()
flag to mark the device uses the safety scanner password \reutrn Bool true for safety password false ...
Definition: sick_generic_parser.cpp:330
sick_scan::SickScanCommon::CMD_GET_ANGLE_COMPENSATION_PARAM
@ CMD_GET_ANGLE_COMPENSATION_PARAM
Definition: sick_scan_common.h:161
sick_scan::SickScanCommon::loopOnce
int loopOnce()
parsing datagram and publishing ros messages
Definition: sick_scan_common.cpp:2802
SopasProtocol
SopasProtocol
Definition: sick_scan_common_nw.h:30
sick_scan::SickScanCommon::convertBigEndianCharArrayToUnsignedLong
unsigned long convertBigEndianCharArrayToUnsignedLong(const unsigned char *vecArr)
Convert little endian to big endian (should be replaced by swap-routine)
Definition: sick_scan_common.cpp:464
ROS_ERROR
#define ROS_ERROR(...)
colaa.hpp
sick_scan::SickScanCommon::diagnostics_
diagnostic_updater::Updater diagnostics_
Definition: sick_scan_common.h:340
PARAM_MAX_ANG
#define PARAM_MAX_ANG
Definition: sick_scan_common.h:177
sick_scan::SickScanCommon::pub_
ros::Publisher pub_
Definition: sick_scan_common.h:347
sick_scan::SickGenericParser::getCurrentParamPtr
ScannerBasicParam * getCurrentParamPtr()
Gets Pointer to parameter object.
Definition: sick_generic_parser.cpp:683
sick_scan::SickScanCommon::sopasReplyVec
std::vector< std::string > sopasReplyVec
Definition: sick_scan_common.h:369
sick_scan::ScannerBasicParam::getExpectedFrequency
double getExpectedFrequency(void)
get expected scan frequency
Definition: sick_generic_parser.cpp:208
sick_scan::SickScanCommon::cloud_pub_
ros::Publisher cloud_pub_
Definition: sick_scan_common.h:275
ros::NodeHandle::param
T param(const std::string &param_name, const T &default_val) const
sick_scan::SickScanCommon::get_datagram
virtual int get_datagram(ros::Time &recvTimeStamp, unsigned char *receiveBuffer, int bufferSize, int *actual_length, bool isBinaryProtocol, int *numberOfRemainingFifoEntries)=0
Read a datagram from the device.
sick_scan::SickScanCommon::CMD_SET_LID_OUTPUTSTATE_ACTIVE
@ CMD_SET_LID_OUTPUTSTATE_ACTIVE
Definition: sick_scan_common.h:167
sick_scan::SickScanCommon::update_config
void update_config(sick_scan::SickScanConfig &new_config, uint32_t level=0)
updating configuration
Definition: sick_scan_common.cpp:4102
CoLa_B
@ CoLa_B
Command Language binary.
Definition: sick_scan_common_nw.h:33
sick_scan::SickScanCommon::CMD_DEVICE_IDENT_LEGACY
@ CMD_DEVICE_IDENT_LEGACY
Definition: sick_scan_common.h:91
sick_scan::SickScanCommon::CMD_SET_LID_INPUTSTATE_ACTIVE
@ CMD_SET_LID_INPUTSTATE_ACTIVE
Definition: sick_scan_common.h:168
sick_scan_common_nw.h
sick_scan::SickScanCommon::~SickScanCommon
virtual ~SickScanCommon()
Destructor of SickScanCommon.
Definition: sick_scan_common.cpp:570
sick_scan::SickScanCommon::CMD_SET_TRANSMIT_RAWTARGETS_ON
@ CMD_SET_TRANSMIT_RAWTARGETS_ON
Definition: sick_scan_common.h:139
ros::Duration::sleep
bool sleep() const
sick_scan::SickScanCommon::CMD_POWER_ON_COUNT
@ CMD_POWER_ON_COUNT
Definition: sick_scan_common.h:99
sick_scan::SickScanCommon::lidoutputstate_pub_
ros::Publisher lidoutputstate_pub_
Definition: sick_scan_common.h:353
sick_scan::SickScanCommon::CMD_SET_ENOCDER_RES_1
@ CMD_SET_ENOCDER_RES_1
Definition: sick_scan_common.h:132
sick_scan::SickScanCommon::CMD_LOCATION_NAME
@ CMD_LOCATION_NAME
Definition: sick_scan_common.h:100
sick_scan::SickScanCommon::diagnosticPub_
diagnostic_updater::DiagnosedPublisher< sensor_msgs::LaserScan > * diagnosticPub_
Definition: sick_scan_common.h:358
SICK_SCANNER_NAV_2XX_NAME
#define SICK_SCANNER_NAV_2XX_NAME
Definition: sick_generic_parser.h:51
SICK_SCANNER_LMS_5XX_NAME
#define SICK_SCANNER_LMS_5XX_NAME
Definition: sick_generic_parser.h:45
sick_scan::SickScanCommon::init
virtual int init()
init routine of scanner
Definition: sick_scan_common.cpp:837
sick_scan::SickScanCommon::CMD_SET_3_4_TO_ENCODER
@ CMD_SET_3_4_TO_ENCODER
Definition: sick_scan_common.h:130
sick_scan::SickScanCommon::CMD_SET_INCREMENTSOURCE_ENC
@ CMD_SET_INCREMENTSOURCE_ENC
Definition: sick_scan_common.h:129
sick_scan::SickScanCommon::CMD_SET_TRACKING_MODE_1
@ CMD_SET_TRACKING_MODE_1
Definition: sick_scan_common.h:146
AngleCompensator::compensateAngleInRadFromRos
double compensateAngleInRadFromRos(double angleInRadFromRos)
Compensate raw angle given in [RAD] in the ROS axis orientation system.
Definition: angle_compensator.cpp:116
s
static sick_scan::SickScanCommonTcp * s
Definition: sick_generic_laser.cpp:92
ROS_INFO
#define ROS_INFO(...)
sick_scan::SickScanCommon::setReadTimeOutInMs
void setReadTimeOutInMs(int timeOutInMs)
set timeout in milliseconds
Definition: sick_scan_common.cpp:799
sick_scan::SickScanCommon::CMD_SET_TRACKING_MODE_0
@ CMD_SET_TRACKING_MODE_0
Definition: sick_scan_common.h:145
sick_scan::SickScanCommon::sopasCmdErrMsg
std::vector< std::string > sopasCmdErrMsg
Definition: sick_scan_common.h:372
sick_scan::ScannerBasicParam::getNumberOfLayers
int getNumberOfLayers(void)
Getting number of scanner layers.
Definition: sick_generic_parser.cpp:109
ROS_ASSERT
#define ROS_ASSERT(cond)
sick_scan::SickScanFieldMonSingleton::parseBinaryDatagram
int parseBinaryDatagram(std::vector< unsigned char > datagramm)
Definition: sick_generic_field_mon.cpp:242
sick_scan::SickScanCommon::replyToString
std::string replyToString(const std::vector< unsigned char > &reply)
Converts reply from sendSOPASCommand to string.
Definition: sick_scan_common.cpp:2640
sick_scan::SickScanCommon::outputChannelFlagId
int outputChannelFlagId
Definition: sick_scan_common.h:375
PARAM_RES_ANG
#define PARAM_RES_ANG
Definition: sick_scan_common.h:178
sick_scan::SickScanCommon::parser_
SickGenericParser * parser_
Definition: sick_scan_common.h:366
ros::Duration
sick_scan::SickScanCommon::init_cmdTables
int init_cmdTables()
init command tables and define startup sequence
Definition: sick_scan_common.cpp:865
sick_scan::SickScanCommon::sopasCmdMaskVec
std::vector< std::string > sopasCmdMaskVec
Definition: sick_scan_common.h:368
sick_scan::SickScanFieldMonSingleton::parseAsciiDatagram
int parseAsciiDatagram(std::vector< unsigned char > datagramm)
Parsing Ascii datagram.
Definition: sick_generic_field_mon.cpp:235
sick_scan::SickScanCommon::publish_lidoutputstate_
bool publish_lidoutputstate_
Definition: sick_scan_common.h:354
SoftwarePLL::fifoSize
static const int fifoSize
Definition: softwarePLL.h:82
sick_scan::SickScanCommon::sopasReplyStrVec
std::vector< std::string > sopasReplyStrVec
Definition: sick_scan_common.h:371
sick_scan::SickScanCommon::generateExpectedAnswerString
std::string generateExpectedAnswerString(const std::vector< unsigned char > requestStr)
Generate expected answer string from the command string.
Definition: sick_scan_common.cpp:584
sick_scan::SickScanCommon::setSensorIsRadar
void setSensorIsRadar(bool _isRadar)
Definition: sick_scan_common.cpp:4652
ros::NodeHandle
sick_scan::SickScanImu
Definition: sick_generic_imu.h:184
sick_scan::ScannerBasicParam::getUseBinaryProtocol
bool getUseBinaryProtocol(void)
flag to decide between usage of ASCII-sopas or BINARY-sopas
Definition: sick_generic_parser.cpp:290
stringToVector
std::vector< unsigned char > stringToVector(std::string s)
Universal swapping function.
Definition: sick_scan_common.cpp:122
sick_scan::sick_crc8
unsigned char sick_crc8(unsigned char *msgBlock, int len)
calculate crc-code for last byte of binary message XOR-calucation is done ONLY over message content (...
Definition: sick_scan_common.cpp:177
binScanfGuessDataLenFromMask
int binScanfGuessDataLenFromMask(const char *scanfMask)
Definition: binScanf.cpp:438
SICK_SCANNER_TIM_240_NAME
#define SICK_SCANNER_TIM_240_NAME
Definition: sick_generic_parser.h:41
ros::Time::now
static Time now()
diagnostic_updater::FrequencyStatusParam


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