sick_scan_common_tcp.cpp
Go to the documentation of this file.
1 
50 #ifdef _MSC_VER
51 #pragma warning(disable: 4996)
52 #pragma warning(disable: 4267)
53 #pragma warning(disable: 4101) // C4101: "e" : Unreferenzierte lokale Variable
54 #define _WIN32_WINNT 0x0501
55 
56 #endif
57 
59 #include <sick_scan/tcp/colaa.hpp>
60 #include <sick_scan/tcp/colab.hpp>
61 
62 #include <boost/asio.hpp>
63 #include <boost/lambda/lambda.hpp>
64 #include <algorithm>
65 #include <iterator>
66 #include <boost/lexical_cast.hpp>
67 #include <vector>
69 
70 #ifdef _MSC_VER
71 #include "sick_scan/rosconsole_simu.hpp"
72 #endif
73 
74 std::vector<unsigned char> exampleData(65536);
75 std::vector<unsigned char> receivedData(65536);
76 static long receivedDataLen = 0;
77 
79 {
80 #ifdef _MSC_VER
81 #undef ERROR
82  return(2);
83 #else
84  return (diagnostic_msgs::DiagnosticStatus::ERROR);
85 #endif
86 }
87 
88 namespace sick_scan
89 {
90  bool emulateReply(UINT8 *requestData, int requestLen, std::vector<unsigned char> *replyVector)
91  {
92  std::string request;
93  std::string reply;
94  std::vector<std::string> keyWordList;
95  std::vector<std::string> answerList;
96 
97  keyWordList.push_back("sMN SetAccessMode");
98  answerList.push_back("sAN SetAccessMode 1");
99 
100  keyWordList.push_back("sWN EIHstCola");
101  answerList.push_back("sWA EIHstCola");
102 
103  keyWordList.push_back("sRN FirmwareVersion");
104  answerList.push_back("sRA FirmwareVersion 8 1.0.0.0R");
105 
106  keyWordList.push_back("sRN OrdNum");
107  answerList.push_back("sRA OrdNum 7 1234567");
108 
109  keyWordList.push_back("sWN TransmitTargets 1");
110  answerList.push_back("sWA TransmitTargets");
111 
112  keyWordList.push_back("sWN TransmitObjects 1");
113  answerList.push_back("sWA TransmitObjects");
114 
115  keyWordList.push_back("sWN TCTrackingMode 0");
116  answerList.push_back("sWA TCTrackingMode");
117 
118  keyWordList.push_back("sRN SCdevicestate");
119  answerList.push_back("sRA SCdevicestate 1");
120 
121  keyWordList.push_back("sRN DItype");
122  answerList.push_back("sRA DItype D RMS3xx-xxxxxx");
123 
124  keyWordList.push_back("sRN ODoprh");
125  answerList.push_back("sRA ODoprh 451");
126 
127  keyWordList.push_back("sMN mSCloadappdef");
128  answerList.push_back("sAN mSCloadappdef");
129 
130 
131  keyWordList.push_back("sRN SerialNumber");
132  answerList.push_back("sRA SerialNumber 8 18020073");
133 
134  keyWordList.push_back("sMN Run");
135  answerList.push_back("sAN Run 1s");
136 
137  keyWordList.push_back("sRN ODpwrc");
138  answerList.push_back("sRA ODpwrc 20");
139 
140  keyWordList.push_back("sRN LocationName");
141  answerList.push_back("sRA LocationName B not defined");
142 
143  keyWordList.push_back("sEN LMDradardata 1");
144  answerList.push_back("sEA LMDradardata 1");
145 
146  for (int i = 0; i < requestLen; i++)
147  {
148  request += (char) requestData[i];
149  }
150  for (int i = 0; i < keyWordList.size(); i++)
151  {
152  if (request.find(keyWordList[i]) != std::string::npos)
153  {
154  reply = (char) 0x02;
155  reply += answerList[i];
156  reply += (char) 0x03;
157  }
158  }
159 
160  replyVector->clear();
161  for (int i = 0; i < reply.length(); i++)
162  {
163  replyVector->push_back((unsigned char) reply[i]);
164  }
165 
166 
167  return (true);
168  }
169 
170 
171  SickScanCommonTcp::SickScanCommonTcp(const std::string &hostname, const std::string &port, int &timelimit,
172  SickGenericParser *parser, char cola_dialect_id)
173  :
174  SickScanCommon(parser),
175  socket_(io_service_),
176  deadline_(io_service_),
177  hostname_(hostname),
178  port_(port),
179  timelimit_(timelimit)
180  {
181 
182  setEmulSensor(false);
183  if ((cola_dialect_id == 'a') || (cola_dialect_id == 'A'))
184  {
185  this->setProtocolType(CoLa_A);
186  }
187 
188  if ((cola_dialect_id == 'b') || (cola_dialect_id == 'B'))
189  {
190  this->setProtocolType(CoLa_B);
191  }
192 
193  assert(this->getProtocolType() != CoLa_Unknown);
194 
197  this->setReplyMode(0);
198  // io_service_.setReadCallbackFunction(boost::bind(&SopasDevice::readCallbackFunction, this, _1, _2));
199 
200  // Set up the deadline actor to implement timeouts.
201  // Based on blocking TCP example on:
202  // http://www.boost.org/doc/libs/1_46_0/doc/html/boost_asio/example/timeouts/blocking_tcp_client.cpp
203  deadline_.expires_at(boost::posix_time::pos_infin);
204  checkDeadline();
205 
206  }
207 
209  {
210  // stop_scanner();
211  close_device();
212  }
213 
214  using boost::asio::ip::tcp;
215  using boost::lambda::var;
216  using boost::lambda::_1;
217 
218 
220  {
221 
222  }
223 
225  {
226  if (obj != NULL)
227  {
228  ((SickScanCommonTcp *) (obj))->disconnectFunction();
229  }
230  }
231 
232  void SickScanCommonTcp::readCallbackFunctionS(void *obj, UINT8 *buffer, UINT32 &numOfBytes)
233  {
234  ((SickScanCommonTcp *) obj)->readCallbackFunction(buffer, numOfBytes);
235  }
236 
237 
239  {
240  m_replyMode = _mode;
241  }
242 
244  {
245  return (m_replyMode);
246  }
247 
248 #if 0
249  void SickScanCommonTcp::setProtocolType(char cola_dialect_id)
250  {
251  if ((cola_dialect_id == 'a') || (cola_dialect_id == 'A'))
252  {
253  this->m_protocol = CoLa_A;
254  }
255  else
256  {
257  this->m_protocol = CoLa_B;
258  }
259  }
260 #endif
261 
267  void SickScanCommonTcp::setEmulSensor(bool _emulFlag)
268  {
269  m_emulSensor = _emulFlag;
270  }
271 
278  {
279  return (m_emulSensor);
280  }
281 
282  //
283  // Look for 23-frame (STX/ETX) in receive buffer.
284  // Move frame to start of buffer
285  //
286  // Return: 0 : No (complete) frame found
287  // >0 : Frame length
288  //
290  {
291  UINT32 frameLen = 0;
292  UINT32 i;
293 
294  // Depends on protocol...
295  if (getProtocolType() == CoLa_A)
296  {
297  //
298  // COLA-A
299  //
300  // Must start with STX (0x02)
301  if (m_receiveBuffer[0] != 0x02)
302  {
303  // Look for starting STX (0x02)
304  for (i = 1; i < m_numberOfBytesInReceiveBuffer; i++)
305  {
306  if (m_receiveBuffer[i] == 0x02)
307  {
308  break;
309  }
310  }
311 
312  // Found beginning of frame?
313  if (i >= m_numberOfBytesInReceiveBuffer)
314  {
315  // No start found, everything can be discarded
316  m_numberOfBytesInReceiveBuffer = 0; // Invalidate buffer
317  return SopasEventMessage(); // No frame found
318  }
319 
320  // Move frame start to index 0
321  UINT32 newLen = m_numberOfBytesInReceiveBuffer - i;
322  memmove(&(m_receiveBuffer[0]), &(m_receiveBuffer[i]), newLen);
323  m_numberOfBytesInReceiveBuffer = newLen;
324  }
325 
326  // Look for ending ETX (0x03)
327  for (i = 1; i < m_numberOfBytesInReceiveBuffer; i++)
328  {
329  if (m_receiveBuffer[i] == 0x03)
330  {
331  break;
332  }
333  }
334 
335  // Found end?
336  if (i >= m_numberOfBytesInReceiveBuffer)
337  {
338  // No end marker found, so it's not a complete frame (yet)
339  return SopasEventMessage(); // No frame found
340  }
341 
342  // Calculate frame length in byte
343  frameLen = i + 1;
344 
345  return SopasEventMessage(m_receiveBuffer, CoLa_A, frameLen);
346  }
347  else if (getProtocolType() == CoLa_B)
348  {
349  UINT32 magicWord;
350  UINT32 payloadlength;
351 
353  {
354  return SopasEventMessage();
355  }
356  UINT16 pos = 0;
357  magicWord = colab::getIntegerFromBuffer<UINT32>(m_receiveBuffer, pos);
358  if (magicWord != 0x02020202)
359  {
360  // Look for starting STX (0x02020202)
361  for (i = 1; i <= m_numberOfBytesInReceiveBuffer - 4; i++)
362  {
363  pos = i; // this is needed, as the position value is updated by getIntegerFromBuffer
364  magicWord = colab::getIntegerFromBuffer<UINT32>(m_receiveBuffer, pos);
365  if (magicWord == 0x02020202)
366  {
367  // found magic word
368  break;
369  }
370  }
371 
372  // Found beginning of frame?
373  if (i > m_numberOfBytesInReceiveBuffer - 4)
374  {
375  // No start found, everything can be discarded
376  m_numberOfBytesInReceiveBuffer = 0; // Invalidate buffer
377  return SopasEventMessage(); // No frame found
378  }
379  else
380  {
381  // Move frame start to index
382  UINT32 bytesToMove = m_numberOfBytesInReceiveBuffer - i;
383  memmove(&(m_receiveBuffer[0]), &(m_receiveBuffer[i]), bytesToMove); // payload+magic+length+s+checksum
384  m_numberOfBytesInReceiveBuffer = bytesToMove;
385  }
386  }
387 
388  // Pruefe Laenge des Pufferinhalts
390  {
391  // Es sind nicht genug Daten fuer einen Frame
392  printInfoMessage("SickScanCommonNw::findFrameInReceiveBuffer: Frame cannot be decoded yet, only " +
393  ::toString(m_numberOfBytesInReceiveBuffer) + " bytes in the buffer.", m_beVerbose);
394  return SopasEventMessage();
395  }
396 
397  // Read length of payload
398  pos = 4;
399  payloadlength = colab::getIntegerFromBuffer<UINT32>(m_receiveBuffer, pos);
401  "SickScanCommonNw::findFrameInReceiveBuffer: Decoded payload length is " + ::toString(payloadlength) +
402  " bytes.", m_beVerbose);
403 
404  // Ist die Datenlaenge plausibel und wuede in den Puffer passen?
405  if (payloadlength > (sizeof(m_receiveBuffer) - 9))
406  {
407  // magic word + length + checksum = 9
408  printWarning(
409  "SickScanCommonNw::findFrameInReceiveBuffer: Frame too big for receive buffer. Frame discarded with length:"
410  + ::toString(payloadlength) + ".");
412  return SopasEventMessage();
413  }
414  if ((payloadlength + 9) > m_numberOfBytesInReceiveBuffer)
415  {
416  // magic word + length + s + checksum = 10
418  "SickScanCommonNw::findFrameInReceiveBuffer: Frame not complete yet. Waiting for the rest of it (" +
419  ::toString(payloadlength + 9 - m_numberOfBytesInReceiveBuffer) + " bytes missing).", m_beVerbose);
420  return SopasEventMessage(); // frame not complete
421  }
422 
423  // Calculate the total frame length in bytes: Len = Frame (9 bytes) + Payload
424  frameLen = payloadlength + 9;
425 
426  //
427  // test checksum of payload
428  //
429  UINT8 temp = 0;
430  UINT8 temp_xor = 0;
431  UINT8 checkSum;
432 
433  // Read original checksum
434  pos = frameLen - 1;
435  checkSum = colab::getIntegerFromBuffer<UINT8>(m_receiveBuffer, pos);
436 
437  // Erzeuge die Pruefsumme zum Vergleich
438  for (UINT16 i = 8; i < (frameLen - 1); i++)
439  {
440  pos = i;
441  temp = colab::getIntegerFromBuffer<UINT8>(m_receiveBuffer, pos);
442  temp_xor = temp_xor ^ temp;
443  }
444 
445  // Vergleiche die Pruefsummen
446  if (temp_xor != checkSum)
447  {
448  printWarning("SickScanCommonNw::findFrameInReceiveBuffer: Wrong checksum, Frame discarded.");
450  return SopasEventMessage();
451  }
452 
453  return SopasEventMessage(m_receiveBuffer, CoLa_B, frameLen);
454  }
455 
456  // Return empty frame
457  return SopasEventMessage();
458  }
459 
460 
467  {
468 
469  if (getProtocolType() == CoLa_A)
470  {
472  "SickScanCommonNw::processFrame: Calling processFrame_CoLa_A() with " + ::toString(frame.size()) + " bytes.",
473  m_beVerbose);
474  // processFrame_CoLa_A(frame);
475  }
476  else if (getProtocolType() == CoLa_B)
477  {
479  "SickScanCommonNw::processFrame: Calling processFrame_CoLa_B() with " + ::toString(frame.size()) + " bytes.",
480  m_beVerbose);
481  // processFrame_CoLa_B(frame);
482  }
483 
484  // Push frame to recvQueue
485 
486  DatagramWithTimeStamp dataGramWidthTimeStamp(timeStamp, std::vector<unsigned char>(frame.getRawData(),
487  frame.getRawData() +
488  frame.size()));
489  // recvQueue.push(std::vector<unsigned char>(frame.getRawData(), frame.getRawData() + frame.size()));
490  recvQueue.push(dataGramWidthTimeStamp);
491  }
492 
494  {
495  ros::Time rcvTimeStamp = ros::Time::now(); // stamp received datagram
496  bool beVerboseHere = false;
498  "SickScanCommonNw::readCallbackFunction(): Called with " + toString(numOfBytes) + " available bytes.",
499  beVerboseHere);
500 
501  ScopedLock lock(&m_receiveDataMutex); // Mutex for access to the input buffer
502  UINT32 remainingSpace = sizeof(m_receiveBuffer) - m_numberOfBytesInReceiveBuffer;
503  UINT32 bytesToBeTransferred = numOfBytes;
504  if (remainingSpace < numOfBytes)
505  {
506  bytesToBeTransferred = remainingSpace;
507  // printWarning("SickScanCommonNw::readCallbackFunction(): Input buffer space is to small, transferring only " +
508  // ::toString(bytesToBeTransferred) + " of " + ::toString(numOfBytes) + " bytes.");
509  }
510  else
511  {
512  // printInfoMessage("SickScanCommonNw::readCallbackFunction(): Transferring " + ::toString(bytesToBeTransferred) +
513  // " bytes from TCP to input buffer.", beVerboseHere);
514  }
515 
516  if (bytesToBeTransferred > 0)
517  {
518  // Data can be transferred into our input buffer
519  memcpy(&(m_receiveBuffer[m_numberOfBytesInReceiveBuffer]), buffer, bytesToBeTransferred);
520  m_numberOfBytesInReceiveBuffer += bytesToBeTransferred;
521 
522  UINT32 size = 0;
523 
524  while (1)
525  {
526  // Now work on the input buffer until all received datasets are processed
528 
529  size = frame.size();
530  if (size == 0)
531  {
532  // Framesize = 0: There is no valid frame in the buffer. The buffer is either empty or the frame
533  // is incomplete, so leave the loop
534  printInfoMessage("SickScanCommonNw::readCallbackFunction(): No complete frame in input buffer, we are done.",
535  beVerboseHere);
536 
537  // Leave the loop
538  break;
539  }
540  else
541  {
542  // A frame was found in the buffer, so process it now.
544  "SickScanCommonNw::readCallbackFunction(): Processing a frame of length " + ::toString(frame.size()) +
545  " bytes.", beVerboseHere);
546  processFrame(rcvTimeStamp, frame);
547  UINT32 bytesToMove = m_numberOfBytesInReceiveBuffer - size;
548  memmove(&(m_receiveBuffer[0]), &(m_receiveBuffer[size]), bytesToMove); // payload+magic+length+s+checksum
549  m_numberOfBytesInReceiveBuffer = bytesToMove;
550 
551  }
552  }
553  }
554  else
555  {
556  // There was input data from the TCP interface, but our input buffer was unable to hold a single byte.
557  // Either we have not read data from our buffer for a long time, or something has gone wrong. To re-sync,
558  // we clear the input buffer here.
560  }
561  }
562 
563 
565  {
566  int portInt;
567  sscanf(port_.c_str(), "%d", &portInt);
568  m_nw.init(hostname_, portInt, disconnectFunctionS, (void *) this);
570  if (this->getEmulSensor())
571  {
572  ROS_INFO("Sensor emulation is switched on - network traffic is switched off.");
573  }
574  else
575  {
576  m_nw.connect();
577  }
578  return ExitSuccess;
579  }
580 
582  {
583  ROS_WARN("Disconnecting TCP-Connection.");
584  m_nw.disconnect();
585  return 0;
586  }
587 
588 
590  {
591  stop_scanner();
592  return (true);
593  }
594 
595  void SickScanCommonTcp::handleRead(boost::system::error_code error, size_t bytes_transfered)
596  {
597  ec_ = error;
598  bytes_transfered_ += bytes_transfered;
599  }
600 
601 
603  {
605  {
606  // The reason the function is called is that the deadline expired. Close
607  // the socket to return all IO operations and reset the deadline
608  socket_.close();
609  deadline_.expires_at(boost::posix_time::pos_infin);
610  }
611 
612  // Nothing bad happened, go back to sleep
613  deadline_.async_wait(boost::bind(&SickScanCommonTcp::checkDeadline, this));
614  }
615 
616 
618  {
619  int ret = 0;
620  ret = this->recvQueue.getNumberOfEntriesInQueue();
621  return(ret);
622  }
623 
624  int SickScanCommonTcp::readWithTimeout(size_t timeout_ms, char *buffer, int buffer_size, int *bytes_read,
625  bool *exception_occured, bool isBinary)
626  {
627  // Set up the deadline to the proper timeout, error and delimiters
628  deadline_.expires_from_now(boost::posix_time::milliseconds(timeout_ms));
629  const char end_delim = static_cast<char>(0x03);
630  int dataLen = 0;
631  ec_ = boost::asio::error::would_block;
632  bytes_transfered_ = 0;
633 
634  size_t to_read;
635 
636  int numBytes = 0;
637  // Polling - should be changed to condition variable in the future
638  int waitingTimeInMs = 1; // try to lookup for new incoming packages
639  int i;
640  for (i = 0; i < timeout_ms; i += waitingTimeInMs)
641  {
642  if (false == this->recvQueue.isQueueEmpty())
643  {
644  break;
645  }
646  boost::this_thread::sleep(boost::posix_time::milliseconds(waitingTimeInMs));
647  }
648  if (i >= timeout_ms)
649  {
650  ROS_ERROR("no answer received after %zu ms. Maybe sopas mode is wrong.\n", timeout_ms);
651  return (ExitError);
652  }
653  boost::condition_variable cond_;
654  DatagramWithTimeStamp datagramWithTimeStamp = this->recvQueue.pop();
655 
656  *bytes_read = datagramWithTimeStamp.datagram.size();
657  memcpy(buffer, &(datagramWithTimeStamp.datagram[0]), datagramWithTimeStamp.datagram.size());
658  return (ExitSuccess);
659  }
660 
664  int SickScanCommonTcp::sendSOPASCommand(const char *request, std::vector<unsigned char> *reply, int cmdLen)
665  {
666 #if 0
667  if (!socket_.is_open()) {
668  ROS_ERROR("sendSOPASCommand: socket not open");
669  diagnostics_.broadcast(getDiagnosticErrorCode(), "sendSOPASCommand: socket not open.");
670  return ExitError;
671  }
672 #endif
673  int sLen = 0;
674  int preambelCnt = 0;
675  bool cmdIsBinary = false;
676 
677  if (request != NULL)
678  {
679  sLen = cmdLen;
680  preambelCnt = 0; // count 0x02 bytes to decide between ascii and binary command
681  if (sLen >= 4)
682  {
683  for (int i = 0; i < 4; i++)
684  {
685  if (request[i] == 0x02)
686  {
687  preambelCnt++;
688  }
689  }
690  }
691 
692  if (preambelCnt < 4)
693  {
694  cmdIsBinary = false;
695  }
696  else
697  {
698  cmdIsBinary = true;
699  }
700  int msgLen = 0;
701  if (cmdIsBinary == false)
702  {
703  msgLen = strlen(request);
704  }
705  else
706  {
707  int dataLen = 0;
708  for (int i = 4; i < 8; i++)
709  {
710  dataLen |= ((unsigned char) request[i] << (7 - i) * 8);
711  }
712  msgLen = 8 + dataLen + 1; // 8 Msg. Header + Packet +
713  }
714 #if 1
715  if (getEmulSensor())
716  {
717  emulateReply((UINT8 *) request, msgLen, reply);
718  }
719  else
720  {
721  bool debugBinCmd = false;
722  if (debugBinCmd)
723  {
724  printf("=== START HEX DUMP ===\n");
725  for (int i = 0; i < msgLen; i++)
726  {
727  unsigned char *ptr = (UINT8 *) request;
728  printf("%02x ", ptr[i]);
729  }
730  printf("\n=== END HEX DUMP ===\n");
731  }
732  m_nw.sendCommandBuffer((UINT8 *) request, msgLen);
733  }
734 #else
735 
736  /*
737  * Write a SOPAS variable read request to the device.
738  */
739  try
740  {
741  boost::asio::write(socket_, boost::asio::buffer(request, msgLen));
742  }
743  catch (boost::system::system_error &e)
744  {
745  ROS_ERROR("write error for command: %s", request);
746  diagnostics_.broadcast(getDiagnosticErrorCode(), "Write error for sendSOPASCommand.");
747  return ExitError;
748  }
749 #endif
750  }
751 
752  // Set timeout in 5 seconds
753  const int BUF_SIZE = 65536;
754  char buffer[BUF_SIZE];
755  int bytes_read;
756  // !!!
757  if (getEmulSensor())
758  {
759 
760  }
761  else
762  {
763  if (readWithTimeout(getReadTimeOutInMs(), buffer, BUF_SIZE, &bytes_read, 0, cmdIsBinary) == ExitError)
764  {
765  ROS_INFO_THROTTLE(1.0, "sendSOPASCommand: no full reply available for read after %d ms", getReadTimeOutInMs());
767  "sendSOPASCommand: no full reply available for read after timeout.");
768  return ExitError;
769  }
770 
771 
772  if (reply)
773  {
774  reply->resize(bytes_read);
775 
776  std::copy(buffer, buffer + bytes_read, &(*reply)[0]);
777  }
778  }
779  return ExitSuccess;
780  }
781 
782 
783  int SickScanCommonTcp::get_datagram(ros::Time &recvTimeStamp, unsigned char *receiveBuffer, int bufferSize,
784  int *actual_length,
785  bool isBinaryProtocol, int *numberOfRemainingFifoEntries)
786  {
787  if (NULL != numberOfRemainingFifoEntries)
788  {
789  *numberOfRemainingFifoEntries = 0;
790  }
791  this->setReplyMode(1);
792 
793  if (this->getEmulSensor())
794  {
795  // boost::this_thread::sleep(boost::posix_time::milliseconds(waitingTimeInMs));
797  uint32_t nanoSec = timeStamp.nsec;
798  double waitTime10Hz = 10.0 * (double) nanoSec / 1E9; // 10th of sec. [0..10[
799 
800  uint32_t waitTime = (int) waitTime10Hz; // round down
801 
802  double waitTimeUntilNextTime10Hz = 1 / 10.0 * (1.0 - (waitTime10Hz - waitTime));
803 
804  ros::Duration(waitTimeUntilNextTime10Hz).sleep();
805 
807  radar->setEmulation(true);
808  radar->simulateAsciiDatagram(receiveBuffer, actual_length);
809  recvTimeStamp = ros::Time::now();
810  }
811  else
812  {
813  const int maxWaitInMs = getReadTimeOutInMs();
814  std::vector<unsigned char> dataBuffer;
815 #if 1 // prepared for reconnect
816  bool retVal = this->recvQueue.waitForIncomingObject(maxWaitInMs);
817  if (retVal == false)
818  {
819  ROS_WARN("Timeout during waiting for new datagram");
820  return ExitError;
821  }
822  else
823  {
824  // Look into receiving queue for new Datagrams
825  //
826  //
827  DatagramWithTimeStamp datagramWithTimeStamp = this->recvQueue.pop();
828  if (NULL != numberOfRemainingFifoEntries)
829  {
830  *numberOfRemainingFifoEntries = this->recvQueue.getNumberOfEntriesInQueue();
831  }
832  recvTimeStamp = datagramWithTimeStamp.timeStamp;
833  dataBuffer = datagramWithTimeStamp.datagram;
834 
835  }
836 #endif
837  // dataBuffer = this->recvQueue.pop();
838  long size = dataBuffer.size();
839  memcpy(receiveBuffer, &(dataBuffer[0]), size);
840  *actual_length = size;
841  }
842 
843 #if 0
844  static int cnt = 0;
845  char szFileName[255];
846  sprintf(szFileName, "/tmp/dg%06d.bin", cnt++);
847 
848  FILE *fout;
849 
850  fout = fopen(szFileName, "wb");
851  if (fout != NULL)
852  {
853  fwrite(receiveBuffer, *actual_length, 1, fout);
854  fclose(fout);
855  }
856 #endif
857  return ExitSuccess;
858 
859  if (!socket_.is_open())
860  {
861  ROS_ERROR("get_datagram: socket not open");
862  diagnostics_.broadcast(getDiagnosticErrorCode(), "get_datagram: socket not open.");
863  return ExitError;
864  }
865 
866  /*
867  * Write a SOPAS variable read request to the device.
868  */
869  std::vector<unsigned char> reply;
870 
871  // Wait at most 5000ms for a new scan
872  size_t timeout = 30000;
873  bool exception_occured = false;
874 
875  char *buffer = reinterpret_cast<char *>(receiveBuffer);
876 
877  if (readWithTimeout(timeout, buffer, bufferSize, actual_length, &exception_occured, isBinaryProtocol) !=
878  ExitSuccess)
879  {
880  ROS_ERROR_THROTTLE(1.0, "get_datagram: no data available for read after %zu ms", timeout);
881  diagnostics_.broadcast(getDiagnosticErrorCode(), "get_datagram: no data available for read after timeout.");
882 
883  // Attempt to reconnect when the connection was terminated
884  if (!socket_.is_open())
885  {
886 #ifdef _MSC_VER
887  Sleep(1000);
888 #else
889  sleep(1);
890 #endif
891  ROS_INFO("Failure - attempting to reconnect");
892  return init();
893  }
894 
895  return exception_occured ? ExitError : ExitSuccess; // keep on trying
896  }
897 
898  return ExitSuccess;
899  }
900 
901 } /* namespace sick_scan */
SickScanCommonTcp(const std::string &hostname, const std::string &port, int &timelimit, SickGenericParser *parser, char cola_dialect_id)
std::vector< unsigned char > exampleData(65536)
uint16_t UINT16
int getReadTimeOutInMs()
get timeout in milliseconds
bool emulateReply(UINT8 *requestData, int requestLen, std::vector< unsigned char > *replyVector)
bool init(std::string ipAddress, unsigned short portNumber, Tcp::DisconnectFunction disconnectFunction, void *obj)
boost::asio::ip::tcp::socket socket_
ros::Time * timeStamp(M &m)
static int getDiagnosticErrorCode()
void sendCommandBuffer(UINT8 *buffer, UINT16 len)
virtual int get_datagram(ros::Time &recvTimeStamp, unsigned char *receiveBuffer, int bufferSize, int *actual_length, bool isBinaryProtocol, int *numberOfRemainingFifoEntries)
Read a datagram from the device.
bool getEmulSensor()
get emulation flag (using emulation instead of "real" scanner - currently implemented for radar ...
ROS::Time now(void)
static void readCallbackFunctionS(void *obj, UINT8 *buffer, UINT32 &numOfBytes)
#define printInfoMessage(a, b)
uint32_t UINT32
bool setReadCallbackFunction(Tcp::ReadFunction readFunction, void *obj)
#define ROS_WARN(...)
virtual int sendSOPASCommand(const char *request, std::vector< unsigned char > *reply, int cmdLen)
Send a SOPAS command to the device and print out the response to the console.
Command Language ASCI.
static void disconnectFunctionS(void *obj)
BYTE * getRawData()
get SOPAS raw data include header and CRC
diagnostic_updater::Updater diagnostics_
int getProtocolType(void)
get protocol type as enum
#define ROS_INFO_THROTTLE(period,...)
Queue< DatagramWithTimeStamp > recvQueue
void setProtocolType(SopasProtocol cola_dialect_id)
set protocol type as enum
#define ROS_INFO(...)
#define ROS_ERROR_THROTTLE(period,...)
std::vector< unsigned char > datagram
Command Language binary.
boost::asio::deadline_timer deadline_
UINT32 m_numberOfBytesInReceiveBuffer
Number of bytes in buffer.
static SickScanRadarSingleton * getInstance()
void readCallbackFunction(UINT8 *buffer, UINT32 &numOfBytes)
Unknown Command Language.
boost::system::error_code ec_
static long receivedDataLen
void processFrame(ros::Time timeStamp, SopasEventMessage &frame)
Class that represents a message that was sent by a sensor. (Event message)
std::string toString(INT32 value)
Definition: toolbox.cpp:279
virtual int init()
init routine of scanner
bool connect()
Connects to a sensor via tcp and reads the device name.
bool disconnect()
Closes the connection to the LMS. This is the opposite of init().
static Time now()
SopasEventMessage findFrameInReceiveBuffer()
virtual int stop_scanner()
Stops sending scan data.
Mutex m_receiveDataMutex
Access mutex for buffer.
void broadcast(int lvl, const std::string msg)
bool sleep() const
void setEmulSensor(bool _emulFlag)
Set emulation flag (using emulation instead of "real" scanner - currently implemented for radar...
UINT8 m_receiveBuffer[480000]
Low-Level receive buffer for all data.
std::vector< unsigned char > receivedData(65536)
#define ROS_ERROR(...)
int readWithTimeout(size_t timeout_ms, char *buffer, int buffer_size, int *bytes_read=0, bool *exception_occured=0, bool isBinary=false)
void printWarning(std::string message)
void simulateAsciiDatagram(unsigned char *receiveBuffer, int *actual_length)
void sleep(double seconds)
void handleRead(boost::system::error_code error, size_t bytes_transfered)
uint8_t UINT8


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