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 
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>();
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 }
std::shared_ptr< mrpt::comms::CSerialPort > m_mySerialPort
Will be !=nullptr only if I created it, so I must destroy it at the end.
double Tac() noexcept
Stops the stopwatch.
Definition: CTicTac.cpp:86
bool LMS_sendMeasuringMode_cm_mm()
Returns false on error.
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
std::string std::string format(std::string_view fmt, ARGS &&... args)
Definition: format.h:26
void internal_notifyGoodScanNow()
Must be called from doProcessSimple() implementations.
void setScanRange(const size_t i, const float val)
std::string m_sensorLabel
See CGenericSensor.
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...
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.
#define RET_ERROR(msg)
int read_int(const std::string &section, const std::string &name, int defaultValue, bool failIfNotFound=false) const
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...
bool waitContinuousSampleFrame(std::vector< float > &ranges, unsigned char &LMS_status, bool &is_mm_mode)
constexpr double DEG2RAD(const double x)
Degrees to radians.
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
This namespace contains representation of robot actions and observations.
#define M_PIf
Definition: common.h:61
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:
#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...
std::shared_ptr< mrpt::io::CStream > m_stream
The I/O channel (will be nullptr if not bound).
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
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
This "software driver" implements the communication protocol for interfacing a SICK LMS 2XX laser sca...
const float & getScanRange(const size_t i) const
The range values of the scan, in meters.
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:24
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.
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
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: d1962bc6a Wed Jan 15 17:38:30 2020 +0100 at miƩ ene 15 17:45:11 CET 2020