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



Page generated by Doxygen 1.8.17 for MRPT 2.0.4 at Sun Jul 19 17:54:30 UTC 2020