Debian Codegraph @

libs/hwdrivers/src/CSickLaserSerial.cpp

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