Debian Codegraph @

libs/hwdrivers/src/CHokuyoURG.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 #include "hwdrivers-precomp.h" // Precompiled headers
11 //
12 #include <mrpt/comms/CClientTCPSocket.h>
13 #include <mrpt/comms/CSerialPort.h>
14 #include <mrpt/hwdrivers/CHokuyoURG.h>
15 #include <mrpt/opengl/CAxis.h>
16 #include <mrpt/opengl/CPlanarLaserScan.h> // in library mrpt-maps
17 #include <mrpt/system/CTimeLogger.h>
18 #include <mrpt/system/os.h>
19
20 #include <chrono>
21 #include <thread>
22
23 IMPLEMENTS_GENERIC_SENSOR(CHokuyoURG, mrpt::hwdrivers)
24
25 using namespace mrpt::obs;
26 using namespace mrpt::hwdrivers;
27 using namespace mrpt::comms;
28 using namespace mrpt::system;
29 using namespace mrpt::opengl;
30 using namespace std;
31
32 const int MINIMUM_PACKETS_TO_SET_TIMESTAMP_REFERENCE = 10;
33
34 CHokuyoURG::CHokuyoURG() : m_rx_buffer(40000) { m_sensorLabel = "Hokuyo"; }
35
36 CHokuyoURG::~CHokuyoURG()
37 {
38 try
39 {
40 m_win.reset();
41 closeStreamConnection();
42 }
43 catch (const std::exception& e)
44 {
45 std::cerr << "[~CHokuyoURG] Exception:\n" << mrpt::exception_to_str(e);
46 }
47 }
48
49 void CHokuyoURG::closeStreamConnection()
50 {
51 if (m_stream)
52 {
53 turnOff();
54 m_stream.reset();
55 }
56 }
57
58 void CHokuyoURG::sendCmd(const char* str)
59 {
60 MRPT_START
61 ASSERT_(str != nullptr);
62 ASSERT_(m_stream != nullptr);
63 const size_t N = strlen(str);
64 const size_t nWriten = m_stream->Write(str, N);
65 ASSERT_EQUAL_(nWriten, N);
66
67 MRPT_LOG_DEBUG_STREAM("[Hokuyo] sendCmd(): `" << str << "`");
68
69 m_lastSentMeasCmd = std::string(str); // for echo verification
70 MRPT_END
71 }
72
73 void CHokuyoURG::doProcessSimple(
74 bool& outThereIsObservation,
75 mrpt::obs::CObservation2DRangeScan& outObservation,
76 bool& hardwareError)
77 {
78 #if 0
79 static mrpt::system::CTimeLogger timLogger("CHokuyoURG");
80 mrpt::system::CTimeLoggerEntry tle(timLogger, "doProcessSimple");
81 #endif
82
83 outThereIsObservation = false;
84 hardwareError = false;
85
86 // Bound? If not, connect to the device and set it up (again) in
87 // continuous read mode:
88 if (!ensureStreamIsOpen())
89 {
90 m_timeStartUI = 0;
91 m_timeStartSynchDelay = 0;
92 hardwareError = true;
93 return;
94 }
95
96 // Wait for a message:
97 int nRanges = m_lastRange - m_firstRange + 1;
98 int expectedSize = nRanges * 3 + 4;
99 if (m_intensity) expectedSize += nRanges * 3;
100
101 m_rcv_data.clear();
102 m_rcv_data.reserve(expectedSize + 1000);
103
104 m_state = ssWorking;
105 if (!parseResponse(false))
106 {
107 if (!internal_notifyNoScanReceived())
108 {
109 // It seems the sensor needs to be reset (?), let this know
110 // to the caller:
111 m_state = ssError;
112 hardwareError = true;
113
114 // And on our side, close the connection to ensure initialization
115 // is called again to set-up the laser in the next call to
116 // ensureStreamIsOpen() above.
117 closeStreamConnection();
118
119 return;
120 }
121
122 // No new data
123 return;
124 }
125
126 // DECODE:
127 if (m_rcv_status0 != '0' && m_rcv_status0 != '9')
128 {
129 hardwareError = true;
130 return;
131 }
132
133 // -----------------------------------------------
134 // Extract the observation:
135 // -----------------------------------------------
136 outObservation.timestamp = mrpt::Clock::now();
137
138 if ((size_t)expectedSize != m_rcv_data.size())
139 {
140 MRPT_LOG_ERROR_STREAM(
141 "[CHokuyoURG::doProcess] ERROR: Expected " << expectedSize << " data bytes, received "
142 << m_rcv_data.size() << "instead!");
143 hardwareError = true;
144 return;
145 }
146 // Delay the sync of timestamps due to instability in the constant rate
147 // during the first few packets.
148 bool do_timestamp_sync = !m_disable_firmware_timestamp;
149 if (do_timestamp_sync && m_timeStartSynchDelay < MINIMUM_PACKETS_TO_SET_TIMESTAMP_REFERENCE)
150 {
151 do_timestamp_sync = false;
152 m_timeStartSynchDelay++;
153 }
154
155 if (do_timestamp_sync)
156 {
157 // Extract the timestamp of the sensor:
158 uint32_t nowUI = ((m_rcv_data[0] - 0x30) << 18) + ((m_rcv_data[1] - 0x30) << 12) +
159 ((m_rcv_data[2] - 0x30) << 6) + (m_rcv_data[3] - 0x30);
160
161 uint32_t AtUI = 0;
162 if (m_timeStartUI == 0)
163 {
164 m_timeStartUI = nowUI;
165 m_timeStartTT = mrpt::Clock::now();
166 }
167 else
168 AtUI = nowUI - m_timeStartUI;
169
170 auto AtDO = std::chrono::milliseconds(AtUI);
171 outObservation.timestamp = m_timeStartTT + AtDO;
172 }
173
174 // And the scan ranges:
175 outObservation.rightToLeft = true;
176
177 outObservation.aperture = nRanges * 2 * M_PI / m_sensor_info.scans_per_360deg;
178
179 outObservation.maxRange = m_sensor_info.d_max;
180 outObservation.stdError = 0.010f;
181 outObservation.sensorPose = m_sensorPose;
182 outObservation.sensorLabel = m_sensorLabel;
183
184 outObservation.resizeScan(nRanges);
185 char* ptr = (char*)&m_rcv_data[4];
186
187 if (m_intensity) outObservation.setScanHasIntensity(true);
188
189 for (int i = 0; i < nRanges; i++)
190 {
191 int b1 = (*ptr++) - 0x30;
192 int b2 = (*ptr++) - 0x30;
193 int b3 = (*ptr++) - 0x30;
194
195 int range_mm = ((b1 << 12) | (b2 << 6) | b3);
196
197 outObservation.setScanRange(i, range_mm * 0.001f);
198 outObservation.setScanRangeValidity(
199 i, range_mm >= 20 && (outObservation.getScanRange(i) <= outObservation.maxRange));
200
201 if (m_intensity)
202 {
203 int b4 = (*ptr++) - 0x30;
204 int b5 = (*ptr++) - 0x30;
205 int b6 = (*ptr++) - 0x30;
206 outObservation.setScanIntensity(i, ((b4 << 12) | (b5 << 6) | b6));
207 }
208 }
209
210 // Do filter:
211 C2DRangeFinderAbstract::filterByExclusionAreas(outObservation);
212 C2DRangeFinderAbstract::filterByExclusionAngles(outObservation);
213 // Do show preview:
214 C2DRangeFinderAbstract::processPreview(outObservation);
215
216 outThereIsObservation = true;
217 internal_notifyGoodScanNow();
218 }
219
220 /*-------------------------------------------------------------
221 loadConfig_sensorSpecific
222 -------------------------------------------------------------*/
223 void CHokuyoURG::loadConfig_sensorSpecific(
224 const mrpt::config::CConfigFileBase& c, const std::string& s)
225 {
226 m_reduced_fov = DEG2RAD(c.read_float(s, "reduced_fov", 0)),
227
228 m_motorSpeed_rpm = c.read_int(s, "HOKUYO_motorSpeed_rpm", 0);
229 m_sensorPose.setFromValues(
230 c.read_float(s, "pose_x", 0), c.read_float(s, "pose_y", 0), c.read_float(s, "pose_z", 0),
231 DEG2RAD(c.read_float(s, "pose_yaw", 0)), DEG2RAD(c.read_float(s, "pose_pitch", 0)),
232 DEG2RAD(c.read_float(s, "pose_roll", 0)));
233
234 m_highSensMode = c.read_bool(s, "HOKUYO_HS_mode", m_highSensMode);
235
236 #ifdef _WIN32
237 const char* comVarName = "COM_port_WIN";
238 #else
239 const char* comVarName = "COM_port_LIN";
240 #endif
241
242 m_com_port = c.read_string(s, comVarName, m_com_port);
243 m_ip_dir = c.read_string(s, "IP_DIR", m_ip_dir);
244 m_port_dir = c.read_int(s, "PORT_DIR", m_port_dir);
245
246 ASSERTMSG_(
247 !m_com_port.empty() || !m_ip_dir.empty(),
248 "Either COM_port or IP_DIR must be defined in the configuration file!");
249 ASSERTMSG_(
250 m_com_port.empty() || m_ip_dir.empty(),
251 "Both COM_port and IP_DIR set! Please, define only one of them.");
252 if (!m_ip_dir.empty())
253 {
254 ASSERTMSG_(
255 m_port_dir,
256 "A TCP/IP port number `PORT_DIR` must be specified for Ethernet "
257 "connection");
258 }
259
260 m_disable_firmware_timestamp =
261 c.read_bool(s, "disable_firmware_timestamp", m_disable_firmware_timestamp);
262 m_intensity = c.read_bool(s, "intensity", m_intensity),
263
264 MRPT_LOAD_HERE_CONFIG_VAR(scan_interval, int, m_scan_interval, c, s);
265 MRPT_LOAD_HERE_CONFIG_VAR(comms_timeout_ms, int, m_comms_timeout_ms, c, s);
266 MRPT_LOAD_HERE_CONFIG_VAR(comms_between_timeout_ms, int, m_comms_between_timeout_ms, c, s);
267
268 // Parent options:
269 C2DRangeFinderAbstract::loadCommonParams(c, s);
270 }
271
272 /*-------------------------------------------------------------
273 turnOn
274 -------------------------------------------------------------*/
275 bool CHokuyoURG::turnOn()
276 {
277 MRPT_START
278
279 // Bound?
280 if (!ensureStreamIsOpen()) return false;
281
282 // If we are over a serial link, set it up:
283 if (m_ip_dir.empty())
284 {
285 auto* COM = dynamic_cast<CSerialPort*>(m_stream.get());
286
287 if (COM != nullptr)
288 {
289 // It is a COM:
290 COM->setConfig(19200);
291 COM->setTimeouts(m_comms_timeout_ms, 0, m_comms_timeout_ms, 0, 0);
292
293 // Assure the laser is off and quiet:
294 switchLaserOff();
295 std::this_thread::sleep_for(10ms);
296
297 COM->purgeBuffers();
298 std::this_thread::sleep_for(10ms);
299
300 COM->setConfig(115200);
301 switchLaserOff();
302 std::this_thread::sleep_for(10ms);
303 COM->purgeBuffers();
304 std::this_thread::sleep_for(10ms);
305 COM->setConfig(19200);
306 }
307
308 if (COM != nullptr)
309 {
310 // Set 115200 baud rate:
311 setHighBaudrate();
312 enableSCIP20();
313 COM->setConfig(115200);
314 }
315 }
316 else
317 {
318 auto* COM = dynamic_cast<CClientTCPSocket*>(m_stream.get());
319
320 if (COM != nullptr)
321 {
322 // Assure the laser is off and quiet:
323 switchLaserOff();
324 std::this_thread::sleep_for(10ms);
325
326 purgeBuffers();
327 std::this_thread::sleep_for(10ms);
328
329 switchLaserOff();
330 std::this_thread::sleep_for(10ms);
331 purgeBuffers();
332 }
333 }
334
335 if (!enableSCIP20()) return false;
336
337 // Turn on the laser:
338 if (!switchLaserOn()) return false;
339
340 // Set the motor speed:
341 if (m_motorSpeed_rpm)
342 if (!setMotorSpeed(m_motorSpeed_rpm)) return false;
343
344 // Set HS mode:
345 setHighSensitivityMode(m_highSensMode);
346
347 // Display sensor information:
348 if (!displaySensorInfo(&m_sensor_info)) return false;
349
350 // Set for scanning angles:
351 m_firstRange = m_sensor_info.scan_first;
352 m_lastRange = m_sensor_info.scan_last;
353
354 // Artificially reduced FOV?
355 if (m_reduced_fov > 0 && m_reduced_fov < 2 * M_PI)
356 {
357 int center = (m_lastRange + m_firstRange) >> 1;
358 const int half_range =
359 static_cast<int>((m_sensor_info.scans_per_360deg / 360.0) * RAD2DEG(m_reduced_fov)) >> 1;
360 m_firstRange = center - half_range;
361 m_lastRange = center + half_range;
362 MRPT_LOG_INFO_STREAM(
363 "[HOKUYO::turnOn] Using reduced FOV: ranges ["
364 << m_firstRange << "-" << m_lastRange << "] for " << RAD2DEG(m_reduced_fov) << " deg. FOV");
365 }
366
367 if (!displayVersionInfo())
368 {
369 // return false; // It's not SO important
370 }
371
372 // Start!
373 if (!startScanningMode()) return false;
374
375 return true;
376
377 MRPT_END
378 }
379
380 /*-------------------------------------------------------------
381 turnOff
382 -------------------------------------------------------------*/
383 bool CHokuyoURG::turnOff()
384 {
385 // Turn off the laser:
386 if (!switchLaserOff()) return false;
387
388 return true;
389 }
390
391 bool CHokuyoURG::setHighBaudrate()
392 {
393 if (!ensureStreamIsOpen()) return false;
394
395 MRPT_LOG_DEBUG("[CHokuyoURG::setHighBaudrate] Changing baudrate to 115200...");
396
397 // Send command:
398 sendCmd("SS115200\x0A");
399
400 // Receive response:
401 if (!parseResponse())
402 {
403 MRPT_LOG_ERROR("[CHokuyoURG::setHighBaudrate] Error waiting for response");
404 return false;
405 }
406
407 MRPT_LOG_DEBUG("OK\n");
408 return true;
409 }
410
411 /*-------------------------------------------------------------
412 ensureBufferHasBytes
413 -------------------------------------------------------------*/
414 bool CHokuyoURG::ensureBufferHasBytes(size_t nDesiredBytes, bool additionalWaitForData)
415 {
416 ASSERT_LT_(nDesiredBytes, m_rx_buffer.capacity());
417
418 if (m_rx_buffer.size() >= nDesiredBytes) return true;
419
420 // Try to read more bytes:
421 std::array<uint8_t, 512> buf;
422 const size_t to_read = std::min(m_rx_buffer.available(), buf.size());
423
424 try
425 {
426 const size_t nRead = [this, &buf, additionalWaitForData, to_read]()
427 {
428 if (auto sock = dynamic_cast<CClientTCPSocket*>(m_stream.get()); sock)
429 {
430 // socket:
431 auto timeout = m_comms_timeout_ms;
432 if (additionalWaitForData) mrpt::keep_max(timeout, 100);
433
434 return sock->readAsync(&buf[0], to_read, timeout, m_comms_between_timeout_ms);
435 }
436 else
437 {
438 // serial port:
439 return m_stream->Read(&buf[0], to_read);
440 }
441 }();
442
443 m_rx_buffer.push_many(&buf[0], nRead);
444 }
445 catch (std::exception&)
446 {
447 // 0 bytes read
448 }
449
450 return (m_rx_buffer.size() >= nDesiredBytes);
451 }
452
453 bool CHokuyoURG::parseResponse(bool additionalWaitForData)
454 {
455 m_rcv_status0 = '\0';
456 m_rcv_status1 = '\0';
457 m_rcv_data.clear();
458
459 if (!ensureStreamIsOpen()) return false;
460 ASSERT_(!m_lastSentMeasCmd.empty());
461
462 try
463 {
464 // Process response:
465 // ---------------------------------
466 char tmp_rcv_status0 = '\0', tmp_rcv_status1 = '\0';
467
468 // COMMAND ECHO ---------
469 size_t peekIdx = 0;
470 const unsigned int verifLen = m_lastSentMeasCmd.size();
471
472 if (verifLen)
473 {
474 unsigned int i = 0;
475 do
476 {
477 if (!ensureBufferHasBytes(verifLen - i, additionalWaitForData)) return false;
478
479 // If matches the echo, go on:
480 if (m_rx_buffer.peek(peekIdx++) == m_lastSentMeasCmd[i])
481 {
482 // Match is ok:
483 i++;
484 }
485 else
486 {
487 // Skip one byte and keep trying to match:
488 m_rx_buffer.pop();
489 i = 0;
490 peekIdx = 0;
491 }
492 } while (i < verifLen);
493 }
494
495 // Now, the status bytes:
496 if (!ensureBufferHasBytes(peekIdx + 2, additionalWaitForData)) return false;
497
498 tmp_rcv_status0 = m_rx_buffer.peek(peekIdx++);
499 tmp_rcv_status1 = m_rx_buffer.peek(peekIdx++);
500
501 // In SCIP2.0, there is an additional sum char:
502 if (tmp_rcv_status1 != 0x0A)
503 {
504 // Yes, it is SCIP2.0
505 if (!ensureBufferHasBytes(peekIdx + 1, additionalWaitForData)) return false;
506 // Ignore this byte: sumStatus
507 peekIdx++;
508 }
509 else
510 {
511 // Continue, it seems a SCIP1.1 response...
512 }
513
514 // After the status bytes, there must be a LF:
515 if (!ensureBufferHasBytes(peekIdx + 1, additionalWaitForData)) return false;
516 char nextChar = m_rx_buffer.peek(peekIdx++);
517 if (nextChar != 0x0A) return false;
518
519 // -----------------------------------------------------------------------------
520 // Now the data:
521 // There's a problem here, we don't know in advance how many bytes to
522 // read,
523 // so rely on the serial port class implemented buffer and call many
524 // times
525 // the read method with only 1 byte each time:
526 // -----------------------------------------------------------------------------
527 bool lastWasLF = false;
528 std::string tmp_rx;
529 for (;;)
530 {
531 if (!ensureBufferHasBytes(peekIdx + 1, additionalWaitForData))
532 {
533 // Do not empty the queue, it seems we need to wait for more
534 // data:
535 return false;
536 }
537 tmp_rx.push_back(m_rx_buffer.peek(peekIdx++));
538
539 // No data?
540 if (tmp_rx.size() == 1 && tmp_rx[0] == 0x0A)
541 {
542 tmp_rx.clear();
543 m_rcv_status0 = tmp_rcv_status0;
544 m_rcv_status1 = tmp_rcv_status1;
545 // Empty read bytes so far:
546 for (size_t k = 0; k < peekIdx; k++) m_rx_buffer.pop();
547 return true;
548 }
549
550 // Is it a LF?
551 if (*tmp_rx.rbegin() != 0x0A)
552 {
553 lastWasLF = false;
554 continue;
555 }
556
557 // This was a single LF:
558 if (!lastWasLF)
559 {
560 // Discard SUM+LF
561 ASSERT_(tmp_rx.size() >= 2);
562 tmp_rx.resize(tmp_rx.size() - 2);
563 }
564 else
565 {
566 // This was a double LF.
567
568 // Discard this last LF:
569 tmp_rx.resize(tmp_rx.size() - 1);
570
571 // Done!
572 m_rcv_data = tmp_rx;
573 m_rcv_status0 = tmp_rcv_status0;
574 m_rcv_status1 = tmp_rcv_status1;
575
576 // Empty read bytes so far:
577 for (size_t k = 0; k < peekIdx; k++) m_rx_buffer.pop();
578
579 MRPT_LOG_DEBUG_STREAM("[Hokuyo] parseResponse(): RX `" << m_rcv_data << "`");
580
581 if (m_rcv_status0 != '0' && (m_rcv_status0 != '9' && m_rcv_status1 != '9'))
582 {
583 MRPT_LOG_ERROR_STREAM(
584 "[Hokuyo] Error LIDAR status: " << (int)m_rcv_status0 << " after command: `"
585 << m_lastSentMeasCmd << "`");
586 return false;
587 }
588 return true;
589 }
590 lastWasLF = true;
591 }
592 }
593 catch (const std::exception& e)
594 {
595 MRPT_LOG_ERROR_FMT("[Hokuyo] parseResponse() Exception: %s", e.what());
596 return false;
597 }
598 catch (...)
599 {
600 return false; // Serial port timeout,...
601 }
602 }
603
604 bool CHokuyoURG::enableSCIP20()
605 {
606 if (!ensureStreamIsOpen()) return false;
607
608 MRPT_LOG_DEBUG("[CHokuyoURG::enableSCIP20] Changing protocol to SCIP2.0...");
609
610 // Send command:
611 sendCmd("SCIP2.0\x0A");
612
613 // Receive response:
614 if (!parseResponse())
615 {
616 MRPT_LOG_ERROR_STREAM(__CURRENT_FUNCTION_NAME__ << ": Error in response");
617 return false;
618 }
619
620 MRPT_LOG_DEBUG("OK\n");
621 return true;
622 }
623
624 bool CHokuyoURG::switchLaserOn()
625 {
626 if (!ensureStreamIsOpen()) return false;
627
628 MRPT_LOG_DEBUG("[CHokuyoURG::switchLaserOn] Switching laser ON...");
629
630 // Send command:
631 sendCmd("BM\x0A");
632
633 // Receive response:
634 if (!parseResponse())
635 {
636 MRPT_LOG_ERROR_STREAM(__CURRENT_FUNCTION_NAME__ << ": Error in response");
637 return false;
638 }
639
640 MRPT_LOG_DEBUG("OK\n");
641 return true;
642 }
643
644 bool CHokuyoURG::switchLaserOff()
645 {
646 if (!ensureStreamIsOpen()) return false;
647
648 MRPT_LOG_DEBUG("[CHokuyoURG::switchLaserOff] Switching laser OFF...");
649
650 // Send command:
651 sendCmd("QT\x0A");
652
653 // Receive response:
654 if (!parseResponse())
655 {
656 MRPT_LOG_ERROR_STREAM(__CURRENT_FUNCTION_NAME__ << ": Error in response");
657 return false;
658 }
659
660 MRPT_LOG_DEBUG("OK\n");
661 return true;
662 }
663
664 void CHokuyoURG::setScanInterval(unsigned int skipScanCount) { m_scan_interval = skipScanCount; }
665 unsigned int CHokuyoURG::getScanInterval() const { return m_scan_interval; }
666 bool CHokuyoURG::setMotorSpeed(int motoSpeed_rpm)
667 {
668 if (!ensureStreamIsOpen()) return false;
669
670 MRPT_LOG_DEBUG_FMT("[CHokuyoURG::setMotorSpeed] Setting to %i rpm...", motoSpeed_rpm);
671
672 // Send command:
673 int motorSpeedCode = (600 - motoSpeed_rpm) / 6;
674 if (motorSpeedCode < 0 || motorSpeedCode > 10)
675 {
676 MRPT_LOG_ERROR_STREAM(
677 __CURRENT_FUNCTION_NAME__ << " Motorspeed must be in the range 540-600 rpm");
678 return false;
679 }
680
681 char cmd[20];
682 os::sprintf(cmd, 20, "CR%02i\x0A", motorSpeedCode);
683 sendCmd(cmd);
684
685 // Receive response:
686 if (!parseResponse())
687 {
688 MRPT_LOG_ERROR_STREAM(__CURRENT_FUNCTION_NAME__ << ": Error in response");
689 return false;
690 }
691
692 MRPT_LOG_DEBUG("OK\n");
693 return true;
694 }
695
696 /*-------------------------------------------------------------
697 setHighSensitivityMode
698 -------------------------------------------------------------*/
699 bool CHokuyoURG::setHighSensitivityMode(bool enabled)
700 {
701 if (!ensureStreamIsOpen()) return false;
702
703 MRPT_LOG_DEBUG_FMT(
704 "[CHokuyoURG::setHighSensitivityMode] Setting HS mode to: %s...", enabled ? "true" : "false");
705
706 // Send command:
707 char cmd[20];
708 os::sprintf(cmd, 20, "HS%i\x0A", enabled ? 1 : 0);
709 sendCmd(cmd);
710
711 // Receive response:
712 if (!parseResponse())
713 {
714 MRPT_LOG_ERROR_STREAM(__CURRENT_FUNCTION_NAME__ << ": Error in response");
715 return false;
716 }
717
718 MRPT_LOG_DEBUG("OK\n");
719 return true;
720 }
721
722 /*-------------------------------------------------------------
723 setIntensityMode
724 -------------------------------------------------------------*/
725 bool CHokuyoURG::setIntensityMode(bool enabled)
726 {
727 m_intensity = enabled;
728 return true;
729 }
730
731 bool CHokuyoURG::displayVersionInfo()
732 {
733 if (!ensureStreamIsOpen()) return false;
734
735 MRPT_LOG_DEBUG("[CHokuyoURG::displayVersionInfo] Asking info...");
736
737 // Send command:
738 sendCmd("VV\x0A");
739
740 // Receive response:
741 if (!parseResponse())
742 {
743 MRPT_LOG_ERROR_STREAM(__CURRENT_FUNCTION_NAME__ << ": Error in response");
744 return false;
745 }
746
747 MRPT_LOG_DEBUG("OK\n");
748
749 // PRINT:
750 for (auto& c : m_rcv_data)
751 if (c == ';') c = '\n';
752 m_rcv_data[m_rcv_data.size()] = '\0';
753
754 if (!m_rcv_data.empty())
755 {
756 MRPT_LOG_INFO_STREAM(
757 "\n------------- HOKUYO Scanner: Version Information ------\n"
758 << &m_rcv_data[0]
759 << "\n"
760 "-------------------------------------------------------\n\n");
761 }
762 return true;
763 }
764
765 /*-------------------------------------------------------------
766 displaySensorInfo
767 -------------------------------------------------------------*/
768 bool CHokuyoURG::displaySensorInfo(TSensorInfo* out_data)
769 {
770 if (!ensureStreamIsOpen()) return false;
771
772 MRPT_LOG_DEBUG("[CHokuyoURG::displaySensorInfo] Asking for info...");
773
774 // Send command:
775 sendCmd("PP\x0A");
776
777 // Receive response:
778 if (!parseResponse())
779 {
780 MRPT_LOG_ERROR_STREAM(__CURRENT_FUNCTION_NAME__ << ": Error in response");
781 return false;
782 }
783 MRPT_LOG_DEBUG("OK\n");
784
785 // PRINT:
786 for (auto& c : m_rcv_data)
787 if (c == ';') c = '\n';
788 m_rcv_data[m_rcv_data.size()] = '\0';
789
790 if (!m_rcv_data.empty())
791 {
792 MRPT_LOG_INFO_STREAM(
793 "\n------------- HOKUYO Scanner: Product Information ------\n"
794 << &m_rcv_data[0]
795 << "\n"
796 "-------------------------------------------------------\n\n");
797 }
798
799 // Parse the data:
800 if (out_data)
801 {
802 const char* ptr;
803
804 if (nullptr != (ptr = strstr(&m_rcv_data[0], "DMAX:")))
805 out_data->d_max = 0.001 * atoi(ptr + 5);
806 else
807 MRPT_LOG_ERROR("[Hokuyo] displayVersionInfo() parse error");
808
809 if (nullptr != (ptr = strstr(&m_rcv_data[0], "DMIN:")))
810 out_data->d_min = 0.001 * atoi(ptr + 5);
811 else
812 MRPT_LOG_ERROR("[Hokuyo] displayVersionInfo() parse error");
813
814 if (nullptr != (ptr = strstr(&m_rcv_data[0], "ARES:")))
815 out_data->scans_per_360deg = atoi(ptr + 5);
816 else
817 MRPT_LOG_ERROR("[Hokuyo] displayVersionInfo() parse error");
818
819 if (nullptr != (ptr = strstr(&m_rcv_data[0], "SCAN:")))
820 out_data->motor_speed_rpm = atoi(ptr + 5);
821 else
822 MRPT_LOG_ERROR("[Hokuyo] displayVersionInfo() parse error");
823
824 if (nullptr != (ptr = strstr(&m_rcv_data[0], "AMIN:")))
825 out_data->scan_first = atoi(ptr + 5);
826 else
827 MRPT_LOG_ERROR("[Hokuyo] displayVersionInfo() parse error");
828
829 if (nullptr != (ptr = strstr(&m_rcv_data[0], "AMAX:")))
830 out_data->scan_last = atoi(ptr + 5);
831 else
832 MRPT_LOG_ERROR("[Hokuyo] displayVersionInfo() parse error");
833
834 if (nullptr != (ptr = strstr(&m_rcv_data[0], "AFRT:")))
835 out_data->scan_front = atoi(ptr + 5);
836 else
837 MRPT_LOG_ERROR("[Hokuyo] displayVersionInfo() parse error");
838
839 if (nullptr != (ptr = strstr(&m_rcv_data[0], "MODL:")))
840 {
841 char aux[30];
842 memcpy(aux, ptr + 5, 8);
843 aux[8] = '\0';
844 out_data->model = aux;
845 }
846 else
847 MRPT_LOG_ERROR("[Hokuyo] displayVersionInfo() parse error");
848 }
849
850 return true;
851 }
852
853 bool CHokuyoURG::startScanningMode()
854 {
855 if (!ensureStreamIsOpen()) return false;
856
857 MRPT_LOG_DEBUG("[CHokuyoURG::startScanningMode] Starting scanning mode...");
858
859 // Send command:
860 // 'M' 'D'
861 // 'XXXX' (starting step)
862 // 'XXXX' (end step)
863 // 'XX' (cluster count)
864 // 'X' (scan interval)
865 // 'XX' (number of scans)
866 char cmd[50];
867 unsigned int scan_interval = m_scan_interval;
868 if (scan_interval > 9) scan_interval = 9;
869 os::sprintf(
870 cmd, 50, "M%c%04u%04u01%u00\x0A", m_intensity ? 'E' : 'D', m_firstRange, m_lastRange,
871 scan_interval);
872
873 sendCmd(cmd);
874
875 // Receive response:
876 if (!parseResponse())
877 {
878 MRPT_LOG_ERROR_STREAM(__CURRENT_FUNCTION_NAME__ << ": Error in response");
879 return false;
880 }
881
882 MRPT_LOG_DEBUG("OK\n");
883 return true;
884 }
885
886 bool CHokuyoURG::ensureStreamIsOpen()
887 {
888 MRPT_START
889
890 if (m_stream)
891 {
892 // Socket or USB connection?
893 if (!m_ip_dir.empty() && m_port_dir)
894 {
895 // Has the port been disconected (USB serial ports)??
896 auto* COM = dynamic_cast<CClientTCPSocket*>(m_stream.get());
897
898 if (COM != nullptr)
899 {
900 if (COM->isConnected()) return true;
901
902 // It has been disconnected... try to reconnect:
903 MRPT_LOG_ERROR(
904 "[CHokuyoURG] Socket connection lost! trying to "
905 "reconnect...");
906
907 try
908 {
909 COM->connect(m_ip_dir, m_port_dir);
910 // OK, reconfigure the laser:
911 turnOn();
912 return true;
913 }
914 catch (...)
915 {
916 // Not yet..
917 return false;
918 }
919 }
920 else
921 {
922 return true; // Assume OK
923 }
924 }
925 else
926 {
927 // Has the port been disconected (USB serial ports)??
928 auto* COM = dynamic_cast<CSerialPort*>(m_stream.get());
929 if (COM != nullptr)
930 {
931 if (COM->isOpen()) return true;
932
933 // It has been disconnected... try to reconnect:
934 MRPT_LOG_ERROR_STREAM(
935 __CURRENT_FUNCTION_NAME__ << ": Serial port connection lost! Trying to reconnect...");
936
937 try
938 {
939 COM->open();
940 // OK, reconfigure the laser:
941 turnOn();
942 return true;
943 }
944 catch (...)
945 {
946 // Not yet..
947 return false;
948 }
949 }
950 else
951 {
952 return true; // Assume OK
953 }
954 }
955 }
956 else
957 {
958 if (m_com_port.empty() && (m_ip_dir.empty() || !m_port_dir))
959 {
960 THROW_EXCEPTION(
961 "No stream bound to the laser nor COM serial port or ip and "
962 "port provided in 'm_com_port','m_ip_dir' and 'm_port_dir'");
963 }
964
965 if (!m_ip_dir.empty())
966 {
967 // Connect to the TCP/IP port:
968 auto theCOM = std::make_shared<CClientTCPSocket>();
969
970 MRPT_LOG_INFO_STREAM(
971 __CURRENT_FUNCTION_NAME__ << " Connecting to " << m_ip_dir << ":" << m_port_dir);
972 theCOM->connect(m_ip_dir, m_port_dir);
973
974 if (!theCOM->isConnected())
975 {
976 MRPT_LOG_ERROR_STREAM(
977 __CURRENT_FUNCTION_NAME__ << " Cannot connect with the server '" << m_com_port << "'");
978 return false;
979 }
980
981 // Bind:
982 bindIO(theCOM);
983 }
984 else
985 {
986 // Try to open the serial port:
987 auto theCOM = std::make_shared<CSerialPort>(m_com_port, true);
988
989 if (!theCOM->isOpen())
990 {
991 MRPT_LOG_ERROR_STREAM(
992 __CURRENT_FUNCTION_NAME__ << " Cannot open serial port '" << m_com_port << "'");
993 return false;
994 }
995
996 // Bind:
997 bindIO(theCOM);
998 }
999
1000 // (re)connected to the sensor. Configure the laser:
1001 turnOn();
1002
1003 return true;
1004 }
1005 MRPT_END
1006 }
1007
1008 void CHokuyoURG::initialize()
1009 {
1010 if (m_verbose) this->setMinLoggingLevel(mrpt::system::LVL_DEBUG);
1011
1012 if (!ensureStreamIsOpen() || !turnOn())
1013 {
1014 m_state = ssError;
1015
1016 MRPT_LOG_ERROR("[Hokuyo] Error initializing HOKUYO scanner");
1017 return;
1018 }
1019 }
1020
1021 void CHokuyoURG::purgeBuffers()
1022 {
1023 if (!ensureStreamIsOpen()) return;
1024
1025 if (m_ip_dir.empty())
1026 {
1027 auto* COM = dynamic_cast<CSerialPort*>(m_stream.get());
1028 if (COM != nullptr)
1029 {
1030 COM->purgeBuffers();
1031 }
1032 }
1033 else // Socket connection
1034 {
1035 auto* COM = dynamic_cast<CClientTCPSocket*>(m_stream.get());
1036
1037 size_t to_read = COM->getReadPendingBytes();
1038
1039 if (to_read)
1040 {
1041 void* buf = malloc(sizeof(uint8_t) * to_read);
1042
1043 size_t nRead = m_stream->Read(buf, to_read);
1044
1045 if (nRead != to_read)
1046 THROW_EXCEPTION(
1047 "Error in purge buffers: read and expected number of bytes "
1048 "are different.");
1049
1050 free(buf);
1051 }
1052 }
1053 }