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



Page generated by Doxygen 1.8.17 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at miƩ 12 jul 2023 10:03:34 CEST