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