MRPT  1.9.9
CHokuyoURG.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 #include "hwdrivers-precomp.h" // Precompiled headers
11 
13 #include <mrpt/comms/CSerialPort.h>
15 #include <mrpt/opengl/CAxis.h>
16 #include <mrpt/opengl/CPlanarLaserScan.h> // in library mrpt-maps
17 #include <mrpt/system/os.h>
18 
20 
21 using namespace mrpt::obs;
22 using namespace mrpt::hwdrivers;
23 using namespace mrpt::comms;
24 using namespace mrpt::system;
25 using namespace mrpt::opengl;
26 using namespace std;
27 
29 
30 CHokuyoURG::CHokuyoURG() : m_rx_buffer(40000) { m_sensorLabel = "Hokuyo"; }
31 
33 {
34  try
35  {
36  m_win.reset();
38  }
39  catch (const std::exception& e)
40  {
41  std::cerr << "[~CHokuyoURG] Exception:\n" << mrpt::exception_to_str(e);
42  }
43 }
44 
46 {
47  if (m_stream)
48  {
49  turnOff();
50 
52  m_stream = nullptr;
53  }
54 }
55 
56 void CHokuyoURG::sendCmd(const char* str)
57 {
59  ASSERT_(str != nullptr);
60  ASSERT_(m_stream != nullptr);
61  const size_t N = strlen(str);
62  const size_t nWriten = m_stream->Write(str, N);
63  ASSERT_EQUAL_(nWriten, N);
64 
65  MRPT_LOG_DEBUG_STREAM("[Hokuyo] sendCmd(): `" << str << "`");
66 
67  m_lastSentMeasCmd = std::string(str); // for echo verification
68  MRPT_END
69 }
70 
72  bool& outThereIsObservation,
73  mrpt::obs::CObservation2DRangeScan& outObservation, bool& hardwareError)
74 {
75  outThereIsObservation = false;
76  hardwareError = false;
77 
78  // Bound? If not, connect to the device and set it up (again) in
79  // continuous read mode:
80  if (!ensureStreamIsOpen())
81  {
82  m_timeStartUI = 0;
84  hardwareError = true;
85  return;
86  }
87 
88  // Wait for a message:
89  char rcv_status0, rcv_status1;
90  int nRanges = m_lastRange - m_firstRange + 1;
91  int expectedSize = nRanges * 3 + 4;
92  if (m_intensity) expectedSize += nRanges * 3;
93 
94  m_rcv_data.clear();
95  m_rcv_data.reserve(expectedSize + 1000);
96 
98  if (!receiveResponse(rcv_status0, rcv_status1))
99  {
101  {
102  // It seems the sensor needs to be reseted (?), let this know
103  // to the caller:
104  m_state = ssError;
105  hardwareError = true;
106 
107  // And on our side, close the connection to ensure initialization
108  // is called again to set-up the laser in the next call to
109  // ensureStreamIsOpen() above.
111 
112  return;
113  }
114 
115  // No new data
116  return;
117  }
118 
119  // DECODE:
120  if (rcv_status0 != '0' && rcv_status0 != '9')
121  {
122  hardwareError = true;
123  return;
124  }
125 
126  // -----------------------------------------------
127  // Extract the observation:
128  // -----------------------------------------------
129  outObservation.timestamp = mrpt::system::now();
130 
131  if ((size_t)expectedSize != m_rcv_data.size())
132  {
134  "[CHokuyoURG::doProcess] ERROR: Expected "
135  << expectedSize << " data bytes, received " << m_rcv_data.size()
136  << "instead!");
137  hardwareError = true;
138  return;
139  }
140  // Delay the sync of timestamps due to instability in the constant rate
141  // during the first few packets.
142  bool do_timestamp_sync = !m_disable_firmware_timestamp;
143  if (do_timestamp_sync &&
145  {
146  do_timestamp_sync = false;
148  }
149 
150  if (do_timestamp_sync)
151  {
152  // Extract the timestamp of the sensor:
153  uint32_t nowUI = ((m_rcv_data[0] - 0x30) << 18) +
154  ((m_rcv_data[1] - 0x30) << 12) +
155  ((m_rcv_data[2] - 0x30) << 6) + (m_rcv_data[3] - 0x30);
156 
157  uint32_t AtUI = 0;
158  if (m_timeStartUI == 0)
159  {
160  m_timeStartUI = nowUI;
162  }
163  else
164  AtUI = nowUI - m_timeStartUI;
165 
166  auto AtDO = std::chrono::milliseconds(AtUI);
167  outObservation.timestamp = m_timeStartTT + AtDO;
168  }
169 
170  // And the scan ranges:
171  outObservation.rightToLeft = true;
172 
173  outObservation.aperture =
174  nRanges * 2 * M_PI / m_sensor_info.scans_per_360deg;
175 
176  outObservation.maxRange = m_sensor_info.d_max;
177  outObservation.stdError = 0.010f;
178  outObservation.sensorPose = m_sensorPose;
179  outObservation.sensorLabel = m_sensorLabel;
180 
181  outObservation.resizeScan(nRanges);
182  char* ptr = (char*)&m_rcv_data[4];
183 
184  if (m_intensity) outObservation.setScanHasIntensity(true);
185 
186  for (int i = 0; i < nRanges; i++)
187  {
188  int b1 = (*ptr++) - 0x30;
189  int b2 = (*ptr++) - 0x30;
190  int b3 = (*ptr++) - 0x30;
191 
192  int range_mm = ((b1 << 12) | (b2 << 6) | b3);
193 
194  outObservation.setScanRange(i, range_mm * 0.001f);
195  outObservation.setScanRangeValidity(
196  i, range_mm >= 20 &&
197  (outObservation.scan[i] <= outObservation.maxRange));
198 
199  if (m_intensity)
200  {
201  int b4 = (*ptr++) - 0x30;
202  int b5 = (*ptr++) - 0x30;
203  int b6 = (*ptr++) - 0x30;
204  outObservation.setScanIntensity(i, ((b4 << 12) | (b5 << 6) | b6));
205  }
206  }
207 
208  // Do filter:
211  // Do show preview:
213 
214  outThereIsObservation = true;
216 }
217 
218 /*-------------------------------------------------------------
219  loadConfig_sensorSpecific
220 -------------------------------------------------------------*/
222  const mrpt::config::CConfigFileBase& configSource,
223  const std::string& iniSection)
224 {
225  m_reduced_fov =
226  DEG2RAD(configSource.read_float(iniSection, "reduced_fov", 0)),
227 
229  configSource.read_int(iniSection, "HOKUYO_motorSpeed_rpm", 0);
231  configSource.read_float(iniSection, "pose_x", 0),
232  configSource.read_float(iniSection, "pose_y", 0),
233  configSource.read_float(iniSection, "pose_z", 0),
234  DEG2RAD(configSource.read_float(iniSection, "pose_yaw", 0)),
235  DEG2RAD(configSource.read_float(iniSection, "pose_pitch", 0)),
236  DEG2RAD(configSource.read_float(iniSection, "pose_roll", 0)));
237 
239  configSource.read_bool(iniSection, "HOKUYO_HS_mode", m_highSensMode);
240 
241 #ifdef _WIN32
242  m_com_port =
243  configSource.read_string(iniSection, "COM_port_WIN", m_com_port);
244 #else
245  m_com_port =
246  configSource.read_string(iniSection, "COM_port_LIN", m_com_port);
247 #endif
248 
249  m_ip_dir = configSource.read_string(iniSection, "IP_DIR", m_ip_dir);
250  m_port_dir = configSource.read_int(iniSection, "PORT_DIR", m_port_dir);
251 
252  ASSERTMSG_(
253  !m_com_port.empty() || !m_ip_dir.empty(),
254  "Either COM_port or IP_DIR must be defined in the configuration file!");
255  ASSERTMSG_(
256  m_com_port.empty() || m_ip_dir.empty(),
257  "Both COM_port and IP_DIR set! Please, define only one of them.");
258  if (!m_ip_dir.empty())
259  {
260  ASSERTMSG_(
261  m_port_dir,
262  "A TCP/IP port number `PORT_DIR` must be specified for Ethernet "
263  "connection");
264  }
265 
267  iniSection, "disable_firmware_timestamp", m_disable_firmware_timestamp);
268  m_intensity = configSource.read_bool(iniSection, "intensity", m_intensity),
269 
271  scan_interval, int, m_scan_interval, configSource, iniSection);
272 
273  // Parent options:
274  C2DRangeFinderAbstract::loadCommonParams(configSource, iniSection);
275 }
276 
277 /*-------------------------------------------------------------
278  turnOn
279 -------------------------------------------------------------*/
281 {
282  MRPT_START
283 
284  // Bound?
285  if (!ensureStreamIsOpen()) return false;
286 
287  // If we are over a serial link, set it up:
288  if (m_ip_dir.empty())
289  {
290  auto* COM = dynamic_cast<CSerialPort*>(m_stream);
291 
292  if (COM != nullptr)
293  {
294  // It is a COM:
295  COM->setConfig(19200);
296  COM->setTimeouts(100, 0, 200, 0, 0);
297 
298  // Assure the laser is off and quiet:
299  switchLaserOff();
300  std::this_thread::sleep_for(10ms);
301 
302  COM->purgeBuffers();
303  std::this_thread::sleep_for(10ms);
304 
305  COM->setConfig(115200);
306  switchLaserOff();
307  std::this_thread::sleep_for(10ms);
308  COM->purgeBuffers();
309  std::this_thread::sleep_for(10ms);
310  COM->setConfig(19200);
311  }
312 
313  if (COM != nullptr)
314  {
315  // Set 115200 baud rate:
316  setHighBaudrate();
317  enableSCIP20();
318  COM->setConfig(115200);
319  }
320  }
321  else
322  {
323  auto* COM = dynamic_cast<CClientTCPSocket*>(m_stream);
324 
325  if (COM != nullptr)
326  {
327  // Assure the laser is off and quiet:
328  switchLaserOff();
329  std::this_thread::sleep_for(10ms);
330 
331  purgeBuffers();
332  std::this_thread::sleep_for(10ms);
333 
334  switchLaserOff();
335  std::this_thread::sleep_for(10ms);
336  purgeBuffers();
337  }
338  }
339 
340  if (!enableSCIP20()) return false;
341 
342  // Turn on the laser:
343  if (!switchLaserOn()) return false;
344 
345  // Set the motor speed:
346  if (m_motorSpeed_rpm)
347  if (!setMotorSpeed(m_motorSpeed_rpm)) return false;
348 
349  // Set HS mode:
351 
352  // Display sensor information:
353  if (!displaySensorInfo(&m_sensor_info)) return false;
354 
355  // Set for scanning angles:
358 
359  // Artificially reduced FOV?
360  if (m_reduced_fov > 0 && m_reduced_fov < 2 * M_PI)
361  {
362  int center = (m_lastRange + m_firstRange) >> 1;
363  const int half_range = static_cast<int>(
364  (m_sensor_info.scans_per_360deg / 360.0) *
366  1;
367  m_firstRange = center - half_range;
368  m_lastRange = center + half_range;
370  "[HOKUYO::turnOn] Using reduced FOV: ranges ["
371  << m_firstRange << "-" << m_lastRange << "] for "
372  << RAD2DEG(m_reduced_fov) << " deg. FOV");
373  }
374 
375  if (!displayVersionInfo())
376  {
377  // return false; // It's not SO important
378  }
379 
380  // Start!
381  if (!startScanningMode()) return false;
382 
383  return true;
384 
385  MRPT_END
386 }
387 
388 /*-------------------------------------------------------------
389  turnOff
390 -------------------------------------------------------------*/
392 {
393  // Turn off the laser:
394  if (!switchLaserOff()) return false;
395 
396  return true;
397 }
398 
400 {
401  char rcv_status0, rcv_status1;
402  if (!ensureStreamIsOpen()) return false;
403 
405  "[CHokuyoURG::setHighBaudrate] Changing baudrate to 115200...");
406 
407  // Send command:
408  sendCmd("SS115200\x0A");
409 
410  // Receive response:
411  if (!receiveResponse(rcv_status0, rcv_status1))
412  {
414  "[CHokuyoURG::setHighBaudrate] Error waiting for response");
415  return false;
416  }
417 
418  MRPT_LOG_DEBUG("OK\n");
419  return true;
420 }
421 
422 /*-------------------------------------------------------------
423  assureBufferHasBytes
424 -------------------------------------------------------------*/
425 bool CHokuyoURG::assureBufferHasBytes(const size_t nDesiredBytes)
426 {
427  ASSERT_(nDesiredBytes < m_rx_buffer.capacity());
428 
429  if (m_rx_buffer.size() >= nDesiredBytes)
430  {
431  return true;
432  }
433  else
434  {
435  // Try to read more bytes:
436  uint8_t buf[128];
437  const size_t to_read = std::min(m_rx_buffer.available(), sizeof(buf));
438 
439  try
440  {
441  size_t nRead;
442 
443  if (!m_ip_dir.empty())
444  {
445  auto* client = dynamic_cast<CClientTCPSocket*>(m_stream);
446  nRead = client->readAsync(buf, to_read, 100, 10);
447  }
448  else
449  {
450  nRead = m_stream->Read(buf, to_read);
451  }
452 
453  m_rx_buffer.push_many(buf, nRead);
454  }
455  catch (std::exception&)
456  {
457  // 0 bytes read
458  }
459 
460  return (m_rx_buffer.size() >= nDesiredBytes);
461  }
462 }
463 
464 bool CHokuyoURG::receiveResponse(char& rcv_status0, char& rcv_status1)
465 {
466  m_rcv_data.clear();
467  if (!ensureStreamIsOpen()) return false;
468  ASSERT_(!m_lastSentMeasCmd.empty());
469 
470  try
471  {
472  // Process response:
473  // ---------------------------------
474 
475  // COMMAND ECHO ---------
476  unsigned int i = 0;
477  const unsigned int verifLen = m_lastSentMeasCmd.size();
478 
479  if (verifLen)
480  {
481  do
482  {
483  if (!assureBufferHasBytes(verifLen - i)) return false;
484 
485  // If matches the echo, go on:
486  if (m_rx_buffer.pop() == m_lastSentMeasCmd[i])
487  i++;
488  else
489  i = 0;
490  } while (i < verifLen);
491  }
492 
493  // Now, the status bytes:
494  if (!assureBufferHasBytes(2)) return false;
495 
496  rcv_status0 = m_rx_buffer.pop();
497  rcv_status1 = m_rx_buffer.pop();
498 
499  // In SCIP2.0, there is an additional sum char:
500  if (rcv_status1 != 0x0A)
501  {
502  // Yes, it is SCIP2.0
503  if (!assureBufferHasBytes(1)) return false;
504 
505  // Ignore this byte: sumStatus
506  m_rx_buffer.pop();
507  }
508  else
509  {
510  // Continue, it seems a SCIP1.1 response...
511  }
512 
513  // After the status bytes, there is a LF:
514  if (!assureBufferHasBytes(1)) return false;
515  char nextChar = m_rx_buffer.pop();
516  if (nextChar != 0x0A) return false;
517 
518  // -----------------------------------------------------------------------------
519  // Now the data:
520  // There's a problem here, we don't know in advance how many bytes to
521  // read,
522  // so rely on the serial port class implemented buffer and call many
523  // times
524  // the read method with only 1 byte each time:
525  // -----------------------------------------------------------------------------
526  bool lastWasLF = false;
527  i = 0;
528  for (;;)
529  {
530  if (!assureBufferHasBytes(1))
531  {
532  return false;
533  }
534  m_rcv_data.push_back(m_rx_buffer.pop());
535  i++; // One more byte in the buffer
536 
537  // No data?
538  if (i == 1 && m_rcv_data[0] == 0x0A)
539  {
540  m_rcv_data.clear();
541  return true;
542  }
543 
544  // Is it a LF?
545  if (m_rcv_data[i - 1] == 0x0A)
546  {
547  if (!lastWasLF)
548  {
549  // Discard SUM+LF
550  ASSERT_(i >= 2);
551  i -= 2;
552  m_rcv_data.resize(i);
553  }
554  else
555  {
556  // Discard this last LF:
557  i--;
558 
559  // Done!
560  m_rcv_data.resize(i);
562  "[Hokuyo] receiveResponse(): RX `" << m_rcv_data
563  << "`");
564 
565  if (rcv_status0 != '0' &&
566  (rcv_status0 != '9' && rcv_status1 != '9'))
567  {
569  "[Hokuyo] Error LIDAR status: "
570  << (int)rcv_status0 << " after command: `"
571  << m_lastSentMeasCmd << "`");
572  return false;
573  }
574 
575  return true;
576  }
577  lastWasLF = true;
578  }
579  else
580  lastWasLF = false;
581  }
582  }
583  catch (const std::exception& e)
584  {
586  "[Hokuyo] receiveResponse() Exception: %s", e.what());
587  return false;
588  }
589  catch (...)
590  {
591  return false; // Serial port timeout,...
592  }
593 }
594 
596 {
597  char rcv_status0, rcv_status1;
598  if (!ensureStreamIsOpen()) return false;
599 
601  "[CHokuyoURG::enableSCIP20] Changing protocol to SCIP2.0...");
602 
603  // Send command:
604  sendCmd("SCIP2.0\x0A");
605 
606  // Receive response:
607  if (!receiveResponse(rcv_status0, rcv_status1))
608  {
610  __CURRENT_FUNCTION_NAME__ << ": Error in response");
611  return false;
612  }
613 
614  MRPT_LOG_DEBUG("OK\n");
615  return true;
616 }
617 
619 {
620  char rcv_status0, rcv_status1;
621 
622  if (!ensureStreamIsOpen()) return false;
623 
624  MRPT_LOG_DEBUG("[CHokuyoURG::switchLaserOn] Switching laser ON...");
625 
626  // Send command:
627  sendCmd("BM\x0A");
628 
629  // Receive response:
630  if (!receiveResponse(rcv_status0, rcv_status1))
631  {
633  __CURRENT_FUNCTION_NAME__ << ": Error in response");
634  return false;
635  }
636 
637  MRPT_LOG_DEBUG("OK\n");
638  return true;
639 }
640 
642 {
643  char rcv_status0, rcv_status1;
644 
645  if (!ensureStreamIsOpen()) return false;
646 
647  MRPT_LOG_DEBUG("[CHokuyoURG::switchLaserOff] Switching laser OFF...");
648 
649  // Send command:
650  sendCmd("QT\x0A");
651 
652  // Receive response:
653  if (!receiveResponse(rcv_status0, rcv_status1))
654  {
656  __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)
665 {
666  m_scan_interval = skipScanCount;
667 }
668 unsigned int CHokuyoURG::getScanInterval() const { return m_scan_interval; }
669 bool CHokuyoURG::setMotorSpeed(int motoSpeed_rpm)
670 {
671  char rcv_status0, rcv_status1;
672  if (!ensureStreamIsOpen()) return false;
673 
675  "[CHokuyoURG::setMotorSpeed] Setting to %i rpm...", motoSpeed_rpm);
676 
677  // Send command:
678  int motorSpeedCode = (600 - motoSpeed_rpm) / 6;
679  if (motorSpeedCode < 0 || motorSpeedCode > 10)
680  {
683  << " Motorspeed must be in the range 540-600 rpm");
684  return false;
685  }
686 
687  char cmd[20];
688  os::sprintf(cmd, 20, "CR%02i\x0A", motorSpeedCode);
689  sendCmd(cmd);
690 
691  // Receive response:
692  if (!receiveResponse(rcv_status0, rcv_status1))
693  {
695  __CURRENT_FUNCTION_NAME__ << ": Error in response");
696  return false;
697  }
698 
699  MRPT_LOG_DEBUG("OK\n");
700  return true;
701 }
702 
703 /*-------------------------------------------------------------
704  setHighSensitivityMode
705 -------------------------------------------------------------*/
707 {
708  char rcv_status0, rcv_status1;
709  if (!ensureStreamIsOpen()) return false;
710 
712  "[CHokuyoURG::setHighSensitivityMode] Setting HS mode to: %s...",
713  enabled ? "true" : "false");
714 
715  // Send command:
716  char cmd[20];
717  os::sprintf(cmd, 20, "HS%i\x0A", enabled ? 1 : 0);
718  sendCmd(cmd);
719 
720  // Receive response:
721  if (!receiveResponse(rcv_status0, rcv_status1))
722  {
724  __CURRENT_FUNCTION_NAME__ << ": Error in response");
725  return false;
726  }
727 
728  MRPT_LOG_DEBUG("OK\n");
729  return true;
730 }
731 
732 /*-------------------------------------------------------------
733  setIntensityMode
734 -------------------------------------------------------------*/
736 {
737  m_intensity = enabled;
738  return true;
739 }
740 
742 {
743  char rcv_status0, rcv_status1;
744  if (!ensureStreamIsOpen()) return false;
745 
746  MRPT_LOG_DEBUG("[CHokuyoURG::displayVersionInfo] Asking info...");
747 
748  // Send command:
749  sendCmd("VV\x0A");
750 
751  // Receive response:
752  if (!receiveResponse(rcv_status0, rcv_status1))
753  {
755  __CURRENT_FUNCTION_NAME__ << ": Error in response");
756  return false;
757  }
758 
759  MRPT_LOG_DEBUG("OK\n");
760 
761  // PRINT:
762  for (auto& c : m_rcv_data)
763  if (c == ';') c = '\n';
764  m_rcv_data[m_rcv_data.size()] = '\0';
765 
766  if (!m_rcv_data.empty())
767  {
769  "\n------------- HOKUYO Scanner: Version Information ------\n"
770  << &m_rcv_data[0]
771  << "\n"
772  "-------------------------------------------------------\n\n");
773  }
774  return true;
775 }
776 
777 /*-------------------------------------------------------------
778  displaySensorInfo
779 -------------------------------------------------------------*/
781 {
782  char rcv_status0, rcv_status1;
783  if (!ensureStreamIsOpen()) return false;
784 
785  MRPT_LOG_DEBUG("[CHokuyoURG::displaySensorInfo] Asking for info...");
786 
787  // Send command:
788  sendCmd("PP\x0A");
789 
790  // Receive response:
791  if (!receiveResponse(rcv_status0, rcv_status1))
792  {
794  __CURRENT_FUNCTION_NAME__ << ": Error in response");
795  return false;
796  }
797  MRPT_LOG_DEBUG("OK\n");
798 
799  // PRINT:
800  for (auto& c : m_rcv_data)
801  if (c == ';') c = '\n';
802  m_rcv_data[m_rcv_data.size()] = '\0';
803 
804  if (!m_rcv_data.empty())
805  {
807  "\n------------- HOKUYO Scanner: Product Information ------\n"
808  << &m_rcv_data[0]
809  << "\n"
810  "-------------------------------------------------------\n\n");
811  }
812 
813  // Parse the data:
814  if (out_data)
815  {
816  const char* ptr;
817 
818  if (nullptr != (ptr = strstr(&m_rcv_data[0], "DMAX:")))
819  out_data->d_max = 0.001 * atoi(ptr + 5);
820  else
821  MRPT_LOG_ERROR("[Hokuyo] displayVersionInfo() parse error");
822 
823  if (nullptr != (ptr = strstr(&m_rcv_data[0], "DMIN:")))
824  out_data->d_min = 0.001 * atoi(ptr + 5);
825  else
826  MRPT_LOG_ERROR("[Hokuyo] displayVersionInfo() parse error");
827 
828  if (nullptr != (ptr = strstr(&m_rcv_data[0], "ARES:")))
829  out_data->scans_per_360deg = atoi(ptr + 5);
830  else
831  MRPT_LOG_ERROR("[Hokuyo] displayVersionInfo() parse error");
832 
833  if (nullptr != (ptr = strstr(&m_rcv_data[0], "SCAN:")))
834  out_data->motor_speed_rpm = atoi(ptr + 5);
835  else
836  MRPT_LOG_ERROR("[Hokuyo] displayVersionInfo() parse error");
837 
838  if (nullptr != (ptr = strstr(&m_rcv_data[0], "AMIN:")))
839  out_data->scan_first = atoi(ptr + 5);
840  else
841  MRPT_LOG_ERROR("[Hokuyo] displayVersionInfo() parse error");
842 
843  if (nullptr != (ptr = strstr(&m_rcv_data[0], "AMAX:")))
844  out_data->scan_last = atoi(ptr + 5);
845  else
846  MRPT_LOG_ERROR("[Hokuyo] displayVersionInfo() parse error");
847 
848  if (nullptr != (ptr = strstr(&m_rcv_data[0], "AFRT:")))
849  out_data->scan_front = atoi(ptr + 5);
850  else
851  MRPT_LOG_ERROR("[Hokuyo] displayVersionInfo() parse error");
852 
853  if (nullptr != (ptr = strstr(&m_rcv_data[0], "MODL:")))
854  {
855  char aux[30];
856  memcpy(aux, ptr + 5, 8);
857  aux[8] = '\0';
858  out_data->model = aux;
859  }
860  else
861  MRPT_LOG_ERROR("[Hokuyo] displayVersionInfo() parse error");
862  }
863 
864  return true;
865 }
866 
868 {
869  char rcv_status0, rcv_status1;
870  if (!ensureStreamIsOpen()) return false;
871 
872  MRPT_LOG_DEBUG("[CHokuyoURG::startScanningMode] Starting scanning mode...");
873 
874  // Send command:
875  // 'M' 'D'
876  // 'XXXX' (starting step)
877  // 'XXXX' (end step)
878  // 'XX' (cluster count)
879  // 'X' (scan interval)
880  // 'XX' (number of scans)
881  char cmd[50];
882  unsigned int scan_interval = m_scan_interval;
883  if (scan_interval > 9) scan_interval = 9;
884  os::sprintf(
885  cmd, 50, "M%c%04u%04u01%u00\x0A", m_intensity ? 'E' : 'D', m_firstRange,
886  m_lastRange, scan_interval);
887 
888  sendCmd(cmd);
889 
890  // Receive response:
891  if (!receiveResponse(rcv_status0, rcv_status1))
892  {
894  __CURRENT_FUNCTION_NAME__ << ": Error in response");
895  return false;
896  }
897 
898  MRPT_LOG_DEBUG("OK\n");
899  return true;
900 }
901 
903 {
904  MRPT_START
905 
906  if (m_stream)
907  {
908  // Socket or USB connection?
909  if (!m_ip_dir.empty() && m_port_dir)
910  {
911  // Has the port been disconected (USB serial ports)??
912  auto* COM = dynamic_cast<CClientTCPSocket*>(m_stream);
913 
914  if (COM != nullptr)
915  {
916  if (COM->isConnected()) return true;
917 
918  // It has been disconnected... try to reconnect:
920  "[CHokuyoURG] Socket connection lost! trying to "
921  "reconnect...");
922 
923  try
924  {
925  COM->connect(m_ip_dir, m_port_dir);
926  // OK, reconfigure the laser:
927  turnOn();
928  return true;
929  }
930  catch (...)
931  {
932  // Not yet..
933  return false;
934  }
935  }
936  else
937  {
938  return true; // Assume OK
939  }
940  }
941  else
942  {
943  // Has the port been disconected (USB serial ports)??
944  auto* COM = dynamic_cast<CSerialPort*>(m_stream);
945  if (COM != nullptr)
946  {
947  if (COM->isOpen()) return true;
948 
949  // It has been disconnected... try to reconnect:
952  << ": Serial port connection lost! Trying to reconnect...");
953 
954  try
955  {
956  COM->open();
957  // OK, reconfigure the laser:
958  turnOn();
959  return true;
960  }
961  catch (...)
962  {
963  // Not yet..
964  return false;
965  }
966  }
967  else
968  {
969  return true; // Assume OK
970  }
971  }
972  }
973  else
974  {
975  if (m_com_port.empty() && (m_ip_dir.empty() || !m_port_dir))
976  {
978  "No stream bound to the laser nor COM serial port or ip and "
979  "port provided in 'm_com_port','m_ip_dir' and 'm_port_dir'");
980  }
981 
982  if (!m_ip_dir.empty())
983  {
984  // Connect to the TCP/IP port:
985  auto* theCOM = new CClientTCPSocket();
986 
988  __CURRENT_FUNCTION_NAME__ << " Connecting to " << m_ip_dir
989  << ":" << m_port_dir);
990  theCOM->connect(m_ip_dir, m_port_dir);
991 
992  if (!theCOM->isConnected())
993  {
996  << " Cannot connect with the server '" << m_com_port
997  << "'");
998  delete theCOM;
999  return false;
1000  }
1001 
1002  // Bind:
1003  bindIO(theCOM);
1004  }
1005  else
1006  {
1007  // Try to open the serial port:
1008  auto* theCOM = new CSerialPort(m_com_port, true);
1009 
1010  if (!theCOM->isOpen())
1011  {
1013  __CURRENT_FUNCTION_NAME__ << " Cannot open serial port '"
1014  << m_com_port << "'");
1015  delete theCOM;
1016  return false;
1017  }
1018 
1019  // Bind:
1020  bindIO(theCOM);
1021  }
1022  m_I_am_owner_serial_port = true;
1023 
1024  // (re)connected to the sensor. Configure the laser:
1025  turnOn();
1026 
1027  return true;
1028  }
1029  MRPT_END
1030 }
1031 
1033 {
1035 
1036  if (!ensureStreamIsOpen()) return;
1037 
1038  if (!turnOn())
1039  {
1040  MRPT_LOG_ERROR("[Hokuyo] Error initializing HOKUYO scanner");
1041  return;
1042  }
1043 }
1044 
1046 {
1047  if (!ensureStreamIsOpen()) return;
1048 
1049  if (m_ip_dir.empty())
1050  {
1051  auto* COM = dynamic_cast<CSerialPort*>(m_stream);
1052  if (COM != nullptr)
1053  {
1054  COM->purgeBuffers();
1055  }
1056  }
1057  else // Socket connection
1058  {
1059  auto* COM = dynamic_cast<CClientTCPSocket*>(m_stream);
1060 
1061  size_t to_read = COM->getReadPendingBytes();
1062 
1063  if (to_read)
1064  {
1065  void* buf = malloc(sizeof(uint8_t) * to_read);
1066 
1067  size_t nRead = m_stream->Read(buf, to_read);
1068 
1069  if (nRead != to_read)
1071  "Error in purge buffers: read and expected number of bytes "
1072  "are different.");
1073 
1074  free(buf);
1075  }
1076  }
1077 }
size_t available() const
The maximum number of elements that can be written ("push") without rising an overflow error...
void setScanInterval(unsigned int skipScanCount)
Set the skip scan count (0 means send all scans).
Definition: CHokuyoURG.cpp:664
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: CHokuyoURG.h:266
uint32_t m_timeStartUI
Time of the first data packet, for synchronization purposes.
Definition: CHokuyoURG.h:281
#define MRPT_START
Definition: exceptions.h:241
#define MRPT_LOG_DEBUG(_STRING)
Use: MRPT_LOG_DEBUG("message");
int m_firstRange
The first and last ranges to consider from the scan.
Definition: CHokuyoURG.h:95
#define min(a, b)
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,...)
std::string m_rcv_data
temp buffer for incoming data packets
Definition: CHokuyoURG.h:249
double RAD2DEG(const double x)
Radians to degrees.
size_t capacity() const
Return the maximum capacity of the buffer.
A communications serial port built as an implementation of a utils::CStream.
Definition: CSerialPort.h:41
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
bool m_intensity
Get intensity from lidar scan (default: false)
Definition: CHokuyoURG.h:288
void internal_notifyGoodScanNow()
Must be called from doProcessSimple() implementations.
void setScanRange(const size_t i, const float val)
std::string m_sensorLabel
See CGenericSensor.
double DEG2RAD(const double x)
Degrees to radians.
unsigned int getScanInterval() const
Definition: CHokuyoURG.cpp:668
virtual size_t Read(void *Buffer, size_t Count)=0
Introduces a pure virtual method responsible for reading from the stream.
mrpt::containers::circular_buffer< uint8_t > m_rx_buffer
Auxiliary buffer for readings.
Definition: CHokuyoURG.h:101
bool assureBufferHasBytes(const size_t nDesiredBytes)
Assures a minimum number of bytes in the input buffer, reading from the serial port only if required...
Definition: CHokuyoURG.cpp:425
~CHokuyoURG() override
Destructor: turns the laser off.
Definition: CHokuyoURG.cpp:32
void setMinLoggingLevel(const VerbosityLevel level)
Set the minimum logging level for which the incoming logs are going to be taken into account...
mrpt::system::TTimeStamp now()
A shortcut for system::getCurrentTime.
Definition: datetime.h:86
Contains classes for various device interfaces.
void setScanHasIntensity(bool setHasIntensityFlag)
Marks this scan as having or not intensity data.
float read_float(const std::string &section, const std::string &name, float defaultValue, bool failIfNotFound=false) const
bool turnOn() override
Enables the scanning mode (which may depend on the specific laser device); this must be called before...
Definition: CHokuyoURG.cpp:280
STL namespace.
void filterByExclusionAreas(mrpt::obs::CObservation2DRangeScan &obs) const
Mark as invalid those points which (x,y) coordinates fall within the exclusion polygons.
bool ensureStreamIsOpen()
Returns true if there is a valid stream bound to the laser scanner, otherwise it first try to open th...
Definition: CHokuyoURG.cpp:902
float stdError
The "sigma" error of the device in meters, used while inserting the scan in an occupancy grid...
void setConfig(int baudRate, int parity=0, int bits=8, int nStopBits=1, bool enableFlowControl=false)
Changes the configuration of the port.
void filterByExclusionAngles(mrpt::obs::CObservation2DRangeScan &obs) const
Mark as invalid those ranges in a set of forbiden angle ranges.
bool switchLaserOn()
Switchs the laser on.
Definition: CHokuyoURG.cpp:618
mrpt::io::CStream * m_stream
The I/O channel (will be nullptr if not bound).
void closeStreamConnection()
Called upon dtor, or when trying to recover from a disconnected sensor.
Definition: CHokuyoURG.cpp:45
int read_int(const std::string &section, const std::string &name, int defaultValue, bool failIfNotFound=false) const
unsigned int m_port_dir
If set to non-empty and m_ip_dir too, the program will try to connect to a Hokuyo using Ethernet comm...
Definition: CHokuyoURG.h:273
std::string m_lastSentMeasCmd
The last sent measurement command (MDXXX), including the last 0x0A.
Definition: CHokuyoURG.h:104
mrpt::gui::CDisplayWindow3D::Ptr m_win
Definition: CHokuyoURG.h:108
unsigned char uint8_t
Definition: rptypes.h:44
float maxRange
The maximum range allowed by the device, in meters (e.g.
const int MINIMUM_PACKETS_TO_SET_TIMESTAMP_REFERENCE
Definition: CHokuyoURG.cpp:28
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
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: CHokuyoURG.cpp:71
This class allows loading and storing values and vectors of different types from a configuration text...
T pop()
Retrieve an element from the buffer.
#define MRPT_LOG_DEBUG_FMT(_FMT_STRING,...)
Use: MRPT_LOG_DEBUG_FMT("i=%u", i);
mrpt::containers::ContainerReadOnlyProxyAccessor< mrpt::aligned_std_vector< float > > scan
The range values of the scan, in meters.
bool turnOff() override
Disables the scanning mode (this can be used to turn the device in low energy mode, if available)
Definition: CHokuyoURG.cpp:391
#define ASSERT_EQUAL_(__A, __B)
Assert comparing two values, reporting their actual values upon failure.
Definition: exceptions.h:137
bool displaySensorInfo(CHokuyoURG::TSensorInfo *out_data=nullptr)
Ask to the device, and print to the debug stream, details about the sensor model. ...
Definition: CHokuyoURG.cpp:780
const GLubyte * c
Definition: glext.h:6406
Used in CHokuyoURG::displayVersionInfo.
Definition: CHokuyoURG.h:79
void setScanIntensity(const size_t i, const int val)
bool setIntensityMode(bool enabled)
If true scans will capture intensity.
Definition: CHokuyoURG.cpp:735
bool displayVersionInfo()
Ask to the device, and print to the debug stream, details about the firmware version,serial number,...
Definition: CHokuyoURG.cpp:741
This namespace contains representation of robot actions and observations.
bool receiveResponse(char &rcv_status0, char &rcv_status1)
Waits for a response from the device.
Definition: CHokuyoURG.cpp:464
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
Definition: exceptions.h:108
int m_timeStartSynchDelay
Counter to discard to first few packets before setting the correspondence between device and computer...
Definition: CHokuyoURG.h:284
bool enableSCIP20()
Enables the SCIP2.0 protocol (this must be called at the very begining!).
Definition: CHokuyoURG.cpp:595
void purgeBuffers()
Purge tx and rx buffers.
mrpt::system::TTimeStamp m_timeStartTT
Definition: CHokuyoURG.h:285
#define MRPT_LOG_DEBUG_STREAM(__CONTENTS)
Use: MRPT_LOG_DEBUG_STREAM("Var=" << value << " foo=" << foo_var);
GLsizei const GLchar ** string
Definition: glext.h:4116
#define IMPLEMENTS_GENERIC_SENSOR(class_name, NameSpace)
This must be inserted in all CGenericSensor classes implementation files:
size_t size() const
Return the number of elements available for read ("pop") in the buffer (this is NOT the maximum size ...
bool switchLaserOff()
Switchs the laser off.
Definition: CHokuyoURG.cpp:641
int m_motorSpeed_rpm
The motor speed (default=600rpm)
Definition: CHokuyoURG.h:97
#define MRPT_LOG_INFO_STREAM(__CONTENTS)
std::string sensorLabel
An arbitrary label that can be used to identify the sensor.
Definition: CObservation.h:62
void purgeBuffers()
Empties the RX buffers of the serial port.
mrpt::system::TTimeStamp timestamp
The associated UTC time-stamp.
Definition: CObservation.h:60
int scan_first
First, last, and front step of the scanner angular span.
Definition: CHokuyoURG.h:88
A "CObservation"-derived class that represents a 2D range scan measurement (typically from a laser sc...
bool startScanningMode()
Start the continuous scanning mode, using parameters stored in the object (loaded from the ...
Definition: CHokuyoURG.cpp:867
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.
virtual size_t Write(const void *Buffer, size_t Count)=0
Introduces a pure virtual method responsible for writing to the stream.
bool read_bool(const std::string &section, const std::string &name, bool defaultValue, bool failIfNotFound=false) const
void initialize() override
Turns the laser on.
#define MRPT_END
Definition: exceptions.h:245
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)...
void setFromValues(const double x0, const double y0, const double z0, const double yaw=0, const double pitch=0, const double roll=0)
Set the pose from a 3D position (meters) and yaw/pitch/roll angles (radians) - This method recomputes...
Definition: CPose3D.cpp:255
A TCP socket that can be connected to a TCP server, implementing MRPT&#39;s CStream interface for passing...
void processPreview(const mrpt::obs::CObservation2DRangeScan &obs)
Must be called inside the capture method to allow optional GUI preview of scans.
bool setMotorSpeed(int motoSpeed_rpm)
Changes the motor speed in rpm&#39;s (default 600rpm)
Definition: CHokuyoURG.cpp:669
double d_min
Min/Max ranges, in meters.
Definition: CHokuyoURG.h:84
The namespace for 3D scene representation and rendering.
Definition: CGlCanvasBase.h:15
This software driver implements the protocol SCIP-2.0 for interfacing HOKUYO URG/UTM/UXM/UST laser sc...
Definition: CHokuyoURG.h:74
float aperture
The "aperture" or field-of-view of the range finder, in radians (typically M_PI = 180 degrees)...
void loadConfig_sensorSpecific(const mrpt::config::CConfigFileBase &configSource, const std::string &iniSection) override
See the class documentation at the top for expected parameters.
Definition: CHokuyoURG.cpp:221
bool setHighBaudrate()
Passes to 115200bps bitrate.
Definition: CHokuyoURG.cpp:399
std::string exception_to_str(const std::exception &e)
Builds a nice textual representation of a nested exception, which if generated using MRPT macros (THR...
Definition: exceptions.cpp:59
poses::CPose3D m_sensorPose
The sensor 6D pose:
Definition: CHokuyoURG.h:99
unsigned int m_scan_interval
Definition: CHokuyoURG.h:289
#define MRPT_LOAD_HERE_CONFIG_VAR( variableName, variableType, targetVariable, configFileObject, sectionNameStr)
TSensorInfo m_sensor_info
The information gathered when the laser is first open.
Definition: CHokuyoURG.h:276
bool m_highSensMode
High sensitivity [HS] mode (default: false)
Definition: CHokuyoURG.h:107
void push_many(T *array_elements, size_t count)
Insert an array of elements in the buffer.
size_t getReadPendingBytes()
Return the number of bytes already in the receive queue (they can be read without waiting) ...
std::string model
The sensor model.
Definition: CHokuyoURG.h:82
Serial and networking devices and utilities.
double m_reduced_fov
Used to reduce artificially the interval of scan ranges.
Definition: CHokuyoURG.h:261
int motor_speed_rpm
Standard motor speed, rpm.
Definition: CHokuyoURG.h:90
size_t readAsync(void *Buffer, const size_t Count, const int timeoutStart_ms=-1, const int timeoutBetween_ms=-1)
A method for reading from the socket with an optional timeout.
#define __CURRENT_FUNCTION_NAME__
A macro for obtaining the name of the current function:
Definition: common.h:150
#define MRPT_LOG_ERROR_STREAM(__CONTENTS)
unsigned __int32 uint32_t
Definition: rptypes.h:50
bool setHighSensitivityMode(bool enabled)
Changes the high sensitivity mode (HS) (default: false)
Definition: CHokuyoURG.cpp:706
void bindIO(mrpt::io::CStream *streamIO)
Binds the object to a given I/O channel.
mrpt::poses::CPose3D sensorPose
The 6D pose of the sensor on the robot at the moment of starting the scan.
void sendCmd(const char *str)
Definition: CHokuyoURG.cpp:56
int sprintf(char *buf, size_t bufSize, const char *format,...) noexcept MRPT_printf_format_check(3
An OS-independent version of sprintf (Notice the bufSize param, which may be ignored in some compiler...
Definition: os.cpp:191
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
std::string m_ip_dir
If set to non-empty and m_port_dir too, the program will try to connect to a Hokuyo using Ethernet co...
Definition: CHokuyoURG.h:270
void setScanRangeValidity(const size_t i, const bool val)
int scans_per_360deg
Number of measuremens per 360 degrees.
Definition: CHokuyoURG.h:86



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 1de0e027c Sat Sep 14 16:15:22 2019 +0200 at sáb sep 14 16:20:14 CEST 2019