MRPT  2.0.3
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 }
os.h
CClientTCPSocket.h
mrpt::comms::CClientTCPSocket::getReadPendingBytes
size_t getReadPendingBytes()
Return the number of bytes already in the receive queue (they can be read without waiting)
Definition: CClientTCPSocket.cpp:449
mrpt::comms::CClientTCPSocket
A TCP socket that can be connected to a TCP server, implementing MRPT's CStream interface for passing...
Definition: CClientTCPSocket.h:34
mrpt::obs::CObservation::sensorLabel
std::string sensorLabel
An arbitrary label that can be used to identify the sensor.
Definition: CObservation.h:62
mrpt::obs::CObservation2DRangeScan::setScanRange
void setScanRange(const size_t i, const float val)
Definition: CObservation2DRangeScan.cpp:507
mrpt::obs::CObservation2DRangeScan::maxRange
float maxRange
The maximum range allowed by the device, in meters (e.g.
Definition: CObservation2DRangeScan.h:119
mrpt::obs::CObservation2DRangeScan::rightToLeft
bool rightToLeft
The scanning direction: true=counterclockwise; false=clockwise.
Definition: CObservation2DRangeScan.h:116
mrpt::hwdrivers::CHokuyoURG::m_rx_buffer
mrpt::containers::circular_buffer< uint8_t > m_rx_buffer
Auxiliary buffer for readings.
Definition: CHokuyoURG.h:101
ASSERT_EQUAL_
#define ASSERT_EQUAL_(__A, __B)
Assert comparing two values, reporting their actual values upon failure.
Definition: exceptions.h:137
__CURRENT_FUNCTION_NAME__
#define __CURRENT_FUNCTION_NAME__
A macro for obtaining the name of the current function:
Definition: common.h:134
mrpt::hwdrivers::C2DRangeFinderAbstract::internal_notifyNoScanReceived
bool internal_notifyNoScanReceived()
Must be called from doProcessSimple() implementations.
Definition: C2DRangeFinderAbstract.cpp:102
MRPT_LOG_DEBUG
#define MRPT_LOG_DEBUG(_STRING)
Use: MRPT_LOG_DEBUG("message");
Definition: system/COutputLogger.h:427
mrpt::obs::CObservation2DRangeScan
A "CObservation"-derived class that represents a 2D range scan measurement (typically from a laser sc...
Definition: CObservation2DRangeScan.h:54
mrpt::hwdrivers::CHokuyoURG::TSensorInfo::scan_last
int scan_last
Definition: CHokuyoURG.h:88
mrpt::hwdrivers::CHokuyoURG::m_lastRange
int m_lastRange
Definition: CHokuyoURG.h:95
mrpt::hwdrivers::C2DRangeFinderAbstract::bindIO
void bindIO(const std::shared_ptr< mrpt::io::CStream > &streamIO)
Binds the object to a given I/O channel.
Definition: C2DRangeFinderAbstract.cpp:35
MINIMUM_PACKETS_TO_SET_TIMESTAMP_REFERENCE
const int MINIMUM_PACKETS_TO_SET_TIMESTAMP_REFERENCE
Definition: CHokuyoURG.cpp:28
mrpt::obs::CObservation2DRangeScan::getScanRange
const float & getScanRange(const size_t i) const
The range values of the scan, in meters.
Definition: CObservation2DRangeScan.cpp:496
mrpt::poses::CPose3D::setFromValues
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
mrpt::hwdrivers::CHokuyoURG::m_timeStartUI
uint32_t m_timeStartUI
Time of the first data packet, for synchronization purposes.
Definition: CHokuyoURG.h:282
CSerialPort.h
mrpt::hwdrivers::CHokuyoURG::TSensorInfo::d_max
double d_max
Definition: CHokuyoURG.h:84
mrpt::hwdrivers::CHokuyoURG::TSensorInfo
Used in CHokuyoURG::displayVersionInfo.
Definition: CHokuyoURG.h:79
mrpt::hwdrivers::CHokuyoURG::closeStreamConnection
void closeStreamConnection()
Called upon dtor, or when trying to recover from a disconnected sensor.
Definition: CHokuyoURG.cpp:45
mrpt::hwdrivers::C2DRangeFinderAbstract::filterByExclusionAngles
void filterByExclusionAngles(mrpt::obs::CObservation2DRangeScan &obs) const
Mark as invalid those ranges in a set of forbiden angle ranges.
Definition: C2DRangeFinderAbstract.cpp:205
mrpt::system::now
mrpt::system::TTimeStamp now()
A shortcut for system::getCurrentTime.
Definition: datetime.h:86
mrpt::hwdrivers::CHokuyoURG::TSensorInfo::scan_front
int scan_front
Definition: CHokuyoURG.h:88
mrpt::comms::CSerialPort
A communications serial port built as an implementation of a utils::CStream.
Definition: CSerialPort.h:41
mrpt::hwdrivers::CGenericSensor::ssWorking
@ ssWorking
Definition: CGenericSensor.h:87
mrpt::hwdrivers::CHokuyoURG::m_timeStartSynchDelay
int m_timeStartSynchDelay
Counter to discard to first few packets before setting the correspondence between device and computer...
Definition: CHokuyoURG.h:285
mrpt::hwdrivers::CHokuyoURG::m_firstRange
int m_firstRange
The first and last ranges to consider from the scan.
Definition: CHokuyoURG.h:95
mrpt::config::CConfigFileBase::read_bool
bool read_bool(const std::string &section, const std::string &name, bool defaultValue, bool failIfNotFound=false) const
Definition: CConfigFileBase.cpp:155
mrpt::hwdrivers
Contains classes for various device interfaces.
Definition: C2DRangeFinderAbstract.h:19
mrpt::hwdrivers::CHokuyoURG::m_rcv_status1
char m_rcv_status1
Definition: CHokuyoURG.h:252
mrpt::hwdrivers::CGenericSensor::ssError
@ ssError
Definition: CGenericSensor.h:88
CHokuyoURG.h
mrpt::comms
Serial and networking devices and utilities.
Definition: CClientTCPSocket.h:21
mrpt::hwdrivers::C2DRangeFinderAbstract::internal_notifyGoodScanNow
void internal_notifyGoodScanNow()
Must be called from doProcessSimple() implementations.
Definition: C2DRangeFinderAbstract.cpp:87
mrpt::hwdrivers::CHokuyoURG::~CHokuyoURG
~CHokuyoURG() override
Destructor: turns the laser off.
Definition: CHokuyoURG.cpp:32
mrpt::hwdrivers::CHokuyoURG::enableSCIP20
bool enableSCIP20()
Enables the SCIP2.0 protocol (this must be called at the very begining!).
Definition: CHokuyoURG.cpp:600
mrpt::obs::CObservation2DRangeScan::resizeScan
void resizeScan(const size_t len)
Resizes all data vectors to allocate a given number of scan rays.
Definition: CObservation2DRangeScan.cpp:541
mrpt::hwdrivers::CHokuyoURG::turnOff
bool turnOff() override
Disables the scanning mode (this can be used to turn the device in low energy mode,...
Definition: CHokuyoURG.cpp:388
THROW_EXCEPTION
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
mrpt::comms::CSerialPort::purgeBuffers
void purgeBuffers()
Purge tx and rx buffers.
Definition: CSerialPort.cpp:899
ASSERT_
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
mrpt::obs::CObservation2DRangeScan::setScanRangeValidity
void setScanRangeValidity(const size_t i, const bool val)
Definition: CObservation2DRangeScan.cpp:534
mrpt::hwdrivers::CGenericSensor::m_state
TSensorState m_state
Definition: CGenericSensor.h:148
mrpt::hwdrivers::CHokuyoURG::TSensorInfo::d_min
double d_min
Min/Max ranges, in meters.
Definition: CHokuyoURG.h:84
MRPT_LOG_DEBUG_FMT
#define MRPT_LOG_DEBUG_FMT(_FMT_STRING,...)
Use: MRPT_LOG_DEBUG_FMT("i=%u", i);
Definition: system/COutputLogger.h:461
mrpt::hwdrivers::CHokuyoURG::ensureStreamIsOpen
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
mrpt::obs
This namespace contains representation of robot actions and observations.
Definition: CParticleFilter.h:17
mrpt::obs::CObservation::timestamp
mrpt::system::TTimeStamp timestamp
The associated UTC time-stamp.
Definition: CObservation.h:60
mrpt::config::CConfigFileBase::read_string
std::string read_string(const std::string &section, const std::string &name, const std::string &defaultValue, bool failIfNotFound=false) const
Definition: CConfigFileBase.cpp:171
mrpt::hwdrivers::CHokuyoURG::m_timeStartTT
mrpt::system::TTimeStamp m_timeStartTT
Definition: CHokuyoURG.h:286
mrpt::hwdrivers::CHokuyoURG::m_motorSpeed_rpm
int m_motorSpeed_rpm
The motor speed (default=600rpm)
Definition: CHokuyoURG.h:97
mrpt::hwdrivers::CHokuyoURG::turnOn
bool turnOn() override
Enables the scanning mode (which may depend on the specific laser device); this must be called before...
Definition: CHokuyoURG.cpp:277
mrpt::config::CConfigFileBase::read_int
int read_int(const std::string &section, const std::string &name, int defaultValue, bool failIfNotFound=false) const
Definition: CConfigFileBase.cpp:130
mrpt::hwdrivers::CHokuyoURG::getScanInterval
unsigned int getScanInterval() const
Definition: CHokuyoURG.cpp:668
mrpt::obs::CObservation2DRangeScan::setScanIntensity
void setScanIntensity(const size_t i, const int val)
Definition: CObservation2DRangeScan.cpp:523
mrpt::system::os::sprintf
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...
mrpt::hwdrivers::CHokuyoURG::m_port_dir
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
mrpt::comms::CSerialPort::setConfig
void setConfig(int baudRate, int parity=0, int bits=8, int nStopBits=1, bool enableFlowControl=false)
Changes the configuration of the port.
Definition: CSerialPort.cpp:202
ASSERT_BELOW_
#define ASSERT_BELOW_(__A, __B)
Definition: exceptions.h:149
mrpt::hwdrivers::CHokuyoURG::m_disable_firmware_timestamp
bool m_disable_firmware_timestamp
Definition: CHokuyoURG.h:287
mrpt::hwdrivers::CHokuyoURG::switchLaserOn
bool switchLaserOn()
Switchs the laser on.
Definition: CHokuyoURG.cpp:622
MRPT_LOAD_HERE_CONFIG_VAR
#define MRPT_LOAD_HERE_CONFIG_VAR( variableName, variableType, targetVariable, configFileObject, sectionNameStr)
Definition: config/CConfigFileBase.h:352
mrpt::hwdrivers::CGenericSensor::m_verbose
bool m_verbose
Definition: CGenericSensor.h:149
mrpt::containers::circular_buffer::capacity
size_t capacity() const
Return the maximum capacity of the buffer.
Definition: circular_buffer.h:154
mrpt::hwdrivers::CHokuyoURG::purgeBuffers
void purgeBuffers()
Empties the RX buffers of the serial port.
Definition: CHokuyoURG.cpp:1037
mrpt::containers::circular_buffer::size
size_t size() const
Return the number of elements available for read ("pop") in the buffer (this is NOT the maximum size ...
Definition: circular_buffer.h:143
mrpt::obs::CObservation2DRangeScan::aperture
float aperture
The "aperture" or field-of-view of the range finder, in radians (typically M_PI = 180 degrees).
Definition: CObservation2DRangeScan.h:114
mrpt::obs::CObservation2DRangeScan::setScanHasIntensity
void setScanHasIntensity(bool setHasIntensityFlag)
Marks this scan as having or not intensity data.
Definition: CObservation2DRangeScan.cpp:253
mrpt::containers::circular_buffer::peek
T peek() const
Peek (see without modifying) what is to be read from the buffer if pop() was to be called.
Definition: circular_buffer.h:106
mrpt::hwdrivers::CHokuyoURG::loadConfig_sensorSpecific
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
MRPT_START
#define MRPT_START
Definition: exceptions.h:241
mrpt::config::CConfigFileBase
This class allows loading and storing values and vectors of different types from a configuration text...
Definition: config/CConfigFileBase.h:44
mrpt::hwdrivers::CHokuyoURG::ensureBufferHasBytes
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
mrpt::hwdrivers::C2DRangeFinderAbstract::loadCommonParams
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).
Definition: C2DRangeFinderAbstract.cpp:119
mrpt::hwdrivers::CHokuyoURG::m_scan_interval
unsigned int m_scan_interval
Definition: CHokuyoURG.h:290
mrpt::RAD2DEG
constexpr double RAD2DEG(const double x)
Radians to degrees.
Definition: core/include/mrpt/core/bits_math.h:56
MRPT_LOG_ERROR_FMT
#define MRPT_LOG_ERROR_FMT(_FMT_STRING,...)
Definition: system/COutputLogger.h:467
MRPT_LOG_ERROR
#define MRPT_LOG_ERROR(_STRING)
Definition: system/COutputLogger.h:433
mrpt::hwdrivers::CHokuyoURG::doProcessSimple
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
mrpt::hwdrivers::CHokuyoURG::m_lastSentMeasCmd
std::string m_lastSentMeasCmd
The last sent measurement command (MDXXX), including the last 0x0A.
Definition: CHokuyoURG.h:104
mrpt::hwdrivers::CHokuyoURG::m_win
mrpt::gui::CDisplayWindow3D::Ptr m_win
Definition: CHokuyoURG.h:108
mrpt::hwdrivers::CHokuyoURG::initialize
void initialize() override
Turns the laser on.
Definition: CHokuyoURG.cpp:1024
mrpt::obs::gnss::b2
double b2
Definition: gnss_messages_novatel.h:642
mrpt::hwdrivers::CHokuyoURG::setIntensityMode
bool setIntensityMode(bool enabled)
If true scans will capture intensity.
Definition: CHokuyoURG.cpp:733
mrpt::system::COutputLogger::setMinLoggingLevel
void setMinLoggingLevel(const VerbosityLevel level)
Set the minimum logging level for which the incoming logs are going to be taken into account.
Definition: COutputLogger.cpp:144
mrpt::hwdrivers::CHokuyoURG::m_reduced_fov
double m_reduced_fov
Used to reduce artificially the interval of scan ranges.
Definition: CHokuyoURG.h:264
mrpt::comms::CClientTCPSocket::readAsync
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.
Definition: CClientTCPSocket.cpp:278
mrpt::hwdrivers::CGenericSensor::m_sensorLabel
std::string m_sensorLabel
See CGenericSensor.
Definition: CGenericSensor.h:141
mrpt::obs::CObservation2DRangeScan::stdError
float stdError
The "sigma" error of the device in meters, used while inserting the scan in an occupancy grid.
Definition: CObservation2DRangeScan.h:125
mrpt::config::CConfigFileBase::read_float
float read_float(const std::string &section, const std::string &name, float defaultValue, bool failIfNotFound=false) const
Definition: CConfigFileBase.cpp:118
mrpt::hwdrivers::CHokuyoURG::displayVersionInfo
bool displayVersionInfo()
Ask to the device, and print to the debug stream, details about the firmware version,...
Definition: CHokuyoURG.cpp:739
mrpt::hwdrivers::CHokuyoURG::displaySensorInfo
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
mrpt::containers::circular_buffer::pop
T pop()
Retrieve an element from the buffer.
Definition: circular_buffer.h:73
mrpt::DEG2RAD
constexpr double DEG2RAD(const double x)
Degrees to radians
Definition: core/include/mrpt/core/bits_math.h:47
mrpt::hwdrivers::CHokuyoURG::m_rcv_status0
char m_rcv_status0
Definition: CHokuyoURG.h:252
mrpt::hwdrivers::CHokuyoURG::TSensorInfo::scan_first
int scan_first
First, last, and front step of the scanner angular span.
Definition: CHokuyoURG.h:88
ASSERTMSG_
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
Definition: exceptions.h:108
mrpt::containers::circular_buffer::push_many
void push_many(T *array_elements, size_t count)
Insert an array of elements in the buffer.
Definition: circular_buffer.h:65
IMPLEMENTS_GENERIC_SENSOR
#define IMPLEMENTS_GENERIC_SENSOR(class_name, NameSpace)
This must be inserted in all CGenericSensor classes implementation files:
Definition: CGenericSensor.h:314
mrpt::containers::circular_buffer::available
size_t available() const
The maximum number of elements that can be written ("push") without rising an overflow error.
Definition: circular_buffer.h:158
mrpt::hwdrivers::CHokuyoURG::sendCmd
void sendCmd(const char *str)
Definition: CHokuyoURG.cpp:54
mrpt::hwdrivers::CHokuyoURG::setHighSensitivityMode
bool setHighSensitivityMode(bool enabled)
Changes the high sensitivity mode (HS) (default: false)
Definition: CHokuyoURG.cpp:705
mrpt::hwdrivers::C2DRangeFinderAbstract::processPreview
void processPreview(const mrpt::obs::CObservation2DRangeScan &obs)
Must be called inside the capture method to allow optional GUI preview of scans.
Definition: C2DRangeFinderAbstract.cpp:211
MRPT_LOG_DEBUG_STREAM
#define MRPT_LOG_DEBUG_STREAM(__CONTENTS)
Use: MRPT_LOG_DEBUG_STREAM("Var=" << value << " foo=" << foo_var);
Definition: system/COutputLogger.h:471
mrpt::system::LVL_DEBUG
@ LVL_DEBUG
Definition: system/COutputLogger.h:30
MRPT_LOG_ERROR_STREAM
#define MRPT_LOG_ERROR_STREAM(__CONTENTS)
Definition: system/COutputLogger.h:477
CAxis.h
mrpt::hwdrivers::C2DRangeFinderAbstract::m_stream
std::shared_ptr< mrpt::io::CStream > m_stream
The I/O channel (will be nullptr if not bound).
Definition: C2DRangeFinderAbstract.h:69
M_PI
#define M_PI
Definition: core/include/mrpt/core/bits_math.h:43
mrpt::hwdrivers::CHokuyoURG::m_ip_dir
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
mrpt::hwdrivers::CHokuyoURG::parseResponse
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
MRPT_END
#define MRPT_END
Definition: exceptions.h:245
mrpt::obs::gnss::b3
double b3
Definition: gnss_messages_novatel.h:642
mrpt::hwdrivers::CHokuyoURG::TSensorInfo::scans_per_360deg
int scans_per_360deg
Number of measuremens per 360 degrees.
Definition: CHokuyoURG.h:86
mrpt::hwdrivers::CHokuyoURG
This software driver implements the protocol SCIP-2.0 for interfacing HOKUYO URG/UTM/UXM/UST laser sc...
Definition: CHokuyoURG.h:74
mrpt::hwdrivers::CHokuyoURG::m_com_port
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
mrpt::hwdrivers::CHokuyoURG::TSensorInfo::motor_speed_rpm
int motor_speed_rpm
Standard motor speed, rpm.
Definition: CHokuyoURG.h:90
MRPT_LOG_INFO_STREAM
#define MRPT_LOG_INFO_STREAM(__CONTENTS)
Definition: system/COutputLogger.h:473
mrpt::hwdrivers::CHokuyoURG::switchLaserOff
bool switchLaserOff()
Switchs the laser off.
Definition: CHokuyoURG.cpp:643
mrpt::hwdrivers::CHokuyoURG::TSensorInfo::model
std::string model
The sensor model.
Definition: CHokuyoURG.h:82
mrpt::hwdrivers::CHokuyoURG::m_rcv_data
std::string m_rcv_data
temp buffer for incoming data packets
Definition: CHokuyoURG.h:251
CPlanarLaserScan.h
mrpt::exception_to_str
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
mrpt::hwdrivers::CHokuyoURG::setMotorSpeed
bool setMotorSpeed(int motoSpeed_rpm)
Changes the motor speed in rpm's (default 600rpm)
Definition: CHokuyoURG.cpp:669
mrpt::obs::CObservation2DRangeScan::sensorPose
mrpt::poses::CPose3D sensorPose
The 6D pose of the sensor on the robot at the moment of starting the scan.
Definition: CObservation2DRangeScan.h:122
mrpt::hwdrivers::CHokuyoURG::startScanningMode
bool startScanningMode()
Start the continuous scanning mode, using parameters stored in the object (loaded from the ....
Definition: CHokuyoURG.cpp:863
mrpt::hwdrivers::CHokuyoURG::setScanInterval
void setScanInterval(unsigned int skipScanCount)
Set the skip scan count (0 means send all scans).
Definition: CHokuyoURG.cpp:664
mrpt::opengl
The namespace for 3D scene representation and rendering.
Definition: CGlCanvasBase.h:13
mrpt::obs::gnss::b1
double b1
Definition: gnss_messages_novatel.h:642
mrpt::hwdrivers::C2DRangeFinderAbstract::filterByExclusionAreas
void filterByExclusionAreas(mrpt::obs::CObservation2DRangeScan &obs) const
Mark as invalid those points which (x,y) coordinates fall within the exclusion polygons.
Definition: C2DRangeFinderAbstract.cpp:196
mrpt::hwdrivers::CHokuyoURG::m_sensor_info
TSensorInfo m_sensor_info
The information gathered when the laser is first open.
Definition: CHokuyoURG.h:279
mrpt::hwdrivers::CHokuyoURG::m_intensity
bool m_intensity
Get intensity from lidar scan (default: false)
Definition: CHokuyoURG.h:289
mrpt::system
Definition: backtrace.h:14
hwdrivers-precomp.h
mrpt::hwdrivers::CHokuyoURG::setHighBaudrate
bool setHighBaudrate()
Passes to 115200bps bitrate.
Definition: CHokuyoURG.cpp:396
mrpt::hwdrivers::CHokuyoURG::m_highSensMode
bool m_highSensMode
High sensitivity [HS] mode (default: false)
Definition: CHokuyoURG.h:107
mrpt::hwdrivers::CHokuyoURG::m_sensorPose
poses::CPose3D m_sensorPose
The sensor 6D pose:
Definition: CHokuyoURG.h:99
mrpt::system::os::memcpy
void memcpy(void *dest, size_t destSize, const void *src, size_t copyCount) noexcept
An OS and compiler independent version of "memcpy".



Page generated by Doxygen 1.8.17 for MRPT 2.0.3 at Fri May 29 13:06:46 UTC 2020