MRPT  1.9.9
CSickLaserSerial.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2019, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 // This file contains portions of code from sicklms200.cc from the Player/Stage
11 // project.
12 
13 #include "hwdrivers-precomp.h" // Precompiled headers
14 
15 #include <mrpt/system/CTicTac.h>
16 #include <mrpt/system/crc.h>
17 #include <mrpt/system/os.h>
18 
20 
22 
23 #define RET_ERROR(msg) \
24  { \
25  cout << "[" << __CURRENT_FUNCTION_NAME__ << "] " << msg << endl; \
26  return false; \
27  }
28 
29 using namespace std;
30 using namespace mrpt;
31 using namespace mrpt::comms;
32 using namespace mrpt::obs;
33 using namespace mrpt::poses;
34 using namespace mrpt::math;
35 using namespace mrpt::hwdrivers;
36 using namespace mrpt::system;
37 
38 int CSickLaserSerial::CRC16_GEN_POL = 0x8005;
39 
40 /*-------------------------------------------------------------
41  CSickLaserSerial
42 -------------------------------------------------------------*/
43 CSickLaserSerial::CSickLaserSerial() : m_com_port()
44 
45 {
46  m_sensorLabel = "SICKLMS";
48 }
49 
50 /*-------------------------------------------------------------
51  ~CSickLaserSerial
52 -------------------------------------------------------------*/
54 {
55  if (m_stream)
56  {
57  try
58  {
60  {
62  }
63  }
64  catch (...)
65  {
66  }
67  }
68 
69  if (m_mySerialPort)
70  {
71  delete m_mySerialPort;
72  m_mySerialPort = nullptr;
73  }
74 }
75 
76 /*-------------------------------------------------------------
77  doProcessSimple
78 -------------------------------------------------------------*/
80  bool& outThereIsObservation,
81  mrpt::obs::CObservation2DRangeScan& outObservation, bool& hardwareError)
82 {
83  outThereIsObservation = false;
84  hardwareError = false;
85 
86  if (!tryToOpenComms())
87  {
88  hardwareError = true;
89  return;
90  }
91 
92  vector<float> ranges;
93  unsigned char LMS_stat;
94  bool is_mm_mode;
95 
97 
98  // Wait for a scan:
99  if (!waitContinuousSampleFrame(ranges, LMS_stat, is_mm_mode))
100  {
102  {
103  hardwareError = true;
104  }
105  return;
106  }
107 
108  // Yes, we have a new scan:
109 
110  // -----------------------------------------------
111  // Extract the observation:
112  // -----------------------------------------------
113  outObservation.timestamp = mrpt::system::now();
114 
115  outObservation.sensorLabel = m_sensorLabel; // Set label
116 
117  // Extract the timestamp of the sensor:
118 
119  // And the scan ranges:
120  outObservation.rightToLeft = true;
121  outObservation.aperture = M_PIf;
122  outObservation.maxRange = is_mm_mode ? 32.7 : 81.0;
123  outObservation.stdError = 0.003f;
124  outObservation.sensorPose = mrpt::poses::CPose3D(m_sensorPose);
125 
126  outObservation.resizeScan(ranges.size());
127 
128  for (size_t i = 0; i < ranges.size(); i++)
129  {
130  outObservation.setScanRange(i, ranges[i]);
131  outObservation.setScanRangeValidity(
132  i, (outObservation.scan[i] <= outObservation.maxRange));
133  }
134 
135  // Do filter:
138  // Do show preview:
140 
141  outThereIsObservation = true;
143 }
144 
145 /*-------------------------------------------------------------
146  loadConfig_sensorSpecific
147 -------------------------------------------------------------*/
149  const mrpt::config::CConfigFileBase& configSource,
150  const std::string& iniSection)
151 {
153  configSource.read_float(iniSection, "pose_x", 0),
154  configSource.read_float(iniSection, "pose_y", 0),
155  configSource.read_float(iniSection, "pose_z", 0),
156  DEG2RAD(configSource.read_float(iniSection, "pose_yaw", 0)),
157  DEG2RAD(configSource.read_float(iniSection, "pose_pitch", 0)),
158  DEG2RAD(configSource.read_float(iniSection, "pose_roll", 0)));
159 
160  m_mm_mode = configSource.read_bool(iniSection, "mm_mode", m_mm_mode);
161 
162 #ifdef _WIN32
163  m_com_port =
164  configSource.read_string(iniSection, "COM_port_WIN", m_com_port, true);
165 #else
166  m_com_port =
167  configSource.read_string(iniSection, "COM_port_LIN", m_com_port, true);
168 #endif
169 
171  configSource.read_int(iniSection, "COM_baudRate", m_com_baudRate);
173  configSource.read_int(iniSection, "nTries_connect", m_nTries_connect);
174 
175  m_scans_FOV = configSource.read_int(iniSection, "FOV", m_scans_FOV);
176  m_scans_res = configSource.read_int(iniSection, "resolution", m_scans_res);
177 
178  m_skip_laser_config = configSource.read_bool(
179  iniSection, "skip_laser_config", m_skip_laser_config);
180 
181  // Parent options:
182  C2DRangeFinderAbstract::loadCommonParams(configSource, iniSection);
183 }
184 
185 /*-------------------------------------------------------------
186  turnOn
187 -------------------------------------------------------------*/
188 bool CSickLaserSerial::turnOn() { return true; }
189 /*-------------------------------------------------------------
190  turnOff
191 -------------------------------------------------------------*/
192 bool CSickLaserSerial::turnOff() { return true; }
193 /*-------------------------------------------------------------
194  Tries to open the com port and setup
195  all the LMS protocol. Returns true if OK or already open.
196 -------------------------------------------------------------*/
198 {
199  if (err_msg) *err_msg = "";
200  try
201  {
202  if (!m_stream)
203  {
204  ASSERT_(m_mySerialPort == nullptr);
205 
206  // There is no COMMS port open yet...
207  if (!m_com_port.empty())
208  {
209  // Create the port myself:
210  m_mySerialPort = new CSerialPort();
212  }
213  else
214  throw std::logic_error(
215  "ERROR: No serial port attached with bindIO, neither it "
216  "set with 'setSerialPort'");
217  }
218 
219  // We assure now we have a stream... try to open it, if it's not done
220  // yet.
221  bool just_open = false;
222  auto* COM = dynamic_cast<CSerialPort*>(m_stream);
223  if (COM != nullptr)
224  {
225  if (!COM->isOpen())
226  {
227  // Try to open it now:
229  COM->open(); // will raise an exception on error.
230 
231  // Set basic params:
232  COM->setConfig(9600);
233  COM->setTimeouts(100, 0, 10, 0, 50);
234 
235  just_open = true;
236  }
237  }
238 
239  // It seems the port was open and working so we are done here.
240  if (!just_open) return true;
241 
242  // ==================================================================
243  // Otherwise, it was just opened now, we must send the
244  // ** initialization commands **
245  // and put the laser in continuous measuring mode:
246  // ==================================================================
247  if (!m_skip_laser_config)
248  {
249  if (!LMS_setupSerialComms()) RET_ERROR("error");
250 
251  bool res;
252  for (int nTry = 0; nTry < 4; nTry++)
253  if (true == (res = LMS_sendMeasuringMode_cm_mm())) break;
254 
255  if (!res) return false;
256 
257  for (int nTry = 0; nTry < 4; nTry++)
258  if (true == (res = LMS_startContinuousMode())) break;
259 
260  return res;
261  }
262  else
263  {
264  // Skip setup:
265  return true;
266  }
267  }
268  catch (const std::exception& e)
269  {
270  std::string s = "[CSickLaserSerial] Error trying to open SICK at port ";
271  s += e.what();
272  if (err_msg) *err_msg = s;
273  MRPT_LOG_ERROR(s);
274  return false;
275  }
276 }
277 
278 /*-------------------------------------------------------------
279  waitContinuousSampleFrame
280 -------------------------------------------------------------*/
282  vector<float>& out_ranges_meters, unsigned char& LMS_status,
283  bool& is_mm_mode)
284 {
285  auto* COM = dynamic_cast<CSerialPort*>(m_stream);
286  ASSERTMSG_(COM != nullptr, "No I/O channel bound to this object");
287 
288  size_t nRead, nBytesToRead;
289  size_t nFrameBytes = 0;
290  size_t lengthField;
291  unsigned char buf[2000];
292  buf[2] = buf[3] = buf[4] = 0;
293 
294  while (nFrameBytes < (lengthField = (6 + (buf[2] | (buf[3] << 8)))))
295  {
296  if (lengthField > 800)
297  {
298  cout << "#";
299  nFrameBytes = 0; // No es cabecera de trama correcta
300  buf[2] = buf[3] = 0;
301  }
302 
303  if (nFrameBytes < 4)
304  nBytesToRead = 1;
305  else
306  nBytesToRead = (lengthField)-nFrameBytes;
307 
308  try
309  {
310  nRead = COM->Read(buf + nFrameBytes, nBytesToRead);
311  }
312  catch (const std::exception& e)
313  {
314  // Disconnected?
316  "[CSickLaserSerial::waitContinuousSampleFrame] Disconnecting "
317  "due to comms error: %s\n",
318  e.what());
319  // m_usbConnection->Close();
320  return false;
321  }
322 
323  if (!nRead && !nFrameBytes) return false;
324 
325  if (nRead < nBytesToRead) std::this_thread::sleep_for(1ms);
326 
327  // Lectura OK:
328  // Era la primera?
329  if (nFrameBytes > 1 || (!nFrameBytes && buf[0] == 0x02) ||
330  (nFrameBytes == 1 && buf[1] == 0x80))
331  {
332  nFrameBytes += nRead;
333  }
334  else
335  {
336  nFrameBytes = 0; // No es cabecera de trama correcta
337  buf[2] = buf[3] = 0;
338  // cerr << "."; //"[CSickLaserSerial] Skipping non-header..." <<
339  // endl;
340  }
341  }
342 
343  // Frame received
344  // --------------------------------------------------------------------------
345  // | STX | ADDR | L1 | L2 | COM | INF1 | INF2 | DATA | STA | CRC1 | CRC2
346  // |
347  // --------------------------------------------------------------------------
348 
349  // Trama completa:
350  // Checkear que el byte de comando es 0xB0:
351  if (buf[4] != 0xB0) return false;
352 
353  // GET FRAME INFO
354  int info = buf[5] | (buf[6] << 8); // Little Endian
355  int n_points = info & 0x01FF;
356  is_mm_mode = 0 != ((info & 0xC000) >> 14); // 0x00: cm 0x01: mm
357 
358  out_ranges_meters.resize(n_points);
359 
360  // Copiar rangos:
361  short mask = is_mm_mode ? 0x7FFF : 0x1FFF;
362  float meters_scale = is_mm_mode ? 0.001f : 0.01f;
363 
364  for (int i = 0; i < n_points; i++)
365  out_ranges_meters[i] =
366  ((buf[7 + i * 2] | (buf[8 + i * 2] << 8)) & mask) * meters_scale;
367 
368  // Status
369  LMS_status = buf[lengthField - 3];
370 
371  // CRC:
372  uint16_t CRC =
373  mrpt::system::compute_CRC16(buf, lengthField - 2, CRC16_GEN_POL);
374  uint16_t CRC_packet = buf[lengthField - 2] | (buf[lengthField - 1] << 8);
375  if (CRC_packet != CRC)
376  {
377  cerr << format(
378  "[CSickLaserSerial::waitContinuousSampleFrame] bad CRC "
379  "len=%u nptns=%u: %i != %i",
380  unsigned(lengthField), unsigned(n_points), CRC_packet, CRC)
381  << endl;
382  return false; // Bad CRC
383  }
384 
385  // All OK
386  return true;
387 }
388 
389 /*-------------------------------------------------------------
390  initialize
391 -------------------------------------------------------------*/
393 {
394  string err_str;
396  if (!tryToOpenComms(&err_str))
397  {
398  cerr << err_str << endl;
399  throw std::logic_error(err_str);
400  }
401 }
402 
403 /*-----------------------------------------------------------------
404  Assures laser is connected and operating at 38400, in
405  its case returns true.
406  -----------------------------------------------------------------*/
408 {
409  ASSERT_(
410  m_com_baudRate == 9600 || m_com_baudRate == 38400 ||
411  m_com_baudRate == 500000);
412 
413  auto* COM = dynamic_cast<CSerialPort*>(m_stream);
414  if (COM == nullptr) return true;
415 
416  int detected_rate = 0;
417  for (size_t reps = 0; !detected_rate && reps < m_nTries_connect; reps++)
418  {
419  m_nTries_current = reps;
420 
421  int rates[] = {0, 9600, 38400, 500000};
422 
423  // Try first the desired rate to speed up the process, just in case
424  // the laser is already setup from a previous run:
425  rates[0] = m_com_baudRate;
426 
427  detected_rate = 0;
428 
429  for (size_t i = 0;
430  !detected_rate && i < sizeof(rates) / sizeof(rates[0]); i++)
431  {
432  // Are we already receiving at 500k?
433  // ------------------------------------------------
434  COM->setConfig(rates[i]);
435  if (m_verbose)
436  printf(
437  "[CSickLaserSerial] Testing if the scanner is "
438  "set to %i bauds...\n",
439  rates[i]);
440 
441  LMS_endContinuousMode(); // Stop continuous mode.
442  std::this_thread::sleep_for(100ms);
443  COM->purgeBuffers();
444 
445  for (int nTry = 0; nTry < 4 && !detected_rate; nTry++)
446  {
447  COM->purgeBuffers();
448  // Ask for the laser status at the current rate:
449  if (LMS_statusQuery())
450  {
451  detected_rate = rates[i];
452  break;
453  }
454  std::this_thread::sleep_for(20ms);
455  } // for tries
456  // There is no link, or the baudrate is wrong...
457  }
458 
459  // Try again in a while:
460  if (!detected_rate && reps != (m_nTries_connect - 1))
461  std::this_thread::sleep_for(5000ms);
462  }
463 
464  // Are we connected at the right rate?
465  if (detected_rate == m_com_baudRate) return true;
466 
467  // Switch to the desired rate now
468  if (!this->LMS_setupBaudrate(m_com_baudRate)) RET_ERROR("error");
469 
470  // Check response is OK:
471  if (!(m_received_frame_buffer[2] == 0x03 &&
472  m_received_frame_buffer[4] == 0xA0 &&
473  m_received_frame_buffer[6] == 0x10))
474  return false;
475 
476  COM->setConfig(m_com_baudRate);
477  COM->purgeBuffers();
478 
479  // Wait...
480  std::this_thread::sleep_for(500ms);
481 
482  // And check comms at the new baud rate:
483  return LMS_statusQuery();
484 }
485 
486 /*-----------------------------------------------------------------
487  Query to LMS a baudrate change command.
488  Returns true if response is read ok.
489  -----------------------------------------------------------------*/
491 {
492  ASSERT_(m_stream);
493  if (m_verbose)
494  printf("[CSickLaserSerial::LMS_setupBaudrate] rate=%i\n", baud);
495 
496  uint8_t cmd[4];
497  cmd[0] = 0x20;
498  switch (baud)
499  {
500  case 9600:
501  cmd[1] = 0x42;
502  break;
503  case 19200:
504  cmd[1] = 0x41;
505  break;
506  case 38400:
507  cmd[1] = 0x40;
508  break;
509  case 500000:
510  cmd[1] = 0x48;
511  break;
512  default:
513  THROW_EXCEPTION("Invalid baud rate value");
514  }
515 
516  uint16_t cmd_len = 2;
517 
518  if (!SendCommandToSICK(cmd, cmd_len)) return false;
519  return LMS_waitIncomingFrame(500);
520 }
521 
522 /*-----------------------------------------------------------------
523  Query to LMS a status query.
524  Returns true if response is read ok.
525  -----------------------------------------------------------------*/
527 {
528  ASSERT_(m_stream);
529 
530  uint8_t cmd[1];
531  cmd[0] = 0x31;
532  uint16_t cmd_len = 1;
533 
534  if (!SendCommandToSICK(cmd, cmd_len)) return false;
535  return LMS_waitIncomingFrame(500);
536 }
537 
538 // Returns false if timeout
540 {
541  auto* COM = dynamic_cast<CSerialPort*>(m_stream);
542  ASSERT_(COM);
543 
544  uint8_t b = 0;
545  CTicTac tictac;
546  tictac.Tic();
547 
548  do
549  {
550  if (COM->Read(&b, 1))
551  { // Byte rx:
552  if (b == 0x06) return true;
553  }
554  } while (tictac.Tac() < timeout_ms * 1e-3);
555 
556  if (b == 0x15)
557  RET_ERROR(format("NACK received."))
558  else if (b != 0)
559  RET_ERROR(format("Unexpected code received: 0x%02X", b))
560  else
561  return false; // RET_ERROR("Timeout")
562 }
563 
564 // Returns false if timeout
566 {
567  auto* COM = dynamic_cast<CSerialPort*>(m_stream);
568  ASSERT_(COM);
569 
570  uint8_t b;
571  unsigned int nBytes = 0;
572 
573  CTicTac tictac;
574  tictac.Tic();
575  const double maxTime = timeout * 1e-3;
576 
577  while (nBytes < 6 ||
578  (nBytes < (6U + m_received_frame_buffer[2] +
579  (uint16_t)(m_received_frame_buffer[3] << 8))))
580  {
581  if (COM->Read(&b, 1))
582  {
583  // First byte must be STX:
584  if (nBytes > 1 || (!nBytes && b == 0x02) ||
585  (nBytes == 1 && b == 0x80))
586  {
587  // Store in frame:
588  m_received_frame_buffer[nBytes] = b;
589  nBytes++;
590  if (m_verbose)
591  {
592  printf("[CSickLaserSerial::Receive] RX: %02X\n", b);
593  }
594  }
595  }
596  if (tictac.Tac() >= maxTime) return false; // Timeout
597  }
598 
599  const uint16_t lengthField =
601  // Check len:
602  if (4U + lengthField + 2U != nBytes)
603  {
604  printf(
605  "[CSickLaserSerial::LMS_waitIncomingFrame] Error: expected %u "
606  "bytes, received %u\n",
607  4U + lengthField + 2U, nBytes);
608  return false;
609  }
610 
611  // Check CRC
613  m_received_frame_buffer, 4 + lengthField, CRC16_GEN_POL);
614  uint16_t CRC_packet = m_received_frame_buffer[4 + lengthField + 0] |
615  (m_received_frame_buffer[4 + lengthField + 1] << 8);
616  if (CRC_packet != CRC)
617  {
618  printf(
619  "[CSickLaserSerial::LMS_waitIncomingFrame] Error in CRC: rx: "
620  "0x%04X, computed: 0x%04X\n",
621  CRC_packet, CRC);
622  return false;
623  }
624 
625 #if 0
626  printf("RX: ");
627  for (unsigned int i=0;i<nBytes;i++)
628  printf("%02X ",m_received_frame_buffer[i]);
629  printf("\n");
630 #endif
631 
632  // OK
633  return true;
634 }
635 
637 {
638  auto* COM = dynamic_cast<CSerialPort*>(m_stream);
639  ASSERT_(COM);
640 
641  // **************************
642  // Send command: Switch to Installation mode
643  // **************************
644  uint8_t cmd[128]; // =
645  // {0x02,0x00,0x0A,0x00,0x20,0x00,0x53,0x49,0x43,0x4B,0x5F,0x4C,0x4D,0x53,0xBE,0xC5};
646  cmd[0] = 0x20; /* mode change command */
647  cmd[1] = 0x00; /* configuration mode */
648  cmd[2] = 0x53; // S - the password
649  cmd[3] = 0x49; // I
650  cmd[4] = 0x43; // C
651  cmd[5] = 0x4B; // K
652  cmd[6] = 0x5F; // _
653  cmd[7] = 0x4C; // L
654  cmd[8] = 0x4D; // M
655  cmd[9] = 0x53; // S
656 
657  uint16_t cmd_len = 10;
658  if (!SendCommandToSICK(cmd, cmd_len))
659  RET_ERROR("Error waiting ACK to installation mode");
660  if (!LMS_waitIncomingFrame(500))
661  RET_ERROR("Error in response to installation mode");
662 
663  // Check response
664  if (!(m_received_frame_buffer[4] == 0xA0 &&
665  m_received_frame_buffer[5] == 0x00))
666  RET_ERROR("Wrong response to installation mode");
667 
668  // **************************
669  // Request LMS Configuration
670  // **************************
671  cmd[0] = 0x74;
672  cmd_len = 1;
673 
674  if (!SendCommandToSICK(cmd, cmd_len))
675  RET_ERROR("No ACK to 0x74 (req. config)");
676  if (!LMS_waitIncomingFrame(500))
677  RET_ERROR("No answer to 0x74 (req. config)");
678 
679  // 2. Check response
680  if (m_received_frame_buffer[4] != 0xF4)
681  RET_ERROR("No expected 0xF4 in response to 0x74 (req. config)");
682 
683  // ***********************************************************************
684  // Configure 1/2: Measuremente Range, Measurement Unit, Master/Slave Role
685  // ***********************************************************************
686  // 4.a Modify some values...
687 
688  // Measuring mode: Measurement range 32m in mm mode, or 80m+reflectance info
689  // in cm mode.
690  // See page 98 in LMS2xx_list_datagrams.pdf.
691  m_received_frame_buffer[10] = this->m_mm_mode ? 0x06 : 0x00;
692  m_received_frame_buffer[11] = this->m_mm_mode ? 0x01 : 0x00;
693 
694  // 4.2 Build the output command
695  m_received_frame_buffer[1] = 0x00; // Address
696  m_received_frame_buffer[2] = 0x23; // Length (low byte)
697  m_received_frame_buffer[3] = 0x00; // Length (high byte)
698  m_received_frame_buffer[4] = 0x77; // Configure command
699 
700  memcpy(cmd, m_received_frame_buffer + 4, 0x23);
701  cmd_len = 0x23;
702 
703  // 4.4 Send to the LMS
704  if (!SendCommandToSICK(cmd, cmd_len))
705  RET_ERROR("No ACK for config command (0x77)");
706  if (!LMS_waitIncomingFrame(600))
707  RET_ERROR("No answer for config command (0x77)");
708 
709  if (!(m_received_frame_buffer[4] == 0xF7 &&
710  m_received_frame_buffer[5] == 0x01))
711  RET_ERROR("Wrong answer for config command (0x77)");
712 
713  // **************************
714  // Switch to Monitoring mode
715  // **************************
716  cmd[0] = 0x20;
717  cmd[1] = 0x25;
718  cmd_len = 2;
719  if (!SendCommandToSICK(cmd, cmd_len))
720  RET_ERROR("No ACK for set monitoring mode");
721  if (!LMS_waitIncomingFrame(500))
722  RET_ERROR("No answer for set monitoring mode");
723 
724  if (!(m_received_frame_buffer[4] == 0xA0 &&
725  m_received_frame_buffer[5] == 0x00))
726  RET_ERROR("Wrong answer for set monitoring mode");
727 
728  // All ok.
729  return true;
730 }
731 
732 /*-----------------------------------------------------------------
733  Start continuous mode measurements.
734  Returns true if response is read ok.
735  -----------------------------------------------------------------*/
737 {
738  ASSERT_(m_scans_FOV == 100 || m_scans_FOV == 180);
739  ASSERT_(m_scans_res == 25 || m_scans_res == 50 || m_scans_res == 100);
740 
741  auto* COM = dynamic_cast<CSerialPort*>(m_stream);
742  ASSERT_(COM);
743 
744  uint8_t cmd[40];
745 
746  // Config angle/resolution
747  cmd[0] = 0x3B;
748  cmd[1] = m_scans_FOV;
749  cmd[2] = 0x00;
750  cmd[3] = m_scans_res; // 25,50 or 100 - 1/100th of deg
751  cmd[4] = 0x00;
752  uint16_t cmd_len = 5;
753  if (!SendCommandToSICK(cmd, cmd_len))
754  RET_ERROR("Error waiting ack for change angle/resolution");
755  if (!LMS_waitIncomingFrame(500))
756  RET_ERROR("Error waiting answer for change angle/resolution");
757 
758  // Start continuous mode:
759  cmd[0] = 0x20;
760  cmd[1] = 0x24;
761  cmd_len = 2;
762  if (!SendCommandToSICK(cmd, cmd_len))
763  RET_ERROR("Error waiting ack for start scanning");
764  if (!LMS_waitIncomingFrame(500))
765  RET_ERROR("Error waiting answer for start scanning");
766 
767  return true;
768 }
769 
771 {
772  auto* COM = dynamic_cast<CSerialPort*>(m_stream);
773  ASSERT_(COM);
774 
775  uint8_t cmd[40];
776 
777  // End continuous mode:
778  cmd[0] = 0x20;
779  cmd[1] = 0x25;
780  uint16_t cmd_len = 2;
781  if (!SendCommandToSICK(cmd, cmd_len)) return false;
782  return LMS_waitIncomingFrame(50);
783 }
784 
786  const uint8_t* cmd, const uint16_t cmd_len)
787 {
788  uint8_t cmd_full[1024];
789  ASSERT_(sizeof(cmd_full) > cmd_len + 4U + 2U);
790 
791  auto* COM = dynamic_cast<CSerialPort*>(m_stream);
792  ASSERT_(COM);
793 
794  // Create header
795  cmd_full[0] = 0x02; // STX
796  cmd_full[1] = 0; // ADDR
797  cmd_full[2] = cmd_len & 0xFF;
798  cmd_full[3] = cmd_len >> 8;
799 
800  memcpy(cmd_full + 4, cmd, cmd_len);
801 
802  const uint16_t crc =
803  mrpt::system::compute_CRC16(cmd_full, 4 + cmd_len, CRC16_GEN_POL);
804  cmd_full[4 + cmd_len + 0] = crc & 0xFF;
805  cmd_full[4 + cmd_len + 1] = crc >> 8;
806 
807  const size_t toWrite = 4 + cmd_len + 2;
808 
809  if (m_verbose)
810  {
811  printf("[CSickLaserSerial::SendCommandToSICK] TX: ");
812  for (unsigned int i = 0; i < toWrite; i++) printf("%02X ", cmd_full[i]);
813  printf("\n");
814  }
815 
816  const int NTRIES = 3;
817 
818  for (int k = 0; k < NTRIES; k++)
819  {
820  if (toWrite != COM->Write(cmd_full, toWrite))
821  {
822  cout << "[CSickLaserSerial::SendCommandToSICK] Error writing data "
823  "to serial port."
824  << endl;
825  return false;
826  }
827  std::this_thread::sleep_for(15ms);
828  if (LMS_waitACK(50)) return true;
829  std::this_thread::sleep_for(10ms);
830  }
831 
832  return false;
833 }
double Tac() noexcept
Stops the stopwatch.
Definition: CTicTac.cpp:86
GLenum GLint GLuint mask
Definition: glext.h:4062
bool LMS_sendMeasuringMode_cm_mm()
Returns false on error.
unsigned __int16 uint16_t
Definition: rptypes.h:47
std::string read_string(const std::string &section, const std::string &name, const std::string &defaultValue, bool failIfNotFound=false) const
#define MRPT_LOG_ERROR_FMT(_FMT_STRING,...)
A communications serial port built as an implementation of a utils::CStream.
Definition: CSerialPort.h:41
void initialize() override
Set-up communication with the laser.
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
void internal_notifyGoodScanNow()
Must be called from doProcessSimple() implementations.
void setScanRange(const size_t i, const float val)
std::string m_sensorLabel
See CGenericSensor.
double DEG2RAD(const double x)
Degrees to radians.
bool LMS_statusQuery()
Send a status query and wait for the answer.
A high-performance stopwatch, with typical resolution of nanoseconds.
mrpt::system::TTimeStamp now()
A shortcut for system::getCurrentTime.
Definition: datetime.h:86
Contains classes for various device interfaces.
void loadConfig_sensorSpecific(const mrpt::config::CConfigFileBase &configSource, const std::string &iniSection) override
See the class documentation at the top for expected parameters.
float read_float(const std::string &section, const std::string &name, float defaultValue, bool failIfNotFound=false) const
STL namespace.
int m_com_baudRate
Baudrate: 9600, 38400, 500000.
void filterByExclusionAreas(mrpt::obs::CObservation2DRangeScan &obs) const
Mark as invalid those points which (x,y) coordinates fall within the exclusion polygons.
float stdError
The "sigma" error of the device in meters, used while inserting the scan in an occupancy grid...
GLdouble s
Definition: glext.h:3682
void filterByExclusionAngles(mrpt::obs::CObservation2DRangeScan &obs) const
Mark as invalid those ranges in a set of forbiden angle ranges.
bool turnOn() override
Enables the scanning mode (in this class this has no effect).
bool LMS_setupSerialComms()
Assures laser is connected and operating at 38400, in its case returns true.
mrpt::io::CStream * m_stream
The I/O channel (will be nullptr if not bound).
#define RET_ERROR(msg)
int read_int(const std::string &section, const std::string &name, int defaultValue, bool failIfNotFound=false) const
unsigned char uint8_t
Definition: rptypes.h:44
float maxRange
The maximum range allowed by the device, in meters (e.g.
bool LMS_setupBaudrate(int baud)
Send a command to change the LMS comms baudrate, return true if ACK is OK.
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
std::string m_com_port
If set to non-empty, the serial port will be attempted to be opened automatically when this class is ...
This class allows loading and storing values and vectors of different types from a configuration text...
This base provides a set of functions for maths stuff.
bool m_skip_laser_config
If true, doesn&#39;t send the initialization commands to the laser and go straight to capturing...
mrpt::containers::ContainerReadOnlyProxyAccessor< mrpt::aligned_std_vector< float > > scan
The range values of the scan, in meters.
bool waitContinuousSampleFrame(std::vector< float > &ranges, unsigned char &LMS_status, bool &is_mm_mode)
void setSerialPortName(const std::string &COM_name)
Sets the serial port to open (it is an error to try to change this while open yet).
Definition: CSerialPort.cpp:77
This namespace contains representation of robot actions and observations.
#define M_PIf
Definition: common.h:61
GLubyte GLubyte b
Definition: glext.h:6372
bool tryToOpenComms(std::string *err_msg=nullptr)
Tries to open the com port and setup all the LMS protocol.
bool LMS_waitIncomingFrame(uint16_t timeout)
Returns false if timeout.
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
Definition: exceptions.h:108
mrpt::math::TPose3D m_sensorPose
The sensor 6D pose:
GLsizei const GLchar ** string
Definition: glext.h:4116
#define IMPLEMENTS_GENERIC_SENSOR(class_name, NameSpace)
This must be inserted in all CGenericSensor classes implementation files:
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
int m_scans_res
1/100th of deg: 100, 50 or 25
std::string sensorLabel
An arbitrary label that can be used to identify the sensor.
Definition: CObservation.h:62
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
bool SendCommandToSICK(const uint8_t *cmd, const uint16_t cmd_len)
Send header+command-data+crc and waits for ACK.
mrpt::system::TTimeStamp timestamp
The associated UTC time-stamp.
Definition: CObservation.h:60
A "CObservation"-derived class that represents a 2D range scan measurement (typically from a laser sc...
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:84
bool internal_notifyNoScanReceived()
Must be called from doProcessSimple() implementations.
#define MRPT_LOG_ERROR(_STRING)
void resizeScan(const size_t len)
Resizes all data vectors to allocate a given number of scan rays.
bool read_bool(const std::string &section, const std::string &name, bool defaultValue, bool failIfNotFound=false) const
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:16
This "software driver" implements the communication protocol for interfacing a SICK LMS 2XX laser sca...
bool turnOff() override
Disables the scanning mode (in this class this has no effect).
void loadCommonParams(const mrpt::config::CConfigFileBase &configSource, const std::string &iniSection)
Should be call by derived classes at "loadConfig" (loads exclusion areas AND exclusion angles)...
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
Definition: TPose3D.h:23
void processPreview(const mrpt::obs::CObservation2DRangeScan &obs)
Must be called inside the capture method to allow optional GUI preview of scans.
float aperture
The "aperture" or field-of-view of the range finder, in radians (typically M_PI = 180 degrees)...
unsigned int m_nTries_connect
Default = 1.
GLuint res
Definition: glext.h:7385
uint16_t compute_CRC16(const std::vector< uint8_t > &data, const uint16_t gen_pol=0x8005)
Computes the CRC16 checksum of a block of data.
Definition: crc.cpp:15
Serial and networking devices and utilities.
void Tic() noexcept
Starts the stopwatch.
Definition: CTicTac.cpp:75
mrpt::comms::CSerialPort * m_mySerialPort
Will be !=nullptr only if I created it, so I must destroy it at the end.
void doProcessSimple(bool &outThereIsObservation, mrpt::obs::CObservation2DRangeScan &outObservation, bool &hardwareError) override
Specific laser scanner "software drivers" must process here new data from the I/O stream...
mrpt::poses::CPose3D sensorPose
The 6D pose of the sensor on the robot at the moment of starting the scan.
bool LMS_waitACK(uint16_t timeout_ms)
Returns false if timeout.
~CSickLaserSerial() override
Destructor.
bool rightToLeft
The scanning direction: true=counterclockwise; false=clockwise.
void memcpy(void *dest, size_t destSize, const void *src, size_t copyCount) noexcept
An OS and compiler independent version of "memcpy".
Definition: os.cpp:358
void setScanRangeValidity(const size_t i, const bool val)



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 8fe78517f Sun Jul 14 19:43:28 2019 +0200 at dom jul 14 20:00:11 CEST 2019